From d5306edce083b4cd8d87f84bf41af65c2d55608e Mon Sep 17 00:00:00 2001
From: Johan Hedberg <johan.hedberg@intel.com>
Date: Tue, 5 Nov 2013 11:31:19 +0200
Subject: [PATCH] emulator/bthost: Add support for CID hooks

This patch adds support for registering CID based hooks to a bthost.
These are intended for test tools to override the default data handling
for specific CIDs.
---
 emulator/bthost.c | 70 +++++++++++++++++++++++++++++++++++++++++++++++
 emulator/bthost.h |  9 ++++++
 2 files changed, 79 insertions(+)

diff --git a/emulator/bthost.c b/emulator/bthost.c
index da56b5cba..0c5836f67 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -60,11 +60,19 @@ struct cmd_queue {
 	struct cmd *tail;
 };
 
+struct cid_hook {
+	uint16_t cid;
+	bthost_cid_hook_func_t func;
+	void *user_data;
+	struct cid_hook *next;
+};
+
 struct btconn {
 	uint16_t handle;
 	uint8_t addr_type;
 	uint16_t next_cid;
 	struct l2conn *l2conns;
+	struct cid_hook *cid_hooks;
 	struct btconn *next;
 };
 
@@ -123,6 +131,13 @@ static void btconn_free(struct btconn *conn)
 		l2conn_free(l2conn);
 	}
 
+	while (conn->cid_hooks) {
+		struct cid_hook *hook = conn->cid_hooks;
+
+		conn->cid_hooks = hook->next;
+		free(hook);
+	}
+
 	free(conn);
 }
 
@@ -311,6 +326,42 @@ static uint8_t l2cap_sig_send(struct bthost *bthost, struct btconn *conn,
 	return ident;
 }
 
+void bthost_add_cid_hook(struct bthost *bthost, uint16_t handle, uint16_t cid,
+				bthost_cid_hook_func_t func, void *user_data)
+{
+	struct cid_hook *hook;
+	struct btconn *conn;
+
+	conn = bthost_find_conn(bthost, handle);
+	if (!conn)
+		return;
+
+	hook = malloc(sizeof(*hook));
+	if (!hook)
+		return;
+
+	memset(hook, 0, sizeof(*hook));
+
+	hook->cid = cid;
+	hook->func = func;
+	hook->user_data = user_data;
+
+	hook->next = conn->cid_hooks;
+	conn->cid_hooks = hook;
+}
+
+void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid,
+					const void *data, uint16_t len)
+{
+	struct btconn *conn;
+
+	conn = bthost_find_conn(bthost, handle);
+	if (!conn)
+		return;
+
+	send_acl(bthost, handle, cid, data, len);
+}
+
 bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t code,
 				const void *data, uint16_t len,
 				bthost_l2cap_rsp_cb cb, void *user_data)
@@ -973,11 +1024,24 @@ reject:
 							&rej, sizeof(rej));
 }
 
+static struct cid_hook *find_cid_hook(struct btconn *conn, uint16_t cid)
+{
+	struct cid_hook *hook;
+
+	for (hook = conn->cid_hooks; hook != NULL; hook = hook->next) {
+		if (hook->cid == cid)
+			return hook;
+	}
+
+	return NULL;
+}
+
 static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
 {
 	const struct bt_hci_acl_hdr *acl_hdr = data;
 	const struct bt_l2cap_hdr *l2_hdr = data + sizeof(*acl_hdr);
 	uint16_t handle, cid, acl_len, l2_len;
+	struct cid_hook *hook;
 	struct btconn *conn;
 	const void *l2_data;
 
@@ -1003,6 +1067,12 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
 
 	cid = le16_to_cpu(l2_hdr->cid);
 
+	hook = find_cid_hook(conn, cid);
+	if (hook) {
+		hook->func(l2_data, l2_len, hook->user_data);
+		return;
+	}
+
 	switch (cid) {
 	case 0x0001:
 		l2cap_sig(bthost, conn, l2_data, l2_len);
diff --git a/emulator/bthost.h b/emulator/bthost.h
index cd5bf1c9d..32b10f9cb 100644
--- a/emulator/bthost.h
+++ b/emulator/bthost.h
@@ -52,6 +52,15 @@ void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
 void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
 							uint8_t addr_type);
 
+typedef void (*bthost_cid_hook_func_t)(const void *data, uint16_t len,
+							void *user_data);
+
+void bthost_add_cid_hook(struct bthost *bthost, uint16_t handle, uint16_t cid,
+				bthost_cid_hook_func_t func, void *user_data);
+
+void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid,
+					const void *data, uint16_t len);
+
 typedef void (*bthost_l2cap_rsp_cb) (uint8_t code, const void *data,
 						uint16_t len, void *user_data);
 
-- 
2.47.3