From 404288a84da19da2ab04829705bfc72179526a56 Mon Sep 17 00:00:00 2001 From: Sudha Bheemanna Date: Thu, 25 Aug 2016 12:13:09 +0530 Subject: [PATCH] Bluetooth: Set Manufacturer data feature Added new MGMT command to set the manufacturer data in the BR/EDR packet. Change-Id: Ie08062f4cad0c676deab94fd95fdc1a8c5602135 Signed-off-by: Sudha Bheemanna Signed-off-by: Amit Purwar Signed-off-by: Wootak Jung --- include/net/bluetooth/hci_core.h | 6 ++ include/net/bluetooth/mgmt_tizen.h | 8 +++ net/bluetooth/mgmt.c | 122 +++++++++++++++++++++++++++++++++++++ 3 files changed, 136 insertions(+) diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 5f560e1..0c77c07 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -344,6 +344,10 @@ struct amp_assoc { #define HCI_MAX_PAGES 3 +#ifdef TIZEN_BT +#define HCI_MAX_EIR_MANUFACTURER_DATA_LENGTH 100 +#endif + struct hci_dev { struct list_head list; struct mutex lock; @@ -654,6 +658,8 @@ struct hci_dev { #ifdef TIZEN_BT __u8 adv_filter_policy; __u8 adv_type; + __u8 manufacturer_len; + __u8 manufacturer_data[HCI_MAX_EIR_MANUFACTURER_DATA_LENGTH]; #endif int (*open)(struct hci_dev *hdev); diff --git a/include/net/bluetooth/mgmt_tizen.h b/include/net/bluetooth/mgmt_tizen.h index c912a69..4f24c42 100644 --- a/include/net/bluetooth/mgmt_tizen.h +++ b/include/net/bluetooth/mgmt_tizen.h @@ -135,6 +135,14 @@ struct mgmt_cp_le_conn_update { } __packed; /* Add LE connection parameter update procedure */ +/* For Set Manufacturer Data */ +#define MGMT_OP_SET_MANUFACTURER_DATA (TIZEN_OP_CODE_BASE + 0x0e) +struct mgmt_cp_set_manufacturer_data { + __u8 data[100]; +} __packed; +#define MGMT_SET_MANUFACTURER_DATA_SIZE 100 +/* Set Manufacturer Data */ + /* EVENTS */ /* For device name update changes */ diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index 07e7c2c..39cd126 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -8643,6 +8643,127 @@ static int le_conn_update(struct sock *sk, struct hci_dev *hdev, void *data, return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_LE_CONN_UPDATE, 0, NULL, 0); } + +static void set_manufacturer_data_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_cp_set_manufacturer_data *cp; + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_MANUFACTURER_DATA, hdev); + if (!cmd) + goto unlock; + + cp = cmd->param; + + if (status) + mgmt_cmd_status(cmd->sk, hdev->id, + MGMT_OP_SET_MANUFACTURER_DATA, + mgmt_status(status)); + else + mgmt_cmd_complete(cmd->sk, hdev->id, + MGMT_OP_SET_MANUFACTURER_DATA, 0, + cp, sizeof(*cp)); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_manufacturer_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_manufacturer_data *cp = data; + u8 old_data[HCI_MAX_EIR_LENGTH] = {0, }; + u8 old_len; + int err; + + BT_DBG("%s", hdev->name); + + if (!lmp_bredr_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_MANUFACTURER_DATA, + MGMT_STATUS_NOT_SUPPORTED); + + if (cp->data[0] == 0 || + cp->data[0] - 1 > sizeof(hdev->manufacturer_data)) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_MANUFACTURER_DATA, + MGMT_STATUS_INVALID_PARAMS); + + if (cp->data[1] != 0xFF) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_MANUFACTURER_DATA, + MGMT_STATUS_NOT_SUPPORTED); + + hci_dev_lock(hdev); + + if (pending_find(MGMT_OP_SET_MANUFACTURER_DATA, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_MANUFACTURER_DATA, + MGMT_STATUS_BUSY); + goto unlocked; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_MANUFACTURER_DATA, hdev, data, + len); + if (!cmd) { + err = -ENOMEM; + goto unlocked; + } + + hci_req_init(&req, hdev); + + /* if new data is same as previous data then return command + * complete event + */ + if (hdev->manufacturer_len == cp->data[0] - 1 && + !memcmp(hdev->manufacturer_data, cp->data + 2, cp->data[0] - 1)) { + mgmt_pending_remove(cmd); + mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_MANUFACTURER_DATA, + 0, cp, sizeof(*cp)); + err = 0; + goto unlocked; + } + + old_len = hdev->manufacturer_len; + if (old_len > 0) + memcpy(old_data, hdev->manufacturer_data, old_len); + + hdev->manufacturer_len = cp->data[0] - 1; + if (hdev->manufacturer_len > 0) + memcpy(hdev->manufacturer_data, cp->data + 2, + hdev->manufacturer_len); + + hci_update_eir_sync(hdev); + + err = hci_req_run(&req, set_manufacturer_data_complete); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } + +unlocked: + hci_dev_unlock(hdev); + + return err; + +failed: + memset(hdev->manufacturer_data, 0x00, sizeof(hdev->manufacturer_data)); + hdev->manufacturer_len = old_len; + if (hdev->manufacturer_len > 0) + memcpy(hdev->manufacturer_data, old_data, + hdev->manufacturer_len); + hci_dev_unlock(hdev); + return err; +} #endif /* TIZEN_BT */ static bool ltk_is_valid(struct mgmt_ltk_info *key) @@ -10932,6 +11053,7 @@ static const struct hci_mgmt_handler tizen_mgmt_handlers[] = { { stop_le_discovery, MGMT_STOP_LE_DISCOVERY_SIZE }, { disable_le_auto_connect, MGMT_DISABLE_LE_AUTO_CONNECT_SIZE }, { le_conn_update, MGMT_LE_CONN_UPDATE_SIZE }, + { set_manufacturer_data, MGMT_SET_MANUFACTURER_DATA_SIZE }, }; #endif -- 2.7.4