This file provides functions that allow sending and recieving async HDLC frames over any transport. Currently only tested with UART.
I am not quite sure where these files should go so I have put them close to Beagleplay greybus driver for now.
Signed-off-by: Ayush Singh ayushdevel1325@gmail.com --- drivers/staging/greybus/hdlc.c | 229 +++++++++++++++++++++++++++++++++ drivers/staging/greybus/hdlc.h | 137 ++++++++++++++++++++ 2 files changed, 366 insertions(+) create mode 100644 drivers/staging/greybus/hdlc.c create mode 100644 drivers/staging/greybus/hdlc.h
diff --git a/drivers/staging/greybus/hdlc.c b/drivers/staging/greybus/hdlc.c new file mode 100644 index 000000000000..079d4c10e476 --- /dev/null +++ b/drivers/staging/greybus/hdlc.c @@ -0,0 +1,229 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (c) 2023 Ayush Singh ayushdevel1325@gmail.com + */ + +#include "hdlc.h" +#include <linux/device.h> +#include <linux/crc-ccitt.h> +#include <linux/serdev.h> + +static void hdlc_write_locked(struct hdlc_driver *drv) +{ + // must be locked already + int head = smp_load_acquire(&drv->tx_circ_buf.head); + int tail = drv->tx_circ_buf.tail; + int count = CIRC_CNT_TO_END(head, tail, TX_CIRC_BUF_SIZE); + int written; + + if (count >= 1) { + written = drv->hdlc_send_frame_cb(dev_get_drvdata(drv->parent), + &drv->tx_circ_buf.buf[tail], + count); + + /* Finish consuming HDLC data */ + smp_store_release(&drv->tx_circ_buf.tail, + (tail + written) & (TX_CIRC_BUF_SIZE - 1)); + } +} + +static void hdlc_append(struct hdlc_driver *drv, u8 value) +{ + // must be locked already + int head, tail; + + head = drv->tx_circ_buf.head; + + while (true) { + tail = READ_ONCE(drv->tx_circ_buf.tail); + + if (CIRC_SPACE(head, tail, TX_CIRC_BUF_SIZE) >= 1) { + drv->tx_circ_buf.buf[head] = value; + + /* Finish producing HDLC byte */ + smp_store_release(&drv->tx_circ_buf.head, + (head + 1) & (TX_CIRC_BUF_SIZE - 1)); + return; + } + dev_warn(drv->parent, "Tx circ buf full\n"); + usleep_range(3000, 5000); + } +} + +static void hdlc_append_escaped(struct hdlc_driver *drv, u8 value) +{ + if (value == HDLC_FRAME || value == HDLC_ESC) { + hdlc_append(drv, HDLC_ESC); + value ^= HDLC_XOR; + } + hdlc_append(drv, value); +} + +static void hdlc_append_tx_frame(struct hdlc_driver *drv) +{ + drv->tx_crc = 0xFFFF; + hdlc_append(drv, HDLC_FRAME); +} + +static void hdlc_append_tx_u8(struct hdlc_driver *drv, u8 value) +{ + drv->tx_crc = crc_ccitt(drv->tx_crc, &value, 1); + hdlc_append_escaped(drv, value); +} + +static void hdlc_append_tx_buffer(struct hdlc_driver *drv, const void *buffer, + size_t len) +{ + size_t i; + + for (i = 0; i < len; i++) + hdlc_append_tx_u8(drv, ((u8 *)buffer)[i]); +} + +static void hdlc_append_tx_crc(struct hdlc_driver *drv) +{ + drv->tx_crc ^= 0xffff; + hdlc_append_escaped(drv, drv->tx_crc & 0xff); + hdlc_append_escaped(drv, (drv->tx_crc >> 8) & 0xff); +} + +static void hdlc_handle_rx_frame(struct hdlc_driver *drv) +{ + u8 address = drv->rx_buffer[0]; + char *buf = &drv->rx_buffer[2]; + size_t buf_len = drv->rx_buffer_len - 4; + + drv->hdlc_process_frame_cb(dev_get_drvdata(drv->parent), buf, buf_len, + address); +} + +static void hdlc_transmit(struct work_struct *work) +{ + struct hdlc_driver *drv = + container_of(work, struct hdlc_driver, tx_work); + + spin_lock_bh(&drv->tx_consumer_lock); + hdlc_write_locked(drv); + spin_unlock_bh(&drv->tx_consumer_lock); +} + +static void hdlc_send_s_frame_ack(struct hdlc_driver *drv) +{ + u8 address = drv->rx_buffer[0]; + + hdlc_send_async(drv, 0, NULL, address, (drv->rx_buffer[1] >> 1) & 0x7); +} + +int hdlc_rx(struct hdlc_driver *drv, const unsigned char *data, size_t count) +{ + u16 crc_check; + size_t i; + u8 c, ctrl; + + for (i = 0; i < count; ++i) { + c = data[i]; + + switch (c) { + case HDLC_FRAME: { + if (drv->rx_buffer_len) { + crc_check = crc_ccitt(0xffff, drv->rx_buffer, + drv->rx_buffer_len); + + if (crc_check == 0xf0b8) { + ctrl = drv->rx_buffer[1]; + if ((ctrl & 1) == 0) { + // I-Frame, send S-Frame ACK + hdlc_send_s_frame_ack(drv); + } + + hdlc_handle_rx_frame(drv); + } else { + dev_err(drv->parent, + "CRC Failed from %02x: 0x%04x\n", + drv->rx_buffer[0], crc_check); + } + } + drv->rx_buffer_len = 0; + break; + } + case HDLC_ESC: + drv->rx_in_esc = 1; + break; + default: + if (drv->rx_in_esc) { + c ^= 0x20; + drv->rx_in_esc = 0; + } + + if (drv->rx_buffer_len < MAX_RX_HDLC) { + drv->rx_buffer[drv->rx_buffer_len] = c; + drv->rx_buffer_len++; + } else { + // buffer overflow + dev_err(drv->parent, "RX Buffer Overflow\n"); + drv->rx_buffer_len = 0; + } + } + } + + return count; +} + +struct hdlc_driver *hdlc_init(struct device *parent, + hdlc_send_frame_callback hdlc_send_frame_cb, + hdlc_process_frame_callback hdlc_process_frame_cb) +{ + struct hdlc_driver *drv; + + drv = devm_kmalloc(parent, sizeof(*drv), GFP_KERNEL); + if (!drv) + goto early_exit; + + drv->parent = parent; + INIT_WORK(&drv->tx_work, hdlc_transmit); + drv->hdlc_send_frame_cb = hdlc_send_frame_cb; + spin_lock_init(&drv->tx_producer_lock); + spin_lock_init(&drv->tx_consumer_lock); + drv->tx_circ_buf.head = 0; + drv->tx_circ_buf.tail = 0; + drv->tx_circ_buf.buf = + devm_kmalloc(parent, TX_CIRC_BUF_SIZE, GFP_KERNEL); + drv->rx_buffer_len = 0; + drv->rx_in_esc = 0; + drv->hdlc_process_frame_cb = hdlc_process_frame_cb; + + return drv; + +early_exit: + return NULL; +} + +void hdlc_deinit(struct hdlc_driver *drv) +{ + flush_work(&drv->tx_work); +} + +void hdlc_send_async(struct hdlc_driver *drv, u16 length, const void *buffer, + u8 address, u8 control) +{ + // HDLC_FRAME + // 0 address : 0x01 + // 1 control : 0x03 + // contents + // x/y crc + // HDLC_FRAME + + spin_lock(&drv->tx_producer_lock); + + hdlc_append_tx_frame(drv); + hdlc_append_tx_u8(drv, + address); // address + hdlc_append_tx_u8(drv, control); // control + hdlc_append_tx_buffer(drv, buffer, length); + hdlc_append_tx_crc(drv); + hdlc_append_tx_frame(drv); + + spin_unlock(&drv->tx_producer_lock); + + schedule_work(&drv->tx_work); +} diff --git a/drivers/staging/greybus/hdlc.h b/drivers/staging/greybus/hdlc.h new file mode 100644 index 000000000000..7eae10871f88 --- /dev/null +++ b/drivers/staging/greybus/hdlc.h @@ -0,0 +1,137 @@ +/* SPDX-License-Identifier: GPL-2.0 + * + * Copyright (c) 2023 Ayush Singh ayushdevel1325@gmail.com + */ + +#ifndef _HDLC_H +#define _HDLC_H + +#include <linux/circ_buf.h> +#include <linux/types.h> +#include <linux/workqueue.h> + +#define MAX_RX_HDLC (1 + RX_HDLC_PAYLOAD + CRC_LEN) +#define RX_HDLC_PAYLOAD 1024 +#define CRC_LEN 2 +#define TX_CIRC_BUF_SIZE 1024 + +#define HDLC_FRAME 0x7E +#define HDLC_ESC 0x7D +#define HDLC_XOR 0x20 + +#define HDLC_MAX_BLOCK_SIZE 512 + +/* + * Callback to process a complete HDLC frame + * + * @param drvdata of hdlc device registered + * @param buffer of hdlc block + * @param length of buffer + * @param HDLC address + */ +typedef void (*hdlc_process_frame_callback)(void *, u8 *, size_t, uint8_t); + +/* + * Callback to send HDLC frame + * + * @param drvdata of hdlc device registered + * @param buffer of hdlc block + * @param length of buffer + */ +typedef int (*hdlc_send_frame_callback)(void *, const unsigned char *, size_t); + +/* + * HDLC driver + * + * @param device HDLC driver is using + * + * @param callback called when hdlc block is received + * @param callback called to send hdlc block + * + * @param transmit work + * @param transmit producer lock + * @param transmit consumer lock + * @param transmit circular buffer + * @param HDCL CRC + * @param current TX ACK sequence number + * + * @param Rx buffer length + * @param Rx HDLC block address + * @param Rx Flag to indicate if ESC + * @parma Rx buffer + */ +struct hdlc_driver { + struct device *parent; + + hdlc_process_frame_callback hdlc_process_frame_cb; + hdlc_send_frame_callback hdlc_send_frame_cb; + + struct work_struct tx_work; + /* tx_producer_lock: HDLC producer lock */ + spinlock_t tx_producer_lock; + /* tx_consumer_lock: HDLC consumer lock */ + spinlock_t tx_consumer_lock; + struct circ_buf tx_circ_buf; + u16 tx_crc; + u8 tx_ack_seq; + + u16 rx_buffer_len; + u8 rx_in_esc; + u8 rx_buffer[HDLC_MAX_BLOCK_SIZE]; +}; + +/* + * Queue data to be sent as an HDLC block + * + * @param hdlc driver + * @param buffer length + * @param buffer + * @param address + * @param control + */ +void hdlc_send_async(struct hdlc_driver *drv, u16 buffer_length, + const void *buffer, u8 address, u8 control); + +/* + * Add HDCL data for processing + * + * @param hdlc driver + * @param buffer + * @param buffer length + * + * @return number of bytes processed + */ +int hdlc_rx(struct hdlc_driver *drv, const unsigned char *buffer, + size_t buffer_length); + +/* + * Initialize HDLC + * + * @param device to use + * @param callback to send HDLC block + * @param callback to process hdlc frame + * + * @return hdlc driver allocated on heap + */ +struct hdlc_driver *hdlc_init(struct device *drv, + hdlc_send_frame_callback send_frame_cb, + hdlc_process_frame_callback process_frame_cb); + +/* + * De-initialize HDLC + * + * @param hdlc driver + */ +void hdlc_deinit(struct hdlc_driver *drv); + +/* + * Wakeup Tx + * + * @param hdlc_driver + */ +static inline void hdlc_tx_wakeup(struct hdlc_driver *drv) +{ + schedule_work(&drv->tx_work); +} + +#endif