drivers/greybus/gb-beagleplay.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Beagleplay Linux Driver for Greybus
   4  *
   5  * Copyright (c) 2023 Ayush Singh <ayushdevel1325@gmail.com>
   6  * Copyright (c) 2023 BeagleBoard.org Foundation
   7  */
   8
   9 #include <linux/gfp.h>
  10 #include <linux/greybus.h>
  11 #include <linux/module.h>
  12 #include <linux/of.h>
  13 #include <linux/printk.h>
  14 #include <linux/serdev.h>
  15 #include <linux/tty.h>
  16 #include <linux/tty_driver.h>
  17 #include <linux/greybus/hd.h>
  18 #include <linux/init.h>
  19 #include <linux/device.h>
  20 #include <linux/crc-ccitt.h>
  21 #include <linux/circ_buf.h>
  22 #include <linux/types.h>
  23 #include <linux/workqueue.h>
  24
  25 #define RX_HDLC_PAYLOAD 256
  26 #define CRC_LEN 2
  27 #define MAX_RX_HDLC (1 + RX_HDLC_PAYLOAD + CRC_LEN)
  28 #define TX_CIRC_BUF_SIZE 1024
  29
  30 #define ADDRESS_GREYBUS 0x01
  31 #define ADDRESS_DBG 0x02
  32 #define ADDRESS_CONTROL 0x03
  33
  34 #define HDLC_FRAME 0x7E
  35 #define HDLC_ESC 0x7D
  36 #define HDLC_XOR 0x20
  37
  38 #define CONTROL_SVC_START 0x01
  39 #define CONTROL_SVC_STOP 0x02
  40
  41 /* The maximum number of CPorts supported by Greybus Host Device */
  42 #define GB_MAX_CPORTS 32
  43
  44 /**
  45  * struct gb_beagleplay - BeaglePlay Greybus driver
  46  *
  47  * @sd: underlying serdev device
  48  *
  49  * @gb_hd: greybus host device
  50  *
  51  * @tx_work: hdlc transmit work
  52  * @tx_producer_lock: hdlc transmit data producer lock. acquired when appending data to buffer.
  53  * @tx_consumer_lock: hdlc transmit data consumer lock. acquired when sending data over uart.
  54  * @tx_circ_buf: hdlc transmit circular buffer.
  55  * @tx_crc: hdlc transmit crc-ccitt fcs
  56  *
  57  * @rx_buffer_len: length of receive buffer filled.
  58  * @rx_buffer: hdlc frame receive buffer
  59  * @rx_in_esc: hdlc rx flag to indicate ESC frame
  60  */
  61 struct gb_beagleplay {
  62         struct serdev_device *sd;
  63
  64         struct gb_host_device *gb_hd;
  65
  66         struct work_struct tx_work;
  67         spinlock_t tx_producer_lock;
  68         spinlock_t tx_consumer_lock;
  69         struct circ_buf tx_circ_buf;
  70         u16 tx_crc;
  71
  72         u16 rx_buffer_len;
  73         bool rx_in_esc;
  74         u8 rx_buffer[MAX_RX_HDLC];
  75 };
  76
  77 /**
  78  * struct hdlc_payload - Structure to represent part of HDCL frame payload data.
  79  *
  80  * @len: buffer length in bytes
  81  * @buf: payload buffer
  82  */
  83 struct hdlc_payload {
  84         u16 len;
  85         void *buf;
  86 };
  87
  88 /**
  89  * struct hdlc_greybus_frame - Structure to represent greybus HDLC frame payload
  90  *
  91  * @cport: cport id
  92  * @hdr: greybus operation header
  93  * @payload: greybus message payload
  94  *
  95  * The HDLC payload sent over UART for greybus address has cport preappended to greybus message
  96  */
  97 struct hdlc_greybus_frame {
  98         __le16 cport;
  99         struct gb_operation_msg_hdr hdr;
 100         u8 payload[];
 101 } __packed;
 102
 103 static void hdlc_rx_greybus_frame(struct gb_beagleplay *bg, u8 *buf, u16 len)
 104 {
 105         struct hdlc_greybus_frame *gb_frame = (struct hdlc_greybus_frame *)buf;
 106         u16 cport_id = le16_to_cpu(gb_frame->cport);
 107         u16 gb_msg_len = le16_to_cpu(gb_frame->hdr.size);
 108
 109         dev_dbg(&bg->sd->dev, "Greybus Operation %u type %X cport %u status %u received",
 110                 gb_frame->hdr.operation_id, gb_frame->hdr.type, cport_id, gb_frame->hdr.result);
 111
 112         greybus_data_rcvd(bg->gb_hd, cport_id, (u8 *)&gb_frame->hdr, gb_msg_len);
 113 }
 114
 115 static void hdlc_rx_dbg_frame(const struct gb_beagleplay *bg, const char *buf, u16 len)
 116 {
 117         dev_dbg(&bg->sd->dev, "CC1352 Log: %.*s", (int)len, buf);
 118 }
 119
 120 /**
 121  * hdlc_write() - Consume HDLC Buffer.
 122  * @bg: beagleplay greybus driver
 123  *
 124  * Assumes that consumer lock has been acquired.
 125  */
 126 static void hdlc_write(struct gb_beagleplay *bg)
 127 {
 128         int written;
 129         /* Start consuming HDLC data */
 130         int head = smp_load_acquire(&bg->tx_circ_buf.head);
 131         int tail = bg->tx_circ_buf.tail;
 132         int count = CIRC_CNT_TO_END(head, tail, TX_CIRC_BUF_SIZE);
 133         const unsigned char *buf = &bg->tx_circ_buf.buf[tail];
 134
 135         if (count > 0) {
 136                 written = serdev_device_write_buf(bg->sd, buf, count);
 137
 138                 /* Finish consuming HDLC data */
 139                 smp_store_release(&bg->tx_circ_buf.tail, (tail + written) & (TX_CIRC_BUF_SIZE - 1));
 140         }
 141 }
 142
 143 /**
 144  * hdlc_append() - Queue HDLC data for sending.
 145  * @bg: beagleplay greybus driver
 146  * @value: hdlc byte to transmit
 147  *
 148  * Assumes that producer lock as been acquired.
 149  */
 150 static void hdlc_append(struct gb_beagleplay *bg, u8 value)
 151 {
 152         int tail, head = bg->tx_circ_buf.head;
 153
 154         while (true) {
 155                 tail = READ_ONCE(bg->tx_circ_buf.tail);
 156
 157                 if (CIRC_SPACE(head, tail, TX_CIRC_BUF_SIZE) >= 1) {
 158                         bg->tx_circ_buf.buf[head] = value;
 159
 160                         /* Finish producing HDLC byte */
 161                         smp_store_release(&bg->tx_circ_buf.head,
 162                                           (head + 1) & (TX_CIRC_BUF_SIZE - 1));
 163                         return;
 164                 }
 165                 dev_warn(&bg->sd->dev, "Tx circ buf full");
 166                 usleep_range(3000, 5000);
 167         }
 168 }
 169
 170 static void hdlc_append_escaped(struct gb_beagleplay *bg, u8 value)
 171 {
 172         if (value == HDLC_FRAME || value == HDLC_ESC) {
 173                 hdlc_append(bg, HDLC_ESC);
 174                 value ^= HDLC_XOR;
 175         }
 176         hdlc_append(bg, value);
 177 }
 178
 179 static void hdlc_append_tx_frame(struct gb_beagleplay *bg)
 180 {
 181         bg->tx_crc = 0xFFFF;
 182         hdlc_append(bg, HDLC_FRAME);
 183 }
 184
 185 static void hdlc_append_tx_u8(struct gb_beagleplay *bg, u8 value)
 186 {
 187         bg->tx_crc = crc_ccitt(bg->tx_crc, &value, 1);
 188         hdlc_append_escaped(bg, value);
 189 }
 190
 191 static void hdlc_append_tx_buf(struct gb_beagleplay *bg, const u8 *buf, u16 len)
 192 {
 193         size_t i;
 194
 195         for (i = 0; i < len; i++)
 196                 hdlc_append_tx_u8(bg, buf[i]);
 197 }
 198
 199 static void hdlc_append_tx_crc(struct gb_beagleplay *bg)
 200 {
 201         bg->tx_crc ^= 0xffff;
 202         hdlc_append_escaped(bg, bg->tx_crc & 0xff);
 203         hdlc_append_escaped(bg, (bg->tx_crc >> 8) & 0xff);
 204 }
 205
 206 static void hdlc_transmit(struct work_struct *work)
 207 {
 208         struct gb_beagleplay *bg = container_of(work, struct gb_beagleplay, tx_work);
 209
 210         spin_lock_bh(&bg->tx_consumer_lock);
 211         hdlc_write(bg);
 212         spin_unlock_bh(&bg->tx_consumer_lock);
 213 }
 214
 215 static void hdlc_tx_frames(struct gb_beagleplay *bg, u8 address, u8 control,
 216                            const struct hdlc_payload payloads[], size_t count)
 217 {
 218         size_t i;
 219
 220         spin_lock(&bg->tx_producer_lock);
 221
 222         hdlc_append_tx_frame(bg);
 223         hdlc_append_tx_u8(bg, address);
 224         hdlc_append_tx_u8(bg, control);
 225
 226         for (i = 0; i < count; ++i)
 227                 hdlc_append_tx_buf(bg, payloads[i].buf, payloads[i].len);
 228
 229         hdlc_append_tx_crc(bg);
 230         hdlc_append_tx_frame(bg);
 231
 232         spin_unlock(&bg->tx_producer_lock);
 233
 234         schedule_work(&bg->tx_work);
 235 }
 236
 237 static void hdlc_tx_s_frame_ack(struct gb_beagleplay *bg)
 238 {
 239         hdlc_tx_frames(bg, bg->rx_buffer[0], (bg->rx_buffer[1] >> 1) & 0x7, NULL, 0);
 240 }
 241
 242 static void hdlc_rx_frame(struct gb_beagleplay *bg)
 243 {
 244         u16 crc, len;
 245         u8 ctrl, *buf;
 246         u8 address = bg->rx_buffer[0];
 247
 248         crc = crc_ccitt(0xffff, bg->rx_buffer, bg->rx_buffer_len);
 249         if (crc != 0xf0b8) {
 250                 dev_warn_ratelimited(&bg->sd->dev, "CRC failed from %02x: 0x%04x", address, crc);
 251                 return;
 252         }
 253
 254         ctrl = bg->rx_buffer[1];
 255         buf = &bg->rx_buffer[2];
 256         len = bg->rx_buffer_len - 4;
 257
 258         /* I-Frame, send S-Frame ACK */
 259         if ((ctrl & 1) == 0)
 260                 hdlc_tx_s_frame_ack(bg);
 261
 262         switch (address) {
 263         case ADDRESS_DBG:
 264                 hdlc_rx_dbg_frame(bg, buf, len);
 265                 break;
 266         case ADDRESS_GREYBUS:
 267                 hdlc_rx_greybus_frame(bg, buf, len);
 268                 break;
 269         default:
 270                 dev_warn_ratelimited(&bg->sd->dev, "unknown frame %u", address);
 271         }
 272 }
 273
 274 static int hdlc_rx(struct gb_beagleplay *bg, const u8 *data, size_t count)
 275 {
 276         size_t i;
 277         u8 c;
 278
 279         for (i = 0; i < count; ++i) {
 280                 c = data[i];
 281
 282                 switch (c) {
 283                 case HDLC_FRAME:
 284                         if (bg->rx_buffer_len)
 285                                 hdlc_rx_frame(bg);
 286
 287                         bg->rx_buffer_len = 0;
 288                         break;
 289                 case HDLC_ESC:
 290                         bg->rx_in_esc = true;
 291                         break;
 292                 default:
 293                         if (bg->rx_in_esc) {
 294                                 c ^= 0x20;
 295                                 bg->rx_in_esc = false;
 296                         }
 297
 298                         if (bg->rx_buffer_len < MAX_RX_HDLC) {
 299                                 bg->rx_buffer[bg->rx_buffer_len] = c;
 300                                 bg->rx_buffer_len++;
 301                         } else {
 302                                 dev_err_ratelimited(&bg->sd->dev, "RX Buffer Overflow");
 303                                 bg->rx_buffer_len = 0;
 304                         }
 305                 }
 306         }
 307
 308         return count;
 309 }
 310
 311 static int hdlc_init(struct gb_beagleplay *bg)
 312 {
 313         INIT_WORK(&bg->tx_work, hdlc_transmit);
 314         spin_lock_init(&bg->tx_producer_lock);
 315         spin_lock_init(&bg->tx_consumer_lock);
 316         bg->tx_circ_buf.head = 0;
 317         bg->tx_circ_buf.tail = 0;
 318
 319         bg->tx_circ_buf.buf = devm_kmalloc(&bg->sd->dev, TX_CIRC_BUF_SIZE, GFP_KERNEL);
 320         if (!bg->tx_circ_buf.buf)
 321                 return -ENOMEM;
 322
 323         bg->rx_buffer_len = 0;
 324         bg->rx_in_esc = false;
 325
 326         return 0;
 327 }
 328
 329 static void hdlc_deinit(struct gb_beagleplay *bg)
 330 {
 331         flush_work(&bg->tx_work);
 332 }
 333
 334 static int gb_tty_receive(struct serdev_device *sd, const unsigned char *data, size_t count)
 335 {
 336         struct gb_beagleplay *bg = serdev_device_get_drvdata(sd);
 337
 338         return hdlc_rx(bg, data, count);
 339 }
 340
 341 static void gb_tty_wakeup(struct serdev_device *serdev)
 342 {
 343         struct gb_beagleplay *bg = serdev_device_get_drvdata(serdev);
 344
 345         schedule_work(&bg->tx_work);
 346 }
 347
 348 static struct serdev_device_ops gb_beagleplay_ops = {
 349         .receive_buf = gb_tty_receive,
 350         .write_wakeup = gb_tty_wakeup,
 351 };
 352
 353 /**
 354  * gb_message_send() - Send greybus message using HDLC over UART
 355  *
 356  * @hd: pointer to greybus host device
 357  * @cport: AP cport where message originates
 358  * @msg: greybus message to send
 359  * @mask: gfp mask
 360  *
 361  * Greybus HDLC frame has the following payload:
 362  * 1. le16 cport
 363  * 2. gb_operation_msg_hdr msg_header
 364  * 3. u8 *msg_payload
 365  */
 366 static int gb_message_send(struct gb_host_device *hd, u16 cport, struct gb_message *msg, gfp_t mask)
 367 {
 368         struct gb_beagleplay *bg = dev_get_drvdata(&hd->dev);
 369         struct hdlc_payload payloads[3];
 370         __le16 cport_id = cpu_to_le16(cport);
 371
 372         dev_dbg(&hd->dev, "Sending greybus message with Operation %u, Type: %X on Cport %u",
 373                 msg->header->operation_id, msg->header->type, cport);
 374
 375         if (le16_to_cpu(msg->header->size) > RX_HDLC_PAYLOAD)
 376                 return dev_err_probe(&hd->dev, -E2BIG, "Greybus message too big");
 377
 378         payloads[0].buf = &cport_id;
 379         payloads[0].len = sizeof(cport_id);
 380         payloads[1].buf = msg->header;
 381         payloads[1].len = sizeof(*msg->header);
 382         payloads[2].buf = msg->payload;
 383         payloads[2].len = msg->payload_size;
 384
 385         hdlc_tx_frames(bg, ADDRESS_GREYBUS, 0x03, payloads, 3);
 386         greybus_message_sent(bg->gb_hd, msg, 0);
 387
 388         return 0;
 389 }
 390
 391 static void gb_message_cancel(struct gb_message *message)
 392 {
 393 }
 394
 395 static struct gb_hd_driver gb_hdlc_driver = { .message_send = gb_message_send,
 396                                               .message_cancel = gb_message_cancel };
 397
 398 static void gb_beagleplay_start_svc(struct gb_beagleplay *bg)
 399 {
 400         const u8 command = CONTROL_SVC_START;
 401         const struct hdlc_payload payload = { .len = 1, .buf = (void *)&command };
 402
 403         hdlc_tx_frames(bg, ADDRESS_CONTROL, 0x03, &payload, 1);
 404 }
 405
 406 static void gb_beagleplay_stop_svc(struct gb_beagleplay *bg)
 407 {
 408         const u8 command = CONTROL_SVC_STOP;
 409         const struct hdlc_payload payload = { .len = 1, .buf = (void *)&command };
 410
 411         hdlc_tx_frames(bg, ADDRESS_CONTROL, 0x03, &payload, 1);
 412 }
 413
 414 static void gb_greybus_deinit(struct gb_beagleplay *bg)
 415 {
 416         gb_hd_del(bg->gb_hd);
 417         gb_hd_put(bg->gb_hd);
 418 }
 419
 420 static int gb_greybus_init(struct gb_beagleplay *bg)
 421 {
 422         int ret;
 423
 424         bg->gb_hd = gb_hd_create(&gb_hdlc_driver, &bg->sd->dev, TX_CIRC_BUF_SIZE, GB_MAX_CPORTS);
 425         if (IS_ERR(bg->gb_hd)) {
 426                 dev_err(&bg->sd->dev, "Failed to create greybus host device");
 427                 return PTR_ERR(bg->gb_hd);
 428         }
 429
 430         ret = gb_hd_add(bg->gb_hd);
 431         if (ret) {
 432                 dev_err(&bg->sd->dev, "Failed to add greybus host device");
 433                 goto free_gb_hd;
 434         }
 435         dev_set_drvdata(&bg->gb_hd->dev, bg);
 436
 437         return 0;
 438
 439 free_gb_hd:
 440         gb_greybus_deinit(bg);
 441         return ret;
 442 }
 443
 444 static void gb_serdev_deinit(struct gb_beagleplay *bg)
 445 {
 446         serdev_device_close(bg->sd);
 447 }
 448
 449 static int gb_serdev_init(struct gb_beagleplay *bg)
 450 {
 451         int ret;
 452
 453         serdev_device_set_drvdata(bg->sd, bg);
 454         serdev_device_set_client_ops(bg->sd, &gb_beagleplay_ops);
 455         ret = serdev_device_open(bg->sd);
 456         if (ret)
 457                 return dev_err_probe(&bg->sd->dev, ret, "Unable to open serial device");
 458
 459         serdev_device_set_baudrate(bg->sd, 115200);
 460         serdev_device_set_flow_control(bg->sd, false);
 461
 462         return 0;
 463 }
 464
 465 static int gb_beagleplay_probe(struct serdev_device *serdev)
 466 {
 467         int ret = 0;
 468         struct gb_beagleplay *bg;
 469
 470         bg = devm_kmalloc(&serdev->dev, sizeof(*bg), GFP_KERNEL);
 471         if (!bg)
 472                 return -ENOMEM;
 473
 474         bg->sd = serdev;
 475         ret = gb_serdev_init(bg);
 476         if (ret)
 477                 return ret;
 478
 479         ret = hdlc_init(bg);
 480         if (ret)
 481                 goto free_serdev;
 482
 483         ret = gb_greybus_init(bg);
 484         if (ret)
 485                 goto free_hdlc;
 486
 487         gb_beagleplay_start_svc(bg);
 488
 489         return 0;
 490
 491 free_hdlc:
 492         hdlc_deinit(bg);
 493 free_serdev:
 494         gb_serdev_deinit(bg);
 495         return ret;
 496 }
 497
 498 static void gb_beagleplay_remove(struct serdev_device *serdev)
 499 {
 500         struct gb_beagleplay *bg = serdev_device_get_drvdata(serdev);
 501
 502         gb_greybus_deinit(bg);
 503         gb_beagleplay_stop_svc(bg);
 504         hdlc_deinit(bg);
 505         gb_serdev_deinit(bg);
 506 }
 507
 508 static const struct of_device_id gb_beagleplay_of_match[] = {
 509         {
 510                 .compatible = "ti,cc1352p7",
 511         },
 512         {},
 513 };
 514 MODULE_DEVICE_TABLE(of, gb_beagleplay_of_match);
 515
 516 static struct serdev_device_driver gb_beagleplay_driver = {
 517         .probe = gb_beagleplay_probe,
 518         .remove = gb_beagleplay_remove,
 519         .driver = {
 520                 .name = "gb_beagleplay",
 521                 .of_match_table = gb_beagleplay_of_match,
 522         },
 523 };
 524
 525 module_serdev_device_driver(gb_beagleplay_driver);
 526
 527 MODULE_LICENSE("GPL");
 528 MODULE_AUTHOR("Ayush Singh <ayushdevel1325@gmail.com>");
 529 MODULE_DESCRIPTION("A Greybus driver for BeaglePlay");