Add the link establishment protocol handling in the kernel:
- le_state enum to handle state machine
- bcsp_timed_event to send LE messages
- bcsp_handle_le_pkt to answer endpoint LE messages
- various tests to muzzle BCSP link before GARULOUS
The patch exports some HCI functions that are required whitin BCSP to properly handle the connection reset.
- hci_dev_reset_stat
- hci_notify
Signed-off-by: Tristan Lelong <tristan@xxxxxxxxxx>
---
drivers/bluetooth/hci_bcsp.c | 202 +++++++++++++++++++++++++++++++--------
include/net/bluetooth/hci_core.h | 2 +
net/bluetooth/hci_core.c | 6 +-
3 files changed, 171 insertions(+), 39 deletions(-)
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index 21cc45b..142b42b 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -1,6 +1,5 @@
/*
*
- * Bluetooth HCI UART driver
*
* Copyright (C) 2002-2003 Fabrizio Gennari <fabrizio.gennari@xxxxxxxxxxx>
* Copyright (C) 2004-2005 Marcel Holtmann <marcel@xxxxxxxxxxxx>
@@ -47,13 +46,25 @@
#include "hci_uart.h"
-#define VERSION "0.3"
+#define VERSION "0.4"
+
+#define LE_POLLING (HZ/10)
+#define TX_POLLING (HZ/4)
static bool txcrc = 1;
static bool hciextn = 1;
+static u8 conf_pkt[4] = { 0xad, 0xef, 0xac, 0xed };
+static u8 conf_rsp_pkt[4] = { 0xde, 0xad, 0xd0, 0xd0 };
+static u8 sync_pkt[4] = { 0xda, 0xdc, 0xed, 0xed };
+static u8 sync_rsp_pkt[4] = { 0xac, 0xaf, 0xef, 0xee };
+
+
#define BCSP_TXWINSIZE 4
+#define BCSP_PKT_LEN(data) ((data[1] >> 4) + (data[2]))
+#define BCSP_LE_PKT_LEN 0x04
+
#define BCSP_ACK_PKT 0x05
#define BCSP_LE_PKT 0x06
@@ -69,6 +80,12 @@ struct bcsp_struct {
struct timer_list tbcsp;
enum {
+ BCSP_SHY = 0,
+ BCSP_CURIOUS,
+ BCSP_GARULOUS
+ } le_state;
+
+ enum {
BCSP_W4_PKT_DELIMITER,
BCSP_W4_PKT_START,
BCSP_W4_BCSP_HDR,
@@ -184,6 +201,9 @@ static struct sk_buff *bcsp_prepare_pkt(struct bcsp_struct *bcsp, u8 *data,
u16 BCSP_CRC_INIT(bcsp_txmsg_crc);
int rel, i;
+ if (bcsp->le_state != BCSP_GARULOUS && pkt_type != BCSP_LE_PKT)
+ return NULL;
+
switch (pkt_type) {
case HCI_ACLDATA_PKT:
chan = 6; /* BCSP ACL channel */
@@ -299,6 +319,8 @@ static struct sk_buff *bcsp_dequeue(struct hci_uart *hu)
return nskb;
} else {
skb_queue_head(&bcsp->unrel, skb);
+ if (bcsp->le_state == BCSP_GARULOUS)
+ return NULL;
BT_ERR("Could not dequeue pkt because alloc_skb failed");
}
}
@@ -316,11 +338,18 @@ static struct sk_buff *bcsp_dequeue(struct hci_uart *hu)
bt_cb(skb)->pkt_type);
if (nskb) {
__skb_queue_tail(&bcsp->unack, skb);
- mod_timer(&bcsp->tbcsp, jiffies + HZ / 4);
+
+ if (bcsp->le_state == BCSP_GARULOUS)
+ /* only re-arm retransmit timer
+ * if the BCSP link is connected */
+ mod_timer(&bcsp->tbcsp,
+ jiffies + HZ / 4);
spin_unlock_irqrestore(&bcsp->unack.lock, flags);
return nskb;
} else {
skb_queue_head(&bcsp->rel, skb);
+ if (bcsp->le_state != BCSP_GARULOUS)
+ return NULL;
BT_ERR("Could not dequeue pkt because alloc_skb failed");
}
}
@@ -386,7 +415,7 @@ static void bcsp_pkt_cull(struct bcsp_struct *bcsp)
kfree_skb(skb);
}
- if (skb_queue_empty(&bcsp->unack))
+ if (skb_queue_empty(&bcsp->unack) && bcsp->le_state == BCSP_GARULOUS)
del_timer(&bcsp->tbcsp);
spin_unlock_irqrestore(&bcsp->unack.lock, flags);
@@ -395,34 +424,100 @@ static void bcsp_pkt_cull(struct bcsp_struct *bcsp)
BT_ERR("Removed only %u out of %u pkts", i, pkts_to_be_removed);
}
-/* Handle BCSP link-establishment packets. When we
- detect a "sync" packet, symptom that the BT module has reset,
- we do nothing :) (yet) */
+static void bcsp_internal_reset(struct hci_uart *hu)
+{
+ struct bcsp_struct *bcsp = hu->priv;
+
+ /* reset the bcsp stack counters */
+ bcsp->rxseq_txack = 0;
+ bcsp->rxack = 0;
+ bcsp->message_crc = 0;
+ bcsp->txack_req = 0;
+ bcsp->msgq_txseq = 0;
+
+ /* reset state machine */
+ bcsp->le_state = BCSP_SHY;
+ bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
+ bcsp->rx_esc_state = BCSP_ESCSTATE_NOESC;
+
+ /* empty all the queues */
+ skb_queue_purge(&bcsp->unack);
+ skb_queue_purge(&bcsp->rel);
+ skb_queue_purge(&bcsp->unrel);
+
+ /* tell upper layer about the reset */
+ hci_dev_reset_stat(hu->hdev->id);
+
+ hci_notify(hu->hdev, HCI_DEV_UNREG);
+
+ bcsp->tbcsp.expires = jiffies + LE_POLLING;
+ add_timer(&bcsp->tbcsp);
+}
+
+/* Handle BCSP link-establishment packets.
+ */
static void bcsp_handle_le_pkt(struct hci_uart *hu)
{
+ struct sk_buff *nskb;
struct bcsp_struct *bcsp = hu->priv;
- u8 conf_pkt[4] = { 0xad, 0xef, 0xac, 0xed };
- u8 conf_rsp_pkt[4] = { 0xde, 0xad, 0xd0, 0xd0 };
- u8 sync_pkt[4] = { 0xda, 0xdc, 0xed, 0xed };
-
- /* spot "conf" pkts and reply with a "conf rsp" pkt */
- if (bcsp->rx_skb->data[1] >> 4 == 4 && bcsp->rx_skb->data[2] == 0 &&
- !memcmp(&bcsp->rx_skb->data[4], conf_pkt, 4)) {
- struct sk_buff *nskb = alloc_skb(4, GFP_ATOMIC);
-
- BT_DBG("Found a LE conf pkt");
- if (!nskb)
- return;
- memcpy(skb_put(nskb, 4), conf_rsp_pkt, 4);
- bt_cb(nskb)->pkt_type = BCSP_LE_PKT;
-
- skb_queue_head(&bcsp->unrel, nskb);
- hci_uart_tx_wakeup(hu);
- }
- /* Spot "sync" pkts. If we find one...disaster! */
- else if (bcsp->rx_skb->data[1] >> 4 == 4 && bcsp->rx_skb->data[2] == 0 &&
- !memcmp(&bcsp->rx_skb->data[4], sync_pkt, 4)) {
- BT_ERR("Found a LE sync pkt, card has reset");
+
+ if (BCSP_PKT_LEN(bcsp->rx_skb->data) == BCSP_LE_PKT_LEN) {
+ /* spot "conf" pkts and reply with a "conf rsp" pkt */
+ if (!memcmp(&bcsp->rx_skb->data[4], conf_pkt, 4)) {
+ if (bcsp->le_state == BCSP_SHY)
+ return;
+
+ BT_DBG("Found a LE conf pkt");
+ nskb = alloc_skb(4, GFP_ATOMIC);
+ if (!nskb)
+ return;
+ memcpy(skb_put(nskb, 4), conf_rsp_pkt, 4);
+ bt_cb(nskb)->pkt_type = BCSP_LE_PKT;
+
+ skb_queue_head(&bcsp->unrel, nskb);
+ hci_uart_tx_wakeup(hu);
+ }
+ /* Spot "sync" pkts and reply with a "sync rsp" pkt */
+ else if (!memcmp(&bcsp->rx_skb->data[4], sync_pkt, 4)) {
+ if (bcsp->le_state == BCSP_GARULOUS)
+ /* we force the connection state to reset */
+ bcsp_internal_reset(hu);
+
+ BT_INFO("BCSP: hci%d LE sync pkt, performing reset",
+ hu->hdev->id);
+
+ nskb = alloc_skb(4, GFP_ATOMIC);
+ if (!nskb)
+ return;
+ memcpy(skb_put(nskb, 4), sync_rsp_pkt, 4);
+ bt_cb(nskb)->pkt_type = BCSP_LE_PKT;
+
+ skb_queue_head(&bcsp->unrel, nskb);
+ hci_uart_tx_wakeup(hu);
+ }
+ /* Spot "conf rsp" pkts and update state machine */
+ else if (!memcmp(&bcsp->rx_skb->data[4], conf_rsp_pkt, 4)) {
+ if (bcsp->le_state != BCSP_CURIOUS) {
+ BT_DBG("BCSP: hci%d ingoring conf_resp packet",
+ hu->hdev->id);
+ return;
+ }
+
+ BT_INFO("BCSP: hci%d connected", hu->hdev->id);
+ bcsp->le_state = BCSP_GARULOUS;
+ }
+ /* Spot "sync rsp" pkts and update state machine */
+ else if (!memcmp(&bcsp->rx_skb->data[4], sync_rsp_pkt, 4)) {
+ if (bcsp->le_state != BCSP_SHY) {
+ BT_DBG("BCSP: hci%d ignoring sync_resp packet",
+ hu->hdev->id);
+ return;
+ }
+
+ BT_DBG("Found a LE sync rsp pkt");
+ bcsp->le_state = BCSP_CURIOUS;
+ }
+
}
}
@@ -537,12 +632,13 @@ static void bcsp_complete_rx_pkt(struct hci_uart *hu)
}
} else
kfree_skb(bcsp->rx_skb);
- } else {
+ } else if (bcsp->le_state == BCSP_GARULOUS) {
/* Pull out BCSP hdr */
skb_pull(bcsp->rx_skb, 4);
hci_recv_frame(hu->hdev, bcsp->rx_skb);
- }
+ } else
+ kfree_skb(bcsp->rx_skb);
bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
bcsp->rx_skb = NULL;
@@ -676,16 +772,42 @@ static void bcsp_timed_event(unsigned long arg)
struct sk_buff *skb;
unsigned long flags;
- BT_DBG("hu %p retransmitting %u pkts", hu, bcsp->unack.qlen);
+ switch (bcsp->le_state) {
+ case BCSP_GARULOUS:
+ BT_DBG("hu %p retransmitting %u pkts", hu, bcsp->unack.qlen);
- spin_lock_irqsave_nested(&bcsp->unack.lock, flags, SINGLE_DEPTH_NESTING);
+ spin_lock_irqsave_nested(&bcsp->unack.lock, flags,
+ SINGLE_DEPTH_NESTING);
- while ((skb = __skb_dequeue_tail(&bcsp->unack)) != NULL) {
- bcsp->msgq_txseq = (bcsp->msgq_txseq - 1) & 0x07;
- skb_queue_head(&bcsp->rel, skb);
- }
+ while ((skb = __skb_dequeue_tail(&bcsp->unack)) != NULL) {
+ bcsp->msgq_txseq = (bcsp->msgq_txseq - 1) & 0x07;
+ skb_queue_head(&bcsp->rel, skb);
+ }
- spin_unlock_irqrestore(&bcsp->unack.lock, flags);
+ spin_unlock_irqrestore(&bcsp->unack.lock, flags);
+ break;
+ case BCSP_CURIOUS:
+ skb = alloc_skb(4, GFP_ATOMIC);
+ memcpy(skb_put(skb, 4), conf_pkt, 4);
+ bt_cb(skb)->pkt_type = BCSP_LE_PKT;
+ skb_queue_head(&bcsp->unrel, skb);
+
+ bcsp->tbcsp.expires += LE_POLLING;
+ add_timer(&bcsp->tbcsp);
+ break;
+ default:
+ BT_INFO("Internal state unknown. Assuming not connected.");
+ bcsp->le_state = BCSP_SHY;
+ case BCSP_SHY:
+ skb = alloc_skb(4, GFP_ATOMIC);
+ memcpy(skb_put(skb, 4), sync_pkt, 4);
+ bt_cb(skb)->pkt_type = BCSP_LE_PKT;
+ skb_queue_head(&bcsp->unrel, skb);
+
+ bcsp->tbcsp.expires += LE_POLLING;
+ add_timer(&bcsp->tbcsp);
+ break;
+ }
hci_uart_tx_wakeup(hu);
}
@@ -708,12 +830,16 @@ static int bcsp_open(struct hci_uart *hu)
init_timer(&bcsp->tbcsp);
bcsp->tbcsp.function = bcsp_timed_event;
bcsp->tbcsp.data = (u_long) hu;
+ bcsp->tbcsp.expires = jiffies + LE_POLLING;
bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
+ bcsp->le_state = BCSP_SHY;
if (txcrc)
bcsp->use_crc = 1;
+ add_timer(&bcsp->tbcsp);
+
return 0;
}
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1dae700..6fd8356 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -735,6 +735,8 @@ int hci_disconnect(struct hci_conn *conn, __u8 reason);
bool hci_setup_sync(struct hci_conn *conn, __u16 handle);
void hci_sco_setup(struct hci_conn *conn, __u8 status);
+void hci_notify(struct hci_dev *hdev, int event);
+
struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst,
u8 role);
int hci_conn_del(struct hci_conn *conn);
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index f001856..2c5130a 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -65,10 +65,12 @@ static DEFINE_IDA(hci_index_ida);
/* ---- HCI notifications ---- */
-static void hci_notify(struct hci_dev *hdev, int event)
+void hci_notify(struct hci_dev *hdev, int event)
{
hci_sock_dev_event(hdev, event);
}
+EXPORT_SYMBOL(hci_notify);
+
/* ---- HCI debugfs entries ---- */
@@ -2783,6 +2785,8 @@ done:
hci_dev_put(hdev);
return ret;
}
+EXPORT_SYMBOL(hci_dev_reset_stat);
+
static void hci_update_scan_state(struct hci_dev *hdev, u8 scan)
{
--
2.1.1
--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
