peripheral: Add initial code for btsensor application

11 files changed, 1589 insertions, 0 deletions

diff --git a/peripheral/attach.c b/peripheral/attach.c
new file mode 100644
index 000000000..a70ca55c7
--- /dev/null
+++ b/peripheral/attach.c
@@ -0,0 +1,144 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
+#include "lib/hci_lib.h"
+#include "tools/hciattach.h"
+#include "peripheral/attach.h"
+
+static const char *serial_dev = "/dev/ttyS1";
+static int serial_fd = -1;
+
+static int open_serial(const char *path)
+{
+	struct termios ti;
+	int fd, saved_ldisc, ldisc = N_HCI;
+
+	fd = open(path, O_RDWR | O_NOCTTY);
+	if (fd < 0) {
+		perror("Failed to open serial port");
+		return -1;
+	}
+
+	if (tcflush(fd, TCIOFLUSH) < 0) {
+		perror("Failed to flush serial port");
+		close(fd);
+		return -1;
+	}
+
+	if (ioctl(fd, TIOCGETD, &saved_ldisc) < 0) {
+		perror("Failed get serial line discipline");
+		close(fd);
+		return -1;
+	}
+
+	/* Switch TTY to raw mode */
+	memset(&ti, 0, sizeof(ti));
+	cfmakeraw(&ti);
+
+	ti.c_cflag |= (B115200 | CLOCAL | CREAD);
+
+	/* Set flow control */
+	ti.c_cflag |= CRTSCTS;
+
+	if (tcsetattr(fd, TCSANOW, &ti) < 0) {
+		perror("Failed to set serial port settings");
+		close(fd);
+		return -1;
+	}
+
+	if (ioctl(fd, TIOCSETD, &ldisc) < 0) {
+		perror("Failed set serial line discipline");
+		close(fd);
+		return -1;
+	}
+
+	printf("Switched line discipline from %d to %d\n", saved_ldisc, ldisc);
+
+	return fd;
+}
+
+static int attach_proto(const char *path, unsigned int proto,
+						unsigned int flags)
+{
+	int fd, dev_id;
+
+	fd = open_serial(path);
+	if (fd < 0)
+		return -1;
+
+	if (ioctl(fd, HCIUARTSETFLAGS, flags) < 0) {
+		perror("Failed to set flags");
+		close(fd);
+		return -1;
+	}
+
+	if (ioctl(fd, HCIUARTSETPROTO, proto) < 0) {
+		perror("Failed to set protocol");
+		close(fd);
+		return -1;
+	}
+
+	dev_id = ioctl(fd, HCIUARTGETDEVICE);
+	if (dev_id < 0) {
+		perror("Failed to get device id");
+		close(fd);
+		return -1;
+	}
+
+	printf("Device index %d attached\n", dev_id);
+
+	return fd;
+}
+
+void attach_start(void)
+{
+	unsigned long flags;
+
+	if (serial_fd >= 0)
+		return;
+
+	printf("Attaching BR/EDR controller to %s\n", serial_dev);
+
+	flags = (1 << HCI_UART_RESET_ON_INIT);
+
+	serial_fd = attach_proto(serial_dev, HCI_UART_H4, flags);
+}
+
+void attach_stop(void)
+{
+	if (serial_fd < 0)
+		return;
+
+	close(serial_fd);
+	serial_fd = -1;
+}
diff --git a/peripheral/attach.h b/peripheral/attach.h
new file mode 100644
index 000000000..f76e2fba2
--- /dev/null
+++ b/peripheral/attach.h
@@ -0,0 +1,25 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+void attach_start(void);
+void attach_stop(void);
diff --git a/peripheral/efivars.c b/peripheral/efivars.c
new file mode 100644
index 000000000..a86031fbd
--- /dev/null
+++ b/peripheral/efivars.c
@@ -0,0 +1,132 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/param.h>
+#include <sys/uio.h>
+
+#include "peripheral/efivars.h"
+
+#define SYSFS_EFIVARS "/sys/firmware/efi/efivars"
+
+typedef struct {
+    uint32_t data1;
+    uint16_t data2;
+    uint16_t data3;
+    uint8_t  data4[8];
+} efi_guid_t;
+
+#define VENDOR_GUID \
+	(efi_guid_t) { 0xd5f9d775, 0x1a09, 0x4e89, \
+			{ 0x96, 0xcf, 0x1d, 0x19, 0x55, 0x4d, 0xa6, 0x67 } }
+
+static void efivars_pathname(const char *name, char *pathname, size_t size)
+{
+	static efi_guid_t guid = VENDOR_GUID;
+
+	snprintf(pathname, size - 1,
+		"%s/%s-%08x-%04x-%04x-%02x%02x-%02x%02x%02x%02x%02x%02x",
+		SYSFS_EFIVARS, name, guid.data1, guid.data2, guid.data3,
+		guid.data4[0], guid.data4[1], guid.data4[2], guid.data4[3],
+		guid.data4[4], guid.data4[5], guid.data4[6], guid.data4[7]);
+}
+
+int efivars_read(const char *name, uint32_t *attributes,
+					void *data, size_t size)
+{
+	char pathname[PATH_MAX];
+	struct iovec iov[2];
+	uint32_t attr;
+	ssize_t len;
+	int fd;
+
+	efivars_pathname(name, pathname, PATH_MAX);
+
+	fd = open(pathname, O_RDONLY | O_CLOEXEC);
+	if (fd < 0)
+		return -EIO;
+
+	iov[0].iov_base = &attr;
+	iov[0].iov_len = sizeof(attr);
+	iov[1].iov_base = data;
+	iov[1].iov_len = size;
+
+	len = readv(fd, iov, 2);
+
+	close(fd);
+
+	if (len < 0)
+		return -EIO;
+
+	if (attributes)
+		*attributes = attr;
+
+	return 0;
+}
+
+int efivars_write(const char *name, uint32_t attributes,
+					const void *data, size_t size)
+{
+	char pathname[PATH_MAX];
+	void *buf;
+	ssize_t written;
+	int fd;
+
+	efivars_pathname(name, pathname, PATH_MAX);
+
+	buf = malloc(size + sizeof(attributes));
+	if (!buf)
+		return -ENOMEM;
+
+	fd = open(pathname, O_CREAT | O_WRONLY | O_TRUNC | O_CLOEXEC,
+				S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+	if (fd < 0) {
+		free(buf);
+		return -EIO;
+	}
+
+	memcpy(buf, &attributes, sizeof(attributes));
+	memcpy(buf + sizeof(attributes), data, size);
+
+	written = write(fd, buf, size + sizeof(attributes));
+
+	close(fd);
+	free(buf);
+
+	if (written < 0)
+		return -EIO;
+
+	return 0;
+}
diff --git a/peripheral/efivars.h b/peripheral/efivars.h
new file mode 100644
index 000000000..430d14330
--- /dev/null
+++ b/peripheral/efivars.h
@@ -0,0 +1,33 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#define EFIVARS_NON_VOLATILE			0x00000001
+#define EFIVARS_BOOTSERVICE_ACCESS		0x00000002
+#define EFIVARS_RUNTIME_ACCESS			0x00000004
+#define EFIVARS_HARDWARE_ERROR_RECORD		0x00000008
+#define EFIVARS_AUTHENTICATED_WRITE_ACCESS	0x00000010
+
+int efivars_read(const char *name, uint32_t *attributes,
+					void *data, size_t size);
+int efivars_write(const char *name, uint32_t attributes,
+					const void *data, size_t size);
diff --git a/peripheral/gap.c b/peripheral/gap.c
new file mode 100644
index 000000000..f659f7fef
--- /dev/null
+++ b/peripheral/gap.c
@@ -0,0 +1,551 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <string.h>
+
+#include "lib/bluetooth.h"
+#include "lib/mgmt.h"
+#include "src/shared/util.h"
+#include "src/shared/mgmt.h"
+#include "peripheral/gatt.h"
+#include "peripheral/gap.h"
+
+static struct mgmt *mgmt = NULL;
+static uint16_t mgmt_index = MGMT_INDEX_NONE;
+
+static bool adv_features = false;
+static bool adv_instances = false;
+static bool require_connectable = true;
+
+static uint8_t static_addr[6] = { 0x90, 0x78, 0x56, 0x34, 0x12, 0xc0 };
+static uint8_t dev_name[260] = { 0x00, };
+static uint8_t dev_name_len = 0;
+
+void gap_set_static_address(uint8_t addr[6])
+{
+	memcpy(static_addr, addr, sizeof(static_addr));
+
+	printf("Using static address %02x:%02x:%02x:%02x:%02x:%02x\n",
+			static_addr[5], static_addr[4], static_addr[3],
+			static_addr[2], static_addr[1], static_addr[0]);
+}
+
+static void clear_long_term_keys(uint16_t index)
+{
+	struct mgmt_cp_load_long_term_keys cp;
+
+	memset(&cp, 0, sizeof(cp));
+	cp.key_count = cpu_to_le16(0);
+
+	mgmt_send(mgmt, MGMT_OP_LOAD_LONG_TERM_KEYS, index,
+					sizeof(cp), &cp, NULL, NULL, NULL);
+}
+
+static void clear_identity_resolving_keys(uint16_t index)
+{
+	struct mgmt_cp_load_irks cp;
+
+	memset(&cp, 0, sizeof(cp));
+	cp.irk_count = cpu_to_le16(0);
+
+	mgmt_send(mgmt, MGMT_OP_LOAD_IRKS, index,
+					sizeof(cp), &cp, NULL, NULL, NULL);
+}
+
+static void add_advertising(uint16_t index)
+{
+	const char ad[] = { 0x11, 0x15,
+			0xd0, 0x00, 0x2d, 0x12, 0x1e, 0x4b, 0x0f, 0xa4,
+			0x99, 0x4e, 0xce, 0xb5, 0x31, 0xf4, 0x05, 0x79 };
+	struct mgmt_cp_add_advertising *cp;
+	void *buf;
+
+	buf = malloc(sizeof(*cp) + sizeof(ad));
+	if (!buf)
+		return;
+
+	memset(buf, 0, sizeof(*cp) + sizeof(ad));
+	cp = buf;
+	cp->instance = 0x01;
+	cp->flags = cpu_to_le32((1 << 0) | (1 << 1) | (1 << 4));
+	cp->duration = cpu_to_le16(0);
+	cp->timeout = cpu_to_le16(0);
+	cp->adv_data_len = sizeof(ad);
+	cp->scan_rsp_len = 0;
+	memcpy(cp->data, ad, sizeof(ad));
+
+	mgmt_send(mgmt, MGMT_OP_ADD_ADVERTISING, index,
+			sizeof(*cp) + sizeof(ad), buf, NULL, NULL, NULL);
+
+	free(buf);
+}
+
+static void enable_advertising(uint16_t index)
+{
+	uint8_t val;
+
+	val = require_connectable ? 0x01 : 0x00;
+	mgmt_send(mgmt, MGMT_OP_SET_CONNECTABLE, index, 1, &val,
+						NULL, NULL, NULL);
+
+	val = 0x01;
+	mgmt_send(mgmt, MGMT_OP_SET_POWERED, index, 1, &val,
+						NULL, NULL, NULL);
+
+	if (adv_instances) {
+		add_advertising(index);
+		return;
+	}
+
+	val = require_connectable ? 0x01 : 0x02;
+	mgmt_send(mgmt, MGMT_OP_SET_ADVERTISING, index, 1, &val,
+						NULL, NULL, NULL);
+}
+
+static void new_settings_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("New settings\n");
+}
+
+static void local_name_changed_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Local name changed\n");
+}
+
+static void new_long_term_key_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("New long term key\n");
+}
+
+static void device_connected_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Device connected\n");
+}
+
+static void device_disconnected_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Device disconnected\n");
+}
+
+static void user_confirm_request_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("User confirm request\n");
+}
+
+static void user_passkey_request_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("User passkey request\n");
+}
+
+static void auth_failed_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Authentication failed\n");
+}
+
+static void device_unpaired_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Device unpaired\n");
+}
+
+static void passkey_notify_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Passkey notification\n");
+}
+
+static void new_irk_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("New identify resolving key\n");
+}
+
+static void new_csrk_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("New connection signature resolving key\n");
+}
+
+static void new_conn_param_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("New connection parameter\n");
+}
+
+static void advertising_added_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Advertising added\n");
+}
+
+static void advertising_removed_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Advertising removed\n");
+}
+
+static void read_adv_features_complete(uint8_t status, uint16_t len,
+					const void *param, void *user_data)
+{
+	const struct mgmt_rp_read_adv_features *rp = param;
+	uint16_t index = PTR_TO_UINT(user_data);
+	uint32_t flags;
+
+	flags = le32_to_cpu(rp->supported_flags);
+
+	if (rp->max_instances > 0) {
+		adv_instances = true;
+
+		if (flags & (1 << 0))
+			require_connectable = false;
+	} else
+		require_connectable = false;
+
+	enable_advertising(index);
+}
+
+static void read_info_complete(uint8_t status, uint16_t len,
+					const void *param, void *user_data)
+{
+	const struct mgmt_rp_read_info *rp = param;
+	uint16_t index = PTR_TO_UINT(user_data);
+	uint32_t required_settings = MGMT_SETTING_LE |
+					MGMT_SETTING_STATIC_ADDRESS;
+	uint32_t supported_settings, current_settings;
+	uint8_t val;
+
+	required_settings = MGMT_SETTING_LE;
+
+	if (status) {
+		fprintf(stderr, "Reading info for index %u failed: %s\n",
+						index, mgmt_errstr(status));
+		return;
+	}
+
+	if (mgmt_index != MGMT_INDEX_NONE)
+		return;
+
+	supported_settings = le32_to_cpu(rp->supported_settings);
+	current_settings = le32_to_cpu(rp->current_settings);
+
+	if ((supported_settings & required_settings) != required_settings)
+		return;
+
+	printf("Selecting index %u\n", index);
+	mgmt_index = index;
+
+	mgmt_register(mgmt, MGMT_EV_NEW_SETTINGS, index,
+					new_settings_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_LOCAL_NAME_CHANGED, index,
+					local_name_changed_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_NEW_LONG_TERM_KEY, index,
+					new_long_term_key_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_DEVICE_CONNECTED, index,
+					device_connected_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_DEVICE_DISCONNECTED, index,
+					device_disconnected_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_USER_CONFIRM_REQUEST, index,
+					user_confirm_request_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_USER_PASSKEY_REQUEST, index,
+					user_passkey_request_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_AUTH_FAILED, index,
+					auth_failed_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_DEVICE_UNPAIRED, index,
+					device_unpaired_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_PASSKEY_NOTIFY, index,
+					passkey_notify_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_NEW_IRK, index,
+					new_irk_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_NEW_CSRK, index,
+					new_csrk_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_NEW_CONN_PARAM, index,
+					new_conn_param_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_ADVERTISING_ADDED, index,
+					advertising_added_event, NULL, NULL);
+	mgmt_register(mgmt, MGMT_EV_ADVERTISING_REMOVED, index,
+					advertising_removed_event, NULL, NULL);
+
+	dev_name_len = snprintf((char *) dev_name, 26, "BlueZ Peripheral");
+
+	if (current_settings & MGMT_SETTING_POWERED) {
+		val = 0x00;
+		mgmt_send(mgmt, MGMT_OP_SET_POWERED, index, 1, &val,
+							NULL, NULL, NULL);
+	}
+
+	if (!(current_settings & MGMT_SETTING_LE)) {
+		val = 0x01;
+		mgmt_send(mgmt, MGMT_OP_SET_LE, index, 1, &val,
+							NULL, NULL, NULL);
+	}
+
+	if (current_settings & MGMT_SETTING_BREDR) {
+		val = 0x00;
+		mgmt_send(mgmt, MGMT_OP_SET_BREDR, index, 1, &val,
+							NULL, NULL, NULL);
+	}
+
+	if ((supported_settings & MGMT_SETTING_SECURE_CONN) &&
+			!(current_settings & MGMT_SETTING_SECURE_CONN)) {
+		val = 0x01;
+		mgmt_send(mgmt, MGMT_OP_SET_SECURE_CONN, index, 1, &val,
+							NULL, NULL, NULL);
+	}
+
+	if (current_settings & MGMT_SETTING_DEBUG_KEYS) {
+		val = 0x00;
+		mgmt_send(mgmt, MGMT_OP_SET_DEBUG_KEYS, index, 1, &val,
+							NULL, NULL, NULL);
+	}
+
+	if (!(current_settings & MGMT_SETTING_BONDABLE)) {
+		val = 0x01;
+		mgmt_send(mgmt, MGMT_OP_SET_BONDABLE, index, 1, &val,
+							NULL, NULL, NULL);
+	}
+
+	clear_long_term_keys(mgmt_index);
+	clear_identity_resolving_keys(mgmt_index);
+
+	mgmt_send(mgmt, MGMT_OP_SET_STATIC_ADDRESS, index,
+					6, static_addr, NULL, NULL, NULL);
+
+	mgmt_send(mgmt, MGMT_OP_SET_LOCAL_NAME, index,
+					260, dev_name, NULL, NULL, NULL);
+
+	gatt_set_static_address(static_addr);
+	gatt_set_device_name(dev_name, dev_name_len);
+	gatt_server_start();
+
+	if (adv_features)
+		mgmt_send(mgmt, MGMT_OP_READ_ADV_FEATURES, index, 0, NULL,
+						read_adv_features_complete,
+						UINT_TO_PTR(index), NULL);
+	else
+		enable_advertising(index);
+}
+
+static void read_index_list_complete(uint8_t status, uint16_t len,
+					const void *param, void *user_data)
+{
+	const struct mgmt_rp_read_index_list *rp = param;
+	uint16_t count;
+	int i;
+
+	if (status) {
+		fprintf(stderr, "Reading index list failed: %s\n",
+						mgmt_errstr(status));
+		return;
+	}
+
+	count = le16_to_cpu(rp->num_controllers);
+
+	printf("Index list: %u\n", count);
+
+	for (i = 0; i < count; i++) {
+		uint16_t index = cpu_to_le16(rp->index[i]);
+
+		mgmt_send(mgmt, MGMT_OP_READ_INFO, index, 0, NULL,
+				read_info_complete, UINT_TO_PTR(index), NULL);
+	}
+}
+
+static void index_added_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Index added\n");
+
+	if (mgmt_index != MGMT_INDEX_NONE)
+		return;
+
+	mgmt_send(mgmt, MGMT_OP_READ_INFO, index, 0, NULL,
+				read_info_complete, UINT_TO_PTR(index), NULL);
+}
+
+static void index_removed_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	printf("Index removed\n");
+
+	if (mgmt_index != index)
+		return;
+
+	mgmt_index = MGMT_INDEX_NONE;
+}
+
+static void read_ext_index_list_complete(uint8_t status, uint16_t len,
+					const void *param, void *user_data)
+{
+	const struct mgmt_rp_read_ext_index_list *rp = param;
+	uint16_t count;
+	int i;
+
+	if (status) {
+		fprintf(stderr, "Reading extended index list failed: %s\n",
+						mgmt_errstr(status));
+		return;
+	}
+
+	count = le16_to_cpu(rp->num_controllers);
+
+	printf("Extended index list: %u\n", count);
+
+	for (i = 0; i < count; i++) {
+		uint16_t index = cpu_to_le16(rp->entry[i].index);
+
+		if (rp->entry[i].type != 0x00)
+			continue;
+
+		mgmt_send(mgmt, MGMT_OP_READ_INFO, index, 0, NULL,
+				read_info_complete, UINT_TO_PTR(index), NULL);
+	}
+}
+
+static void ext_index_added_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	const struct mgmt_ev_ext_index_added *ev = param;
+
+	printf("Extended index added: %u\n", ev->type);
+
+	if (mgmt_index != MGMT_INDEX_NONE)
+		return;
+
+	if (ev->type != 0x00)
+		return;
+
+	mgmt_send(mgmt, MGMT_OP_READ_INFO, index, 0, NULL,
+				read_info_complete, UINT_TO_PTR(index), NULL);
+}
+
+static void ext_index_removed_event(uint16_t index, uint16_t length,
+					const void *param, void *user_data)
+{
+	const struct mgmt_ev_ext_index_added *ev = param;
+
+	printf("Extended index removed: %u\n", ev->type);
+
+	if (mgmt_index != index)
+		return;
+
+	if (ev->type != 0x00)
+		return;
+
+	mgmt_index = MGMT_INDEX_NONE;
+}
+
+static void read_commands_complete(uint8_t status, uint16_t len,
+					const void *param, void *user_data)
+{
+	const struct mgmt_rp_read_commands *rp = param;
+	const uint16_t *opcode;
+	uint16_t num_commands;
+	bool ext_index_list = false;
+	int i;
+
+	if (status) {
+		fprintf(stderr, "Reading index list failed: %s\n",
+						mgmt_errstr(status));
+		return;
+	}
+
+	num_commands = le16_to_cpu(rp->num_commands);
+	opcode = rp->opcodes;
+
+	for (i = 0; i < num_commands; i++) {
+		uint16_t op = get_le16(opcode++);
+
+		if (op == MGMT_OP_READ_EXT_INDEX_LIST)
+			ext_index_list = true;
+		else if (op == MGMT_OP_READ_ADV_FEATURES)
+			adv_features = true;
+	}
+
+	if (ext_index_list) {
+		mgmt_register(mgmt, MGMT_EV_EXT_INDEX_ADDED, MGMT_INDEX_NONE,
+					ext_index_added_event, NULL, NULL);
+		mgmt_register(mgmt, MGMT_EV_EXT_INDEX_REMOVED, MGMT_INDEX_NONE,
+					ext_index_removed_event, NULL, NULL);
+
+		if (!mgmt_send(mgmt, MGMT_OP_READ_EXT_INDEX_LIST,
+				MGMT_INDEX_NONE, 0, NULL,
+				read_ext_index_list_complete, NULL, NULL)) {
+			fprintf(stderr, "Failed to read extended index list\n");
+			return;
+		}
+	} else {
+		mgmt_register(mgmt, MGMT_EV_INDEX_ADDED, MGMT_INDEX_NONE,
+					index_added_event, NULL, NULL);
+		mgmt_register(mgmt, MGMT_EV_INDEX_REMOVED, MGMT_INDEX_NONE,
+					index_removed_event, NULL, NULL);
+
+		if (!mgmt_send(mgmt, MGMT_OP_READ_INDEX_LIST,
+				MGMT_INDEX_NONE, 0, NULL,
+				read_index_list_complete, NULL, NULL)) {
+			fprintf(stderr, "Failed to read index list\n");
+			return;
+		}
+	}
+}
+
+void gap_start(void)
+{
+	mgmt = mgmt_new_default();
+	if (!mgmt) {
+		fprintf(stderr, "Failed to open management socket\n");
+		return;
+	}
+
+	if (!mgmt_send(mgmt, MGMT_OP_READ_COMMANDS,
+				MGMT_INDEX_NONE, 0, NULL,
+				read_commands_complete, NULL, NULL)) {
+		fprintf(stderr, "Failed to read supported commands\n");
+		return;
+	}
+}
+
+void gap_stop(void)
+{
+	if (!mgmt)
+		return;
+
+	gatt_server_stop();
+
+        mgmt_unref(mgmt);
+	mgmt = NULL;
+
+	mgmt_index = MGMT_INDEX_NONE;
+}
diff --git a/peripheral/gap.h b/peripheral/gap.h
new file mode 100644
index 000000000..6d6737810
--- /dev/null
+++ b/peripheral/gap.h
@@ -0,0 +1,29 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <stdint.h>
+
+void gap_set_static_address(uint8_t addr[6]);
+
+void gap_start(void);
+void gap_stop(void);
diff --git a/peripheral/gatt.c b/peripheral/gatt.c
new file mode 100644
index 000000000..e1bf8fbff
--- /dev/null
+++ b/peripheral/gatt.c
@@ -0,0 +1,317 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/epoll.h>
+
+#include "lib/bluetooth.h"
+#include "lib/l2cap.h"
+#include "lib/uuid.h"
+#include "src/shared/mainloop.h"
+#include "src/shared/util.h"
+#include "src/shared/queue.h"
+#include "src/shared/att.h"
+#include "src/shared/gatt-db.h"
+#include "src/shared/gatt-server.h"
+#include "src/shared/gatt-client.h"
+#include "peripheral/gatt.h"
+
+#define ATT_CID 4
+
+#define UUID_GAP 0x1800
+
+struct gatt_conn {
+	struct bt_att *att;
+	struct bt_gatt_server *gatt;
+	struct bt_gatt_client *client;
+};
+
+static int att_fd = -1;
+static struct queue *conn_list = NULL;
+static struct gatt_db *gatt_db = NULL;
+static struct gatt_db *gatt_cache = NULL;
+
+static uint8_t static_addr[6] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+static uint8_t dev_name[20];
+static uint8_t dev_name_len = 0;
+
+void gatt_set_static_address(uint8_t addr[6])
+{
+	memcpy(static_addr, addr, sizeof(static_addr));
+}
+
+void gatt_set_device_name(uint8_t name[20], uint8_t len)
+{
+	memcpy(dev_name, name, sizeof(dev_name));
+	dev_name_len = len;
+}
+
+static void gatt_conn_destroy(void *data)
+{
+	struct gatt_conn *conn = data;
+
+	bt_gatt_client_unref(conn->client);
+	bt_gatt_server_unref(conn->gatt);
+	bt_att_unref(conn->att);
+
+	free(conn);
+}
+
+static void gatt_conn_disconnect(int err, void *user_data)
+{
+	struct gatt_conn *conn = user_data;
+
+	printf("Device disconnected: %s\n", strerror(err));
+
+	queue_remove(conn_list, conn);
+	gatt_conn_destroy(conn);
+}
+
+static void client_ready_callback(bool success, uint8_t att_ecode,
+							void *user_data)
+{
+	printf("GATT client discovery complete\n");
+}
+
+static void client_service_changed_callback(uint16_t start_handle,
+						uint16_t end_handle,
+						void *user_data)
+{
+	printf("GATT client service changed notification\n");
+}
+
+static struct gatt_conn *gatt_conn_new(int fd)
+{
+	struct gatt_conn *conn;
+	uint16_t mtu = 0;
+
+	conn = new0(struct gatt_conn, 1);
+	if (!conn)
+		return NULL;
+
+	conn->att = bt_att_new(fd);
+	if (!conn->att) {
+		fprintf(stderr, "Failed to initialze ATT transport layer\n");
+		free(conn);
+		return NULL;
+	}
+
+	bt_att_set_close_on_unref(conn->att, true);
+	bt_att_register_disconnect(conn->att, gatt_conn_disconnect, conn, NULL);
+
+	bt_att_set_sec_level(conn->att, BT_SECURITY_MEDIUM);
+
+	conn->gatt = bt_gatt_server_new(gatt_db, conn->att, mtu);
+	if (!conn->gatt) {
+		fprintf(stderr, "Failed to create GATT server\n");
+		bt_att_unref(conn->att);
+		free(conn);
+		return NULL;
+	}
+
+	conn->client = bt_gatt_client_new(gatt_cache, conn->att, mtu);
+	if (!conn->gatt) {
+		fprintf(stderr, "Failed to create GATT client\n");
+		bt_gatt_server_unref(conn->gatt);
+		bt_att_unref(conn->att);
+		free(conn);
+		return NULL;
+	}
+
+	bt_gatt_client_set_ready_handler(conn->client,
+				client_ready_callback, conn, NULL);
+	bt_gatt_client_set_service_changed(conn->client,
+				client_service_changed_callback, conn, NULL);
+
+	return conn;
+}
+
+static void att_conn_callback(int fd, uint32_t events, void *user_data)
+{
+	struct gatt_conn *conn;
+	struct sockaddr_l2 addr;
+	socklen_t addrlen;
+	int new_fd;
+
+	if (events & (EPOLLERR | EPOLLHUP)) {
+		mainloop_remove_fd(fd);
+		return;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addrlen = sizeof(addr);
+
+	new_fd = accept(att_fd, (struct sockaddr *) &addr, &addrlen);
+	if (new_fd < 0) {
+		fprintf(stderr, "Failed to accept new ATT connection: %m\n");
+		return;
+	}
+
+	conn = gatt_conn_new(new_fd);
+	if (!conn) {
+		fprintf(stderr, "Failed to create GATT connection\n");
+		close(new_fd);
+	}
+
+	if (!queue_push_tail(conn_list, conn)) {
+		fprintf(stderr, "Failed to add GATT connection\n");
+		gatt_conn_destroy(conn);
+		close(new_fd);
+	}
+
+	printf("New device connected\n");
+}
+
+static void gap_device_name_read(struct gatt_db_attribute *attrib,
+					unsigned int id, uint16_t offset,
+					uint8_t opcode, struct bt_att *att,
+					void *user_data)
+{
+	uint8_t error;
+	const uint8_t *value;
+	size_t len;
+
+	if (offset > dev_name_len) {
+		error = BT_ATT_ERROR_INVALID_OFFSET;
+		value = NULL;
+		len = dev_name_len;
+	} else {
+		error = 0;
+		len = dev_name_len - offset;
+		value = len ? &dev_name[offset] : NULL;
+	}
+
+	gatt_db_attribute_read_result(attrib, id, error, value, len);
+}
+
+static void populate_gap_service(struct gatt_db *db)
+{
+	struct gatt_db_attribute *service;
+	bt_uuid_t uuid;
+
+	bt_uuid16_create(&uuid, UUID_GAP);
+	service = gatt_db_add_service(db, &uuid, true, 6);
+
+	bt_uuid16_create(&uuid, GATT_CHARAC_DEVICE_NAME);
+	gatt_db_service_add_characteristic(service, &uuid,
+					BT_ATT_PERM_READ,
+					BT_GATT_CHRC_PROP_READ,
+					gap_device_name_read, NULL, NULL);
+
+	gatt_db_service_set_active(service, true);
+}
+
+static void populate_devinfo_service(struct gatt_db *db)
+{
+	struct gatt_db_attribute *service;
+	bt_uuid_t uuid;
+
+	bt_uuid16_create(&uuid, 0x180a);
+	service = gatt_db_add_service(db, &uuid, true, 17);
+
+	gatt_db_service_set_active(service, true);
+}
+
+void gatt_server_start(void)
+{
+	struct sockaddr_l2 addr;
+
+	if (att_fd >= 0)
+		return;
+
+	att_fd = socket(PF_BLUETOOTH, SOCK_SEQPACKET | SOCK_CLOEXEC,
+							BTPROTO_L2CAP);
+	if (att_fd < 0) {
+		fprintf(stderr, "Failed to create ATT server socket: %m\n");
+		return;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	addr.l2_cid = htobs(ATT_CID);
+	memcpy(&addr.l2_bdaddr, static_addr, 6);
+	addr.l2_bdaddr_type = BDADDR_LE_RANDOM;
+
+	if (bind(att_fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		fprintf(stderr, "Failed to bind ATT server socket: %m\n");
+		close(att_fd);
+		att_fd = -1;
+		return;
+	}
+
+	if (listen(att_fd, 1) < 0) {
+		fprintf(stderr, "Failed to listen on ATT server socket: %m\n");
+		close(att_fd);
+		att_fd = -1;
+		return;
+	}
+
+	gatt_db = gatt_db_new();
+	if (!gatt_db) {
+		close(att_fd);
+		att_fd = -1;
+		return;
+	}
+
+	populate_gap_service(gatt_db);
+	populate_devinfo_service(gatt_db);
+
+	gatt_cache = gatt_db_new();
+
+	conn_list = queue_new();
+	if (!conn_list) {
+		gatt_db_unref(gatt_db);
+		gatt_db = NULL;
+		close(att_fd);
+		att_fd = -1;
+		return;
+	}
+
+	mainloop_add_fd(att_fd, EPOLLIN, att_conn_callback, NULL, NULL);
+}
+
+void gatt_server_stop(void)
+{
+	if (att_fd < 0)
+		return;
+
+	mainloop_remove_fd(att_fd);
+
+	queue_destroy(conn_list, gatt_conn_destroy);
+
+	gatt_db_unref(gatt_cache);
+	gatt_cache = NULL;
+
+	gatt_db_unref(gatt_db);
+	gatt_db = NULL;
+
+	close(att_fd);
+	att_fd = -1;
+}
diff --git a/peripheral/gatt.h b/peripheral/gatt.h
new file mode 100644
index 000000000..5b68f3573
--- /dev/null
+++ b/peripheral/gatt.h
@@ -0,0 +1,30 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <stdint.h>
+
+void gatt_set_static_address(uint8_t addr[6]);
+void gatt_set_device_name(uint8_t name[20], uint8_t len);
+
+void gatt_server_start(void);
+void gatt_server_stop(void);
diff --git a/peripheral/log.c b/peripheral/log.c
new file mode 100644
index 000000000..7aaeb4d8c
--- /dev/null
+++ b/peripheral/log.c
@@ -0,0 +1,56 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "peripheral/log.h"
+
+static int kmsg_fd = -1;
+
+void log_open(void)
+{
+	if (kmsg_fd >= 0)
+		return;
+
+	kmsg_fd = open("/dev/kmsg", O_WRONLY | O_NOCTTY | O_CLOEXEC);
+	if (kmsg_fd < 0) {
+		fprintf(stderr, "Failed to open kernel logging: %m\n");
+		return;
+	}
+}
+
+void log_close(void)
+{
+	if (kmsg_fd < 0)
+		return;
+
+	close(kmsg_fd);
+	kmsg_fd = -1;
+}
diff --git a/peripheral/log.h b/peripheral/log.h
new file mode 100644
index 000000000..36619b36e
--- /dev/null
+++ b/peripheral/log.h
@@ -0,0 +1,25 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+void log_open(void);
+void log_close(void);
diff --git a/peripheral/main.c b/peripheral/main.c
new file mode 100644
index 000000000..d7e10f3d3
--- /dev/null
+++ b/peripheral/main.c
@@ -0,0 +1,247 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <signal.h>
+#include <string.h>
+#include <poll.h>
+#include <sys/wait.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/mount.h>
+
+#ifndef WAIT_ANY
+#define WAIT_ANY (-1)
+#endif
+
+#include "src/shared/mainloop.h"
+#include "peripheral/efivars.h"
+#include "peripheral/attach.h"
+#include "peripheral/gap.h"
+#include "peripheral/log.h"
+
+static bool is_init = false;
+static pid_t shell_pid = -1;
+
+static const struct {
+	const char *target;
+	const char *linkpath;
+} dev_table[] = {
+	{ "/proc/self/fd",	"/dev/fd"	},
+	{ "/proc/self/fd/0",	"/dev/stdin"	},
+	{ "/proc/self/fd/1",	"/dev/stdout"	},
+	{ "/proc/self/fd/2",	"/dev/stderr"	},
+	{ }
+};
+
+static const struct {
+	const char *fstype;
+	const char *target;
+	const char *options;
+	unsigned long flags;
+} mount_table[] = {
+	{ "sysfs",    "/sys",		NULL,	MS_NOSUID|MS_NOEXEC|MS_NODEV },
+	{ "proc",     "/proc",		NULL,	MS_NOSUID|MS_NOEXEC|MS_NODEV },
+	{ "devtmpfs", "/dev",		NULL,	MS_NOSUID|MS_STRICTATIME },
+	{ "efivarfs", "/sys/firmware/efi/efivars",
+					NULL,	MS_NOSUID|MS_NOEXEC|MS_NODEV },
+	{ "pstore",   "/sys/fs/pstore",	NULL,	MS_NOSUID|MS_NOEXEC|MS_NODEV },
+	{ }
+};
+
+static void prepare_filesystem(void)
+{
+	int i;
+
+	if (!is_init)
+		return;
+
+	for (i = 0; mount_table[i].fstype; i++) {
+		struct stat st;
+
+		if (lstat(mount_table[i].target, &st) < 0) {
+			printf("Creating %s\n", mount_table[i].target);
+			mkdir(mount_table[i].target, 0755);
+		}
+
+		printf("Mounting %s to %s\n", mount_table[i].fstype,
+						mount_table[i].target);
+
+		if (mount(mount_table[i].fstype,
+				mount_table[i].target,
+				mount_table[i].fstype,
+				mount_table[i].flags, NULL) < 0)
+			perror("Failed to mount filesystem");
+	}
+
+	for (i = 0; dev_table[i].target; i++) {
+		printf("Linking %s to %s\n", dev_table[i].linkpath,
+						dev_table[i].target);
+
+		if (symlink(dev_table[i].target, dev_table[i].linkpath) < 0)
+			perror("Failed to create device symlink");
+	}
+}
+
+static void run_shell(void)
+{
+	pid_t pid;
+
+	printf("Starting shell\n");
+
+	pid = fork();
+	if (pid < 0) {
+		perror("Failed to fork new process");
+		return;
+	}
+
+	if (pid == 0) {
+		char *prg_argv[] = { "/bin/sh", NULL };
+		char *prg_envp[] = { NULL };
+
+		execve(prg_argv[0], prg_argv, prg_envp);
+		exit(0);
+	}
+
+	printf("PID %d created\n", pid);
+
+	shell_pid = pid;
+}
+
+static void exit_shell(void)
+{
+	shell_pid = -1;
+
+	if (!is_init) {
+		mainloop_quit();
+		return;
+	}
+
+	run_shell();
+}
+
+static void signal_callback(int signum, void *user_data)
+{
+	switch (signum) {
+	case SIGINT:
+	case SIGTERM:
+		mainloop_quit();
+		break;
+	case SIGCHLD:
+		while (1) {
+			pid_t pid;
+			int status;
+
+			pid = waitpid(WAIT_ANY, &status, WNOHANG);
+			if (pid < 0 || pid == 0)
+				break;
+
+			if (WIFEXITED(status)) {
+				printf("PID %d exited (status=%d)\n",
+						pid, WEXITSTATUS(status));
+
+				if (pid == shell_pid)
+					exit_shell();
+			} else if (WIFSIGNALED(status)) {
+				printf("PID %d terminated (signal=%d)\n",
+							pid, WTERMSIG(status));
+
+				if (pid == shell_pid)
+					exit_shell();
+			}
+		}
+		break;
+	}
+}
+
+int main(int argc, char *argv[])
+{
+	sigset_t mask;
+	int exit_status;
+
+	if (getpid() == 1 && getppid() == 0)
+		is_init = true;
+
+	mainloop_init();
+
+	sigemptyset(&mask);
+	sigaddset(&mask, SIGINT);
+	sigaddset(&mask, SIGTERM);
+	sigaddset(&mask, SIGCHLD);
+
+	mainloop_set_signal(&mask, signal_callback, NULL, NULL);
+
+	printf("Bluetooth periperhal ver %s\n", VERSION);
+
+	prepare_filesystem();
+
+	if (is_init) {
+		uint8_t addr[6];
+
+		if (efivars_read("BluetoothStaticAddress", NULL,
+							addr, 6) < 0) {
+			printf("Generating new persistent static address\n");
+
+			addr[0] = rand();
+			addr[1] = rand();
+			addr[2] = rand();
+			addr[3] = 0x34;
+			addr[4] = 0x12;
+			addr[5] = 0xc0;
+
+			efivars_write("BluetoothStaticAddress",
+					EFIVARS_NON_VOLATILE |
+					EFIVARS_BOOTSERVICE_ACCESS |
+					EFIVARS_RUNTIME_ACCESS,
+					addr, 6);
+		}
+
+		gap_set_static_address(addr);
+
+		run_shell();
+	}
+
+	log_open();
+	gap_start();
+
+	if (is_init)
+		attach_start();
+
+	exit_status = mainloop_run();
+
+	if (is_init)
+		attach_stop();
+
+	gap_stop();
+	log_close();
+
+	return exit_status;
+}