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) {