diff -drupN a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c --- a/drivers/bluetooth/hci_h4.c 2018-08-06 17:23:04.000000000 +0300 +++ b/drivers/bluetooth/hci_h4.c 2022-06-12 05:28:14.000000000 +0300 @@ -44,14 +44,30 @@ #include #include +#include #include "hci_uart.h" +#ifdef BTCOEX +#include "rtk_coex.h" +#endif + +//#define VERSION "1.2" + struct h4_struct { + unsigned long rx_state; + unsigned long rx_count; struct sk_buff *rx_skb; struct sk_buff_head txq; }; +/* H4 receiver States */ +#define H4_W4_PACKET_TYPE 0 +#define H4_W4_EVENT_HDR 1 +#define H4_W4_ACL_HDR 2 +#define H4_W4_SCO_HDR 3 +#define H4_W4_DATA 4 + /* Initialize protocol */ static int h4_open(struct hci_uart *hu) { @@ -120,21 +136,168 @@ static const struct h4_recv_pkt h4_recv_ { H4_RECV_EVENT, .recv = hci_recv_frame }, }; +#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0) +static inline int h4_check_data_len(struct h4_struct *h4, int len) +#else +static inline int h4_check_data_len(struct hci_dev *hdev, struct h4_struct *h4, int len) +#endif +{ + register int room = skb_tailroom(h4->rx_skb); + + BT_DBG("len %d room %d", len, room); + + if (!len) { +#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0) + hci_recv_frame(h4->rx_skb); +#else + hci_recv_frame(hdev, h4->rx_skb); +#endif + } else if (len > room) { + BT_ERR("Data length is too large"); + kfree_skb(h4->rx_skb); + } else { + h4->rx_state = H4_W4_DATA; + h4->rx_count = len; + return len; + } + + h4->rx_state = H4_W4_PACKET_TYPE; + h4->rx_skb = NULL; + h4->rx_count = 0; + + return 0; +} + /* Recv data */ static int h4_recv(struct hci_uart *hu, const void *data, int count) { struct h4_struct *h4 = hu->priv; + register char *ptr; + struct hci_event_hdr *eh; + struct hci_acl_hdr *ah; + struct hci_sco_hdr *sh; + register int len, type, dlen; - if (!test_bit(HCI_UART_REGISTERED, &hu->flags)) - return -EUNATCH; + BT_DBG("hu %p count %d rx_state %ld rx_count %ld", + hu, count, h4->rx_state, h4->rx_count); - h4->rx_skb = h4_recv_buf(hu->hdev, h4->rx_skb, data, count, - h4_recv_pkts, ARRAY_SIZE(h4_recv_pkts)); - if (IS_ERR(h4->rx_skb)) { - int err = PTR_ERR(h4->rx_skb); - BT_ERR("%s: Frame reassembly failed (%d)", hu->hdev->name, err); - h4->rx_skb = NULL; - return err; + ptr = data; + while (count) { + if (h4->rx_count) { + len = min_t(unsigned int, h4->rx_count, count); + memcpy(skb_put(h4->rx_skb, len), ptr, len); + h4->rx_count -= len; count -= len; ptr += len; + + if (h4->rx_count) + continue; + + switch (h4->rx_state) { + case H4_W4_DATA: + BT_DBG("Complete data"); +#ifdef BTCOEX + if (bt_cb(h4->rx_skb)->pkt_type == HCI_EVENT_PKT) + rtk_btcoex_parse_event( + h4->rx_skb->data, + h4->rx_skb->len); + + if (bt_cb(h4->rx_skb)->pkt_type == HCI_ACLDATA_PKT) + rtk_btcoex_parse_l2cap_data_rx( + h4->rx_skb->data, + h4->rx_skb->len); +#endif + +#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0) + hci_recv_frame(h4->rx_skb); +#else + hci_recv_frame(hu->hdev, h4->rx_skb); +#endif + + h4->rx_state = H4_W4_PACKET_TYPE; + h4->rx_skb = NULL; + continue; + + case H4_W4_EVENT_HDR: + eh = hci_event_hdr(h4->rx_skb); + + BT_DBG("Event header: evt 0x%2.2x plen %d", eh->evt, eh->plen); + +#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0) + h4_check_data_len(h4, eh->plen); +#else + h4_check_data_len(hu->hdev, h4, eh->plen); +#endif + continue; + + case H4_W4_ACL_HDR: + ah = hci_acl_hdr(h4->rx_skb); + dlen = __le16_to_cpu(ah->dlen); + + BT_DBG("ACL header: dlen %d", dlen); + +#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0) + h4_check_data_len(h4, dlen); +#else + h4_check_data_len(hu->hdev, h4, dlen); +#endif + continue; + + case H4_W4_SCO_HDR: + sh = hci_sco_hdr(h4->rx_skb); + + BT_DBG("SCO header: dlen %d", sh->dlen); + +#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0) + h4_check_data_len(h4, sh->dlen); +#else + h4_check_data_len(hu->hdev, h4, sh->dlen); +#endif + continue; + } + } + + /* H4_W4_PACKET_TYPE */ + switch (*ptr) { + case HCI_EVENT_PKT: + BT_DBG("Event packet"); + h4->rx_state = H4_W4_EVENT_HDR; + h4->rx_count = HCI_EVENT_HDR_SIZE; + type = HCI_EVENT_PKT; + break; + + case HCI_ACLDATA_PKT: + BT_DBG("ACL packet"); + h4->rx_state = H4_W4_ACL_HDR; + h4->rx_count = HCI_ACL_HDR_SIZE; + type = HCI_ACLDATA_PKT; + break; + + case HCI_SCODATA_PKT: + BT_DBG("SCO packet"); + h4->rx_state = H4_W4_SCO_HDR; + h4->rx_count = HCI_SCO_HDR_SIZE; + type = HCI_SCODATA_PKT; + break; + + default: + BT_ERR("Unknown HCI packet type %2.2x", (__u8)*ptr); + hu->hdev->stat.err_rx++; + ptr++; count--; + continue; + }; + + ptr++; count--; + + /* Allocate packet */ + h4->rx_skb = bt_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC); + if (!h4->rx_skb) { + BT_ERR("Can't allocate mem for new packet"); + h4->rx_state = H4_W4_PACKET_TYPE; + h4->rx_count = 0; + return -ENOMEM; + } + + h4->rx_skb->dev = (void *) hu->hdev; + bt_cb(h4->rx_skb)->pkt_type = type; } return count; @@ -159,7 +322,14 @@ static const struct hci_uart_proto h4p = int __init h4_init(void) { - return hci_uart_register_proto(&h4p); + int err = hci_uart_register_proto(&h4p); + + if (!err) + BT_INFO("HCI H4 protocol initialized"); + else + BT_ERR("HCI H4 protocol registration failed"); + + return err; } int __exit h4_deinit(void)