[PATCH BlueZ v4 03/14] meshd: Infrastructure for Mesh daemon

[Brian Gix <brian.gix@intel.com>]

---
 meshd/src/display.c         |  67 +++++
 meshd/src/hci.c             | 699 ++++++++++++++++++++++++++++++++++++++++++++
 meshd/src/mesh-io-generic.c | 660 +++++++++++++++++++++++++++++++++++++++++
 meshd/src/mesh-io.c         | 187 ++++++++++++
 4 files changed, 1613 insertions(+)
 create mode 100644 meshd/src/display.c
 create mode 100644 meshd/src/hci.c
 create mode 100644 meshd/src/mesh-io-generic.c
 create mode 100644 meshd/src/mesh-io.c

diff --git a/meshd/src/display.c b/meshd/src/display.c
new file mode 100644
index 000000000..adf92cae0
--- /dev/null
+++ b/meshd/src/display.c
@@ -0,0 +1,67 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2018  Intel Corporation. All rights reserved.
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#include "display.h"
+
+#define likely(x)   __builtin_expect(!!(x), 1)
+#define unlikely(x) __builtin_expect(!!(x), 0)
+
+static unsigned int cached_num_columns;
+
+unsigned int num_columns(void)
+{
+	struct winsize ws;
+
+	if (likely(cached_num_columns > 0))
+		return cached_num_columns;
+
+	if (ioctl(STDOUT_FILENO, TIOCGWINSZ, &ws) < 0 || ws.ws_col == 0)
+		cached_num_columns = 80;
+	else
+		cached_num_columns = ws.ws_col;
+
+	return cached_num_columns;
+}
+
+void print_packet(const char *label, const void *data, uint16_t size)
+{
+	struct timeval pkt_time;
+
+	gettimeofday(&pkt_time, NULL);
+
+	if (size > 0) {
+		char *str;
+
+		str = l_util_hexstring(data, size);
+		l_info("%05d.%03d %s: %s",
+				(uint32_t) pkt_time.tv_sec % 100000,
+				(uint32_t) pkt_time.tv_usec/1000, label, str);
+		l_free(str);
+	} else
+		l_info("%05d.%03d %s: empty",
+				(uint32_t) pkt_time.tv_sec % 100000,
+				(uint32_t) pkt_time.tv_usec/1000, label);
+}
diff --git a/meshd/src/hci.c b/meshd/src/hci.c
new file mode 100644
index 000000000..4e5a40d63
--- /dev/null
+++ b/meshd/src/hci.c
@@ -0,0 +1,699 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2018  Intel Corporation. All rights reserved.
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <sys/uio.h>
+#include <sys/socket.h>
+#include <ell/ell.h>
+
+#include "monitor/bt.h"
+
+#include "meshd/src/hci.h"
+
+#define BTPROTO_HCI	1
+struct sockaddr_hci {
+	sa_family_t	hci_family;
+	unsigned short	hci_dev;
+	unsigned short  hci_channel;
+};
+#define HCI_CHANNEL_USER	1
+
+struct bt_hci {
+	int ref_count;
+	struct l_io *io;
+	bool is_stream;
+	bool writer_active;
+	uint8_t num_cmds;
+	uint8_t num_pkts;
+	unsigned int next_cmd_id;
+	unsigned int next_evt_id;
+	struct l_queue *cmd_queue;
+	struct l_queue *rsp_queue;
+	struct l_queue *evt_list;
+	struct l_queue *pkt_queue;
+	bt_hci_receive_func_t receive_callback;
+	bt_hci_destroy_func_t receive_destroy;
+	void *receive_data;
+};
+
+struct cmd {
+	unsigned int id;
+	uint16_t opcode;
+	void *data;
+	uint8_t size;
+	bt_hci_callback_func_t callback;
+	bt_hci_destroy_func_t destroy;
+	void *user_data;
+};
+
+struct evt {
+	unsigned int id;
+	uint8_t event;
+	bt_hci_callback_func_t callback;
+	bt_hci_destroy_func_t destroy;
+	void *user_data;
+};
+
+struct pkt {
+	uint16_t handle;
+	uint8_t flags;
+	void *data;
+	uint16_t size;
+};
+
+static void cmd_free(void *data)
+{
+	struct cmd *cmd = data;
+
+	if (cmd->destroy)
+		cmd->destroy(cmd->user_data);
+
+	l_free(cmd->data);
+	l_free(cmd);
+}
+
+static void evt_free(void *data)
+{
+	struct evt *evt = data;
+
+	if (evt->destroy)
+		evt->destroy(evt->user_data);
+
+	l_free(evt);
+}
+
+static void pkt_free(void *data)
+{
+	struct pkt *pkt = data;
+
+	l_free(pkt->data);
+	l_free(pkt);
+}
+
+static void send_command(struct bt_hci *hci, uint16_t opcode,
+						void *data, uint8_t size)
+{
+	uint8_t type = BT_H4_CMD_PKT;
+	struct bt_hci_cmd_hdr hdr;
+	struct iovec iov[3];
+	int fd, iovcnt;
+
+	hdr.opcode = L_CPU_TO_LE16(opcode);
+	hdr.plen = size;
+
+	iov[0].iov_base = &type;
+	iov[0].iov_len  = 1;
+	iov[1].iov_base = &hdr;
+	iov[1].iov_len  = sizeof(hdr);
+
+	if (size > 0) {
+		iov[2].iov_base = data;
+		iov[2].iov_len  = size;
+		iovcnt = 3;
+	} else
+		iovcnt = 2;
+
+	fd = l_io_get_fd(hci->io);
+	if (fd < 0) {
+		l_error("hci->io Bad");
+		return;
+	}
+
+	if (writev(fd, iov, iovcnt) < 0) {
+		l_error("writev Bad");
+		return;
+	}
+
+	hci->num_cmds--;
+}
+
+static void send_packet(struct bt_hci *hci, uint16_t handle, uint8_t flags,
+						void *data, uint8_t size)
+{
+	uint8_t type = BT_H4_ACL_PKT;
+	struct bt_hci_acl_hdr hdr;
+	struct iovec iov[3];
+	int fd, iovcnt;
+
+	hdr.handle = L_CPU_TO_LE16(flags << 12 | handle);
+	hdr.dlen = L_CPU_TO_LE16(size);
+
+	iov[0].iov_base = &type;
+	iov[0].iov_len  = 1;
+	iov[1].iov_base = &hdr;
+	iov[1].iov_len  = sizeof(hdr);
+
+	if (size > 0) {
+		iov[2].iov_base = data;
+		iov[2].iov_len  = size;
+		iovcnt = 3;
+	} else
+		iovcnt = 2;
+
+	fd = l_io_get_fd(hci->io);
+	if (fd < 0) {
+		l_error("hci->io Bad");
+		return;
+	}
+
+	if (writev(fd, iov, iovcnt) < 0) {
+		l_error("writev Bad");
+		return;
+	}
+
+	hci->num_pkts--;
+}
+
+static bool io_write_callback(struct l_io *io, void *user_data)
+{
+	struct bt_hci *hci = user_data;
+	struct cmd *cmd;
+
+	if (hci->num_cmds > 0)
+		cmd = l_queue_pop_head(hci->cmd_queue);
+	else
+		cmd = NULL;
+
+	if (cmd) {
+		send_command(hci, cmd->opcode, cmd->data, cmd->size);
+		l_queue_push_tail(hci->rsp_queue, cmd);
+	} else {
+		struct pkt *pkt;
+
+		if (hci->num_pkts < 1)
+			goto done;
+
+		pkt = l_queue_pop_head(hci->pkt_queue);
+		if (pkt) {
+			send_packet(hci, pkt->handle, pkt->flags,
+							pkt->data, pkt->size);
+			pkt_free(pkt);
+		}
+	}
+
+	if (!l_queue_isempty(hci->pkt_queue))
+		return true;
+
+done:
+	hci->writer_active = false;
+
+	return false;
+}
+
+static void wakeup_writer(struct bt_hci *hci)
+{
+	if (hci->writer_active)
+		return;
+
+	if (l_queue_isempty(hci->cmd_queue) && l_queue_isempty(hci->pkt_queue))
+		return;
+
+	if (!l_io_set_write_handler(hci->io, io_write_callback, hci, NULL))
+		return;
+
+	hci->writer_active = true;
+}
+
+static bool match_cmd_opcode(const void *a, const void *b)
+{
+	const struct cmd *cmd = a;
+	uint16_t opcode = L_PTR_TO_UINT(b);
+
+	return cmd->opcode == opcode;
+}
+
+static void process_response(struct bt_hci *hci, uint16_t opcode,
+					const void *data, size_t size)
+{
+	struct cmd *cmd;
+
+	if (opcode == BT_HCI_CMD_NOP)
+		goto done;
+
+	cmd = l_queue_remove_if(hci->rsp_queue, match_cmd_opcode,
+						L_UINT_TO_PTR(opcode));
+	if (!cmd)
+		goto done;
+
+	if (cmd->callback)
+		cmd->callback(data, size, cmd->user_data);
+
+	cmd_free(cmd);
+
+done:
+	wakeup_writer(hci);
+}
+
+static void process_notify(void *data, void *user_data)
+{
+	struct bt_hci_evt_hdr *hdr = user_data;
+	struct evt *evt = data;
+
+	if (evt->event == hdr->evt)
+		evt->callback(user_data + sizeof(struct bt_hci_evt_hdr),
+						hdr->plen, evt->user_data);
+}
+
+static void process_event(struct bt_hci *hci, const void *data, size_t size)
+{
+	const struct bt_hci_evt_hdr *hdr = data;
+	const struct bt_hci_evt_cmd_complete *cc;
+	const struct bt_hci_evt_cmd_status *cs;
+	const struct bt_hci_evt_num_completed_packets *cp;
+
+	if (size < sizeof(struct bt_hci_evt_hdr))
+		return;
+
+	data += sizeof(struct bt_hci_evt_hdr);
+	size -= sizeof(struct bt_hci_evt_hdr);
+
+	if (hdr->plen != size)
+		return;
+
+	switch (hdr->evt) {
+	case BT_HCI_EVT_CMD_COMPLETE:
+		if (size < sizeof(*cc))
+			return;
+		cc = data;
+		hci->num_cmds = cc->ncmd;
+		process_response(hci, L_LE16_TO_CPU(cc->opcode),
+						data + sizeof(*cc),
+						size - sizeof(*cc));
+		break;
+
+	case BT_HCI_EVT_CMD_STATUS:
+		if (size < sizeof(*cs))
+			return;
+		cs = data;
+		hci->num_cmds = cs->ncmd;
+		process_response(hci, L_LE16_TO_CPU(cs->opcode),
+							&cs->status, 1);
+		break;
+
+	case BT_HCI_EVT_NUM_COMPLETED_PACKETS:
+		if (size < sizeof(*cp))
+			return;
+		cp = data;
+		l_debug("BT_HCI_EVT_NUM_COMPLETED_PACKETS:%d", cp->count);
+		/* Ignoring handle information for now */
+		hci->num_pkts = cp->count;
+		wakeup_writer(hci);
+		break;
+
+	default:
+		l_queue_foreach(hci->evt_list, process_notify, (void *) hdr);
+		break;
+	}
+}
+
+static void process_acl(struct bt_hci *hci, const void *data, size_t size)
+{
+	const struct bt_hci_acl_hdr *hdr = data;
+	uint16_t handle;
+
+	if (size < sizeof(struct bt_hci_acl_hdr))
+		return;
+
+	data += sizeof(struct bt_hci_acl_hdr);
+	size -= sizeof(struct bt_hci_acl_hdr);
+
+	if (L_LE16_TO_CPU(hdr->dlen) != size)
+		return;
+
+	handle = L_LE16_TO_CPU(hdr->handle);
+
+	if (hci->receive_callback)
+		hci->receive_callback(handle & 0x0fff, handle >> 12,
+					data, size, hci->receive_data);
+}
+
+static bool io_read_callback(struct l_io *io, void *user_data)
+{
+	struct bt_hci *hci = user_data;
+	uint8_t buf[512];
+	ssize_t len;
+	int fd;
+
+	fd = l_io_get_fd(hci->io);
+	if (fd < 0)
+		return false;
+
+	if (hci->is_stream)
+		return false;
+
+	len = read(fd, buf, sizeof(buf));
+	if (len < 0)
+		return false;
+
+	if (len < 1)
+		return true;
+
+	switch (buf[0]) {
+	case BT_H4_EVT_PKT:
+		process_event(hci, buf + 1, len - 1);
+		break;
+	case BT_H4_ACL_PKT:
+		process_acl(hci, buf + 1, len - 1);
+		break;
+	default:
+		l_info("%2.2x-RXed", buf[0]);
+	}
+
+	return true;
+}
+
+static struct bt_hci *create_hci(int fd)
+{
+	struct bt_hci *hci;
+
+	if (fd < 0)
+		return NULL;
+
+	hci = l_new(struct bt_hci, 1);
+
+	hci->io = l_io_new(fd);
+
+	hci->is_stream = true;
+	hci->writer_active = false;
+	hci->num_cmds = 1;
+	hci->num_pkts = 1;
+	hci->next_cmd_id = 1;
+	hci->next_evt_id = 1;
+
+	hci->cmd_queue = l_queue_new();
+	hci->rsp_queue = l_queue_new();
+	hci->evt_list = l_queue_new();
+	hci->pkt_queue = l_queue_new();
+
+	if (!l_io_set_read_handler(hci->io, io_read_callback, hci, NULL)) {
+		l_queue_destroy(hci->pkt_queue, NULL);
+		l_queue_destroy(hci->evt_list, NULL);
+		l_queue_destroy(hci->rsp_queue, NULL);
+		l_queue_destroy(hci->cmd_queue, NULL);
+		l_io_destroy(hci->io);
+		l_free(hci);
+		return NULL;
+	}
+
+	return bt_hci_ref(hci);
+}
+
+struct bt_hci *bt_hci_new(int fd)
+{
+	struct bt_hci *hci;
+
+	hci = create_hci(fd);
+	if (!hci)
+		return NULL;
+
+	return hci;
+}
+
+static int create_socket(uint16_t index, uint16_t channel)
+{
+	struct sockaddr_hci addr;
+	int fd;
+
+	fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
+								BTPROTO_HCI);
+	if (fd < 0)
+		return -1;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.hci_family = AF_BLUETOOTH;
+	addr.hci_dev = index;
+	addr.hci_channel = channel;
+
+	if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+struct bt_hci *bt_hci_new_user_channel(uint16_t index)
+{
+	struct bt_hci *hci;
+	int fd;
+
+	fd = create_socket(index, HCI_CHANNEL_USER);
+	if (fd < 0)
+		return NULL;
+
+	hci = create_hci(fd);
+	if (!hci) {
+		close(fd);
+		return NULL;
+	}
+
+	hci->is_stream = false;
+
+	bt_hci_set_close_on_unref(hci, true);
+
+	return hci;
+}
+
+struct bt_hci *bt_hci_ref(struct bt_hci *hci)
+{
+	if (!hci)
+		return NULL;
+
+	__sync_fetch_and_add(&hci->ref_count, 1);
+
+	return hci;
+}
+
+void bt_hci_unref(struct bt_hci *hci)
+{
+	if (!hci)
+		return;
+
+	if (__sync_sub_and_fetch(&hci->ref_count, 1))
+		return;
+
+	if (hci->receive_destroy)
+		hci->receive_destroy(hci->receive_data);
+
+	l_queue_destroy(hci->pkt_queue, pkt_free);
+	l_queue_destroy(hci->evt_list, evt_free);
+	l_queue_destroy(hci->cmd_queue, cmd_free);
+	l_queue_destroy(hci->rsp_queue, cmd_free);
+
+	l_io_destroy(hci->io);
+
+	l_free(hci);
+}
+
+bool bt_hci_set_close_on_unref(struct bt_hci *hci, bool do_close)
+{
+	if (!hci)
+		return false;
+
+	return l_io_set_close_on_destroy(hci->io, do_close);
+}
+
+unsigned int bt_hci_send(struct bt_hci *hci, uint16_t opcode,
+				const void *data, uint8_t size,
+				bt_hci_callback_func_t callback,
+				void *user_data, bt_hci_destroy_func_t destroy)
+{
+	struct cmd *cmd;
+
+	if (!hci)
+		return 0;
+
+	cmd = l_new(struct cmd, 1);
+
+	cmd->opcode = opcode;
+	cmd->size = size;
+
+	if (cmd->size > 0)
+		cmd->data = l_memdup(data, cmd->size);
+
+	if (hci->next_cmd_id < 1)
+		hci->next_cmd_id = 1;
+
+	cmd->id = hci->next_cmd_id++;
+
+	cmd->callback = callback;
+	cmd->destroy = destroy;
+	cmd->user_data = user_data;
+
+	if (!l_queue_push_tail(hci->cmd_queue, cmd)) {
+		l_free(cmd->data);
+		l_free(cmd);
+		return 0;
+	}
+
+	wakeup_writer(hci);
+
+	return cmd->id;
+}
+
+static bool match_cmd_id(const void *a, const void *b)
+{
+	const struct cmd *cmd = a;
+	unsigned int id = L_PTR_TO_UINT(b);
+
+	return cmd->id == id;
+}
+
+bool bt_hci_cancel(struct bt_hci *hci, unsigned int id)
+{
+	struct cmd *cmd;
+
+	if (!hci || !id)
+		return false;
+
+	cmd = l_queue_remove_if(hci->cmd_queue, match_cmd_id,
+						L_UINT_TO_PTR(id));
+	if (!cmd) {
+		cmd = l_queue_remove_if(hci->rsp_queue, match_cmd_id,
+							L_UINT_TO_PTR(id));
+		if (!cmd)
+			return false;
+	}
+
+	cmd_free(cmd);
+
+	wakeup_writer(hci);
+
+	return true;
+}
+
+bool bt_hci_flush(struct bt_hci *hci)
+{
+	if (!hci)
+		return false;
+
+	if (hci->writer_active) {
+		l_io_set_write_handler(hci->io, NULL, NULL, NULL);
+		hci->writer_active = false;
+	}
+
+	l_queue_clear(hci->cmd_queue, cmd_free);
+	l_queue_clear(hci->rsp_queue, cmd_free);
+
+	return true;
+}
+
+unsigned int bt_hci_register(struct bt_hci *hci, uint8_t event,
+				bt_hci_callback_func_t callback,
+				void *user_data, bt_hci_destroy_func_t destroy)
+{
+	struct evt *evt;
+
+	if (!hci)
+		return 0;
+
+	evt = l_new(struct evt, 1);
+
+	evt->event = event;
+
+	if (hci->next_evt_id < 1)
+		hci->next_evt_id = 1;
+
+	evt->id = hci->next_evt_id++;
+
+	evt->callback = callback;
+	evt->destroy = destroy;
+	evt->user_data = user_data;
+
+	if (!l_queue_push_tail(hci->evt_list, evt)) {
+		l_free(evt);
+		return 0;
+	}
+
+	return evt->id;
+}
+
+static bool match_evt_id(const void *a, const void *b)
+{
+	const struct evt *evt = a;
+	unsigned int id = L_PTR_TO_UINT(b);
+
+	return evt->id == id;
+}
+
+bool bt_hci_unregister(struct bt_hci *hci, unsigned int id)
+{
+	struct evt *evt;
+
+	if (!hci || !id)
+		return false;
+
+	evt = l_queue_remove_if(hci->evt_list, match_evt_id,
+						L_UINT_TO_PTR(id));
+	if (!evt)
+		return false;
+
+	evt_free(evt);
+
+	return true;
+}
+
+bool bt_hci_receive(struct bt_hci *hci, bt_hci_receive_func_t callback,
+				void *user_data, bt_hci_destroy_func_t destroy)
+{
+	if (!hci)
+		return false;
+
+	if (hci->receive_destroy)
+		hci->receive_destroy(hci->receive_data);
+
+	hci->receive_callback = callback;
+	hci->receive_destroy = destroy;
+	hci->receive_data = user_data;
+
+	return true;
+}
+
+bool bt_hci_write(struct bt_hci *hci, uint16_t handle, uint8_t flags,
+				const void *data, uint16_t size)
+{
+	struct pkt *pkt;
+
+	if (!hci)
+		return false;
+
+	pkt = l_new(struct pkt, 1);
+
+	pkt->handle = handle;
+	pkt->flags = flags;
+	pkt->size = size;
+
+	if (pkt->size > 0)
+		pkt->data = l_memdup(data, pkt->size);
+
+	if (!l_queue_push_tail(hci->pkt_queue, pkt)) {
+		l_free(pkt->data);
+		l_free(pkt);
+		return false;
+	}
+
+	wakeup_writer(hci);
+
+	return true;
+}
diff --git a/meshd/src/mesh-io-generic.c b/meshd/src/mesh-io-generic.c
new file mode 100644
index 000000000..e5042bb79
--- /dev/null
+++ b/meshd/src/mesh-io-generic.c
@@ -0,0 +1,660 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2018  Intel Corporation. All rights reserved.
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
+
+#include "monitor/bt.h"
+
+#include "meshd/src/hci.h"
+#include "meshd/src/display.h"
+#include "meshd/src/mesh-io.h"
+#include "meshd/src/mesh-io-api.h"
+
+#include "meshd/src/mesh-io-generic.h"
+
+struct mesh_io_private {
+	uint16_t index;
+	struct bt_hci *hci;
+	struct l_timeout *tx_timeout;
+	struct l_queue *rx_regs;
+	struct l_queue *tx_pkts;
+	uint8_t filters[3]; /* Simple filtering on AD type only */
+	bool sending;
+	struct tx_pkt *tx;
+	uint16_t interval;
+};
+
+struct pvt_rx_reg {
+	uint8_t filter_id;
+	mesh_io_recv_func_t cb;
+	void *user_data;
+};
+
+struct process_data {
+	struct mesh_io_private		*pvt;
+	const uint8_t			*data;
+	uint8_t				len;
+	struct mesh_io_recv_info	info;
+};
+
+struct tx_pkt {
+	struct mesh_io_send_info	info;
+	bool				delete;
+	uint8_t				len;
+	uint8_t				pkt[30];
+};
+
+struct tx_pattern {
+	const uint8_t			*data;
+	uint8_t				len;
+};
+
+static uint32_t get_instant(void)
+{
+	struct timeval tm;
+	uint32_t instant;
+
+	gettimeofday(&tm, NULL);
+	instant = tm.tv_sec * 1000;
+	instant += tm.tv_usec / 1000;
+
+	return instant;
+}
+
+static uint32_t instant_remaining_ms(uint32_t instant)
+{
+	instant -= get_instant();
+	return instant;
+}
+
+static void process_rx_callbacks(void *v_rx, void *v_reg)
+{
+	struct pvt_rx_reg *rx_reg = v_rx;
+	struct process_data *rx = v_reg;
+	uint8_t ad_type;
+
+	ad_type = rx->pvt->filters[rx_reg->filter_id - 1];
+
+	if (rx->data[0] == ad_type && rx_reg->cb)
+		rx_reg->cb(rx_reg->user_data, &rx->info, rx->data, rx->len);
+}
+
+static void process_rx(struct mesh_io_private *pvt, int8_t rssi,
+					uint32_t instant,
+					const uint8_t *data, uint8_t len)
+{
+	struct process_data rx = {
+		.pvt = pvt,
+		.data = data,
+		.len = len,
+		.info.instant = instant,
+		.info.chan = 7,
+		.info.rssi = rssi,
+	};
+
+	l_queue_foreach(pvt->rx_regs, process_rx_callbacks, &rx);
+}
+
+static void event_adv_report(struct mesh_io *io, const void *buf, uint8_t size)
+{
+	const struct bt_hci_evt_le_adv_report *evt = buf;
+	const uint8_t *adv;
+	uint32_t instant;
+	uint8_t adv_len;
+	uint16_t len = 0;
+	int8_t rssi;
+
+	if (evt->event_type != 0x03)
+		return;
+
+	if (evt->addr_type != BDADDR_LE_PUBLIC &&
+			evt->addr_type != BDADDR_LE_RANDOM)
+		return;
+
+	instant = get_instant();
+	adv = evt->data;
+	adv_len = evt->data_len;
+
+	/* rssi is just beyond last byte of data */
+	rssi = (int8_t) adv[adv_len];
+
+	while (len < adv_len - 1) {
+		uint8_t field_len = adv[0];
+
+		/* Check for the end of advertising data */
+		if (field_len == 0)
+			break;
+
+		len += field_len + 1;
+
+		/* Do not continue data parsing if got incorrect length */
+		if (len > adv_len)
+			break;
+
+		/* TODO: Create an Instant to use */
+		process_rx(io->pvt, rssi, instant, adv + 1, adv[0]);
+
+		adv += field_len + 1;
+	}
+}
+
+static void event_callback(const void *buf, uint8_t size, void *user_data)
+{
+	uint8_t event = l_get_u8(buf);
+	struct mesh_io *io = user_data;
+
+	switch (event) {
+	case BT_HCI_EVT_LE_ADV_REPORT:
+		event_adv_report(io, buf + 1, size - 1);
+		break;
+
+	default:
+		l_info("Other Meta Evt - %d", event);
+	}
+}
+
+static bool dev_init(uint16_t index, struct mesh_io *io)
+{
+	struct mesh_io_private *tmp;
+
+	if (!io || io->pvt)
+		return false;
+
+	tmp = l_new(struct mesh_io_private, 1);
+
+	if (tmp == NULL)
+		return false;
+
+	tmp->rx_regs = l_queue_new();
+	tmp->tx_pkts = l_queue_new();
+	if (!tmp->rx_regs || !tmp->tx_pkts)
+		goto fail;
+
+	tmp->hci = bt_hci_new_user_channel(index);
+	if (!tmp->hci)
+		goto fail;
+
+	bt_hci_register(tmp->hci, BT_HCI_EVT_LE_META_EVENT,
+						event_callback, io, NULL);
+
+	io->pvt = tmp;
+	return true;
+
+fail:
+	l_queue_destroy(tmp->rx_regs, l_free);
+	l_queue_destroy(tmp->tx_pkts, l_free);
+	l_free(tmp);
+	return false;
+}
+
+static bool dev_destroy(struct mesh_io *io)
+{
+	struct mesh_io_private *pvt = io->pvt;
+
+	if (!pvt)
+		return true;
+
+	bt_hci_unref(pvt->hci);
+	l_timeout_remove(pvt->tx_timeout);
+	l_queue_destroy(pvt->rx_regs, l_free);
+	l_queue_destroy(pvt->tx_pkts, l_free);
+	l_free(pvt);
+	io->pvt = NULL;
+
+	return true;
+}
+
+static bool dev_caps(struct mesh_io *io, struct mesh_io_caps *caps)
+{
+	struct mesh_io_private *pvt = io->pvt;
+
+	if (!pvt || !caps)
+		return false;
+
+	caps->max_num_filters = sizeof(pvt->filters);
+	caps->window_accuracy = 50;
+
+	return true;
+}
+
+static void send_cancel_done(const void *buf, uint8_t size,
+							void *user_data)
+{
+	struct mesh_io_private *pvt = user_data;
+	struct bt_hci_cmd_le_set_random_address cmd;
+
+	if (!pvt)
+		return;
+
+	pvt->sending = false;
+
+	/* At end of any burst of ADVs, change random address */
+	l_getrandom(cmd.addr, 6);
+	cmd.addr[5] |= 0xc0;
+	bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
+				&cmd, sizeof(cmd), NULL, NULL, NULL);
+}
+
+static void send_cancel(struct mesh_io_private *pvt)
+{
+	struct bt_hci_cmd_le_set_adv_enable cmd;
+
+	if (!pvt)
+		return;
+
+	if (!pvt->sending) {
+		send_cancel_done(NULL, 0, pvt);
+		return;
+	}
+
+	cmd.enable = 0x00;	/* Disable advertising */
+	bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
+				&cmd, sizeof(cmd),
+				send_cancel_done, pvt, NULL);
+}
+
+static void set_send_adv_enable(const void *buf, uint8_t size,
+							void *user_data)
+{
+	struct mesh_io_private *pvt = user_data;
+	struct bt_hci_cmd_le_set_adv_enable cmd;
+
+	if (!pvt)
+		return;
+
+	pvt->sending = true;
+	cmd.enable = 0x01;	/* Enable advertising */
+	bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
+				&cmd, sizeof(cmd), NULL, NULL, NULL);
+}
+
+static void set_send_adv_data(const void *buf, uint8_t size,
+							void *user_data)
+{
+	struct mesh_io_private *pvt = user_data;
+	struct tx_pkt *tx;
+	struct bt_hci_cmd_le_set_adv_data cmd;
+
+	if (!pvt || !pvt->tx)
+		return;
+
+	tx = pvt->tx;
+	if (tx->len >= sizeof(cmd.data))
+		goto done;
+
+	memset(&cmd, 0, sizeof(cmd));
+
+	cmd.len = tx->len + 1;
+	cmd.data[0] = tx->len;
+	memcpy(cmd.data + 1, tx->pkt, tx->len);
+
+	bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_DATA,
+					&cmd, sizeof(cmd),
+					set_send_adv_enable, pvt, NULL);
+done:
+	if (tx->delete)
+		l_free(tx);
+
+	pvt->tx = NULL;
+}
+
+static void set_send_adv_params(const void *buf, uint8_t size,
+							void *user_data)
+{
+	struct mesh_io_private *pvt = user_data;
+	struct bt_hci_cmd_le_set_adv_parameters cmd;
+	uint16_t hci_interval;
+
+	if (!pvt)
+		return;
+
+	hci_interval = (pvt->interval * 16) / 10;
+	cmd.min_interval = L_CPU_TO_LE16(hci_interval);
+	cmd.max_interval = L_CPU_TO_LE16(hci_interval);
+	cmd.type = 0x03; /* ADV_NONCONN_IND */
+	cmd.own_addr_type = 0x01; /* ADDR_TYPE_RANDOM */
+	cmd.direct_addr_type = 0x00;
+	memset(cmd.direct_addr, 0, 6);
+	cmd.channel_map = 0x07;
+	cmd.filter_policy = 0x03;
+
+	bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
+				&cmd, sizeof(cmd),
+				set_send_adv_data, pvt, NULL);
+}
+
+static void send_pkt(struct mesh_io_private *pvt, struct tx_pkt *tx,
+							uint16_t interval)
+{
+	struct bt_hci_cmd_le_set_adv_enable cmd;
+
+	pvt->tx = tx;
+	pvt->interval = interval;
+
+	if (!pvt->sending) {
+		set_send_adv_params(NULL, 0, pvt);
+		return;
+	}
+
+	cmd.enable = 0x00;	/* Disable advertising */
+	bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
+				&cmd, sizeof(cmd),
+				set_send_adv_params, pvt, NULL);
+}
+
+static void tx_timeout(struct l_timeout *timeout, void *user_data)
+{
+	struct mesh_io_private *pvt = user_data;
+	struct tx_pkt *tx;
+	uint16_t ms;
+	uint8_t count;
+
+	if (!pvt)
+		return;
+
+	tx = l_queue_pop_head(pvt->tx_pkts);
+	if (!tx) {
+		l_timeout_remove(timeout);
+		pvt->tx_timeout = NULL;
+		send_cancel(pvt);
+		return;
+	}
+
+	if (tx->info.type == MESH_IO_TIMING_TYPE_GENERAL) {
+		ms = tx->info.u.gen.interval;
+		count = tx->info.u.gen.cnt;
+		if (count != MESH_IO_TX_COUNT_UNLIMITED)
+			tx->info.u.gen.cnt--;
+	} else {
+		ms = 25;
+		count = 1;
+	}
+
+	tx->delete = !!(count == 1);
+
+	send_pkt(pvt, tx, ms);
+
+	if (count == 1) {
+		/* send_pkt will delete when done */
+		tx = l_queue_peek_head(pvt->tx_pkts);
+		if (tx && tx->info.type == MESH_IO_TIMING_TYPE_POLL_RSP) {
+			ms = instant_remaining_ms(tx->info.u.poll_rsp.instant +
+						tx->info.u.poll_rsp.delay);
+		}
+	} else
+		l_queue_push_tail(pvt->tx_pkts, tx);
+
+	if (timeout) {
+		pvt->tx_timeout = timeout;
+		l_timeout_modify_ms(timeout, ms);
+	} else
+		pvt->tx_timeout = l_timeout_create_ms(ms, tx_timeout,
+								pvt, NULL);
+}
+
+static void tx_worker(void *user_data)
+{
+	struct mesh_io_private *pvt = user_data;
+	struct tx_pkt *tx;
+	uint32_t delay;
+
+	tx = l_queue_peek_head(pvt->tx_pkts);
+	if (!tx)
+		return;
+
+	switch (tx->info.type) {
+	case MESH_IO_TIMING_TYPE_GENERAL:
+		if (tx->info.u.gen.min_delay == tx->info.u.gen.max_delay)
+			delay = tx->info.u.gen.min_delay;
+		else {
+			l_getrandom(&delay, sizeof(delay));
+			delay %= tx->info.u.gen.max_delay -
+						tx->info.u.gen.min_delay;
+			delay += tx->info.u.gen.min_delay;
+		}
+		break;
+
+	case MESH_IO_TIMING_TYPE_POLL:
+		if (tx->info.u.poll.min_delay == tx->info.u.poll.max_delay)
+			delay = tx->info.u.poll.min_delay;
+		else {
+			l_getrandom(&delay, sizeof(delay));
+			delay %= tx->info.u.poll.max_delay -
+						tx->info.u.poll.min_delay;
+			delay += tx->info.u.poll.min_delay;
+		}
+		break;
+
+	case MESH_IO_TIMING_TYPE_POLL_RSP:
+		/* Delay until Instant + Delay */
+		delay = instant_remaining_ms(tx->info.u.poll_rsp.instant +
+						tx->info.u.poll_rsp.delay);
+		if (delay > 255)
+			delay = 0;
+		break;
+
+	default:
+		return;
+	}
+
+	if (!delay)
+		tx_timeout(pvt->tx_timeout, pvt);
+	else if (pvt->tx_timeout)
+		l_timeout_modify_ms(pvt->tx_timeout, delay);
+	else
+		pvt->tx_timeout = l_timeout_create_ms(delay, tx_timeout,
+								pvt, NULL);
+}
+
+static bool send_tx(struct mesh_io *io, struct mesh_io_send_info *info,
+					const uint8_t *data, uint16_t len)
+{
+	struct mesh_io_private *pvt = io->pvt;
+	struct tx_pkt *tx;
+	bool sending = false;
+
+	if (!info || !data || !len || len > sizeof(tx->pkt))
+		return false;
+
+
+	tx = l_new(struct tx_pkt, 1);
+	if (!tx)
+		return false;
+
+	memcpy(&tx->info, info, sizeof(tx->info));
+	memcpy(&tx->pkt, data, len);
+	tx->len = len;
+
+	if (info->type == MESH_IO_TIMING_TYPE_POLL_RSP)
+		l_queue_push_head(pvt->tx_pkts, tx);
+	else {
+		sending = !l_queue_isempty(pvt->tx_pkts);
+		l_queue_push_tail(pvt->tx_pkts, tx);
+	}
+
+	if (!sending) {
+		l_timeout_remove(pvt->tx_timeout);
+		pvt->tx_timeout = NULL;
+		l_idle_oneshot(tx_worker, pvt, NULL);
+	}
+
+	return true;
+}
+
+static bool find_by_ad_type(const void *a, const void *b)
+{
+	const struct tx_pkt *tx = a;
+	uint8_t ad_type = L_PTR_TO_UINT(b);
+
+	return !ad_type || ad_type == tx->pkt[0];
+}
+
+static bool find_by_pattern(const void *a, const void *b)
+{
+	const struct tx_pkt *tx = a;
+	const struct tx_pattern *pattern = b;
+
+	if (tx->len < pattern->len)
+		return false;
+
+	return (!memcmp(tx->pkt, pattern->data, pattern->len));
+}
+
+static bool tx_cancel(struct mesh_io *io, uint8_t *data, uint8_t len)
+{
+	struct mesh_io_private *pvt = io->pvt;
+	struct tx_pkt *tx;
+
+	if (!data)
+		return false;
+
+	if (len == 1) {
+		do {
+			tx = l_queue_remove_if(pvt->tx_pkts, find_by_ad_type,
+							L_UINT_TO_PTR(data[0]));
+			l_free(tx);
+		} while (tx);
+	}  else {
+		struct tx_pattern pattern = {
+			.data = data,
+			.len = len
+		};
+
+		do {
+			tx = l_queue_remove_if(pvt->tx_pkts, find_by_pattern,
+								&pattern);
+			l_free(tx);
+		} while (tx);
+	}
+
+	if (l_queue_isempty(pvt->tx_pkts)) {
+		send_cancel(pvt);
+		l_timeout_remove(pvt->tx_timeout);
+		pvt->tx_timeout = NULL;
+	}
+
+	return true;
+}
+
+static bool find_by_filter_id(const void *a, const void *b)
+{
+	const struct pvt_rx_reg *rx_reg = a;
+	uint8_t filter_id = L_PTR_TO_UINT(b);
+
+	return rx_reg->filter_id == filter_id;
+}
+
+static bool recv_register(struct mesh_io *io, uint8_t filter_id,
+				mesh_io_recv_func_t cb, void *user_data)
+{
+	struct bt_hci_cmd_le_set_scan_enable cmd;
+	struct mesh_io_private *pvt = io->pvt;
+	struct pvt_rx_reg *rx_reg;
+	bool scanning;
+
+	l_info("%s %d", __func__, filter_id);
+	if (!cb || !filter_id || filter_id > sizeof(pvt->filters))
+		return false;
+
+	rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter_id,
+						L_UINT_TO_PTR(filter_id));
+
+	if (!rx_reg) {
+		rx_reg = l_new(struct pvt_rx_reg, 1);
+		if (!rx_reg)
+			return false;
+	}
+
+	rx_reg->filter_id = filter_id;
+	rx_reg->cb = cb;
+	rx_reg->user_data = user_data;
+
+	scanning = !l_queue_isempty(pvt->rx_regs);
+
+	l_queue_push_head(pvt->rx_regs, rx_reg);
+
+	if (!scanning) {
+		cmd.enable = 0x01;	/* Enable scanning */
+		cmd.filter_dup = 0x00;	/* Report duplicates */
+		bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
+				&cmd, sizeof(cmd), NULL, NULL, NULL);
+	}
+
+	return true;
+}
+
+static bool recv_deregister(struct mesh_io *io, uint8_t filter_id)
+{
+	struct bt_hci_cmd_le_set_scan_enable cmd;
+	struct mesh_io_private *pvt = io->pvt;
+
+	struct pvt_rx_reg *rx_reg;
+
+	rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter_id,
+						L_UINT_TO_PTR(filter_id));
+
+	if (rx_reg)
+		l_free(rx_reg);
+
+	if (l_queue_isempty(pvt->rx_regs)) {
+		cmd.enable = 0x00;	/* Disable scanning */
+		cmd.filter_dup = 0x00;	/* Report duplicates */
+		bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
+				&cmd, sizeof(cmd), NULL, NULL, NULL);
+
+	}
+
+	return true;
+}
+
+static bool filter_set(struct mesh_io *io,
+		uint8_t filter_id, const uint8_t *data, uint8_t len,
+		mesh_io_status_func_t callback, void *user_data)
+{
+	struct mesh_io_private *pvt = io->pvt;
+
+	l_info("%s id: %d, --> %2.2x", __func__, filter_id, data[0]);
+	if (!data || !len || !filter_id || filter_id > sizeof(pvt->filters))
+		return false;
+
+	pvt->filters[filter_id - 1] = data[0];
+
+	/* TODO: Delayed Call to successful status */
+
+	return true;
+}
+
+const struct mesh_io_api mesh_io_generic = {
+	.init = dev_init,
+	.destroy = dev_destroy,
+	.caps = dev_caps,
+	.send = send_tx,
+	.reg = recv_register,
+	.dereg = recv_deregister,
+	.set = filter_set,
+	.cancel = tx_cancel,
+};
diff --git a/meshd/src/mesh-io.c b/meshd/src/mesh-io.c
new file mode 100644
index 000000000..0f6fae933
--- /dev/null
+++ b/meshd/src/mesh-io.c
@@ -0,0 +1,187 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2018  Intel Corporation. All rights reserved.
+ *
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <ell/ell.h>
+
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
+
+#include "meshd/common/mesh-defs.h"
+
+#include "meshd/src/mesh-io.h"
+#include "meshd/src/mesh-io-api.h"
+
+/* List of Mesh-IO Type headers */
+#include "meshd/src/mesh-io-generic.h"
+
+/* List of Supported Mesh-IO Types */
+static const struct mesh_io_table table[] = {
+	{MESH_IO_TYPE_GENERIC,	&mesh_io_generic},
+};
+
+static struct l_queue *io_list;
+
+static bool match_by_io(const void *a, const void *b)
+{
+	return a == b;
+}
+
+static bool match_by_index(const void *a, const void *b)
+{
+	const struct mesh_io *io = a;
+
+	return io->index == L_PTR_TO_UINT(b);
+}
+
+struct mesh_io *mesh_io_new(uint16_t index, enum mesh_io_type type)
+{
+	const struct mesh_io_api	*api = NULL;
+	struct mesh_io			*io;
+	uint16_t			i;
+
+	l_info("%s %d\n", __func__, type);
+	for (i = 0; i < L_ARRAY_SIZE(table); i++) {
+		if (table[i].type == type) {
+			api = table[i].api;
+			break;
+		}
+	}
+
+	io = l_queue_find(io_list, match_by_index, L_UINT_TO_PTR(index));
+
+	if (api == NULL || api->init == NULL || io != NULL)
+		return NULL;
+
+	io = l_new(struct mesh_io, 1);
+
+	if (io == NULL)
+		return NULL;
+
+	io->type = type;
+	io->index = index;
+	io->api = api;
+	if (!api->init(index, io))
+		goto fail;
+
+	if (io_list == NULL)
+		io_list = l_queue_new();
+
+	if (api->set) {
+		uint8_t pkt = MESH_AD_TYPE_NETWORK;
+		uint8_t bec = MESH_AD_TYPE_BEACON;
+		uint8_t prv = MESH_AD_TYPE_PROVISION;
+
+		api->set(io, 1, &bec, 1, NULL, NULL);
+		api->set(io, 2, &prv, 1, NULL, NULL);
+		api->set(io, 3, &pkt, 1, NULL, NULL);
+	}
+
+	if (l_queue_push_head(io_list, io))
+		return io;
+
+fail:
+	if (api->destroy)
+		api->destroy(io);
+
+	l_free(io);
+	return NULL;
+}
+
+void mesh_io_destroy(struct mesh_io *io)
+{
+	io = l_queue_remove_if(io_list, match_by_io, io);
+
+	if (io && io->api && io->api->destroy)
+		io->api->destroy(io);
+
+	l_free(io);
+
+	if (l_queue_isempty(io_list)) {
+		l_queue_destroy(io_list, NULL);
+		io_list = NULL;
+	}
+}
+
+bool mesh_io_get_caps(struct mesh_io *io, struct mesh_io_caps *caps)
+{
+	io = l_queue_find(io_list, match_by_io, io);
+
+	if (io && io->api && io->api->caps)
+		return io->api->caps(io, caps);
+
+	return false;
+}
+
+bool mesh_io_register_recv_cb(struct mesh_io *io, uint8_t filter_id,
+				mesh_io_recv_func_t cb, void *user_data)
+{
+	io = l_queue_find(io_list, match_by_io, io);
+
+	if (io && io->api && io->api->reg)
+		return io->api->reg(io, filter_id, cb, user_data);
+
+	return false;
+}
+
+bool mesh_io_deregister_recv_cb(struct mesh_io *io, uint8_t filter_id)
+{
+	io = l_queue_find(io_list, match_by_io, io);
+
+	if (io && io->api && io->api->dereg)
+		return io->api->dereg(io, filter_id);
+
+	return false;
+}
+
+bool mesh_set_filter(struct mesh_io *io, uint8_t filter_id,
+				const uint8_t *data, uint8_t len,
+				mesh_io_status_func_t cb, void *user_data)
+{
+	io = l_queue_find(io_list, match_by_io, io);
+
+	if (io && io->api && io->api->set)
+		return io->api->set(io, filter_id, data, len, cb, user_data);
+
+	return false;
+}
+
+bool mesh_io_send(struct mesh_io *io, struct mesh_io_send_info *info,
+					const uint8_t *data, uint16_t len)
+{
+	io = l_queue_find(io_list, match_by_io, io);
+
+	if (io && io->api && io->api->send)
+		return io->api->send(io, info, data, len);
+
+	return false;
+}
+
+bool mesh_io_send_cancel(struct mesh_io *io, uint8_t *pattern, uint8_t len)
+{
+	io = l_queue_find(io_list, match_by_io, io);
+
+	if (io && io->api && io->api->cancel)
+		return io->api->cancel(io, pattern, len);
+
+	return false;
+}
-- 
2.14.3