From db57b625912abc755b3172f59b13b947db69cc4c Mon Sep 17 00:00:00 2001 From: Sean Wang Date: Sat, 20 Nov 2021 06:25:46 +0800 Subject: [PATCH 01/62] Bluetooth: btmtksdio: add support of processing firmware coredump and log Add support of processing the firmware coredump and log for the diagnostic purpose. Co-developed-by: Mark-yw Chen Signed-off-by: Mark-yw Chen Signed-off-by: Sean Wang Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btmtksdio.c | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/drivers/bluetooth/btmtksdio.c b/drivers/bluetooth/btmtksdio.c index ce6a6c00ff98..93c47796ecfd 100644 --- a/drivers/bluetooth/btmtksdio.c +++ b/drivers/bluetooth/btmtksdio.c @@ -324,8 +324,29 @@ err_out: return err; } +static int btmtksdio_recv_acl(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct btmtksdio_dev *bdev = hci_get_drvdata(hdev); + u16 handle = le16_to_cpu(hci_acl_hdr(skb)->handle); + + switch (handle) { + case 0xfc6f: + /* Firmware dump from device: when the firmware hangs, the + * device can no longer suspend and thus disable auto-suspend. + */ + pm_runtime_forbid(bdev->dev); + fallthrough; + case 0x05ff: + case 0x05fe: + /* Firmware debug logging */ + return hci_recv_diag(hdev, skb); + } + + return hci_recv_frame(hdev, skb); +} + static const struct h4_recv_pkt mtk_recv_pkts[] = { - { H4_RECV_ACL, .recv = hci_recv_frame }, + { H4_RECV_ACL, .recv = btmtksdio_recv_acl }, { H4_RECV_SCO, .recv = hci_recv_frame }, { H4_RECV_EVENT, .recv = btmtksdio_recv_event }, }; From d555b1f2c333d442ae9c84760d698f38796e459e Mon Sep 17 00:00:00 2001 From: Sean Wang Date: Sat, 20 Nov 2021 06:25:43 +0800 Subject: [PATCH 02/62] Bluetooth: btmtksdio: drop the unnecessary variable created Use the existent variable to drop the unnecessary variable created. Signed-off-by: Sean Wang Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btmtksdio.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/drivers/bluetooth/btmtksdio.c b/drivers/bluetooth/btmtksdio.c index 93c47796ecfd..d9cf0c492e29 100644 --- a/drivers/bluetooth/btmtksdio.c +++ b/drivers/bluetooth/btmtksdio.c @@ -98,6 +98,7 @@ MODULE_DEVICE_TABLE(sdio, btmtksdio_table); #define MTK_SDIO_BLOCK_SIZE 256 #define BTMTKSDIO_TX_WAIT_VND_EVT 1 +#define BTMTKSDIO_HW_TX_READY 2 struct mtkbtsdio_hdr { __le16 len; @@ -113,7 +114,6 @@ struct btmtksdio_dev { struct work_struct txrx_work; unsigned long tx_state; struct sk_buff_head txq; - bool hw_tx_ready; struct sk_buff *evt_skb; @@ -254,7 +254,7 @@ static int btmtksdio_tx_packet(struct btmtksdio_dev *bdev, sdio_hdr->reserved = cpu_to_le16(0); sdio_hdr->bt_type = hci_skb_pkt_type(skb); - bdev->hw_tx_ready = false; + clear_bit(BTMTKSDIO_HW_TX_READY, &bdev->tx_state); err = sdio_writesb(bdev->func, MTK_REG_CTDR, skb->data, round_up(skb->len, MTK_SDIO_BLOCK_SIZE)); if (err < 0) @@ -484,11 +484,12 @@ static void btmtksdio_txrx_work(struct work_struct *work) bt_dev_dbg(bdev->hdev, "Get fw own back"); if (int_status & TX_EMPTY) - bdev->hw_tx_ready = true; + set_bit(BTMTKSDIO_HW_TX_READY, &bdev->tx_state); + else if (unlikely(int_status & TX_FIFO_OVERFLOW)) bt_dev_warn(bdev->hdev, "Tx fifo overflow"); - if (bdev->hw_tx_ready) { + if (test_bit(BTMTKSDIO_HW_TX_READY, &bdev->tx_state)) { skb = skb_dequeue(&bdev->txq); if (skb) { err = btmtksdio_tx_packet(bdev, skb); @@ -832,7 +833,7 @@ static int btmtksdio_setup(struct hci_dev *hdev) u32 fw_version = 0; calltime = ktime_get(); - bdev->hw_tx_ready = true; + set_bit(BTMTKSDIO_HW_TX_READY, &bdev->tx_state); switch (bdev->data->chipid) { case 0x7921: From 27fe097bc60a344ccd8107522184c2750f45df5c Mon Sep 17 00:00:00 2001 From: Larry Finger Date: Sun, 21 Nov 2021 10:51:48 -0600 Subject: [PATCH 03/62] Bluetooth: btusb: Add one more Bluetooth part for the Realtek RTL8852AE The Realtek RTL8852AE has both wifi and BT components. The latter reports a USB ID of 0bda:385a, which is not in the table. The portion of /sys/kernel/debug/usb/devices pertaining to this device is T: Bus=01 Lev=01 Prnt=01 Port=03 Cnt=02 Dev#= 3 Spd=12 MxCh= 0 D: Ver= 1.00 Cls=e0(wlcon) Sub=01 Prot=01 MxPS=64 #Cfgs= 1 P: Vendor=0bda ProdID=385a Rev= 0.00 S: Manufacturer=Realtek S: Product=Bluetooth Radio S: SerialNumber=00e04c000001 C:* #Ifs= 2 Cfg#= 1 Atr=e0 MxPwr=500mA I:* If#= 0 Alt= 0 #EPs= 3 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=81(I) Atr=03(Int.) MxPS= 16 Ivl=1ms E: Ad=02(O) Atr=02(Bulk) MxPS= 64 Ivl=0ms E: Ad=82(I) Atr=02(Bulk) MxPS= 64 Ivl=0ms I:* If#= 1 Alt= 0 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=03(O) Atr=01(Isoc) MxPS= 0 Ivl=1ms E: Ad=83(I) Atr=01(Isoc) MxPS= 0 Ivl=1ms I: If#= 1 Alt= 1 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=03(O) Atr=01(Isoc) MxPS= 9 Ivl=1ms E: Ad=83(I) Atr=01(Isoc) MxPS= 9 Ivl=1ms I: If#= 1 Alt= 2 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=03(O) Atr=01(Isoc) MxPS= 17 Ivl=1ms E: Ad=83(I) Atr=01(Isoc) MxPS= 17 Ivl=1ms I: If#= 1 Alt= 3 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=03(O) Atr=01(Isoc) MxPS= 25 Ivl=1ms E: Ad=83(I) Atr=01(Isoc) MxPS= 25 Ivl=1ms I: If#= 1 Alt= 4 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=03(O) Atr=01(Isoc) MxPS= 33 Ivl=1ms E: Ad=83(I) Atr=01(Isoc) MxPS= 33 Ivl=1ms I: If#= 1 Alt= 5 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=03(O) Atr=01(Isoc) MxPS= 49 Ivl=1ms E: Ad=83(I) Atr=01(Isoc) MxPS= 49 Ivl=1ms Signed-off-by: Larry Finger Cc: Stable Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btusb.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index 30a057e1d4e3..a092c9842699 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -385,6 +385,8 @@ static const struct usb_device_id blacklist_table[] = { /* Realtek 8852AE Bluetooth devices */ { USB_DEVICE(0x0bda, 0xc852), .driver_info = BTUSB_REALTEK | BTUSB_WIDEBAND_SPEECH }, + { USB_DEVICE(0x0bda, 0x385a), .driver_info = BTUSB_REALTEK | + BTUSB_WIDEBAND_SPEECH }, { USB_DEVICE(0x0bda, 0x4852), .driver_info = BTUSB_REALTEK | BTUSB_WIDEBAND_SPEECH }, { USB_DEVICE(0x04c5, 0x165c), .driver_info = BTUSB_REALTEK | From 741268adb34061c28d8236dd67217f0df580c930 Mon Sep 17 00:00:00 2001 From: "tjiang@codeaurora.org" Date: Tue, 16 Nov 2021 12:48:49 +0800 Subject: [PATCH 04/62] Bluetooth: btusb: re-definition for board_id in struct qca_version The board ID should be split into two bytes. The 1st byte is chip ID, and the 2nd byte is platform ID. For example, board ID 0x010A, 0x01 is platform ID. 0x0A is chip ID. we have several platforms, and platform IDs are continuously added. We would not distinguish different chips if we get these mixed up. Platform ID: 0x00 is for Mobile 0x01 is for X86 0x02 is for Automotive 0x03 is for Consumer electronic Signed-off-by: Tim Jiang Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btusb.c | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index a092c9842699..d5d5e850252c 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -2886,7 +2886,8 @@ struct qca_version { __le32 rom_version; __le32 patch_version; __le32 ram_version; - __le16 board_id; + __u8 chip_id; + __u8 platform_id; __le16 flag; __u8 reserved[4]; } __packed; @@ -3075,7 +3076,17 @@ static void btusb_generate_qca_nvm_name(char *fwname, size_t max_size, u16 flag = le16_to_cpu(ver->flag); if (((flag >> 8) & 0xff) == QCA_FLAG_MULTI_NVM) { - u16 board_id = le16_to_cpu(ver->board_id); + /* The board_id should be split into two bytes + * The 1st byte is chip ID, and the 2nd byte is platform ID + * For example, board ID 0x010A, 0x01 is platform ID. 0x0A is chip ID + * Currently we have several platforms, and platform IDs are continuously added. + * Platform ID: + * 0x00 is for Mobile + * 0x01 is for X86 + * 0x02 is for Automotive + * 0x03 is for Consumer electronic + */ + u16 board_id = (ver->chip_id << 8) + ver->platform_id; const char *variant; switch (le32_to_cpu(ver->ram_version)) { From 21a241b3bc153b346987a28cc132674646589e02 Mon Sep 17 00:00:00 2001 From: "tjiang@codeaurora.org" Date: Tue, 16 Nov 2021 19:02:16 +0800 Subject: [PATCH 05/62] Bluetooth: btusb: Add the new support IDs for WCN6855 Add the more IDs of HP to usb_device_id table for WCN6855. -Device(0489:e0cc) from /sys/kernel/debug/usb/devices T: Bus=01 Lev=01 Prnt=01 Port=00 Cnt=01 Dev#= 2 Spd=12 MxCh= 0 D: Ver= 1.10 Cls=e0(wlcon) Sub=01 Prot=01 MxPS=64 #Cfgs= 1 P: Vendor=0489 ProdID=e0cc Rev= 0.01 C:* #Ifs= 2 Cfg#= 1 Atr=e0 MxPwr=100mA I:* If#= 0 Alt= 0 #EPs= 3 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=81(I) Atr=03(Int.) MxPS= 16 Ivl=1ms E: Ad=82(I) Atr=02(Bulk) MxPS= 64 Ivl=0ms E: Ad=02(O) Atr=02(Bulk) MxPS= 64 Ivl=0ms I:* If#= 1 Alt= 0 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 0 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 0 Ivl=1ms I: If#= 1 Alt= 1 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 9 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 9 Ivl=1ms I: If#= 1 Alt= 2 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 17 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 17 Ivl=1ms I: If#= 1 Alt= 3 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 25 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 25 Ivl=1ms I: If#= 1 Alt= 4 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 33 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 33 Ivl=1ms I: If#= 1 Alt= 5 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 49 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 49 Ivl=1ms I: If#= 1 Alt= 6 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 63 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 63 Ivl=1ms I: If#= 1 Alt= 7 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 65 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 65 Ivl=1ms -Device(0489:e0d6) from /sys/kernel/debug/usb/devices T: Bus=01 Lev=01 Prnt=01 Port=00 Cnt=01 Dev#= 2 Spd=12 MxCh= 0 D: Ver= 1.10 Cls=e0(wlcon) Sub=01 Prot=01 MxPS=64 #Cfgs= 1 P: Vendor=0489 ProdID=e0d6 Rev= 0.01 C:* #Ifs= 2 Cfg#= 1 Atr=e0 MxPwr=100mA I:* If#= 0 Alt= 0 #EPs= 3 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=81(I) Atr=03(Int.) MxPS= 16 Ivl=1ms E: Ad=82(I) Atr=02(Bulk) MxPS= 64 Ivl=0ms E: Ad=02(O) Atr=02(Bulk) MxPS= 64 Ivl=0ms I:* If#= 1 Alt= 0 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 0 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 0 Ivl=1ms I: If#= 1 Alt= 1 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 9 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 9 Ivl=1ms I: If#= 1 Alt= 2 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 17 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 17 Ivl=1ms I: If#= 1 Alt= 3 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 25 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 25 Ivl=1ms I: If#= 1 Alt= 4 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 33 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 33 Ivl=1ms I: If#= 1 Alt= 5 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 49 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 49 Ivl=1ms I: If#= 1 Alt= 6 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 63 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 63 Ivl=1ms I: If#= 1 Alt= 7 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 65 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 65 Ivl=1ms Signed-off-by: Tim Jiang Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btusb.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index d5d5e850252c..ab169fc673ea 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -296,6 +296,12 @@ static const struct usb_device_id blacklist_table[] = { { USB_DEVICE(0x0cf3, 0xe600), .driver_info = BTUSB_QCA_WCN6855 | BTUSB_WIDEBAND_SPEECH | BTUSB_VALID_LE_STATES }, + { USB_DEVICE(0x0489, 0xe0cc), .driver_info = BTUSB_QCA_WCN6855 | + BTUSB_WIDEBAND_SPEECH | + BTUSB_VALID_LE_STATES }, + { USB_DEVICE(0x0489, 0xe0d6), .driver_info = BTUSB_QCA_WCN6855 | + BTUSB_WIDEBAND_SPEECH | + BTUSB_VALID_LE_STATES }, /* Broadcom BCM2035 */ { USB_DEVICE(0x0a5c, 0x2009), .driver_info = BTUSB_BCM92035 }, @@ -3079,7 +3085,7 @@ static void btusb_generate_qca_nvm_name(char *fwname, size_t max_size, /* The board_id should be split into two bytes * The 1st byte is chip ID, and the 2nd byte is platform ID * For example, board ID 0x010A, 0x01 is platform ID. 0x0A is chip ID - * Currently we have several platforms, and platform IDs are continuously added. + * we have several platforms, and platform IDs are continuously added * Platform ID: * 0x00 is for Mobile * 0x01 is for X86 From 899663be5e75dc0174dc8bda0b5e6826edf0b29a Mon Sep 17 00:00:00 2001 From: Brian Gix Date: Wed, 24 Nov 2021 12:16:28 -0800 Subject: [PATCH 06/62] Bluetooth: refactor malicious adv data check Check for out-of-bound read was being performed at the end of while num_reports loop, and would fill journal with false positives. Added check to beginning of loop processing so that it doesn't get checked after ptr has been advanced. Signed-off-by: Brian Gix Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_event.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index efc5458b1345..dee4ef22fc88 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -5920,6 +5920,11 @@ static void hci_le_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) struct hci_ev_le_advertising_info *ev = ptr; s8 rssi; + if (ptr > (void *)skb_tail_pointer(skb) - sizeof(*ev)) { + bt_dev_err(hdev, "Malicious advertising data."); + break; + } + if (ev->length <= HCI_MAX_AD_LENGTH && ev->data + ev->length <= skb_tail_pointer(skb)) { rssi = ev->data[ev->length]; @@ -5931,11 +5936,6 @@ static void hci_le_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) } ptr += sizeof(*ev) + ev->length + 1; - - if (ptr > (void *) skb_tail_pointer(skb) - sizeof(*ev)) { - bt_dev_err(hdev, "Malicious advertising data. Stopping processing"); - break; - } } hci_dev_unlock(hdev); From e88422bccda86808359f0d4179e25e5c2191ab4f Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 24 Nov 2021 16:16:12 -0800 Subject: [PATCH 07/62] Bluetooth: HCI: Fix definition of hci_rp_read_stored_link_key Both max_num_keys and num_key are 2 octects: BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 4, Part E page 1985: Max_Num_Keys: Size: 2 octets Range: 0x0000 to 0xFFFF Num_Keys_Read: Size: 2 octets Range: 0x0000 to 0xFFFF Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 4 ++-- include/net/bluetooth/hci_core.h | 4 ++-- net/bluetooth/hci_event.c | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 84db6b275231..923534da9c0f 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -1047,8 +1047,8 @@ struct hci_cp_read_stored_link_key { } __packed; struct hci_rp_read_stored_link_key { __u8 status; - __u8 max_keys; - __u8 num_keys; + __le16 max_keys; + __le16 num_keys; } __packed; #define HCI_OP_DELETE_STORED_LINK_KEY 0x0c12 diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 2560cfe80db8..bb07a6d0d597 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -352,8 +352,8 @@ struct hci_dev { __u16 lmp_subver; __u16 voice_setting; __u8 num_iac; - __u8 stored_max_keys; - __u8 stored_num_keys; + __u16 stored_max_keys; + __u16 stored_num_keys; __u8 io_capability; __s8 inq_tx_power; __u8 err_data_reporting; diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index dee4ef22fc88..bda7b3ad93fa 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -256,8 +256,8 @@ static void hci_cc_read_stored_link_key(struct hci_dev *hdev, return; if (!rp->status && sent->read_all == 0x01) { - hdev->stored_max_keys = rp->max_keys; - hdev->stored_num_keys = rp->num_keys; + hdev->stored_max_keys = le16_to_cpu(rp->max_keys); + hdev->stored_num_keys = le16_to_cpu(rp->num_keys); } } From 7978656caf2ad3963fb63797128ec8d71caa2798 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 24 Nov 2021 16:16:13 -0800 Subject: [PATCH 08/62] Bluetooth: HCI: Fix definition of hci_rp_delete_stored_link_key num_keys is actually 2 octects not 1: BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 4, Part E page 1989: Num_Keys_Deleted: Size: 2 octets 0xXXXX Number of Link Keys Deleted Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 2 +- net/bluetooth/hci_event.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 923534da9c0f..0d2a9216869b 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -1058,7 +1058,7 @@ struct hci_cp_delete_stored_link_key { } __packed; struct hci_rp_delete_stored_link_key { __u8 status; - __u8 num_keys; + __le16 num_keys; } __packed; #define HCI_MAX_NAME_LENGTH 248 diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index bda7b3ad93fa..394d676c9baa 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -272,7 +272,7 @@ static void hci_cc_delete_stored_link_key(struct hci_dev *hdev, return; if (rp->num_keys <= hdev->stored_num_keys) - hdev->stored_num_keys -= rp->num_keys; + hdev->stored_num_keys -= le16_to_cpu(rp->num_keys); else hdev->stored_num_keys = 0; } From ea13aed5e5dfbabd166759aaf9f4af3cb804875a Mon Sep 17 00:00:00 2001 From: Archie Pusaka Date: Thu, 25 Nov 2021 15:04:36 +0800 Subject: [PATCH 09/62] Bluetooth: Send device found event on name resolve failure Introducing NAME_REQUEST_FAILED flag that will be sent together with device found event on name resolve failure. This will provide the userspace with an information so it can decide not to resolve the name for these devices in the future. Signed-off-by: Archie Pusaka Reviewed-by: Miao-chen Chou Signed-off-by: Marcel Holtmann --- include/net/bluetooth/mgmt.h | 9 +++++---- net/bluetooth/hci_event.c | 11 ++++------- net/bluetooth/mgmt.c | 12 ++++++++++-- 3 files changed, 19 insertions(+), 13 deletions(-) diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h index 23a0524061b7..107b25deae68 100644 --- a/include/net/bluetooth/mgmt.h +++ b/include/net/bluetooth/mgmt.h @@ -936,10 +936,11 @@ struct mgmt_ev_auth_failed { __u8 status; } __packed; -#define MGMT_DEV_FOUND_CONFIRM_NAME 0x01 -#define MGMT_DEV_FOUND_LEGACY_PAIRING 0x02 -#define MGMT_DEV_FOUND_NOT_CONNECTABLE 0x04 -#define MGMT_DEV_FOUND_INITIATED_CONN 0x08 +#define MGMT_DEV_FOUND_CONFIRM_NAME 0x01 +#define MGMT_DEV_FOUND_LEGACY_PAIRING 0x02 +#define MGMT_DEV_FOUND_NOT_CONNECTABLE 0x04 +#define MGMT_DEV_FOUND_INITIATED_CONN 0x08 +#define MGMT_DEV_FOUND_NAME_REQUEST_FAILED 0x10 #define MGMT_EV_DEVICE_FOUND 0x0012 struct mgmt_ev_device_found { diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 394d676c9baa..324d376f97d4 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -2175,13 +2175,10 @@ static void hci_check_pending_name(struct hci_dev *hdev, struct hci_conn *conn, return; list_del(&e->list); - if (name) { - e->name_state = NAME_KNOWN; - mgmt_remote_name(hdev, bdaddr, ACL_LINK, 0x00, - e->data.rssi, name, name_len); - } else { - e->name_state = NAME_NOT_KNOWN; - } + + e->name_state = name ? NAME_KNOWN : NAME_NOT_KNOWN; + mgmt_remote_name(hdev, bdaddr, ACL_LINK, 0x00, e->data.rssi, + name, name_len); if (hci_resolve_next_name(hdev)) return; diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index f8f74d344297..bf989ae03f9f 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -9615,6 +9615,7 @@ void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, struct mgmt_ev_device_found *ev; char buf[sizeof(*ev) + HCI_MAX_NAME_LENGTH + 2]; u16 eir_len; + u32 flags; ev = (struct mgmt_ev_device_found *) buf; @@ -9624,10 +9625,17 @@ void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, ev->addr.type = link_to_bdaddr(link_type, addr_type); ev->rssi = rssi; - eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, name, - name_len); + if (name) { + eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, name, + name_len); + flags = 0; + } else { + eir_len = 0; + flags = MGMT_DEV_FOUND_NAME_REQUEST_FAILED; + } ev->eir_len = cpu_to_le16(eir_len); + ev->flags = cpu_to_le32(flags); mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, sizeof(*ev) + eir_len, NULL); } From dbf6811abbfcc79d3cd5ce1ff53fe1c741167a1f Mon Sep 17 00:00:00 2001 From: Archie Pusaka Date: Thu, 25 Nov 2021 15:04:37 +0800 Subject: [PATCH 10/62] Bluetooth: Limit duration of Remote Name Resolve When doing remote name request, we cannot scan. In the normal case it's OK since we can expect it to finish within a short amount of time. However, there is a possibility to scan lots of devices that (1) requires Remote Name Resolve (2) is unresponsive to Remote Name Resolve When this happens, we are stuck to do Remote Name Resolve until all is done before continue scanning. This patch adds a time limit to stop us spending too long on remote name request. Signed-off-by: Archie Pusaka Reviewed-by: Miao-chen Chou Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci_core.h | 3 +++ net/bluetooth/hci_event.c | 7 +++++++ 2 files changed, 10 insertions(+) diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index bb07a6d0d597..7bae8376fd6f 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -88,6 +88,7 @@ struct discovery_state { u8 (*uuids)[16]; unsigned long scan_start; unsigned long scan_duration; + unsigned long name_resolve_timeout; }; #define SUSPEND_NOTIFIER_TIMEOUT msecs_to_jiffies(2000) /* 2 seconds */ @@ -1759,6 +1760,8 @@ void hci_mgmt_chan_unregister(struct hci_mgmt_chan *c); #define DISCOV_LE_FAST_ADV_INT_MIN 0x00A0 /* 100 msec */ #define DISCOV_LE_FAST_ADV_INT_MAX 0x00F0 /* 150 msec */ +#define NAME_RESOLVE_DURATION msecs_to_jiffies(10240) /* 10.24 sec */ + void mgmt_fill_version_info(void *ver); int mgmt_new_settings(struct hci_dev *hdev); void mgmt_index_added(struct hci_dev *hdev); diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 324d376f97d4..9d8d2d9e5d1f 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -2129,6 +2129,12 @@ static bool hci_resolve_next_name(struct hci_dev *hdev) if (list_empty(&discov->resolve)) return false; + /* We should stop if we already spent too much time resolving names. */ + if (time_after(jiffies, discov->name_resolve_timeout)) { + bt_dev_warn_ratelimited(hdev, "Name resolve takes too long."); + return false; + } + e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY, NAME_NEEDED); if (!e) return false; @@ -2716,6 +2722,7 @@ static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) if (e && hci_resolve_name(hdev, e) == 0) { e->name_state = NAME_PENDING; hci_discovery_set_state(hdev, DISCOVERY_RESOLVING); + discov->name_resolve_timeout = jiffies + NAME_RESOLVE_DURATION; } else { /* When BR/EDR inquiry is active and no LE scanning is in * progress, then change discovery state to indicate completion. From ae422391e17d30a2fb1d0968142dcd1e4cdc172a Mon Sep 17 00:00:00 2001 From: Benjamin Berg Date: Fri, 3 Dec 2021 15:58:59 +0100 Subject: [PATCH 11/62] Bluetooth: Reset more state when cancelling a sync command Resetting the timers and cmd_cnt means that we assume the device will be in a good state after the sync command finishes. Without this a chain of synchronous commands might get stuck if one of them is cancelled. Signed-off-by: Benjamin Berg Signed-off-by: Luiz Augusto von Dentz --- net/bluetooth/hci_request.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/net/bluetooth/hci_request.c b/net/bluetooth/hci_request.c index 8b3205e4b23e..58d640a31bde 100644 --- a/net/bluetooth/hci_request.c +++ b/net/bluetooth/hci_request.c @@ -118,6 +118,11 @@ void hci_req_sync_cancel(struct hci_dev *hdev, int err) if (hdev->req_status == HCI_REQ_PEND) { hdev->req_result = err; hdev->req_status = HCI_REQ_CANCELED; + + cancel_delayed_work_sync(&hdev->cmd_timer); + cancel_delayed_work_sync(&hdev->ncmd_timer); + atomic_set(&hdev->cmd_cnt, 1); + wake_up_interruptible(&hdev->req_wait_q); } } From 914b08b330d6722ed081e14580aae6fe66cd5946 Mon Sep 17 00:00:00 2001 From: Benjamin Berg Date: Fri, 3 Dec 2021 15:59:00 +0100 Subject: [PATCH 12/62] Bluetooth: Add hci_cmd_sync_cancel to public API After transfer errors it makes sense to cancel an ongoing synchronous command that cannot complete anymore. To permit this, export the old hci_req_sync_cancel function as hci_cmd_sync_cancel in the API. Signed-off-by: Benjamin Berg Signed-off-by: Luiz Augusto von Dentz --- include/net/bluetooth/hci_sync.h | 1 + net/bluetooth/hci_request.c | 18 +----------------- net/bluetooth/hci_request.h | 1 - net/bluetooth/hci_sync.c | 17 +++++++++++++++++ 4 files changed, 19 insertions(+), 18 deletions(-) diff --git a/include/net/bluetooth/hci_sync.h b/include/net/bluetooth/hci_sync.h index 0336c1bc5d25..f4034bf8f1ce 100644 --- a/include/net/bluetooth/hci_sync.h +++ b/include/net/bluetooth/hci_sync.h @@ -37,6 +37,7 @@ int __hci_cmd_sync_status_sk(struct hci_dev *hdev, u16 opcode, u32 plen, void hci_cmd_sync_init(struct hci_dev *hdev); void hci_cmd_sync_clear(struct hci_dev *hdev); +void hci_cmd_sync_cancel(struct hci_dev *hdev, int err); int hci_cmd_sync_queue(struct hci_dev *hdev, hci_cmd_sync_work_func_t func, void *data, hci_cmd_sync_work_destroy_t destroy); diff --git a/net/bluetooth/hci_request.c b/net/bluetooth/hci_request.c index 58d640a31bde..a2607db44404 100644 --- a/net/bluetooth/hci_request.c +++ b/net/bluetooth/hci_request.c @@ -111,22 +111,6 @@ void hci_req_sync_complete(struct hci_dev *hdev, u8 result, u16 opcode, } } -void hci_req_sync_cancel(struct hci_dev *hdev, int err) -{ - bt_dev_dbg(hdev, "err 0x%2.2x", err); - - if (hdev->req_status == HCI_REQ_PEND) { - hdev->req_result = err; - hdev->req_status = HCI_REQ_CANCELED; - - cancel_delayed_work_sync(&hdev->cmd_timer); - cancel_delayed_work_sync(&hdev->ncmd_timer); - atomic_set(&hdev->cmd_cnt, 1); - - wake_up_interruptible(&hdev->req_wait_q); - } -} - /* Execute request and wait for completion. */ int __hci_req_sync(struct hci_dev *hdev, int (*func)(struct hci_request *req, unsigned long opt), @@ -2708,7 +2692,7 @@ void hci_request_setup(struct hci_dev *hdev) void hci_request_cancel_all(struct hci_dev *hdev) { - hci_req_sync_cancel(hdev, ENODEV); + hci_cmd_sync_cancel(hdev, ENODEV); cancel_work_sync(&hdev->discov_update); cancel_work_sync(&hdev->scan_update); diff --git a/net/bluetooth/hci_request.h b/net/bluetooth/hci_request.h index 5f8e8846ec74..8d39e9416861 100644 --- a/net/bluetooth/hci_request.h +++ b/net/bluetooth/hci_request.h @@ -64,7 +64,6 @@ int hci_req_sync(struct hci_dev *hdev, int (*req)(struct hci_request *req, int __hci_req_sync(struct hci_dev *hdev, int (*func)(struct hci_request *req, unsigned long opt), unsigned long opt, u32 timeout, u8 *hci_status); -void hci_req_sync_cancel(struct hci_dev *hdev, int err); struct sk_buff *hci_prepare_cmd(struct hci_dev *hdev, u16 opcode, u32 plen, const void *param); diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index ad86caf41f91..7ac6c170ec49 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -335,6 +335,23 @@ void hci_cmd_sync_clear(struct hci_dev *hdev) } } +void hci_cmd_sync_cancel(struct hci_dev *hdev, int err) +{ + bt_dev_dbg(hdev, "err 0x%2.2x", err); + + if (hdev->req_status == HCI_REQ_PEND) { + hdev->req_result = err; + hdev->req_status = HCI_REQ_CANCELED; + + cancel_delayed_work_sync(&hdev->cmd_timer); + cancel_delayed_work_sync(&hdev->ncmd_timer); + atomic_set(&hdev->cmd_cnt, 1); + + wake_up_interruptible(&hdev->req_wait_q); + } +} +EXPORT_SYMBOL(hci_cmd_sync_cancel); + int hci_cmd_sync_queue(struct hci_dev *hdev, hci_cmd_sync_work_func_t func, void *data, hci_cmd_sync_work_destroy_t destroy) { From 2250abadd3508d6961f1aa56cf8cff59f0196cb8 Mon Sep 17 00:00:00 2001 From: Benjamin Berg Date: Fri, 3 Dec 2021 15:59:01 +0100 Subject: [PATCH 13/62] Bluetooth: hci_core: Cancel sync command if sending a frame failed If sending a frame failed any sync command associated with it will never be completed. As such, cancel any such command immediately to avoid timing out. Signed-off-by: Benjamin Berg Signed-off-by: Luiz Augusto von Dentz --- net/bluetooth/hci_core.c | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index fdc0dcf8ee36..5cadecd31f66 100644 --- a/net/bluetooth/hci_core.c +++ b/net/bluetooth/hci_core.c @@ -2906,7 +2906,7 @@ int hci_unregister_cb(struct hci_cb *cb) } EXPORT_SYMBOL(hci_unregister_cb); -static void hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb) +static int hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb) { int err; @@ -2929,14 +2929,17 @@ static void hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb) if (!test_bit(HCI_RUNNING, &hdev->flags)) { kfree_skb(skb); - return; + return -EINVAL; } err = hdev->send(hdev, skb); if (err < 0) { bt_dev_err(hdev, "sending frame failed (%d)", err); kfree_skb(skb); + return err; } + + return 0; } /* Send HCI command */ @@ -3843,10 +3846,15 @@ static void hci_cmd_work(struct work_struct *work) hdev->sent_cmd = skb_clone(skb, GFP_KERNEL); if (hdev->sent_cmd) { + int res; if (hci_req_status_pend(hdev)) hci_dev_set_flag(hdev, HCI_CMD_PENDING); atomic_dec(&hdev->cmd_cnt); - hci_send_frame(hdev, skb); + + res = hci_send_frame(hdev, skb); + if (res < 0) + hci_cmd_sync_cancel(hdev, -res); + if (test_bit(HCI_RESET, &hdev->flags)) cancel_delayed_work(&hdev->cmd_timer); else From c97a747efc93f94a4ad6c707972dfbf8d774edf9 Mon Sep 17 00:00:00 2001 From: Benjamin Berg Date: Fri, 3 Dec 2021 15:59:02 +0100 Subject: [PATCH 14/62] Bluetooth: btusb: Cancel sync commands for certain URB errors Cancel sync commands when transmission of URBs fail. This is done for both failures to send a command URB and also when the interrupt URB used to retrieve a response fails. This approach is sufficient to quickly deal with certain errors such as a device being disconnected while synchronous commands are done during initialization. Signed-off-by: Benjamin Berg Signed-off-by: Luiz Augusto von Dentz --- drivers/bluetooth/btusb.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index ab169fc673ea..d9067a8fad60 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -933,6 +933,8 @@ static void btusb_intr_complete(struct urb *urb) if (err != -EPERM && err != -ENODEV) bt_dev_err(hdev, "urb %p failed to resubmit (%d)", urb, -err); + if (err != -EPERM) + hci_cmd_sync_cancel(hdev, -err); usb_unanchor_urb(urb); } } @@ -976,6 +978,8 @@ static int btusb_submit_intr_urb(struct hci_dev *hdev, gfp_t mem_flags) if (err != -EPERM && err != -ENODEV) bt_dev_err(hdev, "urb %p submission failed (%d)", urb, -err); + if (err != -EPERM) + hci_cmd_sync_cancel(hdev, -err); usb_unanchor_urb(urb); } @@ -1331,10 +1335,13 @@ static void btusb_tx_complete(struct urb *urb) if (!test_bit(HCI_RUNNING, &hdev->flags)) goto done; - if (!urb->status) + if (!urb->status) { hdev->stat.byte_tx += urb->transfer_buffer_length; - else + } else { + if (hci_skb_pkt_type(skb) == HCI_COMMAND_PKT) + hci_cmd_sync_cancel(hdev, -urb->status); hdev->stat.err_tx++; + } done: spin_lock_irqsave(&data->txlock, flags); From 4b4b2228f521c338030b1f310a5dee73fd7d8f26 Mon Sep 17 00:00:00 2001 From: Sean Wang Date: Thu, 2 Dec 2021 02:02:46 +0800 Subject: [PATCH 15/62] Bluetooth: btmtksdio: handle runtime pm only when sdio_func is available Runtime pm ops is not aware the sdio_func status that is probably being disabled by btmtksdio_close. Thus, we are only able to access the sdio_func for the runtime pm operations only when the sdio_func is available. Fixes: 7f3c563c575e7 ("Bluetooth: btmtksdio: Add runtime PM support to SDIO based Bluetooth") Co-developed-by: Mark-yw Chen Signed-off-by: Mark-yw Chen Signed-off-by: Sean Wang Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btmtksdio.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/drivers/bluetooth/btmtksdio.c b/drivers/bluetooth/btmtksdio.c index d9cf0c492e29..fc6317e519e9 100644 --- a/drivers/bluetooth/btmtksdio.c +++ b/drivers/bluetooth/btmtksdio.c @@ -99,6 +99,7 @@ MODULE_DEVICE_TABLE(sdio, btmtksdio_table); #define BTMTKSDIO_TX_WAIT_VND_EVT 1 #define BTMTKSDIO_HW_TX_READY 2 +#define BTMTKSDIO_FUNC_ENABLED 3 struct mtkbtsdio_hdr { __le16 len; @@ -539,6 +540,8 @@ static int btmtksdio_open(struct hci_dev *hdev) if (err < 0) goto err_release_host; + set_bit(BTMTKSDIO_FUNC_ENABLED, &bdev->tx_state); + /* Get ownership from the device */ sdio_writel(bdev->func, C_FW_OWN_REQ_CLR, MTK_REG_CHLPCR, &err); if (err < 0) @@ -640,6 +643,7 @@ static int btmtksdio_close(struct hci_dev *hdev) if (err < 0) bt_dev_err(bdev->hdev, "Cannot return ownership to device"); + clear_bit(BTMTKSDIO_FUNC_ENABLED, &bdev->tx_state); sdio_disable_func(bdev->func); sdio_release_host(bdev->func); @@ -1058,6 +1062,9 @@ static int btmtksdio_runtime_suspend(struct device *dev) if (!bdev) return 0; + if (!test_bit(BTMTKSDIO_FUNC_ENABLED, &bdev->tx_state)) + return 0; + sdio_claim_host(bdev->func); sdio_writel(bdev->func, C_FW_OWN_REQ_SET, MTK_REG_CHLPCR, &err); @@ -1085,6 +1092,9 @@ static int btmtksdio_runtime_resume(struct device *dev) if (!bdev) return 0; + if (!test_bit(BTMTKSDIO_FUNC_ENABLED, &bdev->tx_state)) + return 0; + sdio_claim_host(bdev->func); sdio_writel(bdev->func, C_FW_OWN_REQ_CLR, MTK_REG_CHLPCR, &err); From 561ae1d46a8ddcbc13162d5771f5ed6c8249e730 Mon Sep 17 00:00:00 2001 From: Sean Wang Date: Thu, 2 Dec 2021 02:02:47 +0800 Subject: [PATCH 16/62] Bluetooth: btmtksdio: fix resume failure btmtksdio have to rely on MMC_PM_KEEP_POWER in pm_flags to avoid that SDIO power is being shut off during the device is in suspend. That fixes the SDIO command fails to access the bus after the device is resumed. Fixes: 7f3c563c575e7 ("Bluetooth: btmtksdio: Add runtime PM support to SDIO based Bluetooth") Co-developed-by: Mark-yw Chen Signed-off-by: Mark-yw Chen Signed-off-by: Sean Wang Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btmtksdio.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/drivers/bluetooth/btmtksdio.c b/drivers/bluetooth/btmtksdio.c index fc6317e519e9..143404745240 100644 --- a/drivers/bluetooth/btmtksdio.c +++ b/drivers/bluetooth/btmtksdio.c @@ -1065,6 +1065,8 @@ static int btmtksdio_runtime_suspend(struct device *dev) if (!test_bit(BTMTKSDIO_FUNC_ENABLED, &bdev->tx_state)) return 0; + sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER); + sdio_claim_host(bdev->func); sdio_writel(bdev->func, C_FW_OWN_REQ_SET, MTK_REG_CHLPCR, &err); From 13244cccc2b61ec715f0ac583d3037497004d4a5 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:54:52 -0800 Subject: [PATCH 17/62] skbuff: introduce skb_pull_data Like skb_pull but returns the original data pointer before pulling the data after performing a check against sbk->len. This allows to change code that does "struct foo *p = (void *)skb->data;" which is hard to audit and error prone, to: p = skb_pull_data(skb, sizeof(*p)); if (!p) return; Which is both safer and cleaner. Acked-by: Jakub Kicinski Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Dan Carpenter Signed-off-by: Marcel Holtmann --- include/linux/skbuff.h | 2 ++ net/core/skbuff.c | 24 ++++++++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/include/linux/skbuff.h b/include/linux/skbuff.h index eba256af64a5..877dda38684a 100644 --- a/include/linux/skbuff.h +++ b/include/linux/skbuff.h @@ -2373,6 +2373,8 @@ static inline void *skb_pull_inline(struct sk_buff *skb, unsigned int len) return unlikely(len > skb->len) ? NULL : __skb_pull(skb, len); } +void *skb_pull_data(struct sk_buff *skb, size_t len); + void *__pskb_pull_tail(struct sk_buff *skb, int delta); static inline void *__pskb_pull(struct sk_buff *skb, unsigned int len) diff --git a/net/core/skbuff.c b/net/core/skbuff.c index a33247fdb8f5..dd3ef966efdb 100644 --- a/net/core/skbuff.c +++ b/net/core/skbuff.c @@ -2023,6 +2023,30 @@ void *skb_pull(struct sk_buff *skb, unsigned int len) } EXPORT_SYMBOL(skb_pull); +/** + * skb_pull_data - remove data from the start of a buffer returning its + * original position. + * @skb: buffer to use + * @len: amount of data to remove + * + * This function removes data from the start of a buffer, returning + * the memory to the headroom. A pointer to the original data in the buffer + * is returned after checking if there is enough data to pull. Once the + * data has been pulled future pushes will overwrite the old data. + */ +void *skb_pull_data(struct sk_buff *skb, size_t len) +{ + void *data = skb->data; + + if (skb->len < len) + return NULL; + + skb_pull(skb, len); + + return data; +} +EXPORT_SYMBOL(skb_pull_data); + /** * skb_trim - remove end from a buffer * @skb: buffer to alter From ae61a10d9d46c4a0844c46d0863bf991c4dda66c Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:54:53 -0800 Subject: [PATCH 18/62] Bluetooth: HCI: Use skb_pull_data to parse BR/EDR events This uses skb_pull_data to check the BR/EDR events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 4 + net/bluetooth/hci_event.c | 260 +++++++++++++++++++++++++++++------- 2 files changed, 217 insertions(+), 47 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 0d2a9216869b..ba89b078ceb5 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -2012,6 +2012,10 @@ struct hci_cp_le_reject_cis { } __packed; /* ---- HCI Events ---- */ +struct hci_ev_status { + __u8 status; +} __packed; + #define HCI_EV_INQUIRY_COMPLETE 0x01 #define HCI_EV_INQUIRY_RESULT 0x02 diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 9d8d2d9e5d1f..0266eab8a18c 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -45,6 +45,18 @@ /* Handle HCI Event packets */ +static void *hci_ev_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, + u8 ev, size_t len) +{ + void *data; + + data = skb_pull_data(skb, len); + if (!data) + bt_dev_err(hdev, "Malformed Event: 0x%2.2x", ev); + + return data; +} + static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb, u8 *new_status) { @@ -2682,11 +2694,15 @@ static void hci_cs_switch_role(struct hci_dev *hdev, u8 status) static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *ev; struct discovery_state *discov = &hdev->discovery; struct inquiry_entry *e; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_INQUIRY_COMPLETE, sizeof(*ev)); + if (!ev) + return; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_conn_check_pending(hdev); @@ -2780,9 +2796,13 @@ static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_conn_complete *ev = (void *) skb->data; + struct hci_ev_conn_complete *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CONN_COMPLETE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -2904,12 +2924,16 @@ static void hci_reject_conn(struct hci_dev *hdev, bdaddr_t *bdaddr) static void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_conn_request *ev = (void *) skb->data; + struct hci_ev_conn_request *ev; int mask = hdev->link_mode; struct inquiry_entry *ie; struct hci_conn *conn; __u8 flags = 0; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CONN_REQUEST, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s bdaddr %pMR type 0x%x", hdev->name, &ev->bdaddr, ev->link_type); @@ -3015,12 +3039,16 @@ static u8 hci_to_mgmt_reason(u8 err) static void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_disconn_complete *ev = (void *) skb->data; + struct hci_ev_disconn_complete *ev; u8 reason; struct hci_conn_params *params; struct hci_conn *conn; bool mgmt_connected; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_DISCONN_COMPLETE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -3099,9 +3127,13 @@ unlock: static void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_auth_complete *ev = (void *) skb->data; + struct hci_ev_auth_complete *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_AUTH_COMPLETE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -3169,9 +3201,13 @@ unlock: static void hci_remote_name_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_remote_name *ev = (void *) skb->data; + struct hci_ev_remote_name *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_NAME, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_conn_check_pending(hdev); @@ -3252,9 +3288,13 @@ unlock: static void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_encrypt_change *ev = (void *) skb->data; + struct hci_ev_encrypt_change *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_ENCRYPT_CHANGE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -3367,9 +3407,14 @@ unlock: static void hci_change_link_key_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_change_link_key_complete *ev = (void *) skb->data; + struct hci_ev_change_link_key_complete *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CHANGE_LINK_KEY_COMPLETE, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -3390,9 +3435,13 @@ static void hci_change_link_key_complete_evt(struct hci_dev *hdev, static void hci_remote_features_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_remote_features *ev = (void *) skb->data; + struct hci_ev_remote_features *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_FEATURES, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -3841,9 +3890,11 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb, hci_req_complete_t *req_complete, hci_req_complete_skb_t *req_complete_skb) { - struct hci_ev_cmd_status *ev = (void *) skb->data; + struct hci_ev_cmd_status *ev; - skb_pull(skb, sizeof(*ev)); + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CMD_STATUS, sizeof(*ev)); + if (!ev) + return; *opcode = __le16_to_cpu(ev->opcode); *status = ev->status; @@ -3951,7 +4002,11 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb, static void hci_hardware_error_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_hardware_error *ev = (void *) skb->data; + struct hci_ev_hardware_error *ev; + + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_HARDWARE_ERROR, sizeof(*ev)); + if (!ev) + return; hdev->hw_error_code = ev->code; @@ -3960,9 +4015,13 @@ static void hci_hardware_error_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_role_change *ev = (void *) skb->data; + struct hci_ev_role_change *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_ROLE_CHANGE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -4070,20 +4129,22 @@ static struct hci_conn *__hci_conn_lookup_handle(struct hci_dev *hdev, static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_num_comp_blocks *ev = (void *) skb->data; + struct hci_ev_num_comp_blocks *ev; int i; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_NUM_COMP_BLOCKS, sizeof(*ev)); + if (!ev) + return; + + if (!hci_ev_skb_pull(hdev, skb, HCI_EV_NUM_COMP_BLOCKS, + flex_array_size(ev, handles, ev->num_hndl))) + return; + if (hdev->flow_ctl_mode != HCI_FLOW_CTL_MODE_BLOCK_BASED) { bt_dev_err(hdev, "wrong event for mode %d", hdev->flow_ctl_mode); return; } - if (skb->len < sizeof(*ev) || - skb->len < struct_size(ev, handles, ev->num_hndl)) { - BT_DBG("%s bad parameters", hdev->name); - return; - } - BT_DBG("%s num_blocks %d num_hndl %d", hdev->name, ev->num_blocks, ev->num_hndl); @@ -4121,9 +4182,13 @@ static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_mode_change *ev = (void *) skb->data; + struct hci_ev_mode_change *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_MODE_CHANGE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -4149,9 +4214,13 @@ static void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_pin_code_req *ev = (void *) skb->data; + struct hci_ev_pin_code_req *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_PIN_CODE_REQ, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -4219,11 +4288,15 @@ static void conn_set_key(struct hci_conn *conn, u8 key_type, u8 pin_len) static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_link_key_req *ev = (void *) skb->data; + struct hci_ev_link_key_req *ev; struct hci_cp_link_key_reply cp; struct hci_conn *conn; struct link_key *key; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_LINK_KEY_REQ, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); if (!hci_dev_test_flag(hdev, HCI_MGMT)) @@ -4279,12 +4352,16 @@ not_found: static void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_link_key_notify *ev = (void *) skb->data; + struct hci_ev_link_key_notify *ev; struct hci_conn *conn; struct link_key *key; bool persistent; u8 pin_len = 0; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_LINK_KEY_NOTIFY, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -4339,9 +4416,13 @@ unlock: static void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_clock_offset *ev = (void *) skb->data; + struct hci_ev_clock_offset *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CLOCK_OFFSET, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -4362,9 +4443,13 @@ static void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_pkt_type_change *ev = (void *) skb->data; + struct hci_ev_pkt_type_change *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_PKT_TYPE_CHANGE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -4378,9 +4463,13 @@ static void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_pscan_rep_mode *ev = (void *) skb->data; + struct hci_ev_pscan_rep_mode *ev; struct inquiry_entry *ie; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_PSCAN_REP_MODE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -4468,9 +4557,14 @@ unlock: static void hci_remote_ext_features_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_remote_ext_features *ev = (void *) skb->data; + struct hci_ev_remote_ext_features *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_EXT_FEATURES, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -4532,9 +4626,13 @@ unlock: static void hci_sync_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_sync_conn_complete *ev = (void *) skb->data; + struct hci_ev_sync_conn_complete *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_SYNC_CONN_COMPLETE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -4697,9 +4795,14 @@ static void hci_extended_inquiry_result_evt(struct hci_dev *hdev, static void hci_key_refresh_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_key_refresh_complete *ev = (void *) skb->data; + struct hci_ev_key_refresh_complete *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_KEY_REFRESH_COMPLETE, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x handle 0x%4.4x", hdev->name, ev->status, __le16_to_cpu(ev->handle)); @@ -4806,9 +4909,13 @@ static u8 bredr_oob_data_present(struct hci_conn *conn) static void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_io_capa_request *ev = (void *) skb->data; + struct hci_ev_io_capa_request *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_IO_CAPA_REQUEST, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -4875,9 +4982,13 @@ unlock: static void hci_io_capa_reply_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_io_capa_reply *ev = (void *) skb->data; + struct hci_ev_io_capa_reply *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_IO_CAPA_REPLY, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -4896,10 +5007,15 @@ unlock: static void hci_user_confirm_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_user_confirm_req *ev = (void *) skb->data; + struct hci_ev_user_confirm_req *ev; int loc_mitm, rem_mitm, confirm_hint = 0; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_USER_CONFIRM_REQUEST, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -4981,7 +5097,12 @@ unlock: static void hci_user_passkey_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_user_passkey_req *ev = (void *) skb->data; + struct hci_ev_user_passkey_req *ev; + + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_USER_PASSKEY_REQUEST, + sizeof(*ev)); + if (!ev) + return; BT_DBG("%s", hdev->name); @@ -4992,9 +5113,14 @@ static void hci_user_passkey_request_evt(struct hci_dev *hdev, static void hci_user_passkey_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_user_passkey_notify *ev = (void *) skb->data; + struct hci_ev_user_passkey_notify *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_USER_PASSKEY_NOTIFY, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); @@ -5012,9 +5138,13 @@ static void hci_user_passkey_notify_evt(struct hci_dev *hdev, static void hci_keypress_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_keypress_notify *ev = (void *) skb->data; + struct hci_ev_keypress_notify *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_KEYPRESS_NOTIFY, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); @@ -5051,9 +5181,14 @@ static void hci_keypress_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_simple_pair_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_simple_pair_complete *ev = (void *) skb->data; + struct hci_ev_simple_pair_complete *ev; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_SIMPLE_PAIR_COMPLETE, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -5082,10 +5217,15 @@ unlock: static void hci_remote_host_features_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_remote_host_features *ev = (void *) skb->data; + struct hci_ev_remote_host_features *ev; struct inquiry_entry *ie; struct hci_conn *conn; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_HOST_FEATURES, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -5104,9 +5244,14 @@ static void hci_remote_host_features_evt(struct hci_dev *hdev, static void hci_remote_oob_data_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_remote_oob_data_request *ev = (void *) skb->data; + struct hci_ev_remote_oob_data_request *ev; struct oob_data *data; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_OOB_DATA_REQUEST, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s", hdev->name); hci_dev_lock(hdev); @@ -5158,12 +5303,14 @@ unlock: #if IS_ENABLED(CONFIG_BT_HS) static void hci_chan_selected_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_channel_selected *ev = (void *)skb->data; + struct hci_ev_channel_selected *ev; struct hci_conn *hcon; - BT_DBG("%s handle 0x%2.2x", hdev->name, ev->phy_handle); + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CHANNEL_SELECTED, sizeof(*ev)); + if (!ev) + return; - skb_pull(skb, sizeof(*ev)); + BT_DBG("%s handle 0x%2.2x", hdev->name, ev->phy_handle); hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle); if (!hcon) @@ -5175,9 +5322,13 @@ static void hci_chan_selected_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_phy_link_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_phy_link_complete *ev = (void *) skb->data; + struct hci_ev_phy_link_complete *ev; struct hci_conn *hcon, *bredr_hcon; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_PHY_LINK_COMPLETE, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle, ev->status); @@ -5215,11 +5366,16 @@ unlock: static void hci_loglink_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_logical_link_complete *ev = (void *) skb->data; + struct hci_ev_logical_link_complete *ev; struct hci_conn *hcon; struct hci_chan *hchan; struct amp_mgr *mgr; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_LOGICAL_LINK_COMPLETE, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s log_handle 0x%4.4x phy_handle 0x%2.2x status 0x%2.2x", hdev->name, le16_to_cpu(ev->handle), ev->phy_handle, ev->status); @@ -5255,9 +5411,14 @@ static void hci_loglink_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_disconn_loglink_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_disconn_logical_link_complete *ev = (void *) skb->data; + struct hci_ev_disconn_logical_link_complete *ev; struct hci_chan *hchan; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_DISCONN_LOGICAL_LINK_COMPLETE, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s log handle 0x%4.4x status 0x%2.2x", hdev->name, le16_to_cpu(ev->handle), ev->status); @@ -5279,9 +5440,14 @@ unlock: static void hci_disconn_phylink_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_disconn_phy_link_complete *ev = (void *) skb->data; + struct hci_ev_disconn_phy_link_complete *ev; struct hci_conn *hcon; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_DISCONN_PHY_LINK_COMPLETE, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); if (ev->status) From e3f3a1aea8719ac76bb9443bc3abc975250e1f98 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:54:54 -0800 Subject: [PATCH 19/62] Bluetooth: HCI: Use skb_pull_data to parse Command Complete event This uses skb_pull_data to check the Command Complete events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_event.c | 904 ++++++++++++++++++++++++++++---------- 1 file changed, 663 insertions(+), 241 deletions(-) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 0266eab8a18c..09d7d997c4b1 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -57,12 +57,28 @@ static void *hci_ev_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, return data; } +static void *hci_cc_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, + u16 op, size_t len) +{ + void *data; + + data = skb_pull_data(skb, len); + if (!data) + bt_dev_err(hdev, "Malformed Command Complete: 0x%4.4x", op); + + return data; +} + static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb, u8 *new_status) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_INQUIRY_CANCEL, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); /* It is possible that we receive Inquiry Complete event right * before we receive Inquiry Cancel Command Complete event, in @@ -71,14 +87,14 @@ static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb, * we actually achieve what Inquiry Cancel wants to achieve, * which is to end the last Inquiry session. */ - if (status == 0x0c && !test_bit(HCI_INQUIRY, &hdev->flags)) { + if (rp->status == 0x0c && !test_bit(HCI_INQUIRY, &hdev->flags)) { bt_dev_warn(hdev, "Ignoring error of Inquiry Cancel command"); - status = 0x00; + rp->status = 0x00; } - *new_status = status; + *new_status = rp->status; - if (status) + if (rp->status) return; clear_bit(HCI_INQUIRY, &hdev->flags); @@ -99,11 +115,15 @@ static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb, static void hci_cc_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_PERIODIC_INQ, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; hci_dev_set_flag(hdev, HCI_PERIODIC_INQ); @@ -111,11 +131,15 @@ static void hci_cc_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_exit_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_EXIT_PERIODIC_INQ, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; hci_dev_clear_flag(hdev, HCI_PERIODIC_INQ); @@ -126,15 +150,26 @@ static void hci_cc_exit_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_remote_name_req_cancel(struct hci_dev *hdev, struct sk_buff *skb) { - BT_DBG("%s", hdev->name); + struct hci_ev_status *rp; + + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_REMOTE_NAME_REQ_CANCEL, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); } static void hci_cc_role_discovery(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_role_discovery *rp = (void *) skb->data; + struct hci_rp_role_discovery *rp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_ROLE_DISCOVERY, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -150,10 +185,14 @@ static void hci_cc_role_discovery(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_link_policy(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_link_policy *rp = (void *) skb->data; + struct hci_rp_read_link_policy *rp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LINK_POLICY, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -169,11 +208,15 @@ static void hci_cc_read_link_policy(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_write_link_policy(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_write_link_policy *rp = (void *) skb->data; + struct hci_rp_write_link_policy *rp; struct hci_conn *conn; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_LINK_POLICY, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -194,9 +237,14 @@ static void hci_cc_write_link_policy(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_def_link_policy(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_def_link_policy *rp = (void *) skb->data; + struct hci_rp_read_def_link_policy *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_DEF_LINK_POLICY, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -207,12 +255,17 @@ static void hci_cc_read_def_link_policy(struct hci_dev *hdev, static void hci_cc_write_def_link_policy(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_DEF_LINK_POLICY, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_DEF_LINK_POLICY); @@ -224,13 +277,17 @@ static void hci_cc_write_def_link_policy(struct hci_dev *hdev, static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_RESET, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); clear_bit(HCI_RESET, &hdev->flags); - if (status) + if (rp->status) return; /* Reset all non-persistent flags */ @@ -258,10 +315,15 @@ static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_stored_link_key(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_stored_link_key *rp = (void *)skb->data; + struct hci_rp_read_stored_link_key *rp; struct hci_cp_read_stored_link_key *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_STORED_LINK_KEY, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_READ_STORED_LINK_KEY); if (!sent) @@ -276,9 +338,14 @@ static void hci_cc_read_stored_link_key(struct hci_dev *hdev, static void hci_cc_delete_stored_link_key(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_delete_stored_link_key *rp = (void *)skb->data; + struct hci_rp_delete_stored_link_key *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_DELETE_STORED_LINK_KEY, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -291,10 +358,14 @@ static void hci_cc_delete_stored_link_key(struct hci_dev *hdev, static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_LOCAL_NAME, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LOCAL_NAME); if (!sent) @@ -303,8 +374,8 @@ static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_lock(hdev); if (hci_dev_test_flag(hdev, HCI_MGMT)) - mgmt_set_local_name_complete(hdev, sent, status); - else if (!status) + mgmt_set_local_name_complete(hdev, sent, rp->status); + else if (!rp->status) memcpy(hdev->dev_name, sent, HCI_MAX_NAME_LENGTH); hci_dev_unlock(hdev); @@ -312,9 +383,13 @@ static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_local_name(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_name *rp = (void *) skb->data; + struct hci_rp_read_local_name *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_NAME, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -326,10 +401,14 @@ static void hci_cc_read_local_name(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_write_auth_enable(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_AUTH_ENABLE, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_AUTH_ENABLE); if (!sent) @@ -337,7 +416,7 @@ static void hci_cc_write_auth_enable(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_lock(hdev); - if (!status) { + if (!rp->status) { __u8 param = *((__u8 *) sent); if (param == AUTH_ENABLED) @@ -347,20 +426,24 @@ static void hci_cc_write_auth_enable(struct hci_dev *hdev, struct sk_buff *skb) } if (hci_dev_test_flag(hdev, HCI_MGMT)) - mgmt_auth_enable_complete(hdev, status); + mgmt_auth_enable_complete(hdev, rp->status); hci_dev_unlock(hdev); } static void hci_cc_write_encrypt_mode(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; __u8 param; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_ENCRYPT_MODE, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_ENCRYPT_MODE); @@ -377,11 +460,15 @@ static void hci_cc_write_encrypt_mode(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_write_scan_enable(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; __u8 param; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SCAN_ENABLE, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SCAN_ENABLE); if (!sent) @@ -391,7 +478,7 @@ static void hci_cc_write_scan_enable(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_lock(hdev); - if (status) { + if (rp->status) { hdev->discov_timeout = 0; goto done; } @@ -412,13 +499,17 @@ done: static void hci_cc_set_event_filter(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *)skb->data); + struct hci_ev_status *rp; struct hci_cp_set_event_filter *cp; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SCAN_ENABLE, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_SET_EVENT_FLT); @@ -435,25 +526,33 @@ static void hci_cc_set_event_filter(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_class_of_dev *rp = (void *) skb->data; + struct hci_rp_read_class_of_dev *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_CLASS_OF_DEV, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; memcpy(hdev->dev_class, rp->dev_class, 3); - BT_DBG("%s class 0x%.2x%.2x%.2x", hdev->name, - hdev->dev_class[2], hdev->dev_class[1], hdev->dev_class[0]); + bt_dev_dbg(hdev, "class 0x%.2x%.2x%.2x", hdev->dev_class[2], + hdev->dev_class[1], hdev->dev_class[0]); } static void hci_cc_write_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_CLASS_OF_DEV, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_CLASS_OF_DEV); if (!sent) @@ -461,21 +560,25 @@ static void hci_cc_write_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_lock(hdev); - if (status == 0) + if (!rp->status) memcpy(hdev->dev_class, sent, 3); if (hci_dev_test_flag(hdev, HCI_MGMT)) - mgmt_set_class_of_dev_complete(hdev, sent, status); + mgmt_set_class_of_dev_complete(hdev, sent, rp->status); hci_dev_unlock(hdev); } static void hci_cc_read_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_voice_setting *rp = (void *) skb->data; + struct hci_rp_read_voice_setting *rp; __u16 setting; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_VOICE_SETTING, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -487,7 +590,7 @@ static void hci_cc_read_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) hdev->voice_setting = setting; - BT_DBG("%s voice setting 0x%4.4x", hdev->name, setting); + bt_dev_dbg(hdev, "voice setting 0x%4.4x", setting); if (hdev->notify) hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING); @@ -496,13 +599,18 @@ static void hci_cc_read_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_write_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; __u16 setting; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_VOICE_SETTING, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_VOICE_SETTING); @@ -516,7 +624,7 @@ static void hci_cc_write_voice_setting(struct hci_dev *hdev, hdev->voice_setting = setting; - BT_DBG("%s voice setting 0x%4.4x", hdev->name, setting); + bt_dev_dbg(hdev, "voice setting 0x%4.4x", setting); if (hdev->notify) hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING); @@ -525,24 +633,33 @@ static void hci_cc_write_voice_setting(struct hci_dev *hdev, static void hci_cc_read_num_supported_iac(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_num_supported_iac *rp = (void *) skb->data; + struct hci_rp_read_num_supported_iac *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_NUM_SUPPORTED_IAC, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; hdev->num_iac = rp->num_iac; - BT_DBG("%s num iac %d", hdev->name, hdev->num_iac); + bt_dev_dbg(hdev, "num iac %d", hdev->num_iac); } static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; struct hci_cp_write_ssp_mode *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SSP_MODE, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SSP_MODE); if (!sent) @@ -550,14 +667,14 @@ static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_lock(hdev); - if (!status) { + if (!rp->status) { if (sent->mode) hdev->features[1][0] |= LMP_HOST_SSP; else hdev->features[1][0] &= ~LMP_HOST_SSP; } - if (!status) { + if (!rp->status) { if (sent->mode) hci_dev_set_flag(hdev, HCI_SSP_ENABLED); else @@ -569,10 +686,14 @@ static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_write_sc_support(struct hci_dev *hdev, struct sk_buff *skb) { - u8 status = *((u8 *) skb->data); + struct hci_ev_status *rp; struct hci_cp_write_sc_support *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SC_SUPPORT, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SC_SUPPORT); if (!sent) @@ -580,14 +701,14 @@ static void hci_cc_write_sc_support(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_lock(hdev); - if (!status) { + if (!rp->status) { if (sent->support) hdev->features[1][0] |= LMP_HOST_SC; else hdev->features[1][0] &= ~LMP_HOST_SC; } - if (!hci_dev_test_flag(hdev, HCI_MGMT) && !status) { + if (!hci_dev_test_flag(hdev, HCI_MGMT) && !rp->status) { if (sent->support) hci_dev_set_flag(hdev, HCI_SC_ENABLED); else @@ -599,9 +720,13 @@ static void hci_cc_write_sc_support(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_local_version(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_version *rp = (void *) skb->data; + struct hci_rp_read_local_version *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_VERSION, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -619,9 +744,14 @@ static void hci_cc_read_local_version(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_local_commands(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_commands *rp = (void *) skb->data; + struct hci_rp_read_local_commands *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_COMMANDS, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -634,10 +764,15 @@ static void hci_cc_read_local_commands(struct hci_dev *hdev, static void hci_cc_read_auth_payload_timeout(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_auth_payload_to *rp = (void *)skb->data; + struct hci_rp_read_auth_payload_to *rp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_AUTH_PAYLOAD_TO, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -654,11 +789,15 @@ static void hci_cc_read_auth_payload_timeout(struct hci_dev *hdev, static void hci_cc_write_auth_payload_timeout(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_write_auth_payload_to *rp = (void *)skb->data; + struct hci_rp_write_auth_payload_to *rp; struct hci_conn *conn; void *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_AUTH_PAYLOAD_TO, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -679,9 +818,14 @@ static void hci_cc_write_auth_payload_timeout(struct hci_dev *hdev, static void hci_cc_read_local_features(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_features *rp = (void *) skb->data; + struct hci_rp_read_local_features *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_FEATURES, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -729,9 +873,14 @@ static void hci_cc_read_local_features(struct hci_dev *hdev, static void hci_cc_read_local_ext_features(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_ext_features *rp = (void *) skb->data; + struct hci_rp_read_local_ext_features *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_EXT_FEATURES, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -746,9 +895,14 @@ static void hci_cc_read_local_ext_features(struct hci_dev *hdev, static void hci_cc_read_flow_control_mode(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_flow_control_mode *rp = (void *) skb->data; + struct hci_rp_read_flow_control_mode *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_FLOW_CONTROL_MODE, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -758,9 +912,13 @@ static void hci_cc_read_flow_control_mode(struct hci_dev *hdev, static void hci_cc_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_buffer_size *rp = (void *) skb->data; + struct hci_rp_read_buffer_size *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_BUFFER_SIZE, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -784,9 +942,13 @@ static void hci_cc_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_bd_addr(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_bd_addr *rp = (void *) skb->data; + struct hci_rp_read_bd_addr *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_BD_ADDR, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -801,9 +963,14 @@ static void hci_cc_read_bd_addr(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_local_pairing_opts(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_pairing_opts *rp = (void *) skb->data; + struct hci_rp_read_local_pairing_opts *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_PAIRING_OPTS, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -818,9 +985,14 @@ static void hci_cc_read_local_pairing_opts(struct hci_dev *hdev, static void hci_cc_read_page_scan_activity(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_page_scan_activity *rp = (void *) skb->data; + struct hci_rp_read_page_scan_activity *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_PAGE_SCAN_ACTIVITY, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -834,12 +1006,17 @@ static void hci_cc_read_page_scan_activity(struct hci_dev *hdev, static void hci_cc_write_page_scan_activity(struct hci_dev *hdev, struct sk_buff *skb) { - u8 status = *((u8 *) skb->data); + struct hci_ev_status *rp; struct hci_cp_write_page_scan_activity *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_PAGE_SCAN_ACTIVITY, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_PAGE_SCAN_ACTIVITY); @@ -853,9 +1030,14 @@ static void hci_cc_write_page_scan_activity(struct hci_dev *hdev, static void hci_cc_read_page_scan_type(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_page_scan_type *rp = (void *) skb->data; + struct hci_rp_read_page_scan_type *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_PAGE_SCAN_TYPE, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -867,12 +1049,17 @@ static void hci_cc_read_page_scan_type(struct hci_dev *hdev, static void hci_cc_write_page_scan_type(struct hci_dev *hdev, struct sk_buff *skb) { - u8 status = *((u8 *) skb->data); + struct hci_ev_status *rp; u8 *type; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_PAGE_SCAN_TYPE, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; type = hci_sent_cmd_data(hdev, HCI_OP_WRITE_PAGE_SCAN_TYPE); @@ -883,9 +1070,14 @@ static void hci_cc_write_page_scan_type(struct hci_dev *hdev, static void hci_cc_read_data_block_size(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_data_block_size *rp = (void *) skb->data; + struct hci_rp_read_data_block_size *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_DATA_BLOCK_SIZE, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -902,15 +1094,18 @@ static void hci_cc_read_data_block_size(struct hci_dev *hdev, static void hci_cc_read_clock(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_clock *rp = (void *) skb->data; + struct hci_rp_read_clock *rp; struct hci_cp_read_clock *cp; struct hci_conn *conn; BT_DBG("%s", hdev->name); - if (skb->len < sizeof(*rp)) + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_CLOCK, sizeof(*rp)); + if (!rp) return; + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + if (rp->status) return; @@ -938,9 +1133,14 @@ unlock: static void hci_cc_read_local_amp_info(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_amp_info *rp = (void *) skb->data; + struct hci_rp_read_local_amp_info *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_AMP_INFO, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -960,9 +1160,14 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev, static void hci_cc_read_inq_rsp_tx_power(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_inq_rsp_tx_power *rp = (void *) skb->data; + struct hci_rp_read_inq_rsp_tx_power *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_INQ_RSP_TX_POWER, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -973,9 +1178,14 @@ static void hci_cc_read_inq_rsp_tx_power(struct hci_dev *hdev, static void hci_cc_read_def_err_data_reporting(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_def_err_data_reporting *rp = (void *)skb->data; + struct hci_rp_read_def_err_data_reporting *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_DEF_ERR_DATA_REPORTING, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -986,12 +1196,17 @@ static void hci_cc_read_def_err_data_reporting(struct hci_dev *hdev, static void hci_cc_write_def_err_data_reporting(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *)skb->data); + struct hci_ev_status *rp; struct hci_cp_write_def_err_data_reporting *cp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_DEF_ERR_DATA_REPORTING, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_WRITE_DEF_ERR_DATA_REPORTING); @@ -1003,11 +1218,15 @@ static void hci_cc_write_def_err_data_reporting(struct hci_dev *hdev, static void hci_cc_pin_code_reply(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_pin_code_reply *rp = (void *) skb->data; + struct hci_rp_pin_code_reply *rp; struct hci_cp_pin_code_reply *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_PIN_CODE_REPLY, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); hci_dev_lock(hdev); @@ -1031,9 +1250,13 @@ unlock: static void hci_cc_pin_code_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_pin_code_neg_reply *rp = (void *) skb->data; + struct hci_rp_pin_code_neg_reply *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_PIN_CODE_NEG_REPLY, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); hci_dev_lock(hdev); @@ -1047,9 +1270,14 @@ static void hci_cc_pin_code_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_le_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_buffer_size *rp = (void *) skb->data; + struct hci_rp_le_read_buffer_size *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_BUFFER_SIZE, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1065,7 +1293,12 @@ static void hci_cc_le_read_buffer_size(struct hci_dev *hdev, static void hci_cc_le_read_local_features(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_local_features *rp = (void *) skb->data; + struct hci_rp_le_read_local_features *rp; + + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_LOCAL_FEATURES, + sizeof(*rp)); + if (!rp) + return; BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); @@ -1078,9 +1311,14 @@ static void hci_cc_le_read_local_features(struct hci_dev *hdev, static void hci_cc_le_read_adv_tx_power(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_adv_tx_power *rp = (void *) skb->data; + struct hci_rp_le_read_adv_tx_power *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_ADV_TX_POWER, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1090,9 +1328,13 @@ static void hci_cc_le_read_adv_tx_power(struct hci_dev *hdev, static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + struct hci_rp_user_confirm_reply *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_USER_CONFIRM_REPLY, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); hci_dev_lock(hdev); @@ -1106,9 +1348,14 @@ static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_user_confirm_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + struct hci_rp_user_confirm_reply *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_USER_CONFIRM_NEG_REPLY, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); hci_dev_lock(hdev); @@ -1121,9 +1368,13 @@ static void hci_cc_user_confirm_neg_reply(struct hci_dev *hdev, static void hci_cc_user_passkey_reply(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + struct hci_rp_user_confirm_reply *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_USER_PASSKEY_REPLY, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); hci_dev_lock(hdev); @@ -1137,9 +1388,13 @@ static void hci_cc_user_passkey_reply(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_user_passkey_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + struct hci_rp_user_confirm_reply *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_USER_PASSKEY_NEG_REPLY, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); hci_dev_lock(hdev); @@ -1153,27 +1408,39 @@ static void hci_cc_user_passkey_neg_reply(struct hci_dev *hdev, static void hci_cc_read_local_oob_data(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_oob_data *rp = (void *) skb->data; + struct hci_rp_read_local_oob_data *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_OOB_DATA, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); } static void hci_cc_read_local_oob_ext_data(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_local_oob_ext_data *rp = (void *) skb->data; + struct hci_rp_read_local_oob_ext_data *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_OOB_EXT_DATA, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); } static void hci_cc_le_set_random_addr(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; bdaddr_t *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_RANDOM_ADDR, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_RANDOM_ADDR); @@ -1195,12 +1462,16 @@ static void hci_cc_le_set_random_addr(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_le_set_default_phy(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; struct hci_cp_le_set_default_phy *cp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_DEFAULT_PHY, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_DEFAULT_PHY); @@ -1218,11 +1489,18 @@ static void hci_cc_le_set_default_phy(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_le_set_adv_set_random_addr(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; struct hci_cp_le_set_adv_set_rand_addr *cp; struct adv_info *adv; - if (status) + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_ADV_SET_RAND_ADDR, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_SET_RAND_ADDR); @@ -1251,11 +1529,17 @@ static void hci_cc_le_set_adv_set_random_addr(struct hci_dev *hdev, static void hci_cc_le_remove_adv_set(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *)skb->data); + struct hci_ev_status *rp; u8 *instance; int err; - if (status) + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_REMOVE_ADV_SET, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; instance = hci_sent_cmd_data(hdev, HCI_OP_LE_REMOVE_ADV_SET); @@ -1274,11 +1558,17 @@ static void hci_cc_le_remove_adv_set(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_le_clear_adv_sets(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *)skb->data); + struct hci_ev_status *rp; struct adv_info *adv, *n; int err; - if (status) + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_CLEAR_ADV_SETS, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; if (!hci_sent_cmd_data(hdev, HCI_OP_LE_CLEAR_ADV_SETS)) @@ -1301,9 +1591,14 @@ static void hci_cc_le_clear_adv_sets(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_le_read_transmit_power(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_transmit_power *rp = (void *)skb->data; + struct hci_rp_le_read_transmit_power *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_TRANSMIT_POWER, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1314,11 +1609,16 @@ static void hci_cc_le_read_transmit_power(struct hci_dev *hdev, static void hci_cc_le_set_adv_enable(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 *sent, status = *((__u8 *) skb->data); + struct hci_ev_status *rp; + __u8 *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_ADV_ENABLE, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_ENABLE); @@ -1352,12 +1652,17 @@ static void hci_cc_le_set_ext_adv_enable(struct hci_dev *hdev, { struct hci_cp_le_set_ext_adv_enable *cp; struct hci_cp_ext_adv_set *set; - __u8 status = *((__u8 *) skb->data); struct adv_info *adv = NULL, *n; + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_EXT_ADV_ENABLE, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_EXT_ADV_ENABLE); @@ -1414,11 +1719,15 @@ unlock: static void hci_cc_le_set_scan_param(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_set_scan_param *cp; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_SCAN_PARAM, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_SCAN_PARAM); @@ -1436,12 +1745,17 @@ static void hci_cc_le_set_ext_scan_param(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_set_ext_scan_params *cp; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; struct hci_cp_le_scan_phy_params *phy_param; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_EXT_SCAN_PARAMS, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_EXT_SCAN_PARAMS); @@ -1544,11 +1858,16 @@ static void hci_cc_le_set_scan_enable(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_set_scan_enable *cp; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_SCAN_ENABLE, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_SCAN_ENABLE); @@ -1562,11 +1881,16 @@ static void hci_cc_le_set_ext_scan_enable(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_set_ext_scan_enable *cp; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_EXT_SCAN_ENABLE, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_EXT_SCAN_ENABLE); @@ -1579,10 +1903,15 @@ static void hci_cc_le_set_ext_scan_enable(struct hci_dev *hdev, static void hci_cc_le_read_num_adv_sets(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_num_supported_adv_sets *rp = (void *) skb->data; + struct hci_rp_le_read_num_supported_adv_sets *rp; - BT_DBG("%s status 0x%2.2x No of Adv sets %u", hdev->name, rp->status, - rp->num_of_sets); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_NUM_SUPPORTED_ADV_SETS, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x No of Adv sets %u", rp->status, + rp->num_of_sets); if (rp->status) return; @@ -1593,9 +1922,14 @@ static void hci_cc_le_read_num_adv_sets(struct hci_dev *hdev, static void hci_cc_le_read_accept_list_size(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_accept_list_size *rp = (void *)skb->data; + struct hci_rp_le_read_accept_list_size *rp; - BT_DBG("%s status 0x%2.2x size %u", hdev->name, rp->status, rp->size); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_ACCEPT_LIST_SIZE, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x size %u", rp->status, rp->size); if (rp->status) return; @@ -1606,11 +1940,16 @@ static void hci_cc_le_read_accept_list_size(struct hci_dev *hdev, static void hci_cc_le_clear_accept_list(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_CLEAR_ACCEPT_LIST, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; hci_bdaddr_list_clear(&hdev->le_accept_list); @@ -1620,11 +1959,16 @@ static void hci_cc_le_add_to_accept_list(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_add_to_accept_list *sent; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_ADD_TO_ACCEPT_LIST, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_ADD_TO_ACCEPT_LIST); @@ -1639,11 +1983,16 @@ static void hci_cc_le_del_from_accept_list(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_del_from_accept_list *sent; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_DEL_FROM_ACCEPT_LIST, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_DEL_FROM_ACCEPT_LIST); @@ -1657,9 +2006,14 @@ static void hci_cc_le_del_from_accept_list(struct hci_dev *hdev, static void hci_cc_le_read_supported_states(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_supported_states *rp = (void *) skb->data; + struct hci_rp_le_read_supported_states *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_SUPPORTED_STATES, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1670,9 +2024,14 @@ static void hci_cc_le_read_supported_states(struct hci_dev *hdev, static void hci_cc_le_read_def_data_len(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_def_data_len *rp = (void *) skb->data; + struct hci_rp_le_read_def_data_len *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_DEF_DATA_LEN, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1685,11 +2044,16 @@ static void hci_cc_le_write_def_data_len(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_write_def_data_len *sent; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_WRITE_DEF_DATA_LEN, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_WRITE_DEF_DATA_LEN); @@ -1704,11 +2068,16 @@ static void hci_cc_le_add_to_resolv_list(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_add_to_resolv_list *sent; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_ADD_TO_RESOLV_LIST, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_ADD_TO_RESOLV_LIST); @@ -1724,11 +2093,16 @@ static void hci_cc_le_del_from_resolv_list(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_del_from_resolv_list *sent; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_DEL_FROM_RESOLV_LIST, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_DEL_FROM_RESOLV_LIST); @@ -1742,11 +2116,16 @@ static void hci_cc_le_del_from_resolv_list(struct hci_dev *hdev, static void hci_cc_le_clear_resolv_list(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_CLEAR_RESOLV_LIST, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; hci_bdaddr_list_clear(&hdev->le_resolv_list); @@ -1755,9 +2134,14 @@ static void hci_cc_le_clear_resolv_list(struct hci_dev *hdev, static void hci_cc_le_read_resolv_list_size(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_resolv_list_size *rp = (void *) skb->data; + struct hci_rp_le_read_resolv_list_size *rp; - BT_DBG("%s status 0x%2.2x size %u", hdev->name, rp->status, rp->size); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_RESOLV_LIST_SIZE, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x size %u", rp->status, rp->size); if (rp->status) return; @@ -1768,11 +2152,17 @@ static void hci_cc_le_read_resolv_list_size(struct hci_dev *hdev, static void hci_cc_le_set_addr_resolution_enable(struct hci_dev *hdev, struct sk_buff *skb) { - __u8 *sent, status = *((__u8 *) skb->data); + struct hci_ev_status *rp; + __u8 *sent; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_ADDR_RESOLV_ENABLE, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADDR_RESOLV_ENABLE); @@ -1792,9 +2182,14 @@ static void hci_cc_le_set_addr_resolution_enable(struct hci_dev *hdev, static void hci_cc_le_read_max_data_len(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_read_max_data_len *rp = (void *) skb->data; + struct hci_rp_le_read_max_data_len *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_MAX_DATA_LEN, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1809,11 +2204,16 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_write_le_host_supported *sent; - __u8 status = *((__u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_LE_HOST_SUPPORTED, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED); @@ -1842,11 +2242,15 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev, static void hci_cc_set_adv_param(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_le_set_adv_param *cp; - u8 status = *((u8 *) skb->data); + struct hci_ev_status *rp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_ADV_PARAM, sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_PARAM); @@ -1860,11 +2264,16 @@ static void hci_cc_set_adv_param(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_set_ext_adv_param(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_le_set_ext_adv_params *rp = (void *) skb->data; + struct hci_rp_le_set_ext_adv_params *rp; struct hci_cp_le_set_ext_adv_params *cp; struct adv_info *adv_instance; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_EXT_ADV_PARAMS, + sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1891,10 +2300,14 @@ static void hci_cc_set_ext_adv_param(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_rssi(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_rp_read_rssi *rp = (void *) skb->data; + struct hci_rp_read_rssi *rp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_RSSI, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1911,10 +2324,14 @@ static void hci_cc_read_rssi(struct hci_dev *hdev, struct sk_buff *skb) static void hci_cc_read_tx_power(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_cp_read_tx_power *sent; - struct hci_rp_read_tx_power *rp = (void *) skb->data; + struct hci_rp_read_tx_power *rp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_TX_POWER, sizeof(*rp)); + if (!rp) + return; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) return; @@ -1944,12 +2361,17 @@ unlock: static void hci_cc_write_ssp_debug_mode(struct hci_dev *hdev, struct sk_buff *skb) { - u8 status = *((u8 *) skb->data); + struct hci_ev_status *rp; u8 *mode; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SSP_DEBUG_MODE, + sizeof(*rp)); + if (!rp) + return; - if (status) + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) return; mode = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SSP_DEBUG_MODE); @@ -3505,12 +3927,14 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb, hci_req_complete_t *req_complete, hci_req_complete_skb_t *req_complete_skb) { - struct hci_ev_cmd_complete *ev = (void *) skb->data; + struct hci_ev_cmd_complete *ev; + + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CMD_COMPLETE, sizeof(*ev)); + if (!ev) + return; *opcode = __le16_to_cpu(ev->opcode); - *status = skb->data[sizeof(*ev)]; - - skb_pull(skb, sizeof(*ev)); + *status = skb->data[0]; switch (*opcode) { case HCI_OP_INQUIRY_CANCEL: @@ -6466,13 +6890,9 @@ static bool hci_get_cmd_complete(struct hci_dev *hdev, u16 opcode, if (!skb) return false; - if (skb->len < sizeof(*hdr)) { - bt_dev_err(hdev, "too short HCI event"); + hdr = hci_ev_skb_pull(hdev, skb, event, sizeof(*hdr)); + if (!hdr) return false; - } - - hdr = (void *) skb->data; - skb_pull(skb, HCI_EVENT_HDR_SIZE); if (event) { if (hdr->evt != event) @@ -6492,13 +6912,9 @@ static bool hci_get_cmd_complete(struct hci_dev *hdev, u16 opcode, return false; } - if (skb->len < sizeof(*ev)) { - bt_dev_err(hdev, "too short cmd_complete event"); + ev = hci_cc_skb_pull(hdev, skb, opcode, sizeof(*ev)); + if (!ev) return false; - } - - ev = (void *) skb->data; - skb_pull(skb, sizeof(*ev)); if (opcode != __le16_to_cpu(ev->opcode)) { BT_DBG("opcode doesn't match (0x%2.2x != 0x%2.2x)", opcode, @@ -6584,9 +7000,15 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb) hci_req_complete_t req_complete = NULL; hci_req_complete_skb_t req_complete_skb = NULL; struct sk_buff *orig_skb = NULL; - u8 status = 0, event = hdr->evt, req_evt = 0; + u8 status = 0, event, req_evt = 0; u16 opcode = HCI_OP_NOP; + if (skb->len < sizeof(*hdr)) { + bt_dev_err(hdev, "Malformed HCI Event"); + goto done; + } + + event = hdr->evt; if (!event) { bt_dev_warn(hdev, "Received unexpected HCI Event 00000000"); goto done; From aadc3d2f42a5bfcec597bfd0d997e3982f740846 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:54:55 -0800 Subject: [PATCH 20/62] Bluetooth: HCI: Use skb_pull_data to parse Number of Complete Packets event This uses skb_pull_data to check the Number of Complete Packets events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 2 +- net/bluetooth/hci_event.c | 20 +++++++++++--------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index ba89b078ceb5..3f57fd677b67 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -2139,7 +2139,7 @@ struct hci_comp_pkts_info { } __packed; struct hci_ev_num_comp_pkts { - __u8 num_hndl; + __u8 num; struct hci_comp_pkts_info handles[]; } __packed; diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 09d7d997c4b1..b27a4ad647ca 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -4465,23 +4465,25 @@ static void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_num_comp_pkts *ev = (void *) skb->data; + struct hci_ev_num_comp_pkts *ev; int i; + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_NUM_COMP_PKTS, sizeof(*ev)); + if (!ev) + return; + + if (!hci_ev_skb_pull(hdev, skb, HCI_EV_NUM_COMP_PKTS, + flex_array_size(ev, handles, ev->num))) + return; + if (hdev->flow_ctl_mode != HCI_FLOW_CTL_MODE_PACKET_BASED) { bt_dev_err(hdev, "wrong event for mode %d", hdev->flow_ctl_mode); return; } - if (skb->len < sizeof(*ev) || - skb->len < struct_size(ev, handles, ev->num_hndl)) { - BT_DBG("%s bad parameters", hdev->name); - return; - } + BT_DBG("%s num %d", hdev->name, ev->num); - BT_DBG("%s num_hndl %d", hdev->name, ev->num_hndl); - - for (i = 0; i < ev->num_hndl; i++) { + for (i = 0; i < ev->num; i++) { struct hci_comp_pkts_info *info = &ev->handles[i]; struct hci_conn *conn; __u16 handle, count; From 27d9eb4bcac16446ddbbea8860c11720b7afa891 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:54:56 -0800 Subject: [PATCH 21/62] Bluetooth: HCI: Use skb_pull_data to parse Inquiry Result event This uses skb_pull_data to check the Inquiry Result events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 5 +++++ net/bluetooth/hci_event.c | 19 ++++++++++++++----- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 3f57fd677b67..55466bfb972a 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -2028,6 +2028,11 @@ struct inquiry_info { __le16 clock_offset; } __packed; +struct hci_ev_inquiry_result { + __u8 num; + struct inquiry_info info[]; +}; + #define HCI_EV_CONN_COMPLETE 0x03 struct hci_ev_conn_complete { __u8 status; diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index b27a4ad647ca..0bf062f6f262 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -3180,13 +3180,21 @@ unlock: static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) { + struct hci_ev_inquiry_result *ev; struct inquiry_data data; - struct inquiry_info *info = (void *) (skb->data + 1); - int num_rsp = *((__u8 *) skb->data); + int i; - BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_INQUIRY_RESULT, sizeof(*ev)); + if (!ev) + return; - if (!num_rsp || skb->len < num_rsp * sizeof(*info) + 1) + if (!hci_ev_skb_pull(hdev, skb, HCI_EV_INQUIRY_RESULT, + flex_array_size(ev, info, ev->num))) + return; + + BT_DBG("%s num %d", hdev->name, ev->num); + + if (!ev->num) return; if (hci_dev_test_flag(hdev, HCI_PERIODIC_INQ)) @@ -3194,7 +3202,8 @@ static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_lock(hdev); - for (; num_rsp; num_rsp--, info++) { + for (i = 0; i < ev->num; i++) { + struct inquiry_info *info = &ev->info[i]; u32 flags; bacpy(&data.bdaddr, &info->bdaddr); From 8d08d324fdcb7729f22b236a3e20fa37a8a29e43 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:54:57 -0800 Subject: [PATCH 22/62] Bluetooth: HCI: Use skb_pull_data to parse Inquiry Result with RSSI event This uses skb_pull_data to check the Inquiry Result with RSSI events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 12 +++++++++-- net/bluetooth/hci_event.c | 40 +++++++++++++++++++++---------------- 2 files changed, 33 insertions(+), 19 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 55466bfb972a..d25afd92a46e 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -2194,7 +2194,7 @@ struct hci_ev_pscan_rep_mode { } __packed; #define HCI_EV_INQUIRY_RESULT_WITH_RSSI 0x22 -struct inquiry_info_with_rssi { +struct inquiry_info_rssi { bdaddr_t bdaddr; __u8 pscan_rep_mode; __u8 pscan_period_mode; @@ -2202,7 +2202,7 @@ struct inquiry_info_with_rssi { __le16 clock_offset; __s8 rssi; } __packed; -struct inquiry_info_with_rssi_and_pscan_mode { +struct inquiry_info_rssi_pscan { bdaddr_t bdaddr; __u8 pscan_rep_mode; __u8 pscan_period_mode; @@ -2211,6 +2211,14 @@ struct inquiry_info_with_rssi_and_pscan_mode { __le16 clock_offset; __s8 rssi; } __packed; +struct hci_ev_inquiry_result_rssi { + __u8 num; + struct inquiry_info_rssi info[]; +} __packed; +struct hci_ev_inquiry_result_rssi_pscan { + __u8 num; + struct inquiry_info_rssi_pscan info[]; +} __packed; #define HCI_EV_REMOTE_EXT_FEATURES 0x23 struct hci_ev_remote_ext_features { diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 0bf062f6f262..6560dca8c5ce 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -4921,12 +4921,21 @@ static void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct sk_buff *skb) { + union { + struct hci_ev_inquiry_result_rssi *res1; + struct hci_ev_inquiry_result_rssi_pscan *res2; + } *ev; struct inquiry_data data; - int num_rsp = *((__u8 *) skb->data); + int i; - BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_INQUIRY_RESULT_WITH_RSSI, + sizeof(*ev)); + if (!ev) + return; - if (!num_rsp) + BT_DBG("%s num_rsp %d", hdev->name, ev->res1->num); + + if (!ev->res1->num) return; if (hci_dev_test_flag(hdev, HCI_PERIODIC_INQ)) @@ -4934,16 +4943,13 @@ static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, hci_dev_lock(hdev); - if ((skb->len - 1) / num_rsp != sizeof(struct inquiry_info_with_rssi)) { - struct inquiry_info_with_rssi_and_pscan_mode *info; - info = (void *) (skb->data + 1); + if (skb->len == flex_array_size(ev, res2->info, ev->res2->num)) { + struct inquiry_info_rssi_pscan *info; - if (skb->len < num_rsp * sizeof(*info) + 1) - goto unlock; - - for (; num_rsp; num_rsp--, info++) { + for (i = 0; i < ev->res2->num; i++) { u32 flags; + info = &ev->res2->info[i]; bacpy(&data.bdaddr, &info->bdaddr); data.pscan_rep_mode = info->pscan_rep_mode; data.pscan_period_mode = info->pscan_period_mode; @@ -4959,15 +4965,13 @@ static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, info->dev_class, info->rssi, flags, NULL, 0, NULL, 0); } - } else { - struct inquiry_info_with_rssi *info = (void *) (skb->data + 1); + } else if (skb->len == flex_array_size(ev, res1->info, ev->res1->num)) { + struct inquiry_info_rssi *info; - if (skb->len < num_rsp * sizeof(*info) + 1) - goto unlock; - - for (; num_rsp; num_rsp--, info++) { + for (i = 0; i < ev->res1->num; i++) { u32 flags; + info = &ev->res1->info[i]; bacpy(&data.bdaddr, &info->bdaddr); data.pscan_rep_mode = info->pscan_rep_mode; data.pscan_period_mode = info->pscan_period_mode; @@ -4983,9 +4987,11 @@ static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, info->dev_class, info->rssi, flags, NULL, 0, NULL, 0); } + } else { + bt_dev_err(hdev, "Malformed HCI Event: 0x%2.2x", + HCI_EV_INQUIRY_RESULT_WITH_RSSI); } -unlock: hci_dev_unlock(hdev); } From 70a6b8de6af5c3e517bfd2ce8dccbd65c3f5bb4f Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:54:58 -0800 Subject: [PATCH 23/62] Bluetooth: HCI: Use skb_pull_data to parse Extended Inquiry Result event This uses skb_pull_data to check the Extended Inquiry Result events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 5 +++++ net/bluetooth/hci_event.c | 20 +++++++++++++++----- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index d25afd92a46e..9e721e6efef3 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -2273,6 +2273,11 @@ struct extended_inquiry_info { __u8 data[240]; } __packed; +struct hci_ev_ext_inquiry_result { + __u8 num; + struct extended_inquiry_info info[]; +} __packed; + #define HCI_EV_KEY_REFRESH_COMPLETE 0x30 struct hci_ev_key_refresh_complete { __u8 status; diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 6560dca8c5ce..89c263c1883f 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -5186,14 +5186,23 @@ static inline size_t eir_get_length(u8 *eir, size_t eir_len) static void hci_extended_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) { + struct hci_ev_ext_inquiry_result *ev; struct inquiry_data data; - struct extended_inquiry_info *info = (void *) (skb->data + 1); - int num_rsp = *((__u8 *) skb->data); size_t eir_len; + int i; - BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_EXTENDED_INQUIRY_RESULT, + sizeof(*ev)); + if (!ev) + return; - if (!num_rsp || skb->len < num_rsp * sizeof(*info) + 1) + if (!hci_ev_skb_pull(hdev, skb, HCI_EV_EXTENDED_INQUIRY_RESULT, + flex_array_size(ev, info, ev->num))) + return; + + BT_DBG("%s num %d", hdev->name, ev->num); + + if (!ev->num) return; if (hci_dev_test_flag(hdev, HCI_PERIODIC_INQ)) @@ -5201,7 +5210,8 @@ static void hci_extended_inquiry_result_evt(struct hci_dev *hdev, hci_dev_lock(hdev); - for (; num_rsp; num_rsp--, info++) { + for (i = 0; i < ev->num; i++) { + struct extended_inquiry_info *info = &ev->info[i]; u32 flags; bool name_known; From 12cfe4176ad67e4b31be89dde63220c1569794c4 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:54:59 -0800 Subject: [PATCH 24/62] Bluetooth: HCI: Use skb_pull_data to parse LE Metaevents This uses skb_pull_data to check the LE Metaevents received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_event.c | 75 +++++++++++++++++++++++++++++++++------ 1 file changed, 64 insertions(+), 11 deletions(-) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 89c263c1883f..d30e77f66376 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -69,6 +69,18 @@ static void *hci_cc_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, return data; } +static void *hci_le_ev_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, + u8 ev, size_t len) +{ + void *data; + + data = skb_pull_data(skb, len); + if (!data) + bt_dev_err(hdev, "Malformed LE Event: 0x%2.2x", ev); + + return data; +} + static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb, u8 *new_status) { @@ -6119,7 +6131,12 @@ unlock: static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_le_conn_complete *ev = (void *) skb->data; + struct hci_ev_le_conn_complete *ev; + + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_CONN_COMPLETE, + sizeof(*ev)); + if (!ev) + return; BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); @@ -6133,7 +6150,12 @@ static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_le_enh_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_le_enh_conn_complete *ev = (void *) skb->data; + struct hci_ev_le_enh_conn_complete *ev; + + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_ENHANCED_CONN_COMPLETE, + sizeof(*ev)); + if (!ev) + return; BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); @@ -6146,10 +6168,15 @@ static void hci_le_enh_conn_complete_evt(struct hci_dev *hdev, static void hci_le_ext_adv_term_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_evt_le_ext_adv_set_term *ev = (void *) skb->data; + struct hci_evt_le_ext_adv_set_term *ev; struct hci_conn *conn; struct adv_info *adv, *n; + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_EXT_ADV_SET_TERM, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); adv = hci_find_adv_instance(hdev, ev->handle); @@ -6211,9 +6238,14 @@ static void hci_le_ext_adv_term_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_le_conn_update_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_le_conn_update_complete *ev = (void *) skb->data; + struct hci_ev_le_conn_update_complete *ev; struct hci_conn *conn; + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_CONN_UPDATE_COMPLETE, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); if (ev->status) @@ -6636,9 +6668,14 @@ static void hci_le_ext_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) static void hci_le_remote_feat_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_le_remote_feat_complete *ev = (void *)skb->data; + struct hci_ev_le_remote_feat_complete *ev; struct hci_conn *conn; + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_EXT_ADV_REPORT, + sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); hci_dev_lock(hdev); @@ -6677,12 +6714,16 @@ static void hci_le_remote_feat_complete_evt(struct hci_dev *hdev, static void hci_le_ltk_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_le_ltk_req *ev = (void *) skb->data; + struct hci_ev_le_ltk_req *ev; struct hci_cp_le_ltk_reply cp; struct hci_cp_le_ltk_neg_reply neg; struct hci_conn *conn; struct smp_ltk *ltk; + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_LTK_REQ, sizeof(*ev)); + if (!ev) + return; + BT_DBG("%s handle 0x%4.4x", hdev->name, __le16_to_cpu(ev->handle)); hci_dev_lock(hdev); @@ -6754,11 +6795,16 @@ static void send_conn_param_neg_reply(struct hci_dev *hdev, u16 handle, static void hci_le_remote_conn_param_req_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_le_remote_conn_param_req *ev = (void *) skb->data; + struct hci_ev_le_remote_conn_param_req *ev; struct hci_cp_le_conn_param_req_reply cp; struct hci_conn *hcon; u16 handle, min, max, latency, timeout; + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_REMOTE_CONN_PARAM_REQ, + sizeof(*ev)); + if (!ev) + return; + handle = le16_to_cpu(ev->handle); min = le16_to_cpu(ev->interval_min); max = le16_to_cpu(ev->interval_max); @@ -6831,9 +6877,14 @@ static void hci_le_direct_adv_report_evt(struct hci_dev *hdev, static void hci_le_phy_update_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_le_phy_update_complete *ev = (void *) skb->data; + struct hci_ev_le_phy_update_complete *ev; struct hci_conn *conn; + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_PHY_UPDATE_COMPLETE, + sizeof(*ev)); + if (ev) + return; + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); if (ev->status) @@ -6854,11 +6905,13 @@ unlock: static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb) { - struct hci_ev_le_meta *le_ev = (void *) skb->data; + struct hci_ev_le_meta *ev; - skb_pull(skb, sizeof(*le_ev)); + ev = hci_ev_skb_pull(hdev, skb, HCI_EV_LE_META, sizeof(*ev)); + if (!ev) + return; - switch (le_ev->subevent) { + switch (ev->subevent) { case HCI_EV_LE_CONN_COMPLETE: hci_le_conn_complete_evt(hdev, skb); break; From 47afe93c913a4cd0143667b59ba622086a2acfce Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:55:00 -0800 Subject: [PATCH 25/62] Bluetooth: HCI: Use skb_pull_data to parse LE Advertising Report event This uses skb_pull_data to check the LE Advertising Report events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 7 ++++++- net/bluetooth/hci_event.c | 39 +++++++++++++++++++++++-------------- 2 files changed, 30 insertions(+), 16 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 9e721e6efef3..c005b1ccdbc5 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -2445,13 +2445,18 @@ struct hci_ev_le_conn_complete { #define HCI_EV_LE_ADVERTISING_REPORT 0x02 struct hci_ev_le_advertising_info { - __u8 evt_type; + __u8 type; __u8 bdaddr_type; bdaddr_t bdaddr; __u8 length; __u8 data[]; } __packed; +struct hci_ev_le_advertising_report { + __u8 num; + struct hci_ev_le_advertising_info info[]; +} __packed; + #define HCI_EV_LE_CONN_UPDATE_COMPLETE 0x03 struct hci_ev_le_conn_update_complete { __u8 status; diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index d30e77f66376..42ffd5df6d4b 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -6564,31 +6564,40 @@ static void process_adv_report(struct hci_dev *hdev, u8 type, bdaddr_t *bdaddr, static void hci_le_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) { - u8 num_reports = skb->data[0]; - void *ptr = &skb->data[1]; + struct hci_ev_le_advertising_report *ev; + + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_ADVERTISING_REPORT, + sizeof(*ev)); + if (!ev) + return; + + if (!ev->num) + return; hci_dev_lock(hdev); - while (num_reports--) { - struct hci_ev_le_advertising_info *ev = ptr; + while (ev->num--) { + struct hci_ev_le_advertising_info *info; s8 rssi; - if (ptr > (void *)skb_tail_pointer(skb) - sizeof(*ev)) { - bt_dev_err(hdev, "Malicious advertising data."); + info = hci_le_ev_skb_pull(hdev, skb, + HCI_EV_LE_ADVERTISING_REPORT, + sizeof(*info)); + if (!info) break; - } - if (ev->length <= HCI_MAX_AD_LENGTH && - ev->data + ev->length <= skb_tail_pointer(skb)) { - rssi = ev->data[ev->length]; - process_adv_report(hdev, ev->evt_type, &ev->bdaddr, - ev->bdaddr_type, NULL, 0, rssi, - ev->data, ev->length, false); + if (!hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_ADVERTISING_REPORT, + info->length + 1)) + break; + + if (info->length <= HCI_MAX_AD_LENGTH) { + rssi = info->data[info->length]; + process_adv_report(hdev, info->type, &info->bdaddr, + info->bdaddr_type, NULL, 0, rssi, + info->data, info->length, false); } else { bt_dev_err(hdev, "Dropping invalid advertising data"); } - - ptr += sizeof(*ev) + ev->length + 1; } hci_dev_unlock(hdev); From b48b833f9e8aa0675820a3b5a0f30d7319590ea8 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:55:01 -0800 Subject: [PATCH 26/62] Bluetooth: HCI: Use skb_pull_data to parse LE Ext Advertising Report event This uses skb_pull_data to check the LE Extended Advertising Report events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 17 +++++++++++------ net/bluetooth/hci_event.c | 36 +++++++++++++++++++++++++----------- 2 files changed, 36 insertions(+), 17 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index c005b1ccdbc5..d3f2da9b2ac2 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -2517,8 +2517,8 @@ struct hci_ev_le_phy_update_complete { } __packed; #define HCI_EV_LE_EXT_ADV_REPORT 0x0d -struct hci_ev_le_ext_adv_report { - __le16 evt_type; +struct hci_ev_le_ext_adv_info { + __le16 type; __u8 bdaddr_type; bdaddr_t bdaddr; __u8 primary_phy; @@ -2526,11 +2526,16 @@ struct hci_ev_le_ext_adv_report { __u8 sid; __u8 tx_power; __s8 rssi; - __le16 interval; - __u8 direct_addr_type; + __le16 interval; + __u8 direct_addr_type; bdaddr_t direct_addr; - __u8 length; - __u8 data[]; + __u8 length; + __u8 data[]; +} __packed; + +struct hci_ev_le_ext_adv_report { + __u8 num; + struct hci_ev_le_ext_adv_info info[]; } __packed; #define HCI_EV_LE_ENHANCED_CONN_COMPLETE 0x0a diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 42ffd5df6d4b..23cfcb1c0ca3 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -6649,26 +6649,40 @@ invalid: static void hci_le_ext_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) { - u8 num_reports = skb->data[0]; - void *ptr = &skb->data[1]; + struct hci_ev_le_ext_adv_report *ev; + + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_EXT_ADV_REPORT, + sizeof(*ev)); + if (!ev) + return; + + if (!ev->num) + return; hci_dev_lock(hdev); - while (num_reports--) { - struct hci_ev_le_ext_adv_report *ev = ptr; + while (ev->num--) { + struct hci_ev_le_ext_adv_info *info; u8 legacy_evt_type; u16 evt_type; - evt_type = __le16_to_cpu(ev->evt_type); + info = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_EXT_ADV_REPORT, + sizeof(*info)); + if (!info) + break; + + if (!hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_EXT_ADV_REPORT, + info->length)) + break; + + evt_type = __le16_to_cpu(info->type); legacy_evt_type = ext_evt_type_to_legacy(hdev, evt_type); if (legacy_evt_type != LE_ADV_INVALID) { - process_adv_report(hdev, legacy_evt_type, &ev->bdaddr, - ev->bdaddr_type, NULL, 0, ev->rssi, - ev->data, ev->length, + process_adv_report(hdev, legacy_evt_type, &info->bdaddr, + info->bdaddr_type, NULL, 0, + info->rssi, info->data, info->length, !(evt_type & LE_EXT_ADV_LEGACY_PDU)); } - - ptr += sizeof(*ev) + ev->length; } hci_dev_unlock(hdev); @@ -7019,7 +7033,7 @@ static void hci_store_wake_reason(struct hci_dev *hdev, u8 event, { struct hci_ev_le_advertising_info *adv; struct hci_ev_le_direct_adv_info *direct_adv; - struct hci_ev_le_ext_adv_report *ext_adv; + struct hci_ev_le_ext_adv_info *ext_adv; const struct hci_ev_conn_complete *conn_complete = (void *)skb->data; const struct hci_ev_conn_request *conn_request = (void *)skb->data; From a3679649a19179813c3853b8bb397a801d9dd253 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:55:02 -0800 Subject: [PATCH 27/62] Bluetooth: HCI: Use skb_pull_data to parse LE Direct Advertising Report event This uses skb_pull_data to check the LE Direct Advertising Report events received have the minimum required length. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 7 ++++++- net/bluetooth/hci_event.c | 26 +++++++++++++++++++------- 2 files changed, 25 insertions(+), 8 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index d3f2da9b2ac2..4343f79bd02c 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -2500,7 +2500,7 @@ struct hci_ev_le_data_len_change { #define HCI_EV_LE_DIRECT_ADV_REPORT 0x0B struct hci_ev_le_direct_adv_info { - __u8 evt_type; + __u8 type; __u8 bdaddr_type; bdaddr_t bdaddr; __u8 direct_addr_type; @@ -2508,6 +2508,11 @@ struct hci_ev_le_direct_adv_info { __s8 rssi; } __packed; +struct hci_ev_le_direct_adv_report { + __u8 num; + struct hci_ev_le_direct_adv_info info[]; +} __packed; + #define HCI_EV_LE_PHY_UPDATE_COMPLETE 0x0c struct hci_ev_le_phy_update_complete { __u8 status; diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 23cfcb1c0ca3..8f21405997d1 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -6881,19 +6881,31 @@ static void hci_le_remote_conn_param_req_evt(struct hci_dev *hdev, static void hci_le_direct_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) { - u8 num_reports = skb->data[0]; - struct hci_ev_le_direct_adv_info *ev = (void *)&skb->data[1]; + struct hci_ev_le_direct_adv_report *ev; + int i; - if (!num_reports || skb->len < num_reports * sizeof(*ev) + 1) + ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_DIRECT_ADV_REPORT, + sizeof(*ev)); + if (!ev) + return; + + if (!hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_DIRECT_ADV_REPORT, + flex_array_size(ev, info, ev->num))) + return; + + if (!ev->num) return; hci_dev_lock(hdev); - for (; num_reports; num_reports--, ev++) - process_adv_report(hdev, ev->evt_type, &ev->bdaddr, - ev->bdaddr_type, &ev->direct_addr, - ev->direct_addr_type, ev->rssi, NULL, 0, + for (i = 0; i < ev->num; i++) { + struct hci_ev_le_direct_adv_info *info = &ev->info[i]; + + process_adv_report(hdev, info->type, &info->bdaddr, + info->bdaddr_type, &info->direct_addr, + info->direct_addr_type, info->rssi, NULL, 0, false); + } hci_dev_unlock(hdev); } From 3e54c5890c87a30b1019a3de9dab968ff2b21e06 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:55:03 -0800 Subject: [PATCH 28/62] Bluetooth: hci_event: Use of a function table to handle HCI events This change the use of switch statement to a function table which is easier to extend and can include min/max length of each HCI event. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_event.c | 904 ++++++++++++++++---------------------- net/bluetooth/msft.c | 2 +- net/bluetooth/msft.h | 2 +- 3 files changed, 392 insertions(+), 516 deletions(-) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 8f21405997d1..3953ec188ff6 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -3126,17 +3126,14 @@ static void hci_cs_switch_role(struct hci_dev *hdev, u8 status) hci_dev_unlock(hdev); } -static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_inquiry_complete_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *ev; + struct hci_ev_status *ev = data; struct discovery_state *discov = &hdev->discovery; struct inquiry_entry *e; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_INQUIRY_COMPLETE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_conn_check_pending(hdev); @@ -3190,21 +3187,18 @@ unlock: hci_dev_unlock(hdev); } -static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_inquiry_result_evt(struct hci_dev *hdev, void *edata, + struct sk_buff *skb) { - struct hci_ev_inquiry_result *ev; + struct hci_ev_inquiry_result *ev = edata; struct inquiry_data data; int i; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_INQUIRY_RESULT, sizeof(*ev)); - if (!ev) - return; - if (!hci_ev_skb_pull(hdev, skb, HCI_EV_INQUIRY_RESULT, flex_array_size(ev, info, ev->num))) return; - BT_DBG("%s num %d", hdev->name, ev->num); + bt_dev_dbg(hdev, "num %d", ev->num); if (!ev->num) return; @@ -3237,16 +3231,13 @@ static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_unlock(hdev); } -static void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_conn_complete_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_conn_complete *ev; + struct hci_ev_conn_complete *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CONN_COMPLETE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -3365,20 +3356,16 @@ static void hci_reject_conn(struct hci_dev *hdev, bdaddr_t *bdaddr) hci_send_cmd(hdev, HCI_OP_REJECT_CONN_REQ, sizeof(cp), &cp); } -static void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_conn_request_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_conn_request *ev; + struct hci_ev_conn_request *ev = data; int mask = hdev->link_mode; struct inquiry_entry *ie; struct hci_conn *conn; __u8 flags = 0; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CONN_REQUEST, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s bdaddr %pMR type 0x%x", hdev->name, &ev->bdaddr, - ev->link_type); + bt_dev_dbg(hdev, "bdaddr %pMR type 0x%x", &ev->bdaddr, ev->link_type); mask |= hci_proto_connect_ind(hdev, &ev->bdaddr, ev->link_type, &flags); @@ -3480,19 +3467,16 @@ static u8 hci_to_mgmt_reason(u8 err) } } -static void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_disconn_complete_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_disconn_complete *ev; + struct hci_ev_disconn_complete *ev = data; u8 reason; struct hci_conn_params *params; struct hci_conn *conn; bool mgmt_connected; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_DISCONN_COMPLETE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -3568,16 +3552,13 @@ unlock: hci_dev_unlock(hdev); } -static void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_auth_complete_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_auth_complete *ev; + struct hci_ev_auth_complete *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_AUTH_COMPLETE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -3642,16 +3623,13 @@ unlock: hci_dev_unlock(hdev); } -static void hci_remote_name_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_remote_name_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_remote_name *ev; + struct hci_ev_remote_name *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_NAME, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_conn_check_pending(hdev); @@ -3729,16 +3707,13 @@ unlock: hci_dev_unlock(hdev); } -static void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_encrypt_change_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_encrypt_change *ev; + struct hci_ev_encrypt_change *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_ENCRYPT_CHANGE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -3847,18 +3822,13 @@ unlock: hci_dev_unlock(hdev); } -static void hci_change_link_key_complete_evt(struct hci_dev *hdev, +static void hci_change_link_key_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_change_link_key_complete *ev; + struct hci_ev_change_link_key_complete *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CHANGE_LINK_KEY_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -3875,17 +3845,13 @@ static void hci_change_link_key_complete_evt(struct hci_dev *hdev, hci_dev_unlock(hdev); } -static void hci_remote_features_evt(struct hci_dev *hdev, +static void hci_remote_features_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_remote_features *ev; + struct hci_ev_remote_features *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_FEATURES, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -3943,16 +3909,12 @@ static inline void handle_cmd_cnt_and_timer(struct hci_dev *hdev, u8 ncmd) } } -static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb, - u16 *opcode, u8 *status, +static void hci_cmd_complete_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb, u16 *opcode, u8 *status, hci_req_complete_t *req_complete, hci_req_complete_skb_t *req_complete_skb) { - struct hci_ev_cmd_complete *ev; - - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CMD_COMPLETE, sizeof(*ev)); - if (!ev) - return; + struct hci_ev_cmd_complete *ev = data; *opcode = __le16_to_cpu(ev->opcode); *status = skb->data[0]; @@ -4330,16 +4292,12 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb, queue_work(hdev->workqueue, &hdev->cmd_work); } -static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb, - u16 *opcode, u8 *status, +static void hci_cmd_status_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb, u16 *opcode, u8 *status, hci_req_complete_t *req_complete, hci_req_complete_skb_t *req_complete_skb) { - struct hci_ev_cmd_status *ev; - - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CMD_STATUS, sizeof(*ev)); - if (!ev) - return; + struct hci_ev_cmd_status *ev = data; *opcode = __le16_to_cpu(ev->opcode); *status = ev->status; @@ -4445,29 +4403,25 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb, queue_work(hdev->workqueue, &hdev->cmd_work); } -static void hci_hardware_error_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_hardware_error_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_hardware_error *ev; + struct hci_ev_hardware_error *ev = data; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_HARDWARE_ERROR, sizeof(*ev)); - if (!ev) - return; + bt_dev_dbg(hdev, "code 0x%2.2x", ev->code); hdev->hw_error_code = ev->code; queue_work(hdev->req_workqueue, &hdev->error_reset); } -static void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_role_change_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_role_change *ev; + struct hci_ev_role_change *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_ROLE_CHANGE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -4484,15 +4438,12 @@ static void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_unlock(hdev); } -static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_num_comp_pkts_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_num_comp_pkts *ev; + struct hci_ev_num_comp_pkts *ev = data; int i; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_NUM_COMP_PKTS, sizeof(*ev)); - if (!ev) - return; - if (!hci_ev_skb_pull(hdev, skb, HCI_EV_NUM_COMP_PKTS, flex_array_size(ev, handles, ev->num))) return; @@ -4502,7 +4453,7 @@ static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb) return; } - BT_DBG("%s num %d", hdev->name, ev->num); + bt_dev_dbg(hdev, "num %d", ev->num); for (i = 0; i < ev->num; i++) { struct hci_comp_pkts_info *info = &ev->handles[i]; @@ -4574,26 +4525,24 @@ static struct hci_conn *__hci_conn_lookup_handle(struct hci_dev *hdev, return NULL; } -static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_num_comp_blocks_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_num_comp_blocks *ev; + struct hci_ev_num_comp_blocks *ev = data; int i; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_NUM_COMP_BLOCKS, sizeof(*ev)); - if (!ev) - return; - if (!hci_ev_skb_pull(hdev, skb, HCI_EV_NUM_COMP_BLOCKS, flex_array_size(ev, handles, ev->num_hndl))) return; if (hdev->flow_ctl_mode != HCI_FLOW_CTL_MODE_BLOCK_BASED) { - bt_dev_err(hdev, "wrong event for mode %d", hdev->flow_ctl_mode); + bt_dev_err(hdev, "wrong event for mode %d", + hdev->flow_ctl_mode); return; } - BT_DBG("%s num_blocks %d num_hndl %d", hdev->name, ev->num_blocks, - ev->num_hndl); + bt_dev_dbg(hdev, "num_blocks %d num_hndl %d", ev->num_blocks, + ev->num_hndl); for (i = 0; i < ev->num_hndl; i++) { struct hci_comp_blocks_info *info = &ev->handles[i]; @@ -4627,16 +4576,13 @@ static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb) queue_work(hdev->workqueue, &hdev->tx_work); } -static void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_mode_change_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_mode_change *ev; + struct hci_ev_mode_change *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_MODE_CHANGE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -4659,16 +4605,13 @@ static void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_unlock(hdev); } -static void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_pin_code_request_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_pin_code_req *ev; + struct hci_ev_pin_code_req *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_PIN_CODE_REQ, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -4733,18 +4676,15 @@ static void conn_set_key(struct hci_conn *conn, u8 key_type, u8 pin_len) } } -static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_link_key_request_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_link_key_req *ev; + struct hci_ev_link_key_req *ev = data; struct hci_cp_link_key_reply cp; struct hci_conn *conn; struct link_key *key; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_LINK_KEY_REQ, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); if (!hci_dev_test_flag(hdev, HCI_MGMT)) return; @@ -4753,13 +4693,11 @@ static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) key = hci_find_link_key(hdev, &ev->bdaddr); if (!key) { - BT_DBG("%s link key not found for %pMR", hdev->name, - &ev->bdaddr); + bt_dev_dbg(hdev, "link key not found for %pMR", &ev->bdaddr); goto not_found; } - BT_DBG("%s found key type %u for %pMR", hdev->name, key->type, - &ev->bdaddr); + bt_dev_dbg(hdev, "found key type %u for %pMR", key->type, &ev->bdaddr); conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); if (conn) { @@ -4768,15 +4706,14 @@ static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) if ((key->type == HCI_LK_UNAUTH_COMBINATION_P192 || key->type == HCI_LK_UNAUTH_COMBINATION_P256) && conn->auth_type != 0xff && (conn->auth_type & 0x01)) { - BT_DBG("%s ignoring unauthenticated key", hdev->name); + bt_dev_dbg(hdev, "ignoring unauthenticated key"); goto not_found; } if (key->type == HCI_LK_COMBINATION && key->pin_len < 16 && (conn->pending_sec_level == BT_SECURITY_HIGH || conn->pending_sec_level == BT_SECURITY_FIPS)) { - BT_DBG("%s ignoring key unauthenticated for high security", - hdev->name); + bt_dev_dbg(hdev, "ignoring key unauthenticated for high security"); goto not_found; } @@ -4797,19 +4734,16 @@ not_found: hci_dev_unlock(hdev); } -static void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_link_key_notify_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_link_key_notify *ev; + struct hci_ev_link_key_notify *ev = data; struct hci_conn *conn; struct link_key *key; bool persistent; u8 pin_len = 0; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_LINK_KEY_NOTIFY, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -4861,16 +4795,13 @@ unlock: hci_dev_unlock(hdev); } -static void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_clock_offset_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_clock_offset *ev; + struct hci_ev_clock_offset *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CLOCK_OFFSET, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -4888,16 +4819,13 @@ static void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_unlock(hdev); } -static void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_pkt_type_change_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_pkt_type_change *ev; + struct hci_ev_pkt_type_change *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_PKT_TYPE_CHANGE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -4908,16 +4836,13 @@ static void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_unlock(hdev); } -static void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_pscan_rep_mode_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_pscan_rep_mode *ev; + struct hci_ev_pscan_rep_mode *ev = data; struct inquiry_entry *ie; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_PSCAN_REP_MODE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -4930,22 +4855,17 @@ static void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_unlock(hdev); } -static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, +static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, void *edata, struct sk_buff *skb) { union { struct hci_ev_inquiry_result_rssi *res1; struct hci_ev_inquiry_result_rssi_pscan *res2; - } *ev; + } *ev = edata; struct inquiry_data data; int i; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_INQUIRY_RESULT_WITH_RSSI, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s num_rsp %d", hdev->name, ev->res1->num); + bt_dev_dbg(hdev, "num_rsp %d", ev->res1->num); if (!ev->res1->num) return; @@ -5007,18 +4927,13 @@ static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, hci_dev_unlock(hdev); } -static void hci_remote_ext_features_evt(struct hci_dev *hdev, +static void hci_remote_ext_features_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_remote_ext_features *ev; + struct hci_ev_remote_ext_features *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_EXT_FEATURES, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -5076,17 +4991,13 @@ unlock: hci_dev_unlock(hdev); } -static void hci_sync_conn_complete_evt(struct hci_dev *hdev, +static void hci_sync_conn_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_sync_conn_complete *ev; + struct hci_ev_sync_conn_complete *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_SYNC_CONN_COMPLETE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -5195,24 +5106,19 @@ static inline size_t eir_get_length(u8 *eir, size_t eir_len) return eir_len; } -static void hci_extended_inquiry_result_evt(struct hci_dev *hdev, +static void hci_extended_inquiry_result_evt(struct hci_dev *hdev, void *edata, struct sk_buff *skb) { - struct hci_ev_ext_inquiry_result *ev; + struct hci_ev_ext_inquiry_result *ev = edata; struct inquiry_data data; size_t eir_len; int i; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_EXTENDED_INQUIRY_RESULT, - sizeof(*ev)); - if (!ev) - return; - if (!hci_ev_skb_pull(hdev, skb, HCI_EV_EXTENDED_INQUIRY_RESULT, flex_array_size(ev, info, ev->num))) return; - BT_DBG("%s num %d", hdev->name, ev->num); + bt_dev_dbg(hdev, "num %d", ev->num); if (!ev->num) return; @@ -5255,19 +5161,14 @@ static void hci_extended_inquiry_result_evt(struct hci_dev *hdev, hci_dev_unlock(hdev); } -static void hci_key_refresh_complete_evt(struct hci_dev *hdev, +static void hci_key_refresh_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_key_refresh_complete *ev; + struct hci_ev_key_refresh_complete *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_KEY_REFRESH_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x handle 0x%4.4x", hdev->name, ev->status, - __le16_to_cpu(ev->handle)); + bt_dev_dbg(hdev, "status 0x%2.2x handle 0x%4.4x", ev->status, + __le16_to_cpu(ev->handle)); hci_dev_lock(hdev); @@ -5370,16 +5271,13 @@ static u8 bredr_oob_data_present(struct hci_conn *conn) return 0x01; } -static void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_io_capa_request_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_io_capa_request *ev; + struct hci_ev_io_capa_request *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_IO_CAPA_REQUEST, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -5443,16 +5341,13 @@ unlock: hci_dev_unlock(hdev); } -static void hci_io_capa_reply_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_io_capa_reply_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_io_capa_reply *ev; + struct hci_ev_io_capa_reply *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_IO_CAPA_REPLY, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -5467,19 +5362,14 @@ unlock: hci_dev_unlock(hdev); } -static void hci_user_confirm_request_evt(struct hci_dev *hdev, +static void hci_user_confirm_request_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_user_confirm_req *ev; + struct hci_ev_user_confirm_req *ev = data; int loc_mitm, rem_mitm, confirm_hint = 0; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_USER_CONFIRM_REQUEST, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -5500,7 +5390,7 @@ static void hci_user_confirm_request_evt(struct hci_dev *hdev, */ if (conn->pending_sec_level > BT_SECURITY_MEDIUM && conn->remote_cap == HCI_IO_NO_INPUT_OUTPUT) { - BT_DBG("Rejecting request: remote device can't provide MITM"); + bt_dev_dbg(hdev, "Rejecting request: remote device can't provide MITM"); hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_NEG_REPLY, sizeof(ev->bdaddr), &ev->bdaddr); goto unlock; @@ -5519,7 +5409,7 @@ static void hci_user_confirm_request_evt(struct hci_dev *hdev, if (!test_bit(HCI_CONN_AUTH_PEND, &conn->flags) && conn->io_capability != HCI_IO_NO_INPUT_OUTPUT && (loc_mitm || rem_mitm)) { - BT_DBG("Confirming auto-accept as acceptor"); + bt_dev_dbg(hdev, "Confirming auto-accept as acceptor"); confirm_hint = 1; goto confirm; } @@ -5557,34 +5447,24 @@ unlock: hci_dev_unlock(hdev); } -static void hci_user_passkey_request_evt(struct hci_dev *hdev, +static void hci_user_passkey_request_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_user_passkey_req *ev; + struct hci_ev_user_passkey_req *ev = data; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_USER_PASSKEY_REQUEST, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); if (hci_dev_test_flag(hdev, HCI_MGMT)) mgmt_user_passkey_request(hdev, &ev->bdaddr, ACL_LINK, 0); } -static void hci_user_passkey_notify_evt(struct hci_dev *hdev, +static void hci_user_passkey_notify_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_user_passkey_notify *ev; + struct hci_ev_user_passkey_notify *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_USER_PASSKEY_NOTIFY, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); if (!conn) @@ -5599,16 +5479,13 @@ static void hci_user_passkey_notify_evt(struct hci_dev *hdev, conn->passkey_entered); } -static void hci_keypress_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_keypress_notify_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_keypress_notify *ev; + struct hci_ev_keypress_notify *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_KEYPRESS_NOTIFY, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); if (!conn) @@ -5641,18 +5518,13 @@ static void hci_keypress_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) conn->passkey_entered); } -static void hci_simple_pair_complete_evt(struct hci_dev *hdev, +static void hci_simple_pair_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_simple_pair_complete *ev; + struct hci_ev_simple_pair_complete *ev = data; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_SIMPLE_PAIR_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -5677,19 +5549,14 @@ unlock: hci_dev_unlock(hdev); } -static void hci_remote_host_features_evt(struct hci_dev *hdev, +static void hci_remote_host_features_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_remote_host_features *ev; + struct hci_ev_remote_host_features *ev = data; struct inquiry_entry *ie; struct hci_conn *conn; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_HOST_FEATURES, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -5704,18 +5571,13 @@ static void hci_remote_host_features_evt(struct hci_dev *hdev, hci_dev_unlock(hdev); } -static void hci_remote_oob_data_request_evt(struct hci_dev *hdev, +static void hci_remote_oob_data_request_evt(struct hci_dev *hdev, void *edata, struct sk_buff *skb) { - struct hci_ev_remote_oob_data_request *ev; + struct hci_ev_remote_oob_data_request *ev = edata; struct oob_data *data; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_REMOTE_OOB_DATA_REQUEST, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s", hdev->name); + bt_dev_dbg(hdev, ""); hci_dev_lock(hdev); @@ -5764,16 +5626,13 @@ unlock: } #if IS_ENABLED(CONFIG_BT_HS) -static void hci_chan_selected_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_chan_selected_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_channel_selected *ev; + struct hci_ev_channel_selected *ev = data; struct hci_conn *hcon; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_CHANNEL_SELECTED, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s handle 0x%2.2x", hdev->name, ev->phy_handle); + bt_dev_dbg(hdev, "handle 0x%2.2x", ev->phy_handle); hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle); if (!hcon) @@ -5782,18 +5641,14 @@ static void hci_chan_selected_evt(struct hci_dev *hdev, struct sk_buff *skb) amp_read_loc_assoc_final_data(hdev, hcon); } -static void hci_phy_link_complete_evt(struct hci_dev *hdev, +static void hci_phy_link_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_phy_link_complete *ev; + struct hci_ev_phy_link_complete *ev = data; struct hci_conn *hcon, *bredr_hcon; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_PHY_LINK_COMPLETE, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle, - ev->status); + bt_dev_dbg(hdev, "handle 0x%2.2x status 0x%2.2x", ev->phy_handle, + ev->status); hci_dev_lock(hdev); @@ -5827,21 +5682,16 @@ unlock: hci_dev_unlock(hdev); } -static void hci_loglink_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_loglink_complete_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_logical_link_complete *ev; + struct hci_ev_logical_link_complete *ev = data; struct hci_conn *hcon; struct hci_chan *hchan; struct amp_mgr *mgr; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_LOGICAL_LINK_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s log_handle 0x%4.4x phy_handle 0x%2.2x status 0x%2.2x", - hdev->name, le16_to_cpu(ev->handle), ev->phy_handle, - ev->status); + bt_dev_dbg(hdev, "log_handle 0x%4.4x phy_handle 0x%2.2x status 0x%2.2x", + le16_to_cpu(ev->handle), ev->phy_handle, ev->status); hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle); if (!hcon) @@ -5871,19 +5721,14 @@ static void hci_loglink_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) } } -static void hci_disconn_loglink_complete_evt(struct hci_dev *hdev, +static void hci_disconn_loglink_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_disconn_logical_link_complete *ev; + struct hci_ev_disconn_logical_link_complete *ev = data; struct hci_chan *hchan; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_DISCONN_LOGICAL_LINK_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s log handle 0x%4.4x status 0x%2.2x", hdev->name, - le16_to_cpu(ev->handle), ev->status); + bt_dev_dbg(hdev, "handle 0x%4.4x status 0x%2.2x", + le16_to_cpu(ev->handle), ev->status); if (ev->status) return; @@ -5900,18 +5745,13 @@ unlock: hci_dev_unlock(hdev); } -static void hci_disconn_phylink_complete_evt(struct hci_dev *hdev, +static void hci_disconn_phylink_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_disconn_phy_link_complete *ev; + struct hci_ev_disconn_phy_link_complete *ev = data; struct hci_conn *hcon; - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_DISCONN_PHY_LINK_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); if (ev->status) return; @@ -6938,13 +6778,10 @@ unlock: hci_dev_unlock(hdev); } -static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_le_meta_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_le_meta *ev; - - ev = hci_ev_skb_pull(hdev, skb, HCI_EV_LE_META, sizeof(*ev)); - if (!ev) - return; + struct hci_ev_le_meta *ev = data; switch (ev->subevent) { case HCI_EV_LE_CONN_COMPLETE: @@ -7109,6 +6946,225 @@ unlock: hci_dev_unlock(hdev); } +#define HCI_EV_VL(_op, _func, _min_len, _max_len) \ +[_op] = { \ + .req = false, \ + .func = _func, \ + .min_len = _min_len, \ + .max_len = _max_len, \ +} + +#define HCI_EV(_op, _func, _len) \ + HCI_EV_VL(_op, _func, _len, _len) + +#define HCI_EV_STATUS(_op, _func) \ + HCI_EV(_op, _func, sizeof(struct hci_ev_status)) + +#define HCI_EV_REQ_VL(_op, _func, _min_len, _max_len) \ +[_op] = { \ + .req = true, \ + .func_req = _func, \ + .min_len = _min_len, \ + .max_len = _max_len, \ +} + +#define HCI_EV_REQ(_op, _func, _len) \ + HCI_EV_REQ_VL(_op, _func, _len, _len) + +/* Entries in this table shall have their position according to the event opcode + * they handle so the use of the macros above is recommend since it does attempt + * to initialize at its proper index using Designated Initializers that way + * events without a callback function don't have entered. + */ +static const struct hci_ev { + bool req; + union { + void (*func)(struct hci_dev *hdev, void *data, + struct sk_buff *skb); + void (*func_req)(struct hci_dev *hdev, void *data, + struct sk_buff *skb, u16 *opcode, u8 *status, + hci_req_complete_t *req_complete, + hci_req_complete_skb_t *req_complete_skb); + }; + u16 min_len; + u16 max_len; +} hci_ev_table[U8_MAX + 1] = { + /* [0x01 = HCI_EV_INQUIRY_COMPLETE] */ + HCI_EV_STATUS(HCI_EV_INQUIRY_COMPLETE, hci_inquiry_complete_evt), + /* [0x02 = HCI_EV_INQUIRY_RESULT] */ + HCI_EV_VL(HCI_EV_INQUIRY_RESULT, hci_inquiry_result_evt, + sizeof(struct hci_ev_inquiry_result), HCI_MAX_EVENT_SIZE), + /* [0x03 = HCI_EV_CONN_COMPLETE] */ + HCI_EV(HCI_EV_CONN_COMPLETE, hci_conn_complete_evt, + sizeof(struct hci_ev_conn_complete)), + /* [0x04 = HCI_EV_CONN_REQUEST] */ + HCI_EV(HCI_EV_CONN_REQUEST, hci_conn_request_evt, + sizeof(struct hci_ev_conn_request)), + /* [0x05 = HCI_EV_DISCONN_COMPLETE] */ + HCI_EV(HCI_EV_DISCONN_COMPLETE, hci_disconn_complete_evt, + sizeof(struct hci_ev_disconn_complete)), + /* [0x06 = HCI_EV_AUTH_COMPLETE] */ + HCI_EV(HCI_EV_AUTH_COMPLETE, hci_auth_complete_evt, + sizeof(struct hci_ev_auth_complete)), + /* [0x07 = HCI_EV_REMOTE_NAME] */ + HCI_EV(HCI_EV_REMOTE_NAME, hci_remote_name_evt, + sizeof(struct hci_ev_remote_name)), + /* [0x08 = HCI_EV_ENCRYPT_CHANGE] */ + HCI_EV(HCI_EV_ENCRYPT_CHANGE, hci_encrypt_change_evt, + sizeof(struct hci_ev_encrypt_change)), + /* [0x09 = HCI_EV_CHANGE_LINK_KEY_COMPLETE] */ + HCI_EV(HCI_EV_CHANGE_LINK_KEY_COMPLETE, + hci_change_link_key_complete_evt, + sizeof(struct hci_ev_change_link_key_complete)), + /* [0x0b = HCI_EV_REMOTE_FEATURES] */ + HCI_EV(HCI_EV_REMOTE_FEATURES, hci_remote_features_evt, + sizeof(struct hci_ev_remote_features)), + /* [0x0e = HCI_EV_CMD_COMPLETE] */ + HCI_EV_REQ_VL(HCI_EV_CMD_COMPLETE, hci_cmd_complete_evt, + sizeof(struct hci_ev_cmd_complete), HCI_MAX_EVENT_SIZE), + /* [0x0f = HCI_EV_CMD_STATUS] */ + HCI_EV_REQ(HCI_EV_CMD_STATUS, hci_cmd_status_evt, + sizeof(struct hci_ev_cmd_status)), + /* [0x10 = HCI_EV_CMD_STATUS] */ + HCI_EV(HCI_EV_HARDWARE_ERROR, hci_hardware_error_evt, + sizeof(struct hci_ev_hardware_error)), + /* [0x12 = HCI_EV_ROLE_CHANGE] */ + HCI_EV(HCI_EV_ROLE_CHANGE, hci_role_change_evt, + sizeof(struct hci_ev_role_change)), + /* [0x13 = HCI_EV_NUM_COMP_PKTS] */ + HCI_EV_VL(HCI_EV_NUM_COMP_PKTS, hci_num_comp_pkts_evt, + sizeof(struct hci_ev_num_comp_pkts), HCI_MAX_EVENT_SIZE), + /* [0x14 = HCI_EV_MODE_CHANGE] */ + HCI_EV(HCI_EV_MODE_CHANGE, hci_mode_change_evt, + sizeof(struct hci_ev_mode_change)), + /* [0x16 = HCI_EV_PIN_CODE_REQ] */ + HCI_EV(HCI_EV_PIN_CODE_REQ, hci_pin_code_request_evt, + sizeof(struct hci_ev_pin_code_req)), + /* [0x17 = HCI_EV_LINK_KEY_REQ] */ + HCI_EV(HCI_EV_LINK_KEY_REQ, hci_link_key_request_evt, + sizeof(struct hci_ev_link_key_req)), + /* [0x18 = HCI_EV_LINK_KEY_NOTIFY] */ + HCI_EV(HCI_EV_LINK_KEY_NOTIFY, hci_link_key_notify_evt, + sizeof(struct hci_ev_link_key_notify)), + /* [0x1c = HCI_EV_CLOCK_OFFSET] */ + HCI_EV(HCI_EV_CLOCK_OFFSET, hci_clock_offset_evt, + sizeof(struct hci_ev_clock_offset)), + /* [0x1d = HCI_EV_PKT_TYPE_CHANGE] */ + HCI_EV(HCI_EV_PKT_TYPE_CHANGE, hci_pkt_type_change_evt, + sizeof(struct hci_ev_pkt_type_change)), + /* [0x20 = HCI_EV_PSCAN_REP_MODE] */ + HCI_EV(HCI_EV_PSCAN_REP_MODE, hci_pscan_rep_mode_evt, + sizeof(struct hci_ev_pscan_rep_mode)), + /* [0x22 = HCI_EV_INQUIRY_RESULT_WITH_RSSI] */ + HCI_EV_VL(HCI_EV_INQUIRY_RESULT_WITH_RSSI, + hci_inquiry_result_with_rssi_evt, + sizeof(struct hci_ev_inquiry_result_rssi), + HCI_MAX_EVENT_SIZE), + /* [0x23 = HCI_EV_REMOTE_EXT_FEATURES] */ + HCI_EV(HCI_EV_REMOTE_EXT_FEATURES, hci_remote_ext_features_evt, + sizeof(struct hci_ev_remote_ext_features)), + /* [0x2c = HCI_EV_SYNC_CONN_COMPLETE] */ + HCI_EV(HCI_EV_SYNC_CONN_COMPLETE, hci_sync_conn_complete_evt, + sizeof(struct hci_ev_sync_conn_complete)), + /* [0x2d = HCI_EV_EXTENDED_INQUIRY_RESULT] */ + HCI_EV_VL(HCI_EV_EXTENDED_INQUIRY_RESULT, + hci_extended_inquiry_result_evt, + sizeof(struct hci_ev_ext_inquiry_result), HCI_MAX_EVENT_SIZE), + /* [0x30 = HCI_EV_KEY_REFRESH_COMPLETE] */ + HCI_EV(HCI_EV_KEY_REFRESH_COMPLETE, hci_key_refresh_complete_evt, + sizeof(struct hci_ev_key_refresh_complete)), + /* [0x31 = HCI_EV_IO_CAPA_REQUEST] */ + HCI_EV(HCI_EV_IO_CAPA_REQUEST, hci_io_capa_request_evt, + sizeof(struct hci_ev_io_capa_request)), + /* [0x32 = HCI_EV_IO_CAPA_REPLY] */ + HCI_EV(HCI_EV_IO_CAPA_REPLY, hci_io_capa_reply_evt, + sizeof(struct hci_ev_io_capa_reply)), + /* [0x33 = HCI_EV_USER_CONFIRM_REQUEST] */ + HCI_EV(HCI_EV_USER_CONFIRM_REQUEST, hci_user_confirm_request_evt, + sizeof(struct hci_ev_user_confirm_req)), + /* [0x34 = HCI_EV_USER_PASSKEY_REQUEST] */ + HCI_EV(HCI_EV_USER_PASSKEY_REQUEST, hci_user_passkey_request_evt, + sizeof(struct hci_ev_user_passkey_req)), + /* [0x35 = HCI_EV_REMOTE_OOB_DATA_REQUEST] */ + HCI_EV(HCI_EV_REMOTE_OOB_DATA_REQUEST, hci_remote_oob_data_request_evt, + sizeof(struct hci_ev_remote_oob_data_request)), + /* [0x36 = HCI_EV_SIMPLE_PAIR_COMPLETE] */ + HCI_EV(HCI_EV_SIMPLE_PAIR_COMPLETE, hci_simple_pair_complete_evt, + sizeof(struct hci_ev_simple_pair_complete)), + /* [0x3b = HCI_EV_USER_PASSKEY_NOTIFY] */ + HCI_EV(HCI_EV_USER_PASSKEY_NOTIFY, hci_user_passkey_notify_evt, + sizeof(struct hci_ev_user_passkey_notify)), + /* [0x3c = HCI_EV_KEYPRESS_NOTIFY] */ + HCI_EV(HCI_EV_KEYPRESS_NOTIFY, hci_keypress_notify_evt, + sizeof(struct hci_ev_keypress_notify)), + /* [0x3d = HCI_EV_REMOTE_HOST_FEATURES] */ + HCI_EV(HCI_EV_REMOTE_HOST_FEATURES, hci_remote_host_features_evt, + sizeof(struct hci_ev_remote_host_features)), + /* [0x3e = HCI_EV_LE_META] */ + HCI_EV_VL(HCI_EV_LE_META, hci_le_meta_evt, + sizeof(struct hci_ev_le_meta), HCI_MAX_EVENT_SIZE), +#if IS_ENABLED(CONFIG_BT_HS) + /* [0x40 = HCI_EV_PHY_LINK_COMPLETE] */ + HCI_EV(HCI_EV_PHY_LINK_COMPLETE, hci_phy_link_complete_evt, + sizeof(struct hci_ev_phy_link_complete)), + /* [0x41 = HCI_EV_CHANNEL_SELECTED] */ + HCI_EV(HCI_EV_CHANNEL_SELECTED, hci_chan_selected_evt, + sizeof(struct hci_ev_channel_selected)), + /* [0x42 = HCI_EV_DISCONN_PHY_LINK_COMPLETE] */ + HCI_EV(HCI_EV_DISCONN_LOGICAL_LINK_COMPLETE, + hci_disconn_loglink_complete_evt, + sizeof(struct hci_ev_disconn_logical_link_complete)), + /* [0x45 = HCI_EV_LOGICAL_LINK_COMPLETE] */ + HCI_EV(HCI_EV_LOGICAL_LINK_COMPLETE, hci_loglink_complete_evt, + sizeof(struct hci_ev_logical_link_complete)), + /* [0x46 = HCI_EV_DISCONN_LOGICAL_LINK_COMPLETE] */ + HCI_EV(HCI_EV_DISCONN_PHY_LINK_COMPLETE, + hci_disconn_phylink_complete_evt, + sizeof(struct hci_ev_disconn_phy_link_complete)), +#endif + /* [0x48 = HCI_EV_NUM_COMP_BLOCKS] */ + HCI_EV(HCI_EV_NUM_COMP_BLOCKS, hci_num_comp_blocks_evt, + sizeof(struct hci_ev_num_comp_blocks)), + /* [0xff = HCI_EV_VENDOR] */ + HCI_EV(HCI_EV_VENDOR, msft_vendor_evt, 0), +}; + +static void hci_event_func(struct hci_dev *hdev, u8 event, struct sk_buff *skb, + u16 *opcode, u8 *status, + hci_req_complete_t *req_complete, + hci_req_complete_skb_t *req_complete_skb) +{ + const struct hci_ev *ev = &hci_ev_table[event]; + void *data; + + if (!ev->func) + return; + + if (skb->len < ev->min_len) { + bt_dev_err(hdev, "unexpected event 0x%2.2x length: %u < %u", + event, skb->len, ev->min_len); + return; + } + + /* Just warn if the length is over max_len size it still be + * possible to partially parse the event so leave to callback to + * decide if that is acceptable. + */ + if (skb->len > ev->max_len) + bt_dev_warn(hdev, "unexpected event 0x%2.2x length: %u > %u", + event, skb->len, ev->max_len); + + data = hci_ev_skb_pull(hdev, skb, event, ev->min_len); + if (!data) + return; + + if (ev->req) + ev->func_req(hdev, data, skb, opcode, status, req_complete, + req_complete_skb); + else + ev->func(hdev, data, skb); +} + void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_event_hdr *hdr = (void *) skb->data; @@ -7125,7 +7181,8 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb) event = hdr->evt; if (!event) { - bt_dev_warn(hdev, "Received unexpected HCI Event 00000000"); + bt_dev_warn(hdev, "Received unexpected HCI Event 0x%2.2x", + event); goto done; } @@ -7151,191 +7208,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb) /* Store wake reason if we're suspended */ hci_store_wake_reason(hdev, event, skb); - switch (event) { - case HCI_EV_INQUIRY_COMPLETE: - hci_inquiry_complete_evt(hdev, skb); - break; + bt_dev_dbg(hdev, "event 0x%2.2x", event); - case HCI_EV_INQUIRY_RESULT: - hci_inquiry_result_evt(hdev, skb); - break; - - case HCI_EV_CONN_COMPLETE: - hci_conn_complete_evt(hdev, skb); - break; - - case HCI_EV_CONN_REQUEST: - hci_conn_request_evt(hdev, skb); - break; - - case HCI_EV_DISCONN_COMPLETE: - hci_disconn_complete_evt(hdev, skb); - break; - - case HCI_EV_AUTH_COMPLETE: - hci_auth_complete_evt(hdev, skb); - break; - - case HCI_EV_REMOTE_NAME: - hci_remote_name_evt(hdev, skb); - break; - - case HCI_EV_ENCRYPT_CHANGE: - hci_encrypt_change_evt(hdev, skb); - break; - - case HCI_EV_CHANGE_LINK_KEY_COMPLETE: - hci_change_link_key_complete_evt(hdev, skb); - break; - - case HCI_EV_REMOTE_FEATURES: - hci_remote_features_evt(hdev, skb); - break; - - case HCI_EV_CMD_COMPLETE: - hci_cmd_complete_evt(hdev, skb, &opcode, &status, - &req_complete, &req_complete_skb); - break; - - case HCI_EV_CMD_STATUS: - hci_cmd_status_evt(hdev, skb, &opcode, &status, &req_complete, - &req_complete_skb); - break; - - case HCI_EV_HARDWARE_ERROR: - hci_hardware_error_evt(hdev, skb); - break; - - case HCI_EV_ROLE_CHANGE: - hci_role_change_evt(hdev, skb); - break; - - case HCI_EV_NUM_COMP_PKTS: - hci_num_comp_pkts_evt(hdev, skb); - break; - - case HCI_EV_MODE_CHANGE: - hci_mode_change_evt(hdev, skb); - break; - - case HCI_EV_PIN_CODE_REQ: - hci_pin_code_request_evt(hdev, skb); - break; - - case HCI_EV_LINK_KEY_REQ: - hci_link_key_request_evt(hdev, skb); - break; - - case HCI_EV_LINK_KEY_NOTIFY: - hci_link_key_notify_evt(hdev, skb); - break; - - case HCI_EV_CLOCK_OFFSET: - hci_clock_offset_evt(hdev, skb); - break; - - case HCI_EV_PKT_TYPE_CHANGE: - hci_pkt_type_change_evt(hdev, skb); - break; - - case HCI_EV_PSCAN_REP_MODE: - hci_pscan_rep_mode_evt(hdev, skb); - break; - - case HCI_EV_INQUIRY_RESULT_WITH_RSSI: - hci_inquiry_result_with_rssi_evt(hdev, skb); - break; - - case HCI_EV_REMOTE_EXT_FEATURES: - hci_remote_ext_features_evt(hdev, skb); - break; - - case HCI_EV_SYNC_CONN_COMPLETE: - hci_sync_conn_complete_evt(hdev, skb); - break; - - case HCI_EV_EXTENDED_INQUIRY_RESULT: - hci_extended_inquiry_result_evt(hdev, skb); - break; - - case HCI_EV_KEY_REFRESH_COMPLETE: - hci_key_refresh_complete_evt(hdev, skb); - break; - - case HCI_EV_IO_CAPA_REQUEST: - hci_io_capa_request_evt(hdev, skb); - break; - - case HCI_EV_IO_CAPA_REPLY: - hci_io_capa_reply_evt(hdev, skb); - break; - - case HCI_EV_USER_CONFIRM_REQUEST: - hci_user_confirm_request_evt(hdev, skb); - break; - - case HCI_EV_USER_PASSKEY_REQUEST: - hci_user_passkey_request_evt(hdev, skb); - break; - - case HCI_EV_USER_PASSKEY_NOTIFY: - hci_user_passkey_notify_evt(hdev, skb); - break; - - case HCI_EV_KEYPRESS_NOTIFY: - hci_keypress_notify_evt(hdev, skb); - break; - - case HCI_EV_SIMPLE_PAIR_COMPLETE: - hci_simple_pair_complete_evt(hdev, skb); - break; - - case HCI_EV_REMOTE_HOST_FEATURES: - hci_remote_host_features_evt(hdev, skb); - break; - - case HCI_EV_LE_META: - hci_le_meta_evt(hdev, skb); - break; - - case HCI_EV_REMOTE_OOB_DATA_REQUEST: - hci_remote_oob_data_request_evt(hdev, skb); - break; - -#if IS_ENABLED(CONFIG_BT_HS) - case HCI_EV_CHANNEL_SELECTED: - hci_chan_selected_evt(hdev, skb); - break; - - case HCI_EV_PHY_LINK_COMPLETE: - hci_phy_link_complete_evt(hdev, skb); - break; - - case HCI_EV_LOGICAL_LINK_COMPLETE: - hci_loglink_complete_evt(hdev, skb); - break; - - case HCI_EV_DISCONN_LOGICAL_LINK_COMPLETE: - hci_disconn_loglink_complete_evt(hdev, skb); - break; - - case HCI_EV_DISCONN_PHY_LINK_COMPLETE: - hci_disconn_phylink_complete_evt(hdev, skb); - break; -#endif - - case HCI_EV_NUM_COMP_BLOCKS: - hci_num_comp_blocks_evt(hdev, skb); - break; - - case HCI_EV_VENDOR: - msft_vendor_evt(hdev, skb); - break; - - default: - BT_DBG("%s event 0x%2.2x", hdev->name, event); - break; - } + hci_event_func(hdev, event, skb, &opcode, &status, &req_complete, + &req_complete_skb); if (req_complete) { req_complete(hdev, status, opcode); diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c index 1122097e1e49..6a943634b31a 100644 --- a/net/bluetooth/msft.c +++ b/net/bluetooth/msft.c @@ -590,7 +590,7 @@ void msft_unregister(struct hci_dev *hdev) kfree(msft); } -void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb) +void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { struct msft_data *msft = hdev->msft_data; u8 event; diff --git a/net/bluetooth/msft.h b/net/bluetooth/msft.h index b59b63dc0ea8..7fefc0655b6f 100644 --- a/net/bluetooth/msft.h +++ b/net/bluetooth/msft.h @@ -17,7 +17,7 @@ void msft_register(struct hci_dev *hdev); void msft_unregister(struct hci_dev *hdev); void msft_do_open(struct hci_dev *hdev); void msft_do_close(struct hci_dev *hdev); -void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb); +void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb); __u64 msft_get_features(struct hci_dev *hdev); int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor); int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor, From 95118dd4edfec950898a00180c6f998df0a6406d Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:55:04 -0800 Subject: [PATCH 29/62] Bluetooth: hci_event: Use of a function table to handle LE subevents This change the use of switch statement to a function table which is easier to extend and can include min/max length of each subevent. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_event.c | 251 +++++++++++++++++++------------------- 1 file changed, 124 insertions(+), 127 deletions(-) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 3953ec188ff6..cecd81e0504c 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -5969,16 +5969,12 @@ unlock: hci_dev_unlock(hdev); } -static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_le_conn_complete_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_le_conn_complete *ev; + struct hci_ev_le_conn_complete *ev = data; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_CONN_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); le_conn_complete_evt(hdev, ev->status, &ev->bdaddr, ev->bdaddr_type, NULL, ev->role, le16_to_cpu(ev->handle), @@ -5987,17 +5983,12 @@ static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) le16_to_cpu(ev->supervision_timeout)); } -static void hci_le_enh_conn_complete_evt(struct hci_dev *hdev, +static void hci_le_enh_conn_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_le_enh_conn_complete *ev; + struct hci_ev_le_enh_conn_complete *ev = data; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_ENHANCED_CONN_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); le_conn_complete_evt(hdev, ev->status, &ev->bdaddr, ev->bdaddr_type, &ev->local_rpa, ev->role, le16_to_cpu(ev->handle), @@ -6006,18 +5997,14 @@ static void hci_le_enh_conn_complete_evt(struct hci_dev *hdev, le16_to_cpu(ev->supervision_timeout)); } -static void hci_le_ext_adv_term_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_le_ext_adv_term_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_evt_le_ext_adv_set_term *ev; + struct hci_evt_le_ext_adv_set_term *ev = data; struct hci_conn *conn; struct adv_info *adv, *n; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_EXT_ADV_SET_TERM, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); adv = hci_find_adv_instance(hdev, ev->handle); @@ -6075,18 +6062,13 @@ static void hci_le_ext_adv_term_evt(struct hci_dev *hdev, struct sk_buff *skb) } } -static void hci_le_conn_update_complete_evt(struct hci_dev *hdev, +static void hci_le_conn_update_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_le_conn_update_complete *ev; + struct hci_ev_le_conn_update_complete *ev = data; struct hci_conn *conn; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_CONN_UPDATE_COMPLETE, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); if (ev->status) return; @@ -6402,14 +6384,10 @@ static void process_adv_report(struct hci_dev *hdev, u8 type, bdaddr_t *bdaddr, clear_pending_adv_report(hdev); } -static void hci_le_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_le_adv_report_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_le_advertising_report *ev; - - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_ADVERTISING_REPORT, - sizeof(*ev)); - if (!ev) - return; + struct hci_ev_le_advertising_report *ev = data; if (!ev->num) return; @@ -6487,14 +6465,10 @@ invalid: return LE_ADV_INVALID; } -static void hci_le_ext_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_le_ext_adv_report_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_le_ext_adv_report *ev; - - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_EXT_ADV_REPORT, - sizeof(*ev)); - if (!ev) - return; + struct hci_ev_le_ext_adv_report *ev = data; if (!ev->num) return; @@ -6528,18 +6502,13 @@ static void hci_le_ext_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_unlock(hdev); } -static void hci_le_remote_feat_complete_evt(struct hci_dev *hdev, +static void hci_le_remote_feat_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_le_remote_feat_complete *ev; + struct hci_ev_le_remote_feat_complete *ev = data; struct hci_conn *conn; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_EXT_ADV_REPORT, - sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); hci_dev_lock(hdev); @@ -6575,19 +6544,16 @@ static void hci_le_remote_feat_complete_evt(struct hci_dev *hdev, hci_dev_unlock(hdev); } -static void hci_le_ltk_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_le_ltk_request_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_le_ltk_req *ev; + struct hci_ev_le_ltk_req *ev = data; struct hci_cp_le_ltk_reply cp; struct hci_cp_le_ltk_neg_reply neg; struct hci_conn *conn; struct smp_ltk *ltk; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_LTK_REQ, sizeof(*ev)); - if (!ev) - return; - - BT_DBG("%s handle 0x%4.4x", hdev->name, __le16_to_cpu(ev->handle)); + bt_dev_dbg(hdev, "handle 0x%4.4x", __le16_to_cpu(ev->handle)); hci_dev_lock(hdev); @@ -6655,18 +6621,15 @@ static void send_conn_param_neg_reply(struct hci_dev *hdev, u16 handle, &cp); } -static void hci_le_remote_conn_param_req_evt(struct hci_dev *hdev, +static void hci_le_remote_conn_param_req_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_le_remote_conn_param_req *ev; + struct hci_ev_le_remote_conn_param_req *ev = data; struct hci_cp_le_conn_param_req_reply cp; struct hci_conn *hcon; u16 handle, min, max, latency, timeout; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_REMOTE_CONN_PARAM_REQ, - sizeof(*ev)); - if (!ev) - return; + bt_dev_dbg(hdev, "handle 0x%4.4x", __le16_to_cpu(ev->handle)); handle = le16_to_cpu(ev->handle); min = le16_to_cpu(ev->interval_min); @@ -6718,17 +6681,12 @@ static void hci_le_remote_conn_param_req_evt(struct hci_dev *hdev, hci_send_cmd(hdev, HCI_OP_LE_CONN_PARAM_REQ_REPLY, sizeof(cp), &cp); } -static void hci_le_direct_adv_report_evt(struct hci_dev *hdev, +static void hci_le_direct_adv_report_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_le_direct_adv_report *ev; + struct hci_ev_le_direct_adv_report *ev = data; int i; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_DIRECT_ADV_REPORT, - sizeof(*ev)); - if (!ev) - return; - if (!hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_DIRECT_ADV_REPORT, flex_array_size(ev, info, ev->num))) return; @@ -6750,17 +6708,13 @@ static void hci_le_direct_adv_report_evt(struct hci_dev *hdev, hci_dev_unlock(hdev); } -static void hci_le_phy_update_evt(struct hci_dev *hdev, struct sk_buff *skb) +static void hci_le_phy_update_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_le_phy_update_complete *ev; + struct hci_ev_le_phy_update_complete *ev = data; struct hci_conn *conn; - ev = hci_le_ev_skb_pull(hdev, skb, HCI_EV_LE_PHY_UPDATE_COMPLETE, - sizeof(*ev)); - if (ev) - return; - - BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); if (ev->status) return; @@ -6778,59 +6732,102 @@ unlock: hci_dev_unlock(hdev); } +#define HCI_LE_EV_VL(_op, _func, _min_len, _max_len) \ +[_op] = { \ + .func = _func, \ + .min_len = _min_len, \ + .max_len = _max_len, \ +} + +#define HCI_LE_EV(_op, _func, _len) \ + HCI_LE_EV_VL(_op, _func, _len, _len) + +#define HCI_LE_EV_STATUS(_op, _func) \ + HCI_LE_EV(_op, _func, sizeof(struct hci_ev_status)) + +/* Entries in this table shall have their position according to the subevent + * opcode they handle so the use of the macros above is recommend since it does + * attempt to initialize at its proper index using Designated Initializers that + * way events without a callback function can be ommited. + */ +static const struct hci_le_ev { + void (*func)(struct hci_dev *hdev, void *data, struct sk_buff *skb); + u16 min_len; + u16 max_len; +} hci_le_ev_table[U8_MAX + 1] = { + /* [0x01 = HCI_EV_LE_CONN_COMPLETE] */ + HCI_LE_EV(HCI_EV_LE_CONN_COMPLETE, hci_le_conn_complete_evt, + sizeof(struct hci_ev_le_conn_complete)), + /* [0x02 = HCI_EV_LE_ADVERTISING_REPORT] */ + HCI_LE_EV_VL(HCI_EV_LE_ADVERTISING_REPORT, hci_le_adv_report_evt, + sizeof(struct hci_ev_le_advertising_report), + HCI_MAX_EVENT_SIZE), + /* [0x03 = HCI_EV_LE_CONN_UPDATE_COMPLETE] */ + HCI_LE_EV(HCI_EV_LE_CONN_UPDATE_COMPLETE, + hci_le_conn_update_complete_evt, + sizeof(struct hci_ev_le_conn_update_complete)), + /* [0x04 = HCI_EV_LE_REMOTE_FEAT_COMPLETE] */ + HCI_LE_EV(HCI_EV_LE_REMOTE_FEAT_COMPLETE, + hci_le_remote_feat_complete_evt, + sizeof(struct hci_ev_le_remote_feat_complete)), + /* [0x05 = HCI_EV_LE_LTK_REQ] */ + HCI_LE_EV(HCI_EV_LE_LTK_REQ, hci_le_ltk_request_evt, + sizeof(struct hci_ev_le_ltk_req)), + /* [0x06 = HCI_EV_LE_REMOTE_CONN_PARAM_REQ] */ + HCI_LE_EV(HCI_EV_LE_REMOTE_CONN_PARAM_REQ, + hci_le_remote_conn_param_req_evt, + sizeof(struct hci_ev_le_remote_conn_param_req)), + /* [0x0a = HCI_EV_LE_ENHANCED_CONN_COMPLETE] */ + HCI_LE_EV(HCI_EV_LE_ENHANCED_CONN_COMPLETE, + hci_le_enh_conn_complete_evt, + sizeof(struct hci_ev_le_enh_conn_complete)), + /* [0x0b = HCI_EV_LE_DIRECT_ADV_REPORT] */ + HCI_LE_EV_VL(HCI_EV_LE_DIRECT_ADV_REPORT, hci_le_direct_adv_report_evt, + sizeof(struct hci_ev_le_direct_adv_report), + HCI_MAX_EVENT_SIZE), + /* [0x0c = HCI_EV_LE_PHY_UPDATE_COMPLETE] */ + HCI_LE_EV(HCI_EV_LE_PHY_UPDATE_COMPLETE, hci_le_phy_update_evt, + sizeof(struct hci_ev_le_phy_update_complete)), + /* [0x0d = HCI_EV_LE_EXT_ADV_REPORT] */ + HCI_LE_EV_VL(HCI_EV_LE_EXT_ADV_REPORT, hci_le_ext_adv_report_evt, + sizeof(struct hci_ev_le_ext_adv_report), + HCI_MAX_EVENT_SIZE), + /* [0x12 = HCI_EV_LE_EXT_ADV_SET_TERM] */ + HCI_LE_EV(HCI_EV_LE_EXT_ADV_SET_TERM, hci_le_ext_adv_term_evt, + sizeof(struct hci_evt_le_ext_adv_set_term)), +}; + static void hci_le_meta_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) { struct hci_ev_le_meta *ev = data; + const struct hci_le_ev *subev; - switch (ev->subevent) { - case HCI_EV_LE_CONN_COMPLETE: - hci_le_conn_complete_evt(hdev, skb); - break; + bt_dev_dbg(hdev, "subevent 0x%2.2x", ev->subevent); - case HCI_EV_LE_CONN_UPDATE_COMPLETE: - hci_le_conn_update_complete_evt(hdev, skb); - break; + subev = &hci_le_ev_table[ev->subevent]; + if (!subev->func) + return; - case HCI_EV_LE_ADVERTISING_REPORT: - hci_le_adv_report_evt(hdev, skb); - break; - - case HCI_EV_LE_REMOTE_FEAT_COMPLETE: - hci_le_remote_feat_complete_evt(hdev, skb); - break; - - case HCI_EV_LE_LTK_REQ: - hci_le_ltk_request_evt(hdev, skb); - break; - - case HCI_EV_LE_REMOTE_CONN_PARAM_REQ: - hci_le_remote_conn_param_req_evt(hdev, skb); - break; - - case HCI_EV_LE_DIRECT_ADV_REPORT: - hci_le_direct_adv_report_evt(hdev, skb); - break; - - case HCI_EV_LE_PHY_UPDATE_COMPLETE: - hci_le_phy_update_evt(hdev, skb); - break; - - case HCI_EV_LE_EXT_ADV_REPORT: - hci_le_ext_adv_report_evt(hdev, skb); - break; - - case HCI_EV_LE_ENHANCED_CONN_COMPLETE: - hci_le_enh_conn_complete_evt(hdev, skb); - break; - - case HCI_EV_LE_EXT_ADV_SET_TERM: - hci_le_ext_adv_term_evt(hdev, skb); - break; - - default: - break; + if (skb->len < subev->min_len) { + bt_dev_err(hdev, "unexpected subevent 0x%2.2x length: %u < %u", + ev->subevent, skb->len, subev->min_len); + return; } + + /* Just warn if the length is over max_len size it still be + * possible to partially parse the event so leave to callback to + * decide if that is acceptable. + */ + if (skb->len > subev->max_len) + bt_dev_warn(hdev, "unexpected subevent 0x%2.2x length: %u > %u", + ev->subevent, skb->len, subev->max_len); + + data = hci_le_ev_skb_pull(hdev, skb, ev->subevent, subev->min_len); + if (!data) + return; + + subev->func(hdev, data, skb); } static bool hci_get_cmd_complete(struct hci_dev *hdev, u16 opcode, From c8992cffbe7411c6da4c4416d5eecfc6b78e0fec Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:55:05 -0800 Subject: [PATCH 30/62] Bluetooth: hci_event: Use of a function table to handle Command Complete This change the use of switch statement to a function table which is easier to extend and can include min/max length of each command. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_event.c | 1852 +++++++++++++++---------------------- 1 file changed, 754 insertions(+), 1098 deletions(-) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index cecd81e0504c..313b7b79d5b5 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -81,14 +81,10 @@ static void *hci_le_ev_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, return data; } -static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb, - u8 *new_status) +static u8 hci_cc_inquiry_cancel(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_INQUIRY_CANCEL, sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); @@ -104,10 +100,8 @@ static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb, rp->status = 0x00; } - *new_status = rp->status; - if (rp->status) - return; + return rp->status; clear_bit(HCI_INQUIRY, &hdev->flags); smp_mb__after_atomic(); /* wake_up_bit advises about this barrier */ @@ -123,68 +117,62 @@ static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb, hci_dev_unlock(hdev); hci_conn_check_pending(hdev); + + return rp->status; } -static void hci_cc_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_periodic_inq(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_PERIODIC_INQ, sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_dev_set_flag(hdev, HCI_PERIODIC_INQ); + + return rp->status; } -static void hci_cc_exit_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_exit_periodic_inq(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_EXIT_PERIODIC_INQ, sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_dev_clear_flag(hdev, HCI_PERIODIC_INQ); hci_conn_check_pending(hdev); + + return rp->status; } -static void hci_cc_remote_name_req_cancel(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_remote_name_req_cancel(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_REMOTE_NAME_REQ_CANCEL, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + return rp->status; } -static void hci_cc_role_discovery(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_role_discovery(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_role_discovery *rp; + struct hci_rp_role_discovery *rp = data; struct hci_conn *conn; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_ROLE_DISCOVERY, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_dev_lock(hdev); @@ -193,21 +181,20 @@ static void hci_cc_role_discovery(struct hci_dev *hdev, struct sk_buff *skb) conn->role = rp->role; hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_link_policy(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_link_policy(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_link_policy *rp; + struct hci_rp_read_link_policy *rp = data; struct hci_conn *conn; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LINK_POLICY, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_dev_lock(hdev); @@ -216,26 +203,25 @@ static void hci_cc_read_link_policy(struct hci_dev *hdev, struct sk_buff *skb) conn->link_policy = __le16_to_cpu(rp->policy); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_write_link_policy(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_link_policy(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_write_link_policy *rp; + struct hci_rp_write_link_policy *rp = data; struct hci_conn *conn; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_LINK_POLICY, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LINK_POLICY); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -244,63 +230,55 @@ static void hci_cc_write_link_policy(struct hci_dev *hdev, struct sk_buff *skb) conn->link_policy = get_unaligned_le16(sent + 2); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_def_link_policy(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_def_link_policy(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_def_link_policy *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_DEF_LINK_POLICY, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_def_link_policy *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->link_policy = __le16_to_cpu(rp->policy); + + return rp->status; } -static void hci_cc_write_def_link_policy(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_write_def_link_policy(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_DEF_LINK_POLICY, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_DEF_LINK_POLICY); if (!sent) - return; + return rp->status; hdev->link_policy = get_unaligned_le16(sent); + + return rp->status; } -static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_reset(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_RESET, sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); clear_bit(HCI_RESET, &hdev->flags); if (rp->status) - return; + return rp->status; /* Reset all non-persistent flags */ hci_dev_clear_volatile_flags(hdev); @@ -322,66 +300,59 @@ static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb) hci_bdaddr_list_clear(&hdev->le_accept_list); hci_bdaddr_list_clear(&hdev->le_resolv_list); + + return rp->status; } -static void hci_cc_read_stored_link_key(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_stored_link_key(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_stored_link_key *rp; + struct hci_rp_read_stored_link_key *rp = data; struct hci_cp_read_stored_link_key *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_STORED_LINK_KEY, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_READ_STORED_LINK_KEY); if (!sent) - return; + return rp->status; if (!rp->status && sent->read_all == 0x01) { hdev->stored_max_keys = le16_to_cpu(rp->max_keys); hdev->stored_num_keys = le16_to_cpu(rp->num_keys); } + + return rp->status; } -static void hci_cc_delete_stored_link_key(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_delete_stored_link_key(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_delete_stored_link_key *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_DELETE_STORED_LINK_KEY, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_delete_stored_link_key *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (rp->num_keys <= hdev->stored_num_keys) hdev->stored_num_keys -= le16_to_cpu(rp->num_keys); else hdev->stored_num_keys = 0; + + return rp->status; } -static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_local_name(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_LOCAL_NAME, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LOCAL_NAME); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -391,40 +362,38 @@ static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb) memcpy(hdev->dev_name, sent, HCI_MAX_NAME_LENGTH); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_local_name(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_local_name(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_name *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_NAME, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_name *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (hci_dev_test_flag(hdev, HCI_SETUP) || hci_dev_test_flag(hdev, HCI_CONFIG)) memcpy(hdev->dev_name, rp->name, HCI_MAX_NAME_LENGTH); + + return rp->status; } -static void hci_cc_write_auth_enable(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_auth_enable(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_AUTH_ENABLE, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_AUTH_ENABLE); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -441,26 +410,25 @@ static void hci_cc_write_auth_enable(struct hci_dev *hdev, struct sk_buff *skb) mgmt_auth_enable_complete(hdev, rp->status); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_write_encrypt_mode(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_encrypt_mode(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; __u8 param; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_ENCRYPT_MODE, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_ENCRYPT_MODE); if (!sent) - return; + return rp->status; param = *((__u8 *) sent); @@ -468,23 +436,22 @@ static void hci_cc_write_encrypt_mode(struct hci_dev *hdev, struct sk_buff *skb) set_bit(HCI_ENCRYPT, &hdev->flags); else clear_bit(HCI_ENCRYPT, &hdev->flags); + + return rp->status; } -static void hci_cc_write_scan_enable(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_scan_enable(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; __u8 param; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SCAN_ENABLE, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SCAN_ENABLE); if (!sent) - return; + return rp->status; param = *((__u8 *) sent); @@ -507,26 +474,25 @@ static void hci_cc_write_scan_enable(struct hci_dev *hdev, struct sk_buff *skb) done: hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_set_event_filter(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_set_event_filter(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct hci_cp_set_event_filter *cp; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SCAN_ENABLE, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_SET_EVENT_FLT); if (!sent) - return; + return rp->status; cp = (struct hci_cp_set_event_filter *)sent; @@ -534,41 +500,39 @@ static void hci_cc_set_event_filter(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_clear_flag(hdev, HCI_EVENT_FILTER_CONFIGURED); else hci_dev_set_flag(hdev, HCI_EVENT_FILTER_CONFIGURED); + + return rp->status; } -static void hci_cc_read_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_class_of_dev(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_class_of_dev *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_CLASS_OF_DEV, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_class_of_dev *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; memcpy(hdev->dev_class, rp->dev_class, 3); bt_dev_dbg(hdev, "class 0x%.2x%.2x%.2x", hdev->dev_class[2], hdev->dev_class[1], hdev->dev_class[0]); + + return rp->status; } -static void hci_cc_write_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_class_of_dev(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_CLASS_OF_DEV, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_CLASS_OF_DEV); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -579,26 +543,25 @@ static void hci_cc_write_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) mgmt_set_class_of_dev_complete(hdev, sent, rp->status); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_voice_setting(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_voice_setting *rp; + struct hci_rp_read_voice_setting *rp = data; __u16 setting; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_VOICE_SETTING, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; setting = __le16_to_cpu(rp->voice_setting); if (hdev->voice_setting == setting) - return; + return rp->status; hdev->voice_setting = setting; @@ -606,33 +569,30 @@ static void hci_cc_read_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) if (hdev->notify) hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING); + + return rp->status; } -static void hci_cc_write_voice_setting(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_write_voice_setting(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; __u16 setting; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_VOICE_SETTING, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_VOICE_SETTING); if (!sent) - return; + return rp->status; setting = get_unaligned_le16(sent); if (hdev->voice_setting == setting) - return; + return rp->status; hdev->voice_setting = setting; @@ -640,42 +600,38 @@ static void hci_cc_write_voice_setting(struct hci_dev *hdev, if (hdev->notify) hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING); + + return rp->status; } -static void hci_cc_read_num_supported_iac(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_num_supported_iac(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_num_supported_iac *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_NUM_SUPPORTED_IAC, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_num_supported_iac *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->num_iac = rp->num_iac; bt_dev_dbg(hdev, "num iac %d", hdev->num_iac); + + return rp->status; } -static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_ssp_mode(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct hci_cp_write_ssp_mode *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SSP_MODE, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SSP_MODE); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -694,22 +650,21 @@ static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb) } hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_write_sc_support(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_sc_support(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct hci_cp_write_sc_support *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SC_SUPPORT, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SC_SUPPORT); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -728,20 +683,19 @@ static void hci_cc_write_sc_support(struct hci_dev *hdev, struct sk_buff *skb) } hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_local_version(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_local_version(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_version *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_VERSION, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_version *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (hci_dev_test_flag(hdev, HCI_SETUP) || hci_dev_test_flag(hdev, HCI_CONFIG)) { @@ -751,43 +705,37 @@ static void hci_cc_read_local_version(struct hci_dev *hdev, struct sk_buff *skb) hdev->manufacturer = __le16_to_cpu(rp->manufacturer); hdev->lmp_subver = __le16_to_cpu(rp->lmp_subver); } + + return rp->status; } -static void hci_cc_read_local_commands(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_local_commands(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_commands *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_COMMANDS, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_commands *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (hci_dev_test_flag(hdev, HCI_SETUP) || hci_dev_test_flag(hdev, HCI_CONFIG)) memcpy(hdev->commands, rp->commands, sizeof(hdev->commands)); + + return rp->status; } -static void hci_cc_read_auth_payload_timeout(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_auth_payload_timeout(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_auth_payload_to *rp; + struct hci_rp_read_auth_payload_to *rp = data; struct hci_conn *conn; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_AUTH_PAYLOAD_TO, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_dev_lock(hdev); @@ -796,27 +744,25 @@ static void hci_cc_read_auth_payload_timeout(struct hci_dev *hdev, conn->auth_payload_timeout = __le16_to_cpu(rp->timeout); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_write_auth_payload_timeout(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_write_auth_payload_timeout(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_write_auth_payload_to *rp; + struct hci_rp_write_auth_payload_to *rp = data; struct hci_conn *conn; void *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_AUTH_PAYLOAD_TO, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_AUTH_PAYLOAD_TO); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -825,22 +771,19 @@ static void hci_cc_write_auth_payload_timeout(struct hci_dev *hdev, conn->auth_payload_timeout = get_unaligned_le16(sent + 2); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_local_features(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_local_features(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_features *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_FEATURES, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_features *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; memcpy(hdev->features, rp->features, 8); @@ -880,60 +823,53 @@ static void hci_cc_read_local_features(struct hci_dev *hdev, if (hdev->features[0][5] & LMP_EDR_3S_ESCO) hdev->esco_type |= (ESCO_2EV5 | ESCO_3EV5); + + return rp->status; } -static void hci_cc_read_local_ext_features(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_local_ext_features(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_ext_features *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_EXT_FEATURES, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_ext_features *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (hdev->max_page < rp->max_page) hdev->max_page = rp->max_page; if (rp->page < HCI_MAX_PAGES) memcpy(hdev->features[rp->page], rp->features, 8); + + return rp->status; } -static void hci_cc_read_flow_control_mode(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_flow_control_mode(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_flow_control_mode *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_FLOW_CONTROL_MODE, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_flow_control_mode *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->flow_ctl_mode = rp->mode; + + return rp->status; } -static void hci_cc_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_buffer_size(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_buffer_size *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_BUFFER_SIZE, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_buffer_size *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->acl_mtu = __le16_to_cpu(rp->acl_mtu); hdev->sco_mtu = rp->sco_mtu; @@ -950,149 +886,130 @@ static void hci_cc_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb) BT_DBG("%s acl mtu %d:%d sco mtu %d:%d", hdev->name, hdev->acl_mtu, hdev->acl_pkts, hdev->sco_mtu, hdev->sco_pkts); + + return rp->status; } -static void hci_cc_read_bd_addr(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_bd_addr(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_bd_addr *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_BD_ADDR, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_bd_addr *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (test_bit(HCI_INIT, &hdev->flags)) bacpy(&hdev->bdaddr, &rp->bdaddr); if (hci_dev_test_flag(hdev, HCI_SETUP)) bacpy(&hdev->setup_addr, &rp->bdaddr); + + return rp->status; } -static void hci_cc_read_local_pairing_opts(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_local_pairing_opts(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_pairing_opts *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_PAIRING_OPTS, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_pairing_opts *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (hci_dev_test_flag(hdev, HCI_SETUP) || hci_dev_test_flag(hdev, HCI_CONFIG)) { hdev->pairing_opts = rp->pairing_opts; hdev->max_enc_key_size = rp->max_key_size; } + + return rp->status; } -static void hci_cc_read_page_scan_activity(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_page_scan_activity(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_page_scan_activity *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_PAGE_SCAN_ACTIVITY, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_page_scan_activity *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (test_bit(HCI_INIT, &hdev->flags)) { hdev->page_scan_interval = __le16_to_cpu(rp->interval); hdev->page_scan_window = __le16_to_cpu(rp->window); } + + return rp->status; } -static void hci_cc_write_page_scan_activity(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_write_page_scan_activity(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct hci_cp_write_page_scan_activity *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_PAGE_SCAN_ACTIVITY, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_PAGE_SCAN_ACTIVITY); if (!sent) - return; + return rp->status; hdev->page_scan_interval = __le16_to_cpu(sent->interval); hdev->page_scan_window = __le16_to_cpu(sent->window); + + return rp->status; } -static void hci_cc_read_page_scan_type(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_page_scan_type(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_page_scan_type *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_PAGE_SCAN_TYPE, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_page_scan_type *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (test_bit(HCI_INIT, &hdev->flags)) hdev->page_scan_type = rp->type; + + return rp->status; } -static void hci_cc_write_page_scan_type(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_write_page_scan_type(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; u8 *type; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_PAGE_SCAN_TYPE, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; type = hci_sent_cmd_data(hdev, HCI_OP_WRITE_PAGE_SCAN_TYPE); if (type) hdev->page_scan_type = *type; + + return rp->status; } -static void hci_cc_read_data_block_size(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_data_block_size(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_data_block_size *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_DATA_BLOCK_SIZE, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_data_block_size *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->block_mtu = __le16_to_cpu(rp->max_acl_len); hdev->block_len = __le16_to_cpu(rp->block_len); @@ -1102,24 +1019,21 @@ static void hci_cc_read_data_block_size(struct hci_dev *hdev, BT_DBG("%s blk mtu %d cnt %d len %d", hdev->name, hdev->block_mtu, hdev->block_cnt, hdev->block_len); + + return rp->status; } -static void hci_cc_read_clock(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_clock(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_clock *rp; + struct hci_rp_read_clock *rp = data; struct hci_cp_read_clock *cp; struct hci_conn *conn; - BT_DBG("%s", hdev->name); - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_CLOCK, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_dev_lock(hdev); @@ -1140,22 +1054,18 @@ static void hci_cc_read_clock(struct hci_dev *hdev, struct sk_buff *skb) unlock: hci_dev_unlock(hdev); + return rp->status; } -static void hci_cc_read_local_amp_info(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_local_amp_info(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_amp_info *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_AMP_INFO, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_amp_info *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->amp_status = rp->amp_status; hdev->amp_total_bw = __le32_to_cpu(rp->total_bw); @@ -1167,77 +1077,67 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev, hdev->amp_assoc_size = __le16_to_cpu(rp->max_assoc_size); hdev->amp_be_flush_to = __le32_to_cpu(rp->be_flush_to); hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to); + + return rp->status; } -static void hci_cc_read_inq_rsp_tx_power(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_inq_rsp_tx_power(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_inq_rsp_tx_power *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_INQ_RSP_TX_POWER, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_inq_rsp_tx_power *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->inq_tx_power = rp->tx_power; + + return rp->status; } -static void hci_cc_read_def_err_data_reporting(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_def_err_data_reporting(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_def_err_data_reporting *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_DEF_ERR_DATA_REPORTING, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_def_err_data_reporting *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->err_data_reporting = rp->err_data_reporting; + + return rp->status; } -static void hci_cc_write_def_err_data_reporting(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_write_def_err_data_reporting(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct hci_cp_write_def_err_data_reporting *cp; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_DEF_ERR_DATA_REPORTING, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_WRITE_DEF_ERR_DATA_REPORTING); if (!cp) - return; + return rp->status; hdev->err_data_reporting = cp->err_data_reporting; + + return rp->status; } -static void hci_cc_pin_code_reply(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_pin_code_reply(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_pin_code_reply *rp; + struct hci_rp_pin_code_reply *rp = data; struct hci_cp_pin_code_reply *cp; struct hci_conn *conn; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_PIN_CODE_REPLY, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); hci_dev_lock(hdev); @@ -1258,15 +1158,13 @@ static void hci_cc_pin_code_reply(struct hci_dev *hdev, struct sk_buff *skb) unlock: hci_dev_unlock(hdev); + return rp->status; } -static void hci_cc_pin_code_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_pin_code_neg_reply(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_pin_code_neg_reply *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_PIN_CODE_NEG_REPLY, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_pin_code_neg_reply *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); @@ -1277,22 +1175,19 @@ static void hci_cc_pin_code_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) rp->status); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_read_buffer_size(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_buffer_size(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_buffer_size *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_BUFFER_SIZE, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_buffer_size *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->le_mtu = __le16_to_cpu(rp->le_mtu); hdev->le_pkts = rp->le_max_pkt; @@ -1300,51 +1195,44 @@ static void hci_cc_le_read_buffer_size(struct hci_dev *hdev, hdev->le_cnt = hdev->le_pkts; BT_DBG("%s le mtu %d:%d", hdev->name, hdev->le_mtu, hdev->le_pkts); + + return rp->status; } -static void hci_cc_le_read_local_features(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_local_features(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_local_features *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_LOCAL_FEATURES, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_local_features *rp = data; BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); if (rp->status) - return; + return rp->status; memcpy(hdev->le_features, rp->features, 8); + + return rp->status; } -static void hci_cc_le_read_adv_tx_power(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_adv_tx_power(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_adv_tx_power *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_ADV_TX_POWER, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_adv_tx_power *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->adv_tx_power = rp->tx_power; + + return rp->status; } -static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_user_confirm_reply(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_user_confirm_reply *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_USER_CONFIRM_REPLY, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_user_confirm_reply *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); @@ -1355,17 +1243,14 @@ static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb) rp->status); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_user_confirm_neg_reply(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_user_confirm_neg_reply(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_user_confirm_reply *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_USER_CONFIRM_NEG_REPLY, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_user_confirm_reply *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); @@ -1376,15 +1261,14 @@ static void hci_cc_user_confirm_neg_reply(struct hci_dev *hdev, ACL_LINK, 0, rp->status); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_user_passkey_reply(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_user_passkey_reply(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_user_confirm_reply *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_USER_PASSKEY_REPLY, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_user_confirm_reply *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); @@ -1395,16 +1279,14 @@ static void hci_cc_user_passkey_reply(struct hci_dev *hdev, struct sk_buff *skb) 0, rp->status); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_user_passkey_neg_reply(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_user_passkey_neg_reply(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_user_confirm_reply *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_USER_PASSKEY_NEG_REPLY, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_user_confirm_reply *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); @@ -1415,49 +1297,44 @@ static void hci_cc_user_passkey_neg_reply(struct hci_dev *hdev, ACL_LINK, 0, rp->status); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_local_oob_data(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_local_oob_data(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_oob_data *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_OOB_DATA, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_oob_data *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + return rp->status; } -static void hci_cc_read_local_oob_ext_data(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_read_local_oob_ext_data(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_local_oob_ext_data *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_LOCAL_OOB_EXT_DATA, sizeof(*rp)); - if (!rp) - return; + struct hci_rp_read_local_oob_ext_data *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + return rp->status; } -static void hci_cc_le_set_random_addr(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_le_set_random_addr(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; bdaddr_t *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_RANDOM_ADDR, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_RANDOM_ADDR); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -1470,25 +1347,24 @@ static void hci_cc_le_set_random_addr(struct hci_dev *hdev, struct sk_buff *skb) } hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_set_default_phy(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_le_set_default_phy(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct hci_cp_le_set_default_phy *cp; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_DEFAULT_PHY, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_DEFAULT_PHY); if (!cp) - return; + return rp->status; hci_dev_lock(hdev); @@ -1496,24 +1372,21 @@ static void hci_cc_le_set_default_phy(struct hci_dev *hdev, struct sk_buff *skb) hdev->le_rx_def_phys = cp->rx_phys; hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_set_adv_set_random_addr(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_set_adv_set_random_addr(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct hci_cp_le_set_adv_set_rand_addr *cp; struct adv_info *adv; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_ADV_SET_RAND_ADDR, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_SET_RAND_ADDR); /* Update only in case the adv instance since handle 0x00 shall be using @@ -1521,7 +1394,7 @@ static void hci_cc_le_set_adv_set_random_addr(struct hci_dev *hdev, * non-extended adverting. */ if (!cp || !cp->handle) - return; + return rp->status; hci_dev_lock(hdev); @@ -1537,26 +1410,25 @@ static void hci_cc_le_set_adv_set_random_addr(struct hci_dev *hdev, } hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_remove_adv_set(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_le_remove_adv_set(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; u8 *instance; int err; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_REMOVE_ADV_SET, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; instance = hci_sent_cmd_data(hdev, HCI_OP_LE_REMOVE_ADV_SET); if (!instance) - return; + return rp->status; hci_dev_lock(hdev); @@ -1566,25 +1438,24 @@ static void hci_cc_le_remove_adv_set(struct hci_dev *hdev, struct sk_buff *skb) *instance); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_clear_adv_sets(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_le_clear_adv_sets(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct adv_info *adv, *n; int err; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_CLEAR_ADV_SETS, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; if (!hci_sent_cmd_data(hdev, HCI_OP_LE_CLEAR_ADV_SETS)) - return; + return rp->status; hci_dev_lock(hdev); @@ -1598,44 +1469,40 @@ static void hci_cc_le_clear_adv_sets(struct hci_dev *hdev, struct sk_buff *skb) } hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_read_transmit_power(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_transmit_power(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_transmit_power *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_TRANSMIT_POWER, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_transmit_power *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->min_le_tx_power = rp->min_le_tx_power; hdev->max_le_tx_power = rp->max_le_tx_power; + + return rp->status; } -static void hci_cc_le_set_adv_enable(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_le_set_adv_enable(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; __u8 *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_ADV_ENABLE, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_ENABLE); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -1657,29 +1524,26 @@ static void hci_cc_le_set_adv_enable(struct hci_dev *hdev, struct sk_buff *skb) } hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_set_ext_adv_enable(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_set_ext_adv_enable(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_set_ext_adv_enable *cp; struct hci_cp_ext_adv_set *set; struct adv_info *adv = NULL, *n; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_EXT_ADV_ENABLE, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_EXT_ADV_ENABLE); if (!cp) - return; + return rp->status; set = (void *)cp->data; @@ -1726,53 +1590,48 @@ static void hci_cc_le_set_ext_adv_enable(struct hci_dev *hdev, unlock: hci_dev_unlock(hdev); + return rp->status; } -static void hci_cc_le_set_scan_param(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_le_set_scan_param(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_set_scan_param *cp; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_SCAN_PARAM, sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_SCAN_PARAM); if (!cp) - return; + return rp->status; hci_dev_lock(hdev); hdev->le_scan_type = cp->type; hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_set_ext_scan_param(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_set_ext_scan_param(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_set_ext_scan_params *cp; - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; struct hci_cp_le_scan_phy_params *phy_param; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_EXT_SCAN_PARAMS, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_EXT_SCAN_PARAMS); if (!cp) - return; + return rp->status; phy_param = (void *)cp->data; @@ -1781,6 +1640,8 @@ static void hci_cc_le_set_ext_scan_param(struct hci_dev *hdev, hdev->le_scan_type = phy_param->type; hci_dev_unlock(hdev); + + return rp->status; } static bool has_pending_adv_report(struct hci_dev *hdev) @@ -1866,320 +1727,273 @@ static void le_set_scan_enable_complete(struct hci_dev *hdev, u8 enable) hci_dev_unlock(hdev); } -static void hci_cc_le_set_scan_enable(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_set_scan_enable(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_set_scan_enable *cp; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_SCAN_ENABLE, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_SCAN_ENABLE); if (!cp) - return; + return rp->status; le_set_scan_enable_complete(hdev, cp->enable); + + return rp->status; } -static void hci_cc_le_set_ext_scan_enable(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_set_ext_scan_enable(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_set_ext_scan_enable *cp; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_EXT_SCAN_ENABLE, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_EXT_SCAN_ENABLE); if (!cp) - return; + return rp->status; le_set_scan_enable_complete(hdev, cp->enable); + + return rp->status; } -static void hci_cc_le_read_num_adv_sets(struct hci_dev *hdev, +static u8 hci_cc_le_read_num_adv_sets(struct hci_dev *hdev, void *data, struct sk_buff *skb) { - struct hci_rp_le_read_num_supported_adv_sets *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_NUM_SUPPORTED_ADV_SETS, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_num_supported_adv_sets *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x No of Adv sets %u", rp->status, rp->num_of_sets); if (rp->status) - return; + return rp->status; hdev->le_num_of_adv_sets = rp->num_of_sets; + + return rp->status; } -static void hci_cc_le_read_accept_list_size(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_accept_list_size(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_accept_list_size *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_ACCEPT_LIST_SIZE, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_accept_list_size *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x size %u", rp->status, rp->size); if (rp->status) - return; + return rp->status; hdev->le_accept_list_size = rp->size; + + return rp->status; } -static void hci_cc_le_clear_accept_list(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_clear_accept_list(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_CLEAR_ACCEPT_LIST, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_bdaddr_list_clear(&hdev->le_accept_list); + + return rp->status; } -static void hci_cc_le_add_to_accept_list(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_add_to_accept_list(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_add_to_accept_list *sent; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_ADD_TO_ACCEPT_LIST, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_ADD_TO_ACCEPT_LIST); if (!sent) - return; + return rp->status; hci_bdaddr_list_add(&hdev->le_accept_list, &sent->bdaddr, sent->bdaddr_type); + + return rp->status; } -static void hci_cc_le_del_from_accept_list(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_del_from_accept_list(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_del_from_accept_list *sent; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_DEL_FROM_ACCEPT_LIST, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_DEL_FROM_ACCEPT_LIST); if (!sent) - return; + return rp->status; hci_bdaddr_list_del(&hdev->le_accept_list, &sent->bdaddr, sent->bdaddr_type); + + return rp->status; } -static void hci_cc_le_read_supported_states(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_supported_states(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_supported_states *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_SUPPORTED_STATES, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_supported_states *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; memcpy(hdev->le_states, rp->le_states, 8); + + return rp->status; } -static void hci_cc_le_read_def_data_len(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_def_data_len(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_def_data_len *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_DEF_DATA_LEN, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_def_data_len *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->le_def_tx_len = le16_to_cpu(rp->tx_len); hdev->le_def_tx_time = le16_to_cpu(rp->tx_time); + + return rp->status; } -static void hci_cc_le_write_def_data_len(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_write_def_data_len(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_write_def_data_len *sent; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_WRITE_DEF_DATA_LEN, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_WRITE_DEF_DATA_LEN); if (!sent) - return; + return rp->status; hdev->le_def_tx_len = le16_to_cpu(sent->tx_len); hdev->le_def_tx_time = le16_to_cpu(sent->tx_time); + + return rp->status; } -static void hci_cc_le_add_to_resolv_list(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_add_to_resolv_list(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_add_to_resolv_list *sent; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_ADD_TO_RESOLV_LIST, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_ADD_TO_RESOLV_LIST); if (!sent) - return; + return rp->status; hci_bdaddr_list_add_with_irk(&hdev->le_resolv_list, &sent->bdaddr, sent->bdaddr_type, sent->peer_irk, sent->local_irk); + + return rp->status; } -static void hci_cc_le_del_from_resolv_list(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_del_from_resolv_list(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_del_from_resolv_list *sent; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_DEL_FROM_RESOLV_LIST, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_DEL_FROM_RESOLV_LIST); if (!sent) - return; + return rp->status; hci_bdaddr_list_del_with_irk(&hdev->le_resolv_list, &sent->bdaddr, sent->bdaddr_type); + + return rp->status; } -static void hci_cc_le_clear_resolv_list(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_clear_resolv_list(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_CLEAR_RESOLV_LIST, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_bdaddr_list_clear(&hdev->le_resolv_list); + + return rp->status; } -static void hci_cc_le_read_resolv_list_size(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_resolv_list_size(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_resolv_list_size *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_RESOLV_LIST_SIZE, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_resolv_list_size *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x size %u", rp->status, rp->size); if (rp->status) - return; + return rp->status; hdev->le_resolv_list_size = rp->size; + + return rp->status; } -static void hci_cc_le_set_addr_resolution_enable(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_set_addr_resolution_enable(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; __u8 *sent; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_ADDR_RESOLV_ENABLE, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADDR_RESOLV_ENABLE); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -2189,48 +2003,42 @@ static void hci_cc_le_set_addr_resolution_enable(struct hci_dev *hdev, hci_dev_clear_flag(hdev, HCI_LL_RPA_RESOLUTION); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_le_read_max_data_len(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_le_read_max_data_len(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_read_max_data_len *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_READ_MAX_DATA_LEN, - sizeof(*rp)); - if (!rp) - return; + struct hci_rp_le_read_max_data_len *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hdev->le_max_tx_len = le16_to_cpu(rp->tx_len); hdev->le_max_tx_time = le16_to_cpu(rp->tx_time); hdev->le_max_rx_len = le16_to_cpu(rp->rx_len); hdev->le_max_rx_time = le16_to_cpu(rp->rx_time); + + return rp->status; } -static void hci_cc_write_le_host_supported(struct hci_dev *hdev, - struct sk_buff *skb) +static u8 hci_cc_write_le_host_supported(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_write_le_host_supported *sent; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_LE_HOST_SUPPORTED, - sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -2249,50 +2057,47 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev, hdev->features[1][0] &= ~LMP_HOST_LE_BREDR; hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_set_adv_param(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_set_adv_param(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_le_set_adv_param *cp; - struct hci_ev_status *rp; - - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_ADV_PARAM, sizeof(*rp)); - if (!rp) - return; + struct hci_ev_status *rp = data; bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_PARAM); if (!cp) - return; + return rp->status; hci_dev_lock(hdev); hdev->adv_addr_type = cp->own_address_type; hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_set_ext_adv_param(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_set_ext_adv_param(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_le_set_ext_adv_params *rp; + struct hci_rp_le_set_ext_adv_params *rp = data; struct hci_cp_le_set_ext_adv_params *cp; struct adv_info *adv_instance; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_LE_SET_EXT_ADV_PARAMS, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_EXT_ADV_PARAMS); if (!cp) - return; + return rp->status; hci_dev_lock(hdev); hdev->adv_addr_type = cp->own_addr_type; @@ -2308,21 +2113,20 @@ static void hci_cc_set_ext_adv_param(struct hci_dev *hdev, struct sk_buff *skb) hci_req_update_adv_data(hdev, cp->handle); hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_rssi(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_rssi(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_rp_read_rssi *rp; + struct hci_rp_read_rssi *rp = data; struct hci_conn *conn; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_RSSI, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; hci_dev_lock(hdev); @@ -2331,26 +2135,25 @@ static void hci_cc_read_rssi(struct hci_dev *hdev, struct sk_buff *skb) conn->rssi = rp->rssi; hci_dev_unlock(hdev); + + return rp->status; } -static void hci_cc_read_tx_power(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_read_tx_power(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { struct hci_cp_read_tx_power *sent; - struct hci_rp_read_tx_power *rp; + struct hci_rp_read_tx_power *rp = data; struct hci_conn *conn; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_READ_TX_POWER, sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; sent = hci_sent_cmd_data(hdev, HCI_OP_READ_TX_POWER); if (!sent) - return; + return rp->status; hci_dev_lock(hdev); @@ -2369,26 +2172,25 @@ static void hci_cc_read_tx_power(struct hci_dev *hdev, struct sk_buff *skb) unlock: hci_dev_unlock(hdev); + return rp->status; } -static void hci_cc_write_ssp_debug_mode(struct hci_dev *hdev, struct sk_buff *skb) +static u8 hci_cc_write_ssp_debug_mode(struct hci_dev *hdev, void *data, + struct sk_buff *skb) { - struct hci_ev_status *rp; + struct hci_ev_status *rp = data; u8 *mode; - rp = hci_cc_skb_pull(hdev, skb, HCI_OP_WRITE_SSP_DEBUG_MODE, - sizeof(*rp)); - if (!rp) - return; - bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); if (rp->status) - return; + return rp->status; mode = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SSP_DEBUG_MODE); if (mode) hdev->ssp_debug_mode = *mode; + + return rp->status; } static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status) @@ -3909,372 +3711,226 @@ static inline void handle_cmd_cnt_and_timer(struct hci_dev *hdev, u8 ncmd) } } +#define HCI_CC_VL(_op, _func, _min, _max) \ +{ \ + .op = _op, \ + .func = _func, \ + .min_len = _min, \ + .max_len = _max, \ +} + +#define HCI_CC(_op, _func, _len) \ + HCI_CC_VL(_op, _func, _len, _len) + +#define HCI_CC_STATUS(_op, _func) \ + HCI_CC(_op, _func, sizeof(struct hci_ev_status)) + +static const struct hci_cc { + u16 op; + u8 (*func)(struct hci_dev *hdev, void *data, struct sk_buff *skb); + u16 min_len; + u16 max_len; +} hci_cc_table[] = { + HCI_CC_STATUS(HCI_OP_INQUIRY_CANCEL, hci_cc_inquiry_cancel), + HCI_CC_STATUS(HCI_OP_PERIODIC_INQ, hci_cc_periodic_inq), + HCI_CC_STATUS(HCI_OP_EXIT_PERIODIC_INQ, hci_cc_exit_periodic_inq), + HCI_CC_STATUS(HCI_OP_REMOTE_NAME_REQ_CANCEL, + hci_cc_remote_name_req_cancel), + HCI_CC(HCI_OP_ROLE_DISCOVERY, hci_cc_role_discovery, + sizeof(struct hci_rp_role_discovery)), + HCI_CC(HCI_OP_READ_LINK_POLICY, hci_cc_read_link_policy, + sizeof(struct hci_rp_read_link_policy)), + HCI_CC(HCI_OP_WRITE_LINK_POLICY, hci_cc_write_link_policy, + sizeof(struct hci_rp_write_link_policy)), + HCI_CC(HCI_OP_READ_DEF_LINK_POLICY, hci_cc_read_def_link_policy, + sizeof(struct hci_rp_read_def_link_policy)), + HCI_CC_STATUS(HCI_OP_WRITE_DEF_LINK_POLICY, + hci_cc_write_def_link_policy), + HCI_CC_STATUS(HCI_OP_RESET, hci_cc_reset), + HCI_CC(HCI_OP_READ_STORED_LINK_KEY, hci_cc_read_stored_link_key, + sizeof(struct hci_rp_read_stored_link_key)), + HCI_CC(HCI_OP_DELETE_STORED_LINK_KEY, hci_cc_delete_stored_link_key, + sizeof(struct hci_rp_delete_stored_link_key)), + HCI_CC_STATUS(HCI_OP_WRITE_LOCAL_NAME, hci_cc_write_local_name), + HCI_CC(HCI_OP_READ_LOCAL_NAME, hci_cc_read_local_name, + sizeof(struct hci_rp_read_local_name)), + HCI_CC_STATUS(HCI_OP_WRITE_AUTH_ENABLE, hci_cc_write_auth_enable), + HCI_CC_STATUS(HCI_OP_WRITE_ENCRYPT_MODE, hci_cc_write_encrypt_mode), + HCI_CC_STATUS(HCI_OP_WRITE_SCAN_ENABLE, hci_cc_write_scan_enable), + HCI_CC_STATUS(HCI_OP_SET_EVENT_FLT, hci_cc_set_event_filter), + HCI_CC(HCI_OP_READ_CLASS_OF_DEV, hci_cc_read_class_of_dev, + sizeof(struct hci_rp_read_class_of_dev)), + HCI_CC_STATUS(HCI_OP_WRITE_CLASS_OF_DEV, hci_cc_write_class_of_dev), + HCI_CC(HCI_OP_READ_VOICE_SETTING, hci_cc_read_voice_setting, + sizeof(struct hci_rp_read_voice_setting)), + HCI_CC_STATUS(HCI_OP_WRITE_VOICE_SETTING, hci_cc_write_voice_setting), + HCI_CC(HCI_OP_READ_NUM_SUPPORTED_IAC, hci_cc_read_num_supported_iac, + sizeof(struct hci_rp_read_num_supported_iac)), + HCI_CC_STATUS(HCI_OP_WRITE_SSP_MODE, hci_cc_write_ssp_mode), + HCI_CC_STATUS(HCI_OP_WRITE_SC_SUPPORT, hci_cc_write_sc_support), + HCI_CC(HCI_OP_READ_AUTH_PAYLOAD_TO, hci_cc_read_auth_payload_timeout, + sizeof(struct hci_rp_read_auth_payload_to)), + HCI_CC(HCI_OP_WRITE_AUTH_PAYLOAD_TO, hci_cc_write_auth_payload_timeout, + sizeof(struct hci_rp_write_auth_payload_to)), + HCI_CC(HCI_OP_READ_LOCAL_VERSION, hci_cc_read_local_version, + sizeof(struct hci_rp_read_local_version)), + HCI_CC(HCI_OP_READ_LOCAL_COMMANDS, hci_cc_read_local_commands, + sizeof(struct hci_rp_read_local_commands)), + HCI_CC(HCI_OP_READ_LOCAL_FEATURES, hci_cc_read_local_features, + sizeof(struct hci_rp_read_local_features)), + HCI_CC(HCI_OP_READ_LOCAL_EXT_FEATURES, hci_cc_read_local_ext_features, + sizeof(struct hci_rp_read_local_ext_features)), + HCI_CC(HCI_OP_READ_BUFFER_SIZE, hci_cc_read_buffer_size, + sizeof(struct hci_rp_read_buffer_size)), + HCI_CC(HCI_OP_READ_BD_ADDR, hci_cc_read_bd_addr, + sizeof(struct hci_rp_read_bd_addr)), + HCI_CC(HCI_OP_READ_LOCAL_PAIRING_OPTS, hci_cc_read_local_pairing_opts, + sizeof(struct hci_rp_read_local_pairing_opts)), + HCI_CC(HCI_OP_READ_PAGE_SCAN_ACTIVITY, hci_cc_read_page_scan_activity, + sizeof(struct hci_rp_read_page_scan_activity)), + HCI_CC_STATUS(HCI_OP_WRITE_PAGE_SCAN_ACTIVITY, + hci_cc_write_page_scan_activity), + HCI_CC(HCI_OP_READ_PAGE_SCAN_TYPE, hci_cc_read_page_scan_type, + sizeof(struct hci_rp_read_page_scan_type)), + HCI_CC_STATUS(HCI_OP_WRITE_PAGE_SCAN_TYPE, hci_cc_write_page_scan_type), + HCI_CC(HCI_OP_READ_DATA_BLOCK_SIZE, hci_cc_read_data_block_size, + sizeof(struct hci_rp_read_data_block_size)), + HCI_CC(HCI_OP_READ_FLOW_CONTROL_MODE, hci_cc_read_flow_control_mode, + sizeof(struct hci_rp_read_flow_control_mode)), + HCI_CC(HCI_OP_READ_LOCAL_AMP_INFO, hci_cc_read_local_amp_info, + sizeof(struct hci_rp_read_local_amp_info)), + HCI_CC(HCI_OP_READ_CLOCK, hci_cc_read_clock, + sizeof(struct hci_rp_read_clock)), + HCI_CC(HCI_OP_READ_INQ_RSP_TX_POWER, hci_cc_read_inq_rsp_tx_power, + sizeof(struct hci_rp_read_inq_rsp_tx_power)), + HCI_CC(HCI_OP_READ_DEF_ERR_DATA_REPORTING, + hci_cc_read_def_err_data_reporting, + sizeof(struct hci_rp_read_def_err_data_reporting)), + HCI_CC_STATUS(HCI_OP_WRITE_DEF_ERR_DATA_REPORTING, + hci_cc_write_def_err_data_reporting), + HCI_CC(HCI_OP_PIN_CODE_REPLY, hci_cc_pin_code_reply, + sizeof(struct hci_rp_pin_code_reply)), + HCI_CC(HCI_OP_PIN_CODE_NEG_REPLY, hci_cc_pin_code_neg_reply, + sizeof(struct hci_rp_pin_code_neg_reply)), + HCI_CC(HCI_OP_READ_LOCAL_OOB_DATA, hci_cc_read_local_oob_data, + sizeof(struct hci_rp_read_local_oob_data)), + HCI_CC(HCI_OP_READ_LOCAL_OOB_EXT_DATA, hci_cc_read_local_oob_ext_data, + sizeof(struct hci_rp_read_local_oob_ext_data)), + HCI_CC(HCI_OP_LE_READ_BUFFER_SIZE, hci_cc_le_read_buffer_size, + sizeof(struct hci_rp_le_read_buffer_size)), + HCI_CC(HCI_OP_LE_READ_LOCAL_FEATURES, hci_cc_le_read_local_features, + sizeof(struct hci_rp_le_read_local_features)), + HCI_CC(HCI_OP_LE_READ_ADV_TX_POWER, hci_cc_le_read_adv_tx_power, + sizeof(struct hci_rp_le_read_adv_tx_power)), + HCI_CC(HCI_OP_USER_CONFIRM_REPLY, hci_cc_user_confirm_reply, + sizeof(struct hci_rp_user_confirm_reply)), + HCI_CC(HCI_OP_USER_CONFIRM_NEG_REPLY, hci_cc_user_confirm_neg_reply, + sizeof(struct hci_rp_user_confirm_reply)), + HCI_CC(HCI_OP_USER_PASSKEY_REPLY, hci_cc_user_passkey_reply, + sizeof(struct hci_rp_user_confirm_reply)), + HCI_CC(HCI_OP_USER_PASSKEY_NEG_REPLY, hci_cc_user_passkey_neg_reply, + sizeof(struct hci_rp_user_confirm_reply)), + HCI_CC_STATUS(HCI_OP_LE_SET_RANDOM_ADDR, hci_cc_le_set_random_addr), + HCI_CC_STATUS(HCI_OP_LE_SET_ADV_ENABLE, hci_cc_le_set_adv_enable), + HCI_CC_STATUS(HCI_OP_LE_SET_SCAN_PARAM, hci_cc_le_set_scan_param), + HCI_CC_STATUS(HCI_OP_LE_SET_SCAN_ENABLE, hci_cc_le_set_scan_enable), + HCI_CC(HCI_OP_LE_READ_ACCEPT_LIST_SIZE, + hci_cc_le_read_accept_list_size, + sizeof(struct hci_rp_le_read_accept_list_size)), + HCI_CC_STATUS(HCI_OP_LE_CLEAR_ACCEPT_LIST, hci_cc_le_clear_accept_list), + HCI_CC_STATUS(HCI_OP_LE_ADD_TO_ACCEPT_LIST, + hci_cc_le_add_to_accept_list), + HCI_CC_STATUS(HCI_OP_LE_DEL_FROM_ACCEPT_LIST, + hci_cc_le_del_from_accept_list), + HCI_CC(HCI_OP_LE_READ_SUPPORTED_STATES, hci_cc_le_read_supported_states, + sizeof(struct hci_rp_le_read_supported_states)), + HCI_CC(HCI_OP_LE_READ_DEF_DATA_LEN, hci_cc_le_read_def_data_len, + sizeof(struct hci_rp_le_read_def_data_len)), + HCI_CC_STATUS(HCI_OP_LE_WRITE_DEF_DATA_LEN, + hci_cc_le_write_def_data_len), + HCI_CC_STATUS(HCI_OP_LE_ADD_TO_RESOLV_LIST, + hci_cc_le_add_to_resolv_list), + HCI_CC_STATUS(HCI_OP_LE_DEL_FROM_RESOLV_LIST, + hci_cc_le_del_from_resolv_list), + HCI_CC_STATUS(HCI_OP_LE_CLEAR_RESOLV_LIST, + hci_cc_le_clear_resolv_list), + HCI_CC(HCI_OP_LE_READ_RESOLV_LIST_SIZE, hci_cc_le_read_resolv_list_size, + sizeof(struct hci_rp_le_read_resolv_list_size)), + HCI_CC_STATUS(HCI_OP_LE_SET_ADDR_RESOLV_ENABLE, + hci_cc_le_set_addr_resolution_enable), + HCI_CC(HCI_OP_LE_READ_MAX_DATA_LEN, hci_cc_le_read_max_data_len, + sizeof(struct hci_rp_le_read_max_data_len)), + HCI_CC_STATUS(HCI_OP_WRITE_LE_HOST_SUPPORTED, + hci_cc_write_le_host_supported), + HCI_CC_STATUS(HCI_OP_LE_SET_ADV_PARAM, hci_cc_set_adv_param), + HCI_CC(HCI_OP_READ_RSSI, hci_cc_read_rssi, + sizeof(struct hci_rp_read_rssi)), + HCI_CC(HCI_OP_READ_TX_POWER, hci_cc_read_tx_power, + sizeof(struct hci_rp_read_tx_power)), + HCI_CC_STATUS(HCI_OP_WRITE_SSP_DEBUG_MODE, hci_cc_write_ssp_debug_mode), + HCI_CC_STATUS(HCI_OP_LE_SET_EXT_SCAN_PARAMS, + hci_cc_le_set_ext_scan_param), + HCI_CC_STATUS(HCI_OP_LE_SET_EXT_SCAN_ENABLE, + hci_cc_le_set_ext_scan_enable), + HCI_CC_STATUS(HCI_OP_LE_SET_DEFAULT_PHY, hci_cc_le_set_default_phy), + HCI_CC(HCI_OP_LE_READ_NUM_SUPPORTED_ADV_SETS, + hci_cc_le_read_num_adv_sets, + sizeof(struct hci_rp_le_read_num_supported_adv_sets)), + HCI_CC(HCI_OP_LE_SET_EXT_ADV_PARAMS, hci_cc_set_ext_adv_param, + sizeof(struct hci_rp_le_set_ext_adv_params)), + HCI_CC_STATUS(HCI_OP_LE_SET_EXT_ADV_ENABLE, + hci_cc_le_set_ext_adv_enable), + HCI_CC_STATUS(HCI_OP_LE_SET_ADV_SET_RAND_ADDR, + hci_cc_le_set_adv_set_random_addr), + HCI_CC_STATUS(HCI_OP_LE_REMOVE_ADV_SET, hci_cc_le_remove_adv_set), + HCI_CC_STATUS(HCI_OP_LE_CLEAR_ADV_SETS, hci_cc_le_clear_adv_sets), + HCI_CC(HCI_OP_LE_READ_TRANSMIT_POWER, hci_cc_le_read_transmit_power, + sizeof(struct hci_rp_le_read_transmit_power)) +}; + +static u8 hci_cc_func(struct hci_dev *hdev, const struct hci_cc *cc, + struct sk_buff *skb) +{ + void *data; + + if (skb->len < cc->min_len) { + bt_dev_err(hdev, "unexpected cc 0x%4.4x length: %u < %u", + cc->op, skb->len, cc->min_len); + return HCI_ERROR_UNSPECIFIED; + } + + /* Just warn if the length is over max_len size it still be possible to + * partially parse the cc so leave to callback to decide if that is + * acceptable. + */ + if (skb->len > cc->max_len) + bt_dev_warn(hdev, "unexpected cc 0x%4.4x length: %u > %u", + cc->op, skb->len, cc->max_len); + + data = hci_cc_skb_pull(hdev, skb, cc->op, cc->min_len); + if (!data) + return HCI_ERROR_UNSPECIFIED; + + return cc->func(hdev, data, skb); +} + static void hci_cmd_complete_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb, u16 *opcode, u8 *status, hci_req_complete_t *req_complete, hci_req_complete_skb_t *req_complete_skb) { struct hci_ev_cmd_complete *ev = data; + int i; *opcode = __le16_to_cpu(ev->opcode); - *status = skb->data[0]; - switch (*opcode) { - case HCI_OP_INQUIRY_CANCEL: - hci_cc_inquiry_cancel(hdev, skb, status); - break; + bt_dev_dbg(hdev, "opcode 0x%4.4x", *opcode); - case HCI_OP_PERIODIC_INQ: - hci_cc_periodic_inq(hdev, skb); - break; - - case HCI_OP_EXIT_PERIODIC_INQ: - hci_cc_exit_periodic_inq(hdev, skb); - break; - - case HCI_OP_REMOTE_NAME_REQ_CANCEL: - hci_cc_remote_name_req_cancel(hdev, skb); - break; - - case HCI_OP_ROLE_DISCOVERY: - hci_cc_role_discovery(hdev, skb); - break; - - case HCI_OP_READ_LINK_POLICY: - hci_cc_read_link_policy(hdev, skb); - break; - - case HCI_OP_WRITE_LINK_POLICY: - hci_cc_write_link_policy(hdev, skb); - break; - - case HCI_OP_READ_DEF_LINK_POLICY: - hci_cc_read_def_link_policy(hdev, skb); - break; - - case HCI_OP_WRITE_DEF_LINK_POLICY: - hci_cc_write_def_link_policy(hdev, skb); - break; - - case HCI_OP_RESET: - hci_cc_reset(hdev, skb); - break; - - case HCI_OP_READ_STORED_LINK_KEY: - hci_cc_read_stored_link_key(hdev, skb); - break; - - case HCI_OP_DELETE_STORED_LINK_KEY: - hci_cc_delete_stored_link_key(hdev, skb); - break; - - case HCI_OP_WRITE_LOCAL_NAME: - hci_cc_write_local_name(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_NAME: - hci_cc_read_local_name(hdev, skb); - break; - - case HCI_OP_WRITE_AUTH_ENABLE: - hci_cc_write_auth_enable(hdev, skb); - break; - - case HCI_OP_WRITE_ENCRYPT_MODE: - hci_cc_write_encrypt_mode(hdev, skb); - break; - - case HCI_OP_WRITE_SCAN_ENABLE: - hci_cc_write_scan_enable(hdev, skb); - break; - - case HCI_OP_SET_EVENT_FLT: - hci_cc_set_event_filter(hdev, skb); - break; - - case HCI_OP_READ_CLASS_OF_DEV: - hci_cc_read_class_of_dev(hdev, skb); - break; - - case HCI_OP_WRITE_CLASS_OF_DEV: - hci_cc_write_class_of_dev(hdev, skb); - break; - - case HCI_OP_READ_VOICE_SETTING: - hci_cc_read_voice_setting(hdev, skb); - break; - - case HCI_OP_WRITE_VOICE_SETTING: - hci_cc_write_voice_setting(hdev, skb); - break; - - case HCI_OP_READ_NUM_SUPPORTED_IAC: - hci_cc_read_num_supported_iac(hdev, skb); - break; - - case HCI_OP_WRITE_SSP_MODE: - hci_cc_write_ssp_mode(hdev, skb); - break; - - case HCI_OP_WRITE_SC_SUPPORT: - hci_cc_write_sc_support(hdev, skb); - break; - - case HCI_OP_READ_AUTH_PAYLOAD_TO: - hci_cc_read_auth_payload_timeout(hdev, skb); - break; - - case HCI_OP_WRITE_AUTH_PAYLOAD_TO: - hci_cc_write_auth_payload_timeout(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_VERSION: - hci_cc_read_local_version(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_COMMANDS: - hci_cc_read_local_commands(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_FEATURES: - hci_cc_read_local_features(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_EXT_FEATURES: - hci_cc_read_local_ext_features(hdev, skb); - break; - - case HCI_OP_READ_BUFFER_SIZE: - hci_cc_read_buffer_size(hdev, skb); - break; - - case HCI_OP_READ_BD_ADDR: - hci_cc_read_bd_addr(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_PAIRING_OPTS: - hci_cc_read_local_pairing_opts(hdev, skb); - break; - - case HCI_OP_READ_PAGE_SCAN_ACTIVITY: - hci_cc_read_page_scan_activity(hdev, skb); - break; - - case HCI_OP_WRITE_PAGE_SCAN_ACTIVITY: - hci_cc_write_page_scan_activity(hdev, skb); - break; - - case HCI_OP_READ_PAGE_SCAN_TYPE: - hci_cc_read_page_scan_type(hdev, skb); - break; - - case HCI_OP_WRITE_PAGE_SCAN_TYPE: - hci_cc_write_page_scan_type(hdev, skb); - break; - - case HCI_OP_READ_DATA_BLOCK_SIZE: - hci_cc_read_data_block_size(hdev, skb); - break; - - case HCI_OP_READ_FLOW_CONTROL_MODE: - hci_cc_read_flow_control_mode(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_AMP_INFO: - hci_cc_read_local_amp_info(hdev, skb); - break; - - case HCI_OP_READ_CLOCK: - hci_cc_read_clock(hdev, skb); - break; - - case HCI_OP_READ_INQ_RSP_TX_POWER: - hci_cc_read_inq_rsp_tx_power(hdev, skb); - break; - - case HCI_OP_READ_DEF_ERR_DATA_REPORTING: - hci_cc_read_def_err_data_reporting(hdev, skb); - break; - - case HCI_OP_WRITE_DEF_ERR_DATA_REPORTING: - hci_cc_write_def_err_data_reporting(hdev, skb); - break; - - case HCI_OP_PIN_CODE_REPLY: - hci_cc_pin_code_reply(hdev, skb); - break; - - case HCI_OP_PIN_CODE_NEG_REPLY: - hci_cc_pin_code_neg_reply(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_OOB_DATA: - hci_cc_read_local_oob_data(hdev, skb); - break; - - case HCI_OP_READ_LOCAL_OOB_EXT_DATA: - hci_cc_read_local_oob_ext_data(hdev, skb); - break; - - case HCI_OP_LE_READ_BUFFER_SIZE: - hci_cc_le_read_buffer_size(hdev, skb); - break; - - case HCI_OP_LE_READ_LOCAL_FEATURES: - hci_cc_le_read_local_features(hdev, skb); - break; - - case HCI_OP_LE_READ_ADV_TX_POWER: - hci_cc_le_read_adv_tx_power(hdev, skb); - break; - - case HCI_OP_USER_CONFIRM_REPLY: - hci_cc_user_confirm_reply(hdev, skb); - break; - - case HCI_OP_USER_CONFIRM_NEG_REPLY: - hci_cc_user_confirm_neg_reply(hdev, skb); - break; - - case HCI_OP_USER_PASSKEY_REPLY: - hci_cc_user_passkey_reply(hdev, skb); - break; - - case HCI_OP_USER_PASSKEY_NEG_REPLY: - hci_cc_user_passkey_neg_reply(hdev, skb); - break; - - case HCI_OP_LE_SET_RANDOM_ADDR: - hci_cc_le_set_random_addr(hdev, skb); - break; - - case HCI_OP_LE_SET_ADV_ENABLE: - hci_cc_le_set_adv_enable(hdev, skb); - break; - - case HCI_OP_LE_SET_SCAN_PARAM: - hci_cc_le_set_scan_param(hdev, skb); - break; - - case HCI_OP_LE_SET_SCAN_ENABLE: - hci_cc_le_set_scan_enable(hdev, skb); - break; - - case HCI_OP_LE_READ_ACCEPT_LIST_SIZE: - hci_cc_le_read_accept_list_size(hdev, skb); - break; - - case HCI_OP_LE_CLEAR_ACCEPT_LIST: - hci_cc_le_clear_accept_list(hdev, skb); - break; - - case HCI_OP_LE_ADD_TO_ACCEPT_LIST: - hci_cc_le_add_to_accept_list(hdev, skb); - break; - - case HCI_OP_LE_DEL_FROM_ACCEPT_LIST: - hci_cc_le_del_from_accept_list(hdev, skb); - break; - - case HCI_OP_LE_READ_SUPPORTED_STATES: - hci_cc_le_read_supported_states(hdev, skb); - break; - - case HCI_OP_LE_READ_DEF_DATA_LEN: - hci_cc_le_read_def_data_len(hdev, skb); - break; - - case HCI_OP_LE_WRITE_DEF_DATA_LEN: - hci_cc_le_write_def_data_len(hdev, skb); - break; - - case HCI_OP_LE_ADD_TO_RESOLV_LIST: - hci_cc_le_add_to_resolv_list(hdev, skb); - break; - - case HCI_OP_LE_DEL_FROM_RESOLV_LIST: - hci_cc_le_del_from_resolv_list(hdev, skb); - break; - - case HCI_OP_LE_CLEAR_RESOLV_LIST: - hci_cc_le_clear_resolv_list(hdev, skb); - break; - - case HCI_OP_LE_READ_RESOLV_LIST_SIZE: - hci_cc_le_read_resolv_list_size(hdev, skb); - break; - - case HCI_OP_LE_SET_ADDR_RESOLV_ENABLE: - hci_cc_le_set_addr_resolution_enable(hdev, skb); - break; - - case HCI_OP_LE_READ_MAX_DATA_LEN: - hci_cc_le_read_max_data_len(hdev, skb); - break; - - case HCI_OP_WRITE_LE_HOST_SUPPORTED: - hci_cc_write_le_host_supported(hdev, skb); - break; - - case HCI_OP_LE_SET_ADV_PARAM: - hci_cc_set_adv_param(hdev, skb); - break; - - case HCI_OP_READ_RSSI: - hci_cc_read_rssi(hdev, skb); - break; - - case HCI_OP_READ_TX_POWER: - hci_cc_read_tx_power(hdev, skb); - break; - - case HCI_OP_WRITE_SSP_DEBUG_MODE: - hci_cc_write_ssp_debug_mode(hdev, skb); - break; - - case HCI_OP_LE_SET_EXT_SCAN_PARAMS: - hci_cc_le_set_ext_scan_param(hdev, skb); - break; - - case HCI_OP_LE_SET_EXT_SCAN_ENABLE: - hci_cc_le_set_ext_scan_enable(hdev, skb); - break; - - case HCI_OP_LE_SET_DEFAULT_PHY: - hci_cc_le_set_default_phy(hdev, skb); - break; - - case HCI_OP_LE_READ_NUM_SUPPORTED_ADV_SETS: - hci_cc_le_read_num_adv_sets(hdev, skb); - break; - - case HCI_OP_LE_SET_EXT_ADV_PARAMS: - hci_cc_set_ext_adv_param(hdev, skb); - break; - - case HCI_OP_LE_SET_EXT_ADV_ENABLE: - hci_cc_le_set_ext_adv_enable(hdev, skb); - break; - - case HCI_OP_LE_SET_ADV_SET_RAND_ADDR: - hci_cc_le_set_adv_set_random_addr(hdev, skb); - break; - - case HCI_OP_LE_REMOVE_ADV_SET: - hci_cc_le_remove_adv_set(hdev, skb); - break; - - case HCI_OP_LE_CLEAR_ADV_SETS: - hci_cc_le_clear_adv_sets(hdev, skb); - break; - - case HCI_OP_LE_READ_TRANSMIT_POWER: - hci_cc_le_read_transmit_power(hdev, skb); - break; - - default: - BT_DBG("%s opcode 0x%4.4x", hdev->name, *opcode); - break; + for (i = 0; i < ARRAY_SIZE(hci_cc_table); i++) { + if (hci_cc_table[i].op == *opcode) { + *status = hci_cc_func(hdev, &hci_cc_table[i], skb); + break; + } } handle_cmd_cnt_and_timer(hdev, ev->ncmd); From 147306ccbbba23e89481980141d11637948e345d Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 10:55:06 -0800 Subject: [PATCH 31/62] Bluetooth: hci_event: Use of a function table to handle Command Status This change the use of switch statement to a function table which is easier to extend. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_event.c | 152 +++++++++++++++----------------------- 1 file changed, 59 insertions(+), 93 deletions(-) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 313b7b79d5b5..df1aca5f28da 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -2195,7 +2195,7 @@ static u8 hci_cc_write_ssp_debug_mode(struct hci_dev *hdev, void *data, static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status) { - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (status) { hci_conn_check_pending(hdev); @@ -2210,7 +2210,7 @@ static void hci_cs_create_conn(struct hci_dev *hdev, __u8 status) struct hci_cp_create_conn *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_CONN); if (!cp) @@ -2220,7 +2220,7 @@ static void hci_cs_create_conn(struct hci_dev *hdev, __u8 status) conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr); - BT_DBG("%s bdaddr %pMR hcon %p", hdev->name, &cp->bdaddr, conn); + bt_dev_dbg(hdev, "bdaddr %pMR hcon %p", &cp->bdaddr, conn); if (status) { if (conn && conn->state == BT_CONNECT) { @@ -2249,7 +2249,7 @@ static void hci_cs_add_sco(struct hci_dev *hdev, __u8 status) struct hci_conn *acl, *sco; __u16 handle; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2260,7 +2260,7 @@ static void hci_cs_add_sco(struct hci_dev *hdev, __u8 status) handle = __le16_to_cpu(cp->handle); - BT_DBG("%s handle 0x%4.4x", hdev->name, handle); + bt_dev_dbg(hdev, "handle 0x%4.4x", handle); hci_dev_lock(hdev); @@ -2283,7 +2283,7 @@ static void hci_cs_auth_requested(struct hci_dev *hdev, __u8 status) struct hci_cp_auth_requested *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2310,7 +2310,7 @@ static void hci_cs_set_conn_encrypt(struct hci_dev *hdev, __u8 status) struct hci_cp_set_conn_encrypt *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2446,7 +2446,7 @@ static void hci_cs_remote_name_req(struct hci_dev *hdev, __u8 status) struct hci_cp_remote_name_req *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); /* If successful wait for the name req complete event before * checking for the need to do authentication */ @@ -2489,7 +2489,7 @@ static void hci_cs_read_remote_features(struct hci_dev *hdev, __u8 status) struct hci_cp_read_remote_features *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2516,7 +2516,7 @@ static void hci_cs_read_remote_ext_features(struct hci_dev *hdev, __u8 status) struct hci_cp_read_remote_ext_features *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2544,7 +2544,7 @@ static void hci_cs_setup_sync_conn(struct hci_dev *hdev, __u8 status) struct hci_conn *acl, *sco; __u16 handle; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2555,7 +2555,7 @@ static void hci_cs_setup_sync_conn(struct hci_dev *hdev, __u8 status) handle = __le16_to_cpu(cp->handle); - BT_DBG("%s handle 0x%4.4x", hdev->name, handle); + bt_dev_dbg(hdev, "handle 0x%4.4x", handle); hci_dev_lock(hdev); @@ -2613,7 +2613,7 @@ static void hci_cs_sniff_mode(struct hci_dev *hdev, __u8 status) struct hci_cp_sniff_mode *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2640,7 +2640,7 @@ static void hci_cs_exit_sniff_mode(struct hci_dev *hdev, __u8 status) struct hci_cp_exit_sniff_mode *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2669,6 +2669,8 @@ static void hci_cs_disconnect(struct hci_dev *hdev, u8 status) struct hci_conn *conn; bool mgmt_conn; + bt_dev_dbg(hdev, "status 0x%2.2x", status); + /* Wait for HCI_EV_DISCONN_COMPLETE if status 0x00 and not suspended * otherwise cleanup the connection immediately. */ @@ -2802,7 +2804,7 @@ static void hci_cs_le_create_conn(struct hci_dev *hdev, u8 status) { struct hci_cp_le_create_conn *cp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); /* All connection failure handling is taken care of by the * hci_le_conn_failed function which is triggered by the HCI @@ -2827,7 +2829,7 @@ static void hci_cs_le_ext_create_conn(struct hci_dev *hdev, u8 status) { struct hci_cp_le_ext_create_conn *cp; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); /* All connection failure handling is taken care of by the * hci_le_conn_failed function which is triggered by the HCI @@ -2853,7 +2855,7 @@ static void hci_cs_le_read_remote_features(struct hci_dev *hdev, u8 status) struct hci_cp_le_read_remote_features *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -2880,7 +2882,7 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status) struct hci_cp_le_start_enc *cp; struct hci_conn *conn; - BT_DBG("%s status 0x%2.2x", hdev->name, status); + bt_dev_dbg(hdev, "status 0x%2.2x", status); if (!status) return; @@ -3948,92 +3950,56 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, void *data, queue_work(hdev->workqueue, &hdev->cmd_work); } +#define HCI_CS(_op, _func) \ +{ \ + .op = _op, \ + .func = _func, \ +} + +static const struct hci_cs { + u16 op; + void (*func)(struct hci_dev *hdev, __u8 status); +} hci_cs_table[] = { + HCI_CS(HCI_OP_INQUIRY, hci_cs_inquiry), + HCI_CS(HCI_OP_CREATE_CONN, hci_cs_create_conn), + HCI_CS(HCI_OP_DISCONNECT, hci_cs_disconnect), + HCI_CS(HCI_OP_ADD_SCO, hci_cs_add_sco), + HCI_CS(HCI_OP_AUTH_REQUESTED, hci_cs_auth_requested), + HCI_CS(HCI_OP_SET_CONN_ENCRYPT, hci_cs_set_conn_encrypt), + HCI_CS(HCI_OP_REMOTE_NAME_REQ, hci_cs_remote_name_req), + HCI_CS(HCI_OP_READ_REMOTE_FEATURES, hci_cs_read_remote_features), + HCI_CS(HCI_OP_READ_REMOTE_EXT_FEATURES, + hci_cs_read_remote_ext_features), + HCI_CS(HCI_OP_SETUP_SYNC_CONN, hci_cs_setup_sync_conn), + HCI_CS(HCI_OP_ENHANCED_SETUP_SYNC_CONN, + hci_cs_enhanced_setup_sync_conn), + HCI_CS(HCI_OP_SNIFF_MODE, hci_cs_sniff_mode), + HCI_CS(HCI_OP_EXIT_SNIFF_MODE, hci_cs_exit_sniff_mode), + HCI_CS(HCI_OP_SWITCH_ROLE, hci_cs_switch_role), + HCI_CS(HCI_OP_LE_CREATE_CONN, hci_cs_le_create_conn), + HCI_CS(HCI_OP_LE_READ_REMOTE_FEATURES, hci_cs_le_read_remote_features), + HCI_CS(HCI_OP_LE_START_ENC, hci_cs_le_start_enc), + HCI_CS(HCI_OP_LE_EXT_CREATE_CONN, hci_cs_le_ext_create_conn) +}; + static void hci_cmd_status_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb, u16 *opcode, u8 *status, hci_req_complete_t *req_complete, hci_req_complete_skb_t *req_complete_skb) { struct hci_ev_cmd_status *ev = data; + int i; *opcode = __le16_to_cpu(ev->opcode); *status = ev->status; - switch (*opcode) { - case HCI_OP_INQUIRY: - hci_cs_inquiry(hdev, ev->status); - break; + bt_dev_dbg(hdev, "opcode 0x%4.4x", *opcode); - case HCI_OP_CREATE_CONN: - hci_cs_create_conn(hdev, ev->status); - break; - - case HCI_OP_DISCONNECT: - hci_cs_disconnect(hdev, ev->status); - break; - - case HCI_OP_ADD_SCO: - hci_cs_add_sco(hdev, ev->status); - break; - - case HCI_OP_AUTH_REQUESTED: - hci_cs_auth_requested(hdev, ev->status); - break; - - case HCI_OP_SET_CONN_ENCRYPT: - hci_cs_set_conn_encrypt(hdev, ev->status); - break; - - case HCI_OP_REMOTE_NAME_REQ: - hci_cs_remote_name_req(hdev, ev->status); - break; - - case HCI_OP_READ_REMOTE_FEATURES: - hci_cs_read_remote_features(hdev, ev->status); - break; - - case HCI_OP_READ_REMOTE_EXT_FEATURES: - hci_cs_read_remote_ext_features(hdev, ev->status); - break; - - case HCI_OP_SETUP_SYNC_CONN: - hci_cs_setup_sync_conn(hdev, ev->status); - break; - - case HCI_OP_ENHANCED_SETUP_SYNC_CONN: - hci_cs_enhanced_setup_sync_conn(hdev, ev->status); - break; - - case HCI_OP_SNIFF_MODE: - hci_cs_sniff_mode(hdev, ev->status); - break; - - case HCI_OP_EXIT_SNIFF_MODE: - hci_cs_exit_sniff_mode(hdev, ev->status); - break; - - case HCI_OP_SWITCH_ROLE: - hci_cs_switch_role(hdev, ev->status); - break; - - case HCI_OP_LE_CREATE_CONN: - hci_cs_le_create_conn(hdev, ev->status); - break; - - case HCI_OP_LE_READ_REMOTE_FEATURES: - hci_cs_le_read_remote_features(hdev, ev->status); - break; - - case HCI_OP_LE_START_ENC: - hci_cs_le_start_enc(hdev, ev->status); - break; - - case HCI_OP_LE_EXT_CREATE_CONN: - hci_cs_le_ext_create_conn(hdev, ev->status); - break; - - default: - BT_DBG("%s opcode 0x%4.4x", hdev->name, *opcode); - break; + for (i = 0; i < ARRAY_SIZE(hci_cs_table); i++) { + if (hci_cs_table[i].op == *opcode) { + hci_cs_table[i].func(hdev, ev->status); + break; + } } handle_cmd_cnt_and_timer(hdev, ev->ncmd); From e8c42585dc6032624a9728d8cf99d974e931d4bc Mon Sep 17 00:00:00 2001 From: Zijun Hu Date: Fri, 3 Dec 2021 18:01:37 +0800 Subject: [PATCH 32/62] Bluetooth: btusb: Add one more Bluetooth part for WCN6855 Add a USB ID 0489:e0e3 of HP to usb_device_id table for WCN6855. -Device(0489:e0e3) from /sys/kernel/debug/usb/devices T: Bus=01 Lev=01 Prnt=01 Port=00 Cnt=01 Dev#= 2 Spd=12 MxCh= 0 D: Ver= 1.10 Cls=e0(wlcon) Sub=01 Prot=01 MxPS=64 #Cfgs= 1 P: Vendor=0489 ProdID=e0e3 Rev= 0.01 C:* #Ifs= 2 Cfg#= 1 Atr=e0 MxPwr=100mA I:* If#= 0 Alt= 0 #EPs= 3 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=81(I) Atr=03(Int.) MxPS= 16 Ivl=1ms E: Ad=82(I) Atr=02(Bulk) MxPS= 64 Ivl=0ms E: Ad=02(O) Atr=02(Bulk) MxPS= 64 Ivl=0ms I:* If#= 1 Alt= 0 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 0 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 0 Ivl=1ms I: If#= 1 Alt= 1 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 9 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 9 Ivl=1ms I: If#= 1 Alt= 2 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 17 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 17 Ivl=1ms I: If#= 1 Alt= 3 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 25 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 25 Ivl=1ms I: If#= 1 Alt= 4 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 33 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 33 Ivl=1ms I: If#= 1 Alt= 5 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 49 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 49 Ivl=1ms I: If#= 1 Alt= 6 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 63 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 63 Ivl=1ms I: If#= 1 Alt= 7 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 65 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 65 Ivl=1ms Signed-off-by: Zijun Hu Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btusb.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index d9067a8fad60..974b36a02375 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -302,6 +302,9 @@ static const struct usb_device_id blacklist_table[] = { { USB_DEVICE(0x0489, 0xe0d6), .driver_info = BTUSB_QCA_WCN6855 | BTUSB_WIDEBAND_SPEECH | BTUSB_VALID_LE_STATES }, + { USB_DEVICE(0x0489, 0xe0e3), .driver_info = BTUSB_QCA_WCN6855 | + BTUSB_WIDEBAND_SPEECH | + BTUSB_VALID_LE_STATES }, /* Broadcom BCM2035 */ { USB_DEVICE(0x0a5c, 0x2009), .driver_info = BTUSB_BCM92035 }, From 630491ffd53cd2237445bd058e8d95662666947b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C5=81ukasz=20Bartosik?= Date: Wed, 1 Dec 2021 21:13:51 +0100 Subject: [PATCH 33/62] Bluetooth: btmtksdio: enable msft opcode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Enable msft opcode for mt7921 chip. Signed-off-by: Łukasz Bartosik Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btmtksdio.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/drivers/bluetooth/btmtksdio.c b/drivers/bluetooth/btmtksdio.c index 143404745240..3a12ea0cf285 100644 --- a/drivers/bluetooth/btmtksdio.c +++ b/drivers/bluetooth/btmtksdio.c @@ -791,6 +791,8 @@ static int mt79xx_setup(struct hci_dev *hdev, const char *fwname) return err; } + hci_set_msft_opcode(hdev, 0xFD30); + return err; } From 16ada83b9a59a50a6e869d2986fb757007392939 Mon Sep 17 00:00:00 2001 From: Sean Wang Date: Fri, 3 Dec 2021 11:30:04 +0800 Subject: [PATCH 34/62] Bluetooth: btmtksdio: enable AOSP extension for MT7921 Similar to MT7921 btusb, we enable AOSP extension for MT7921 btmtksdio as well. Signed-off-by: Sean Wang Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btmtksdio.c | 1 + 1 file changed, 1 insertion(+) diff --git a/drivers/bluetooth/btmtksdio.c b/drivers/bluetooth/btmtksdio.c index 3a12ea0cf285..b5ea8d3bffaa 100644 --- a/drivers/bluetooth/btmtksdio.c +++ b/drivers/bluetooth/btmtksdio.c @@ -792,6 +792,7 @@ static int mt79xx_setup(struct hci_dev *hdev, const char *fwname) } hci_set_msft_opcode(hdev, 0xFD30); + hci_set_aosp_capable(hdev); return err; } From d2f8114f9574509580a8506d2ef72e7e43d1a5bd Mon Sep 17 00:00:00 2001 From: Aditya Garg Date: Thu, 2 Dec 2021 12:41:59 +0000 Subject: [PATCH 35/62] Bluetooth: add quirk disabling LE Read Transmit Power Some devices have a bug causing them to not work if they query LE tx power on startup. Thus we add a quirk in order to not query it and default min/max tx power values to HCI_TX_POWER_INVALID. Signed-off-by: Aditya Garg Reported-by: Orlando Chamberlain Tested-by: Orlando Chamberlain Link: https://lore.kernel.org/r/4970a940-211b-25d6-edab-21a815313954@protonmail.com Fixes: 7c395ea521e6 ("Bluetooth: Query LE tx power on startup") Cc: stable@vger.kernel.org Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 9 +++++++++ net/bluetooth/hci_sync.c | 3 ++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 4343f79bd02c..c5f6b82b9d11 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -246,6 +246,15 @@ enum { * HCI after resume. */ HCI_QUIRK_NO_SUSPEND_NOTIFIER, + + /* + * When this quirk is set, LE tx power is not queried on startup + * and the min/max tx power values default to HCI_TX_POWER_INVALID. + * + * This quirk can be set before hci_register_dev is called or + * during the hdev->setup vendor callback. + */ + HCI_QUIRK_BROKEN_READ_TRANSMIT_POWER, }; /* HCI device flags */ diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index 7ac6c170ec49..ff3f561a45a2 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -3300,7 +3300,8 @@ static int hci_le_read_adv_tx_power_sync(struct hci_dev *hdev) /* Read LE Min/Max Tx Power*/ static int hci_le_read_tx_power_sync(struct hci_dev *hdev) { - if (!(hdev->commands[38] & 0x80)) + if (!(hdev->commands[38] & 0x80) || + test_bit(HCI_QUIRK_BROKEN_READ_TRANSMIT_POWER, &hdev->quirks)) return 0; return __hci_cmd_sync_status(hdev, HCI_OP_LE_READ_TRANSMIT_POWER, From 801b4c027b44a185292007d3cf7513999d644723 Mon Sep 17 00:00:00 2001 From: Aditya Garg Date: Thu, 2 Dec 2021 12:42:59 +0000 Subject: [PATCH 36/62] Bluetooth: btbcm: disable read tx power for some Macs with the T2 Security chip Some Macs with the T2 security chip had Bluetooth not working. To fix it we add DMI based quirks to disable querying of LE Tx power. Signed-off-by: Aditya Garg Reported-by: Orlando Chamberlain Tested-by: Orlando Chamberlain Link: https://lore.kernel.org/r/4970a940-211b-25d6-edab-21a815313954@protonmail.com Fixes: 7c395ea521e6 ("Bluetooth: Query LE tx power on startup") Cc: stable@vger.kernel.org Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btbcm.c | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/drivers/bluetooth/btbcm.c b/drivers/bluetooth/btbcm.c index e4182acee488..07fabaa5aa29 100644 --- a/drivers/bluetooth/btbcm.c +++ b/drivers/bluetooth/btbcm.c @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -343,6 +344,40 @@ static struct sk_buff *btbcm_read_usb_product(struct hci_dev *hdev) return skb; } +static const struct dmi_system_id disable_broken_read_transmit_power[] = { + { + .matches = { + DMI_MATCH(DMI_BOARD_VENDOR, "Apple Inc."), + DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro16,1"), + }, + }, + { + .matches = { + DMI_MATCH(DMI_BOARD_VENDOR, "Apple Inc."), + DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro16,2"), + }, + }, + { + .matches = { + DMI_MATCH(DMI_BOARD_VENDOR, "Apple Inc."), + DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro16,4"), + }, + }, + { + .matches = { + DMI_MATCH(DMI_BOARD_VENDOR, "Apple Inc."), + DMI_MATCH(DMI_PRODUCT_NAME, "iMac20,1"), + }, + }, + { + .matches = { + DMI_MATCH(DMI_BOARD_VENDOR, "Apple Inc."), + DMI_MATCH(DMI_PRODUCT_NAME, "iMac20,2"), + }, + }, + { } +}; + static int btbcm_read_info(struct hci_dev *hdev) { struct sk_buff *skb; @@ -363,6 +398,10 @@ static int btbcm_read_info(struct hci_dev *hdev) bt_dev_info(hdev, "BCM: features 0x%2.2x", skb->data[1]); kfree_skb(skb); + /* Read DMI and disable broken Read LE Min/Max Tx Power */ + if (dmi_first_match(disable_broken_read_transmit_power)) + set_bit(HCI_QUIRK_BROKEN_READ_TRANSMIT_POWER, &hdev->quirks); + return 0; } From 6f59f991b4e735323f099ac6490e725ae8c750a5 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 11:49:49 -0800 Subject: [PATCH 37/62] Bluetooth: MGMT: Use hci_dev_test_and_{set,clear}_flag This make use of hci_dev_test_and_{set,clear}_flag instead of doing 2 operations in a row. Fixes: cbbdfa6f33198 ("Bluetooth: Enable controller RPA resolution using Experimental feature") Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/mgmt.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index bf989ae03f9f..ff6d7c9333be 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -4041,10 +4041,10 @@ static int set_zero_key_func(struct sock *sk, struct hci_dev *hdev, #endif if (hdev && use_ll_privacy(hdev) && !hdev_is_powered(hdev)) { - bool changed = hci_dev_test_flag(hdev, HCI_ENABLE_LL_PRIVACY); - - hci_dev_clear_flag(hdev, HCI_ENABLE_LL_PRIVACY); + bool changed; + changed = hci_dev_test_and_clear_flag(hdev, + HCI_ENABLE_LL_PRIVACY); if (changed) exp_ll_privacy_feature_changed(false, hdev, sk); } @@ -4139,15 +4139,15 @@ static int set_rpa_resolution_func(struct sock *sk, struct hci_dev *hdev, val = !!cp->param[0]; if (val) { - changed = !hci_dev_test_flag(hdev, HCI_ENABLE_LL_PRIVACY); - hci_dev_set_flag(hdev, HCI_ENABLE_LL_PRIVACY); + changed = !hci_dev_test_and_set_flag(hdev, + HCI_ENABLE_LL_PRIVACY); hci_dev_clear_flag(hdev, HCI_ADVERTISING); /* Enable LL privacy + supported settings changed */ flags = BIT(0) | BIT(1); } else { - changed = hci_dev_test_flag(hdev, HCI_ENABLE_LL_PRIVACY); - hci_dev_clear_flag(hdev, HCI_ENABLE_LL_PRIVACY); + changed = hci_dev_test_and_clear_flag(hdev, + HCI_ENABLE_LL_PRIVACY); /* Disable LL privacy + supported settings changed */ flags = BIT(1); From fe92ee6425a2c79ee84f0b9b8a14117200d15f7d Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 1 Dec 2021 11:49:50 -0800 Subject: [PATCH 38/62] Bluetooth: hci_core: Rework hci_conn_params flags This reworks hci_conn_params flags to use bitmap_* helpers and add support for setting the supported flags in hdev->conn_flags so it can easily be accessed. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci_core.h | 24 ++++++++++++------------ net/bluetooth/hci_core.c | 8 +++++++- net/bluetooth/hci_request.c | 4 ++-- net/bluetooth/hci_sync.c | 7 +++---- net/bluetooth/mgmt.c | 30 ++++++++++++++++++++---------- 5 files changed, 44 insertions(+), 29 deletions(-) diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 7bae8376fd6f..d1b67755a373 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -152,23 +152,22 @@ struct bdaddr_list_with_irk { u8 local_irk[16]; }; +enum hci_conn_flags { + HCI_CONN_FLAG_REMOTE_WAKEUP, + + __HCI_CONN_NUM_FLAGS, +}; + +/* Make sure number of flags doesn't exceed sizeof(current_flags) */ +static_assert(__HCI_CONN_NUM_FLAGS < 32); + struct bdaddr_list_with_flags { struct list_head list; bdaddr_t bdaddr; u8 bdaddr_type; - u32 current_flags; + DECLARE_BITMAP(flags, __HCI_CONN_NUM_FLAGS); }; -enum hci_conn_flags { - HCI_CONN_FLAG_REMOTE_WAKEUP, - HCI_CONN_FLAG_MAX -}; - -#define hci_conn_test_flag(nr, flags) ((flags) & (1U << nr)) - -/* Make sure number of flags doesn't exceed sizeof(current_flags) */ -static_assert(HCI_CONN_FLAG_MAX < 32); - struct bt_uuid { struct list_head list; u8 uuid[16]; @@ -560,6 +559,7 @@ struct hci_dev { struct rfkill *rfkill; DECLARE_BITMAP(dev_flags, __HCI_NUM_FLAGS); + DECLARE_BITMAP(conn_flags, __HCI_CONN_NUM_FLAGS); __s8 adv_tx_power; __u8 adv_data[HCI_MAX_EXT_AD_LENGTH]; @@ -755,7 +755,7 @@ struct hci_conn_params { struct hci_conn *conn; bool explicit_connect; - u32 current_flags; + DECLARE_BITMAP(flags, __HCI_CONN_NUM_FLAGS); }; extern struct list_head hci_dev_list; diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index 5cadecd31f66..38063bf1fdc5 100644 --- a/net/bluetooth/hci_core.c +++ b/net/bluetooth/hci_core.c @@ -2153,7 +2153,7 @@ int hci_bdaddr_list_add_with_flags(struct list_head *list, bdaddr_t *bdaddr, bacpy(&entry->bdaddr, bdaddr); entry->bdaddr_type = type; - entry->current_flags = flags; + bitmap_from_u64(entry->flags, flags); list_add(&entry->list, list); @@ -2629,6 +2629,12 @@ int hci_register_dev(struct hci_dev *hdev) if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks)) hci_dev_set_flag(hdev, HCI_UNCONFIGURED); + /* Mark Remote Wakeup connection flag as supported if driver has wakeup + * callback. + */ + if (hdev->wakeup) + set_bit(HCI_CONN_FLAG_REMOTE_WAKEUP, hdev->conn_flags); + hci_sock_dev_event(hdev, HCI_DEV_REG); hci_dev_hold(hdev); diff --git a/net/bluetooth/hci_request.c b/net/bluetooth/hci_request.c index a2607db44404..329c66456cf1 100644 --- a/net/bluetooth/hci_request.c +++ b/net/bluetooth/hci_request.c @@ -481,8 +481,8 @@ static int add_to_accept_list(struct hci_request *req, } /* During suspend, only wakeable devices can be in accept list */ - if (hdev->suspended && !hci_conn_test_flag(HCI_CONN_FLAG_REMOTE_WAKEUP, - params->current_flags)) + if (hdev->suspended && + !test_bit(HCI_CONN_FLAG_REMOTE_WAKEUP, params->flags)) return 0; *num_entries += 1; diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index ff3f561a45a2..c1b01718a797 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -1623,8 +1623,8 @@ static int hci_le_add_accept_list_sync(struct hci_dev *hdev, } /* During suspend, only wakeable devices can be in acceptlist */ - if (hdev->suspended && !hci_conn_test_flag(HCI_CONN_FLAG_REMOTE_WAKEUP, - params->current_flags)) + if (hdev->suspended && + !test_bit(HCI_CONN_FLAG_REMOTE_WAKEUP, params->flags)) return 0; /* Attempt to program the device in the resolving list first to avoid @@ -4767,8 +4767,7 @@ static int hci_update_event_filter_sync(struct hci_dev *hdev) hci_clear_event_filter_sync(hdev); list_for_each_entry(b, &hdev->accept_list, list) { - if (!hci_conn_test_flag(HCI_CONN_FLAG_REMOTE_WAKEUP, - b->current_flags)) + if (!test_bit(HCI_CONN_FLAG_REMOTE_WAKEUP, b->flags)) continue; bt_dev_dbg(hdev, "Adding event filters for %pMR", &b->bdaddr); diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index ff6d7c9333be..7d3c69c3cf62 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -4349,8 +4349,6 @@ static int set_exp_feature(struct sock *sk, struct hci_dev *hdev, MGMT_STATUS_NOT_SUPPORTED); } -#define SUPPORTED_DEVICE_FLAGS() ((1U << HCI_CONN_FLAG_MAX) - 1) - static int get_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, u16 data_len) { @@ -4358,7 +4356,7 @@ static int get_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, struct mgmt_rp_get_device_flags rp; struct bdaddr_list_with_flags *br_params; struct hci_conn_params *params; - u32 supported_flags = SUPPORTED_DEVICE_FLAGS(); + u32 supported_flags; u32 current_flags = 0; u8 status = MGMT_STATUS_INVALID_PARAMS; @@ -4367,6 +4365,9 @@ static int get_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, hci_dev_lock(hdev); + bitmap_to_arr32(&supported_flags, hdev->conn_flags, + __HCI_CONN_NUM_FLAGS); + memset(&rp, 0, sizeof(rp)); if (cp->addr.type == BDADDR_BREDR) { @@ -4376,7 +4377,8 @@ static int get_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, if (!br_params) goto done; - current_flags = br_params->current_flags; + bitmap_to_arr32(¤t_flags, br_params->flags, + __HCI_CONN_NUM_FLAGS); } else { params = hci_conn_params_lookup(hdev, &cp->addr.bdaddr, le_addr_type(cp->addr.type)); @@ -4384,7 +4386,8 @@ static int get_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, if (!params) goto done; - current_flags = params->current_flags; + bitmap_to_arr32(¤t_flags, params->flags, + __HCI_CONN_NUM_FLAGS); } bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); @@ -4422,13 +4425,16 @@ static int set_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, struct bdaddr_list_with_flags *br_params; struct hci_conn_params *params; u8 status = MGMT_STATUS_INVALID_PARAMS; - u32 supported_flags = SUPPORTED_DEVICE_FLAGS(); + u32 supported_flags; u32 current_flags = __le32_to_cpu(cp->current_flags); bt_dev_dbg(hdev, "Set device flags %pMR (type 0x%x) = 0x%x", &cp->addr.bdaddr, cp->addr.type, __le32_to_cpu(current_flags)); + bitmap_to_arr32(&supported_flags, hdev->conn_flags, + __HCI_CONN_NUM_FLAGS); + if ((supported_flags | current_flags) != supported_flags) { bt_dev_warn(hdev, "Bad flag given (0x%x) vs supported (0x%0x)", current_flags, supported_flags); @@ -4443,7 +4449,7 @@ static int set_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, cp->addr.type); if (br_params) { - br_params->current_flags = current_flags; + bitmap_from_u64(br_params->flags, current_flags); status = MGMT_STATUS_SUCCESS; } else { bt_dev_warn(hdev, "No such BR/EDR device %pMR (0x%x)", @@ -4453,7 +4459,7 @@ static int set_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, params = hci_conn_params_lookup(hdev, &cp->addr.bdaddr, le_addr_type(cp->addr.type)); if (params) { - params->current_flags = current_flags; + bitmap_from_u64(params->flags, current_flags); status = MGMT_STATUS_SUCCESS; } else { bt_dev_warn(hdev, "No such LE device %pMR (0x%x)", @@ -6979,6 +6985,7 @@ static int add_device(struct sock *sk, struct hci_dev *hdev, struct hci_conn_params *params; int err; u32 current_flags = 0; + u32 supported_flags; bt_dev_dbg(hdev, "sock %p", sk); @@ -7050,7 +7057,8 @@ static int add_device(struct sock *sk, struct hci_dev *hdev, params = hci_conn_params_lookup(hdev, &cp->addr.bdaddr, addr_type); if (params) - current_flags = params->current_flags; + bitmap_to_arr32(¤t_flags, params->flags, + __HCI_CONN_NUM_FLAGS); } err = hci_cmd_sync_queue(hdev, add_device_sync, NULL, NULL); @@ -7059,8 +7067,10 @@ static int add_device(struct sock *sk, struct hci_dev *hdev, added: device_added(sk, hdev, &cp->addr.bdaddr, cp->addr.type, cp->action); + bitmap_to_arr32(&supported_flags, hdev->conn_flags, + __HCI_CONN_NUM_FLAGS); device_flags_changed(NULL, hdev, &cp->addr.bdaddr, cp->addr.type, - SUPPORTED_DEVICE_FLAGS(), current_flags); + supported_flags, current_flags); err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_DEVICE, MGMT_STATUS_SUCCESS, &cp->addr, From 800fe5ec302e1ebbf5e3f891f886deecd49c7132 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Fri, 3 Dec 2021 14:38:13 -0800 Subject: [PATCH 39/62] Bluetooth: btusb: Add support for queuing during polling interval This makes btusb to queue ACL and events during a polling interval by using of a delayed work, with the interval working as a time window where frames received from different endpoints are considered to be arrived at same time and then attempt to resolve potential conflics by processing the events ahead of ACL packets. It worth noting though that priorizing events over ACL data may result in inverting the order compared to how they appeared over the air, for instance there may be packets received before a disconnect event that will be discarded and unencrypted packets received before encryption change which would considered encrypted, because of these potential changes on the order the support for queuing during the polling interval is not enabled by default so it requires setting force_poll_sync debugfs while the adapter is down. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btusb.c | 129 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 124 insertions(+), 5 deletions(-) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index 974b36a02375..fda5622b1d6e 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -31,7 +32,6 @@ static bool disable_scofix; static bool force_scofix; static bool enable_autosuspend = IS_ENABLED(CONFIG_BT_HCIBTUSB_AUTOSUSPEND); - static bool reset = true; static struct usb_driver btusb_driver; @@ -561,8 +561,13 @@ struct btusb_data { unsigned long flags; - struct work_struct work; - struct work_struct waker; + bool poll_sync; + int intr_interval; + struct work_struct work; + struct work_struct waker; + struct delayed_work rx_work; + + struct sk_buff_head acl_q; struct usb_anchor deferred; struct usb_anchor tx_anchor; @@ -727,6 +732,16 @@ static inline void btusb_free_frags(struct btusb_data *data) spin_unlock_irqrestore(&data->rxlock, flags); } +static int btusb_recv_event(struct btusb_data *data, struct sk_buff *skb) +{ + if (data->intr_interval) { + /* Trigger dequeue immediatelly if an event is received */ + schedule_delayed_work(&data->rx_work, 0); + } + + return data->recv_event(data->hdev, skb); +} + static int btusb_recv_intr(struct btusb_data *data, void *buffer, int count) { struct sk_buff *skb; @@ -772,7 +787,7 @@ static int btusb_recv_intr(struct btusb_data *data, void *buffer, int count) if (!hci_skb_expect(skb)) { /* Complete frame */ - data->recv_event(data->hdev, skb); + btusb_recv_event(data, skb); skb = NULL; } } @@ -783,6 +798,20 @@ static int btusb_recv_intr(struct btusb_data *data, void *buffer, int count) return err; } +static int btusb_recv_acl(struct btusb_data *data, struct sk_buff *skb) +{ + /* Only queue ACL packet if intr_interval is set as it means + * force_poll_sync has been enabled. + */ + if (!data->intr_interval) + return data->recv_acl(data->hdev, skb); + + skb_queue_tail(&data->acl_q, skb); + schedule_delayed_work(&data->rx_work, data->intr_interval); + + return 0; +} + static int btusb_recv_bulk(struct btusb_data *data, void *buffer, int count) { struct sk_buff *skb; @@ -830,7 +859,7 @@ static int btusb_recv_bulk(struct btusb_data *data, void *buffer, int count) if (!hci_skb_expect(skb)) { /* Complete frame */ - data->recv_acl(data->hdev, skb); + btusb_recv_acl(data, skb); skb = NULL; } } @@ -986,6 +1015,28 @@ static int btusb_submit_intr_urb(struct hci_dev *hdev, gfp_t mem_flags) usb_unanchor_urb(urb); } + /* Only initialize intr_interval if URB poll sync is enabled */ + if (!data->poll_sync) + goto done; + + /* The units are frames (milliseconds) for full and low speed devices, + * and microframes (1/8 millisecond) for highspeed and SuperSpeed + * devices. + * + * This is done once on open/resume so it shouldn't change even if + * force_poll_sync changes. + */ + switch (urb->dev->speed) { + case USB_SPEED_SUPER_PLUS: + case USB_SPEED_SUPER: /* units are 125us */ + data->intr_interval = usecs_to_jiffies(urb->interval * 125); + break; + default: + data->intr_interval = msecs_to_jiffies(urb->interval); + break; + } + +done: usb_free_urb(urb); return err; @@ -1448,9 +1499,12 @@ static int btusb_close(struct hci_dev *hdev) BT_DBG("%s", hdev->name); + cancel_delayed_work(&data->rx_work); cancel_work_sync(&data->work); cancel_work_sync(&data->waker); + skb_queue_purge(&data->acl_q); + clear_bit(BTUSB_ISOC_RUNNING, &data->flags); clear_bit(BTUSB_BULK_RUNNING, &data->flags); clear_bit(BTUSB_INTR_RUNNING, &data->flags); @@ -1482,6 +1536,10 @@ static int btusb_flush(struct hci_dev *hdev) BT_DBG("%s", hdev->name); + cancel_delayed_work(&data->rx_work); + + skb_queue_purge(&data->acl_q); + usb_kill_anchored_urbs(&data->tx_anchor); btusb_free_frags(data); @@ -1845,6 +1903,17 @@ static void btusb_waker(struct work_struct *work) usb_autopm_put_interface(data->intf); } +static void btusb_rx_work(struct work_struct *work) +{ + struct btusb_data *data = container_of(work, struct btusb_data, + rx_work.work); + struct sk_buff *skb; + + /* Dequeue ACL data received during the interval */ + while ((skb = skb_dequeue(&data->acl_q))) + data->recv_acl(data->hdev, skb); +} + static int btusb_setup_bcm92035(struct hci_dev *hdev) { struct sk_buff *skb; @@ -3402,6 +3471,49 @@ static int btusb_shutdown_qca(struct hci_dev *hdev) return 0; } +static ssize_t force_poll_sync_read(struct file *file, char __user *user_buf, + size_t count, loff_t *ppos) +{ + struct btusb_data *data = file->private_data; + char buf[3]; + + buf[0] = data->poll_sync ? 'Y' : 'N'; + buf[1] = '\n'; + buf[2] = '\0'; + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); +} + +static ssize_t force_poll_sync_write(struct file *file, + const char __user *user_buf, + size_t count, loff_t *ppos) +{ + struct btusb_data *data = file->private_data; + bool enable; + int err; + + err = kstrtobool_from_user(user_buf, count, &enable); + if (err) + return err; + + /* Only allow changes while the adapter is down */ + if (test_bit(HCI_UP, &data->hdev->flags)) + return -EPERM; + + if (data->poll_sync == enable) + return -EALREADY; + + data->poll_sync = enable; + + return count; +} + +static const struct file_operations force_poll_sync_fops = { + .open = simple_open, + .read = force_poll_sync_read, + .write = force_poll_sync_write, + .llseek = default_llseek, +}; + static int btusb_probe(struct usb_interface *intf, const struct usb_device_id *id) { @@ -3485,6 +3597,10 @@ static int btusb_probe(struct usb_interface *intf, INIT_WORK(&data->work, btusb_work); INIT_WORK(&data->waker, btusb_waker); + INIT_DELAYED_WORK(&data->rx_work, btusb_rx_work); + + skb_queue_head_init(&data->acl_q); + init_usb_anchor(&data->deferred); init_usb_anchor(&data->tx_anchor); spin_lock_init(&data->txlock); @@ -3750,6 +3866,9 @@ static int btusb_probe(struct usb_interface *intf, usb_set_intfdata(intf, data); + debugfs_create_file("force_poll_sync", 0644, hdev->debugfs, data, + &force_poll_sync_fops); + return 0; out_free_dev: From 6126ffabba6b415bd6bf3e1b091ef074e86765c6 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Fri, 3 Dec 2021 14:07:22 -0800 Subject: [PATCH 40/62] Bluetooth: Introduce HCI_CONN_FLAG_DEVICE_PRIVACY device flag This introduces HCI_CONN_FLAG_DEVICE_PRIVACY which can be used by userspace to indicate to the controller to use Device Privacy Mode to a specific device. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci_core.h | 4 ++++ net/bluetooth/mgmt.c | 12 ++++++++++++ 2 files changed, 16 insertions(+) diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index d1b67755a373..cf24af649c7f 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -154,6 +154,7 @@ struct bdaddr_list_with_irk { enum hci_conn_flags { HCI_CONN_FLAG_REMOTE_WAKEUP, + HCI_CONN_FLAG_DEVICE_PRIVACY, __HCI_CONN_NUM_FLAGS, }; @@ -1466,6 +1467,9 @@ void hci_conn_del_sysfs(struct hci_conn *conn); #define use_ll_privacy(dev) (ll_privacy_capable(dev) && \ hci_dev_test_flag(dev, HCI_ENABLE_LL_PRIVACY)) +#define privacy_mode_capable(dev) (use_ll_privacy(dev) && \ + (hdev->commands[39] & 0x04)) + /* Use enhanced synchronous connection if command is supported */ #define enhanced_sco_capable(dev) ((dev)->commands[29] & 0x08) diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 7d3c69c3cf62..17a5f972cc65 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -3978,6 +3978,11 @@ static int exp_ll_privacy_feature_changed(bool enabled, struct hci_dev *hdev, memcpy(ev.uuid, rpa_resolution_uuid, 16); ev.flags = cpu_to_le32((enabled ? BIT(0) : 0) | BIT(1)); + if (enabled && privacy_mode_capable(hdev)) + set_bit(HCI_CONN_FLAG_DEVICE_PRIVACY, hdev->conn_flags); + else + clear_bit(HCI_CONN_FLAG_DEVICE_PRIVACY, hdev->conn_flags); + return mgmt_limited_event(MGMT_EV_EXP_FEATURE_CHANGED, hdev, &ev, sizeof(ev), HCI_MGMT_EXP_FEATURE_EVENTS, skip); @@ -4461,6 +4466,13 @@ static int set_device_flags(struct sock *sk, struct hci_dev *hdev, void *data, if (params) { bitmap_from_u64(params->flags, current_flags); status = MGMT_STATUS_SUCCESS; + + /* Update passive scan if HCI_CONN_FLAG_DEVICE_PRIVACY + * has been set. + */ + if (test_bit(HCI_CONN_FLAG_DEVICE_PRIVACY, + params->flags)) + hci_update_passive_scan(hdev); } else { bt_dev_warn(hdev, "No such LE device %pMR (0x%x)", &cp->addr.bdaddr, From 853b70b506a20cddf96419e8ac198df92e51bc4c Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Fri, 3 Dec 2021 14:07:23 -0800 Subject: [PATCH 41/62] Bluetooth: hci_sync: Set Privacy Mode when updating the resolving list This adds support for Set Privacy Mode when updating the resolving list when HCI_CONN_FLAG_DEVICE_PRIVACY so the controller shall use Device Mode for devices programmed in the resolving list, Device Mode is actually required when the remote device are not able to use RPA as otherwise the default mode is Network Privacy Mode in which only allows RPAs thus the controller would filter out advertisement using identity addresses for which there is an IRK. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 10 +++++++ include/net/bluetooth/hci_core.h | 1 + net/bluetooth/hci_event.c | 30 ++++++++++++++++++- net/bluetooth/hci_sync.c | 51 ++++++++++++++++++++++++++++---- 4 files changed, 85 insertions(+), 7 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index c5f6b82b9d11..7f5f00ff53da 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -1940,6 +1940,16 @@ struct hci_rp_le_read_transmit_power { __s8 max_le_tx_power; } __packed; +#define HCI_NETWORK_PRIVACY 0x00 +#define HCI_DEVICE_PRIVACY 0x01 + +#define HCI_OP_LE_SET_PRIVACY_MODE 0x204e +struct hci_cp_le_set_privacy_mode { + __u8 bdaddr_type; + bdaddr_t bdaddr; + __u8 mode; +} __packed; + #define HCI_OP_LE_READ_BUFFER_SIZE_V2 0x2060 struct hci_rp_le_read_buffer_size_v2 { __u8 status; diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index cf24af649c7f..4d69dcfebd63 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -757,6 +757,7 @@ struct hci_conn_params { struct hci_conn *conn; bool explicit_connect; DECLARE_BITMAP(flags, __HCI_CONN_NUM_FLAGS); + u8 privacy_mode; }; extern struct list_head hci_dev_list; diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index df1aca5f28da..c15289b59c3c 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -1489,6 +1489,33 @@ static u8 hci_cc_le_read_transmit_power(struct hci_dev *hdev, void *data, return rp->status; } +static u8 hci_cc_le_set_privacy_mode(struct hci_dev *hdev, void *data, + struct sk_buff *skb) +{ + struct hci_ev_status *rp = data; + struct hci_cp_le_set_privacy_mode *cp; + struct hci_conn_params *params; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) + return rp->status; + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_PRIVACY_MODE); + if (!cp) + return rp->status; + + hci_dev_lock(hdev); + + params = hci_conn_params_lookup(hdev, &cp->bdaddr, cp->bdaddr_type); + if (params) + params->privacy_mode = cp->mode; + + hci_dev_unlock(hdev); + + return rp->status; +} + static u8 hci_cc_le_set_adv_enable(struct hci_dev *hdev, void *data, struct sk_buff *skb) { @@ -3887,7 +3914,8 @@ static const struct hci_cc { HCI_CC_STATUS(HCI_OP_LE_REMOVE_ADV_SET, hci_cc_le_remove_adv_set), HCI_CC_STATUS(HCI_OP_LE_CLEAR_ADV_SETS, hci_cc_le_clear_adv_sets), HCI_CC(HCI_OP_LE_READ_TRANSMIT_POWER, hci_cc_le_read_transmit_power, - sizeof(struct hci_rp_le_read_transmit_power)) + sizeof(struct hci_rp_le_read_transmit_power)), + HCI_CC_STATUS(HCI_OP_LE_SET_PRIVACY_MODE, hci_cc_le_set_privacy_mode) }; static u8 hci_cc_func(struct hci_dev *hdev, const struct hci_cc *cc, diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index c1b01718a797..3d28ca7ebe45 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -1597,8 +1597,40 @@ done: sizeof(cp), &cp, HCI_CMD_TIMEOUT); } +/* Set Device Privacy Mode. */ +static int hci_le_set_privacy_mode_sync(struct hci_dev *hdev, + struct hci_conn_params *params) +{ + struct hci_cp_le_set_privacy_mode cp; + struct smp_irk *irk; + + /* If device privacy mode has already been set there is nothing to do */ + if (params->privacy_mode == HCI_DEVICE_PRIVACY) + return 0; + + /* Check if HCI_CONN_FLAG_DEVICE_PRIVACY has been set as it also + * indicates that LL Privacy has been enabled and + * HCI_OP_LE_SET_PRIVACY_MODE is supported. + */ + if (!test_bit(HCI_CONN_FLAG_DEVICE_PRIVACY, params->flags)) + return 0; + + irk = hci_find_irk_by_addr(hdev, ¶ms->addr, params->addr_type); + if (!irk) + return 0; + + memset(&cp, 0, sizeof(cp)); + cp.bdaddr_type = irk->addr_type; + bacpy(&cp.bdaddr, &irk->bdaddr); + cp.mode = HCI_DEVICE_PRIVACY; + + return __hci_cmd_sync_status(hdev, HCI_OP_LE_SET_PRIVACY_MODE, + sizeof(cp), &cp, HCI_CMD_TIMEOUT); +} + /* Adds connection to allow list if needed, if the device uses RPA (has IRK) - * this attempts to program the device in the resolving list as well. + * this attempts to program the device in the resolving list as well and + * properly set the privacy mode. */ static int hci_le_add_accept_list_sync(struct hci_dev *hdev, struct hci_conn_params *params, @@ -1607,11 +1639,6 @@ static int hci_le_add_accept_list_sync(struct hci_dev *hdev, struct hci_cp_le_add_to_accept_list cp; int err; - /* Already in accept list */ - if (hci_bdaddr_list_lookup(&hdev->le_accept_list, ¶ms->addr, - params->addr_type)) - return 0; - /* Select filter policy to accept all advertising */ if (*num_entries >= hdev->le_accept_list_size) return -ENOSPC; @@ -1637,6 +1664,18 @@ static int hci_le_add_accept_list_sync(struct hci_dev *hdev, return err; } + /* Set Privacy Mode */ + err = hci_le_set_privacy_mode_sync(hdev, params); + if (err) { + bt_dev_err(hdev, "Unable to set privacy mode: %d", err); + return err; + } + + /* Check if already in accept list */ + if (hci_bdaddr_list_lookup(&hdev->le_accept_list, ¶ms->addr, + params->addr_type)) + return 0; + *num_entries += 1; cp.bdaddr_type = params->addr_type; bacpy(&cp.bdaddr, ¶ms->addr); From 9a667031b9228222e7c577b39189caca5ca5bd96 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Mon, 6 Dec 2021 11:27:01 -0800 Subject: [PATCH 42/62] Bluetooth: msft: Fix compilation when CONFIG_BT_MSFTEXT is not set This fixes compilation when CONFIG_BT_MSFTEXT is not set. Fixes: 6b3d4c8fcf3f2 ("Bluetooth: hci_event: Use of a function table to handle HCI events") Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/msft.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/net/bluetooth/msft.h b/net/bluetooth/msft.h index 7fefc0655b6f..afcaf7d3b1cb 100644 --- a/net/bluetooth/msft.h +++ b/net/bluetooth/msft.h @@ -39,7 +39,8 @@ static inline void msft_register(struct hci_dev *hdev) {} static inline void msft_unregister(struct hci_dev *hdev) {} static inline void msft_do_open(struct hci_dev *hdev) {} static inline void msft_do_close(struct hci_dev *hdev) {} -static inline void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb) {} +static inline void msft_vendor_evt(struct hci_dev *hdev, void *data, + struct sk_buff *skb) {} static inline __u64 msft_get_features(struct hci_dev *hdev) { return 0; } static inline int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) From 00c0ee9850b7b0cb7c40b8daba806ae2245e59d4 Mon Sep 17 00:00:00 2001 From: Mark Chen Date: Tue, 7 Dec 2021 01:33:42 +0800 Subject: [PATCH 43/62] Bluetooth: btusb: Handle download_firmware failure cases For Mediatek chipset, it can not enabled if there are something wrong in btmtk_setup_firmware_79xx(). Thus, the process must be terminated and returned error code. Fixes: fc342c4dc4087 ("Bluetooth: btusb: Add protocol support for MediaTek MT7921U USB devices") Co-developed-by: Sean Wang Signed-off-by: Sean Wang Signed-off-by: Mark Chen Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btusb.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index fda5622b1d6e..c9619096d763 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -2633,6 +2633,10 @@ static int btusb_mtk_setup(struct hci_dev *hdev) dev_id & 0xffff, (fw_version & 0xff) + 1); err = btmtk_setup_firmware_79xx(hdev, fw_bin_name, btusb_mtk_hci_wmt_sync); + if (err < 0) { + bt_dev_err(hdev, "Failed to set up firmware (%d)", err); + return err; + } /* It's Device EndPoint Reset Option Register */ btusb_mtk_uhw_reg_write(data, MTK_EP_RST_OPT, MTK_EP_RST_IN_OUT_OPT); From 995d948cf2e45834275f07afc1c9881a9902e73c Mon Sep 17 00:00:00 2001 From: Mark Chen Date: Tue, 7 Dec 2021 01:33:43 +0800 Subject: [PATCH 44/62] Bluetooth: btusb: Return error code when getting patch status failed If there are failure cases in getting patch status, it should return the error code (-EIO). Fixes: fc342c4dc4087 ("Bluetooth: btusb: Add protocol support for MediaTek MT7921U USB devices") Co-developed-by: Sean Wang Signed-off-by: Sean Wang Signed-off-by: Mark Chen Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btmtk.c | 1 + 1 file changed, 1 insertion(+) diff --git a/drivers/bluetooth/btmtk.c b/drivers/bluetooth/btmtk.c index c2ee5c4b975a..526dfdf1fe01 100644 --- a/drivers/bluetooth/btmtk.c +++ b/drivers/bluetooth/btmtk.c @@ -121,6 +121,7 @@ int btmtk_setup_firmware_79xx(struct hci_dev *hdev, const char *fwname, } else { bt_dev_err(hdev, "Failed wmt patch dwnld status (%d)", status); + err = -EIO; goto err_release_fw; } } From 8aca46f91c42020bc58cd56e464a1101e517aa10 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Fri, 3 Dec 2021 16:15:40 -0800 Subject: [PATCH 45/62] Bluetooth: mgmt: Introduce mgmt_alloc_skb and mgmt_send_event_skb This introduces mgmt_alloc_skb and mgmt_send_event_skb which are convenient when building MGMT events that have variable length as the likes of skb_put_data can be used to insert portion directly on the skb instead of having to first build an intermediate buffer just to be copied over the skb. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/bluetooth.h | 6 +++ net/bluetooth/mgmt_util.c | 66 +++++++++++++++++++++++-------- net/bluetooth/mgmt_util.h | 4 ++ 3 files changed, 59 insertions(+), 17 deletions(-) diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h index 2f31e571f34c..77906832353f 100644 --- a/include/net/bluetooth/bluetooth.h +++ b/include/net/bluetooth/bluetooth.h @@ -390,6 +390,11 @@ struct hci_ctrl { }; }; +struct mgmt_ctrl { + struct hci_dev *hdev; + u16 opcode; +}; + struct bt_skb_cb { u8 pkt_type; u8 force_active; @@ -399,6 +404,7 @@ struct bt_skb_cb { struct l2cap_ctrl l2cap; struct sco_ctrl sco; struct hci_ctrl hci; + struct mgmt_ctrl mgmt; }; }; #define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb)) diff --git a/net/bluetooth/mgmt_util.c b/net/bluetooth/mgmt_util.c index 83875f2a0604..edee60bbc7b4 100644 --- a/net/bluetooth/mgmt_util.c +++ b/net/bluetooth/mgmt_util.c @@ -56,40 +56,72 @@ static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie, return skb; } -int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, - void *data, u16 data_len, int flag, struct sock *skip_sk) +struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode, + unsigned int size) { struct sk_buff *skb; - struct mgmt_hdr *hdr; - skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL); + skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL); if (!skb) - return -ENOMEM; + return skb; - hdr = skb_put(skb, sizeof(*hdr)); - hdr->opcode = cpu_to_le16(event); - if (hdev) - hdr->index = cpu_to_le16(hdev->id); - else - hdr->index = cpu_to_le16(MGMT_INDEX_NONE); - hdr->len = cpu_to_le16(data_len); + skb_reserve(skb, sizeof(struct mgmt_hdr)); + bt_cb(skb)->mgmt.hdev = hdev; + bt_cb(skb)->mgmt.opcode = opcode; - if (data) - skb_put_data(skb, data, data_len); + return skb; +} + +int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag, + struct sock *skip_sk) +{ + struct hci_dev *hdev; + struct mgmt_hdr *hdr; + int len = skb->len; + + if (!skb) + return -EINVAL; + + hdev = bt_cb(skb)->mgmt.hdev; /* Time stamp */ __net_timestamp(skb); - hci_send_to_channel(channel, skb, flag, skip_sk); - + /* Send just the data, without headers, to the monitor */ if (channel == HCI_CHANNEL_CONTROL) - hci_send_monitor_ctrl_event(hdev, event, data, data_len, + hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode, + skb->data, skb->len, skb_get_ktime(skb), flag, skip_sk); + hdr = skb_push(skb, sizeof(*hdr)); + hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode); + if (hdev) + hdr->index = cpu_to_le16(hdev->id); + else + hdr->index = cpu_to_le16(MGMT_INDEX_NONE); + hdr->len = cpu_to_le16(len); + + hci_send_to_channel(channel, skb, flag, skip_sk); + kfree_skb(skb); return 0; } +int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, + void *data, u16 data_len, int flag, struct sock *skip_sk) +{ + struct sk_buff *skb; + + skb = mgmt_alloc_skb(hdev, event, data_len); + if (!skb) + return -ENOMEM; + + if (data) + skb_put_data(skb, data, data_len); + + return mgmt_send_event_skb(channel, skb, flag, skip_sk); +} + int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) { struct sk_buff *skb, *mskb; diff --git a/net/bluetooth/mgmt_util.h b/net/bluetooth/mgmt_util.h index 63b965eaaaac..98e40395a383 100644 --- a/net/bluetooth/mgmt_util.h +++ b/net/bluetooth/mgmt_util.h @@ -32,6 +32,10 @@ struct mgmt_pending_cmd { int (*cmd_complete)(struct mgmt_pending_cmd *cmd, u8 status); }; +struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode, + unsigned int size); +int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag, + struct sock *skip_sk); int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, void *data, u16 data_len, int flag, struct sock *skip_sk); int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status); From cf1bce1de7eeb9b48259662cdce5c9d6842a1599 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Fri, 3 Dec 2021 16:15:41 -0800 Subject: [PATCH 46/62] Bluetooth: mgmt: Make use of mgmt_send_event_skb in MGMT_EV_DEVICE_FOUND This makes use of mgmt_alloc_skb to build MGMT_EV_DEVICE_FOUND so the data is copied directly to skb that is then sent using mgmt_send_event_skb eliminating the necessity of intermediary buffers. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/mgmt.c | 52 ++++++++++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 21 deletions(-) diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 17a5f972cc65..5a4f418c814d 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -335,6 +335,12 @@ static int mgmt_event(u16 event, struct hci_dev *hdev, void *data, u16 len, HCI_SOCK_TRUSTED, skip_sk); } +static int mgmt_event_skb(struct sk_buff *skb, struct sock *skip_sk) +{ + return mgmt_send_event_skb(HCI_CHANNEL_CONTROL, skb, HCI_SOCK_TRUSTED, + skip_sk); +} + static u8 le_addr_type(u8 mgmt_addr_type) { if (mgmt_addr_type == BDADDR_LE_PUBLIC) @@ -9550,9 +9556,8 @@ void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, u8 addr_type, u8 *dev_class, s8 rssi, u32 flags, u8 *eir, u16 eir_len, u8 *scan_rsp, u8 scan_rsp_len) { - char buf[512]; - struct mgmt_ev_device_found *ev = (void *)buf; - size_t ev_size; + struct sk_buff *skb; + struct mgmt_ev_device_found *ev; /* Don't send events for a non-kernel initiated discovery. With * LE one exception is if we have pend_le_reports > 0 in which @@ -9587,13 +9592,13 @@ void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, } } - /* Make sure that the buffer is big enough. The 5 extra bytes - * are for the potential CoD field. - */ - if (sizeof(*ev) + eir_len + scan_rsp_len + 5 > sizeof(buf)) + /* Allocate skb. The 5 extra bytes are for the potential CoD field */ + skb = mgmt_alloc_skb(hdev, MGMT_EV_DEVICE_FOUND, + sizeof(*ev) + eir_len + scan_rsp_len + 5); + if (!skb) return; - memset(buf, 0, sizeof(buf)); + ev = skb_put(skb, sizeof(*ev)); /* In case of device discovery with BR/EDR devices (pre 1.2), the * RSSI value was reported as 0 when not available. This behavior @@ -9614,35 +9619,39 @@ void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, if (eir_len > 0) /* Copy EIR or advertising data into event */ - memcpy(ev->eir, eir, eir_len); + skb_put_data(skb, eir, eir_len); - if (dev_class && !eir_get_data(ev->eir, eir_len, EIR_CLASS_OF_DEV, - NULL)) - eir_len = eir_append_data(ev->eir, eir_len, EIR_CLASS_OF_DEV, - dev_class, 3); + if (dev_class && !eir_get_data(eir, eir_len, EIR_CLASS_OF_DEV, NULL)) { + u8 eir_cod[5]; + + eir_len += eir_append_data(eir_cod, 0, EIR_CLASS_OF_DEV, + dev_class, 3); + skb_put_data(skb, eir_cod, sizeof(eir_cod)); + } if (scan_rsp_len > 0) /* Append scan response data to event */ - memcpy(ev->eir + eir_len, scan_rsp, scan_rsp_len); + skb_put_data(skb, scan_rsp, scan_rsp_len); ev->eir_len = cpu_to_le16(eir_len + scan_rsp_len); - ev_size = sizeof(*ev) + eir_len + scan_rsp_len; - mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL); + mgmt_event_skb(skb, NULL); } void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, u8 addr_type, s8 rssi, u8 *name, u8 name_len) { + struct sk_buff *skb; struct mgmt_ev_device_found *ev; - char buf[sizeof(*ev) + HCI_MAX_NAME_LENGTH + 2]; u16 eir_len; u32 flags; - ev = (struct mgmt_ev_device_found *) buf; - - memset(buf, 0, sizeof(buf)); + if (name_len) + skb = mgmt_alloc_skb(hdev, MGMT_EV_DEVICE_FOUND, 2 + name_len); + else + skb = mgmt_alloc_skb(hdev, MGMT_EV_DEVICE_FOUND, 0); + ev = skb_put(skb, sizeof(*ev)); bacpy(&ev->addr.bdaddr, bdaddr); ev->addr.type = link_to_bdaddr(link_type, addr_type); ev->rssi = rssi; @@ -9651,6 +9660,7 @@ void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, name, name_len); flags = 0; + skb_put(skb, eir_len); } else { eir_len = 0; flags = MGMT_DEV_FOUND_NAME_REQUEST_FAILED; @@ -9659,7 +9669,7 @@ void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, ev->eir_len = cpu_to_le16(eir_len); ev->flags = cpu_to_le32(flags); - mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, sizeof(*ev) + eir_len, NULL); + mgmt_event_skb(skb, NULL); } void mgmt_discovering(struct hci_dev *hdev, u8 discovering) From e96741437ef0a5d18144e790ac894397efda0924 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Fri, 3 Dec 2021 16:15:42 -0800 Subject: [PATCH 47/62] Bluetooth: mgmt: Make use of mgmt_send_event_skb in MGMT_EV_DEVICE_CONNECTED This makes use of mgmt_alloc_skb to build MGMT_EV_DEVICE_CONNECTED so the data is copied directly to skb that is then sent using mgmt_send_event_skb eliminating the necessity of intermediary buffers. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/mgmt.c | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 5a4f418c814d..962bb747d2cd 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -9027,11 +9027,19 @@ void mgmt_new_conn_param(struct hci_dev *hdev, bdaddr_t *bdaddr, void mgmt_device_connected(struct hci_dev *hdev, struct hci_conn *conn, u8 *name, u8 name_len) { - char buf[512]; - struct mgmt_ev_device_connected *ev = (void *) buf; + struct sk_buff *skb; + struct mgmt_ev_device_connected *ev; u16 eir_len = 0; u32 flags = 0; + if (conn->le_adv_data_len > 0) + skb = mgmt_alloc_skb(hdev, MGMT_EV_DEVICE_CONNECTED, + conn->le_adv_data_len); + else + skb = mgmt_alloc_skb(hdev, MGMT_EV_DEVICE_CONNECTED, + 2 + name_len + 5); + + ev = skb_put(skb, sizeof(*ev)); bacpy(&ev->addr.bdaddr, &conn->dst); ev->addr.type = link_to_bdaddr(conn->type, conn->dst_type); @@ -9045,24 +9053,26 @@ void mgmt_device_connected(struct hci_dev *hdev, struct hci_conn *conn, * adding any BR/EDR data to the LE adv. */ if (conn->le_adv_data_len > 0) { - memcpy(&ev->eir[eir_len], - conn->le_adv_data, conn->le_adv_data_len); + skb_put_data(skb, conn->le_adv_data, conn->le_adv_data_len); eir_len = conn->le_adv_data_len; } else { - if (name_len > 0) + if (name_len > 0) { eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, name, name_len); + skb_put(skb, eir_len); + } - if (memcmp(conn->dev_class, "\0\0\0", 3) != 0) + if (memcmp(conn->dev_class, "\0\0\0", 3) != 0) { eir_len = eir_append_data(ev->eir, eir_len, EIR_CLASS_OF_DEV, conn->dev_class, 3); + skb_put(skb, 5); + } } ev->eir_len = cpu_to_le16(eir_len); - mgmt_event(MGMT_EV_DEVICE_CONNECTED, hdev, buf, - sizeof(*ev) + eir_len, NULL); + mgmt_event_skb(skb, NULL); } static void disconnect_rsp(struct mgmt_pending_cmd *cmd, void *data) From 9446bdde51ac9cc8023c732ec0cc05e820ae8d20 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 8 Dec 2021 15:43:24 -0800 Subject: [PATCH 48/62] Bluetooth: hci_sync: Fix not always pausing advertising when necessary hci_pause_advertising_sync shall always pause advertising until hci_resume_advertising_sync but instance 0x00 doesn't count in adv_instance_cnt so it was causing it to be skipped. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_sync.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index 3d28ca7ebe45..fd15fb37a52a 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -1701,10 +1701,8 @@ static int hci_pause_advertising_sync(struct hci_dev *hdev) int err; int old_state; - /* If there are no instances or advertising has already been paused - * there is nothing to do. - */ - if (!hdev->adv_instance_cnt || hdev->advertising_paused) + /* If already been paused there is nothing to do. */ + if (hdev->advertising_paused) return 0; bt_dev_dbg(hdev, "Pausing directed advertising"); From 30d57722732d9736554f85f75f9d7ad5402d192e Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 8 Dec 2021 15:35:48 -0800 Subject: [PATCH 49/62] Bluetooth: L2CAP: Fix using wrong mode If user has a set to use SOCK_STREAM the socket would default to L2CAP_MODE_ERTM which later needs to be adjusted if the destination address is LE which doesn't support such mode. Fixes: 15f02b9105625 ("Bluetooth: L2CAP: Add initial code for Enhanced Credit Based Mode") Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/l2cap_sock.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c index 4574c5cb1b59..251017c69ab7 100644 --- a/net/bluetooth/l2cap_sock.c +++ b/net/bluetooth/l2cap_sock.c @@ -161,7 +161,11 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen) break; } - if (chan->psm && bdaddr_type_is_le(chan->src_type)) + /* Use L2CAP_MODE_LE_FLOWCTL (CoC) in case of LE address and + * L2CAP_MODE_EXT_FLOWCTL (ECRED) has not been set. + */ + if (chan->psm && bdaddr_type_is_le(chan->src_type) && + chan->mode != L2CAP_MODE_EXT_FLOWCTL) chan->mode = L2CAP_MODE_LE_FLOWCTL; chan->state = BT_BOUND; @@ -255,7 +259,11 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, return -EINVAL; } - if (chan->psm && bdaddr_type_is_le(chan->src_type) && !chan->mode) + /* Use L2CAP_MODE_LE_FLOWCTL (CoC) in case of LE address and + * L2CAP_MODE_EXT_FLOWCTL (ECRED) has not been set. + */ + if (chan->psm && bdaddr_type_is_le(chan->src_type) && + chan->mode != L2CAP_MODE_EXT_FLOWCTL) chan->mode = L2CAP_MODE_LE_FLOWCTL; l2cap_sock_init_pid(sk); From d2666be51d5f09662929888dd84d1f4d38c97127 Mon Sep 17 00:00:00 2001 From: Zijun Hu Date: Thu, 9 Dec 2021 14:34:01 +0800 Subject: [PATCH 50/62] Bluetooth: btusb: Add two more Bluetooth parts for WCN6855 Add USB IDs (0x10ab, 0x9309) and (0x10ab, 0x9409) to usb_device_id table for WCN6855. * /sys/kernel/debug/usb/devices T: Bus=02 Lev=01 Prnt=01 Port=00 Cnt=01 Dev#= 10 Spd=12 MxCh= 0 D: Ver= 1.10 Cls=e0(wlcon) Sub=01 Prot=01 MxPS=64 #Cfgs= 1 P: Vendor=10ab ProdID=9309 Rev= 0.01 C:* #Ifs= 2 Cfg#= 1 Atr=e0 MxPwr=100mA I:* If#= 0 Alt= 0 #EPs= 3 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=81(I) Atr=03(Int.) MxPS= 16 Ivl=1ms E: Ad=82(I) Atr=02(Bulk) MxPS= 64 Ivl=0ms E: Ad=02(O) Atr=02(Bulk) MxPS= 64 Ivl=0ms I:* If#= 1 Alt= 0 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 0 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 0 Ivl=1ms I: If#= 1 Alt= 1 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 9 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 9 Ivl=1ms I: If#= 1 Alt= 2 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 17 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 17 Ivl=1ms I: If#= 1 Alt= 3 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 25 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 25 Ivl=1ms I: If#= 1 Alt= 4 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 33 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 33 Ivl=1ms I: If#= 1 Alt= 5 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 49 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 49 Ivl=1ms I: If#= 1 Alt= 6 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 63 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 63 Ivl=1ms I: If#= 1 Alt= 7 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 65 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 65 Ivl=1ms T: Bus=02 Lev=01 Prnt=01 Port=00 Cnt=01 Dev#= 11 Spd=12 MxCh= 0 D: Ver= 1.10 Cls=e0(wlcon) Sub=01 Prot=01 MxPS=64 #Cfgs= 1 P: Vendor=10ab ProdID=9409 Rev= 0.01 C:* #Ifs= 2 Cfg#= 1 Atr=e0 MxPwr=100mA I:* If#= 0 Alt= 0 #EPs= 3 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=81(I) Atr=03(Int.) MxPS= 16 Ivl=1ms E: Ad=82(I) Atr=02(Bulk) MxPS= 64 Ivl=0ms E: Ad=02(O) Atr=02(Bulk) MxPS= 64 Ivl=0ms I:* If#= 1 Alt= 0 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 0 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 0 Ivl=1ms I: If#= 1 Alt= 1 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 9 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 9 Ivl=1ms I: If#= 1 Alt= 2 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 17 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 17 Ivl=1ms I: If#= 1 Alt= 3 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 25 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 25 Ivl=1ms I: If#= 1 Alt= 4 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 33 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 33 Ivl=1ms I: If#= 1 Alt= 5 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 49 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 49 Ivl=1ms I: If#= 1 Alt= 6 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 63 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 63 Ivl=1ms I: If#= 1 Alt= 7 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 65 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 65 Ivl=1ms Signed-off-by: Zijun Hu Signed-off-by: Marcel Holtmann Signed-off-by: Luiz Augusto von Dentz --- drivers/bluetooth/btusb.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index c9619096d763..d1bd9ee0a6ab 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -305,6 +305,12 @@ static const struct usb_device_id blacklist_table[] = { { USB_DEVICE(0x0489, 0xe0e3), .driver_info = BTUSB_QCA_WCN6855 | BTUSB_WIDEBAND_SPEECH | BTUSB_VALID_LE_STATES }, + { USB_DEVICE(0x10ab, 0x9309), .driver_info = BTUSB_QCA_WCN6855 | + BTUSB_WIDEBAND_SPEECH | + BTUSB_VALID_LE_STATES }, + { USB_DEVICE(0x10ab, 0x9409), .driver_info = BTUSB_QCA_WCN6855 | + BTUSB_WIDEBAND_SPEECH | + BTUSB_VALID_LE_STATES }, /* Broadcom BCM2035 */ { USB_DEVICE(0x0a5c, 0x2009), .driver_info = BTUSB_BCM92035 }, From 3547a008c8962df2175db1e78b80f27e027ec549 Mon Sep 17 00:00:00 2001 From: Tedd Ho-Jeong An Date: Mon, 13 Dec 2021 23:32:14 -0800 Subject: [PATCH 51/62] Bluetooth: btintel: Add missing quirks and msft ext for legacy bootloader This patch add missing HCI quirks and MSFT extension for legacy bootloader when it is running in the operational firmware. Signed-off-by: Tedd Ho-Jeong An Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btintel.c | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/drivers/bluetooth/btintel.c b/drivers/bluetooth/btintel.c index 8f9109b40961..e1f96df847b8 100644 --- a/drivers/bluetooth/btintel.c +++ b/drivers/bluetooth/btintel.c @@ -2498,10 +2498,14 @@ static int btintel_setup_combined(struct hci_dev *hdev) case 0x12: /* ThP */ case 0x13: /* HrP */ case 0x14: /* CcP */ - /* Some legacy bootloader devices from JfP supports both old - * and TLV based HCI_Intel_Read_Version command. But we don't - * want to use the TLV based setup routines for those legacy - * bootloader device. + /* Some legacy bootloader devices starting from JfP, + * the operational firmware supports both old and TLV based + * HCI_Intel_Read_Version command based on the command + * parameter. + * + * For upgrading firmware case, the TLV based version cannot + * be used because the firmware filename for legacy bootloader + * is based on the old format. * * Also, it is not easy to convert TLV based version from the * legacy version format. @@ -2513,6 +2517,20 @@ static int btintel_setup_combined(struct hci_dev *hdev) err = btintel_read_version(hdev, &ver); if (err) return err; + + /* Apply the device specific HCI quirks + * + * All Legacy bootloader devices support WBS + */ + set_bit(HCI_QUIRK_WIDEBAND_SPEECH_SUPPORTED, &hdev->quirks); + + /* Valid LE States quirk for JfP/ThP familiy */ + if (ver.hw_variant == 0x11 || ver.hw_variant == 0x12) + set_bit(HCI_QUIRK_VALID_LE_STATES, &hdev->quirks); + + /* Setup MSFT Extension support */ + btintel_set_msft_opcode(hdev, ver.hw_variant); + err = btintel_bootloader_setup(hdev, &ver); break; case 0x17: From 6932627425d6d3849aecd43c02158a5312895ad4 Mon Sep 17 00:00:00 2001 From: Aaron Ma Date: Fri, 17 Dec 2021 17:51:50 +0800 Subject: [PATCH 52/62] Bluetooth: btusb: Add support for Foxconn MT7922A Add 2 USB IDs for MT7922A chip. These 2 devices got the same description. T: Bus=01 Lev=01 Prnt=01 Port=02 Cnt=01 Dev#= 2 Spd=480 MxCh= 0 D: Ver= 2.10 Cls=ef(misc ) Sub=02 Prot=01 MxPS=64 #Cfgs= 1 P: Vendor=0489 ProdID=e0d8 Rev= 1.00 T: Bus=03 Lev=01 Prnt=01 Port=02 Cnt=02 Dev#= 3 Spd=480 MxCh= 0 D: Ver= 2.10 Cls=ef(misc ) Sub=02 Prot=01 MxPS=64 #Cfgs= 1 P: Vendor=0489 ProdID=e0d9 Rev= 1.00 S: Manufacturer=MediaTek Inc. S: Product=Wireless_Device S: SerialNumber=000000000 C:* #Ifs= 3 Cfg#= 1 Atr=e0 MxPwr=100mA A: FirstIf#= 0 IfCount= 3 Cls=e0(wlcon) Sub=01 Prot=01 I:* If#= 0 Alt= 0 #EPs= 3 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=81(I) Atr=03(Int.) MxPS= 16 Ivl=125us E: Ad=82(I) Atr=02(Bulk) MxPS= 512 Ivl=0ms E: Ad=02(O) Atr=02(Bulk) MxPS= 512 Ivl=0ms I:* If#= 1 Alt= 0 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 0 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 0 Ivl=1ms I: If#= 1 Alt= 1 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 9 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 9 Ivl=1ms I: If#= 1 Alt= 2 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 17 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 17 Ivl=1ms I: If#= 1 Alt= 3 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 25 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 25 Ivl=1ms I: If#= 1 Alt= 4 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 33 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 33 Ivl=1ms I: If#= 1 Alt= 5 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 49 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 49 Ivl=1ms I: If#= 1 Alt= 6 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=btusb E: Ad=83(I) Atr=01(Isoc) MxPS= 63 Ivl=1ms E: Ad=03(O) Atr=01(Isoc) MxPS= 63 Ivl=1ms I:* If#= 2 Alt= 0 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=(none) E: Ad=8a(I) Atr=03(Int.) MxPS= 64 Ivl=125us E: Ad=0a(O) Atr=03(Int.) MxPS= 64 Ivl=125us I: If#= 2 Alt= 1 #EPs= 2 Cls=e0(wlcon) Sub=01 Prot=01 Driver=(none) E: Ad=8a(I) Atr=03(Int.) MxPS= 512 Ivl=125us E: Ad=0a(O) Atr=03(Int.) MxPS= 512 Ivl=125us Signed-off-by: Aaron Ma Signed-off-by: Marcel Holtmann --- drivers/bluetooth/btusb.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c index d1bd9ee0a6ab..74ec7ca96b69 100644 --- a/drivers/bluetooth/btusb.c +++ b/drivers/bluetooth/btusb.c @@ -441,6 +441,14 @@ static const struct usb_device_id blacklist_table[] = { BTUSB_WIDEBAND_SPEECH | BTUSB_VALID_LE_STATES }, + /* MediaTek MT7922A Bluetooth devices */ + { USB_DEVICE(0x0489, 0xe0d8), .driver_info = BTUSB_MEDIATEK | + BTUSB_WIDEBAND_SPEECH | + BTUSB_VALID_LE_STATES }, + { USB_DEVICE(0x0489, 0xe0d9), .driver_info = BTUSB_MEDIATEK | + BTUSB_WIDEBAND_SPEECH | + BTUSB_VALID_LE_STATES }, + /* Additional Realtek 8723AE Bluetooth devices */ { USB_DEVICE(0x0930, 0x021d), .driver_info = BTUSB_REALTEK }, { USB_DEVICE(0x13d3, 0x3394), .driver_info = BTUSB_REALTEK }, From df1e5c51492fd93ffc293acdcc6f00698d19fedc Mon Sep 17 00:00:00 2001 From: Panicker Harish Date: Wed, 22 Dec 2021 12:59:05 +0530 Subject: [PATCH 53/62] Bluetooth: hci_qca: Stop IBS timer during BT OFF The IBS timers are not stopped properly once BT OFF is triggered. we could see IBS commands being sent along with version command, so stopped IBS timers while Bluetooth is off. Fixes: 3e4be65eb82c ("Bluetooth: hci_qca: Add poweroff support during hci down for wcn3990") Signed-off-by: Panicker Harish Signed-off-by: Marcel Holtmann --- drivers/bluetooth/hci_qca.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/drivers/bluetooth/hci_qca.c b/drivers/bluetooth/hci_qca.c index dd768a8ed7cb..9e99311038ae 100644 --- a/drivers/bluetooth/hci_qca.c +++ b/drivers/bluetooth/hci_qca.c @@ -1928,6 +1928,9 @@ static int qca_power_off(struct hci_dev *hdev) hu->hdev->hw_error = NULL; hu->hdev->cmd_timeout = NULL; + del_timer_sync(&qca->wake_retrans_timer); + del_timer_sync(&qca->tx_idle_timer); + /* Stop sending shutdown command if soc crashes. */ if (soc_type != QCA_ROME && qca->memdump_state == QCA_MEMDUMP_IDLE) { From 744451c162a514044a912cbbd64b7a386035cc5b Mon Sep 17 00:00:00 2001 From: Benjamin Berg Date: Fri, 17 Dec 2021 16:28:09 +0100 Subject: [PATCH 54/62] Bluetooth: hci_sync: Push sync command cancellation to workqueue syzbot reported that hci_cmd_sync_cancel may sleep from the wrong context. To avoid this, create a new work item that pushes the relevant parts into a different context. Note that we keep the old implementation with the name __hci_cmd_sync_cancel as the sleeping behaviour is desired in some cases. Reported-and-tested-by: syzbot+485cc00ea7cf41dfdbf1@syzkaller.appspotmail.com Fixes: c97a747efc93 ("Bluetooth: btusb: Cancel sync commands for certain URB errors") Signed-off-by: Benjamin Berg Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci_core.h | 1 + include/net/bluetooth/hci_sync.h | 1 + net/bluetooth/hci_core.c | 2 +- net/bluetooth/hci_request.c | 2 +- net/bluetooth/hci_sync.c | 27 ++++++++++++++++++++++++++- 5 files changed, 30 insertions(+), 3 deletions(-) diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 4d69dcfebd63..6509109c2413 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -480,6 +480,7 @@ struct hci_dev { struct work_struct cmd_sync_work; struct list_head cmd_sync_work_list; struct mutex cmd_sync_work_lock; + struct work_struct cmd_sync_cancel_work; __u16 discov_timeout; struct delayed_work discov_off; diff --git a/include/net/bluetooth/hci_sync.h b/include/net/bluetooth/hci_sync.h index f4034bf8f1ce..435674cf388e 100644 --- a/include/net/bluetooth/hci_sync.h +++ b/include/net/bluetooth/hci_sync.h @@ -38,6 +38,7 @@ int __hci_cmd_sync_status_sk(struct hci_dev *hdev, u16 opcode, u32 plen, void hci_cmd_sync_init(struct hci_dev *hdev); void hci_cmd_sync_clear(struct hci_dev *hdev); void hci_cmd_sync_cancel(struct hci_dev *hdev, int err); +void __hci_cmd_sync_cancel(struct hci_dev *hdev, int err); int hci_cmd_sync_queue(struct hci_dev *hdev, hci_cmd_sync_work_func_t func, void *data, hci_cmd_sync_work_destroy_t destroy); diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index 38063bf1fdc5..2b7bd3655b07 100644 --- a/net/bluetooth/hci_core.c +++ b/net/bluetooth/hci_core.c @@ -3859,7 +3859,7 @@ static void hci_cmd_work(struct work_struct *work) res = hci_send_frame(hdev, skb); if (res < 0) - hci_cmd_sync_cancel(hdev, -res); + __hci_cmd_sync_cancel(hdev, -res); if (test_bit(HCI_RESET, &hdev->flags)) cancel_delayed_work(&hdev->cmd_timer); diff --git a/net/bluetooth/hci_request.c b/net/bluetooth/hci_request.c index 329c66456cf1..ef5ced467f75 100644 --- a/net/bluetooth/hci_request.c +++ b/net/bluetooth/hci_request.c @@ -2692,7 +2692,7 @@ void hci_request_setup(struct hci_dev *hdev) void hci_request_cancel_all(struct hci_dev *hdev) { - hci_cmd_sync_cancel(hdev, ENODEV); + __hci_cmd_sync_cancel(hdev, ENODEV); cancel_work_sync(&hdev->discov_update); cancel_work_sync(&hdev->scan_update); diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index fd15fb37a52a..2fb8bc496d18 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -313,11 +313,24 @@ static void hci_cmd_sync_work(struct work_struct *work) } } +static void hci_cmd_sync_cancel_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, cmd_sync_cancel_work); + + cancel_delayed_work_sync(&hdev->cmd_timer); + cancel_delayed_work_sync(&hdev->ncmd_timer); + atomic_set(&hdev->cmd_cnt, 1); + + wake_up_interruptible(&hdev->req_wait_q); +} + void hci_cmd_sync_init(struct hci_dev *hdev) { INIT_WORK(&hdev->cmd_sync_work, hci_cmd_sync_work); INIT_LIST_HEAD(&hdev->cmd_sync_work_list); mutex_init(&hdev->cmd_sync_work_lock); + + INIT_WORK(&hdev->cmd_sync_cancel_work, hci_cmd_sync_cancel_work); } void hci_cmd_sync_clear(struct hci_dev *hdev) @@ -335,7 +348,7 @@ void hci_cmd_sync_clear(struct hci_dev *hdev) } } -void hci_cmd_sync_cancel(struct hci_dev *hdev, int err) +void __hci_cmd_sync_cancel(struct hci_dev *hdev, int err) { bt_dev_dbg(hdev, "err 0x%2.2x", err); @@ -350,6 +363,18 @@ void hci_cmd_sync_cancel(struct hci_dev *hdev, int err) wake_up_interruptible(&hdev->req_wait_q); } } + +void hci_cmd_sync_cancel(struct hci_dev *hdev, int err) +{ + bt_dev_dbg(hdev, "err 0x%2.2x", err); + + if (hdev->req_status == HCI_REQ_PEND) { + hdev->req_result = err; + hdev->req_status = HCI_REQ_CANCELED; + + queue_work(hdev->workqueue, &hdev->cmd_sync_cancel_work); + } +} EXPORT_SYMBOL(hci_cmd_sync_cancel); int hci_cmd_sync_queue(struct hci_dev *hdev, hci_cmd_sync_work_func_t func, From fee645033e2c57fdbd8bace4d5ddc2b18bc4032a Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Mon, 13 Dec 2021 13:38:57 -0800 Subject: [PATCH 55/62] Bluetooth: hci_event: Use skb_pull_data when processing inquiry results This makes each result entry to be checked using skb_pull_data instead of acessing them by index. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_event.c | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index c15289b59c3c..240bffeca170 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -4531,7 +4531,15 @@ static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, void *edata, for (i = 0; i < ev->res2->num; i++) { u32 flags; - info = &ev->res2->info[i]; + info = hci_ev_skb_pull(hdev, skb, + HCI_EV_INQUIRY_RESULT_WITH_RSSI, + sizeof(*info)); + if (!info) { + bt_dev_err(hdev, "Malformed HCI Event: 0x%2.2x", + HCI_EV_INQUIRY_RESULT_WITH_RSSI); + return; + } + bacpy(&data.bdaddr, &info->bdaddr); data.pscan_rep_mode = info->pscan_rep_mode; data.pscan_period_mode = info->pscan_period_mode; @@ -4553,7 +4561,15 @@ static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, void *edata, for (i = 0; i < ev->res1->num; i++) { u32 flags; - info = &ev->res1->info[i]; + info = hci_ev_skb_pull(hdev, skb, + HCI_EV_INQUIRY_RESULT_WITH_RSSI, + sizeof(*info)); + if (!info) { + bt_dev_err(hdev, "Malformed HCI Event: 0x%2.2x", + HCI_EV_INQUIRY_RESULT_WITH_RSSI); + return; + } + bacpy(&data.bdaddr, &info->bdaddr); data.pscan_rep_mode = info->pscan_rep_mode; data.pscan_period_mode = info->pscan_period_mode; From 8e8b92ee60de5341e9db83c11f75a525e555e2b3 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 22 Dec 2021 12:21:56 -0800 Subject: [PATCH 56/62] Bluetooth: hci_sync: Add hci_le_create_conn_sync This adds hci_le_create_conn_sync and make hci_le_connect use it instead of queueing multiple commands which may conflict with the likes of hci_update_passive_scan which uses hci_cmd_sync_queue. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci_core.h | 3 +- include/net/bluetooth/hci_sync.h | 4 + net/bluetooth/hci_conn.c | 305 ++----------------------------- net/bluetooth/hci_event.c | 6 +- net/bluetooth/hci_request.c | 50 ----- net/bluetooth/hci_request.h | 2 - net/bluetooth/hci_sync.c | 277 ++++++++++++++++++++++++++++ net/bluetooth/l2cap_core.c | 2 +- 8 files changed, 300 insertions(+), 349 deletions(-) diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 6509109c2413..1ab94960764e 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -1121,8 +1121,7 @@ struct hci_conn *hci_connect_le_scan(struct hci_dev *hdev, bdaddr_t *dst, enum conn_reasons conn_reason); struct hci_conn *hci_connect_le(struct hci_dev *hdev, bdaddr_t *dst, u8 dst_type, bool dst_resolved, u8 sec_level, - u16 conn_timeout, u8 role, - bdaddr_t *direct_rpa); + u16 conn_timeout, u8 role); struct hci_conn *hci_connect_acl(struct hci_dev *hdev, bdaddr_t *dst, u8 sec_level, u8 auth_type, enum conn_reasons conn_reason); diff --git a/include/net/bluetooth/hci_sync.h b/include/net/bluetooth/hci_sync.h index 435674cf388e..2492e3b46a8f 100644 --- a/include/net/bluetooth/hci_sync.h +++ b/include/net/bluetooth/hci_sync.h @@ -102,3 +102,7 @@ int hci_stop_discovery_sync(struct hci_dev *hdev); int hci_suspend_sync(struct hci_dev *hdev); int hci_resume_sync(struct hci_dev *hdev); + +struct hci_conn; + +int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn); diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c index cd6e1cf7e396..04ebe901e86f 100644 --- a/net/bluetooth/hci_conn.c +++ b/net/bluetooth/hci_conn.c @@ -911,267 +911,45 @@ void hci_le_conn_failed(struct hci_conn *conn, u8 status) hci_enable_advertising(hdev); } -static void create_le_conn_complete(struct hci_dev *hdev, u8 status, u16 opcode) +static void create_le_conn_complete(struct hci_dev *hdev, void *data, int err) { - struct hci_conn *conn; + struct hci_conn *conn = data; hci_dev_lock(hdev); - conn = hci_lookup_le_connect(hdev); - - if (hdev->adv_instance_cnt) - hci_req_resume_adv_instances(hdev); - - if (!status) { + if (!err) { hci_connect_le_scan_cleanup(conn); goto done; } - bt_dev_err(hdev, "request failed to create LE connection: " - "status 0x%2.2x", status); + bt_dev_err(hdev, "request failed to create LE connection: err %d", err); if (!conn) goto done; - hci_le_conn_failed(conn, status); + hci_le_conn_failed(conn, err); done: hci_dev_unlock(hdev); } -static bool conn_use_rpa(struct hci_conn *conn) +static int hci_connect_le_sync(struct hci_dev *hdev, void *data) { - struct hci_dev *hdev = conn->hdev; + struct hci_conn *conn = data; - return hci_dev_test_flag(hdev, HCI_PRIVACY); -} + bt_dev_dbg(hdev, "conn %p", conn); -static void set_ext_conn_params(struct hci_conn *conn, - struct hci_cp_le_ext_conn_param *p) -{ - struct hci_dev *hdev = conn->hdev; - - memset(p, 0, sizeof(*p)); - - p->scan_interval = cpu_to_le16(hdev->le_scan_int_connect); - p->scan_window = cpu_to_le16(hdev->le_scan_window_connect); - p->conn_interval_min = cpu_to_le16(conn->le_conn_min_interval); - p->conn_interval_max = cpu_to_le16(conn->le_conn_max_interval); - p->conn_latency = cpu_to_le16(conn->le_conn_latency); - p->supervision_timeout = cpu_to_le16(conn->le_supv_timeout); - p->min_ce_len = cpu_to_le16(0x0000); - p->max_ce_len = cpu_to_le16(0x0000); -} - -static void hci_req_add_le_create_conn(struct hci_request *req, - struct hci_conn *conn, - bdaddr_t *direct_rpa) -{ - struct hci_dev *hdev = conn->hdev; - u8 own_addr_type; - - /* If direct address was provided we use it instead of current - * address. - */ - if (direct_rpa) { - if (bacmp(&req->hdev->random_addr, direct_rpa)) - hci_req_add(req, HCI_OP_LE_SET_RANDOM_ADDR, 6, - direct_rpa); - - /* direct address is always RPA */ - own_addr_type = ADDR_LE_DEV_RANDOM; - } else { - /* Update random address, but set require_privacy to false so - * that we never connect with an non-resolvable address. - */ - if (hci_update_random_address(req, false, conn_use_rpa(conn), - &own_addr_type)) - return; - } - - if (use_ext_conn(hdev)) { - struct hci_cp_le_ext_create_conn *cp; - struct hci_cp_le_ext_conn_param *p; - u8 data[sizeof(*cp) + sizeof(*p) * 3]; - u32 plen; - - cp = (void *) data; - p = (void *) cp->data; - - memset(cp, 0, sizeof(*cp)); - - bacpy(&cp->peer_addr, &conn->dst); - cp->peer_addr_type = conn->dst_type; - cp->own_addr_type = own_addr_type; - - plen = sizeof(*cp); - - if (scan_1m(hdev)) { - cp->phys |= LE_SCAN_PHY_1M; - set_ext_conn_params(conn, p); - - p++; - plen += sizeof(*p); - } - - if (scan_2m(hdev)) { - cp->phys |= LE_SCAN_PHY_2M; - set_ext_conn_params(conn, p); - - p++; - plen += sizeof(*p); - } - - if (scan_coded(hdev)) { - cp->phys |= LE_SCAN_PHY_CODED; - set_ext_conn_params(conn, p); - - plen += sizeof(*p); - } - - hci_req_add(req, HCI_OP_LE_EXT_CREATE_CONN, plen, data); - - } else { - struct hci_cp_le_create_conn cp; - - memset(&cp, 0, sizeof(cp)); - - cp.scan_interval = cpu_to_le16(hdev->le_scan_int_connect); - cp.scan_window = cpu_to_le16(hdev->le_scan_window_connect); - - bacpy(&cp.peer_addr, &conn->dst); - cp.peer_addr_type = conn->dst_type; - cp.own_address_type = own_addr_type; - cp.conn_interval_min = cpu_to_le16(conn->le_conn_min_interval); - cp.conn_interval_max = cpu_to_le16(conn->le_conn_max_interval); - cp.conn_latency = cpu_to_le16(conn->le_conn_latency); - cp.supervision_timeout = cpu_to_le16(conn->le_supv_timeout); - cp.min_ce_len = cpu_to_le16(0x0000); - cp.max_ce_len = cpu_to_le16(0x0000); - - hci_req_add(req, HCI_OP_LE_CREATE_CONN, sizeof(cp), &cp); - } - - conn->state = BT_CONNECT; - clear_bit(HCI_CONN_SCANNING, &conn->flags); -} - -static void hci_req_directed_advertising(struct hci_request *req, - struct hci_conn *conn) -{ - struct hci_dev *hdev = req->hdev; - u8 own_addr_type; - u8 enable; - - if (ext_adv_capable(hdev)) { - struct hci_cp_le_set_ext_adv_params cp; - bdaddr_t random_addr; - - /* Set require_privacy to false so that the remote device has a - * chance of identifying us. - */ - if (hci_get_random_address(hdev, false, conn_use_rpa(conn), NULL, - &own_addr_type, &random_addr) < 0) - return; - - memset(&cp, 0, sizeof(cp)); - - cp.evt_properties = cpu_to_le16(LE_LEGACY_ADV_DIRECT_IND); - cp.own_addr_type = own_addr_type; - cp.channel_map = hdev->le_adv_channel_map; - cp.tx_power = HCI_TX_POWER_INVALID; - cp.primary_phy = HCI_ADV_PHY_1M; - cp.secondary_phy = HCI_ADV_PHY_1M; - cp.handle = 0; /* Use instance 0 for directed adv */ - cp.own_addr_type = own_addr_type; - cp.peer_addr_type = conn->dst_type; - bacpy(&cp.peer_addr, &conn->dst); - - /* As per Core Spec 5.2 Vol 2, PART E, Sec 7.8.53, for - * advertising_event_property LE_LEGACY_ADV_DIRECT_IND - * does not supports advertising data when the advertising set already - * contains some, the controller shall return erroc code 'Invalid - * HCI Command Parameters(0x12). - * So it is required to remove adv set for handle 0x00. since we use - * instance 0 for directed adv. - */ - __hci_req_remove_ext_adv_instance(req, cp.handle); - - hci_req_add(req, HCI_OP_LE_SET_EXT_ADV_PARAMS, sizeof(cp), &cp); - - if (own_addr_type == ADDR_LE_DEV_RANDOM && - bacmp(&random_addr, BDADDR_ANY) && - bacmp(&random_addr, &hdev->random_addr)) { - struct hci_cp_le_set_adv_set_rand_addr cp; - - memset(&cp, 0, sizeof(cp)); - - cp.handle = 0; - bacpy(&cp.bdaddr, &random_addr); - - hci_req_add(req, - HCI_OP_LE_SET_ADV_SET_RAND_ADDR, - sizeof(cp), &cp); - } - - __hci_req_enable_ext_advertising(req, 0x00); - } else { - struct hci_cp_le_set_adv_param cp; - - /* Clear the HCI_LE_ADV bit temporarily so that the - * hci_update_random_address knows that it's safe to go ahead - * and write a new random address. The flag will be set back on - * as soon as the SET_ADV_ENABLE HCI command completes. - */ - hci_dev_clear_flag(hdev, HCI_LE_ADV); - - /* Set require_privacy to false so that the remote device has a - * chance of identifying us. - */ - if (hci_update_random_address(req, false, conn_use_rpa(conn), - &own_addr_type) < 0) - return; - - memset(&cp, 0, sizeof(cp)); - - /* Some controllers might reject command if intervals are not - * within range for undirected advertising. - * BCM20702A0 is known to be affected by this. - */ - cp.min_interval = cpu_to_le16(0x0020); - cp.max_interval = cpu_to_le16(0x0020); - - cp.type = LE_ADV_DIRECT_IND; - cp.own_address_type = own_addr_type; - cp.direct_addr_type = conn->dst_type; - bacpy(&cp.direct_addr, &conn->dst); - cp.channel_map = hdev->le_adv_channel_map; - - hci_req_add(req, HCI_OP_LE_SET_ADV_PARAM, sizeof(cp), &cp); - - enable = 0x01; - hci_req_add(req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(enable), - &enable); - } - - conn->state = BT_CONNECT; + return hci_le_create_conn_sync(hdev, conn); } struct hci_conn *hci_connect_le(struct hci_dev *hdev, bdaddr_t *dst, u8 dst_type, bool dst_resolved, u8 sec_level, - u16 conn_timeout, u8 role, bdaddr_t *direct_rpa) + u16 conn_timeout, u8 role) { - struct hci_conn_params *params; struct hci_conn *conn; struct smp_irk *irk; - struct hci_request req; int err; - /* This ensures that during disable le_scan address resolution - * will not be disabled if it is followed by le_create_conn - */ - bool rpa_le_conn = true; - /* Let's make sure that le is enabled.*/ if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED)) { if (lmp_le_capable(hdev)) @@ -1230,68 +1008,13 @@ struct hci_conn *hci_connect_le(struct hci_dev *hdev, bdaddr_t *dst, conn->sec_level = BT_SECURITY_LOW; conn->conn_timeout = conn_timeout; - hci_req_init(&req, hdev); + conn->state = BT_CONNECT; + clear_bit(HCI_CONN_SCANNING, &conn->flags); - /* Disable advertising if we're active. For central role - * connections most controllers will refuse to connect if - * advertising is enabled, and for peripheral role connections we - * anyway have to disable it in order to start directed - * advertising. Any registered advertisements will be - * re-enabled after the connection attempt is finished. - */ - if (hci_dev_test_flag(hdev, HCI_LE_ADV)) - __hci_req_pause_adv_instances(&req); - - /* If requested to connect as peripheral use directed advertising */ - if (conn->role == HCI_ROLE_SLAVE) { - /* If we're active scanning most controllers are unable - * to initiate advertising. Simply reject the attempt. - */ - if (hci_dev_test_flag(hdev, HCI_LE_SCAN) && - hdev->le_scan_type == LE_SCAN_ACTIVE) { - hci_req_purge(&req); - hci_conn_del(conn); - return ERR_PTR(-EBUSY); - } - - hci_req_directed_advertising(&req, conn); - goto create_conn; - } - - params = hci_conn_params_lookup(hdev, &conn->dst, conn->dst_type); - if (params) { - conn->le_conn_min_interval = params->conn_min_interval; - conn->le_conn_max_interval = params->conn_max_interval; - conn->le_conn_latency = params->conn_latency; - conn->le_supv_timeout = params->supervision_timeout; - } else { - conn->le_conn_min_interval = hdev->le_conn_min_interval; - conn->le_conn_max_interval = hdev->le_conn_max_interval; - conn->le_conn_latency = hdev->le_conn_latency; - conn->le_supv_timeout = hdev->le_supv_timeout; - } - - /* If controller is scanning, we stop it since some controllers are - * not able to scan and connect at the same time. Also set the - * HCI_LE_SCAN_INTERRUPTED flag so that the command complete - * handler for scan disabling knows to set the correct discovery - * state. - */ - if (hci_dev_test_flag(hdev, HCI_LE_SCAN)) { - hci_req_add_le_scan_disable(&req, rpa_le_conn); - hci_dev_set_flag(hdev, HCI_LE_SCAN_INTERRUPTED); - } - - hci_req_add_le_create_conn(&req, conn, direct_rpa); - -create_conn: - err = hci_req_run(&req, create_le_conn_complete); + err = hci_cmd_sync_queue(hdev, hci_connect_le_sync, conn, + create_le_conn_complete); if (err) { hci_conn_del(conn); - - if (hdev->adv_instance_cnt) - hci_req_resume_adv_instances(hdev); - return ERR_PTR(err); } diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 240bffeca170..ca8e022a88e6 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -5755,7 +5755,7 @@ static void hci_le_conn_update_complete_evt(struct hci_dev *hdev, void *data, static struct hci_conn *check_pending_le_conn(struct hci_dev *hdev, bdaddr_t *addr, u8 addr_type, bool addr_resolved, - u8 adv_type, bdaddr_t *direct_rpa) + u8 adv_type) { struct hci_conn *conn; struct hci_conn_params *params; @@ -5810,7 +5810,7 @@ static struct hci_conn *check_pending_le_conn(struct hci_dev *hdev, conn = hci_connect_le(hdev, addr, addr_type, addr_resolved, BT_SECURITY_LOW, hdev->def_le_autoconnect_timeout, - HCI_ROLE_MASTER, direct_rpa); + HCI_ROLE_MASTER); if (!IS_ERR(conn)) { /* If HCI_AUTO_CONN_EXPLICIT is set, conn is already owned * by higher layer that tried to connect, if no then @@ -5933,7 +5933,7 @@ static void process_adv_report(struct hci_dev *hdev, u8 type, bdaddr_t *bdaddr, * for advertising reports) and is already verified to be RPA above. */ conn = check_pending_le_conn(hdev, bdaddr, bdaddr_type, bdaddr_resolved, - type, direct_addr); + type); if (!ext_adv && conn && type == LE_ADV_IND && len <= HCI_MAX_AD_LENGTH) { /* Store report for later inclusion by * mgmt_device_connected diff --git a/net/bluetooth/hci_request.c b/net/bluetooth/hci_request.c index ef5ced467f75..42c8047a9897 100644 --- a/net/bluetooth/hci_request.c +++ b/net/bluetooth/hci_request.c @@ -818,56 +818,6 @@ static void cancel_adv_timeout(struct hci_dev *hdev) } } -/* This function requires the caller holds hdev->lock */ -void __hci_req_pause_adv_instances(struct hci_request *req) -{ - bt_dev_dbg(req->hdev, "Pausing advertising instances"); - - /* Call to disable any advertisements active on the controller. - * This will succeed even if no advertisements are configured. - */ - __hci_req_disable_advertising(req); - - /* If we are using software rotation, pause the loop */ - if (!ext_adv_capable(req->hdev)) - cancel_adv_timeout(req->hdev); -} - -/* This function requires the caller holds hdev->lock */ -static void __hci_req_resume_adv_instances(struct hci_request *req) -{ - struct adv_info *adv; - - bt_dev_dbg(req->hdev, "Resuming advertising instances"); - - if (ext_adv_capable(req->hdev)) { - /* Call for each tracked instance to be re-enabled */ - list_for_each_entry(adv, &req->hdev->adv_instances, list) { - __hci_req_enable_ext_advertising(req, - adv->instance); - } - - } else { - /* Schedule for most recent instance to be restarted and begin - * the software rotation loop - */ - __hci_req_schedule_adv_instance(req, - req->hdev->cur_adv_instance, - true); - } -} - -/* This function requires the caller holds hdev->lock */ -int hci_req_resume_adv_instances(struct hci_dev *hdev) -{ - struct hci_request req; - - hci_req_init(&req, hdev); - __hci_req_resume_adv_instances(&req); - - return hci_req_run(&req, NULL); -} - static bool adv_cur_instance_is_scannable(struct hci_dev *hdev) { return hci_adv_instance_is_scannable(hdev, hdev->cur_adv_instance); diff --git a/net/bluetooth/hci_request.h b/net/bluetooth/hci_request.h index 8d39e9416861..7f8df258e295 100644 --- a/net/bluetooth/hci_request.h +++ b/net/bluetooth/hci_request.h @@ -80,8 +80,6 @@ void hci_req_add_le_passive_scan(struct hci_request *req); void hci_req_prepare_suspend(struct hci_dev *hdev, enum suspended_state next); void hci_req_disable_address_resolution(struct hci_dev *hdev); -void __hci_req_pause_adv_instances(struct hci_request *req); -int hci_req_resume_adv_instances(struct hci_dev *hdev); void hci_req_reenable_advertising(struct hci_dev *hdev); void __hci_req_enable_advertising(struct hci_request *req); void __hci_req_disable_advertising(struct hci_request *req); diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index 2fb8bc496d18..a3cf84873baa 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -4999,3 +4999,280 @@ int hci_resume_sync(struct hci_dev *hdev) return 0; } + +static bool conn_use_rpa(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + return hci_dev_test_flag(hdev, HCI_PRIVACY); +} + +static int hci_le_ext_directed_advertising_sync(struct hci_dev *hdev, + struct hci_conn *conn) +{ + struct hci_cp_le_set_ext_adv_params cp; + int err; + bdaddr_t random_addr; + u8 own_addr_type; + + err = hci_update_random_address_sync(hdev, false, conn_use_rpa(conn), + &own_addr_type); + if (err) + return err; + + /* Set require_privacy to false so that the remote device has a + * chance of identifying us. + */ + err = hci_get_random_address(hdev, false, conn_use_rpa(conn), NULL, + &own_addr_type, &random_addr); + if (err) + return err; + + memset(&cp, 0, sizeof(cp)); + + cp.evt_properties = cpu_to_le16(LE_LEGACY_ADV_DIRECT_IND); + cp.own_addr_type = own_addr_type; + cp.channel_map = hdev->le_adv_channel_map; + cp.tx_power = HCI_TX_POWER_INVALID; + cp.primary_phy = HCI_ADV_PHY_1M; + cp.secondary_phy = HCI_ADV_PHY_1M; + cp.handle = 0x00; /* Use instance 0 for directed adv */ + cp.own_addr_type = own_addr_type; + cp.peer_addr_type = conn->dst_type; + bacpy(&cp.peer_addr, &conn->dst); + + /* As per Core Spec 5.2 Vol 2, PART E, Sec 7.8.53, for + * advertising_event_property LE_LEGACY_ADV_DIRECT_IND + * does not supports advertising data when the advertising set already + * contains some, the controller shall return erroc code 'Invalid + * HCI Command Parameters(0x12). + * So it is required to remove adv set for handle 0x00. since we use + * instance 0 for directed adv. + */ + err = hci_remove_ext_adv_instance_sync(hdev, cp.handle, NULL); + if (err) + return err; + + err = __hci_cmd_sync_status(hdev, HCI_OP_LE_SET_EXT_ADV_PARAMS, + sizeof(cp), &cp, HCI_CMD_TIMEOUT); + if (err) + return err; + + /* Check if random address need to be updated */ + if (own_addr_type == ADDR_LE_DEV_RANDOM && + bacmp(&random_addr, BDADDR_ANY) && + bacmp(&random_addr, &hdev->random_addr)) { + err = hci_set_adv_set_random_addr_sync(hdev, 0x00, + &random_addr); + if (err) + return err; + } + + return hci_enable_ext_advertising_sync(hdev, 0x00); +} + +static int hci_le_directed_advertising_sync(struct hci_dev *hdev, + struct hci_conn *conn) +{ + struct hci_cp_le_set_adv_param cp; + u8 status; + u8 own_addr_type; + u8 enable; + + if (ext_adv_capable(hdev)) + return hci_le_ext_directed_advertising_sync(hdev, conn); + + /* Clear the HCI_LE_ADV bit temporarily so that the + * hci_update_random_address knows that it's safe to go ahead + * and write a new random address. The flag will be set back on + * as soon as the SET_ADV_ENABLE HCI command completes. + */ + hci_dev_clear_flag(hdev, HCI_LE_ADV); + + /* Set require_privacy to false so that the remote device has a + * chance of identifying us. + */ + status = hci_update_random_address_sync(hdev, false, conn_use_rpa(conn), + &own_addr_type); + if (status) + return status; + + memset(&cp, 0, sizeof(cp)); + + /* Some controllers might reject command if intervals are not + * within range for undirected advertising. + * BCM20702A0 is known to be affected by this. + */ + cp.min_interval = cpu_to_le16(0x0020); + cp.max_interval = cpu_to_le16(0x0020); + + cp.type = LE_ADV_DIRECT_IND; + cp.own_address_type = own_addr_type; + cp.direct_addr_type = conn->dst_type; + bacpy(&cp.direct_addr, &conn->dst); + cp.channel_map = hdev->le_adv_channel_map; + + status = __hci_cmd_sync_status(hdev, HCI_OP_LE_SET_ADV_PARAM, + sizeof(cp), &cp, HCI_CMD_TIMEOUT); + if (status) + return status; + + enable = 0x01; + + return __hci_cmd_sync_status(hdev, HCI_OP_LE_SET_ADV_ENABLE, + sizeof(enable), &enable, HCI_CMD_TIMEOUT); +} + +static void set_ext_conn_params(struct hci_conn *conn, + struct hci_cp_le_ext_conn_param *p) +{ + struct hci_dev *hdev = conn->hdev; + + memset(p, 0, sizeof(*p)); + + p->scan_interval = cpu_to_le16(hdev->le_scan_int_connect); + p->scan_window = cpu_to_le16(hdev->le_scan_window_connect); + p->conn_interval_min = cpu_to_le16(conn->le_conn_min_interval); + p->conn_interval_max = cpu_to_le16(conn->le_conn_max_interval); + p->conn_latency = cpu_to_le16(conn->le_conn_latency); + p->supervision_timeout = cpu_to_le16(conn->le_supv_timeout); + p->min_ce_len = cpu_to_le16(0x0000); + p->max_ce_len = cpu_to_le16(0x0000); +} + +int hci_le_ext_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn, + u8 own_addr_type) +{ + struct hci_cp_le_ext_create_conn *cp; + struct hci_cp_le_ext_conn_param *p; + u8 data[sizeof(*cp) + sizeof(*p) * 3]; + u32 plen; + + cp = (void *)data; + p = (void *)cp->data; + + memset(cp, 0, sizeof(*cp)); + + bacpy(&cp->peer_addr, &conn->dst); + cp->peer_addr_type = conn->dst_type; + cp->own_addr_type = own_addr_type; + + plen = sizeof(*cp); + + if (scan_1m(hdev)) { + cp->phys |= LE_SCAN_PHY_1M; + set_ext_conn_params(conn, p); + + p++; + plen += sizeof(*p); + } + + if (scan_2m(hdev)) { + cp->phys |= LE_SCAN_PHY_2M; + set_ext_conn_params(conn, p); + + p++; + plen += sizeof(*p); + } + + if (scan_coded(hdev)) { + cp->phys |= LE_SCAN_PHY_CODED; + set_ext_conn_params(conn, p); + + plen += sizeof(*p); + } + + return __hci_cmd_sync_status(hdev, HCI_OP_LE_EXT_CREATE_CONN, + plen, data, HCI_CMD_TIMEOUT); +} + +int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn) +{ + struct hci_cp_le_create_conn cp; + struct hci_conn_params *params; + u8 own_addr_type; + int err; + + /* Disable advertising if we're active. For central role + * connections most controllers will refuse to connect if + * advertising is enabled, and for peripheral role connections we + * anyway have to disable it in order to start directed + * advertising. Any registered advertisements will be + * re-enabled after the connection attempt is finished. + */ + hci_pause_advertising_sync(hdev); + + /* If requested to connect as peripheral use directed advertising */ + if (conn->role == HCI_ROLE_SLAVE) { + /* If we're active scanning most controllers are unable + * to initiate advertising. Simply reject the attempt. + */ + if (hci_dev_test_flag(hdev, HCI_LE_SCAN) && + hdev->le_scan_type == LE_SCAN_ACTIVE) { + hci_conn_del(conn); + return -EBUSY; + } + + err = hci_le_directed_advertising_sync(hdev, conn); + goto done; + } + + params = hci_conn_params_lookup(hdev, &conn->dst, conn->dst_type); + if (params) { + conn->le_conn_min_interval = params->conn_min_interval; + conn->le_conn_max_interval = params->conn_max_interval; + conn->le_conn_latency = params->conn_latency; + conn->le_supv_timeout = params->supervision_timeout; + } else { + conn->le_conn_min_interval = hdev->le_conn_min_interval; + conn->le_conn_max_interval = hdev->le_conn_max_interval; + conn->le_conn_latency = hdev->le_conn_latency; + conn->le_supv_timeout = hdev->le_supv_timeout; + } + + /* If controller is scanning, we stop it since some controllers are + * not able to scan and connect at the same time. Also set the + * HCI_LE_SCAN_INTERRUPTED flag so that the command complete + * handler for scan disabling knows to set the correct discovery + * state. + */ + if (hci_dev_test_flag(hdev, HCI_LE_SCAN)) { + hci_scan_disable_sync(hdev); + hci_dev_set_flag(hdev, HCI_LE_SCAN_INTERRUPTED); + } + + /* Update random address, but set require_privacy to false so + * that we never connect with an non-resolvable address. + */ + err = hci_update_random_address_sync(hdev, false, conn_use_rpa(conn), + &own_addr_type); + if (err) + goto done; + + if (use_ext_conn(hdev)) { + err = hci_le_ext_create_conn_sync(hdev, conn, own_addr_type); + goto done; + } + + memset(&cp, 0, sizeof(cp)); + + cp.scan_interval = cpu_to_le16(hdev->le_scan_int_connect); + cp.scan_window = cpu_to_le16(hdev->le_scan_window_connect); + + bacpy(&cp.peer_addr, &conn->dst); + cp.peer_addr_type = conn->dst_type; + cp.own_address_type = own_addr_type; + cp.conn_interval_min = cpu_to_le16(conn->le_conn_min_interval); + cp.conn_interval_max = cpu_to_le16(conn->le_conn_max_interval); + cp.conn_latency = cpu_to_le16(conn->le_conn_latency); + cp.supervision_timeout = cpu_to_le16(conn->le_supv_timeout); + cp.min_ce_len = cpu_to_le16(0x0000); + cp.max_ce_len = cpu_to_le16(0x0000); + + err = __hci_cmd_sync_status(hdev, HCI_OP_LE_CREATE_CONN, + sizeof(cp), &cp, HCI_CMD_TIMEOUT); + +done: + hci_resume_advertising_sync(hdev); + return err; +} diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index 4f8f37599962..e817ff0607a0 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -7905,7 +7905,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, hcon = hci_connect_le(hdev, dst, dst_type, false, chan->sec_level, HCI_LE_CONN_TIMEOUT, - HCI_ROLE_SLAVE, NULL); + HCI_ROLE_SLAVE); else hcon = hci_connect_le_scan(hdev, dst, dst_type, chan->sec_level, From 85b56857e194635b772be8af1c7650535d5d112a Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 22 Dec 2021 12:21:57 -0800 Subject: [PATCH 57/62] Bluetooth: hci_sync: Add support for waiting specific LE subevents This adds support for waiting for specific LE subevents instead of command status which may only indicate that the commands is in progress and a different event is used to complete the operation. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/bluetooth.h | 1 + net/bluetooth/hci_event.c | 41 ++++++++++++++++++++----------- net/bluetooth/hci_sync.c | 2 +- 3 files changed, 28 insertions(+), 16 deletions(-) diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h index 77906832353f..4b3d0b16c185 100644 --- a/include/net/bluetooth/bluetooth.h +++ b/include/net/bluetooth/bluetooth.h @@ -412,6 +412,7 @@ struct bt_skb_cb { #define hci_skb_pkt_type(skb) bt_cb((skb))->pkt_type #define hci_skb_expect(skb) bt_cb((skb))->expect #define hci_skb_opcode(skb) bt_cb((skb))->hci.opcode +#define hci_skb_event(skb) bt_cb((skb))->hci.req_event #define hci_skb_sk(skb) bt_cb((skb))->hci.sk static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how) diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index ca8e022a88e6..f1082b7c0218 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -4038,15 +4038,14 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, void *data, * (since for this kind of commands there will not be a command * complete event). */ - if (ev->status || - (hdev->sent_cmd && !bt_cb(hdev->sent_cmd)->hci.req_event)) + if (ev->status || (hdev->sent_cmd && !hci_skb_event(hdev->sent_cmd))) { hci_req_cmd_complete(hdev, *opcode, ev->status, req_complete, req_complete_skb); - - if (hci_dev_test_flag(hdev, HCI_CMD_PENDING)) { - bt_dev_err(hdev, - "unexpected event for opcode 0x%4.4x", *opcode); - return; + if (hci_dev_test_flag(hdev, HCI_CMD_PENDING)) { + bt_dev_err(hdev, "unexpected event for opcode 0x%4.4x", + *opcode); + return; + } } if (atomic_read(&hdev->cmd_cnt) && !skb_queue_empty(&hdev->cmd_q)) @@ -6464,13 +6463,24 @@ static const struct hci_le_ev { }; static void hci_le_meta_evt(struct hci_dev *hdev, void *data, - struct sk_buff *skb) + struct sk_buff *skb, u16 *opcode, u8 *status, + hci_req_complete_t *req_complete, + hci_req_complete_skb_t *req_complete_skb) { struct hci_ev_le_meta *ev = data; const struct hci_le_ev *subev; bt_dev_dbg(hdev, "subevent 0x%2.2x", ev->subevent); + /* Only match event if command OGF is for LE */ + if (hdev->sent_cmd && + hci_opcode_ogf(hci_skb_opcode(hdev->sent_cmd)) == 0x08 && + hci_skb_event(hdev->sent_cmd) == ev->subevent) { + *opcode = hci_skb_opcode(hdev->sent_cmd); + hci_req_cmd_complete(hdev, *opcode, 0x00, req_complete, + req_complete_skb); + } + subev = &hci_le_ev_table[ev->subevent]; if (!subev->func) return; @@ -6764,8 +6774,8 @@ static const struct hci_ev { HCI_EV(HCI_EV_REMOTE_HOST_FEATURES, hci_remote_host_features_evt, sizeof(struct hci_ev_remote_host_features)), /* [0x3e = HCI_EV_LE_META] */ - HCI_EV_VL(HCI_EV_LE_META, hci_le_meta_evt, - sizeof(struct hci_ev_le_meta), HCI_MAX_EVENT_SIZE), + HCI_EV_REQ_VL(HCI_EV_LE_META, hci_le_meta_evt, + sizeof(struct hci_ev_le_meta), HCI_MAX_EVENT_SIZE), #if IS_ENABLED(CONFIG_BT_HS) /* [0x40 = HCI_EV_PHY_LINK_COMPLETE] */ HCI_EV(HCI_EV_PHY_LINK_COMPLETE, hci_phy_link_complete_evt, @@ -6849,11 +6859,12 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb) goto done; } - if (hdev->sent_cmd && bt_cb(hdev->sent_cmd)->hci.req_event == event) { - struct hci_command_hdr *cmd_hdr = (void *) hdev->sent_cmd->data; - opcode = __le16_to_cpu(cmd_hdr->opcode); - hci_req_cmd_complete(hdev, opcode, status, &req_complete, - &req_complete_skb); + /* Only match event if command OGF is not for LE */ + if (hdev->sent_cmd && + hci_opcode_ogf(hci_skb_opcode(hdev->sent_cmd)) != 0x08 && + hci_skb_event(hdev->sent_cmd) == event) { + hci_req_cmd_complete(hdev, hci_skb_opcode(hdev->sent_cmd), + status, &req_complete, &req_complete_skb); req_evt = event; } diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index a3cf84873baa..334dc5f436a0 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -103,7 +103,7 @@ static void hci_cmd_sync_add(struct hci_request *req, u16 opcode, u32 plen, if (skb_queue_empty(&req->cmd_q)) bt_cb(skb)->hci.req_flags |= HCI_REQ_START; - bt_cb(skb)->hci.req_event = event; + hci_skb_event(skb) = event; skb_queue_tail(&req->cmd_q, skb); } From 6cd29ec6ae5e3694b779e02da9f3e50ae4398b59 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 22 Dec 2021 12:21:58 -0800 Subject: [PATCH 58/62] Bluetooth: hci_sync: Wait for proper events when connecting LE When using HCI_OP_LE_CREATE_CONN wait for HCI_EV_LE_CONN_COMPLETE before completing it and for HCI_OP_LE_EXT_CREATE_CONN wait for HCI_EV_LE_ENHANCED_CONN_COMPLETE before resuming advertising. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- net/bluetooth/hci_sync.c | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index 334dc5f436a0..8c50a12ba5f5 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -5182,8 +5182,10 @@ int hci_le_ext_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn, plen += sizeof(*p); } - return __hci_cmd_sync_status(hdev, HCI_OP_LE_EXT_CREATE_CONN, - plen, data, HCI_CMD_TIMEOUT); + return __hci_cmd_sync_status_sk(hdev, HCI_OP_LE_EXT_CREATE_CONN, + plen, data, + HCI_EV_LE_ENHANCED_CONN_COMPLETE, + HCI_CMD_TIMEOUT, NULL); } int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn) @@ -5269,8 +5271,9 @@ int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn) cp.min_ce_len = cpu_to_le16(0x0000); cp.max_ce_len = cpu_to_le16(0x0000); - err = __hci_cmd_sync_status(hdev, HCI_OP_LE_CREATE_CONN, - sizeof(cp), &cp, HCI_CMD_TIMEOUT); + err = __hci_cmd_sync_status_sk(hdev, HCI_OP_LE_CREATE_CONN, + sizeof(cp), &cp, HCI_EV_LE_CONN_COMPLETE, + HCI_CMD_TIMEOUT, NULL); done: hci_resume_advertising_sync(hdev); From 4fc9857ab8c6cfe2152df3288c8cf3300b929f1a Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 22 Dec 2021 12:21:59 -0800 Subject: [PATCH 59/62] Bluetooth: hci_sync: Add check simultaneous roles support This attempts to check if the controller can act as both central and peripheral simultaneously and in case it does skip suspending advertising or in case of directed advertising don't fail if scanning. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci_core.h | 6 ++++++ net/bluetooth/hci_sync.c | 24 ++++++++++++------------ net/bluetooth/mgmt.c | 5 +---- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 1ab94960764e..586f69d084a2 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -783,6 +783,12 @@ extern struct mutex hci_cb_list_lock; hci_dev_clear_flag(hdev, HCI_QUALITY_REPORT); \ } while (0) +#define hci_dev_le_state_simultaneous(hdev) \ + (test_bit(HCI_QUIRK_VALID_LE_STATES, &hdev->quirks) && \ + (hdev->le_states[4] & 0x08) && /* Central */ \ + (hdev->le_states[4] & 0x40) && /* Peripheral */ \ + (hdev->le_states[3] & 0x10)) /* Simultaneous */ + /* ----- HCI interface to upper protocols ----- */ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr); int l2cap_disconn_ind(struct hci_conn *hcon); diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index 8c50a12ba5f5..61d8a076a3f3 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -5195,30 +5195,29 @@ int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn) u8 own_addr_type; int err; - /* Disable advertising if we're active. For central role - * connections most controllers will refuse to connect if - * advertising is enabled, and for peripheral role connections we - * anyway have to disable it in order to start directed - * advertising. Any registered advertisements will be - * re-enabled after the connection attempt is finished. - */ - hci_pause_advertising_sync(hdev); - /* If requested to connect as peripheral use directed advertising */ if (conn->role == HCI_ROLE_SLAVE) { - /* If we're active scanning most controllers are unable - * to initiate advertising. Simply reject the attempt. + /* If we're active scanning and the controller doesn't support + * simultaneous roles simply reject the attempt. */ if (hci_dev_test_flag(hdev, HCI_LE_SCAN) && - hdev->le_scan_type == LE_SCAN_ACTIVE) { + hdev->le_scan_type == LE_SCAN_ACTIVE && + !hci_dev_le_state_simultaneous(hdev)) { hci_conn_del(conn); return -EBUSY; } + /* Pause advertising while doing directed advertising. */ + hci_pause_advertising_sync(hdev); + err = hci_le_directed_advertising_sync(hdev, conn); goto done; } + /* Disable advertising if simultaneous roles is not supported. */ + if (!hci_dev_le_state_simultaneous(hdev)) + hci_pause_advertising_sync(hdev); + params = hci_conn_params_lookup(hdev, &conn->dst, conn->dst_type); if (params) { conn->le_conn_min_interval = params->conn_min_interval; @@ -5276,6 +5275,7 @@ int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn) HCI_CMD_TIMEOUT, NULL); done: + /* Re-enable advertising after the connection attempt is finished. */ hci_resume_advertising_sync(hdev); return err; } diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 962bb747d2cd..3326d9459dd3 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -3916,10 +3916,7 @@ static int read_exp_features_info(struct sock *sk, struct hci_dev *hdev, #endif if (hdev) { - if (test_bit(HCI_QUIRK_VALID_LE_STATES, &hdev->quirks) && - (hdev->le_states[4] & 0x08) && /* Central */ - (hdev->le_states[4] & 0x40) && /* Peripheral */ - (hdev->le_states[3] & 0x10)) /* Simultaneous */ + if (hci_dev_le_state_simultaneous(hdev)) flags = BIT(0); else flags = 0; From 76d0685bbac8ae017e5d12eba25fb3c4f0ec77ac Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 22 Dec 2021 12:22:00 -0800 Subject: [PATCH 60/62] Bluetooth: MGMT: Fix LE simultaneous roles UUID if not supported If controller/driver don't support LE simultaneous roles its UUID shall be omitted when responding to MGMT_OP_READ_EXP_FEATURES_INFO. This also rework the support introducing HCI_LE_SIMULTANEOUS_ROLES flag so it can be detected when userspace wants to use or not. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- include/net/bluetooth/hci.h | 1 + net/bluetooth/hci_sync.c | 10 ++-- net/bluetooth/mgmt.c | 114 +++++++++++++++++++++++------------- 3 files changed, 78 insertions(+), 47 deletions(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 7f5f00ff53da..e2b06bb79e2e 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -341,6 +341,7 @@ enum { HCI_FORCE_NO_MITM, HCI_QUALITY_REPORT, HCI_OFFLOAD_CODECS_ENABLED, + HCI_LE_SIMULTANEOUS_ROLES, __HCI_NUM_FLAGS, }; diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index 61d8a076a3f3..0feb68f12545 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -5197,12 +5197,12 @@ int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn) /* If requested to connect as peripheral use directed advertising */ if (conn->role == HCI_ROLE_SLAVE) { - /* If we're active scanning and the controller doesn't support - * simultaneous roles simply reject the attempt. + /* If we're active scanning and simultaneous roles is not + * enabled simply reject the attempt. */ if (hci_dev_test_flag(hdev, HCI_LE_SCAN) && hdev->le_scan_type == LE_SCAN_ACTIVE && - !hci_dev_le_state_simultaneous(hdev)) { + !hci_dev_test_flag(hdev, HCI_LE_SIMULTANEOUS_ROLES)) { hci_conn_del(conn); return -EBUSY; } @@ -5214,8 +5214,8 @@ int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn) goto done; } - /* Disable advertising if simultaneous roles is not supported. */ - if (!hci_dev_le_state_simultaneous(hdev)) + /* Disable advertising if simultaneous roles is not in use. */ + if (!hci_dev_test_flag(hdev, HCI_LE_SIMULTANEOUS_ROLES)) hci_pause_advertising_sync(hdev); params = hci_conn_params_lookup(hdev, &conn->dst, conn->dst_type); diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 3326d9459dd3..6f192efd9da0 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -3882,7 +3882,7 @@ static const u8 offload_codecs_uuid[16] = { }; /* 671b10b5-42c0-4696-9227-eb28d1b049d6 */ -static const u8 simult_central_periph_uuid[16] = { +static const u8 le_simultaneous_roles_uuid[16] = { 0xd6, 0x49, 0xb0, 0xd1, 0x28, 0xeb, 0x27, 0x92, 0x96, 0x46, 0xc0, 0x42, 0xb5, 0x10, 0x1b, 0x67, }; @@ -3915,13 +3915,13 @@ static int read_exp_features_info(struct sock *sk, struct hci_dev *hdev, } #endif - if (hdev) { - if (hci_dev_le_state_simultaneous(hdev)) + if (hdev && hci_dev_le_state_simultaneous(hdev)) { + if (hci_dev_test_flag(hdev, HCI_LE_SIMULTANEOUS_ROLES)) flags = BIT(0); else flags = 0; - memcpy(rp->features[idx].uuid, simult_central_periph_uuid, 16); + memcpy(rp->features[idx].uuid, le_simultaneous_roles_uuid, 16); rp->features[idx].flags = cpu_to_le32(flags); idx++; } @@ -3992,29 +3992,13 @@ static int exp_ll_privacy_feature_changed(bool enabled, struct hci_dev *hdev, } -#ifdef CONFIG_BT_FEATURE_DEBUG -static int exp_debug_feature_changed(bool enabled, struct sock *skip) +static int exp_feature_changed(struct hci_dev *hdev, const u8 *uuid, + bool enabled, struct sock *skip) { struct mgmt_ev_exp_feature_changed ev; memset(&ev, 0, sizeof(ev)); - memcpy(ev.uuid, debug_uuid, 16); - ev.flags = cpu_to_le32(enabled ? BIT(0) : 0); - - return mgmt_limited_event(MGMT_EV_EXP_FEATURE_CHANGED, NULL, - &ev, sizeof(ev), - HCI_MGMT_EXP_FEATURE_EVENTS, skip); -} -#endif - -static int exp_quality_report_feature_changed(bool enabled, - struct hci_dev *hdev, - struct sock *skip) -{ - struct mgmt_ev_exp_feature_changed ev; - - memset(&ev, 0, sizeof(ev)); - memcpy(ev.uuid, quality_report_uuid, 16); + memcpy(ev.uuid, uuid, 16); ev.flags = cpu_to_le32(enabled ? BIT(0) : 0); return mgmt_limited_event(MGMT_EV_EXP_FEATURE_CHANGED, hdev, @@ -4044,7 +4028,7 @@ static int set_zero_key_func(struct sock *sk, struct hci_dev *hdev, bt_dbg_set(false); if (changed) - exp_debug_feature_changed(false, sk); + exp_feature_changed(NULL, ZERO_KEY, false, sk); } #endif @@ -4054,7 +4038,8 @@ static int set_zero_key_func(struct sock *sk, struct hci_dev *hdev, changed = hci_dev_test_and_clear_flag(hdev, HCI_ENABLE_LL_PRIVACY); if (changed) - exp_ll_privacy_feature_changed(false, hdev, sk); + exp_feature_changed(hdev, rpa_resolution_uuid, false, + sk); } hci_sock_set_flag(sk, HCI_MGMT_EXP_FEATURE_EVENTS); @@ -4105,7 +4090,7 @@ static int set_debug_func(struct sock *sk, struct hci_dev *hdev, &rp, sizeof(rp)); if (changed) - exp_debug_feature_changed(val, sk); + exp_feature_changed(hdev, debug_uuid, val, sk); return err; } @@ -4243,27 +4228,13 @@ static int set_quality_report_func(struct sock *sk, struct hci_dev *hdev, &rp, sizeof(rp)); if (changed) - exp_quality_report_feature_changed(val, hdev, sk); + exp_feature_changed(hdev, quality_report_uuid, val, sk); unlock_quality_report: hci_req_sync_unlock(hdev); return err; } -static int exp_offload_codec_feature_changed(bool enabled, struct hci_dev *hdev, - struct sock *skip) -{ - struct mgmt_ev_exp_feature_changed ev; - - memset(&ev, 0, sizeof(ev)); - memcpy(ev.uuid, offload_codecs_uuid, 16); - ev.flags = cpu_to_le32(enabled ? BIT(0) : 0); - - return mgmt_limited_event(MGMT_EV_EXP_FEATURE_CHANGED, hdev, - &ev, sizeof(ev), - HCI_MGMT_EXP_FEATURE_EVENTS, skip); -} - static int set_offload_codec_func(struct sock *sk, struct hci_dev *hdev, struct mgmt_cp_set_exp_feature *cp, u16 data_len) @@ -4317,7 +4288,65 @@ static int set_offload_codec_func(struct sock *sk, struct hci_dev *hdev, &rp, sizeof(rp)); if (changed) - exp_offload_codec_feature_changed(val, hdev, sk); + exp_feature_changed(hdev, offload_codecs_uuid, val, sk); + + return err; +} + +static int set_le_simultaneous_roles_func(struct sock *sk, struct hci_dev *hdev, + struct mgmt_cp_set_exp_feature *cp, + u16 data_len) +{ + bool val, changed; + int err; + struct mgmt_rp_set_exp_feature rp; + + /* Command requires to use a valid controller index */ + if (!hdev) + return mgmt_cmd_status(sk, MGMT_INDEX_NONE, + MGMT_OP_SET_EXP_FEATURE, + MGMT_STATUS_INVALID_INDEX); + + /* Parameters are limited to a single octet */ + if (data_len != MGMT_SET_EXP_FEATURE_SIZE + 1) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_EXP_FEATURE, + MGMT_STATUS_INVALID_PARAMS); + + /* Only boolean on/off is supported */ + if (cp->param[0] != 0x00 && cp->param[0] != 0x01) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_EXP_FEATURE, + MGMT_STATUS_INVALID_PARAMS); + + val = !!cp->param[0]; + changed = (val != hci_dev_test_flag(hdev, HCI_LE_SIMULTANEOUS_ROLES)); + + if (!hci_dev_le_state_simultaneous(hdev)) { + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_EXP_FEATURE, + MGMT_STATUS_NOT_SUPPORTED); + } + + if (changed) { + if (val) + hci_dev_set_flag(hdev, HCI_LE_SIMULTANEOUS_ROLES); + else + hci_dev_clear_flag(hdev, HCI_LE_SIMULTANEOUS_ROLES); + } + + bt_dev_info(hdev, "LE simultanous roles enable %d changed %d", + val, changed); + + memcpy(rp.uuid, le_simultaneous_roles_uuid, 16); + rp.flags = cpu_to_le32(val ? BIT(0) : 0); + hci_sock_set_flag(sk, HCI_MGMT_EXP_FEATURE_EVENTS); + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_SET_EXP_FEATURE, 0, + &rp, sizeof(rp)); + + if (changed) + exp_feature_changed(hdev, le_simultaneous_roles_uuid, val, sk); return err; } @@ -4334,6 +4363,7 @@ static const struct mgmt_exp_feature { EXP_FEAT(rpa_resolution_uuid, set_rpa_resolution_func), EXP_FEAT(quality_report_uuid, set_quality_report_func), EXP_FEAT(offload_codecs_uuid, set_offload_codec_func), + EXP_FEAT(le_simultaneous_roles_uuid, set_le_simultaneous_roles_func), /* end with a null feature */ EXP_FEAT(NULL, NULL) From cfb4c313be670fd4bd09650216620fa4514cdb93 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 22 Dec 2021 12:22:01 -0800 Subject: [PATCH 61/62] Bluetooth: vhci: Set HCI_QUIRK_VALID_LE_STATES This set HCI_QUIRK_VALID_LE_STATES quirk which is required for the likes of experimental LE simultaneous roles. Signed-off-by: Luiz Augusto von Dentz Signed-off-by: Marcel Holtmann --- drivers/bluetooth/hci_vhci.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c index 49ac884d996e..c443c3b0a4da 100644 --- a/drivers/bluetooth/hci_vhci.c +++ b/drivers/bluetooth/hci_vhci.c @@ -331,6 +331,8 @@ static int __vhci_create_device(struct vhci_data *data, __u8 opcode) if (opcode & 0x80) set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks); + set_bit(HCI_QUIRK_VALID_LE_STATES, &hdev->quirks); + if (hci_register_dev(hdev) < 0) { BT_ERR("Can't register HCI device"); hci_free_dev(hdev); From 5d1dd2e5a681b126a04192e37abb2011c2fb719c Mon Sep 17 00:00:00 2001 From: Colin Ian King Date: Thu, 23 Dec 2021 09:47:17 +0000 Subject: [PATCH 62/62] Bluetooth: MGMT: Fix spelling mistake "simultanous" -> "simultaneous" There is a spelling mistake in a bt_dev_info message. Fix it. Signed-off-by: Colin Ian King Signed-off-by: Luiz Augusto von Dentz --- net/bluetooth/mgmt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 6f192efd9da0..37087cf7dc5a 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -4335,7 +4335,7 @@ static int set_le_simultaneous_roles_func(struct sock *sk, struct hci_dev *hdev, hci_dev_clear_flag(hdev, HCI_LE_SIMULTANEOUS_ROLES); } - bt_dev_info(hdev, "LE simultanous roles enable %d changed %d", + bt_dev_info(hdev, "LE simultaneous roles enable %d changed %d", val, changed); memcpy(rp.uuid, le_simultaneous_roles_uuid, 16);