Bluetooth: hci_usb: Add implementation of Read/Set USB Transport Mode

This implements Read/Set USB Transport Mode in the Bluetooth class.

Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

include/bluetooth/hci_vs.h

@@ -184,6 +184,23 @@ struct bt_hci_rp_vs_read_tx_power_level {
 	s8_t  tx_power_level;
 } __packed;
 
+#define BT_HCI_OP_VS_READ_USB_TRANSPORT_MODE    BT_OP(BT_OGF_VS, 0x0010)
+
+struct bt_hci_rp_vs_read_usb_transport_mode {
+	u8_t  status;
+	u8_t  num_supported_modes;
+	u8_t  supported_mode[0];
+} __packed;
+
+#define BT_HCI_VS_USB_H2_MODE                  0x00
+#define BT_HCI_VS_USB_H4_MODE                  0x01
+
+#define BT_HCI_OP_VS_SET_USB_TRANSPORT_MODE    BT_OP(BT_OGF_VS, 0x0011)
+
+struct bt_hci_cp_vs_set_usb_transport_mode {
+	u8_t  mode;
+} __packed;
+
 /* Events */
 
 struct bt_hci_evt_vs {

samples/bluetooth/hci_usb/prj.conf

@@ -9,3 +9,4 @@ CONFIG_BT_HCI_RAW=y
 CONFIG_USB=y
 CONFIG_USB_DEVICE_STACK=y
 CONFIG_USB_DEVICE_BLUETOOTH=y
+CONFIG_USB_DEVICE_BLUETOOTH_VS_H4=y

subsys/bluetooth/controller/hci/hci.c

@@ -1901,6 +1901,12 @@ static void vs_read_supported_commands(struct net_buf *buf,
 	/* Write Tx Power, Read Tx Power */
 	rp->commands[1] |= BIT(5) | BIT(6);
 #endif /* CONFIG_BT_CTLR_TX_PWR_DYNAMIC_CONTROL */
+#if defined(CONFIG_USB_DEVICE_BLUETOOTH_VS_H4)
+	/* Read Supported USB Transport Modes */
+	rp->commands[1] |= BIT(7);
+	/* Set USB Transport Mode */
+	rp->commands[2] |= BIT(0);
+#endif /* USB_DEVICE_BLUETOOTH_VS_H4 */
 #endif /* CONFIG_BT_HCI_VS_EXT */
 }
 
@@ -2207,6 +2213,14 @@ int hci_vendor_cmd_handle_common(u16_t ocf, struct net_buf *cmd,
 		vs_read_supported_features(cmd, evt);
 		break;
 
+#if defined(CONFIG_USB_DEVICE_BLUETOOTH_VS_H4)
+	case BT_OCF(BT_HCI_OP_VS_READ_USB_TRANSPORT_MODE):
+		break;
+	case BT_OCF(BT_HCI_OP_VS_SET_USB_TRANSPORT_MODE):
+		reset(cmd, evt);
+		break;
+#endif /* CONFIG_USB_DEVICE_BLUETOOTH_VS_H4 */
+
 #if defined(CONFIG_BT_HCI_VS_EXT)
 	case BT_OCF(BT_HCI_OP_VS_READ_BUILD_INFO):
 		vs_read_build_info(cmd, evt);

subsys/usb/class/Kconfig

@@ -107,6 +107,13 @@ config BLUETOOTH_BULK_EP_MPS
 	help
 	  Bluetooth device class bulk endpoint size
 
+config USB_DEVICE_BLUETOOTH_VS_H4
+	bool "Enable USB Bluetooth H4 vendor command"
+	depends on USB_DEVICE_BLUETOOTH
+	help
+	  Enables vendor command to switch to H:4 transport using the bulk
+	  endpoint.
+
 config USB_DEVICE_BT_H4
 	bool "USB Bluetooth H4 Device Class Driver"
 	select BT

subsys/usb/class/bluetooth.c

@@ -18,6 +18,8 @@
 #include <bluetooth/buf.h>
 #include <bluetooth/hci_raw.h>
 #include <bluetooth/l2cap.h>
+#include <bluetooth/hci_vs.h>
+#include <drivers/bluetooth/hci_driver.h>
 
 #define LOG_LEVEL CONFIG_USB_DEVICE_LOG_LEVEL
 #include <logging/log.h>
@@ -28,15 +30,23 @@ static K_FIFO_DEFINE(tx_queue);
 
 /* HCI command buffers */
 #define CMD_BUF_SIZE BT_BUF_RX_SIZE
-NET_BUF_POOL_DEFINE(tx_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE,
+NET_BUF_POOL_DEFINE(rx_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE,
 		    sizeof(u8_t), NULL);
 
 /* ACL data TX buffers */
+#if defined(CONFIG_USB_DEVICE_BLUETOOTH_VS_H4)
+#if defined(CONFIG_BT_CTLR_TX_BUFFERS)
+#define ACL_BUF_COUNT (CONFIG_BT_HCI_CMD_COUNT + CONFIG_BT_CTLR_TX_BUFFERS)
+#else
+#define ACL_BUF_COUNT (CONFIG_BT_HCI_CMD_COUNT + 4)
+#endif
+#else
 #if defined(CONFIG_BT_CTLR_TX_BUFFERS)
 #define ACL_BUF_COUNT CONFIG_BT_CTLR_TX_BUFFERS
 #else
 #define ACL_BUF_COUNT 4
 #endif
+#endif /* CONFIG_USB_DEVICE_BLUETOOTH_VS_H4 */
 
 #if defined(CONFIG_BT_CTLR_TX_BUFFER_SIZE)
 #define BT_L2CAP_MTU (CONFIG_BT_CTLR_TX_BUFFER_SIZE - BT_L2CAP_HDR_SIZE)
@@ -45,8 +55,12 @@ NET_BUF_POOL_DEFINE(tx_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE,
 #endif
 
 /* Data size needed for ACL buffers */
+#if defined(CONFIG_USB_DEVICE_BLUETOOTH_VS_H4)
+#define BT_BUF_ACL_SIZE MAX(BT_BUF_RX_SIZE, BT_L2CAP_BUF_SIZE(BT_L2CAP_MTU))
+#else
 #define BT_BUF_ACL_SIZE BT_L2CAP_BUF_SIZE(BT_L2CAP_MTU)
-NET_BUF_POOL_DEFINE(acl_tx_pool, ACL_BUF_COUNT, BT_BUF_ACL_SIZE,
+#endif
+NET_BUF_POOL_DEFINE(acl_rx_pool, ACL_BUF_COUNT, BT_BUF_ACL_SIZE,
 		    sizeof(u8_t), NULL);
 
 #define BLUETOOTH_INT_EP_ADDR		0x81
@@ -58,6 +72,7 @@ static K_THREAD_STACK_DEFINE(rx_thread_stack, 512);
 static struct k_thread rx_thread_data;
 static K_THREAD_STACK_DEFINE(tx_thread_stack, 512);
 static struct k_thread tx_thread_data;
+static u8_t mode = BT_HCI_VS_USB_H2_MODE;
 
 struct usb_bluetooth_config {
 	struct usb_if_descriptor if0;
@@ -137,14 +152,48 @@ static struct usb_ep_cfg_data bluetooth_ep_data[] = {
 	},
 };
 
-static void hci_rx_thread(void)
+#define H4_CMD 0x01
+#define H4_ACL 0x02
+#define H4_SCO 0x03
+#define H4_EVT 0x04
+
+static void usb_h4_send(struct net_buf *buf)
+{
+	LOG_DBG("buf %p type %u len %u", buf, bt_buf_get_type(buf), buf->len);
+
+	switch (bt_buf_get_type(buf)) {
+	case BT_BUF_ACL_IN:
+		net_buf_push_u8(buf, H4_ACL);
+		break;
+	case BT_BUF_EVT:
+		net_buf_push_u8(buf, H4_EVT);
+		break;
+	default:
+		LOG_ERR("Unknown type %u", bt_buf_get_type(buf));
+		net_buf_unref(buf);
+		return;
+	}
+
+	usb_transfer_sync(bluetooth_ep_data[HCI_IN_EP_IDX].ep_addr, buf->data,
+			  buf->len, USB_TRANS_WRITE);
+
+	net_buf_unref(buf);
+}
+
+static void hci_tx_thread(void)
 {
 	LOG_DBG("Start USB Bluetooth thread");
 
 	while (true) {
 		struct net_buf *buf;
 
-		buf = net_buf_get(&rx_queue, K_FOREVER);
+		buf = net_buf_get(&tx_queue, K_FOREVER);
+
+		if (IS_ENABLED(CONFIG_USB_DEVICE_BLUETOOTH_VS_H4) &&
+		    mode == BT_HCI_VS_USB_H4_MODE) {
+			usb_h4_send(buf);
+			continue;
+		}
 
 		switch (bt_buf_get_type(buf)) {
 		case BT_BUF_EVT:
@@ -168,14 +217,43 @@ static void hci_rx_thread(void)
 	}
 }
 
-static void hci_tx_thread(void)
+static void usb_h4_recv(struct net_buf *buf)
+{
+	u8_t type;
+
+	type = net_buf_pull_u8(buf);
+
+	switch (type) {
+	case H4_CMD:
+		bt_buf_set_type(buf, BT_BUF_CMD);
+		break;
+	case H4_ACL:
+		bt_buf_set_type(buf, BT_BUF_ACL_OUT);
+		break;
+	default:
+		LOG_ERR("Unknown H4 type %u", type);
+		return;
+	}
+
+	LOG_DBG("buf %p type %u len %u", buf, bt_buf_get_type(buf), buf->len);
+
+	if (bt_send(buf)) {
+		LOG_ERR("Error sending to driver");
+		net_buf_unref(buf);
+	}
+}
+
+static void hci_rx_thread(void)
 {
 	while (true) {
 		struct net_buf *buf;
 
-		buf = net_buf_get(&tx_queue, K_FOREVER);
+		buf = net_buf_get(&rx_queue, K_FOREVER);
 
-		if (bt_send(buf)) {
+		if (IS_ENABLED(CONFIG_USB_DEVICE_BLUETOOTH_VS_H4) &&
+		    mode == BT_HCI_VS_USB_H4_MODE) {
+			usb_h4_recv(buf);
+		} else if (bt_send(buf)) {
 			LOG_ERR("Error sending to driver");
 			net_buf_unref(buf);
 		}
@@ -189,7 +267,7 @@ static void acl_read_cb(u8_t ep, int size, void *priv)
 	if (size > 0) {
 		buf->len += size;
 		bt_buf_set_type(buf, BT_BUF_ACL_OUT);
-		net_buf_put(&tx_queue, buf);
+		net_buf_put(&rx_queue, buf);
 		buf = NULL;
 	}
 
@@ -197,7 +275,7 @@ static void acl_read_cb(u8_t ep, int size, void *priv)
 		net_buf_unref(buf);
 	}
 
-	buf = net_buf_alloc(&acl_tx_pool, K_FOREVER);
+	buf = net_buf_alloc(&acl_rx_pool, K_FOREVER);
 	__ASSERT_NO_MSG(buf);
 
 	net_buf_reserve(buf, BT_BUF_RESERVE);
@@ -251,6 +329,88 @@ static void bluetooth_status_cb(struct usb_cfg_data *cfg,
 	}
 }
 
+#define BT_HCI_ERR_VS_HANDLED  0xff
+
+#define CMD_EXT(_op, _min_len, _func) \
+	{ \
+		.op = _op, \
+		.min_len = _min_len, \
+		.func = _func, \
+	}
+
+static u8_t vs_read_usb_transport_mode(struct net_buf *buf)
+{
+	struct net_buf *rsp;
+	struct bt_hci_rp_vs_read_usb_transport_mode *rp;
+
+	rsp = bt_hci_cmd_complete_create(BT_HCI_OP_VS_READ_USB_TRANSPORT_MODE,
+					 sizeof(*rp) + 2);
+	rp = net_buf_add(rsp, sizeof(*rp));
+	rp->status = BT_HCI_ERR_SUCCESS;
+	rp->num_supported_modes = 2;
+
+	net_buf_add_u8(rsp, BT_HCI_VS_USB_H2_MODE);
+	net_buf_add_u8(rsp, BT_HCI_VS_USB_H4_MODE);
+
+	net_buf_put(&tx_queue, rsp);
+
+	return BT_HCI_ERR_VS_HANDLED;
+}
+
+static u8_t vs_set_usb_transport_mode(struct net_buf *buf)
+{
+	struct bt_hci_cp_vs_set_usb_transport_mode *cp;
+
+	cp = net_buf_pull_mem(buf, sizeof(*cp));
+
+	if (mode == cp->mode) {
+		return BT_HCI_ERR_INVALID_PARAM;
+	}
+
+	switch (cp->mode) {
+	case BT_HCI_VS_USB_H2_MODE:
+	case BT_HCI_VS_USB_H4_MODE:
+		break;
+	default:
+		LOG_DBG("Invalid mode: %u", cp->mode);
+		return BT_HCI_ERR_INVALID_PARAM;
+	}
+
+	mode = cp->mode;
+
+	LOG_DBG("mode %u", mode);
+
+	return BT_HCI_ERR_SUCCESS;
+}
+
+static struct hci_cmd_ext {
+	u16_t  op;
+	size_t min_len;
+	u8_t   (*func)(struct net_buf *buf);
+} cmd_ext[] = {
+	CMD_EXT(BT_OCF(BT_HCI_OP_VS_READ_USB_TRANSPORT_MODE), 0,
+		vs_read_usb_transport_mode),
+	CMD_EXT(BT_OCF(BT_HCI_OP_VS_SET_USB_TRANSPORT_MODE),
+		sizeof(struct bt_hci_cp_vs_set_usb_transport_mode),
+		vs_set_usb_transport_mode),
+};
+
+static void vs_cmd_complete(u16_t op, u8_t status)
+{
+	struct net_buf *buf;
+	struct bt_hci_evt_cc_status *cc;
+
+	if (status == BT_HCI_ERR_VS_HANDLED) {
+		return;
+	}
+
+	buf = bt_hci_cmd_complete_create(op, sizeof(*cc));
+	cc = net_buf_add(buf, sizeof(*cc));
+	cc->status = status;
+
+	net_buf_put(&tx_queue, buf);
+}
+
 static int bluetooth_class_handler(struct usb_setup_packet *setup,
 				   s32_t *len, u8_t **data)
 {
@@ -263,7 +423,7 @@ static int bluetooth_class_handler(struct usb_setup_packet *setup,
 		return -EINVAL;
 	}
 
-	buf = net_buf_alloc(&tx_pool, K_NO_WAIT);
+	buf = net_buf_alloc(&rx_pool, K_NO_WAIT);
 	if (!buf) {
 		LOG_ERR("Cannot get free buffer\n");
 		return -ENOMEM;
@@ -274,7 +434,47 @@ static int bluetooth_class_handler(struct usb_setup_packet *setup,
 
 	net_buf_add_mem(buf, *data, *len);
 
-	net_buf_put(&tx_queue, buf);
+	if (IS_ENABLED(CONFIG_USB_DEVICE_BLUETOOTH_VS_H4)) {
+		struct bt_hci_cmd_hdr *hdr;
+		struct net_buf_simple_state state;
+		int i;
+
+		net_buf_simple_save(&buf->b, &state);
+
+		if (buf->len < sizeof(*hdr)) {
+			LOG_ERR("No HCI Command header");
+			return -EINVAL;
+		}
+
+		hdr = net_buf_pull_mem(buf, sizeof(*hdr));
+		if (buf->len < hdr->param_len) {
+			LOG_ERR("Invalid HCI CMD packet length");
+			return -EINVAL;
+		}
+
+		for (i = 0; i < ARRAY_SIZE(cmd_ext); i++) {
+			struct hci_cmd_ext *cmd = &cmd_ext[i];
+
+			if (cmd->op == sys_le16_to_cpu(hdr->opcode)) {
+				u8_t status;
+
+				if (buf->len < cmd->min_len) {
+					status = BT_HCI_ERR_INVALID_PARAM;
+				} else {
+					status = cmd->func(buf);
+				}
+
+				if (status) {
+					vs_cmd_complete(cmd->op, status);
+					return 0;
+				}
+			}
+		}
+
+		net_buf_simple_restore(&buf->b, &state);
+	}
+
+	net_buf_put(&rx_queue, buf);
 
 	return 0;
 }
@@ -307,7 +507,7 @@ static int bluetooth_init(struct device *dev)
 
 	LOG_DBG("Initialization");
 
-	ret = bt_enable_raw(&rx_queue);
+	ret = bt_enable_raw(&tx_queue);
 	if (ret) {
 		LOG_ERR("Failed to open Bluetooth raw channel: %d", ret);
 		return ret;