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(hd: bg->gb_hd, cport_id, data: (u8 *)&gb_frame->hdr, length: 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(min: 3000, max: 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(crc: bg->tx_crc, buffer: &value, len: 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, value: 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, value: bg->tx_crc & 0xff); |
203 | hdlc_append_escaped(bg, value: (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(lock: &bg->tx_consumer_lock); |
211 | hdlc_write(bg); |
212 | spin_unlock_bh(lock: &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(lock: &bg->tx_producer_lock); |
221 | |
222 | hdlc_append_tx_frame(bg); |
223 | hdlc_append_tx_u8(bg, value: address); |
224 | hdlc_append_tx_u8(bg, value: control); |
225 | |
226 | for (i = 0; i < count; ++i) |
227 | hdlc_append_tx_buf(bg, buf: payloads[i].buf, len: payloads[i].len); |
228 | |
229 | hdlc_append_tx_crc(bg); |
230 | hdlc_append_tx_frame(bg); |
231 | |
232 | spin_unlock(lock: &bg->tx_producer_lock); |
233 | |
234 | schedule_work(work: &bg->tx_work); |
235 | } |
236 | |
237 | static void hdlc_tx_s_frame_ack(struct gb_beagleplay *bg) |
238 | { |
239 | hdlc_tx_frames(bg, address: bg->rx_buffer[0], control: (bg->rx_buffer[1] >> 1) & 0x7, NULL, count: 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(crc: 0xffff, buffer: bg->rx_buffer, len: 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 size_t 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(dev: &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(work: &bg->tx_work); |
332 | } |
333 | |
334 | static size_t gb_tty_receive(struct serdev_device *sd, const u8 *data, |
335 | size_t count) |
336 | { |
337 | struct gb_beagleplay *bg = serdev_device_get_drvdata(serdev: sd); |
338 | |
339 | return hdlc_rx(bg, data, count); |
340 | } |
341 | |
342 | static void gb_tty_wakeup(struct serdev_device *serdev) |
343 | { |
344 | struct gb_beagleplay *bg = serdev_device_get_drvdata(serdev); |
345 | |
346 | schedule_work(work: &bg->tx_work); |
347 | } |
348 | |
349 | static struct serdev_device_ops gb_beagleplay_ops = { |
350 | .receive_buf = gb_tty_receive, |
351 | .write_wakeup = gb_tty_wakeup, |
352 | }; |
353 | |
354 | /** |
355 | * gb_message_send() - Send greybus message using HDLC over UART |
356 | * |
357 | * @hd: pointer to greybus host device |
358 | * @cport: AP cport where message originates |
359 | * @msg: greybus message to send |
360 | * @mask: gfp mask |
361 | * |
362 | * Greybus HDLC frame has the following payload: |
363 | * 1. le16 cport |
364 | * 2. gb_operation_msg_hdr msg_header |
365 | * 3. u8 *msg_payload |
366 | */ |
367 | static int gb_message_send(struct gb_host_device *hd, u16 cport, struct gb_message *msg, gfp_t mask) |
368 | { |
369 | struct gb_beagleplay *bg = dev_get_drvdata(dev: &hd->dev); |
370 | struct hdlc_payload payloads[3]; |
371 | __le16 cport_id = cpu_to_le16(cport); |
372 | |
373 | dev_dbg(&hd->dev, "Sending greybus message with Operation %u, Type: %X on Cport %u" , |
374 | msg->header->operation_id, msg->header->type, cport); |
375 | |
376 | if (le16_to_cpu(msg->header->size) > RX_HDLC_PAYLOAD) |
377 | return dev_err_probe(dev: &hd->dev, err: -E2BIG, fmt: "Greybus message too big" ); |
378 | |
379 | payloads[0].buf = &cport_id; |
380 | payloads[0].len = sizeof(cport_id); |
381 | payloads[1].buf = msg->header; |
382 | payloads[1].len = sizeof(*msg->header); |
383 | payloads[2].buf = msg->payload; |
384 | payloads[2].len = msg->payload_size; |
385 | |
386 | hdlc_tx_frames(bg, ADDRESS_GREYBUS, control: 0x03, payloads, count: 3); |
387 | greybus_message_sent(hd: bg->gb_hd, message: msg, status: 0); |
388 | |
389 | return 0; |
390 | } |
391 | |
392 | static void gb_message_cancel(struct gb_message *message) |
393 | { |
394 | } |
395 | |
396 | static struct gb_hd_driver gb_hdlc_driver = { .message_send = gb_message_send, |
397 | .message_cancel = gb_message_cancel }; |
398 | |
399 | static void gb_beagleplay_start_svc(struct gb_beagleplay *bg) |
400 | { |
401 | const u8 command = CONTROL_SVC_START; |
402 | const struct hdlc_payload payload = { .len = 1, .buf = (void *)&command }; |
403 | |
404 | hdlc_tx_frames(bg, ADDRESS_CONTROL, control: 0x03, payloads: &payload, count: 1); |
405 | } |
406 | |
407 | static void gb_beagleplay_stop_svc(struct gb_beagleplay *bg) |
408 | { |
409 | const u8 command = CONTROL_SVC_STOP; |
410 | const struct hdlc_payload payload = { .len = 1, .buf = (void *)&command }; |
411 | |
412 | hdlc_tx_frames(bg, ADDRESS_CONTROL, control: 0x03, payloads: &payload, count: 1); |
413 | } |
414 | |
415 | static void gb_greybus_deinit(struct gb_beagleplay *bg) |
416 | { |
417 | gb_hd_del(hd: bg->gb_hd); |
418 | gb_hd_put(hd: bg->gb_hd); |
419 | } |
420 | |
421 | static int gb_greybus_init(struct gb_beagleplay *bg) |
422 | { |
423 | int ret; |
424 | |
425 | bg->gb_hd = gb_hd_create(driver: &gb_hdlc_driver, parent: &bg->sd->dev, TX_CIRC_BUF_SIZE, GB_MAX_CPORTS); |
426 | if (IS_ERR(ptr: bg->gb_hd)) { |
427 | dev_err(&bg->sd->dev, "Failed to create greybus host device" ); |
428 | return PTR_ERR(ptr: bg->gb_hd); |
429 | } |
430 | |
431 | ret = gb_hd_add(hd: bg->gb_hd); |
432 | if (ret) { |
433 | dev_err(&bg->sd->dev, "Failed to add greybus host device" ); |
434 | goto free_gb_hd; |
435 | } |
436 | dev_set_drvdata(dev: &bg->gb_hd->dev, data: bg); |
437 | |
438 | return 0; |
439 | |
440 | free_gb_hd: |
441 | gb_greybus_deinit(bg); |
442 | return ret; |
443 | } |
444 | |
445 | static void gb_serdev_deinit(struct gb_beagleplay *bg) |
446 | { |
447 | serdev_device_close(bg->sd); |
448 | } |
449 | |
450 | static int gb_serdev_init(struct gb_beagleplay *bg) |
451 | { |
452 | int ret; |
453 | |
454 | serdev_device_set_drvdata(serdev: bg->sd, data: bg); |
455 | serdev_device_set_client_ops(serdev: bg->sd, ops: &gb_beagleplay_ops); |
456 | ret = serdev_device_open(bg->sd); |
457 | if (ret) |
458 | return dev_err_probe(dev: &bg->sd->dev, err: ret, fmt: "Unable to open serial device" ); |
459 | |
460 | serdev_device_set_baudrate(bg->sd, 115200); |
461 | serdev_device_set_flow_control(bg->sd, false); |
462 | |
463 | return 0; |
464 | } |
465 | |
466 | static int gb_beagleplay_probe(struct serdev_device *serdev) |
467 | { |
468 | int ret = 0; |
469 | struct gb_beagleplay *bg; |
470 | |
471 | bg = devm_kmalloc(dev: &serdev->dev, size: sizeof(*bg), GFP_KERNEL); |
472 | if (!bg) |
473 | return -ENOMEM; |
474 | |
475 | bg->sd = serdev; |
476 | ret = gb_serdev_init(bg); |
477 | if (ret) |
478 | return ret; |
479 | |
480 | ret = hdlc_init(bg); |
481 | if (ret) |
482 | goto free_serdev; |
483 | |
484 | ret = gb_greybus_init(bg); |
485 | if (ret) |
486 | goto free_hdlc; |
487 | |
488 | gb_beagleplay_start_svc(bg); |
489 | |
490 | return 0; |
491 | |
492 | free_hdlc: |
493 | hdlc_deinit(bg); |
494 | free_serdev: |
495 | gb_serdev_deinit(bg); |
496 | return ret; |
497 | } |
498 | |
499 | static void gb_beagleplay_remove(struct serdev_device *serdev) |
500 | { |
501 | struct gb_beagleplay *bg = serdev_device_get_drvdata(serdev); |
502 | |
503 | gb_greybus_deinit(bg); |
504 | gb_beagleplay_stop_svc(bg); |
505 | hdlc_deinit(bg); |
506 | gb_serdev_deinit(bg); |
507 | } |
508 | |
509 | static const struct of_device_id gb_beagleplay_of_match[] = { |
510 | { |
511 | .compatible = "ti,cc1352p7" , |
512 | }, |
513 | {}, |
514 | }; |
515 | MODULE_DEVICE_TABLE(of, gb_beagleplay_of_match); |
516 | |
517 | static struct serdev_device_driver gb_beagleplay_driver = { |
518 | .probe = gb_beagleplay_probe, |
519 | .remove = gb_beagleplay_remove, |
520 | .driver = { |
521 | .name = "gb_beagleplay" , |
522 | .of_match_table = gb_beagleplay_of_match, |
523 | }, |
524 | }; |
525 | |
526 | module_serdev_device_driver(gb_beagleplay_driver); |
527 | |
528 | MODULE_LICENSE("GPL" ); |
529 | MODULE_AUTHOR("Ayush Singh <ayushdevel1325@gmail.com>" ); |
530 | MODULE_DESCRIPTION("A Greybus driver for BeaglePlay" ); |
531 | |