Bluetooth: controller: Add LE Read Chan Map command

Implement the LE Read Channel Map HCI command, along with making the
reading of the multi-byte channel map value from the connection pointer
thread-safe in case the ISR triggers while we are reading the value.

Signed-off-by: Carles Cufi <[email protected]>

Author: carlescufi

include/bluetooth/hci.h

@@ -822,7 +822,7 @@ struct bt_hci_cp_le_set_host_chan_classif {
 struct bt_hci_cp_le_read_chan_map {
 	u16_t handle;
 } __packed;
-struct bt_hci_rp_le_read_ch_map {
+struct bt_hci_rp_le_read_chan_map {
 	u8_t  status;
 	u16_t handle;
 	u8_t  ch_map[5];

subsys/bluetooth/controller/hci/hci.c

@@ -506,8 +506,8 @@ static void read_supported_commands(struct net_buf *buf, struct net_buf **evt)
 #if defined(CONFIG_BT_CONN)
 	/* Disconnect. */
 	rp->commands[0] |= BIT(5);
-	/* LE Connection Update, LE Read Remote Features */
-	rp->commands[27] |= BIT(2) | BIT(5);
+	/* LE Connection Update, LE Read Channel Map, LE Read Remote Features */
+	rp->commands[27] |= BIT(2) | BIT(4) | BIT(5);
 	/* LE Remote Conn Param Req and Neg Reply */
 	rp->commands[33] |= BIT(4) | BIT(5);
 #if defined(CONFIG_BT_CTLR_LE_PING)
@@ -1029,6 +1029,21 @@ static void le_read_remote_features(struct net_buf *buf, struct net_buf **evt)
 
 	*evt = cmd_status((!status) ? 0x00 : BT_HCI_ERR_CMD_DISALLOWED);
 }
+static void le_read_chan_map(struct net_buf *buf, struct net_buf **evt)
+{
+	struct bt_hci_cp_le_read_chan_map *cmd = (void *)buf->data;
+	struct bt_hci_rp_le_read_chan_map *rp;
+	u32_t status;
+	u16_t handle;
+
+	handle = sys_le16_to_cpu(cmd->handle);
+
+	rp = cmd_complete(evt, sizeof(*rp));
+	status = ll_chm_get(handle, rp->ch_map);
+
+	rp->status = (!status) ?  0x00 : BT_HCI_ERR_UNKNOWN_CONN_ID;
+	rp->handle = sys_le16_to_cpu(handle);
+}
 
 static void le_conn_update(struct net_buf *buf, struct net_buf **evt)
 {
@@ -1441,6 +1456,10 @@ static int controller_cmd_handle(u16_t  ocf, struct net_buf *cmd,
 #endif /* CONFIG_BT_CTLR_LE_ENC */
 #endif /* CONFIG_BT_PERIPHERAL */
 
+	case BT_OCF(BT_HCI_OP_LE_READ_CHAN_MAP):
+		le_read_chan_map(cmd, evt);
+		break;
+
 	case BT_OCF(BT_HCI_OP_LE_READ_REMOTE_FEATURES):
 		le_read_remote_features(cmd, evt);
 		break;

subsys/bluetooth/controller/ll_sw/ctrl.c

@@ -6317,6 +6317,7 @@ static inline void event_ch_map_prep(struct connection *conn,
 		conn->data_chan_count =
 			util_ones_count_get(&conn->data_chan_map[0],
 					    sizeof(conn->data_chan_map));
+		conn->chm_update = 1;
 	}
 
 }
@@ -9577,8 +9578,13 @@ u32_t ll_chm_get(u16_t handle, u8_t *chm)
 		return 1;
 	}
 
-	/** @todo make reading context-safe */
-	memcpy(chm, conn->data_chan_map, sizeof(conn->data_chan_map));
+	/* Iterate until we are sure the ISR did not modify the value while
+	 * we were reading it from memory.
+	 */
+	do {
+		conn->chm_update = 0;
+		memcpy(chm, conn->data_chan_map, sizeof(conn->data_chan_map));
+	} while (conn->chm_update);
 
 	return 0;
 }

subsys/bluetooth/controller/ll_sw/ctrl_internal.h

@@ -41,6 +41,7 @@ struct connection {
 	u8_t  access_addr[4];
 	u8_t  crc_init[3];
 	u8_t  data_chan_map[5];
+	u8_t  chm_update;
 
 	u8_t  data_chan_count:6;
 	u8_t  data_chan_sel:1;