[PATCH 04/18 v2] Bluetooth: Add basic packet parsing to Three-wire UART driver

[Johan Hedberg <johan.hedberg@gmail.com>]

From: Johan Hedberg <johan.hedberg@intel.com>

This patch adds basic packet parsing to the Three-wire UART HCI driver
for packets received from the controller.

Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>
---
 drivers/bluetooth/hci_h5.c |  230 ++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 222 insertions(+), 8 deletions(-)

diff --git a/drivers/bluetooth/hci_h5.c b/drivers/bluetooth/hci_h5.c
index fe711bf..a76c927 100644
--- a/drivers/bluetooth/hci_h5.c
+++ b/drivers/bluetooth/hci_h5.c
@@ -34,20 +34,37 @@
 
 #define H5_ACK_TIMEOUT	msecs_to_jiffies(250)
 
+/*
+ * Maximum Three-wire packet:
+ *     4 byte header + max value for 12-bit length + 2 bytes for CRC
+ */
+#define H5_MAX_LEN (4 + 0xfff + 2)
+
+#define SLIP_DELIMITER	0xc0
+#define SLIP_ESC	0xdb
+#define SLIP_ESC_DELIM	0xdc
+#define SLIP_ESC_ESC	0xdd
+
 struct h5 {
-	struct sk_buff_head unack;	/* Unack'ed packets queue */
-	struct sk_buff_head rel;	/* Reliable packets queue */
-	struct sk_buff_head unrel;	/* Unreliable packets queue */
+	struct sk_buff_head	unack;		/* Unack'ed packets queue */
+	struct sk_buff_head	rel;		/* Reliable packets queue */
+	struct sk_buff_head	unrel;		/* Unreliable packets queue */
 
-	struct sk_buff *rx_skb;
+	struct sk_buff		*rx_skb;	/* Receive buffer */
+	size_t			rx_pending;	/* Expecting more bytes */
+	bool			rx_esc;		/* SLIP escape mode */
 
-	struct timer_list timer;	/* Retransmission timer */
+	int			(*rx_func) (struct hci_uart *hu, u8 c);
 
-	bool txack_req;
+	struct timer_list	timer;		/* Retransmission timer */
 
-	u8 msgq_txseq;
+	bool			txack_req;
+
+	u8			msgq_txseq;
 };
 
+static void h5_reset_rx(struct h5 *h5);
+
 static void h5_timed_event(unsigned long arg)
 {
 	struct hci_uart *hu = (struct hci_uart *) arg;
@@ -85,6 +102,8 @@ static int h5_open(struct hci_uart *hu)
 	skb_queue_head_init(&h5->rel);
 	skb_queue_head_init(&h5->unrel);
 
+	h5_reset_rx(h5);
+
 	init_timer(&h5->timer);
 	h5->timer.function = h5_timed_event;
 	h5->timer.data = (unsigned long) hu;
@@ -107,9 +126,204 @@ static int h5_close(struct hci_uart *hu)
 	return 0;
 }
 
+static void h5_handle_internal_rx(struct hci_uart *hu)
+{
+	BT_DBG("%s", hu->hdev->name);
+}
+
+static void h5_complete_rx_pkt(struct hci_uart *hu)
+{
+	struct h5 *h5 = hu->priv;
+	u8 pkt_type;
+
+	BT_DBG("%s", hu->hdev->name);
+
+	pkt_type = h5->rx_skb->data[1] & 0x0f;
+
+	switch (pkt_type) {
+	case HCI_EVENT_PKT:
+	case HCI_ACLDATA_PKT:
+	case HCI_SCODATA_PKT:
+		bt_cb(h5->rx_skb)->pkt_type = pkt_type;
+
+		/* Remove Three-wire header */
+		skb_pull(h5->rx_skb, 4);
+
+		hci_recv_frame(h5->rx_skb);
+		h5->rx_skb = NULL;
+
+		break;
+
+	default:
+		h5_handle_internal_rx(hu);
+		break;
+	}
+
+	h5_reset_rx(h5);
+}
+
+static int h5_rx_crc(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+
+	BT_DBG("%s 0x%02hhx", hu->hdev->name, c);
+
+	h5_complete_rx_pkt(hu);
+	h5_reset_rx(h5);
+
+	return 0;
+}
+
+static int h5_rx_payload(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+	const unsigned char *hdr = h5->rx_skb->data;
+
+	BT_DBG("%s 0x%02hhx", hu->hdev->name, c);
+
+	if ((hdr[0] >> 4) & 0x01) {
+		h5->rx_func = h5_rx_crc;
+		h5->rx_pending = 2;
+	} else {
+		h5_complete_rx_pkt(hu);
+		h5_reset_rx(h5);
+	}
+
+	return 0;
+}
+
+static int h5_rx_3wire_hdr(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+	const unsigned char *hdr = h5->rx_skb->data;
+
+	BT_DBG("%s 0x%02hhx", hu->hdev->name, c);
+
+	if (((hdr[0] + hdr[1] + hdr[2] + hdr[3]) & 0xff) != 0xff) {
+		BT_ERR("Invalid header checksum");
+		h5_reset_rx(h5);
+		return 0;
+	}
+
+	h5->rx_func = h5_rx_payload;
+	h5->rx_pending = ((hdr[1] >> 4) & 0xff) + (hdr[2] << 4);
+
+	return 0;
+}
+
+static int h5_rx_pkt_start(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+
+	BT_DBG("%s 0x%02hhx", hu->hdev->name, c);
+
+	if (c == SLIP_DELIMITER)
+		return 1;
+
+	h5->rx_func = h5_rx_3wire_hdr;
+	h5->rx_pending = 4;
+
+	h5->rx_skb = bt_skb_alloc(H5_MAX_LEN, GFP_ATOMIC);
+	if (!h5->rx_skb) {
+		BT_ERR("Can't allocate mem for new packet");
+		h5_reset_rx(h5);
+		return -ENOMEM;
+	}
+
+	h5->rx_skb->dev = (void *) hu->hdev;
+
+	return 0;
+}
+
+static int h5_rx_delimiter(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+
+	BT_DBG("%s 0x%02hhx", hu->hdev->name, c);
+
+	if (c == SLIP_DELIMITER)
+		h5->rx_func = h5_rx_pkt_start;
+
+	return 1;
+}
+
+static void h5_unslip_one_byte(struct h5 *h5, unsigned char c)
+{
+	const u8 delim = SLIP_DELIMITER, esc = SLIP_ESC;
+	const u8 *byte = &c;
+
+	if (!h5->rx_esc && c == SLIP_ESC) {
+		h5->rx_esc = true;
+		return;
+	}
+
+	if (h5->rx_esc) {
+		switch (c) {
+		case SLIP_ESC_DELIM:
+			byte = &delim;
+			break;
+		case SLIP_ESC_ESC:
+			byte = &esc;
+			break;
+		default:
+			BT_ERR("Invalid esc byte 0x%02hhx", c);
+			h5_reset_rx(h5);
+			return;
+		}
+
+		h5->rx_esc = false;
+	}
+
+	memcpy(skb_put(h5->rx_skb, 1), byte, 1);
+	h5->rx_pending--;
+
+	BT_DBG("unsliped 0x%02hhx", *byte);
+}
+
+static void h5_reset_rx(struct h5 *h5)
+{
+	if (h5->rx_skb) {
+		kfree_skb(h5->rx_skb);
+		h5->rx_skb = NULL;
+	}
+
+	h5->rx_func = h5_rx_delimiter;
+	h5->rx_pending = 0;
+	h5->rx_esc = false;
+}
+
 static int h5_recv(struct hci_uart *hu, void *data, int count)
 {
-	return -ENOSYS;
+	struct h5 *h5 = hu->priv;
+	unsigned char *ptr = data;
+
+	BT_DBG("%s count %d", hu->hdev->name, count);
+
+	while (count > 0) {
+		int processed;
+
+		if (h5->rx_pending > 0) {
+			if (*ptr == SLIP_DELIMITER) {
+				BT_ERR("Too short H5 packet");
+				h5_reset_rx(h5);
+				continue;
+			}
+
+			h5_unslip_one_byte(h5, *ptr);
+
+			ptr++; count--;
+			continue;
+		}
+
+		processed = h5->rx_func(hu, *ptr);
+		if (processed < 0)
+			return processed;
+
+		ptr += processed;
+		count -= processed;
+	}
+
+	return 0;
 }
 
 static int h5_enqueue(struct hci_uart *hu, struct sk_buff *skb)
-- 
1.7.10.4