Store remote version and features information

3 files changed, 240 insertions, 0 deletions

diff --git a/hcid/hcid.h b/hcid/hcid.h
index 36cba71f..3beebd49 100644
--- a/hcid/hcid.h
+++ b/hcid/hcid.h
@@ -112,6 +112,8 @@ gboolean hcid_dbus_init(void);
 
 int write_device_name(const bdaddr_t *local, const bdaddr_t *peer, const char *name);
 int read_device_name(const bdaddr_t *local, const bdaddr_t *peer, char *name);
+int write_version_info(const bdaddr_t *local, const bdaddr_t *peer, const uint16_t manufacturer, const uint8_t lmp_ver, const uint16_t lmp_subver);
+int write_features_info(const bdaddr_t *local, const bdaddr_t *peer, const unsigned char *features);
 int write_link_key(const bdaddr_t *local, const bdaddr_t *peer, const unsigned char *key, const int type);
 int read_link_key(const bdaddr_t *local, const bdaddr_t *peer, unsigned char *key);
 int read_pin_code(const bdaddr_t *local, const bdaddr_t *peer, char *pin);
diff --git a/hcid/security.c b/hcid/security.c
index 8ac3d759..8f5c69bc 100644
--- a/hcid/security.c
+++ b/hcid/security.c
@@ -73,6 +73,38 @@ void toggle_pairing(int enable)
 	syslog(LOG_INFO, "Pairing %s", pairing ? "enabled" : "disabled");
 }
 
+static int get_bdaddr(int dev, bdaddr_t *sba, uint16_t handle, bdaddr_t *dba)
+{
+	struct hci_conn_list_req *cl;
+	struct hci_conn_info *ci;
+	char addr[18];
+	int i;
+
+	cl = malloc(10 * sizeof(*ci) + sizeof(*cl));
+	if (!cl)
+		return -ENOMEM;
+
+	ba2str(sba, addr);
+	cl->dev_id = hci_devid(addr);
+	cl->conn_num = 10;
+	ci = cl->conn_info;
+
+	if (ioctl(dev, HCIGETCONNLIST, (void *) cl) < 0) {
+		free(cl);
+		return -EIO;
+	}
+
+	for (i = 0; i < cl->conn_num; i++, ci++)
+		if (ci->handle == handle) {
+			bacpy(dba, &ci->bdaddr);
+			free(cl);
+			return 0;
+		}
+
+	free(cl);
+	return -ENOENT;
+}
+
 /* Link Key handling */
 
 /* This function is not reentrable */
@@ -420,6 +452,35 @@ static void remote_name_information(int dev, bdaddr_t *sba, void *ptr)
 	write_device_name(sba, dba, evt->name);
 }
 
+static void remote_version_information(int dev, bdaddr_t *sba, void *ptr)
+{
+	evt_read_remote_version_complete *evt = ptr;
+	bdaddr_t dba;
+
+	if (evt->status)
+		return;
+
+	if (get_bdaddr(dev, sba, btohs(evt->handle), &dba) < 0)
+		return;
+
+	write_version_info(sba, &dba, btohs(evt->manufacturer),
+				evt->lmp_ver, btohs(evt->lmp_subver));
+}
+
+static void remote_features_information(int dev, bdaddr_t *sba, void *ptr)
+{
+	evt_read_remote_features_complete *evt = ptr;
+	bdaddr_t dba;
+
+	if (evt->status)
+		return;
+
+	if (get_bdaddr(dev, sba, btohs(evt->handle), &dba) < 0)
+		return;
+
+	write_features_info(sba, &dba, evt->features);
+}
+
 static gboolean io_security_event(GIOChannel *chan, GIOCondition cond, gpointer data)
 {
 	unsigned char buf[HCI_MAX_EVENT_SIZE], *ptr = buf;
@@ -462,6 +523,14 @@ static gboolean io_security_event(GIOChannel *chan, GIOCondition cond, gpointer
 	case EVT_REMOTE_NAME_REQ_COMPLETE:
 		remote_name_information(dev, &di->bdaddr, ptr);
 		break;
+
+	case EVT_READ_REMOTE_VERSION_COMPLETE:
+		remote_version_information(dev, &di->bdaddr, ptr);
+		break;
+
+	case EVT_READ_REMOTE_FEATURES_COMPLETE:
+		remote_features_information(dev, &di->bdaddr, ptr);
+		break;
 	}
 
 	if (hci_test_bit(HCI_SECMGR, &di->flags))
@@ -509,6 +578,8 @@ void start_security_manager(int hdev)
 	hci_filter_set_event(EVT_LINK_KEY_REQ, &flt);
 	hci_filter_set_event(EVT_LINK_KEY_NOTIFY, &flt);
 	hci_filter_set_event(EVT_REMOTE_NAME_REQ_COMPLETE, &flt);
+	hci_filter_set_event(EVT_READ_REMOTE_VERSION_COMPLETE, &flt);
+	hci_filter_set_event(EVT_READ_REMOTE_FEATURES_COMPLETE, &flt);
 	if (setsockopt(dev, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
 		syslog(LOG_ERR, "Can't set filter on hci%d: %s (%d)",
 						hdev, strerror(errno), errno);
diff --git a/hcid/storage.c b/hcid/storage.c
index ca7a7d07..ffc92b44 100644
--- a/hcid/storage.c
+++ b/hcid/storage.c
@@ -303,6 +303,173 @@ close:
 	return err;
 }
 
+int write_version_info(const bdaddr_t *local, const bdaddr_t *peer, const uint16_t manufacturer, const uint8_t lmp_ver, const uint16_t lmp_subver)
+{
+	struct list *temp, *list = NULL;
+	char filename[PATH_MAX + 1], addr[18], str[16], *buf, *ptr;
+	bdaddr_t bdaddr;
+	struct stat st;
+	int fd, pos, err = 0;
+
+	ba2str(local, addr);
+	snprintf(filename, PATH_MAX, "%s/%s/manufacturers", DEVPATH, addr);
+
+	umask(S_IWGRP | S_IWOTH);
+	create_dirs(filename, S_IRUSR | S_IWUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH);
+
+	fd = open(filename, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+	if (fd < 0)
+		return -errno;
+
+	if (flock(fd, LOCK_EX) < 0) {
+		err = -errno;
+		goto close;
+	}
+
+	if (fstat(fd, &st) < 0) {
+		err = -errno;
+		goto unlock;
+	}
+
+	buf = malloc(st.st_size + 100);
+	if (!buf) {
+		err = -ENOMEM;
+		goto unlock;
+	}
+
+	if (st.st_size > 0) {
+		read(fd, buf, st.st_size);
+
+		ptr = buf;
+
+		memset(str, 0, sizeof(str));
+		while (sscanf(ptr, "%17s %[^\n]\n%n", addr, str, &pos) != EOF) {
+			str2ba(addr, &bdaddr);
+			str[sizeof(str) - 1] = '\0';
+
+			list = list_add(list, &bdaddr, str, sizeof(str));
+
+			memset(str, 0, sizeof(str));
+			ptr += pos;
+			if (ptr - buf >= st.st_size)
+				break;
+		};
+
+		lseek(fd, 0, SEEK_SET);
+		ftruncate(fd, 0);
+	}
+
+	memset(str, 0, sizeof(str));
+	sprintf(str, "%d %d %d", manufacturer, lmp_ver, lmp_subver);
+
+	list = list_add(list, peer, str, sizeof(str));
+	if (!list) {
+		err = -EIO;
+		goto unlock;
+	}
+
+	list_foreach(list, temp) {
+		ba2str(&temp->bdaddr, addr);
+		if (temp->data && temp->size > 0) {
+			memset(buf, 0, 100);
+			snprintf(buf, 99, "%s %s\n", addr, temp->data);
+			write(fd, buf, strlen(buf));
+		}
+	}
+
+unlock:
+	flock(fd, LOCK_UN);
+
+close:
+	close(fd);
+	list_free(list);
+	return err;
+}
+
+int write_features_info(const bdaddr_t *local, const bdaddr_t *peer, const unsigned char *features)
+{
+	struct list *temp, *list = NULL;
+	char filename[PATH_MAX + 1], addr[18], str[17], *buf, *ptr;
+	bdaddr_t bdaddr;
+	struct stat st;
+	int i, fd, pos, err = 0;
+
+	ba2str(local, addr);
+	snprintf(filename, PATH_MAX, "%s/%s/features", DEVPATH, addr);
+
+	umask(S_IWGRP | S_IWOTH);
+	create_dirs(filename, S_IRUSR | S_IWUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH);
+
+	fd = open(filename, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+	if (fd < 0)
+		return -errno;
+
+	if (flock(fd, LOCK_EX) < 0) {
+		err = -errno;
+		goto close;
+	}
+
+	if (fstat(fd, &st) < 0) {
+		err = -errno;
+		goto unlock;
+	}
+
+	buf = malloc(st.st_size + 100);
+	if (!buf) {
+		err = -ENOMEM;
+		goto unlock;
+	}
+
+	if (st.st_size > 0) {
+		read(fd, buf, st.st_size);
+
+		ptr = buf;
+
+		memset(str, 0, sizeof(str));
+		while (sscanf(ptr, "%17s %[^\n]\n%n", addr, str, &pos) != EOF) {
+			str2ba(addr, &bdaddr);
+			str[sizeof(str) - 1] = '\0';
+
+			list = list_add(list, &bdaddr, str, sizeof(str));
+
+			memset(str, 0, sizeof(str));
+			ptr += pos;
+			if (ptr - buf >= st.st_size)
+				break;
+		};
+
+		lseek(fd, 0, SEEK_SET);
+		ftruncate(fd, 0);
+	}
+
+	memset(str, 0, sizeof(str));
+	for (i = 0; i < 8; i++)
+		sprintf(str + (i * 2), "%2.2X", features[i]);
+
+	list = list_add(list, peer, str, sizeof(str));
+	if (!list) {
+		err = -EIO;
+		goto unlock;
+	}
+
+	list_foreach(list, temp) {
+		ba2str(&temp->bdaddr, addr);
+		if (temp->data && temp->size > 0) {
+			memset(buf, 0, 100);
+			snprintf(buf, 99, "%s %s\n", addr, temp->data);
+			write(fd, buf, strlen(buf));
+		}
+	}
+
+unlock:
+	flock(fd, LOCK_UN);
+
+close:
+	close(fd);
+	list_free(list);
+	return err;
+}
+
 int write_link_key(const bdaddr_t *local, const bdaddr_t *peer, const unsigned char *key, const int type)
 {
 	struct list *temp, *list = NULL;