Bluetooth: hci_raw: Add support for command extention

This adds support for registering a command extention table which is
used to match incoming commands and then pass the buffer to its
function handler.

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

include/bluetooth/hci_raw.h

@@ -67,7 +67,53 @@ int bt_hci_raw_set_mode(u8_t mode);
  */
 u8_t bt_hci_raw_get_mode(void);
 
-/** @brief Enable Bluetooth RAW channel
+#define BT_HCI_ERR_EXT_HANDLED  0xff
+
+/** Helper macro to define a command extension
+ *
+ *  @param _op Opcode of the command.
+ *  @param _min_len Minimal length of the command.
+ *  @param _func Handler function to be called.
+ */
+#define BT_HCI_RAW_CMD_EXT(_op, _min_len, _func) \
+	{ \
+		.op = _op, \
+		.min_len = _min_len, \
+		.func = _func, \
+	}
+
+struct bt_hci_raw_cmd_ext {
+	/** Opcode of the command */
+	u16_t  op;
+
+	/** Minimal length of the command */
+	size_t min_len;
+
+	/** Handler function.
+	 *
+	 *  Handler function to be called when a command is intercepted.
+	 *
+	 *  @param buf Buffer containing the command.
+	 *
+	 *  @return HCI Status code or BT_HCI_ERR_EXT_HANDLED if command has
+	 *  been handled already and a response has been sent as oppose to
+	 *  BT_HCI_ERR_SUCCESS which just indicates that the command can be
+	 *  sent to the controller to be processed.
+	 */
+	u8_t   (*func)(struct net_buf *buf);
+};
+
+/** @brief Register Bluetooth RAW command extension table
+ *
+ *  Register Bluetooth RAW channel command extension table, opcodes in this
+ *  table are intercepted to sent to the handler function.
+ *
+ *  @param cmds Pointer to the command extension table.
+ *  @param size Size of the command extension table.
+ */
+void bt_hci_raw_cmd_ext_register(struct bt_hci_raw_cmd_ext *cmds, size_t size);
+
+/** @brief Enable Bluetooth RAW channel:
  *
  *  Enable Bluetooth RAW HCI channel.
  *

subsys/bluetooth/Kconfig

@@ -71,6 +71,12 @@ config BT_HCI_RAW_RESERVE
 	  This option is used by the HCI raw transport implementation to
 	  declare how much headroom it needs for any HCI transport headers.
 
+config BT_HCI_RAW_CMD_EXT
+	bool "RAW HCI Command Extension"
+	help
+	  This option enables HCI RAW command extension so the driver can
+	  register it own command table extension.
+
 config BT_PERIPHERAL
 	bool "Peripheral Role support"
 	select BT_BROADCASTER

subsys/bluetooth/host/hci_raw.c

@@ -8,6 +8,7 @@
 
 #include <errno.h>
 #include <sys/atomic.h>
+#include <sys/byteorder.h>
 
 #include <drivers/bluetooth/hci_driver.h>
 #include <bluetooth/hci_raw.h>
@@ -37,6 +38,8 @@ NET_BUF_POOL_FIXED_DEFINE(hci_rx_pool, CONFIG_BT_RX_BUF_COUNT,
 			  BT_BUF_RX_SIZE, NULL);
 
 struct bt_dev_raw bt_dev;
+struct bt_hci_raw_cmd_ext *cmd_ext;
+static size_t cmd_ext_size;
 
 int bt_hci_driver_register(const struct bt_hci_driver *drv)
 {
@@ -151,6 +154,75 @@ static int bt_h4_send(struct net_buf *buf)
 	return 0;
 }
 
+static void bt_cmd_complete_ext(u16_t op, u8_t status)
+{
+	struct net_buf *buf;
+	struct bt_hci_evt_cc_status *cc;
+
+	if (status == BT_HCI_ERR_EXT_HANDLED) {
+		return;
+	}
+
+	buf = bt_hci_cmd_complete_create(op, sizeof(*cc));
+	cc = net_buf_add(buf, sizeof(*cc));
+	cc->status = status;
+
+	bt_recv(buf);
+}
+
+static u8_t bt_send_ext(struct net_buf *buf)
+{
+	struct bt_hci_cmd_hdr *hdr;
+	struct net_buf_simple_state state;
+	int i;
+	u16_t op;
+	u8_t status;
+
+	status = BT_HCI_ERR_SUCCESS;
+
+	if (!cmd_ext) {
+		return status;
+	}
+
+	net_buf_simple_save(&buf->b, &state);
+
+	if (buf->len < sizeof(*hdr)) {
+		BT_ERR("No HCI Command header");
+		return BT_HCI_ERR_INVALID_PARAM;
+	}
+
+	hdr = net_buf_pull_mem(buf, sizeof(*hdr));
+	if (buf->len < hdr->param_len) {
+		BT_ERR("Invalid HCI CMD packet length");
+		return BT_HCI_ERR_INVALID_PARAM;
+	}
+
+	op = sys_le16_to_cpu(hdr->opcode);
+
+	for (i = 0; i < cmd_ext_size; i++) {
+		struct bt_hci_raw_cmd_ext *cmd = &cmd_ext[i];
+
+		if (cmd->op == op) {
+			if (buf->len < cmd->min_len) {
+				status = BT_HCI_ERR_INVALID_PARAM;
+			} else {
+				status = cmd->func(buf);
+			}
+
+			break;
+		}
+	}
+
+	if (status) {
+		bt_cmd_complete_ext(op, status);
+		return status;
+	}
+
+	net_buf_simple_restore(&buf->b, &state);
+
+	return status;
+}
+
 int bt_send(struct net_buf *buf)
 {
 	BT_DBG("buf %p len %u", buf, buf->len);
@@ -166,6 +238,16 @@ int bt_send(struct net_buf *buf)
 
 	bt_monitor_send(bt_monitor_opcode(buf), buf->data, buf->len);
 
+	if (IS_ENABLED(CONFIG_BT_HCI_RAW_CMD_EXT) &&
+	    bt_buf_get_type(buf) == BT_BUF_CMD) {
+		u8_t status;
+
+		status = bt_send_ext(buf);
+		if (status) {
+			return status;
+		}
+	}
+
 	if (IS_ENABLED(CONFIG_BT_TINYCRYPT_ECC)) {
 		return bt_hci_ecc_send(buf);
 	}
@@ -198,6 +280,14 @@ u8_t bt_hci_raw_get_mode(void)
 	return BT_HCI_RAW_MODE_PASSTHROUGH;
 }
 
+void bt_hci_raw_cmd_ext_register(struct bt_hci_raw_cmd_ext *cmds, size_t size)
+{
+	if (IS_ENABLED(CONFIG_BT_HCI_RAW_CMD_EXT)) {
+		cmd_ext = cmds;
+		cmd_ext_size = size;
+	}
+}
+
 int bt_enable_raw(struct k_fifo *rx_queue)
 {
 	const struct bt_hci_driver *drv = bt_dev.drv;

subsys/usb/class/Kconfig

@@ -95,6 +95,7 @@ config USB_DEVICE_BLUETOOTH_VS_H4
 	bool "Enable USB Bluetooth H4 vendor command"
 	depends on USB_DEVICE_BLUETOOTH
 	select BT_HCI_RAW_H4
+	select BT_HCI_RAW_CMD_EXT
 	help
 	  Enables vendor command to switch to H:4 transport using the bulk
 	  endpoint.

subsys/usb/class/bluetooth.c

@@ -271,15 +271,6 @@ 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;
@@ -296,7 +287,7 @@ static u8_t vs_read_usb_transport_mode(struct net_buf *buf)
 
 	net_buf_put(&tx_queue, rsp);
 
-	return BT_HCI_ERR_VS_HANDLED;
+	return BT_HCI_ERR_EXT_HANDLED;
 }
 
 static u8_t vs_set_usb_transport_mode(struct net_buf *buf)
@@ -325,34 +316,14 @@ static u8_t vs_set_usb_transport_mode(struct net_buf *buf)
 	return BT_HCI_ERR_SUCCESS;
 }
 
-static struct hci_cmd_ext {
+static struct bt_hci_raw_cmd_ext cmd_ext[] = {
-	u16_t  op;
+	BT_HCI_RAW_CMD_EXT(BT_OCF(BT_HCI_OP_VS_READ_USB_TRANSPORT_MODE), 0,
-	size_t min_len;
+			   vs_read_usb_transport_mode),
-	u8_t   (*func)(struct net_buf *buf);
+	BT_HCI_RAW_CMD_EXT(BT_OCF(BT_HCI_OP_VS_SET_USB_TRANSPORT_MODE),
-} cmd_ext[] = {
+			   sizeof(struct bt_hci_cp_vs_set_usb_transport_mode),
-	CMD_EXT(BT_OCF(BT_HCI_OP_VS_READ_USB_TRANSPORT_MODE), 0,
+			   vs_set_usb_transport_mode),
-		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)
 {
@@ -376,46 +347,6 @@ static int bluetooth_class_handler(struct usb_setup_packet *setup,
 
 	net_buf_add_mem(buf, *data, *len);
 
-	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;
@@ -455,6 +386,10 @@ static int bluetooth_init(struct device *dev)
 		return ret;
 	}
 
+	if (IS_ENABLED(CONFIG_USB_DEVICE_BLUETOOTH_VS_H4)) {
+		bt_hci_raw_cmd_ext_register(cmd_ext, ARRAY_SIZE(cmd_ext));
+	}
+
 	k_thread_create(&rx_thread_data, rx_thread_stack,
 			K_THREAD_STACK_SIZEOF(rx_thread_stack),
 			(k_thread_entry_t)hci_rx_thread, NULL, NULL, NULL,