Bluetooth: controller: Implement Write BD_ADDR VS cmd

Implement the Zephyr VS command that allows a Host to write a public
Bluetooth Address to the Controller in order to allow Hosts to provide
their own public Bluetooth addresses.

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

Author: carlescufi

subsys/bluetooth/controller/hci/hci.c

@@ -1674,6 +1674,8 @@ static void vs_read_supported_commands(struct net_buf *buf,
 	/* Set Version Information, Supported Commands, Supported Features. */
 	rp->commands[0] |= BIT(0) | BIT(1) | BIT(2);
 #if defined(CONFIG_BT_CTLR_HCI_VS_EXT)
+	/* Write BD_ADDR */
+	rp->commands[0] |= BIT(5);
 	/* Read Static Addresses, Read Key Hierarchy Roots */
 	rp->commands[1] |= BIT(0) | BIT(1);
 #endif /* CONFIG_BT_CTLR_HCI_VS_EXT */
@@ -1691,6 +1693,17 @@ static void vs_read_supported_features(struct net_buf *buf,
 }
 
 #if defined(CONFIG_BT_CTLR_HCI_VS_EXT)
+static void vs_write_bd_addr(struct net_buf *buf, struct net_buf **evt)
+{
+	struct bt_hci_cp_vs_write_bd_addr *cmd = (void *)buf->data;
+	struct bt_hci_evt_cc_status *ccst;
+
+	ll_addr_set(0, &cmd->bdaddr.val[0]);
+
+	ccst = cmd_complete(evt, sizeof(*ccst));
+	ccst->status = 0x00;
+}
+
 static void vs_read_static_addrs(struct net_buf *buf, struct net_buf **evt)
 {
 	struct bt_hci_rp_vs_read_static_addrs *rp;
@@ -1796,6 +1809,10 @@ static int vendor_cmd_handle(u16_t ocf, struct net_buf *cmd,
 		break;
 
 #if defined(CONFIG_BT_CTLR_HCI_VS_EXT)
+	case BT_OCF(BT_HCI_OP_VS_WRITE_BD_ADDR):
+		vs_write_bd_addr(cmd, evt);
+		break;
+
 	case BT_OCF(BT_HCI_OP_VS_READ_STATIC_ADDRS):
 		vs_read_static_addrs(cmd, evt);
 		break;