Add support for fetching IO capability from an agent

5 files changed, 62 insertions, 7 deletions

diff --git a/hcid/agent.c b/hcid/agent.c
index 2335f09c..5706ea94 100644
--- a/hcid/agent.c
+++ b/hcid/agent.c
@@ -518,6 +518,11 @@ failed:
 	return -1;
 }
 
+uint8_t agent_get_io_capability(struct agent *agent)
+{
+	return agent->capability;
+}
+
 gboolean agent_matches(struct agent *agent, const char *name, const char *path)
 {
 	if (g_str_equal(agent->name, name) && g_str_equal(agent->path, path))
diff --git a/hcid/agent.h b/hcid/agent.h
index 22f79aa4..2bfe003e 100644
--- a/hcid/agent.h
+++ b/hcid/agent.h
@@ -49,6 +49,8 @@ int agent_confirm_mode_change(struct agent *agent, const char *new_mode,
 
 int agent_cancel(struct agent *agent);
 
+uint8_t agent_get_io_capability(struct agent *agent);
+
 gboolean agent_matches(struct agent *agent, const char *name, const char *path);
 
 void agent_init(void);
diff --git a/hcid/dbus-hci.c b/hcid/dbus-hci.c
index b28b5c7d..14261bfe 100644
--- a/hcid/dbus-hci.c
+++ b/hcid/dbus-hci.c
@@ -2269,6 +2269,41 @@ void hcid_dbus_pin_code_reply(bdaddr_t *local, void *ptr)
 	}
 }
 
+int hcid_dbus_get_io_cap(bdaddr_t *local, bdaddr_t *remote, uint8_t *cap,
+				uint8_t *auth)
+{
+	struct adapter *adapter;
+	struct device *device;
+	struct agent *agent;
+	char addr[18];
+
+	adapter = find_adapter(local);
+	if (!adapter) {
+		error("No matching adapter found");
+		return -1;
+	}
+
+	ba2str(remote, addr);
+
+	device = adapter_find_device(adapter, addr);
+	if (device && device->agent) {
+		agent = device->agent;
+		*auth = 0x03;
+	} else {
+		agent = adapter->agent;
+		*auth = 0x01;
+	}
+
+	if (!agent) {
+		error("No agent available for IO capability");
+		return -1;
+	}
+
+	*cap = agent_get_io_capability(agent);
+
+	return 0;
+}
+
 static int inquiry_cancel(int dd, int to)
 {
 	struct hci_request rq;
diff --git a/hcid/dbus-hci.h b/hcid/dbus-hci.h
index 5b8c89e3..671c5f1a 100644
--- a/hcid/dbus-hci.h
+++ b/hcid/dbus-hci.h
@@ -47,6 +47,8 @@ void hcid_dbus_setscan_enable_complete(bdaddr_t *local);
 void hcid_dbus_write_class_complete(bdaddr_t *local);
 void hcid_dbus_write_simple_pairing_mode_complete(bdaddr_t *local);
 void hcid_dbus_pin_code_reply(bdaddr_t *local, void *ptr);
+int hcid_dbus_get_io_cap(bdaddr_t *local, bdaddr_t *remote, uint8_t *cap,
+				uint8_t *auth);
 
 int unregister_adapter_path(const char *path);
 
diff --git a/hcid/security.c b/hcid/security.c
index c9e7dda7..6f8151f8 100644
--- a/hcid/security.c
+++ b/hcid/security.c
@@ -375,18 +375,29 @@ static void remote_oob_data_request(int dev, bdaddr_t *sba, void *ptr)
 
 static void io_capa_request(int dev, bdaddr_t *sba, bdaddr_t *dba)
 {
-	io_capability_neg_reply_cp cp;
 	char sa[18], da[18];
+	uint8_t cap, auth;
 
 	ba2str(sba, sa); ba2str(dba, da);
 	info("io_capa_request (sba=%s, dba=%s)", sa, da);
 
-	memset(&cp, 0, sizeof(cp));
-	bacpy(&cp.bdaddr, dba);
-	cp.reason = HCI_PAIRING_NOT_ALLOWED;
-
-	hci_send_cmd(dev, OGF_LINK_CTL, OCF_IO_CAPABILITY_NEG_REPLY,
-					IO_CAPABILITY_NEG_REPLY_CP_SIZE, &cp);
+	if (hcid_dbus_get_io_cap(sba, dba, &cap, &auth) < 0) {
+		io_capability_neg_reply_cp cp;
+		memset(&cp, 0, sizeof(cp));
+		bacpy(&cp.bdaddr, dba);
+		cp.reason = HCI_PAIRING_NOT_ALLOWED;
+		hci_send_cmd(dev, OGF_LINK_CTL, OCF_IO_CAPABILITY_NEG_REPLY,
+				IO_CAPABILITY_NEG_REPLY_CP_SIZE, &cp);
+	} else {
+		io_capability_reply_cp cp;
+		memset(&cp, 0, sizeof(cp));
+		bacpy(&cp.bdaddr, dba);
+		cp.capability = cap;
+		cp.oob_data = 0x00;
+		cp.authentication = auth;
+		hci_send_cmd(dev, OGF_LINK_CTL, OCF_IO_CAPABILITY_REPLY,
+				IO_CAPABILITY_REPLY_CP_SIZE, &cp);
+	}
 }
 
 /* PIN code handling */