Add Nimble in libs directory

16 files changed, 7923 insertions, 0 deletions

diff --git a/src/libs/mynewt-nimble/apps/bttester/README b/src/libs/mynewt-nimble/apps/bttester/README
new file mode 100644
index 00000000..29db2eba
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/README
@@ -0,0 +1,14 @@
+Title: Bluetooth tester application
+
+Description:
+
+Tester application uses binary protocol to control Mynewt Nimble stack
+and is aimed at automated testing. It uses Bluetooth Testing Protocol (BTP)
+to drive Bluetooth stack. BTP commands and events are received and buffered for
+further processing.
+--------------------------------------------------------------------------------
+Supported Profiles:
+
+GAP, GATT, SM, L2CAP, MESH
+--------------------------------------------------------------------------------
+
diff --git a/src/libs/mynewt-nimble/apps/bttester/pkg.yml b/src/libs/mynewt-nimble/apps/bttester/pkg.yml
new file mode 100644
index 00000000..00e7a760
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/pkg.yml
@@ -0,0 +1,44 @@
+#
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+# 
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+pkg.name: apps/bttester
+pkg.type: app
+pkg.description: Bluetooth tester application
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+
+pkg.deps:
+    - "@apache-mynewt-core/kernel/os"
+    - "@apache-mynewt-core/sys/console/full"
+    - "@apache-mynewt-core/sys/log/full"
+    - "@apache-mynewt-core/sys/log/modlog"
+    - "@apache-mynewt-core/sys/stats/full"
+    - "@apache-mynewt-core/sys/shell"
+    - "@apache-mynewt-nimble/nimble/controller"
+    - "@apache-mynewt-nimble/nimble/host"
+    - "@apache-mynewt-nimble/nimble/host/util"
+    - "@apache-mynewt-nimble/nimble/host/services/gap"
+    - "@apache-mynewt-nimble/nimble/host/services/gatt"
+    - "@apache-mynewt-nimble/nimble/host/services/dis"
+    - "@apache-mynewt-nimble/nimble/host/store/ram"
+    - "@apache-mynewt-nimble/nimble/transport/ram"
+    - "@apache-mynewt-core/hw/drivers/uart"
+    - "@apache-mynewt-core/hw/drivers/rtt"
+
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/atomic.h b/src/libs/mynewt-nimble/apps/bttester/src/atomic.h
new file mode 100644
index 00000000..66283e9a
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/atomic.h
@@ -0,0 +1,405 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/* atomic operations */
+
+/*
+ * Copyright (c) 1997-2015, Wind River Systems, Inc.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __ATOMIC_H__
+#define __ATOMIC_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+typedef int atomic_t;
+typedef atomic_t atomic_val_t;
+
+/**
+ * @defgroup atomic_apis Atomic Services APIs
+ * @ingroup kernel_apis
+ * @{
+ */
+
+/**
+ * @brief Atomic compare-and-set.
+ *
+ * This routine performs an atomic compare-and-set on @a target. If the current
+ * value of @a target equals @a old_value, @a target is set to @a new_value.
+ * If the current value of @a target does not equal @a old_value, @a target
+ * is left unchanged.
+ *
+ * @param target Address of atomic variable.
+ * @param old_value Original value to compare against.
+ * @param new_value New value to store.
+ * @return 1 if @a new_value is written, 0 otherwise.
+ */
+static inline int atomic_cas(atomic_t *target, atomic_val_t old_value,
+        atomic_val_t new_value)
+{
+    return __atomic_compare_exchange_n(target, &old_value, new_value,
+            0, __ATOMIC_SEQ_CST,
+            __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic addition.
+ *
+ * This routine performs an atomic addition on @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to add.
+ *
+ * @return Previous value of @a target.
+ */
+static inline atomic_val_t atomic_add(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_add(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic subtraction.
+ *
+ * This routine performs an atomic subtraction on @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to subtract.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_sub(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic increment.
+ *
+ * This routine performs an atomic increment by 1 on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_inc(atomic_t *target)
+{
+    return atomic_add(target, 1);
+}
+
+/**
+ *
+ * @brief Atomic decrement.
+ *
+ * This routine performs an atomic decrement by 1 on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_dec(atomic_t *target)
+{
+    return atomic_sub(target, 1);
+}
+
+/**
+ *
+ * @brief Atomic get.
+ *
+ * This routine performs an atomic read on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Value of @a target.
+ */
+
+static inline atomic_val_t atomic_get(const atomic_t *target)
+{
+    return __atomic_load_n(target, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic get-and-set.
+ *
+ * This routine atomically sets @a target to @a value and returns
+ * the previous value of @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to write to @a target.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_set(atomic_t *target, atomic_val_t value)
+{
+    /* This builtin, as described by Intel, is not a traditional
+     * test-and-set operation, but rather an atomic exchange operation. It
+     * writes value into *ptr, and returns the previous contents of *ptr.
+     */
+    return __atomic_exchange_n(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic clear.
+ *
+ * This routine atomically sets @a target to zero and returns its previous
+ * value. (Hence, it is equivalent to atomic_set(target, 0).)
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_clear(atomic_t *target)
+{
+    return atomic_set(target, 0);
+}
+
+/**
+ *
+ * @brief Atomic bitwise inclusive OR.
+ *
+ * This routine atomically sets @a target to the bitwise inclusive OR of
+ * @a target and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to OR.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_or(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_or(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise exclusive OR (XOR).
+ *
+ * This routine atomically sets @a target to the bitwise exclusive OR (XOR) of
+ * @a target and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to XOR
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_xor(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise AND.
+ *
+ * This routine atomically sets @a target to the bitwise AND of @a target
+ * and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to AND.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_and(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_and(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise NAND.
+ *
+ * This routine atomically sets @a target to the bitwise NAND of @a target
+ * and @a value. (This operation is equivalent to target = ~(target & value).)
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to NAND.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_nand(target, value, __ATOMIC_SEQ_CST);
+}
+
+    /**
+     * @brief Initialize an atomic variable.
+     *
+     * This macro can be used to initialize an atomic variable. For example,
+     * @code atomic_t my_var = ATOMIC_INIT(75); @endcode
+     *
+     * @param i Value to assign to atomic variable.
+     */
+#define ATOMIC_INIT(i) (i)
+
+    /**
+     * @cond INTERNAL_HIDDEN
+     */
+
+#define ATOMIC_BITS (sizeof(atomic_val_t) * 8)
+#define ATOMIC_MASK(bit) (1 << ((bit) & (ATOMIC_BITS - 1)))
+#define ATOMIC_ELEM(addr, bit) ((addr) + ((bit) / ATOMIC_BITS))
+
+    /**
+     * INTERNAL_HIDDEN @endcond
+     */
+
+    /**
+     * @brief Define an array of atomic variables.
+     *
+     * This macro defines an array of atomic variables containing at least
+     * @a num_bits bits.
+     *
+     * @note
+     * If used from file scope, the bits of the array are initialized to zero;
+     * if used from within a function, the bits are left uninitialized.
+     *
+     * @param name Name of array of atomic variables.
+     * @param num_bits Number of bits needed.
+     */
+#define ATOMIC_DEFINE(name, num_bits) \
+	atomic_t name[1 + ((num_bits) - 1) / ATOMIC_BITS]
+
+    /**
+     * @brief Atomically test a bit.
+     *
+     * This routine tests whether bit number @a bit of @a target is set or not.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_bit(const atomic_t *target, int bit)
+    {
+        atomic_val_t val = atomic_get(ATOMIC_ELEM(target, bit));
+
+        return (1 & (val >> (bit & (ATOMIC_BITS - 1))));
+    }
+
+    /**
+     * @brief Atomically test and clear a bit.
+     *
+     * Atomically clear bit number @a bit of @a target and return its old value.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_and_clear_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+        atomic_val_t old;
+
+        old = atomic_and(ATOMIC_ELEM(target, bit), ~mask);
+
+        return (old & mask) != 0;
+    }
+
+    /**
+     * @brief Atomically set a bit.
+     *
+     * Atomically set bit number @a bit of @a target and return its old value.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_and_set_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+        atomic_val_t old;
+
+        old = atomic_or(ATOMIC_ELEM(target, bit), mask);
+
+        return (old & mask) != 0;
+    }
+
+    /**
+     * @brief Atomically clear a bit.
+     *
+     * Atomically clear bit number @a bit of @a target.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return N/A
+     */
+    static inline void
+    atomic_clear_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+
+        atomic_and(ATOMIC_ELEM(target, bit), ~mask);
+    }
+
+    /**
+     * @brief Atomically set a bit.
+     *
+     * Atomically set bit number @a bit of @a target.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return N/A
+     */
+    static inline void
+    atomic_set_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+
+        atomic_or(ATOMIC_ELEM(target, bit), mask);
+    }
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __ATOMIC_H__ */
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/bttester.c b/src/libs/mynewt-nimble/apps/bttester/src/bttester.c
new file mode 100644
index 00000000..54b14daa
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/bttester.c
@@ -0,0 +1,374 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/* bttester.c - Bluetooth Tester */
+
+/*
+ * Copyright (c) 2015-2016 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "syscfg/syscfg.h"
+#include "console/console.h"
+
+#include "bttester_pipe.h"
+#include "bttester.h"
+
+#define CMD_QUEUED 2
+
+static struct os_eventq avail_queue;
+static struct os_eventq *cmds_queue;
+static struct os_event bttester_ev[CMD_QUEUED];
+
+struct btp_buf {
+	struct os_event *ev;
+	union {
+		u8_t data[BTP_MTU];
+		struct btp_hdr hdr;
+	};
+};
+
+static struct btp_buf cmd_buf[CMD_QUEUED];
+
+static void supported_commands(u8_t *data, u16_t len)
+{
+	u8_t buf[1];
+	struct core_read_supported_commands_rp *rp = (void *) buf;
+
+	memset(buf, 0, sizeof(buf));
+
+	tester_set_bit(buf, CORE_READ_SUPPORTED_COMMANDS);
+	tester_set_bit(buf, CORE_READ_SUPPORTED_SERVICES);
+	tester_set_bit(buf, CORE_REGISTER_SERVICE);
+	tester_set_bit(buf, CORE_UNREGISTER_SERVICE);
+
+	tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_COMMANDS,
+		    BTP_INDEX_NONE, (u8_t *) rp, sizeof(buf));
+}
+
+static void supported_services(u8_t *data, u16_t len)
+{
+	u8_t buf[1];
+	struct core_read_supported_services_rp *rp = (void *) buf;
+
+	memset(buf, 0, sizeof(buf));
+
+	tester_set_bit(buf, BTP_SERVICE_ID_CORE);
+	tester_set_bit(buf, BTP_SERVICE_ID_GAP);
+	tester_set_bit(buf, BTP_SERVICE_ID_GATT);
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+	tester_set_bit(buf, BTP_SERVICE_ID_L2CAP);
+#endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */
+#if MYNEWT_VAL(BLE_MESH)
+	tester_set_bit(buf, BTP_SERVICE_ID_MESH);
+#endif /* MYNEWT_VAL(BLE_MESH) */
+
+	tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_SERVICES,
+		    BTP_INDEX_NONE, (u8_t *) rp, sizeof(buf));
+}
+
+static void register_service(u8_t *data, u16_t len)
+{
+	struct core_register_service_cmd *cmd = (void *) data;
+	u8_t status;
+
+	switch (cmd->id) {
+	case BTP_SERVICE_ID_GAP:
+		status = tester_init_gap();
+		/* Rsp with success status will be handled by bt enable cb */
+		if (status == BTP_STATUS_FAILED) {
+			goto rsp;
+		}
+		return;
+	case BTP_SERVICE_ID_GATT:
+		status = tester_init_gatt();
+		break;
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+	case BTP_SERVICE_ID_L2CAP:
+		status = tester_init_l2cap();
+		break;
+#endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */
+#if MYNEWT_VAL(BLE_MESH)
+	case BTP_SERVICE_ID_MESH:
+		status = tester_init_mesh();
+		break;
+#endif /* MYNEWT_VAL(BLE_MESH) */
+	default:
+		status = BTP_STATUS_FAILED;
+		break;
+	}
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE,
+		   status);
+}
+
+static void unregister_service(u8_t *data, u16_t len)
+{
+	struct core_unregister_service_cmd *cmd = (void *) data;
+	u8_t status;
+
+	switch (cmd->id) {
+	case BTP_SERVICE_ID_GAP:
+		status = tester_unregister_gap();
+		break;
+	case BTP_SERVICE_ID_GATT:
+		status = tester_unregister_gatt();
+		break;
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+	case BTP_SERVICE_ID_L2CAP:
+		status = tester_unregister_l2cap();
+		break;
+#endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */
+#if MYNEWT_VAL(BLE_MESH)
+	case BTP_SERVICE_ID_MESH:
+		status = tester_unregister_mesh();
+		break;
+#endif /* MYNEWT_VAL(BLE_MESH) */
+	default:
+		status = BTP_STATUS_FAILED;
+		break;
+	}
+
+	tester_rsp(BTP_SERVICE_ID_CORE, CORE_UNREGISTER_SERVICE, BTP_INDEX_NONE,
+		   status);
+}
+
+static void handle_core(u8_t opcode, u8_t index, u8_t *data,
+			u16_t len)
+{
+	if (index != BTP_INDEX_NONE) {
+		tester_rsp(BTP_SERVICE_ID_CORE, opcode, index,
+			   BTP_STATUS_FAILED);
+		return;
+	}
+
+	switch (opcode) {
+	case CORE_READ_SUPPORTED_COMMANDS:
+		supported_commands(data, len);
+		return;
+	case CORE_READ_SUPPORTED_SERVICES:
+		supported_services(data, len);
+		return;
+	case CORE_REGISTER_SERVICE:
+		register_service(data, len);
+		return;
+	case CORE_UNREGISTER_SERVICE:
+		unregister_service(data, len);
+		return;
+	default:
+		tester_rsp(BTP_SERVICE_ID_CORE, opcode, BTP_INDEX_NONE,
+			   BTP_STATUS_UNKNOWN_CMD);
+		return;
+	}
+}
+
+static void cmd_handler(struct os_event *ev)
+{
+	u16_t len;
+	struct btp_buf *cmd;
+
+	if (!ev || !ev->ev_arg) {
+		return;
+	}
+
+	cmd = ev->ev_arg;
+
+	len = sys_le16_to_cpu(cmd->hdr.len);
+	if (MYNEWT_VAL(BTTESTER_BTP_LOG)) {
+		console_printf("[DBG] received %d bytes: %s\n",
+			       sizeof(cmd->hdr) + len,
+			       bt_hex(cmd->data,
+				      sizeof(cmd->hdr) + len));
+	}
+
+	/* TODO
+	 * verify if service is registered before calling handler
+	 */
+
+	switch (cmd->hdr.service) {
+		case BTP_SERVICE_ID_CORE:
+			handle_core(cmd->hdr.opcode, cmd->hdr.index,
+				    cmd->hdr.data, len);
+			break;
+		case BTP_SERVICE_ID_GAP:
+			tester_handle_gap(cmd->hdr.opcode, cmd->hdr.index,
+					  cmd->hdr.data, len);
+			break;
+		case BTP_SERVICE_ID_GATT:
+			tester_handle_gatt(cmd->hdr.opcode, cmd->hdr.index,
+					   cmd->hdr.data, len);
+			break;
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+		case BTP_SERVICE_ID_L2CAP:
+			tester_handle_l2cap(cmd->hdr.opcode, cmd->hdr.index,
+					    cmd->hdr.data, len);
+			break;
+#endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */
+#if MYNEWT_VAL(BLE_MESH)
+		case BTP_SERVICE_ID_MESH:
+			tester_handle_mesh(cmd->hdr.opcode, cmd->hdr.index,
+					   cmd->hdr.data, len);
+			break;
+#endif /* MYNEWT_VAL(BLE_MESH) */
+		default:
+			tester_rsp(cmd->hdr.service, cmd->hdr.opcode,
+				   cmd->hdr.index, BTP_STATUS_FAILED);
+			break;
+	}
+
+	os_eventq_put(&avail_queue, ev);
+}
+
+static u8_t *recv_cb(u8_t *buf, size_t *off)
+{
+	struct btp_hdr *cmd = (void *) buf;
+	struct os_event *new_ev;
+	struct btp_buf *new_buf, *old_buf;
+	u16_t len;
+
+	if (*off < sizeof(*cmd)) {
+		return buf;
+	}
+
+	len = sys_le16_to_cpu(cmd->len);
+	if (len > BTP_MTU - sizeof(*cmd)) {
+		*off = 0;
+		return buf;
+	}
+
+	if (*off < sizeof(*cmd) + len) {
+		return buf;
+	}
+
+	new_ev = os_eventq_get_no_wait(&avail_queue);
+	if (!new_ev) {
+		SYS_LOG_ERR("BT tester: RX overflow");
+		*off = 0;
+		return buf;
+	}
+
+	old_buf = CONTAINER_OF(buf, struct btp_buf, data);
+	os_eventq_put(cmds_queue, old_buf->ev);
+
+	new_buf = new_ev->ev_arg;
+	*off = 0;
+	return new_buf->data;
+}
+
+static void avail_queue_init(void)
+{
+	int i;
+
+	os_eventq_init(&avail_queue);
+
+	for (i = 0; i < CMD_QUEUED; i++) {
+		cmd_buf[i].ev = &bttester_ev[i];
+		bttester_ev[i].ev_cb = cmd_handler;
+		bttester_ev[i].ev_arg = &cmd_buf[i];
+
+		os_eventq_put(&avail_queue, &bttester_ev[i]);
+	}
+}
+
+void bttester_evq_set(struct os_eventq *evq)
+{
+	cmds_queue = evq;
+}
+
+void tester_init(void)
+{
+	struct os_event *ev;
+	struct btp_buf *buf;
+
+	avail_queue_init();
+	bttester_evq_set(os_eventq_dflt_get());
+
+	ev = os_eventq_get(&avail_queue);
+	buf = ev->ev_arg;
+
+	if (bttester_pipe_init()) {
+		SYS_LOG_ERR("Failed to initialize pipe");
+		return;
+	}
+
+	bttester_pipe_register(buf->data, BTP_MTU, recv_cb);
+
+	tester_send(BTP_SERVICE_ID_CORE, CORE_EV_IUT_READY, BTP_INDEX_NONE,
+		    NULL, 0);
+}
+
+void tester_send(u8_t service, u8_t opcode, u8_t index, u8_t *data,
+		 size_t len)
+{
+	struct btp_hdr msg;
+
+	msg.service = service;
+	msg.opcode = opcode;
+	msg.index = index;
+	msg.len = len;
+
+	bttester_pipe_send((u8_t *)&msg, sizeof(msg));
+	if (data && len) {
+		bttester_pipe_send(data, len);
+	}
+
+	if (MYNEWT_VAL(BTTESTER_BTP_LOG)) {
+		console_printf("[DBG] send %d bytes hdr: %s\n", sizeof(msg),
+			       bt_hex((char *) &msg, sizeof(msg)));
+		if (data && len) {
+			console_printf("[DBG] send %d bytes data: %s\n", len,
+				       bt_hex((char *) data, len));
+		}
+	}
+}
+
+void tester_send_buf(u8_t service, u8_t opcode, u8_t index,
+		     struct os_mbuf *data)
+{
+	struct btp_hdr msg;
+
+	msg.service = service;
+	msg.opcode = opcode;
+	msg.index = index;
+	msg.len = os_mbuf_len(data);
+
+	bttester_pipe_send((u8_t *)&msg, sizeof(msg));
+	if (data && msg.len) {
+		bttester_pipe_send_buf(data);
+	}
+}
+
+void tester_rsp(u8_t service, u8_t opcode, u8_t index, u8_t status)
+{
+	struct btp_status s;
+
+	if (status == BTP_STATUS_SUCCESS) {
+		tester_send(service, opcode, index, NULL, 0);
+		return;
+	}
+
+	s.code = status;
+	tester_send(service, BTP_STATUS, index, (u8_t *) &s, sizeof(s));
+}
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/bttester.h b/src/libs/mynewt-nimble/apps/bttester/src/bttester.h
new file mode 100644
index 00000000..f4e66a6f
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/bttester.h
@@ -0,0 +1,1010 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/* bttester.h - Bluetooth tester headers */
+
+/*
+ * Copyright (c) 2015-2016 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __BTTESTER_H__
+#define __BTTESTER_H__
+
+#include "syscfg/syscfg.h"
+#include "host/ble_gatt.h"
+
+#if MYNEWT_VAL(BLE_MESH)
+#include "mesh/glue.h"
+#else
+#include "glue.h"
+#endif
+
+#define BTP_MTU MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX)
+#define BTP_DATA_MAX_SIZE (BTP_MTU - sizeof(struct btp_hdr))
+
+#define BTP_INDEX_NONE		0xff
+
+#define BTP_SERVICE_ID_CORE	0
+#define BTP_SERVICE_ID_GAP	1
+#define BTP_SERVICE_ID_GATT	2
+#define BTP_SERVICE_ID_L2CAP	3
+#define BTP_SERVICE_ID_MESH	4
+
+#define BTP_STATUS_SUCCESS	0x00
+#define BTP_STATUS_FAILED	0x01
+#define BTP_STATUS_UNKNOWN_CMD	0x02
+#define BTP_STATUS_NOT_READY	0x03
+
+#define SYS_LOG_DBG(fmt, ...) \
+	if (MYNEWT_VAL(BTTESTER_DEBUG)) { \
+		console_printf("[DBG] %s: " fmt "\n", \
+			       __func__, ## __VA_ARGS__); \
+	}
+#define SYS_LOG_INF(fmt, ...)   console_printf("[INF] %s: " fmt "\n", \
+					       __func__, ## __VA_ARGS__);
+#define SYS_LOG_ERR(fmt, ...)   console_printf("[WRN] %s: " fmt "\n", \
+					       __func__, ## __VA_ARGS__);
+
+#define SYS_LOG_LEVEL SYS_LOG_LEVEL_DEBUG
+#define SYS_LOG_DOMAIN "bttester"
+
+#define sys_cpu_to_le32 htole32
+#define sys_le32_to_cpu le32toh
+#define sys_cpu_to_le16 htole16
+
+struct btp_hdr {
+	u8_t  service;
+	u8_t  opcode;
+	u8_t  index;
+	u16_t len;
+	u8_t  data[0];
+} __packed;
+
+#define BTP_STATUS			0x00
+struct btp_status {
+	u8_t code;
+} __packed;
+
+/* Core Service */
+#define CORE_READ_SUPPORTED_COMMANDS	0x01
+struct core_read_supported_commands_rp {
+	u8_t data[0];
+} __packed;
+
+#define CORE_READ_SUPPORTED_SERVICES	0x02
+struct core_read_supported_services_rp {
+	u8_t data[0];
+} __packed;
+
+#define CORE_REGISTER_SERVICE		0x03
+struct core_register_service_cmd {
+	u8_t id;
+} __packed;
+
+#define CORE_UNREGISTER_SERVICE		0x04
+struct core_unregister_service_cmd {
+	u8_t id;
+} __packed;
+
+/* events */
+#define CORE_EV_IUT_READY		0x80
+
+/* GAP Service */
+/* commands */
+#define GAP_READ_SUPPORTED_COMMANDS	0x01
+struct gap_read_supported_commands_rp {
+	u8_t data[0];
+} __packed;
+
+#define GAP_READ_CONTROLLER_INDEX_LIST	0x02
+struct gap_read_controller_index_list_rp {
+	u8_t num;
+	u8_t index[0];
+} __packed;
+
+#define GAP_SETTINGS_POWERED		0
+#define GAP_SETTINGS_CONNECTABLE	1
+#define GAP_SETTINGS_FAST_CONNECTABLE	2
+#define GAP_SETTINGS_DISCOVERABLE	3
+#define GAP_SETTINGS_BONDABLE		4
+#define GAP_SETTINGS_LINK_SEC_3		5
+#define GAP_SETTINGS_SSP		6
+#define GAP_SETTINGS_BREDR		7
+#define GAP_SETTINGS_HS			8
+#define GAP_SETTINGS_LE			9
+#define GAP_SETTINGS_ADVERTISING	10
+#define GAP_SETTINGS_SC			11
+#define GAP_SETTINGS_DEBUG_KEYS		12
+#define GAP_SETTINGS_PRIVACY		13
+#define GAP_SETTINGS_CONTROLLER_CONFIG	14
+#define GAP_SETTINGS_STATIC_ADDRESS	15
+
+#define GAP_READ_CONTROLLER_INFO	0x03
+struct gap_read_controller_info_rp {
+	u8_t  address[6];
+	u32_t supported_settings;
+	u32_t current_settings;
+	u8_t  cod[3];
+	u8_t  name[249];
+	u8_t  short_name[11];
+} __packed;
+
+#define GAP_RESET			0x04
+struct gap_reset_rp {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_SET_POWERED			0x05
+struct gap_set_powered_cmd {
+	u8_t powered;
+} __packed;
+struct gap_set_powered_rp {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_SET_CONNECTABLE		0x06
+struct gap_set_connectable_cmd {
+	u8_t connectable;
+} __packed;
+struct gap_set_connectable_rp {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_SET_FAST_CONNECTABLE	0x07
+struct gap_set_fast_connectable_cmd {
+	u8_t fast_connectable;
+} __packed;
+struct gap_set_fast_connectable_rp {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_NON_DISCOVERABLE		0x00
+#define GAP_GENERAL_DISCOVERABLE	0x01
+#define GAP_LIMITED_DISCOVERABLE	0x02
+
+#define GAP_SET_DISCOVERABLE		0x08
+struct gap_set_discoverable_cmd {
+	u8_t discoverable;
+} __packed;
+struct gap_set_discoverable_rp {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_SET_BONDABLE		0x09
+struct gap_set_bondable_cmd {
+	u8_t bondable;
+} __packed;
+struct gap_set_bondable_rp {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_START_ADVERTISING	0x0a
+struct gap_start_advertising_cmd {
+	u8_t adv_data_len;
+	u8_t scan_rsp_len;
+	u8_t adv_data[0];
+	u8_t scan_rsp[0];
+} __packed;
+struct gap_start_advertising_rp {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_STOP_ADVERTISING		0x0b
+struct gap_stop_advertising_rp {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_DISCOVERY_FLAG_LE			0x01
+#define GAP_DISCOVERY_FLAG_BREDR		0x02
+#define GAP_DISCOVERY_FLAG_LIMITED		0x04
+#define GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN	0x08
+#define GAP_DISCOVERY_FLAG_LE_OBSERVE		0x10
+
+#define GAP_START_DISCOVERY		0x0c
+struct gap_start_discovery_cmd {
+	u8_t flags;
+} __packed;
+
+#define GAP_STOP_DISCOVERY		0x0d
+
+#define GAP_CONNECT			0x0e
+struct gap_connect_cmd {
+	u8_t address_type;
+	u8_t address[6];
+} __packed;
+
+#define GAP_DISCONNECT			0x0f
+struct gap_disconnect_cmd {
+	u8_t  address_type;
+	u8_t  address[6];
+} __packed;
+
+#define GAP_IO_CAP_DISPLAY_ONLY		0
+#define GAP_IO_CAP_DISPLAY_YESNO	1
+#define GAP_IO_CAP_KEYBOARD_ONLY	2
+#define GAP_IO_CAP_NO_INPUT_OUTPUT	3
+#define GAP_IO_CAP_KEYBOARD_DISPLAY	4
+
+#define GAP_SET_IO_CAP			0x10
+struct gap_set_io_cap_cmd {
+	u8_t io_cap;
+} __packed;
+
+#define GAP_PAIR			0x11
+struct gap_pair_cmd {
+	u8_t address_type;
+	u8_t address[6];
+} __packed;
+
+#define GAP_UNPAIR			0x12
+struct gap_unpair_cmd {
+	u8_t address_type;
+	u8_t address[6];
+} __packed;
+
+#define GAP_PASSKEY_ENTRY		0x13
+struct gap_passkey_entry_cmd {
+	u8_t  address_type;
+	u8_t  address[6];
+	u32_t passkey;
+} __packed;
+
+#define GAP_PASSKEY_CONFIRM		0x14
+struct gap_passkey_confirm_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u8_t match;
+} __packed;
+
+#define GAP_START_DIRECT_ADV		0x15
+struct gap_start_direct_adv_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u8_t high_duty;
+} __packed;
+
+#define GAP_CONN_PARAM_UPDATE		0x16
+struct gap_conn_param_update_cmd {
+    u8_t address_type;
+    u8_t address[6];
+    u16_t conn_itvl_min;
+    u16_t conn_itvl_max;
+    u16_t conn_latency;
+    u16_t supervision_timeout;
+} __packed;
+
+#define GAP_PAIRING_CONSENT_RSP		0x17
+struct gap_pairing_consent_rsp_cmd {
+    u8_t address_type;
+    u8_t address[6];
+    u8_t consent;
+} __packed;
+
+#define GAP_OOB_LEGACY_SET_DATA		0x18
+struct gap_oob_legacy_set_data_cmd {
+    u8_t oob_data[16];
+} __packed;
+
+#define GAP_OOB_SC_GET_LOCAL_DATA		0x19
+struct gap_oob_sc_get_local_data_rp {
+    u8_t r[16];
+    u8_t c[16];
+} __packed;
+
+#define GAP_OOB_SC_SET_REMOTE_DATA		0x1a
+struct gap_oob_sc_set_remote_data_cmd {
+    u8_t r[16];
+    u8_t c[16];
+} __packed;
+
+#define GAP_SET_MITM		0x1b
+struct gap_set_mitm_cmd {
+    u8_t mitm;
+} __packed;
+
+/* events */
+#define GAP_EV_NEW_SETTINGS		0x80
+struct gap_new_settings_ev {
+	u32_t current_settings;
+} __packed;
+
+#define GAP_DEVICE_FOUND_FLAG_RSSI	0x01
+#define GAP_DEVICE_FOUND_FLAG_AD	0x02
+#define GAP_DEVICE_FOUND_FLAG_SD	0x04
+
+#define GAP_EV_DEVICE_FOUND		0x81
+struct gap_device_found_ev {
+	u8_t  address_type;
+	u8_t  address[6];
+	s8_t   rssi;
+	u8_t  flags;
+	u16_t eir_data_len;
+	u8_t  eir_data[0];
+} __packed;
+
+#define GAP_EV_DEVICE_CONNECTED		0x82
+struct gap_device_connected_ev {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t conn_itvl;
+	u16_t conn_latency;
+	u16_t supervision_timeout;
+} __packed;
+
+#define GAP_EV_DEVICE_DISCONNECTED	0x83
+struct gap_device_disconnected_ev {
+	u8_t address_type;
+	u8_t address[6];
+} __packed;
+
+#define GAP_EV_PASSKEY_DISPLAY		0x84
+struct gap_passkey_display_ev {
+	u8_t  address_type;
+	u8_t  address[6];
+	u32_t passkey;
+} __packed;
+
+#define GAP_EV_PASSKEY_ENTRY_REQ	0x85
+struct gap_passkey_entry_req_ev {
+	u8_t address_type;
+	u8_t address[6];
+} __packed;
+
+#define GAP_EV_PASSKEY_CONFIRM_REQ	0x86
+struct gap_passkey_confirm_req_ev {
+	u8_t  address_type;
+	u8_t  address[6];
+	u32_t passkey;
+} __packed;
+
+#define GAP_EV_IDENTITY_RESOLVED	0x87
+struct gap_identity_resolved_ev {
+	u8_t address_type;
+	u8_t address[6];
+	u8_t identity_address_type;
+	u8_t identity_address[6];
+} __packed;
+
+#define GAP_EV_CONN_PARAM_UPDATE	0x88
+struct gap_conn_param_update_ev {
+    u8_t address_type;
+    u8_t address[6];
+    u16_t conn_itvl;
+    u16_t conn_latency;
+    u16_t supervision_timeout;
+} __packed;
+
+#define GAP_EV_SEC_LEVEL_CHANGED	0x89
+struct gap_sec_level_changed_ev {
+    u8_t address_type;
+    u8_t address[6];
+    u8_t level;
+} __packed;
+
+#define GAP_EV_PAIRING_CONSENT_REQ	0x8a
+struct gap_pairing_consent_req_ev {
+    u8_t address_type;
+    u8_t address[6];
+} __packed;
+
+/* GATT Service */
+/* commands */
+#define GATT_READ_SUPPORTED_COMMANDS	0x01
+struct gatt_read_supported_commands_rp {
+	u8_t data[0];
+} __packed;
+
+#define GATT_SERVICE_PRIMARY		0x00
+#define GATT_SERVICE_SECONDARY		0x01
+
+#define GATT_ADD_SERVICE		0x02
+struct gatt_add_service_cmd {
+	u8_t type;
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+struct gatt_add_service_rp {
+	u16_t svc_id;
+} __packed;
+
+#define GATT_ADD_CHARACTERISTIC		0x03
+struct gatt_add_characteristic_cmd {
+	u16_t svc_id;
+	u8_t properties;
+	u8_t permissions;
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+struct gatt_add_characteristic_rp {
+	u16_t char_id;
+} __packed;
+
+#define GATT_ADD_DESCRIPTOR		0x04
+struct gatt_add_descriptor_cmd {
+	u16_t char_id;
+	u8_t permissions;
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+struct gatt_add_descriptor_rp {
+	u16_t desc_id;
+} __packed;
+
+#define GATT_ADD_INCLUDED_SERVICE	0x05
+struct gatt_add_included_service_cmd {
+	u16_t svc_id;
+} __packed;
+struct gatt_add_included_service_rp {
+	u16_t included_service_id;
+} __packed;
+
+#define GATT_SET_VALUE			0x06
+	struct gatt_set_value_cmd {
+	u16_t attr_id;
+	u16_t len;
+	u8_t value[0];
+} __packed;
+
+#define GATT_START_SERVER		0x07
+struct gatt_start_server_rp {
+	u16_t db_attr_off;
+	u8_t db_attr_cnt;
+} __packed;
+
+#define GATT_SET_ENC_KEY_SIZE		0x09
+struct gatt_set_enc_key_size_cmd {
+	u16_t attr_id;
+	u8_t key_size;
+} __packed;
+
+/* Gatt Client */
+struct gatt_service {
+	u16_t start_handle;
+	u16_t end_handle;
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+
+struct gatt_included {
+	u16_t included_handle;
+	struct gatt_service service;
+} __packed;
+
+struct gatt_characteristic {
+	u16_t characteristic_handle;
+	u16_t value_handle;
+	u8_t properties;
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+
+struct gatt_descriptor {
+	u16_t descriptor_handle;
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+
+#define GATT_EXCHANGE_MTU		0x0a
+
+#define GATT_DISC_ALL_PRIM_SVCS		0x0b
+struct gatt_disc_all_prim_svcs_cmd {
+    u8_t address_type;
+    u8_t address[6];
+} __packed;
+struct gatt_disc_all_prim_svcs_rp {
+    u8_t services_count;
+    struct gatt_service services[0];
+} __packed;
+
+#define GATT_DISC_PRIM_UUID		0x0c
+struct gatt_disc_prim_uuid_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+struct gatt_disc_prim_uuid_rp {
+	u8_t services_count;
+	struct gatt_service services[0];
+} __packed;
+
+#define GATT_FIND_INCLUDED		0x0d
+struct gatt_find_included_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t start_handle;
+	u16_t end_handle;
+} __packed;
+struct gatt_find_included_rp {
+	u8_t services_count;
+	struct gatt_included included[0];
+} __packed;
+
+#define GATT_DISC_ALL_CHRC		0x0e
+struct gatt_disc_all_chrc_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t start_handle;
+	u16_t end_handle;
+} __packed;
+struct gatt_disc_chrc_rp {
+	u8_t characteristics_count;
+	struct gatt_characteristic characteristics[0];
+} __packed;
+
+#define GATT_DISC_CHRC_UUID		0x0f
+struct gatt_disc_chrc_uuid_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t start_handle;
+	u16_t end_handle;
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+
+#define GATT_DISC_ALL_DESC		0x10
+struct gatt_disc_all_desc_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t start_handle;
+	u16_t end_handle;
+} __packed;
+struct gatt_disc_all_desc_rp {
+	u8_t descriptors_count;
+	struct gatt_descriptor descriptors[0];
+} __packed;
+
+#define GATT_READ			0x11
+struct gatt_read_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t handle;
+} __packed;
+struct gatt_read_rp {
+	u8_t att_response;
+	u16_t data_length;
+	u8_t data[0];
+} __packed;
+
+#define GATT_READ_UUID			0x12
+struct gatt_read_uuid_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t start_handle;
+	u16_t end_handle;
+	u8_t uuid_length;
+	u8_t uuid[0];
+} __packed;
+
+#define GATT_READ_LONG			0x13
+struct gatt_read_long_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t handle;
+	u16_t offset;
+} __packed;
+
+#define GATT_READ_MULTIPLE		0x14
+struct gatt_read_multiple_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u8_t handles_count;
+	u16_t handles[0];
+} __packed;
+
+#define GATT_WRITE_WITHOUT_RSP		0x15
+struct gatt_write_without_rsp_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t handle;
+	u16_t data_length;
+	u8_t data[0];
+} __packed;
+
+#define GATT_SIGNED_WRITE_WITHOUT_RSP	0x16
+struct gatt_signed_write_without_rsp_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t handle;
+	u16_t data_length;
+	u8_t data[0];
+} __packed;
+
+#define GATT_WRITE			0x17
+struct gatt_write_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t handle;
+	u16_t data_length;
+	u8_t data[0];
+} __packed;
+
+#define GATT_WRITE_LONG			0x18
+struct gatt_write_long_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t handle;
+	u16_t offset;
+	u16_t data_length;
+	u8_t data[0];
+} __packed;
+
+#define GATT_RELIABLE_WRITE		0x19
+struct gatt_reliable_write_cmd {
+    u8_t address_type;
+    u8_t address[6];
+    u16_t handle;
+    u16_t offset;
+    u16_t data_length;
+    u8_t data[0];
+} __packed;
+
+#define GATT_CFG_NOTIFY			0x1a
+#define GATT_CFG_INDICATE		0x1b
+struct gatt_cfg_notify_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u8_t enable;
+	u16_t ccc_handle;
+} __packed;
+
+#define GATT_GET_ATTRIBUTES		0x1c
+struct gatt_get_attributes_cmd {
+	u16_t start_handle;
+	u16_t end_handle;
+	u8_t type_length;
+	u8_t type[0];
+} __packed;
+struct gatt_get_attributes_rp {
+	u8_t attrs_count;
+	u8_t attrs[0];
+} __packed;
+struct gatt_attr {
+	u16_t handle;
+	u8_t permission;
+	u8_t type_length;
+	u8_t type[0];
+} __packed;
+
+#define GATT_GET_ATTRIBUTE_VALUE	0x1d
+struct gatt_get_attribute_value_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t handle;
+} __packed;
+struct gatt_get_attribute_value_rp {
+	u8_t att_response;
+	u16_t value_length;
+	u8_t value[0];
+} __packed;
+
+#define GATT_CHANGE_DATABASE		0x1e
+struct gatt_change_database {
+    u16_t start_handle;
+    u16_t end_handle;
+    u8_t visibility;
+} __packed;
+
+/* GATT events */
+#define GATT_EV_NOTIFICATION		0x80
+struct gatt_notification_ev {
+	u8_t address_type;
+	u8_t address[6];
+	u8_t type;
+	u16_t handle;
+	u16_t data_length;
+	u8_t data[0];
+} __packed;
+
+#define GATT_EV_ATTR_VALUE_CHANGED	0x81
+struct gatt_attr_value_changed_ev {
+	u16_t handle;
+	u16_t data_length;
+	u8_t data[0];
+} __packed;
+
+static inline void tester_set_bit(u8_t *addr, unsigned int bit)
+{
+	u8_t *p = addr + (bit / 8);
+
+	*p |= BIT(bit % 8);
+}
+
+static inline u8_t tester_test_bit(const u8_t *addr, unsigned int bit)
+{
+	const u8_t *p = addr + (bit / 8);
+
+	return *p & BIT(bit % 8);
+}
+
+/* L2CAP Service */
+/* commands */
+#define L2CAP_READ_SUPPORTED_COMMANDS	0x01
+struct l2cap_read_supported_commands_rp {
+	u8_t data[0];
+} __packed;
+
+#define L2CAP_CONNECT			0x02
+struct l2cap_connect_cmd {
+	u8_t address_type;
+	u8_t address[6];
+	u16_t psm;
+} __packed;
+
+struct l2cap_connect_rp {
+	u8_t chan_id;
+} __packed;
+
+#define L2CAP_DISCONNECT		0x03
+struct l2cap_disconnect_cmd {
+	u8_t chan_id;
+} __packed;
+
+#define L2CAP_SEND_DATA			0x04
+struct l2cap_send_data_cmd {
+	u8_t chan_id;
+	u16_t data_len;
+	u8_t data[];
+} __packed;
+
+#define L2CAP_TRANSPORT_BREDR		0x00
+#define L2CAP_TRANSPORT_LE		0x01
+
+#define L2CAP_LISTEN			0x05
+struct l2cap_listen_cmd {
+	u16_t psm;
+	u8_t transport;
+} __packed;
+
+#define L2CAP_ACCEPT_CONNECTION		0x06
+struct l2cap_accept_connection_cmd {
+	u8_t chan_id;
+	u16_t result;
+} __packed;
+
+/* events */
+#define L2CAP_EV_CONNECTION_REQ		0x80
+struct l2cap_connection_req_ev {
+	u8_t chan_id;
+	u16_t psm;
+	u8_t address_type;
+	u8_t address[6];
+} __packed;
+
+#define L2CAP_EV_CONNECTED		0x81
+struct l2cap_connected_ev {
+	u8_t chan_id;
+	u16_t psm;
+	u8_t address_type;
+	u8_t address[6];
+} __packed;
+
+#define L2CAP_EV_DISCONNECTED		0x82
+struct l2cap_disconnected_ev {
+	u16_t result;
+	u8_t chan_id;
+	u16_t psm;
+	u8_t address_type;
+	u8_t address[6];
+} __packed;
+
+#define L2CAP_EV_DATA_RECEIVED		0x83
+struct l2cap_data_received_ev {
+	u8_t chan_id;
+	u16_t data_length;
+	u8_t data[0];
+} __packed;
+
+/* MESH Service */
+/* commands */
+#define MESH_READ_SUPPORTED_COMMANDS	0x01
+struct mesh_read_supported_commands_rp {
+	u8_t data[0];
+} __packed;
+
+#define MESH_OUT_BLINK			BIT(0)
+#define MESH_OUT_BEEP			BIT(1)
+#define MESH_OUT_VIBRATE		BIT(2)
+#define MESH_OUT_DISPLAY_NUMBER		BIT(3)
+#define MESH_OUT_DISPLAY_STRING		BIT(4)
+
+#define MESH_IN_PUSH			BIT(0)
+#define MESH_IN_TWIST			BIT(1)
+#define MESH_IN_ENTER_NUMBER		BIT(2)
+#define MESH_IN_ENTER_STRING		BIT(3)
+
+#define MESH_CONFIG_PROVISIONING	0x02
+struct mesh_config_provisioning_cmd {
+	u8_t uuid[16];
+	u8_t static_auth[16];
+	u8_t out_size;
+	u16_t out_actions;
+	u8_t in_size;
+	u16_t in_actions;
+} __packed;
+
+#define MESH_PROVISION_NODE		0x03
+struct mesh_provision_node_cmd {
+	u8_t net_key[16];
+	u16_t net_key_idx;
+	u8_t flags;
+	u32_t iv_index;
+	u32_t seq_num;
+	u16_t addr;
+	u8_t dev_key[16];
+} __packed;
+
+#define MESH_INIT			0x04
+#define MESH_RESET			0x05
+#define MESH_INPUT_NUMBER		0x06
+struct mesh_input_number_cmd {
+	u32_t number;
+} __packed;
+
+#define MESH_INPUT_STRING		0x07
+struct mesh_input_string_cmd {
+	u8_t string_len;
+	u8_t string[0];
+} __packed;
+
+#define MESH_IVU_TEST_MODE		0x08
+struct mesh_ivu_test_mode_cmd {
+	u8_t enable;
+} __packed;
+
+#define MESH_IVU_TOGGLE_STATE			0x09
+
+#define MESH_NET_SEND			0x0a
+struct mesh_net_send_cmd {
+	u8_t ttl;
+	u16_t src;
+	u16_t dst;
+	u8_t payload_len;
+	u8_t payload[0];
+} __packed;
+
+#define MESH_HEALTH_GENERATE_FAULTS	0x0b
+struct mesh_health_generate_faults_rp {
+	u8_t test_id;
+	u8_t cur_faults_count;
+	u8_t reg_faults_count;
+	u8_t current_faults[0];
+	u8_t registered_faults[0];
+} __packed;
+
+#define MESH_HEALTH_CLEAR_FAULTS	0x0c
+
+#define MESH_LPN			0x0d
+struct mesh_lpn_set_cmd {
+	u8_t enable;
+} __packed;
+
+#define MESH_LPN_POLL			0x0e
+
+#define MESH_MODEL_SEND			0x0f
+struct mesh_model_send_cmd {
+	u16_t src;
+	u16_t dst;
+	u8_t payload_len;
+	u8_t payload[0];
+} __packed;
+
+#define MESH_LPN_SUBSCRIBE		0x10
+struct mesh_lpn_subscribe_cmd {
+	u16_t address;
+} __packed;
+
+#define MESH_LPN_UNSUBSCRIBE		0x11
+struct mesh_lpn_unsubscribe_cmd {
+	u16_t address;
+} __packed;
+
+#define MESH_RPL_CLEAR			0x12
+#define MESH_PROXY_IDENTITY		0x13
+
+/* events */
+#define MESH_EV_OUT_NUMBER_ACTION	0x80
+struct mesh_out_number_action_ev {
+	u16_t action;
+	u32_t number;
+} __packed;
+
+#define MESH_EV_OUT_STRING_ACTION	0x81
+struct mesh_out_string_action_ev {
+	u8_t string_len;
+	u8_t string[0];
+} __packed;
+
+#define MESH_EV_IN_ACTION		0x82
+struct mesh_in_action_ev {
+	u16_t action;
+	u8_t size;
+} __packed;
+
+#define MESH_EV_PROVISIONED		0x83
+
+#define MESH_PROV_BEARER_PB_ADV		0x00
+#define MESH_PROV_BEARER_PB_GATT	0x01
+#define MESH_EV_PROV_LINK_OPEN		0x84
+struct mesh_prov_link_open_ev {
+	u8_t bearer;
+} __packed;
+
+#define MESH_EV_PROV_LINK_CLOSED	0x85
+struct mesh_prov_link_closed_ev {
+	u8_t bearer;
+} __packed;
+
+#define MESH_EV_NET_RECV		0x86
+struct mesh_net_recv_ev {
+	u8_t ttl;
+	u8_t ctl;
+	u16_t src;
+	u16_t dst;
+	u8_t payload_len;
+	u8_t payload[0];
+} __packed;
+
+#define MESH_EV_INVALID_BEARER		0x87
+struct mesh_invalid_bearer_ev {
+	u8_t opcode;
+} __packed;
+
+#define MESH_EV_INCOMP_TIMER_EXP	0x88
+
+void tester_init(void);
+void tester_rsp(u8_t service, u8_t opcode, u8_t index, u8_t status);
+void tester_send(u8_t service, u8_t opcode, u8_t index, u8_t *data,
+		 size_t len);
+void tester_send_buf(u8_t service, u8_t opcode, u8_t index,
+		     struct os_mbuf *buf);
+
+u8_t tester_init_gap(void);
+u8_t tester_unregister_gap(void);
+void tester_handle_gap(u8_t opcode, u8_t index, u8_t *data,
+		       u16_t len);
+u8_t tester_init_gatt(void);
+u8_t tester_unregister_gatt(void);
+void tester_handle_gatt(u8_t opcode, u8_t index, u8_t *data,
+			u16_t len);
+int tester_gatt_notify_rx_ev(u16_t conn_handle, u16_t attr_handle,
+			     u8_t indication, struct os_mbuf *om);
+int tester_gatt_subscribe_ev(u16_t conn_handle, u16_t attr_handle, u8_t reason,
+			     u8_t prev_notify, u8_t cur_notify,
+			     u8_t prev_indicate, u8_t cur_indicate);
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+u8_t tester_init_l2cap(void);
+u8_t tester_unregister_l2cap(void);
+void tester_handle_l2cap(u8_t opcode, u8_t index, u8_t *data,
+			 u16_t len);
+#endif
+
+#if MYNEWT_VAL(BLE_MESH)
+u8_t tester_init_mesh(void);
+u8_t tester_unregister_mesh(void);
+void tester_handle_mesh(u8_t opcode, u8_t index, u8_t *data, u16_t len);
+#endif /* MYNEWT_VAL(BLE_MESH) */
+
+void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg);
+int gatt_svr_init(void);
+
+#endif /* __BTTESTER_H__ */
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/bttester_pipe.h b/src/libs/mynewt-nimble/apps/bttester/src/bttester_pipe.h
new file mode 100644
index 00000000..c54d42de
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/bttester_pipe.h
@@ -0,0 +1,40 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef __BTTESTER_PIPE_H__
+#define __BTTESTER_PIPE_H__
+
+#include <stdlib.h>
+#include "bttester.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef u8_t *(*bttester_pipe_recv_cb)(u8_t *buf, size_t *off);
+void bttester_pipe_register(u8_t *buf, size_t len, bttester_pipe_recv_cb cb);
+int bttester_pipe_send(const u8_t *data, int len);
+int bttester_pipe_send_buf(struct os_mbuf *buf);
+int bttester_pipe_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BTTESTER_PIPE_H__ */
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/gap.c b/src/libs/mynewt-nimble/apps/bttester/src/gap.c
new file mode 100644
index 00000000..9d6de043
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/gap.c
@@ -0,0 +1,1688 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/* gap.c - Bluetooth GAP Tester */
+
+/*
+ * Copyright (c) 2015-2016 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "host/ble_gap.h"
+#include "host/util/util.h"
+#include "console/console.h"
+
+#include "../../../nimble/host/src/ble_hs_pvcy_priv.h"
+#include "../../../nimble/host/src/ble_hs_hci_priv.h"
+#include "../../../nimble/host/src/ble_sm_priv.h"
+
+#include "bttester.h"
+
+#define CONTROLLER_INDEX 0
+#define CONTROLLER_NAME "btp_tester"
+
+#define BLE_AD_DISCOV_MASK (BLE_HS_ADV_F_DISC_LTD | BLE_HS_ADV_F_DISC_GEN)
+#define ADV_BUF_LEN (sizeof(struct gap_device_found_ev) + 2 * 31)
+
+const uint8_t irk[16] = {
+	0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11,
+	0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11,
+};
+
+static uint8_t oob[16];
+static struct ble_sm_sc_oob_data oob_data_local;
+static struct ble_sm_sc_oob_data oob_data_remote;
+
+static uint16_t current_settings;
+u8_t own_addr_type;
+static ble_addr_t peer_id_addr;
+static ble_addr_t peer_ota_addr;
+static bool encrypted = false;
+
+static struct os_callout update_params_co;
+static struct gap_conn_param_update_cmd update_params;
+
+static struct os_callout connected_ev_co;
+static struct gap_device_connected_ev connected_ev;
+#define CONNECTED_EV_DELAY_MS(itvl) 8 * BLE_HCI_CONN_ITVL * itvl / 1000
+static int connection_attempts;
+
+static const struct ble_gap_conn_params dflt_conn_params = {
+	.scan_itvl = 0x0010,
+	.scan_window = 0x0010,
+	.itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN,
+	.itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX,
+	.latency = 0,
+	.supervision_timeout = 0x0100,
+	.min_ce_len = 0x0010,
+	.max_ce_len = 0x0300,
+};
+
+static void conn_param_update(struct os_event *ev);
+
+
+static int gap_conn_find_by_addr(const ble_addr_t *dev_addr,
+				 struct ble_gap_conn_desc *out_desc)
+{
+	ble_addr_t addr = *dev_addr;
+
+	if (memcmp(BLE_ADDR_ANY, &peer_id_addr, 6) == 0) {
+		return ble_gap_conn_find_by_addr(&addr, out_desc);
+	}
+
+	if (BLE_ADDR_IS_RPA(&addr)) {
+		if(ble_addr_cmp(&peer_ota_addr, &addr) != 0) {
+			return -1;
+		}
+
+		return ble_gap_conn_find_by_addr(&addr, out_desc);
+	} else {
+		if(ble_addr_cmp(&peer_id_addr, &addr) != 0) {
+			return -1;
+		}
+
+		if (BLE_ADDR_IS_RPA(&peer_ota_addr)) {
+			/* Change addr type to ID addr */
+			addr.type |= 2;
+		}
+
+		return ble_gap_conn_find_by_addr(&addr, out_desc);
+	}
+}
+
+static int gap_event_cb(struct ble_gap_event *event, void *arg);
+
+static void supported_commands(u8_t *data, u16_t len)
+{
+	u8_t cmds[3];
+	struct gap_read_supported_commands_rp *rp = (void *) &cmds;
+
+	SYS_LOG_DBG("");
+
+	memset(cmds, 0, sizeof(cmds));
+
+	tester_set_bit(cmds, GAP_READ_SUPPORTED_COMMANDS);
+	tester_set_bit(cmds, GAP_READ_CONTROLLER_INDEX_LIST);
+	tester_set_bit(cmds, GAP_READ_CONTROLLER_INFO);
+	tester_set_bit(cmds, GAP_SET_CONNECTABLE);
+	tester_set_bit(cmds, GAP_SET_DISCOVERABLE);
+	tester_set_bit(cmds, GAP_SET_BONDABLE);
+	tester_set_bit(cmds, GAP_START_ADVERTISING);
+	tester_set_bit(cmds, GAP_STOP_ADVERTISING);
+	tester_set_bit(cmds, GAP_START_DISCOVERY);
+	tester_set_bit(cmds, GAP_STOP_DISCOVERY);
+	tester_set_bit(cmds, GAP_CONNECT);
+	tester_set_bit(cmds, GAP_DISCONNECT);
+	tester_set_bit(cmds, GAP_SET_IO_CAP);
+	tester_set_bit(cmds, GAP_PAIR);
+	tester_set_bit(cmds, GAP_UNPAIR);
+	tester_set_bit(cmds, GAP_PASSKEY_ENTRY);
+	tester_set_bit(cmds, GAP_PASSKEY_CONFIRM);
+	tester_set_bit(cmds, GAP_START_DIRECT_ADV);
+	tester_set_bit(cmds, GAP_CONN_PARAM_UPDATE);
+	tester_set_bit(cmds, GAP_OOB_LEGACY_SET_DATA);
+	tester_set_bit(cmds, GAP_OOB_SC_GET_LOCAL_DATA);
+	tester_set_bit(cmds, GAP_OOB_SC_SET_REMOTE_DATA);
+	tester_set_bit(cmds, GAP_SET_MITM);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_READ_SUPPORTED_COMMANDS,
+		    CONTROLLER_INDEX, (u8_t *) rp, sizeof(cmds));
+}
+
+static void controller_index_list(u8_t *data,  u16_t len)
+{
+	struct gap_read_controller_index_list_rp *rp;
+	u8_t buf[sizeof(*rp) + 1];
+
+	SYS_LOG_DBG("");
+
+	rp = (void *) buf;
+
+	rp->num = 1;
+	rp->index[0] = CONTROLLER_INDEX;
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_READ_CONTROLLER_INDEX_LIST,
+		    BTP_INDEX_NONE, (u8_t *) rp, sizeof(buf));
+}
+
+static int check_pub_addr_unassigned(void)
+{
+#ifdef ARCH_sim
+	return 0;
+#else
+	uint8_t zero_addr[BLE_DEV_ADDR_LEN] = { 0 };
+
+	return memcmp(MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR),
+		      zero_addr, BLE_DEV_ADDR_LEN) == 0;
+#endif
+}
+
+static void controller_info(u8_t *data, u16_t len)
+{
+	struct gap_read_controller_info_rp rp;
+	u32_t supported_settings = 0;
+	ble_addr_t addr;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_hs_pvcy_set_our_irk(irk);
+	assert(rc == 0);
+
+	memset(&rp, 0, sizeof(rp));
+
+	/* Make sure we have proper identity address set (public preferred) */
+	rc = ble_hs_util_ensure_addr(1);
+	assert(rc == 0);
+	rc = ble_hs_id_copy_addr(BLE_ADDR_RANDOM, addr.val, NULL);
+	assert(rc == 0);
+
+	if (MYNEWT_VAL(BTTESTER_PRIVACY_MODE)) {
+		if (MYNEWT_VAL(BTTESTER_USE_NRPA)) {
+			own_addr_type = BLE_OWN_ADDR_RANDOM;
+			rc = ble_hs_id_gen_rnd(1, &addr);
+			assert(rc == 0);
+			rc = ble_hs_id_set_rnd(addr.val);
+			assert(rc == 0);
+		} else {
+			own_addr_type = BLE_OWN_ADDR_RPA_RANDOM_DEFAULT;
+		}
+		current_settings |= BIT(GAP_SETTINGS_PRIVACY);
+		supported_settings |= BIT(GAP_SETTINGS_PRIVACY);
+		memcpy(rp.address, addr.val, sizeof(rp.address));
+	} else {
+		if (check_pub_addr_unassigned()) {
+			own_addr_type = BLE_OWN_ADDR_RANDOM;
+			memcpy(rp.address, addr.val, sizeof(rp.address));
+			supported_settings |= BIT(GAP_SETTINGS_STATIC_ADDRESS);
+			current_settings |= BIT(GAP_SETTINGS_STATIC_ADDRESS);
+		} else {
+			own_addr_type = BLE_OWN_ADDR_PUBLIC;
+			memcpy(rp.address, MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR),
+			       sizeof(rp.address));
+		}
+	}
+
+	supported_settings |= BIT(GAP_SETTINGS_POWERED);
+	supported_settings |= BIT(GAP_SETTINGS_CONNECTABLE);
+	supported_settings |= BIT(GAP_SETTINGS_BONDABLE);
+	supported_settings |= BIT(GAP_SETTINGS_LE);
+	supported_settings |= BIT(GAP_SETTINGS_ADVERTISING);
+	supported_settings |= BIT(GAP_SETTINGS_SC);
+
+	if (ble_hs_cfg.sm_bonding) {
+		current_settings |= BIT(GAP_SETTINGS_BONDABLE);
+	}
+	if (ble_hs_cfg.sm_sc) {
+		current_settings |= BIT(GAP_SETTINGS_SC);
+	}
+
+	rp.supported_settings = sys_cpu_to_le32(supported_settings);
+	rp.current_settings = sys_cpu_to_le32(current_settings);
+
+	memcpy(rp.name, CONTROLLER_NAME, sizeof(CONTROLLER_NAME));
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_READ_CONTROLLER_INFO,
+		    CONTROLLER_INDEX, (u8_t *) &rp, sizeof(rp));
+}
+
+static struct ble_gap_adv_params adv_params = {
+	.conn_mode = BLE_GAP_CONN_MODE_NON,
+	.disc_mode = BLE_GAP_DISC_MODE_NON,
+};
+
+static void set_connectable(u8_t *data, u16_t len)
+{
+	const struct gap_set_connectable_cmd *cmd = (void *) data;
+	struct gap_set_connectable_rp rp;
+
+	SYS_LOG_DBG("");
+
+	if (cmd->connectable) {
+		current_settings |= BIT(GAP_SETTINGS_CONNECTABLE);
+		adv_params.conn_mode = BLE_GAP_CONN_MODE_UND;
+	} else {
+		current_settings &= ~BIT(GAP_SETTINGS_CONNECTABLE);
+		adv_params.conn_mode = BLE_GAP_CONN_MODE_NON;
+	}
+
+	rp.current_settings = sys_cpu_to_le32(current_settings);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_SET_CONNECTABLE, CONTROLLER_INDEX,
+		    (u8_t *) &rp, sizeof(rp));
+}
+
+static u8_t ad_flags = BLE_HS_ADV_F_BREDR_UNSUP;
+
+static void set_discoverable(u8_t *data, u16_t len)
+{
+	const struct gap_set_discoverable_cmd *cmd = (void *) data;
+	struct gap_set_discoverable_rp rp;
+
+	SYS_LOG_DBG("");
+
+	switch (cmd->discoverable) {
+	case GAP_NON_DISCOVERABLE:
+		ad_flags &= ~(BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_DISC_LTD);
+		adv_params.disc_mode = BLE_GAP_DISC_MODE_NON;
+		current_settings &= ~BIT(GAP_SETTINGS_DISCOVERABLE);
+		break;
+	case GAP_GENERAL_DISCOVERABLE:
+		ad_flags &= ~BLE_HS_ADV_F_DISC_LTD;
+		ad_flags |= BLE_HS_ADV_F_DISC_GEN;
+		adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN;
+		current_settings |= BIT(GAP_SETTINGS_DISCOVERABLE);
+		break;
+	case GAP_LIMITED_DISCOVERABLE:
+		ad_flags &= ~BLE_HS_ADV_F_DISC_GEN;
+		ad_flags |= BLE_HS_ADV_F_DISC_LTD;
+		adv_params.disc_mode = BLE_GAP_DISC_MODE_LTD;
+		current_settings |= BIT(GAP_SETTINGS_DISCOVERABLE);
+		break;
+	default:
+		tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		return;
+	}
+
+	rp.current_settings = sys_cpu_to_le32(current_settings);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE, CONTROLLER_INDEX,
+		    (u8_t *) &rp, sizeof(rp));
+}
+
+static void set_bondable(const u8_t *data, u16_t len)
+{
+	const struct gap_set_bondable_cmd *cmd = (void *) data;
+	struct gap_set_bondable_rp rp;
+
+	SYS_LOG_DBG("");
+
+	ble_hs_cfg.sm_bonding = cmd->bondable;
+	if (ble_hs_cfg.sm_bonding) {
+		current_settings |= BIT(GAP_SETTINGS_BONDABLE);
+	} else {
+		current_settings &= ~BIT(GAP_SETTINGS_BONDABLE);
+	}
+
+	rp.current_settings = sys_cpu_to_le32(current_settings);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_SET_BONDABLE, CONTROLLER_INDEX,
+		    (u8_t *) &rp, sizeof(rp));
+}
+
+static struct bt_data ad[10] = {
+	BT_DATA(BLE_HS_ADV_TYPE_FLAGS, &ad_flags, sizeof(ad_flags)),
+};
+static struct bt_data sd[10];
+
+static int set_ad(const struct bt_data *ad, size_t ad_len,
+		  u8_t *buf, u8_t *buf_len)
+{
+	int i;
+
+	for (i = 0; i < ad_len; i++) {
+		buf[(*buf_len)++] = ad[i].data_len + 1;
+		buf[(*buf_len)++] = ad[i].type;
+
+		memcpy(&buf[*buf_len], ad[i].data,
+		       ad[i].data_len);
+		*buf_len += ad[i].data_len;
+	}
+
+	return 0;
+}
+
+static void start_advertising(const u8_t *data, u16_t len)
+{
+	const struct gap_start_advertising_cmd *cmd = (void *) data;
+	struct gap_start_advertising_rp rp;
+	int32_t duration_ms = BLE_HS_FOREVER;
+	uint8_t buf[BLE_HS_ADV_MAX_SZ];
+	uint8_t buf_len = 0;
+	u8_t adv_len, sd_len;
+	int err;
+
+	int i;
+
+	SYS_LOG_DBG("");
+
+	for (i = 0, adv_len = 1; i < cmd->adv_data_len; adv_len++) {
+		if (adv_len >= ARRAY_SIZE(ad)) {
+			SYS_LOG_ERR("ad[] Out of memory");
+			goto fail;
+		}
+
+		ad[adv_len].type = cmd->adv_data[i++];
+		ad[adv_len].data_len = cmd->adv_data[i++];
+		ad[adv_len].data = &cmd->adv_data[i];
+		i += ad[adv_len].data_len;
+	}
+
+	for (i = 0, sd_len = 0; i < cmd->scan_rsp_len; sd_len++) {
+		if (sd_len >= ARRAY_SIZE(sd)) {
+			SYS_LOG_ERR("sd[] Out of memory");
+			goto fail;
+		}
+
+		sd[sd_len].type = cmd->scan_rsp[i++];
+		sd[sd_len].data_len = cmd->scan_rsp[i++];
+		sd[sd_len].data = &cmd->scan_rsp[i];
+		i += sd[sd_len].data_len;
+	}
+
+	err = set_ad(ad, adv_len, buf, &buf_len);
+	if (err) {
+		goto fail;
+	}
+
+	err = ble_gap_adv_set_data(buf, buf_len);
+	if (err != 0) {
+		goto fail;
+	}
+
+	if (sd_len) {
+		buf_len = 0;
+
+		err = set_ad(sd, sd_len, buf, &buf_len);
+		if (err) {
+			SYS_LOG_ERR("Advertising failed: err %d", err);
+			goto fail;
+		}
+
+		err = ble_gap_adv_rsp_set_data(buf, buf_len);
+		if (err != 0) {
+			SYS_LOG_ERR("Advertising failed: err %d", err);
+			goto fail;
+		}
+	}
+
+	if (adv_params.disc_mode == BLE_GAP_DISC_MODE_LTD) {
+		duration_ms = MYNEWT_VAL(BTTESTER_LTD_ADV_TIMEOUT);
+	}
+
+	err = ble_gap_adv_start(own_addr_type, NULL, duration_ms,
+				&adv_params, gap_event_cb, NULL);
+	if (err) {
+		SYS_LOG_ERR("Advertising failed: err %d", err);
+		goto fail;
+	}
+
+	current_settings |= BIT(GAP_SETTINGS_ADVERTISING);
+	rp.current_settings = sys_cpu_to_le32(current_settings);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX,
+		    (u8_t *) &rp, sizeof(rp));
+	return;
+fail:
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void stop_advertising(const u8_t *data, u16_t len)
+{
+	struct gap_stop_advertising_rp rp;
+
+	SYS_LOG_DBG("");
+
+	if (ble_gap_adv_stop() != 0) {
+		tester_rsp(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		return;
+	}
+
+	current_settings &= ~BIT(GAP_SETTINGS_ADVERTISING);
+	rp.current_settings = sys_cpu_to_le32(current_settings);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING, CONTROLLER_INDEX,
+		    (u8_t *) &rp, sizeof(rp));
+}
+
+static u8_t get_ad_flags(const u8_t *data, u8_t data_len)
+{
+	u8_t len, i;
+
+	/* Parse advertisement to get flags */
+	for (i = 0; i < data_len; i += len - 1) {
+		len = data[i++];
+		if (!len) {
+			break;
+		}
+
+		/* Check if field length is correct */
+		if (len > (data_len - i) || (data_len - i) < 1) {
+			break;
+		}
+
+		switch (data[i++]) {
+		case BLE_HS_ADV_TYPE_FLAGS:
+			return data[i];
+		default:
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static u8_t discovery_flags;
+static struct os_mbuf *adv_buf;
+
+static void store_adv(const ble_addr_t *addr, s8_t rssi,
+		      const u8_t *data, u8_t len)
+{
+	struct gap_device_found_ev *ev;
+
+	/* cleanup */
+	net_buf_simple_init(adv_buf, 0);
+
+	ev = net_buf_simple_add(adv_buf, sizeof(*ev));
+
+	memcpy(ev->address, addr->val, sizeof(ev->address));
+	ev->address_type = addr->type;
+	ev->rssi = rssi;
+	ev->flags = GAP_DEVICE_FOUND_FLAG_AD | GAP_DEVICE_FOUND_FLAG_RSSI;
+	ev->eir_data_len = len;
+	memcpy(net_buf_simple_add(adv_buf, len), data, len);
+}
+
+static void device_found(ble_addr_t *addr, s8_t rssi, u8_t evtype,
+			 const u8_t *data, u8_t len)
+{
+	struct gap_device_found_ev *ev;
+	ble_addr_t a;
+
+	/* if General/Limited Discovery - parse Advertising data to get flags */
+	if (!(discovery_flags & GAP_DISCOVERY_FLAG_LE_OBSERVE) &&
+	    (evtype != BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP)) {
+		u8_t flags = get_ad_flags(data, len);
+
+		/* ignore non-discoverable devices */
+		if (!(flags & BLE_AD_DISCOV_MASK)) {
+			SYS_LOG_DBG("Non discoverable, skipping");
+			return;
+		}
+
+		/* if Limited Discovery - ignore general discoverable devices */
+		if ((discovery_flags & GAP_DISCOVERY_FLAG_LIMITED) &&
+		    !(flags & BLE_HS_ADV_F_DISC_LTD)) {
+			SYS_LOG_DBG("General discoverable, skipping");
+			return;
+		}
+	}
+
+	/* attach Scan Response data */
+	if (evtype == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
+		/* skip if there is no pending advertisement */
+		if (!adv_buf->om_len) {
+			SYS_LOG_INF("No pending advertisement, skipping");
+			return;
+		}
+
+		ev = (void *) adv_buf->om_data;
+		a.type = ev->address_type;
+		memcpy(a.val, ev->address, sizeof(a.val));
+
+		/*
+		 * in general, the Scan Response comes right after the
+		 * Advertisement, but if not if send stored event and ignore
+		 * this one
+		 */
+		if (ble_addr_cmp(addr, &a)) {
+			SYS_LOG_INF("Address does not match, skipping");
+			goto done;
+		}
+
+		ev->eir_data_len += len;
+		ev->flags |= GAP_DEVICE_FOUND_FLAG_SD;
+
+		memcpy(net_buf_simple_add(adv_buf, len), data, len);
+
+		goto done;
+	}
+
+	/*
+	 * if there is another pending advertisement, send it and store the
+	 * current one
+	 */
+	if (adv_buf->om_len) {
+		tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_FOUND,
+			    CONTROLLER_INDEX, adv_buf->om_data,
+			    adv_buf->om_len);
+	}
+
+	store_adv(addr, rssi, data, len);
+
+	/* if Active Scan and scannable event - wait for Scan Response */
+	if ((discovery_flags & GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN) &&
+	    (evtype == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND ||
+	     evtype == BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND)) {
+		SYS_LOG_DBG("Waiting for scan response");
+		return;
+	}
+done:
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_FOUND,
+		    CONTROLLER_INDEX, adv_buf->om_data, adv_buf->om_len);
+}
+
+static int discovery_cb(struct ble_gap_event *event, void *arg)
+{
+	if (event->type == BLE_GAP_EVENT_DISC) {
+		device_found(&event->disc.addr, event->disc.rssi,
+			     event->disc.event_type, event->disc.data,
+			     event->disc.length_data);
+	}
+
+	return 0;
+}
+
+static void start_discovery(const u8_t *data, u16_t len)
+{
+	const struct gap_start_discovery_cmd *cmd = (void *) data;
+	struct ble_gap_disc_params params = {0};
+	u8_t status;
+
+	SYS_LOG_DBG("");
+
+	/* only LE scan is supported */
+	if (cmd->flags & GAP_DISCOVERY_FLAG_BREDR) {
+		status = BTP_STATUS_FAILED;
+		goto reply;
+	}
+
+	params.passive = (cmd->flags & GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN) == 0;
+	params.limited = (cmd->flags & GAP_DISCOVERY_FLAG_LIMITED) > 0;
+	params.filter_duplicates = 1;
+
+	if (ble_gap_disc(own_addr_type, BLE_HS_FOREVER,
+			 &params, discovery_cb, NULL) != 0) {
+		status = BTP_STATUS_FAILED;
+		goto reply;
+	}
+
+	net_buf_simple_init(adv_buf, 0);
+	discovery_flags = cmd->flags;
+
+	status = BTP_STATUS_SUCCESS;
+reply:
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_DISCOVERY, CONTROLLER_INDEX,
+		   status);
+}
+
+static void stop_discovery(const u8_t *data, u16_t len)
+{
+	u8_t status = BTP_STATUS_SUCCESS;
+
+	SYS_LOG_DBG("");
+
+	if (ble_gap_disc_cancel() != 0) {
+		status = BTP_STATUS_FAILED;
+	}
+
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_STOP_DISCOVERY, CONTROLLER_INDEX,
+		   status);
+}
+
+
+/* Bluetooth Core Spec v5.1 | Section 10.7.1
+ * If a privacy-enabled Peripheral, that has a stored bond,
+ * receives a resolvable private address, the Host may resolve
+ * the resolvable private address [...]
+ * If the resolution is successful, the Host may accept the connection.
+ * If the resolution procedure fails, then the Host shall disconnect
+ * with the error code "Authentication failure" [...]
+ */
+static void periph_privacy(struct ble_gap_conn_desc desc)
+{
+#if !MYNEWT_VAL(BTTESTER_PRIVACY_MODE)
+	return;
+#endif
+	int count;
+
+	SYS_LOG_DBG("");
+
+	ble_store_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC, &count);
+	if (count > 0 && BLE_ADDR_IS_RPA(&desc.peer_id_addr)) {
+		SYS_LOG_DBG("Authentication failure, disconnecting");
+		ble_gap_terminate(desc.conn_handle, BLE_ERR_AUTH_FAIL);
+	}
+}
+
+static void device_connected_ev_send(struct os_event *ev)
+{
+	struct ble_gap_conn_desc desc;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = gap_conn_find_by_addr((ble_addr_t *)&connected_ev, &desc);
+	if (rc) {
+		tester_rsp(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		return;
+	}
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED,
+		    CONTROLLER_INDEX, (u8_t *) &connected_ev,
+		    sizeof(connected_ev));
+
+	periph_privacy(desc);
+}
+
+static void le_connected(u16_t conn_handle, int status)
+{
+	struct ble_gap_conn_desc desc;
+	ble_addr_t *addr;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	if (status != 0) {
+		return;
+	}
+
+	rc = ble_gap_conn_find(conn_handle, &desc);
+	if (rc) {
+		return;
+	}
+
+	peer_id_addr = desc.peer_id_addr;
+	peer_ota_addr = desc.peer_ota_addr;
+
+	addr = &desc.peer_id_addr;
+
+	memcpy(connected_ev.address, addr->val, sizeof(connected_ev.address));
+	connected_ev.address_type = addr->type;
+	connected_ev.conn_itvl = desc.conn_itvl;
+	connected_ev.conn_latency = desc.conn_latency;
+	connected_ev.supervision_timeout = desc.supervision_timeout;
+
+#if MYNEWT_VAL(BTTESTER_CONN_RETRY)
+	os_callout_reset(&connected_ev_co,
+			 os_time_ms_to_ticks32(
+				 CONNECTED_EV_DELAY_MS(desc.conn_itvl)));
+#else
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED,
+		    CONTROLLER_INDEX, (u8_t *) &connected_ev,
+		    sizeof(connected_ev));
+#endif
+}
+
+static void le_disconnected(struct ble_gap_conn_desc *conn, int reason)
+{
+	struct gap_device_disconnected_ev ev;
+	ble_addr_t *addr = &conn->peer_ota_addr;
+
+	SYS_LOG_DBG("");
+
+#if MYNEWT_VAL(BTTESTER_CONN_RETRY)
+	int rc;
+
+	if ((reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_ESTABLISHMENT)) &&
+	    os_callout_queued(&connected_ev_co)) {
+		if (connection_attempts < MYNEWT_VAL(BTTESTER_CONN_RETRY)) {
+			os_callout_stop(&connected_ev_co);
+
+			/* try connecting again */
+			rc = ble_gap_connect(own_addr_type, addr, 0,
+					     &dflt_conn_params, gap_event_cb,
+					     NULL);
+
+			if (rc == 0) {
+				connection_attempts++;
+				return;
+			}
+		}
+	} else if (os_callout_queued(&connected_ev_co)) {
+		os_callout_stop(&connected_ev_co);
+		return;
+	}
+#endif
+
+	connection_attempts = 0;
+	memset(&connected_ev, 0, sizeof(connected_ev));
+
+	memcpy(ev.address, addr->val, sizeof(ev.address));
+	ev.address_type = addr->type;
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_DISCONNECTED,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void auth_passkey_oob(u16_t conn_handle)
+{
+	struct ble_gap_conn_desc desc;
+	struct ble_sm_io pk;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find(conn_handle, &desc);
+	if (rc) {
+		return;
+	}
+
+	memcpy(pk.oob, oob, sizeof(oob));
+	pk.action = BLE_SM_IOACT_OOB;
+
+	rc = ble_sm_inject_io(conn_handle, &pk);
+	assert(rc == 0);
+}
+
+static void auth_passkey_display(u16_t conn_handle, unsigned int passkey)
+{
+	struct ble_gap_conn_desc desc;
+	struct gap_passkey_display_ev ev;
+	ble_addr_t *addr;
+	struct ble_sm_io pk;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find(conn_handle, &desc);
+	if (rc) {
+		return;
+	}
+
+	rc = ble_hs_hci_util_rand(&pk.passkey, sizeof(pk.passkey));
+	assert(rc == 0);
+	/* Max value is 999999 */
+	pk.passkey %= 1000000;
+	pk.action = BLE_SM_IOACT_DISP;
+
+	rc = ble_sm_inject_io(conn_handle, &pk);
+	assert(rc == 0);
+
+	addr = &desc.peer_ota_addr;
+
+	memcpy(ev.address, addr->val, sizeof(ev.address));
+	ev.address_type = addr->type;
+	ev.passkey = sys_cpu_to_le32(pk.passkey);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_DISPLAY,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void auth_passkey_entry(u16_t conn_handle)
+{
+	struct ble_gap_conn_desc desc;
+	struct gap_passkey_entry_req_ev ev;
+	ble_addr_t *addr;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find(conn_handle, &desc);
+	if (rc) {
+		return;
+	}
+
+	addr = &desc.peer_ota_addr;
+
+	memcpy(ev.address, addr->val, sizeof(ev.address));
+	ev.address_type = addr->type;
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_ENTRY_REQ,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void auth_passkey_numcmp(u16_t conn_handle, unsigned int passkey)
+{
+	struct ble_gap_conn_desc desc;
+	struct gap_passkey_confirm_req_ev ev;
+	ble_addr_t *addr;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find(conn_handle, &desc);
+	if (rc) {
+		return;
+	}
+
+	addr = &desc.peer_ota_addr;
+
+	memcpy(ev.address, addr->val, sizeof(ev.address));
+	ev.address_type = addr->type;
+	ev.passkey = sys_cpu_to_le32(passkey);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_CONFIRM_REQ,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void auth_passkey_oob_sc(u16_t conn_handle)
+{
+	int rc;
+	struct ble_sm_io pk;
+
+	SYS_LOG_DBG("");
+
+	memset(&pk, 0, sizeof(pk));
+
+	pk.oob_sc_data.local = &oob_data_local;
+
+	if (ble_hs_cfg.sm_oob_data_flag) {
+		pk.oob_sc_data.remote = &oob_data_remote;
+	}
+
+	pk.action = BLE_SM_IOACT_OOB_SC;
+	rc = ble_sm_inject_io(conn_handle, &pk);
+	if (rc != 0) {
+		console_printf("error providing oob; rc=%d\n", rc);
+	}
+}
+
+static void le_passkey_action(u16_t conn_handle,
+			      struct ble_gap_passkey_params *params)
+{
+	SYS_LOG_DBG("");
+
+	switch (params->action) {
+	case BLE_SM_IOACT_NONE:
+		break;
+	case BLE_SM_IOACT_OOB:
+		auth_passkey_oob(conn_handle);
+		break;
+	case BLE_SM_IOACT_INPUT:
+		auth_passkey_entry(conn_handle);
+		break;
+	case BLE_SM_IOACT_DISP:
+		auth_passkey_display(conn_handle, params->numcmp);
+		break;
+	case BLE_SM_IOACT_NUMCMP:
+		auth_passkey_numcmp(conn_handle, params->numcmp);
+		break;
+	case BLE_SM_IOACT_OOB_SC:
+		auth_passkey_oob_sc(conn_handle);
+		break;
+	default:
+		assert(0);
+	}
+}
+
+static void le_identity_resolved(u16_t conn_handle)
+{
+	struct ble_gap_conn_desc desc;
+	struct gap_identity_resolved_ev ev;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find(conn_handle, &desc);
+	if (rc) {
+		return;
+	}
+
+	peer_id_addr = desc.peer_id_addr;
+	peer_ota_addr = desc.peer_ota_addr;
+
+	ev.address_type = desc.peer_ota_addr.type;
+	memcpy(ev.address, desc.peer_ota_addr.val, sizeof(ev.address));
+
+	ev.identity_address_type = desc.peer_id_addr.type;
+	memcpy(ev.identity_address, desc.peer_id_addr.val,
+	       sizeof(ev.identity_address));
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_IDENTITY_RESOLVED,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void le_conn_param_update(struct ble_gap_conn_desc *desc)
+{
+	struct gap_conn_param_update_ev ev;
+
+	SYS_LOG_DBG("");
+
+	ev.address_type = desc->peer_ota_addr.type;
+	memcpy(ev.address, desc->peer_ota_addr.val, sizeof(ev.address));
+
+	ev.conn_itvl = desc->conn_itvl;
+	ev.conn_latency = desc->conn_latency;
+	ev.supervision_timeout = desc->supervision_timeout;
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_CONN_PARAM_UPDATE,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void le_encryption_changed(struct ble_gap_conn_desc *desc)
+{
+	struct gap_sec_level_changed_ev ev;
+
+	SYS_LOG_DBG("");
+
+	encrypted = (bool) desc->sec_state.encrypted;
+
+	ev.address_type = desc->peer_ota_addr.type;
+	memcpy(ev.address, desc->peer_ota_addr.val, sizeof(ev.address));
+	ev.level = 0;
+
+	if (desc->sec_state.encrypted) {
+		if (desc->sec_state.authenticated) {
+			if (desc->sec_state.key_size == 16) {
+				ev.level = 3;
+			} else {
+				ev.level = 2;
+			}
+		} else {
+			ev.level = 1;
+		}
+	}
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_SEC_LEVEL_CHANGED,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void print_bytes(const uint8_t *bytes, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++) {
+		console_printf("%s0x%02x", i != 0 ? ":" : "", bytes[i]);
+	}
+}
+
+static void print_mbuf(const struct os_mbuf *om)
+{
+	int colon;
+
+	colon = 0;
+	while (om != NULL) {
+		if (colon) {
+			console_printf(":");
+		} else {
+			colon = 1;
+		}
+		print_bytes(om->om_data, om->om_len);
+		om = SLIST_NEXT(om, om_next);
+	}
+}
+
+static void print_addr(const void *addr)
+{
+	const uint8_t *u8p;
+
+	u8p = addr;
+	console_printf("%02x:%02x:%02x:%02x:%02x:%02x",
+		       u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]);
+}
+
+static void print_conn_desc(const struct ble_gap_conn_desc *desc)
+{
+	console_printf("handle=%d our_ota_addr_type=%d our_ota_addr=",
+		       desc->conn_handle, desc->our_ota_addr.type);
+	print_addr(desc->our_ota_addr.val);
+	console_printf(" our_id_addr_type=%d our_id_addr=",
+		       desc->our_id_addr.type);
+	print_addr(desc->our_id_addr.val);
+	console_printf(" peer_ota_addr_type=%d peer_ota_addr=",
+		       desc->peer_ota_addr.type);
+	print_addr(desc->peer_ota_addr.val);
+	console_printf(" peer_id_addr_type=%d peer_id_addr=",
+		       desc->peer_id_addr.type);
+	print_addr(desc->peer_id_addr.val);
+	console_printf(" conn_itvl=%d conn_latency=%d supervision_timeout=%d "
+		       "key_sz=%d encrypted=%d authenticated=%d bonded=%d\n",
+		       desc->conn_itvl, desc->conn_latency,
+		       desc->supervision_timeout,
+		       desc->sec_state.key_size,
+		       desc->sec_state.encrypted,
+		       desc->sec_state.authenticated,
+		       desc->sec_state.bonded);
+}
+
+static void adv_complete(void)
+{
+	struct gap_new_settings_ev ev;
+
+	current_settings &= ~BIT(GAP_SETTINGS_ADVERTISING);
+	ev.current_settings = sys_cpu_to_le32(current_settings);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_EV_NEW_SETTINGS, CONTROLLER_INDEX,
+		    (u8_t *) &ev, sizeof(ev));
+}
+
+static int gap_event_cb(struct ble_gap_event *event, void *arg)
+{
+	struct ble_gap_conn_desc desc;
+	int rc;
+
+	switch (event->type) {
+	case BLE_GAP_EVENT_ADV_COMPLETE:
+		console_printf("advertising complete; reason=%d\n",
+			       event->adv_complete.reason);
+		break;
+	case BLE_GAP_EVENT_CONNECT:
+		console_printf("connection %s; status=%d ",
+			       event->connect.status == 0 ? "established" : "failed",
+			       event->connect.status);
+		if (event->connect.status == 0) {
+			rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
+			assert(rc == 0);
+			print_conn_desc(&desc);
+		}
+
+		if (desc.role == BLE_GAP_ROLE_SLAVE) {
+			adv_complete();
+		}
+
+		le_connected(event->connect.conn_handle,
+			     event->connect.status);
+		break;
+	case BLE_GAP_EVENT_DISCONNECT:
+		console_printf("disconnect; reason=%d ", event->disconnect.reason);
+		print_conn_desc(&event->disconnect.conn);
+		le_disconnected(&event->disconnect.conn,
+				event->disconnect.reason);
+		break;
+	case BLE_GAP_EVENT_ENC_CHANGE:
+		console_printf("encryption change event; status=%d ", event->enc_change.status);
+		rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
+		assert(rc == 0);
+		print_conn_desc(&desc);
+		le_encryption_changed(&desc);
+		break;
+	case BLE_GAP_EVENT_PASSKEY_ACTION:
+		console_printf("passkey action event; action=%d",
+			       event->passkey.params.action);
+		if (event->passkey.params.action == BLE_SM_IOACT_NUMCMP) {
+			console_printf(" numcmp=%lu",
+				       (unsigned long)event->passkey.params.numcmp);
+		}
+		console_printf("\n");
+		le_passkey_action(event->passkey.conn_handle,
+				  &event->passkey.params);
+		break;
+	case BLE_GAP_EVENT_IDENTITY_RESOLVED:
+		console_printf("identity resolved ");
+		rc = ble_gap_conn_find(event->identity_resolved.conn_handle, &desc);
+		assert(rc == 0);
+		print_conn_desc(&desc);
+		le_identity_resolved(event->identity_resolved.conn_handle);
+		break;
+	case BLE_GAP_EVENT_NOTIFY_RX:
+		console_printf("notification rx event; attr_handle=%d indication=%d "
+			       "len=%d data=",
+			       event->notify_rx.attr_handle,
+			       event->notify_rx.indication,
+			       OS_MBUF_PKTLEN(event->notify_rx.om));
+
+		print_mbuf(event->notify_rx.om);
+		console_printf("\n");
+		tester_gatt_notify_rx_ev(event->notify_rx.conn_handle,
+					 event->notify_rx.attr_handle,
+					 event->notify_rx.indication,
+					 event->notify_rx.om);
+		break;
+	case BLE_GAP_EVENT_SUBSCRIBE:
+		console_printf("subscribe event; conn_handle=%d attr_handle=%d "
+			       "reason=%d prevn=%d curn=%d previ=%d curi=%d\n",
+			       event->subscribe.conn_handle,
+			       event->subscribe.attr_handle,
+			       event->subscribe.reason,
+			       event->subscribe.prev_notify,
+			       event->subscribe.cur_notify,
+			       event->subscribe.prev_indicate,
+			       event->subscribe.cur_indicate);
+		tester_gatt_subscribe_ev(event->subscribe.conn_handle,
+					 event->subscribe.attr_handle,
+					 event->subscribe.reason,
+					 event->subscribe.prev_notify,
+					 event->subscribe.cur_notify,
+					 event->subscribe.prev_indicate,
+					 event->subscribe.cur_indicate);
+		break;
+	case BLE_GAP_EVENT_REPEAT_PAIRING:
+		console_printf("repeat pairing event; conn_handle=%d "
+			       "cur_key_sz=%d cur_auth=%d cur_sc=%d "
+			       "new_key_sz=%d new_auth=%d new_sc=%d "
+			       "new_bonding=%d\n",
+			event->repeat_pairing.conn_handle,
+			event->repeat_pairing.cur_key_size,
+			event->repeat_pairing.cur_authenticated,
+			event->repeat_pairing.cur_sc,
+			event->repeat_pairing.new_key_size,
+			event->repeat_pairing.new_authenticated,
+			event->repeat_pairing.new_sc,
+			event->repeat_pairing.new_bonding);
+		rc = ble_gap_conn_find(event->repeat_pairing.conn_handle, &desc);
+		assert(rc == 0);
+		rc = ble_store_util_delete_peer(&desc.peer_id_addr);
+		assert(rc == 0);
+		return BLE_GAP_REPEAT_PAIRING_RETRY;
+	case BLE_GAP_EVENT_CONN_UPDATE:
+		console_printf("connection update event; status=%d ",
+			       event->conn_update.status);
+			rc = ble_gap_conn_find(event->conn_update.conn_handle, &desc);
+			assert(rc == 0);
+			print_conn_desc(&desc);
+			le_conn_param_update(&desc);
+		break;
+	case BLE_GAP_EVENT_CONN_UPDATE_REQ:
+		console_printf("connection update request event; "
+			       "conn_handle=%d itvl_min=%d itvl_max=%d "
+			       "latency=%d supervision_timoeut=%d "
+			       "min_ce_len=%d max_ce_len=%d\n",
+			       event->conn_update_req.conn_handle,
+			       event->conn_update_req.peer_params->itvl_min,
+			       event->conn_update_req.peer_params->itvl_max,
+			       event->conn_update_req.peer_params->latency,
+			       event->conn_update_req.peer_params->supervision_timeout,
+			       event->conn_update_req.peer_params->min_ce_len,
+			       event->conn_update_req.peer_params->max_ce_len);
+
+		*event->conn_update_req.self_params =
+			*event->conn_update_req.peer_params;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static void connect(const u8_t *data, u16_t len)
+{
+	u8_t status = BTP_STATUS_SUCCESS;
+
+	SYS_LOG_DBG("");
+
+	if (ble_gap_connect(own_addr_type, (ble_addr_t *) data, 0,
+			    &dflt_conn_params, gap_event_cb, NULL)) {
+		status = BTP_STATUS_FAILED;
+	}
+
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_CONNECT, CONTROLLER_INDEX, status);
+}
+
+static void disconnect(const u8_t *data, u16_t len)
+{
+	struct ble_gap_conn_desc desc;
+	u8_t status;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = gap_conn_find_by_addr((ble_addr_t *)data, &desc);
+	if (rc) {
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	if (ble_gap_terminate(desc.conn_handle, BLE_ERR_REM_USER_CONN_TERM)) {
+		status = BTP_STATUS_FAILED;
+	} else {
+		status = BTP_STATUS_SUCCESS;
+	}
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_DISCONNECT, CONTROLLER_INDEX,
+		   status);
+}
+
+static void set_io_cap(const u8_t *data, u16_t len)
+{
+	const struct gap_set_io_cap_cmd *cmd = (void *) data;
+	u8_t status;
+
+	SYS_LOG_DBG("");
+
+	switch (cmd->io_cap) {
+	case GAP_IO_CAP_DISPLAY_ONLY:
+		ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_DISP_ONLY;
+		ble_hs_cfg.sm_mitm = 1;
+		break;
+	case GAP_IO_CAP_KEYBOARD_DISPLAY:
+		ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_KEYBOARD_DISP;
+		ble_hs_cfg.sm_mitm = 1;
+		break;
+	case GAP_IO_CAP_NO_INPUT_OUTPUT:
+		ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_NO_IO;
+		ble_hs_cfg.sm_mitm = 0;
+		break;
+	case GAP_IO_CAP_KEYBOARD_ONLY:
+		ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_KEYBOARD_ONLY;
+		ble_hs_cfg.sm_mitm = 1;
+		break;
+	case GAP_IO_CAP_DISPLAY_YESNO:
+		ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_DISP_YES_NO;
+		ble_hs_cfg.sm_mitm = 1;
+		break;
+	default:
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	status = BTP_STATUS_SUCCESS;
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_IO_CAP, CONTROLLER_INDEX,
+		   status);
+}
+
+static void pair(const u8_t *data, u16_t len)
+{
+	struct ble_gap_conn_desc desc;
+	u8_t status;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = gap_conn_find_by_addr((ble_addr_t *)data, &desc);
+	if (rc) {
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	if (ble_gap_security_initiate(desc.conn_handle)) {
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	status = BTP_STATUS_SUCCESS;
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_PAIR, CONTROLLER_INDEX, status);
+}
+
+static void unpair(const u8_t *data, u16_t len)
+{
+	u8_t status;
+	int err;
+
+	SYS_LOG_DBG("");
+
+	err = ble_gap_unpair((ble_addr_t *) data);
+	status = (uint8_t) (err != 0 ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_UNPAIR, CONTROLLER_INDEX, status);
+}
+
+static void passkey_entry(const u8_t *data, u16_t len)
+{
+	const struct gap_passkey_entry_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc desc;
+	struct ble_sm_io pk;
+	u8_t status;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = gap_conn_find_by_addr((ble_addr_t *)data, &desc);
+	if (rc) {
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	pk.action = BLE_SM_IOACT_INPUT;
+	pk.passkey = sys_le32_to_cpu(cmd->passkey);
+
+	rc = ble_sm_inject_io(desc.conn_handle, &pk);
+	if (rc) {
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	status = BTP_STATUS_SUCCESS;
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_ENTRY, CONTROLLER_INDEX,
+		   status);
+}
+
+static void passkey_confirm(const u8_t *data, u16_t len)
+{
+	const struct gap_passkey_confirm_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc desc;
+	struct ble_sm_io pk;
+	u8_t status;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = gap_conn_find_by_addr((ble_addr_t *)data, &desc);
+	if (rc) {
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	pk.action = BLE_SM_IOACT_NUMCMP;
+	pk.numcmp_accept = cmd->match;
+
+	rc = ble_sm_inject_io(desc.conn_handle, &pk);
+	if (rc) {
+		console_printf("sm inject io failed");
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	status = BTP_STATUS_SUCCESS;
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_CONFIRM, CONTROLLER_INDEX,
+		   status);
+}
+
+static void start_direct_adv(const u8_t *data, u16_t len)
+{
+	const struct gap_start_direct_adv_cmd *cmd = (void *) data;
+	struct gap_start_advertising_rp rp;
+	static struct ble_gap_adv_params adv_params = {
+		.conn_mode = BLE_GAP_CONN_MODE_DIR,
+	};
+	int err;
+
+	SYS_LOG_DBG("");
+
+	adv_params.high_duty_cycle = cmd->high_duty;
+
+	err = ble_gap_adv_start(own_addr_type, (ble_addr_t *)data,
+				BLE_HS_FOREVER, &adv_params,
+				gap_event_cb, NULL);
+	if (err) {
+		SYS_LOG_ERR("Advertising failed: err %d", err);
+		goto fail;
+	}
+
+	current_settings |= BIT(GAP_SETTINGS_ADVERTISING);
+	rp.current_settings = sys_cpu_to_le32(current_settings);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_START_DIRECT_ADV, CONTROLLER_INDEX,
+		    (u8_t *) &rp, sizeof(rp));
+	return;
+fail:
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_DIRECT_ADV, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void conn_param_update_cb(uint16_t conn_handle, int status, void *arg)
+{
+	console_printf("conn param update complete; conn_handle=%d status=%d\n",
+		       conn_handle, status);
+}
+
+static int conn_param_update_slave(u16_t conn_handle,
+				   const struct gap_conn_param_update_cmd *cmd)
+{
+	int rc;
+	struct ble_l2cap_sig_update_params params;
+
+	params.itvl_min = cmd->conn_itvl_min;
+	params.itvl_max = cmd->conn_itvl_max;
+	params.slave_latency = cmd->conn_latency;
+	params.timeout_multiplier = cmd->supervision_timeout;
+
+	rc = ble_l2cap_sig_update(conn_handle, &params,
+				  conn_param_update_cb, NULL);
+	if (rc) {
+		SYS_LOG_ERR("Failed to send update params: rc=%d", rc);
+	}
+
+	return 0;
+}
+
+static int conn_param_update_master(u16_t conn_handle,
+				    const struct gap_conn_param_update_cmd *cmd)
+{
+	int rc;
+	struct ble_gap_upd_params params;
+
+	params.itvl_min = cmd->conn_itvl_min;
+	params.itvl_max = cmd->conn_itvl_max;
+	params.latency = cmd->conn_latency;
+	params.supervision_timeout = cmd->supervision_timeout;
+	params.min_ce_len = 0;
+	params.max_ce_len = 0;
+	rc = ble_gap_update_params(conn_handle, &params);
+	if (rc) {
+		SYS_LOG_ERR("Failed to send update params: rc=%d", rc);
+	}
+
+	return rc;
+}
+
+static void conn_param_update(struct os_event *ev)
+{
+	struct ble_gap_conn_desc desc;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = gap_conn_find_by_addr((ble_addr_t *)&update_params, &desc);
+	if (rc) {
+		goto rsp;
+	}
+
+	if ((desc.conn_itvl >= update_params.conn_itvl_min) &&
+	    (desc.conn_itvl <= update_params.conn_itvl_max) &&
+	    (desc.conn_latency == update_params.conn_latency) &&
+	    (desc.supervision_timeout == update_params.supervision_timeout)) {
+		goto rsp;
+	}
+
+	if (desc.role == BLE_GAP_ROLE_MASTER) {
+		rc = conn_param_update_master(desc.conn_handle, &update_params);
+	} else {
+		rc = conn_param_update_slave(desc.conn_handle, &update_params);
+	}
+
+	if (rc == 0) {
+		return;
+	}
+
+rsp:
+	SYS_LOG_ERR("Conn param update fail; rc=%d", rc);
+}
+
+static void conn_param_update_async(const u8_t *data, u16_t len)
+{
+	const struct gap_conn_param_update_cmd *cmd = (void *) data;
+	update_params = *cmd;
+
+	os_callout_reset(&update_params_co, 0);
+
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_CONN_PARAM_UPDATE, CONTROLLER_INDEX,
+		   BTP_STATUS_SUCCESS);
+}
+
+static void oob_legacy_set_data(const u8_t *data, u16_t len)
+{
+	const struct gap_oob_legacy_set_data_cmd *cmd = (void *) data;
+
+	ble_hs_cfg.sm_oob_data_flag = 1;
+	memcpy(oob, cmd->oob_data, sizeof(oob));
+
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_OOB_LEGACY_SET_DATA,
+		   CONTROLLER_INDEX, BTP_STATUS_SUCCESS);
+}
+
+static void oob_sc_get_local_data(const u8_t *data, u16_t len)
+{
+	struct gap_oob_sc_get_local_data_rp rp;
+
+	memcpy(rp.r, oob_data_local.r, 16);
+	memcpy(rp.c, oob_data_local.c, 16);
+
+	tester_send(BTP_SERVICE_ID_GAP, GAP_OOB_SC_GET_LOCAL_DATA,
+		    CONTROLLER_INDEX, (u8_t *) &rp, sizeof(rp));
+}
+
+static void oob_sc_set_remote_data(const u8_t *data, u16_t len)
+{
+	const struct gap_oob_sc_set_remote_data_cmd *cmd = (void *) data;
+
+	ble_hs_cfg.sm_oob_data_flag = 1;
+	memcpy(oob_data_remote.r, cmd->r, 16);
+	memcpy(oob_data_remote.c, cmd->c, 16);
+
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_OOB_SC_SET_REMOTE_DATA,
+		   CONTROLLER_INDEX, BTP_STATUS_SUCCESS);
+}
+
+static void set_mitm(const u8_t *data, u16_t len)
+{
+	const struct gap_set_mitm_cmd *cmd = (void *) data;
+
+	ble_hs_cfg.sm_mitm = cmd->mitm;
+
+	tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_MITM,
+		   CONTROLLER_INDEX, BTP_STATUS_SUCCESS);
+}
+
+void tester_handle_gap(u8_t opcode, u8_t index, u8_t *data,
+		       u16_t len)
+{
+	switch (opcode) {
+	case GAP_READ_SUPPORTED_COMMANDS:
+	case GAP_READ_CONTROLLER_INDEX_LIST:
+		if (index != BTP_INDEX_NONE){
+			tester_rsp(BTP_SERVICE_ID_GAP, opcode, index,
+				   BTP_STATUS_FAILED);
+			return;
+		}
+		break;
+	default:
+		if (index != CONTROLLER_INDEX){
+			tester_rsp(BTP_SERVICE_ID_GAP, opcode, index,
+				   BTP_STATUS_FAILED);
+			return;
+		}
+		break;
+	}
+
+	switch (opcode) {
+	case GAP_READ_SUPPORTED_COMMANDS:
+		supported_commands(data, len);
+		return;
+	case GAP_READ_CONTROLLER_INDEX_LIST:
+		controller_index_list(data, len);
+		return;
+	case GAP_READ_CONTROLLER_INFO:
+		controller_info(data, len);
+		return;
+	case GAP_SET_CONNECTABLE:
+		set_connectable(data, len);
+		return;
+	case GAP_SET_DISCOVERABLE:
+		set_discoverable(data, len);
+		return;
+	case GAP_SET_BONDABLE:
+		set_bondable(data, len);
+		return;
+	case GAP_START_ADVERTISING:
+		start_advertising(data, len);
+		return;
+	case GAP_STOP_ADVERTISING:
+		stop_advertising(data, len);
+		return;
+	case GAP_START_DISCOVERY:
+		start_discovery(data, len);
+		return;
+	case GAP_STOP_DISCOVERY:
+		stop_discovery(data, len);
+		return;
+	case GAP_CONNECT:
+		connect(data, len);
+		return;
+	case GAP_DISCONNECT:
+		disconnect(data, len);
+		return;
+	case GAP_SET_IO_CAP:
+		set_io_cap(data, len);
+		return;
+	case GAP_PAIR:
+		pair(data, len);
+		return;
+	case GAP_UNPAIR:
+		unpair(data, len);
+		return;
+	case GAP_PASSKEY_ENTRY:
+		passkey_entry(data, len);
+		return;
+	case GAP_PASSKEY_CONFIRM:
+		passkey_confirm(data, len);
+		return;
+	case GAP_START_DIRECT_ADV:
+		start_direct_adv(data, len);
+		return;
+	case GAP_CONN_PARAM_UPDATE:
+		conn_param_update_async(data, len);
+		return;
+	case GAP_OOB_LEGACY_SET_DATA:
+		oob_legacy_set_data(data, len);
+		return;
+	case GAP_OOB_SC_GET_LOCAL_DATA:
+		oob_sc_get_local_data(data, len);
+		return;
+	case GAP_OOB_SC_SET_REMOTE_DATA:
+		oob_sc_set_remote_data(data, len);
+		return;
+	case GAP_SET_MITM:
+		set_mitm(data, len);
+		return;
+	default:
+		tester_rsp(BTP_SERVICE_ID_GAP, opcode, index,
+			   BTP_STATUS_UNKNOWN_CMD);
+		return;
+	}
+}
+
+static void tester_init_gap_cb(int err)
+{
+	if (err) {
+		tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE,
+			   BTP_INDEX_NONE, BTP_STATUS_FAILED);
+		return;
+	}
+
+	current_settings = 0;
+	current_settings |= BIT(GAP_SETTINGS_POWERED);
+	current_settings |= BIT(GAP_SETTINGS_LE);
+
+	os_callout_init(&update_params_co, os_eventq_dflt_get(),
+			conn_param_update, NULL);
+
+	os_callout_init(&connected_ev_co, os_eventq_dflt_get(),
+			device_connected_ev_send, NULL);
+
+	tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE,
+		   BTP_STATUS_SUCCESS);
+}
+
+u8_t tester_init_gap(void)
+{
+#if MYNEWT_VAL(BLE_SM_SC)
+	int rc;
+
+	rc = ble_sm_sc_oob_generate_data(&oob_data_local);
+	if (rc) {
+		console_printf("Error: generating oob data; reason=%d\n", rc);
+		return BTP_STATUS_FAILED;
+	}
+#endif
+
+	adv_buf = NET_BUF_SIMPLE(ADV_BUF_LEN);
+
+	tester_init_gap_cb(BTP_STATUS_SUCCESS);
+	return BTP_STATUS_SUCCESS;
+}
+
+u8_t tester_unregister_gap(void)
+{
+	return BTP_STATUS_SUCCESS;
+}
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/gatt.c b/src/libs/mynewt-nimble/apps/bttester/src/gatt.c
new file mode 100644
index 00000000..7e7d1d3b
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/gatt.c
@@ -0,0 +1,2098 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements. See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.    You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/* gatt.c - Bluetooth GATT Server Tester */
+
+/*
+ * Copyright (c) 2015-2016 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+
+#include "host/ble_gap.h"
+#include "host/ble_gatt.h"
+#include "console/console.h"
+#include "services/gatt/ble_svc_gatt.h"
+#include "../../../nimble/host/src/ble_att_priv.h"
+#include "../../../nimble/host/src/ble_gatt_priv.h"
+
+#include "bttester.h"
+
+#define CONTROLLER_INDEX 0
+#define MAX_BUFFER_SIZE 2048
+
+/* 0000xxxx-8c26-476f-89a7-a108033a69c7 */
+#define PTS_UUID_DECLARE(uuid16)                                    \
+    ((const ble_uuid_t *) (&(ble_uuid128_t) BLE_UUID128_INIT(   \
+	0xc7, 0x69, 0x3a, 0x03, 0x08, 0xa1, 0xa7, 0x89,             \
+	0x6f, 0x47, 0x26, 0x8c, uuid16, uuid16 >> 8, 0x00, 0x00     \
+    )))
+
+/* 0000xxxx-8c26-476f-89a7-a108033a69c6 */
+#define PTS_UUID_DECLARE_ALT(uuid16)                            \
+    ((const ble_uuid_t *) (&(ble_uuid128_t) BLE_UUID128_INIT(   \
+	0xc6, 0x69, 0x3a, 0x03, 0x08, 0xa1, 0xa7, 0x89,             \
+	0x6f, 0x47, 0x26, 0x8c, uuid16, uuid16 >> 8, 0x00, 0x00     \
+    )))
+
+#define  PTS_SVC                           0x0001
+#define  PTS_CHR_READ                      0x0002
+#define  PTS_CHR_WRITE                     0x0003
+#define  PTS_CHR_RELIABLE_WRITE            0x0004
+#define  PTS_CHR_WRITE_NO_RSP              0x0005
+#define  PTS_CHR_READ_WRITE                0x0006
+#define  PTS_CHR_READ_WRITE_ENC            0x0007
+#define  PTS_CHR_READ_WRITE_AUTHEN         0x0008
+#define  PTS_DSC_READ                      0x0009
+#define  PTS_DSC_WRITE                     0x000a
+#define  PTS_DSC_READ_WRITE                0x000b
+#define  PTS_CHR_NOTIFY                    0x0025
+#define  PTS_LONG_CHR_READ_WRITE           0x0015
+#define  PTS_LONG_CHR_READ_WRITE_ALT       0x0016
+#define  PTS_LONG_DSC_READ_WRITE           0x001b
+#define  PTS_INC_SVC                       0x001e
+#define  PTS_CHR_READ_WRITE_ALT            0x001f
+
+static uint8_t gatt_svr_pts_static_long_val[300];
+static uint8_t gatt_svr_pts_static_val[30];
+static uint8_t gatt_svr_pts_static_short_val;
+static u8_t notify_state;
+static u8_t indicate_state;
+static uint16_t myconn_handle;
+static struct os_callout notify_tx_timer;
+uint16_t notify_handle;
+uint8_t notify_value = 90;
+
+struct find_attr_data {
+                ble_uuid_any_t *uuid;
+                int attr_type;
+                void *ptr;
+                uint16_t handle;
+};
+
+static int
+gatt_svr_read_write_test(uint16_t conn_handle, uint16_t attr_handle,
+			 struct ble_gatt_access_ctxt *ctxt,
+			 void *arg);
+
+static int
+gatt_svr_read_write_auth_test(uint16_t conn_handle, uint16_t attr_handle,
+			      struct ble_gatt_access_ctxt *ctxt,
+			      void *arg);
+
+static int
+gatt_svr_read_write_enc_test(uint16_t conn_handle, uint16_t attr_handle,
+			     struct ble_gatt_access_ctxt *ctxt,
+			     void *arg);
+
+static int
+gatt_svr_dsc_read_write_test(uint16_t conn_handle, uint16_t attr_handle,
+			     struct ble_gatt_access_ctxt *ctxt,
+			     void *arg);
+
+static int
+gatt_svr_write_no_rsp_test(uint16_t conn_handle, uint16_t attr_handle,
+			   struct ble_gatt_access_ctxt *ctxt,
+			   void *arg);
+
+static int
+gatt_svr_rel_write_test(uint16_t conn_handle, uint16_t attr_handle,
+			struct ble_gatt_access_ctxt *ctxt,
+			void *arg);
+
+static int
+gatt_svr_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle,
+			      struct ble_gatt_access_ctxt *ctxt,
+			      void *arg);
+
+static int
+gatt_svr_dsc_read_test(uint16_t conn_handle, uint16_t attr_handle,
+		       struct ble_gatt_access_ctxt *ctxt,
+		       void *arg);
+
+static int
+gatt_svr_dsc_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle,
+				  struct ble_gatt_access_ctxt *ctxt,
+				  void *arg);
+
+static const struct ble_gatt_svc_def gatt_svr_inc_svcs[] = {
+	{
+		.type = BLE_GATT_SVC_TYPE_PRIMARY,
+		.uuid = PTS_UUID_DECLARE(PTS_INC_SVC),
+		.characteristics = (struct ble_gatt_chr_def[]) {{
+			.uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_ALT),
+			.access_cb = gatt_svr_read_write_test,
+			.flags = BLE_GATT_CHR_F_WRITE |
+				 BLE_GATT_CHR_F_READ,
+		}, {
+		0,
+		} },
+	},
+
+	{
+		0, /* No more services. */
+	},
+};
+
+static const struct ble_gatt_svc_def *inc_svcs[] = {
+	&gatt_svr_inc_svcs[0],
+	NULL,
+};
+
+static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
+	{
+		/*** Service: PTS test. */
+		.type = BLE_GATT_SVC_TYPE_PRIMARY,
+		.uuid = PTS_UUID_DECLARE(PTS_SVC),
+		.includes = inc_svcs,
+		.characteristics = (struct ble_gatt_chr_def[]) { {
+			.uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE),
+			.access_cb = gatt_svr_read_write_test,
+			.flags = BLE_GATT_CHR_F_READ |
+				 BLE_GATT_CHR_F_WRITE,
+			.descriptors = (struct ble_gatt_dsc_def[]) { {
+				.uuid = PTS_UUID_DECLARE(PTS_DSC_READ_WRITE),
+				.access_cb = gatt_svr_dsc_read_write_test,
+				.att_flags = BLE_ATT_F_READ |
+					     BLE_ATT_F_WRITE,
+			}, {
+			.uuid = PTS_UUID_DECLARE(PTS_LONG_DSC_READ_WRITE),
+			.access_cb = gatt_svr_dsc_read_write_long_test,
+			.att_flags = BLE_ATT_F_READ |
+				     BLE_ATT_F_WRITE,
+			}, {
+			.uuid = PTS_UUID_DECLARE(PTS_DSC_READ),
+			.access_cb = gatt_svr_dsc_read_test,
+			.att_flags = BLE_ATT_F_READ,
+			}, {
+				0, /* No more descriptors in this characteristic */
+			} }
+		}, {
+			.uuid = PTS_UUID_DECLARE(PTS_CHR_WRITE_NO_RSP),
+			.access_cb = gatt_svr_write_no_rsp_test,
+			.flags = BLE_GATT_CHR_F_READ |
+				 BLE_GATT_CHR_F_WRITE |
+				 BLE_GATT_CHR_F_WRITE_NO_RSP,
+		}, {
+			.uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_AUTHEN),
+			.access_cb = gatt_svr_read_write_auth_test,
+			.flags = BLE_GATT_CHR_F_READ_AUTHEN |
+				 BLE_GATT_CHR_F_READ |
+				 BLE_GATT_CHR_F_WRITE_AUTHEN |
+				 BLE_GATT_CHR_F_WRITE |
+				 BLE_GATT_CHR_F_WRITE_AUTHEN,
+		}, {
+			.uuid = PTS_UUID_DECLARE(PTS_CHR_RELIABLE_WRITE),
+			.access_cb = gatt_svr_rel_write_test,
+			.flags = BLE_GATT_CHR_F_WRITE |
+				 BLE_GATT_CHR_F_RELIABLE_WRITE,
+		}, {
+			.uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_ENC),
+			.access_cb = gatt_svr_read_write_enc_test,
+			.flags = BLE_GATT_CHR_F_READ_ENC |
+				 BLE_GATT_CHR_F_READ |
+				 BLE_GATT_CHR_F_WRITE |
+				 BLE_GATT_CHR_F_WRITE_ENC,
+			.min_key_size = 16,
+		}, {
+			.uuid = PTS_UUID_DECLARE(PTS_LONG_CHR_READ_WRITE),
+			.access_cb = gatt_svr_read_write_long_test,
+			.flags = BLE_GATT_CHR_F_WRITE |
+				 BLE_GATT_CHR_F_READ,
+		}, {
+			.uuid = PTS_UUID_DECLARE(PTS_LONG_CHR_READ_WRITE_ALT),
+			.access_cb = gatt_svr_read_write_long_test,
+			.flags = BLE_GATT_CHR_F_WRITE |
+				 BLE_GATT_CHR_F_READ,
+		}, {
+			.uuid = PTS_UUID_DECLARE(PTS_CHR_NOTIFY),
+			.access_cb = gatt_svr_read_write_test,
+			.val_handle = &notify_handle,
+			.flags = BLE_GATT_CHR_F_NOTIFY |
+				 BLE_GATT_CHR_F_INDICATE,
+		}, {
+			0, /* No more characteristics in this service. */
+		} },
+	},
+
+	{
+		.type = BLE_GATT_SVC_TYPE_PRIMARY,
+		.uuid = PTS_UUID_DECLARE_ALT(PTS_SVC),
+		.characteristics = (struct ble_gatt_chr_def[]) { {
+			.uuid = PTS_UUID_DECLARE_ALT(PTS_CHR_READ_WRITE),
+			.access_cb = gatt_svr_read_write_test,
+			.flags = BLE_GATT_CHR_F_WRITE |
+				 BLE_GATT_CHR_F_READ,
+		}, {
+			0, /* No more characteristics in this service */
+		} },
+	},
+
+	{
+		0, /* No more services. */
+	},
+};
+
+static void attr_value_changed_ev(u16_t handle, struct os_mbuf *data)
+{
+	struct gatt_attr_value_changed_ev *ev;
+	struct os_mbuf *buf = os_msys_get(0, 0);
+
+	SYS_LOG_DBG("");
+
+	net_buf_simple_init(buf, 0);
+	ev = net_buf_simple_add(buf, sizeof(*ev));
+
+	ev->handle = sys_cpu_to_le16(handle);
+	ev->data_length = sys_cpu_to_le16(os_mbuf_len(data));
+	os_mbuf_appendfrom(buf, data, 0, os_mbuf_len(data));
+
+	tester_send_buf(BTP_SERVICE_ID_GATT, GATT_EV_ATTR_VALUE_CHANGED,
+			CONTROLLER_INDEX, buf);
+}
+
+static int
+gatt_svr_chr_write(uint16_t conn_handle, uint16_t attr_handle,
+		   struct os_mbuf *om, uint16_t min_len, uint16_t max_len,
+		   void *dst, uint16_t *len)
+{
+	uint16_t om_len;
+	int rc;
+
+	om_len = OS_MBUF_PKTLEN(om);
+	if (om_len < min_len || om_len > max_len) {
+		return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+	}
+
+	rc = ble_hs_mbuf_to_flat(om, dst, max_len, len);
+	if (rc != 0) {
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+
+	attr_value_changed_ev(attr_handle, om);
+
+	return 0;
+}
+
+static uint16_t
+extract_uuid16_from_pts_uuid128(const ble_uuid_t *uuid)
+{
+	const uint8_t *u8ptr;
+	uint16_t uuid16;
+
+	u8ptr = BLE_UUID128(uuid)->value;
+	uuid16 = u8ptr[12];
+	uuid16 |= (uint16_t)u8ptr[13] << 8;
+	return uuid16;
+}
+
+static int
+gatt_svr_read_write_test(uint16_t conn_handle, uint16_t attr_handle,
+			 struct ble_gatt_access_ctxt *ctxt,
+			 void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_CHR_READ_WRITE:
+	case PTS_CHR_READ_WRITE_ALT:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+			rc = gatt_svr_chr_write(conn_handle, attr_handle,
+						ctxt->om, 0,
+						sizeof gatt_svr_pts_static_short_val,
+						&gatt_svr_pts_static_short_val, NULL);
+			return rc;
+		} else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+			rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val,
+					    sizeof gatt_svr_pts_static_short_val);
+			return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+		}
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static int
+gatt_svr_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle,
+			      struct ble_gatt_access_ctxt *ctxt,
+			      void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_LONG_CHR_READ_WRITE:
+	case PTS_LONG_CHR_READ_WRITE_ALT:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+			rc = gatt_svr_chr_write(conn_handle, attr_handle,
+						ctxt->om, 0,
+						sizeof gatt_svr_pts_static_long_val,
+						&gatt_svr_pts_static_long_val, NULL);
+			return rc;
+		} else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+			rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val,
+					    sizeof gatt_svr_pts_static_long_val);
+			return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+		}
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static int
+gatt_svr_read_write_auth_test(uint16_t conn_handle, uint16_t attr_handle,
+			      struct ble_gatt_access_ctxt *ctxt,
+			      void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_CHR_READ_WRITE_AUTHEN:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+			rc = gatt_svr_chr_write(conn_handle, attr_handle,
+						ctxt->om, 0,
+						sizeof gatt_svr_pts_static_val,
+						&gatt_svr_pts_static_val, NULL);
+			return rc;
+		} else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+			rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_val,
+					    sizeof gatt_svr_pts_static_val);
+			return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+		}
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static int
+gatt_svr_read_write_enc_test(uint16_t conn_handle, uint16_t attr_handle,
+			     struct ble_gatt_access_ctxt *ctxt,
+			     void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_CHR_READ_WRITE_ENC:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+			rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_val,
+					    sizeof gatt_svr_pts_static_val);
+			return rc;
+		} else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+			rc = gatt_svr_chr_write(conn_handle, attr_handle,
+						ctxt->om, 0,
+						sizeof gatt_svr_pts_static_val,
+						&gatt_svr_pts_static_val, NULL);
+			return rc;
+		}
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static int
+gatt_svr_dsc_read_write_test(uint16_t conn_handle, uint16_t attr_handle,
+			     struct ble_gatt_access_ctxt *ctxt,
+			     void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_DSC_READ_WRITE:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) {
+			rc = gatt_svr_chr_write(conn_handle, attr_handle,
+						ctxt->om, 0,
+						sizeof gatt_svr_pts_static_short_val,
+						&gatt_svr_pts_static_short_val, NULL);
+			return rc;
+		} else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) {
+			rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val,
+					    sizeof gatt_svr_pts_static_short_val);
+			return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+		}
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static int
+gatt_svr_dsc_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle,
+				  struct ble_gatt_access_ctxt *ctxt,
+				  void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_LONG_DSC_READ_WRITE:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) {
+			rc = gatt_svr_chr_write(conn_handle, attr_handle,
+						ctxt->om, 0,
+						sizeof gatt_svr_pts_static_long_val,
+						&gatt_svr_pts_static_long_val, NULL);
+			return rc;
+		} else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) {
+			rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val,
+					    sizeof gatt_svr_pts_static_long_val);
+			return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+		}
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static int
+gatt_svr_dsc_read_test(uint16_t conn_handle, uint16_t attr_handle,
+		       struct ble_gatt_access_ctxt *ctxt,
+		       void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_DSC_READ:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) {
+			rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val,
+					    sizeof gatt_svr_pts_static_long_val);
+			return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+	}
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static int
+gatt_svr_write_no_rsp_test(uint16_t conn_handle, uint16_t attr_handle,
+			   struct ble_gatt_access_ctxt *ctxt,
+			   void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_CHR_WRITE_NO_RSP:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+			rc = gatt_svr_chr_write(conn_handle, attr_handle,
+						ctxt->om, 0,
+						sizeof gatt_svr_pts_static_short_val,
+						&gatt_svr_pts_static_short_val, NULL);
+			return rc;
+	} else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+		rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val,
+				    sizeof gatt_svr_pts_static_short_val);
+		return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+	}
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static int
+gatt_svr_rel_write_test(uint16_t conn_handle, uint16_t attr_handle,
+			struct ble_gatt_access_ctxt *ctxt,
+			void *arg)
+{
+	uint16_t uuid16;
+	int rc;
+
+	uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid);
+	assert(uuid16 != 0);
+
+	switch (uuid16) {
+	case PTS_CHR_RELIABLE_WRITE:
+		if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+			rc = gatt_svr_chr_write(conn_handle, attr_handle,
+						ctxt->om, 0,
+						sizeof gatt_svr_pts_static_val,
+						&gatt_svr_pts_static_val, NULL);
+			return rc;
+	}
+
+	default:
+		assert(0);
+		return BLE_ATT_ERR_UNLIKELY;
+	}
+}
+
+static void start_server(u8_t *data, u16_t len)
+{
+	struct gatt_start_server_rp rp;
+
+	SYS_LOG_DBG("");
+
+	ble_gatts_show_local();
+
+	ble_svc_gatt_changed(0x0001, 0xffff);
+
+	rp.db_attr_off = 0;
+	rp.db_attr_cnt = 0;
+
+	tester_send(BTP_SERVICE_ID_GATT, GATT_START_SERVER, CONTROLLER_INDEX,
+		    (u8_t *) &rp, sizeof(rp));
+}
+
+/* Convert UUID from BTP command to bt_uuid */
+static u8_t btp2bt_uuid(const u8_t *uuid, u8_t len,
+			ble_uuid_any_t *bt_uuid)
+{
+	u16_t le16;
+
+	switch (len) {
+	case 0x02: /* UUID 16 */
+		bt_uuid->u.type = BLE_UUID_TYPE_16;
+		memcpy(&le16, uuid, sizeof(le16));
+		BLE_UUID16(bt_uuid)->value = sys_le16_to_cpu(le16);
+		break;
+	case 0x10: /* UUID 128*/
+		bt_uuid->u.type = BLE_UUID_TYPE_128;
+		memcpy(BLE_UUID128(bt_uuid)->value, uuid, 16);
+	break;
+	default:
+		return BTP_STATUS_FAILED;
+	}
+	return BTP_STATUS_SUCCESS;
+}
+
+/*
+ * gatt_buf - cache used by a gatt client (to cache data read/discovered)
+ * and gatt server (to store attribute user_data).
+ * It is not intended to be used by client and server at the same time.
+ */
+static struct {
+	u16_t len;
+	u8_t buf[MAX_BUFFER_SIZE];
+} gatt_buf;
+
+static void *gatt_buf_add(const void *data, size_t len)
+{
+	void *ptr = gatt_buf.buf + gatt_buf.len;
+
+	if ((len + gatt_buf.len) > MAX_BUFFER_SIZE) {
+		return NULL;
+	}
+
+	if (data) {
+		memcpy(ptr, data, len);
+	} else {
+		(void)memset(ptr, 0, len);
+	}
+
+	gatt_buf.len += len;
+
+	SYS_LOG_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE);
+
+	return ptr;
+}
+
+static void *gatt_buf_reserve(size_t len)
+{
+	return gatt_buf_add(NULL, len);
+}
+
+static void gatt_buf_clear(void)
+{
+	(void)memset(&gatt_buf, 0, sizeof(gatt_buf));
+}
+
+static void discover_destroy(void)
+{
+	gatt_buf_clear();
+}
+
+static void read_destroy()
+{
+	gatt_buf_clear();
+}
+
+static int read_cb(uint16_t conn_handle,
+		   const struct ble_gatt_error *error,
+		   struct ble_gatt_attr *attr,
+		   void *arg)
+{
+	struct gatt_read_rp *rp = (void *) gatt_buf.buf;
+	u8_t btp_opcode = (uint8_t) (int) arg;
+
+	SYS_LOG_DBG("status=%d", error->status);
+
+	if (error->status != 0 && error->status != BLE_HS_EDONE) {
+		rp->att_response = (uint8_t) BLE_HS_ATT_ERR(error->status);
+		tester_send(BTP_SERVICE_ID_GATT, btp_opcode,
+			    CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len);
+		read_destroy();
+		return 0;
+	}
+
+	if (!gatt_buf_add(attr->om->om_data, attr->om->om_len)) {
+		tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		read_destroy();
+		return 0;
+	}
+
+	rp->data_length += attr->om->om_len;
+	tester_send(BTP_SERVICE_ID_GATT, btp_opcode,
+		    CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len);
+	read_destroy();
+
+	return 0;
+}
+
+static void read(u8_t *data, u16_t len)
+{
+	const struct gatt_read_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	/* Clear buffer */
+	read_destroy();
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) {
+		goto fail;
+	}
+
+	if (ble_gattc_read(conn.conn_handle, sys_le16_to_cpu(cmd->handle),
+			   read_cb, (void *)GATT_READ)) {
+		read_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static int read_long_cb(uint16_t conn_handle,
+			const struct ble_gatt_error *error,
+			struct ble_gatt_attr *attr,
+			void *arg)
+{
+	struct gatt_read_rp *rp = (void *) gatt_buf.buf;
+	u8_t btp_opcode = (uint8_t) (int) arg;
+
+	SYS_LOG_DBG("status=%d", error->status);
+
+	if (error->status != 0 && error->status != BLE_HS_EDONE) {
+		rp->att_response = (uint8_t) BLE_HS_ATT_ERR(error->status);
+		tester_send(BTP_SERVICE_ID_GATT, btp_opcode,
+			    CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len);
+		read_destroy();
+		return 0;
+	}
+
+	if (error->status == BLE_HS_EDONE) {
+		tester_send(BTP_SERVICE_ID_GATT, btp_opcode,
+			    CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len);
+		read_destroy();
+		return 0;
+	}
+
+	if (gatt_buf_add(attr->om->om_data, attr->om->om_len) == NULL) {
+		tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		read_destroy();
+		return BLE_HS_ENOMEM;
+	}
+
+	rp->data_length += attr->om->om_len;
+
+	return 0;
+}
+
+static void read_long(u8_t *data, u16_t len)
+{
+	const struct gatt_read_long_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	/* Clear buffer */
+	read_destroy();
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) {
+		goto fail;
+	}
+
+	if (ble_gattc_read_long(conn.conn_handle,
+				sys_le16_to_cpu(cmd->handle),
+				sys_le16_to_cpu(cmd->offset),
+				read_long_cb, (void *)GATT_READ_LONG)) {
+		read_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_LONG, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void read_multiple(u8_t *data, u16_t len)
+{
+	const struct gatt_read_multiple_cmd *cmd = (void *) data;
+	u16_t handles[cmd->handles_count];
+	struct ble_gap_conn_desc conn;
+	int rc, i;
+
+	SYS_LOG_DBG("");
+
+	for (i = 0; i < ARRAY_SIZE(handles); i++) {
+		handles[i] = sys_le16_to_cpu(cmd->handles[i]);
+	}
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	/* Clear buffer */
+	read_destroy();
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) {
+		goto fail;
+	}
+
+	if (ble_gattc_read_mult(conn.conn_handle, handles,
+				cmd->handles_count, read_cb,
+				(void *)GATT_READ_MULTIPLE)) {
+		read_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_MULTIPLE, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void write_without_rsp(u8_t *data, u16_t len, u8_t op, bool sign)
+{
+	const struct gatt_write_without_rsp_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	u8_t status = BTP_STATUS_SUCCESS;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	if (ble_gattc_write_no_rsp_flat(conn.conn_handle,
+					sys_le16_to_cpu(cmd->handle), cmd->data,
+					sys_le16_to_cpu(cmd->data_length))) {
+		status = BTP_STATUS_FAILED;
+	}
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status);
+}
+
+static int write_rsp(uint16_t conn_handle, const struct ble_gatt_error *error,
+		     struct ble_gatt_attr *attr,
+		     void *arg)
+{
+	uint8_t err = (uint8_t) error->status;
+	u8_t btp_opcode = (uint8_t) (int) arg;
+
+	SYS_LOG_DBG("");
+
+	tester_send(BTP_SERVICE_ID_GATT, btp_opcode,
+		    CONTROLLER_INDEX, &err, sizeof(err));
+	return 0;
+}
+
+static void write(u8_t *data, u16_t len)
+{
+	const struct gatt_write_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	if (ble_gattc_write_flat(conn.conn_handle, sys_le16_to_cpu(cmd->handle),
+				 cmd->data, sys_le16_to_cpu(cmd->data_length),
+				 write_rsp, (void *) GATT_WRITE)) {
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void write_long(u8_t *data, u16_t len)
+{
+	const struct gatt_write_long_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	struct os_mbuf *om = NULL;
+	int rc = 0;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length));
+	if (!om) {
+		SYS_LOG_ERR("Insufficient resources");
+		goto fail;
+	}
+
+	rc = ble_gattc_write_long(conn.conn_handle,
+				  sys_le16_to_cpu(cmd->handle),
+				  sys_le16_to_cpu(cmd->offset),
+				  om, write_rsp,
+				  (void *) GATT_WRITE_LONG);
+	if (!rc) {
+		return;
+	}
+
+fail:
+	SYS_LOG_ERR("Failed to send Write Long request, rc=%d", rc);
+	os_mbuf_free_chain(om);
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static int reliable_write_rsp(uint16_t conn_handle,
+			      const struct ble_gatt_error *error,
+			      struct ble_gatt_attr *attrs,
+			      uint8_t num_attrs,
+			      void *arg)
+{
+	uint8_t err = (uint8_t) error->status;
+
+	SYS_LOG_DBG("Reliable write status %d", err);
+
+	tester_send(BTP_SERVICE_ID_GATT, GATT_RELIABLE_WRITE,
+		    CONTROLLER_INDEX, &err, sizeof(err));
+	return 0;
+}
+
+static void reliable_write(u8_t *data, u16_t len)
+{
+	const struct gatt_reliable_write_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	struct ble_gatt_attr attr;
+	struct os_mbuf *om = NULL;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length));
+	/* This is required, because Nimble checks if
+	* the data is longer than offset
+	*/
+	if (os_mbuf_extend(om, sys_le16_to_cpu(cmd->offset) + 1) == NULL) {
+		goto fail;
+	}
+
+	attr.handle = sys_le16_to_cpu(cmd->handle);
+	attr.offset = sys_le16_to_cpu(cmd->offset);
+	attr.om = om;
+
+	if (ble_gattc_write_reliable(conn.conn_handle, &attr, 1,
+				     reliable_write_rsp, NULL)) {
+		goto fail;
+	}
+
+	return;
+
+fail:
+	os_mbuf_free_chain(om);
+
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static struct bt_gatt_subscribe_params {
+	u16_t ccc_handle;
+	u16_t value;
+	u16_t value_handle;
+} subscribe_params;
+
+static void read_uuid(u8_t *data, u16_t len)
+{
+	const struct gatt_read_uuid_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	ble_uuid_any_t uuid;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) {
+		goto fail;
+	}
+
+	/* Clear buffer */
+	read_destroy();
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) {
+		goto fail;
+	}
+
+	if (ble_gattc_read_by_uuid(conn.conn_handle,
+				   sys_le16_to_cpu(cmd->start_handle),
+				   sys_le16_to_cpu(cmd->end_handle), &uuid.u,
+				   read_long_cb, (void *)GATT_READ_UUID)) {
+		read_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static int disc_prim_uuid_cb(uint16_t conn_handle,
+			     const struct ble_gatt_error *error,
+			     const struct ble_gatt_svc *gatt_svc, void *arg)
+{
+	struct gatt_disc_prim_uuid_rp *rp = (void *) gatt_buf.buf;
+	struct gatt_service *service;
+	const ble_uuid_any_t *uuid;
+	u8_t uuid_length;
+	u8_t opcode = (u8_t) (int) arg;
+
+	SYS_LOG_DBG("");
+
+	if (error->status != 0 && error->status != BLE_HS_EDONE) {
+		tester_rsp(BTP_SERVICE_ID_GATT, opcode,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		discover_destroy();
+		return 0;
+	}
+
+	if (error->status == BLE_HS_EDONE) {
+		tester_send(BTP_SERVICE_ID_GATT, opcode,
+			    CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len);
+		discover_destroy();
+		return 0;
+	}
+
+	uuid = &gatt_svc->uuid;
+	uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16);
+
+	service = gatt_buf_reserve(sizeof(*service) + uuid_length);
+	if (!service) {
+		tester_rsp(BTP_SERVICE_ID_GATT, opcode,
+		CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		discover_destroy();
+		return BLE_HS_ENOMEM;
+	}
+
+	service->start_handle = sys_cpu_to_le16(gatt_svc->start_handle);
+	service->end_handle = sys_cpu_to_le16(gatt_svc->end_handle);
+	service->uuid_length = uuid_length;
+
+	if (uuid->u.type == BLE_UUID_TYPE_16) {
+		u16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value);
+		memcpy(service->uuid, &u16, uuid_length);
+	} else {
+		memcpy(service->uuid, BLE_UUID128(uuid)->value,
+		uuid_length);
+	}
+
+	rp->services_count++;
+
+	return 0;
+}
+
+static int disc_all_desc_cb(uint16_t conn_handle,
+			    const struct ble_gatt_error *error,
+			    uint16_t chr_val_handle,
+			    const struct ble_gatt_dsc *gatt_dsc,
+			    void *arg)
+{
+	struct gatt_disc_all_desc_rp *rp = (void *) gatt_buf.buf;
+	struct gatt_descriptor *dsc;
+	const ble_uuid_any_t *uuid;
+	u8_t uuid_length;
+
+	SYS_LOG_DBG("");
+
+	if (error->status != 0 && error->status != BLE_HS_EDONE) {
+		tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		discover_destroy();
+		return 0;
+	}
+
+	if (error->status == BLE_HS_EDONE) {
+		tester_send(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC,
+			    CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len);
+		discover_destroy();
+		return 0;
+	}
+
+	uuid = &gatt_dsc->uuid;
+	uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16);
+
+	dsc = gatt_buf_reserve(sizeof(*dsc) + uuid_length);
+	if (!dsc) {
+		tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		discover_destroy();
+		return BLE_HS_ENOMEM;
+	}
+
+	dsc->descriptor_handle = sys_cpu_to_le16(gatt_dsc->handle);
+	dsc->uuid_length = uuid_length;
+
+	if (uuid->u.type == BLE_UUID_TYPE_16) {
+		u16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value);
+		memcpy(dsc->uuid, &u16, uuid_length);
+	} else {
+		memcpy(dsc->uuid, BLE_UUID128(uuid)->value, uuid_length);
+	}
+
+	rp->descriptors_count++;
+
+	return 0;
+}
+
+static void disc_all_prim_svcs(u8_t *data, u16_t len)
+{
+	struct ble_gap_conn_desc conn;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_prim_svcs_rp))) {
+		goto fail;
+	}
+
+	if (ble_gattc_disc_all_svcs(conn.conn_handle, disc_prim_uuid_cb,
+				    (void *) GATT_DISC_ALL_PRIM_SVCS)) {
+		discover_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_PRIM_SVCS,
+		   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+}
+
+static void disc_all_desc(u8_t *data, u16_t len)
+{
+	const struct gatt_disc_all_desc_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	uint16_t start_handle, end_handle;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_desc_rp))) {
+		goto fail;
+	}
+
+	start_handle = sys_le16_to_cpu(cmd->start_handle) - 1;
+	end_handle = sys_le16_to_cpu(cmd->end_handle);
+
+	rc = ble_gattc_disc_all_dscs(conn.conn_handle, start_handle, end_handle,
+	disc_all_desc_cb, NULL);
+
+	SYS_LOG_DBG("rc=%d", rc);
+
+	if (rc) {
+		discover_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static int find_included_cb(uint16_t conn_handle,
+			    const struct ble_gatt_error *error,
+			    const struct ble_gatt_svc *gatt_svc, void *arg)
+{
+	struct gatt_find_included_rp *rp = (void *) gatt_buf.buf;
+	struct gatt_included *included;
+	const ble_uuid_any_t *uuid;
+	int service_handle = (int) arg;
+	u8_t uuid_length;
+
+	SYS_LOG_DBG("");
+
+	if (error->status != 0 && error->status != BLE_HS_EDONE) {
+		tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		discover_destroy();
+		return 0;
+	}
+
+	if (error->status == BLE_HS_EDONE) {
+		tester_send(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED,
+			    CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len);
+		discover_destroy();
+		return 0;
+	}
+
+	uuid = &gatt_svc->uuid;
+	uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16);
+
+
+	included = gatt_buf_reserve(sizeof(*included) + uuid_length);
+	if (!included) {
+		tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		discover_destroy();
+		return BLE_HS_ENOMEM;
+	}
+
+	/* FIXME */
+	included->included_handle = sys_cpu_to_le16(service_handle + 1 +
+	rp->services_count);
+	included->service.start_handle = sys_cpu_to_le16(gatt_svc->start_handle);
+	included->service.end_handle = sys_cpu_to_le16(gatt_svc->end_handle);
+	included->service.uuid_length = uuid_length;
+
+	if (uuid->u.type == BLE_UUID_TYPE_16) {
+		u16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value);
+		memcpy(included->service.uuid, &u16, uuid_length);
+	} else {
+		memcpy(included->service.uuid, BLE_UUID128(uuid)->value,
+		       uuid_length);
+	}
+
+	rp->services_count++;
+
+	return 0;
+}
+
+static int disc_chrc_cb(uint16_t conn_handle,
+			const struct ble_gatt_error *error,
+			const struct ble_gatt_chr *gatt_chr, void *arg)
+{
+	struct gatt_disc_chrc_rp *rp = (void *) gatt_buf.buf;
+	struct gatt_characteristic *chrc;
+	const ble_uuid_any_t *uuid;
+	u8_t btp_opcode = (uint8_t) (int) arg;
+	u8_t uuid_length;
+
+	SYS_LOG_DBG("");
+
+	if (error->status != 0 && error->status != BLE_HS_EDONE) {
+		tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		discover_destroy();
+		return 0;
+	}
+
+	if (error->status == BLE_HS_EDONE) {
+		tester_send(BTP_SERVICE_ID_GATT, btp_opcode,
+			    CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len);
+		discover_destroy();
+		return 0;
+	}
+
+	uuid = &gatt_chr->uuid;
+	uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16);
+
+	chrc = gatt_buf_reserve(sizeof(*chrc) + uuid_length);
+	if (!chrc) {
+		tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+		discover_destroy();
+		return BLE_HS_ENOMEM;
+	}
+
+	chrc->characteristic_handle = sys_cpu_to_le16(gatt_chr->def_handle);
+	chrc->properties = gatt_chr->properties;
+	chrc->value_handle = sys_cpu_to_le16(gatt_chr->val_handle);
+	chrc->uuid_length = uuid_length;
+
+	if (uuid->u.type == BLE_UUID_TYPE_16) {
+		u16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value);
+		memcpy(chrc->uuid, &u16, uuid_length);
+	} else {
+		memcpy(chrc->uuid, BLE_UUID128(uuid)->value,
+		uuid_length);
+	}
+
+	rp->characteristics_count++;
+
+	return 0;
+}
+
+static void disc_chrc_uuid(u8_t *data, u16_t len)
+{
+	const struct gatt_disc_chrc_uuid_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	uint16_t start_handle, end_handle;
+	ble_uuid_any_t uuid;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) {
+		goto fail;
+	}
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) {
+		goto fail;
+	}
+
+	start_handle = sys_le16_to_cpu(cmd->start_handle);
+	end_handle = sys_le16_to_cpu(cmd->end_handle);
+
+	if (ble_gattc_disc_chrs_by_uuid(conn.conn_handle, start_handle,
+					end_handle, &uuid.u, disc_chrc_cb,
+					(void *)GATT_DISC_CHRC_UUID)) {
+		discover_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_CHRC_UUID, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void disc_prim_uuid(u8_t *data, u16_t len)
+{
+	const struct gatt_disc_prim_uuid_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	ble_uuid_any_t uuid;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) {
+		goto fail;
+	}
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_disc_prim_uuid_rp))) {
+		goto fail;
+	}
+
+	if (ble_gattc_disc_svc_by_uuid(conn.conn_handle,
+				       &uuid.u, disc_prim_uuid_cb,
+				       (void *) GATT_DISC_PRIM_UUID)) {
+		discover_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_PRIM_UUID, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void disc_all_chrc(u8_t *data, u16_t len)
+{
+	const struct gatt_disc_all_chrc_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	uint16_t start_handle, end_handle;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		SYS_LOG_DBG("Conn find failed");
+		goto fail;
+	}
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) {
+		SYS_LOG_DBG("Buf reserve failed");
+		goto fail;
+	}
+
+	start_handle = sys_le16_to_cpu(cmd->start_handle);
+	end_handle = sys_le16_to_cpu(cmd->end_handle);
+
+	rc = ble_gattc_disc_all_chrs(conn.conn_handle, start_handle, end_handle,
+	disc_chrc_cb, (void *)GATT_DISC_ALL_CHRC);
+	if (rc) {
+		discover_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_CHRC, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void find_included(u8_t *data, u16_t len)
+{
+	const struct gatt_find_included_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	uint16_t start_handle, end_handle;
+	int service_handle_arg;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	if (!gatt_buf_reserve(sizeof(struct gatt_find_included_rp))) {
+		goto fail;
+	}
+
+	start_handle = sys_le16_to_cpu(cmd->start_handle);
+	end_handle = sys_le16_to_cpu(cmd->end_handle);
+	service_handle_arg = start_handle;
+
+	if (ble_gattc_find_inc_svcs(conn.conn_handle, start_handle, end_handle,
+				    find_included_cb,
+				    (void *)service_handle_arg)) {
+		discover_destroy();
+		goto fail;
+	}
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static int exchange_func(uint16_t conn_handle,
+			 const struct ble_gatt_error *error,
+			 uint16_t mtu, void *arg)
+{
+	SYS_LOG_DBG("");
+
+	if (error->status) {
+		tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+
+	return 0;
+}
+
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, CONTROLLER_INDEX,
+		   BTP_STATUS_SUCCESS);
+
+	return 0;
+}
+
+static void exchange_mtu(u8_t *data, u16_t len)
+{
+	struct ble_gap_conn_desc conn;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		goto fail;
+	}
+
+	if (ble_gattc_exchange_mtu(conn.conn_handle, exchange_func, NULL)) {
+		goto fail;
+	}
+
+	return;
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU,
+		   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+}
+
+static int enable_subscription(u16_t conn_handle, u16_t ccc_handle,
+			       u16_t value)
+{
+	u8_t op;
+
+	SYS_LOG_DBG("");
+
+	op = (uint8_t) (value == 0x0001 ? GATT_CFG_NOTIFY : GATT_CFG_INDICATE);
+
+	if (ble_gattc_write_flat(conn_handle, ccc_handle,
+				 &value, sizeof(value), NULL, NULL)) {
+		return -EINVAL;
+	}
+
+	subscribe_params.ccc_handle = value;
+
+	tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX,
+		   BTP_STATUS_SUCCESS);
+	return 0;
+}
+
+static int disable_subscription(u16_t conn_handle, u16_t ccc_handle)
+{
+	u16_t value = 0x00;
+
+	SYS_LOG_DBG("");
+
+	/* Fail if CCC handle doesn't match */
+	if (ccc_handle != subscribe_params.ccc_handle) {
+		SYS_LOG_ERR("CCC handle doesn't match");
+		return -EINVAL;
+	}
+
+	if (ble_gattc_write_no_rsp_flat(conn_handle, ccc_handle,
+					&value, sizeof(value))) {
+		return -EINVAL;
+	}
+
+	subscribe_params.ccc_handle = 0;
+	return 0;
+}
+
+static void config_subscription(u8_t *data, u16_t len, u8_t op)
+{
+	const struct gatt_cfg_notify_cmd *cmd = (void *) data;
+	struct ble_gap_conn_desc conn;
+	u16_t ccc_handle = sys_le16_to_cpu(cmd->ccc_handle);
+	u8_t status;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+	if (rc) {
+		tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX,
+			   BTP_STATUS_FAILED);
+		return;
+	}
+
+	if (cmd->enable) {
+		u16_t value;
+
+		if (op == GATT_CFG_NOTIFY) {
+			value = 0x0001;
+		} else {
+			value = 0x0002;
+		}
+
+		/* on success response will be sent from callback */
+		if (enable_subscription(conn.conn_handle,
+					ccc_handle, value) == 0) {
+			return;
+		}
+
+		status = BTP_STATUS_FAILED;
+	} else {
+		if (disable_subscription(conn.conn_handle, ccc_handle) < 0) {
+			status = BTP_STATUS_FAILED;
+		} else {
+			status = BTP_STATUS_SUCCESS;
+		}
+	}
+
+	SYS_LOG_DBG("Config subscription (op %u) status %u", op, status);
+
+	tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status);
+}
+
+#define BTP_PERM_F_READ                      0x01
+#define BTP_PERM_F_WRITE                     0x02
+#define BTP_PERM_F_READ_ENC                  0x04
+#define BTP_PERM_F_WRITE_ENC                 0x08
+#define BTP_PERM_F_READ_AUTHEN               0x10
+#define BTP_PERM_F_WRITE_AUTHEN              0x20
+#define BTP_PERM_F_READ_AUTHOR               0x40
+#define BTP_PERM_F_WRITE_AUTHOR              0x80
+
+static int flags_hs2btp_map[] = {
+	BTP_PERM_F_READ,
+	BTP_PERM_F_WRITE,
+	BTP_PERM_F_READ_ENC,
+	BTP_PERM_F_READ_AUTHEN,
+	BTP_PERM_F_READ_AUTHOR,
+	BTP_PERM_F_WRITE_ENC,
+	BTP_PERM_F_WRITE_AUTHEN,
+	BTP_PERM_F_WRITE_AUTHOR,
+};
+
+static u8_t flags_hs2btp(u8_t flags)
+{
+	int i;
+	u8_t ret = 0;
+
+	for (i = 0; i < 8; ++i) {
+		if (flags & BIT(i)) {
+			ret |= flags_hs2btp_map[i];
+		}
+	}
+
+	return ret;
+}
+
+static void get_attrs(u8_t *data, u16_t len)
+{
+	const struct gatt_get_attributes_cmd *cmd = (void *) data;
+	struct gatt_get_attributes_rp *rp;
+	struct gatt_attr *gatt_attr;
+	struct os_mbuf *buf = os_msys_get(0, 0);
+	u16_t start_handle, end_handle;
+	struct ble_att_svr_entry *entry = NULL;
+	ble_uuid_any_t uuid;
+	ble_uuid_t *uuid_ptr = NULL;
+	u8_t count = 0;
+	char str[BLE_UUID_STR_LEN];
+
+	SYS_LOG_DBG("");
+
+	memset(str, 0, sizeof(str));
+	memset(&uuid, 0, sizeof(uuid));
+	start_handle = sys_le16_to_cpu(cmd->start_handle);
+	end_handle = sys_le16_to_cpu(cmd->end_handle);
+
+	if (cmd->type_length) {
+		if (btp2bt_uuid(cmd->type, cmd->type_length, &uuid)) {
+			goto fail;
+		}
+
+		ble_uuid_to_str(&uuid.u, str);
+		SYS_LOG_DBG("start 0x%04x end 0x%04x, uuid %s", start_handle,
+			    end_handle, str);
+
+		uuid_ptr = &uuid.u;
+	} else {
+		SYS_LOG_DBG("start 0x%04x end 0x%04x", start_handle, end_handle);
+	}
+
+	net_buf_simple_init(buf, 0);
+	rp = net_buf_simple_add(buf, sizeof(*rp));
+
+	entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle);
+	while (entry) {
+
+		if (entry->ha_handle_id < start_handle) {
+			entry = ble_att_svr_find_by_uuid(entry,
+			uuid_ptr, end_handle);
+			continue;
+		}
+
+		gatt_attr = net_buf_simple_add(buf, sizeof(*gatt_attr));
+		gatt_attr->handle = sys_cpu_to_le16(entry->ha_handle_id);
+		gatt_attr->permission = flags_hs2btp(entry->ha_flags);
+
+		if (entry->ha_uuid->type == BLE_UUID_TYPE_16) {
+			gatt_attr->type_length = 2;
+			net_buf_simple_add_le16(buf,
+			BLE_UUID16(entry->ha_uuid)->value);
+		} else {
+			gatt_attr->type_length = 16;
+			net_buf_simple_add_mem(buf,
+			BLE_UUID128(entry->ha_uuid)->value,
+			gatt_attr->type_length);
+		}
+
+		count++;
+
+		entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle);
+	}
+
+	rp->attrs_count = count;
+
+	tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES,
+			CONTROLLER_INDEX, buf);
+
+	goto free;
+fail:
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+free:
+	os_mbuf_free_chain(buf);
+}
+
+static void get_attr_val(u8_t *data, u16_t len)
+{
+	const struct gatt_get_attribute_value_cmd *cmd = (void *) data;
+	struct gatt_get_attribute_value_rp *rp;
+	struct ble_gap_conn_desc conn;
+	struct os_mbuf *buf = os_msys_get(0, 0);
+	u16_t handle = sys_cpu_to_le16(cmd->handle);
+	uint8_t out_att_err;
+	int conn_status;
+
+	conn_status = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn);
+
+	if (conn_status) {
+		net_buf_simple_init(buf, 0);
+		rp = net_buf_simple_add(buf, sizeof(*rp));
+
+		ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE,
+					handle, 0, buf,
+					&out_att_err);
+
+		rp->att_response = out_att_err;
+		rp->value_length = os_mbuf_len(buf) - sizeof(*rp);
+
+		tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTE_VALUE,
+				CONTROLLER_INDEX, buf);
+
+		goto free;
+	} else {
+		net_buf_simple_init(buf, 0);
+		rp = net_buf_simple_add(buf, sizeof(*rp));
+
+		ble_att_svr_read_handle(conn.conn_handle,
+		handle, 0, buf,
+		&out_att_err);
+
+		rp->att_response = out_att_err;
+		rp->value_length = os_mbuf_len(buf) - sizeof(*rp);
+
+		tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTE_VALUE,
+				CONTROLLER_INDEX, buf);
+
+		goto free;
+	}
+
+free:
+	os_mbuf_free_chain(buf);
+}
+
+static void change_database(u8_t *data, u16_t len)
+{
+	const struct gatt_change_database *cmd = (void *) data;
+
+	SYS_LOG_DBG("")
+
+	ble_gatts_show_local();
+
+	ble_svc_gatt_changed(cmd->start_handle, cmd->end_handle);
+
+	tester_rsp(BTP_SERVICE_ID_GATT, GATT_CHANGE_DATABASE, CONTROLLER_INDEX,
+		   BTP_STATUS_SUCCESS);
+
+	return;
+}
+
+static void supported_commands(u8_t *data, u16_t len)
+{
+	u8_t cmds[4];
+	struct gatt_read_supported_commands_rp *rp = (void *) cmds;
+
+	SYS_LOG_DBG("");
+
+	memset(cmds, 0, sizeof(cmds));
+
+	tester_set_bit(cmds, GATT_READ_SUPPORTED_COMMANDS);
+	tester_set_bit(cmds, GATT_START_SERVER);
+	tester_set_bit(cmds, GATT_EXCHANGE_MTU);
+	tester_set_bit(cmds, GATT_DISC_ALL_PRIM_SVCS);
+	tester_set_bit(cmds, GATT_DISC_PRIM_UUID);
+	tester_set_bit(cmds, GATT_FIND_INCLUDED);
+	tester_set_bit(cmds, GATT_DISC_ALL_CHRC);
+	tester_set_bit(cmds, GATT_DISC_CHRC_UUID);
+	tester_set_bit(cmds, GATT_DISC_ALL_DESC);
+	tester_set_bit(cmds, GATT_READ);
+	tester_set_bit(cmds, GATT_READ_LONG);
+	tester_set_bit(cmds, GATT_READ_MULTIPLE);
+	tester_set_bit(cmds, GATT_WRITE_WITHOUT_RSP);
+#if 0
+	tester_set_bit(cmds, GATT_SIGNED_WRITE_WITHOUT_RSP);
+#endif
+	tester_set_bit(cmds, GATT_WRITE);
+	tester_set_bit(cmds, GATT_WRITE_LONG);
+	tester_set_bit(cmds, GATT_CFG_NOTIFY);
+	tester_set_bit(cmds, GATT_CFG_INDICATE);
+	tester_set_bit(cmds, GATT_GET_ATTRIBUTES);
+	tester_set_bit(cmds, GATT_GET_ATTRIBUTE_VALUE);
+	tester_set_bit(cmds, GATT_CHANGE_DATABASE);
+
+	tester_send(BTP_SERVICE_ID_GATT, GATT_READ_SUPPORTED_COMMANDS,
+	CONTROLLER_INDEX, (u8_t *) rp, sizeof(cmds));
+}
+
+enum attr_type {
+	BLE_GATT_ATTR_SVC = 0,
+	BLE_GATT_ATTR_CHR,
+	BLE_GATT_ATTR_DSC,
+};
+
+void tester_handle_gatt(u8_t opcode, u8_t index, u8_t *data,
+                                                u16_t len)
+{
+	switch (opcode) {
+	case GATT_READ_SUPPORTED_COMMANDS:
+		supported_commands(data, len);
+		return;
+	case GATT_START_SERVER:
+		start_server(data, len);
+		return;
+	case GATT_EXCHANGE_MTU:
+		exchange_mtu(data, len);
+		return;
+	case GATT_DISC_ALL_PRIM_SVCS:
+		disc_all_prim_svcs(data, len);
+		return;
+	case GATT_DISC_PRIM_UUID:
+		disc_prim_uuid(data, len);
+		return;
+	case GATT_FIND_INCLUDED:
+		find_included(data, len);
+		return;
+	case GATT_DISC_ALL_CHRC:
+		disc_all_chrc(data, len);
+		return;
+	case GATT_DISC_CHRC_UUID:
+		disc_chrc_uuid(data, len);
+		return;
+	case GATT_DISC_ALL_DESC:
+		disc_all_desc(data, len);
+		return;
+	case GATT_CHANGE_DATABASE:
+		change_database(data, len);
+		return;
+	case GATT_READ:
+		read(data, len);
+		return;
+	case GATT_READ_UUID:
+		read_uuid(data, len);
+		return;
+	case GATT_READ_LONG:
+		read_long(data, len);
+		return;
+	case GATT_READ_MULTIPLE:
+		read_multiple(data, len);
+		return;
+	case GATT_WRITE_WITHOUT_RSP:
+		write_without_rsp(data, len, opcode, false);
+		return;
+#if 0
+	case GATT_SIGNED_WRITE_WITHOUT_RSP:
+		write_without_rsp(data, len, opcode, true);
+		return;
+#endif
+	case GATT_WRITE:
+		write(data, len);
+		return;
+	case GATT_WRITE_LONG:
+		write_long(data, len);
+		return;
+	case GATT_RELIABLE_WRITE:
+		reliable_write(data, len);
+		return;
+	case GATT_CFG_NOTIFY:
+	case GATT_CFG_INDICATE:
+		config_subscription(data, len, opcode);
+		return;
+	case GATT_GET_ATTRIBUTES:
+		get_attrs(data, len);
+		return;
+	case GATT_GET_ATTRIBUTE_VALUE:
+		get_attr_val(data, len);
+		return;
+	default:
+		tester_rsp(BTP_SERVICE_ID_GATT, opcode, index,
+			   BTP_STATUS_UNKNOWN_CMD);
+	return;
+	}
+}
+
+int tester_gatt_notify_rx_ev(u16_t conn_handle, u16_t attr_handle,
+			     u8_t indication, struct os_mbuf *om)
+{
+	struct gatt_notification_ev *ev;
+	struct ble_gap_conn_desc conn;
+	struct os_mbuf *buf = os_msys_get(0, 0);
+	const ble_addr_t *addr;
+
+	SYS_LOG_DBG("");
+
+	if (!subscribe_params.ccc_handle) {
+		goto fail;
+	}
+
+	if (ble_gap_conn_find(conn_handle, &conn)) {
+		goto fail;
+	}
+
+	net_buf_simple_init(buf, 0);
+	ev = net_buf_simple_add(buf, sizeof(*ev));
+
+	addr = &conn.peer_ota_addr;
+
+	ev->address_type = addr->type;
+	memcpy(ev->address, addr->val, sizeof(ev->address));
+	ev->type = (u8_t) (indication ? 0x02 : 0x01);
+	ev->handle = sys_cpu_to_le16(attr_handle);
+	ev->data_length = sys_cpu_to_le16(os_mbuf_len(om));
+	os_mbuf_appendfrom(buf, om, 0, os_mbuf_len(om));
+
+	tester_send_buf(BTP_SERVICE_ID_GATT, GATT_EV_NOTIFICATION,
+			CONTROLLER_INDEX, buf);
+
+fail:
+	os_mbuf_free_chain(buf);
+	return 0;
+}
+
+void notify_test_stop(void)
+{
+	os_callout_stop(&notify_tx_timer);
+}
+
+void notify_test_reset(void)
+{
+	int rc;
+
+	rc = os_callout_reset(&notify_tx_timer, OS_TICKS_PER_SEC);
+	assert(rc == 0);
+}
+
+void notify_test(struct os_event *ev)
+{
+	static uint8_t ntf[1];
+	struct os_mbuf *om;
+	int rc;
+
+	if (!notify_state && !indicate_state) {
+		notify_test_stop();
+		notify_value = 90;
+		return;
+	}
+
+	ntf[0] = notify_value;
+
+	notify_value++;
+	if (notify_value == 160) {
+		notify_value = 90;
+	}
+
+	om = ble_hs_mbuf_from_flat(ntf, sizeof(ntf));
+
+	if (notify_state) {
+		rc = ble_gattc_notify_custom(myconn_handle, notify_handle, om);
+		assert(rc == 0);
+	}
+
+	if (indicate_state) {
+		rc = ble_gattc_indicate_custom(myconn_handle, notify_handle, om);
+		assert(rc == 0);
+	}
+}
+
+int tester_gatt_subscribe_ev(u16_t conn_handle, u16_t attr_handle, u8_t reason,
+			     u8_t prev_notify, u8_t cur_notify,
+			     u8_t prev_indicate, u8_t cur_indicate)
+{
+	SYS_LOG_DBG("");
+	myconn_handle = conn_handle;
+
+	if (cur_notify == 0 && cur_indicate == 0) {
+		SYS_LOG_INF("Unsubscribed");
+		memset(&subscribe_params, 0, sizeof(subscribe_params));
+		return 0;
+	}
+
+	if (cur_notify) {
+		SYS_LOG_INF("Subscribed to notifications");
+		if (attr_handle == notify_handle) {
+			notify_state = cur_notify;
+		}
+	}
+
+	if (cur_indicate) {
+		SYS_LOG_INF("Subscribed to indications");
+		if (attr_handle == notify_handle) {
+			indicate_state = cur_indicate;
+		}
+	}
+
+
+	if (notify_state || indicate_state) {
+		notify_test_reset();
+	} else {
+		notify_test_stop();
+	}
+
+	return 0;
+}
+
+void
+gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
+{
+	char buf[BLE_UUID_STR_LEN];
+
+	switch (ctxt->op) {
+	case BLE_GATT_REGISTER_OP_SVC:
+		MODLOG_DFLT(DEBUG, "registered service %s with handle=%d\n",
+		ble_uuid_to_str(ctxt->svc.svc_def->uuid, buf),
+				ctxt->svc.handle);
+		break;
+
+	case BLE_GATT_REGISTER_OP_CHR:
+		MODLOG_DFLT(DEBUG, "registering characteristic %s with "
+			    "def_handle=%d val_handle=%d\n",
+			    ble_uuid_to_str(ctxt->chr.chr_def->uuid, buf),
+			    ctxt->chr.def_handle,
+			    ctxt->chr.val_handle);
+	break;
+
+	case BLE_GATT_REGISTER_OP_DSC:
+		MODLOG_DFLT(DEBUG, "registering descriptor %s with handle=%d\n",
+			    ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buf),
+			    ctxt->dsc.handle);
+		break;
+
+	default:
+		assert(0);
+		break;
+	}
+}
+
+int gatt_svr_init(void)
+{
+	int rc;
+
+	rc = ble_gatts_count_cfg(gatt_svr_inc_svcs);
+	if (rc != 0) {
+		return rc;
+	}
+
+	rc = ble_gatts_add_svcs(gatt_svr_inc_svcs);
+	if (rc != 0) {
+		return rc;
+	}
+
+	rc = ble_gatts_count_cfg(gatt_svr_svcs);
+	if (rc != 0) {
+		return rc;
+	}
+
+	rc = ble_gatts_add_svcs(gatt_svr_svcs);
+	if (rc != 0) {
+		return rc;
+	}
+
+	return 0;
+}
+
+u8_t tester_init_gatt(void)
+{
+	os_callout_init(&notify_tx_timer, os_eventq_dflt_get(),
+			notify_test, NULL);
+
+	return BTP_STATUS_SUCCESS;
+}
+
+u8_t tester_unregister_gatt(void)
+{
+	return BTP_STATUS_SUCCESS;
+}
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/glue.c b/src/libs/mynewt-nimble/apps/bttester/src/glue.c
new file mode 100644
index 00000000..6cd7643c
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/glue.c
@@ -0,0 +1,129 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "syscfg/syscfg.h"
+
+#if !MYNEWT_VAL(BLE_MESH)
+#include <assert.h>
+#include <string.h>
+#include "os/os.h"
+#include "os/os_mbuf.h"
+#include "glue.h"
+
+
+#define ASSERT_NOT_CHAIN(om) assert(SLIST_NEXT(om, om_next) == NULL)
+
+const char *bt_hex(const void *buf, size_t len)
+{
+	static const char hex[] = "0123456789abcdef";
+	static char hexbufs[4][137];
+	static u8_t curbuf;
+	const u8_t *b = buf;
+	char *str;
+	int i;
+
+	str = hexbufs[curbuf++];
+	curbuf %= ARRAY_SIZE(hexbufs);
+
+	len = min(len, (sizeof(hexbufs[0]) - 1) / 2);
+
+	for (i = 0; i < len; i++) {
+		str[i * 2] = hex[b[i] >> 4];
+		str[i * 2 + 1] = hex[b[i] & 0xf];
+	}
+
+	str[i * 2] = '\0';
+
+	return str;
+}
+
+struct os_mbuf * NET_BUF_SIMPLE(uint16_t size)
+{
+	struct os_mbuf *buf;
+
+	buf = os_msys_get(size, 0);
+	assert(buf);
+
+	return buf;
+}
+
+/* This is by purpose */
+void net_buf_simple_init(struct os_mbuf *buf,
+			 size_t reserve_head)
+{
+	/* This is called in Zephyr after init.
+	 * Note in Mynewt case we don't care abour reserved head*/
+	buf->om_data = &buf->om_databuf[buf->om_pkthdr_len] + reserve_head;
+	buf->om_len = 0;
+}
+
+void
+net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val)
+{
+	val = htole16(val);
+	os_mbuf_append(om, &val, sizeof(val));
+	ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_add_be16(struct os_mbuf *om, uint16_t val)
+{
+	val = htobe16(val);
+	os_mbuf_append(om, &val, sizeof(val));
+	ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_add_be32(struct os_mbuf *om, uint32_t val)
+{
+	val = htobe32(val);
+	os_mbuf_append(om, &val, sizeof(val));
+	ASSERT_NOT_CHAIN(om);
+}
+
+void
+net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val)
+{
+	os_mbuf_append(om, &val, 1);
+	ASSERT_NOT_CHAIN(om);
+}
+
+void*
+net_buf_simple_add(struct os_mbuf *om, uint8_t len)
+{
+	void * tmp;
+
+	tmp = os_mbuf_extend(om, len);
+	ASSERT_NOT_CHAIN(om);
+
+	return tmp;
+}
+
+uint8_t *
+net_buf_simple_push(struct os_mbuf *om, uint8_t len)
+{
+	uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len];
+
+	assert(headroom >= len);
+	om->om_data -= len;
+	om->om_len += len;
+
+	return om->om_data;
+}
+#endif
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/glue.h b/src/libs/mynewt-nimble/apps/bttester/src/glue.h
new file mode 100644
index 00000000..e563331e
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/glue.h
@@ -0,0 +1,63 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef __GLUE_H__
+#define __GLUE_H__
+
+#include "os/endian.h"
+
+#define u8_t    uint8_t
+#define s8_t    int8_t
+#define u16_t   uint16_t
+#define u32_t   uint32_t
+#define s32_t   int32_t
+
+#ifndef BIT
+#define BIT(n)  (1UL << (n))
+#endif
+
+#define __packed    __attribute__((__packed__))
+
+#define sys_le16_to_cpu le16toh
+
+struct bt_data {
+    u8_t type;
+    u8_t data_len;
+    const u8_t *data;
+};
+
+#define BT_DATA(_type, _data, _data_len) \
+    { \
+        .type = (_type), \
+        .data_len = (_data_len), \
+        .data = (const u8_t *)(_data), \
+    }
+
+struct os_mbuf * NET_BUF_SIMPLE(uint16_t size);
+void net_buf_simple_init(struct os_mbuf *buf, size_t reserve_head);
+void net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val);
+void net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val);
+void *net_buf_simple_add(struct os_mbuf *om, uint8_t len);
+uint8_t *net_buf_simple_push(struct os_mbuf *om, uint8_t len);
+
+#define net_buf_simple_add_mem(a,b,c) os_mbuf_append(a,b,c)
+
+const char *bt_hex(const void *buf, size_t len);
+
+#endif /* __GLUE_H__ */
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/l2cap.c b/src/libs/mynewt-nimble/apps/bttester/src/l2cap.c
new file mode 100644
index 00000000..45b904a1
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/l2cap.c
@@ -0,0 +1,477 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/* l2cap.c - Bluetooth L2CAP Tester */
+
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+
+#include "console/console.h"
+#include "host/ble_gap.h"
+#include "host/ble_l2cap.h"
+
+#include "bttester.h"
+
+#define CONTROLLER_INDEX             0
+#define CHANNELS                     MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
+#define TESTER_COC_MTU               (230)
+#define TESTER_COC_BUF_COUNT         (3 * MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM))
+
+static os_membuf_t tester_sdu_coc_mem[
+	OS_MEMPOOL_SIZE(TESTER_COC_BUF_COUNT, TESTER_COC_MTU)
+];
+
+struct os_mbuf_pool sdu_os_mbuf_pool;
+static struct os_mempool sdu_coc_mbuf_mempool;
+
+static struct channel {
+	u8_t chan_id; /* Internal number that identifies L2CAP channel. */
+	u8_t state;
+	struct ble_l2cap_chan *chan;
+} channels[CHANNELS];
+
+static u8_t recv_cb_buf[TESTER_COC_MTU + sizeof(struct l2cap_data_received_ev)];
+
+struct channel *find_channel(struct ble_l2cap_chan *chan) {
+	int i;
+
+	for (i = 0; i < CHANNELS; ++i) {
+		if (channels[i].chan == chan) {
+			return &channels[i];
+		}
+	}
+
+	return NULL;
+}
+
+static void
+tester_l2cap_coc_recv(struct ble_l2cap_chan *chan, struct os_mbuf *sdu)
+{
+	SYS_LOG_DBG("LE CoC SDU received, chan: 0x%08lx, data len %d",
+		    (uint32_t) chan, OS_MBUF_PKTLEN(sdu));
+
+	os_mbuf_free_chain(sdu);
+	sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+	assert(sdu != NULL);
+
+	ble_l2cap_recv_ready(chan, sdu);
+}
+
+static void recv_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan,
+		    struct os_mbuf *buf, void *arg)
+{
+	struct l2cap_data_received_ev *ev = (void *) recv_cb_buf;
+	struct channel *channel = arg;
+
+	ev->chan_id = channel->chan_id;
+	ev->data_length = buf->om_len;
+	memcpy(ev->data, buf->om_data, buf->om_len);
+
+	tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DATA_RECEIVED,
+		    CONTROLLER_INDEX, recv_cb_buf, sizeof(*ev) + buf->om_len);
+
+	tester_l2cap_coc_recv(chan, buf);
+}
+
+static void unstalled_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan,
+			 int status, void *arg)
+{
+	if (status) {
+		tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA,
+			   CONTROLLER_INDEX, BTP_STATUS_FAILED);
+	} else {
+		tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA,
+			   CONTROLLER_INDEX, BTP_STATUS_SUCCESS);
+	}
+}
+
+static struct channel *get_free_channel(void)
+{
+	u8_t i;
+	struct channel *chan;
+
+	for (i = 0; i < CHANNELS; i++) {
+		if (channels[i].state) {
+			continue;
+		}
+
+		chan = &channels[i];
+		chan->chan_id = i;
+
+		return chan;
+	}
+
+	return NULL;
+}
+
+static void connected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan,
+			 void *arg)
+{
+	struct l2cap_connected_ev ev;
+	struct ble_gap_conn_desc desc;
+	struct channel *channel;
+
+	channel = get_free_channel();
+	if (!channel) {
+		assert(0);
+	}
+
+	channel->chan = chan;
+	channel->state = 0;
+
+	ev.chan_id = channel->chan_id;
+	channel->state = 1;
+	channel->chan = chan;
+	/* TODO: ev.psm */
+
+	if (!ble_gap_conn_find(conn_handle, &desc)) {
+		ev.address_type = desc.peer_ota_addr.type;
+		memcpy(ev.address, desc.peer_ota_addr.val,
+		       sizeof(ev.address));
+	}
+
+	tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_CONNECTED, CONTROLLER_INDEX,
+		    (u8_t *) &ev, sizeof(ev));
+}
+
+static void disconnected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan,
+			    void *arg)
+{
+	struct l2cap_disconnected_ev ev;
+	struct ble_gap_conn_desc desc;
+	struct channel *channel;
+
+	memset(&ev, 0, sizeof(struct l2cap_disconnected_ev));
+
+	channel = find_channel(chan);
+	if (channel != NULL) {
+		channel->state = 0;
+		channel->chan = chan;
+
+		ev.chan_id = channel->chan_id;
+		/* TODO: ev.result */
+		/* TODO: ev.psm */
+	}
+
+	if (!ble_gap_conn_find(conn_handle, &desc)) {
+		ev.address_type = desc.peer_ota_addr.type;
+		memcpy(ev.address, desc.peer_ota_addr.val,
+		       sizeof(ev.address));
+	}
+
+	tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DISCONNECTED,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static int accept_cb(uint16_t conn_handle, uint16_t peer_mtu,
+		     struct ble_l2cap_chan *chan)
+{
+	struct os_mbuf *sdu_rx;
+
+	SYS_LOG_DBG("LE CoC accepting, chan: 0x%08lx, peer_mtu %d",
+		    (uint32_t) chan, peer_mtu);
+
+	sdu_rx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+	if (!sdu_rx) {
+		return BLE_HS_ENOMEM;
+	}
+
+	ble_l2cap_recv_ready(chan, sdu_rx);
+
+	return 0;
+}
+
+static int
+tester_l2cap_event(struct ble_l2cap_event *event, void *arg)
+{
+	struct ble_l2cap_chan_info chan_info;
+
+	switch (event->type) {
+	case BLE_L2CAP_EVENT_COC_CONNECTED:
+		if (event->connect.status) {
+			console_printf("LE COC error: %d\n", event->connect.status);
+			disconnected_cb(event->connect.conn_handle,
+					event->connect.chan, arg);
+			return 0;
+		}
+
+		ble_l2cap_get_chan_info(event->connect.chan, &chan_info);
+
+		console_printf("LE COC connected, conn: %d, chan: 0x%08lx, scid: 0x%04x, "
+			       "dcid: 0x%04x, our_mtu: 0x%04x, peer_mtu: 0x%04x\n",
+			       event->connect.conn_handle,
+			       (uint32_t) event->connect.chan,
+			       chan_info.scid,
+			       chan_info.dcid,
+			       chan_info.our_l2cap_mtu,
+			       chan_info.peer_l2cap_mtu);
+
+		connected_cb(event->connect.conn_handle,
+			     event->connect.chan, arg);
+
+		return 0;
+	case BLE_L2CAP_EVENT_COC_DISCONNECTED:
+		console_printf("LE CoC disconnected, chan: 0x%08lx\n",
+			       (uint32_t) event->disconnect.chan);
+
+		disconnected_cb(event->disconnect.conn_handle,
+				event->disconnect.chan, arg);
+		return 0;
+	case BLE_L2CAP_EVENT_COC_ACCEPT:
+		console_printf("LE CoC accept, chan: 0x%08lx, handle: %u, sdu_size: %u\n",
+			       (uint32_t) event->accept.chan,
+			       event->accept.conn_handle,
+			       event->accept.peer_sdu_size);
+
+		return accept_cb(event->accept.conn_handle,
+				 event->accept.peer_sdu_size,
+				 event->accept.chan);
+
+	case BLE_L2CAP_EVENT_COC_DATA_RECEIVED:
+		console_printf("LE CoC data received, chan: 0x%08lx, handle: %u, sdu_len: %u\n",
+			       (uint32_t) event->receive.chan,
+			       event->receive.conn_handle,
+			       event->receive.sdu_rx->om_len);
+		recv_cb(event->receive.conn_handle, event->receive.chan,
+			event->receive.sdu_rx, arg);
+		return 0;
+	case BLE_L2CAP_EVENT_COC_TX_UNSTALLED:
+		console_printf("LE CoC tx unstalled, chan: 0x%08lx, handle: %u, status: %d\n",
+			       (uint32_t) event->tx_unstalled.chan,
+			       event->tx_unstalled.conn_handle,
+			       event->tx_unstalled.status);
+		unstalled_cb(event->tx_unstalled.conn_handle,
+			     event->tx_unstalled.chan,
+			     event->tx_unstalled.status, arg);
+		return 0;
+	default:
+		return 0;
+	}
+}
+
+static void connect(u8_t *data, u16_t len)
+{
+	const struct l2cap_connect_cmd *cmd = (void *) data;
+	struct l2cap_connect_rp rp;
+	struct ble_gap_conn_desc desc;
+	struct channel *chan;
+	struct os_mbuf *sdu_rx;
+	ble_addr_t *addr = (void *) data;
+	int rc;
+
+	SYS_LOG_DBG("connect: type: %d addr: %s", addr->type, bt_hex(addr->val, 6));
+
+	rc = ble_gap_conn_find_by_addr(addr, &desc);
+	if (rc) {
+		SYS_LOG_ERR("GAP conn find failed");
+		goto fail;
+	}
+
+	chan = get_free_channel();
+	if (!chan) {
+		SYS_LOG_ERR("No free channels");
+		goto fail;
+	}
+
+	sdu_rx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+	if (sdu_rx == NULL) {
+		SYS_LOG_ERR("Failed to alloc buf");
+		goto fail;
+	}
+
+	rc = ble_l2cap_connect(desc.conn_handle, htole16(cmd->psm),
+			       TESTER_COC_MTU, sdu_rx,
+			       tester_l2cap_event, chan);
+	if (rc) {
+		SYS_LOG_ERR("L2CAP connect failed\n");
+		goto fail;
+	}
+
+	rp.chan_id = chan->chan_id;
+
+	tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX,
+		    (u8_t *) &rp, sizeof(rp));
+
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void disconnect(u8_t *data, u16_t len)
+{
+	const struct l2cap_disconnect_cmd *cmd = (void *) data;
+	struct channel *chan;
+	u8_t status;
+	int err;
+
+	SYS_LOG_DBG("");
+
+	chan = &channels[cmd->chan_id];
+
+	err = ble_l2cap_disconnect(chan->chan);
+	if (err) {
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	status = BTP_STATUS_SUCCESS;
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_DISCONNECT, CONTROLLER_INDEX,
+		   status);
+}
+
+static void send_data(u8_t *data, u16_t len)
+{
+	const struct l2cap_send_data_cmd *cmd = (void *) data;
+	struct channel *chan = &channels[cmd->chan_id];
+	struct os_mbuf *sdu_tx = NULL;
+	int rc;
+	u16_t data_len = sys_le16_to_cpu(cmd->data_len);
+
+	SYS_LOG_DBG("cmd->chan_id=%d", cmd->chan_id);
+
+	/* FIXME: For now, fail if data length exceeds buffer length */
+	if (data_len > TESTER_COC_MTU) {
+		SYS_LOG_ERR("Data length exceeds buffer length");
+		goto fail;
+	}
+
+	sdu_tx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
+	if (sdu_tx == NULL) {
+		SYS_LOG_ERR("No memory in the test sdu pool\n");
+		goto fail;
+	}
+
+	os_mbuf_append(sdu_tx, cmd->data, data_len);
+
+	/* ble_l2cap_send takes ownership of the sdu */
+	rc = ble_l2cap_send(chan->chan, sdu_tx);
+	if (rc == 0 || rc == BLE_HS_ESTALLED) {
+		tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, CONTROLLER_INDEX,
+			   BTP_STATUS_SUCCESS);
+		return;
+	}
+
+	SYS_LOG_ERR("Unable to send data: %d", rc);
+	os_mbuf_free_chain(sdu_tx);
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void listen(u8_t *data, u16_t len)
+{
+	const struct l2cap_listen_cmd *cmd = (void *) data;
+	int rc;
+
+	SYS_LOG_DBG("");
+
+	/* TODO: Handle cmd->transport flag */
+	rc = ble_l2cap_create_server(cmd->psm, TESTER_COC_MTU,
+				     tester_l2cap_event, NULL);
+	if (rc) {
+		goto fail;
+	}
+
+	tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_LISTEN, CONTROLLER_INDEX,
+		   BTP_STATUS_SUCCESS);
+	return;
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_LISTEN, CONTROLLER_INDEX,
+		   BTP_STATUS_FAILED);
+}
+
+static void supported_commands(u8_t *data, u16_t len)
+{
+	u8_t cmds[1];
+	struct l2cap_read_supported_commands_rp *rp = (void *) cmds;
+
+	memset(cmds, 0, sizeof(cmds));
+
+	tester_set_bit(cmds, L2CAP_READ_SUPPORTED_COMMANDS);
+	tester_set_bit(cmds, L2CAP_CONNECT);
+	tester_set_bit(cmds, L2CAP_DISCONNECT);
+	tester_set_bit(cmds, L2CAP_LISTEN);
+	tester_set_bit(cmds, L2CAP_SEND_DATA);
+
+	tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_READ_SUPPORTED_COMMANDS,
+		    CONTROLLER_INDEX, (u8_t *) rp, sizeof(cmds));
+}
+
+void tester_handle_l2cap(u8_t opcode, u8_t index, u8_t *data,
+			 u16_t len)
+{
+	switch (opcode) {
+	case L2CAP_READ_SUPPORTED_COMMANDS:
+		supported_commands(data, len);
+		return;
+	case L2CAP_CONNECT:
+		connect(data, len);
+		return;
+	case L2CAP_DISCONNECT:
+		disconnect(data, len);
+		return;
+	case L2CAP_SEND_DATA:
+		send_data(data, len);
+		return;
+	case L2CAP_LISTEN:
+		listen(data, len);
+		return;
+	default:
+		tester_rsp(BTP_SERVICE_ID_L2CAP, opcode, index,
+			   BTP_STATUS_UNKNOWN_CMD);
+		return;
+	}
+}
+
+u8_t tester_init_l2cap(void)
+{
+	int rc;
+
+	/* For testing we want to support all the available channels */
+	rc = os_mempool_init(&sdu_coc_mbuf_mempool, TESTER_COC_BUF_COUNT,
+			     TESTER_COC_MTU, tester_sdu_coc_mem,
+			     "tester_coc_sdu_pool");
+	assert(rc == 0);
+
+	rc = os_mbuf_pool_init(&sdu_os_mbuf_pool, &sdu_coc_mbuf_mempool,
+			       TESTER_COC_MTU, TESTER_COC_BUF_COUNT);
+	assert(rc == 0);
+
+	return BTP_STATUS_SUCCESS;
+}
+
+u8_t tester_unregister_l2cap(void)
+{
+	return BTP_STATUS_SUCCESS;
+}
+
+#endif
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/main.c b/src/libs/mynewt-nimble/apps/bttester/src/main.c
new file mode 100644
index 00000000..ea130805
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/main.c
@@ -0,0 +1,72 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/* main.c - Application main entry point */
+
+/*
+ * Copyright (c) 2015-2016 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "sysinit/sysinit.h"
+
+#include "modlog/modlog.h"
+#include "host/ble_uuid.h"
+#include "host/ble_hs.h"
+
+#include "bttester.h"
+
+static void on_reset(int reason)
+{
+	MODLOG_DFLT(ERROR, "Resetting state; reason=%d\n", reason);
+}
+
+static void on_sync(void)
+{
+	MODLOG_DFLT(INFO, "Bluetooth initialized\n");
+
+	tester_init();
+}
+
+int main(int argc, char **argv)
+{
+	int rc;
+
+#ifdef ARCH_sim
+	mcu_sim_parse_args(argc, argv);
+#endif
+
+	/* Initialize OS */
+	sysinit();
+
+	/* Initialize the NimBLE host configuration. */
+	ble_hs_cfg.reset_cb = on_reset;
+	ble_hs_cfg.sync_cb = on_sync;
+	ble_hs_cfg.gatts_register_cb = gatt_svr_register_cb,
+	ble_hs_cfg.store_status_cb = ble_store_util_status_rr;
+
+	rc = gatt_svr_init();
+	assert(rc == 0);
+
+	while (1) {
+		os_eventq_run(os_eventq_dflt_get());
+	}
+	return 0;
+}
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/mesh.c b/src/libs/mynewt-nimble/apps/bttester/src/mesh.c
new file mode 100644
index 00000000..e18a2a4e
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/mesh.c
@@ -0,0 +1,970 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+/* mesh.c - Bluetooth Mesh Tester */
+
+/*
+ * Copyright (c) 2017 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "syscfg/syscfg.h"
+
+#if MYNEWT_VAL(BLE_MESH)
+
+#include <errno.h>
+
+#include "mesh/mesh.h"
+#include "mesh/glue.h"
+#include "mesh/testing.h"
+#include "console/console.h"
+
+#include "bttester.h"
+
+extern u8_t own_addr_type;
+
+#define CONTROLLER_INDEX 0
+#define CID_LOCAL 0xffff
+
+/* Health server data */
+#define CUR_FAULTS_MAX 4
+#define HEALTH_TEST_ID 0x00
+
+static u8_t cur_faults[CUR_FAULTS_MAX];
+static u8_t reg_faults[CUR_FAULTS_MAX * 2];
+
+/* Provision node data */
+static u8_t net_key[16];
+static u16_t net_key_idx;
+static u8_t flags;
+static u32_t iv_index;
+static u16_t addr;
+static u8_t dev_key[16];
+static u8_t input_size;
+
+/* Configured provisioning data */
+static u8_t dev_uuid[16];
+static u8_t static_auth[16];
+
+/* Vendor Model data */
+#define VND_MODEL_ID_1 0x1234
+
+/* Model send data */
+#define MODEL_BOUNDS_MAX 2
+
+static struct model_data {
+	struct bt_mesh_model *model;
+	u16_t addr;
+	u16_t appkey_idx;
+} model_bound[MODEL_BOUNDS_MAX];
+
+static struct {
+	u16_t local;
+	u16_t dst;
+	u16_t net_idx;
+} net = {
+	.local = BT_MESH_ADDR_UNASSIGNED,
+	.dst = BT_MESH_ADDR_UNASSIGNED,
+};
+
+static void supported_commands(u8_t *data, u16_t len)
+{
+	struct os_mbuf *buf = NET_BUF_SIMPLE(BTP_DATA_MAX_SIZE);
+
+	net_buf_simple_init(buf, 0);
+
+	/* 1st octet */
+	memset(net_buf_simple_add(buf, 1), 0, 1);
+	tester_set_bit(buf->om_data, MESH_READ_SUPPORTED_COMMANDS);
+	tester_set_bit(buf->om_data, MESH_CONFIG_PROVISIONING);
+	tester_set_bit(buf->om_data, MESH_PROVISION_NODE);
+	tester_set_bit(buf->om_data, MESH_INIT);
+	tester_set_bit(buf->om_data, MESH_RESET);
+	tester_set_bit(buf->om_data, MESH_INPUT_NUMBER);
+	tester_set_bit(buf->om_data, MESH_INPUT_STRING);
+	/* 2nd octet */
+	tester_set_bit(buf->om_data, MESH_IVU_TEST_MODE);
+	tester_set_bit(buf->om_data, MESH_IVU_TOGGLE_STATE);
+	tester_set_bit(buf->om_data, MESH_NET_SEND);
+	tester_set_bit(buf->om_data, MESH_HEALTH_GENERATE_FAULTS);
+	tester_set_bit(buf->om_data, MESH_HEALTH_CLEAR_FAULTS);
+	tester_set_bit(buf->om_data, MESH_LPN);
+	tester_set_bit(buf->om_data, MESH_LPN_POLL);
+	tester_set_bit(buf->om_data, MESH_MODEL_SEND);
+	/* 3rd octet */
+	memset(net_buf_simple_add(buf, 1), 0, 1);
+#if MYNEWT_VAL(BLE_MESH_TESTING)
+	tester_set_bit(buf->om_data, MESH_LPN_SUBSCRIBE);
+	tester_set_bit(buf->om_data, MESH_LPN_UNSUBSCRIBE);
+	tester_set_bit(buf->om_data, MESH_RPL_CLEAR);
+#endif /* CONFIG_BT_TESTING */
+	tester_set_bit(buf->om_data, MESH_PROXY_IDENTITY);
+
+	tester_send_buf(BTP_SERVICE_ID_MESH, MESH_READ_SUPPORTED_COMMANDS,
+			CONTROLLER_INDEX, buf);
+}
+
+static struct bt_mesh_cfg_srv cfg_srv = {
+	.relay = BT_MESH_RELAY_ENABLED,
+	.beacon = BT_MESH_BEACON_ENABLED,
+#if MYNEWT_VAL(BLE_MESH_FRIEND)
+	.frnd = BT_MESH_FRIEND_ENABLED,
+#else
+	.frnd = BT_MESH_FRIEND_NOT_SUPPORTED,
+#endif
+#if MYNEWT_VAL(BLE_MESH_GATT_PROXY)
+	.gatt_proxy = BT_MESH_GATT_PROXY_ENABLED,
+#else
+	.gatt_proxy = BT_MESH_GATT_PROXY_NOT_SUPPORTED,
+#endif
+	.default_ttl = 7,
+
+	/* 3 transmissions with 20ms interval */
+	.net_transmit = BT_MESH_TRANSMIT(2, 20),
+	.relay_retransmit = BT_MESH_TRANSMIT(2, 20),
+};
+
+static void get_faults(u8_t *faults, u8_t faults_size, u8_t *dst, u8_t *count)
+{
+	u8_t i, limit = *count;
+
+	for (i = 0, *count = 0; i < faults_size && *count < limit; i++) {
+		if (faults[i]) {
+			*dst++ = faults[i];
+			(*count)++;
+		}
+	}
+}
+
+static int fault_get_cur(struct bt_mesh_model *model, u8_t *test_id,
+			 u16_t *company_id, u8_t *faults, u8_t *fault_count)
+{
+	SYS_LOG_DBG("");
+
+	*test_id = HEALTH_TEST_ID;
+	*company_id = CID_LOCAL;
+
+	get_faults(cur_faults, sizeof(cur_faults), faults, fault_count);
+
+	return 0;
+}
+
+static int fault_get_reg(struct bt_mesh_model *model, u16_t company_id,
+			 u8_t *test_id, u8_t *faults, u8_t *fault_count)
+{
+	SYS_LOG_DBG("company_id 0x%04x", company_id);
+
+	if (company_id != CID_LOCAL) {
+		return -EINVAL;
+	}
+
+	*test_id = HEALTH_TEST_ID;
+
+	get_faults(reg_faults, sizeof(reg_faults), faults, fault_count);
+
+	return 0;
+}
+
+static int fault_clear(struct bt_mesh_model *model, uint16_t company_id)
+{
+	SYS_LOG_DBG("company_id 0x%04x", company_id);
+
+	if (company_id != CID_LOCAL) {
+		return -EINVAL;
+	}
+
+	memset(reg_faults, 0, sizeof(reg_faults));
+
+	return 0;
+}
+
+static int fault_test(struct bt_mesh_model *model, uint8_t test_id,
+		      uint16_t company_id)
+{
+	SYS_LOG_DBG("test_id 0x%02x company_id 0x%04x", test_id, company_id);
+
+	if (company_id != CID_LOCAL || test_id != HEALTH_TEST_ID) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct bt_mesh_health_srv_cb health_srv_cb = {
+	.fault_get_cur = fault_get_cur,
+	.fault_get_reg = fault_get_reg,
+	.fault_clear = fault_clear,
+	.fault_test = fault_test,
+};
+
+static struct bt_mesh_health_srv health_srv = {
+	.cb = &health_srv_cb,
+};
+
+static struct bt_mesh_model_pub health_pub;
+
+static void
+health_pub_init(void)
+{
+	health_pub.msg  = BT_MESH_HEALTH_FAULT_MSG(CUR_FAULTS_MAX);
+}
+
+static struct bt_mesh_cfg_cli cfg_cli = {
+};
+
+void show_faults(u8_t test_id, u16_t cid, u8_t *faults, size_t fault_count)
+{
+	size_t i;
+
+	if (!fault_count) {
+		SYS_LOG_DBG("Health Test ID 0x%02x Company ID 0x%04x: "
+			    "no faults", test_id, cid);
+		return;
+	}
+
+	SYS_LOG_DBG("Health Test ID 0x%02x Company ID 0x%04x Fault Count %zu: ",
+		    test_id, cid, fault_count);
+
+	for (i = 0; i < fault_count; i++) {
+		SYS_LOG_DBG("0x%02x", faults[i]);
+	}
+}
+
+static void health_current_status(struct bt_mesh_health_cli *cli, u16_t addr,
+				  u8_t test_id, u16_t cid, u8_t *faults,
+				  size_t fault_count)
+{
+	SYS_LOG_DBG("Health Current Status from 0x%04x", addr);
+	show_faults(test_id, cid, faults, fault_count);
+}
+
+static struct bt_mesh_health_cli health_cli = {
+	.current_status = health_current_status,
+};
+
+static struct bt_mesh_model root_models[] = {
+	BT_MESH_MODEL_CFG_SRV(&cfg_srv),
+	BT_MESH_MODEL_CFG_CLI(&cfg_cli),
+	BT_MESH_MODEL_HEALTH_SRV(&health_srv, &health_pub),
+	BT_MESH_MODEL_HEALTH_CLI(&health_cli),
+};
+
+static struct bt_mesh_model vnd_models[] = {
+	BT_MESH_MODEL_VND(CID_LOCAL, VND_MODEL_ID_1, BT_MESH_MODEL_NO_OPS, NULL,
+			  NULL),
+};
+
+static struct bt_mesh_elem elements[] = {
+	BT_MESH_ELEM(0, root_models, vnd_models),
+};
+
+static void link_open(bt_mesh_prov_bearer_t bearer)
+{
+	struct mesh_prov_link_open_ev ev;
+
+	SYS_LOG_DBG("bearer 0x%02x", bearer);
+
+	switch (bearer) {
+	case BT_MESH_PROV_ADV:
+		ev.bearer = MESH_PROV_BEARER_PB_ADV;
+		break;
+	case BT_MESH_PROV_GATT:
+		ev.bearer = MESH_PROV_BEARER_PB_GATT;
+		break;
+	default:
+		SYS_LOG_ERR("Invalid bearer");
+
+		return;
+	}
+
+	tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROV_LINK_OPEN,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void link_close(bt_mesh_prov_bearer_t bearer)
+{
+	struct mesh_prov_link_closed_ev ev;
+
+	SYS_LOG_DBG("bearer 0x%02x", bearer);
+
+	switch (bearer) {
+	case BT_MESH_PROV_ADV:
+		ev.bearer = MESH_PROV_BEARER_PB_ADV;
+		break;
+	case BT_MESH_PROV_GATT:
+		ev.bearer = MESH_PROV_BEARER_PB_GATT;
+		break;
+	default:
+		SYS_LOG_ERR("Invalid bearer");
+
+		return;
+	}
+
+	tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROV_LINK_CLOSED,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static int output_number(bt_mesh_output_action_t action, u32_t number)
+{
+	struct mesh_out_number_action_ev ev;
+
+	SYS_LOG_DBG("action 0x%04x number 0x%08lx", action, number);
+
+	ev.action = sys_cpu_to_le16(action);
+	ev.number = sys_cpu_to_le32(number);
+
+	tester_send(BTP_SERVICE_ID_MESH, MESH_EV_OUT_NUMBER_ACTION,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+
+	return 0;
+}
+
+static int output_string(const char *str)
+{
+	struct mesh_out_string_action_ev *ev;
+	struct os_mbuf *buf = NET_BUF_SIMPLE(BTP_DATA_MAX_SIZE);
+
+	SYS_LOG_DBG("str %s", str);
+
+	net_buf_simple_init(buf, 0);
+
+	ev = net_buf_simple_add(buf, sizeof(*ev));
+	ev->string_len = strlen(str);
+
+	net_buf_simple_add_mem(buf, str, ev->string_len);
+
+	tester_send_buf(BTP_SERVICE_ID_MESH, MESH_EV_OUT_STRING_ACTION,
+			CONTROLLER_INDEX, buf);
+
+	os_mbuf_free_chain(buf);
+	return 0;
+}
+
+static int input(bt_mesh_input_action_t action, u8_t size)
+{
+	struct mesh_in_action_ev ev;
+
+	SYS_LOG_DBG("action 0x%04x number 0x%02x", action, size);
+
+	input_size = size;
+
+	ev.action = sys_cpu_to_le16(action);
+	ev.size = size;
+
+	tester_send(BTP_SERVICE_ID_MESH, MESH_EV_IN_ACTION, CONTROLLER_INDEX,
+		    (u8_t *) &ev, sizeof(ev));
+
+	return 0;
+}
+
+static void prov_complete(u16_t net_idx, u16_t addr)
+{
+	SYS_LOG_DBG("net_idx 0x%04x addr 0x%04x", net_idx, addr);
+
+	net.net_idx = net_idx,
+	net.local = addr;
+	net.dst = addr;
+
+	tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROVISIONED, CONTROLLER_INDEX,
+		    NULL, 0);
+}
+
+static void prov_reset(void)
+{
+	SYS_LOG_DBG("");
+
+	bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT);
+}
+
+static const struct bt_mesh_comp comp = {
+	.cid = CID_LOCAL,
+	.elem = elements,
+	.elem_count = ARRAY_SIZE(elements),
+};
+
+static struct bt_mesh_prov prov = {
+	.uuid = dev_uuid,
+	.static_val = static_auth,
+	.static_val_len = sizeof(static_auth),
+	.output_number = output_number,
+	.output_string = output_string,
+	.input = input,
+	.link_open = link_open,
+	.link_close = link_close,
+	.complete = prov_complete,
+	.reset = prov_reset,
+};
+
+static void config_prov(u8_t *data, u16_t len)
+{
+	const struct mesh_config_provisioning_cmd *cmd = (void *) data;
+
+	SYS_LOG_DBG("");
+
+	memcpy(dev_uuid, cmd->uuid, sizeof(dev_uuid));
+	memcpy(static_auth, cmd->static_auth, sizeof(static_auth));
+
+	prov.output_size = cmd->out_size;
+	prov.output_actions = sys_le16_to_cpu(cmd->out_actions);
+	prov.input_size = cmd->in_size;
+	prov.input_actions = sys_le16_to_cpu(cmd->in_actions);
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_CONFIG_PROVISIONING,
+		   CONTROLLER_INDEX, BTP_STATUS_SUCCESS);
+}
+
+static void provision_node(u8_t *data, u16_t len)
+{
+	const struct mesh_provision_node_cmd *cmd = (void *) data;
+
+	SYS_LOG_DBG("");
+
+	memcpy(dev_key, cmd->dev_key, sizeof(dev_key));
+	memcpy(net_key, cmd->net_key, sizeof(net_key));
+
+	addr = sys_le16_to_cpu(cmd->addr);
+	flags = cmd->flags;
+	iv_index = sys_le32_to_cpu(cmd->iv_index);
+	net_key_idx = sys_le16_to_cpu(cmd->net_key_idx);
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_PROVISION_NODE,
+		   CONTROLLER_INDEX, BTP_STATUS_SUCCESS);
+}
+
+static void init(u8_t *data, u16_t len)
+{
+	u8_t status = BTP_STATUS_SUCCESS;
+	int err;
+
+	SYS_LOG_DBG("");
+
+	err = bt_mesh_init(own_addr_type, &prov, &comp);
+	if (err) {
+		status = BTP_STATUS_FAILED;
+
+		goto rsp;
+	}
+
+	if (addr) {
+		err = bt_mesh_provision(net_key, net_key_idx, flags, iv_index,
+					addr, dev_key);
+		if (err) {
+			status = BTP_STATUS_FAILED;
+		}
+	} else {
+		err = bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT);
+		if (err) {
+			status = BTP_STATUS_FAILED;
+		}
+	}
+
+	/* Set device key for vendor model */
+	vnd_models[0].keys[0] = BT_MESH_KEY_DEV;
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_INIT, CONTROLLER_INDEX,
+		   status);
+}
+
+static void reset(u8_t *data, u16_t len)
+{
+	SYS_LOG_DBG("");
+
+	bt_mesh_reset();
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_RESET, CONTROLLER_INDEX,
+		   BTP_STATUS_SUCCESS);
+}
+
+static void input_number(u8_t *data, u16_t len)
+{
+	const struct mesh_input_number_cmd *cmd = (void *) data;
+	u8_t status = BTP_STATUS_SUCCESS;
+	u32_t number;
+	int err;
+
+	number = sys_le32_to_cpu(cmd->number);
+
+	SYS_LOG_DBG("number 0x%04lx", number);
+
+	err = bt_mesh_input_number(number);
+	if (err) {
+		status = BTP_STATUS_FAILED;
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_INPUT_NUMBER, CONTROLLER_INDEX,
+		   status);
+}
+
+static void input_string(u8_t *data, u16_t len)
+{
+	const struct mesh_input_string_cmd *cmd = (void *) data;
+	u8_t status = BTP_STATUS_SUCCESS;
+	u8_t str_auth[16];
+	int err;
+
+	SYS_LOG_DBG("");
+
+	if (cmd->string_len > sizeof(str_auth)) {
+		SYS_LOG_ERR("Too long input (%u chars required)", input_size);
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	} else if (cmd->string_len < input_size) {
+		SYS_LOG_ERR("Too short input (%u chars required)", input_size);
+		status = BTP_STATUS_FAILED;
+		goto rsp;
+	}
+
+	strncpy((char *)str_auth, (char *)cmd->string, cmd->string_len);
+
+	err = bt_mesh_input_string((char *)str_auth);
+	if (err) {
+		status = BTP_STATUS_FAILED;
+	}
+
+rsp:
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_INPUT_STRING, CONTROLLER_INDEX,
+		   status);
+}
+
+static void ivu_test_mode(u8_t *data, u16_t len)
+{
+	const struct mesh_ivu_test_mode_cmd *cmd = (void *) data;
+
+	SYS_LOG_DBG("enable 0x%02x", cmd->enable);
+
+	bt_mesh_iv_update_test(cmd->enable ? true : false);
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_IVU_TEST_MODE, CONTROLLER_INDEX,
+		   BTP_STATUS_SUCCESS);
+}
+
+static void ivu_toggle_state(u8_t *data, u16_t len)
+{
+	bool result;
+
+	SYS_LOG_DBG("");
+
+	result = bt_mesh_iv_update();
+	if (!result) {
+		SYS_LOG_ERR("Failed to toggle the IV Update state");
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_IVU_TOGGLE_STATE, CONTROLLER_INDEX,
+		   result ? BTP_STATUS_SUCCESS : BTP_STATUS_FAILED);
+}
+
+static void lpn(u8_t *data, u16_t len)
+{
+	struct mesh_lpn_set_cmd *cmd = (void *) data;
+	bool enable;
+	int err;
+
+	SYS_LOG_DBG("enable 0x%02x", cmd->enable);
+
+	enable = cmd->enable ? true : false;
+	err = bt_mesh_lpn_set(enable);
+	if (err) {
+		SYS_LOG_ERR("Failed to toggle LPN (err %d)", err);
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN, CONTROLLER_INDEX,
+		   err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+}
+
+static void lpn_poll(u8_t *data, u16_t len)
+{
+	int err;
+
+	SYS_LOG_DBG("");
+
+	err = bt_mesh_lpn_poll();
+	if (err) {
+		SYS_LOG_ERR("Failed to send poll msg (err %d)", err);
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_POLL, CONTROLLER_INDEX,
+		   err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+}
+
+static void net_send(u8_t *data, u16_t len)
+{
+	struct mesh_net_send_cmd *cmd = (void *) data;
+	struct os_mbuf *msg = NET_BUF_SIMPLE(UINT8_MAX);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net.net_idx,
+		.app_idx = BT_MESH_KEY_DEV,
+		.addr = sys_le16_to_cpu(cmd->dst),
+		.send_ttl = cmd->ttl,
+	};
+	int err;
+
+	SYS_LOG_DBG("ttl 0x%02x dst 0x%04x payload_len %d", ctx.send_ttl,
+		    ctx.addr, cmd->payload_len);
+
+	net_buf_simple_add_mem(msg, cmd->payload, cmd->payload_len);
+
+	err = bt_mesh_model_send(&vnd_models[0], &ctx, msg, NULL, NULL);
+	if (err) {
+		SYS_LOG_ERR("Failed to send (err %d)", err);
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_NET_SEND, CONTROLLER_INDEX,
+		   err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+
+	os_mbuf_free_chain(msg);
+}
+
+static void health_generate_faults(u8_t *data, u16_t len)
+{
+	struct mesh_health_generate_faults_rp *rp;
+	struct os_mbuf *buf = NET_BUF_SIMPLE(sizeof(*rp) + sizeof(cur_faults) +
+			      sizeof(reg_faults));
+	u8_t some_faults[] = { 0x01, 0x02, 0x03, 0xff, 0x06 };
+	u8_t cur_faults_count, reg_faults_count;
+
+	rp = net_buf_simple_add(buf, sizeof(*rp));
+
+	cur_faults_count = min(sizeof(cur_faults), sizeof(some_faults));
+	memcpy(cur_faults, some_faults, cur_faults_count);
+	net_buf_simple_add_mem(buf, cur_faults, cur_faults_count);
+	rp->cur_faults_count = cur_faults_count;
+
+	reg_faults_count = min(sizeof(reg_faults), sizeof(some_faults));
+	memcpy(reg_faults, some_faults, reg_faults_count);
+	net_buf_simple_add_mem(buf, reg_faults, reg_faults_count);
+	rp->reg_faults_count = reg_faults_count;
+
+	bt_mesh_fault_update(&elements[0]);
+
+	tester_send_buf(BTP_SERVICE_ID_MESH, MESH_HEALTH_GENERATE_FAULTS,
+			CONTROLLER_INDEX, buf);
+}
+
+static void health_clear_faults(u8_t *data, u16_t len)
+{
+	SYS_LOG_DBG("");
+
+	memset(cur_faults, 0, sizeof(cur_faults));
+	memset(reg_faults, 0, sizeof(reg_faults));
+
+	bt_mesh_fault_update(&elements[0]);
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_HEALTH_CLEAR_FAULTS,
+		   CONTROLLER_INDEX, BTP_STATUS_SUCCESS);
+}
+
+static void model_send(u8_t *data, u16_t len)
+{
+	struct mesh_model_send_cmd *cmd = (void *) data;
+	struct os_mbuf *msg = NET_BUF_SIMPLE(UINT8_MAX);
+	struct bt_mesh_msg_ctx ctx = {
+		.net_idx = net.net_idx,
+		.app_idx = BT_MESH_KEY_DEV,
+		.addr = sys_le16_to_cpu(cmd->dst),
+		.send_ttl = BT_MESH_TTL_DEFAULT,
+	};
+	struct bt_mesh_model *model = NULL;
+	int err, i;
+	u16_t src = sys_le16_to_cpu(cmd->src);
+
+	/* Lookup source address */
+	for (i = 0; i < ARRAY_SIZE(model_bound); i++) {
+		if (bt_mesh_model_elem(model_bound[i].model)->addr == src) {
+			model = model_bound[i].model;
+			ctx.app_idx = model_bound[i].appkey_idx;
+
+			break;
+		}
+	}
+
+	if (!model) {
+		SYS_LOG_ERR("Model not found");
+		err = -EINVAL;
+
+		goto fail;
+	}
+
+	SYS_LOG_DBG("src 0x%04x dst 0x%04x model %p payload_len %d", src,
+		    ctx.addr, model, cmd->payload_len);
+
+	net_buf_simple_add_mem(msg, cmd->payload, cmd->payload_len);
+
+	err = bt_mesh_model_send(model, &ctx, msg, NULL, NULL);
+	if (err) {
+		SYS_LOG_ERR("Failed to send (err %d)", err);
+	}
+
+fail:
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_MODEL_SEND, CONTROLLER_INDEX,
+		   err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+
+	os_mbuf_free_chain(msg);
+}
+
+#if MYNEWT_VAL(BLE_MESH_TESTING)
+static void lpn_subscribe(u8_t *data, u16_t len)
+{
+	struct mesh_lpn_subscribe_cmd *cmd = (void *) data;
+	u16_t address = sys_le16_to_cpu(cmd->address);
+	int err;
+
+	SYS_LOG_DBG("address 0x%04x", address);
+
+	err = bt_test_mesh_lpn_group_add(address);
+	if (err) {
+		SYS_LOG_ERR("Failed to subscribe (err %d)", err);
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_SUBSCRIBE, CONTROLLER_INDEX,
+		   err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+}
+
+static void lpn_unsubscribe(u8_t *data, u16_t len)
+{
+	struct mesh_lpn_unsubscribe_cmd *cmd = (void *) data;
+	u16_t address = sys_le16_to_cpu(cmd->address);
+	int err;
+
+	SYS_LOG_DBG("address 0x%04x", address);
+
+	err = bt_test_mesh_lpn_group_remove(&address, 1);
+	if (err) {
+		SYS_LOG_ERR("Failed to unsubscribe (err %d)", err);
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_UNSUBSCRIBE, CONTROLLER_INDEX,
+		   err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+}
+
+static void rpl_clear(u8_t *data, u16_t len)
+{
+	int err;
+
+	SYS_LOG_DBG("");
+
+	err = bt_test_mesh_rpl_clear();
+	if (err) {
+		SYS_LOG_ERR("Failed to clear RPL (err %d)", err);
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_RPL_CLEAR, CONTROLLER_INDEX,
+		   err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+}
+#endif /* MYNEWT_VAL(BLE_MESH_TESTING) */
+
+static void proxy_identity_enable(u8_t *data, u16_t len)
+{
+	int err;
+
+	SYS_LOG_DBG("");
+
+	err = bt_mesh_proxy_identity_enable();
+	if (err) {
+		SYS_LOG_ERR("Failed to enable proxy identity (err %d)", err);
+	}
+
+	tester_rsp(BTP_SERVICE_ID_MESH, MESH_PROXY_IDENTITY, CONTROLLER_INDEX,
+		   err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS);
+}
+
+void tester_handle_mesh(u8_t opcode, u8_t index, u8_t *data, u16_t len)
+{
+	switch (opcode) {
+	case MESH_READ_SUPPORTED_COMMANDS:
+		supported_commands(data, len);
+		break;
+	case MESH_CONFIG_PROVISIONING:
+		config_prov(data, len);
+		break;
+	case MESH_PROVISION_NODE:
+		provision_node(data, len);
+		break;
+	case MESH_INIT:
+		init(data, len);
+		break;
+	case MESH_RESET:
+		reset(data, len);
+		break;
+	case MESH_INPUT_NUMBER:
+		input_number(data, len);
+		break;
+	case MESH_INPUT_STRING:
+		input_string(data, len);
+		break;
+	case MESH_IVU_TEST_MODE:
+		ivu_test_mode(data, len);
+		break;
+	case MESH_IVU_TOGGLE_STATE:
+		ivu_toggle_state(data, len);
+		break;
+	case MESH_LPN:
+		lpn(data, len);
+		break;
+	case MESH_LPN_POLL:
+		lpn_poll(data, len);
+		break;
+	case MESH_NET_SEND:
+		net_send(data, len);
+		break;
+	case MESH_HEALTH_GENERATE_FAULTS:
+		health_generate_faults(data, len);
+		break;
+	case MESH_HEALTH_CLEAR_FAULTS:
+		health_clear_faults(data, len);
+		break;
+	case MESH_MODEL_SEND:
+		model_send(data, len);
+		break;
+#if MYNEWT_VAL(BLE_MESH_TESTING)
+	case MESH_LPN_SUBSCRIBE:
+		lpn_subscribe(data, len);
+		break;
+	case MESH_LPN_UNSUBSCRIBE:
+		lpn_unsubscribe(data, len);
+		break;
+	case MESH_RPL_CLEAR:
+		rpl_clear(data, len);
+		break;
+#endif /* MYNEWT_VAL(BLE_MESH_TESTING) */
+	case MESH_PROXY_IDENTITY:
+		proxy_identity_enable(data, len);
+		break;
+	default:
+		tester_rsp(BTP_SERVICE_ID_MESH, opcode, index,
+			   BTP_STATUS_UNKNOWN_CMD);
+		break;
+	}
+}
+
+void net_recv_ev(u8_t ttl, u8_t ctl, u16_t src, u16_t dst, const void *payload,
+		 size_t payload_len)
+{
+	struct os_mbuf *buf = NET_BUF_SIMPLE(UINT8_MAX);
+	struct mesh_net_recv_ev *ev;
+
+	SYS_LOG_DBG("ttl 0x%02x ctl 0x%02x src 0x%04x dst 0x%04x "
+		    "payload_len %d", ttl, ctl, src, dst, payload_len);
+
+	if (payload_len > net_buf_simple_tailroom(buf)) {
+		SYS_LOG_ERR("Payload size exceeds buffer size");
+
+		goto done;
+	}
+
+	ev = net_buf_simple_add(buf, sizeof(*ev));
+	ev->ttl = ttl;
+	ev->ctl = ctl;
+	ev->src = sys_cpu_to_le16(src);
+	ev->dst = sys_cpu_to_le16(dst);
+	ev->payload_len = payload_len;
+	net_buf_simple_add_mem(buf, payload, payload_len);
+
+	tester_send_buf(BTP_SERVICE_ID_MESH, MESH_EV_NET_RECV, CONTROLLER_INDEX,
+			buf);
+done:
+	os_mbuf_free_chain(buf);
+}
+
+static void model_bound_cb(u16_t addr, struct bt_mesh_model *model,
+			   u16_t key_idx)
+{
+	int i;
+
+	SYS_LOG_DBG("remote addr 0x%04x key_idx 0x%04x model %p",
+		    addr, key_idx, model);
+
+	for (i = 0; i < ARRAY_SIZE(model_bound); i++) {
+		if (!model_bound[i].model) {
+			model_bound[i].model = model;
+			model_bound[i].addr = addr;
+			model_bound[i].appkey_idx = key_idx;
+
+			return;
+		}
+	}
+
+	SYS_LOG_ERR("model_bound is full");
+}
+
+static void model_unbound_cb(u16_t addr, struct bt_mesh_model *model,
+			     u16_t key_idx)
+{
+	int i;
+
+	SYS_LOG_DBG("remote addr 0x%04x key_idx 0x%04x model %p",
+		    addr, key_idx, model);
+
+	for (i = 0; i < ARRAY_SIZE(model_bound); i++) {
+		if (model_bound[i].model == model) {
+			model_bound[i].model = NULL;
+			model_bound[i].addr = 0x0000;
+			model_bound[i].appkey_idx = BT_MESH_KEY_UNUSED;
+
+			return;
+		}
+	}
+
+	SYS_LOG_INF("model not found");
+}
+
+static void invalid_bearer_cb(u8_t opcode)
+{
+	struct mesh_invalid_bearer_ev ev = {
+		.opcode = opcode,
+	};
+
+	SYS_LOG_DBG("opcode 0x%02x", opcode);
+
+	tester_send(BTP_SERVICE_ID_MESH, MESH_EV_INVALID_BEARER,
+		    CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev));
+}
+
+static void incomp_timer_exp_cb(void)
+{
+	tester_send(BTP_SERVICE_ID_MESH, MESH_EV_INCOMP_TIMER_EXP,
+		    CONTROLLER_INDEX, NULL, 0);
+}
+
+static struct bt_test_cb bt_test_cb = {
+	.mesh_net_recv = net_recv_ev,
+	.mesh_model_bound = model_bound_cb,
+	.mesh_model_unbound = model_unbound_cb,
+	.mesh_prov_invalid_bearer = invalid_bearer_cb,
+	.mesh_trans_incomp_timer_exp = incomp_timer_exp_cb,
+};
+
+u8_t tester_init_mesh(void)
+{
+	health_pub_init();
+
+	if (IS_ENABLED(CONFIG_BT_TESTING)) {
+		bt_test_cb_register(&bt_test_cb);
+	}
+
+	return BTP_STATUS_SUCCESS;
+}
+
+u8_t tester_unregister_mesh(void)
+{
+	return BTP_STATUS_SUCCESS;
+}
+
+#endif /* MYNEWT_VAL(BLE_MESH) */
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/rtt_pipe.c b/src/libs/mynewt-nimble/apps/bttester/src/rtt_pipe.c
new file mode 100644
index 00000000..379345a0
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/rtt_pipe.c
@@ -0,0 +1,136 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "syscfg/syscfg.h"
+
+#if MYNEWT_VAL(BTTESTER_PIPE_RTT)
+
+#include "os/mynewt.h"
+#include "console/console.h"
+#include "rtt/SEGGER_RTT.h"
+
+#include "bttester_pipe.h"
+
+static struct hal_timer rtt_timer;
+
+static bttester_pipe_recv_cb app_cb;
+
+static u8_t *recv_buf;
+static size_t recv_buf_len;
+static size_t recv_off;
+
+static uint8_t rtt_buf_up[MYNEWT_VAL(BTTESTER_RTT_BUFFER_SIZE_UP)];
+static uint8_t rtt_buf_down[MYNEWT_VAL(BTTESTER_RTT_BUFFER_SIZE_DOWN)];
+static int rtt_index_up, rtt_index_down;
+
+#define RTT_INPUT_POLL_INTERVAL_MIN     10 /* ms */
+#define RTT_INPUT_POLL_INTERVAL_STEP    10 /* ms */
+#define RTT_INPUT_POLL_INTERVAL_MAX     250 /* ms */
+
+static int rtt_pipe_get_char(unsigned int index)
+{
+    char c;
+    int r;
+
+    r = (int)SEGGER_RTT_Read(index, &c, 1u);
+    if (r == 1) {
+        r = (int)(unsigned char)c;
+    } else {
+        r = -1;
+    }
+
+    return r;
+}
+
+static void
+rtt_pipe_poll_func(void *arg)
+{
+    static uint32_t itvl_ms = RTT_INPUT_POLL_INTERVAL_MIN;
+    static int key = -1;
+    int avail = recv_buf_len - recv_off;
+
+    if (key < 0) {
+        key = rtt_pipe_get_char((unsigned int) rtt_index_down);
+    }
+
+    if (key < 0) {
+        itvl_ms += RTT_INPUT_POLL_INTERVAL_STEP;
+        itvl_ms = min(itvl_ms, RTT_INPUT_POLL_INTERVAL_MAX);
+    } else {
+        while (key >= 0 && avail > 0) {
+            recv_buf[recv_off] = (u8_t) key;
+            recv_off++;
+            avail = recv_buf_len - recv_off;
+            key = rtt_pipe_get_char((unsigned int) rtt_index_down);
+        }
+
+        /*
+         * Call application callback with received data. Application
+         * may provide new buffer or alter data offset.
+         */
+        recv_buf = app_cb(recv_buf, &recv_off);
+
+        itvl_ms = RTT_INPUT_POLL_INTERVAL_MIN;
+    }
+
+    os_cputime_timer_relative(&rtt_timer, itvl_ms * 1000);
+}
+
+int
+bttester_pipe_send(const u8_t *data, int len)
+{
+    SEGGER_RTT_Write((unsigned int) rtt_index_up, data, (unsigned int) len);
+    return 0;
+}
+
+void
+bttester_pipe_register(u8_t *buf, size_t len, bttester_pipe_recv_cb cb)
+{
+    recv_buf = buf;
+    recv_buf_len = len;
+    app_cb = cb;
+}
+
+int
+bttester_pipe_init(void)
+{
+    rtt_index_up = SEGGER_RTT_AllocUpBuffer(MYNEWT_VAL(BTTESTER_RTT_BUFFER_NAME),
+                                            rtt_buf_up, sizeof(rtt_buf_up),
+                                            SEGGER_RTT_MODE_BLOCK_IF_FIFO_FULL);
+
+    if (rtt_index_up < 0) {
+        return -1;
+    }
+
+    rtt_index_down = SEGGER_RTT_AllocDownBuffer(MYNEWT_VAL(BTTESTER_RTT_BUFFER_NAME),
+                                                rtt_buf_down, sizeof(rtt_buf_down),
+                                                SEGGER_RTT_MODE_BLOCK_IF_FIFO_FULL);
+
+    if (rtt_index_down < 0) {
+        return -1;
+    }
+
+    console_printf("Using up-buffer #%d\n", rtt_index_up);
+    console_printf("Using down-buffer #%d\n", rtt_index_down);
+
+    os_cputime_timer_init(&rtt_timer, rtt_pipe_poll_func, NULL);
+    os_cputime_timer_relative(&rtt_timer, 200000);
+    return 0;
+}
+#endif /* MYNEWT_VAL(BTTESTER_PIPE_RTT) */
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/uart_pipe.c b/src/libs/mynewt-nimble/apps/bttester/src/uart_pipe.c
new file mode 100644
index 00000000..ecbefa02
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/src/uart_pipe.c
@@ -0,0 +1,281 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "syscfg/syscfg.h"
+
+#if MYNEWT_VAL(BTTESTER_PIPE_UART)
+
+#include "os/mynewt.h"
+#include "uart/uart.h"
+
+#include "bttester_pipe.h"
+
+static u8_t *recv_buf;
+static size_t recv_buf_len;
+static bttester_pipe_recv_cb app_cb;
+static size_t recv_off;
+
+struct uart_pipe_ring {
+    uint8_t head;
+    uint8_t tail;
+    uint16_t size;
+    uint8_t *buf;
+};
+
+static struct uart_dev *uart_dev;
+static struct uart_pipe_ring cr_tx;
+static uint8_t cr_tx_buf[MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX)];
+typedef void (*console_write_char)(struct uart_dev*, uint8_t);
+static console_write_char write_char_cb;
+
+static struct uart_pipe_ring cr_rx;
+static uint8_t cr_rx_buf[MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX)];
+static volatile bool uart_console_rx_stalled;
+
+struct os_event rx_ev;
+
+static inline int
+inc_and_wrap(int i, int max)
+{
+    return (i + 1) & (max - 1);
+}
+
+static void
+uart_pipe_ring_add_char(struct uart_pipe_ring *cr, char ch)
+{
+    cr->buf[cr->head] = ch;
+    cr->head = inc_and_wrap(cr->head, cr->size);
+}
+
+static uint8_t
+uart_pipe_ring_pull_char(struct uart_pipe_ring *cr)
+{
+    uint8_t ch;
+
+    ch = cr->buf[cr->tail];
+    cr->tail = inc_and_wrap(cr->tail, cr->size);
+    return ch;
+}
+
+static bool
+uart_pipe_ring_is_full(const struct uart_pipe_ring *cr)
+{
+    return inc_and_wrap(cr->head, cr->size) == cr->tail;
+}
+
+static bool
+uart_pipe_ring_is_empty(const struct uart_pipe_ring *cr)
+{
+    return cr->head == cr->tail;
+}
+
+static void
+uart_pipe_queue_char(struct uart_dev *uart_dev, uint8_t ch)
+{
+    int sr;
+
+    if ((uart_dev->ud_dev.od_flags & OS_DEV_F_STATUS_OPEN) == 0) {
+        return;
+    }
+
+    OS_ENTER_CRITICAL(sr);
+    while (uart_pipe_ring_is_full(&cr_tx)) {
+        /* TX needs to drain */
+        uart_start_tx(uart_dev);
+        OS_EXIT_CRITICAL(sr);
+        if (os_started()) {
+            os_time_delay(1);
+        }
+        OS_ENTER_CRITICAL(sr);
+    }
+    uart_pipe_ring_add_char(&cr_tx, ch);
+    OS_EXIT_CRITICAL(sr);
+}
+
+/*
+ * Interrupts disabled when console_tx_char/console_rx_char are called.
+ * Characters sent only in blocking mode.
+ */
+static int
+uart_console_tx_char(void *arg)
+{
+    if (uart_pipe_ring_is_empty(&cr_tx)) {
+        return -1;
+    }
+    return uart_pipe_ring_pull_char(&cr_tx);
+}
+
+/*
+ * Interrupts disabled when console_tx_char/console_rx_char are called.
+ */
+static int
+uart_console_rx_char(void *arg, uint8_t byte)
+{
+    if (uart_pipe_ring_is_full(&cr_rx)) {
+        uart_console_rx_stalled = true;
+        return -1;
+    }
+
+    uart_pipe_ring_add_char(&cr_rx, byte);
+
+    if (!rx_ev.ev_queued) {
+        os_eventq_put(os_eventq_dflt_get(), &rx_ev);
+    }
+
+    return 0;
+}
+
+static int
+uart_pipe_handle_char(int key)
+{
+    recv_buf[recv_off] = (u8_t) key;
+    recv_off++;
+
+    return 0;
+}
+
+static void
+uart_console_rx_char_event(struct os_event *ev)
+{
+    static int b = -1;
+    int sr;
+    int ret;
+
+    /* We may have unhandled character - try it first */
+    if (b >= 0) {
+        ret = uart_pipe_handle_char(b);
+        if (ret < 0) {
+            return;
+        }
+    }
+
+    while (!uart_pipe_ring_is_empty(&cr_rx)) {
+        OS_ENTER_CRITICAL(sr);
+        b = uart_pipe_ring_pull_char(&cr_rx);
+        OS_EXIT_CRITICAL(sr);
+
+        /* If UART RX was stalled due to a full receive buffer, restart RX now
+         * that we have removed a byte from the buffer.
+         */
+        if (uart_console_rx_stalled) {
+            uart_console_rx_stalled = false;
+            uart_start_rx(uart_dev);
+        }
+
+        ret = uart_pipe_handle_char(b);
+        if (ret < 0) {
+            return;
+        }
+    }
+
+    /*
+     * Call application callback with received data. Application
+     * may provide new buffer or alter data offset.
+     */
+    recv_buf = app_cb(recv_buf, &recv_off);
+
+    b = -1;
+}
+
+int
+bttester_pipe_send(const u8_t *data, int len)
+{
+    int i;
+
+    /* Assure that there is a write cb installed; this enables to debug
+     * code that is faulting before the console was initialized.
+     */
+    if (!write_char_cb) {
+        return -1;
+    }
+
+    for (i = 0; i < len; ++i) {
+        write_char_cb(uart_dev, data[i]);
+    }
+
+    uart_start_tx(uart_dev);
+
+    return 0;
+}
+
+int
+bttester_pipe_send_buf(struct os_mbuf *buf)
+{
+	int i, len;
+	struct os_mbuf *om;
+
+	/* Assure that there is a write cb installed; this enables to debug
+	 * code that is faulting before the console was initialized.
+	 */
+	if (!write_char_cb) {
+		return -1;
+	}
+
+	for (om = buf; om; om = SLIST_NEXT(om, om_next)) {
+		len = om->om_len;
+		for (i = 0; i < len; ++i) {
+			write_char_cb(uart_dev, om->om_data[i]);
+		}
+	}
+
+	uart_start_tx(uart_dev);
+
+	return 0;
+}
+
+int
+bttester_pipe_init(void)
+{
+    struct uart_conf uc = {
+            .uc_speed = MYNEWT_VAL(CONSOLE_UART_BAUD),
+            .uc_databits = 8,
+            .uc_stopbits = 1,
+            .uc_parity = UART_PARITY_NONE,
+            .uc_flow_ctl = MYNEWT_VAL(CONSOLE_UART_FLOW_CONTROL),
+            .uc_tx_char = uart_console_tx_char,
+            .uc_rx_char = uart_console_rx_char,
+    };
+
+    cr_tx.size = sizeof(cr_tx_buf);
+    cr_tx.buf = cr_tx_buf;
+    write_char_cb = uart_pipe_queue_char;
+
+    cr_rx.size = sizeof(cr_rx_buf);
+    cr_rx.buf = cr_rx_buf;
+
+    rx_ev.ev_cb = uart_console_rx_char_event;
+
+    if (!uart_dev) {
+        uart_dev = (struct uart_dev *)os_dev_open(MYNEWT_VAL(CONSOLE_UART_DEV),
+                                                  OS_TIMEOUT_NEVER, &uc);
+        if (!uart_dev) {
+            return -1;
+        }
+    }
+    return 0;
+}
+
+void
+bttester_pipe_register(u8_t *buf, size_t len, bttester_pipe_recv_cb cb)
+{
+	recv_buf = buf;
+	recv_buf_len = len;
+	app_cb = cb;
+}
+#endif /* MYNEWT_VAL(BTTESTER_PIPE_UART) */
diff --git a/src/libs/mynewt-nimble/apps/bttester/syscfg.yml b/src/libs/mynewt-nimble/apps/bttester/syscfg.yml
new file mode 100644
index 00000000..d0fffe13
--- /dev/null
+++ b/src/libs/mynewt-nimble/apps/bttester/syscfg.yml
@@ -0,0 +1,122 @@
+# Licensed to the Apache Software Foundation (ASF) under one
+# or more contributor license agreements.  See the NOTICE file
+# distributed with this work for additional information
+# regarding copyright ownership.  The ASF licenses this file
+# to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance
+# with the License.  You may obtain a copy of the License at
+#
+#  http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing,
+# software distributed under the License is distributed on an
+# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+# KIND, either express or implied.  See the License for the
+# specific language governing permissions and limitations
+# under the License.
+#
+
+# Package: apps/blemesh
+
+syscfg.defs:
+    BTTESTER_PIPE_UART:
+        description: 'Set communication pipe to UART'
+        value: 1
+
+    BTTESTER_PIPE_RTT:
+        description: 'Set communication pipe to RTT'
+        value: 0
+
+    BTTESTER_RTT_BUFFER_NAME:
+        description: Bttester rtt pipe buffer name
+        value: '"bttester"'
+
+    BTTESTER_RTT_BUFFER_SIZE_UP:
+        description: Bttester upstream buffer size
+        value: MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX)
+
+    BTTESTER_RTT_BUFFER_SIZE_DOWN:
+        description: Bttester downstream buffer size
+        value: MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX)
+
+    BTTESTER_PRIVACY_MODE:
+        description: Enable privacy mode (RPA or NRPA)
+        value: 0
+
+    BTTESTER_USE_NRPA:
+        description: Use Non Resolvable Private Address
+        value: 0
+
+    BTTESTER_LTD_ADV_TIMEOUT:
+        description: Limited advertising timeout
+        value: 30000
+
+    BTTESTER_CONN_RETRY:
+        description: Retry connections when connection failed to be established
+        value: 3
+
+    BTTESTER_BTP_DATA_SIZE_MAX:
+        description: Maximum BTP payload
+        value: 2048
+
+    BTTESTER_CONN_PARAM_UPDATE:
+        description: Trigger conn param update after connection establish
+        value: 0
+
+    BTTESTER_DEBUG:
+        description: Enable debug logging
+        value: 0
+
+    BTTESTER_BTP_LOG:
+        description: Enable logging BTP traffic
+        value: 0
+
+syscfg.vals:
+    OS_MAIN_STACK_SIZE: 512
+    SHELL_TASK: 0
+    SHELL_NEWTMGR: 0
+    LOG_LEVEL: 12
+    MSYS_1_BLOCK_COUNT: 48
+
+    BLE_MONITOR_RTT: 1
+    CONSOLE_RTT: 0
+    CONSOLE_UART: 0
+    RTT_NUM_BUFFERS_UP: 0
+    RTT_NUM_BUFFERS_DOWN: 0
+
+    BLE_L2CAP_COC_MAX_NUM: 2
+    BLE_L2CAP_SIG_MAX_PROCS: 2
+    # Some testcases require MPS < MTU
+    BLE_L2CAP_COC_MPS: 100
+    BLE_RPA_TIMEOUT: 30
+    BLE_SM_BONDING: 1
+    BLE_SM_MITM: 0
+    BLE_SM_SC: 1
+    BLE_SM_OUR_KEY_DIST: 7
+    BLE_SM_THEIR_KEY_DIST: 7
+    BLE_SVC_GAP_CENTRAL_ADDRESS_RESOLUTION: 1
+    BLE_SVC_GAP_PPCP_MIN_CONN_INTERVAL: 9
+    BLE_SVC_GAP_PPCP_MAX_CONN_INTERVAL: 30
+    BLE_SVC_GAP_PPCP_SUPERVISION_TMO: 2000
+
+    BLE_MESH: 1
+    BLE_MESH_SHELL: 0
+    BLE_MESH_PROV: 1
+    BLE_MESH_RELAY: 1
+    BLE_MESH_PB_ADV: 1
+    BLE_MESH_PB_GATT: 1
+    BLE_MESH_LOW_POWER: 1
+    BLE_MESH_LPN_AUTO: 0
+    BLE_MESH_GATT_PROXY: 1
+    BLE_MESH_LABEL_COUNT: 2
+    BLE_MESH_SUBNET_COUNT: 2
+    BLE_MESH_MODEL_GROUP_COUNT: 2
+    BLE_MESH_APP_KEY_COUNT: 4
+    BLE_MESH_IV_UPDATE_TEST: 1
+    BLE_MESH_TESTING: 1
+    BLE_MESH_FRIEND: 1
+    BLE_MESH_CFG_CLI: 1
+    BLE_MESH_RX_SDU_MAX: 110
+
+    BLE_MESH_ADV_BUF_COUNT: 20
+    BLE_MESH_TX_SEG_MAX: 6