From 7bfa0ef47bd532ffec9e24da97245ce56dfffcce Mon Sep 17 00:00:00 2001 From: Carles Cufi Date: Wed, 23 Aug 2017 14:57:16 +0200 Subject: [PATCH] Bluetooth: controller: Implement Read RSSI command When the CONFIG_BT_CTLR_CONN_RSSI option is set, the connection RSSI is available in the controller, and can be reported to the Host via the Read RSSI command. Implement the command, which is required for qualification. Signed-off-by: Carles Cufi --- include/bluetooth/hci.h | 10 +++++ subsys/bluetooth/controller/hci/hci.c | 48 ++++++++++++++++++++++++ subsys/bluetooth/controller/include/ll.h | 1 + subsys/bluetooth/controller/ll_sw/ctrl.c | 16 ++++++++ 4 files changed, 75 insertions(+) diff --git a/include/bluetooth/hci.h b/include/bluetooth/hci.h index ab51ca2e387..9c9cfe5eccd 100644 --- a/include/bluetooth/hci.h +++ b/include/bluetooth/hci.h @@ -636,6 +636,16 @@ struct bt_hci_rp_read_bd_addr { bt_addr_t bdaddr; } __packed; +#define BT_HCI_OP_READ_RSSI BT_OP(BT_OGF_STATUS, 0x0005) +struct bt_hci_cp_read_rssi { + u16_t handle; +} __packed; +struct bt_hci_rp_read_rssi { + u8_t status; + u16_t handle; + s8_t rssi; +} __packed; + #define BT_HCI_OP_READ_ENCRYPTION_KEY_SIZE BT_OP(BT_OGF_STATUS, 0x0008) struct bt_hci_cp_read_encryption_key_size { u16_t handle; diff --git a/subsys/bluetooth/controller/hci/hci.c b/subsys/bluetooth/controller/hci/hci.c index e9f96026d47..adb525b7713 100644 --- a/subsys/bluetooth/controller/hci/hci.c +++ b/subsys/bluetooth/controller/hci/hci.c @@ -436,6 +436,10 @@ static void read_supported_commands(struct net_buf *buf, struct net_buf **evt) rp->commands[14] |= BIT(3) | BIT(5); /* Read BD ADDR. */ rp->commands[15] |= BIT(1); +#if defined(CONFIG_BT_CTLR_CONN_RSSI) + /* Read RSSI. */ + rp->commands[15] |= BIT(5); +#endif /* CONFIG_BT_CTLR_CONN_RSSI */ /* Set Event Mask Page 2 */ rp->commands[22] |= BIT(2); /* LE Set Event Mask, LE Read Buffer Size, LE Read Local Supp Feats, @@ -561,6 +565,44 @@ static int info_cmd_handle(u16_t ocf, struct net_buf *cmd, return 0; } +#if defined(CONFIG_BT_CTLR_CONN_RSSI) +static void read_rssi(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_cp_read_rssi *cmd = (void *)buf->data; + struct bt_hci_rp_read_rssi *rp; + u32_t status; + u16_t handle; + + handle = sys_le16_to_cpu(cmd->handle); + + rp = cmd_complete(evt, sizeof(*rp)); + + status = ll_rssi_get(handle, &rp->rssi); + + rp->status = (!status) ? 0x00 : BT_HCI_ERR_UNKNOWN_CONN_ID; + rp->handle = sys_cpu_to_le16(handle); + /* The Link Layer currently returns RSSI as an absolute value */ + rp->rssi = (!status) ? -rp->rssi : 127; +} +#endif /* CONFIG_BT_CTLR_CONN_RSSI */ + +static int status_cmd_handle(u16_t ocf, struct net_buf *cmd, + struct net_buf **evt) +{ + switch (ocf) { +#if defined(CONFIG_BT_CTLR_CONN_RSSI) + case BT_OCF(BT_HCI_OP_READ_RSSI): + read_rssi(cmd, evt); + break; +#endif /* CONFIG_BT_CTLR_CONN_RSSI */ + + default: + return -EINVAL; + } + + return 0; +} + static void le_set_event_mask(struct net_buf *buf, struct net_buf **evt) { struct bt_hci_cp_set_event_mask *cmd = (void *)buf->data; @@ -1551,6 +1593,9 @@ struct net_buf *hci_cmd_handle(struct net_buf *cmd) case BT_OGF_INFO: err = info_cmd_handle(ocf, cmd, &evt); break; + case BT_OGF_STATUS: + err = status_cmd_handle(ocf, cmd, &evt); + break; case BT_OGF_LE: err = controller_cmd_handle(ocf, cmd, &evt); break; @@ -1727,6 +1772,7 @@ static void le_advertising_report(struct pdu_data *pdu_data, u8_t *b, data_len = 0; } + /* The Link Layer currently returns RSSI as an absolute value */ rssi = -b[offsetof(struct radio_pdu_node_rx, pdu_data) + offsetof(struct pdu_adv, payload) + adv->len]; @@ -1814,6 +1860,7 @@ static void le_adv_ext_report(struct pdu_data *pdu_data, u8_t *b, struct pdu_adv *adv = (struct pdu_adv *)pdu_data; s8_t rssi; + /* The Link Layer currently returns RSSI as an absolute value */ rssi = -b[offsetof(struct radio_pdu_node_rx, pdu_data) + offsetof(struct pdu_adv, payload) + adv->len]; @@ -1899,6 +1946,7 @@ static void le_scan_req_received(struct pdu_data *pdu_data, u8_t *b, addr.type = adv->tx_addr; memcpy(&addr.a.val[0], &adv->payload.scan_req.scan_addr[0], sizeof(bt_addr_t)); + /* The Link Layer currently returns RSSI as an absolute value */ rssi = -b[offsetof(struct radio_pdu_node_rx, pdu_data) + offsetof(struct pdu_adv, payload) + adv->len]; diff --git a/subsys/bluetooth/controller/include/ll.h b/subsys/bluetooth/controller/include/ll.h index bc857c6c437..6b99de7b725 100644 --- a/subsys/bluetooth/controller/include/ll.h +++ b/subsys/bluetooth/controller/include/ll.h @@ -72,6 +72,7 @@ u32_t ll_feature_req_send(u16_t handle); u32_t ll_version_ind_send(u16_t handle); u32_t ll_terminate_ind_send(u16_t handle, u8_t reason); void ll_timeslice_ticker_id_get(u8_t * const instance_index, u8_t * const user_id); +u32_t ll_rssi_get(u16_t handle, u8_t *rssi); #if defined(CONFIG_BT_CTLR_LE_PING) u32_t ll_apto_get(u16_t handle, u16_t *apto); diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.c b/subsys/bluetooth/controller/ll_sw/ctrl.c index a981667114f..aae1ca24fb8 100644 --- a/subsys/bluetooth/controller/ll_sw/ctrl.c +++ b/subsys/bluetooth/controller/ll_sw/ctrl.c @@ -9743,6 +9743,22 @@ u32_t ll_terminate_ind_send(u16_t handle, u8_t reason) return 0; } +#if defined(CONFIG_BT_CTLR_CONN_RSSI) +u32_t ll_rssi_get(u16_t handle, u8_t *rssi) +{ + struct connection *conn; + + conn = connection_get(handle); + if (!conn) { + return 1; + } + + *rssi = conn->rssi_latest; + + return 0; +} +#endif /* CONFIG_BT_CTLR_CONN_RSSI */ + #if defined(CONFIG_BT_CTLR_LE_PING) u32_t ll_apto_get(u16_t handle, u16_t *apto) {