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 <carles.cufi@nordicsemi.no>

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;

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];
 

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

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