From e46957aa53ebeb083ac5160d9c22078c68fcef8a Mon Sep 17 00:00:00 2001 From: Sudha Bheemanna Date: Wed, 24 Aug 2016 12:17:16 +0530 Subject: [PATCH] Bluetooth: Add Advertising Packet Configuration This patch provides new MGMT commands to configure the advertising data and scan response data packets for LE peripheral devices. Change-Id: I914d13795f4fb58e5f2e1cadb55086f4bcbc82df Signed-off-by: Sudha Bheemanna Signed-off-by: DoHyun Pyun Signed-off-by: Amit Purwar [jhoon20.kim: adjust some codes to apply it in 5.4 kernel] Signed-off-by: Junghoon Kim --- include/net/bluetooth/hci.h | 2 + include/net/bluetooth/hci_core.h | 5 + include/net/bluetooth/mgmt_tizen.h | 23 ++++ net/bluetooth/hci_core.c | 4 + net/bluetooth/hci_request.c | 39 ++++++- net/bluetooth/mgmt.c | 224 +++++++++++++++++++++++++++++++++++++ 6 files changed, 296 insertions(+), 1 deletion(-) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 9ce46cb..4707591 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -600,6 +600,8 @@ enum { #define EIR_SSP_HASH_C192 0x0E /* Simple Pairing Hash C-192 */ #define EIR_SSP_RAND_R192 0x0F /* Simple Pairing Randomizer R-192 */ #define EIR_DEVICE_ID 0x10 /* device ID */ +#define EIR_SOLICIT_UUID16 0x14 /* 16-bit Solicitation UUID */ +#define EIR_MANUFACTURER_DATA 0XFF /* Manufacturer Specific Data*/ #define EIR_APPEARANCE 0x19 /* Device appearance */ #define EIR_LE_BDADDR 0x1B /* LE Bluetooth device address */ #define EIR_LE_ROLE 0x1C /* LE role */ diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 89630a1..3383825 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -596,6 +596,11 @@ struct hci_dev { bool aosp_capable; #endif +#ifdef TIZEN_BT + __u8 adv_filter_policy; + __u8 adv_type; +#endif + int (*open)(struct hci_dev *hdev); int (*close)(struct hci_dev *hdev); int (*flush)(struct hci_dev *hdev); diff --git a/include/net/bluetooth/mgmt_tizen.h b/include/net/bluetooth/mgmt_tizen.h index 6bad3c9..04bd255 100644 --- a/include/net/bluetooth/mgmt_tizen.h +++ b/include/net/bluetooth/mgmt_tizen.h @@ -23,4 +23,27 @@ #define TIZEN_OP_CODE_BASE 0xff00 #define TIZEN_EV_BASE 0xff00 +#define MGMT_OP_SET_ADVERTISING_PARAMS (TIZEN_OP_CODE_BASE + 0x01) +struct mgmt_cp_set_advertising_params { + __le16 interval_min; + __le16 interval_max; + __u8 filter_policy; + __u8 type; +} __packed; +#define MGMT_SET_ADVERTISING_PARAMS_SIZE 6 + +#define MGMT_OP_SET_ADVERTISING_DATA (TIZEN_OP_CODE_BASE + 0x02) +struct mgmt_cp_set_advertising_data { + __u8 data[HCI_MAX_AD_LENGTH]; +} __packed; +#define MGMT_SET_ADVERTISING_DATA_SIZE HCI_MAX_AD_LENGTH +#define MGMT_SET_ADV_MIN_APP_DATA_SIZE 1 + +#define MGMT_OP_SET_SCAN_RSP_DATA (TIZEN_OP_CODE_BASE + 0x03) +struct mgmt_cp_set_scan_rsp_data { + __u8 data[HCI_MAX_AD_LENGTH]; +} __packed; +#define MGMT_SET_SCAN_RSP_DATA_SIZE HCI_MAX_AD_LENGTH +#define MGMT_SET_SCAN_RSP_MIN_APP_DATA_SIZE 1 + #endif /* __MGMT_TIZEN_H */ diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index 3966962..1254fe8 100644 --- a/net/bluetooth/hci_core.c +++ b/net/bluetooth/hci_core.c @@ -3796,6 +3796,10 @@ struct hci_dev *hci_alloc_dev_priv(int sizeof_priv) hdev->le_adv_channel_map = 0x07; hdev->le_adv_min_interval = 0x0800; hdev->le_adv_max_interval = 0x0800; +#ifdef TIZEN_BT + hdev->adv_filter_policy = 0x00; + hdev->adv_type = 0x00; +#endif hdev->le_scan_interval = 0x0060; hdev->le_scan_window = 0x0030; hdev->le_scan_int_suspend = 0x0400; diff --git a/net/bluetooth/hci_request.c b/net/bluetooth/hci_request.c index c2db60a..f119747 100644 --- a/net/bluetooth/hci_request.c +++ b/net/bluetooth/hci_request.c @@ -1620,6 +1620,12 @@ void __hci_req_enable_advertising(struct hci_request *req) cp.min_interval = cpu_to_le16(adv_min_interval); cp.max_interval = cpu_to_le16(adv_max_interval); + +#ifdef TIZEN_BT + cp.filter_policy = hdev->adv_filter_policy; + cp.type = hdev->adv_type; +#endif + cp.own_address_type = own_addr_type; cp.channel_map = hdev->le_adv_channel_map; @@ -1728,6 +1734,13 @@ void __hci_req_update_scan_rsp_data(struct hci_request *req, u8 instance) pdu.data); else len = create_default_scan_rsp_data(hdev, pdu.data); +#ifdef TIZEN_BT + /* Advertising scan response data is handled in bluez. + * This value will be updated only when application request the update + * using adapter_set_scan_rsp_data() + */ + return; +#else if (hdev->scan_rsp_data_len == len && !memcmp(pdu.data, hdev->scan_rsp_data, len)) @@ -1743,6 +1756,7 @@ void __hci_req_update_scan_rsp_data(struct hci_request *req, u8 instance) hci_req_add(req, HCI_OP_LE_SET_EXT_SCAN_RSP_DATA, sizeof(pdu.cp) + len, &pdu.cp); +#endif } else { struct hci_cp_le_set_scan_rsp_data cp; @@ -1753,7 +1767,13 @@ void __hci_req_update_scan_rsp_data(struct hci_request *req, u8 instance) cp.data); else len = create_default_scan_rsp_data(hdev, cp.data); - +#ifdef TIZEN_BT + /* Advertising scan response data is handled in bluez. + * This value will be updated only when application request the update + * using adapter_set_scan_rsp_data() + */ + return; +#else if (hdev->scan_rsp_data_len == len && !memcmp(cp.data, hdev->scan_rsp_data, len)) return; @@ -1764,6 +1784,7 @@ void __hci_req_update_scan_rsp_data(struct hci_request *req, u8 instance) cp.length = len; hci_req_add(req, HCI_OP_LE_SET_SCAN_RSP_DATA, sizeof(cp), &cp); +#endif } } @@ -1874,6 +1895,13 @@ void __hci_req_update_adv_data(struct hci_request *req, u8 instance) len = create_instance_adv_data(hdev, instance, pdu.data); +#ifdef TIZEN_BT + /* Bluez will handle the advertising data including the flag and tx + * power. This value will be updated only when application request the + * update using adapter_set_advertising_data(). + */ + return; +#else /* There's nothing to do if the data hasn't changed */ if (hdev->adv_data_len == len && memcmp(pdu.data, hdev->adv_data, len) == 0) @@ -1889,6 +1917,7 @@ void __hci_req_update_adv_data(struct hci_request *req, u8 instance) hci_req_add(req, HCI_OP_LE_SET_EXT_ADV_DATA, sizeof(pdu.cp) + len, &pdu.cp); +#endif } else { struct hci_cp_le_set_adv_data cp; @@ -1896,6 +1925,13 @@ void __hci_req_update_adv_data(struct hci_request *req, u8 instance) len = create_instance_adv_data(hdev, instance, cp.data); +#ifdef TIZEN_BT + /* Bluez will handle the advertising data including the flag and tx + * power. This value will be updated only when application request the + * update using adapter_set_advertising_data(). + */ + return; +#else /* There's nothing to do if the data hasn't changed */ if (hdev->adv_data_len == len && memcmp(cp.data, hdev->adv_data, len) == 0) @@ -1907,6 +1943,7 @@ void __hci_req_update_adv_data(struct hci_request *req, u8 instance) cp.length = len; hci_req_add(req, HCI_OP_LE_SET_ADV_DATA, sizeof(cp), &cp); +#endif } } diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 5716a65..bbe262d 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -6206,6 +6206,225 @@ static int load_irks(struct sock *sk, struct hci_dev *hdev, void *cp_data, return err; } +#ifdef TIZEN_BT +static int set_advertising_params(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_set_advertising_params *cp = data; + __u16 min_interval; + __u16 max_interval; + int err; + + BT_DBG("%s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_ADVERTISING_PARAMS, + MGMT_STATUS_NOT_SUPPORTED); + + if (hci_dev_test_flag(hdev, HCI_ADVERTISING)) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_ADVERTISING_PARAMS, + MGMT_STATUS_BUSY); + + min_interval = __le16_to_cpu(cp->interval_min); + max_interval = __le16_to_cpu(cp->interval_max); + + if (min_interval > max_interval || + min_interval < 0x0020 || max_interval > 0x4000) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_ADVERTISING_PARAMS, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + hdev->le_adv_min_interval = min_interval; + hdev->le_adv_max_interval = max_interval; + hdev->adv_filter_policy = cp->filter_policy; + hdev->adv_type = cp->type; + + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_SET_ADVERTISING_PARAMS, 0, NULL, 0); + + hci_dev_unlock(hdev); + + return err; +} + +static void set_advertising_data_complete(struct hci_dev *hdev, + u8 status, u16 opcode) +{ + struct mgmt_cp_set_advertising_data *cp; + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_ADVERTISING_DATA, hdev); + if (!cmd) + goto unlock; + + cp = cmd->param; + + if (status) + mgmt_cmd_status(cmd->sk, hdev->id, + MGMT_OP_SET_ADVERTISING_DATA, + mgmt_status(status)); + else + mgmt_cmd_complete(cmd->sk, hdev->id, + MGMT_OP_SET_ADVERTISING_DATA, 0, + cp, sizeof(*cp)); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_advertising_data(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_pending_cmd *cmd; + struct hci_request req; + struct mgmt_cp_set_advertising_data *cp = data; + struct hci_cp_le_set_adv_data adv; + int err; + + BT_DBG("%s", hdev->name); + + if (!lmp_le_capable(hdev)) { + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_ADVERTISING_DATA, + MGMT_STATUS_NOT_SUPPORTED); + } + + hci_dev_lock(hdev); + + if (pending_find(MGMT_OP_SET_ADVERTISING_DATA, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_ADVERTISING_DATA, + MGMT_STATUS_BUSY); + goto unlocked; + } + + if (len > HCI_MAX_AD_LENGTH) { + err = mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_ADVERTISING_DATA, + MGMT_STATUS_INVALID_PARAMS); + goto unlocked; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_ADVERTISING_DATA, + hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlocked; + } + + hci_req_init(&req, hdev); + + memset(&adv, 0, sizeof(adv)); + memcpy(adv.data, cp->data, len); + adv.length = len; + + hci_req_add(&req, HCI_OP_LE_SET_ADV_DATA, sizeof(adv), &adv); + + err = hci_req_run(&req, set_advertising_data_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlocked: + hci_dev_unlock(hdev); + + return err; +} + +static void set_scan_rsp_data_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_cp_set_scan_rsp_data *cp; + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_SCAN_RSP_DATA, hdev); + if (!cmd) + goto unlock; + + cp = cmd->param; + + if (status) + mgmt_cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_SCAN_RSP_DATA, + mgmt_status(status)); + else + mgmt_cmd_complete(cmd->sk, hdev->id, + MGMT_OP_SET_SCAN_RSP_DATA, 0, + cp, sizeof(*cp)); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_scan_rsp_data(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_pending_cmd *cmd; + struct hci_request req; + struct mgmt_cp_set_scan_rsp_data *cp = data; + struct hci_cp_le_set_scan_rsp_data rsp; + int err; + + BT_DBG("%s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_SCAN_RSP_DATA, + MGMT_STATUS_NOT_SUPPORTED); + + hci_dev_lock(hdev); + + if (pending_find(MGMT_OP_SET_SCAN_RSP_DATA, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_RSP_DATA, + MGMT_STATUS_BUSY); + goto unlocked; + } + + if (len > HCI_MAX_AD_LENGTH) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_RSP_DATA, + MGMT_STATUS_INVALID_PARAMS); + goto unlocked; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_SCAN_RSP_DATA, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlocked; + } + + hci_req_init(&req, hdev); + + memset(&rsp, 0, sizeof(rsp)); + memcpy(rsp.data, cp->data, len); + rsp.length = len; + + hci_req_add(&req, HCI_OP_LE_SET_SCAN_RSP_DATA, sizeof(rsp), &rsp); + + err = hci_req_run(&req, set_scan_rsp_data_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlocked: + hci_dev_unlock(hdev); + + return err; +} +#endif /* TIZEN_BT */ + static bool ltk_is_valid(struct mgmt_ltk_info *key) { if (key->initiator != 0x00 && key->initiator != 0x01) @@ -8502,6 +8721,11 @@ static const struct hci_mgmt_handler mgmt_handlers[] = { #ifdef TIZEN_BT static const struct hci_mgmt_handler tizen_mgmt_handlers[] = { { NULL }, /* 0x0000 (no command) */ + { set_advertising_params, MGMT_SET_ADVERTISING_PARAMS_SIZE }, + { set_advertising_data, MGMT_SET_ADV_MIN_APP_DATA_SIZE, + HCI_MGMT_VAR_LEN }, + { set_scan_rsp_data, MGMT_SET_SCAN_RSP_MIN_APP_DATA_SIZE, + HCI_MGMT_VAR_LEN }, }; #endif -- 2.7.4