Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit bc1f35b9 authored by Johan Hedberg's avatar Johan Hedberg Committed by Gustavo Padovan
Browse files

Bluetooth: Add basic packet parsing to Three-wire UART driver 



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

Signed-off-by: default avatarJohan Hedberg <johan.hedberg@intel.com>
Signed-off-by: default avatarGustavo Padovan <gustavo.padovan@collabora.co.uk>
parent 3f27e95b

drivers/bluetooth/hci_h5.c

+222 −8
Original line number Diff line number Diff line
@@ -34,12 +34,27 @@ 


#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 *rx_skb;

	struct sk_buff		*rx_skb;	/* Receive buffer */

	size_t			rx_pending;	/* Expecting more bytes */

	bool			rx_esc;		/* SLIP escape mode */



	int			(*rx_func) (struct hci_uart *hu, u8 c);



	struct timer_list	timer;		/* Retransmission timer */
@@ -48,6 +63,8 @@ struct h5 { 
	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)