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: Johan Hedberg <johan.hedberg@intel.com> Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>hifive-unleashed-5.1
Johan Hedberg
Gustavo Padovan
parent
3f27e95b83
commit
bc1f35b911
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue