Merge branch 'develop' - in preparation for

backwards-compatibility-breaking changes to develop.

* develop: (290 commits)
  sim compiler - replace objsize with size
  Fix warnings reported by clang.
  MYNEWT-329
  MYNEWT-354
  STM32f407 discovery board BSP
  mbedtls; use smaller version of SHA256.
  boot; boot loader does not need to call os_init() anymore, as bsp_init() has been exported.
  boot; app does not need the dependency to mbedtls
  slinky; time-based waits must use OS_TICKS_PER_SEC.
  bootutil; adjust unit tests to work with status upkeep outside sys/config.
  bootutil; was returning wrong image header in response when swithing images. Add boot_set_req() routine for unit test use.
  boot/bootutil; remove debug console use from bootloader.
  bootutil/imgmgr; output of boot now shows the fallback image.
  imgmgr; automatically confirm image as good for now.
  bootutil; add 'confirm' step, telling that image was confirmed as good. Otherwise next restart we'll go back to old image.
  bootutil; make status element size depend on flash alignment restrictions.
  boot, imgmgr; return the slot number for test image.
  bootutil; move routines reading boot-copy-status from loader.c to bootutil_misc.c.
  boot; return full flash location of status bytes, instead of just offset.
  boot; don't use NFFS or FCB for keeping status. Interim commit.
  ...

283 files changed, 25726 insertions, 9731 deletions

diff --git a/CODING_STANDARDS.md b/CODING_STANDARDS.md
index 4727f1e1..fc8523da 100644
--- a/CODING_STANDARDS.md
+++ b/CODING_STANDARDS.md
@@ -254,6 +254,8 @@ applications to use pointers to those structures opaquely.
 hide or alias the underlying type used (e.g. ```os_time_t```.)   Indicate
 typedefs by applying the ```_t``` marker to them.
 
+* Place all function-local variable definitions at the top of the function body, before any statements.
+
 ## Compiler Directives
 
 * Code must compile cleanly with -Wall enabled.
diff --git a/apps/blecent/pkg.yml b/apps/blecent/pkg.yml
new file mode 100644
index 00000000..b413fad1
--- /dev/null
+++ b/apps/blecent/pkg.yml
@@ -0,0 +1,39 @@
+# 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/blecent
+pkg.type: app
+pkg.description: Simple BLE central application.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+
+pkg.deps: 
+    - libs/os 
+    - sys/log
+    - net/nimble/controller
+    - net/nimble/host
+    - net/nimble/host/services/gap
+    - net/nimble/host/services/gatt
+    - net/nimble/host/store/ram
+    - net/nimble/transport/ram
+    - libs/console/full
+    - libs/baselibc
+
+pkg.cflags:
+    # DEBUG logging is a bit noisy; use INFO.
+    - "-DLOG_LEVEL=1"
diff --git a/apps/blecent/src/blecent.h b/apps/blecent/src/blecent.h
new file mode 100644
index 00000000..4731eb94
--- /dev/null
+++ b/apps/blecent/src/blecent.h
@@ -0,0 +1,117 @@
+/**
+ * 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 H_BLECENT_
+#define H_BLECENT_
+
+#include "os/queue.h"
+#include "log/log.h"
+struct ble_hs_adv_fields;
+struct ble_gap_conn_desc;
+struct ble_hs_cfg;
+union ble_store_value;
+union ble_store_key;
+
+extern struct log blecent_log;
+
+/* blecent uses the first "peruser" log module. */
+#define BLECENT_LOG_MODULE  (LOG_MODULE_PERUSER + 0)
+
+/* Convenience macro for logging to the blecent module. */
+#define BLECENT_LOG(lvl, ...) \
+    LOG_ ## lvl(&blecent_log, BLECENT_LOG_MODULE, __VA_ARGS__)
+
+#define BLECENT_SVC_ALERT_UUID              0x1811
+#define BLECENT_CHR_SUP_NEW_ALERT_CAT_UUID  0x2A47
+#define BLECENT_CHR_NEW_ALERT               0x2A46
+#define BLECENT_CHR_SUP_UNR_ALERT_CAT_UUID  0x2A48
+#define BLECENT_CHR_UNR_ALERT_STAT_UUID     0x2A45
+#define BLECENT_CHR_ALERT_NOT_CTRL_PT       0x2A44
+
+/** GATT server. */
+void gatt_svr_register(void);
+void gatt_svr_init_cfg(struct ble_hs_cfg *cfg);
+
+
+/** Misc. */
+void print_bytes(const uint8_t *bytes, int len);
+void print_mbuf(const struct os_mbuf *om);
+char *addr_str(const void *addr);
+void print_uuid(const void *uuid128);
+void print_conn_desc(const struct ble_gap_conn_desc *desc);
+void print_adv_fields(const struct ble_hs_adv_fields *fields);
+
+/** Peer. */
+struct peer_dsc {
+    SLIST_ENTRY(peer_dsc) next;
+    struct ble_gatt_dsc dsc;
+};
+SLIST_HEAD(peer_dsc_list, peer_dsc);
+
+struct peer_chr {
+    SLIST_ENTRY(peer_chr) next;
+    struct ble_gatt_chr chr;
+
+    struct peer_dsc_list dscs;
+};
+SLIST_HEAD(peer_chr_list, peer_chr);
+
+struct peer_svc {
+    SLIST_ENTRY(peer_svc) next;
+    struct ble_gatt_svc svc;
+
+    struct peer_chr_list chrs;
+};
+SLIST_HEAD(peer_svc_list, peer_svc);
+
+struct peer;
+typedef void peer_disc_fn(const struct peer *peer, int status, void *arg);
+
+struct peer {
+    SLIST_ENTRY(peer) next;
+
+    uint16_t conn_handle;
+
+    /** List of discovered GATT services. */
+    struct peer_svc_list svcs;
+
+    /** Keeps track of where we are in the service discovery process. */
+    uint16_t disc_prev_chr_val;
+    struct peer_svc *cur_svc;
+
+    /** Callback that gets executed when service discovery completes. */
+    peer_disc_fn *disc_cb;
+    void *disc_cb_arg;
+};
+
+int peer_disc_all(uint16_t conn_handle, peer_disc_fn *disc_cb,
+                  void *disc_cb_arg);
+const struct peer_dsc *
+peer_dsc_find_uuid(const struct peer *peer, const uint8_t *svc_uuid128,
+                   const uint8_t *chr_uuid128, const uint8_t *dsc_uuid128);
+const struct peer_chr *
+peer_chr_find_uuid(const struct peer *peer, const uint8_t *svc_uuid128,
+                   const uint8_t *chr_uuid128);
+const struct peer_svc *
+peer_svc_find_uuid(const struct peer *peer, const uint8_t *uuid128);
+int peer_delete(uint16_t conn_handle);
+int peer_add(uint16_t conn_handle);
+int peer_init(int max_peers, int max_svcs, int max_chrs, int max_dscs);
+
+#endif
diff --git a/apps/blecent/src/main.c b/apps/blecent/src/main.c
new file mode 100755
index 00000000..7056cc68
--- /dev/null
+++ b/apps/blecent/src/main.c
@@ -0,0 +1,620 @@
+/**
+ * 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 <assert.h>
+#include <string.h>
+#include "bsp/bsp.h"
+#include "os/os.h"
+#include "hal/hal_cputime.h"
+#include "console/console.h"
+
+/* BLE */
+#include "nimble/ble.h"
+#include "controller/ble_ll.h"
+#include "host/ble_hs.h"
+
+/* RAM HCI transport. */
+#include "transport/ram/ble_hci_ram.h"
+
+/* RAM persistence layer. */
+#include "store/ram/ble_store_ram.h"
+
+/* Mandatory services. */
+#include "services/gap/ble_svc_gap.h"
+#include "services/gatt/ble_svc_gatt.h"
+
+/* Application-specified header. */
+#include "blecent.h"
+
+#define BSWAP16(x)  ((uint16_t)(((x) << 8) | (((x) & 0xff00) >> 8)))
+
+/** Mbuf settings. */
+#define MBUF_NUM_MBUFS      (12)
+#define MBUF_BUF_SIZE       OS_ALIGN(BLE_MBUF_PAYLOAD_SIZE, 4)
+#define MBUF_MEMBLOCK_SIZE  (MBUF_BUF_SIZE + BLE_MBUF_MEMBLOCK_OVERHEAD)
+#define MBUF_MEMPOOL_SIZE   OS_MEMPOOL_SIZE(MBUF_NUM_MBUFS, MBUF_MEMBLOCK_SIZE)
+
+static os_membuf_t blecent_mbuf_mpool_data[MBUF_MEMPOOL_SIZE];
+struct os_mbuf_pool blecent_mbuf_pool;
+struct os_mempool blecent_mbuf_mpool;
+
+/** Log data. */
+static struct log_handler blecent_log_console_handler;
+struct log blecent_log;
+
+/** Priority of the nimble host and controller tasks. */
+#define BLE_LL_TASK_PRI             (OS_TASK_PRI_HIGHEST)
+
+/** blecent task settings. */
+#define BLECENT_TASK_PRIO           1
+#define BLECENT_STACK_SIZE          (OS_STACK_ALIGN(336))
+
+struct os_eventq blecent_evq;
+struct os_task blecent_task;
+bssnz_t os_stack_t blecent_stack[BLECENT_STACK_SIZE];
+
+/** Our global device address (public) */
+uint8_t g_dev_addr[BLE_DEV_ADDR_LEN] = {0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c};
+
+/** Our random address (in case we need it) */
+uint8_t g_random_addr[BLE_DEV_ADDR_LEN];
+
+static int blecent_gap_event(struct ble_gap_event *event, void *arg);
+
+/**
+ * Application callback.  Called when the read of the ANS Supported New Alert
+ * Category characteristic has completed.
+ */
+static int
+blecent_on_read(uint16_t conn_handle,
+                const struct ble_gatt_error *error,
+                struct ble_gatt_attr *attr,
+                void *arg)
+{
+    BLECENT_LOG(INFO, "Read complete; status=%d conn_handle=%d", error->status,
+                conn_handle);
+    if (error->status == 0) {
+        BLECENT_LOG(INFO, " attr_handle=%d value=", attr->handle);
+        print_mbuf(attr->om);
+    }
+    BLECENT_LOG(INFO, "\n");
+
+    return 0;
+}
+
+/**
+ * Application callback.  Called when the write to the ANS Alert Notification
+ * Control Point characteristic has completed.
+ */
+static int
+blecent_on_write(uint16_t conn_handle,
+                 const struct ble_gatt_error *error,
+                 struct ble_gatt_attr *attr,
+                 void *arg)
+{
+    BLECENT_LOG(INFO, "Write complete; status=%d conn_handle=%d "
+                      "attr_handle=%d\n",
+                error->status, conn_handle, attr->handle);
+
+    return 0;
+}
+
+/**
+ * Application callback.  Called when the attempt to subscribe to notifications
+ * for the ANS Unread Alert Status characteristic has completed.
+ */
+static int
+blecent_on_subscribe(uint16_t conn_handle,
+                     const struct ble_gatt_error *error,
+                     struct ble_gatt_attr *attr,
+                     void *arg)
+{
+    BLECENT_LOG(INFO, "Subscribe complete; status=%d conn_handle=%d "
+                      "attr_handle=%d\n",
+                error->status, conn_handle, attr->handle);
+
+    return 0;
+}
+
+/**
+ * Performs three concurrent GATT operations against the specified peer:
+ * 1. Reads the ANS Supported New Alert Category characteristic.
+ * 2. Writes the ANS Alert Notification Control Point characteristic.
+ * 3. Subscribes to notifications for the ANS Unread Alert Status
+ *    characteristic.
+ *
+ * If the peer does not support a required service, characteristic, or
+ * descriptor, then the peer lied when it claimed support for the alert
+ * notification service!  When this happens, or if a GATT procedure fails,
+ * this function immediately terminates the connection.
+ */
+static void
+blecent_read_write_subscribe(const struct peer *peer)
+{
+    const struct peer_chr *chr;
+    const struct peer_dsc *dsc;
+    uint8_t value[2];
+    int rc;
+
+    /* Read the supported-new-alert-category characteristic. */
+    chr = peer_chr_find_uuid(peer,
+                             BLE_UUID16(BLECENT_SVC_ALERT_UUID),
+                             BLE_UUID16(BLECENT_CHR_SUP_NEW_ALERT_CAT_UUID));
+    if (chr == NULL) {
+        BLECENT_LOG(ERROR, "Error: Peer doesn't support the Supported New "
+                           "Alert Category characteristic\n");
+        goto err;
+    }
+
+    rc = ble_gattc_read(peer->conn_handle, chr->chr.val_handle,
+                        blecent_on_read, NULL);
+    if (rc != 0) {
+        BLECENT_LOG(ERROR, "Error: Failed to read characteristic; rc=%d\n",
+                    rc);
+        goto err;
+    }
+
+    /* Write two bytes (99, 100) to the alert-notification-control-point
+     * characteristic.
+     */
+    chr = peer_chr_find_uuid(peer,
+                             BLE_UUID16(BLECENT_SVC_ALERT_UUID),
+                             BLE_UUID16(BLECENT_CHR_ALERT_NOT_CTRL_PT));
+    if (chr == NULL) {
+        BLECENT_LOG(ERROR, "Error: Peer doesn't support the Alert "
+                           "Notification Control Point characteristic\n");
+        goto err;
+    }
+
+    value[0] = 99;
+    value[1] = 100;
+    rc = ble_gattc_write_flat(peer->conn_handle, chr->chr.val_handle,
+                              value, sizeof value, blecent_on_write, NULL);
+    if (rc != 0) {
+        BLECENT_LOG(ERROR, "Error: Failed to write characteristic; rc=%d\n",
+                    rc);
+    }
+
+    /* Subscribe to notifications for the Unread Alert Status characteristic.
+     * A central enables notifications by writing two bytes (1, 0) to the
+     * characteristic's client-characteristic-configuration-descriptor (CCCD).
+     */
+    dsc = peer_dsc_find_uuid(peer,
+                             BLE_UUID16(BLECENT_SVC_ALERT_UUID),
+                             BLE_UUID16(BLECENT_CHR_UNR_ALERT_STAT_UUID),
+                             BLE_UUID16(BLE_GATT_DSC_CLT_CFG_UUID16));
+    if (dsc == NULL) {
+        BLECENT_LOG(ERROR, "Error: Peer lacks a CCCD for the Unread Alert "
+                           "Status characteristic\n");
+        goto err;
+    }
+
+    value[0] = 1;
+    value[1] = 0;
+    rc = ble_gattc_write_flat(peer->conn_handle, dsc->dsc.handle,
+                              value, sizeof value, blecent_on_subscribe, NULL);
+    if (rc != 0) {
+        BLECENT_LOG(ERROR, "Error: Failed to subscribe to characteristic; "
+                           "rc=%d\n", rc);
+        goto err;
+    }
+
+    return;
+
+err:
+    /* Terminate the connection. */
+    ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+}
+
+/**
+ * Called when service discovery of the specified peer has completed.
+ */
+static void
+blecent_on_disc_complete(const struct peer *peer, int status, void *arg)
+{
+
+    if (status != 0) {
+        /* Service discovery failed.  Terminate the connection. */
+        BLECENT_LOG(ERROR, "Error: Service discovery failed; status=%d "
+                           "conn_handle=%d\n", status, peer->conn_handle);
+        ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+        return;
+    }
+
+    /* Service discovery has completed successfully.  Now we have a complete
+     * list of services, characteristics, and descriptors that the peer
+     * supports.
+     */
+    BLECENT_LOG(ERROR, "Service discovery complete; status=%d "
+                       "conn_handle=%d\n", status, peer->conn_handle);
+
+    /* Now perform three concurrent GATT procedures against the peer: read,
+     * write, and subscribe to notifications.
+     */
+    blecent_read_write_subscribe(peer);
+}
+
+/**
+ * Initiates the GAP general discovery procedure.
+ */
+static void
+blecent_scan(void)
+{
+    struct ble_gap_disc_params disc_params;
+    int rc;
+
+    /* Tell the controller to filter duplicates; we don't want to process
+     * repeated advertisements from the same device.
+     */
+    disc_params.filter_duplicates = 1;
+
+    /**
+     * Perform a passive scan.  I.e., don't send follow-up scan requests to
+     * each advertiser.
+     */
+    disc_params.passive = 1;
+
+    /* Use defaults for the rest of the parameters. */
+    disc_params.itvl = 0;
+    disc_params.window = 0;
+    disc_params.filter_policy = 0;
+    disc_params.limited = 0;
+
+    rc = ble_gap_disc(BLE_ADDR_TYPE_PUBLIC, BLE_HS_FOREVER, &disc_params,
+                      blecent_gap_event, NULL);
+    if (rc != 0) {
+        BLECENT_LOG(ERROR, "Error initiating GAP discovery procedure; rc=%d\n",
+                    rc);
+    }
+}
+
+/**
+ * Indicates whether we should tre to connect to the sender of the specified
+ * advertisement.  The function returns a positive result if the device
+ * advertises connectability and support for the Alert Notification service.
+ */
+static int
+blecent_should_connect(const struct ble_gap_disc_desc *disc)
+{
+    int i;
+
+    /* The device has to be advertising connectability. */
+    if (disc->event_type != BLE_HCI_ADV_RPT_EVTYPE_ADV_IND &&
+        disc->event_type != BLE_HCI_ADV_RPT_EVTYPE_DIR_IND) {
+
+        return 0;
+    }
+
+    /* The device has to advertise support for the Alert Notification
+     * service (0x1811).
+     */
+    for (i = 0; i < disc->fields->num_uuids16; i++) {
+        if (disc->fields->uuids16[i] == BLECENT_SVC_ALERT_UUID) {
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * Connects to the sender of the specified advertisement of it looks
+ * interesting.  A device is "interesting" if it advertises connectability and
+ * support for the Alert Notification service.
+ */
+static void
+blecent_connect_if_interesting(const struct ble_gap_disc_desc *disc)
+{
+    int rc;
+
+    /* Don't do anything if we don't care about this advertiser. */
+    if (!blecent_should_connect(disc)) {
+        return;
+    }
+
+    /* Scanning must be stopped before a connection can be initiated. */
+    rc = ble_gap_disc_cancel();
+    if (rc != 0) {
+        BLECENT_LOG(DEBUG, "Failed to cancel scan; rc=%d\n", rc);
+        return;
+    }
+
+    /* Try to connect the the advertiser.  Allow 30 seconds (30000 ms) for
+     * timeout.
+     */
+    rc = ble_gap_connect(BLE_ADDR_TYPE_PUBLIC, disc->addr_type, disc->addr,
+                         30000, NULL, blecent_gap_event, NULL);
+    if (rc != 0) {
+        BLECENT_LOG(ERROR, "Error: Failed to connect to device; addr_type=%d "
+                           "addr=%s\n", disc->addr_type, addr_str(disc->addr));
+        return;
+    }
+}
+
+/**
+ * The nimble host executes this callback when a GAP event occurs.  The
+ * application associates a GAP event callback with each connection that is
+ * established.  blecent uses the same callback for all connections.
+ *
+ * @param event                 The event being signalled.
+ * @param arg                   Application-specified argument; unused by
+ *                                  blecent.
+ *
+ * @return                      0 if the application successfully handled the
+ *                                  event; nonzero on failure.  The semantics
+ *                                  of the return code is specific to the
+ *                                  particular GAP event being signalled.
+ */
+static int
+blecent_gap_event(struct ble_gap_event *event, void *arg)
+{
+    struct ble_gap_conn_desc desc;
+    int rc;
+
+    switch (event->type) {
+    case BLE_GAP_EVENT_DISC:
+        /* An advertisment report was received during GAP discovery. */
+        print_adv_fields(event->disc.fields);
+
+        /* Try to connect to the advertiser if it looks interesting. */
+        blecent_connect_if_interesting(&event->disc);
+        return 0;
+
+    case BLE_GAP_EVENT_CONNECT:
+        /* A new connection was established or a connection attempt failed. */
+        if (event->connect.status == 0) {
+            /* Connection successfully established. */
+            BLECENT_LOG(INFO, "Connection established ");
+
+            rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
+            assert(rc == 0);
+            print_conn_desc(&desc);
+            BLECENT_LOG(INFO, "\n");
+
+            /* Remember peer. */
+            rc = peer_add(event->connect.conn_handle);
+            if (rc != 0) {
+                BLECENT_LOG(ERROR, "Failed to add peer; rc=%d\n", rc);
+                return 0;
+            }
+
+            /* Perform service discovery. */
+            rc = peer_disc_all(event->connect.conn_handle,
+                               blecent_on_disc_complete, NULL);
+            if (rc != 0) {
+                BLECENT_LOG(ERROR, "Failed to discover services; rc=%d\n", rc);
+                return 0;
+            }
+        } else {
+            /* Connection attempt failed; resume scanning. */
+            BLECENT_LOG(ERROR, "Error: Connection failed; status=%d\n",
+                        event->connect.status);
+            blecent_scan();
+        }
+
+        return 0;
+
+    case BLE_GAP_EVENT_DISCONNECT:
+        /* Connection terminated. */
+        BLECENT_LOG(INFO, "disconnect; reason=%d ", event->disconnect.reason);
+        print_conn_desc(&event->disconnect.conn);
+        BLECENT_LOG(INFO, "\n");
+
+        /* Forget about peer. */
+        peer_delete(event->disconnect.conn.conn_handle);
+
+        /* Resume scanning. */
+        blecent_scan();
+        return 0;
+
+    case BLE_GAP_EVENT_ENC_CHANGE:
+        /* Encryption has been enabled or disabled for this connection. */
+        BLECENT_LOG(INFO, "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);
+        return 0;
+
+    case BLE_GAP_EVENT_NOTIFY_RX:
+        /* Peer sent us a notification or indication. */
+        BLECENT_LOG(INFO, "received %s; conn_handle=%d attr_handle=%d "
+                          "attr_len=%d\n",
+                    event->notify_rx.indication ?
+                        "indication" :
+                        "notification",
+                    event->notify_rx.conn_handle,
+                    event->notify_rx.attr_handle,
+                    OS_MBUF_PKTLEN(event->notify_rx.om));
+
+        /* Attribute data is contained in event->notify_rx.attr_data. */
+        return 0;
+
+    case BLE_GAP_EVENT_MTU:
+        BLECENT_LOG(INFO, "mtu update event; conn_handle=%d cid=%d mtu=%d\n",
+                    event->mtu.conn_handle,
+                    event->mtu.channel_id,
+                    event->mtu.value);
+        return 0;
+
+    default:
+        return 0;
+    }
+}
+
+static void
+blecent_on_reset(int reason)
+{
+    BLECENT_LOG(ERROR, "Resetting state; reason=%d\n", reason);
+}
+
+static void
+blecent_on_sync(void)
+{
+    /* Begin scanning for a peripheral to connect to. */
+    blecent_scan();
+}
+
+/**
+ * Event loop for the main blecent task.
+ */
+static void
+blecent_task_handler(void *unused)
+{
+    struct os_event *ev;
+    struct os_callout_func *cf;
+    int rc;
+
+    /* Activate the host.  This causes the host to synchronize with the
+     * controller.
+     */
+    rc = ble_hs_start();
+    assert(rc == 0);
+
+    while (1) {
+        ev = os_eventq_get(&blecent_evq);
+        switch (ev->ev_type) {
+        case OS_EVENT_T_TIMER:
+            cf = (struct os_callout_func *)ev;
+            assert(cf->cf_func);
+            cf->cf_func(CF_ARG(cf));
+            break;
+
+        default:
+            assert(0);
+            break;
+        }
+    }
+}
+
+/**
+ * main
+ *
+ * The main function for the project. This function initializes the os, calls
+ * init_tasks to initialize tasks (and possibly other objects), then starts the
+ * OS. We should not return from os start.
+ *
+ * @return int NOTE: this function should never return!
+ */
+int
+main(void)
+{
+    struct ble_hci_ram_cfg hci_cfg;
+    struct ble_hs_cfg cfg;
+    uint32_t seed;
+    int rc;
+    int i;
+
+    /* Initialize OS */
+    os_init();
+
+    /* Set cputime to count at 1 usec increments */
+    rc = cputime_init(1000000);
+    assert(rc == 0);
+
+    /* Seed random number generator with least significant bytes of device
+     * address.
+     */
+    seed = 0;
+    for (i = 0; i < 4; ++i) {
+        seed |= g_dev_addr[i];
+        seed <<= 8;
+    }
+    srand(seed);
+
+    /* Initialize msys mbufs. */
+    rc = os_mempool_init(&blecent_mbuf_mpool, MBUF_NUM_MBUFS,
+                         MBUF_MEMBLOCK_SIZE, blecent_mbuf_mpool_data,
+                         "blecent_mbuf_data");
+    assert(rc == 0);
+
+    rc = os_mbuf_pool_init(&blecent_mbuf_pool, &blecent_mbuf_mpool,
+                           MBUF_MEMBLOCK_SIZE, MBUF_NUM_MBUFS);
+    assert(rc == 0);
+
+    rc = os_msys_register(&blecent_mbuf_pool);
+    assert(rc == 0);
+
+    /* Initialize the console (for log output). */
+    rc = console_init(NULL);
+    assert(rc == 0);
+
+    /* Initialize the logging system. */
+    log_init();
+    log_console_handler_init(&blecent_log_console_handler);
+    log_register("blecent", &blecent_log, &blecent_log_console_handler);
+
+    /* Initialize the eventq for the application task. */
+    os_eventq_init(&blecent_evq);
+
+    /* Create the blecent task.  All application logic and NimBLE host
+     * operations are performed in this task.
+     */
+    os_task_init(&blecent_task, "blecent", blecent_task_handler,
+                 NULL, BLECENT_TASK_PRIO, OS_WAIT_FOREVER,
+                 blecent_stack, BLECENT_STACK_SIZE);
+
+    /* Initialize the BLE LL */
+    rc = ble_ll_init(BLE_LL_TASK_PRI, MBUF_NUM_MBUFS, BLE_MBUF_PAYLOAD_SIZE);
+    assert(rc == 0);
+
+    /* Initialize the RAM HCI transport. */
+    hci_cfg = ble_hci_ram_cfg_dflt;
+    rc = ble_hci_ram_init(&hci_cfg);
+    assert(rc == 0);
+
+    /* Configure the host. */
+    cfg = ble_hs_cfg_dflt;
+    cfg.max_hci_bufs = hci_cfg.num_evt_hi_bufs + hci_cfg.num_evt_lo_bufs;
+    cfg.max_gattc_procs = 5;
+    cfg.sm_bonding = 1;
+    cfg.sm_our_key_dist = BLE_SM_PAIR_KEY_DIST_ENC;
+    cfg.sm_their_key_dist = BLE_SM_PAIR_KEY_DIST_ENC;
+    cfg.reset_cb = blecent_on_reset;
+    cfg.sync_cb = blecent_on_sync;
+    cfg.store_read_cb = ble_store_ram_read;
+    cfg.store_write_cb = ble_store_ram_write;
+
+    /* Initialize GATT services. */
+    rc = ble_svc_gap_init(&cfg);
+    assert(rc == 0);
+
+    rc = ble_svc_gatt_init(&cfg);
+    assert(rc == 0);
+
+    /* Initialize the BLE host. */
+    rc = ble_hs_init(&blecent_evq, &cfg);
+    assert(rc == 0);
+
+    /* Initialize data structures to track connected peers. */
+    rc = peer_init(cfg.max_connections, 64, 64, 64);
+    assert(rc == 0);
+
+    /* Set the default device name. */
+    rc = ble_svc_gap_device_name_set("nimble-blecent");
+    assert(rc == 0);
+
+    /* Start the OS */
+    os_start();
+
+    /* os start should never return. If it does, this should be an error */
+    assert(0);
+
+    return 0;
+}
diff --git a/apps/blecent/src/misc.c b/apps/blecent/src/misc.c
new file mode 100644
index 00000000..83667499
--- /dev/null
+++ b/apps/blecent/src/misc.c
@@ -0,0 +1,237 @@
+/**
+ * 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 <assert.h>
+#include <stdio.h>
+#include <string.h>
+#include "host/ble_hs.h"
+#include "blecent.h"
+
+/**
+ * Utility function to log an array of bytes.
+ */
+void
+print_bytes(const uint8_t *bytes, int len)
+{
+    int i;
+
+    for (i = 0; i < len; i++) {
+        BLECENT_LOG(DEBUG, "%s0x%02x", i != 0 ? ":" : "", bytes[i]);
+    }
+}
+
+void
+print_mbuf(const struct os_mbuf *om)
+{
+    int colon;
+
+    colon = 0;
+    while (om != NULL) {
+        if (colon) {
+            BLECENT_LOG(DEBUG, ":");
+        } else {
+            colon = 1;
+        }
+        print_bytes(om->om_data, om->om_len);
+        om = SLIST_NEXT(om, om_next);
+    }
+}
+
+char *
+addr_str(const void *addr)
+{
+    static char buf[6 * 2 + 5 + 1];
+    const uint8_t *u8p;
+
+    u8p = addr;
+    sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x",
+            u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]);
+
+    return buf;
+}
+
+void
+print_uuid(const void *uuid128)
+{
+    uint16_t uuid16;
+    const uint8_t *u8p;
+
+    uuid16 = ble_uuid_128_to_16(uuid128);
+    if (uuid16 != 0) {
+        BLECENT_LOG(DEBUG, "0x%04x", uuid16);
+        return;
+    }
+
+    u8p = uuid128;
+
+    /* 00001101-0000-1000-8000-00805f9b34fb */
+    BLECENT_LOG(DEBUG, "%02x%02x%02x%02x-", u8p[15], u8p[14], u8p[13],
+                u8p[12]);
+    BLECENT_LOG(DEBUG, "%02x%02x-%02x%02x-", u8p[11], u8p[10], u8p[9], u8p[8]);
+    BLECENT_LOG(DEBUG, "%02x%02x%02x%02x%02x%02x%02x%02x",
+                   u8p[7], u8p[6], u8p[5], u8p[4],
+                   u8p[3], u8p[2], u8p[1], u8p[0]);
+}
+
+/**
+ * Logs information about a connection to the console.
+ */
+void
+print_conn_desc(const struct ble_gap_conn_desc *desc)
+{
+    BLECENT_LOG(DEBUG, "handle=%d our_ota_addr_type=%d our_ota_addr=%s ",
+                desc->conn_handle, desc->our_ota_addr_type,
+                addr_str(desc->our_ota_addr));
+    BLECENT_LOG(DEBUG, "our_id_addr_type=%d our_id_addr=%s ",
+                desc->our_id_addr_type, addr_str(desc->our_id_addr));
+    BLECENT_LOG(DEBUG, "peer_ota_addr_type=%d peer_ota_addr=%s ",
+                desc->peer_ota_addr_type, addr_str(desc->peer_ota_addr));
+    BLECENT_LOG(DEBUG, "peer_id_addr_type=%d peer_id_addr=%s ",
+                desc->peer_id_addr_type, addr_str(desc->peer_id_addr));
+    BLECENT_LOG(DEBUG, "conn_itvl=%d conn_latency=%d supervision_timeout=%d "
+                "encrypted=%d authenticated=%d bonded=%d",
+                desc->conn_itvl, desc->conn_latency,
+                desc->supervision_timeout,
+                desc->sec_state.encrypted,
+                desc->sec_state.authenticated,
+                desc->sec_state.bonded);
+}
+
+
+void
+print_adv_fields(const struct ble_hs_adv_fields *fields)
+{
+    char s[BLE_HCI_MAX_ADV_DATA_LEN];
+    const uint8_t *u8p;
+    int i;
+
+    if (fields->flags_is_present) {
+        BLECENT_LOG(DEBUG, "    flags=0x%02x\n", fields->flags);
+    }
+
+    if (fields->uuids16 != NULL) {
+        BLECENT_LOG(DEBUG, "    uuids16(%scomplete)=",
+                    fields->uuids16_is_complete ? "" : "in");
+        for (i = 0; i < fields->num_uuids16; i++) {
+            BLECENT_LOG(DEBUG, "0x%04x ", fields->uuids16[i]);
+        }
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->uuids32 != NULL) {
+        BLECENT_LOG(DEBUG, "    uuids32(%scomplete)=",
+                    fields->uuids32_is_complete ? "" : "in");
+        for (i = 0; i < fields->num_uuids32; i++) {
+            BLECENT_LOG(DEBUG, "0x%08x ", fields->uuids32[i]);
+        }
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->uuids128 != NULL) {
+        BLECENT_LOG(DEBUG, "    uuids128(%scomplete)=",
+                    fields->uuids128_is_complete ? "" : "in");
+        u8p = fields->uuids128;
+        for (i = 0; i < fields->num_uuids128; i++) {
+            print_uuid(u8p);
+            BLECENT_LOG(DEBUG, " ");
+            u8p += 16;
+        }
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->name != NULL) {
+        assert(fields->name_len < sizeof s - 1);
+        memcpy(s, fields->name, fields->name_len);
+        s[fields->name_len] = '\0';
+        BLECENT_LOG(DEBUG, "    name(%scomplete)=%s\n",
+                    fields->name_is_complete ? "" : "in", s);
+    }
+
+    if (fields->tx_pwr_lvl_is_present) {
+        BLECENT_LOG(DEBUG, "    tx_pwr_lvl=%d\n", fields->tx_pwr_lvl);
+    }
+
+    if (fields->device_class != NULL) {
+        BLECENT_LOG(DEBUG, "    device_class=");
+        print_bytes(fields->device_class, BLE_HS_ADV_DEVICE_CLASS_LEN);
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->slave_itvl_range != NULL) {
+        BLECENT_LOG(DEBUG, "    slave_itvl_range=");
+        print_bytes(fields->slave_itvl_range, BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN);
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->svc_data_uuid16 != NULL) {
+        BLECENT_LOG(DEBUG, "    svc_data_uuid16=");
+        print_bytes(fields->svc_data_uuid16, fields->svc_data_uuid16_len);
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->public_tgt_addr != NULL) {
+        BLECENT_LOG(DEBUG, "    public_tgt_addr=");
+        u8p = fields->public_tgt_addr;
+        for (i = 0; i < fields->num_public_tgt_addrs; i++) {
+            BLECENT_LOG(DEBUG, "public_tgt_addr=%s ", addr_str(u8p));
+            u8p += BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN;
+        }
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->appearance_is_present) {
+        BLECENT_LOG(DEBUG, "    appearance=0x%04x\n", fields->appearance);
+    }
+
+    if (fields->adv_itvl_is_present) {
+        BLECENT_LOG(DEBUG, "    adv_itvl=0x%04x\n", fields->adv_itvl);
+    }
+
+    if (fields->le_addr != NULL) {
+        BLECENT_LOG(DEBUG, "    le_addr=%s\n", addr_str(fields->le_addr));
+    }
+
+    if (fields->le_role_is_present) {
+        BLECENT_LOG(DEBUG, "    le_role=0x%02x\n", fields->le_role);
+    }
+
+    if (fields->svc_data_uuid32 != NULL) {
+        BLECENT_LOG(DEBUG, "    svc_data_uuid32=");
+        print_bytes(fields->svc_data_uuid32, fields->svc_data_uuid32_len);
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->svc_data_uuid128 != NULL) {
+        BLECENT_LOG(DEBUG, "    svc_data_uuid128=");
+        print_bytes(fields->svc_data_uuid128, fields->svc_data_uuid128_len);
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->uri != NULL) {
+        BLECENT_LOG(DEBUG, "    uri=");
+        print_bytes(fields->uri, fields->uri_len);
+        BLECENT_LOG(DEBUG, "\n");
+    }
+
+    if (fields->mfg_data != NULL) {
+        BLECENT_LOG(DEBUG, "    mfg_data=");
+        print_bytes(fields->mfg_data, fields->mfg_data_len);
+        BLECENT_LOG(DEBUG, "\n");
+    }
+}
diff --git a/apps/blecent/src/peer.c b/apps/blecent/src/peer.c
new file mode 100644
index 00000000..e636fa98
--- /dev/null
+++ b/apps/blecent/src/peer.c
@@ -0,0 +1,807 @@
+/**
+ * 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 <assert.h>
+#include <string.h>
+#include "host/ble_hs.h"
+#include "blecent.h"
+
+static void *peer_svc_mem;
+static struct os_mempool peer_svc_pool;
+
+static void *peer_chr_mem;
+static struct os_mempool peer_chr_pool;
+
+static void *peer_dsc_mem;
+static struct os_mempool peer_dsc_pool;
+
+static void *peer_mem;
+static struct os_mempool peer_pool;
+static SLIST_HEAD(, peer) peers;
+
+static struct peer_svc *
+peer_svc_find_range(struct peer *peer, uint16_t attr_handle);
+static struct peer_svc *
+peer_svc_find(struct peer *peer, uint16_t svc_start_handle,
+              struct peer_svc **out_prev);
+int
+peer_svc_is_empty(const struct peer_svc *svc);
+
+uint16_t
+chr_end_handle(const struct peer_svc *svc, const struct peer_chr *chr);
+int
+chr_is_empty(const struct peer_svc *svc, const struct peer_chr *chr);
+static struct peer_chr *
+peer_chr_find(const struct peer_svc *svc, uint16_t chr_def_handle,
+              struct peer_chr **out_prev);
+static void
+peer_disc_chrs(struct peer *peer);
+
+static int
+peer_dsc_disced(uint16_t conn_handle, const struct ble_gatt_error *error,
+                uint16_t chr_def_handle, const struct ble_gatt_dsc *dsc,
+                void *arg);
+
+static struct peer *
+peer_find(uint16_t conn_handle)
+{
+    struct peer *peer;
+
+    SLIST_FOREACH(peer, &peers, next) {
+        if (peer->conn_handle == conn_handle) {
+            return peer;
+        }
+    }
+
+    return NULL;
+}
+
+static void
+peer_disc_complete(struct peer *peer, int rc)
+{
+    peer->disc_prev_chr_val = 0;
+
+    /* Notify caller that discovery has completed. */
+    if (peer->disc_cb != NULL) {
+        peer->disc_cb(peer, rc, peer->disc_cb_arg);
+    }
+}
+
+static struct peer_dsc *
+peer_dsc_find_prev(const struct peer_chr *chr, uint16_t dsc_handle)
+{
+    struct peer_dsc *prev;
+    struct peer_dsc *dsc;
+
+    prev = NULL;
+    SLIST_FOREACH(dsc, &chr->dscs, next) {
+        if (dsc->dsc.handle >= dsc_handle) {
+            break;
+        }
+
+        prev = dsc;
+    }
+
+    return prev;
+}
+
+static struct peer_dsc *
+peer_dsc_find(const struct peer_chr *chr, uint16_t dsc_handle,
+              struct peer_dsc **out_prev)
+{
+    struct peer_dsc *prev;
+    struct peer_dsc *dsc;
+
+    prev = peer_dsc_find_prev(chr, dsc_handle);
+    if (prev == NULL) {
+        dsc = SLIST_FIRST(&chr->dscs);
+    } else {
+        dsc = SLIST_NEXT(prev, next);
+    }
+
+    if (dsc != NULL && dsc->dsc.handle != dsc_handle) {
+        dsc = NULL;
+    }
+
+    if (out_prev != NULL) {
+        *out_prev = prev;
+    }
+    return dsc;
+}
+
+static int
+peer_dsc_add(struct peer *peer, uint16_t chr_val_handle,
+             const struct ble_gatt_dsc *gatt_dsc)
+{
+    struct peer_dsc *prev;
+    struct peer_dsc *dsc;
+    struct peer_svc *svc;
+    struct peer_chr *chr;
+
+    svc = peer_svc_find_range(peer, chr_val_handle);
+    if (svc == NULL) {
+        /* Can't find service for discovered descriptor; this shouldn't
+         * happen.
+         */
+        assert(0);
+        return BLE_HS_EUNKNOWN;
+    }
+
+    chr = peer_chr_find(svc, chr_val_handle, NULL);
+    if (chr == NULL) {
+        /* Can't find characteristic for discovered descriptor; this shouldn't
+         * happen.
+         */
+        assert(0);
+        return BLE_HS_EUNKNOWN;
+    }
+
+    dsc = peer_dsc_find(chr, gatt_dsc->handle, &prev);
+    if (dsc != NULL) {
+        /* Descriptor already discovered. */
+        return 0;
+    }
+
+    dsc = os_memblock_get(&peer_dsc_pool);
+    if (dsc == NULL) {
+        /* Out of memory. */
+        return BLE_HS_ENOMEM;
+    }
+    memset(dsc, 0, sizeof *dsc);
+
+    dsc->dsc = *gatt_dsc;
+
+    if (prev == NULL) {
+        SLIST_INSERT_HEAD(&chr->dscs, dsc, next);
+    } else {
+        SLIST_NEXT(prev, next) = dsc;
+    }
+
+    return 0;
+}
+
+static void
+peer_disc_dscs(struct peer *peer)
+{
+    struct peer_chr *chr;
+    struct peer_svc *svc;
+    int rc;
+
+    /* Search through the list of discovered characteristics for the first
+     * characteristic that contains undiscovered descriptors.  Then, discover
+     * all descriptors belonging to that characteristic.
+     */
+    SLIST_FOREACH(svc, &peer->svcs, next) {
+        SLIST_FOREACH(chr, &svc->chrs, next) {
+            if (!chr_is_empty(svc, chr) &&
+                SLIST_EMPTY(&chr->dscs) &&
+                peer->disc_prev_chr_val <= chr->chr.def_handle) {
+
+                rc = ble_gattc_disc_all_dscs(peer->conn_handle,
+                                             chr->chr.val_handle,
+                                             chr_end_handle(svc, chr),
+                                             peer_dsc_disced, peer);
+                if (rc != 0) {
+                    peer_disc_complete(peer, rc);
+                }
+
+                peer->disc_prev_chr_val = chr->chr.val_handle;
+                return;
+            }
+        }
+    }
+
+    /* All descriptors discovered. */
+    peer_disc_complete(peer, 0);
+}
+
+static int
+peer_dsc_disced(uint16_t conn_handle, const struct ble_gatt_error *error,
+                uint16_t chr_val_handle, const struct ble_gatt_dsc *dsc,
+                void *arg)
+{
+    struct peer *peer;
+    int rc;
+
+    peer = arg;
+    assert(peer->conn_handle == conn_handle);
+
+    switch (error->status) {
+    case 0:
+        rc = peer_dsc_add(peer, chr_val_handle, dsc);
+        break;
+
+    case BLE_HS_EDONE:
+        /* All descriptors in this characteristic discovered; start discovering
+         * descriptors in the next characteristic.
+         */
+        if (peer->disc_prev_chr_val > 0) {
+            peer_disc_dscs(peer);
+        }
+        rc = 0;
+        break;
+
+    default:
+        /* Error; abort discovery. */
+        rc = error->status;
+        break;
+    }
+
+    if (rc != 0) {
+        /* Error; abort discovery. */
+        peer_disc_complete(peer, rc);
+    }
+
+    return rc;
+}
+
+uint16_t
+chr_end_handle(const struct peer_svc *svc, const struct peer_chr *chr)
+{
+    const struct peer_chr *next_chr;
+
+    next_chr = SLIST_NEXT(chr, next);
+    if (next_chr != NULL) {
+        return next_chr->chr.def_handle - 1;
+    } else {
+        return svc->svc.end_handle;
+    }
+}
+
+int
+chr_is_empty(const struct peer_svc *svc, const struct peer_chr *chr)
+{
+    return chr_end_handle(svc, chr) <= chr->chr.val_handle;
+}
+
+static struct peer_chr *
+peer_chr_find_prev(const struct peer_svc *svc, uint16_t chr_val_handle)
+{
+    struct peer_chr *prev;
+    struct peer_chr *chr;
+
+    prev = NULL;
+    SLIST_FOREACH(chr, &svc->chrs, next) {
+        if (chr->chr.val_handle >= chr_val_handle) {
+            break;
+        }
+
+        prev = chr;
+    }
+
+    return prev;
+}
+
+static struct peer_chr *
+peer_chr_find(const struct peer_svc *svc, uint16_t chr_val_handle,
+              struct peer_chr **out_prev)
+{
+    struct peer_chr *prev;
+    struct peer_chr *chr;
+
+    prev = peer_chr_find_prev(svc, chr_val_handle);
+    if (prev == NULL) {
+        chr = SLIST_FIRST(&svc->chrs);
+    } else {
+        chr = SLIST_NEXT(prev, next);
+    }
+
+    if (chr != NULL && chr->chr.val_handle != chr_val_handle) {
+        chr = NULL;
+    }
+
+    if (out_prev != NULL) {
+        *out_prev = prev;
+    }
+    return chr;
+}
+
+static void
+peer_chr_delete(struct peer_chr *chr)
+{
+    struct peer_dsc *dsc;
+
+    while ((dsc = SLIST_FIRST(&chr->dscs)) != NULL) {
+        SLIST_REMOVE_HEAD(&chr->dscs, next);
+        os_memblock_put(&peer_dsc_pool, dsc);
+    }
+
+    os_memblock_put(&peer_chr_pool, chr);
+}
+
+static int
+peer_chr_add(struct peer *peer,  uint16_t svc_start_handle,
+             const struct ble_gatt_chr *gatt_chr)
+{
+    struct peer_chr *prev;
+    struct peer_chr *chr;
+    struct peer_svc *svc;
+
+    svc = peer_svc_find(peer, svc_start_handle, NULL);
+    if (svc == NULL) {
+        /* Can't find service for discovered characteristic; this shouldn't
+         * happen.
+         */
+        assert(0);
+        return BLE_HS_EUNKNOWN;
+    }
+
+    chr = peer_chr_find(svc, gatt_chr->def_handle, &prev);
+    if (chr != NULL) {
+        /* Characteristic already discovered. */
+        return 0;
+    }
+
+    chr = os_memblock_get(&peer_chr_pool);
+    if (chr == NULL) {
+        /* Out of memory. */
+        return BLE_HS_ENOMEM;
+    }
+    memset(chr, 0, sizeof *chr);
+
+    chr->chr = *gatt_chr;
+
+    if (prev == NULL) {
+        SLIST_INSERT_HEAD(&svc->chrs, chr, next);
+    } else {
+        SLIST_NEXT(prev, next) = chr;
+    }
+
+    return 0;
+}
+
+static int
+peer_chr_disced(uint16_t conn_handle, const struct ble_gatt_error *error,
+                const struct ble_gatt_chr *chr, void *arg)
+{
+    struct peer *peer;
+    int rc;
+
+    peer = arg;
+    assert(peer->conn_handle == conn_handle);
+
+    switch (error->status) {
+    case 0:
+        rc = peer_chr_add(peer, peer->cur_svc->svc.start_handle, chr);
+        break;
+
+    case BLE_HS_EDONE:
+        /* All characteristics in this service discovered; start discovering
+         * characteristics in the next service.
+         */
+        if (peer->disc_prev_chr_val > 0) {
+             peer_disc_chrs(peer);
+        }
+        rc = 0;
+        break;
+
+    default:
+        rc = error->status;
+        break;
+    }
+
+    if (rc != 0) {
+        /* Error; abort discovery. */
+        peer_disc_complete(peer, rc);
+    }
+
+    return rc;
+}
+
+static void
+peer_disc_chrs(struct peer *peer)
+{
+    struct peer_svc *svc;
+    int rc;
+
+    /* Search through the list of discovered service for the first service that
+     * contains undiscovered characteristics.  Then, discover all
+     * characteristics belonging to that service.
+     */
+    SLIST_FOREACH(svc, &peer->svcs, next) {
+        if (!peer_svc_is_empty(svc) && SLIST_EMPTY(&svc->chrs)) {
+            peer->cur_svc = svc;
+            rc = ble_gattc_disc_all_chrs(peer->conn_handle,
+                                         svc->svc.start_handle,
+                                         svc->svc.end_handle,
+                                         peer_chr_disced, peer);
+            if (rc != 0) {
+                peer_disc_complete(peer, rc);
+            }
+            return;
+        }
+    }
+
+    /* All characteristics discovered. */
+    peer_disc_dscs(peer);
+}
+
+int
+peer_svc_is_empty(const struct peer_svc *svc)
+{
+    return svc->svc.end_handle < svc->svc.start_handle;
+}
+
+static struct peer_svc *
+peer_svc_find_prev(struct peer *peer, uint16_t svc_start_handle)
+{
+    struct peer_svc *prev;
+    struct peer_svc *svc;
+
+    prev = NULL;
+    SLIST_FOREACH(svc, &peer->svcs, next) {
+        if (svc->svc.start_handle >= svc_start_handle) {
+            break;
+        }
+
+        prev = svc;
+    }
+
+    return prev;
+}
+
+static struct peer_svc *
+peer_svc_find(struct peer *peer, uint16_t svc_start_handle,
+              struct peer_svc **out_prev)
+{
+    struct peer_svc *prev;
+    struct peer_svc *svc;
+
+    prev = peer_svc_find_prev(peer, svc_start_handle);
+    if (prev == NULL) {
+        svc = SLIST_FIRST(&peer->svcs);
+    } else {
+        svc = SLIST_NEXT(prev, next);
+    }
+
+    if (svc != NULL && svc->svc.start_handle != svc_start_handle) {
+        svc = NULL;
+    }
+
+    if (out_prev != NULL) {
+        *out_prev = prev;
+    }
+    return svc;
+}
+
+static struct peer_svc *
+peer_svc_find_range(struct peer *peer, uint16_t attr_handle)
+{
+    struct peer_svc *svc;
+
+    SLIST_FOREACH(svc, &peer->svcs, next) {
+        if (svc->svc.start_handle <= attr_handle &&
+            svc->svc.end_handle >= attr_handle) {
+
+            return svc;
+        }
+    }
+
+    return NULL;
+}
+
+const struct peer_svc *
+peer_svc_find_uuid(const struct peer *peer, const uint8_t *uuid128)
+{
+    const struct peer_svc *svc;
+
+    SLIST_FOREACH(svc, &peer->svcs, next) {
+        if (memcmp(svc->svc.uuid128, uuid128, 16) == 0) {
+            return svc;
+        }
+    }
+
+    return NULL;
+}
+
+const struct peer_chr *
+peer_chr_find_uuid(const struct peer *peer, const uint8_t *svc_uuid128,
+                   const uint8_t *chr_uuid128)
+{
+    const struct peer_svc *svc;
+    const struct peer_chr *chr;
+
+    svc = peer_svc_find_uuid(peer, svc_uuid128);
+    if (svc == NULL) {
+        return NULL;
+    }
+
+    SLIST_FOREACH(chr, &svc->chrs, next) {
+        if (memcmp(chr->chr.uuid128, chr_uuid128, 16) == 0) {
+            return chr;
+        }
+    }
+
+    return NULL;
+}
+
+const struct peer_dsc *
+peer_dsc_find_uuid(const struct peer *peer, const uint8_t *svc_uuid128,
+                   const uint8_t *chr_uuid128, const uint8_t *dsc_uuid128)
+{
+    const struct peer_chr *chr;
+    const struct peer_dsc *dsc;
+
+    chr = peer_chr_find_uuid(peer, svc_uuid128, chr_uuid128);
+    if (chr == NULL) {
+        return NULL;
+    }
+
+    SLIST_FOREACH(dsc, &chr->dscs, next) {
+        if (memcmp(dsc->dsc.uuid128, dsc_uuid128, 16) == 0) {
+            return dsc;
+        }
+    }
+
+    return NULL;
+}
+
+static int
+peer_svc_add(struct peer *peer, const struct ble_gatt_svc *gatt_svc)
+{
+    struct peer_svc *prev;
+    struct peer_svc *svc;
+
+    svc = peer_svc_find(peer, gatt_svc->start_handle, &prev);
+    if (svc != NULL) {
+        /* Service already discovered. */
+        return 0;
+    }
+
+    svc = os_memblock_get(&peer_svc_pool);
+    if (svc == NULL) {
+        /* Out of memory. */
+        return BLE_HS_ENOMEM;
+    }
+    memset(svc, 0, sizeof *svc);
+
+    svc->svc = *gatt_svc;
+    SLIST_INIT(&svc->chrs);
+
+    if (prev == NULL) {
+        SLIST_INSERT_HEAD(&peer->svcs, svc, next);
+    } else {
+        SLIST_INSERT_AFTER(prev, svc, next);
+    }
+
+    return 0;
+}
+
+static void
+peer_svc_delete(struct peer_svc *svc)
+{
+    struct peer_chr *chr;
+
+    while ((chr = SLIST_FIRST(&svc->chrs)) != NULL) {
+        SLIST_REMOVE_HEAD(&svc->chrs, next);
+        peer_chr_delete(chr);
+    }
+
+    os_memblock_put(&peer_svc_pool, svc);
+}
+
+static int
+peer_svc_disced(uint16_t conn_handle, const struct ble_gatt_error *error,
+                const struct ble_gatt_svc *service, void *arg)
+{
+    struct peer *peer;
+    int rc;
+
+    peer = arg;
+    assert(peer->conn_handle == conn_handle);
+
+    switch (error->status) {
+    case 0:
+        rc = peer_svc_add(peer, service);
+        break;
+
+    case BLE_HS_EDONE:
+        /* All services discovered; start discovering characteristics. */
+        if (peer->disc_prev_chr_val > 0) {
+            peer_disc_chrs(peer);
+        }
+        rc = 0;
+        break;
+
+    default:
+        rc = error->status;
+        break;
+    }
+
+    if (rc != 0) {
+        /* Error; abort discovery. */
+        peer_disc_complete(peer, rc);
+    }
+
+    return rc;
+}
+
+
+int
+peer_disc_all(uint16_t conn_handle, peer_disc_fn *disc_cb, void *disc_cb_arg)
+{
+    struct peer_svc *svc;
+    struct peer *peer;
+    int rc;
+
+    peer = peer_find(conn_handle);
+    if (peer == NULL) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    /* Undiscover everything first. */
+    while ((svc = SLIST_FIRST(&peer->svcs)) != NULL) {
+        SLIST_REMOVE_HEAD(&peer->svcs, next);
+        peer_svc_delete(svc);
+    }
+
+    peer->disc_prev_chr_val = 1;
+    peer->disc_cb = disc_cb;
+    peer->disc_cb_arg = disc_cb_arg;
+
+    rc = ble_gattc_disc_all_svcs(conn_handle, peer_svc_disced, peer);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+int
+peer_delete(uint16_t conn_handle)
+{
+    struct peer_svc *svc;
+    struct peer *peer;
+    int rc;
+
+    peer = peer_find(conn_handle);
+    if (peer == NULL) {
+        return BLE_HS_ENOTCONN;
+    }
+
+    SLIST_REMOVE(&peers, peer, peer, next);
+
+    while ((svc = SLIST_FIRST(&peer->svcs)) != NULL) {
+        SLIST_REMOVE_HEAD(&peer->svcs, next);
+        peer_svc_delete(svc);
+    }
+
+    rc = os_memblock_put(&peer_pool, peer);
+    if (rc != 0) {
+        return BLE_HS_EOS;
+    }
+
+    return 0;
+}
+
+int
+peer_add(uint16_t conn_handle)
+{
+    struct peer *peer;
+
+    /* Make sure the connection handle is unique. */
+    peer = peer_find(conn_handle);
+    if (peer != NULL) {
+        return BLE_HS_EALREADY;
+    }
+
+    peer = os_memblock_get(&peer_pool);
+    if (peer == NULL) {
+        /* Out of memory. */
+        return BLE_HS_ENOMEM;
+    }
+
+    memset(peer, 0, sizeof *peer);
+    peer->conn_handle = conn_handle;
+
+    SLIST_INSERT_HEAD(&peers, peer, next);
+
+    return 0;
+}
+
+static void
+peer_free_mem(void)
+{
+    free(peer_mem);
+    peer_mem = NULL;
+
+    free(peer_svc_mem);
+    peer_svc_mem = NULL;
+
+    free(peer_chr_mem);
+    peer_chr_mem = NULL;
+
+    free(peer_dsc_mem);
+    peer_dsc_mem = NULL;
+}
+
+int
+peer_init(int max_peers, int max_svcs, int max_chrs, int max_dscs)
+{
+    int rc;
+
+    /* Free memory first in case this function gets called more than once. */
+    peer_free_mem();
+
+    peer_mem = malloc(
+        OS_MEMPOOL_BYTES(max_peers, sizeof (struct peer)));
+    if (peer_mem == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    rc = os_mempool_init(&peer_pool, max_peers,
+                         sizeof (struct peer), peer_mem,
+                         "peer_pool");
+    if (rc != 0) {
+        rc = BLE_HS_EOS;
+        goto err;
+    }
+
+    peer_svc_mem = malloc(
+        OS_MEMPOOL_BYTES(max_svcs, sizeof (struct peer_svc)));
+    if (peer_svc_mem == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    rc = os_mempool_init(&peer_svc_pool, max_svcs,
+                         sizeof (struct peer_svc), peer_svc_mem,
+                         "peer_svc_pool");
+    if (rc != 0) {
+        rc = BLE_HS_EOS;
+        goto err;
+    }
+
+    peer_chr_mem = malloc(
+        OS_MEMPOOL_BYTES(max_chrs, sizeof (struct peer_chr)));
+    if (peer_chr_mem == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    rc = os_mempool_init(&peer_chr_pool, max_chrs,
+                         sizeof (struct peer_chr), peer_chr_mem,
+                         "peer_chr_pool");
+    if (rc != 0) {
+        rc = BLE_HS_EOS;
+        goto err;
+    }
+
+    peer_dsc_mem = malloc(
+        OS_MEMPOOL_BYTES(max_dscs, sizeof (struct peer_dsc)));
+    if (peer_dsc_mem == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    rc = os_mempool_init(&peer_dsc_pool, max_dscs,
+                         sizeof (struct peer_dsc), peer_dsc_mem,
+                         "peer_dsc_pool");
+    if (rc != 0) {
+        rc = BLE_HS_EOS;
+        goto err;
+    }
+
+    return 0;
+
+err:
+    peer_free_mem();
+    return rc;
+}
diff --git a/apps/blehci/pkg.yml b/apps/blehci/pkg.yml
index f36e383f..198e3957 100644
--- a/apps/blehci/pkg.yml
+++ b/apps/blehci/pkg.yml
@@ -23,7 +23,8 @@ pkg.homepage: "http://mynewt.apache.org/"
 pkg.keywords:
 
 pkg.deps:
-    - libs/os
-    - net/nimble/controller
     - libs/baselibc
     - libs/console/stub
+    - libs/os
+    - net/nimble/controller
+    - net/nimble/transport/uart
diff --git a/apps/blehci/src/main.c b/apps/blehci/src/main.c
index f0736b7e..e95f402f 100755
--- a/apps/blehci/src/main.c
+++ b/apps/blehci/src/main.c
@@ -17,24 +17,14 @@
  * under the License.
  */
 #include <assert.h>
-#include <string.h>
-#include <stdio.h>
-#include <errno.h>
-#include "bsp/bsp.h"
 #include "os/os.h"
-#include "bsp/bsp.h"
-#include "hal/hal_gpio.h"
 #include "hal/hal_cputime.h"
 #include "hal/hal_uart.h"
 
 /* BLE */
 #include "nimble/ble.h"
-#include "nimble/nimble_opt.h"
-#include "nimble/hci_transport.h"
 #include "controller/ble_ll.h"
-
-#define HCI_UART_SPEED 1000000
-#define HCI_UART CONSOLE_UART
+#include "transport/uart/ble_hci_uart.h"
 
 /* Nimble task priorities */
 #define BLE_LL_TASK_PRI         (OS_TASK_PRI_HIGHEST)
@@ -53,294 +43,15 @@ uint8_t g_random_addr[BLE_DEV_ADDR_LEN] = { 0 };
 
 #define HCI_MAX_BUFS        (5)
 
-#define HCI_CMD_BUF_SIZE    (260)
-struct os_mempool g_hci_cmd_pool;
-static void *hci_cmd_buf;
-
-#define HCI_OS_EVENT_BUF_SIZE   (sizeof(struct os_event))
-
-#define BLE_HOST_HCI_EVENT_CTLR_EVENT   (OS_EVENT_T_PERUSER + 0)
-#define BLE_HOST_HCI_EVENT_CTLR_DATA    (OS_EVENT_T_PERUSER + 1)
-
-struct os_mempool g_hci_os_event_pool;
-static void *hci_os_event_buf;
-
 os_membuf_t default_mbuf_mpool_data[MBUF_MEMPOOL_SIZE];
 
 struct os_mbuf_pool default_mbuf_pool;
 struct os_mempool default_mbuf_mpool;
 
-#define H4_NONE 0x00
-#define H4_CMD  0x01
-#define H4_ACL  0x02
-#define H4_SCO  0x03
-#define H4_EVT  0x04
-
-#define HCI_CMD_HDR_LEN 3
-#define HCI_ACL_HDR_LEN 4
-#define HCI_EVT_HDR_LEN 2
-
-struct memblock {
-    uint8_t *data;      /* Pointer to memblock data */
-    uint16_t cur;       /* Number of bytes read/written */
-    uint16_t len;       /* Total number of bytes to read/write */
-};
-
-struct tx_acl {
-    struct os_mbuf *buf; /* Buffer containing the data */
-    uint16_t len;        /* Target size when buf is considered complete */
-};
-
-static struct {
-    /* State of data from host to controller */
-    uint8_t tx_type;    /* Pending packet type. 0 means nothing pending */
-    union {
-        struct memblock tx_cmd;
-        struct tx_acl tx_acl;
-    };
-
-    /* State of data from controller to host */
-    uint8_t rx_type;    /* Pending packet type. 0 means nothing pending */
-    union {
-        struct memblock rx_evt;
-        struct os_mbuf *rx_acl;
-    };
-    STAILQ_HEAD(, os_event) rx_pkts; /* Packet queue to send to UART */
-} hci;
-
-int
-ble_hs_rx_data(struct os_mbuf *om)
-{
-    struct os_event *ev;
-    os_sr_t sr;
-
-    ev = os_memblock_get(&g_hci_os_event_pool);
-    if (!ev) {
-        os_mbuf_free_chain(om);
-        return -1;
-    }
-
-    ev->ev_type = BLE_HOST_HCI_EVENT_CTLR_DATA;
-    ev->ev_arg = om;
-    ev->ev_queued = 1;
-
-    OS_ENTER_CRITICAL(sr);
-    STAILQ_INSERT_TAIL(&hci.rx_pkts, ev, ev_next);
-    OS_EXIT_CRITICAL(sr);
-
-    hal_uart_start_tx(HCI_UART);
-
-    return 0;
-}
-
-int
-ble_hci_transport_ctlr_event_send(uint8_t *hci_ev)
-{
-    struct os_event *ev;
-    os_sr_t sr;
-
-    ev = os_memblock_get(&g_hci_os_event_pool);
-    if (!ev) {
-        os_error_t err;
-
-        err = os_memblock_put(&g_hci_cmd_pool, hci_ev);
-        assert(err == OS_OK);
-
-        return -1;
-    }
-
-    ev->ev_type = BLE_HOST_HCI_EVENT_CTLR_EVENT;
-    ev->ev_arg = hci_ev;
-    ev->ev_queued = 1;
-
-    OS_ENTER_CRITICAL(sr);
-    STAILQ_INSERT_TAIL(&hci.rx_pkts, ev, ev_next);
-    OS_EXIT_CRITICAL(sr);
-
-    hal_uart_start_tx(HCI_UART);
-
-    return 0;
-}
-
-static int
-uart_tx_pkt_type(void)
-{
-    struct os_event *ev;
-    os_sr_t sr;
-    int rc;
-
-    OS_ENTER_CRITICAL(sr);
-
-    ev = STAILQ_FIRST(&hci.rx_pkts);
-    if (!ev) {
-        OS_EXIT_CRITICAL(sr);
-        return -1;
-    }
-
-    STAILQ_REMOVE(&hci.rx_pkts, ev, os_event, ev_next);
-    ev->ev_queued = 0;
-
-    OS_EXIT_CRITICAL(sr);
-
-    switch (ev->ev_type) {
-    case BLE_HOST_HCI_EVENT_CTLR_EVENT:
-        hci.rx_type = H4_EVT;
-        hci.rx_evt.data = ev->ev_arg;
-        hci.rx_evt.cur = 0;
-        hci.rx_evt.len = hci.rx_evt.data[1] + HCI_EVT_HDR_LEN;
-        rc = H4_EVT;
-        break;
-    case BLE_HOST_HCI_EVENT_CTLR_DATA:
-        hci.rx_type = H4_ACL;
-        hci.rx_acl = ev->ev_arg;
-        rc = H4_ACL;
-        break;
-    default:
-        rc = -1;
-        break;
-    }
-
-    os_memblock_put(&g_hci_os_event_pool, ev);
-
-    return rc;
-}
-
-static int
-uart_tx_char(void *arg)
-{
-    int rc = -1;
-
-    switch (hci.rx_type) {
-    case H4_NONE: /* No pending packet, pick one from the queue */
-        rc = uart_tx_pkt_type();
-        break;
-    case H4_EVT:
-        rc = hci.rx_evt.data[hci.rx_evt.cur++];
-
-        if (hci.rx_evt.cur == hci.rx_evt.len) {
-            os_memblock_put(&g_hci_cmd_pool, hci.rx_evt.data);
-            hci.rx_type = H4_NONE;
-        }
-
-        break;
-    case H4_ACL:
-        rc = *OS_MBUF_DATA(hci.rx_acl, uint8_t *);
-        os_mbuf_adj(hci.rx_acl, 1);
-        if (!OS_MBUF_PKTLEN(hci.rx_acl)) {
-            os_mbuf_free_chain(hci.rx_acl);
-            hci.rx_type = H4_NONE;
-        }
-
-        break;
-    }
-
-    return rc;
-}
-
-static int
-uart_rx_pkt_type(uint8_t data)
-{
-    hci.tx_type = data;
-
-    switch (hci.tx_type) {
-    case H4_CMD:
-        hci.tx_cmd.data = os_memblock_get(&g_hci_cmd_pool);
-        hci.tx_cmd.len = 0;
-        hci.tx_cmd.cur = 0;
-        break;
-    case H4_ACL:
-        hci.tx_acl.buf = os_msys_get_pkthdr(HCI_ACL_HDR_LEN, 0);
-        hci.tx_acl.len = 0;
-        break;
-    default:
-        hci.tx_type = H4_NONE;
-        return -1;
-    }
-
-    return 0;
-}
-
-static int
-uart_rx_cmd(uint8_t data)
-{
-    int rc;
-
-    hci.tx_cmd.data[hci.tx_cmd.cur++] = data;
-
-    if (hci.tx_cmd.cur < HCI_CMD_HDR_LEN) {
-        return 0;
-    } else if (hci.tx_cmd.cur == HCI_CMD_HDR_LEN) {
-        hci.tx_cmd.len = hci.tx_cmd.data[2] + HCI_CMD_HDR_LEN;
-    }
-
-    if (hci.tx_cmd.cur == hci.tx_cmd.len) {
-        rc = ble_hci_transport_host_cmd_send(hci.tx_cmd.data);
-        if (rc != 0) {
-            os_memblock_put(&g_hci_cmd_pool, hci.tx_cmd.data);
-        }
-        hci.tx_type = H4_NONE;
-    }
-
-    return 0;
-}
-
-static int
-uart_rx_acl(uint8_t data)
-{
-    os_mbuf_append(hci.tx_acl.buf, &data, 1);
-
-    if (OS_MBUF_PKTLEN(hci.tx_acl.buf) < HCI_ACL_HDR_LEN) {
-        return 0;
-    } else if (OS_MBUF_PKTLEN(hci.tx_acl.buf) == HCI_ACL_HDR_LEN) {
-        os_mbuf_copydata(hci.tx_acl.buf, 2, sizeof(hci.tx_acl.len),
-                         &hci.tx_acl.len);
-        hci.tx_acl.len = le16toh(&hci.tx_acl.len) + HCI_ACL_HDR_LEN;
-    }
-
-    if (OS_MBUF_PKTLEN(hci.tx_acl.buf) == hci.tx_acl.len) {
-        ble_hci_transport_host_acl_data_send(hci.tx_acl.buf);
-        hci.tx_type = H4_NONE;
-    }
-
-    return 0;
-}
-
-static int
-uart_rx_char(void *arg, uint8_t data)
-{
-    switch (hci.tx_type) {
-    case H4_NONE:
-        return uart_rx_pkt_type(data);
-    case H4_CMD:
-        return uart_rx_cmd(data);
-    case H4_ACL:
-        return uart_rx_acl(data);
-    default:
-        return -1;
-    }
-}
-
-static int
-uart_init(void)
-{
-    int rc;
-
-    memset(&hci, 0, sizeof(hci));
-
-    STAILQ_INIT(&hci.rx_pkts);
-
-    rc = hal_uart_init_cbs(HCI_UART, uart_tx_char, NULL, uart_rx_char, NULL);
-    if (rc) {
-        return rc;
-    }
-
-    return hal_uart_config(HCI_UART, HCI_UART_SPEED, 8, 1, HAL_UART_PARITY_NONE,
-                           HAL_UART_FLOW_CTL_RTS_CTS);
-}
-
 int
 main(void)
 {
+    struct ble_hci_uart_cfg hci_cfg;
     int rc;
 
     /* Initialize OS */
@@ -366,25 +77,8 @@ main(void)
     rc = ble_ll_init(BLE_LL_TASK_PRI, MBUF_NUM_MBUFS, BLE_MBUF_PAYLOAD_SIZE);
     assert(rc == 0);
 
-    hci_cmd_buf = malloc(OS_MEMPOOL_BYTES(HCI_MAX_BUFS, HCI_CMD_BUF_SIZE));
-    assert(hci_cmd_buf != NULL);
-
-    /* Create memory pool of command buffers */
-    rc = os_mempool_init(&g_hci_cmd_pool, HCI_MAX_BUFS, HCI_CMD_BUF_SIZE,
-                         hci_cmd_buf, "HCICmdPool");
-    assert(rc == 0);
-
-    hci_os_event_buf = malloc(OS_MEMPOOL_BYTES(HCI_MAX_BUFS,
-                              HCI_OS_EVENT_BUF_SIZE));
-    assert(hci_os_event_buf != NULL);
-
-    /* Create memory pool of OS events */
-    rc = os_mempool_init(&g_hci_os_event_pool, HCI_MAX_BUFS,
-                         HCI_OS_EVENT_BUF_SIZE, hci_os_event_buf,
-                         "HCIOsEventPool");
-    assert(rc == 0);
-
-    rc = uart_init();
+    hci_cfg = ble_hci_uart_cfg_dflt;
+    rc = ble_hci_uart_init(&hci_cfg);
     assert(rc == 0);
 
     /* Start the OS */
diff --git a/apps/bleprph/pkg.yml b/apps/bleprph/pkg.yml
index df57ac26..cfc8e3ba 100644
--- a/apps/bleprph/pkg.yml
+++ b/apps/bleprph/pkg.yml
@@ -27,5 +27,22 @@ pkg.deps:
     - sys/log
     - net/nimble/controller
     - net/nimble/host
+    - net/nimble/host/services/gap
+    - net/nimble/host/services/gatt
+    - net/nimble/host/store/ram
+    - net/nimble/transport/ram
     - libs/console/full
     - libs/baselibc
+    - libs/newtmgr
+    - libs/newtmgr/transport/ble
+
+pkg.cflags:
+    # Use INFO log level to reduce code size.  DEBUG is too large for nRF51.
+    - "-DLOG_LEVEL=1"
+
+    # Disable unused roles; bleprph is a peripheral-only app.
+    - "-DNIMBLE_OPT_ROLE_OBSERVER=0"
+    - "-DNIMBLE_OPT_ROLE_CENTRAL=0"
+
+    # Disable unused eddystone feature.
+    - "-DNIMBLE_OPT_EDDYSTONE=0"
diff --git a/apps/bleprph/src/bleprph.h b/apps/bleprph/src/bleprph.h
index a353bac1..2e3c539c 100644
--- a/apps/bleprph/src/bleprph.h
+++ b/apps/bleprph/src/bleprph.h
@@ -21,18 +21,11 @@
 #define H_BLEPRPH_
 
 #include "log/log.h"
-union ble_store_value;
-union ble_store_key;
+struct ble_hs_cfg;
+struct ble_gatt_register_ctxt;
 
 extern struct log bleprph_log;
 
-extern const char *bleprph_device_name;
-extern const uint16_t bleprph_appearance;
-extern const uint8_t bleprph_privacy_flag;
-extern uint8_t bleprph_reconnect_addr[6];
-extern uint8_t bleprph_pref_conn_params[8];
-extern uint8_t bleprph_gatt_service_changed[4];
-
 /* bleprph uses the first "peruser" log module. */
 #define BLEPRPH_LOG_MODULE  (LOG_MODULE_PERUSER + 0)
 
@@ -47,19 +40,12 @@ extern uint8_t bleprph_gatt_service_changed[4];
 #define GATT_SVR_CHR_SUP_UNR_ALERT_CAT_UUID   0x2A48
 #define GATT_SVR_CHR_UNR_ALERT_STAT_UUID      0x2A45
 #define GATT_SVR_CHR_ALERT_NOT_CTRL_PT        0x2A44
-extern const uint8_t gatt_svr_svc_bleprph[16];
-extern const uint8_t gatt_svr_chr_bleprph_read[16];
-extern const uint8_t gatt_svr_chr_bleprph_write[16];
-
-void gatt_svr_init(void);
 
-/** Store. */
-int store_read(int obj_type, union ble_store_key *key,
-               union ble_store_value *dst);
-int store_write(int obj_type, union ble_store_value *val);
+void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg);
+int gatt_svr_init(struct ble_hs_cfg *cfg);
 
 /** Misc. */
-void print_bytes(uint8_t *bytes, int len);
-void print_addr(void *addr);
+void print_bytes(const uint8_t *bytes, int len);
+void print_addr(const void *addr);
 
 #endif
diff --git a/apps/bleprph/src/gatt_svr.c b/apps/bleprph/src/gatt_svr.c
index fe2a9ed3..061db76d 100644
--- a/apps/bleprph/src/gatt_svr.c
+++ b/apps/bleprph/src/gatt_svr.c
@@ -21,104 +21,50 @@
 #include <stdio.h>
 #include <string.h>
 #include "bsp/bsp.h"
-#include "console/console.h"
 #include "host/ble_hs.h"
 #include "bleprph.h"
 
 /**
- * The vendor specific "bleprph" service consists of two characteristics:
- *     o "read": a single-byte characteristic that can only be read of an
- *       encryptted connection.
- *     o "write": a single-byte characteristic that can always be read, but
- *       can only be written over an encrypted connection.
+ * The vendor specific security test service consists of two characteristics:
+ *     o random-number-generator: generates a random 32-bit number each time
+ *       it is read.  This characteristic can only be read over an encrypted
+ *       connection.
+ *     o static-value: a single-byte characteristic that can always be read,
+ *       but can only be written over an encrypted connection.
  */
 
 /* 59462f12-9543-9999-12c8-58b459a2712d */
-const uint8_t gatt_svr_svc_bleprph[16] = {
+const uint8_t gatt_svr_svc_sec_test_uuid[16] = {
     0x2d, 0x71, 0xa2, 0x59, 0xb4, 0x58, 0xc8, 0x12,
     0x99, 0x99, 0x43, 0x95, 0x12, 0x2f, 0x46, 0x59
 };
 
 /* 5c3a659e-897e-45e1-b016-007107c96df6 */
-const uint8_t gatt_svr_chr_bleprph_read[16] = {
+const uint8_t gatt_svr_chr_sec_test_rand_uuid[16] = {
     0xf6, 0x6d, 0xc9, 0x07, 0x71, 0x00, 0x16, 0xb0,
     0xe1, 0x45, 0x7e, 0x89, 0x9e, 0x65, 0x3a, 0x5c
 };
 
 /* 5c3a659e-897e-45e1-b016-007107c96df7 */
-const uint8_t gatt_svr_chr_bleprph_write[16] = {
+const uint8_t gatt_svr_chr_sec_test_static_uuid[16] = {
     0xf7, 0x6d, 0xc9, 0x07, 0x71, 0x00, 0x16, 0xb0,
     0xe1, 0x45, 0x7e, 0x89, 0x9e, 0x65, 0x3a, 0x5c
 };
 
-static uint8_t gatt_svr_nimble_test_read_val;
-static uint8_t gatt_svr_nimble_test_write_val;
+static uint8_t gatt_svr_sec_test_static_val;
 
 static int
-gatt_svr_chr_access_gap(uint16_t conn_handle, uint16_t attr_handle, uint8_t op,
-                        union ble_gatt_access_ctxt *ctxt, void *arg);
-static int
-gatt_svr_chr_access_gatt(uint16_t conn_handle, uint16_t attr_handle, uint8_t op,
-                         union ble_gatt_access_ctxt *ctxt, void *arg);
-static int
 gatt_svr_chr_access_alert(uint16_t conn_handle, uint16_t attr_handle,
-                          uint8_t op, union ble_gatt_access_ctxt *ctxt,
+                          struct ble_gatt_access_ctxt *ctxt,
                           void *arg);
 
 static int
-gatt_svr_chr_access_bleprph(uint16_t conn_handle, uint16_t attr_handle,
-                                uint8_t op, union ble_gatt_access_ctxt *ctxt,
-                                void *arg);
+gatt_svr_chr_access_sec_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_svcs[] = {
     {
-        /*** Service: GAP. */
-        .type = BLE_GATT_SVC_TYPE_PRIMARY,
-        .uuid128 = BLE_UUID16(BLE_GAP_SVC_UUID16),
-        .characteristics = (struct ble_gatt_chr_def[]) { {
-            /*** Characteristic: Device Name. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_DEVICE_NAME),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_READ,
-        }, {
-            /*** Characteristic: Appearance. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_APPEARANCE),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_READ,
-        }, {
-            /*** Characteristic: Peripheral Privacy Flag. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_PERIPH_PRIV_FLAG),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_READ,
-        }, {
-            /*** Characteristic: Reconnection Address. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_RECONNECT_ADDR),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_WRITE,
-        }, {
-            /*** Characteristic: Peripheral Preferred Connection Parameters. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_READ,
-        }, {
-            0, /* No more characteristics in this service. */
-        } },
-    },
-
-    {
-        /*** Service: GATT */
-        .type = BLE_GATT_SVC_TYPE_PRIMARY,
-        .uuid128 = BLE_UUID16(BLE_GATT_SVC_UUID16),
-        .characteristics = (struct ble_gatt_chr_def[]) { {
-            .uuid128 = BLE_UUID16(BLE_GATT_CHR_SERVICE_CHANGED_UUID16),
-            .access_cb = gatt_svr_chr_access_gatt,
-            .flags = BLE_GATT_CHR_F_INDICATE,
-        }, {
-            0, /* No more characteristics in this service. */
-        } },
-    },
-
-    {
         /*** Alert Notification Service. */
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(GATT_SVR_SVC_ALERT_UUID),
@@ -148,18 +94,18 @@ static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
     },
 
     {
-        /*** Service: bleprph. */
+        /*** Service: Security test. */
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
-        .uuid128 = (void *)gatt_svr_svc_bleprph,
+        .uuid128 = gatt_svr_svc_sec_test_uuid,
         .characteristics = (struct ble_gatt_chr_def[]) { {
-            /*** Characteristic: Read. */
-            .uuid128 = (void *)gatt_svr_chr_bleprph_read,
-            .access_cb = gatt_svr_chr_access_bleprph,
+            /*** Characteristic: Random number generator. */
+            .uuid128 = gatt_svr_chr_sec_test_rand_uuid,
+            .access_cb = gatt_svr_chr_access_sec_test,
             .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_READ_ENC,
         }, {
-            /*** Characteristic: Write. */
-            .uuid128 = (void *)gatt_svr_chr_bleprph_write,
-            .access_cb = gatt_svr_chr_access_bleprph,
+            /*** Characteristic: Static value. */
+            .uuid128 = gatt_svr_chr_sec_test_static_uuid,
+            .access_cb = gatt_svr_chr_access_sec_test,
             .flags = BLE_GATT_CHR_F_READ |
                      BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_ENC,
         }, {
@@ -173,100 +119,20 @@ static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
 };
 
 static int
-gatt_svr_chr_write(uint8_t op, union ble_gatt_access_ctxt *ctxt,
-                   uint16_t min_len, uint16_t max_len, void *dst,
-                   uint16_t *len)
+gatt_svr_chr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len,
+                   void *dst, uint16_t *len)
 {
-    assert(op == BLE_GATT_ACCESS_OP_WRITE_CHR);
-    if (ctxt->chr_access.len < min_len ||
-        ctxt->chr_access.len > max_len) {
-        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
-    }
-    memcpy(dst, ctxt->chr_access.data, ctxt->chr_access.len);
-    if (len != NULL) {
-        *len = ctxt->chr_access.len;
-    }
-
-    return 0;
-}
-
-static int
-gatt_svr_chr_access_gap(uint16_t conn_handle, uint16_t attr_handle, uint8_t op,
-                        union ble_gatt_access_ctxt *ctxt, void *arg)
-{
-    uint16_t uuid16;
-
-    uuid16 = ble_uuid_128_to_16(ctxt->chr_access.chr->uuid128);
-    assert(uuid16 != 0);
-
-    switch (uuid16) {
-    case BLE_GAP_CHR_UUID16_DEVICE_NAME:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)bleprph_device_name;
-        ctxt->chr_access.len = strlen(bleprph_device_name);
-        break;
-
-    case BLE_GAP_CHR_UUID16_APPEARANCE:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&bleprph_appearance;
-        ctxt->chr_access.len = sizeof bleprph_appearance;
-        break;
-
-    case BLE_GAP_CHR_UUID16_PERIPH_PRIV_FLAG:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&bleprph_privacy_flag;
-        ctxt->chr_access.len = sizeof bleprph_privacy_flag;
-        break;
-
-    case BLE_GAP_CHR_UUID16_RECONNECT_ADDR:
-        assert(op == BLE_GATT_ACCESS_OP_WRITE_CHR);
-        if (ctxt->chr_access.len != sizeof bleprph_reconnect_addr) {
-            return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
-        }
-        memcpy(bleprph_reconnect_addr, ctxt->chr_access.data,
-               sizeof bleprph_reconnect_addr);
-        break;
-
-    case BLE_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&bleprph_pref_conn_params;
-        ctxt->chr_access.len = sizeof bleprph_pref_conn_params;
-        break;
+    uint16_t om_len;
+    int rc;
 
-    default:
-        assert(0);
-        break;
+    om_len = OS_MBUF_PKTLEN(om);
+    if (om_len < min_len || om_len > max_len) {
+        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
     }
 
-    return 0;
-}
-
-static int
-gatt_svr_chr_access_gatt(uint16_t conn_handle, uint16_t attr_handle, uint8_t op,
-                         union ble_gatt_access_ctxt *ctxt, void *arg)
-{
-    uint16_t uuid16;
-
-    uuid16 = ble_uuid_128_to_16(ctxt->chr_access.chr->uuid128);
-    assert(uuid16 != 0);
-
-    switch (uuid16) {
-    case BLE_GATT_CHR_SERVICE_CHANGED_UUID16:
-        if (op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            if (ctxt->chr_access.len != sizeof bleprph_gatt_service_changed) {
-                return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
-            }
-            memcpy(bleprph_gatt_service_changed, ctxt->chr_access.data,
-                   sizeof bleprph_gatt_service_changed);
-        } else if (op == BLE_GATT_ACCESS_OP_READ_CHR) {
-            ctxt->chr_access.data = (void *)&bleprph_gatt_service_changed;
-            ctxt->chr_access.len = sizeof bleprph_gatt_service_changed;
-        }
-        break;
-
-    default:
-        assert(0);
-        break;
+    rc = ble_hs_mbuf_to_flat(om, dst, max_len, len);
+    if (rc != 0) {
+        return BLE_ATT_ERR_UNLIKELY;
     }
 
     return 0;
@@ -283,54 +149,55 @@ static uint16_t gatt_svr_alert_not_ctrl_pt;
 
 static int
 gatt_svr_chr_access_alert(uint16_t conn_handle, uint16_t attr_handle,
-                          uint8_t op, union ble_gatt_access_ctxt *ctxt,
+                          struct ble_gatt_access_ctxt *ctxt,
                           void *arg)
 {
     uint16_t uuid16;
     int rc;
 
-    uuid16 = ble_uuid_128_to_16(ctxt->chr_access.chr->uuid128);
+    uuid16 = ble_uuid_128_to_16(ctxt->chr->uuid128);
     assert(uuid16 != 0);
 
     switch (uuid16) {
     case GATT_SVR_CHR_SUP_NEW_ALERT_CAT_UUID:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&gatt_svr_new_alert_cat;
-        ctxt->chr_access.len = sizeof gatt_svr_new_alert_cat;
-        return 0;
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &gatt_svr_new_alert_cat,
+                            sizeof gatt_svr_new_alert_cat);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
 
     case GATT_SVR_CHR_NEW_ALERT:
-        if (op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            rc = gatt_svr_chr_write(op, ctxt, 0, sizeof gatt_svr_new_alert_val,
-                                  gatt_svr_new_alert_val,
-                                  &gatt_svr_new_alert_val_len);
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = gatt_svr_chr_write(ctxt->om, 0,
+                                    sizeof gatt_svr_new_alert_val,
+                                    gatt_svr_new_alert_val,
+                                    &gatt_svr_new_alert_val_len);
             return rc;
-        } else if (op == BLE_GATT_ACCESS_OP_READ_CHR) {
-            ctxt->chr_access.data = (void *)&gatt_svr_new_alert_val;
-            ctxt->chr_access.len = sizeof gatt_svr_new_alert_val;
-            return 0;
+        } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+            rc = os_mbuf_append(ctxt->om, &gatt_svr_new_alert_val,
+                                sizeof gatt_svr_new_alert_val);
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
         }
 
     case GATT_SVR_CHR_SUP_UNR_ALERT_CAT_UUID:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&gatt_svr_unr_alert_cat;
-        ctxt->chr_access.len = sizeof gatt_svr_unr_alert_cat;
-        return 0;
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &gatt_svr_unr_alert_cat,
+                            sizeof gatt_svr_unr_alert_cat);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
 
     case GATT_SVR_CHR_UNR_ALERT_STAT_UUID:
-        if (op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            rc = gatt_svr_chr_write(op, ctxt, 2, 2, &gatt_svr_unr_alert_stat,
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = gatt_svr_chr_write(ctxt->om, 2, 2, &gatt_svr_unr_alert_stat,
                                     NULL);
+            return rc;
         } else {
-            ctxt->chr_access.data = (void *)&gatt_svr_unr_alert_stat;
-            ctxt->chr_access.len = sizeof gatt_svr_unr_alert_stat;
-            rc = 0;
+            rc = os_mbuf_append(ctxt->om, &gatt_svr_unr_alert_stat,
+                                sizeof gatt_svr_unr_alert_stat);
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
         }
-        return rc;
 
     case GATT_SVR_CHR_ALERT_NOT_CTRL_PT:
-        if (op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            rc = gatt_svr_chr_write(op, ctxt, 2, 2,
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = gatt_svr_chr_write(ctxt->om, 2, 2,
                                     &gatt_svr_alert_not_ctrl_pt, NULL);
         } else {
             rc = BLE_ATT_ERR_UNLIKELY;
@@ -344,38 +211,41 @@ gatt_svr_chr_access_alert(uint16_t conn_handle, uint16_t attr_handle,
 }
 
 static int
-gatt_svr_chr_access_bleprph(uint16_t conn_handle, uint16_t attr_handle,
-                            uint8_t op, union ble_gatt_access_ctxt *ctxt,
-                            void *arg)
+gatt_svr_chr_access_sec_test(uint16_t conn_handle, uint16_t attr_handle,
+                             struct ble_gatt_access_ctxt *ctxt,
+                             void *arg)
 {
-    void *uuid128;
+    const void *uuid128;
+    int rand_num;
     int rc;
 
-    uuid128 = ctxt->chr_access.chr->uuid128;
+    uuid128 = ctxt->chr->uuid128;
 
     /* Determine which characteristic is being accessed by examining its
      * 128-bit UUID.
      */
 
-    if (memcmp(uuid128, gatt_svr_chr_bleprph_read, 16) == 0) {
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = &gatt_svr_nimble_test_read_val;
-        ctxt->chr_access.len = sizeof gatt_svr_nimble_test_read_val;
-        return 0;
+    if (memcmp(uuid128, gatt_svr_chr_sec_test_rand_uuid, 16) == 0) {
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+
+        /* Respond with a 32-bit random number. */
+        rand_num = rand();
+        rc = os_mbuf_append(ctxt->om, &rand_num, sizeof rand_num);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
     }
 
-    if (memcmp(uuid128, gatt_svr_chr_bleprph_write, 16) == 0) {
-        switch (op) {
+    if (memcmp(uuid128, gatt_svr_chr_sec_test_static_uuid, 16) == 0) {
+        switch (ctxt->op) {
         case BLE_GATT_ACCESS_OP_READ_CHR:
-            ctxt->chr_access.data = &gatt_svr_nimble_test_write_val;
-            ctxt->chr_access.len = sizeof gatt_svr_nimble_test_write_val;
-            return 0;
+            rc = os_mbuf_append(ctxt->om, &gatt_svr_sec_test_static_val,
+                                sizeof gatt_svr_sec_test_static_val);
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
 
         case BLE_GATT_ACCESS_OP_WRITE_CHR:
-            rc = gatt_svr_chr_write(op, ctxt,
-                                    sizeof gatt_svr_nimble_test_write_val,
-                                    sizeof gatt_svr_nimble_test_write_val,
-                                    &gatt_svr_nimble_test_write_val, NULL);
+            rc = gatt_svr_chr_write(ctxt->om,
+                                    sizeof gatt_svr_sec_test_static_val,
+                                    sizeof gatt_svr_sec_test_static_val,
+                                    &gatt_svr_sec_test_static_val, NULL);
             return rc;
 
         default:
@@ -392,10 +262,10 @@ gatt_svr_chr_access_bleprph(uint16_t conn_handle, uint16_t attr_handle,
 }
 
 static char *
-gatt_svr_uuid128_to_s(void *uuid128, char *dst)
+gatt_svr_uuid_to_s(const void *uuid128, char *dst)
 {
+    const uint8_t *u8p;
     uint16_t uuid16;
-    uint8_t *u8p;
 
     uuid16 = ble_uuid_128_to_16(uuid128);
     if (uuid16 != 0) {
@@ -413,32 +283,30 @@ gatt_svr_uuid128_to_s(void *uuid128, char *dst)
     return dst;
 }
 
-static void
-gatt_svr_register_cb(uint8_t op, union ble_gatt_register_ctxt *ctxt, void *arg)
+void
+gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
 {
     char buf[40];
 
-    switch (op) {
+    switch (ctxt->op) {
     case BLE_GATT_REGISTER_OP_SVC:
         BLEPRPH_LOG(DEBUG, "registered service %s with handle=%d\n",
-                    gatt_svr_uuid128_to_s(ctxt->svc_reg.svc->uuid128, buf),
-                    ctxt->svc_reg.handle);
+                    gatt_svr_uuid_to_s(ctxt->svc.svc_def->uuid128, buf),
+                    ctxt->svc.handle);
         break;
 
     case BLE_GATT_REGISTER_OP_CHR:
         BLEPRPH_LOG(DEBUG, "registering characteristic %s with "
                            "def_handle=%d val_handle=%d\n",
-                    gatt_svr_uuid128_to_s(ctxt->chr_reg.chr->uuid128, buf),
-                    ctxt->chr_reg.def_handle,
-                    ctxt->chr_reg.val_handle);
+                    gatt_svr_uuid_to_s(ctxt->chr.chr_def->uuid128, buf),
+                    ctxt->chr.def_handle,
+                    ctxt->chr.val_handle);
         break;
 
     case BLE_GATT_REGISTER_OP_DSC:
-        BLEPRPH_LOG(DEBUG, "registering descriptor %s with handle=%d "
-                           "chr_handle=%d\n",
-                    gatt_svr_uuid128_to_s(ctxt->dsc_reg.dsc->uuid128, buf),
-                    ctxt->dsc_reg.dsc_handle,
-                    ctxt->dsc_reg.chr_def_handle);
+        BLEPRPH_LOG(DEBUG, "registering descriptor %s with handle=%d\n",
+                    gatt_svr_uuid_to_s(ctxt->dsc.dsc_def->uuid128, buf),
+                    ctxt->dsc.handle);
         break;
 
     default:
@@ -447,11 +315,20 @@ gatt_svr_register_cb(uint8_t op, union ble_gatt_register_ctxt *ctxt, void *arg)
     }
 }
 
-void
-gatt_svr_init(void)
+int
+gatt_svr_init(struct ble_hs_cfg *cfg)
 {
     int rc;
 
-    rc = ble_gatts_register_svcs(gatt_svr_svcs, gatt_svr_register_cb, NULL);
-    assert(rc == 0);
+    rc = ble_gatts_count_cfg(gatt_svr_svcs, cfg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_gatts_add_svcs(gatt_svr_svcs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
 }
diff --git a/apps/bleprph/src/main.c b/apps/bleprph/src/main.c
index 69a4c7a6..0233bc15 100755
--- a/apps/bleprph/src/main.c
+++ b/apps/bleprph/src/main.c
@@ -27,10 +27,10 @@
 #include "hal/hal_gpio.h"
 #include "hal/hal_cputime.h"
 #include "console/console.h"
+#include <imgmgr/imgmgr.h>
 
 /* BLE */
 #include "nimble/ble.h"
-#include "host/host_hci.h"
 #include "host/ble_hs.h"
 #include "host/ble_hs_adv.h"
 #include "host/ble_uuid.h"
@@ -41,9 +41,22 @@
 #include "host/ble_sm.h"
 #include "controller/ble_ll.h"
 
-#include "bleprph.h"
+/* RAM HCI transport. */
+#include "transport/ram/ble_hci_ram.h"
+
+/* RAM persistence layer. */
+#include "store/ram/ble_store_ram.h"
+
+/* Mandatory services. */
+#include "services/gap/ble_svc_gap.h"
+#include "services/gatt/ble_svc_gatt.h"
+
+/* Newtmgr include */
+#include "newtmgr/newtmgr.h"
+#include "nmgrble/newtmgr_ble.h"
 
-#define BSWAP16(x)  ((uint16_t)(((x) << 8) | (((x) & 0xff00) >> 8)))
+/* Application-specified header. */
+#include "bleprph.h"
 
 /** Mbuf settings. */
 #define MBUF_NUM_MBUFS      (12)
@@ -66,6 +79,10 @@ struct log bleprph_log;
 #define BLEPRPH_TASK_PRIO           1
 #define BLEPRPH_STACK_SIZE          (OS_STACK_ALIGN(336))
 
+#define NEWTMGR_TASK_PRIO (4)
+#define NEWTMGR_TASK_STACK_SIZE (OS_STACK_ALIGN(512))
+os_stack_t newtmgr_stack[NEWTMGR_TASK_STACK_SIZE];
+
 struct os_eventq bleprph_evq;
 struct os_task bleprph_task;
 bssnz_t os_stack_t bleprph_stack[BLEPRPH_STACK_SIZE];
@@ -76,18 +93,7 @@ uint8_t g_dev_addr[BLE_DEV_ADDR_LEN] = {0x0a, 0x0a, 0x0a, 0x0a, 0x0a, 0x0a};
 /** Our random address (in case we need it) */
 uint8_t g_random_addr[BLE_DEV_ADDR_LEN];
 
-/** Device name - included in advertisements and exposed by GAP service. */
-const char *bleprph_device_name = "nimble-bleprph";
-
-/** Device properties - exposed by GAP service. */
-const uint16_t bleprph_appearance = BSWAP16(BLE_GAP_APPEARANCE_GEN_COMPUTER);
-const uint8_t bleprph_privacy_flag = 0;
-uint8_t bleprph_reconnect_addr[6];
-uint8_t bleprph_pref_conn_params[8];
-uint8_t bleprph_gatt_service_changed[4];
-
-static int bleprph_gap_event(int event, struct ble_gap_conn_ctxt *ctxt,
-                             void *arg);
+static int bleprph_gap_event(struct ble_gap_event *event, void *arg);
 
 /**
  * Logs information about a connection to the console.
@@ -124,11 +130,14 @@ bleprph_print_conn_desc(struct ble_gap_conn_desc *desc)
 static void
 bleprph_advertise(void)
 {
+    struct ble_gap_adv_params adv_params;
     struct ble_hs_adv_fields fields;
+    const char *name;
     int rc;
 
     /**
      *  Set the advertisement data included in our advertisements:
+     *     o Flags (indicates advertisement type and other general info).
      *     o Advertising tx power.
      *     o Device name.
      *     o 16-bit service UUIDs (alert notifications).
@@ -136,10 +145,22 @@ bleprph_advertise(void)
 
     memset(&fields, 0, sizeof fields);
 
+    /* Indicate that the flags field should be included; specify a value of 0
+     * to instruct the stack to fill the value in for us.
+     */
+    fields.flags_is_present = 1;
+    fields.flags = 0;
+
+    /* Indicate that the TX power level field should be included; have the
+     * stack fill this one automatically as well.  This is done by assiging the
+     * special value BLE_HS_ADV_TX_PWR_LVL_AUTO.
+     */
     fields.tx_pwr_lvl_is_present = 1;
+    fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
 
-    fields.name = (uint8_t *)bleprph_device_name;
-    fields.name_len = strlen(bleprph_device_name);
+    name = ble_svc_gap_device_name();
+    fields.name = (uint8_t *)name;
+    fields.name_len = strlen(name);
     fields.name_is_complete = 1;
 
     fields.uuids16 = (uint16_t[]){ GATT_SVR_SVC_ALERT_UUID };
@@ -153,8 +174,11 @@ bleprph_advertise(void)
     }
 
     /* Begin advertising. */
-    rc = ble_gap_adv_start(BLE_GAP_DISC_MODE_GEN, BLE_GAP_CONN_MODE_UND,
-                           NULL, 0, NULL, bleprph_gap_event, NULL);
+    memset(&adv_params, 0, sizeof adv_params);
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_UND;
+    adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN;
+    rc = ble_gap_adv_start(BLE_ADDR_TYPE_PUBLIC, 0, NULL, BLE_HS_FOREVER,
+                           &adv_params, bleprph_gap_event, NULL);
     if (rc != 0) {
         BLEPRPH_LOG(ERROR, "error enabling advertisement; rc=%d\n", rc);
         return;
@@ -177,26 +201,33 @@ bleprph_advertise(void)
  *                                  particular GAP event being signalled.
  */
 static int
-bleprph_gap_event(int event, struct ble_gap_conn_ctxt *ctxt, void *arg)
+bleprph_gap_event(struct ble_gap_event *event, void *arg)
 {
-    switch (event) {
+    struct ble_gap_conn_desc desc;
+    int rc;
+
+    switch (event->type) {
     case BLE_GAP_EVENT_CONNECT:
         /* A new connection was established or a connection attempt failed. */
         BLEPRPH_LOG(INFO, "connection %s; status=%d ",
-                       ctxt->connect.status == 0 ? "established" : "failed",
-                       ctxt->connect.status);
-        bleprph_print_conn_desc(ctxt->desc);
+                       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);
+            bleprph_print_conn_desc(&desc);
+        }
         BLEPRPH_LOG(INFO, "\n");
 
-        if (ctxt->connect.status != 0) {
+        if (event->connect.status != 0) {
             /* Connection failed; resume advertising. */
             bleprph_advertise();
         }
         return 0;
 
     case BLE_GAP_EVENT_DISCONNECT:
-        BLEPRPH_LOG(INFO, "disconnect; reason=%d ", ctxt->disconnect.reason);
-        bleprph_print_conn_desc(ctxt->desc);
+        BLEPRPH_LOG(INFO, "disconnect; reason=%d ", event->disconnect.reason);
+        bleprph_print_conn_desc(&event->disconnect.conn);
         BLEPRPH_LOG(INFO, "\n");
 
         /* Connection terminated; resume advertising. */
@@ -206,23 +237,59 @@ bleprph_gap_event(int event, struct ble_gap_conn_ctxt *ctxt, void *arg)
     case BLE_GAP_EVENT_CONN_UPDATE:
         /* The central has updated the connection parameters. */
         BLEPRPH_LOG(INFO, "connection updated; status=%d ",
-                    ctxt->conn_update.status);
-        bleprph_print_conn_desc(ctxt->desc);
+                    event->conn_update.status);
+        rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
+        assert(rc == 0);
+        bleprph_print_conn_desc(&desc);
         BLEPRPH_LOG(INFO, "\n");
         return 0;
 
     case BLE_GAP_EVENT_ENC_CHANGE:
         /* Encryption has been enabled or disabled for this connection. */
         BLEPRPH_LOG(INFO, "encryption change event; status=%d ",
-                    ctxt->enc_change.status);
-        bleprph_print_conn_desc(ctxt->desc);
+                    event->enc_change.status);
+        rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
+        assert(rc == 0);
+        bleprph_print_conn_desc(&desc);
         BLEPRPH_LOG(INFO, "\n");
         return 0;
+
+    case BLE_GAP_EVENT_SUBSCRIBE:
+        BLEPRPH_LOG(INFO, "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);
+        return 0;
+
+    case BLE_GAP_EVENT_MTU:
+        BLEPRPH_LOG(INFO, "mtu update event; conn_handle=%d cid=%d mtu=%d\n",
+                    event->mtu.conn_handle,
+                    event->mtu.channel_id,
+                    event->mtu.value);
+        return 0;
     }
 
     return 0;
 }
 
+static void
+bleprph_on_reset(int reason)
+{
+    BLEPRPH_LOG(ERROR, "Resetting state; reason=%d\n", reason);
+}
+
+static void
+bleprph_on_sync(void)
+{
+    /* Begin advertising. */
+    bleprph_advertise();
+}
+
 /**
  * Event loop for the main bleprph task.
  */
@@ -233,14 +300,20 @@ bleprph_task_handler(void *unused)
     struct os_callout_func *cf;
     int rc;
 
+    /* Activate the host.  This causes the host to synchronize with the
+     * controller.
+     */
     rc = ble_hs_start();
-    assert(rc == 0);
-
-    /* Begin advertising. */
-    bleprph_advertise();
 
     while (1) {
         ev = os_eventq_get(&bleprph_evq);
+
+        /* Check if the event is a nmgr ble mqueue event */
+        rc = nmgr_ble_proc_mq_evt(ev);
+        if (!rc) {
+            continue;
+        }
+
         switch (ev->ev_type) {
         case OS_EVENT_T_TIMER:
             cf = (struct os_callout_func *)ev;
@@ -266,6 +339,7 @@ bleprph_task_handler(void *unused)
 int
 main(void)
 {
+    struct ble_hci_ram_cfg hci_cfg;
     struct ble_hs_cfg cfg;
     uint32_t seed;
     int rc;
@@ -301,11 +375,21 @@ main(void)
     rc = os_msys_register(&bleprph_mbuf_pool);
     assert(rc == 0);
 
+    /* Initialize the console (for log output). */
+    rc = console_init(NULL);
+    assert(rc == 0);
+
     /* Initialize the logging system. */
     log_init();
     log_console_handler_init(&bleprph_log_console_handler);
     log_register("bleprph", &bleprph_log, &bleprph_log_console_handler);
 
+    /* Initialize eventq */
+    os_eventq_init(&bleprph_evq);
+
+    /* Create the bleprph task.  All application logic and NimBLE host
+     * operations are performed in this task.
+     */
     os_task_init(&bleprph_task, "bleprph", bleprph_task_handler,
                  NULL, BLEPRPH_TASK_PRIO, OS_WAIT_FOREVER,
                  bleprph_stack, BLEPRPH_STACK_SIZE);
@@ -314,36 +398,47 @@ main(void)
     rc = ble_ll_init(BLE_LL_TASK_PRI, MBUF_NUM_MBUFS, BLE_MBUF_PAYLOAD_SIZE);
     assert(rc == 0);
 
-    /* Initialize the BLE host. */
+    /* Initialize the RAM HCI transport. */
+    hci_cfg = ble_hci_ram_cfg_dflt;
+    rc = ble_hci_ram_init(&hci_cfg);
+    assert(rc == 0);
+
+    /* Initialize the NimBLE host configuration. */
     cfg = ble_hs_cfg_dflt;
-    cfg.max_hci_bufs = 3;
-    cfg.max_connections = 1;
-    cfg.max_attrs = 42;
-    cfg.max_services = 5;
-    cfg.max_client_configs = 6;
+    cfg.max_hci_bufs = hci_cfg.num_evt_hi_bufs + hci_cfg.num_evt_lo_bufs;
     cfg.max_gattc_procs = 2;
-    cfg.max_l2cap_chans = 3;
-    cfg.max_l2cap_sig_procs = 1;
     cfg.sm_bonding = 1;
     cfg.sm_our_key_dist = BLE_SM_PAIR_KEY_DIST_ENC;
     cfg.sm_their_key_dist = BLE_SM_PAIR_KEY_DIST_ENC;
-    cfg.store_read_cb = store_read;
-    cfg.store_write_cb = store_write;
+    cfg.reset_cb = bleprph_on_reset;
+    cfg.sync_cb = bleprph_on_sync;
+    cfg.store_read_cb = ble_store_ram_read;
+    cfg.store_write_cb = ble_store_ram_write;
+    cfg.gatts_register_cb = gatt_svr_register_cb;
+
+    /* Initialize GATT services. */
+    rc = ble_svc_gap_init(&cfg);
+    assert(rc == 0);
 
-    /* Initialize eventq */
-    os_eventq_init(&bleprph_evq);
+    rc = ble_svc_gatt_init(&cfg);
+    assert(rc == 0);
 
-    rc = ble_hs_init(&bleprph_evq, &cfg);
+    rc = nmgr_ble_gatt_svr_init(&bleprph_evq, &cfg);
     assert(rc == 0);
 
-    /* Initialize the console (for log output). */
-    rc = console_init(NULL);
+    rc = gatt_svr_init(&cfg);
     assert(rc == 0);
 
-    /* Register GATT attributes (services, characteristics, and
-     * descriptors).
-     */
-    gatt_svr_init();
+    /* Initialize NimBLE host. */
+    rc = ble_hs_init(&bleprph_evq, &cfg);
+    assert(rc == 0);
+
+    nmgr_task_init(NEWTMGR_TASK_PRIO, newtmgr_stack, NEWTMGR_TASK_STACK_SIZE);
+    imgmgr_module_init();
+
+    /* Set the default device name. */
+    rc = ble_svc_gap_device_name_set("nimble-bleprph");
+    assert(rc == 0);
 
     /* Start the OS */
     os_start();
diff --git a/apps/bleprph/src/misc.c b/apps/bleprph/src/misc.c
index d5d4808f..7e27765a 100644
--- a/apps/bleprph/src/misc.c
+++ b/apps/bleprph/src/misc.c
@@ -23,7 +23,7 @@
  * Utility function to log an array of bytes.
  */
 void
-print_bytes(uint8_t *bytes, int len)
+print_bytes(const uint8_t *bytes, int len)
 {
     int i;
 
@@ -33,9 +33,9 @@ print_bytes(uint8_t *bytes, int len)
 }
 
 void
-print_addr(void *addr)
+print_addr(const void *addr)
 {
-    uint8_t *u8p;
+    const uint8_t *u8p;
 
     u8p = addr;
     BLEPRPH_LOG(INFO, "%02x:%02x:%02x:%02x:%02x:%02x",
diff --git a/apps/bletest/pkg.yml b/apps/bletest/pkg.yml
index 84b6e711..3e94ede4 100644
--- a/apps/bletest/pkg.yml
+++ b/apps/bletest/pkg.yml
@@ -28,6 +28,7 @@ pkg.deps:
     - fs/nffs
     - net/nimble/controller
     - net/nimble/host
+    - net/nimble/transport/ram
     - libs/os 
     - libs/console/full
     - libs/shell
diff --git a/apps/bletest/src/bletest_hci.c b/apps/bletest/src/bletest_hci.c
index 9a692bd7..354b5685 100755
--- a/apps/bletest/src/bletest_hci.c
+++ b/apps/bletest/src/bletest_hci.c
@@ -24,9 +24,8 @@
 
 /* BLE */
 #include "nimble/ble.h"
-#include "nimble/hci_transport.h"
+#include "nimble/ble_hci_trans.h"
 #include "nimble/hci_common.h"
-#include "host/host_hci.h"
 #include "host/ble_hs.h"
 #include "controller/ble_ll.h"
 #include "controller/ble_ll_hci.h"
@@ -37,7 +36,6 @@
 /* XXX: An app should not include private headers from a library.  The bletest
  * app uses some of nimble's internal details for logging.
  */
-#include "../src/ble_hci_util_priv.h"
 #include "../src/ble_hs_priv.h"
 #include "bletest_priv.h"
 
@@ -57,7 +55,7 @@ bletest_send_conn_update(uint16_t handle)
     hcu.min_ce_len = 4;
     hcu.max_ce_len = 4;
 
-    rc = host_hci_cmd_le_conn_update(&hcu);
+    rc = ble_hs_hci_cmd_le_conn_update(&hcu);
     assert(rc == 0);
 }
 
@@ -65,7 +63,7 @@ bletest_send_conn_update(uint16_t handle)
 void
 bletest_ltk_req_reply(uint16_t handle)
 {
-    g_bletest_ltk_reply_handle = handle;
+    g_bletest_ltk_reply_handle |= (1 << (handle-1));
 }
 
 int
@@ -78,12 +76,12 @@ bletest_send_ltk_req_neg_reply(uint16_t handle)
     uint8_t rsplen;
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY,
                        sizeof(uint16_t), dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     htole16(dst, handle);
-    rc = ble_hci_cmd_tx(buf, &ack_conn_handle, 2, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, &ack_conn_handle, 2, &rsplen);
     if (rc == 0) {
         if (rsplen != 2) {
             rc = -1;
@@ -105,8 +103,8 @@ bletest_send_ltk_req_reply(uint16_t handle)
     hkr.conn_handle = handle;
     swap_buf(hkr.long_term_key, (uint8_t *)g_bletest_LTK, 16);
 
-    host_hci_cmd_build_le_lt_key_req_reply(&hkr, buf, sizeof buf);
-    rc = ble_hci_cmd_tx(buf, &ack_conn_handle, sizeof ack_conn_handle,
+    ble_hs_hci_cmd_build_le_lt_key_req_reply(&hkr, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx(buf, &ack_conn_handle, sizeof ack_conn_handle,
                         &ack_params_len);
     if (rc != 0) {
         return rc;
@@ -128,8 +126,9 @@ bletest_hci_reset_ctlr(void)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN];
 
-    host_hci_write_hdr(BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_RESET, 0, buf);
-    return ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_RESET,
+                             0, buf);
+    return ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
 }
 
 int
@@ -140,9 +139,9 @@ bletest_hci_rd_bd_addr(void)
     uint8_t rspbuf[BLE_DEV_ADDR_LEN];
     uint8_t rsplen;
 
-    host_hci_write_hdr(BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_BD_ADDR, 0,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_BD_ADDR, 0,
                        buf);
-    rc = ble_hci_cmd_tx(buf, rspbuf, BLE_DEV_ADDR_LEN, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_DEV_ADDR_LEN, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -165,13 +164,13 @@ bletest_hci_le_encrypt(uint8_t *key, uint8_t *pt)
     uint8_t rsplen;
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ENCRYPT,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ENCRYPT,
                        BLE_HCI_LE_ENCRYPT_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     swap_buf(dst, key, BLE_ENC_BLOCK_SIZE);
     swap_buf(dst + BLE_ENC_BLOCK_SIZE, pt, BLE_ENC_BLOCK_SIZE);
-    rc = ble_hci_cmd_tx(buf, rspbuf, 16, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, 16, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -193,14 +192,14 @@ bletest_hci_le_set_datalen(uint16_t handle, uint16_t txoctets, uint16_t txtime)
     uint8_t rsplen;
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_DATA_LEN,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_DATA_LEN,
                        BLE_HCI_SET_DATALEN_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     htole16(dst, handle);
     htole16(dst + 2, txoctets);
     htole16(dst + 4, txtime);
-    rc = ble_hci_cmd_tx(buf, rspbuf, 2, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, 2, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -219,13 +218,13 @@ bletest_hci_le_write_sugg_datalen(uint16_t txoctets, uint16_t txtime)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_WR_SUGG_DATALEN_LEN];
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_WR_SUGG_DEF_DATA_LEN,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_WR_SUGG_DEF_DATA_LEN,
                        BLE_HCI_WR_SUGG_DATALEN_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     htole16(dst, txoctets);
     htole16(dst + 2, txtime);
-    return ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    return ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
 }
 
 int
@@ -236,10 +235,10 @@ bletest_hci_le_rd_sugg_datalen(void)
     uint8_t rspbuf[BLE_HCI_RD_SUGG_DATALEN_RSPLEN];
     uint8_t rsplen;
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_SUGG_DEF_DATA_LEN, 0,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_SUGG_DEF_DATA_LEN, 0,
                        buf);
 
-    rc = ble_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_SUGG_DATALEN_RSPLEN, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_SUGG_DATALEN_RSPLEN, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -259,9 +258,9 @@ bletest_hci_rd_local_version(void)
     uint8_t rspbuf[BLE_HCI_RD_LOC_VER_INFO_RSPLEN];
     uint8_t rsplen;
 
-    host_hci_write_hdr(BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOCAL_VER, 0,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOCAL_VER, 0,
                        buf);
-    rc = ble_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_LOC_VER_INFO_RSPLEN, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_LOC_VER_INFO_RSPLEN, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -280,9 +279,9 @@ bletest_hci_rd_local_feat(void)
     uint8_t rspbuf[BLE_HCI_RD_LOC_SUPP_FEAT_RSPLEN];
     uint8_t rsplen;
 
-    host_hci_write_hdr(BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOC_SUPP_FEAT,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOC_SUPP_FEAT,
                        0, buf);
-    rc = ble_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_LOC_SUPP_FEAT_RSPLEN, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_LOC_SUPP_FEAT_RSPLEN, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -301,9 +300,9 @@ bletest_hci_rd_local_supp_cmd(void)
     uint8_t rspbuf[BLE_HCI_RD_LOC_SUPP_CMD_RSPLEN];
     uint8_t rsplen;
 
-    host_hci_write_hdr(BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOC_SUPP_CMD,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOC_SUPP_CMD,
                        0, buf);
-    rc = ble_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_LOC_SUPP_CMD_RSPLEN, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_LOC_SUPP_CMD_RSPLEN, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -330,8 +329,8 @@ bletest_hci_le_read_supp_states(void)
     uint8_t rspbuf[BLE_HCI_RD_SUPP_STATES_RSPLEN];
     uint8_t rsplen;
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_SUPP_STATES, 0, buf);
-    rc = ble_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_SUPP_STATES_RSPLEN, &rsplen);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_SUPP_STATES, 0, buf);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_SUPP_STATES_RSPLEN, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -350,8 +349,8 @@ bletest_hci_le_rd_max_datalen(void)
     uint8_t rspbuf[BLE_HCI_RD_MAX_DATALEN_RSPLEN];
     uint8_t rsplen;
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_MAX_DATA_LEN, 0, buf);
-    rc = ble_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_MAX_DATALEN_RSPLEN, &rsplen);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_MAX_DATA_LEN, 0, buf);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_MAX_DATALEN_RSPLEN, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -368,9 +367,9 @@ bletest_hci_le_set_adv_data(uint8_t *data, uint8_t len)
     int rc;
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_DATA_LEN];
 
-    rc = host_hci_cmd_build_le_set_adv_data(data, len, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_set_adv_data(data, len, buf, sizeof buf);
     assert(rc == 0);
-    return ble_hci_cmd_tx_empty_ack(buf);
+    return ble_hs_hci_cmd_tx_empty_ack(buf);
 }
 
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
@@ -379,8 +378,8 @@ bletest_hci_le_start_encrypt(struct hci_start_encrypt *cmd)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_LE_START_ENCRYPT_LEN];
 
-    host_hci_cmd_build_le_start_encrypt(cmd, buf, sizeof buf);
-    return ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_start_encrypt(cmd, buf, sizeof buf);
+    return ble_hs_hci_cmd_tx_empty_ack(buf);
 }
 #endif
 
@@ -391,12 +390,12 @@ bletest_hci_le_read_rem_used_feat(uint16_t handle)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_CONN_RD_REM_FEAT_LEN];
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_REM_FEAT,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_REM_FEAT,
                        BLE_HCI_CONN_RD_REM_FEAT_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     htole16(dst, handle);
-    return ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    return ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
 }
 
 int
@@ -405,9 +404,9 @@ bletest_hci_le_set_adv_params(struct hci_adv_params *adv)
     int rc;
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_PARAM_LEN];
 
-    rc = host_hci_cmd_build_le_set_adv_params(adv, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_set_adv_params(adv, buf, sizeof buf);
     if (!rc) {
-        rc = ble_hci_cmd_tx_empty_ack(buf);
+        rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     }
     return rc;
 }
@@ -419,12 +418,12 @@ bletest_hci_le_set_rand_addr(uint8_t *addr)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_DATALEN_LEN];
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_RAND_ADDR,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_RAND_ADDR,
                        BLE_DEV_ADDR_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     memcpy(dst, addr, BLE_DEV_ADDR_LEN);
-    return ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    return ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
 }
 
 int
@@ -434,12 +433,12 @@ bletest_hci_rd_rem_version(uint16_t handle)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + sizeof(uint16_t)];
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LINK_CTRL, BLE_HCI_OCF_RD_REM_VER_INFO,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LINK_CTRL, BLE_HCI_OCF_RD_REM_VER_INFO,
                        sizeof(uint16_t), dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     htole16(dst, handle);
-    return ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    return ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
 }
 
 int
@@ -449,12 +448,12 @@ bletest_hci_le_set_host_chan_class(uint8_t *chanmap)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_HOST_CHAN_CLASS_LEN];
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_HOST_CHAN_CLASS,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_HOST_CHAN_CLASS,
                        BLE_HCI_SET_HOST_CHAN_CLASS_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     memcpy(dst, chanmap, BLE_HCI_SET_HOST_CHAN_CLASS_LEN);
-    return ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    return ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
 }
 
 int
@@ -467,12 +466,12 @@ bletest_hci_le_rd_chanmap(uint16_t handle)
     uint8_t rsplen;
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_CHAN_MAP,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_CHAN_MAP,
                        BLE_HCI_RD_CHANMAP_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     htole16(dst, handle);
-    rc = ble_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_CHANMAP_RSP_LEN, &rsplen);
+    rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_CHANMAP_RSP_LEN, &rsplen);
     if (rc != 0) {
         return rc;
     }
@@ -491,12 +490,12 @@ bletest_hci_le_set_adv_enable(uint8_t enable)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_ENABLE_LEN];
 
     dst = buf;
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_ENABLE,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_ENABLE,
                        BLE_HCI_SET_ADV_ENABLE_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     dst[0] = enable;
-    return ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    return ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
 }
 
 int
@@ -504,8 +503,8 @@ bletest_hci_le_set_event_mask(uint64_t event_mask)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_LE_EVENT_MASK_LEN];
 
-    host_hci_cmd_build_le_set_event_mask(event_mask, buf, sizeof buf);
-    return ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_set_event_mask(event_mask, buf, sizeof buf);
+    return ble_hs_hci_cmd_tx_empty_ack(buf);
 }
 
 int
@@ -513,8 +512,8 @@ bletest_hci_set_event_mask(uint64_t event_mask)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_EVENT_MASK_LEN];
 
-    host_hci_cmd_build_set_event_mask(event_mask, buf, sizeof buf);
-    return ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_set_event_mask(event_mask, buf, sizeof buf);
+    return ble_hs_hci_cmd_tx_empty_ack(buf);
 }
 
 int
@@ -523,9 +522,9 @@ bletest_hci_le_set_scan_rsp_data(uint8_t *data, uint8_t len)
     int rc;
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_RSP_DATA_LEN];
 
-    rc = host_hci_cmd_build_le_set_scan_rsp_data(data, len, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_set_scan_rsp_data(data, len, buf, sizeof buf);
     assert(rc == 0);
-    return ble_hci_cmd_tx_empty_ack(buf);
+    return ble_hs_hci_cmd_tx_empty_ack(buf);
 }
 
 int
@@ -535,11 +534,11 @@ bletest_hci_cmd_le_set_scan_params(uint8_t scan_type, uint16_t scan_itvl,
     int rc;
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_PARAM_LEN];
 
-    rc = host_hci_cmd_build_le_set_scan_params(scan_type, scan_itvl,
+    rc = ble_hs_hci_cmd_build_le_set_scan_params(scan_type, scan_itvl,
                                                scan_window, own_addr_type,
                                                filter_policy, buf, sizeof buf);
     if (!rc) {
-        rc = ble_hci_cmd_tx_empty_ack(buf);
+        rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     }
     return rc;
 }
@@ -550,10 +549,10 @@ bletest_hci_le_add_to_whitelist(uint8_t *addr, uint8_t addr_type)
     int rc;
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_PARAM_LEN];
 
-    rc = host_hci_cmd_build_le_add_to_whitelist(addr, addr_type, buf,
+    rc = ble_hs_hci_cmd_build_le_add_to_whitelist(addr, addr_type, buf,
                                                 sizeof buf);
     if (!rc) {
-        rc = ble_hci_cmd_tx_empty_ack(buf);
+        rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     }
     return rc;
 }
@@ -563,8 +562,8 @@ bletest_hci_le_set_scan_enable(uint8_t enable, uint8_t filter_dups)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_ENABLE_LEN];
 
-    host_hci_cmd_build_le_set_scan_enable(enable, filter_dups, buf, sizeof buf);
-    return ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_set_scan_enable(enable, filter_dups, buf, sizeof buf);
+    return ble_hs_hci_cmd_tx_empty_ack(buf);
 }
 
 int
@@ -573,9 +572,9 @@ bletest_hci_le_create_connection(struct hci_create_conn *hcc)
     int rc;
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_CREATE_CONN_LEN];
 
-    rc = host_hci_cmd_build_le_create_connection(hcc, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_create_connection(hcc, buf, sizeof buf);
     if (!rc) {
-        rc = ble_hci_cmd_tx_empty_ack(buf);
+        rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     }
     return rc;
 }
@@ -592,9 +591,9 @@ bletest_hci_le_add_resolv_list(uint8_t *local_irk, uint8_t *peer_irk,
     memcpy(padd.addr, peer_ident_addr, BLE_DEV_ADDR_LEN);
     swap_buf(padd.local_irk, local_irk, 16);
     swap_buf(padd.peer_irk, peer_irk, 16);
-    rc = host_hci_cmd_build_add_to_resolv_list(&padd, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_add_to_resolv_list(&padd, buf, sizeof buf);
     if (!rc) {
-        rc = ble_hci_cmd_tx_empty_ack(buf);
+        rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     }
     return rc;
 }
@@ -606,9 +605,9 @@ bletest_hci_le_enable_resolv_list(uint8_t enable)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADDR_RESOL_ENA_LEN];
 
 
-    rc = host_hci_cmd_build_set_addr_res_en(enable, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_set_addr_res_en(enable, buf, sizeof buf);
     if (!rc) {
-        rc = ble_hci_cmd_tx_empty_ack(buf);
+        rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     }
     return rc;
 }
diff --git a/apps/bletest/src/main.c b/apps/bletest/src/main.c
index d9efe7e9..b6468a84 100755
--- a/apps/bletest/src/main.c
+++ b/apps/bletest/src/main.c
@@ -36,9 +36,8 @@
 
 /* BLE */
 #include "nimble/ble.h"
-#include "nimble/hci_transport.h"
+#include "nimble/ble_hci_trans.h"
 #include "nimble/hci_common.h"
-#include "host/host_hci.h"
 #include "host/ble_hs.h"
 #include "controller/ble_ll.h"
 #include "controller/ble_ll_hci.h"
@@ -46,10 +45,15 @@
 #include "controller/ble_ll_scan.h"
 #include "controller/ble_ll_adv.h"
 
+/* RAM HCI transport. */
+#include "transport/ram/ble_hci_ram.h"
+
+/* RAM HCI transport. */
+#include "transport/ram/ble_hci_ram.h"
+
 /* XXX: An app should not include private headers from a library.  The bletest
  * app uses some of nimble's internal details for logging.
  */
-#include "../src/ble_hci_util_priv.h"
 #include "../src/ble_hs_priv.h"
 #include "bletest_priv.h"
 
@@ -421,14 +425,14 @@ bletest_init_scanner(void)
     uint8_t add_whitelist;
 
     own_addr_type = BLETEST_CFG_SCAN_OWN_ADDR_TYPE;
-    rc = host_hci_cmd_build_le_set_scan_params(BLETEST_CFG_SCAN_TYPE,
+    rc = ble_hs_hci_cmd_build_le_set_scan_params(BLETEST_CFG_SCAN_TYPE,
                                                BLETEST_CFG_SCAN_ITVL,
                                                BLETEST_CFG_SCAN_WINDOW,
                                                BLETEST_CFG_SCAN_OWN_ADDR_TYPE,
                                                BLETEST_CFG_SCAN_FILT_POLICY,
                                                buf, sizeof buf);
     assert(rc == 0);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc == 0) {
         add_whitelist = BLETEST_CFG_SCAN_FILT_POLICY;
 #if (BLE_LL_CFG_FEAT_LL_PRIVACY == 1)
@@ -616,7 +620,7 @@ bletest_execute_initiator(void)
             } else {
                 for (i = 0; i < g_bletest_current_conns; ++i) {
                     if (ble_ll_conn_find_active_conn(i + 1)) {
-                        ble_hci_util_read_rssi(i+1, &rssi);
+                        ble_hs_hci_util_read_rssi(i+1, &rssi);
                     }
                 }
             }
@@ -655,6 +659,8 @@ bletest_execute_advertiser(void)
     int i;
 #if (BLETEST_CONCURRENT_CONN_TEST == 1)
     int j;
+    uint16_t mask;
+    uint16_t reply_handle;
 #endif
     int rc;
     uint16_t handle;
@@ -726,10 +732,16 @@ bletest_execute_advertiser(void)
     if ((int32_t)(os_time_get() - g_next_os_time) >= 0) {
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
         /* Do we need to send a LTK reply? */
-        if (g_bletest_ltk_reply_handle) {
-            //bletest_send_ltk_req_neg_reply(g_bletest_ltk_reply_handle);
-            bletest_send_ltk_req_reply(g_bletest_ltk_reply_handle);
-            g_bletest_ltk_reply_handle = 0;
+        mask = 1;
+        reply_handle = 1;
+        while (g_bletest_ltk_reply_handle && mask) {
+            if (g_bletest_ltk_reply_handle & mask) {
+                bletest_send_ltk_req_reply(reply_handle);
+                //bletest_send_ltk_req_neg_reply(reply_handle);
+                g_bletest_ltk_reply_handle &= ~mask;
+            }
+            ++reply_handle;
+            mask <<= 1;
         }
 #endif
         if (g_bletest_current_conns) {
@@ -769,7 +781,7 @@ bletest_execute_advertiser(void)
 
                         /* Add length */
                         OS_MBUF_PKTHDR(om)->omp_len = om->om_len;
-                        ble_hci_transport_host_acl_data_send(om);
+                        ble_hci_trans_hs_acl_tx(om);
 
                         /* Increment last handle used */
                         ++g_last_handle_used;
@@ -825,7 +837,7 @@ bletest_execute_advertiser(void)
 
                 /* Add length */
                 OS_MBUF_PKTHDR(om)->omp_len = om->om_len;
-                ble_hci_transport_host_acl_data_send(om);
+                ble_hci_trans_hs_acl_data_send(om);
 
                 ++g_bletest_outstanding_pkts;
             }
@@ -897,6 +909,8 @@ bletest_task_handler(void *arg)
     os_callout_func_init(&g_bletest_timer, &g_bletest_evq, bletest_timer_cb,
                          NULL);
 
+    ble_hs_dbg_set_sync_state(BLE_HS_SYNC_STATE_GOOD);
+
     /* Send the reset command first */
     rc = bletest_hci_reset_ctlr();
     assert(rc == 0);
@@ -990,7 +1004,7 @@ bletest_task_handler(void *arg)
 #endif
 
     /* Get a random number */
-    rc = ble_hci_util_rand(&rand64, 8);
+    rc = ble_hs_hci_util_rand(&rand64, 8);
     assert(rc == 0);
 
     /* Wait some time before starting */
@@ -1106,10 +1120,6 @@ main(void)
     g_led_pin = LED_BLINK_PIN;
     hal_gpio_init_out(g_led_pin, 1);
 
-    /* Init the console */
-    rc = console_init(shell_console_rx_cb);
-    assert(rc == 0);
-
 #if 0
     rc = hal_flash_init();
     assert(rc == 0);
@@ -1157,6 +1167,10 @@ main(void)
     rc = ble_hs_init(&g_bletest_evq, NULL);
     assert(rc == 0);
 
+    /* Initialize the RAM HCI transport. */
+    rc = ble_hci_ram_init(&ble_hci_ram_cfg_dflt);
+    assert(rc == 0);
+
     rc = os_task_init(&bletest_task, "bletest", bletest_task_handler, NULL,
                       BLETEST_TASK_PRIO, OS_WAIT_FOREVER, bletest_stack,
                       BLETEST_STACK_SIZE);
diff --git a/apps/bletiny/pkg.yml b/apps/bletiny/pkg.yml
index 361840b7..9a8b08f5 100644
--- a/apps/bletiny/pkg.yml
+++ b/apps/bletiny/pkg.yml
@@ -27,7 +27,21 @@ pkg.deps:
     - sys/log
     - net/nimble/controller
     - net/nimble/host
+    - net/nimble/host/services/gap
+    - net/nimble/host/services/gatt
+    - net/nimble/host/store/ram
+    - net/nimble/transport/ram
     - libs/console/full
     - libs/shell
 
 pkg.cflags:
+    ### Disable some features to make bletiny small enough to fit on the nRF51.
+    ### These features can be re-enabled in the target definition.
+    # Set log level to info (disable debug logging).
+    - "-DLOG_LEVEL=1"
+
+    # Disable security manager (pairing and bonding).
+    - "-DNIMBLE_OPT_SM=0"
+
+    # Disable eddystone beacons.
+    - "-DNIMBLE_OPT_EDDYSTONE=0"
diff --git a/apps/bletiny/src/bletiny.h b/apps/bletiny/src/bletiny.h
index 29e1d63d..df6e1ad9 100644
--- a/apps/bletiny/src/bletiny.h
+++ b/apps/bletiny/src/bletiny.h
@@ -29,7 +29,7 @@
 struct ble_gap_white_entry;
 struct ble_hs_adv_fields;
 struct ble_gap_upd_params;
-struct ble_gap_crt_params;
+struct ble_gap_conn_params;
 struct hci_adv_params;
 struct ble_l2cap_sig_update_req;
 struct ble_l2cap_sig_update_params;
@@ -37,6 +37,7 @@ union ble_store_value;
 union ble_store_key;
 struct ble_gap_adv_params;
 struct ble_gap_conn_desc;
+struct ble_gap_disc_params;
 
 typedef int cmd_fn(int argc, char **argv);
 struct cmd_entry {
@@ -80,37 +81,33 @@ struct bletiny_conn {
 extern struct bletiny_conn bletiny_conns[NIMBLE_OPT(MAX_CONNECTIONS)];
 extern int bletiny_num_conns;
 
-extern const char *bletiny_device_name;
-extern const uint16_t bletiny_appearance;
-extern const uint8_t bletiny_privacy_flag;
-extern uint8_t bletiny_reconnect_addr[6];
-extern uint8_t bletiny_pref_conn_params[8];
-extern uint8_t bletiny_gatt_service_changed[4];
-
-extern struct nmgr_transport nm_ble_transport;
 extern uint16_t nm_attr_val_handle;
 
 extern struct log bletiny_log;
 
-void print_addr(void *addr);
-void print_uuid(void *uuid128);
-void print_bytes(uint8_t *bytes, int len);
 const struct cmd_entry *parse_cmd_find(const struct cmd_entry *cmds,
                                        char *name);
 struct kv_pair *parse_kv_find(struct kv_pair *kvs, char *name);
-char *parse_arg_find(char *key);
+int parse_arg_find_idx(const char *key);
+char *parse_arg_extract(const char *key);
 long parse_arg_long_bounds(char *name, long min, long max, int *out_status);
+long parse_arg_long_bounds_default(char *name, long min, long max,
+                                   long dflt, int *out_status);
 uint64_t parse_arg_uint64_bounds(char *name, uint64_t min,
                                  uint64_t max, int *out_status);
 long parse_arg_long(char *name, int *staus);
 uint8_t parse_arg_bool(char *name, int *status);
+uint8_t parse_arg_bool_default(char *name, uint8_t dflt, int *out_status);
 uint8_t parse_arg_uint8(char *name, int *status);
+uint8_t parse_arg_uint8_dflt(char *name, uint8_t dflt, int *out_status);
 uint16_t parse_arg_uint16(char *name, int *status);
 uint16_t parse_arg_uint16_dflt(char *name, uint16_t dflt, int *out_status);
 uint32_t parse_arg_uint32(char *name, int *out_status);
+uint32_t parse_arg_uint32_dflt(char *name, uint32_t dflt, int *out_status);
 uint64_t parse_arg_uint64(char *name, int *out_status);
-int parse_arg_kv(char *name, struct kv_pair *kvs);
-int parse_arg_kv_default(char *name, struct kv_pair *kvs, int def_val);
+int parse_arg_kv(char *name, struct kv_pair *kvs, int *out_status);
+int parse_arg_kv_default(char *name, struct kv_pair *kvs, int def_val,
+                         int *out_status);
 int parse_arg_byte_stream(char *name, int max_len, uint8_t *dst, int *out_len);
 int parse_arg_byte_stream_exact_length(char *name, uint8_t *dst, int len);
 int parse_arg_mac(char *name, uint8_t *dst);
@@ -119,7 +116,7 @@ int parse_err_too_few_args(char *cmd_name);
 int parse_arg_all(int argc, char **argv);
 int cmd_init(void);
 int nm_chr_access(uint16_t conn_handle, uint16_t attr_handle,
-                  uint8_t op, union ble_gatt_access_ctxt *ctxt,
+                  uint8_t op, struct ble_gatt_access_ctxt *ctxt,
                   void *arg);
 int nm_rx_rsp(uint8_t *attr_val, uint16_t attr_len);
 void nm_init(void);
@@ -143,26 +140,27 @@ int bletiny_read_by_uuid(uint16_t conn_handle, uint16_t start_handle,
                           uint16_t end_handle, uint8_t *uuid128);
 int bletiny_read_mult(uint16_t conn_handle, uint16_t *attr_handles,
                        int num_attr_handles);
-int bletiny_write(uint16_t conn_handle, uint16_t attr_handle, void *value,
-                   uint16_t value_len);
+int bletiny_write(uint16_t conn_handle, uint16_t attr_handle,
+                  struct os_mbuf *om);
 int bletiny_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle,
-                          void *value, uint16_t value_len);
+                         struct os_mbuf *om);
 int bletiny_write_long(uint16_t conn_handle, uint16_t attr_handle,
-                        void *value, uint16_t value_len);
-int bletiny_write_reliable(uint16_t conn_handle, struct ble_gatt_attr *attrs,
-                            int num_attrs);
-int bletiny_adv_start(int disc, int conn,
-                     uint8_t *peer_addr, uint8_t peer_addr_type,
-                     struct ble_gap_adv_params *params);
+                       struct os_mbuf *om);
+int bletiny_write_reliable(uint16_t conn_handle,
+                           struct ble_gatt_attr *attrs, int num_attrs);
+int bletiny_adv_start(uint8_t own_addr_type, uint8_t peer_addr_type,
+                      const uint8_t *peer_addr, int32_t duration_ms,
+                      const struct ble_gap_adv_params *params);
 int bletiny_adv_stop(void);
-int bletiny_conn_initiate(int addr_type, uint8_t *peer_addr,
-                           struct ble_gap_crt_params *params);
+int bletiny_conn_initiate(uint8_t own_addr_type, uint8_t peer_addr_type,
+                          uint8_t *peer_addr, int32_t duration_ms,
+                          struct ble_gap_conn_params *params);
 int bletiny_conn_cancel(void);
-int bletiny_term_conn(uint16_t conn_handle);
+int bletiny_term_conn(uint16_t conn_handle, uint8_t reason);
 int bletiny_wl_set(struct ble_gap_white_entry *white_list,
                     int white_list_count);
-int bletiny_scan(uint32_t dur_ms, uint8_t disc_mode, uint8_t scan_type,
-                  uint8_t filter_policy, uint8_t our_addr_mode);
+int bletiny_scan(uint8_t own_addr_type, int32_t duration_ms,
+                 const struct ble_gap_disc_params *disc_params);
 int bletiny_scan_cancel(void);
 int bletiny_set_adv_data(struct ble_hs_adv_fields *adv_fields);
 int bletiny_update_conn(uint16_t conn_handle,
@@ -191,22 +189,19 @@ int bletiny_rssi(uint16_t conn_handle, int8_t *out_rssi);
 #define GATT_SVR_CHR_SUP_UNR_ALERT_CAT_UUID   0x2A48
 #define GATT_SVR_CHR_UNR_ALERT_STAT_UUID      0x2A45
 #define GATT_SVR_CHR_ALERT_NOT_CTRL_PT        0x2A44
-extern const uint8_t gatt_svr_svc_bleprph[16];
-extern const uint8_t gatt_svr_chr_bleprph_read[16];
-extern const uint8_t gatt_svr_chr_bleprph_write[16];
-
-void gatt_svr_init(void);
 
-/** Store. */
-int store_read(int obj_type, union ble_store_key *key,
-               union ble_store_value *dst);
-int store_write(int obj_type, union ble_store_value *val);
+void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg);
+int gatt_svr_init(struct ble_hs_cfg *cfg);
 
 /** Misc. */
-void print_bytes(uint8_t *bytes, int len);
-int svc_is_empty(struct bletiny_svc *svc);
-uint16_t chr_end_handle(struct bletiny_svc *svc, struct bletiny_chr *chr);
-int chr_is_empty(struct bletiny_svc *svc, struct bletiny_chr *chr);
-void print_conn_desc(struct ble_gap_conn_desc *desc);
+void print_bytes(const uint8_t *bytes, int len);
+void print_mbuf(const struct os_mbuf *om);
+void print_addr(const void *addr);
+void print_uuid(const void *uuid128);
+int svc_is_empty(const struct bletiny_svc *svc);
+uint16_t chr_end_handle(const struct bletiny_svc *svc,
+                        const struct bletiny_chr *chr);
+int chr_is_empty(const struct bletiny_svc *svc, const struct bletiny_chr *chr);
+void print_conn_desc(const struct ble_gap_conn_desc *desc);
 
 #endif
diff --git a/apps/bletiny/src/cmd.c b/apps/bletiny/src/cmd.c
index f6ad11dc..5118aeb2 100644
--- a/apps/bletiny/src/cmd.c
+++ b/apps/bletiny/src/cmd.c
@@ -24,11 +24,13 @@
 #include "shell/shell.h"
 
 #include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
 #include "nimble/hci_common.h"
 #include "host/ble_gap.h"
 #include "host/ble_hs_adv.h"
 #include "host/ble_sm.h"
 #include "host/ble_eddystone.h"
+#include "host/ble_hs_id.h"
 #include "../src/ble_l2cap_priv.h"
 #include "../src/ble_hs_priv.h"
 
@@ -259,20 +261,11 @@ static struct kv_pair cmd_adv_filt_types[] = {
 static int
 cmd_adv(int argc, char **argv)
 {
-    struct ble_gap_adv_params params = {
-        .adv_itvl_min = 0,
-        .adv_itvl_max = 0,
-        .adv_type = BLE_HCI_ADV_TYPE_ADV_IND,
-        .own_addr_type = BLE_HCI_ADV_OWN_ADDR_PUBLIC,
-        .adv_channel_map = BLE_HCI_ADV_CHANMASK_DEF,
-        .adv_filter_policy = BLE_HCI_ADV_FILT_DEF,
-    };
-    uint8_t u8;
+    struct ble_gap_adv_params params;
+    int32_t duration_ms;
     uint8_t peer_addr_type;
+    uint8_t own_addr_type;
     uint8_t peer_addr[8];
-    int addr_type;
-    int conn;
-    int disc;
     int rc;
 
     if (argc > 1 && strcmp(argv[1], "stop") == 0) {
@@ -285,22 +278,24 @@ cmd_adv(int argc, char **argv)
         return 0;
     }
 
-    conn = parse_arg_kv("conn", cmd_adv_conn_modes);
-    if (conn <  0) {
+    params.conn_mode = parse_arg_kv_default("conn", cmd_adv_conn_modes,
+                                            BLE_GAP_CONN_MODE_UND, &rc);
+    if (rc != 0) {
         console_printf("invalid 'conn' parameter\n");
-        return -1;
+        return rc;
     }
 
-    disc = parse_arg_kv("disc", cmd_adv_disc_modes);
-    if (conn <  0) {
-        console_printf("missing 'disc' parameter\n");
-        return -1;
+    params.disc_mode = parse_arg_kv_default("disc", cmd_adv_disc_modes,
+                                            BLE_GAP_DISC_MODE_GEN, &rc);
+    if (rc != 0) {
+        console_printf("invalid 'disc' parameter\n");
+        return rc;
     }
 
-    addr_type = parse_arg_kv_default
-            ("peer_addr_type", cmd_adv_addr_types, BLE_ADDR_TYPE_PUBLIC);
-    if (addr_type == -1) {
-        return -1;
+    peer_addr_type = parse_arg_kv_default(
+        "peer_addr_type", cmd_adv_addr_types, BLE_ADDR_TYPE_PUBLIC, &rc);
+    if (rc != 0) {
+        return rc;
     }
 
     rc = parse_arg_mac("peer_addr", peer_addr);
@@ -310,31 +305,49 @@ cmd_adv(int argc, char **argv)
         return rc;
     }
 
-    peer_addr_type = addr_type;
+    own_addr_type = parse_arg_kv_default(
+        "own_addr_type", cmd_adv_addr_types, BLE_ADDR_TYPE_PUBLIC, &rc);
+    if (rc != 0) {
+        return rc;
+    }
 
-    rc = parse_arg_kv_default
-                ("own_addr_type", cmd_adv_addr_types, BLE_ADDR_TYPE_PUBLIC);
-    if (rc == -1) {
+    params.channel_map = parse_arg_long_bounds_default("chan_map", 0, 0xff, 0,
+                                                       &rc);
+    if (rc != 0) {
         return rc;
     }
 
-    params.own_addr_type = rc;
+    params.filter_policy = parse_arg_kv_default("filt", cmd_adv_filt_types,
+                                                BLE_HCI_ADV_FILT_NONE, &rc);
+    if (rc != 0) {
+        return rc;
+    }
 
-    u8 = parse_arg_long_bounds("chan_map", 0, 0xff, &rc);
-    if (rc == 0) {
-        params.adv_channel_map = u8;
-    } else if (rc != ENOENT) {
+    params.itvl_min = parse_arg_long_bounds_default("itvl_min", 0, UINT16_MAX,
+                                                    0, &rc);
+    if (rc != 0) {
         return rc;
     }
 
-    if (parse_arg_find("filt") != NULL) {
-        params.adv_filter_policy = parse_arg_kv("filt", cmd_adv_filt_types);
-        if (params.adv_filter_policy == -1) {
-            return EINVAL;
-        }
+    params.itvl_max = parse_arg_long_bounds_default("itvl_max", 0, UINT16_MAX,
+                                                    0, &rc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    params.high_duty_cycle = parse_arg_long_bounds_default("hd", 0, 1, 0, &rc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    duration_ms = parse_arg_long_bounds_default("dur", 1, INT32_MAX,
+                                                BLE_HS_FOREVER, &rc);
+    if (rc != 0) {
+        return rc;
     }
 
-    rc = bletiny_adv_start(disc, conn, peer_addr, peer_addr_type, &params);
+    rc = bletiny_adv_start(own_addr_type, peer_addr_type, peer_addr,
+                           duration_ms, &params);
     if (rc != 0) {
         console_printf("advertise fail: %d\n", rc);
         return rc;
@@ -350,8 +363,8 @@ cmd_adv(int argc, char **argv)
 static struct kv_pair cmd_conn_peer_addr_types[] = {
     { "public",         BLE_HCI_CONN_PEER_ADDR_PUBLIC },
     { "random",         BLE_HCI_CONN_PEER_ADDR_RANDOM },
-    { "public_ident",   BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT },
-    { "random_ident",   BLE_HCI_CONN_PEER_ADDR_RANDOM_IDENT },
+    { "rpa_pub",        BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT },
+    { "rpa_rnd",        BLE_HCI_CONN_PEER_ADDR_RANDOM_IDENT },
     { "wl",             BLE_GAP_ADDR_TYPE_WL },
     { NULL }
 };
@@ -367,9 +380,11 @@ static struct kv_pair cmd_conn_own_addr_types[] = {
 static int
 cmd_conn(int argc, char **argv)
 {
-    struct ble_gap_crt_params params;
+    struct ble_gap_conn_params params;
+    int32_t duration_ms;
     uint8_t peer_addr[6];
-    int addr_type;
+    int peer_addr_type;
+    int own_addr_type;
     int rc;
 
     if (argc > 1 && strcmp(argv[1], "cancel") == 0) {
@@ -382,12 +397,14 @@ cmd_conn(int argc, char **argv)
         return 0;
     }
 
-    addr_type = parse_arg_kv("peer_addr_type", cmd_conn_peer_addr_types);
-    if (addr_type == -1) {
-        return -1;
+    peer_addr_type = parse_arg_kv_default("peer_addr_type",
+                                          cmd_conn_peer_addr_types,
+                                          BLE_ADDR_TYPE_PUBLIC, &rc);
+    if (rc != 0) {
+        return rc;
     }
 
-    if (addr_type != BLE_GAP_ADDR_TYPE_WL) {
+    if (peer_addr_type != BLE_GAP_ADDR_TYPE_WL) {
         rc = parse_arg_mac("peer_addr", peer_addr);
         if (rc == ENOENT) {
             /* Allow "addr" for backwards compatibility. */
@@ -401,12 +418,12 @@ cmd_conn(int argc, char **argv)
         memset(peer_addr, 0, sizeof peer_addr);
     }
 
-    rc = parse_arg_kv_default("own_addr_type",
-                        cmd_conn_own_addr_types, BLE_ADDR_TYPE_PUBLIC);
-    if (rc < 0) {
+    own_addr_type = parse_arg_kv_default("own_addr_type",
+                                         cmd_conn_own_addr_types,
+                                         BLE_ADDR_TYPE_PUBLIC, &rc);
+    if (rc != 0) {
         return rc;
     }
-    params.our_addr_type = rc;
 
     params.scan_itvl = parse_arg_uint16_dflt("scan_itvl", 0x0010, &rc);
     if (rc != 0) {
@@ -450,7 +467,13 @@ cmd_conn(int argc, char **argv)
         return rc;
     }
 
-    rc = bletiny_conn_initiate(addr_type, peer_addr, &params);
+    duration_ms = parse_arg_long_bounds_default("dur", 1, INT32_MAX, 0, &rc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = bletiny_conn_initiate(own_addr_type, peer_addr_type, peer_addr,
+                               duration_ms, &params);
     if (rc != 0) {
         return rc;
     }
@@ -897,18 +920,6 @@ cmd_rssi(int argc, char **argv)
  * $scan                                                                     *
  *****************************************************************************/
 
-static struct kv_pair cmd_scan_disc_modes[] = {
-    { "ltd", BLE_GAP_DISC_MODE_LTD },
-    { "gen", BLE_GAP_DISC_MODE_GEN },
-    { NULL }
-};
-
-static struct kv_pair cmd_scan_types[] = {
-    { "passive", BLE_HCI_SCAN_TYPE_PASSIVE },
-    { "active", BLE_HCI_SCAN_TYPE_ACTIVE },
-    { NULL }
-};
-
 static struct kv_pair cmd_scan_filt_policies[] = {
     { "no_wl", BLE_HCI_SCAN_FILT_NO_WL },
     { "use_wl", BLE_HCI_SCAN_FILT_USE_WL },
@@ -928,12 +939,10 @@ static struct kv_pair cmd_scan_addr_types[] = {
 static int
 cmd_scan(int argc, char **argv)
 {
-    uint32_t dur;
-    int disc;
-    int type;
-    int filt;
+    struct ble_gap_disc_params params;
+    int32_t duration_ms;
+    uint8_t own_addr_type;
     int rc;
-    int addr_mode;
 
     if (argc > 1 && strcmp(argv[1], "cancel") == 0) {
         rc = bletiny_scan_cancel();
@@ -945,33 +954,50 @@ cmd_scan(int argc, char **argv)
         return 0;
     }
 
-    dur = parse_arg_uint16("dur", &rc);
+    duration_ms = parse_arg_long_bounds_default("dur", 1, INT32_MAX,
+                                                BLE_HS_FOREVER, &rc);
     if (rc != 0) {
         return rc;
     }
 
-    disc = parse_arg_kv("disc", cmd_scan_disc_modes);
-    if (disc < 0) {
-        return EINVAL;
+    params.limited = parse_arg_bool_default("ltd", 0, &rc);
+    if (rc != 0) {
+        return rc;
     }
 
-    type = parse_arg_kv("type", cmd_scan_types);
-    if (type < 0) {
-        return EINVAL;
+    params.passive = parse_arg_bool_default("passive", 0, &rc);
+    if (rc != 0) {
+        return rc;
     }
 
-    filt = parse_arg_kv("filt", cmd_scan_filt_policies);
-    if (filt < 0) {
-        return EINVAL;
+    params.itvl = parse_arg_uint16_dflt("itvl", 0, &rc);
+    if (rc != 0) {
+        return rc;
     }
 
-    addr_mode = parse_arg_kv_default("addr_mode",
-                                    cmd_scan_addr_types, BLE_ADDR_TYPE_PUBLIC);
-    if (addr_mode == -1) {
-        return EINVAL;
+    params.window = parse_arg_uint16_dflt("window", 0, &rc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    params.filter_policy = parse_arg_kv_default(
+        "filt", cmd_scan_filt_policies, BLE_HCI_SCAN_FILT_NO_WL, &rc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    params.filter_duplicates = parse_arg_bool_default("nodups", 0, &rc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    own_addr_type = parse_arg_kv_default("own_addr_type", cmd_scan_addr_types,
+                                         BLE_ADDR_TYPE_PUBLIC, &rc);
+    if (rc != 0) {
+        return rc;
     }
 
-    rc = bletiny_scan(dur, disc, type, filt, addr_mode);
+    rc = bletiny_scan(own_addr_type, duration_ms, &params);
     if (rc != 0) {
         console_printf("error scanning; rc=%d\n", rc);
         return rc;
@@ -987,13 +1013,24 @@ cmd_scan(int argc, char **argv)
 static int
 cmd_show_addr(int argc, char **argv)
 {
-    uint8_t *id_addr;
-    uint8_t id_addr_type;
+    uint8_t id_addr[6];
+    int rc;
 
-    id_addr = ble_hs_pvcy_our_id_addr(&id_addr_type);
+    console_printf("public_id_addr=");
+    rc = ble_hs_id_copy_addr(BLE_ADDR_TYPE_PUBLIC, id_addr, NULL);
+    if (rc == 0) {
+        print_addr(id_addr);
+    } else {
+        console_printf("none");
+    }
 
-    console_printf("id_addr_type=%d id_addr=", id_addr_type);
-    print_addr(id_addr);
+    console_printf(" random_id_addr=");
+    rc = ble_hs_id_copy_addr(BLE_ADDR_TYPE_RANDOM, id_addr, NULL);
+    if (rc == 0) {
+        print_addr(id_addr);
+    } else {
+        console_printf("none");
+    }
     console_printf("\n");
 
     return 0;
@@ -1030,7 +1067,7 @@ cmd_show_conn(int argc, char **argv)
     for (i = 0; i < bletiny_num_conns; i++) {
         conn = bletiny_conns + i;
 
-        rc = ble_gap_find_conn(conn->handle, &conn_desc);
+        rc = ble_gap_conn_find(conn->handle, &conn_desc);
         if (rc == 0) {
             print_conn_desc(&conn_desc);
         }
@@ -1175,11 +1212,11 @@ cmd_sec(int argc, char **argv)
 #define CMD_ADV_DATA_MAX_UUIDS32                8
 #define CMD_ADV_DATA_MAX_UUIDS128               2
 #define CMD_ADV_DATA_MAX_PUBLIC_TGT_ADDRS       8
-#define CMD_ADV_DATA_SVC_DATA_UUID16_MAX_LEN    32
-#define CMD_ADV_DATA_SVC_DATA_UUID32_MAX_LEN    32
-#define CMD_ADV_DATA_SVC_DATA_UUID128_MAX_LEN   32
-#define CMD_ADV_DATA_URI_MAX_LEN                32
-#define CMD_ADV_DATA_MFG_DATA_MAX_LEN           32
+#define CMD_ADV_DATA_SVC_DATA_UUID16_MAX_LEN    BLE_HS_ADV_MAX_FIELD_SZ
+#define CMD_ADV_DATA_SVC_DATA_UUID32_MAX_LEN    BLE_HS_ADV_MAX_FIELD_SZ
+#define CMD_ADV_DATA_SVC_DATA_UUID128_MAX_LEN   BLE_HS_ADV_MAX_FIELD_SZ
+#define CMD_ADV_DATA_URI_MAX_LEN                BLE_HS_ADV_MAX_FIELD_SZ
+#define CMD_ADV_DATA_MFG_DATA_MAX_LEN           BLE_HS_ADV_MAX_FIELD_SZ
 
 static int
 cmd_set_adv_data(void)
@@ -1221,6 +1258,14 @@ cmd_set_adv_data(void)
 
     memset(&adv_fields, 0, sizeof adv_fields);
 
+    tmp = parse_arg_long_bounds("flags", 0, UINT8_MAX, &rc);
+    if (rc == 0) {
+        adv_fields.flags = tmp;
+        adv_fields.flags_is_present = 1;
+    } else if (rc != ENOENT) {
+        return rc;
+    }
+
     while (1) {
         uuid16 = parse_arg_uint16("uuid16", &rc);
         if (rc == 0) {
@@ -1296,12 +1341,12 @@ cmd_set_adv_data(void)
         return rc;
     }
 
-    adv_fields.name = (uint8_t *)parse_arg_find("name");
+    adv_fields.name = (uint8_t *)parse_arg_extract("name");
     if (adv_fields.name != NULL) {
         adv_fields.name_len = strlen((char *)adv_fields.name);
     }
 
-    tmp = parse_arg_long_bounds("tx_pwr_lvl", 0, 0xff, &rc);
+    tmp = parse_arg_long_bounds("tx_pwr_lvl", INT8_MIN, INT8_MAX, &rc);
     if (rc == 0) {
         adv_fields.tx_pwr_lvl = tmp;
         adv_fields.tx_pwr_lvl_is_present = 1;
@@ -1425,7 +1470,7 @@ cmd_set_adv_data(void)
         return rc;
     }
 
-    eddystone_url_full = parse_arg_find("eddystone_url");
+    eddystone_url_full = parse_arg_extract("eddystone_url");
     if (eddystone_url_full != NULL) {
         rc = cmd_parse_eddystone_url(eddystone_url_full, &eddystone_url_scheme,
                                      eddystone_url_body,
@@ -1523,11 +1568,60 @@ cmd_set_sm_data(void)
     return 0;
 }
 
+static struct kv_pair cmd_set_addr_types[] = {
+    { "public",         BLE_ADDR_TYPE_PUBLIC },
+    { "random",         BLE_ADDR_TYPE_RANDOM },
+    { NULL }
+};
+
+static int
+cmd_set_addr(void)
+{
+    uint8_t addr[6];
+    int addr_type;
+    int rc;
+
+    addr_type = parse_arg_kv_default("addr_type", cmd_set_addr_types,
+                                     BLE_ADDR_TYPE_PUBLIC, &rc);
+    if (rc != 0) {
+        console_printf("invalid 'addr_type' parameter\n");
+        return rc;
+    }
+
+    rc = parse_arg_mac("addr", addr);
+    if (rc != 0) {
+        return rc;
+    }
+
+    switch (addr_type) {
+    case BLE_ADDR_TYPE_PUBLIC:
+        /* We shouldn't be writing to the controller's address (g_dev_addr).
+         * There is no standard way to set the local public address, so this is
+         * our only option at the moment.
+         */
+        memcpy(g_dev_addr, addr, 6);
+        ble_hs_id_set_pub(g_dev_addr);
+        break;
+
+    case BLE_ADDR_TYPE_RANDOM:
+        rc = ble_hs_id_set_rnd(addr);
+        if (rc != 0) {
+            return rc;
+        }
+        break;
+
+    default:
+        assert(0);
+        return BLE_HS_EUNKNOWN;
+    }
+
+    return 0;
+}
+
 static int
 cmd_set(int argc, char **argv)
 {
     uint16_t mtu;
-    uint8_t addr[6];
     uint8_t irk[16];
     int good;
     int rc;
@@ -1544,16 +1638,14 @@ cmd_set(int argc, char **argv)
 
     good = 0;
 
-    rc = parse_arg_mac("addr", addr);
-    if (rc == 0) {
+    rc = parse_arg_find_idx("addr");
+    if (rc != -1) {
+        rc = cmd_set_addr();
+        if (rc != 0) {
+            return rc;
+        }
+
         good = 1;
-        /* XXX: There are a lot of problems with this.  This command probably
-         * needs to be removed.
-         */
-        memcpy(g_dev_addr, addr, 6);
-        ble_gap_init_identity_addr(g_dev_addr);
-    } else if (rc != ENOENT) {
-        return rc;
     }
 
     mtu = parse_arg_uint16("mtu", &rc);
@@ -1590,6 +1682,7 @@ static int
 cmd_term(int argc, char **argv)
 {
     uint16_t conn_handle;
+    uint8_t reason;
     int rc;
 
     conn_handle = parse_arg_uint16("conn", &rc);
@@ -1597,7 +1690,12 @@ cmd_term(int argc, char **argv)
         return rc;
     }
 
-    rc = bletiny_term_conn(conn_handle);
+    reason = parse_arg_uint8_dflt("reason", BLE_ERR_REM_USER_CONN_TERM, &rc);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = bletiny_term_conn(conn_handle, reason);
     if (rc != 0) {
         console_printf("error terminating connection; rc=%d\n", rc);
         return rc;
@@ -1697,9 +1795,9 @@ cmd_wl(int argc, char **argv)
             return rc;
         }
 
-        addr_type = parse_arg_kv("addr_type", cmd_wl_addr_types);
-        if (addr_type == -1) {
-            return EINVAL;
+        addr_type = parse_arg_kv("addr_type", cmd_wl_addr_types, &rc);
+        if (rc != 0) {
+            return rc;
         }
 
         memcpy(white_list[wl_cnt].addr, addr, 6);
@@ -1720,12 +1818,10 @@ cmd_wl(int argc, char **argv)
  * $write                                                                    *
  *****************************************************************************/
 
-#define CMD_WRITE_MAX_ATTRS 16
-
 static int
 cmd_write(int argc, char **argv)
 {
-    static struct ble_gatt_attr attrs[CMD_WRITE_MAX_ATTRS];
+    struct ble_gatt_attr attrs[NIMBLE_OPT(GATT_WRITE_MAX_ATTRS)] = { { 0 } };
     uint16_t attr_handle;
     uint16_t conn_handle;
     int total_attr_len;
@@ -1734,6 +1830,7 @@ cmd_write(int argc, char **argv)
     int is_long;
     int no_rsp;
     int rc;
+    int i;
 
     conn_handle = parse_arg_uint16("conn", &rc);
     if (rc != 0) {
@@ -1761,7 +1858,8 @@ cmd_write(int argc, char **argv)
         if (rc == ENOENT) {
             break;
         } else if (rc != 0) {
-            return rc;
+            rc = -rc;
+            goto done;
         }
 
         rc = parse_arg_byte_stream("value", sizeof cmd_buf - total_attr_len,
@@ -1769,17 +1867,21 @@ cmd_write(int argc, char **argv)
         if (rc == ENOENT) {
             break;
         } else if (rc != 0) {
-            return rc;
+            goto done;
         }
 
-        if (num_attrs >= CMD_WRITE_MAX_ATTRS) {
-            return EINVAL;
+        if (num_attrs >= sizeof attrs / sizeof attrs[0]) {
+            rc = -EINVAL;
+            goto done;
         }
 
         attrs[num_attrs].handle = attr_handle;
         attrs[num_attrs].offset = 0;
-        attrs[num_attrs].value_len = attr_len;
-        attrs[num_attrs].value = cmd_buf + total_attr_len;
+        attrs[num_attrs].om = ble_hs_mbuf_from_flat(cmd_buf + total_attr_len,
+                                                    attr_len);
+        if (attrs[num_attrs].om == NULL) {
+            goto done;
+        }
 
         total_attr_len += attr_len;
         num_attrs++;
@@ -1787,31 +1889,37 @@ cmd_write(int argc, char **argv)
 
     if (no_rsp) {
         if (num_attrs != 1) {
-            return EINVAL;
+            rc = -EINVAL;
+            goto done;
         }
-        rc = bletiny_write_no_rsp(conn_handle, attrs[0].handle,
-                                   attrs[0].value, attrs[0].value_len);
+        rc = bletiny_write_no_rsp(conn_handle, attrs[0].handle, attrs[0].om);
+        attrs[0].om = NULL;
     } else if (is_long) {
         if (num_attrs != 1) {
-            return EINVAL;
+            rc = -EINVAL;
+            goto done;
         }
-        rc = bletiny_write_long(conn_handle, attrs[0].handle,
-                                attrs[0].value, attrs[0].value_len);
+        rc = bletiny_write_long(conn_handle, attrs[0].handle, attrs[0].om);
+        attrs[0].om = NULL;
     } else if (num_attrs > 1) {
         rc = bletiny_write_reliable(conn_handle, attrs, num_attrs);
     } else if (num_attrs == 1) {
-        rc = bletiny_write(conn_handle, attrs[0].handle,
-                           attrs[0].value, attrs[0].value_len);
+        rc = bletiny_write(conn_handle, attrs[0].handle, attrs[0].om);
+        attrs[0].om = NULL;
     } else {
-        return EINVAL;
+        rc = -EINVAL;
+    }
+
+done:
+    for (i = 0; i < sizeof attrs / sizeof attrs[0]; i++) {
+        os_mbuf_free_chain(attrs[i].om);
     }
 
     if (rc != 0) {
         console_printf("error writing characteristic; rc=%d\n", rc);
-        return rc;
     }
 
-    return 0;
+    return rc;
 }
 
 /*****************************************************************************
@@ -1838,14 +1946,17 @@ cmd_keystore_parse_keydata(int argc, char **argv, union ble_store_key *out,
     int rc;
 
     memset(out, 0, sizeof(*out));
-    *obj_type = parse_arg_kv("type", cmd_keystore_entry_type);
+    *obj_type = parse_arg_kv("type", cmd_keystore_entry_type, &rc);
+    if (rc != 0) {
+        return rc;
+    }
 
     switch (*obj_type) {
     case BLE_STORE_OBJ_TYPE_PEER_SEC:
     case BLE_STORE_OBJ_TYPE_OUR_SEC:
-        rc = parse_arg_kv("addr_type", cmd_keystore_addr_type);
-        if (rc < 0) {
-            return EINVAL;
+        rc = parse_arg_kv("addr_type", cmd_keystore_addr_type, &rc);
+        if (rc != 0) {
+            return rc;
         }
 
         rc = parse_arg_mac("addr", out->sec.peer_addr);
@@ -1865,7 +1976,7 @@ cmd_keystore_parse_keydata(int argc, char **argv, union ble_store_key *out,
         return 0;
 
     default:
-        return -1;
+        return EINVAL;
     }
 }
 
@@ -2013,11 +2124,11 @@ static int
 cmd_keystore_show(int argc, char **argv)
 {
     int type;
+    int rc;
 
-    type = parse_arg_kv("type", cmd_keystore_entry_type);
-
-    if (type < 0) {
-        return type;
+    type = parse_arg_kv("type", cmd_keystore_entry_type, &rc);
+    if (rc != 0) {
+        return rc;
     }
 
     ble_store_iterate(type, &cmd_keystore_iterator, NULL);
@@ -2088,7 +2199,7 @@ cmd_passkey(int argc, char **argv)
             break;
 
         case BLE_SM_IOACT_NUMCMP:
-            yesno = parse_arg_find("yesno");
+            yesno = parse_arg_extract("yesno");
             if (yesno == NULL) {
                 return EINVAL;
             }
diff --git a/apps/bletiny/src/gatt_svr.c b/apps/bletiny/src/gatt_svr.c
index e380b3c8..ca920e21 100644
--- a/apps/bletiny/src/gatt_svr.c
+++ b/apps/bletiny/src/gatt_svr.c
@@ -26,99 +26,46 @@
 #include "bletiny.h"
 
 /**
- * The vendor specific "bleprph" service consists of two characteristics:
- *     o "read": a single-byte characteristic that can only be read of an
- *       encryptted connection.
- *     o "write": a single-byte characteristic that can always be read, but
- *       can only be written over an encrypted connection.
+ * The vendor specific security test service consists of two characteristics:
+ *     o random-number-generator: generates a random 32-bit number each time
+ *       it is read.  This characteristic can only be read over an encrypted
+ *       connection.
+ *     o static-value: a single-byte characteristic that can always be read,
+ *       but can only be written over an encrypted connection.
  */
 
 /* 59462f12-9543-9999-12c8-58b459a2712d */
-const uint8_t gatt_svr_svc_bleprph[16] = {
+const uint8_t gatt_svr_svc_sec_test_uuid[16] = {
     0x2d, 0x71, 0xa2, 0x59, 0xb4, 0x58, 0xc8, 0x12,
     0x99, 0x99, 0x43, 0x95, 0x12, 0x2f, 0x46, 0x59
 };
 
 /* 5c3a659e-897e-45e1-b016-007107c96df6 */
-const uint8_t gatt_svr_chr_bleprph_read[16] = {
+const uint8_t gatt_svr_chr_sec_test_rand_uuid[16] = {
     0xf6, 0x6d, 0xc9, 0x07, 0x71, 0x00, 0x16, 0xb0,
     0xe1, 0x45, 0x7e, 0x89, 0x9e, 0x65, 0x3a, 0x5c
 };
 
 /* 5c3a659e-897e-45e1-b016-007107c96df7 */
-const uint8_t gatt_svr_chr_bleprph_write[16] = {
+const uint8_t gatt_svr_chr_sec_test_static_uuid[16] = {
     0xf7, 0x6d, 0xc9, 0x07, 0x71, 0x00, 0x16, 0xb0,
     0xe1, 0x45, 0x7e, 0x89, 0x9e, 0x65, 0x3a, 0x5c
 };
 
-static uint8_t gatt_svr_nimble_test_read_val;
-static uint8_t gatt_svr_nimble_test_write_val;
+static uint8_t gatt_svr_sec_test_static_val;
 
 static int
-gatt_svr_chr_access_gap(uint16_t conn_handle, uint16_t attr_handle, uint8_t op,
-                        union ble_gatt_access_ctxt *ctxt, void *arg);
-static int
-gatt_svr_chr_access_gatt(uint16_t conn_handle, uint16_t attr_handle, uint8_t op,
-                         union ble_gatt_access_ctxt *ctxt, void *arg);
-static int
 gatt_svr_chr_access_alert(uint16_t conn_handle, uint16_t attr_handle,
-                          uint8_t op, union ble_gatt_access_ctxt *ctxt,
+                          struct ble_gatt_access_ctxt *ctxt,
                           void *arg);
 
 static int
-gatt_svr_chr_access_bleprph(uint16_t conn_handle, uint16_t attr_handle,
-                                uint8_t op, union ble_gatt_access_ctxt *ctxt,
-                                void *arg);
+gatt_svr_chr_access_sec_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_svcs[] = {
     {
-        /*** Service: GAP. */
-        .type = BLE_GATT_SVC_TYPE_PRIMARY,
-        .uuid128 = BLE_UUID16(BLE_GAP_SVC_UUID16),
-        .characteristics = (struct ble_gatt_chr_def[]) { {
-            /*** Characteristic: Device Name. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_DEVICE_NAME),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_READ,
-        }, {
-            /*** Characteristic: Appearance. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_APPEARANCE),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_READ,
-        }, {
-            /*** Characteristic: Peripheral Privacy Flag. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_PERIPH_PRIV_FLAG),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_READ,
-        }, {
-            /*** Characteristic: Reconnection Address. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_RECONNECT_ADDR),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_WRITE,
-        }, {
-            /*** Characteristic: Peripheral Preferred Connection Parameters. */
-            .uuid128 = BLE_UUID16(BLE_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS),
-            .access_cb = gatt_svr_chr_access_gap,
-            .flags = BLE_GATT_CHR_F_READ,
-        }, {
-            0, /* No more characteristics in this service. */
-        } },
-    },
-
-    {
-        /*** Service: GATT */
-        .type = BLE_GATT_SVC_TYPE_PRIMARY,
-        .uuid128 = BLE_UUID16(BLE_GATT_SVC_UUID16),
-        .characteristics = (struct ble_gatt_chr_def[]) { {
-            .uuid128 = BLE_UUID16(BLE_GATT_CHR_SERVICE_CHANGED_UUID16),
-            .access_cb = gatt_svr_chr_access_gatt,
-            .flags = BLE_GATT_CHR_F_INDICATE,
-        }, {
-            0, /* No more characteristics in this service. */
-        } },
-    },
-
-    {
         /*** Alert Notification Service. */
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(GATT_SVR_SVC_ALERT_UUID),
@@ -148,18 +95,18 @@ static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
     },
 
     {
-        /*** Service: bleprph. */
+        /*** Service: Security test. */
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
-        .uuid128 = (void *)gatt_svr_svc_bleprph,
+        .uuid128 = gatt_svr_svc_sec_test_uuid,
         .characteristics = (struct ble_gatt_chr_def[]) { {
-            /*** Characteristic: Read. */
-            .uuid128 = (void *)gatt_svr_chr_bleprph_read,
-            .access_cb = gatt_svr_chr_access_bleprph,
+            /*** Characteristic: Random number generator. */
+            .uuid128 = gatt_svr_chr_sec_test_rand_uuid,
+            .access_cb = gatt_svr_chr_access_sec_test,
             .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_READ_ENC,
         }, {
-            /*** Characteristic: Write. */
-            .uuid128 = (void *)gatt_svr_chr_bleprph_write,
-            .access_cb = gatt_svr_chr_access_bleprph,
+            /*** Characteristic: Static value. */
+            .uuid128 = gatt_svr_chr_sec_test_static_uuid,
+            .access_cb = gatt_svr_chr_access_sec_test,
             .flags = BLE_GATT_CHR_F_READ |
                      BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_ENC,
         }, {
@@ -173,100 +120,20 @@ static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
 };
 
 static int
-gatt_svr_chr_write(uint8_t op, union ble_gatt_access_ctxt *ctxt,
-                   uint16_t min_len, uint16_t max_len, void *dst,
-                   uint16_t *len)
-{
-    assert(op == BLE_GATT_ACCESS_OP_WRITE_CHR);
-    if (ctxt->chr_access.len < min_len ||
-        ctxt->chr_access.len > max_len) {
-        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
-    }
-    memcpy(dst, ctxt->chr_access.data, ctxt->chr_access.len);
-    if (len != NULL) {
-        *len = ctxt->chr_access.len;
-    }
-
-    return 0;
-}
-
-static int
-gatt_svr_chr_access_gap(uint16_t conn_handle, uint16_t attr_handle, uint8_t op,
-                        union ble_gatt_access_ctxt *ctxt, void *arg)
+gatt_svr_chr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len,
+                   void *dst, uint16_t *len)
 {
-    uint16_t uuid16;
-
-    uuid16 = ble_uuid_128_to_16(ctxt->chr_access.chr->uuid128);
-    assert(uuid16 != 0);
-
-    switch (uuid16) {
-    case BLE_GAP_CHR_UUID16_DEVICE_NAME:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)bletiny_device_name;
-        ctxt->chr_access.len = strlen(bletiny_device_name);
-        break;
-
-    case BLE_GAP_CHR_UUID16_APPEARANCE:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&bletiny_appearance;
-        ctxt->chr_access.len = sizeof bletiny_appearance;
-        break;
-
-    case BLE_GAP_CHR_UUID16_PERIPH_PRIV_FLAG:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&bletiny_privacy_flag;
-        ctxt->chr_access.len = sizeof bletiny_privacy_flag;
-        break;
-
-    case BLE_GAP_CHR_UUID16_RECONNECT_ADDR:
-        assert(op == BLE_GATT_ACCESS_OP_WRITE_CHR);
-        if (ctxt->chr_access.len != sizeof bletiny_reconnect_addr) {
-            return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
-        }
-        memcpy(bletiny_reconnect_addr, ctxt->chr_access.data,
-               sizeof bletiny_reconnect_addr);
-        break;
-
-    case BLE_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&bletiny_pref_conn_params;
-        ctxt->chr_access.len = sizeof bletiny_pref_conn_params;
-        break;
+    uint16_t om_len;
+    int rc;
 
-    default:
-        assert(0);
-        break;
+    om_len = OS_MBUF_PKTLEN(om);
+    if (om_len < min_len || om_len > max_len) {
+        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
     }
 
-    return 0;
-}
-
-static int
-gatt_svr_chr_access_gatt(uint16_t conn_handle, uint16_t attr_handle, uint8_t op,
-                         union ble_gatt_access_ctxt *ctxt, void *arg)
-{
-    uint16_t uuid16;
-
-    uuid16 = ble_uuid_128_to_16(ctxt->chr_access.chr->uuid128);
-    assert(uuid16 != 0);
-
-    switch (uuid16) {
-    case BLE_GATT_CHR_SERVICE_CHANGED_UUID16:
-        if (op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            if (ctxt->chr_access.len != sizeof bletiny_gatt_service_changed) {
-                return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
-            }
-            memcpy(bletiny_gatt_service_changed, ctxt->chr_access.data,
-                   sizeof bletiny_gatt_service_changed);
-        } else if (op == BLE_GATT_ACCESS_OP_READ_CHR) {
-            ctxt->chr_access.data = (void *)&bletiny_gatt_service_changed;
-            ctxt->chr_access.len = sizeof bletiny_gatt_service_changed;
-        }
-        break;
-
-    default:
-        assert(0);
-        break;
+    rc = ble_hs_mbuf_to_flat(om, dst, max_len, len);
+    if (rc != 0) {
+        return BLE_ATT_ERR_UNLIKELY;
     }
 
     return 0;
@@ -283,54 +150,55 @@ static uint16_t gatt_svr_alert_not_ctrl_pt;
 
 static int
 gatt_svr_chr_access_alert(uint16_t conn_handle, uint16_t attr_handle,
-                          uint8_t op, union ble_gatt_access_ctxt *ctxt,
+                          struct ble_gatt_access_ctxt *ctxt,
                           void *arg)
 {
     uint16_t uuid16;
     int rc;
 
-    uuid16 = ble_uuid_128_to_16(ctxt->chr_access.chr->uuid128);
+    uuid16 = ble_uuid_128_to_16(ctxt->chr->uuid128);
     assert(uuid16 != 0);
 
     switch (uuid16) {
     case GATT_SVR_CHR_SUP_NEW_ALERT_CAT_UUID:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&gatt_svr_new_alert_cat;
-        ctxt->chr_access.len = sizeof gatt_svr_new_alert_cat;
-        return 0;
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &gatt_svr_new_alert_cat,
+                            sizeof gatt_svr_new_alert_cat);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
 
     case GATT_SVR_CHR_NEW_ALERT:
-        if (op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            rc = gatt_svr_chr_write(op, ctxt, 0, sizeof gatt_svr_new_alert_val,
-                                  gatt_svr_new_alert_val,
-                                  &gatt_svr_new_alert_val_len);
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = gatt_svr_chr_write(ctxt->om, 0,
+                                    sizeof gatt_svr_new_alert_val,
+                                    gatt_svr_new_alert_val,
+                                    &gatt_svr_new_alert_val_len);
             return rc;
-        } else if (op == BLE_GATT_ACCESS_OP_READ_CHR) {
-            ctxt->chr_access.data = (void *)&gatt_svr_new_alert_val;
-            ctxt->chr_access.len = sizeof gatt_svr_new_alert_val;
-            return 0;
+        } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+            rc = os_mbuf_append(ctxt->om, &gatt_svr_new_alert_val,
+                                sizeof gatt_svr_new_alert_val);
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
         }
 
     case GATT_SVR_CHR_SUP_UNR_ALERT_CAT_UUID:
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = (void *)&gatt_svr_unr_alert_cat;
-        ctxt->chr_access.len = sizeof gatt_svr_unr_alert_cat;
-        return 0;
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &gatt_svr_unr_alert_cat,
+                            sizeof gatt_svr_unr_alert_cat);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
 
     case GATT_SVR_CHR_UNR_ALERT_STAT_UUID:
-        if (op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            rc = gatt_svr_chr_write(op, ctxt, 2, 2, &gatt_svr_unr_alert_stat,
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = gatt_svr_chr_write(ctxt->om, 2, 2, &gatt_svr_unr_alert_stat,
                                     NULL);
+            return rc;
         } else {
-            ctxt->chr_access.data = (void *)&gatt_svr_unr_alert_stat;
-            ctxt->chr_access.len = sizeof gatt_svr_unr_alert_stat;
-            rc = 0;
+            rc = os_mbuf_append(ctxt->om, &gatt_svr_unr_alert_stat,
+                                sizeof gatt_svr_unr_alert_stat);
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
         }
-        return rc;
 
     case GATT_SVR_CHR_ALERT_NOT_CTRL_PT:
-        if (op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
-            rc = gatt_svr_chr_write(op, ctxt, 2, 2,
+        if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
+            rc = gatt_svr_chr_write(ctxt->om, 2, 2,
                                     &gatt_svr_alert_not_ctrl_pt, NULL);
         } else {
             rc = BLE_ATT_ERR_UNLIKELY;
@@ -344,38 +212,41 @@ gatt_svr_chr_access_alert(uint16_t conn_handle, uint16_t attr_handle,
 }
 
 static int
-gatt_svr_chr_access_bleprph(uint16_t conn_handle, uint16_t attr_handle,
-                            uint8_t op, union ble_gatt_access_ctxt *ctxt,
-                            void *arg)
+gatt_svr_chr_access_sec_test(uint16_t conn_handle, uint16_t attr_handle,
+                             struct ble_gatt_access_ctxt *ctxt,
+                             void *arg)
 {
-    void *uuid128;
+    const void *uuid128;
+    int rand_num;
     int rc;
 
-    uuid128 = ctxt->chr_access.chr->uuid128;
+    uuid128 = ctxt->chr->uuid128;
 
     /* Determine which characteristic is being accessed by examining its
      * 128-bit UUID.
      */
 
-    if (memcmp(uuid128, gatt_svr_chr_bleprph_read, 16) == 0) {
-        assert(op == BLE_GATT_ACCESS_OP_READ_CHR);
-        ctxt->chr_access.data = &gatt_svr_nimble_test_read_val;
-        ctxt->chr_access.len = sizeof gatt_svr_nimble_test_read_val;
-        return 0;
+    if (memcmp(uuid128, gatt_svr_chr_sec_test_rand_uuid, 16) == 0) {
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+
+        /* Respond with a 32-bit random number. */
+        rand_num = rand();
+        rc = os_mbuf_append(ctxt->om, &rand_num, sizeof rand_num);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
     }
 
-    if (memcmp(uuid128, gatt_svr_chr_bleprph_write, 16) == 0) {
-        switch (op) {
+    if (memcmp(uuid128, gatt_svr_chr_sec_test_static_uuid, 16) == 0) {
+        switch (ctxt->op) {
         case BLE_GATT_ACCESS_OP_READ_CHR:
-            ctxt->chr_access.data = &gatt_svr_nimble_test_write_val;
-            ctxt->chr_access.len = sizeof gatt_svr_nimble_test_write_val;
-            return 0;
+            rc = os_mbuf_append(ctxt->om, &gatt_svr_sec_test_static_val,
+                                sizeof gatt_svr_sec_test_static_val);
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
 
         case BLE_GATT_ACCESS_OP_WRITE_CHR:
-            rc = gatt_svr_chr_write(op, ctxt,
-                                    sizeof gatt_svr_nimble_test_write_val,
-                                    sizeof gatt_svr_nimble_test_write_val,
-                                    &gatt_svr_nimble_test_write_val, NULL);
+            rc = gatt_svr_chr_write(ctxt->om,
+                                    sizeof gatt_svr_sec_test_static_val,
+                                    sizeof gatt_svr_sec_test_static_val,
+                                    &gatt_svr_sec_test_static_val, NULL);
             return rc;
 
         default:
@@ -392,10 +263,10 @@ gatt_svr_chr_access_bleprph(uint16_t conn_handle, uint16_t attr_handle,
 }
 
 static char *
-gatt_svr_uuid_to_s(void *uuid128, char *dst)
+gatt_svr_uuid_to_s(const void *uuid128, char *dst)
 {
+    const uint8_t *u8p;
     uint16_t uuid16;
-    uint8_t *u8p;
 
     uuid16 = ble_uuid_128_to_16(uuid128);
     if (uuid16 != 0) {
@@ -413,32 +284,30 @@ gatt_svr_uuid_to_s(void *uuid128, char *dst)
     return dst;
 }
 
-static void
-gatt_svr_register_cb(uint8_t op, union ble_gatt_register_ctxt *ctxt, void *arg)
+void
+gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
 {
     char buf[40];
 
-    switch (op) {
+    switch (ctxt->op) {
     case BLE_GATT_REGISTER_OP_SVC:
         BLETINY_LOG(DEBUG, "registered service %s with handle=%d\n",
-                    gatt_svr_uuid_to_s(ctxt->svc_reg.svc->uuid128, buf),
-                    ctxt->svc_reg.handle);
+                    gatt_svr_uuid_to_s(ctxt->svc.svc_def->uuid128, buf),
+                    ctxt->svc.handle);
         break;
 
     case BLE_GATT_REGISTER_OP_CHR:
         BLETINY_LOG(DEBUG, "registering characteristic %s with "
                            "def_handle=%d val_handle=%d\n",
-                    gatt_svr_uuid_to_s(ctxt->chr_reg.chr->uuid128, buf),
-                    ctxt->chr_reg.def_handle,
-                    ctxt->chr_reg.val_handle);
+                    gatt_svr_uuid_to_s(ctxt->chr.chr_def->uuid128, buf),
+                    ctxt->chr.def_handle,
+                    ctxt->chr.val_handle);
         break;
 
     case BLE_GATT_REGISTER_OP_DSC:
-        BLETINY_LOG(DEBUG, "registering descriptor %s with handle=%d "
-                           "chr_handle=%d\n",
-                    gatt_svr_uuid_to_s(ctxt->dsc_reg.dsc->uuid128, buf),
-                    ctxt->dsc_reg.dsc_handle,
-                    ctxt->dsc_reg.chr_def_handle);
+        BLETINY_LOG(DEBUG, "registering descriptor %s with handle=%d\n",
+                    gatt_svr_uuid_to_s(ctxt->dsc.dsc_def->uuid128, buf),
+                    ctxt->dsc.handle);
         break;
 
     default:
@@ -447,11 +316,29 @@ gatt_svr_register_cb(uint8_t op, union ble_gatt_register_ctxt *ctxt, void *arg)
     }
 }
 
-void
-gatt_svr_init(void)
+int
+gatt_svr_register(void)
 {
     int rc;
 
     rc = ble_gatts_register_svcs(gatt_svr_svcs, gatt_svr_register_cb, NULL);
-    assert(rc == 0);
+    return rc;
+}
+
+int
+gatt_svr_init(struct ble_hs_cfg *cfg)
+{
+    int rc;
+
+    rc = ble_gatts_count_cfg(gatt_svr_svcs, cfg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_gatts_add_svcs(gatt_svr_svcs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
 }
diff --git a/apps/bletiny/src/main.c b/apps/bletiny/src/main.c
index edb19844..d06fa0ac 100755
--- a/apps/bletiny/src/main.c
+++ b/apps/bletiny/src/main.c
@@ -35,8 +35,8 @@
 /* BLE */
 #include "nimble/ble.h"
 #include "nimble/nimble_opt.h"
-#include "nimble/hci_transport.h"
-#include "host/host_hci.h"
+#include "nimble/ble_hci_trans.h"
+#include "controller/ble_ll.h"
 #include "host/ble_hs.h"
 #include "host/ble_hs_adv.h"
 #include "host/ble_uuid.h"
@@ -45,16 +45,23 @@
 #include "host/ble_gatt.h"
 #include "host/ble_store.h"
 #include "host/ble_sm.h"
-#include "controller/ble_ll.h"
+
+/* RAM HCI transport. */
+#include "transport/ram/ble_hci_ram.h"
+
+/* RAM persistence layer. */
+#include "store/ram/ble_store_ram.h"
+
+/* Mandatory services. */
+#include "services/gap/ble_svc_gap.h"
+#include "services/gatt/ble_svc_gatt.h"
 
 /* XXX: An app should not include private headers from a library.  The bletiny
  * app uses some of nimble's internal details for logging.
  */
 #include "../src/ble_hs_conn_priv.h"
-#include "../src/ble_hci_util_priv.h"
 #include "../src/ble_hs_atomic_priv.h"
-
-#define BSWAP16(x)  ((uint16_t)(((x) << 8) | (((x) & 0xff00) >> 8)))
+#include "../src/ble_hs_hci_priv.h"
 
 /* Nimble task priorities */
 #define BLE_LL_TASK_PRI         (OS_TASK_PRI_HIGHEST)
@@ -120,13 +127,6 @@ static struct os_mempool bletiny_chr_pool;
 static void *bletiny_dsc_mem;
 static struct os_mempool bletiny_dsc_pool;
 
-const char *bletiny_device_name = "nimble-bletiny";
-const uint16_t bletiny_appearance = BSWAP16(BLE_GAP_APPEARANCE_GEN_COMPUTER);
-const uint8_t bletiny_privacy_flag = 0;
-uint8_t bletiny_reconnect_addr[6];
-uint8_t bletiny_pref_conn_params[8];
-uint8_t bletiny_gatt_service_changed[4];
-
 static struct os_callout_func bletiny_tx_timer;
 struct bletiny_tx_data_s
 {
@@ -136,7 +136,7 @@ struct bletiny_tx_data_s
     uint16_t tx_len;
 };
 static struct bletiny_tx_data_s bletiny_tx_data;
-int bletiny_full_disc_prev_chr_def;
+int bletiny_full_disc_prev_chr_val;
 
 #define XSTR(s) STR(s)
 #define STR(s) #s
@@ -149,7 +149,7 @@ int bletiny_full_disc_prev_chr_def;
 
 static void
 bletiny_print_error(char *msg, uint16_t conn_handle,
-                    struct ble_gatt_error *error)
+                    const struct ble_gatt_error *error)
 {
     if (msg == NULL) {
         msg = "ERROR";
@@ -160,10 +160,8 @@ bletiny_print_error(char *msg, uint16_t conn_handle,
 }
 
 static void
-bletiny_print_adv_fields(struct ble_hs_adv_fields *fields)
+bletiny_print_adv_fields(const struct ble_hs_adv_fields *fields)
 {
-    uint32_t u32;
-    uint16_t u16;
     uint8_t *u8p;
     int i;
 
@@ -174,10 +172,8 @@ bletiny_print_adv_fields(struct ble_hs_adv_fields *fields)
     if (fields->uuids16 != NULL) {
         console_printf("    uuids16(%scomplete)=",
                        fields->uuids16_is_complete ? "" : "in");
-        u8p = fields->uuids16;
         for (i = 0; i < fields->num_uuids16; i++) {
-            memcpy(&u16, u8p + i * 2, 2);
-            console_printf("0x%04x ", u16);
+            console_printf("0x%04x ", fields->uuids16[i]);
         }
         console_printf("\n");
     }
@@ -185,10 +181,8 @@ bletiny_print_adv_fields(struct ble_hs_adv_fields *fields)
     if (fields->uuids32 != NULL) {
         console_printf("    uuids32(%scomplete)=",
                        fields->uuids32_is_complete ? "" : "in");
-        u8p = fields->uuids32;
         for (i = 0; i < fields->num_uuids32; i++) {
-            memcpy(&u32, u8p + i * 4, 4);
-            console_printf("0x%08x ", (unsigned)u32);
+            console_printf("0x%08x ", (unsigned int)fields->uuids32[i]);
         }
         console_printf("\n");
     }
@@ -209,6 +203,7 @@ bletiny_print_adv_fields(struct ble_hs_adv_fields *fields)
         console_printf("    name(%scomplete)=",
                        fields->name_is_complete ? "" : "in");
         console_write((char *)fields->name, fields->name_len);
+        console_printf("\n");
     }
 
     if (fields->tx_pwr_lvl_is_present) {
@@ -338,7 +333,7 @@ bletiny_svc_find_prev(struct bletiny_conn *conn, uint16_t svc_start_handle)
 
 static struct bletiny_svc *
 bletiny_svc_find(struct bletiny_conn *conn, uint16_t svc_start_handle,
-                  struct bletiny_svc **out_prev)
+                 struct bletiny_svc **out_prev)
 {
     struct bletiny_svc *prev;
     struct bletiny_svc *svc;
@@ -403,7 +398,7 @@ bletiny_svc_delete(struct bletiny_svc *svc)
 }
 
 static struct bletiny_svc *
-bletiny_svc_add(uint16_t conn_handle, struct ble_gatt_svc *gatt_svc)
+bletiny_svc_add(uint16_t conn_handle, const struct ble_gatt_svc *gatt_svc)
 {
     struct bletiny_conn *conn;
     struct bletiny_svc *prev;
@@ -443,14 +438,14 @@ bletiny_svc_add(uint16_t conn_handle, struct ble_gatt_svc *gatt_svc)
 }
 
 static struct bletiny_chr *
-bletiny_chr_find_prev(struct bletiny_svc *svc, uint16_t chr_def_handle)
+bletiny_chr_find_prev(const struct bletiny_svc *svc, uint16_t chr_val_handle)
 {
     struct bletiny_chr *prev;
     struct bletiny_chr *chr;
 
     prev = NULL;
     SLIST_FOREACH(chr, &svc->chrs, next) {
-        if (chr->chr.def_handle >= chr_def_handle) {
+        if (chr->chr.val_handle >= chr_val_handle) {
             break;
         }
 
@@ -461,20 +456,20 @@ bletiny_chr_find_prev(struct bletiny_svc *svc, uint16_t chr_def_handle)
 }
 
 static struct bletiny_chr *
-bletiny_chr_find(struct bletiny_svc *svc, uint16_t chr_def_handle,
-                  struct bletiny_chr **out_prev)
+bletiny_chr_find(const struct bletiny_svc *svc, uint16_t chr_val_handle,
+                 struct bletiny_chr **out_prev)
 {
     struct bletiny_chr *prev;
     struct bletiny_chr *chr;
 
-    prev = bletiny_chr_find_prev(svc, chr_def_handle);
+    prev = bletiny_chr_find_prev(svc, chr_val_handle);
     if (prev == NULL) {
         chr = SLIST_FIRST(&svc->chrs);
     } else {
         chr = SLIST_NEXT(prev, next);
     }
 
-    if (chr != NULL && chr->chr.def_handle != chr_def_handle) {
+    if (chr != NULL && chr->chr.val_handle != chr_val_handle) {
         chr = NULL;
     }
 
@@ -484,10 +479,9 @@ bletiny_chr_find(struct bletiny_svc *svc, uint16_t chr_def_handle,
     return chr;
 }
 
-
 static struct bletiny_chr *
 bletiny_chr_add(uint16_t conn_handle,  uint16_t svc_start_handle,
-                 struct ble_gatt_chr *gatt_chr)
+                const struct ble_gatt_chr *gatt_chr)
 {
     struct bletiny_conn *conn;
     struct bletiny_chr *prev;
@@ -509,7 +503,7 @@ bletiny_chr_add(uint16_t conn_handle,  uint16_t svc_start_handle,
         return NULL;
     }
 
-    chr = bletiny_chr_find(svc, gatt_chr->def_handle, &prev);
+    chr = bletiny_chr_find(svc, gatt_chr->val_handle, &prev);
     if (chr != NULL) {
         /* Characteristic already discovered. */
         return chr;
@@ -534,7 +528,7 @@ bletiny_chr_add(uint16_t conn_handle,  uint16_t svc_start_handle,
 }
 
 static struct bletiny_dsc *
-bletiny_dsc_find_prev(struct bletiny_chr *chr, uint16_t dsc_handle)
+bletiny_dsc_find_prev(const struct bletiny_chr *chr, uint16_t dsc_handle)
 {
     struct bletiny_dsc *prev;
     struct bletiny_dsc *dsc;
@@ -552,8 +546,8 @@ bletiny_dsc_find_prev(struct bletiny_chr *chr, uint16_t dsc_handle)
 }
 
 static struct bletiny_dsc *
-bletiny_dsc_find(struct bletiny_chr *chr, uint16_t dsc_handle,
-                  struct bletiny_dsc **out_prev)
+bletiny_dsc_find(const struct bletiny_chr *chr, uint16_t dsc_handle,
+                 struct bletiny_dsc **out_prev)
 {
     struct bletiny_dsc *prev;
     struct bletiny_dsc *dsc;
@@ -576,8 +570,8 @@ bletiny_dsc_find(struct bletiny_chr *chr, uint16_t dsc_handle,
 }
 
 static struct bletiny_dsc *
-bletiny_dsc_add(uint16_t conn_handle, uint16_t chr_def_handle,
-                 struct ble_gatt_dsc *gatt_dsc)
+bletiny_dsc_add(uint16_t conn_handle, uint16_t chr_val_handle,
+                const struct ble_gatt_dsc *gatt_dsc)
 {
     struct bletiny_conn *conn;
     struct bletiny_dsc *prev;
@@ -593,14 +587,14 @@ bletiny_dsc_add(uint16_t conn_handle, uint16_t chr_def_handle,
         return NULL;
     }
 
-    svc = bletiny_svc_find_range(conn, chr_def_handle);
+    svc = bletiny_svc_find_range(conn, chr_val_handle);
     if (svc == NULL) {
         BLETINY_LOG(DEBUG, "CAN'T FIND SERVICE FOR DISCOVERED DSC; HANDLE=%d\n",
                     conn_handle);
         return NULL;
     }
 
-    chr = bletiny_chr_find(svc, chr_def_handle, NULL);
+    chr = bletiny_chr_find(svc, chr_val_handle, NULL);
     if (chr == NULL) {
         BLETINY_LOG(DEBUG, "CAN'T FIND CHARACTERISTIC FOR DISCOVERED DSC; "
                            "HANDLE=%d\n",
@@ -676,14 +670,18 @@ bletiny_conn_delete_idx(int idx)
 }
 
 static int
-bletiny_on_mtu(uint16_t conn_handle, struct ble_gatt_error *error,
+bletiny_on_mtu(uint16_t conn_handle, const struct ble_gatt_error *error,
                uint16_t mtu, void *arg)
 {
-    if (error != NULL) {
-        bletiny_print_error(NULL, conn_handle, error);
-    } else {
+    switch (error->status) {
+    case 0:
         console_printf("mtu exchange complete: conn_handle=%d mtu=%d\n",
                        conn_handle, mtu);
+        break;
+
+    default:
+        bletiny_print_error(NULL, conn_handle, error);
+        break;
     }
 
     return 0;
@@ -693,7 +691,7 @@ static void
 bletiny_full_disc_complete(int rc)
 {
     console_printf("full discovery complete; rc=%d\n", rc);
-    bletiny_full_disc_prev_chr_def = 0;
+    bletiny_full_disc_prev_chr_val = 0;
 }
 
 static void
@@ -716,16 +714,16 @@ bletiny_disc_full_dscs(uint16_t conn_handle)
         SLIST_FOREACH(chr, &svc->chrs, next) {
             if (!chr_is_empty(svc, chr) &&
                 SLIST_EMPTY(&chr->dscs) &&
-                bletiny_full_disc_prev_chr_def <= chr->chr.def_handle) {
+                bletiny_full_disc_prev_chr_val <= chr->chr.def_handle) {
 
                 rc = bletiny_disc_all_dscs(conn_handle,
-                                           chr->chr.def_handle,
+                                           chr->chr.val_handle,
                                            chr_end_handle(svc, chr));
                 if (rc != 0) {
                     bletiny_full_disc_complete(rc);
                 }
 
-                bletiny_full_disc_prev_chr_def = chr->chr.val_handle;
+                bletiny_full_disc_prev_chr_val = chr->chr.val_handle;
                 return;
             }
         }
@@ -766,193 +764,280 @@ bletiny_disc_full_chrs(uint16_t conn_handle)
 }
 
 static int
-bletiny_on_disc_s(uint16_t conn_handle, struct ble_gatt_error *error,
-                  struct ble_gatt_svc *service, void *arg)
+bletiny_on_disc_s(uint16_t conn_handle, const struct ble_gatt_error *error,
+                  const struct ble_gatt_svc *service, void *arg)
 {
-    if (error != NULL) {
-        bletiny_print_error(NULL, conn_handle, error);
-    } else if (service != NULL) {
+    switch (error->status) {
+    case 0:
         bletiny_svc_add(conn_handle, service);
-    } else {
+        break;
+
+    case BLE_HS_EDONE:
         console_printf("service discovery successful\n");
-        if (bletiny_full_disc_prev_chr_def > 0) {
+        if (bletiny_full_disc_prev_chr_val > 0) {
             bletiny_disc_full_chrs(conn_handle);
         }
+        break;
+
+    default:
+        bletiny_print_error(NULL, conn_handle, error);
+        break;
     }
 
     return 0;
 }
 
 static int
-bletiny_on_disc_c(uint16_t conn_handle, struct ble_gatt_error *error,
-                  struct ble_gatt_chr *chr, void *arg)
+bletiny_on_disc_c(uint16_t conn_handle, const struct ble_gatt_error *error,
+                  const struct ble_gatt_chr *chr, void *arg)
 {
     intptr_t svc_start_handle;
 
     svc_start_handle = (intptr_t)arg;
 
-    if (error != NULL) {
-        bletiny_print_error(NULL, conn_handle, error);
-    } else if (chr != NULL) {
+    switch (error->status) {
+    case 0:
         bletiny_chr_add(conn_handle, svc_start_handle, chr);
-    } else {
+        break;
+
+    case BLE_HS_EDONE:
         console_printf("characteristic discovery successful\n");
-        if (bletiny_full_disc_prev_chr_def > 0) {
+        if (bletiny_full_disc_prev_chr_val > 0) {
             bletiny_disc_full_chrs(conn_handle);
         }
+        break;
+
+    default:
+        bletiny_print_error(NULL, conn_handle, error);
+        break;
     }
 
     return 0;
 }
 
 static int
-bletiny_on_disc_d(uint16_t conn_handle, struct ble_gatt_error *error,
-                   uint16_t chr_def_handle, struct ble_gatt_dsc *dsc,
-                   void *arg)
+bletiny_on_disc_d(uint16_t conn_handle, const struct ble_gatt_error *error,
+                  uint16_t chr_val_handle, const struct ble_gatt_dsc *dsc,
+                  void *arg)
 {
-    if (error != NULL) {
-        bletiny_print_error(NULL, conn_handle, error);
-    } else if (dsc != NULL) {
-        bletiny_dsc_add(conn_handle, chr_def_handle, dsc);
-    } else {
+    switch (error->status) {
+    case 0:
+        bletiny_dsc_add(conn_handle, chr_val_handle, dsc);
+        break;
+
+    case BLE_HS_EDONE:
         console_printf("descriptor discovery successful\n");
-        if (bletiny_full_disc_prev_chr_def > 0) {
+        if (bletiny_full_disc_prev_chr_val > 0) {
             bletiny_disc_full_dscs(conn_handle);
         }
+        break;
+
+    default:
+        bletiny_print_error(NULL, conn_handle, error);
+        break;
     }
 
     return 0;
 }
 
 static int
-bletiny_on_read(uint16_t conn_handle, struct ble_gatt_error *error,
-                 struct ble_gatt_attr *attr, void *arg)
+bletiny_on_read(uint16_t conn_handle, const struct ble_gatt_error *error,
+                struct ble_gatt_attr *attr, void *arg)
 {
-    if (error != NULL) {
-        bletiny_print_error(NULL, conn_handle, error);
-    } else if (attr != NULL) {
+    switch (error->status) {
+    case 0:
         console_printf("characteristic read; conn_handle=%d "
                        "attr_handle=%d len=%d value=", conn_handle,
-                       attr->handle, attr->value_len);
-        print_bytes(attr->value, attr->value_len);
+                       attr->handle, OS_MBUF_PKTLEN(attr->om));
+        print_mbuf(attr->om);
         console_printf("\n");
-    } else {
+        break;
+
+    case BLE_HS_EDONE:
         console_printf("characteristic read complete\n");
+        break;
+
+    default:
+        bletiny_print_error(NULL, conn_handle, error);
+        break;
     }
 
     return 0;
 }
 
 static int
-bletiny_on_write(uint16_t conn_handle, struct ble_gatt_error *error,
-                  struct ble_gatt_attr *attr, void *arg)
+bletiny_on_write(uint16_t conn_handle, const struct ble_gatt_error *error,
+                 struct ble_gatt_attr *attr, void *arg)
 {
-    if (error != NULL) {
-        bletiny_print_error(NULL, conn_handle, error);
-    } else {
+    switch (error->status) {
+    case 0:
         console_printf("characteristic write complete; conn_handle=%d "
-                       "attr_handle=%d len=%d value=", conn_handle,
-                       attr->handle, attr->value_len);
-        print_bytes(attr->value, attr->value_len);
-        console_printf("\n");
+                       "attr_handle=%d\n", conn_handle, attr->handle);
+        break;
+
+    default:
+        bletiny_print_error(NULL, conn_handle, error);
+        break;
     }
 
     return 0;
 }
 
 static int
-bletiny_on_write_reliable(uint16_t conn_handle, struct ble_gatt_error *error,
-                           struct ble_gatt_attr *attrs, uint8_t num_attrs,
-                           void *arg)
+bletiny_on_write_reliable(uint16_t conn_handle,
+                          const struct ble_gatt_error *error,
+                          struct ble_gatt_attr *attrs, uint8_t num_attrs,
+                          void *arg)
 {
     int i;
 
-    if (error != NULL) {
-        bletiny_print_error(NULL, conn_handle, error);
-    } else {
+    switch (error->status) {
+    case 0:
         console_printf("characteristic write reliable complete; "
                        "conn_handle=%d", conn_handle);
 
         for (i = 0; i < num_attrs; i++) {
             console_printf(" attr_handle=%d len=%d value=", attrs[i].handle,
-                           attrs[i].value_len);
-            print_bytes(attrs[i].value, attrs[i].value_len);
+                           OS_MBUF_PKTLEN(attrs[i].om));
+            print_mbuf(attrs[i].om);
         }
         console_printf("\n");
+        break;
+
+    default:
+        bletiny_print_error(NULL, conn_handle, error);
+        break;
     }
 
     return 0;
 }
 
 static int
-bletiny_gap_event(int event, struct ble_gap_conn_ctxt *ctxt, void *arg)
+bletiny_gap_event(struct ble_gap_event *event, void *arg)
 {
+    struct ble_gap_conn_desc desc;
     int conn_idx;
+    int rc;
 
-    switch (event) {
+    switch (event->type) {
     case BLE_GAP_EVENT_CONNECT:
-        
         console_printf("connection %s; status=%d ",
-                       ctxt->connect.status == 0 ? "established" : "failed",
-                       ctxt->connect.status);
-        print_conn_desc(ctxt->desc);
-
-        if (ctxt->connect.status == 0) {
-            bletiny_conn_add(ctxt->desc);
+                       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);
+            bletiny_conn_add(&desc);
         }
         return 0;
 
     case BLE_GAP_EVENT_DISCONNECT:
-        console_printf("disconnect; reason=%d ", ctxt->disconnect.reason);
-        print_conn_desc(ctxt->desc);
+        console_printf("disconnect; reason=%d ", event->disconnect.reason);
+        print_conn_desc(&event->disconnect.conn);
 
-        conn_idx = bletiny_conn_find_idx(ctxt->desc->conn_handle);
+        conn_idx = bletiny_conn_find_idx(event->disconnect.conn.conn_handle);
         if (conn_idx != -1) {
             bletiny_conn_delete_idx(conn_idx);
         }
         return 0;
 
+    case BLE_GAP_EVENT_DISC:
+        console_printf("received advertisement; event_type=%d addr_type=%d "
+                       "addr=", event->disc.event_type,
+                       event->disc.addr_type);
+        print_addr(event->disc.addr);
+        console_printf(" length_data=%d rssi=%d data=",
+                       event->disc.length_data, event->disc.rssi);
+        print_bytes(event->disc.data, event->disc.length_data);
+        console_printf(" fields:\n");
+        bletiny_print_adv_fields(event->disc.fields);
+        console_printf("\n");
+        return 0;
+
+    case BLE_GAP_EVENT_DISC_COMPLETE:
+        console_printf("scanning finished\n");
+        return 0;
+
+    case BLE_GAP_EVENT_ADV_COMPLETE:
+        console_printf("advertising complete.\n");
+        return 0;
+
     case BLE_GAP_EVENT_CONN_CANCEL:
         console_printf("connection procedure cancelled.\n");
         return 0;
 
     case BLE_GAP_EVENT_CONN_UPDATE:
         console_printf("connection updated; status=%d ",
-                       ctxt->conn_update.status);
-        print_conn_desc(ctxt->desc);
+                       event->conn_update.status);
+        rc = ble_gap_conn_find(event->conn_update.conn_handle, &desc);
+        assert(rc == 0);
+        print_conn_desc(&desc);
         return 0;
 
     case BLE_GAP_EVENT_CONN_UPDATE_REQ:
         console_printf("connection update request\n");
-        *ctxt->conn_update_req.self_params =
-            *ctxt->conn_update_req.peer_params;
+        *event->conn_update_req.self_params =
+            *event->conn_update_req.peer_params;
         return 0;
 
     case BLE_GAP_EVENT_PASSKEY_ACTION:
         console_printf("passkey action event; action=%d",
-                       ctxt->passkey_action.action);
-        if (ctxt->passkey_action.action == BLE_SM_IOACT_NUMCMP) {
+                       event->passkey.params.action);
+        if (event->passkey.params.action == BLE_SM_IOACT_NUMCMP) {
             console_printf(" numcmp=%lu",
-                           (unsigned long)ctxt->passkey_action.numcmp);
+                           (unsigned long)event->passkey.params.numcmp);
         }
         console_printf("\n");
         return 0;
 
     case BLE_GAP_EVENT_ENC_CHANGE:
         console_printf("encryption change event; status=%d ",
-                       ctxt->enc_change.status);
-        print_conn_desc(ctxt->desc);
+                       event->enc_change.status);
+        rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
+        assert(rc == 0);
+        print_conn_desc(&desc);
         return 0;
 
-    case BLE_GAP_EVENT_NOTIFY:
-        console_printf("notification event; attr_handle=%d indication=%d "
+    case BLE_GAP_EVENT_NOTIFY_RX:
+        console_printf("notification rx event; attr_handle=%d indication=%d "
                        "len=%d data=",
-                       ctxt->notify.attr_handle, ctxt->notify.indication,
-                       ctxt->notify.attr_len);
+                       event->notify_rx.attr_handle,
+                       event->notify_rx.indication,
+                       OS_MBUF_PKTLEN(event->notify_rx.om));
 
-        print_bytes(ctxt->notify.attr_data, ctxt->notify.attr_len);
+        print_mbuf(event->notify_rx.om);
         console_printf("\n");
         return 0;
+
+    case BLE_GAP_EVENT_NOTIFY_TX:
+        console_printf("notification tx event; status=%d attr_handle=%d "
+                       "indication=%d\n",
+                       event->notify_tx.status,
+                       event->notify_tx.attr_handle,
+                       event->notify_tx.indication);
+        return 0;
+
+    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);
+        return 0;
+
+    case BLE_GAP_EVENT_MTU:
+        console_printf("mtu update event; conn_handle=%d cid=%d mtu=%d\n",
+                       event->mtu.conn_handle,
+                       event->mtu.channel_id,
+                       event->mtu.value);
+        return 0;
+
     default:
         return 0;
     }
@@ -965,33 +1050,6 @@ bletiny_on_l2cap_update(int status, void *arg)
 }
 
 static void
-bletiny_on_scan(int event, int status, struct ble_gap_disc_desc *desc,
-                void *arg)
-{
-    switch (event) {
-    case BLE_GAP_EVENT_DISC_SUCCESS:
-        console_printf("received advertisement; event_type=%d addr_type=%d "
-                       "addr=", desc->event_type, desc->addr_type);
-        print_addr(desc->addr);
-        console_printf(" length_data=%d rssi=%d data=", desc->length_data,
-                       desc->rssi);
-        print_bytes(desc->data, desc->length_data);
-        console_printf(" fields:\n");
-        bletiny_print_adv_fields(desc->fields);
-        console_printf("\n");
-        break;
-
-    case BLE_GAP_EVENT_DISC_COMPLETE:
-        console_printf("scanning finished; status=%d\n", status);
-        break;
-
-    default:
-        assert(0);
-        break;
-    }
-}
-
-static void
 bletiny_tx_timer_cb(void *arg)
 {
     int i;
@@ -1035,7 +1093,7 @@ bletiny_tx_timer_cb(void *arg)
 
         /* Set packet header length */
         OS_MBUF_PKTHDR(om)->omp_len = om->om_len;
-        ble_hci_transport_host_acl_data_send(om);
+        ble_hci_trans_hs_acl_tx(om);
 
         --bletiny_tx_data.tx_num;
     }
@@ -1130,7 +1188,7 @@ bletiny_disc_full(uint16_t conn_handle)
         bletiny_svc_delete(svc);
     }
 
-    bletiny_full_disc_prev_chr_def = 1;
+    bletiny_full_disc_prev_chr_val = 1;
     bletiny_disc_svcs(conn_handle);
 
     return 0;
@@ -1150,9 +1208,22 @@ bletiny_find_inc_svcs(uint16_t conn_handle, uint16_t start_handle,
 int
 bletiny_read(uint16_t conn_handle, uint16_t attr_handle)
 {
+    struct os_mbuf *om;
     int rc;
 
-    rc = ble_gattc_read(conn_handle, attr_handle, bletiny_on_read, NULL);
+    if (conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+        rc = ble_att_svr_read_local(attr_handle, &om);
+        if (rc == 0) {
+            console_printf("read local; attr_handle=%d len=%d value=",
+                           attr_handle, OS_MBUF_PKTLEN(om));
+            print_mbuf(om);
+            console_printf("\n");
+
+            os_mbuf_free_chain(om);
+        }
+    } else {
+        rc = ble_gattc_read(conn_handle, attr_handle, bletiny_on_read, NULL);
+    }
     return rc;
 }
 
@@ -1188,15 +1259,14 @@ bletiny_read_mult(uint16_t conn_handle, uint16_t *attr_handles,
 }
 
 int
-bletiny_write(uint16_t conn_handle, uint16_t attr_handle, void *value,
-               uint16_t value_len)
+bletiny_write(uint16_t conn_handle, uint16_t attr_handle, struct os_mbuf *om)
 {
     int rc;
 
     if (conn_handle == BLE_HS_CONN_HANDLE_NONE) {
-        rc = ble_att_svr_write_local(attr_handle, value, value_len);
+        rc = ble_att_svr_write_local(attr_handle, om);
     } else {
-        rc = ble_gattc_write(conn_handle, attr_handle, value, value_len,
+        rc = ble_gattc_write(conn_handle, attr_handle, om,
                              bletiny_on_write, NULL);
     }
 
@@ -1204,30 +1274,31 @@ bletiny_write(uint16_t conn_handle, uint16_t attr_handle, void *value,
 }
 
 int
-bletiny_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle, void *value,
-                      uint16_t value_len)
+bletiny_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle,
+                     struct os_mbuf *om)
 {
     int rc;
 
-    rc = ble_gattc_write_no_rsp(conn_handle, attr_handle, value, value_len);
+    rc = ble_gattc_write_no_rsp(conn_handle, attr_handle, om);
 
     return rc;
 }
 
 int
-bletiny_write_long(uint16_t conn_handle, uint16_t attr_handle, void *value,
-                    uint16_t value_len)
+bletiny_write_long(uint16_t conn_handle, uint16_t attr_handle,
+                   struct os_mbuf *om)
 {
     int rc;
 
-    rc = ble_gattc_write_long(conn_handle, attr_handle, value, value_len,
+    rc = ble_gattc_write_long(conn_handle, attr_handle, om,
                               bletiny_on_write, NULL);
     return rc;
 }
 
 int
-bletiny_write_reliable(uint16_t conn_handle, struct ble_gatt_attr *attrs,
-                        int num_attrs)
+bletiny_write_reliable(uint16_t conn_handle,
+                       struct ble_gatt_attr *attrs,
+                       int num_attrs)
 {
     int rc;
 
@@ -1246,25 +1317,27 @@ bletiny_adv_stop(void)
 }
 
 int
-bletiny_adv_start(int disc, int conn,
-                uint8_t *peer_addr, uint8_t peer_addr_type,
-                struct ble_gap_adv_params *params)
+bletiny_adv_start(uint8_t own_addr_type, uint8_t peer_addr_type,
+                  const uint8_t *peer_addr, int32_t duration_ms,
+                  const struct ble_gap_adv_params *params)
 {
     int rc;
 
-    rc = ble_gap_adv_start(disc, conn, peer_addr, peer_addr_type, params,
-                           bletiny_gap_event, NULL);
+    rc = ble_gap_adv_start(own_addr_type, peer_addr_type, peer_addr,
+                           duration_ms, params, bletiny_gap_event, NULL);
     return rc;
 }
 
 int
-bletiny_conn_initiate(int addr_type, uint8_t *peer_addr,
-                       struct ble_gap_crt_params *params)
+bletiny_conn_initiate(uint8_t own_addr_type, uint8_t peer_addr_type,
+                      uint8_t *peer_addr, int32_t duration_ms,
+                      struct ble_gap_conn_params *params)
 {
     int rc;
 
-    rc = ble_gap_conn_initiate(addr_type, peer_addr, params, bletiny_gap_event,
-                               NULL);
+    rc = ble_gap_connect(own_addr_type, peer_addr_type, peer_addr, duration_ms,
+                         params, bletiny_gap_event, NULL);
+
     return rc;
 }
 
@@ -1273,16 +1346,16 @@ bletiny_conn_cancel(void)
 {
     int rc;
 
-    rc = ble_gap_cancel();
+    rc = ble_gap_conn_cancel();
     return rc;
 }
 
 int
-bletiny_term_conn(uint16_t conn_handle)
+bletiny_term_conn(uint16_t conn_handle, uint8_t reason)
 {
     int rc;
 
-    rc = ble_gap_terminate(conn_handle);
+    rc = ble_gap_terminate(conn_handle, reason);
     return rc;
 }
 
@@ -1296,13 +1369,13 @@ bletiny_wl_set(struct ble_gap_white_entry *white_list, int white_list_count)
 }
 
 int
-bletiny_scan(uint32_t dur_ms, uint8_t disc_mode, uint8_t scan_type,
-             uint8_t filter_policy, uint8_t addr_mode)
+bletiny_scan(uint8_t own_addr_type, int32_t duration_ms,
+             const struct ble_gap_disc_params *disc_params)
 {
     int rc;
 
-    rc = ble_gap_disc(dur_ms, disc_mode, scan_type, filter_policy, addr_mode,
-                      bletiny_on_scan, NULL);
+    rc = ble_gap_disc(own_addr_type, duration_ms, disc_params,
+                      bletiny_gap_event, NULL);
     return rc;
 }
 
@@ -1344,7 +1417,7 @@ bletiny_datalen(uint16_t conn_handle, uint16_t tx_octets, uint16_t tx_time)
 {
     int rc;
 
-    rc = ble_hci_util_set_data_len(conn_handle, tx_octets, tx_time);
+    rc = ble_hs_hci_util_set_data_len(conn_handle, tx_octets, tx_time);
     return rc;
 }
 
@@ -1404,7 +1477,7 @@ bletiny_sec_restart(uint16_t conn_handle,
 
     if (ltk == NULL) {
         /* The user is requesting a store lookup. */
-        rc = ble_gap_find_conn(conn_handle, &desc);
+        rc = ble_gap_conn_find(conn_handle, &desc);
         if (rc != 0) {
             return rc;
         }
@@ -1480,7 +1553,7 @@ bletiny_rssi(uint16_t conn_handle, int8_t *out_rssi)
 {
     int rc;
 
-    rc = ble_hci_util_read_rssi(conn_handle, out_rssi);
+    rc = ble_gap_conn_rssi(conn_handle, out_rssi);
     if (rc != 0) {
         return rc;
     }
@@ -1488,6 +1561,12 @@ bletiny_rssi(uint16_t conn_handle, int8_t *out_rssi)
     return 0;
 }
 
+static void
+bletiny_on_reset(int reason)
+{
+    console_printf("Error: Resetting state; reason=%d\n", reason);
+}
+
 /**
  * BLE test task
  *
@@ -1530,6 +1609,7 @@ bletiny_task_handler(void *arg)
 int
 main(void)
 {
+    struct ble_hci_ram_cfg hci_cfg;
     struct ble_hs_cfg cfg;
     uint32_t seed;
     int rc;
@@ -1556,6 +1636,7 @@ main(void)
     }
     srand(seed);
 
+    /* Allocate some application specific memory pools. */
     bletiny_svc_mem = malloc(
         OS_MEMPOOL_BYTES(BLETINY_MAX_SVCS, sizeof (struct bletiny_svc)));
     assert(bletiny_svc_mem != NULL);
@@ -1583,6 +1664,7 @@ main(void)
                          "bletiny_dsc_pool");
     assert(rc == 0);
 
+    /* Initialize msys mbufs. */
     rc = os_mempool_init(&default_mbuf_mpool, MBUF_NUM_MBUFS,
                          MBUF_MEMBLOCK_SIZE, default_mbuf_mpool_data,
                          "default_mbuf_data");
@@ -1595,56 +1677,71 @@ main(void)
     rc = os_msys_register(&default_mbuf_pool);
     assert(rc == 0);
 
+    /* Create the shell task. */
+    rc = shell_task_init(SHELL_TASK_PRIO, shell_stack, SHELL_TASK_STACK_SIZE,
+                         SHELL_MAX_INPUT_LEN);
+    assert(rc == 0);
+
+    /* Initialize the logging system. */
     log_init();
     log_console_handler_init(&bletiny_log_console_handler);
     log_register("bletiny", &bletiny_log, &bletiny_log_console_handler);
 
+    /* Initialize eventq for the application task. */
     os_eventq_init(&bletiny_evq);
+
+    /* Create the bletiny task.  All application logic and NimBLE host
+     * operations are performed in this task.
+     */
     os_task_init(&bletiny_task, "bletiny", bletiny_task_handler,
                  NULL, BLETINY_TASK_PRIO, OS_WAIT_FOREVER,
                  bletiny_stack, BLETINY_STACK_SIZE);
 
-    rc = shell_task_init(SHELL_TASK_PRIO, shell_stack, SHELL_TASK_STACK_SIZE,
-                         SHELL_MAX_INPUT_LEN);
+    rc = stats_module_init();
     assert(rc == 0);
 
-    /* Init the console */
-    rc = console_init(shell_console_rx_cb);
+    /* Initialize the BLE LL */
+    rc = ble_ll_init(BLE_LL_TASK_PRI, MBUF_NUM_MBUFS, BLE_MBUF_PAYLOAD_SIZE);
     assert(rc == 0);
 
-    rc = stats_module_init();
+    /* Initialize the RAM HCI transport. */
+    hci_cfg = ble_hci_ram_cfg_dflt;
+    rc = ble_hci_ram_init(&hci_cfg);
     assert(rc == 0);
 
-    /* Initialize the BLE host. */
+    /* Initialize the NimBLE host configuration. */
     cfg = ble_hs_cfg_dflt;
-    cfg.max_hci_bufs = 3;
-    cfg.max_attrs = 36;
-    cfg.max_services = 5;
-    cfg.max_client_configs = (NIMBLE_OPT(MAX_CONNECTIONS) + 1) * 3;
+    cfg.max_hci_bufs = hci_cfg.num_evt_hi_bufs + hci_cfg.num_evt_lo_bufs;
     cfg.max_gattc_procs = 2;
-    cfg.max_l2cap_chans = NIMBLE_OPT(MAX_CONNECTIONS) * 3;
-    cfg.max_l2cap_sig_procs = 2;
-    cfg.store_read_cb = store_read;
-    cfg.store_write_cb = store_write;
+    cfg.reset_cb = bletiny_on_reset;
+    cfg.store_read_cb = ble_store_ram_read;
+    cfg.store_write_cb = ble_store_ram_write;
+    cfg.gatts_register_cb = gatt_svr_register_cb;
 
-    rc = ble_hs_init(&bletiny_evq, &cfg);
+    /* Initialize GATT services. */
+    rc = ble_svc_gap_init(&cfg);
     assert(rc == 0);
 
-    /* Initialize the BLE LL */
-    rc = ble_ll_init(BLE_LL_TASK_PRI, MBUF_NUM_MBUFS, BLE_MBUF_PAYLOAD_SIZE);
+    rc = ble_svc_gatt_init(&cfg);
     assert(rc == 0);
 
-    rc = cmd_init();
+    rc = gatt_svr_init(&cfg);
+    assert(rc == 0);
+
+    /* Initialize NimBLE host. */
+    rc = ble_hs_init(&bletiny_evq, &cfg);
     assert(rc == 0);
 
-    /* Initialize the preferred parameters. */
-    htole16(bletiny_pref_conn_params + 0, BLE_GAP_INITIAL_CONN_ITVL_MIN);
-    htole16(bletiny_pref_conn_params + 2, BLE_GAP_INITIAL_CONN_ITVL_MAX);
-    htole16(bletiny_pref_conn_params + 4, 0);
-    htole16(bletiny_pref_conn_params + 6, BSWAP16(0x100));
+    rc = cmd_init();
+    assert(rc == 0);
 
-    gatt_svr_init();
+    /* Set the default device name. */
+    rc = ble_svc_gap_device_name_set("nimble-bletiny");
+    assert(rc == 0);
 
+    /* Create a callout (timer).  This callout is used by the "tx" bletiny
+     * command to repeatedly send packets of sequential data bytes.
+     */
     os_callout_func_init(&bletiny_tx_timer, &bletiny_evq, bletiny_tx_timer_cb,
                          NULL);
 
diff --git a/apps/bletiny/src/misc.c b/apps/bletiny/src/misc.c
index e9f8eee0..a7f21880 100644
--- a/apps/bletiny/src/misc.c
+++ b/apps/bletiny/src/misc.c
@@ -27,7 +27,7 @@
  * Utility function to log an array of bytes.
  */
 void
-print_bytes(uint8_t *bytes, int len)
+print_bytes(const uint8_t *bytes, int len)
 {
     int i;
 
@@ -37,9 +37,26 @@ print_bytes(uint8_t *bytes, int len)
 }
 
 void
-print_addr(void *addr)
+print_mbuf(const struct os_mbuf *om)
 {
-    uint8_t *u8p;
+    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);
+    }
+}
+
+void
+print_addr(const void *addr)
+{
+    const uint8_t *u8p;
 
     u8p = addr;
     console_printf("%02x:%02x:%02x:%02x:%02x:%02x",
@@ -47,10 +64,10 @@ print_addr(void *addr)
 }
 
 void
-print_uuid(void *uuid128)
+print_uuid(const void *uuid128)
 {
     uint16_t uuid16;
-    uint8_t *u8p;
+    const uint8_t *u8p;
 
     uuid16 = ble_uuid_128_to_16(uuid128);
     if (uuid16 != 0) {
@@ -69,15 +86,15 @@ print_uuid(void *uuid128)
 }
 
 int
-svc_is_empty(struct bletiny_svc *svc)
+svc_is_empty(const struct bletiny_svc *svc)
 {
     return svc->svc.end_handle < svc->svc.start_handle;
 }
 
 uint16_t
-chr_end_handle(struct bletiny_svc *svc, struct bletiny_chr *chr)
+chr_end_handle(const struct bletiny_svc *svc, const struct bletiny_chr *chr)
 {
-    struct bletiny_chr *next_chr;
+    const struct bletiny_chr *next_chr;
 
     next_chr = SLIST_NEXT(chr, next);
     if (next_chr != NULL) {
@@ -88,13 +105,13 @@ chr_end_handle(struct bletiny_svc *svc, struct bletiny_chr *chr)
 }
 
 int
-chr_is_empty(struct bletiny_svc *svc, struct bletiny_chr *chr)
+chr_is_empty(const struct bletiny_svc *svc, const struct bletiny_chr *chr)
 {
     return chr_end_handle(svc, chr) <= chr->chr.val_handle;
 }
 
 void
-print_conn_desc(struct ble_gap_conn_desc *desc)
+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);
diff --git a/apps/bletiny/src/parse.c b/apps/bletiny/src/parse.c
index a739d3d2..a2ae28ae 100644
--- a/apps/bletiny/src/parse.c
+++ b/apps/bletiny/src/parse.c
@@ -72,8 +72,22 @@ parse_kv_find(struct kv_pair *kvs, char *name)
     return NULL;
 }
 
+int
+parse_arg_find_idx(const char *key)
+{
+    int i;
+
+    for (i = 0; i < cmd_num_args; i++) {
+        if (strcmp(cmd_args[i][0], key) == 0) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
 char *
-parse_arg_find(char *key)
+parse_arg_extract(const char *key)
 {
     int i;
 
@@ -111,7 +125,7 @@ parse_arg_long_bounds(char *name, long min, long max, int *out_status)
     char *sval;
     long lval;
 
-    sval = parse_arg_find(name);
+    sval = parse_arg_extract(name);
     if (sval == NULL) {
         *out_status = ENOENT;
         return 0;
@@ -129,6 +143,24 @@ parse_arg_long_bounds(char *name, long min, long max, int *out_status)
     return 0;
 }
 
+long
+parse_arg_long_bounds_default(char *name, long min, long max,
+                              long dflt, int *out_status)
+{
+    long val;
+    int rc;
+
+    val = parse_arg_long_bounds(name, min, max, &rc);
+    if (rc == ENOENT) {
+        rc = 0;
+        val = dflt;
+    }
+
+    *out_status = rc;
+
+    return val;
+}
+
 uint64_t
 parse_arg_uint64_bounds(char *name, uint64_t min, uint64_t max, int *out_status)
 {
@@ -136,7 +168,7 @@ parse_arg_uint64_bounds(char *name, uint64_t min, uint64_t max, int *out_status)
     char *sval;
     uint64_t lval;
 
-    sval = parse_arg_find(name);
+    sval = parse_arg_extract(name);
     if (sval == NULL) {
         *out_status = ENOENT;
         return 0;
@@ -167,6 +199,12 @@ parse_arg_bool(char *name, int *out_status)
 }
 
 uint8_t
+parse_arg_bool_default(char *name, uint8_t dflt, int *out_status)
+{
+    return parse_arg_long_bounds_default(name, 0, 1, dflt, out_status);
+}
+
+uint8_t
 parse_arg_uint8(char *name, int *out_status)
 {
     return parse_arg_long_bounds(name, 0, UINT8_MAX, out_status);
@@ -190,6 +228,22 @@ parse_arg_uint64(char *name, int *out_status)
     return parse_arg_uint64_bounds(name, 0, UINT64_MAX, out_status);
 }
 
+uint8_t
+parse_arg_uint8_dflt(char *name, uint8_t dflt, int *out_status)
+{
+    uint8_t val;
+    int rc;
+
+    val = parse_arg_uint8(name, &rc);
+    if (rc == ENOENT) {
+        val = dflt;
+        rc = 0;
+    }
+
+    *out_status = rc;
+    return val;
+}
+
 uint16_t
 parse_arg_uint16_dflt(char *name, uint16_t dflt, int *out_status)
 {
@@ -206,42 +260,60 @@ parse_arg_uint16_dflt(char *name, uint16_t dflt, int *out_status)
     return val;
 }
 
+uint32_t
+parse_arg_uint32_dflt(char *name, uint32_t dflt, int *out_status)
+{
+    uint32_t val;
+    int rc;
+
+    val = parse_arg_uint32(name, &rc);
+    if (rc == ENOENT) {
+        val = dflt;
+        rc = 0;
+    }
+
+    *out_status = rc;
+    return val;
+}
+
 int
-parse_arg_kv(char *name, struct kv_pair *kvs)
+parse_arg_kv(char *name, struct kv_pair *kvs, int *out_status)
 {
     struct kv_pair *kv;
     char *sval;
 
-    sval = parse_arg_find(name);
+    sval = parse_arg_extract(name);
     if (sval == NULL) {
+        *out_status = ENOENT;
         return -1;
     }
 
     kv = parse_kv_find(kvs, sval);
     if (kv == NULL) {
-        return -2;
+        *out_status = EINVAL;
+        return -1;
     }
 
+    *out_status = 0;
     return kv->val;
 }
 
 int
-parse_arg_kv_default(char *name, struct kv_pair *kvs, int def_val)
+parse_arg_kv_default(char *name, struct kv_pair *kvs, int def_val,
+                     int *out_status)
 {
-    struct kv_pair *kv;
-    char *sval;
+    int val;
+    int rc;
 
-    sval = parse_arg_find(name);
-    if (sval == NULL) {
-        return def_val;
+    val = parse_arg_kv(name, kvs, &rc);
+    if (rc == ENOENT) {
+        rc = 0;
+        val = def_val;
     }
 
-    kv = parse_kv_find(kvs, sval);
-    if (kv == NULL) {
-        return -1;
-    }
+    *out_status = rc;
 
-    return kv->val;
+    return val;
 }
 
 
@@ -282,7 +354,7 @@ parse_arg_byte_stream(char *name, int max_len, uint8_t *dst, int *out_len)
 {
     char *sval;
 
-    sval = parse_arg_find(name);
+    sval = parse_arg_extract(name);
     if (sval == NULL) {
         return ENOENT;
     }
diff --git a/apps/bletiny/src/store.c b/apps/bletiny/src/store.c
deleted file mode 100644
index 2cf46828..00000000
--- a/apps/bletiny/src/store.c
+++ /dev/null
@@ -1,399 +0,0 @@
-/**
- * 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.
- */
-
-/**
- * This file implements a simple in-RAM key database for long-term keys.  A key
- * is inserted into the database immediately after a successful pairing
- * procedure.  A key is retrieved from the database when the central performs
- * the encryption procedure (bonding).
- *
- * As this database is only stored in RAM, its contents are lost if bleprph is
- * restarted.
- */
-
-#include <inttypes.h>
-#include <string.h>
-#include <assert.h>
-
-#include "console/console.h"
-#include "host/ble_hs.h"
-
-#include "bletiny.h"
-
-#define STORE_MAX_SLV_LTKS   4
-#define STORE_MAX_MST_LTKS   4
-#define STORE_MAX_CCCDS      16
-
-static struct ble_store_value_sec store_our_secs[STORE_MAX_SLV_LTKS];
-static int store_num_our_secs;
-
-static struct ble_store_value_sec store_peer_secs[STORE_MAX_MST_LTKS];
-static int store_num_peer_secs;
-
-static struct ble_store_value_cccd store_cccds[STORE_MAX_CCCDS];
-static int store_num_cccds;
-
-/*****************************************************************************
- * $sec                                                                      *
- *****************************************************************************/
-
-static void
-store_print_value_sec(struct ble_store_value_sec *sec)
-{
-    if (sec->ltk_present) {
-        console_printf("ediv=%u rand=%llu authenticated=%d ltk=",
-                       sec->ediv, sec->rand_num, sec->authenticated);
-        print_bytes(sec->ltk, 16);
-        console_printf(" ");
-    }
-    if (sec->irk_present) {
-        console_printf("irk=");
-        print_bytes(sec->irk, 16);
-        console_printf(" ");
-    }
-    if (sec->csrk_present) {
-        console_printf("csrk=");
-        print_bytes(sec->csrk, 16);
-        console_printf(" ");
-    }
-
-    console_printf("\n");
-}
-
-static void
-store_print_key_sec(struct ble_store_key_sec *key_sec)
-{
-    if (key_sec->peer_addr_type != BLE_STORE_ADDR_TYPE_NONE) {
-        console_printf("peer_addr_type=%d peer_addr=",
-                       key_sec->peer_addr_type);
-        print_bytes(key_sec->peer_addr, 6);
-        console_printf(" ");
-    }
-    if (key_sec->ediv_rand_present) {
-        console_printf("ediv=0x%02x rand=0x%llx ",
-                       key_sec->ediv, key_sec->rand_num);
-    }
-}
-
-static void
-store_log_sec_lookup(int obj_type, struct ble_store_key_sec *key_sec)
-{
-    char *obj_name;
-
-    if (key_sec->peer_addr_type == BLE_STORE_ADDR_TYPE_NONE &&
-       !key_sec->ediv_rand_present) {
-
-        return;
-    }
-
-    switch (obj_type) {
-    case BLE_STORE_OBJ_TYPE_PEER_SEC:
-        obj_name = "peer sec";
-        break;
-    case BLE_STORE_OBJ_TYPE_OUR_SEC:
-        obj_name = "our sec";
-        break;
-    default:
-        assert(0);
-        return;
-    }
-
-    console_printf("looking up %s; ", obj_name);
-    store_print_key_sec(key_sec);
-    console_printf("\n");
-}
-
-static int
-store_find_sec(struct ble_store_key_sec *key_sec,
-               struct ble_store_value_sec *value_secs, int num_value_secs)
-{
-    struct ble_store_value_sec *cur;
-    int skipped;
-    int i;
-
-    skipped = 0;
-
-    for (i = 0; i < num_value_secs; i++) {
-        cur = value_secs + i;
-
-        if (key_sec->peer_addr_type != BLE_STORE_ADDR_TYPE_NONE) {
-            if (cur->peer_addr_type != key_sec->peer_addr_type) {
-                continue;
-            }
-
-            if (memcmp(cur->peer_addr, key_sec->peer_addr,
-                       sizeof cur->peer_addr) != 0) {
-                continue;
-            }
-        }
-
-        if (key_sec->ediv_rand_present) {
-            if (cur->ediv != key_sec->ediv) {
-                continue;
-            }
-
-            if (cur->rand_num != key_sec->rand_num) {
-                continue;
-            }
-        }
-
-        if (key_sec->idx > skipped) {
-            skipped++;
-            continue;
-        }
-
-        return i;
-    }
-
-    return -1;
-}
-
-static int
-store_read_our_sec(struct ble_store_key_sec *key_sec,
-                   struct ble_store_value_sec *value_sec)
-{
-    int idx;
-
-    idx = store_find_sec(key_sec, store_our_secs, store_num_our_secs);
-    if (idx == -1) {
-        return BLE_HS_ENOENT;
-    }
-
-    *value_sec = store_our_secs[idx];
-    return 0;
-}
-
-static int
-store_write_our_sec(struct ble_store_value_sec *value_sec)
-{
-    struct ble_store_key_sec key_sec;
-    int idx;
-
-    console_printf("persisting our sec; ");
-    store_print_value_sec(value_sec);
-
-    ble_store_key_from_value_sec(&key_sec, value_sec);
-    idx = store_find_sec(&key_sec, store_our_secs, store_num_our_secs);
-    if (idx == -1) {
-        if (store_num_our_secs >= STORE_MAX_SLV_LTKS) {
-            console_printf("error persisting our sec; too many entries (%d)\n",
-                           store_num_our_secs);
-            return BLE_HS_ENOMEM;
-        }
-
-        idx = store_num_our_secs;
-        store_num_our_secs++;
-    }
-
-    store_our_secs[idx] = *value_sec;
-    return 0;
-}
-
-static int
-store_read_peer_sec(struct ble_store_key_sec *key_sec,
-                   struct ble_store_value_sec *value_sec)
-{
-    int idx;
-
-    idx = store_find_sec(key_sec, store_peer_secs, store_num_peer_secs);
-    if (idx == -1) {
-        return BLE_HS_ENOENT;
-    }
-
-    *value_sec = store_peer_secs[idx];
-    return 0;
-}
-
-static int
-store_write_peer_sec(struct ble_store_value_sec *value_sec)
-{
-    struct ble_store_key_sec key_sec;
-    int idx;
-
-    console_printf("persisting peer sec; ");
-    store_print_value_sec(value_sec);
-
-    ble_store_key_from_value_sec(&key_sec, value_sec);
-    idx = store_find_sec(&key_sec, store_peer_secs, store_num_peer_secs);
-    if (idx == -1) {
-        if (store_num_peer_secs >= STORE_MAX_MST_LTKS) {
-            console_printf("error persisting peer sec; too many entries "
-                           "(%d)\n", store_num_peer_secs);
-            return BLE_HS_ENOMEM;
-        }
-
-        idx = store_num_peer_secs;
-        store_num_peer_secs++;
-    }
-
-    store_peer_secs[idx] = *value_sec;
-    return 0;
-}
-
-/*****************************************************************************
- * $cccd                                                                     *
- *****************************************************************************/
-
-static int
-store_find_cccd(struct ble_store_key_cccd *key)
-{
-    struct ble_store_value_cccd *cccd;
-    int skipped;
-    int i;
-
-    skipped = 0;
-    for (i = 0; i < store_num_cccds; i++) {
-        cccd = store_cccds + i;
-
-        if (key->peer_addr_type != BLE_STORE_ADDR_TYPE_NONE) {
-            if (cccd->peer_addr_type != key->peer_addr_type) {
-                continue;
-            }
-
-            if (memcmp(cccd->peer_addr, key->peer_addr, 6) != 0) {
-                continue;
-            }
-        }
-
-        if (key->chr_val_handle != 0) {
-            if (cccd->chr_val_handle != key->chr_val_handle) {
-                continue;
-            }
-        }
-
-        if (key->idx > skipped) {
-            skipped++;
-            continue;
-        }
-
-        return i;
-    }
-
-    return -1;
-}
-
-static int
-store_read_cccd(struct ble_store_key_cccd *key_cccd,
-                struct ble_store_value_cccd *value_cccd)
-{
-    int idx;
-
-    idx = store_find_cccd(key_cccd);
-    if (idx == -1) {
-        return BLE_HS_ENOENT;
-    }
-
-    *value_cccd = store_cccds[idx];
-    return 0;
-}
-
-static int
-store_write_cccd(struct ble_store_value_cccd *value_cccd)
-{
-    struct ble_store_key_cccd key_cccd;
-    int idx;
-
-    ble_store_key_from_value_cccd(&key_cccd, value_cccd);
-    idx = store_find_cccd(&key_cccd);
-    if (idx == -1) {
-        if (store_num_cccds >= STORE_MAX_SLV_LTKS) {
-            console_printf("error persisting cccd; too many entries (%d)\n",
-                           store_num_cccds);
-            return BLE_HS_ENOMEM;
-        }
-
-        idx = store_num_cccds;
-        store_num_cccds++;
-    }
-
-    store_cccds[idx] = *value_cccd;
-    return 0;
-}
-
-/*****************************************************************************
- * $api                                                                      *
- *****************************************************************************/
-
-/**
- * Searches the database for an object matching the specified criteria.
- *
- * @return                      0 if a key was found; else BLE_HS_ENOENT.
- */
-int
-store_read(int obj_type, union ble_store_key *key,
-           union ble_store_value *value)
-{
-    int rc;
-
-    switch (obj_type) {
-    case BLE_STORE_OBJ_TYPE_PEER_SEC:
-        /* An encryption procedure (bonding) is being attempted.  The nimble
-         * stack is asking us to look in our key database for a long-term key
-         * corresponding to the specified ediv and random number.
-         *
-         * Perform a key lookup and populate the context object with the
-         * result.  The nimble stack will use this key if this function returns
-         * success.
-         */
-        store_log_sec_lookup(BLE_STORE_OBJ_TYPE_PEER_SEC, &key->sec);
-        rc = store_read_peer_sec(&key->sec, &value->sec);
-        return rc;
-
-    case BLE_STORE_OBJ_TYPE_OUR_SEC:
-        store_log_sec_lookup(BLE_STORE_OBJ_TYPE_OUR_SEC, &key->sec);
-        rc = store_read_our_sec(&key->sec, &value->sec);
-        return rc;
-
-    case BLE_STORE_OBJ_TYPE_CCCD:
-        rc = store_read_cccd(&key->cccd, &value->cccd);
-        return rc;
-
-    default:
-        return BLE_HS_ENOTSUP;
-    }
-}
-
-/**
- * Adds the specified object to the database.
- *
- * @return                      0 on success; BLE_HS_ENOMEM if the database is
- *                                  full.
- */
-int
-store_write(int obj_type, union ble_store_value *val)
-{
-    int rc;
-
-    switch (obj_type) {
-    case BLE_STORE_OBJ_TYPE_PEER_SEC:
-        rc = store_write_peer_sec(&val->sec);
-        return rc;
-
-    case BLE_STORE_OBJ_TYPE_OUR_SEC:
-        rc = store_write_our_sec(&val->sec);
-        return rc;
-
-    case BLE_STORE_OBJ_TYPE_CCCD:
-        rc = store_write_cccd(&val->cccd);
-        return rc;
-
-    default:
-        return BLE_HS_ENOTSUP;
-    }
-}
diff --git a/apps/bleuart/pkg.yml b/apps/bleuart/pkg.yml
new file mode 100644
index 00000000..7a7d20e3
--- /dev/null
+++ b/apps/bleuart/pkg.yml
@@ -0,0 +1,43 @@
+# 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/bleuart
+pkg.type: app
+pkg.description: Simple BLE uart application.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+
+pkg.deps:
+    - libs/os
+    - sys/log
+    - net/nimble/controller
+    - net/nimble/host
+    - net/nimble/host/services/gap
+    - net/nimble/host/services/gatt
+    - net/nimble/host/store/ram
+    - net/nimble/transport/ram
+    - libs/console/full
+    - libs/baselibc
+    - libs/newtmgr
+    - libs/newtmgr/transport/ble
+    - libs/bleuart
+
+pkg.cflags:
+    # Disable unused roles; bleuart is a peripheral-only app.
+    - "-DNIMBLE_OPT_ROLE_OBSERVER=0"
+    - "-DNIMBLE_OPT_ROLE_CENTRAL=0"
diff --git a/apps/bleuart/src/main.c b/apps/bleuart/src/main.c
new file mode 100755
index 00000000..8ff7d825
--- /dev/null
+++ b/apps/bleuart/src/main.c
@@ -0,0 +1,362 @@
+/**
+ * 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 <assert.h>
+#include <string.h>
+#include <stdio.h>
+#include <errno.h>
+#include "bsp/bsp.h"
+#include "os/os.h"
+#include "bsp/bsp.h"
+#include "hal/hal_gpio.h"
+#include "hal/hal_cputime.h"
+#include <imgmgr/imgmgr.h>
+
+/* BLE */
+#include "nimble/ble.h"
+#include "host/ble_hs.h"
+#include "host/ble_hs_adv.h"
+#include "host/ble_uuid.h"
+#include "host/ble_att.h"
+#include "host/ble_gap.h"
+#include "host/ble_gatt.h"
+#include "host/ble_l2cap.h"
+#include "host/ble_sm.h"
+#include "controller/ble_ll.h"
+
+/* RAM HCI transport. */
+#include "transport/ram/ble_hci_ram.h"
+
+/* RAM persistence layer. */
+#include "store/ram/ble_store_ram.h"
+
+/* Mandatory services. */
+#include "services/gap/ble_svc_gap.h"
+#include "services/gatt/ble_svc_gatt.h"
+
+/* Newtmgr include */
+#include "newtmgr/newtmgr.h"
+#include "nmgrble/newtmgr_ble.h"
+#include "bleuart/bleuart.h"
+
+/** Mbuf settings. */
+#define MBUF_NUM_MBUFS      (12)
+#define MBUF_BUF_SIZE       OS_ALIGN(BLE_MBUF_PAYLOAD_SIZE, 4)
+#define MBUF_MEMBLOCK_SIZE  (MBUF_BUF_SIZE + BLE_MBUF_MEMBLOCK_OVERHEAD)
+#define MBUF_MEMPOOL_SIZE   OS_MEMPOOL_SIZE(MBUF_NUM_MBUFS, MBUF_MEMBLOCK_SIZE)
+
+#define MAX_CONSOLE_INPUT 120
+static os_membuf_t bleuart_mbuf_mpool_data[MBUF_MEMPOOL_SIZE];
+struct os_mbuf_pool bleuart_mbuf_pool;
+struct os_mempool bleuart_mbuf_mpool;
+
+/** Priority of the nimble host and controller tasks. */
+#define BLE_LL_TASK_PRI             (OS_TASK_PRI_HIGHEST)
+
+/** bleuart task settings. */
+#define bleuart_TASK_PRIO           1
+#define bleuart_STACK_SIZE          (OS_STACK_ALIGN(336))
+
+#define NEWTMGR_TASK_PRIO (4)
+#define NEWTMGR_TASK_STACK_SIZE (OS_STACK_ALIGN(512))
+os_stack_t newtmgr_stack[NEWTMGR_TASK_STACK_SIZE];
+
+struct os_eventq bleuart_evq;
+struct os_task bleuart_task;
+bssnz_t os_stack_t bleuart_stack[bleuart_STACK_SIZE];
+
+/** Our global device address (public) */
+uint8_t g_dev_addr[BLE_DEV_ADDR_LEN] = {0xba, 0xaa, 0xad, 0xba, 0xaa, 0xad};
+
+/** Our random address (in case we need it) */
+uint8_t g_random_addr[BLE_DEV_ADDR_LEN];
+
+static int bleuart_gap_event(struct ble_gap_event *event, void *arg);
+
+/**
+ * Enables advertising with the following parameters:
+ *     o General discoverable mode.
+ *     o Undirected connectable mode.
+ */
+static void
+bleuart_advertise(void)
+{
+    struct ble_gap_adv_params adv_params;
+    struct ble_hs_adv_fields fields;
+    int rc;
+
+    /*
+     *  Set the advertisement data included in our advertisements:
+     *     o Flags (indicates advertisement type and other general info).
+     *     o Advertising tx power.
+     *     o 128 bit UUID
+     */
+
+    memset(&fields, 0, sizeof fields);
+
+    /* Indicate that the flags field should be included; specify a value of 0
+     * to instruct the stack to fill the value in for us.
+     */
+    fields.flags_is_present = 1;
+    fields.flags = 0;
+
+    /* Indicate that the TX power level field should be included; have the
+     * stack fill this one automatically as well.  This is done by assiging the
+     * special value BLE_HS_ADV_TX_PWR_LVL_AUTO.
+     */
+    fields.tx_pwr_lvl_is_present = 1;
+    fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
+
+    fields.uuids128 = (void *)gatt_svr_svc_uart;
+    fields.num_uuids128 = 1;
+    fields.uuids128_is_complete = 1;
+
+    rc = ble_gap_adv_set_fields(&fields);
+    if (rc != 0) {
+        return;
+    }
+
+    memset(&fields, 0, sizeof fields);
+    fields.name = (uint8_t *)ble_svc_gap_device_name();
+    fields.name_len = strlen((char *)fields.name);
+    fields.name_is_complete = 1;
+
+    rc = ble_gap_adv_rsp_set_fields(&fields);
+    if (rc != 0) {
+        return;
+    }
+
+    /* Begin advertising. */
+    memset(&adv_params, 0, sizeof adv_params);
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_UND;
+    adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN;
+    rc = ble_gap_adv_start(BLE_ADDR_TYPE_PUBLIC, 0, NULL, BLE_HS_FOREVER,
+                           &adv_params, bleuart_gap_event, NULL);
+    if (rc != 0) {
+        return;
+    }
+}
+
+/**
+ * The nimble host executes this callback when a GAP event occurs.  The
+ * application associates a GAP event callback with each connection that forms.
+ * bleuart uses the same callback for all connections.
+ *
+ * @param event                 The type of event being signalled.
+ * @param ctxt                  Various information pertaining to the event.
+ * @param arg                   Application-specified argument; unuesd by
+ *                                  bleuart.
+ *
+ * @return                      0 if the application successfully handled the
+ *                                  event; nonzero on failure.  The semantics
+ *                                  of the return code is specific to the
+ *                                  particular GAP event being signalled.
+ */
+static int
+bleuart_gap_event(struct ble_gap_event *event, void *arg)
+{
+    struct ble_gap_conn_desc desc;
+    int rc;
+
+    switch (event->type) {
+    case BLE_GAP_EVENT_CONNECT:
+        /* A new connection was established or a connection attempt failed. */
+        if (event->connect.status == 0) {
+            rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
+            assert(rc == 0);
+            bleuart_set_conn_handle(event->connect.conn_handle);
+        }
+
+        if (event->connect.status != 0) {
+            /* Connection failed; resume advertising. */
+            bleuart_advertise();
+        }
+        return 0;
+
+    case BLE_GAP_EVENT_DISCONNECT:
+        /* Connection terminated; resume advertising. */
+        bleuart_advertise();
+        return 0;
+    }
+
+    return 0;
+}
+
+static void
+bleuart_on_sync(void)
+{
+    /* Begin advertising. */
+    bleuart_advertise();
+}
+
+/**
+ * Event loop for the main bleuart task.
+ */
+static void
+bleuart_task_handler(void *unused)
+{
+    struct os_event *ev;
+    struct os_callout_func *cf;
+    int rc;
+
+    rc = ble_hs_start();
+    assert(rc == 0);
+
+    while (1) {
+        ev = os_eventq_get(&bleuart_evq);
+
+        /* Check if the event is a nmgr ble mqueue event */
+        rc = nmgr_ble_proc_mq_evt(ev);
+        if (!rc) {
+            continue;
+        }
+
+        switch (ev->ev_type) {
+        case OS_EVENT_T_TIMER:
+            cf = (struct os_callout_func *)ev;
+            assert(cf->cf_func);
+            cf->cf_func(CF_ARG(cf));
+            break;
+        default:
+            assert(0);
+            break;
+        }
+    }
+}
+
+/**
+ * main
+ *
+ * The main function for the project. This function initializes the os, calls
+ * init_tasks to initialize tasks (and possibly other objects), then starts the
+ * OS. We should not return from os start.
+ *
+ * @return int NOTE: this function should never return!
+ */
+int
+main(void)
+{
+    struct ble_hci_ram_cfg hci_cfg;
+    struct ble_hs_cfg cfg;
+    uint32_t seed;
+    int rc;
+    int i;
+
+    /* Initialize OS */
+    os_init();
+
+    /* Set cputime to count at 1 usec increments */
+    rc = cputime_init(1000000);
+    assert(rc == 0);
+
+    /* Seed random number generator with least significant bytes of device
+     * address.
+     */
+    seed = 0;
+    for (i = 0; i < 4; ++i) {
+        seed |= g_dev_addr[i];
+        seed <<= 8;
+    }
+    srand(seed);
+
+    /* Initialize msys mbufs. */
+    rc = os_mempool_init(&bleuart_mbuf_mpool, MBUF_NUM_MBUFS,
+                         MBUF_MEMBLOCK_SIZE, bleuart_mbuf_mpool_data,
+                         "bleuart_mbuf_data");
+    assert(rc == 0);
+
+    rc = os_mbuf_pool_init(&bleuart_mbuf_pool, &bleuart_mbuf_mpool,
+                           MBUF_MEMBLOCK_SIZE, MBUF_NUM_MBUFS);
+    assert(rc == 0);
+
+    rc = os_msys_register(&bleuart_mbuf_pool);
+    assert(rc == 0);
+
+    os_task_init(&bleuart_task, "bleuart", bleuart_task_handler,
+                 NULL, bleuart_TASK_PRIO, OS_WAIT_FOREVER,
+                 bleuart_stack, bleuart_STACK_SIZE);
+
+    /* Initialize the BLE LL */
+    rc = ble_ll_init(BLE_LL_TASK_PRI, MBUF_NUM_MBUFS, BLE_MBUF_PAYLOAD_SIZE);
+    assert(rc == 0);
+
+    /* Initialize the RAM HCI transport. */
+    hci_cfg = ble_hci_ram_cfg_dflt;
+    rc = ble_hci_ram_init(&hci_cfg);
+    assert(rc == 0);
+
+    /* Initialize the BLE host. */
+    cfg = ble_hs_cfg_dflt;
+    cfg.max_hci_bufs = hci_cfg.num_evt_hi_bufs + hci_cfg.num_evt_lo_bufs;
+    cfg.max_connections = 1;
+    cfg.max_gattc_procs = 2;
+    cfg.max_l2cap_chans = 3;
+    cfg.max_l2cap_sig_procs = 1;
+    cfg.sm_bonding = 1;
+    cfg.sm_our_key_dist = BLE_SM_PAIR_KEY_DIST_ENC;
+    cfg.sm_their_key_dist = BLE_SM_PAIR_KEY_DIST_ENC;
+    cfg.sync_cb = bleuart_on_sync;
+    cfg.store_read_cb = ble_store_ram_read;
+    cfg.store_write_cb = ble_store_ram_write;
+
+    /* Populate config with the required GATT server settings. */
+    cfg.max_attrs = 0;
+    cfg.max_services = 0;
+    cfg.max_client_configs = 0;
+
+    rc = ble_svc_gap_init(&cfg);
+    assert(rc == 0);
+
+    rc = ble_svc_gatt_init(&cfg);
+    assert(rc == 0);
+
+    rc = bleuart_gatt_svr_init(&cfg);
+    assert(rc == 0);
+
+    /* Initialize eventq */
+    os_eventq_init(&bleuart_evq);
+
+    /* Nmgr ble GATT server initialization */
+    rc = nmgr_ble_gatt_svr_init(&bleuart_evq, &cfg);
+    assert(rc == 0);
+
+    rc = ble_hs_init(&bleuart_evq, &cfg);
+    assert(rc == 0);
+
+    bleuart_init(MAX_CONSOLE_INPUT);
+
+    nmgr_task_init(NEWTMGR_TASK_PRIO, newtmgr_stack, NEWTMGR_TASK_STACK_SIZE);
+    imgmgr_module_init();
+
+    /* Register GATT attributes (services, characteristics, and
+     * descriptors).
+     */
+
+    /* Set the default device name. */
+    rc = ble_svc_gap_device_name_set("Mynewt_BLEuart");
+    assert(rc == 0);
+
+    /* Start the OS */
+    os_start();
+
+    /* os start should never return. If it does, this should be an error */
+    assert(0);
+
+    return 0;
+}
diff --git a/apps/blinky/src/main.c b/apps/blinky/src/main.c
index 24d03112..4d797b36 100755
--- a/apps/blinky/src/main.c
+++ b/apps/blinky/src/main.c
@@ -178,8 +178,6 @@ main(int argc, char **argv)
     shell_task_init(SHELL_TASK_PRIO, shell_stack, SHELL_TASK_STACK_SIZE,
                     SHELL_MAX_INPUT_LEN);
 
-    (void) console_init(shell_console_rx_cb);
-
     stats_module_init();
 
     rc = init_tasks();
diff --git a/apps/boot/pkg.yml b/apps/boot/pkg.yml
index 10ec4821..0ceb3d17 100644
--- a/apps/boot/pkg.yml
+++ b/apps/boot/pkg.yml
@@ -29,9 +29,7 @@ pkg.features: bootloader
 
 pkg.deps:
     - sys/config
-    - fs/nffs
     - libs/bootutil
-    - libs/mbedtls
     - libs/os
     - libs/util
     - libs/console/stub
diff --git a/apps/boot/src/boot.c b/apps/boot/src/boot.c
index b546cc16..73237015 100755
--- a/apps/boot/src/boot.c
+++ b/apps/boot/src/boot.c
@@ -23,23 +23,16 @@
 #include <hal/flash_map.h>
 #include <os/os.h>
 #include <bsp/bsp.h>
+#include <hal/hal_bsp.h>
 #include <hal/hal_system.h>
 #include <hal/hal_flash.h>
 #include <config/config.h>
 #include <config/config_file.h>
-#ifdef NFFS_PRESENT
-#include <fs/fs.h>
-#include <nffs/nffs.h>
-#elif FCB_PRESENT
-#include <fcb/fcb.h>
-#include <config/config_fcb.h>
-#else
-#error "Need NFFS or FCB for config storage"
-#endif
 #ifdef BOOT_SERIAL
 #include <hal/hal_gpio.h>
 #include <boot_serial/boot_serial.h>
 #endif
+#include <console/console.h>
 #include "bootutil/image.h"
 #include "bootutil/loader.h"
 #include "bootutil/bootutil_misc.h"
@@ -58,78 +51,11 @@ static struct os_task boot_ser_task;
 static os_stack_t boot_ser_stack[BOOT_SER_STACK_SZ];
 #endif
 
-#ifdef NFFS_PRESENT
-#define MY_CONFIG_FILE "/cfg/run"
-
-static struct conf_file my_conf = {
-    .cf_name = MY_CONFIG_FILE
-};
-
-static void
-setup_for_nffs(void)
-{
-    struct nffs_area_desc nffs_descs[NFFS_AREA_MAX + 1];
-    int cnt;
-    int rc;
-
-    /*
-     * Make sure we have enough left to initialize the NFFS with the
-     * right number of maximum areas otherwise the file-system will not
-     * be readable.
-     */
-    cnt = NFFS_AREA_MAX;
-    rc = flash_area_to_nffs_desc(FLASH_AREA_NFFS, &cnt, nffs_descs);
-    assert(rc == 0);
-
-    /*
-     * Initializes the flash driver and file system for use by the boot loader.
-     */
-    rc = nffs_init();
-    if (rc == 0) {
-        /* Look for an nffs file system in internal flash.  If no file
-         * system gets detected, all subsequent file operations will fail,
-         * but the boot loader should proceed anyway.
-         */
-        nffs_detect(nffs_descs);
-    }
-
-    rc = conf_file_src(&my_conf);
-    assert(rc == 0);
-    rc = conf_file_dst(&my_conf);
-    assert(rc == 0);
-}
-#else
-struct flash_area conf_fcb_area[NFFS_AREA_MAX + 1];
-
-static struct conf_fcb my_conf = {
-    .cf_fcb.f_magic = 0xc09f6e5e,
-    .cf_fcb.f_sectors = conf_fcb_area
-};
-
-static void
-setup_for_fcb(void)
-{
-    int cnt;
-    int rc;
-
-    rc = flash_area_to_sectors(FLASH_AREA_NFFS, &cnt, NULL);
-    assert(rc == 0);
-    assert(cnt <= sizeof(conf_fcb_area) / sizeof(conf_fcb_area[0]));
-    flash_area_to_sectors(FLASH_AREA_NFFS, &cnt, conf_fcb_area);
-
-    my_conf.cf_fcb.f_sector_cnt = cnt;
-
-    rc = conf_fcb_src(&my_conf);
-    assert(rc == 0);
-    rc = conf_fcb_dst(&my_conf);
-    assert(rc == 0);
-}
-#endif
-
 int
 main(void)
 {
     struct flash_area descs[AREA_DESC_MAX];
+    const struct flash_area *fap;
     /** Areas representing the beginning of image slots. */
     uint8_t img_starts[2];
     int cnt;
@@ -141,7 +67,11 @@ main(void)
         .br_slot_areas = img_starts,
     };
 
+#ifdef BOOT_SERIAL
     os_init();
+#else
+    bsp_init();
+#endif
 
     rc = hal_flash_init();
     assert(rc == 0);
@@ -151,6 +81,9 @@ main(void)
     img_starts[0] = 0;
     total = cnt;
 
+    flash_area_open(FLASH_AREA_IMAGE_0, &fap);
+    req.br_img_sz = fap->fa_size;
+
     cnt = BOOT_AREA_DESC_MAX - total;
     assert(cnt >= 0);
     rc = flash_area_to_sectors(FLASH_AREA_IMAGE_1, &cnt, &descs[total]);
@@ -169,13 +102,6 @@ main(void)
 
     conf_init();
 
-#ifdef NFFS_PRESENT
-    setup_for_nffs();
-#elif FCB_PRESENT
-    setup_for_fcb();
-#endif
-    bootutil_cfg_register();
-
 #ifdef BOOT_SERIAL
     /*
      * Configure a GPIO as input, and compare it against expected value.
@@ -191,6 +117,8 @@ main(void)
 #endif
     rc = boot_go(&req, &rsp);
     assert(rc == 0);
+    console_blocking_mode();
+    console_printf("\nboot_go = %d\n", rc);
 
     system_start((void *)(rsp.br_image_addr + rsp.br_hdr->ih_hdr_size));
 
diff --git a/apps/ffs2native/src/main.c b/apps/ffs2native/src/main.c
index 570d5afa..19a53306 100644
--- a/apps/ffs2native/src/main.c
+++ b/apps/ffs2native/src/main.c
@@ -53,6 +53,7 @@ int file_scratch_idx;
 #define MAX_AREAS    16
 static struct nffs_area_desc area_descs[MAX_AREAS];
 int nffs_version;
+int force_version;
 
 /** On-disk representation of a version 0 inode (file or directory). */
 struct nffs_disk_V0inode {
@@ -415,6 +416,7 @@ print_nffs_flash_V0object(struct nffs_area_desc *area, uint32_t off)
     uint32_t magic;
     int rc;
 
+    printf("print_nffs_flash_V0object(area:%d off%d)\n", area->nad_flash_id, off);
     rc = file_flash_read(area->nad_offset + off, &magic, sizeof magic);
     assert(rc == 0);
 
@@ -440,25 +442,35 @@ print_nffs_flash_inode(struct nffs_area_desc *area, uint32_t off)
     char filename[128];
     int len;
     int rc;
+    uint16_t crc16;
+    int badcrc = 0;
 
     rc = file_flash_read(area->nad_offset + off, &ndi, sizeof(ndi));
     assert(rc == 0);
 
+    crc16 = crc16_ccitt(0, (void*)&ndi, NFFS_DISK_INODE_OFFSET_CRC);
+
     memset(filename, 0, sizeof(filename));
     len = min(sizeof(filename) - 1, ndi.ndi_filename_len);
     rc = file_flash_read(area->nad_offset + off + sizeof(ndi), filename, len);
 
-    printf("  off %x %s id %x flen %d seq %d last %x prnt %x flgs %x %s\n",
+    crc16 = crc16_ccitt(crc16, filename, ndi.ndi_filename_len);
+    if (crc16 != ndi.ndi_crc16) {
+        badcrc = 1;
+    }
+
+    printf("  off %x %s id %x flen %d seq %d last %x prnt %x flgs %x %s%s\n",
            off,
            (nffs_hash_id_is_file(ndi.ndi_id) ? "File" :
-            (nffs_hash_id_is_dir(ndi.ndi_id) ? "Dir" : "???")),
+            nffs_hash_id_is_dir(ndi.ndi_id) ? "Dir" : "???"),
            ndi.ndi_id,
            ndi.ndi_filename_len,
            ndi.ndi_seq,
            ndi.ndi_lastblock_id,
            ndi.ndi_parent_id,
            ndi.ndi_flags,
-           filename);
+           filename,
+           badcrc ? " (Bad CRC!)" : "");
     return sizeof(ndi) + ndi.ndi_filename_len;
 }
 
@@ -467,34 +479,56 @@ print_nffs_flash_block(struct nffs_area_desc *area, uint32_t off)
 {
     struct nffs_disk_block ndb;
     int rc;
+    uint16_t crc16;
+    int badcrc = 0;
+    int dataover = 0;
 
     rc = file_flash_read(area->nad_offset + off, &ndb, sizeof(ndb));
     assert(rc == 0);
 
-    printf("  off %x Block id %x len %d seq %d prev %x own ino %x\n",
+    if (off + ndb.ndb_data_len > area->nad_length) {
+        dataover++;
+    } else {
+        crc16 = crc16_ccitt(0, (void*)&ndb, NFFS_DISK_BLOCK_OFFSET_CRC);
+        crc16 = crc16_ccitt(crc16,
+            (void*)(file_flash_area + area->nad_offset + off + sizeof(ndb)),
+            ndb.ndb_data_len);
+        if (crc16 != ndb.ndb_crc16) {
+            badcrc++;
+        }
+    }
+
+    printf("  off %x Block id %x len %d seq %d prev %x own ino %x%s%s\n",
            off,
            ndb.ndb_id,
            ndb.ndb_data_len,
            ndb.ndb_seq,
            ndb.ndb_prev_id,
-           ndb.ndb_inode_id);
+           ndb.ndb_inode_id,
+           dataover ? " (Bad data length)" : "",
+           badcrc ? " (Bad CRC!)" : "");
+    if (dataover) {
+        return 1;
+    }
     return sizeof(ndb) + ndb.ndb_data_len;
 }
 
 static int
 print_nffs_flash_object(struct nffs_area_desc *area, uint32_t off)
 {
-    struct nffs_disk_object ndo;
+    struct nffs_disk_inode *ndi;
+    struct nffs_disk_block *ndb;
 
-    file_flash_read(area->nad_offset + off, &ndo, sizeof(ndo));
+    ndi = (struct nffs_disk_inode*)(file_flash_area + area->nad_offset + off);
+    ndb = (struct nffs_disk_block*)ndi;
 
-    if (nffs_hash_id_is_inode(ndo.ndo_disk_inode.ndi_id)) {
+    if (nffs_hash_id_is_inode(ndi->ndi_id)) {
         return print_nffs_flash_inode(area, off);
 
-    } else if (nffs_hash_id_is_block(ndo.ndo_disk_block.ndb_id)) {
+    } else if (nffs_hash_id_is_block(ndb->ndb_id)) {
         return print_nffs_flash_block(area, off);
 
-    } else if (ndo.ndo_disk_block.ndb_id == 0xffffffff) {
+    } else if (ndb->ndb_id == 0xffffffff) {
         return area->nad_length;
 
     } else {
@@ -510,6 +544,7 @@ print_nffs_file_flash(char *flash_area, size_t size)
     struct nffs_disk_area *nda;
     int nad_cnt = 0;    /* Nffs Area Descriptor count */
     int off;
+    int objsz;
 
     daptr = flash_area;
     eoda = flash_area + size;
@@ -520,7 +555,11 @@ print_nffs_file_flash(char *flash_area, size_t size)
             area_descs[nad_cnt].nad_offset = (daptr - flash_area);
             area_descs[nad_cnt].nad_length = nda->nda_length;
             area_descs[nad_cnt].nad_flash_id = nda->nda_id;
-            nffs_version = nda->nda_ver;
+            if (force_version >= 0) {
+                nffs_version = force_version;
+            } else {
+                nffs_version = nda->nda_ver;
+            }
 
             if (nda->nda_id == 0xff)
                 file_scratch_idx = nad_cnt;
@@ -538,8 +577,13 @@ print_nffs_file_flash(char *flash_area, size_t size)
                    nda->nda_ver != NFFS_AREA_VER ? " (V0)" : "",
                    nad_cnt == file_scratch_idx ? " (Scratch)" : "");
 
+            if (nffs_version == 0) {
+                objsz = sizeof (struct nffs_disk_V0object);
+            } else {
+                objsz = sizeof (struct nffs_disk_object);
+            }
             off = sizeof (struct nffs_disk_area);
-            while (off < area_descs[nad_cnt].nad_length) {
+            while (off + objsz < area_descs[nad_cnt].nad_length) {
                 if (nffs_version == 0) {
                     off += print_nffs_flash_V0object(&area_descs[nad_cnt], off);
                 } else if (nffs_version == NFFS_AREA_VER) {
@@ -593,8 +637,9 @@ main(int argc, char **argv)
     int standalone = 0;
             
     progname = argv[0];
+    force_version = -1;
 
-    while ((ch = getopt(argc, argv, "c:d:f:sv")) != -1) {
+    while ((ch = getopt(argc, argv, "c:d:f:sv01")) != -1) {
         switch (ch) {
         case 'c':
             fp = fopen(optarg, "rb");
@@ -614,6 +659,12 @@ main(int argc, char **argv)
         case 'v':
             print_verbose++;
             break;
+        case '0':
+            force_version = 0;
+            break;
+        case '1':
+            force_version = 1;
+            break;
         case '?':
         default:
             usage(0);
diff --git a/apps/luatest/src/main.c b/apps/luatest/src/main.c
index 76dd13fd..9cd3d5ae 100755
--- a/apps/luatest/src/main.c
+++ b/apps/luatest/src/main.c
@@ -88,7 +88,6 @@ main(int argc, char **argv)
     /* Init tasks */
     shell_task_init(SHELL_TASK_PRIO, shell_stack, SHELL_TASK_STACK_SIZE,
                          SHELL_MAX_INPUT_LEN);
-    console_init(shell_console_rx_cb);
 
     nffs_init();
 
diff --git a/apps/slinky/src/main.c b/apps/slinky/src/main.c
index 16cb3ac2..1ee66ac8 100755
--- a/apps/slinky/src/main.c
+++ b/apps/slinky/src/main.c
@@ -58,15 +58,15 @@ volatile int tasks_initialized;
 int init_tasks(void);
 
 /* Task 1 */
-#define TASK1_PRIO (1)
-#define TASK1_STACK_SIZE    OS_STACK_ALIGN(128)
+#define TASK1_PRIO (8)
+#define TASK1_STACK_SIZE    OS_STACK_ALIGN(192)
 #define MAX_CBMEM_BUF 600
 struct os_task task1;
 os_stack_t stack1[TASK1_STACK_SIZE];
 static volatile int g_task1_loops;
 
 /* Task 2 */
-#define TASK2_PRIO (2)
+#define TASK2_PRIO (9)
 #define TASK2_STACK_SIZE    OS_STACK_ALIGN(128)
 struct os_task task2;
 os_stack_t stack2[TASK2_STACK_SIZE];
@@ -199,11 +199,20 @@ task1_handler(void *arg)
 {
     struct os_task *t;
     int prev_pin_state, curr_pin_state;
+    struct image_version ver;
 
     /* Set the led pin for the E407 devboard */
     g_led_pin = LED_BLINK_PIN;
     hal_gpio_init_out(g_led_pin, 1);
 
+    if (imgr_my_version(&ver) == 0) {
+        console_printf("\nSlinky %u.%u.%u.%u\n",
+          ver.iv_major, ver.iv_minor, ver.iv_revision,
+          (unsigned int)ver.iv_build_num);
+    } else {
+        console_printf("\nSlinky\n");
+    }
+
     while (1) {
         t = os_sched_get_current_task();
         assert(t->t_func == task1_handler);
@@ -211,7 +220,7 @@ task1_handler(void *arg)
         ++g_task1_loops;
 
         /* Wait one second */
-        os_time_delay(1000);
+        os_time_delay(OS_TICKS_PER_SEC);
 
         /* Toggle the LED */
         prev_pin_state = hal_gpio_read(g_led_pin);
@@ -317,6 +326,13 @@ setup_for_fcb(void)
     my_conf.cf_fcb.f_sector_cnt = cnt;
 
     rc = conf_fcb_src(&my_conf);
+    if (rc) {
+        for (cnt = 0; cnt < my_conf.cf_fcb.f_sector_cnt; cnt++) {
+            flash_area_erase(&conf_fcb_area[cnt], 0,
+              conf_fcb_area[cnt].fa_size);
+        }
+        rc = conf_fcb_src(&my_conf);
+    }
     assert(rc == 0);
     rc = conf_fcb_dst(&my_conf);
     assert(rc == 0);
@@ -337,7 +353,6 @@ int
 main(int argc, char **argv)
 {
     int rc;
-    struct image_version ver;
 
 #ifdef ARCH_sim
     mcu_sim_parse_args(argc, argv);
@@ -380,11 +395,8 @@ main(int argc, char **argv)
     shell_task_init(SHELL_TASK_PRIO, shell_stack, SHELL_TASK_STACK_SIZE,
                     SHELL_MAX_INPUT_LEN);
 
-    (void) console_init(shell_console_rx_cb);
-
     nmgr_task_init(NEWTMGR_TASK_PRIO, newtmgr_stack, NEWTMGR_TASK_STACK_SIZE);
     imgmgr_module_init();
-    bootutil_cfg_register();
 
     stats_module_init();
 
@@ -404,15 +416,6 @@ main(int argc, char **argv)
 
     rc = init_tasks();
 
-    rc = imgr_my_version(&ver);
-    if (rc == 0) {
-        console_printf("\nSlinky %u.%u.%u.%u\n",
-          ver.iv_major, ver.iv_minor, ver.iv_revision,
-          (unsigned int)ver.iv_build_num);
-    } else {
-        console_printf("\nSlinky\n");
-    }
-
     os_start();
 
     /* os start should never return. If it does, this should be an error */
diff --git a/compiler/arm-none-eabi-m4/compiler.yml b/compiler/arm-none-eabi-m4/compiler.yml
index 182d144c..2f87c29b 100644
--- a/compiler/arm-none-eabi-m4/compiler.yml
+++ b/compiler/arm-none-eabi-m4/compiler.yml
@@ -24,9 +24,10 @@ compiler.path.objdump: arm-none-eabi-objdump
 compiler.path.objsize: arm-none-eabi-size
 compiler.path.objcopy: arm-none-eabi-objcopy
 
-compiler.flags.default: -mcpu=cortex-m4 -mthumb-interwork -mthumb -Wall -Werror -fno-exceptions -ffunction-sections -fdata-sections
-compiler.flags.optimized: [compiler.flags.default, -Os -ggdb]
-compiler.flags.debug: [compiler.flags.default, -O1 -ggdb]
+compiler.flags.base: -mcpu=cortex-m4 -mthumb-interwork -mthumb -Wall -Werror -fno-exceptions -ffunction-sections -fdata-sections
+compiler.flags.default: [compiler.flags.base, -O1 -ggdb]
+compiler.flags.optimized: [compiler.flags.base, -Os -ggdb]
+compiler.flags.debug: [compiler.flags.base, -O1 -ggdb]
 
 compiler.ld.flags: -static -lgcc -Wl,--gc-sections
 compiler.ld.resolve_circular_deps: true
diff --git a/compiler/sim/compiler.yml b/compiler/sim/compiler.yml
index eaeccec5..225a4e9a 100644
--- a/compiler/sim/compiler.yml
+++ b/compiler/sim/compiler.yml
@@ -31,6 +31,7 @@ compiler.ld.resolve_circular_deps: true
 compiler.flags.default: [compiler.flags.base, -O1]
 compiler.flags.debug: [compiler.flags.base, -O0]
 compiler.ld.mapfile: false
+compiler.ld.binfile: false
 
 # Linux.
 compiler.flags.base.LINUX: >
@@ -41,7 +42,6 @@ compiler.ld.flags.LINUX: -lutil
 compiler.path.cc.DARWIN.OVERWRITE: "/usr/local/bin/gcc-5"
 compiler.path.as.DARWIN.OVERWRITE: "/usr/local/bin/gcc-5 -x assembler-with-cpp"
 compiler.path.objdump.DARWIN.OVERWRITE: "gobjdump"
-compiler.path.objsize.DARWIN.OVERWRITE: "objsize"
 compiler.path.objcopy.DARWIN.OVERWRITE: "gobjcopy"
 compiler.flags.base.DARWIN: >
     -DMN_OSX
diff --git a/drivers/uart_bitbang/include/uart_bitbang/uart_bitbang.h b/drivers/uart_bitbang/include/uart_bitbang/uart_bitbang.h
new file mode 100644
index 00000000..47e2a8bf
--- /dev/null
+++ b/drivers/uart_bitbang/include/uart_bitbang/uart_bitbang.h
@@ -0,0 +1,25 @@
+/*
+ * 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 __UART_BITBANG_H__
+#define __UART_BITBANG_H__
+
+int uart_bitbang_init(int rxpin, int txpin, uint32_t cputimer_freq);
+
+#endif /* __UART_BITBANG_H__ */
diff --git a/drivers/uart_bitbang/include/uart_bitbang/uart_bitbang_api.h b/drivers/uart_bitbang/include/uart_bitbang/uart_bitbang_api.h
new file mode 100644
index 00000000..cff9792e
--- /dev/null
+++ b/drivers/uart_bitbang/include/uart_bitbang/uart_bitbang_api.h
@@ -0,0 +1,33 @@
+/*
+ * 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 __UART_BITBANG_API_H__
+#define __UART_BITBANG_API_H__
+
+int uart_bitbang_config(int port, int32_t baudrate, uint8_t databits,
+  uint8_t stopbits, enum hal_uart_parity parity,
+  enum hal_uart_flow_ctl flow_ctl);
+int uart_bitbang_init_cbs(int port, hal_uart_tx_char tx_func,
+  hal_uart_tx_done tx_done, hal_uart_rx_char rx_func, void *arg);
+void uart_bitbang_start_rx(int port);
+void uart_bitbang_start_tx(int port);
+void uart_bitbang_blocking_tx(int port, uint8_t data);
+int uart_bitbang_close(int port);
+
+#endif
diff --git a/drivers/uart_bitbang/pkg.yml b/drivers/uart_bitbang/pkg.yml
new file mode 100644
index 00000000..1e74a1e5
--- /dev/null
+++ b/drivers/uart_bitbang/pkg.yml
@@ -0,0 +1,27 @@
+#
+# 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: drivers/uart_bitbang
+pkg.description: Async UART port with a bitbanger.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+pkg.deps.TEST:
+   - hw/hal
+   - libs/testutil
diff --git a/drivers/uart_bitbang/src/uart_bitbang.c b/drivers/uart_bitbang/src/uart_bitbang.c
new file mode 100644
index 00000000..3a5ab70d
--- /dev/null
+++ b/drivers/uart_bitbang/src/uart_bitbang.c
@@ -0,0 +1,325 @@
+/*
+ * 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 <inttypes.h>
+#include <string.h>
+#include <stdio.h>
+#include <assert.h>
+
+#include <hal/hal_gpio.h>
+#include <hal/hal_uart.h>
+#include <hal/hal_cputime.h>
+
+#include <os/os.h>
+#include <bsp/bsp.h>
+
+#include "uart_bitbang/uart_bitbang.h"
+#include "uart_bitbang/uart_bitbang_api.h"
+
+/*
+ * Async UART as a bitbanger.
+ * Cannot run very fast, as it relies on cputimer to time sampling and
+ * bit tx start times.
+ */
+struct uart_bitbang {
+    int ub_bittime;             /* number of cputimer ticks per bit */
+    struct {
+        int pin;                /* RX pin */
+        struct cpu_timer timer;
+        uint32_t start;         /* cputime when byte rx started */
+        uint8_t byte;           /* receiving this byte */
+        uint8_t bits;           /* how many bits we've seen */
+        int false_irq;
+    } ub_rx;
+    struct {
+        int pin;                /* TX pin */
+        struct cpu_timer timer;
+        uint32_t start;         /* cputime when byte tx started */
+        uint8_t byte;           /* byte being transmitted */
+        uint8_t bits;           /* how many bits have been sent */
+    } ub_tx;
+
+    uint8_t ub_open:1;
+    uint8_t ub_rx_stall:1;
+    uint8_t ub_txing:1;
+    uint32_t ub_cputimer_freq;
+    hal_uart_rx_char ub_rx_func;
+    hal_uart_tx_char ub_tx_func;
+    hal_uart_tx_done ub_tx_done;
+    void *ub_func_arg;
+};
+
+struct uart_bitbang uart_bitbang;
+
+/*
+ * Bytes start with START bit (0) followed by 8 data bits and then the
+ * STOP bit (1). STOP bit should be configurable. Data bits are sent LSB first.
+ */
+static void
+uart_bitbang_tx_timer(void *arg)
+{
+    struct uart_bitbang *ub = (struct uart_bitbang *)arg;
+    uint32_t next = 0;
+    int data;
+
+    if (!ub->ub_txing || ub->ub_tx.bits > 9) {
+        if (ub->ub_tx.bits > 9) {
+            if (ub->ub_tx_done) {
+                ub->ub_tx_done(ub->ub_func_arg);
+            }
+        }
+        data = ub->ub_tx_func(ub->ub_func_arg);
+        if (data < 0) {
+            ub->ub_txing = 0;
+            return;
+        } else {
+            ub->ub_tx.byte = data;
+        }
+        /*
+         * Start bit
+         */
+        hal_gpio_write(ub->ub_tx.pin, 0);
+        ub->ub_tx.start = cputime_get32();
+        next = ub->ub_tx.start + ub->ub_bittime;
+        ub->ub_txing = 1;
+        ub->ub_tx.bits = 0;
+    } else {
+        if (ub->ub_tx.bits++ < 8) {
+            hal_gpio_write(ub->ub_tx.pin, ub->ub_tx.byte & 0x01);
+            ub->ub_tx.byte = ub->ub_tx.byte >> 1;
+            next = ub->ub_tx.start + (ub->ub_bittime * (ub->ub_tx.bits + 1));
+        } else {
+            /*
+             * STOP bit.
+             */
+            hal_gpio_write(ub->ub_tx.pin, 1);
+            next = ub->ub_tx.start + (ub->ub_bittime * 10);
+        }
+    }
+    cputime_timer_start(&ub->ub_tx.timer, next);
+}
+
+static void
+uart_bitbang_rx_timer(void *arg)
+{
+    struct uart_bitbang *ub = (struct uart_bitbang *)arg;
+    int val;
+
+    val = hal_gpio_read(ub->ub_rx.pin);
+
+    if (val) {
+        ub->ub_rx.byte = 0x80 | (ub->ub_rx.byte >> 1);
+    } else {
+        ub->ub_rx.byte = (ub->ub_rx.byte >> 1);
+    }
+    if (ub->ub_rx.bits == 7) {
+        val = ub->ub_rx_func(ub->ub_func_arg, ub->ub_rx.byte);
+        if (val) {
+            ub->ub_rx_stall = 1;
+        } else {
+            /*
+             * Re-enable GPIO IRQ after we've sampled last bit. STOP bit
+             * is ignored.
+             */
+            hal_gpio_irq_enable(ub->ub_rx.pin);
+        }
+    } else {
+        ub->ub_rx.bits++;
+        cputime_timer_start(&ub->ub_rx.timer,
+          ub->ub_rx.start + (ub->ub_bittime * (ub->ub_rx.bits + 1)) +
+          (ub->ub_bittime >> 1));
+    }
+}
+
+/*
+ * Byte RX starts when we get transition from high to low.
+ * We disable RX irq after we've seen start until end of RX of one byte.
+ */
+static void
+uart_bitbang_isr(void *arg)
+{
+    struct uart_bitbang *ub = (struct uart_bitbang *)arg;
+    uint32_t time;
+
+    time = cputime_get32();
+    if (ub->ub_rx.start - time < (9 * ub->ub_bittime)) {
+        ++ub->ub_rx.false_irq;
+        return;
+    }
+    ub->ub_rx.start = time;
+    ub->ub_rx.byte = 0;
+    ub->ub_rx.bits = 0;
+
+    /*
+     * We try to sample in the middle of a bit. First sample is taken
+     * 1.5 bittimes after beginning of start bit.
+     */
+    cputime_timer_start(&ub->ub_rx.timer, time +
+      ub->ub_bittime + (ub->ub_bittime >> 1));
+
+    hal_gpio_irq_disable(ub->ub_rx.pin);
+}
+
+void
+uart_bitbang_blocking_tx(int port, uint8_t data)
+{
+    struct uart_bitbang *ub = &uart_bitbang;
+    int i;
+    uint32_t start, next;
+
+    if (!ub->ub_open) {
+        return;
+    }
+    hal_gpio_write(ub->ub_tx.pin, 0);
+    start = cputime_get32();
+    next = start + ub->ub_bittime;
+    while (cputime_get32() < next);
+    for (i = 0; i < 8; i++) {
+        hal_gpio_write(ub->ub_tx.pin, data & 0x01);
+        data = data >> 1;
+        next = start + (ub->ub_bittime * i + 1);
+        while (cputime_get32() < next);
+    }
+    next = start + (ub->ub_bittime * 10);
+    hal_gpio_write(ub->ub_tx.pin, 1);
+    while (cputime_get32() < next);
+}
+
+int
+uart_bitbang_init(int rxpin, int txpin, uint32_t cputimer_freq)
+{
+    struct uart_bitbang *ub = &uart_bitbang;
+
+    ub->ub_rx.pin = rxpin;
+    ub->ub_tx.pin = txpin;
+    ub->ub_cputimer_freq = cputimer_freq;
+
+    return 0;
+}
+
+void
+uart_bitbang_start_tx(int port)
+{
+    struct uart_bitbang *ub = &uart_bitbang;
+    int sr;
+
+    if (!ub->ub_open) {
+        return;
+    }
+    if (!ub->ub_txing) {
+        OS_ENTER_CRITICAL(sr);
+        uart_bitbang_tx_timer(ub);
+        OS_EXIT_CRITICAL(sr);
+    }
+}
+
+void
+uart_bitbang_start_rx(int port)
+{
+    struct uart_bitbang *ub = &uart_bitbang;
+    int sr;
+    int rc;
+
+    if (ub->ub_rx_stall) {
+        rc = ub->ub_rx_func(ub->ub_func_arg, ub->ub_rx.byte);
+        if (rc == 0) {
+            OS_ENTER_CRITICAL(sr);
+            ub->ub_rx_stall = 0;
+            OS_EXIT_CRITICAL(sr);
+
+            /*
+             * Start looking for start bit again.
+             */
+            hal_gpio_irq_enable(ub->ub_rx.pin);
+        }
+    }
+}
+
+int
+uart_bitbang_init_cbs(int port, hal_uart_tx_char tx_func,
+  hal_uart_tx_done tx_done, hal_uart_rx_char rx_func, void *arg)
+{
+    struct uart_bitbang *ub = &uart_bitbang;
+
+    if (port != 0) {
+        return -1;
+    }
+
+    if (ub->ub_open) {
+        return -1;
+    }
+    ub->ub_rx_func = rx_func;
+    ub->ub_tx_func = tx_func;
+    ub->ub_tx_done = tx_done;
+    ub->ub_func_arg = arg;
+    return 0;
+}
+
+int
+uart_bitbang_config(int port, int32_t baudrate, uint8_t databits,
+  uint8_t stopbits, enum hal_uart_parity parity,
+  enum hal_uart_flow_ctl flow_ctl)
+{
+    struct uart_bitbang *ub = &uart_bitbang;
+
+    if (databits != 8 || parity != HAL_UART_PARITY_NONE ||
+      flow_ctl != HAL_UART_FLOW_CTL_NONE) {
+        return -1;
+    }
+
+    assert(ub->ub_rx.pin != ub->ub_tx.pin); /* make sure it's initialized */
+
+    if (baudrate > 19200) {
+        return -1;
+    }
+    ub->ub_bittime = ub->ub_cputimer_freq / baudrate;
+
+    cputime_timer_init(&ub->ub_rx.timer, uart_bitbang_rx_timer, &uart_bitbang);
+    cputime_timer_init(&ub->ub_tx.timer, uart_bitbang_tx_timer, &uart_bitbang);
+
+    if (hal_gpio_init_out(ub->ub_tx.pin, 1)) {
+        return -1;
+    }
+
+    if (hal_gpio_irq_init(ub->ub_rx.pin, uart_bitbang_isr, ub,
+        GPIO_TRIG_FALLING, GPIO_PULL_UP)) {
+        return -1;
+    }
+    hal_gpio_irq_enable(ub->ub_rx.pin);
+
+    ub->ub_open = 1;
+    return 0;
+}
+
+int
+uart_bitbang_close(int port)
+{
+    struct uart_bitbang *ub = &uart_bitbang;
+    int sr;
+
+    OS_ENTER_CRITICAL(sr);
+    hal_gpio_irq_disable(ub->ub_rx.pin);
+    hal_gpio_irq_release(ub->ub_rx.pin);
+    ub->ub_open = 0;
+    ub->ub_txing = 0;
+    ub->ub_rx_stall = 0;
+    cputime_timer_stop(&ub->ub_tx.timer);
+    cputime_timer_stop(&ub->ub_rx.timer);
+    OS_EXIT_CRITICAL(sr);
+    return 0;
+}
diff --git a/fs/fs/src/fsutil.c b/fs/fs/src/fsutil.c
index 1754130a..ad3a063a 100644
--- a/fs/fs/src/fsutil.c
+++ b/fs/fs/src/fsutil.c
@@ -49,7 +49,7 @@ fsutil_write_file(const char *path, const void *data, uint32_t len)
     struct fs_file *file;
     int rc;
 
-    rc = fs_open(path, FS_ACCESS_WRITE | FS_ACCESS_TRUNCATE, &file);
+    rc = fs_open(path, FS_ACCESS_WRITE, &file);
     if (rc != 0) {
         goto done;
     }
diff --git a/fs/nffs/src/nffs.c b/fs/nffs/src/nffs.c
index f3b03721..28fdeea6 100644
--- a/fs/nffs/src/nffs.c
+++ b/fs/nffs/src/nffs.c
@@ -118,6 +118,14 @@ nffs_unlock(void)
     assert(rc == 0 || rc == OS_NOT_STARTED);
 }
 
+static void
+nffs_stats_init(void)
+{
+    nffs_hashcnt_ins = 0;
+    nffs_hashcnt_rm = 0;
+    nffs_object_count = 0;
+}
+
 /**
  * Opens a file at the specified path.  The result of opening a nonexistent
  * file depends on the access flags specified.  All intermediate directories
@@ -624,6 +632,8 @@ nffs_init(void)
 
     nffs_cache_clear();
 
+    nffs_stats_init();
+
     rc = os_mutex_init(&nffs_mutex);
     if (rc != 0) {
         return FS_EOS;
diff --git a/fs/nffs/src/nffs_block.c b/fs/nffs/src/nffs_block.c
index 953e4ddc..d1d291ad 100644
--- a/fs/nffs/src/nffs_block.c
+++ b/fs/nffs/src/nffs_block.c
@@ -255,6 +255,9 @@ nffs_block_delete_from_ram(struct nffs_hash_entry *block_entry)
     if (rc == 0 || rc == FS_ECORRUPT) {
         /* If file system corruption was detected, the resulting block is still
          * valid and can be removed from RAM.
+         * Note that FS_CORRUPT can occur because the owning inode was not
+         * found in the hash table - this can occur during the sweep where
+         * the inodes were deleted ahead of the blocks.
          */
         inode_entry = block.nb_inode_entry;
         if (inode_entry != NULL &&
diff --git a/fs/nffs/src/nffs_cache.c b/fs/nffs/src/nffs_cache.c
index 51ebc30c..ddbc904d 100644
--- a/fs/nffs/src/nffs_cache.c
+++ b/fs/nffs/src/nffs_cache.c
@@ -313,10 +313,10 @@ nffs_cache_log_block(struct nffs_cache_inode *cache_inode,
 {
     NFFS_LOG(DEBUG, "id=%u inode=%u flash_off=0x%08x "
                     "file_off=%u len=%d (entry=%p)\n",
-             cache_block->ncb_block.nb_hash_entry->nhe_id,
-             cache_inode->nci_inode.ni_inode_entry->nie_hash_entry.nhe_id,
-             cache_block->ncb_block.nb_hash_entry->nhe_flash_loc,
-             cache_block->ncb_file_offset,
+             (unsigned int)cache_block->ncb_block.nb_hash_entry->nhe_id,
+             (unsigned int)cache_inode->nci_inode.ni_inode_entry->nie_hash_entry.nhe_id,
+             (unsigned int)cache_block->ncb_block.nb_hash_entry->nhe_flash_loc,
+             (unsigned int)cache_block->ncb_file_offset,
              cache_block->ncb_block.nb_data_len,
              cache_block->ncb_block.nb_hash_entry);
 }
diff --git a/fs/nffs/src/nffs_hash.c b/fs/nffs/src/nffs_hash.c
index 805ed26b..299210b3 100644
--- a/fs/nffs/src/nffs_hash.c
+++ b/fs/nffs/src/nffs_hash.c
@@ -156,6 +156,7 @@ nffs_hash_insert(struct nffs_hash_entry *entry)
     list = nffs_hash + idx;
 
     SLIST_INSERT_HEAD(list, entry, nhe_next);
+    nffs_hashcnt_ins++;
 
     if (nffs_hash_id_is_inode(entry->nhe_id)) {
         nie = nffs_hash_find_inode(entry->nhe_id);
@@ -185,6 +186,7 @@ nffs_hash_remove(struct nffs_hash_entry *entry)
     list = nffs_hash + idx;
 
     SLIST_REMOVE(list, entry, nffs_hash_entry, nhe_next);
+    nffs_hashcnt_rm++;
 
     if (nffs_hash_id_is_inode(entry->nhe_id) && nie) {
         nffs_inode_unsetflags(nie, NFFS_INODE_FLAG_INHASH);
diff --git a/fs/nffs/src/nffs_inode.c b/fs/nffs/src/nffs_inode.c
index d8b881c3..a72bc827 100644
--- a/fs/nffs/src/nffs_inode.c
+++ b/fs/nffs/src/nffs_inode.c
@@ -114,6 +114,9 @@ nffs_inode_write_disk(const struct nffs_disk_inode *disk_inode,
 {
     int rc;
 
+    /* Only the DELETED flag is ever written out to flash */
+    assert((disk_inode->ndi_flags & ~NFFS_INODE_FLAG_DELETED) == 0);
+
     rc = nffs_flash_write(area_idx, area_offset, disk_inode,
                           sizeof *disk_inode);
     if (rc != 0) {
@@ -611,6 +614,7 @@ nffs_inode_rename(struct nffs_inode_entry *inode_entry,
     disk_inode.ndi_id = inode_entry->nie_hash_entry.nhe_id;
     disk_inode.ndi_seq = inode.ni_seq + 1;
     disk_inode.ndi_parent_id = nffs_inode_parent_id(&inode);
+    disk_inode.ndi_flags = 0;
     disk_inode.ndi_filename_len = filename_len;
     if (inode_entry->nie_last_block_entry &&
         inode_entry->nie_last_block_entry->nhe_id != NFFS_ID_NONE)
diff --git a/fs/nffs/src/nffs_priv.h b/fs/nffs/src/nffs_priv.h
index b654b85b..47c4e9a6 100644
--- a/fs/nffs/src/nffs_priv.h
+++ b/fs/nffs/src/nffs_priv.h
@@ -236,6 +236,10 @@ struct nffs_dir {
     struct nffs_dirent nd_dirent;
 };
 
+uint32_t nffs_hashcnt_ins;
+uint32_t nffs_hashcnt_rm;
+uint32_t nffs_object_count;
+
 extern void *nffs_file_mem;
 extern void *nffs_block_entry_mem;
 extern void *nffs_inode_mem;
diff --git a/fs/nffs/src/nffs_restore.c b/fs/nffs/src/nffs_restore.c
index 83b2ef96..fd425fc3 100644
--- a/fs/nffs/src/nffs_restore.c
+++ b/fs/nffs/src/nffs_restore.c
@@ -282,7 +282,7 @@ nffs_restore_sweep(void)
     struct nffs_hash_list *list;
     struct nffs_inode inode;
     struct nffs_block block;
-    int del;
+    int del = 0;
     int rc;
     int i;
 
@@ -319,17 +319,6 @@ nffs_restore_sweep(void)
                     return rc;
                 }
 
-#if 0 /* for now, don't preserve corrupted directories */
-                /*
-                 * if this inode doesn't have a parent, move it to
-                 * the lost_found directory
-                 */
-                if (inode_entry != nffs_root_dir && inode.ni_parent == NULL) {
-                    rc = nffs_inode_rename(inode_entry,
-                                           nffs_lost_found_dir, NULL);
-                }
-#endif
-
                 if (del) {
 
                     /* Remove the inode and all its children from RAM.  We
@@ -346,13 +335,19 @@ nffs_restore_sweep(void)
                 }
             } else if (nffs_hash_id_is_block(entry->nhe_id)) {
                 if (nffs_hash_id_is_dummy(entry->nhe_id)) {
+                    del = 1;
                     nffs_block_delete_from_ram(entry);
                 } else {
                     rc = nffs_block_from_hash_entry(&block, entry);
                     if (rc != 0 && rc != FS_ENOENT) {
+                        del = 1;
                         nffs_block_delete_from_ram(entry);
                     }
                 }
+                if (del) {
+                    del = 0;
+                    next = SLIST_FIRST(list);
+                }
             }
 
             entry = next;
@@ -504,7 +499,8 @@ nffs_restore_inode(const struct nffs_disk_inode *disk_inode, uint8_t area_idx,
              * Restore this inode even though deleted on disk
              * so the additional restored blocks have a place to go
              */
-            NFFS_LOG(DEBUG, "restoring deleted inode %x\n", disk_inode->ndi_id);
+            NFFS_LOG(DEBUG, "restoring deleted inode %x\n",
+                     (unsigned int)disk_inode->ndi_id);
             nffs_inode_setflags(inode_entry, NFFS_INODE_FLAG_DELETED);
         }
 
@@ -629,7 +625,8 @@ nffs_restore_inode(const struct nffs_disk_inode *disk_inode, uint8_t area_idx,
              * Restore this inode even though deleted on disk
              * so the additional restored blocks have a place to go
              */
-            NFFS_LOG(DEBUG, "restoring deleted inode %x\n", disk_inode->ndi_id);
+            NFFS_LOG(DEBUG, "restoring deleted inode %x\n",
+                     (unsigned int)disk_inode->ndi_id);
             nffs_inode_setflags(inode_entry, NFFS_INODE_FLAG_DELETED);
         }
 
@@ -649,12 +646,6 @@ nffs_restore_inode(const struct nffs_disk_inode *disk_inode, uint8_t area_idx,
             if (lastblock_entry != NULL) {
                 if (lastblock_entry->nhe_id == disk_inode->ndi_lastblock_id) {
                     inode_entry->nie_last_block_entry = lastblock_entry;
-                    /*
-                     * This flag should have been turned unset
-                     * when the block was restored.
-                     */
-                    assert(!nffs_inode_getflags(inode_entry,
-                                               NFFS_INODE_FLAG_DUMMYLSTBLK));
                 }
 
             } else {
@@ -717,13 +708,13 @@ nffs_restore_inode(const struct nffs_disk_inode *disk_inode, uint8_t area_idx,
 
     if (nffs_hash_id_is_file(inode_entry->nie_hash_entry.nhe_id)) {
         NFFS_LOG(DEBUG, "restoring file; id=0x%08x\n",
-                 inode_entry->nie_hash_entry.nhe_id);
+                 (unsigned int)inode_entry->nie_hash_entry.nhe_id);
         if (inode_entry->nie_hash_entry.nhe_id >= nffs_hash_next_file_id) {
             nffs_hash_next_file_id = inode_entry->nie_hash_entry.nhe_id + 1;
         }
     } else {
         NFFS_LOG(DEBUG, "restoring dir; id=0x%08x\n",
-                 inode_entry->nie_hash_entry.nhe_id);
+                 (unsigned int)inode_entry->nie_hash_entry.nhe_id);
         if (inode_entry->nie_hash_entry.nhe_id >= nffs_hash_next_dir_id) {
             nffs_hash_next_dir_id = inode_entry->nie_hash_entry.nhe_id + 1;
         }
@@ -890,8 +881,11 @@ nffs_restore_block(const struct nffs_disk_block *disk_block, uint8_t area_idx,
     }
 
     NFFS_LOG(DEBUG, "restoring block; id=0x%08x seq=%u inode_id=%u prev_id=%u "
-             "data_len=%u\n", disk_block->ndb_id, disk_block->ndb_seq,
-             disk_block->ndb_inode_id, disk_block->ndb_prev_id,
+             "data_len=%u\n",
+             (unsigned int)disk_block->ndb_id,
+             (unsigned int)disk_block->ndb_seq,
+             (unsigned int)disk_block->ndb_inode_id,
+             (unsigned int)disk_block->ndb_prev_id,
              disk_block->ndb_data_len);
 
     inode_entry = nffs_hash_find_inode(disk_block->ndb_inode_id);
@@ -1065,6 +1059,7 @@ nffs_restore_area_contents(int area_idx)
             if (rc == FS_ECORRUPT) {
                 area->na_cur++;
             } else {
+                nffs_object_count++; /* total count of restored objects */
                 area->na_cur += nffs_restore_disk_object_size(&disk_object);
             }
             break;
@@ -1219,19 +1214,20 @@ nffs_log_contents(void)
         if (nffs_hash_id_is_block(entry->nhe_id)) {
             rc = nffs_block_from_hash_entry(&block, entry);
             assert(rc == 0 || rc == FS_ENOENT);
-            NFFS_LOG(DEBUG, "block; id=%u inode_id=", entry->nhe_id);
+            NFFS_LOG(DEBUG, "block; id=%u inode_id=",
+                     (unsigned int)entry->nhe_id);
             if (block.nb_inode_entry == NULL) {
                 NFFS_LOG(DEBUG, "null ");
             } else {
                 NFFS_LOG(DEBUG, "%u ",
-                         block.nb_inode_entry->nie_hash_entry.nhe_id);
+                     (unsigned int)block.nb_inode_entry->nie_hash_entry.nhe_id);
             }
 
             NFFS_LOG(DEBUG, "prev_id=");
             if (block.nb_prev == NULL) {
                 NFFS_LOG(DEBUG, "null ");
             } else {
-                NFFS_LOG(DEBUG, "%u ", block.nb_prev->nhe_id);
+                NFFS_LOG(DEBUG, "%u ", (unsigned int)block.nb_prev->nhe_id);
             }
 
             NFFS_LOG(DEBUG, "data_len=%u\n", block.nb_data_len);
@@ -1245,7 +1241,7 @@ nffs_log_contents(void)
                     NFFS_LOG(DEBUG, "null");
                 } else {
                     NFFS_LOG(DEBUG, "%x",
-                             (unsigned int)inode_entry->nie_last_block_entry->nhe_id);
+                     (unsigned int)inode_entry->nie_last_block_entry->nhe_id);
                 }
             } else if (rc != 0) {
                 continue;
@@ -1254,18 +1250,18 @@ nffs_log_contents(void)
 
             if (nffs_hash_id_is_file(entry->nhe_id)) {
                 NFFS_LOG(DEBUG, "file; id=%u name=%.3s block_id=",
-                         entry->nhe_id, inode.ni_filename);
+                         (unsigned int)entry->nhe_id, inode.ni_filename);
                 if (inode_entry->nie_last_block_entry == NULL) {
                     NFFS_LOG(DEBUG, "null");
                 } else {
                     NFFS_LOG(DEBUG, "%u",
-                             inode_entry->nie_last_block_entry->nhe_id);
+                     (unsigned int)inode_entry->nie_last_block_entry->nhe_id);
                 }
                 NFFS_LOG(DEBUG, "\n");
             } else {
                 inode_entry = (void *)entry;
-                NFFS_LOG(DEBUG, "dir; id=%u name=%.3s\n", entry->nhe_id,
-                         inode.ni_filename);
+                NFFS_LOG(DEBUG, "dir; id=%u name=%.3s\n",
+                         (unsigned int)entry->nhe_id, inode.ni_filename);
 
             }
         }
diff --git a/hw/bsp/arduino_primo_nrf52/primo_debug.sh b/hw/bsp/arduino_primo_nrf52/primo_debug.sh
index 53b715e3..aa98cf5f 100755
--- a/hw/bsp/arduino_primo_nrf52/primo_debug.sh
+++ b/hw/bsp/arduino_primo_nrf52/primo_debug.sh
@@ -51,8 +51,11 @@ if [ $USE_OPENOCD -eq 1 ]; then
     # Block Ctrl-C from getting passed to openocd.
     # Exit openocd when gdb detaches.
     #
+    # Note that openocd behaves differently than Primo. We reset the target
+    # as we attach with openocd. If you don't want that, replace "reset halt"
+    # with just "halt"
     set -m
-    openocd -s $MY_PATH -f arduino_primo.cfg -c "gdb_port 3333; telnet_port 4444; nrf52.cpu configure -event gdb-detach {shutdown}" -c init -c "halt" &
+    openocd -s $MY_PATH -f arduino_primo.cfg -c "gdb_port 3333; telnet_port 4444; nrf52.cpu configure -event gdb-detach {shutdown}" -c init -c "reset halt" &
     set +m
 else
     #
diff --git a/hw/bsp/nrf51-blenano/src/os_bsp.c b/hw/bsp/nrf51-blenano/src/os_bsp.c
index 2b88f27b..5f7e7a92 100644
--- a/hw/bsp/nrf51-blenano/src/os_bsp.c
+++ b/hw/bsp/nrf51-blenano/src/os_bsp.c
@@ -64,7 +64,7 @@ bsp_imgr_current_slot(void)
 }
 
 void
-os_bsp_init(void)
+bsp_init(void)
 {
     /*
      * XXX this reference is here to keep this function in.
diff --git a/hw/bsp/olimex_stm32-e407_devboard/olimex_stm32-e407_devboard_debug.sh b/hw/bsp/olimex_stm32-e407_devboard/olimex_stm32-e407_devboard_debug.sh
index d4027537..2f307730 100755
--- a/hw/bsp/olimex_stm32-e407_devboard/olimex_stm32-e407_devboard_debug.sh
+++ b/hw/bsp/olimex_stm32-e407_devboard/olimex_stm32-e407_devboard_debug.sh
@@ -23,7 +23,6 @@
 #  - identities is the project identities string.
 # 
 #
-set -x
 if [ $# -lt 2 ]; then
     echo "Need binary to debug"
     exit 1
diff --git a/hw/bsp/olimex_stm32-e407_devboard/src/hal_bsp.c b/hw/bsp/olimex_stm32-e407_devboard/src/hal_bsp.c
index b68e370a..763002f5 100644
--- a/hw/bsp/olimex_stm32-e407_devboard/src/hal_bsp.c
+++ b/hw/bsp/olimex_stm32-e407_devboard/src/hal_bsp.c
@@ -32,8 +32,8 @@ static const struct stm32f4_uart_cfg uart_cfg[UART_CNT] = {
         .suc_rcc_dev = RCC_APB2ENR_USART6EN,
         .suc_pin_tx = 38,
         .suc_pin_rx = 39,
-        .suc_pin_rts = -1,
-        .suc_pin_cts = -1,
+        .suc_pin_rts = 34,
+        .suc_pin_cts = 35,
         .suc_pin_af = GPIO_AF8_USART6,
         .suc_irqn = USART6_IRQn
     }
diff --git a/hw/bsp/stm32f4discovery/boot-stm32f4discovery.ld b/hw/bsp/stm32f4discovery/boot-stm32f4discovery.ld
new file mode 100644
index 00000000..5f41879a
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/boot-stm32f4discovery.ld
@@ -0,0 +1,202 @@
+/**
+ * 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.
+ */
+ 
+/* Linker script for STM32F407 when running code from SRAM */
+
+/* Linker script to configure memory regions. */
+MEMORY
+{ 
+  FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K
+  CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+  RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+}
+
+/* Linker script to place sections and symbol values. Should be used together
+ * with other linker script that defines memory regions FLASH and RAM.
+ * It references following symbols, which must be defined in code:
+ *   Reset_Handler : Entry of reset handler
+ * 
+ * It defines following symbols, which code can use without definition:
+ *   __exidx_start
+ *   __exidx_end
+ *   __etext
+ *   __data_start__
+ *   __preinit_array_start
+ *   __preinit_array_end
+ *   __init_array_start
+ *   __init_array_end
+ *   __fini_array_start
+ *   __fini_array_end
+ *   __data_end__
+ *   __bss_start__
+ *   __bss_end__
+ *   __end__
+ *   end
+ *   __HeapBase
+ *   __HeapLimit
+ *   __StackLimit
+ *   __StackTop
+ *   __stack
+ *   __coredata_start__
+ *   __coredata_end__
+ *   __corebss_start__
+ *   __corebss_end__
+ *   __ecoredata
+ *   __ecorebss
+ */
+ENTRY(Reset_Handler)
+
+SECTIONS
+{
+    .text :
+    {
+        __vector_tbl_reloc__ = .;
+        KEEP(*(.isr_vector))
+        *(.text*)
+
+        KEEP(*(.init))
+        KEEP(*(.fini))
+
+        /* .ctors */
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+
+        /* .dtors */
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+
+        *(.rodata*)
+
+        KEEP(*(.eh_frame*))
+    } > FLASH
+
+    .ARM.extab : 
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > FLASH
+
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > FLASH
+
+    __exidx_end = .;
+
+    . = ALIGN(8);
+    __etext = .;
+
+    .coredata : AT (__etext)
+    {
+        __coredata_start__ = .;
+        *(.data.core)
+        . = ALIGN(8);
+        __coredata_end__ = .;
+    } > CCM
+
+    __ecoredata = __etext + SIZEOF(.coredata);
+
+    /* This section is here so that the start of .data has the same VMA and LMA */
+    .ram_coredata (NOLOAD):
+    {
+        . = . + SIZEOF(.coredata);
+    } > RAM
+
+    .data : AT (__ecoredata)
+    {
+        __data_start__ = .;
+        *(vtable)
+        *(.data*)
+
+        /* preinit data */
+        PROVIDE_HIDDEN (__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE_HIDDEN (__preinit_array_end = .);
+
+        . = ALIGN(4);
+        /* init data */
+        PROVIDE_HIDDEN (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE_HIDDEN (__init_array_end = .);
+
+
+        . = ALIGN(4);
+        /* finit data */
+        PROVIDE_HIDDEN (__fini_array_start = .);
+        KEEP(*(SORT(.fini_array.*)))
+        KEEP(*(.fini_array))
+        PROVIDE_HIDDEN (__fini_array_end = .);
+
+        KEEP(*(.jcr*))
+        . = ALIGN(4);
+        /* All data end */
+        __data_end__ = .;
+
+    } > RAM
+
+    .corebss (NOLOAD):
+    {
+        . = ALIGN(4);
+        __corebss_start__ = .;
+        *(.bss.core)
+        . = ALIGN(4);
+        __corebss_end__ = .;
+        *(.corebss*)
+        *(.bss.core.nz)
+        . = ALIGN(4);
+        __ecorebss = .;
+    } > CCM
+
+    .bss :
+    {
+        . = ALIGN(4);
+        __bss_start__ = .;
+        *(.bss*)
+        *(COMMON)
+        . = ALIGN(4);
+        __bss_end__ = .;
+    } > RAM
+    
+    __HeapBase = .;
+    __HeapLimit = ORIGIN(RAM) + LENGTH(RAM);
+
+    /* .stack_dummy section doesn't contains any symbols. It is only
+     * used for linker to calculate size of stack sections, and assign
+     * values to stack symbols later */
+    .stack_dummy (COPY):
+    {
+        *(.stack*)
+    } > CCM
+
+    /* Set stack top to end of CCM; stack limit is bottom of stack */
+    __StackTop = ORIGIN(CCM) + LENGTH(CCM);
+    __StackLimit = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+    
+    /* Check for CCM overflow */
+    ASSERT(__StackLimit >= __ecorebss, "CCM overflow!")
+}
+
diff --git a/hw/bsp/stm32f4discovery/f407.cfg b/hw/bsp/stm32f4discovery/f407.cfg
new file mode 100644
index 00000000..7c46d8ba
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/f407.cfg
@@ -0,0 +1,82 @@
+#
+# 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.
+#
+
+# script for stm32f4x family
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME stm32f4x
+}
+
+if { [info exists ENDIAN] } {
+   set _ENDIAN $ENDIAN
+} else {
+   set _ENDIAN little
+}
+
+# Work-area is a space in RAM used for flash programming
+# By default use 64kB
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x10000
+}
+
+# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz
+#
+# Since we may be running of an RC oscilator, we crank down the speed a
+# bit more to be on the safe side. Perhaps superstition, but if are
+# running off a crystal, we can run closer to the limit. Note
+# that there can be a pretty wide band where things are more or less stable.
+adapter_khz 1000
+
+adapter_nsrst_delay 100
+jtag_ntrst_delay 100
+
+#jtag scan chain
+if { [info exists CPUTAPID] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+  # See STM Document RM0090
+  # Section 32.6.2 - corresponds to Cortex-M4 r0p1
+   set _CPUTAPID 0x4ba00477
+}
+jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+
+if { [info exists BSTAPID] } {
+   set _BSTAPID $BSTAPID
+} else {
+  # See STM Document RM0090
+  # Section 32.6.3
+  set _BSTAPID 0x06413041
+}
+jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
+
+# if srst is not fitted use SYSRESETREQ to
+# perform a soft reset
+cortex_m3 reset_config sysresetreq
diff --git a/hw/bsp/stm32f4discovery/include/bsp/bsp.h b/hw/bsp/stm32f4discovery/include/bsp/bsp.h
new file mode 100644
index 00000000..7ab3e6e8
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/include/bsp/bsp.h
@@ -0,0 +1,55 @@
+/**
+ * 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 H_BSP_H
+#define H_BSP_H
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Define special stackos sections */
+#define sec_data_core   __attribute__((section(".data.core")))
+#define sec_bss_core    __attribute__((section(".bss.core")))
+#define sec_bss_nz_core __attribute__((section(".bss.core.nz")))
+
+/* More convenient section placement macros. */
+#define bssnz_t         sec_bss_nz_core
+
+extern uint8_t _ram_start;
+extern uint8_t _ccram_start;
+
+#define RAM_SIZE        (128 * 1024)
+#define CCRAM_SIZE      (64 * 1024)
+
+/* LED pins */
+#define LED_BLINK_PIN   (60)
+
+/* UART */
+#define UART_CNT 1
+#define CONSOLE_UART 0
+
+#define NFFS_AREA_MAX    (8)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* H_BSP_H */
diff --git a/net/nimble/include/nimble/hci_transport.h b/hw/bsp/stm32f4discovery/include/bsp/bsp_sysid.h
index 7de737c7..7b0a24b6 100644
--- a/net/nimble/include/nimble/hci_transport.h
+++ b/hw/bsp/stm32f4discovery/include/bsp/bsp_sysid.h
@@ -16,17 +16,21 @@
  * specific language governing permissions and limitations
  * under the License.
  */
+#ifndef BSP_SYSID_H
+#define BSP_SYSID_H
 
-#ifndef H_HCI_TRANSPORT_
-#define H_HCI_TRANSPORT_
+#ifdef __cplusplus
+extern "C" {
+#endif
 
-/* Send a HCI command from the host to the controller */
-int ble_hci_transport_host_cmd_send(uint8_t *cmd);
+/* stub until this BSP gets new HAL */
+enum system_device_id  
+{
+    RESERVED,
+};
 
-/* Send a HCI event from the controller to the host */
-int ble_hci_transport_ctlr_event_send(uint8_t *hci_ev);
+#ifdef __cplusplus
+}
+#endif
 
-/* Send ACL data from host to contoller */
-int ble_hci_transport_host_acl_data_send(struct os_mbuf *om);
-
-#endif /* H_HCI_TRANSPORT_ */
+#endif /* BSP_SYSID_H */
\ No newline at end of file
diff --git a/hw/bsp/stm32f4discovery/include/bsp/cmsis_nvic.h b/hw/bsp/stm32f4discovery/include/bsp/cmsis_nvic.h
new file mode 100644
index 00000000..008b247d
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/include/bsp/cmsis_nvic.h
@@ -0,0 +1,29 @@
+/* mbed Microcontroller Library - cmsis_nvic
+ * Copyright (c) 2009-2011 ARM Limited. All rights reserved.
+ *
+ * CMSIS-style functionality to support dynamic vectors
+ */
+
+#ifndef MBED_CMSIS_NVIC_H
+#define MBED_CMSIS_NVIC_H
+
+#include <stdint.h>
+
+#define NVIC_NUM_VECTORS      (16 + 81)   // CORE + MCU Peripherals
+#define NVIC_USER_IRQ_OFFSET  16
+
+#include "mcu/stm32f4xx.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void NVIC_Relocate(void);
+void NVIC_SetVector(IRQn_Type IRQn, uint32_t vector);
+uint32_t NVIC_GetVector(IRQn_Type IRQn);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/hw/bsp/stm32f4discovery/include/bsp/stm32f4xx_hal_conf.h b/hw/bsp/stm32f4discovery/include/bsp/stm32f4xx_hal_conf.h
new file mode 100644
index 00000000..8e4216cc
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/include/bsp/stm32f4xx_hal_conf.h
@@ -0,0 +1,413 @@
+/**
+  ******************************************************************************
+  * @file    stm32f4xx_hal_conf.h
+  * @author  MCD Application Team
+  * @version V1.2.1
+  * @date    13-March-2015
+  * @brief   HAL configuration file
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+  *
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. Redistributions in binary form must reproduce the above copyright notice,
+  *      this list of conditions and the following disclaimer in the documentation
+  *      and/or other materials provided with the distribution.
+  *   3. Neither the name of STMicroelectronics nor the names of its contributors
+  *      may be used to endorse or promote products derived from this software
+  *      without specific prior written permission.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  ******************************************************************************
+  */ 
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __STM32F4xx_HAL_CONF_H
+#define __STM32F4xx_HAL_CONF_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported constants --------------------------------------------------------*/
+
+/* ########################## Module Selection ############################## */
+/**
+  * @brief This is the list of modules to be used in the HAL driver 
+  */
+#define HAL_MODULE_ENABLED
+#if 0
+#define HAL_ADC_MODULE_ENABLED
+#define HAL_CAN_MODULE_ENABLED
+#define HAL_CRC_MODULE_ENABLED
+#define HAL_CRYP_MODULE_ENABLED
+#define HAL_DAC_MODULE_ENABLED
+#define HAL_DCMI_MODULE_ENABLED
+#define HAL_DMA_MODULE_ENABLED
+/* #define HAL_DMA2D_MODULE_ENABLED */
+#define HAL_ETH_MODULE_ENABLED
+#define HAL_FLASH_MODULE_ENABLED
+#define HAL_NAND_MODULE_ENABLED
+#define HAL_NOR_MODULE_ENABLED
+#define HAL_PCCARD_MODULE_ENABLED
+#define HAL_SRAM_MODULE_ENABLED
+/* #define HAL_SDRAM_MODULE_ENABLED */
+#define HAL_HASH_MODULE_ENABLED
+#define HAL_GPIO_MODULE_ENABLED
+#define HAL_I2C_MODULE_ENABLED
+#define HAL_I2S_MODULE_ENABLED
+#define HAL_IWDG_MODULE_ENABLED
+#define HAL_LTDC_MODULE_ENABLED
+#define HAL_PWR_MODULE_ENABLED
+#define HAL_RCC_MODULE_ENABLED
+#define HAL_RNG_MODULE_ENABLED
+#define HAL_RTC_MODULE_ENABLED
+/* #define HAL_SAI_MODULE_ENABLED */
+#define HAL_SD_MODULE_ENABLED
+#define HAL_SPI_MODULE_ENABLED
+#define HAL_TIM_MODULE_ENABLED
+#define HAL_UART_MODULE_ENABLED
+#define HAL_USART_MODULE_ENABLED
+#define HAL_IRDA_MODULE_ENABLED
+#define HAL_SMARTCARD_MODULE_ENABLED
+#define HAL_WWDG_MODULE_ENABLED
+#define HAL_CORTEX_MODULE_ENABLED
+#define HAL_PCD_MODULE_ENABLED
+#define HAL_HCD_MODULE_ENABLED
+#else
+#define HAL_PWR_MODULE_ENABLED
+#define HAL_FLASH_MODULE_ENABLED
+#define HAL_GPIO_MODULE_ENABLED
+#define HAL_RCC_MODULE_ENABLED
+#endif
+
+/* ########################## HSE/HSI Values adaptation ##################### */
+/**
+  * @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
+  *        This value is used by the RCC HAL module to compute the system frequency
+  *        (when HSE is used as system clock source, directly or through the PLL).  
+  */
+#if !defined  (HSE_VALUE) 
+  #define HSE_VALUE    ((uint32_t)12000000) /*!< Value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSE_STARTUP_TIMEOUT)
+  #define HSE_STARTUP_TIMEOUT    ((uint32_t)5000)   /*!< Time out for HSE start up, in ms */
+#endif /* HSE_STARTUP_TIMEOUT */
+
+/**
+  * @brief Internal High Speed oscillator (HSI) value.
+  *        This value is used by the RCC HAL module to compute the system frequency
+  *        (when HSI is used as system clock source, directly or through the PLL). 
+  */
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE    ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
+/**
+  * @brief Internal Low Speed oscillator (LSI) value.
+  */
+#if !defined  (LSI_VALUE) 
+ #define LSI_VALUE  ((uint32_t)32000)    
+#endif /* LSI_VALUE */                      /*!< Value of the Internal Low Speed oscillator in Hz
+                                             The real value may vary depending on the variations
+                                             in voltage and temperature.  */
+/**
+  * @brief External Low Speed oscillator (LSE) value.
+  */
+#if !defined  (LSE_VALUE)
+ #define LSE_VALUE  ((uint32_t)32768)    /*!< Value of the External Low Speed oscillator in Hz */
+#endif /* LSE_VALUE */
+
+/**
+  * @brief External clock source for I2S peripheral
+  *        This value is used by the I2S HAL module to compute the I2S clock source 
+  *        frequency, this source is inserted directly through I2S_CKIN pad. 
+  */
+#if !defined  (EXTERNAL_CLOCK_VALUE)
+  #define EXTERNAL_CLOCK_VALUE    ((uint32_t)12288000) /*!< Value of the Internal oscillator in Hz*/
+#endif /* EXTERNAL_CLOCK_VALUE */
+
+/* Tip: To avoid modifying this file each time you need to use different HSE,
+   ===  you can define the HSE value in your toolchain compiler preprocessor. */
+
+/* ########################### System Configuration ######################### */
+/**
+  * @brief This is the HAL system configuration section
+  */     
+#define  VDD_VALUE                    ((uint32_t)3300) /*!< Value of VDD in mv */
+#define  TICK_INT_PRIORITY            ((uint32_t)0x0F) /*!< tick interrupt priority */
+#define  USE_RTOS                     0
+#define  PREFETCH_ENABLE              0 /* The prefetch will be enabled in SystemClock_Config(), depending on the used 
+                                           STM32F405/415/07/417 device: RevA (prefetch must be off) or RevZ (prefetch can be on/off) */
+#define  INSTRUCTION_CACHE_ENABLE     1
+#define  DATA_CACHE_ENABLE            1
+
+/* ########################## Assert Selection ############################## */
+/**
+  * @brief Uncomment the line below to expanse the "assert_param" macro in the 
+  *        HAL drivers code
+  */
+/* #define USE_FULL_ASSERT    1 */
+
+/* ################## Ethernet peripheral configuration ##################### */
+
+/* Section 1 : Ethernet peripheral configuration */
+
+/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */
+#define MAC_ADDR0   2
+#define MAC_ADDR1   0
+#define MAC_ADDR2   0
+#define MAC_ADDR3   0
+#define MAC_ADDR4   0
+#define MAC_ADDR5   0
+
+/* Definition of the Ethernet driver buffers size and count */   
+#define ETH_RX_BUF_SIZE                ETH_MAX_PACKET_SIZE /* buffer size for receive               */
+#define ETH_TX_BUF_SIZE                ETH_MAX_PACKET_SIZE /* buffer size for transmit              */
+#define ETH_RXBUFNB                    ((uint32_t)4)       /* 4 Rx buffers of size ETH_RX_BUF_SIZE  */
+#define ETH_TXBUFNB                    ((uint32_t)4)       /* 4 Tx buffers of size ETH_TX_BUF_SIZE  */
+
+/* Section 2: PHY configuration section */
+
+/* DP83848 PHY Address*/ 
+#define DP83848_PHY_ADDRESS             0x01
+/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/ 
+#define PHY_RESET_DELAY                 ((uint32_t)0x000000FF)
+/* PHY Configuration delay */
+#define PHY_CONFIG_DELAY                ((uint32_t)0x00000FFF)
+
+#define PHY_READ_TO                     ((uint32_t)0x0000FFFF)
+#define PHY_WRITE_TO                    ((uint32_t)0x0000FFFF)
+
+/* Section 3: Common PHY Registers */
+
+#define PHY_BCR                         ((uint16_t)0x00)    /*!< Transceiver Basic Control Register   */
+#define PHY_BSR                         ((uint16_t)0x01)    /*!< Transceiver Basic Status Register    */
+ 
+#define PHY_RESET                       ((uint16_t)0x8000)  /*!< PHY Reset */
+#define PHY_LOOPBACK                    ((uint16_t)0x4000)  /*!< Select loop-back mode */
+#define PHY_FULLDUPLEX_100M             ((uint16_t)0x2100)  /*!< Set the full-duplex mode at 100 Mb/s */
+#define PHY_HALFDUPLEX_100M             ((uint16_t)0x2000)  /*!< Set the half-duplex mode at 100 Mb/s */
+#define PHY_FULLDUPLEX_10M              ((uint16_t)0x0100)  /*!< Set the full-duplex mode at 10 Mb/s  */
+#define PHY_HALFDUPLEX_10M              ((uint16_t)0x0000)  /*!< Set the half-duplex mode at 10 Mb/s  */
+#define PHY_AUTONEGOTIATION             ((uint16_t)0x1000)  /*!< Enable auto-negotiation function     */
+#define PHY_RESTART_AUTONEGOTIATION     ((uint16_t)0x0200)  /*!< Restart auto-negotiation function    */
+#define PHY_POWERDOWN                   ((uint16_t)0x0800)  /*!< Select the power down mode           */
+#define PHY_ISOLATE                     ((uint16_t)0x0400)  /*!< Isolate PHY from MII                 */
+
+#define PHY_AUTONEGO_COMPLETE           ((uint16_t)0x0020)  /*!< Auto-Negotiation process completed   */
+#define PHY_LINKED_STATUS               ((uint16_t)0x0004)  /*!< Valid link established               */
+#define PHY_JABBER_DETECTION            ((uint16_t)0x0002)  /*!< Jabber condition detected            */
+  
+/* Section 4: Extended PHY Registers */
+
+#define PHY_SR                          ((uint16_t)0x10)    /*!< PHY status register Offset                      */
+#define PHY_MICR                        ((uint16_t)0x11)    /*!< MII Interrupt Control Register                  */
+#define PHY_MISR                        ((uint16_t)0x12)    /*!< MII Interrupt Status and Misc. Control Register */
+ 
+#define PHY_LINK_STATUS                 ((uint16_t)0x0001)  /*!< PHY Link mask                                   */
+#define PHY_SPEED_STATUS                ((uint16_t)0x0002)  /*!< PHY Speed mask                                  */
+#define PHY_DUPLEX_STATUS               ((uint16_t)0x0004)  /*!< PHY Duplex mask                                 */
+
+#define PHY_MICR_INT_EN                 ((uint16_t)0x0002)  /*!< PHY Enable interrupts                           */
+#define PHY_MICR_INT_OE                 ((uint16_t)0x0001)  /*!< PHY Enable output interrupt events              */
+
+#define PHY_MISR_LINK_INT_EN            ((uint16_t)0x0020)  /*!< Enable Interrupt on change of link status       */
+#define PHY_LINK_INTERRUPT              ((uint16_t)0x2000)  /*!< PHY link status interrupt mask                  */
+
+/* Includes ------------------------------------------------------------------*/
+/**
+  * @brief Include module's header file 
+  */
+
+#ifdef HAL_RCC_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_rcc.h"
+#endif /* HAL_RCC_MODULE_ENABLED */
+
+#ifdef HAL_GPIO_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_gpio.h"
+#endif /* HAL_GPIO_MODULE_ENABLED */
+
+#ifdef HAL_DMA_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_dma.h"
+#endif /* HAL_DMA_MODULE_ENABLED */
+   
+#ifdef HAL_CORTEX_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_cortex.h"
+#endif /* HAL_CORTEX_MODULE_ENABLED */
+
+#ifdef HAL_ADC_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_adc.h"
+#endif /* HAL_ADC_MODULE_ENABLED */
+
+#ifdef HAL_CAN_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_can.h"
+#endif /* HAL_CAN_MODULE_ENABLED */
+
+#ifdef HAL_CRC_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_crc.h"
+#endif /* HAL_CRC_MODULE_ENABLED */
+
+#ifdef HAL_CRYP_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_cryp.h" 
+#endif /* HAL_CRYP_MODULE_ENABLED */
+
+#ifdef HAL_DMA2D_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_dma2d.h"
+#endif /* HAL_DMA2D_MODULE_ENABLED */
+
+#ifdef HAL_DAC_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_dac.h"
+#endif /* HAL_DAC_MODULE_ENABLED */
+
+#ifdef HAL_DCMI_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_dcmi.h"
+#endif /* HAL_DCMI_MODULE_ENABLED */
+
+#ifdef HAL_ETH_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_eth.h"
+#endif /* HAL_ETH_MODULE_ENABLED */
+
+#ifdef HAL_FLASH_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_flash.h"
+#endif /* HAL_FLASH_MODULE_ENABLED */
+ 
+#ifdef HAL_SRAM_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_sram.h"
+#endif /* HAL_SRAM_MODULE_ENABLED */
+
+#ifdef HAL_NOR_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_nor.h"
+#endif /* HAL_NOR_MODULE_ENABLED */
+
+#ifdef HAL_NAND_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_nand.h"
+#endif /* HAL_NAND_MODULE_ENABLED */
+
+#ifdef HAL_PCCARD_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_pccard.h"
+#endif /* HAL_PCCARD_MODULE_ENABLED */ 
+  
+#ifdef HAL_SDRAM_MODULE_ENABLED
+  #include "mcu/stm32f4xx_hal_sdram.h"
+#endif /* HAL_SDRAM_MODULE_ENABLED */      
+
+#ifdef HAL_HASH_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_hash.h"
+#endif /* HAL_HASH_MODULE_ENABLED */
+
+#ifdef HAL_I2C_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_i2c.h"
+#endif /* HAL_I2C_MODULE_ENABLED */
+
+#ifdef HAL_I2S_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_i2s.h"
+#endif /* HAL_I2S_MODULE_ENABLED */
+
+#ifdef HAL_IWDG_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_iwdg.h"
+#endif /* HAL_IWDG_MODULE_ENABLED */
+
+#ifdef HAL_LTDC_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_ltdc.h"
+#endif /* HAL_LTDC_MODULE_ENABLED */
+
+#ifdef HAL_PWR_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_pwr.h"
+#endif /* HAL_PWR_MODULE_ENABLED */
+
+#ifdef HAL_RNG_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_rng.h"
+#endif /* HAL_RNG_MODULE_ENABLED */
+
+#ifdef HAL_RTC_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_rtc.h"
+#endif /* HAL_RTC_MODULE_ENABLED */
+
+#ifdef HAL_SAI_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_sai.h"
+#endif /* HAL_SAI_MODULE_ENABLED */
+
+#ifdef HAL_SD_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_sd.h"
+#endif /* HAL_SD_MODULE_ENABLED */
+
+#ifdef HAL_SPI_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_spi.h"
+#endif /* HAL_SPI_MODULE_ENABLED */
+
+#ifdef HAL_TIM_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_tim.h"
+#endif /* HAL_TIM_MODULE_ENABLED */
+
+#ifdef HAL_UART_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_uart.h"
+#endif /* HAL_UART_MODULE_ENABLED */
+
+#ifdef HAL_USART_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_usart.h"
+#endif /* HAL_USART_MODULE_ENABLED */
+
+#ifdef HAL_IRDA_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_irda.h"
+#endif /* HAL_IRDA_MODULE_ENABLED */
+
+#ifdef HAL_SMARTCARD_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_smartcard.h"
+#endif /* HAL_SMARTCARD_MODULE_ENABLED */
+
+#ifdef HAL_WWDG_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_wwdg.h"
+#endif /* HAL_WWDG_MODULE_ENABLED */
+
+#ifdef HAL_PCD_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_pcd.h"
+#endif /* HAL_PCD_MODULE_ENABLED */
+
+#ifdef HAL_HCD_MODULE_ENABLED
+ #include "mcu/stm32f4xx_hal_hcd.h"
+#endif /* HAL_HCD_MODULE_ENABLED */
+   
+/* Exported macro ------------------------------------------------------------*/
+#ifdef  USE_FULL_ASSERT
+/**
+  * @brief  The assert_param macro is used for function's parameters check.
+  * @param  expr: If expr is false, it calls assert_failed function
+  *         which reports the name of the source file and the source
+  *         line number of the call that failed. 
+  *         If expr is true, it returns no value.
+  * @retval None
+  */
+  #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
+/* Exported functions ------------------------------------------------------- */
+  void assert_failed(uint8_t* file, uint32_t line);
+#else
+  #define assert_param(expr) ((void)0)
+#endif /* USE_FULL_ASSERT */    
+    
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __STM32F4xx_HAL_CONF_H */
+ 
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/hw/bsp/stm32f4discovery/pkg.yml b/hw/bsp/stm32f4discovery/pkg.yml
new file mode 100644
index 00000000..04e74d38
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/pkg.yml
@@ -0,0 +1,39 @@
+#
+# 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: hw/bsp/stm32f4discovery
+pkg.type: bsp
+pkg.description: BSP definition for the stm32f4 discovery board.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - stm32
+    - stm32f4
+    - discovery
+
+pkg.arch: cortex_m4
+pkg.compiler: compiler/arm-none-eabi-m4
+pkg.linkerscript: "stm32f4discovery.ld"
+pkg.linkerscript.bootloader.OVERWRITE: "boot-stm32f4discovery.ld"
+pkg.downloadscript: "stm32f4discovery_download.sh"
+pkg.debugscript: "stm32f4discovery_debug.sh"
+pkg.cflags: -DSTM32F407xx
+pkg.deps:
+    - hw/mcu/stm/stm32f4xx
+    - libs/baselibc
diff --git a/hw/bsp/stm32f4discovery/run_from_flash.ld b/hw/bsp/stm32f4discovery/run_from_flash.ld
new file mode 100644
index 00000000..658d9238
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/run_from_flash.ld
@@ -0,0 +1,204 @@
+/**
+ * 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.
+ */
+ 
+/* Linker script for STM32F407 when running from flash and not using the bootloader */
+
+/* Linker script to configure memory regions. */
+MEMORY
+{ 
+  FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1024K
+  CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+  RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x20000 
+}
+
+/* Linker script to place sections and symbol values. Should be used together
+ * with other linker script that defines memory regions FLASH and RAM.
+ * It references following symbols, which must be defined in code:
+ *   Reset_Handler : Entry of reset handler
+ * 
+ * It defines following symbols, which code can use without definition:
+ *   __exidx_start
+ *   __exidx_end
+ *   __etext
+ *   __data_start__
+ *   __preinit_array_start
+ *   __preinit_array_end
+ *   __init_array_start
+ *   __init_array_end
+ *   __fini_array_start
+ *   __fini_array_end
+ *   __data_end__
+ *   __bss_start__
+ *   __bss_end__
+ *   __end__
+ *   end
+ *   __HeapBase
+ *   __HeapLimit
+ *   __StackLimit
+ *   __StackTop
+ *   __stack
+ *   __coredata_start__
+ *   __coredata_end__
+ *   __corebss_start__
+ *   __corebss_end__
+ *   __ecoredata
+ *   __ecorebs
+ */
+ENTRY(Reset_Handler)
+
+SECTIONS
+{
+    .text :
+    {
+        __isr_vector_start = .;
+        KEEP(*(.isr_vector))
+        __isr_vector_end = .;
+        *(.text*)
+
+        KEEP(*(.init))
+        KEEP(*(.fini))
+
+        /* .ctors */
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+
+        /* .dtors */
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+
+        *(.rodata*)
+
+        KEEP(*(.eh_frame*))
+    } > FLASH
+
+    .ARM.extab : 
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > FLASH
+
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > FLASH
+
+    __exidx_end = .;
+
+    __etext = .;
+
+    .vector_relocation :
+    {
+        . = ALIGN(4);
+        __vector_tbl_reloc__ = .;
+        . = . + (__isr_vector_end - __isr_vector_start);
+        . = ALIGN(4);
+    } > RAM
+
+    .coredata : AT (__etext)
+    {
+        __coredata_start__ = .;
+        *(.data.core)
+        . = ALIGN(4);
+        __coredata_end__ = .;
+    } > CCM
+
+    __ecoredata = __etext + SIZEOF(.coredata);
+        
+    .data : AT (__ecoredata)
+    {
+        __data_start__ = .;
+        *(vtable)
+        *(.data*)
+
+        /* preinit data */
+        PROVIDE_HIDDEN (__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE_HIDDEN (__preinit_array_end = .);
+
+        . = ALIGN(4);
+        /* init data */
+        PROVIDE_HIDDEN (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE_HIDDEN (__init_array_end = .);
+
+
+        . = ALIGN(4);
+        /* finit data */
+        PROVIDE_HIDDEN (__fini_array_start = .);
+        KEEP(*(SORT(.fini_array.*)))
+        KEEP(*(.fini_array))
+        PROVIDE_HIDDEN (__fini_array_end = .);
+
+        KEEP(*(.jcr*))
+        . = ALIGN(4);
+        /* All data end */
+        __data_end__ = .;
+
+    } > RAM
+
+    .corebss (NOLOAD):
+    {
+        . = ALIGN(4);
+        __corebss_start__ = .;
+        *(.bss.core)
+        . = ALIGN(4);
+        __corebss_end__ = .;
+        *(.corebss*)
+        *(.bss.core.nz)
+        . = ALIGN(4);
+        __ecorebss = .;
+    } > CCM
+
+    .bss :
+    {
+        . = ALIGN(4);
+        __bss_start__ = .;
+        *(.bss*)
+        *(COMMON)
+        . = ALIGN(4);
+        __bss_end__ = .;
+    } > RAM
+
+    __HeapBase = .;
+    __HeapLimit = ORIGIN(RAM) + LENGTH(RAM);
+
+    /* .stack_dummy section doesn't contains any symbols. It is only
+     * used for linker to calculate size of stack sections, and assign
+     * values to stack symbols later */
+    .stack_dummy (COPY):
+    {
+        *(.stack*)
+    } > CCM
+
+    /* Set stack top to end of CCM; stack limit is bottom of stack */
+    __StackTop = ORIGIN(CCM) + LENGTH(CCM);
+    __StackLimit = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+    
+    /* Check for CCM overflow */
+    ASSERT(__StackLimit >= __ecorebss, "CCM overflow!")
+}
+
diff --git a/hw/bsp/stm32f4discovery/run_from_loader.ld b/hw/bsp/stm32f4discovery/run_from_loader.ld
new file mode 100644
index 00000000..50b82909
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/run_from_loader.ld
@@ -0,0 +1,210 @@
+/**
+ * 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.
+ */
+
+/* Linker script for STM32F407 when running from flash and using the bootloader */
+
+/* Linker script to configure memory regions. */
+MEMORY
+{ 
+  FLASH (rx) :  ORIGIN = 0x08020000, LENGTH = 384K /* First image slot. */
+  CCM (rwx) :   ORIGIN = 0x10000000, LENGTH = 64K
+  RAM (rwx) :   ORIGIN = 0x20000000, LENGTH = 128K
+}
+
+/* Linker script to place sections and symbol values. Should be used together
+ * with other linker script that defines memory regions FLASH and RAM.
+ * It references following symbols, which must be defined in code:
+ *   Reset_Handler : Entry of reset handler
+ * 
+ * It defines following symbols, which code can use without definition:
+ *   __exidx_start
+ *   __exidx_end
+ *   __etext
+ *   __data_start__
+ *   __preinit_array_start
+ *   __preinit_array_end
+ *   __init_array_start
+ *   __init_array_end
+ *   __fini_array_start
+ *   __fini_array_end
+ *   __data_end__
+ *   __bss_start__
+ *   __bss_end__
+ *   __end__
+ *   end
+ *   __HeapBase
+ *   __HeapLimit
+ *   __StackLimit
+ *   __StackTop
+ *   __stack
+ *   __coredata_start__
+ *   __coredata_end__
+ *   __corebss_start__
+ *   __corebss_end__
+ *   __ecoredata
+ *   __ecorebss
+ */
+ENTRY(Reset_Handler)
+
+SECTIONS
+{
+    /* Reserve space at the start of the image for the header. */
+    .imghdr (NOLOAD):
+    {
+        . = . + 0x20;
+    } > FLASH
+
+    .text :
+    {
+        __isr_vector_start = .;
+        KEEP(*(.isr_vector))
+        __isr_vector_end = .;
+        *(.text*)
+
+        KEEP(*(.init))
+        KEEP(*(.fini))
+
+        /* .ctors */
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+
+        /* .dtors */
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+
+        *(.rodata*)
+
+        KEEP(*(.eh_frame*))
+    } > FLASH
+
+    .ARM.extab : 
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > FLASH
+
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > FLASH
+
+    __exidx_end = .;
+
+    __etext = .;
+
+    .vector_relocation :
+    {
+        . = ALIGN(4);
+        __vector_tbl_reloc__ = .;
+        . = . + (__isr_vector_end - __isr_vector_start);
+        . = ALIGN(4);
+    } > RAM
+
+    .coredata : AT (__etext)
+    {
+        __coredata_start__ = .;
+        *(.data.core)
+        . = ALIGN(4);
+        __coredata_end__ = .;
+    } > CCM
+
+    __ecoredata = __etext + SIZEOF(.coredata);
+        
+    .data : AT (__ecoredata)
+    {
+        __data_start__ = .;
+        *(vtable)
+        *(.data*)
+
+        /* preinit data */
+        PROVIDE_HIDDEN (__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE_HIDDEN (__preinit_array_end = .);
+
+        . = ALIGN(4);
+        /* init data */
+        PROVIDE_HIDDEN (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE_HIDDEN (__init_array_end = .);
+
+
+        . = ALIGN(4);
+        /* finit data */
+        PROVIDE_HIDDEN (__fini_array_start = .);
+        KEEP(*(SORT(.fini_array.*)))
+        KEEP(*(.fini_array))
+        PROVIDE_HIDDEN (__fini_array_end = .);
+
+        KEEP(*(.jcr*))
+        . = ALIGN(4);
+        /* All data end */
+        __data_end__ = .;
+
+    } > RAM
+
+    .corebss (NOLOAD):
+    {
+        . = ALIGN(4);
+        __corebss_start__ = .;
+        *(.bss.core)
+        . = ALIGN(4);
+        __corebss_end__ = .;
+        *(.corebss*)
+        *(.bss.core.nz)
+        . = ALIGN(4);
+        __ecorebss = .;
+    } > CCM
+
+    .bss :
+    {
+        . = ALIGN(4);
+        __bss_start__ = .;
+        *(.bss*)
+        *(COMMON)
+        . = ALIGN(4);
+        __bss_end__ = .;
+    } > RAM
+
+    __HeapBase = .;
+    __HeapLimit = ORIGIN(RAM) + LENGTH(RAM);
+
+    /* .stack_dummy section doesn't contains any symbols. It is only
+     * used for linker to calculate size of stack sections, and assign
+     * values to stack symbols later */
+    .stack_dummy (COPY):
+    {
+        *(.stack*)
+    } > CCM
+
+    /* Set stack top to end of CCM; stack limit is bottom of stack */
+    __StackTop = ORIGIN(CCM) + LENGTH(CCM);
+    __StackLimit = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+    
+    /* Check for CCM overflow */
+    ASSERT(__StackLimit >= __ecorebss, "CCM overflow!")
+}
+
diff --git a/hw/bsp/stm32f4discovery/run_from_sram.ld b/hw/bsp/stm32f4discovery/run_from_sram.ld
new file mode 100644
index 00000000..cdf5efe5
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/run_from_sram.ld
@@ -0,0 +1,202 @@
+/**
+ * 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.
+ */
+
+/* Linker script for STM32F407 when running code from SRAM */
+
+/* Linker script to configure memory regions. */
+MEMORY
+{ 
+  FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1024K
+  CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+  RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x20000
+}
+
+/* Linker script to place sections and symbol values. Should be used together
+ * with other linker script that defines memory regions FLASH and RAM.
+ * It references following symbols, which must be defined in code:
+ *   Reset_Handler : Entry of reset handler
+ * 
+ * It defines following symbols, which code can use without definition:
+ *   __exidx_start
+ *   __exidx_end
+ *   __etext
+ *   __data_start__
+ *   __preinit_array_start
+ *   __preinit_array_end
+ *   __init_array_start
+ *   __init_array_end
+ *   __fini_array_start
+ *   __fini_array_end
+ *   __data_end__
+ *   __bss_start__
+ *   __bss_end__
+ *   __end__
+ *   end
+ *   __HeapBase
+ *   __HeapLimit
+ *   __StackLimit
+ *   __StackTop
+ *   __stack
+ *   __coredata_start__
+ *   __coredata_end__
+ *   __corebss_start__
+ *   __corebss_end__
+ *   __ecoredata
+ *   __ecorebss
+ */
+ENTRY(Reset_Handler)
+
+SECTIONS
+{
+    .text :
+    {
+        __vector_tbl_reloc__ = .;
+        KEEP(*(.isr_vector))
+        *(.text*)
+
+        KEEP(*(.init))
+        KEEP(*(.fini))
+
+        /* .ctors */
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+
+        /* .dtors */
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+
+        *(.rodata*)
+
+        KEEP(*(.eh_frame*))
+    } > RAM
+
+    .ARM.extab : 
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > RAM
+
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > RAM
+
+    __exidx_end = .;
+
+    . = ALIGN(8);
+    __etext = .;
+
+    .coredata : AT (__etext)
+    {
+        __coredata_start__ = .;
+        *(.data.core)
+        . = ALIGN(8);
+        __coredata_end__ = .;
+    } > CCM
+
+    __ecoredata = __etext + SIZEOF(.coredata);
+
+    /* This section is here so that the start of .data has the same VMA and LMA */
+    .ram_coredata (NOLOAD):
+    {
+        . = . + SIZEOF(.coredata);
+    } > RAM
+
+    .data : AT (__ecoredata)
+    {
+        __data_start__ = .;
+        *(vtable)
+        *(.data*)
+
+        /* preinit data */
+        PROVIDE_HIDDEN (__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE_HIDDEN (__preinit_array_end = .);
+
+        . = ALIGN(4);
+        /* init data */
+        PROVIDE_HIDDEN (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE_HIDDEN (__init_array_end = .);
+
+
+        . = ALIGN(4);
+        /* finit data */
+        PROVIDE_HIDDEN (__fini_array_start = .);
+        KEEP(*(SORT(.fini_array.*)))
+        KEEP(*(.fini_array))
+        PROVIDE_HIDDEN (__fini_array_end = .);
+
+        KEEP(*(.jcr*))
+        . = ALIGN(4);
+        /* All data end */
+        __data_end__ = .;
+
+    } > RAM
+
+    .corebss (NOLOAD):
+    {
+        . = ALIGN(4);
+        __corebss_start__ = .;
+        *(.bss.core)
+        . = ALIGN(4);
+        __corebss_end__ = .;
+        *(.corebss*)
+        *(.bss.core.nz)
+        . = ALIGN(4);
+        __ecorebss = .;
+    } > CCM
+
+    .bss :
+    {
+        . = ALIGN(4);
+        __bss_start__ = .;
+        *(.bss*)
+        *(COMMON)
+        . = ALIGN(4);
+        __bss_end__ = .;
+    } > RAM
+
+    __HeapBase = .;
+    __HeapLimit = ORIGIN(RAM) + LENGTH(RAM);
+
+    /* .stack_dummy section doesn't contains any symbols. It is only
+     * used for linker to calculate size of stack sections, and assign
+     * values to stack symbols later */
+    .stack_dummy (COPY):
+    {
+        *(.stack*)
+    } > CCM
+
+    /* Set stack top to end of CCM; stack limit is bottom of stack */
+    __StackTop = ORIGIN(CCM) + LENGTH(CCM);
+    __StackLimit = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+    
+    /* Check for CCM overflow */
+    ASSERT(__StackLimit >= __ecorebss, "CCM overflow!")
+}
+
diff --git a/hw/bsp/stm32f4discovery/src/arch/cortex_m4/startup_STM32F40x.s b/hw/bsp/stm32f4discovery/src/arch/cortex_m4/startup_STM32F40x.s
new file mode 100644
index 00000000..646b4bf4
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/src/arch/cortex_m4/startup_STM32F40x.s
@@ -0,0 +1,343 @@
+/* File: startup_STM32F40x.S
+ * Purpose: startup file for Cortex-M4 devices. Should use with
+ *   GCC for ARM Embedded Processors
+ * Version: V1.4
+ * Date: 09 July 2012
+ *
+ * Copyright (c) 2011, 2012, ARM Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the ARM Limited nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL ARM LIMITED BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+    .syntax unified
+    .arch armv7-m
+
+    .section .stack
+    .align 3
+#ifdef __STACK_SIZE
+    .equ    Stack_Size, __STACK_SIZE
+#else
+    .equ    Stack_Size, 0xc00
+#endif
+    .globl    __StackTop
+    .globl    __StackLimit
+__StackLimit:
+    .space    Stack_Size
+    .size __StackLimit, . - __StackLimit
+__StackTop:
+    .size __StackTop, . - __StackTop
+
+    .section .heap
+    .align 3
+#ifdef __HEAP_SIZE
+    .equ    Heap_Size, __HEAP_SIZE
+#else
+    .equ    Heap_Size, 0
+#endif
+    .globl    __HeapBase
+    .globl    __HeapLimit
+__HeapBase:
+    .if    Heap_Size
+    .space    Heap_Size
+    .endif
+    .size __HeapBase, . - __HeapBase
+__HeapLimit:
+    .size __HeapLimit, . - __HeapLimit
+
+    .section .isr_vector
+    .align 2
+    .globl __isr_vector
+__isr_vector:
+    .long    __StackTop            /* Top of Stack */
+    .long    Reset_Handler         /* Reset Handler */
+    .long    NMI_Handler           /* NMI Handler */
+    .long    HardFault_Handler     /* Hard Fault Handler */
+    .long    MemManage_Handler     /* MPU Fault Handler */
+    .long    BusFault_Handler      /* Bus Fault Handler */
+    .long    UsageFault_Handler    /* Usage Fault Handler */
+    .long    0                     /* Reserved */
+    .long    0                     /* Reserved */
+    .long    0                     /* Reserved */
+    .long    0                     /* Reserved */
+    .long    SVC_Handler           /* SVCall Handler */
+    .long    DebugMon_Handler      /* Debug Monitor Handler */
+    .long    0                     /* Reserved */
+    .long    PendSV_Handler        /* PendSV Handler */
+    .long    SysTick_Handler       /* SysTick Handler */
+
+    /* External interrupts */
+    .long     WWDG_IRQHandler               /* Window WatchDog */
+    .long     PVD_IRQHandler                /* PVD through EXTI Line detection */
+    .long     TAMP_STAMP_IRQHandler         /* Tamper and TimeStamps through the EXTI line */
+    .long     RTC_WKUP_IRQHandler           /* RTC Wakeup through the EXTI line */
+    .long     FLASH_IRQHandler              /* FLASH */
+    .long     RCC_IRQHandler                /* RCC */
+    .long     EXTI0_IRQHandler              /* EXTI Line0 */
+    .long     EXTI1_IRQHandler              /* EXTI Line1 */
+    .long     EXTI2_IRQHandler              /* EXTI Line2 */
+    .long     EXTI3_IRQHandler              /* EXTI Line3 */
+    .long     EXTI4_IRQHandler              /* EXTI Line4 */
+    .long     DMA1_Stream0_IRQHandler       /* DMA1 Stream 0 */
+    .long     DMA1_Stream1_IRQHandler       /* DMA1 Stream 1 */
+    .long     DMA1_Stream2_IRQHandler       /* DMA1 Stream 2 */
+    .long     DMA1_Stream3_IRQHandler       /* DMA1 Stream 3 */
+    .long     DMA1_Stream4_IRQHandler       /* DMA1 Stream 4 */
+    .long     DMA1_Stream5_IRQHandler       /* DMA1 Stream 5 */
+    .long     DMA1_Stream6_IRQHandler       /* DMA1 Stream 6 */
+    .long     ADC_IRQHandler                /* ADC1, ADC2 and ADC3s */
+    .long     CAN1_TX_IRQHandler            /* CAN1 TX */
+    .long     CAN1_RX0_IRQHandler           /* CAN1 RX0 */
+    .long     CAN1_RX1_IRQHandler           /* CAN1 RX1 */
+    .long     CAN1_SCE_IRQHandler           /* CAN1 SCE */
+    .long     EXTI9_5_IRQHandler            /* External Line[9:5]s */
+    .long     TIM1_BRK_TIM9_IRQHandler      /* TIM1 Break and TIM9 */
+    .long     TIM1_UP_TIM10_IRQHandler      /* TIM1 Update and TIM10 */
+    .long     TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */
+    .long     TIM1_CC_IRQHandler            /* TIM1 Capture Compare */
+    .long     TIM2_IRQHandler               /* TIM2 */
+    .long     TIM3_IRQHandler               /* TIM3 */
+    .long     TIM4_IRQHandler               /* TIM4 */
+    .long     I2C1_EV_IRQHandler            /* I2C1 Event */
+    .long     I2C1_ER_IRQHandler            /* I2C1 Error */
+    .long     I2C2_EV_IRQHandler            /* I2C2 Event */
+    .long     I2C2_ER_IRQHandler            /* I2C2 Error */
+    .long     SPI1_IRQHandler               /* SPI1 */
+    .long     SPI2_IRQHandler               /* SPI2 */
+    .long     USART1_IRQHandler             /* USART1 */
+    .long     USART2_IRQHandler             /* USART2 */
+    .long     USART3_IRQHandler             /* USART3 */
+    .long     EXTI15_10_IRQHandler          /* External Line[15:10]s */
+    .long     RTC_Alarm_IRQHandler          /* RTC Alarm (A and B) through EXTI Line */
+    .long     OTG_FS_WKUP_IRQHandler        /* USB OTG FS Wakeup through EXTI line */
+    .long     TIM8_BRK_TIM12_IRQHandler     /* TIM8 Break and TIM12 */
+    .long     TIM8_UP_TIM13_IRQHandler      /* TIM8 Update and TIM13 */
+    .long     TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */
+    .long     TIM8_CC_IRQHandler            /* TIM8 Capture Compare */
+    .long     DMA1_Stream7_IRQHandler       /* DMA1 Stream7 */
+    .long     FSMC_IRQHandler               /* FSMC */
+    .long     SDIO_IRQHandler               /* SDIO */
+    .long     TIM5_IRQHandler               /* TIM5 */
+    .long     SPI3_IRQHandler               /* SPI3 */
+    .long     UART4_IRQHandler              /* UART4 */
+    .long     UART5_IRQHandler              /* UART5 */
+    .long     TIM6_DAC_IRQHandler           /* TIM6 and DAC1&2 underrun errors */
+    .long     TIM7_IRQHandler               /* TIM7 */
+    .long     DMA2_Stream0_IRQHandler       /* DMA2 Stream 0 */
+    .long     DMA2_Stream1_IRQHandler       /* DMA2 Stream 1 */
+    .long     DMA2_Stream2_IRQHandler       /* DMA2 Stream 2 */
+    .long     DMA2_Stream3_IRQHandler       /* DMA2 Stream 3 */
+    .long     DMA2_Stream4_IRQHandler       /* DMA2 Stream 4 */
+    .long     ETH_IRQHandler                /* Ethernet */
+    .long     ETH_WKUP_IRQHandler           /* Ethernet Wakeup through EXTI line */
+    .long     CAN2_TX_IRQHandler            /* CAN2 TX */
+    .long     CAN2_RX0_IRQHandler           /* CAN2 RX0 */
+    .long     CAN2_RX1_IRQHandler           /* CAN2 RX1 */
+    .long     CAN2_SCE_IRQHandler           /* CAN2 SCE */
+    .long     OTG_FS_IRQHandler             /* USB OTG FS */
+    .long     DMA2_Stream5_IRQHandler       /* DMA2 Stream 5 */
+    .long     DMA2_Stream6_IRQHandler       /* DMA2 Stream 6 */
+    .long     DMA2_Stream7_IRQHandler       /* DMA2 Stream 7 */
+    .long     USART6_IRQHandler             /* USART6 */
+    .long     I2C3_EV_IRQHandler            /* I2C3 event */
+    .long     I2C3_ER_IRQHandler            /* I2C3 error */
+    .long     OTG_HS_EP1_OUT_IRQHandler     /* USB OTG HS End Point 1 Out */
+    .long     OTG_HS_EP1_IN_IRQHandler      /* USB OTG HS End Point 1 In */
+    .long     OTG_HS_WKUP_IRQHandler        /* USB OTG HS Wakeup through EXTI */
+    .long     OTG_HS_IRQHandler             /* USB OTG HS */
+    .long     DCMI_IRQHandler               /* DCMI */
+    .long     CRYP_IRQHandler               /* CRYP crypto */
+    .long     HASH_RNG_IRQHandler           /* Hash and Rng */
+    .long     FPU_IRQHandler                /* FPU */
+
+    .size    __isr_vector, . - __isr_vector
+
+    .text
+    .thumb
+    .thumb_func
+    .align 2
+    .globl    Reset_Handler
+    .type    Reset_Handler, %function
+Reset_Handler:
+/* Copy data core section from flash to RAM */
+    ldr    r1, =__etext
+    ldr    r2, =__coredata_start__
+    ldr    r3, =__coredata_end__
+
+.LC0:
+    cmp     r2, r3
+    ittt    lt
+    ldrlt   r0, [r1], #4
+    strlt   r0, [r2], #4
+    blt    .LC0
+
+/*     Loop to copy data from read only memory to RAM. The ranges
+ *      of copy from/to are specified by following symbols evaluated in
+ *      linker script.
+ *      __etext: End of code section, i.e., begin of data sections to copy from.
+ *      __data_start__/__data_end__: RAM address range that data should be
+ *      copied to. Both must be aligned to 4 bytes boundary.  */
+    ldr    r1, =__ecoredata
+    ldr    r2, =__data_start__
+    ldr    r3, =__data_end__
+
+.LC1:
+    cmp     r2, r3
+    ittt    lt
+    ldrlt   r0, [r1], #4
+    strlt   r0, [r2], #4
+    blt    .LC1
+
+/* Set the bss core section to zero */
+    mov     r0, #0
+    ldr     r1, =__corebss_start__
+    ldr     r2, =__corebss_end__
+
+.LC2:
+    cmp     r1, r2
+    itt     lt
+    strlt   r0, [r1], #4
+    blt    .LC2
+
+/* Call system initialization and startup routines */
+    ldr    r0, =SystemInit
+    blx    r0
+    ldr    r0, =_start
+    bx     r0
+    .pool
+    .size Reset_Handler, . - Reset_Handler
+
+    .text
+/*    Macro to define default handlers. Default handler
+ *    will be weak symbol and just dead loops. They can be
+ *    overwritten by other handlers */
+    .macro    def_default_handler    handler_name
+    .align 1
+    .thumb_func
+    .weak    \handler_name
+    .type    \handler_name, %function
+\handler_name :
+    b    .
+    .size    \handler_name, . - \handler_name
+    .endm
+
+    def_default_handler    NMI_Handler
+    def_default_handler    HardFault_Handler
+    def_default_handler    MemManage_Handler
+    def_default_handler    BusFault_Handler
+    def_default_handler    UsageFault_Handler
+    def_default_handler    SVC_Handler
+    def_default_handler    DebugMon_Handler
+    def_default_handler    PendSV_Handler
+    def_default_handler    SysTick_Handler
+    def_default_handler    Default_Handler
+
+    .macro    def_irq_default_handler    handler_name
+    .weak     \handler_name
+    .set      \handler_name, Default_Handler
+    .endm
+
+    def_irq_default_handler     WWDG_IRQHandler
+    def_irq_default_handler     PVD_IRQHandler
+    def_irq_default_handler     TAMP_STAMP_IRQHandler
+    def_irq_default_handler     RTC_WKUP_IRQHandler
+    def_irq_default_handler     FLASH_IRQHandler
+    def_irq_default_handler     RCC_IRQHandler
+    def_irq_default_handler     EXTI0_IRQHandler
+    def_irq_default_handler     EXTI1_IRQHandler
+    def_irq_default_handler     EXTI2_IRQHandler
+    def_irq_default_handler     EXTI3_IRQHandler
+    def_irq_default_handler     EXTI4_IRQHandler
+    def_irq_default_handler     DMA1_Stream0_IRQHandler
+    def_irq_default_handler     DMA1_Stream1_IRQHandler
+    def_irq_default_handler     DMA1_Stream2_IRQHandler
+    def_irq_default_handler     DMA1_Stream3_IRQHandler
+    def_irq_default_handler     DMA1_Stream4_IRQHandler
+    def_irq_default_handler     DMA1_Stream5_IRQHandler
+    def_irq_default_handler     DMA1_Stream6_IRQHandler
+    def_irq_default_handler     ADC_IRQHandler
+    def_irq_default_handler     CAN1_TX_IRQHandler
+    def_irq_default_handler     CAN1_RX0_IRQHandler
+    def_irq_default_handler     CAN1_RX1_IRQHandler
+    def_irq_default_handler     CAN1_SCE_IRQHandler
+    def_irq_default_handler     EXTI9_5_IRQHandler
+    def_irq_default_handler     TIM1_BRK_TIM9_IRQHandler
+    def_irq_default_handler     TIM1_UP_TIM10_IRQHandler
+    def_irq_default_handler     TIM1_TRG_COM_TIM11_IRQHandler
+    def_irq_default_handler     TIM1_CC_IRQHandler
+    def_irq_default_handler     TIM2_IRQHandler
+    def_irq_default_handler     TIM3_IRQHandler
+    def_irq_default_handler     TIM4_IRQHandler
+    def_irq_default_handler     I2C1_EV_IRQHandler
+    def_irq_default_handler     I2C1_ER_IRQHandler
+    def_irq_default_handler     I2C2_EV_IRQHandler
+    def_irq_default_handler     I2C2_ER_IRQHandler
+    def_irq_default_handler     SPI1_IRQHandler
+    def_irq_default_handler     SPI2_IRQHandler
+    def_irq_default_handler     USART1_IRQHandler
+    def_irq_default_handler     USART2_IRQHandler
+    def_irq_default_handler     USART3_IRQHandler
+    def_irq_default_handler     EXTI15_10_IRQHandler
+    def_irq_default_handler     RTC_Alarm_IRQHandler
+    def_irq_default_handler     OTG_FS_WKUP_IRQHandler
+    def_irq_default_handler     TIM8_BRK_TIM12_IRQHandler
+    def_irq_default_handler     TIM8_UP_TIM13_IRQHandler
+    def_irq_default_handler     TIM8_TRG_COM_TIM14_IRQHandler
+    def_irq_default_handler     TIM8_CC_IRQHandler
+    def_irq_default_handler     DMA1_Stream7_IRQHandler
+    def_irq_default_handler     FSMC_IRQHandler
+    def_irq_default_handler     SDIO_IRQHandler
+    def_irq_default_handler     TIM5_IRQHandler
+    def_irq_default_handler     SPI3_IRQHandler
+    def_irq_default_handler     UART4_IRQHandler
+    def_irq_default_handler     UART5_IRQHandler
+    def_irq_default_handler     TIM6_DAC_IRQHandler
+    def_irq_default_handler     TIM7_IRQHandler
+    def_irq_default_handler     DMA2_Stream0_IRQHandler
+    def_irq_default_handler     DMA2_Stream1_IRQHandler
+    def_irq_default_handler     DMA2_Stream2_IRQHandler
+    def_irq_default_handler     DMA2_Stream3_IRQHandler
+    def_irq_default_handler     DMA2_Stream4_IRQHandler
+    def_irq_default_handler     ETH_IRQHandler
+    def_irq_default_handler     ETH_WKUP_IRQHandler
+    def_irq_default_handler     CAN2_TX_IRQHandler
+    def_irq_default_handler     CAN2_RX0_IRQHandler
+    def_irq_default_handler     CAN2_RX1_IRQHandler
+    def_irq_default_handler     CAN2_SCE_IRQHandler
+    def_irq_default_handler     OTG_FS_IRQHandler
+    def_irq_default_handler     DMA2_Stream5_IRQHandler
+    def_irq_default_handler     DMA2_Stream6_IRQHandler
+    def_irq_default_handler     DMA2_Stream7_IRQHandler
+    def_irq_default_handler     USART6_IRQHandler
+    def_irq_default_handler     I2C3_EV_IRQHandler
+    def_irq_default_handler     I2C3_ER_IRQHandler
+    def_irq_default_handler     OTG_HS_EP1_OUT_IRQHandler
+    def_irq_default_handler     OTG_HS_EP1_IN_IRQHandler
+    def_irq_default_handler     OTG_HS_WKUP_IRQHandler
+    def_irq_default_handler     OTG_HS_IRQHandler
+    def_irq_default_handler     DCMI_IRQHandler
+    def_irq_default_handler     CRYP_IRQHandler
+    def_irq_default_handler     HASH_RNG_IRQHandler
+    def_irq_default_handler     FPU_IRQHandler
+    def_irq_default_handler     DEF_IRQHandler
+
+    .end
diff --git a/hw/bsp/stm32f4discovery/src/hal_bsp.c b/hw/bsp/stm32f4discovery/src/hal_bsp.c
new file mode 100644
index 00000000..763002f5
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/src/hal_bsp.c
@@ -0,0 +1,77 @@
+/**
+ * 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 "hal/hal_bsp.h"
+#include "hal/hal_gpio.h"
+#include "hal/hal_flash_int.h"
+#include "mcu/stm32f407xx.h"
+#include "mcu/stm32f4xx_hal_gpio_ex.h"
+#include "mcu/stm32f4_bsp.h"
+#include "bsp/bsp.h"
+#include <assert.h>
+
+static const struct stm32f4_uart_cfg uart_cfg[UART_CNT] = {
+    [0] = {
+        .suc_uart = USART6,
+        .suc_rcc_reg = &RCC->APB2ENR,
+        .suc_rcc_dev = RCC_APB2ENR_USART6EN,
+        .suc_pin_tx = 38,
+        .suc_pin_rx = 39,
+        .suc_pin_rts = 34,
+        .suc_pin_cts = 35,
+        .suc_pin_af = GPIO_AF8_USART6,
+        .suc_irqn = USART6_IRQn
+    }
+};
+
+static const struct bsp_mem_dump dump_cfg[] = {
+    [0] = {
+        .bmd_start = &_ram_start,
+        .bmd_size = RAM_SIZE
+    },
+    [1] = {
+        .bmd_start = &_ccram_start,
+        .bmd_size = CCRAM_SIZE
+    }
+};
+
+const struct stm32f4_uart_cfg *
+bsp_uart_config(int port)
+{
+    assert(port < UART_CNT);
+    return &uart_cfg[port];
+}
+
+const struct hal_flash *
+bsp_flash_dev(uint8_t id)
+{
+    /*
+     * Internal flash mapped to id 0.
+     */
+    if (id != 0) {
+        return NULL;
+    }
+    return &stm32f4_flash_dev;
+}
+
+const struct bsp_mem_dump *
+bsp_core_dump(int *area_cnt)
+{
+    *area_cnt = sizeof(dump_cfg) / sizeof(dump_cfg[0]);
+    return dump_cfg;
+}
diff --git a/hw/bsp/stm32f4discovery/src/libc_stubs.c b/hw/bsp/stm32f4discovery/src/libc_stubs.c
new file mode 100644
index 00000000..b1b6b8b1
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/src/libc_stubs.c
@@ -0,0 +1,84 @@
+/**
+ * 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 <hal/hal_system.h>
+#include <hal/hal_bsp.h>
+
+int _close(int fd);
+int _fstat(int fd, void *s);
+void _exit(int s);
+int _kill(int pid, int sig);
+int _write(int fd, void *b, int nb);
+int _isatty(int c);
+int _lseek(int fd, int off, int w);
+int _read(int fd, void *b, int nb);
+int _getpid(void);
+
+int
+_close(int fd)
+{
+    return -1;
+}
+
+int
+_fstat(int fd, void *s)
+{
+    return -1;
+}
+
+
+void
+_exit(int s)
+{
+    system_reset();
+}
+
+int
+_kill(int pid, int sig)
+{
+    return -1;
+}
+
+int
+_write(int fd, void *b, int nb)
+{
+    return -1;
+}
+
+int
+_isatty(int c)
+{
+    return -1;
+}
+
+int
+_lseek(int fd, int off, int w)
+{
+    return -1;
+}
+
+int
+_read(int fd, void *b, int nb)
+{
+    return -1;
+}
+
+int
+_getpid(void) {
+    return -1;
+}
diff --git a/hw/bsp/stm32f4discovery/src/os_bsp.c b/hw/bsp/stm32f4discovery/src/os_bsp.c
new file mode 100644
index 00000000..f2fe9e1a
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/src/os_bsp.c
@@ -0,0 +1,82 @@
+/**
+ * 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.
+ */
+
+/*
+ * XXXX for now have this here.
+ */
+#include <hal/flash_map.h>
+#include <hal/hal_bsp.h>
+
+static struct flash_area bsp_flash_areas[] = {
+    [FLASH_AREA_BOOTLOADER] = {
+        .fa_flash_id = 0,	/* internal flash */
+        .fa_off = 0x08000000,	/* beginning */
+        .fa_size = (32 * 1024)
+    },
+    /* 2 * 16K and 1*64K sectors here */
+    [FLASH_AREA_IMAGE_0] = {
+        .fa_flash_id = 0,
+        .fa_off = 0x08020000,
+        .fa_size = (384 * 1024)
+    },
+    [FLASH_AREA_IMAGE_1] = {
+        .fa_flash_id = 0,
+        .fa_off = 0x08080000,
+        .fa_size = (384 * 1024)
+    },
+    [FLASH_AREA_IMAGE_SCRATCH] = {
+        .fa_flash_id = 0,
+        .fa_off = 0x080e0000,
+        .fa_size = (128 * 1024)
+    },
+    [FLASH_AREA_NFFS] = {
+        .fa_flash_id = 0,
+        .fa_off = 0x08008000,
+        .fa_size = (32 * 1024)
+    }
+};
+
+void _close(int fd);
+
+/*
+ * Returns the flash map slot where the currently active image is located.
+ * If executing from internal flash from fixed location, that slot would
+ * be easy to find.
+ * If images are in external flash, and copied to RAM for execution, then
+ * this routine would have to figure out which one of those slots is being
+ * used.
+ */
+int
+bsp_imgr_current_slot(void)
+{
+    return FLASH_AREA_IMAGE_0;
+}
+
+void
+bsp_init(void)
+{
+    /*
+     * XXX this reference is here to keep this function in.
+     */
+    _sbrk(0);
+    _close(0);
+    flash_area_init(bsp_flash_areas,
+                    sizeof(bsp_flash_areas) / sizeof(bsp_flash_areas[0]));
+}
+
diff --git a/hw/bsp/stm32f4discovery/src/sbrk.c b/hw/bsp/stm32f4discovery/src/sbrk.c
new file mode 100644
index 00000000..e95a5d7a
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/src/sbrk.c
@@ -0,0 +1,50 @@
+/**
+ * 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.
+ */
+
+extern char __HeapBase;
+extern char __HeapLimit;
+
+void *
+_sbrk(int incr)
+{
+    static char *brk = &__HeapBase;
+
+    void *prev_brk;
+
+    if (incr < 0) {
+        /* Returning memory to the heap. */
+        incr = -incr;
+        if (brk - incr < &__HeapBase) {
+            prev_brk = (void *)-1;
+        } else {
+            prev_brk = brk;
+            brk -= incr;
+        }
+    } else {
+        /* Allocating memory from the heap. */
+        if (&__HeapLimit - brk >= incr) {
+            prev_brk = brk;
+            brk += incr;
+        } else {
+            prev_brk = (void *)-1;
+        }
+    }
+
+    return prev_brk;
+}
diff --git a/hw/bsp/stm32f4discovery/src/system_stm32f4xx.c b/hw/bsp/stm32f4discovery/src/system_stm32f4xx.c
new file mode 100644
index 00000000..34036cac
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/src/system_stm32f4xx.c
@@ -0,0 +1,351 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32f4xx.c
+  * @author  MCD Application Team
+  * @version V1.3.0
+  * @date    01-July-2015
+  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
+  *
+  *   This file provides two functions and one global variable to be called from 
+  *   user application:
+  *      - SystemInit(): This function is called at startup just after reset and 
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32f4xx.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick 
+  *                                  timer or configure other parameters.
+  *                                     
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  *
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT 2015 STMicroelectronics</center></h2>
+  *
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. Redistributions in binary form must reproduce the above copyright notice,
+  *      this list of conditions and the following disclaimer in the documentation
+  *      and/or other materials provided with the distribution.
+  *   3. Neither the name of STMicroelectronics nor the names of its contributors
+  *      may be used to endorse or promote products derived from this software
+  *      without specific prior written permission.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32f4xx_system
+  * @{
+  */  
+  
+/** @addtogroup STM32F4xx_System_Private_Includes
+  * @{
+  */
+
+#include "mcu/stm32f4xx.h"
+#include "bsp/cmsis_nvic.h"
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_Defines
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_Variables
+  * @{
+  */
+  /* This variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+uint32_t SystemCoreClock = 168000000;
+
+const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+static void SystemClock_Config(void);
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F4xx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system
+  *         Initialize the FPU setting, vector table location and External memory 
+  *         configuration.
+  * @param  None
+  * @retval None
+  */
+void SystemInit(void)
+{
+  /* FPU settings ------------------------------------------------------------*/
+  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+    SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */
+  #endif
+  /* Reset the RCC clock configuration to the default reset state ------------*/
+  /* Set HSION bit */
+  RCC->CR |= (uint32_t)0x00000001;
+
+  /* Reset CFGR register */
+  RCC->CFGR = 0x00000000;
+
+  /* Reset HSEON, CSSON and PLLON bits */
+  RCC->CR &= (uint32_t)0xFEF6FFFF;
+
+  /* Reset PLLCFGR register */
+  RCC->PLLCFGR = 0x24003010;
+
+  /* Reset HSEBYP bit */
+  RCC->CR &= (uint32_t)0xFFFBFFFF;
+
+  /* Disable all interrupts */
+  RCC->CIR = 0x00000000;
+
+  /* Configure System Clock */
+  SystemClock_Config();
+
+  /* Relocate the vector table */
+  NVIC_Relocate();
+}
+
+/**
+   * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *           
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.         
+  *     
+  * @note   - The system frequency computed by this function is not the real 
+  *           frequency in the chip. It is calculated based on the predefined 
+  *           constant and the selected clock source:
+  *             
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
+  *                                              
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
+  *                          
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) 
+  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.
+  *         
+  *         (*) HSI_VALUE is a constant defined in stm32f4xx_hal_conf.h file (default value
+  *             16 MHz) but the real value may vary depending on the variations
+  *             in voltage and temperature.   
+  *    
+  *         (**) HSE_VALUE is a constant defined in stm32f4xx_hal_conf.h file (its value
+  *              depends on the application requirements), user has to ensure that HSE_VALUE
+  *              is same as the real frequency of the crystal used. Otherwise, this function
+  *              may have wrong result.
+  *                
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  *     
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate(void)
+{
+  uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
+  
+  /* Get SYSCLK source -------------------------------------------------------*/
+  tmp = RCC->CFGR & RCC_CFGR_SWS;
+
+  switch (tmp)
+  {
+    case 0x00:  /* HSI used as system clock source */
+      SystemCoreClock = HSI_VALUE;
+      break;
+    case 0x04:  /* HSE used as system clock source */
+      SystemCoreClock = HSE_VALUE;
+      break;
+    case 0x08:  /* PLL used as system clock source */
+
+      /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
+         SYSCLK = PLL_VCO / PLL_P
+         */    
+      pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
+      pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
+      
+      if (pllsource != 0)
+      {
+        /* HSE used as PLL clock source */
+        pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
+      }
+      else
+      {
+        /* HSI used as PLL clock source */
+        pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
+      }
+
+      pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
+      SystemCoreClock = pllvco/pllp;
+      break;
+    default:
+      SystemCoreClock = HSI_VALUE;
+      break;
+  }
+  /* Compute HCLK frequency --------------------------------------------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
+  /* HCLK frequency */
+  SystemCoreClock >>= tmp;
+}
+
+/**
+  * @brief  System Clock Configuration
+  *         The system Clock is configured as follow : 
+  *            System Clock source            = PLL (HSE)
+  *            SYSCLK(Hz)                     = 168000000
+  *            HCLK(Hz)                       = 168000000
+  *            AHB Prescaler                  = 1
+  *            APB1 Prescaler                 = 4
+  *            APB2 Prescaler                 = 2
+  *            HSE Frequency(Hz)              = 12000000
+  *            PLL_M                          = 12
+  *            PLL_N                          = 336
+  *            PLL_P                          = 2
+  *            PLL_Q                          = 7
+  *            VDD(V)                         = 3.3
+  *            Main regulator output voltage  = Scale1 mode
+  *            Flash Latency(WS)              = 5
+  * @param  None
+  * @retval None
+  */
+static void SystemClock_Config(void)
+{
+  /* Configure Flash prefetch, Instruction cache, Data cache */ 
+#if (INSTRUCTION_CACHE_ENABLE != 0)
+    __HAL_FLASH_INSTRUCTION_CACHE_ENABLE();
+#endif /* INSTRUCTION_CACHE_ENABLE */
+
+#if (DATA_CACHE_ENABLE != 0)
+    __HAL_FLASH_DATA_CACHE_ENABLE();
+#endif /* DATA_CACHE_ENABLE */
+
+#if (PREFETCH_ENABLE != 0)
+    __HAL_FLASH_PREFETCH_BUFFER_ENABLE();
+#endif /* PREFETCH_ENABLE */
+
+    /* Enable Power Control clock */
+    __HAL_RCC_PWR_CLK_ENABLE();
+  
+    /* The voltage scaling allows optimizing the power consumption when the device is 
+     clocked below the maximum system frequency, to update the voltage scaling value 
+     regarding system frequency refer to product datasheet.  */
+    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
+
+    /* Enable HSE and wait till HSE is ready */  
+    RCC->CR |= ((uint32_t)RCC_CR_HSEON);
+    while(__HAL_RCC_GET_FLAG(RCC_FLAG_HSERDY) == RESET) {
+        /* XXX: some error should occur here */
+    }
+
+    /* HCLK Configuration */
+    MODIFY_REG(RCC->CFGR, RCC_CFGR_HPRE, RCC_SYSCLK_DIV1);
+
+    /* PCLK1 Configuration */ 
+    MODIFY_REG(RCC->CFGR, RCC_CFGR_PPRE1, RCC_HCLK_DIV4);
+
+    /* PCLK2 Configuration */ 
+    MODIFY_REG(RCC->CFGR, RCC_CFGR_PPRE2, (RCC_HCLK_DIV2 << 3));
+
+    /* Configure the main PLL clock source, multiplication and division factors. */
+    WRITE_REG(RCC->PLLCFGR, (RCC_PLLSOURCE_HSE  | \
+                             12                 | \
+                             (336 << POSITION_VAL(RCC_PLLCFGR_PLLN))  | \
+                             (((RCC_PLLP_DIV2 >> 1) -1) << POSITION_VAL(RCC_PLLCFGR_PLLP)) | \
+                             (7 << POSITION_VAL(RCC_PLLCFGR_PLLQ))));
+
+    /* Enable the main PLL. */
+    __HAL_RCC_PLL_ENABLE();
+
+    /* Wait till PLL is ready */  
+    while(__HAL_RCC_GET_FLAG(RCC_FLAG_PLLRDY) == RESET) {
+        /* XXX: handle this */
+    }
+
+    /* Enable the Flash prefetch */
+    __HAL_FLASH_PREFETCH_BUFFER_ENABLE();
+
+    /* Set flash wait states */
+    __HAL_FLASH_SET_LATENCY(FLASH_LATENCY_5);
+  
+    /* Start PLL */  
+    __HAL_RCC_SYSCLK_CONFIG(RCC_SYSCLKSOURCE_PLLCLK);
+
+    while (__HAL_RCC_GET_SYSCLK_SOURCE() != RCC_SYSCLKSOURCE_STATUS_PLLCLK) {
+        /* XXX: deal with this*/
+    }
+}
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+  
+/**
+  * @}
+  */    
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/hw/bsp/stm32f4discovery/stm32f4discovery.ld b/hw/bsp/stm32f4discovery/stm32f4discovery.ld
new file mode 100644
index 00000000..4c1541d3
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/stm32f4discovery.ld
@@ -0,0 +1,213 @@
+/**
+ * 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.
+ */
+
+/* Linker script for STM32F407 when running from flash and using the bootloader */
+
+/* Linker script to configure memory regions. */
+MEMORY
+{ 
+  FLASH (rx) :  ORIGIN = 0x08020000, LENGTH = 384K /* First image slot. */
+  CCM (rwx) :   ORIGIN = 0x10000000, LENGTH = 64K
+  RAM (rwx) :   ORIGIN = 0x20000000, LENGTH = 128K
+}
+
+/* Linker script to place sections and symbol values. Should be used together
+ * with other linker script that defines memory regions FLASH and RAM.
+ * It references following symbols, which must be defined in code:
+ *   Reset_Handler : Entry of reset handler
+ * 
+ * It defines following symbols, which code can use without definition:
+ *   __exidx_start
+ *   __exidx_end
+ *   __etext
+ *   __data_start__
+ *   __preinit_array_start
+ *   __preinit_array_end
+ *   __init_array_start
+ *   __init_array_end
+ *   __fini_array_start
+ *   __fini_array_end
+ *   __data_end__
+ *   __bss_start__
+ *   __bss_end__
+ *   __end__
+ *   end
+ *   __HeapBase
+ *   __HeapLimit
+ *   __StackLimit
+ *   __StackTop
+ *   __stack
+ *   __coredata_start__
+ *   __coredata_end__
+ *   __corebss_start__
+ *   __corebss_end__
+ *   __ecoredata
+ *   __ecorebss
+ */
+ENTRY(Reset_Handler)
+
+SECTIONS
+{
+    /* Reserve space at the start of the image for the header. */
+    .imghdr (NOLOAD):
+    {
+        . = . + 0x20;
+    } > FLASH
+
+    .text :
+    {
+        __isr_vector_start = .;
+        KEEP(*(.isr_vector))
+        __isr_vector_end = .;
+        *(.text*)
+
+        KEEP(*(.init))
+        KEEP(*(.fini))
+
+        /* .ctors */
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+
+        /* .dtors */
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+
+        *(.rodata*)
+
+        KEEP(*(.eh_frame*))
+    } > FLASH
+
+    .ARM.extab : 
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > FLASH
+
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > FLASH
+
+    __exidx_end = .;
+
+    __etext = .;
+
+    .vector_relocation :
+    {
+        . = ALIGN(4);
+        __vector_tbl_reloc__ = .;
+        . = . + (__isr_vector_end - __isr_vector_start);
+        . = ALIGN(4);
+    } > RAM
+
+    .coredata : AT (__etext)
+    {
+        __coredata_start__ = .;
+        *(.data.core)
+        . = ALIGN(4);
+        __coredata_end__ = .;
+    } > CCM
+
+    __ecoredata = __etext + SIZEOF(.coredata);
+        
+    .data : AT (__ecoredata)
+    {
+        __data_start__ = .;
+        *(vtable)
+        *(.data*)
+
+        /* preinit data */
+        PROVIDE_HIDDEN (__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE_HIDDEN (__preinit_array_end = .);
+
+        . = ALIGN(4);
+        /* init data */
+        PROVIDE_HIDDEN (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE_HIDDEN (__init_array_end = .);
+
+
+        . = ALIGN(4);
+        /* finit data */
+        PROVIDE_HIDDEN (__fini_array_start = .);
+        KEEP(*(SORT(.fini_array.*)))
+        KEEP(*(.fini_array))
+        PROVIDE_HIDDEN (__fini_array_end = .);
+
+        KEEP(*(.jcr*))
+        . = ALIGN(4);
+        /* All data end */
+        __data_end__ = .;
+
+    } > RAM
+
+    .corebss (NOLOAD):
+    {
+        . = ALIGN(4);
+        __corebss_start__ = .;
+        *(.bss.core)
+        . = ALIGN(4);
+        __corebss_end__ = .;
+        *(.corebss*)
+        *(.bss.core.nz)
+        . = ALIGN(4);
+        __ecorebss = .;
+    } > CCM
+
+    .bss :
+    {
+        . = ALIGN(4);
+        __bss_start__ = .;
+        *(.bss*)
+        *(COMMON)
+        . = ALIGN(4);
+        __bss_end__ = .;
+    } > RAM
+
+    __HeapBase = .;
+    __HeapLimit = ORIGIN(RAM) + LENGTH(RAM);
+
+    _ram_start = ORIGIN(RAM);
+    _ccram_start = ORIGIN(CCRAM);
+
+    /* .stack_dummy section doesn't contains any symbols. It is only
+     * used for linker to calculate size of stack sections, and assign
+     * values to stack symbols later */
+    .stack_dummy (COPY):
+    {
+        *(.stack*)
+    } > CCM
+
+    /* Set stack top to end of CCM; stack limit is bottom of stack */
+    __StackTop = ORIGIN(CCM) + LENGTH(CCM);
+    __StackLimit = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+    
+    /* Check for CCM overflow */
+    ASSERT(__StackLimit >= __ecorebss, "CCM overflow!")
+}
+
diff --git a/hw/bsp/stm32f4discovery/stm32f4discovery_debug.sh b/hw/bsp/stm32f4discovery/stm32f4discovery_debug.sh
new file mode 100644
index 00000000..c1a889ba
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/stm32f4discovery_debug.sh
@@ -0,0 +1,47 @@
+#!/bin/sh
+# 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.
+#
+# Called: $0 <bsp_path> <binary> [identities...]
+#  - bsp_directory_path is absolute path to hw/bsp/bsp_name
+#  - binary is the path to prefix to target binary, .elf appended to name is
+#    the ELF file
+#  - identities is the project identities string.
+# 
+#
+if [ $# -lt 2 ]; then
+    echo "Need binary to debug"
+    exit 1
+fi
+
+MY_PATH=$1
+FILE_NAME=$2.elf
+GDB_CMD_FILE=.gdb_cmds
+
+echo "Debugging" $FILE_NAME
+
+#
+# Block Ctrl-C from getting passed to openocd.
+# Exit openocd when gdb detaches.
+#
+set -m
+openocd -f board/stm32f4discovery.cfg -s $MY_PATH -c "gdb_port 3333; telnet_port 4444; stm32f4x.cpu configure -event gdb-detach {shutdown}" -c init -c "reset halt" &
+set +m
+
+echo "target remote localhost:3333" > $GDB_CMD_FILE
+arm-none-eabi-gdb -x $GDB_CMD_FILE $FILE_NAME
+rm $GDB_CMD_FILE
diff --git a/hw/bsp/stm32f4discovery/stm32f4discovery_download.sh b/hw/bsp/stm32f4discovery/stm32f4discovery_download.sh
new file mode 100644
index 00000000..87013c65
--- /dev/null
+++ b/hw/bsp/stm32f4discovery/stm32f4discovery_download.sh
@@ -0,0 +1,56 @@
+#!/bin/bash
+# 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.
+#
+# Called: $0 <bsp_directory_path> <binary> [features...]
+#  - bsp_directory_path is absolute path to hw/bsp/bsp_name
+#  - binary is the path to prefix to target binary, .elf.bin appended to this
+#    name is the raw binary format of the binary.
+#  - features are the target features. So you can have e.g. different
+#    flash offset for bootloader 'feature'
+# 
+#
+if [ $# -lt 2 ]; then
+    echo "Need binary to download"
+    exit 1
+fi
+
+MYPATH=$1
+BASENAME=$2
+IS_BOOTLOADER=0
+
+# Look for 'bootloader' from 3rd arg onwards
+shift
+shift
+while [ $# -gt 0 ]; do
+    if [ $1 = "bootloader" ]; then
+	IS_BOOTLOADER=1
+    fi
+    shift
+done
+
+if [ $IS_BOOTLOADER -eq 1 ]; then
+    FLASH_OFFSET=0x08000000
+    FILE_NAME=$BASENAME.elf.bin
+else
+    FLASH_OFFSET=0x08020000
+    FILE_NAME=$BASENAME.img
+fi
+echo "Downloading" $FILE_NAME "to" $FLASH_OFFSET
+
+openocd -f board/stm32f4discovery.cfg -s $MYPATH -c init -c "reset halt" -c "flash write_image erase $FILE_NAME $FLASH_OFFSET" -c "reset run" -c shutdown
+
diff --git a/hw/hal/include/hal/hal_cputime.h b/hw/hal/include/hal/hal_cputime.h
index 7932de65..08cf2dec 100644
--- a/hw/hal/include/hal/hal_cputime.h
+++ b/hw/hal/include/hal/hal_cputime.h
@@ -26,6 +26,22 @@ extern "C" {
 
 #include "os/queue.h"
 
+/*
+ * NOTE: these definitions allow one to override the cputime frequency used.
+ * The reason these definitions exist is to make the code more efficient/smaller
+ * when CPUTIME counts at 1 MHz.
+ *
+ * For those who want a different cputime frequency, you can set the macro
+ * HAL_CPUTIME to the desired frequency in your project, target or bsp.
+ */
+#ifndef HAL_CPUTIME
+#define HAL_CPUTIME 1000000
+#endif
+
+#if (HAL_CPUTIME == 1000000)
+#define HAL_CPUTIME_1MHZ
+#endif
+
 /* CPU timer callback function */
 struct cpu_timer;
 typedef void (*cputimer_func)(void *arg);
diff --git a/hw/hal/include/hal/hal_uart.h b/hw/hal/include/hal/hal_uart.h
index 4809bfff..12337278 100644
--- a/hw/hal/include/hal/hal_uart.h
+++ b/hw/hal/include/hal/hal_uart.h
@@ -76,6 +76,12 @@ enum hal_uart_flow_ctl {
 int hal_uart_config(int uart, int32_t speed, uint8_t databits, uint8_t stopbits,
   enum hal_uart_parity parity, enum hal_uart_flow_ctl flow_ctl);
 
+/*
+ * Close UART port. Can call hal_uart_config() with different settings after
+ * calling this.
+ */
+int hal_uart_close(int port);
+
 /**
  * hal uart start tx
  *
diff --git a/hw/mcu/native/src/hal_uart.c b/hw/mcu/native/src/hal_uart.c
index c30a4148..41afcc66 100644
--- a/hw/mcu/native/src/hal_uart.c
+++ b/hw/mcu/native/src/hal_uart.c
@@ -148,9 +148,9 @@ uart_transmit_char(struct uart *uart)
         OS_EXIT_CRITICAL(sr);
         return 0;
     }
+    ch = rc;
     uart_log_data(uart, 1, ch);
     OS_EXIT_CRITICAL(sr);
-    ch = rc;
     rc = write(uart->u_fd, &ch, 1);
     if (rc <= 0) {
         /* XXX EOF/error, what now? */
diff --git a/hw/mcu/nordic/nrf52xxx/src/hal_gpio.c b/hw/mcu/nordic/nrf52xxx/src/hal_gpio.c
index d803ec0b..490b8a3f 100644
--- a/hw/mcu/nordic/nrf52xxx/src/hal_gpio.c
+++ b/hw/mcu/nordic/nrf52xxx/src/hal_gpio.c
@@ -21,289 +21,35 @@
 #include "bsp/cmsis_nvic.h"
 #include "mcu/nrf52.h"
 #include "mcu/nrf52_bitfields.h"
+#include <stdlib.h>
 #include <assert.h>
 
- /* XXX: Notes
- * 1) Right now, we are not disabling the NVIC interrupt source; we only
- * disable the external interrupt from occurring. Dont think either way
- * to do it is an issue... when we release we may want to disable the NVIC
- *
- * 2) investigate how thread safe these routines are. HAL_GPIO_Init, for
- * example. Looks like if it gets interrupted while doing config an error
- * may occur. Read/modify write could cause screw-ups.
- *
- * 3) Currently, this code does not change the interrupt priority of the
+/*
+ * 1) Currently, this code does not change the interrupt priority of the
  * external interrupt vectors in the NVIC. The application developer must
  * decide on the priority level for each external interrupt and program that
  * by using the CMSIS NVIC API  (NVIC_SetPriority and NVIC_SetPriorityGrouping)
  *
- * 4) The code probably does not handle "re-purposing" gpio very well.
+ * 2) The code probably does not handle "re-purposing" gpio very well.
  * "Re-purposing" means changing a gpio from input to output, or calling
  * gpio_init_in and expecting previously enabled interrupts to be stopped.
  *
- * 5) Possbily add access to HAL_GPIO_DeInit.
  */
 
 /*
  * GPIO pin mapping
  *
  */
-#define GPIO_INDEX(pin)     (pin)
-#define GPIO_MASK(pin)      (1 << GPIO_INDEX(pin))
+#define HAL_GPIO_MAX_IRQ        8
+#define HAL_GPIO_MASK(pin)      (1 << pin)
 
 /* Storage for GPIO callbacks. */
-struct gpio_irq_obj
-{
+struct hal_gpio_irq {
+    gpio_irq_handler_t func;
     void *arg;
-    gpio_irq_handler_t isr;
-};
-
-#if 0
-static struct gpio_irq_obj gpio_irq_handlers[16];
-#endif
-
-struct ext_irqs
-{
-    volatile uint32_t irq0;
-    volatile uint32_t irq1;
-    volatile uint32_t irq2;
-    volatile uint32_t irq3;
-    volatile uint32_t irq4;
-    volatile uint32_t irq9_5;
-    volatile uint32_t irq15_10;
 };
-struct ext_irqs ext_irq_counts;
-
-#if 0
-/**
- * ext irq handler
- *
- * Handles the gpio interrupt attached to a gpio pin.
- *
- * @param index
- */
-static void
-ext_irq_handler(int index)
-{
-    uint32_t mask;
-
-    mask = 1 << index;
-    if (__HAL_GPIO_EXTI_GET_IT(mask) != RESET) {
-        __HAL_GPIO_EXTI_CLEAR_IT(mask);
-        gpio_irq_handlers[index].isr(gpio_irq_handlers[index].arg);
-    }
-}
 
-/* External interrupt 0 */
-static void
-ext_irq0(void)
-{
-    ++ext_irq_counts.irq0;
-    ext_irq_handler(0);
-}
-
-/* External interrupt 1 */
-static void
-ext_irq1(void)
-{
-    ++ext_irq_counts.irq1;
-    ext_irq_handler(1);
-}
-
-/* External interrupt 2 */
-static void
-ext_irq2(void)
-{
-    ++ext_irq_counts.irq2;
-    ext_irq_handler(2);
-}
-
-/* External interrupt 3 */
-static void
-ext_irq3(void)
-{
-    ++ext_irq_counts.irq3;
-    ext_irq_handler(3);
-}
-
-/**
- * ext irq4
- *
- *  External interrupt handler for external interrupt 4.
- *
- */
-static void
-ext_irq4(void)
-{
-    ++ext_irq_counts.irq4;
-    ext_irq_handler(4);
-}
-
-/**
- * ext irq9_5
- *
- *  External interrupt handler for irqs 9 through 5.
- *
- */
-static void
-ext_irq9_5(void)
-{
-    int index;
-
-    ++ext_irq_counts.irq9_5;
-    for (index = 5; index <= 9; ++index) {
-        ext_irq_handler(index);
-    }
-}
-
-/**
- * ext irq15_10
- *
- *  External interrupt handler for irqs 15 through 10.
- *
- */
-static void
-ext_irq15_10(void)
-{
-    int index;
-
-    ++ext_irq_counts.irq15_10;
-    for (index = 10; index <= 15; ++index) {
-        ext_irq_handler(index);
-    }
-}
-
-/**
- * hal gpio clk enable
- *
- * Enable the port peripheral clock
- *
- * @param port_idx
- */
-static void
-hal_gpio_clk_enable(uint32_t port_idx)
-{
-    switch (port_idx) {
-        case 0:
-            __HAL_RCC_GPIOA_CLK_ENABLE();
-            break;
-        case 1:
-            __HAL_RCC_GPIOB_CLK_ENABLE();
-            break;
-        case 2:
-            __HAL_RCC_GPIOC_CLK_ENABLE();
-            break;
-        case 3:
-            __HAL_RCC_GPIOD_CLK_ENABLE();
-            break;
-        case 4:
-            __HAL_RCC_GPIOE_CLK_ENABLE();
-            break;
-#if defined GPIOF_BASE
-        case 5:
-            __HAL_RCC_GPIOF_CLK_ENABLE();
-            break;
-#endif
-#if defined GPIOG_BASE
-        case 6:
-            __HAL_RCC_GPIOG_CLK_ENABLE();
-            break;
-#endif
-#if defined GPIOH_BASE
-        case 7:
-            __HAL_RCC_GPIOH_CLK_ENABLE();
-            break;
-#endif
-#if defined GPIOI_BASE
-        case 8:
-            __HAL_RCC_GPIOI_CLK_ENABLE();
-            break;
-#endif
-#if defined GPIOJ_BASE
-        case 9:
-            __HAL_RCC_GPIOJ_CLK_ENABLE();
-            break;
-#endif
-#if defined GPIOK_BASE
-        case 10:
-            __HAL_RCC_GPIOK_CLK_ENABLE();
-            break;
-#endif
-        default:
-            assert(0);
-            break;
-    }
-}
-
-/**
- * hal gpio pin to irq
- *
- * Converts the logical pin number to the IRQ number associated with the
- * external interrupt for that particular GPIO.
- *
- * @param pin
- *
- * @return IRQn_Type
- */
-static IRQn_Type
-hal_gpio_pin_to_irq(int pin)
-{
-    int index;
-    IRQn_Type irqn;
-
-    index = GPIO_INDEX(pin);
-    if (index <= 4) {
-        irqn = GPIOTE_IRQn + index;
-    } else if (index <=  9) {
-        irqn = EXTI9_5_IRQn;
-    } else {
-        irqn = EXTI15_10_IRQn;
-    }
-
-    return irqn;
-}
-#endif
-
-static void
-hal_gpio_set_nvic(IRQn_Type irqn)
-{
-#if 0
-    uint32_t isr;
-
-    switch (irqn) {
-    case EXTI0_IRQn:
-        isr = (uint32_t)&ext_irq0;
-        break;
-    case EXTI1_IRQn:
-        isr = (uint32_t)&ext_irq1;
-        break;
-    case EXTI2_IRQn:
-        isr = (uint32_t)&ext_irq2;
-        break;
-    case EXTI3_IRQn:
-        isr = (uint32_t)&ext_irq3;
-        break;
-    case EXTI4_IRQn:
-        isr = (uint32_t)&ext_irq4;
-        break;
-    case EXTI9_5_IRQn:
-        isr = (uint32_t)&ext_irq9_5;
-        break;
-    case EXTI15_10_IRQn:
-        isr = (uint32_t)&ext_irq15_10;
-        break;
-    default:
-        assert(0);
-        break;
-    }
-
-    /* Set isr in vector table if not yet set */
-    if (NVIC_GetVector(irqn) != isr) {
-        NVIC_SetVector(irqn, isr);
-        NVIC_EnableIRQ(irqn);
-    }
-#endif
-}
+static struct hal_gpio_irq hal_gpio_irqs[HAL_GPIO_MAX_IRQ];
 
 /**
  * gpio init in
@@ -334,7 +80,7 @@ hal_gpio_init_in(int pin, gpio_pull_t pull)
     }
 
     NRF_P0->PIN_CNF[pin] = conf;
-    NRF_P0->DIRCLR = GPIO_MASK(pin);
+    NRF_P0->DIRCLR = HAL_GPIO_MASK(pin);
 
     return 0;
 }
@@ -353,12 +99,12 @@ hal_gpio_init_in(int pin, gpio_pull_t pull)
 int hal_gpio_init_out(int pin, int val)
 {
     if (val) {
-        NRF_P0->OUTSET = GPIO_MASK(pin);
+        NRF_P0->OUTSET = HAL_GPIO_MASK(pin);
     } else {
-        NRF_P0->OUTCLR = GPIO_MASK(pin);
+        NRF_P0->OUTCLR = HAL_GPIO_MASK(pin);
     }
     NRF_P0->PIN_CNF[pin] = GPIO_PIN_CNF_DIR_Output;
-    NRF_P0->DIRSET = GPIO_MASK(pin);
+    NRF_P0->DIRSET = HAL_GPIO_MASK(pin);
     return 0;
 }
 
@@ -371,7 +117,7 @@ int hal_gpio_init_out(int pin, int val)
  */
 void hal_gpio_set(int pin)
 {
-    NRF_P0->OUTSET = GPIO_MASK(pin);
+    NRF_P0->OUTSET = HAL_GPIO_MASK(pin);
 }
 
 /**
@@ -383,7 +129,7 @@ void hal_gpio_set(int pin)
  */
 void hal_gpio_clear(int pin)
 {
-    NRF_P0->OUTCLR = GPIO_MASK(pin);
+    NRF_P0->OUTCLR = HAL_GPIO_MASK(pin);
 }
 
 /**
@@ -414,7 +160,7 @@ void hal_gpio_write(int pin, int val)
  */
 int hal_gpio_read(int pin)
 {
-    return (NRF_P0->IN & GPIO_MASK(pin));
+    return (NRF_P0->IN & HAL_GPIO_MASK(pin));
 }
 
 /**
@@ -433,6 +179,79 @@ int hal_gpio_toggle(int pin)
     return pin_state;
 }
 
+/*
+ * GPIO irq handler
+ *
+ * Handles the gpio interrupt attached to a gpio pin.
+ *
+ * @param index
+ */
+static void
+hal_gpio_irq_handler(void)
+{
+    int i;
+
+    for (i = 0; i < HAL_GPIO_MAX_IRQ; i++) {
+        if (NRF_GPIOTE->EVENTS_IN[i]) {
+            NRF_GPIOTE->EVENTS_IN[i] = 0;
+            if (hal_gpio_irqs[i].func) {
+                hal_gpio_irqs[i].func(hal_gpio_irqs[i].arg);
+            }
+        }
+    }
+}
+
+/*
+ * Register IRQ handler for GPIOTE, and enable it.
+ * Only executed once, during first registration.
+ */
+static void
+hal_gpio_irq_setup(void)
+{
+    static uint8_t irq_setup = 0;
+
+    if (!irq_setup) {
+        NVIC_SetVector(GPIOTE_IRQn, (uint32_t)hal_gpio_irq_handler);
+        NVIC_EnableIRQ(GPIOTE_IRQn);
+        irq_setup = 1;
+    }
+}
+
+/*
+ * Find out whether we have an GPIOTE pin event to use.
+ */
+static int
+hal_gpio_find_empty_slot(void)
+{
+    int i;
+
+    for (i = 0; i < HAL_GPIO_MAX_IRQ; i++) {
+        if (hal_gpio_irqs[i].func == NULL) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+/*
+ * Find the GPIOTE event which handles this pin.
+ */
+static int
+hal_gpio_find_pin(int pin)
+{
+    int i;
+
+    pin = pin << GPIOTE_CONFIG_PSEL_Pos;
+
+    for (i = 0; i < HAL_GPIO_MAX_IRQ; i++) {
+        if (hal_gpio_irqs[i].func &&
+          (NRF_GPIOTE->CONFIG[i] & GPIOTE_CONFIG_PSEL_Msk) == pin) {
+            return i;
+        }
+    }
+    return -1;
+}
+
 /**
  * gpio irq init
  *
@@ -450,101 +269,65 @@ int
 hal_gpio_irq_init(int pin, gpio_irq_handler_t handler, void *arg,
                   gpio_irq_trig_t trig, gpio_pull_t pull)
 {
-#if 0
-    int rc;
-    int irqn;
-    int index;
-    uint32_t pin_mask;
-    uint32_t mode;
-    GPIO_InitTypeDef init_cfg;
-
-    /* Configure the gpio for an external interrupt */
-    rc = 0;
+    uint32_t conf;
+    int i;
+
+    hal_gpio_irq_setup();
+    i = hal_gpio_find_empty_slot();
+    if (i < 0) {
+        return -1;
+    }
+    hal_gpio_init_in(pin, pull);
+
     switch (trig) {
-    case GPIO_TRIG_NONE:
-        rc = -1;
-        break;
     case GPIO_TRIG_RISING:
-        mode = GPIO_MODE_IT_RISING;
+        conf = GPIOTE_CONFIG_POLARITY_LoToHi << GPIOTE_CONFIG_POLARITY_Pos;
         break;
     case GPIO_TRIG_FALLING:
-        mode = GPIO_MODE_IT_FALLING;
+        conf = GPIOTE_CONFIG_POLARITY_HiToLo << GPIOTE_CONFIG_POLARITY_Pos;
         break;
     case GPIO_TRIG_BOTH:
-        mode = GPIO_MODE_IT_RISING_FALLING;
-        break;
-    case GPIO_TRIG_LOW:
-        rc = -1;
-        break;
-    case GPIO_TRIG_HIGH:
-        rc = -1;
+        conf = GPIOTE_CONFIG_POLARITY_Toggle << GPIOTE_CONFIG_POLARITY_Pos;
         break;
     default:
-        rc = -1;
-        break;
+        return -1;
     }
+    conf |= pin << GPIOTE_CONFIG_PSEL_Pos;
+    conf |= GPIOTE_CONFIG_MODE_Event << GPIOTE_CONFIG_MODE_Pos;
 
-    /* Check to make sure no error has occurred */
-    if (!rc) {
-        /* Disable interrupt and clear any pending */
-        gpio_irq_disable(pin);
-        pin_mask = GPIO_MASK(pin);
-        __HAL_GPIO_EXTI_CLEAR_FLAG(pin_mask);
-
-        /* Set the gpio irq handler */
-        index = GPIO_INDEX(pin);
-        gpio_irq_handlers[index].isr = handler;
-        gpio_irq_handlers[index].arg = arg;
-
-        /* Configure the GPIO */
-        init_cfg.Mode = mode;
-        init_cfg.Pull = pull;
-        rc = hal_gpio_init(pin, &init_cfg);
-        if (!rc) {
-            /* Enable interrupt vector in NVIC */
-            irqn = hal_gpio_pin_to_irq(pin);
-            hal_gpio_set_nvic(irqn);
-        }
-    }
+    NRF_GPIOTE->CONFIG[i] = conf;
+
+    hal_gpio_irqs[i].func = handler;
+    hal_gpio_irqs[i].arg = arg;
 
-    return rc;
-#else
-    hal_gpio_set_nvic(0);
     return 0;
-#endif
 }
 
 /**
  * gpio irq release
  *
  * No longer interrupt when something occurs on the pin. NOTE: this function
- * does not change the GPIO push/pull setting nor does it change the
- * SYSCFG EXTICR registers. It also does not disable the NVIC interrupt enable
- * setting for the irq.
+ * does not change the GPIO push/pull setting.
+ * It also does not disable the NVIC interrupt enable setting for the irq.
  *
  * @param pin
  */
 void
 hal_gpio_irq_release(int pin)
 {
-#if 0
-    int index;
-    uint32_t pin_mask;
-
-    /* Disable the interrupt */
-    gpio_irq_disable(pin);
-
-    /* Clear any pending interrupts */
-    pin_mask = GPIO_MASK(pin);
-    __HAL_GPIO_EXTI_CLEAR_FLAG(pin_mask);
-
-    /* Clear out the irq handler */
-    index = GPIO_INDEX(pin);
-    gpio_irq_handlers[index].arg = NULL;
-    gpio_irq_handlers[index].isr = NULL;
-#else
-    return;
-#endif
+    int i;
+
+    i = hal_gpio_find_pin(pin);
+    if (i < 0) {
+        return;
+    }
+    hal_gpio_irq_disable(i);
+
+    NRF_GPIOTE->CONFIG[i] = 0;
+    NRF_GPIOTE->EVENTS_IN[i] = 0;
+
+    hal_gpio_irqs[i].arg = NULL;
+    hal_gpio_irqs[i].func = NULL;
 }
 
 /**
@@ -557,18 +340,14 @@ hal_gpio_irq_release(int pin)
 void
 hal_gpio_irq_enable(int pin)
 {
-#if 0
-    uint32_t ctx;
-    uint32_t mask;
-
-    mask = GPIO_MASK(pin);
-
-    __HAL_DISABLE_INTERRUPTS(ctx);
-    EXTI->IMR |= mask;
-    __HAL_ENABLE_INTERRUPTS(ctx);
-#else
-    return;
-#endif
+    int i;
+
+    i = hal_gpio_find_pin(pin);
+    if (i < 0) {
+        return;
+    }
+    NRF_GPIOTE->EVENTS_IN[i] = 0;
+    NRF_GPIOTE->INTENSET = 1 << i;
 }
 
 /**
@@ -580,15 +359,11 @@ hal_gpio_irq_enable(int pin)
 void
 hal_gpio_irq_disable(int pin)
 {
-#if 0
-    uint32_t ctx;
-    uint32_t mask;
-
-    mask = GPIO_MASK(pin);
-    __HAL_DISABLE_INTERRUPTS(ctx);
-    EXTI->IMR &= ~mask;
-    __HAL_ENABLE_INTERRUPTS(ctx);
-#else
-    return;
-#endif
+    int i;
+
+    i = hal_gpio_find_pin(pin);
+    if (i < 0) {
+        return;
+    }
+    NRF_GPIOTE->INTENCLR = 1 << i;
 }
diff --git a/hw/mcu/nordic/nrf52xxx/src/hal_uart.c b/hw/mcu/nordic/nrf52xxx/src/hal_uart.c
index 49861003..eb9cc32b 100644
--- a/hw/mcu/nordic/nrf52xxx/src/hal_uart.c
+++ b/hw/mcu/nordic/nrf52xxx/src/hal_uart.c
@@ -6,7 +6,7 @@
  * 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,
@@ -31,6 +31,7 @@
 #define UARTE_CONFIG_PARITY	UARTE_CONFIG_PARITY_Msk
 #define UARTE_CONFIG_HWFC	UARTE_CONFIG_HWFC_Msk
 #define UARTE_ENABLE		UARTE_ENABLE_ENABLE_Enabled
+#define UARTE_DISABLE           UARTE_ENABLE_ENABLE_Disabled
 
 /*
  * Only one UART on NRF 52832.
@@ -91,6 +92,9 @@ hal_uart_start_tx(int port)
     int sr;
     int rc;
 
+    if (port != 0) {
+        return;
+    }
     u = &uart;
     __HAL_DISABLE_INTERRUPTS(sr);
     if (u->u_tx_started == 0) {
@@ -113,6 +117,9 @@ hal_uart_start_rx(int port)
     int sr;
     int rc;
 
+    if (port != 0) {
+        return;
+    }
     u = &uart;
     if (u->u_rx_stall) {
         __HAL_DISABLE_INTERRUPTS(sr);
@@ -131,6 +138,10 @@ hal_uart_blocking_tx(int port, uint8_t data)
 {
     struct hal_uart *u;
 
+    if (port != 0) {
+        return;
+    }
+
     u = &uart;
     if (!u->u_open) {
         return;
@@ -202,6 +213,8 @@ hal_uart_baudrate(int baudrate)
         return UARTE_BAUDRATE_BAUDRATE_Baud38400;
     case 57600:
         return UARTE_BAUDRATE_BAUDRATE_Baud57600;
+    case 76800:
+        return UARTE_BAUDRATE_BAUDRATE_Baud76800;
     case 115200:
         return UARTE_BAUDRATE_BAUDRATE_Baud115200;
     case 230400:
@@ -303,7 +316,25 @@ hal_uart_config(int port, int32_t baudrate, uint8_t databits, uint8_t stopbits,
     NRF_UARTE0->RXD.MAXCNT = sizeof(u->u_rx_buf);
     NRF_UARTE0->TASKS_STARTRX = 1;
 
+    u->u_rx_stall = 0;
+    u->u_tx_started = 0;
     u->u_open = 1;
 
     return 0;
 }
+
+int
+hal_uart_close(int port)
+{
+    struct hal_uart *u;
+
+    u = &uart;
+
+    if (port == 0) {
+        u->u_open = 0;
+        NRF_UARTE0->ENABLE = 0;
+        NRF_UARTE0->INTENCLR = 0xffffffff;
+        return 0;
+    }
+    return -1;
+}
diff --git a/hw/mcu/stm/stm32f4xx/src/hal_gpio.c b/hw/mcu/stm/stm32f4xx/src/hal_gpio.c
index 56b457e4..f5707dde 100644
--- a/hw/mcu/stm/stm32f4xx/src/hal_gpio.c
+++ b/hw/mcu/stm/stm32f4xx/src/hal_gpio.c
@@ -59,7 +59,7 @@
  *          - Multiply by 16.
  *          - Add port pin number.
  *
- *      Ex: PD11 = (4 * 16) + 11 = 75.
+ *      Ex: PE11 = (4 * 16) + 11 = 75.
  *          PA0 = (0 * 16) + 0 = 0
  */
 #define GPIO_INDEX(pin)     ((pin) & 0x0F)
diff --git a/hw/mcu/stm/stm32f4xx/src/hal_uart.c b/hw/mcu/stm/stm32f4xx/src/hal_uart.c
index 3b2868fc..43d83eb3 100644
--- a/hw/mcu/stm/stm32f4xx/src/hal_uart.c
+++ b/hw/mcu/stm/stm32f4xx/src/hal_uart.c
@@ -371,3 +371,19 @@ hal_uart_config(int port, int32_t baudrate, uint8_t databits, uint8_t stopbits,
 
     return 0;
 }
+
+int
+hal_uart_close(int port)
+{
+    struct hal_uart *u;
+
+    if (port >= UART_CNT) {
+        return -1;
+    }
+    u = &uarts[port];
+
+    u->u_open = 0;
+    u->u_regs->CR1 = 0;
+
+    return 0;
+}
diff --git a/libs/bleuart/include/bleuart/bleuart.h b/libs/bleuart/include/bleuart/bleuart.h
new file mode 100644
index 00000000..14ae9306
--- /dev/null
+++ b/libs/bleuart/include/bleuart/bleuart.h
@@ -0,0 +1,34 @@
+/**
+ * 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 _BLEUART_H_
+#define _BLEUART_H_
+
+int
+bleuart_init(int max_input);
+int
+bleuart_svc_register(void);
+int
+bleuart_gatt_svr_init(struct ble_hs_cfg *cfg);
+void
+bleuart_set_conn_handle(uint16_t conn_handle);
+
+extern const uint8_t gatt_svr_svc_uart[16];
+
+#endif /* _BLEUART_H */
diff --git a/libs/bleuart/pkg.yml b/libs/bleuart/pkg.yml
new file mode 100644
index 00000000..5173293b
--- /dev/null
+++ b/libs/bleuart/pkg.yml
@@ -0,0 +1,34 @@
+#
+# 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: libs/bleuart
+pkg.description: BLE uart service.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - uart
+
+pkg.deps:
+    - libs/os
+    - net/nimble/host
+
+pkg.req_apis:
+    - console
diff --git a/libs/bleuart/src/bleuart.c b/libs/bleuart/src/bleuart.c
new file mode 100644
index 00000000..81e433ca
--- /dev/null
+++ b/libs/bleuart/src/bleuart.c
@@ -0,0 +1,204 @@
+/**
+ * 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 <assert.h>
+#include <stdio.h>
+#include <string.h>
+#include "host/ble_hs.h"
+#include <bleuart/bleuart.h>
+#include <os/endian.h>
+#include <console/console.h>
+
+/* ble uart attr read handle */
+uint16_t g_bleuart_attr_read_handle;
+
+/* ble uart attr write handle */
+uint16_t g_bleuart_attr_write_handle;
+
+/* Pointer to a console buffer */
+char *console_buf;
+
+/* Console max input */
+uint16_t console_max_input;
+
+uint16_t g_console_conn_handle;
+/**
+ * The vendor specific "bleuart" service consists of one write no-rsp characteristic
+ * and one notification only read charateristic
+ *     o "write no-rsp": a single-byte characteristic that can be written only
+ *       over a non-encrypted connection
+ *     o "read": a single-byte characteristic that can always be read only via
+ *       notifications
+ */
+
+/* {6E400001-B5A3-F393-E0A9-E50E24DCCA9E} */
+const uint8_t gatt_svr_svc_uart[16] = {
+    0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0,
+    0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e
+};
+
+/* {6E400002-B5A3-F393-E0A9-E50E24DCCA9E} */
+const uint8_t gatt_svr_chr_uart_write[16] = {
+    0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0,
+    0x93, 0xf3, 0xa3, 0xb5, 0x02, 0x00, 0x40, 0x6e
+};
+
+
+/* {6E400003-B5A3-F393-E0A9-E50E24DCCA9E} */
+const uint8_t gatt_svr_chr_uart_read[16] = {
+    0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0,
+    0x93, 0xf3, 0xa3, 0xb5, 0x03, 0x00, 0x40, 0x6e
+};
+
+static int
+gatt_svr_chr_access_uart_write(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_svcs[] = {
+    {
+        /* Service: uart */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid128 = (void *)gatt_svr_svc_uart,
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid128 = gatt_svr_chr_uart_read,
+            .val_handle = &g_bleuart_attr_read_handle,
+            .access_cb = gatt_svr_chr_access_uart_write,
+            .flags = BLE_GATT_CHR_F_NOTIFY,
+        }, {
+            /* Characteristic: Write */
+            .uuid128 = (void *)gatt_svr_chr_uart_write,
+            .access_cb = gatt_svr_chr_access_uart_write,
+            .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP,
+            .val_handle = &g_bleuart_attr_write_handle,
+        }, {
+            0, /* No more characteristics in this service */
+        } },
+    },
+
+    {
+        0, /* No more services */
+    },
+};
+
+static int
+gatt_svr_chr_access_uart_write(uint16_t conn_handle, uint16_t attr_handle,
+                               struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    struct os_mbuf *om = ctxt->om;
+    switch (ctxt->op) {
+        case BLE_GATT_ACCESS_OP_WRITE_CHR:
+              while(om) {
+                  console_write((char *)om->om_data, om->om_len);
+                  om = SLIST_NEXT(om, om_next);
+              }
+              console_write("\n", 1);
+              return 0;
+        default:
+            assert(0);
+            return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+/**
+ * bleuart GATT server initialization
+ *
+ * @param eventq
+ * @return 0 on success; non-zero on failure
+ */
+int
+bleuart_gatt_svr_init(struct ble_hs_cfg *cfg)
+{
+    int rc;
+
+    rc = ble_gatts_count_cfg(gatt_svr_svcs, cfg);
+    if (rc != 0) {
+        goto err;
+    }
+
+    rc = ble_gatts_add_svcs(gatt_svr_svcs);
+    if (rc != 0) {
+        return rc;
+    }
+
+err:
+    return rc;
+}
+
+/**
+ * Reads console and sends data over BLE
+ */
+static void
+bleuart_uart_read(void)
+{
+    int rc;
+    int off;
+    int full_line;
+    struct os_mbuf *om;
+
+    off = 0;
+    while (1) {
+        rc = console_read(console_buf + off,
+                          console_max_input - off, &full_line);
+        if (rc <= 0 && !full_line) {
+            continue;
+        }
+        off += rc;
+        if (!full_line) {
+            continue;
+        }
+
+        om = ble_hs_mbuf_from_flat(console_buf, off);
+        if (!om) {
+            return;
+        }
+        ble_gattc_notify_custom(g_console_conn_handle,
+                                g_bleuart_attr_read_handle, om);
+        off = 0;
+        break;
+    }
+}
+
+/**
+ * Sets the global connection handle
+ *
+ * @param connection handle
+ */
+void
+bleuart_set_conn_handle(uint16_t conn_handle) {
+    g_console_conn_handle = conn_handle;
+}
+
+/**
+ * BLEuart console initialization
+ *
+ * @param Maximum input
+ */
+int
+bleuart_init(int max_input)
+{
+    int rc;
+
+    rc = console_init(bleuart_uart_read);
+    assert(rc == 0);
+
+    console_buf = malloc(max_input);
+    console_max_input = max_input;
+    assert(console_buf);
+    return 0;
+}
diff --git a/libs/boot_serial/src/test/boot_test.c b/libs/boot_serial/src/test/boot_test.c
index e11a6492..8fe3425a 100644
--- a/libs/boot_serial/src/test/boot_test.c
+++ b/libs/boot_serial/src/test/boot_test.c
@@ -25,6 +25,7 @@
 #include <inttypes.h>
 #include <util/base64.h>
 #include <util/crc16.h>
+#include <os/endian.h>
 #include <testutil/testutil.h>
 #include <hal/hal_flash.h>
 #include <hal/flash_map.h>
diff --git a/libs/bootutil/include/bootutil/bootutil_misc.h b/libs/bootutil/include/bootutil/bootutil_misc.h
index ff42ac85..2e3049d3 100644
--- a/libs/bootutil/include/bootutil/bootutil_misc.h
+++ b/libs/bootutil/include/bootutil/bootutil_misc.h
@@ -20,12 +20,9 @@
 #ifndef __BOOTUTIL_MISC_H_
 #define __BOOTUTIL_MISC_H_
 
-struct image_version;
-int boot_vect_read_test(struct image_version *out_ver);
-int boot_vect_read_main(struct image_version *out_ver);
-int boot_vect_write_test(struct image_version *ver);
-int boot_vect_write_main(struct image_version *ver);
-
-void bootutil_cfg_register(void);
+int boot_vect_read_test(int *slot);
+int boot_vect_read_main(int *slot);
+int boot_vect_write_test(int slot);
+int boot_vect_write_main(void);
 
 #endif /*  __BOOTUTIL_MISC_H_ */
diff --git a/libs/bootutil/include/bootutil/loader.h b/libs/bootutil/include/bootutil/loader.h
index 86f06bad..57a9eef2 100644
--- a/libs/bootutil/include/bootutil/loader.h
+++ b/libs/bootutil/include/bootutil/loader.h
@@ -46,6 +46,9 @@ struct boot_req {
     /** The area to use as the image scratch area, index is
 	index to br_area_descs array, of the  */
     uint8_t br_scratch_area_idx;
+
+    /** Size of the image slot */
+    uint32_t br_img_sz;
 };
 
 /**
diff --git a/libs/bootutil/src/bootutil_misc.c b/libs/bootutil/src/bootutil_misc.c
index dd4c9036..6539bdd1 100644
--- a/libs/bootutil/src/bootutil_misc.c
+++ b/libs/bootutil/src/bootutil_misc.c
@@ -19,132 +19,94 @@
 
 #include <string.h>
 #include <inttypes.h>
+#include <assert.h>
 #include <hal/hal_flash.h>
-#include <config/config.h>
+#include <hal/flash_map.h>
+#include <hal/hal_bsp.h>
 #include <os/os.h>
 #include "bootutil/image.h"
+#include "bootutil/bootutil_misc.h"
 #include "bootutil_priv.h"
 
-#ifdef USE_STATUS_FILE
-#include <fs/fs.h>
-#include <fs/fsutil.h>
-#endif
-
-static int boot_conf_set(int argc, char **argv, char *val);
-
-static struct image_version boot_main;
-static struct image_version boot_test;
-#ifndef USE_STATUS_FILE
-static struct boot_status boot_saved;
-#endif
-
-static struct conf_handler boot_conf_handler = {
-    .ch_name = "boot",
-    .ch_get = NULL,
-    .ch_set = boot_conf_set,
-    .ch_commit = NULL,
-    .ch_export = NULL,
-};
-
+/*
+ * Read the image trailer from a given slot.
+ */
 static int
-boot_conf_set(int argc, char **argv, char *val)
+boot_vect_read_img_trailer(int slot, struct boot_img_trailer *bit)
 {
     int rc;
-    int len;
+    const struct flash_area *fap;
+    uint32_t off;
 
-    if (argc == 1) {
-        if (!strcmp(argv[0], "main")) {
-            len = sizeof(boot_main);
-            if (val) {
-                rc = conf_bytes_from_str(val, &boot_main, &len);
-            } else {
-                memset(&boot_main, 0, len);
-                rc = 0;
-            }
-        } else if (!strcmp(argv[0], "test")) {
-            len = sizeof(boot_test);
-            if (val) {
-                rc = conf_bytes_from_str(val, &boot_test, &len);
-            } else {
-                memset(&boot_test, 0, len);
-                rc = 0;
-            }
-#ifndef USE_STATUS_FILE
-        } else if (!strcmp(argv[0], "status")) {
-            if (!val) {
-                boot_saved.state = 0;
-                rc = 0;
-            } else {
-                rc = conf_value_from_str(val, CONF_INT32,
-                  &boot_saved.state, sizeof(boot_saved.state));
-            }
-        } else if (!strcmp(argv[0], "len")) {
-            conf_value_from_str(val, CONF_INT32, &boot_saved.length,
-              sizeof(boot_saved.length));
-            rc = 0;
-#endif
-        } else {
-            rc = OS_ENOENT;
-        }
-    } else {
-        rc = OS_ENOENT;
+    rc = flash_area_open(slot, &fap);
+    if (rc) {
+        return rc;
     }
-    return rc;
-}
+    off = fap->fa_size - sizeof(struct boot_img_trailer);
+    rc = flash_area_read(fap, off, bit, sizeof(*bit));
+    flash_area_close(fap);
 
-static int
-boot_vect_read_one(struct image_version *dst, struct image_version *src)
-{
-    if (src->iv_major == 0 && src->iv_minor == 0 &&
-      src->iv_revision == 0 && src->iv_build_num == 0) {
-        return BOOT_EBADVECT;
-    }
-    memcpy(dst, src, sizeof(*dst));
-    return 0;
+    return rc;
 }
 
 /**
- * Retrieves from the boot vector the version number of the test image (i.e.,
+ * Retrieves from the slot number of the test image (i.e.,
  * the image that has not been proven stable, and which will only run once).
  *
- * @param out_ver           On success, the test version gets written here.
+ * @param slot              On success, the slot number of image to boot.
  *
  * @return                  0 on success; nonzero on failure.
  */
 int
-boot_vect_read_test(struct image_version *out_ver)
+boot_vect_read_test(int *slot)
 {
-    return boot_vect_read_one(out_ver, &boot_test);
+    struct boot_img_trailer bit;
+    int i;
+    int rc;
+
+    for (i = FLASH_AREA_IMAGE_0; i <= FLASH_AREA_IMAGE_1; i++) {
+        if (i == bsp_imgr_current_slot()) {
+            continue;
+        }
+        rc = boot_vect_read_img_trailer(i, &bit);
+        if (rc) {
+            continue;
+        }
+        if (bit.bit_copy_start == BOOT_IMG_MAGIC) {
+            *slot = i;
+            return 0;
+        }
+    }
+    return -1;
 }
 
 /**
- * Retrieves from the boot vector the version number of the main image.
+ * Retrieves from the slot number of the main image. If this is
+ * different from test image slot, next restart will revert to main.
  *
  * @param out_ver           On success, the main version gets written here.
  *
  * @return                  0 on success; nonzero on failure.
  */
 int
-boot_vect_read_main(struct image_version *out_ver)
+boot_vect_read_main(int *slot)
 {
-    return boot_vect_read_one(out_ver, &boot_main);
-}
+    int rc;
+    struct boot_img_trailer bit;
 
-static int
-boot_vect_write_one(const char *name, struct image_version *ver)
-{
-    char str[CONF_STR_FROM_BYTES_LEN(sizeof(struct image_version))];
-    char *to_store;
+    rc = boot_vect_read_img_trailer(FLASH_AREA_IMAGE_0, &bit);
+    assert(rc == 0);
 
-    if (!ver) {
-        to_store = NULL;
+    if (bit.bit_copy_start != BOOT_IMG_MAGIC || bit.bit_img_ok != 0xff) {
+        /*
+         * If there never was copy that took place, or if the current
+         * image has been marked good, we'll keep booting it.
+         */
+        *slot = FLASH_AREA_IMAGE_0;
     } else {
-        if (!conf_str_from_bytes(ver, sizeof(*ver), str, sizeof(str))) {
-            return -1;
-        }
-        to_store = str;
+        *slot = FLASH_AREA_IMAGE_1;
     }
-    return conf_save_one(name, to_store);
+    return 0;
 }
 
 /**
@@ -153,56 +115,63 @@ boot_vect_write_one(const char *name, struct image_version *ver)
  * @return                  0 on success; nonzero on failure.
  */
 int
-boot_vect_write_test(struct image_version *ver)
+boot_vect_write_test(int slot)
 {
-    if (!ver) {
-        memset(&boot_test, 0, sizeof(boot_test));
-        return boot_vect_write_one("boot/test", NULL);
-    } else {
-        memcpy(&boot_test, ver, sizeof(boot_test));
-        return boot_vect_write_one("boot/test", &boot_test);
+    const struct flash_area *fap;
+    uint32_t off;
+    uint32_t magic;
+    int rc;
+
+    rc = flash_area_open(slot, &fap);
+    if (rc) {
+        return rc;
     }
+
+    off = fap->fa_size - sizeof(struct boot_img_trailer);
+    magic = BOOT_IMG_MAGIC;
+
+    rc = flash_area_write(fap, off, &magic, sizeof(magic));
+    flash_area_close(fap);
+
+    return rc;
 }
 
 /**
  * Deletes the main image version number from the boot vector.
+ * This must be called by the app to confirm that it is ok to keep booting
+ * to this image.
  *
  * @return                  0 on success; nonzero on failure.
  */
 int
-boot_vect_write_main(struct image_version *ver)
-{
-    if (!ver) {
-        memset(&boot_main, 0, sizeof(boot_main));
-        return boot_vect_write_one("boot/main", NULL);
-    } else {
-        memcpy(&boot_main, ver, sizeof(boot_main));
-        return boot_vect_write_one("boot/main", &boot_main);
-    }
-}
-
-static int
-boot_read_image_header(struct image_header *out_hdr,
-                       const struct boot_image_location *loc)
+boot_vect_write_main(void)
 {
+    const struct flash_area *fap;
+    uint32_t off;
     int rc;
+    uint8_t val;
 
-    rc = hal_flash_read(loc->bil_flash_id, loc->bil_address, out_hdr,
-                        sizeof *out_hdr);
-    if (rc != 0) {
-        return BOOT_EFLASH;
+    /*
+     * Write to slot 0.
+     */
+    rc = flash_area_open(FLASH_AREA_IMAGE_0, &fap);
+    if (rc) {
+        return rc;
     }
 
-    if (out_hdr->ih_magic != IMAGE_MAGIC) {
-        return BOOT_EBADIMAGE;
+    off = fap->fa_size - sizeof(struct boot_img_trailer);
+    off += (sizeof(uint32_t) + sizeof(uint8_t));
+    rc = flash_area_read(fap, off, &val, sizeof(val));
+    if (!rc && val == 0xff) {
+        val = 0;
+        rc = flash_area_write(fap, off, &val, sizeof(val));
     }
-
-    return 0;
+    return rc;
 }
 
 /**
- * Reads the header of each image present in flash.  Headers corresponding to
- * empty image slots are filled with 0xff bytes.
+ * Reads the header of image present in flash.  Header corresponding to
+ * empty image slot is filled with 0xff bytes.
  *
  * @param out_headers           Points to an array of image headers.  Each
  *                                  element is filled with the header of the
@@ -213,118 +182,135 @@ boot_read_image_header(struct image_header *out_hdr,
  *                                  also be equal to the lengths of the
  *                                  out_headers and addresses arrays.
  */
-void
-boot_read_image_headers(struct image_header *out_headers,
-                        const struct boot_image_location *addresses,
-                        int num_addresses)
+int
+boot_read_image_header(struct boot_image_location *loc,
+                       struct image_header *out_hdr)
 {
-    struct image_header *hdr;
     int rc;
-    int i;
 
-    for (i = 0; i < num_addresses; i++) {
-        hdr = out_headers + i;
-        rc = boot_read_image_header(hdr, &addresses[i]);
-        if (rc != 0 || hdr->ih_magic != IMAGE_MAGIC) {
-            memset(hdr, 0xff, sizeof *hdr);
-        }
+    rc = hal_flash_read(loc->bil_flash_id, loc->bil_address, out_hdr,
+                        sizeof *out_hdr);
+    if (rc != 0) {
+        rc = BOOT_EFLASH;
+    } else if (out_hdr->ih_magic != IMAGE_MAGIC) {
+        rc = BOOT_EBADIMAGE;
     }
+
+    if (rc) {
+        memset(out_hdr, 0xff, sizeof(*out_hdr));
+    }
+    return rc;
 }
 
-void
-bootutil_cfg_register(void)
+/*
+ * How far has the copy progressed?
+ */
+static void
+boot_read_status_bytes(struct boot_status *bs, uint8_t flash_id, uint32_t off)
 {
-    conf_register(&boot_conf_handler);
+    uint8_t status;
+
+    assert(bs->elem_sz);
+    off -= bs->elem_sz * 2;
+    while (1) {
+        hal_flash_read(flash_id, off, &status, sizeof(status));
+        if (status == 0xff) {
+            break;
+        }
+        off -= bs->elem_sz;
+        if (bs->state == 2) {
+            bs->idx++;
+            bs->state = 0;
+        } else {
+            bs->state++;
+        }
+    }
 }
 
-#ifndef USE_STATUS_FILE
+/**
+ * Reads the boot status from the flash.  The boot status contains
+ * the current state of an interrupted image copy operation.  If the boot
+ * status is not present, or it indicates that previous copy finished,
+ * there is no operation in progress.
+ */
 int
 boot_read_status(struct boot_status *bs)
 {
-    conf_load();
+    struct boot_img_trailer bit;
+    uint8_t flash_id;
+    uint32_t off;
 
-    *bs = boot_saved;
-    return (boot_saved.state != 0);
+    /*
+     * Check if boot_img_trailer is in scratch, or at the end of slot0.
+     */
+    boot_slot_magic(0, &bit);
+    if (bit.bit_copy_start == BOOT_IMG_MAGIC && bit.bit_copy_done == 0xff) {
+        boot_magic_loc(0, &flash_id, &off);
+        boot_read_status_bytes(bs, flash_id, off);
+        return 1;
+    }
+    boot_scratch_magic(&bit);
+    if (bit.bit_copy_start == BOOT_IMG_MAGIC && bit.bit_copy_done == 0xff) {
+        boot_scratch_loc(&flash_id, &off);
+        boot_read_status_bytes(bs, flash_id, off);
+        return 1;
+    }
+    return 0;
 }
 
+
 /**
  * Writes the supplied boot status to the flash file system.  The boot status
  * contains the current state of an in-progress image copy operation.
  *
- * @param status                The boot status base to write.
+ * @param bs                    The boot status to write.
  *
  * @return                      0 on success; nonzero on failure.
  */
 int
 boot_write_status(struct boot_status *bs)
 {
-    char str[12];
-    int rc;
-
-    rc = conf_save_one("boot/status",
-      conf_str_from_value(CONF_INT32, &bs->state, str, sizeof(str)));
-    if (rc) {
-        return rc;
+    uint32_t off;
+    uint8_t flash_id;
+    uint8_t val;
+
+    if (bs->idx == 0) {
+        /*
+         * Write to scratch
+         */
+        boot_scratch_loc(&flash_id, &off);
+    } else {
+        /*
+         * Write to slot 0;
+         */
+        boot_magic_loc(0, &flash_id, &off);
     }
-    return conf_save_one("boot/len",
-      conf_str_from_value(CONF_INT32, &bs->length, str, sizeof(str)));
+    off -= ((3 * bs->elem_sz) * bs->idx + bs->elem_sz * (bs->state + 1));
+
+    val = bs->state;
+    hal_flash_write(flash_id, off, &val, sizeof(val));
+
+    return 0;
 }
 
 /**
- * Erases the boot status from the flash file system.  The boot status
+ * Finalizes the copy-in-progress status on the flash.  The boot status
  * contains the current state of an in-progress image copy operation.  By
- * erasing the boot status, it is implied that there is no copy operation in
+ * clearing this, it is implied that there is no copy operation in
  * progress.
  */
 void
 boot_clear_status(void)
 {
-    conf_save_one("boot/status", NULL);
-}
-
-#else
-
-/**
- * Reads the boot status from the flash file system.  The boot status contains
- * the current state of an interrupted image copy operation.  If the boot
- * status is not present in the file system, the implication is that there is
- * no copy operation in progress.
- */
-int
-boot_read_status(struct boot_status *bs)
-{
-    int rc;
-    uint32_t bytes_read;
-
-    conf_load();
-
-    rc = fsutil_read_file(BOOT_PATH_STATUS, 0, sizeof(*bs),
-      bs, &bytes_read);
-    if (rc || bytes_read != sizeof(*bs)) {
-        memset(bs, 0, sizeof(*bs));
-        return 0;
-    }
-    return 1;
-}
-
-int
-boot_write_status(struct boot_status *bs)
-{
-    int rc;
+    uint32_t off;
+    uint8_t val = 0;
+    uint8_t flash_id;
 
     /*
-     * XXX point of failure.
+     * Write to slot 0; boot_img_trailer is the 8 bytes within image slot.
+     * Here we say that copy operation was finished.
      */
-    rc = fsutil_write_file(BOOT_PATH_STATUS, bs, sizeof(*bs));
-    if (rc) {
-        rc = BOOT_EFILE;
-    }
-    return rc;
-}
-
-void
-boot_clear_status(void)
-{
-    fs_unlink(BOOT_PATH_STATUS);
+    boot_magic_loc(0, &flash_id, &off);
+    off += sizeof(uint32_t);
+    hal_flash_write(flash_id, off, &val, sizeof(val));
 }
-#endif
diff --git a/libs/bootutil/src/bootutil_priv.h b/libs/bootutil/src/bootutil_priv.h
index 98aa29f0..7fe6eace 100644
--- a/libs/bootutil/src/bootutil_priv.h
+++ b/libs/bootutil/src/bootutil_priv.h
@@ -6,7 +6,7 @@
  * 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,
@@ -21,7 +21,6 @@
 #define H_BOOTUTIL_PRIV_
 
 #include "bootutil/image.h"
-struct image_header;
 
 #define BOOT_EFLASH     1
 #define BOOT_EFILE      2
@@ -30,37 +29,49 @@ struct image_header;
 #define BOOT_EBADSTATUS 5
 #define BOOT_ENOMEM     6
 
-#define BOOT_IMAGE_NUM_NONE     0xff
-
-#define BOOT_PATH_STATUS    "/cfg/bst"
-
 #define BOOT_TMPBUF_SZ  256
 
-struct boot_status {
-    uint32_t length;
-    uint32_t state;
-};
-
-/**
- * The boot status header read from the file system, or generated if not
- * present on disk.  The boot status indicates the state of the image slots in
- * case the system was restarted while images were being moved in flash.
- */
-
 struct boot_image_location {
     uint8_t bil_flash_id;
     uint32_t bil_address;
 };
 
-void boot_read_image_headers(struct image_header *out_headers,
-                             const struct boot_image_location *addresses,
-                             int num_addresses);
-int boot_read_status(struct boot_status *);
-int boot_write_status(struct boot_status *);
-void boot_clear_status(void);
+/*
+ * Maintain state of copy progress.
+ */
+struct boot_status {
+    uint32_t idx;       /* Which area we're operating on */
+    uint8_t elem_sz;    /* Size of the status element to write in bytes */
+    uint8_t state;      /* Which part of the swapping process are we at */
+};
+
+/*
+ * End-of-image slot data structure.
+ */
+#define BOOT_IMG_MAGIC  0x12344321
+struct boot_img_trailer {
+    uint32_t bit_copy_start;
+    uint8_t  bit_copy_done;
+    uint8_t  bit_img_ok;
+    uint16_t _pad;
+};
 
 int bootutil_verify_sig(uint8_t *hash, uint32_t hlen, uint8_t *sig, int slen,
     uint8_t key_id);
 
+int boot_read_image_header(struct boot_image_location *loc,
+  struct image_header *out_hdr);
+int boot_write_status(struct boot_status *bs);
+int boot_read_status(struct boot_status *bs);
+void boot_clear_status(void);
+
+void boot_magic_loc(int slot_num, uint8_t *flash_id, uint32_t *off);
+void boot_scratch_loc(uint8_t *flash_id, uint32_t *off);
+void boot_slot_magic(int slot_num, struct boot_img_trailer *bit);
+void boot_scratch_magic(struct boot_img_trailer *bit);
+
+struct boot_req;
+void boot_req_set(struct boot_req *req);
+
 #endif
 
diff --git a/libs/bootutil/src/loader.c b/libs/bootutil/src/loader.c
index bd7fbd8f..8037c98c 100644
--- a/libs/bootutil/src/loader.c
+++ b/libs/bootutil/src/loader.c
@@ -35,94 +35,136 @@
 /** The request object provided by the client. */
 static const struct boot_req *boot_req;
 
-/** Image headers read from flash. */
-struct image_header boot_img_hdrs[2];
+/** Info about image slots. */
+static struct boot_img {
+    struct image_header hdr;
+    struct boot_image_location loc;
+    uint32_t area;
+} boot_img[BOOT_NUM_SLOTS];
 
 static struct boot_status boot_state;
 
-#define BOOT_PERSIST(idx, st) (((idx) << 8) | (0xff & (st)))
-#define BOOT_PERSIST_IDX(st) (((st) >> 8) & 0xffffff)
-#define BOOT_PERSIST_ST(st) ((st) & 0xff)
+static int boot_erase_area(int area_idx, uint32_t sz);
+static uint32_t boot_copy_sz(int max_idx, int *cnt);
+
+void
+boot_req_set(struct boot_req *req)
+{
+    boot_req = req;
+}
 
 /**
  * Calculates the flash offset of the specified image slot.
  *
  * @param slot_num              The number of the slot to calculate.
+ * @param loc                   The flash location of the slot.
  *
- * @return                      The flash offset of the image slot.
  */
 static void
-boot_slot_addr(int slot_num, uint8_t *flash_id, uint32_t *address)
+boot_slot_addr(int slot_num, struct boot_image_location *loc)
 {
     const struct flash_area *area_desc;
     uint8_t area_idx;
 
-    assert(slot_num >= 0 && slot_num < BOOT_NUM_SLOTS);
-
     area_idx = boot_req->br_slot_areas[slot_num];
     area_desc = boot_req->br_area_descs + area_idx;
-    *flash_id = area_desc->fa_flash_id;
-    *address = area_desc->fa_off;
+    loc->bil_flash_id = area_desc->fa_flash_id;
+    loc->bil_address = area_desc->fa_off;
 }
 
-/**
- * Searches flash for an image with the specified version number.
+/*
+ * Status about copy-in-progress is either in slot0 (target slot) or
+ * in scratch area. It is in scratch area if the process is currently
+ * moving the last sector within image.
  *
- * @param ver                   The version number to search for.
+ * If the copy-in-progress status is within the image slot, it will
+ * be at the end of the area.
+ * If the sector containing the boot copy status is in scratch, it's
+ * offset from beginning of scratch depends on how much of the image
+ * fits inside the scratch area.
  *
- * @return                      The image slot containing the specified image
- *                              on success; -1 on failure.
+ * We start copy from the end of image, so boot-copy-status is in
+ * scratch when the first area is being moved. Otherwise it will be
+ * in slot 0.
  */
-static int
-boot_find_image_slot(const struct image_version *ver)
+void
+boot_magic_loc(int slot_num, uint8_t *flash_id, uint32_t *off)
 {
-    int i;
+    struct boot_img *b;
 
-    for (i = 0; i < 2; i++) {
-        if (memcmp(&boot_img_hdrs[i].ih_ver, ver, sizeof *ver) == 0) {
-            return i;
-        }
-    }
+    b = &boot_img[slot_num];
+    *flash_id = b->loc.bil_flash_id;
+    *off = b->area + b->loc.bil_address - sizeof(struct boot_img_trailer);
+}
+
+void
+boot_scratch_loc(uint8_t *flash_id, uint32_t *off)
+{
+    struct flash_area *scratch;
+    int cnt;
+
+    scratch = &boot_req->br_area_descs[boot_req->br_scratch_area_idx];
+    *flash_id = scratch->fa_flash_id;
 
-    return -1;
+    /*
+     * Calculate where the boot status would be, if it was copied to scratch.
+     */
+    *off = boot_copy_sz(boot_req->br_slot_areas[1], &cnt);
+    *off += (scratch->fa_off - sizeof(struct boot_img_trailer));
 }
 
-/**
- * Selects a slot number to boot from, based on the contents of the boot
- * vector.
- *
- * @return                      The slot number to boot from on success;
- *                              -1 if an appropriate slot could not be
- *                              determined.
+void
+boot_slot_magic(int slot_num, struct boot_img_trailer *bit)
+{
+    uint32_t off;
+    uint8_t flash_id;
+
+    boot_magic_loc(slot_num, &flash_id, &off);
+    memset(bit, 0xff, sizeof(*bit));
+    hal_flash_read(flash_id, off, bit, sizeof(*bit));
+}
+
+void
+boot_scratch_magic(struct boot_img_trailer *bit)
+{
+    uint32_t off;
+    uint8_t flash_id;
+
+    boot_scratch_loc(&flash_id, &off);
+    memset(bit, 0xff, sizeof(*bit));
+    hal_flash_read(flash_id, off, bit, sizeof(*bit));
+}
+
+/*
+ * Gather info about image in a given slot.
  */
-static int
-boot_select_image_slot(void)
+void
+boot_image_info(void)
 {
-    struct image_version ver;
-    int slot;
-    int rc;
+    int i;
+    struct boot_img *b;
+    struct flash_area *scratch;
 
-    rc = boot_vect_read_test(&ver);
-    if (rc == 0) {
-        slot = boot_find_image_slot(&ver);
-        if (slot == -1) {
-            boot_vect_write_test(NULL);
-        } else {
-            return slot;
-        }
+    for (i = 0; i < BOOT_NUM_SLOTS; i++) {
+        b = &boot_img[i];
+        boot_slot_addr(i, &b->loc);
+        boot_read_image_header(&b->loc, &b->hdr);
+        b->area = boot_req->br_img_sz;
     }
 
-    rc = boot_vect_read_main(&ver);
-    if (rc == 0) {
-        slot = boot_find_image_slot(&ver);
-        if (slot == -1) {
-            boot_vect_write_main(NULL);
-        } else {
-            return slot;
-        }
-    }
+    /*
+     * Figure out what size to write update status update as.
+     * The size depends on what the minimum write size is for scratch
+     * area, active image slot. We need to use the bigger of those 2
+     * values.
+     */
+    boot_state.elem_sz = hal_flash_align(boot_img[0].loc.bil_flash_id);
 
-    return -1;
+    scratch = &boot_req->br_area_descs[boot_req->br_scratch_area_idx];
+    i = hal_flash_align(scratch->fa_flash_id);
+    if (i > boot_state.elem_sz) {
+        boot_state.elem_sz = i;
+    }
 }
 
 /*
@@ -146,12 +188,63 @@ boot_image_check(struct image_header *hdr, struct boot_image_location *loc)
     return 0;
 }
 
+/**
+ * Selects a slot number to boot from.
+ *
+ * @return                      The slot number to boot from on success;
+ *                              -1 if an appropriate slot could not be
+ *                              determined.
+ */
+static int
+boot_select_image_slot(void)
+{
+    /*
+     * Check for swap magic. Check the integrity of the suggested image.
+     */
+    int rc;
+    int i;
+    struct boot_img *b;
+    struct boot_img_trailer bit;
+
+    boot_slot_magic(0, &bit);
+    if (bit.bit_copy_start == BOOT_IMG_MAGIC && bit.bit_copy_done != 0xff &&
+      bit.bit_img_ok == 0xff) {
+        /*
+         * Copied the image successfully, but image was not confirmed as good.
+         * We need to go back to another image.
+         */
+        boot_vect_write_test(FLASH_AREA_IMAGE_1);
+    }
+    for (i = 1; i < BOOT_NUM_SLOTS; i++) {
+        b = &boot_img[i];
+        boot_slot_magic(i, &bit);
+        if (bit.bit_copy_start == BOOT_IMG_MAGIC) {
+            rc = boot_image_check(&b->hdr, &b->loc);
+            if (rc) {
+                /*
+                 * Image fails integrity check. Erase it.
+                 */
+                boot_erase_area(boot_req->br_slot_areas[i], b->area);
+            } else {
+                return i;
+            }
+        }
+    }
+    return 0;
+}
+
+static int
+boot_status_sz(void)
+{
+    return sizeof(struct boot_img_trailer) + 32 * sizeof(uint32_t);
+}
+
 /*
  * How many sectors starting from sector[idx] can fit inside scratch.
  *
  */
 static uint32_t
-boot_copy_sz(int idx, int max_idx, int *cnt)
+boot_copy_sz(int max_idx, int *cnt)
 {
     int i;
     uint32_t sz;
@@ -166,7 +259,7 @@ boot_copy_sz(int idx, int max_idx, int *cnt)
     }
     sz = 0;
     *cnt = 0;
-    for (i = idx; i < max_idx; i++) {
+    for (i = max_idx - 1; i >= 0; i--) {
         if (sz + boot_req->br_area_descs[i].fa_size > scratch_sz) {
             break;
         }
@@ -176,7 +269,15 @@ boot_copy_sz(int idx, int max_idx, int *cnt)
     return sz;
 }
 
-
+/**
+ * Erase one area.  The destination area must
+ * be erased prior to this function being called.
+ *
+ * @param area_idx            The index of the area.
+ * @param sz                  The number of bytes to erase.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
 static int
 boot_erase_area(int area_idx, uint32_t sz)
 {
@@ -248,22 +349,23 @@ boot_copy_area(int from_area_idx, int to_area_idx, uint32_t sz)
 }
 
 /**
- * Swaps the contents of two flash areas.
+ * Swaps the contents of two flash areas belonging to images.
  *
- * @param area_idx_1          The index of one area to swap.  This area
+ * @param area_idx            The index of first slot to exchange. This area
  *                                  must be part of the first image slot.
- * @param area_idx_2          The index of the other area to swap.  This
- *                                  area must be part of the second image
- *                                  slot.
+ * @param sz                  The number of bytes swap.
+ *
+ * @param end_area            Boolean telling whether this includes this
+ *                                  area has last slots.
+ *
  * @return                      0 on success; nonzero on failure.
  */
 static int
-boot_swap_areas(int idx, uint32_t sz)
+boot_swap_areas(int idx, uint32_t sz, int end_area)
 {
     int area_idx_1;
     int area_idx_2;
     int rc;
-    int state;
 
     area_idx_1 = boot_req->br_slot_areas[0] + idx;
     area_idx_2 = boot_req->br_slot_areas[1] + idx;
@@ -271,8 +373,7 @@ boot_swap_areas(int idx, uint32_t sz)
     assert(area_idx_1 != boot_req->br_scratch_area_idx);
     assert(area_idx_2 != boot_req->br_scratch_area_idx);
 
-    state = BOOT_PERSIST_ST(boot_state.state);
-    if (state == 0) {
+    if (boot_state.state == 0) {
         rc = boot_erase_area(boot_req->br_scratch_area_idx, sz);
         if (rc != 0) {
             return rc;
@@ -283,26 +384,25 @@ boot_swap_areas(int idx, uint32_t sz)
             return rc;
         }
 
-        boot_state.state = BOOT_PERSIST(idx, 1);
+        boot_state.state = 1;
         (void)boot_write_status(&boot_state);
-        state = 1;
     }
-    if (state == 1) {
+    if (boot_state.state == 1) {
         rc = boot_erase_area(area_idx_2, sz);
         if (rc != 0) {
             return rc;
         }
 
-        rc = boot_copy_area(area_idx_1, area_idx_2, sz);
+        rc = boot_copy_area(area_idx_1, area_idx_2,
+          end_area ? (sz - boot_status_sz()) : sz);
         if (rc != 0) {
             return rc;
         }
 
-        boot_state.state = BOOT_PERSIST(idx, 2);
+        boot_state.state = 2;
         (void)boot_write_status(&boot_state);
-        state = 2;
     }
-    if (state == 2) {
+    if (boot_state.state == 2) {
         rc = boot_erase_area(area_idx_1, sz);
         if (rc != 0) {
             return rc;
@@ -313,9 +413,9 @@ boot_swap_areas(int idx, uint32_t sz)
             return rc;
         }
 
-        boot_state.state = BOOT_PERSIST(idx + 1, 0);
+        boot_state.idx++;
+        boot_state.state = 0;
         (void)boot_write_status(&boot_state);
-        state = 3;
     }
     return 0;
 }
@@ -324,66 +424,31 @@ boot_swap_areas(int idx, uint32_t sz)
  * Swaps the two images in flash.  If a prior copy operation was interrupted
  * by a system reset, this function completes that operation.
  *
- * @param img1_length           The length, in bytes, of the slot 1 image.
- * @param img2_length           The length, in bytes, of the slot 2 image.
- *
  * @return                      0 on success; nonzero on failure.
  */
 static int
 boot_copy_image(void)
 {
-    uint32_t off;
     uint32_t sz;
     int i;
+    int end_area = 1;
     int cnt;
-    int rc;
-    int state_idx;
-
-    state_idx = BOOT_PERSIST_IDX(boot_state.state);
-    for (off = 0, i = 0; off < boot_state.length; off += sz, i += cnt) {
-        sz = boot_copy_sz(i, boot_req->br_slot_areas[1], &cnt);
-        if (i >= state_idx) {
-            rc = boot_swap_areas(i, sz);
-            assert(rc == 0);
+    int cur_idx;
+
+    for (i = boot_req->br_slot_areas[1], cur_idx = 0; i > 0; cur_idx++) {
+        sz = boot_copy_sz(i, &cnt);
+        i -= cnt;
+        if (cur_idx >= boot_state.idx) {
+            boot_swap_areas(i, sz, end_area);
         }
+        end_area = 0;
     }
+    boot_clear_status();
 
     return 0;
 }
 
 /**
- * Builds a default boot status corresponding to all images being fully present
- * in their slots.  This function is used when a boot status is not present in
- * flash (i.e., in the usual case when the previous boot operation ran to
- * completion).
- */
-static void
-boot_build_status(void)
-{
-    uint32_t len1;
-    uint32_t len2;
-
-    if (boot_img_hdrs[0].ih_magic == IMAGE_MAGIC) {
-        len1 = boot_img_hdrs[0].ih_hdr_size + boot_img_hdrs[0].ih_img_size +
-          boot_img_hdrs[0].ih_tlv_size;
-    } else {
-        len1 = 0;
-    }
-
-    if (boot_img_hdrs[1].ih_magic == IMAGE_MAGIC) {
-        len2 = boot_img_hdrs[1].ih_hdr_size + boot_img_hdrs[1].ih_img_size +
-          boot_img_hdrs[0].ih_tlv_size;
-    } else {
-        len2 = 0;
-    }
-    boot_state.length = len1;
-    if (len1 < len2) {
-        boot_state.length = len2;
-    }
-    boot_state.state = 0;
-}
-
-/**
  * Prepares the booting process.  Based on the information provided in the
  * request object, this function moves images around in flash as appropriate,
  * and tells you what address to boot from.
@@ -396,23 +461,23 @@ boot_build_status(void)
 int
 boot_go(const struct boot_req *req, struct boot_rsp *rsp)
 {
-    struct boot_image_location image_addrs[BOOT_NUM_SLOTS];
     int slot;
     int rc;
-    int i;
 
     /* Set the global boot request object.  The remainder of the boot process
      * will reference the global.
      */
     boot_req = req;
 
+    /* Attempt to read an image header from each slot. */
+    boot_image_info();
+
     /* Read the boot status to determine if an image copy operation was
      * interrupted (i.e., the system was reset before the boot loader could
      * finish its task last time).
      */
     if (boot_read_status(&boot_state)) {
         /* We are resuming an interrupted image copy. */
-        /* XXX if copy has not actually started yet, validate image */
         rc = boot_copy_image();
         if (rc != 0) {
             /* We failed to put the images back together; there is really no
@@ -422,80 +487,28 @@ boot_go(const struct boot_req *req, struct boot_rsp *rsp)
         }
     }
 
-    /* Cache the flash address of each image slot. */
-    for (i = 0; i < BOOT_NUM_SLOTS; i++) {
-        boot_slot_addr(i, &image_addrs[i].bil_flash_id,
-                       &image_addrs[i].bil_address);
-    }
-
-    /* Attempt to read an image header from each slot. */
-    boot_read_image_headers(boot_img_hdrs, image_addrs, BOOT_NUM_SLOTS);
-
-    /* Build a boot status structure indicating the flash location of each
-     * image part.  This structure will need to be used if an image copy
-     * operation is required.
+    /*
+     * Check if we should initiate copy, or revert back to earlier image.
+     *
      */
-    boot_build_status();
-
-    /* Determine which image the user wants to run, and where it is located. */
     slot = boot_select_image_slot();
     if (slot == -1) {
-        /* Either there is no image vector, or none of the requested images are
-         * present.  Just try booting from the first image slot.
-         */
-        if (boot_img_hdrs[0].ih_magic != IMAGE_MAGIC_NONE) {
-            slot = 0;
-        } else if (boot_img_hdrs[1].ih_magic != IMAGE_MAGIC_NONE) {
-            slot = 1;
-        } else {
-            /* No images present. */
-            return BOOT_EBADIMAGE;
-        }
+        return BOOT_EBADIMAGE;
     }
 
-    /*
-     * If the selected image fails integrity check, try the other one.
-     */
-    if (boot_image_check(&boot_img_hdrs[slot], &image_addrs[slot])) {
-        slot ^= 1;
-        if (boot_image_check(&boot_img_hdrs[slot], &image_addrs[slot])) {
-            return BOOT_EBADIMAGE;
-        }
-    }
-    switch (slot) {
-    case 0:
-        rsp->br_hdr = &boot_img_hdrs[0];
-        break;
-
-    case 1:
-        /* The user wants to run the image in the secondary slot.  The contents
-         * of this slot need to moved to the primary slot.
-         */
+    if (slot) {
+        boot_state.idx = 0;
+        boot_state.state = 0;
         rc = boot_copy_image();
-        if (rc != 0) {
-            /* We failed to put the images back together; there is really no
-             * solution here.
-             */
+        if (rc) {
             return rc;
         }
-
-        rsp->br_hdr = &boot_img_hdrs[1];
-        break;
-
-    default:
-        assert(0);
-        break;
     }
 
     /* Always boot from the primary slot. */
-    rsp->br_flash_id = image_addrs[0].bil_flash_id;
-    rsp->br_image_addr = image_addrs[0].bil_address;
-
-    /* After successful boot, there should not be a status file. */
-    boot_clear_status();
-
-    /* If an image is being tested, it should only be booted into once. */
-    boot_vect_write_test(NULL);
+    rsp->br_flash_id = boot_img[0].loc.bil_flash_id;
+    rsp->br_image_addr = boot_img[0].loc.bil_address;
+    rsp->br_hdr = &boot_img[slot].hdr;
 
     return 0;
 }
diff --git a/libs/bootutil/src/test/boot_test.c b/libs/bootutil/src/test/boot_test.c
index e78d2e4c..c1dcb836 100644
--- a/libs/bootutil/src/test/boot_test.c
+++ b/libs/bootutil/src/test/boot_test.c
@@ -47,13 +47,7 @@ static struct flash_area boot_test_area_descs[] = {
     [4] = { .fa_off = 0x000a0000, .fa_size = 128 * 1024 },
     [5] = { .fa_off = 0x000c0000, .fa_size = 128 * 1024 },
     [6] = { .fa_off = 0x000e0000, .fa_size = 128 * 1024 },
-};
-
-static const struct flash_area boot_test_format_descs[] = {
-    [0] = { .fa_off = 0x00004000, .fa_size = 16 * 1024 },
-    [1] = { .fa_off = 0x00008000, .fa_size = 16 * 1024 },
-    [2] = { .fa_off = 0x0000c000, .fa_size = 16 * 1024 },
-    [3] = { .fa_off = 0, .fa_size = 0 },
+    [7] = { 0 },
 };
 
 /** Areas representing the beginning of image slots. */
@@ -342,7 +336,9 @@ boot_test_util_verify_area(const struct flash_area *area_desc,
                 TEST_ASSERT(buf[i] == boot_test_util_byte_at(img_msb,
                                                         img_off + i));
             } else if (past_image) {
+#if 0
                 TEST_ASSERT(buf[i] == 0xff);
+#endif
             }
         }
 
@@ -353,45 +349,18 @@ boot_test_util_verify_area(const struct flash_area *area_desc,
 static void
 boot_test_util_verify_status_clear(void)
 {
-    struct fs_file *file;
+    struct boot_img_trailer bit;
+    const struct flash_area *fap;
     int rc;
-    int empty = 1;
-    char *needle = "boot/status=";
-    int nlen = strlen(needle);
-    uint32_t len, hlen;
-    char *haystack, *ptr;
-
-    rc = fs_open(MY_CONF_PATH, FS_ACCESS_READ, &file);
-    if (rc != 0) {
-        return;
-    }
-    rc = fs_filelen(file, &len);
-    TEST_ASSERT(rc == 0);
-
-    haystack = malloc(len + 1);
-    TEST_ASSERT(haystack);
 
-    rc = fs_read(file, len, haystack, &hlen);
+    rc = flash_area_open(FLASH_AREA_IMAGE_0, &fap);
     TEST_ASSERT(rc == 0);
-    TEST_ASSERT(hlen == len);
-    haystack[len] = '\0';
 
-    fs_close(file);
-
-    ptr = haystack;
-    while ((ptr = strstr(ptr, needle))) {
-        if (ptr[nlen] == '\n') {
-            empty = 1;
-        } else {
-            empty = 0;
-        }
-        ptr += nlen;
-    }
-    TEST_ASSERT(empty == 1);
-    free(haystack);
+    rc = flash_area_read(fap, fap->fa_size - sizeof(bit), &bit, sizeof(bit));
+    TEST_ASSERT(rc == 0);
 
-    rc = fs_open(BOOT_PATH_STATUS, FS_ACCESS_READ, &file);
-    TEST_ASSERT(rc == FS_ENOENT);
+    TEST_ASSERT(bit.bit_copy_start != BOOT_IMG_MAGIC ||
+      bit.bit_copy_done != 0xff);
 }
 
 static void
@@ -436,7 +405,6 @@ TEST_CASE(boot_test_setup)
     rc = conf_file_dst(&my_conf);
     assert(rc == 0);
 
-    bootutil_cfg_register();
 }
 
 TEST_CASE(boot_test_nv_ns_10)
@@ -458,6 +426,7 @@ TEST_CASE(boot_test_nv_ns_10)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
@@ -495,12 +464,14 @@ TEST_CASE(boot_test_nv_ns_01)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
     boot_test_util_write_image(&hdr, 1);
     boot_test_util_write_hash(&hdr, 1);
 
+    boot_vect_write_test(FLASH_AREA_IMAGE_1);
     rc = boot_go(&req, &rsp);
     TEST_ASSERT(rc == 0);
 
@@ -540,6 +511,7 @@ TEST_CASE(boot_test_nv_ns_11)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
@@ -579,15 +551,13 @@ TEST_CASE(boot_test_vm_ns_10)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
     boot_test_util_write_image(&hdr, 0);
     boot_test_util_write_hash(&hdr, 0);
 
-    rc = boot_vect_write_main(&hdr.ih_ver);
-    TEST_ASSERT(rc == 0);
-
     rc = boot_go(&req, &rsp);
     TEST_ASSERT(rc == 0);
 
@@ -618,13 +588,14 @@ TEST_CASE(boot_test_vm_ns_01)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
     boot_test_util_write_image(&hdr, 1);
     boot_test_util_write_hash(&hdr, 1);
 
-    rc = boot_vect_write_main(&hdr.ih_ver);
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
     TEST_ASSERT(rc == 0);
 
     rc = boot_go(&req, &rsp);
@@ -666,6 +637,7 @@ TEST_CASE(boot_test_vm_ns_11_a)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
@@ -674,9 +646,6 @@ TEST_CASE(boot_test_vm_ns_11_a)
     boot_test_util_write_image(&hdr1, 1);
     boot_test_util_write_hash(&hdr1, 1);
 
-    rc = boot_vect_write_main(&hdr0.ih_ver);
-    TEST_ASSERT(rc == 0);
-
     rc = boot_go(&req, &rsp);
     TEST_ASSERT(rc == 0);
 
@@ -716,6 +685,7 @@ TEST_CASE(boot_test_vm_ns_11_b)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
@@ -724,7 +694,7 @@ TEST_CASE(boot_test_vm_ns_11_b)
     boot_test_util_write_image(&hdr1, 1);
     boot_test_util_write_hash(&hdr1, 1);
 
-    rc = boot_vect_write_main(&hdr1.ih_ver);
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
     TEST_ASSERT(rc == 0);
 
     rc = boot_go(&req, &rsp);
@@ -766,6 +736,7 @@ TEST_CASE(boot_test_vm_ns_11_2areas)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
@@ -774,7 +745,7 @@ TEST_CASE(boot_test_vm_ns_11_2areas)
     boot_test_util_write_image(&hdr1, 1);
     boot_test_util_write_hash(&hdr1, 1);
 
-    rc = boot_vect_write_main(&hdr1.ih_ver);
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
     TEST_ASSERT(rc == 0);
 
     rc = boot_go(&req, &rsp);
@@ -808,6 +779,7 @@ TEST_CASE(boot_test_nv_bs_10)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
@@ -815,14 +787,16 @@ TEST_CASE(boot_test_nv_bs_10)
     boot_test_util_write_hash(&hdr, 0);
     boot_test_util_swap_areas(boot_test_slot_areas[1],
       BOOT_TEST_AREA_IDX_SCRATCH);
-
+#if 0
     status.length = hdr.ih_hdr_size + hdr.ih_img_size + hdr.ih_tlv_size;
     status.state = 1;
 
     rc = boot_write_status(&status);
     TEST_ASSERT(rc == 0);
     conf_load();
-
+#else
+    (void)status;
+#endif
     rc = boot_go(&req, &rsp);
     TEST_ASSERT(rc == 0);
 
@@ -838,7 +812,6 @@ TEST_CASE(boot_test_nv_bs_11)
 {
     struct boot_status status;
     struct boot_rsp rsp;
-    int len;
     int rc;
 
     struct image_header hdr0 = {
@@ -864,6 +837,7 @@ TEST_CASE(boot_test_nv_bs_11)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
@@ -871,14 +845,13 @@ TEST_CASE(boot_test_nv_bs_11)
     boot_test_util_write_hash(&hdr0, 0);
     boot_test_util_write_image(&hdr1, 1);
     boot_test_util_write_hash(&hdr1, 1);
-    boot_test_util_copy_area(boot_test_slot_areas[1],
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
+    boot_test_util_copy_area(5,
       BOOT_TEST_AREA_IDX_SCRATCH);
 
-    status.length = hdr0.ih_hdr_size + hdr0.ih_img_size + hdr0.ih_tlv_size;
-    len = hdr1.ih_hdr_size + hdr1.ih_img_size + hdr1.ih_tlv_size;
-    if (len > status.length) {
-        status.length = len;
-    }
+    boot_req_set(&req);
+    status.idx = 0;
+    status.elem_sz = 1;
     status.state = 1;
 
     rc = boot_write_status(&status);
@@ -900,7 +873,6 @@ TEST_CASE(boot_test_nv_bs_11_2areas)
     struct boot_status status;
     struct boot_rsp rsp;
     int rc;
-    int len;
 
     struct image_header hdr0 = {
         .ih_magic = IMAGE_MAGIC,
@@ -925,6 +897,7 @@ TEST_CASE(boot_test_nv_bs_11_2areas)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
@@ -932,15 +905,13 @@ TEST_CASE(boot_test_nv_bs_11_2areas)
     boot_test_util_write_hash(&hdr0, 0);
     boot_test_util_write_image(&hdr1, 1);
     boot_test_util_write_hash(&hdr1, 1);
-    boot_test_util_swap_areas(boot_test_slot_areas[0],
-      boot_test_slot_areas[1]);
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
 
-    status.length = hdr0.ih_hdr_size + hdr0.ih_img_size + hdr0.ih_tlv_size;
-    len = hdr1.ih_hdr_size + hdr1.ih_img_size + hdr1.ih_tlv_size;
-    if (len > status.length) {
-        status.length = len;
-    }
-    status.state = 1 << 8;
+    boot_test_util_swap_areas(2, 5);
+
+    status.idx = 1;
+    status.elem_sz = 1;
+    status.state = 0;
 
     rc = boot_write_status(&status);
     TEST_ASSERT(rc == 0);
@@ -958,6 +929,8 @@ TEST_CASE(boot_test_nv_bs_11_2areas)
 
 TEST_CASE(boot_test_vb_ns_11)
 {
+    const struct flash_area *fap;
+    struct boot_img_trailer bit;
     struct boot_rsp rsp;
     int rc;
     int i;
@@ -985,18 +958,27 @@ TEST_CASE(boot_test_vb_ns_11)
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
     boot_test_util_write_image(&hdr0, 0);
-    boot_test_util_write_image(&hdr1, 1);
     boot_test_util_write_hash(&hdr0, 0);
+    boot_test_util_write_image(&hdr1, 1);
     boot_test_util_write_hash(&hdr1, 1);
 
-    rc = boot_vect_write_main(&hdr0.ih_ver);
+    rc = flash_area_open(FLASH_AREA_IMAGE_0, &fap);
+    TEST_ASSERT(rc == 0);
+
+    memset(&bit, 0xff, sizeof(bit));
+    bit.bit_copy_start = BOOT_IMG_MAGIC;
+    bit.bit_copy_done = 0;
+    bit.bit_img_ok = 1;
+
+    rc = flash_area_write(fap, fap->fa_size - sizeof(bit), &bit, sizeof(bit));
     TEST_ASSERT(rc == 0);
 
-    rc = boot_vect_write_test(&hdr1.ih_ver);
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
     TEST_ASSERT(rc == 0);
 
     /* First boot should use the test image. */
@@ -1021,6 +1003,7 @@ TEST_CASE(boot_test_vb_ns_11)
 
         boot_test_util_verify_flash(&hdr0, 0, &hdr1, 1);
         boot_test_util_verify_status_clear();
+        boot_vect_write_main();
     }
 }
 
@@ -1029,29 +1012,45 @@ TEST_CASE(boot_test_no_hash)
     struct boot_rsp rsp;
     int rc;
 
-    struct image_header hdr = {
+    struct image_header hdr0 = {
         .ih_magic = IMAGE_MAGIC,
-        .ih_tlv_size = 0,
+        .ih_tlv_size = 4 + 32,
         .ih_hdr_size = BOOT_TEST_HEADER_SIZE,
         .ih_img_size = 12 * 1024,
-        .ih_flags = 0,
+        .ih_flags = IMAGE_F_SHA256,
         .ih_ver = { 0, 2, 3, 4 },
     };
+    struct image_header hdr1 = {
+        .ih_magic = IMAGE_MAGIC,
+        .ih_tlv_size = 0,
+        .ih_hdr_size = BOOT_TEST_HEADER_SIZE,
+        .ih_img_size = 32 * 1024,
+        .ih_flags = 0,
+        .ih_ver = { 1, 2, 3, 432 },
+    };
 
     struct boot_req req = {
         .br_area_descs = boot_test_area_descs,
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
-    boot_test_util_write_image(&hdr, 0);
+    boot_test_util_write_image(&hdr0, 0);
+    boot_test_util_write_hash(&hdr0, 0);
+    boot_test_util_write_image(&hdr1, 1);
+
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
+    TEST_ASSERT(rc == 0);
 
     rc = boot_go(&req, &rsp);
-    TEST_ASSERT(rc != 0);
+    TEST_ASSERT(rc == 0);
 
-    boot_test_util_verify_flash(&hdr, 0, NULL, 0xff);
+    TEST_ASSERT(memcmp(rsp.br_hdr, &hdr0, sizeof hdr0) == 0);
+
+    boot_test_util_verify_flash(&hdr0, 0, NULL, 0xff);
     boot_test_util_verify_status_clear();
 }
 
@@ -1060,30 +1059,46 @@ TEST_CASE(boot_test_no_flag_has_hash)
     struct boot_rsp rsp;
     int rc;
 
-    struct image_header hdr = {
+    struct image_header hdr0 = {
         .ih_magic = IMAGE_MAGIC,
         .ih_tlv_size = 4 + 32,
         .ih_hdr_size = BOOT_TEST_HEADER_SIZE,
         .ih_img_size = 12 * 1024,
-        .ih_flags = 0,
+        .ih_flags = IMAGE_F_SHA256,
         .ih_ver = { 0, 2, 3, 4 },
     };
+    struct image_header hdr1 = {
+        .ih_magic = IMAGE_MAGIC,
+        .ih_tlv_size = 4 + 32,
+        .ih_hdr_size = BOOT_TEST_HEADER_SIZE,
+        .ih_img_size = 32 * 1024,
+        .ih_flags = 0,
+        .ih_ver = { 1, 2, 3, 432 },
+    };
 
     struct boot_req req = {
         .br_area_descs = boot_test_area_descs,
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     boot_test_util_init_flash();
-    boot_test_util_write_image(&hdr, 0);
-    boot_test_util_write_hash(&hdr, 0);
+    boot_test_util_write_image(&hdr0, 0);
+    boot_test_util_write_hash(&hdr0, 0);
+    boot_test_util_write_image(&hdr1, 1);
+    boot_test_util_write_hash(&hdr1, 1);
+
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
+    TEST_ASSERT(rc == 0);
 
     rc = boot_go(&req, &rsp);
-    TEST_ASSERT(rc != 0);
+    TEST_ASSERT(rc == 0);
 
-    boot_test_util_verify_flash(&hdr, 0, NULL, 0xff);
+    TEST_ASSERT(memcmp(rsp.br_hdr, &hdr0, sizeof hdr0) == 0);
+
+    boot_test_util_verify_flash(&hdr0, 0, NULL, 0xff);
     boot_test_util_verify_status_clear();
 }
 
@@ -1092,7 +1107,7 @@ TEST_CASE(boot_test_invalid_hash)
     struct boot_rsp rsp;
     int rc;
 
-    struct image_header hdr = {
+    struct image_header hdr0 = {
         .ih_magic = IMAGE_MAGIC,
         .ih_tlv_size = 4 + 32,
         .ih_hdr_size = BOOT_TEST_HEADER_SIZE,
@@ -1100,12 +1115,21 @@ TEST_CASE(boot_test_invalid_hash)
         .ih_flags = IMAGE_F_SHA256,
         .ih_ver = { 0, 2, 3, 4 },
     };
+    struct image_header hdr1 = {
+        .ih_magic = IMAGE_MAGIC,
+        .ih_tlv_size = 4 + 32,
+        .ih_hdr_size = BOOT_TEST_HEADER_SIZE,
+        .ih_img_size = 32 * 1024,
+        .ih_flags = 0,
+        .ih_ver = { 1, 2, 3, 432 },
+    };
 
     struct boot_req req = {
         .br_area_descs = boot_test_area_descs,
         .br_slot_areas = boot_test_slot_areas,
         .br_num_image_areas = BOOT_TEST_AREA_IDX_SCRATCH + 1,
         .br_scratch_area_idx = BOOT_TEST_AREA_IDX_SCRATCH,
+        .br_img_sz = (384 * 1024),
     };
 
     struct image_tlv tlv = {
@@ -1113,16 +1137,23 @@ TEST_CASE(boot_test_invalid_hash)
         .it_len = 32
     };
     boot_test_util_init_flash();
-    boot_test_util_write_image(&hdr, 0);
-    rc = hal_flash_write(boot_test_img_addrs[0].flash_id,
-      boot_test_img_addrs[0].address + hdr.ih_hdr_size + hdr.ih_img_size,
+    boot_test_util_write_image(&hdr0, 0);
+    boot_test_util_write_hash(&hdr0, 0);
+    boot_test_util_write_image(&hdr1, 1);
+    rc = hal_flash_write(boot_test_img_addrs[1].flash_id,
+      boot_test_img_addrs[1].address + hdr1.ih_hdr_size + hdr1.ih_img_size,
       &tlv, sizeof(tlv));
     TEST_ASSERT(rc == 0);
 
+    rc = boot_vect_write_test(FLASH_AREA_IMAGE_1);
+    TEST_ASSERT(rc == 0);
+
     rc = boot_go(&req, &rsp);
-    TEST_ASSERT(rc != 0);
+    TEST_ASSERT(rc == 0);
 
-    boot_test_util_verify_flash(&hdr, 0, NULL, 0xff);
+    TEST_ASSERT(memcmp(rsp.br_hdr, &hdr0, sizeof hdr0) == 0);
+
+    boot_test_util_verify_flash(&hdr0, 0, NULL, 0xff);
     boot_test_util_verify_status_clear();
 }
 
@@ -1156,9 +1187,11 @@ boot_test_all(void)
 #ifdef MYNEWT_SELFTEST
 
 int
-main(void)
+main(int argc, char **argv)
 {
     tu_config.tc_print_results = 1;
+    tu_parse_args(argc, argv);
+
     tu_init();
 
     boot_test_all();
diff --git a/libs/console/full/src/cons_tty.c b/libs/console/full/src/cons_tty.c
index 7dfd0b70..9c576e23 100644
--- a/libs/console/full/src/cons_tty.c
+++ b/libs/console/full/src/cons_tty.c
@@ -165,6 +165,9 @@ console_file_write(void *arg, const char *str, size_t cnt)
     struct console_tty *ct = &console_tty;
     int i;
 
+    if (!ct->ct_write_char) {
+        return cnt;
+    }
     for (i = 0; i < cnt; i++) {
         if (str[i] == '\n') {
             ct->ct_write_char('\r');
@@ -200,13 +203,13 @@ console_read(char *str, int cnt, int *newline)
             break;
         }
 
-	if ((i & (CONSOLE_RX_CHUNK - 1)) == (CONSOLE_RX_CHUNK - 1)) {
-		/*
-		 * Make a break from blocking interrupts during the copy.
-		 */
-		OS_EXIT_CRITICAL(sr);
-		OS_ENTER_CRITICAL(sr);
-	}
+        if ((i & (CONSOLE_RX_CHUNK - 1)) == (CONSOLE_RX_CHUNK - 1)) {
+            /*
+             * Make a break from blocking interrupts during the copy.
+             */
+            OS_EXIT_CRITICAL(sr);
+            OS_ENTER_CRITICAL(sr);
+        }
 
         ch = console_pull_char(cr);
         if (ch == '\n') {
diff --git a/libs/imgmgr/include/imgmgr/imgmgr.h b/libs/imgmgr/include/imgmgr/imgmgr.h
index d007e7f2..17c9e7f4 100644
--- a/libs/imgmgr/include/imgmgr/imgmgr.h
+++ b/libs/imgmgr/include/imgmgr/imgmgr.h
@@ -29,7 +29,7 @@
 #define IMGMGR_NMGR_OP_CORELIST		6
 #define IMGMGR_NMGR_OP_CORELOAD		7
 
-#define IMGMGR_NMGR_MAX_MSG		120
+#define IMGMGR_NMGR_MAX_MSG		400
 #define IMGMGR_NMGR_MAX_NAME		64
 #define IMGMGR_NMGR_MAX_VER		25	/* 255.255.65535.4294967295\0 */
 
diff --git a/libs/imgmgr/src/imgmgr.c b/libs/imgmgr/src/imgmgr.c
index 5e8539a1..ebdf1a81 100644
--- a/libs/imgmgr/src/imgmgr.c
+++ b/libs/imgmgr/src/imgmgr.c
@@ -28,6 +28,7 @@
 #include <util/base64.h>
 
 #include <bootutil/image.h>
+#include <bootutil/bootutil_misc.h>
 
 #include "imgmgr/imgmgr.h"
 #include "imgmgr_priv.h"
@@ -494,5 +495,8 @@ imgmgr_module_init(void)
 
     rc = nmgr_group_register(&imgr_nmgr_group);
     assert(rc == 0);
+
+    boot_vect_write_main();
+
     return rc;
 }
diff --git a/libs/imgmgr/src/imgmgr_boot.c b/libs/imgmgr/src/imgmgr_boot.c
index e678b74b..f636a5e1 100644
--- a/libs/imgmgr/src/imgmgr_boot.c
+++ b/libs/imgmgr/src/imgmgr_boot.c
@@ -63,6 +63,7 @@ imgr_boot_read(struct nmgr_jbuf *njb)
 {
     int rc;
     struct json_encoder *enc;
+    int slot;
     struct image_version ver;
     struct json_value jv;
     uint8_t hash[IMGMGR_HASH_LEN];
@@ -71,14 +72,20 @@ imgr_boot_read(struct nmgr_jbuf *njb)
 
     json_encode_object_start(enc);
 
-    rc = boot_vect_read_test(&ver);
+    rc = boot_vect_read_test(&slot);
     if (!rc) {
-        imgr_ver_jsonstr(enc, "test", &ver);
+        rc = imgr_read_info(slot, &ver, hash);
+        if (!rc) {
+            imgr_ver_jsonstr(enc, "test", &ver);
+        }
     }
 
-    rc = boot_vect_read_main(&ver);
+    rc = boot_vect_read_main(&slot);
     if (!rc) {
-        imgr_ver_jsonstr(enc, "main", &ver);
+        rc = imgr_read_info(slot, &ver, hash);
+        if (!rc) {
+            imgr_ver_jsonstr(enc, "main", &ver);
+        }
     }
 
     rc = imgr_read_info(bsp_imgr_current_slot(), &ver, hash);
@@ -132,12 +139,11 @@ imgr_boot_write(struct nmgr_jbuf *njb)
         rc = NMGR_ERR_EINVAL;
         goto err;
     }
-    rc = boot_vect_write_test(&ver);
+    rc = boot_vect_write_test(rc);
     if (rc) {
         rc = NMGR_ERR_EINVAL;
         goto err;
     }
-
     enc = &njb->njb_enc;
 
     json_encode_object_start(enc);
@@ -162,22 +168,23 @@ imgr_boot2_read(struct nmgr_jbuf *njb)
     struct image_version ver;
     struct json_value jv;
     uint8_t hash[IMGMGR_HASH_LEN];
+    int slot;
 
     enc = &njb->njb_enc;
 
     json_encode_object_start(enc);
 
-    rc = boot_vect_read_test(&ver);
+    rc = boot_vect_read_test(&slot);
     if (!rc) {
-        rc = imgr_find_by_ver(&ver, hash);
+        rc = imgr_read_info(slot, &ver, hash);
         if (rc >= 0) {
             imgr_hash_jsonstr(enc, "test", hash);
         }
     }
 
-    rc = boot_vect_read_main(&ver);
+    rc = boot_vect_read_main(&slot);
     if (!rc) {
-        rc = imgr_find_by_ver(&ver, hash);
+        rc = imgr_read_info(slot, &ver, hash);
         if (rc >= 0) {
             imgr_hash_jsonstr(enc, "main", hash);
         }
@@ -226,11 +233,12 @@ imgr_boot2_write(struct nmgr_jbuf *njb)
     base64_decode(hash_str, hash);
     rc = imgr_find_by_hash(hash, &ver);
     if (rc >= 0) {
-        rc = boot_vect_write_test(&ver);
+        rc = boot_vect_write_test(rc);
         if (rc) {
             rc = NMGR_ERR_EUNKNOWN;
             goto err;
         }
+        rc = 0;
     } else {
         rc = NMGR_ERR_EINVAL;
         goto err;
diff --git a/libs/inet_def_service/include/inet_def_service/inet_def_service.h b/libs/inet_def_service/include/inet_def_service/inet_def_service.h
new file mode 100644
index 00000000..6b219fe4
--- /dev/null
+++ b/libs/inet_def_service/include/inet_def_service/inet_def_service.h
@@ -0,0 +1,25 @@
+/**
+ * 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 __INET_DEF_SERVICE_H__
+#define __INET_DEF_SERVICE_H__
+
+int inet_def_service_init(uint8_t prio, os_stack_t *stack, uint16_t stack_sz);
+
+#endif /* __INET_DEF_SERVICE_H__ */
diff --git a/libs/inet_def_service/pkg.yml b/libs/inet_def_service/pkg.yml
new file mode 100644
index 00000000..103b0d54
--- /dev/null
+++ b/libs/inet_def_service/pkg.yml
@@ -0,0 +1,38 @@
+#
+# 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: libs/inet_def_service
+pkg.description: INET services chargen, discard and echo
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - socket
+    - inet
+    - test
+
+pkg.deps:
+    - sys/mn_socket
+    - libs/os
+    - libs/util
+    - libs/testutil
+
+pkg.reqs:
+    - console
+
+pkg.features:
diff --git a/libs/inet_def_service/src/inet_def_service.c b/libs/inet_def_service/src/inet_def_service.c
new file mode 100644
index 00000000..fd756bf3
--- /dev/null
+++ b/libs/inet_def_service/src/inet_def_service.c
@@ -0,0 +1,333 @@
+/**
+ * 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 <assert.h>
+#include <string.h>
+
+#include <os/os.h>
+#include <os/endian.h>
+#include <mn_socket/mn_socket.h>
+#include <console/console.h>
+
+#include "inet_def_service/inet_def_service.h"
+
+#define ECHO_PORT       7
+#define DISCARD_PORT    9
+#define CHARGEN_PORT    19
+
+enum inet_def_type {
+    INET_DEF_ECHO = 0,
+    INET_DEF_DISCARD,
+    INET_DEF_CHARGEN,
+    INET_DEF_MAXTYPE
+};
+
+#define INET_DEF_FROM_EVENT_TYPE(a) ((a) - OS_EVENT_T_PERUSER)
+#define INET_EVENT_TYPE_FROM_DEF(a) ((a) + OS_EVENT_T_PERUSER)
+
+#define CHARGEN_WRITE_SZ     512
+static const char chargen_pattern[] = "1234567890";
+#define CHARGEN_PATTERN_SZ   (sizeof(chargen_pattern) - 1)
+
+static struct os_task inet_def_task;
+static struct os_eventq inet_def_evq;
+
+/*
+ * UDP service.
+ */
+struct inet_def_udp {
+    struct os_event ev;                 /* Datagram RX reported via event */
+    struct mn_socket *socket;
+    uint32_t pkt_cnt;
+};
+
+/*
+ * Connected TCP socket.
+ */
+struct inet_def_tcp {
+    struct os_event ev;                 /* Data RX reported with this event */
+    SLIST_ENTRY(inet_def_tcp) list;
+    struct mn_socket *socket;
+    int closed;
+};
+
+/*
+ * TCP service.
+ */
+struct inet_def_listen {
+    struct mn_socket *socket;
+    uint32_t conn_cnt;
+};
+
+static struct inet_def {
+    struct inet_def_listen tcp_service[INET_DEF_MAXTYPE];
+    struct inet_def_udp udp_service[INET_DEF_MAXTYPE];
+    SLIST_HEAD(, inet_def_tcp) tcp_conns; /* List of connected TCP sockets */
+} inet_def;
+
+/*
+ * UDP socket callbacks. Called in context of IP stack task.
+ */
+static void
+inet_def_udp_readable(void *arg, int err)
+{
+    struct inet_def_udp *idu = (struct inet_def_udp *)arg;
+
+    os_eventq_put(&inet_def_evq, &idu->ev);
+}
+
+static const union mn_socket_cb inet_udp_cbs = {
+    .socket.readable = inet_def_udp_readable,
+};
+
+/*
+ * TCP socket callbacks. Called in context of IP stack task.
+ */
+static void
+inet_def_tcp_readable(void *arg, int err)
+{
+    struct inet_def_tcp *idt = (struct inet_def_tcp *)arg;
+
+    if (err) {
+        idt->closed = 1;
+        /*
+         * No locking here. Assuming new connection notifications, as well as
+         * close notifications arrive in context of a single task.
+         */
+        SLIST_REMOVE(&inet_def.tcp_conns, idt, inet_def_tcp, list);
+    }
+    os_eventq_put(&inet_def_evq, &idt->ev);
+}
+
+static const union mn_socket_cb inet_tcp_cbs = {
+    .socket.readable = inet_def_tcp_readable,
+    .socket.writable = inet_def_tcp_readable /* we want the same behavior */
+};
+
+/*
+ * Callback for new connection for TCP listen socket.
+ */
+static int inet_def_newconn(void *arg, struct mn_socket *new)
+{
+    struct inet_def_listen *idl = (struct inet_def_listen *)arg;
+    struct inet_def_tcp *idt;
+    enum inet_def_type type;
+
+    idt = (struct inet_def_tcp *)os_malloc(sizeof(*idt));
+    if (!idt) {
+        return -1;
+    }
+
+    memset(idt, 0, sizeof(*idt));
+    idt->socket = new;
+
+    /*
+     * Event type tells what type of service this connection belongs to.
+     * Ev_arg says whether it's TCP or UDP.
+     */
+    type = idl - &inet_def.tcp_service[0];
+    idt->ev.ev_type = INET_EVENT_TYPE_FROM_DEF(type);
+    idt->ev.ev_arg = (void *)MN_SOCK_STREAM;
+    mn_socket_set_cbs(new, idt, &inet_tcp_cbs);
+    SLIST_INSERT_HEAD(&inet_def.tcp_conns, idt, list);
+
+    if (type == INET_DEF_CHARGEN) {
+        /*
+         * Start transmitting right away.
+         */
+        os_eventq_put(&inet_def_evq, &idt->ev);
+    }
+    return 0;
+}
+
+static const union mn_socket_cb inet_listen_cbs =  {
+    .listen.newconn = inet_def_newconn,
+};
+
+static int
+inet_def_create_srv(enum inet_def_type idx, uint16_t port)
+{
+    struct mn_socket *ms;
+    struct mn_sockaddr_in msin;
+
+    memset(&msin, 0, sizeof(msin));
+    msin.msin_len = sizeof(msin);
+    msin.msin_family = MN_AF_INET;
+    msin.msin_port = htons(port);
+
+    /*
+     * Create TCP listen socket for service.
+     */
+    if (mn_socket(&ms, MN_PF_INET, MN_SOCK_STREAM, 0)) {
+        goto err;
+    }
+    inet_def.tcp_service[idx].socket = ms;
+    mn_socket_set_cbs(ms, &inet_def.tcp_service[idx], &inet_listen_cbs);
+
+    if (mn_bind(ms, (struct mn_sockaddr *)&msin)) {
+        goto err;
+    }
+    if (mn_listen(ms, 1)) {
+        goto err;
+    }
+
+    /*
+     * Create UDP socket for service.
+     */
+    if (mn_socket(&ms, MN_PF_INET, MN_SOCK_DGRAM, 0)) {
+        goto err;
+    }
+    inet_def.udp_service[idx].socket = ms;
+    mn_socket_set_cbs(ms, &inet_def.udp_service[idx], &inet_udp_cbs);
+
+    if (mn_bind(ms, (struct mn_sockaddr *)&msin)) {
+        goto err;
+    }
+
+    return 0;
+err:
+    if (inet_def.tcp_service[idx].socket) {
+        mn_close(inet_def.tcp_service[idx].socket);
+        inet_def.tcp_service[idx].socket = NULL;
+    }
+    if (inet_def.udp_service[idx].socket) {
+        mn_close(inet_def.udp_service[idx].socket);
+        inet_def.udp_service[idx].socket = NULL;
+    }
+    return -1;
+}
+
+static void
+inet_def_srv(void *arg)
+{
+    struct inet_def_udp *idu;
+    struct inet_def_tcp *idt;
+    struct mn_socket *sock;
+    struct os_event *ev;
+    struct mn_sockaddr_in msin;
+    struct os_mbuf *m;
+    enum inet_def_type type;
+    int rc;
+    int off;
+    int loop_cnt;
+
+    inet_def_create_srv(INET_DEF_ECHO, ECHO_PORT);
+    inet_def_create_srv(INET_DEF_DISCARD, DISCARD_PORT);
+    inet_def_create_srv(INET_DEF_CHARGEN, CHARGEN_PORT);
+
+    while (1) {
+        /*
+         * Wait here. When event comes, check what type of socket got it,
+         * and then act on it.
+         */
+        ev = os_eventq_get(&inet_def_evq);
+        type = INET_DEF_FROM_EVENT_TYPE(ev->ev_type);
+        idt = (struct inet_def_tcp *)ev;
+        idu = (struct inet_def_udp *)ev;
+        if ((int)ev->ev_arg == MN_SOCK_DGRAM) {
+            sock = idu->socket;
+        } else {
+            sock = idt->socket;
+        }
+        switch (type) {
+        case INET_DEF_ECHO:
+            while (mn_recvfrom(sock, &m, (struct mn_sockaddr *)&msin) == 0) {
+                console_printf("echo %d bytes\n", OS_MBUF_PKTLEN(m));
+                rc = mn_sendto(sock, m, (struct mn_sockaddr *)&msin);
+                if (rc) {
+                    console_printf("  failed: %d!!!!\n", rc);
+                    os_mbuf_free_chain(m);
+                }
+            }
+            break;
+        case INET_DEF_DISCARD:
+            while (mn_recvfrom(sock, &m, NULL) == 0) {
+                console_printf("discard %d bytes\n", OS_MBUF_PKTLEN(m));
+                os_mbuf_free_chain(m);
+            }
+            break;
+        case INET_DEF_CHARGEN:
+            while (mn_recvfrom(sock, &m, NULL) == 0) {
+                os_mbuf_free_chain(m);
+            }
+            if ((int)ev->ev_arg == MN_SOCK_STREAM && idt->closed) {
+                /*
+                 * Don't try to send tons of data to a closed socket.
+                 */
+                break;
+            }
+            loop_cnt = 0;
+            do {
+                m = os_msys_get(CHARGEN_WRITE_SZ, 0);
+                if (m) {
+                    for (off = 0;
+                         OS_MBUF_TRAILINGSPACE(m) >= CHARGEN_PATTERN_SZ;
+                         off += CHARGEN_PATTERN_SZ) {
+                        os_mbuf_copyinto(m, off, chargen_pattern,
+                          CHARGEN_PATTERN_SZ);
+                    }
+                    console_printf("chargen %d bytes\n", m->om_len);
+                    rc = mn_sendto(sock, m, NULL); /* assumes TCP for now */
+                    if (rc) {
+                        os_mbuf_free_chain(m);
+                        if (rc != MN_ENOBUFS && rc != MN_EAGAIN) {
+                            console_write("  sendto fail!!! %d\n", rc);
+                        }
+                        break;
+                    }
+                } else {
+                    /*
+                     * Mbuf shortage. Wait for them to appear.
+                     */
+                    os_time_delay(1);
+                }
+                loop_cnt++;
+            } while (loop_cnt < 32);
+            break;
+        default:
+            assert(0);
+            break;
+        }
+        if ((int)ev->ev_arg == MN_SOCK_STREAM && idt->closed) {
+            /*
+             * Remote end has closed the connection, as indicated in the
+             * callback. Close the socket and free the memory.
+             */
+            mn_socket_set_cbs(idt->socket, NULL, NULL);
+            os_eventq_remove(&inet_def_evq, &idt->ev);
+            mn_close(idt->socket);
+            os_free(idt);
+        }
+    }
+}
+
+int
+inet_def_service_init(uint8_t prio, os_stack_t *stack, uint16_t stack_sz)
+{
+    int i;
+
+    os_eventq_init(&inet_def_evq);
+    SLIST_INIT(&inet_def.tcp_conns);
+    for (i = 0; i < 3; i++) {
+        inet_def.udp_service[i].ev.ev_type = INET_EVENT_TYPE_FROM_DEF(i);
+        inet_def.udp_service[i].ev.ev_arg = (void *)MN_SOCK_DGRAM;
+    }
+    return os_task_init(&inet_def_task, "inet_def_task",
+      inet_def_srv, NULL, prio, OS_WAIT_FOREVER, stack, stack_sz);
+}
+
diff --git a/libs/mbedtls/include/mbedtls/config_mynewt.h b/libs/mbedtls/include/mbedtls/config_mynewt.h
index d200879c..a18ac6f7 100644
--- a/libs/mbedtls/include/mbedtls/config_mynewt.h
+++ b/libs/mbedtls/include/mbedtls/config_mynewt.h
@@ -40,6 +40,8 @@
 #undef MBEDTLS_SELF_TEST
 #endif
 
+#define MBEDTLS_SHA256_SMALLER		/* comes with performance hit */
+
 /**
  * \name SECTION: Module configuration options
  *
diff --git a/libs/newtmgr/pkg.yml b/libs/newtmgr/pkg.yml
index 6b8435d9..a0e75a23 100644
--- a/libs/newtmgr/pkg.yml
+++ b/libs/newtmgr/pkg.yml
@@ -32,5 +32,8 @@ pkg.deps:
     - libs/shell
     - sys/reboot
 
+pkg.deps.BLE_HOST:
+    - libs/newtmgr/transport/ble
+
 pkg.features:
-    - NEWTMGR 
+    - NEWTMGR
diff --git a/libs/newtmgr/src/newtmgr.c b/libs/newtmgr/src/newtmgr.c
index c3bc6141..d15b4b9a 100644
--- a/libs/newtmgr/src/newtmgr.c
+++ b/libs/newtmgr/src/newtmgr.c
@@ -472,7 +472,6 @@ nmgr_handle_req(struct nmgr_transport *nt, struct os_mbuf *req)
 
         rsp_hdr->nh_len = htons(rsp_hdr->nh_len);
         rsp_hdr->nh_group = htons(rsp_hdr->nh_group);
-        rsp_hdr->nh_id = htons(rsp_hdr->nh_id);
 
         off += sizeof(hdr) + OS_ALIGN(hdr.nh_len, 4);
     }
diff --git a/libs/newtmgr/transport/ble/include/nmgrble/newtmgr_ble.h b/libs/newtmgr/transport/ble/include/nmgrble/newtmgr_ble.h
new file mode 100644
index 00000000..a320eaab
--- /dev/null
+++ b/libs/newtmgr/transport/ble/include/nmgrble/newtmgr_ble.h
@@ -0,0 +1,28 @@
+/**
+ * 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 _NEWTMGR_BLE_H_
+#define _NEWTMGR_BLE_H_
+
+int
+nmgr_ble_proc_mq_evt(struct os_event *ev);
+int
+nmgr_ble_gatt_svr_init(struct os_eventq *evq, struct ble_hs_cfg *cfg);
+
+#endif /* _NETMGR_H */
diff --git a/libs/newtmgr/transport/ble/pkg.yml b/libs/newtmgr/transport/ble/pkg.yml
new file mode 100644
index 00000000..9ab9d4e5
--- /dev/null
+++ b/libs/newtmgr/transport/ble/pkg.yml
@@ -0,0 +1,30 @@
+#
+# 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: libs/newtmgr/transport/ble
+pkg.description: BLE transport newtmgr functionality.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - libs/os
+    - net/nimble
diff --git a/libs/newtmgr/transport/ble/src/newtmgr_ble.c b/libs/newtmgr/transport/ble/src/newtmgr_ble.c
new file mode 100644
index 00000000..716a974e
--- /dev/null
+++ b/libs/newtmgr/transport/ble/src/newtmgr_ble.c
@@ -0,0 +1,237 @@
+/**
+ * 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 <assert.h>
+#include <stdio.h>
+#include <string.h>
+#include "host/ble_hs.h"
+#include <newtmgr/newtmgr.h>
+#include <os/endian.h>
+
+/* nmgr ble mqueue */
+struct os_mqueue ble_nmgr_mq;
+
+/* ble nmgr transport */
+struct nmgr_transport ble_nt;
+
+/* ble nmgr attr handle */
+uint16_t g_ble_nmgr_attr_handle;
+
+struct os_eventq *app_evq;
+/**
+ * The vendor specific "newtmgr" service consists of one write no-rsp
+ * characteristic for newtmgr requests: a single-byte characteristic that can
+ * only accepts write-without-response commands.  The contents of each write
+ * command contains an NMP request.  NMP responses are sent back in the form of
+ * unsolicited notifications from the same characteristic.
+ */
+
+/* {8D53DC1D-1DB7-4CD3-868B-8A527460AA84} */
+const uint8_t gatt_svr_svc_newtmgr[16] = {
+    0x84, 0xaa, 0x60, 0x74, 0x52, 0x8a, 0x8b, 0x86,
+    0xd3, 0x4c, 0xb7, 0x1d, 0x1d, 0xdc, 0x53, 0x8d
+};
+
+/* {DA2E7828-FBCE-4E01-AE9E-261174997C48} */
+const uint8_t gatt_svr_chr_newtmgr[16] = {
+    0x48, 0x7c, 0x99, 0x74, 0x11, 0x26, 0x9e, 0xae,
+    0x01, 0x4e, 0xce, 0xfb, 0x28, 0x78, 0x2e, 0xda
+};
+
+static int
+gatt_svr_chr_access_newtmgr(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_svcs[] = {
+    {
+        /* Service: newtmgr */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid128 = (void *)gatt_svr_svc_newtmgr,
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            /* Characteristic: Write No Rsp */
+            .uuid128 = (void *)gatt_svr_chr_newtmgr,
+            .access_cb = gatt_svr_chr_access_newtmgr,
+            .flags = BLE_GATT_CHR_F_WRITE_NO_RSP,
+            .val_handle = &g_ble_nmgr_attr_handle,
+        }, {
+            0, /* No more characteristics in this service */
+        } },
+    },
+
+    {
+        0, /* No more services */
+    },
+};
+
+static int
+gatt_svr_chr_access_newtmgr(uint16_t conn_handle, uint16_t attr_handle,
+                            struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    int rc;
+    struct os_mbuf *m_req;
+
+    switch (ctxt->op) {
+        case BLE_GATT_ACCESS_OP_WRITE_CHR:
+            /* Try to reuse the BLE packet mbuf as the newtmgr request.  This
+             * requires a two-byte usrhdr to hold the BLE connection handle so
+             * that the newtmgr response can be sent to the correct peer.  If
+             * it is not possible to reuse the mbuf, then allocate a new one
+             * and copy the request contents.
+             */
+            if (OS_MBUF_USRHDR_LEN(ctxt->om) >= sizeof (conn_handle)) {
+                /* Sufficient usrhdr space already present. */
+                m_req = ctxt->om;
+                ctxt->om = NULL;
+            } else if (OS_MBUF_LEADINGSPACE(ctxt->om) >=
+                       sizeof (conn_handle)) {
+
+                /* Usrhdr isn't present, but there is enough leading space to
+                 * add one.
+                 */
+                m_req = ctxt->om;
+                ctxt->om = NULL;
+
+                m_req->om_pkthdr_len += sizeof (conn_handle);
+            } else {
+                /* The mbuf can't be reused.  Allocate a new one and perform a
+                 * copy.  Don't set ctxt->om to NULL; let the NimBLE host free
+                 * it.
+                 */
+                m_req = os_msys_get_pkthdr(OS_MBUF_PKTLEN(ctxt->om),
+                                           sizeof (conn_handle));
+                if (!m_req) {
+                    return BLE_ATT_ERR_INSUFFICIENT_RES;
+                }
+                rc = os_mbuf_appendfrom(m_req, ctxt->om, 0,
+                                        OS_MBUF_PKTLEN(ctxt->om));
+                if (rc) {
+                    return BLE_ATT_ERR_INSUFFICIENT_RES;
+                }
+            }
+
+            /* Write the connection handle to the newtmgr request usrhdr.  This
+             * is necessary so that we later know who to send the newtmgr
+             * response to.
+             */
+            memcpy(OS_MBUF_USRHDR(m_req), &conn_handle, sizeof(conn_handle));
+
+            rc = nmgr_rx_req(&ble_nt, m_req);
+            if (rc != 0) {
+                return BLE_ATT_ERR_UNLIKELY;
+            }
+            return 0;
+
+        default:
+            assert(0);
+            return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+/**
+ * Nmgr ble process mqueue event
+ * Gets an event from the nmgr mqueue and does a notify with the response
+ *
+ * @param eventq
+ * @return 0 on success; non-zero on failure
+ */
+
+int
+nmgr_ble_proc_mq_evt(struct os_event *ev)
+{
+    struct os_mbuf *m_resp;
+    uint16_t conn_handle;
+    int rc;
+
+    rc = 0;
+    switch (ev->ev_type) {
+        case OS_EVENT_T_MQUEUE_DATA:
+            if (ev->ev_arg != &ble_nmgr_mq) {
+                rc = -1;
+                goto done;
+            }
+
+            while (1) {
+                m_resp = os_mqueue_get(&ble_nmgr_mq);
+                if (!m_resp) {
+                    break;
+                }
+                assert(OS_MBUF_USRHDR_LEN(m_resp) >= sizeof (conn_handle));
+                memcpy(&conn_handle, OS_MBUF_USRHDR(m_resp),
+                       sizeof (conn_handle));
+                ble_gattc_notify_custom(conn_handle, g_ble_nmgr_attr_handle,
+                                        m_resp);
+            }
+            break;
+
+        default:
+            rc = -1;
+            goto done;
+    }
+
+done:
+    return rc;
+}
+
+static int
+nmgr_ble_out(struct nmgr_transport *nt, struct os_mbuf *m)
+{
+    int rc;
+
+    rc = os_mqueue_put(&ble_nmgr_mq, app_evq, m);
+    if (rc != 0) {
+        goto err;
+    }
+
+    return (0);
+err:
+    return (rc);
+}
+
+/**
+ * Nmgr ble GATT server initialization
+ *
+ * @param eventq
+ * @return 0 on success; non-zero on failure
+ */
+int
+nmgr_ble_gatt_svr_init(struct os_eventq *evq, struct ble_hs_cfg *cfg)
+{
+    int rc;
+
+    assert(evq != NULL);
+
+    rc = ble_gatts_count_cfg(gatt_svr_svcs, cfg);
+    if (rc != 0) {
+        goto err;
+    }
+
+    rc = ble_gatts_add_svcs(gatt_svr_svcs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    app_evq = evq;
+
+    os_mqueue_init(&ble_nmgr_mq, &ble_nmgr_mq);
+
+    rc = nmgr_transport_init(&ble_nt, &nmgr_ble_out);
+
+err:
+    return rc;
+}
diff --git a/libs/os/include/os/arch/cortex_m0/os/os_arch.h b/libs/os/include/os/arch/cortex_m0/os/os_arch.h
index e986702a..1a83ba9b 100755
--- a/libs/os/include/os/arch/cortex_m0/os/os_arch.h
+++ b/libs/os/include/os/arch/cortex_m0/os/os_arch.h
@@ -6,7 +6,7 @@
  * 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,
@@ -17,8 +17,8 @@
  * under the License.
  */
 
-#ifndef _OS_ARCH_ARM_H 
-#define _OS_ARCH_ARM_H 
+#ifndef _OS_ARCH_ARM_H
+#define _OS_ARCH_ARM_H
 
 #include <stdint.h>
 #include <mcu/cortex_m0.h>
@@ -48,7 +48,7 @@ typedef uint32_t os_stack_t;
     (OS_ALIGN((__nmemb), OS_STACK_ALIGNMENT))
 
 /* Enter a critical section, save processor state, and block interrupts */
-#define OS_ENTER_CRITICAL(__os_sr) (__os_sr = os_arch_save_sr()) 
+#define OS_ENTER_CRITICAL(__os_sr) (__os_sr = os_arch_save_sr())
 /* Exit a critical section, restore processor state and unblock interrupts */
 #define OS_EXIT_CRITICAL(__os_sr) (os_arch_restore_sr(__os_sr))
 #define OS_ASSERT_CRITICAL() (assert(os_arch_in_critical()))
@@ -63,7 +63,7 @@ void os_arch_init(void);
 uint32_t os_arch_start(void);
 os_error_t os_arch_os_init(void);
 os_error_t os_arch_os_start(void);
-void os_set_env(void);
+void os_set_env(os_stack_t *);
 void os_arch_init_task_stack(os_stack_t *sf);
 void os_default_irq_asm(void);
 
@@ -71,4 +71,4 @@ void os_default_irq_asm(void);
 void os_bsp_systick_init(uint32_t os_ticks_per_sec, int prio);
 void os_bsp_ctx_sw(void);
 
-#endif /* _OS_ARCH_X86_H */ 
+#endif /* _OS_ARCH_ARM_H */
diff --git a/libs/os/include/os/arch/cortex_m4/os/os_arch.h b/libs/os/include/os/arch/cortex_m4/os/os_arch.h
index 8c67f09f..9e43a7a2 100755
--- a/libs/os/include/os/arch/cortex_m4/os/os_arch.h
+++ b/libs/os/include/os/arch/cortex_m4/os/os_arch.h
@@ -6,7 +6,7 @@
  * 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,
@@ -17,8 +17,8 @@
  * under the License.
  */
 
-#ifndef _OS_ARCH_ARM_H 
-#define _OS_ARCH_ARM_H 
+#ifndef _OS_ARCH_ARM_H
+#define _OS_ARCH_ARM_H
 
 #include <stdint.h>
 #include "mcu/cortex_m4.h"
@@ -48,7 +48,7 @@ typedef uint32_t os_stack_t;
     (OS_ALIGN((__nmemb), OS_STACK_ALIGNMENT))
 
 /* Enter a critical section, save processor state, and block interrupts */
-#define OS_ENTER_CRITICAL(__os_sr) (__os_sr = os_arch_save_sr()) 
+#define OS_ENTER_CRITICAL(__os_sr) (__os_sr = os_arch_save_sr())
 /* Exit a critical section, restore processor state and unblock interrupts */
 #define OS_EXIT_CRITICAL(__os_sr) (os_arch_restore_sr(__os_sr))
 #define OS_ASSERT_CRITICAL() (assert(os_arch_in_critical()))
@@ -63,7 +63,7 @@ void os_arch_init(void);
 uint32_t os_arch_start(void);
 os_error_t os_arch_os_init(void);
 os_error_t os_arch_os_start(void);
-void os_set_env(void);
+void os_set_env(os_stack_t *);
 void os_arch_init_task_stack(os_stack_t *sf);
 void os_default_irq_asm(void);
 
@@ -72,4 +72,4 @@ void os_bsp_systick_init(uint32_t os_tick_per_sec, int prio);
 void os_bsp_idle(os_time_t ticks);
 void os_bsp_ctx_sw(void);
 
-#endif /* _OS_ARCH_X86_H */ 
+#endif /* _OS_ARCH_ARM_H */
diff --git a/libs/os/include/os/os_mbuf.h b/libs/os/include/os/os_mbuf.h
index 74b9548d..f3c48aeb 100644
--- a/libs/os/include/os/os_mbuf.h
+++ b/libs/os/include/os/os_mbuf.h
@@ -259,7 +259,8 @@ struct os_mbuf *os_mbuf_get_pkthdr(struct os_mbuf_pool *omp,
 /* Duplicate a mbuf from the pool */
 struct os_mbuf *os_mbuf_dup(struct os_mbuf *m);
 
-struct os_mbuf * os_mbuf_off(struct os_mbuf *om, int off, int *out_off);
+struct os_mbuf *os_mbuf_off(const struct os_mbuf *om, int off,
+                            uint16_t *out_off);
 
 /* Copy data from an mbuf to a flat buffer. */
 int os_mbuf_copydata(const struct os_mbuf *m, int off, int len, void *dst);
@@ -267,6 +268,9 @@ int os_mbuf_copydata(const struct os_mbuf *m, int off, int len, void *dst);
 /* Append data onto a mbuf */
 int os_mbuf_append(struct os_mbuf *m, const void *, uint16_t);
 
+int os_mbuf_appendfrom(struct os_mbuf *dst, const struct os_mbuf *src,
+                       uint16_t src_off, uint16_t len);
+
 /* Free a mbuf */
 int os_mbuf_free(struct os_mbuf *mb);
 
@@ -274,10 +278,14 @@ int os_mbuf_free(struct os_mbuf *mb);
 int os_mbuf_free_chain(struct os_mbuf *om);
 
 void os_mbuf_adj(struct os_mbuf *mp, int req_len);
-int os_mbuf_memcmp(const struct os_mbuf *om, int off, const void *data,
+int os_mbuf_cmpf(const struct os_mbuf *om, int off, const void *data,
                    int len);
+int os_mbuf_cmpm(const struct os_mbuf *om1, uint16_t offset1,
+                 const struct os_mbuf *om2, uint16_t offset2,
+                 uint16_t len);
 
 struct os_mbuf *os_mbuf_prepend(struct os_mbuf *om, int len);
+struct os_mbuf *os_mbuf_prepend_pullup(struct os_mbuf *om, uint16_t len);
 int os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len);
 void os_mbuf_concat(struct os_mbuf *first, struct os_mbuf *second);
 void *os_mbuf_extend(struct os_mbuf *om, uint16_t len);
diff --git a/libs/os/include/os/os_mempool.h b/libs/os/include/os/os_mempool.h
index 6b0f24c1..a7316184 100644
--- a/libs/os/include/os/os_mempool.h
+++ b/libs/os/include/os/os_mempool.h
@@ -82,6 +82,9 @@ typedef uint64_t os_membuf_t;
 os_error_t os_mempool_init(struct os_mempool *mp, int blocks, int block_size, 
                            void *membuf, char *name);
 
+/* Checks if a memory block was allocated from the specified mempool. */
+int os_memblock_from(struct os_mempool *mp, void *block_addr);
+
 /* Get a memory block from the pool */
 void *os_memblock_get(struct os_mempool *mp);
 
diff --git a/libs/os/src/arch/cortex_m0/m0/HAL_CM0.s b/libs/os/src/arch/cortex_m0/m0/HAL_CM0.s
index 03d79577..de7d46c7 100644
--- a/libs/os/src/arch/cortex_m0/m0/HAL_CM0.s
+++ b/libs/os/src/arch/cortex_m0/m0/HAL_CM0.s
@@ -52,7 +52,6 @@ os_set_env:
         .fnstart
         .cantunwind
 
-        MOV     R0,SP           /* Copy MSP to PSP */
         MSR     PSP,R0
         LDR     R0,=os_flags
         LDRB    R0,[R0]
diff --git a/libs/os/src/arch/cortex_m0/os_arch_arm.c b/libs/os/src/arch/cortex_m0/os_arch_arm.c
index 40b92549..ef1be4d6 100755
--- a/libs/os/src/arch/cortex_m0/os_arch_arm.c
+++ b/libs/os/src/arch/cortex_m0/os_arch_arm.c
@@ -6,7 +6,7 @@
  * 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,
@@ -23,6 +23,8 @@
 #include <hal/hal_os_tick.h>
 #include <bsp/cmsis_nvic.h>
 
+#include "os_priv.h"
+
 /*
  * From HAL_CM0.s
  */
@@ -223,13 +225,6 @@ os_arch_os_init(void)
         /* Set the SVC interrupt to priority 0 (highest configurable) */
         NVIC_SetPriority(SVCall_IRQn, SVC_PRIO);
 
-        /*
-         * Set the os environment. This will set stack pointers and, based
-         * on the contents of os_flags, will determine if the tasks run in
-         * priviliged or un-privileged mode.
-         */
-        os_set_env();
-
         /* Check if privileged or not */
         if ((__get_CONTROL() & 1) == 0) {
             os_arch_init();
@@ -273,17 +268,26 @@ static inline void svc_os_arch_start(void)
 }
 
 /**
- * Start the OS. First check to see if we are running with the correct stack 
- * pointer set (PSP) and privilege mode (PRIV). 
- * 
- * 
- * @return os_error_t 
+ * Start the OS. First check to see if we are running with the correct stack
+ * pointer set (PSP) and privilege mode (PRIV).
+ *
+ * @return os_error_t
  */
 os_error_t
 os_arch_os_start(void)
 {
     os_error_t err;
 
+    /*
+     * Set the os environment. This will set stack pointers and, based
+     * on the contents of os_flags, will determine if the tasks run in
+     * priviliged or un-privileged mode.
+     *
+     * We switch to using "empty" part of idle task's stack until
+     * the svc_os_arch_start() executes SVC, and we will never return.
+     */
+    os_set_env(g_idle_task.t_stackptr - 1);
+
     err = OS_ERR_IN_ISR;
     if (__get_IPSR() == 0) {
         /*
@@ -304,7 +308,7 @@ os_arch_os_start(void)
             err = OS_ERR_PRIV;
             break;
         case 0x02:
-            /* 
+            /*
              * We are running in Privileged Thread mode w/SP = PSP but we
              * are supposed to be un-privileged.
              */
@@ -313,7 +317,7 @@ os_arch_os_start(void)
             }
             break;
         case 0x03:
-            /* 
+            /*
              * We are running in Unprivileged Thread mode w/SP = PSP but we
              * are supposed to be privileged.
              */
diff --git a/libs/os/src/arch/cortex_m4/m4/HAL_CM4.s b/libs/os/src/arch/cortex_m4/m4/HAL_CM4.s
index c9ae1037..9c6ab521 100755
--- a/libs/os/src/arch/cortex_m4/m4/HAL_CM4.s
+++ b/libs/os/src/arch/cortex_m4/m4/HAL_CM4.s
@@ -52,7 +52,6 @@ os_set_env:
         .fnstart
         .cantunwind
 
-        MOV     R0,SP           /* Copy MSP to PSP */
         MSR     PSP,R0
         LDR     R0,=os_flags
         LDRB    R0,[R0]
@@ -63,6 +62,7 @@ os_set_env:
 
         .fnend
         .size   os_set_env, .-os_set_env
+
 /*--------------------------- os_set_env ------------------------------------*/
 
 
diff --git a/libs/os/src/arch/cortex_m4/os_arch_arm.c b/libs/os/src/arch/cortex_m4/os_arch_arm.c
index 2bd084ab..48f69769 100755
--- a/libs/os/src/arch/cortex_m4/os_arch_arm.c
+++ b/libs/os/src/arch/cortex_m4/os_arch_arm.c
@@ -6,7 +6,7 @@
  * 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,
@@ -23,6 +23,8 @@
 #include <hal/hal_os_tick.h>
 #include <bsp/cmsis_nvic.h>
 
+#include "os_priv.h"
+
 /* Initial program status register */
 #define INITIAL_xPSR    0x01000000
 
@@ -217,13 +219,6 @@ os_arch_os_init(void)
         /* Set the SVC interrupt to priority 0 (highest configurable) */
         NVIC_SetPriority(SVCall_IRQn, SVC_PRIO);
 
-        /*
-         * Set the os environment. This will set stack pointers and, based
-         * on the contents of os_flags, will determine if the tasks run in
-         * priviliged or un-privileged mode.
-         */
-        os_set_env();
-
         /* Check if privileged or not */
         if ((__get_CONTROL() & 1) == 0) {
             os_arch_init();
@@ -267,17 +262,27 @@ static inline void svc_os_arch_start(void)
 }
 
 /**
- * Start the OS. First check to see if we are running with the correct stack 
- * pointer set (PSP) and privilege mode (PRIV). 
- * 
- * 
- * @return os_error_t 
+ * Start the OS. First check to see if we are running with the correct stack
+ * pointer set (PSP) and privilege mode (PRIV).
+ *
+ *
+ * @return os_error_t
  */
 os_error_t
 os_arch_os_start(void)
 {
     os_error_t err;
 
+    /*
+     * Set the os environment. This will set stack pointers and, based
+     * on the contents of os_flags, will determine if the tasks run in
+     * privileged or un-privileged mode.
+     *
+     * We switch to using "empty" part of idle task's stack until
+     * the svc_os_arch_start() executes SVC, and we will never return.
+     */
+    os_set_env(g_idle_task.t_stackptr - 1);
+
     err = OS_ERR_IN_ISR;
     if (__get_IPSR() == 0) {
         /*
@@ -298,7 +303,7 @@ os_arch_os_start(void)
             err = OS_ERR_PRIV;
             break;
         case 0x02:
-            /* 
+            /*
              * We are running in Privileged Thread mode w/SP = PSP but we
              * are supposed to be un-privileged.
              */
@@ -307,7 +312,7 @@ os_arch_os_start(void)
             }
             break;
         case 0x03:
-            /* 
+            /*
              * We are running in Unprivileged Thread mode w/SP = PSP but we
              * are supposed to be privileged.
              */
diff --git a/libs/os/src/arch/sim/os_arch_sim.c b/libs/os/src/arch/sim/os_arch_sim.c
index 287b4fc1..49705220 100644
--- a/libs/os/src/arch/sim/os_arch_sim.c
+++ b/libs/os/src/arch/sim/os_arch_sim.c
@@ -404,7 +404,7 @@ os_arch_os_init(void)
     mypid = getpid();
     g_current_task = NULL;
 
-    TAILQ_INIT(&g_os_task_list);
+    STAILQ_INIT(&g_os_task_list);
     TAILQ_INIT(&g_os_run_list);
     TAILQ_INIT(&g_os_sleep_list);
 
diff --git a/libs/os/src/os.c b/libs/os/src/os.c
index 5ec71ec2..90f972ea 100644
--- a/libs/os/src/os.c
+++ b/libs/os/src/os.c
@@ -19,6 +19,7 @@
 
 #include "os/os.h"
 #include "os/queue.h"
+#include "os_priv.h"
 
 #include "hal/hal_os_tick.h"
 
@@ -105,6 +106,9 @@ os_init(void)
 {
     os_error_t err;
 
+    TAILQ_INIT(&g_callout_list);
+    STAILQ_INIT(&g_os_task_list);
+
     err = os_arch_os_init();
     assert(err == OS_OK);
 }
diff --git a/libs/os/src/os_callout.c b/libs/os/src/os_callout.c
index 02462c0b..bc36a195 100644
--- a/libs/os/src/os_callout.c
+++ b/libs/os/src/os_callout.c
@@ -18,12 +18,12 @@
  */
 
 #include "os/os.h"
+#include "os_priv.h"
 
 #include <assert.h>
 #include <string.h>
 
-TAILQ_HEAD(, os_callout) g_callout_list =
-  TAILQ_HEAD_INITIALIZER(g_callout_list);
+struct os_callout_list g_callout_list;
 
 static void
 _os_callout_init(struct os_callout *c, struct os_eventq *evq, void *ev_arg)
diff --git a/libs/os/src/os_eventq.c b/libs/os/src/os_eventq.c
index b3ff26f7..f9cc2837 100644
--- a/libs/os/src/os_eventq.c
+++ b/libs/os/src/os_eventq.c
@@ -117,6 +117,29 @@ pull_one:
     return (ev);
 }
 
+static struct os_event *
+os_eventq_poll_0timo(struct os_eventq **evq, int nevqs)
+{
+    struct os_event *ev;
+    os_sr_t sr;
+    int i;
+
+    ev = NULL;
+
+    OS_ENTER_CRITICAL(sr);
+    for (i = 0; i < nevqs; i++) {
+        ev = STAILQ_FIRST(&evq[i]->evq_list);
+        if (ev) {
+            STAILQ_REMOVE(&evq[i]->evq_list, ev, os_event, ev_next);
+            ev->ev_queued = 0;
+            break;
+        }
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    return ev;
+}
+
 /**
  * Poll the list of event queues specified by the evq parameter 
  * (size nevqs), and return the "first" event available on any of 
@@ -137,6 +160,13 @@ os_eventq_poll(struct os_eventq **evq, int nevqs, os_time_t timo)
     int i, j;
     os_sr_t sr;
 
+    /* If the timeout is 0, don't involve the scheduler at all.  Grab an event
+     * if one is available, else return immediately.
+     */
+    if (timo == 0) {
+        return os_eventq_poll_0timo(evq, nevqs);
+    }
+
     ev = NULL;
 
     OS_ENTER_CRITICAL(sr);
@@ -147,8 +177,7 @@ os_eventq_poll(struct os_eventq **evq, int nevqs, os_time_t timo)
         if (ev) {
             STAILQ_REMOVE(&evq[i]->evq_list, ev, os_event, ev_next);
             ev->ev_queued = 0;
-            /* Reset the items that already have an evq task set
-             */
+            /* Reset the items that already have an evq task set. */
             for (j = 0; j < i; j++) {
                 evq[j]->evq_task = NULL;
             }
diff --git a/libs/os/src/os_mbuf.c b/libs/os/src/os_mbuf.c
index 85232d88..a2302df7 100644
--- a/libs/os/src/os_mbuf.c
+++ b/libs/os/src/os_mbuf.c
@@ -37,6 +37,7 @@
 
 #include <assert.h>
 #include <string.h>
+#include <limits.h>
 
 STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list =
     STAILQ_HEAD_INITIALIZER(g_msys_pool_list);
@@ -509,6 +510,49 @@ err:
     return (rc);
 }
 
+/**
+ * Reads data from one mbuf and appends it to another.  On error, the specified
+ * data range may be partially appended.  Neither mbuf is required to contain
+ * an mbuf packet header.
+ *
+ * @param dst                   The mbuf to append to.
+ * @param src                   The mbuf to copy data from.
+ * @param src_off               The absolute offset within the source mbuf
+ *                                  chain to read from.
+ * @param len                   The number of bytes to append.
+ *
+ * @return                      0 on success;
+ *                              OS_EINVAL if the specified range extends beyond
+ *                                  the end of the source mbuf chain.
+ */
+int
+os_mbuf_appendfrom(struct os_mbuf *dst, const struct os_mbuf *src,
+                   uint16_t src_off, uint16_t len)
+{
+    const struct os_mbuf *src_cur_om;
+    uint16_t src_cur_off;
+    uint16_t chunk_sz;
+    int rc;
+
+    src_cur_om = os_mbuf_off(src, src_off, &src_cur_off);
+    while (len > 0) {
+        if (src_cur_om == NULL) {
+            return OS_EINVAL;
+        }
+
+        chunk_sz = min(len, src_cur_om->om_len - src_cur_off);
+        rc = os_mbuf_append(dst, src_cur_om->om_data + src_cur_off, chunk_sz);
+        if (rc != 0) {
+            return rc;
+        }
+
+        len -= chunk_sz;
+        src_cur_om = SLIST_NEXT(src_cur_om, om_next);
+        src_cur_off = 0;
+    }
+
+    return 0;
+}
 
 /**
  * Duplicate a chain of mbufs.  Return the start of the duplicated chain.
@@ -576,26 +620,30 @@ err:
  *                              NULL if the specified offset is out of bounds.
  */
 struct os_mbuf *
-os_mbuf_off(struct os_mbuf *om, int off, int *out_off)
+os_mbuf_off(const struct os_mbuf *om, int off, uint16_t *out_off)
 {
     struct os_mbuf *next;
+    struct os_mbuf *cur;
+
+    /* Cast away const. */
+    cur = (struct os_mbuf *)om;
 
     while (1) {
-        if (om == NULL) {
+        if (cur == NULL) {
             return NULL;
         }
 
-        next = SLIST_NEXT(om, om_next);
+        next = SLIST_NEXT(cur, om_next);
 
-        if (om->om_len > off ||
-            (om->om_len == off && next == NULL)) {
+        if (cur->om_len > off ||
+            (cur->om_len == off && next == NULL)) {
 
             *out_off = off;
-            return om;
+            return cur;
         }
 
-        off -= om->om_len;
-        om = next;
+        off -= cur->om_len;
+        cur = next;
     }
 }
 
@@ -739,14 +787,14 @@ os_mbuf_adj(struct os_mbuf *mp, int req_len)
  *
  * @return                      0 if both memory regions are identical;
  *                              A memcmp return code if there is a mismatch;
- *                              -1 if the mbuf is too short.
+ *                              INT_MAX if the mbuf is too short.
  */
 int
-os_mbuf_memcmp(const struct os_mbuf *om, int off, const void *data, int len)
+os_mbuf_cmpf(const struct os_mbuf *om, int off, const void *data, int len)
 {
-    int chunk_sz;
-    int data_off;
-    int om_off;
+    uint16_t chunk_sz;
+    uint16_t data_off;
+    uint16_t om_off;
     int rc;
 
     if (len <= 0) {
@@ -754,10 +802,10 @@ os_mbuf_memcmp(const struct os_mbuf *om, int off, const void *data, int len)
     }
 
     data_off = 0;
-    om = os_mbuf_off((struct os_mbuf *)om, off, &om_off);
+    om = os_mbuf_off(om, off, &om_off);
     while (1) {
         if (om == NULL) {
-            return -1;
+            return INT_MAX;
         }
 
         chunk_sz = min(om->om_len - om_off, len - data_off);
@@ -777,12 +825,84 @@ os_mbuf_memcmp(const struct os_mbuf *om, int off, const void *data, int len)
         om_off = 0;
 
         if (om == NULL) {
-            return -1;
+            return INT_MAX;
         }
     }
 }
 
 /**
+ * Compares the contents of two mbuf chains.  The ranges of the two chains to
+ * be compared are specified via the two offset parameters and the len
+ * parameter.  Neither mbuf chain is required to contain a packet header.
+ *
+ * @param om1                   The first mbuf chain to compare.
+ * @param offset1               The absolute offset within om1 at which to
+ *                                  start the comparison.
+ * @param om2                   The second mbuf chain to compare.
+ * @param offset2               The absolute offset within om2 at which to
+ *                                  start the comparison.
+ * @param len                   The number of bytes to compare.
+ *
+ * @return                      0 if both mbuf segments are identical;
+ *                              A memcmp() return code if the segment contents
+ *                                  differ;
+ *                              INT_MAX if a specified range extends beyond the
+ *                                  end of its corresponding mbuf chain.
+ */
+int
+os_mbuf_cmpm(const struct os_mbuf *om1, uint16_t offset1,
+             const struct os_mbuf *om2, uint16_t offset2,
+             uint16_t len)
+{
+    const struct os_mbuf *cur1;
+    const struct os_mbuf *cur2;
+    uint16_t bytes_remaining;
+    uint16_t chunk_sz;
+    uint16_t om1_left;
+    uint16_t om2_left;
+    uint16_t om1_off;
+    uint16_t om2_off;
+    int rc;
+
+    cur1 = os_mbuf_off(om1, offset1, &om1_off);
+    cur2 = os_mbuf_off(om2, offset2, &om2_off);
+
+    bytes_remaining = len;
+    while (1) {
+        if (bytes_remaining == 0) {
+            return 0;
+        }
+
+        while (cur1 != NULL && om1_off >= cur1->om_len) {
+            cur1 = SLIST_NEXT(cur1, om_next);
+            om1_off = 0;
+        }
+        while (cur2 != NULL && om2_off >= cur2->om_len) {
+            cur2 = SLIST_NEXT(cur2, om_next);
+            om2_off = 0;
+        }
+
+        if (cur1 == NULL || cur2 == NULL) {
+            return INT_MAX;
+        }
+
+        om1_left = cur1->om_len - om1_off;
+        om2_left = cur2->om_len - om2_off;
+        chunk_sz = min(min(om1_left, om2_left), bytes_remaining);
+
+        rc = memcmp(cur1->om_data + om1_off, cur2->om_data + om2_off,
+                    chunk_sz);
+        if (rc != 0) {
+            return rc;
+        }
+
+        om1_off += chunk_sz;
+        om2_off += chunk_sz;
+        bytes_remaining -= chunk_sz;
+    }
+}
+
+/**
  * Increases the length of an mbuf chain by adding data to the front.  If there
  * is insufficient room in the leading mbuf, additional mbufs are allocated and
  * prepended as necessary.  If this function fails to allocate an mbuf, the
@@ -851,6 +971,33 @@ os_mbuf_prepend(struct os_mbuf *om, int len)
 }
 
 /**
+ * Prepends a chunk of empty data to the specified mbuf chain and ensures the
+ * chunk is contiguous.  If either operation fails, the specified mbuf chain is
+ * freed and NULL is returned.
+ *
+ * @param om                    The mbuf chain to prepend to.
+ * @param len                   The number of bytes to prepend and pullup.
+ *
+ * @return                      The modified mbuf on success;
+ *                              NULL on failure (and the mbuf chain is freed).
+ */
+struct os_mbuf *
+os_mbuf_prepend_pullup(struct os_mbuf *om, uint16_t len)
+{
+    om = os_mbuf_prepend(om, len);
+    if (om == NULL) {
+        return NULL;
+    }
+
+    om = os_mbuf_pullup(om, len);
+    if (om == NULL) {
+        return NULL;
+    }
+
+    return om;
+}
+
+/**
  * Copies the contents of a flat buffer into an mbuf chain, starting at the
  * specified destination offset.  If the mbuf is too small for the source data,
  * it is extended as necessary.  If the destination mbuf contains a packet
@@ -870,8 +1017,8 @@ os_mbuf_copyinto(struct os_mbuf *om, int off, const void *src, int len)
     struct os_mbuf *next;
     struct os_mbuf *cur;
     const uint8_t *sptr;
+    uint16_t cur_off;
     int copylen;
-    int cur_off;
     int rc;
 
     /* Find the mbuf,offset pair for the start of the destination. */
diff --git a/libs/os/src/os_mempool.c b/libs/os/src/os_mempool.c
index 1e7428ca..e940ba42 100644
--- a/libs/os/src/os_mempool.c
+++ b/libs/os/src/os_mempool.c
@@ -41,21 +41,29 @@ STAILQ_HEAD(, os_mempool) g_os_mempool_list =
  * @return os_error_t 
  */
 os_error_t
-os_mempool_init(struct os_mempool *mp, int blocks, int block_size, void *membuf,
-                char *name)
+os_mempool_init(struct os_mempool *mp, int blocks, int block_size,
+                void *membuf, char *name)
 {
     int true_block_size;
     uint8_t *block_addr;
     struct os_memblock *block_ptr;
 
     /* Check for valid parameters */
-    if ((!mp) || (!membuf) || (blocks <= 0) || (block_size <= 0)) {
+    if ((!mp) || (blocks < 0) || (block_size <= 0)) {
         return OS_INVALID_PARM;
     }
 
-    /* Blocks need to be sized properly and memory buffer should be aligned */
-    if (((uint32_t)membuf & (OS_ALIGNMENT - 1)) != 0) {
-        return OS_MEM_NOT_ALIGNED;
+    if ((!membuf) && (blocks != 0)) {
+        return OS_INVALID_PARM;
+    }
+
+    if (membuf != NULL) {
+        /* Blocks need to be sized properly and memory buffer should be
+         * aligned
+         */
+        if (((uint32_t)membuf & (OS_ALIGNMENT - 1)) != 0) {
+            return OS_MEM_NOT_ALIGNED;
+        }
     }
     true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(block_size);
 
@@ -86,6 +94,42 @@ os_mempool_init(struct os_mempool *mp, int blocks, int block_size, void *membuf,
 }
 
 /**
+ * Checks if a memory block was allocated from the specified mempool.
+ *
+ * @param mp                    The mempool to check as parent.
+ * @param block_addr            The memory block to check as child.
+ *
+ * @return                      0 if the block does not belong to the mempool;
+ *                              1 if the block does belong to the mempool.
+ */
+int
+os_memblock_from(struct os_mempool *mp, void *block_addr)
+{
+    uint32_t true_block_size;
+    uint32_t baddr32;
+    uint32_t end;
+
+    _Static_assert(sizeof block_addr == sizeof baddr32,
+                   "Pointer to void must be 32-bits.");
+
+    baddr32 = (uint32_t)block_addr;
+    true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp->mp_block_size);
+    end = mp->mp_membuf_addr + (mp->mp_num_blocks * true_block_size);
+
+    /* Check that the block is in the memory buffer range. */
+    if ((baddr32 < mp->mp_membuf_addr) || (baddr32 >= end)) {
+        return 0;
+    }
+
+    /* All freed blocks should be on true block size boundaries! */
+    if (((baddr32 - mp->mp_membuf_addr) % true_block_size) != 0) {
+        return 0;
+    }
+
+    return 1;
+}
+
+/**
  * os memblock get 
  *  
  * Get a memory block from a memory pool 
@@ -135,9 +179,6 @@ os_error_t
 os_memblock_put(struct os_mempool *mp, void *block_addr)
 {
     os_sr_t sr;
-    uint32_t end;
-    uint32_t true_block_size;
-    uint32_t baddr32;
     struct os_memblock *block;
 
     /* Make sure parameters are valid */
@@ -146,23 +187,10 @@ os_memblock_put(struct os_mempool *mp, void *block_addr)
     }
 
     /* Check that the block we are freeing is a valid block! */
-    baddr32 = (uint32_t)block_addr;
-    true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp->mp_block_size);
-    end = mp->mp_membuf_addr + (mp->mp_num_blocks * true_block_size);
-    if ((baddr32 < mp->mp_membuf_addr) || (baddr32 >= end)) {
-        return OS_INVALID_PARM;
-    }
-
-    /* All freed blocks should be on true block size boundaries! */
-    if (((baddr32 - mp->mp_membuf_addr) % true_block_size) != 0) {
+    if (!os_memblock_from(mp, block_addr)) {
         return OS_INVALID_PARM;
     }
 
-    /* 
-     * XXX: we should do boundary checks here! The block had better be within 
-     * the pool. If it fails, do we return an error or assert()? Add this when 
-     * we add the memory debug. 
-     */ 
     block = (struct os_memblock *)block_addr;
     OS_ENTER_CRITICAL(sr);
     
diff --git a/libs/os/src/os_priv.h b/libs/os/src/os_priv.h
index dfc9e384..b5c8f65e 100644
--- a/libs/os/src/os_priv.h
+++ b/libs/os/src/os_priv.h
@@ -6,7 +6,7 @@
  * 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,
@@ -20,11 +20,17 @@
 #ifndef H_OS_PRIV_
 #define H_OS_PRIV_
 
+#include "os/queue.h"
+
 TAILQ_HEAD(os_task_list, os_task);
+TAILQ_HEAD(os_callout_list, os_callout);
+STAILQ_HEAD(os_task_stailq, os_task);
 
+extern struct os_task g_idle_task;
 extern struct os_task_list g_os_run_list;
 extern struct os_task_list g_os_sleep_list;
-extern struct os_task_list g_os_task_list;
+extern struct os_task_stailq g_os_task_list;
 extern struct os_task *g_current_task;
+extern struct os_callout_list g_callout_list;
 
 #endif
diff --git a/libs/os/src/os_task.c b/libs/os/src/os_task.c
index 55c1da24..e48fdf97 100644
--- a/libs/os/src/os_task.c
+++ b/libs/os/src/os_task.c
@@ -19,12 +19,13 @@
 
 
 #include "os/os.h"
+#include "os_priv.h"
 
 #include <string.h>
 
 uint8_t g_task_id;
 
-STAILQ_HEAD(, os_task) g_os_task_list = STAILQ_HEAD_INITIALIZER(g_os_task_list);
+struct os_task_stailq g_os_task_list;
 
 static void
 _clear_stack(os_stack_t *stack_bottom, int size)
diff --git a/libs/os/src/os_time.c b/libs/os/src/os_time.c
index 2ce379e8..4ef439f5 100644
--- a/libs/os/src/os_time.c
+++ b/libs/os/src/os_time.c
@@ -97,10 +97,14 @@ os_time_advance(int ticks)
     assert(ticks >= 0);
 
     if (ticks > 0) {
-        os_time_tick(ticks);
-        os_callout_tick();
-        os_sched_os_timer_exp();
-        os_sched(NULL);
+        if (!os_started()) {
+            g_os_time += ticks;
+        } else {
+            os_time_tick(ticks);
+            os_callout_tick();
+            os_sched_os_timer_exp();
+            os_sched(NULL);
+        }
     }
 }
 
diff --git a/libs/os/src/test/callout_test.c b/libs/os/src/test/callout_test.c
new file mode 100644
index 00000000..4e3811d7
--- /dev/null
+++ b/libs/os/src/test/callout_test.c
@@ -0,0 +1,330 @@
+/**
+ * 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 "testutil/testutil.h"
+#include "os/os.h"
+#include "os_test_priv.h"
+#include "os/os_eventq.h"
+#include "os/os_callout.h"
+#include "os/os_time.h"
+
+/* Task 1 for sending */
+#define CALLOUT_STACK_SIZE        (5120)
+#define SEND_CALLOUT_TASK_PRIO        (1)
+struct os_task callout_task_struct_send;
+os_stack_t callout_task_stack_send[CALLOUT_STACK_SIZE];
+
+#define RECEIVE_CALLOUT_TASK_PRIO        (2)
+struct os_task callout_task_struct_receive;
+os_stack_t callout_task_stack_receive[CALLOUT_STACK_SIZE];
+
+/* Delearing variables for callout_func */
+struct os_callout_func callout_func_test;
+
+/* The event to be sent*/
+struct os_eventq callout_evq;
+struct os_event callout_ev;
+
+/* The callout_stop task */
+#define SEND_STOP_CALLOUT_TASK_PRIO        (3)
+struct os_task callout_task_struct_stop_send;
+os_stack_t callout_task_stack_stop_send[CALLOUT_STACK_SIZE];
+
+#define RECEIVE_STOP_CALLOUT_TASK_PRIO        (4)
+struct os_task callout_task_struct_stop_receive;
+os_stack_t callout_task_stack_stop_receive[CALLOUT_STACK_SIZE];
+
+/* Delearing variables for callout_stop_func */
+#define MULTI_SIZE    (2)
+struct os_callout_func callout_func_stop_test[MULTI_SIZE];
+
+/* The event to be sent*/
+struct os_eventq callout_stop_evq[MULTI_SIZE];
+struct os_event callout_stop_ev;
+
+/* Declearing varables for callout_speak */
+#define SPEAK_CALLOUT_TASK_PRIO        (5)
+struct os_task callout_task_struct_speak;
+os_stack_t callout_task_stack_speak[CALLOUT_STACK_SIZE];
+
+/* Declearing varaibles for listen */
+#define LISTEN_CALLOUT_TASK_PRIO        (6)
+struct os_task callout_task_struct_listen;
+os_stack_t callout_task_stack_listen[CALLOUT_STACK_SIZE];
+
+struct os_callout_func callout_func_speak;
+
+/* Global variables to be used by the callout functions */
+int p;
+int q;
+int t;
+/* This is the function for callout_init*/
+void
+my_callout_func(void *arg)
+{
+    p = 4;
+}
+
+/* This is the function for callout_init of stop test_case*/
+void
+my_callout_stop_func(void *arg)
+{
+    q = 1;
+}
+/* This is the function for callout_init for speak test_case*/
+void
+my_callout_speak_func(void *arg)
+{
+    t = 2;
+}
+
+/* This is a callout task to send data */
+void
+callout_task_send(void *arg )
+{
+   int i;
+    /* Should say whether callout is armed or not */
+    i= os_callout_queued(&callout_func_test.cf_c);
+    TEST_ASSERT(i == 0);
+
+    /* Arm the callout */
+    i = os_callout_reset(&callout_func_test.cf_c, OS_TICKS_PER_SEC/ 50);
+    TEST_ASSERT_FATAL(i == 0);
+
+    /* Should say whether callout is armed or not */
+    i = os_callout_queued(&callout_func_test.cf_c);
+    TEST_ASSERT(i == 1);
+
+    /* Send the callout */ 
+    os_time_delay(OS_TICKS_PER_SEC );
+}
+
+/* This is the callout to receive data */
+void
+callout_task_receive( void *arg)
+{
+    int i;
+    struct os_event *event;
+    struct os_callout_func *callout;
+    os_time_t now;
+    os_time_t tm;
+    os_sr_t sr; 
+    /* Recieve using the os_eventq_poll */
+    event = os_eventq_poll(&callout_func_test.cf_c.c_evq, 1, OS_WAIT_FOREVER);
+    TEST_ASSERT(event->ev_type ==  OS_EVENT_T_TIMER);
+    TEST_ASSERT(event->ev_arg == NULL);
+    callout = (struct os_callout_func *)event;
+    TEST_ASSERT(callout->cf_func == my_callout_func);
+
+    /* Should say whether callout is armed or not */
+    i = os_callout_queued(&callout_func_test.cf_c);
+    TEST_ASSERT(i == 0);
+
+    OS_ENTER_CRITICAL(sr);
+    now = os_time_get();
+    tm = os_callout_wakeup_ticks(now);
+    TEST_ASSERT(tm == OS_TIMEOUT_NEVER);
+    OS_EXIT_CRITICAL(sr);
+    
+    /* Finishes the test when OS has been started */
+    os_test_restart();
+
+}
+
+/* This is callout to send the stop_callout */
+void
+callout_task_stop_send( void *arg)
+{
+    int k;
+    int j;    
+     /* Should say whether callout is armed or not */
+    for(k = 0; k<MULTI_SIZE; k++){
+        j = os_callout_queued(&callout_func_stop_test[k].cf_c);
+        TEST_ASSERT(j == 0);
+    }
+
+
+    /* Show that  callout is not armed after calling callout_stop */
+    for(k = 0; k<MULTI_SIZE; k++){
+        os_callout_stop(&callout_func_stop_test[k].cf_c);
+        j = os_callout_queued(&callout_func_stop_test[k].cf_c);
+        TEST_ASSERT(j == 0);
+    }
+    /* Arm the callout */
+    for(k = 0; k<MULTI_SIZE; k++){
+        j = os_callout_reset(&callout_func_stop_test[k].cf_c, OS_TICKS_PER_SEC/ 50);
+        TEST_ASSERT_FATAL(j == 0);
+    }
+    os_time_delay( OS_TICKS_PER_SEC );
+}
+
+/* This is the callout to receive stop_callout data */
+void
+callout_task_stop_receive( void *arg )
+{
+    int k;
+    struct os_event *event;
+    struct os_callout_func *callout;
+    /* Recieving using the os_eventq_poll */
+    for(k=0; k<MULTI_SIZE; k++){
+        event = os_eventq_poll(&callout_func_stop_test[k].cf_c.c_evq, 1,
+           OS_WAIT_FOREVER);
+        TEST_ASSERT(event->ev_type ==  OS_EVENT_T_TIMER);
+        TEST_ASSERT(event->ev_arg == NULL);
+        callout = (struct os_callout_func *)event;
+        TEST_ASSERT(callout->cf_func == my_callout_stop_func);
+
+
+     }
+     
+    /* Show that event is removed from the queued after calling callout_stop */
+    for(k=0; k<MULTI_SIZE; k++){
+        os_callout_stop(&callout_func_stop_test[k].cf_c);
+        /* Testing that the event has been removed from queue */
+        TEST_ASSERT_FATAL(1); 
+     }
+    /* Finishes the test when OS has been started */
+    os_test_restart();
+
+}
+
+/* This is a callout task to send data */
+void
+callout_task_stop_speak( void *arg )
+{
+    int i;
+    /* Arm the callout */
+    i = os_callout_reset(&callout_func_speak.cf_c, OS_TICKS_PER_SEC/ 50);
+    TEST_ASSERT_FATAL(i == 0);
+
+    /* should say whether callout is armed or not */
+    i = os_callout_queued(&callout_func_speak.cf_c);
+    TEST_ASSERT(i == 1);
+
+    os_callout_stop(&callout_func_speak.cf_c);
+    
+    /* Send the callout */ 
+    os_time_delay(OS_TICKS_PER_SEC/ 100 );
+    /* Finishes the test when OS has been started */
+    os_test_restart();
+}
+
+void
+callout_task_stop_listen( void *arg )
+{
+    struct os_event *event;
+    struct os_callout_func *callout;
+    event = os_eventq_get(callout_func_speak.cf_c.c_evq);
+    TEST_ASSERT_FATAL(0);
+    callout = (struct os_callout_func *)event;
+    TEST_ASSERT(callout->cf_func == my_callout_speak_func);
+
+}
+
+/* Test case to test the basics of the callout */
+TEST_CASE(callout_test)
+{
+
+    /* Initializing the OS */
+    os_init();
+    
+    /* Initialize the sending task */
+    os_task_init(&callout_task_struct_send, "callout_task_send",
+        callout_task_send, NULL, SEND_CALLOUT_TASK_PRIO, OS_WAIT_FOREVER,
+        callout_task_stack_send, CALLOUT_STACK_SIZE);
+
+    /* Initialize the receive task */
+    os_task_init(&callout_task_struct_receive, "callout_task_receive",
+        callout_task_receive, NULL, RECEIVE_CALLOUT_TASK_PRIO, OS_WAIT_FOREVER,
+        callout_task_stack_receive, CALLOUT_STACK_SIZE);
+
+    os_eventq_init(&callout_evq);
+    
+    /* Initialize the callout function */
+    os_callout_func_init(&callout_func_test, &callout_evq,
+        my_callout_func, NULL);
+
+    /* Does not return until OS_restart is called */
+    os_start();
+}
+
+/* Test case of the callout_task_stop */
+TEST_CASE(callout_test_stop)
+{
+    int k;
+    /* Initializing the OS */
+    os_init();
+
+    /* Initialize the sending task */
+    os_task_init(&callout_task_struct_stop_send, "callout_task_stop_send",
+        callout_task_stop_send, NULL, SEND_STOP_CALLOUT_TASK_PRIO, OS_WAIT_FOREVER,
+        callout_task_stack_stop_send, CALLOUT_STACK_SIZE);
+
+    /* Initialize the receiving task */
+    os_task_init(&callout_task_struct_stop_receive, "callout_task_stop_receive",
+        callout_task_stop_receive, NULL, RECEIVE_STOP_CALLOUT_TASK_PRIO,
+        OS_WAIT_FOREVER, callout_task_stack_stop_receive, CALLOUT_STACK_SIZE);
+
+    for(k = 0; k< MULTI_SIZE; k++){
+        os_eventq_init(&callout_stop_evq[k]);
+    }
+    
+    /* Initialize the callout function */
+    for(k = 0; k<MULTI_SIZE; k++){
+        os_callout_func_init(&callout_func_stop_test[k], &callout_stop_evq[k],
+           my_callout_stop_func, NULL);
+    }
+
+    /* Does not return until OS_restart is called */
+    os_start();
+
+}
+
+/* Test case to test case for speak and listen */
+TEST_CASE(callout_test_speak)
+{
+    /* Initializing the OS */
+    os_init();
+    
+    /* Initialize the sending task */
+    os_task_init(&callout_task_struct_speak, "callout_task_speak",
+        callout_task_stop_speak, NULL, SPEAK_CALLOUT_TASK_PRIO, OS_WAIT_FOREVER,
+        callout_task_stack_speak, CALLOUT_STACK_SIZE);
+
+    /* Initialize the receive task */
+    os_task_init(&callout_task_struct_listen, "callout_task_listen",
+        callout_task_stop_listen, NULL, LISTEN_CALLOUT_TASK_PRIO, OS_WAIT_FOREVER,
+        callout_task_stack_listen, CALLOUT_STACK_SIZE);
+
+    os_eventq_init(&callout_evq);
+    
+    /* Initialize the callout function */
+    os_callout_func_init(&callout_func_speak, &callout_evq,
+        my_callout_speak_func, NULL);    
+    /* Does not return until OS_restart is called */
+    os_start();
+
+}
+
+TEST_SUITE(os_callout_test_suite)
+{   
+    callout_test();
+    callout_test_stop();
+    callout_test_speak();
+}
diff --git a/libs/os/src/test/eventq_test.c b/libs/os/src/test/eventq_test.c
index 29e5fe30..cb1ed94b 100644
--- a/libs/os/src/test/eventq_test.c
+++ b/libs/os/src/test/eventq_test.c
@@ -17,21 +17,21 @@
  * under the License.
  */
  
-
+#include <string.h>
 #include "testutil/testutil.h"
 #include "os/os.h"
 #include "os_test_priv.h"
 #include "os/os_eventq.h"
 
 #define MY_STACK_SIZE        (5120)
-
-/*Task 1 sending task*/
+#define POLL_STACK_SIZE        (4096)
+/* Task 1 sending task */
 /* Define task stack and task object */
 #define SEND_TASK_PRIO        (1)
 struct os_task eventq_task_s;
 os_stack_t eventq_task_stack_s[MY_STACK_SIZE];
 
-/*Task 2 receiving task*/
+/* Task 2 receiving task */
 #define RECEIVE_TASK_PRIO     (2)
 struct os_task eventq_task_r;
 os_stack_t eventq_task_stack_r[MY_STACK_SIZE];
@@ -48,6 +48,39 @@ struct os_event m_event[SIZE_MULTI_EVENT];
 /* Setting the event to send and receive multiple data */
 uint8_t my_event_type = 1;
 
+/* Setting up data for the poll */
+/* Define the task stack for the eventq_task_poll_send */
+#define SEND_TASK_POLL_PRIO        (3)
+struct os_task eventq_task_poll_s;
+os_stack_t eventq_task_stack_poll_s[POLL_STACK_SIZE];
+
+/* Define the task stack for the eventq_task_poll_receive */
+#define RECEIVE_TASK_POLL_PRIO     (4)
+struct os_task eventq_task_poll_r;
+os_stack_t eventq_task_stack_poll_r[POLL_STACK_SIZE ];
+
+/* Setting the data for the poll timeout */
+/* Define the task stack for the eventq_task_poll_timeout_send */
+#define SEND_TASK_POLL_TIMEOUT_PRIO        (5)
+struct os_task eventq_task_poll_timeout_s;
+os_stack_t eventq_task_stack_poll_timeout_s[POLL_STACK_SIZE];
+
+/* Define the task stack for the eventq_task_poll_receive */
+#define RECEIVE_TASK_POLL_TIMEOUT_PRIO     (6)
+struct os_task eventq_task_poll_timeout_r;
+os_stack_t eventq_task_stack_poll_timeout_r[POLL_STACK_SIZE];
+
+/* Setting the data for the poll single */
+/* Define the task stack for the eventq_task_poll_single_send */
+#define SEND_TASK_POLL_SINGLE_PRIO        (7)
+struct os_task eventq_task_poll_single_s;
+os_stack_t eventq_task_stack_poll_single_s[POLL_STACK_SIZE];
+
+/* Define the task stack for the eventq_task_poll_single_receive */
+#define RECEIVE_TASK_POLL_SINGLE_PRIO     (8)
+struct os_task eventq_task_poll_single_r;
+os_stack_t eventq_task_stack_poll_single_r[POLL_STACK_SIZE];
+
 /* This is the task function  to send data */
 void
 eventq_task_send(void *arg)
@@ -95,6 +128,138 @@ eventq_task_receive(void *arg)
     os_test_restart();
 }
 
+void
+eventq_task_poll_send(void *arg)
+{
+    struct os_eventq *eventqs[SIZE_MULTI_EVENT];
+    int i;
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        eventqs[i] = &multi_eventq[i];
+    }
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        m_event[i].ev_type = i + 10;
+        m_event[i].ev_arg = NULL;
+
+        /* Put and send */
+        os_eventq_put(eventqs[i], &m_event[i]);
+        os_time_delay(OS_TICKS_PER_SEC / 2);
+    }
+
+    /* This task sleeps until the receive task completes the test. */
+    os_time_delay(1000000);
+}
+
+void
+eventq_task_poll_receive(void *arg)
+{
+    struct os_eventq *eventqs[SIZE_MULTI_EVENT];
+    struct os_event *event;
+    int i;
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        eventqs[i] = &multi_eventq[i];
+    }
+
+    /* Recieving using the os_eventq_poll*/
+    for (i = 0; i < SIZE_MULTI_EVENT; i++) {
+        event = os_eventq_poll(eventqs, SIZE_MULTI_EVENT, OS_WAIT_FOREVER);
+        TEST_ASSERT(event->ev_type == i +10);
+    }
+
+    /* Finishes the test when OS has been started */
+    os_test_restart();
+
+}
+
+/* Sending with a time failure */
+void
+eventq_task_poll_timeout_send(void *arg)
+{
+    struct os_eventq *eventqs[SIZE_MULTI_EVENT];
+    int i;
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        eventqs[i] = &multi_eventq[i];
+    }
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+         os_time_delay(1000);
+
+        /* Put and send */
+        os_eventq_put(eventqs[i], &m_event[i]);
+        os_time_delay(OS_TICKS_PER_SEC / 2);
+    }
+
+    /* This task sleeps until the receive task completes the test. */
+    os_time_delay(1000000);
+    
+}
+
+/* Receiving multiple event queues with a time failure */
+void
+eventq_task_poll_timeout_receive(void *arg)
+{
+    struct os_eventq *eventqs[SIZE_MULTI_EVENT];
+    struct os_event *event;
+    int i;
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        eventqs[i] = &multi_eventq[i];
+    }
+
+    /* Recieving using the os_eventq_poll_timeout*/
+    for (i = 0; i < SIZE_MULTI_EVENT; i++) {
+        event = os_eventq_poll(eventqs, SIZE_MULTI_EVENT, 200);
+        TEST_ASSERT(event == NULL);
+    }
+
+    /* Finishes the test when OS has been started */
+    os_test_restart();
+
+}
+
+/* Sending a single event to poll */
+void
+eventq_task_poll_single_send(void *arg)
+{
+    struct os_eventq *eventqs[SIZE_MULTI_EVENT];
+    int i;
+    int position = 2;
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        eventqs[i] = &multi_eventq[i];
+    }
+
+    /* Put and send */
+    os_eventq_put(eventqs[position], &m_event[position]);
+    os_time_delay(OS_TICKS_PER_SEC / 2);
+
+    /* This task sleeps until the receive task completes the test. */
+    os_time_delay(1000000);
+}
+
+/* Recieving the single event */
+void
+eventq_task_poll_single_receive(void *arg)
+{
+    struct os_eventq *eventqs[SIZE_MULTI_EVENT];
+    struct os_event *event;
+    int i;
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        eventqs[i] = &multi_eventq[i];
+    }
+
+    /* Recieving using the os_eventq_poll*/
+    event = os_eventq_poll(eventqs, SIZE_MULTI_EVENT, OS_WAIT_FOREVER);
+    TEST_ASSERT(event->ev_type == 20);
+
+    /* Finishes the test when OS has been started */
+    os_test_restart();
+}
+
 TEST_CASE(event_test_sr)
 {
     int i;
@@ -121,7 +286,131 @@ TEST_CASE(event_test_sr)
 
 }
 
+/* To test for the basic function of os_eventq_poll() */
+TEST_CASE(event_test_poll_sr)
+{
+    int i;
+
+    /* Initializing the OS */
+    os_init();
+    /* Initialize the task */
+    os_task_init(&eventq_task_poll_s, "eventq_task_poll_s", eventq_task_poll_send,
+        NULL, SEND_TASK_POLL_PRIO, OS_WAIT_FOREVER, eventq_task_stack_poll_s, 
+        POLL_STACK_SIZE);
+
+    /* Receive events and check whether the eevnts are correctly received */
+    os_task_init(&eventq_task_poll_r, "eventq_task_r", eventq_task_poll_receive,
+        NULL, RECEIVE_TASK_POLL_PRIO, OS_WAIT_FOREVER, eventq_task_stack_poll_r,
+        POLL_STACK_SIZE);
+
+    /* Initializing the eventqs. */
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        os_eventq_init(&multi_eventq[i]);
+    }
+
+    /* Does not return until OS_restart is called */
+    os_start();
+
+}
+
+/* Test case for poll timeout */
+TEST_CASE(event_test_poll_timeout_sr)
+{
+    int i;
+
+    /* Initializing the OS */
+    os_init();
+    /* Initialize the task */
+    os_task_init(&eventq_task_poll_timeout_s, "eventq_task_poll_timeout_s", 
+        eventq_task_poll_timeout_send, NULL, SEND_TASK_POLL_TIMEOUT_PRIO,
+        OS_WAIT_FOREVER, eventq_task_stack_poll_timeout_s, POLL_STACK_SIZE);
+
+    /* Receive events and check whether the eevnts are correctly received */
+    os_task_init(&eventq_task_poll_timeout_r, "eventq_task_timeout_r",
+        eventq_task_poll_timeout_receive, NULL, RECEIVE_TASK_POLL_TIMEOUT_PRIO,
+        OS_WAIT_FOREVER, eventq_task_stack_poll_timeout_r, POLL_STACK_SIZE);
+
+    /* Initializing the eventqs. */
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        os_eventq_init(&multi_eventq[i]);
+
+        m_event[i].ev_type = i + 10;
+        m_event[i].ev_arg = NULL;
+    }
+
+    /* Does not return until OS_restart is called */
+    os_start();
+
+}
+
+/* The case for poll single */
+/* Test case for poll timeout */
+TEST_CASE(event_test_poll_single_sr)
+{
+    int i;
+
+    /* Initializing the OS */
+    os_init();
+    /* Initialize the task */
+    os_task_init(&eventq_task_poll_single_s, "eventq_task_poll_single_s", 
+        eventq_task_poll_single_send, NULL, SEND_TASK_POLL_SINGLE_PRIO,
+        OS_WAIT_FOREVER, eventq_task_stack_poll_single_s, POLL_STACK_SIZE);
+
+    /* Receive events and check whether the eevnts are correctly received */
+    os_task_init(&eventq_task_poll_single_r, "eventq_task_single_r",
+        eventq_task_poll_single_receive, NULL, RECEIVE_TASK_POLL_SINGLE_PRIO,
+        OS_WAIT_FOREVER, eventq_task_stack_poll_single_r, POLL_STACK_SIZE);
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        os_eventq_init(&multi_eventq[i]);
+
+        m_event[i].ev_type = 10 * i;
+        m_event[i].ev_arg = NULL;
+    }
+
+    /* Does not return until OS_restart is called */
+    os_start();
+
+}
+
+/**
+ * Tests eventq_poll() with a timeout of 0.  This should not involve the
+ * scheduler at all, so it should work without starting the OS.
+ */
+TEST_CASE(event_test_poll_0timo)
+{
+    struct os_eventq *eventqs[SIZE_MULTI_EVENT];
+    struct os_event *evp;
+    struct os_event ev;
+    int i;
+
+    for (i = 0; i < SIZE_MULTI_EVENT; i++){
+        os_eventq_init(&multi_eventq[i]);
+        eventqs[i] = &multi_eventq[i];
+    }
+
+    evp = os_eventq_poll(eventqs, SIZE_MULTI_EVENT, 0);
+    TEST_ASSERT(evp == NULL);
+
+    /* Ensure no eventq thinks a task is waiting on it. */
+    for (i = 0; i < SIZE_MULTI_EVENT; i++) {
+        TEST_ASSERT(eventqs[i]->evq_task == NULL);
+    }
+
+    /* Put an event on one of the queues. */
+    memset(&ev, 0, sizeof ev);
+    ev.ev_type = 1;
+    os_eventq_put(eventqs[3], &ev);
+
+    evp = os_eventq_poll(eventqs, SIZE_MULTI_EVENT, 0);
+    TEST_ASSERT(evp == &ev);
+}
+
 TEST_SUITE(os_eventq_test_suite)
 {
     event_test_sr();
+    event_test_poll_sr();
+    event_test_poll_timeout_sr();
+    event_test_poll_single_sr();
+    event_test_poll_0timo();
 }
diff --git a/libs/os/src/test/os_test.c b/libs/os/src/test/os_test.c
index 1b3af1ab..48f07206 100644
--- a/libs/os/src/test/os_test.c
+++ b/libs/os/src/test/os_test.c
@@ -31,7 +31,7 @@ os_test_all(void)
     os_sem_test_suite();
     os_mbuf_test_suite();
     os_eventq_test_suite();
-    
+    os_callout_test_suite();
 
     return tu_case_failed;
 }
diff --git a/libs/os/src/test/os_test_priv.h b/libs/os/src/test/os_test_priv.h
index 5193c33b..e923a6fa 100644
--- a/libs/os/src/test/os_test_priv.h
+++ b/libs/os/src/test/os_test_priv.h
@@ -27,6 +27,6 @@ int os_mbuf_test_suite(void);
 int os_mutex_test_suite(void);
 int os_sem_test_suite(void);
 int os_eventq_test_suite(void);
-
+int os_callout_test_suite(void);
 
 #endif
diff --git a/libs/shell/src/shell.c b/libs/shell/src/shell.c
index 95ddcb03..d38573c9 100644
--- a/libs/shell/src/shell.c
+++ b/libs/shell/src/shell.c
@@ -572,6 +572,8 @@ shell_task_init(uint8_t prio, os_stack_t *stack, uint16_t stack_size,
     os_eventq_init(&shell_evq);
     os_mqueue_init(&g_shell_nlip_mq, NULL);
 
+    console_init(shell_console_rx_cb);
+
     rc = os_task_init(&shell_task, "shell", shell_task_func,
             NULL, prio, OS_WAIT_FOREVER, stack, stack_size);
     if (rc != 0) {
diff --git a/libs/testutil/include/testutil/testutil.h b/libs/testutil/include/testutil/testutil.h
index da3f618e..9c6192a2 100644
--- a/libs/testutil/include/testutil/testutil.h
+++ b/libs/testutil/include/testutil/testutil.h
@@ -57,6 +57,9 @@ extern const char *tu_suite_name;
 extern const char *tu_case_name;
 extern int tu_first_idx;
 
+typedef void tu_post_test_fn_t(void *arg);
+
+void tu_suite_set_post_test_cb(tu_post_test_fn_t *cb, void *cb_arg);
 int tu_parse_args(int argc, char **argv);
 int tu_init(void);
 void tu_restart(void);
@@ -65,6 +68,7 @@ void tu_restart(void);
  * Private declarations                                                      *
  *****************************************************************************/
 
+void tu_suite_complete(void);
 void tu_suite_init(const char *name);
 
 void tu_case_init(const char *name);
@@ -74,7 +78,7 @@ void tu_case_fail_assert(int fatal, const char *file, int line,
 void tu_case_write_pass_auto(void);
 void tu_case_pass_manual(const char *file, int line,
                          const char *format, ...);
-
+void tu_case_post_test(void);
 
 extern int tu_any_failed;
 extern int tu_suite_failed;
@@ -91,6 +95,7 @@ extern jmp_buf tu_case_jb;
     {                                                                         \
         tu_suite_init(#suite_name);                                           \
         TEST_SUITE_##suite_name();                                            \
+        tu_suite_complete();                                                  \
                                                                               \
         return tu_suite_failed;                                               \
     }                                                                         \
@@ -113,6 +118,7 @@ extern jmp_buf tu_case_jb;
                                                                               \
             if (setjmp(tu_case_jb) == 0) {                                    \
                 TEST_CASE_##case_name();                                      \
+                tu_case_post_test();                                          \
                 tu_case_write_pass_auto();                                    \
             }                                                                 \
         }                                                                     \
diff --git a/libs/testutil/src/case.c b/libs/testutil/src/case.c
index bea6efa3..614b1425 100644
--- a/libs/testutil/src/case.c
+++ b/libs/testutil/src/case.c
@@ -30,6 +30,8 @@ int tu_case_failed;
 int tu_case_idx;
 
 const char *tu_case_name;
+tu_post_test_fn_t *tu_case_post_test_cb;
+void *tu_case_post_test_cb_arg;
 
 #define TU_CASE_BUF_SZ      1024
 
@@ -101,6 +103,14 @@ tu_case_complete(void)
     tu_case_idx++;
 }
 
+void
+tu_case_post_test(void)
+{
+    if (tu_case_post_test_cb != NULL) {
+        tu_case_post_test_cb(tu_case_post_test_cb_arg);
+    }
+}
+
 static void
 tu_case_write_fail_buf(void)
 {
diff --git a/libs/testutil/src/suite.c b/libs/testutil/src/suite.c
index 5c2e5cbc..93ca639f 100644
--- a/libs/testutil/src/suite.c
+++ b/libs/testutil/src/suite.c
@@ -30,6 +30,30 @@ tu_suite_set_name(const char *name)
     tu_suite_name = name;
 }
 
+/**
+ * Configures a callback that gets executed at the end of each test case in the
+ * current suite.  This is useful when there are some checks that should be
+ * performed at the end of each test (e.g., verify no memory leaks).  This
+ * callback is cleared when the current suite completes.
+ *
+ * @param cb                    The callback to execute at the end of each test
+ *                                  case.
+ * @param cb_arg                An optional argument that gets passed to the
+ *                                  callback.
+ */
+void
+tu_suite_set_post_test_cb(tu_post_test_fn_t *cb, void *cb_arg)
+{
+    tu_case_post_test_cb = cb;
+    tu_case_post_test_cb_arg = cb_arg;
+}
+
+void
+tu_suite_complete(void)
+{
+    tu_suite_set_post_test_cb(NULL, NULL);
+}
+
 void
 tu_suite_init(const char *name)
 {
diff --git a/libs/testutil/src/testutil_priv.h b/libs/testutil/src/testutil_priv.h
index 0047874e..3f8cb2dc 100644
--- a/libs/testutil/src/testutil_priv.h
+++ b/libs/testutil/src/testutil_priv.h
@@ -23,7 +23,12 @@
 #include <stddef.h>
 #include <inttypes.h>
 
+#include "testutil/testutil.h"
+
 void tu_arch_restart(void);
 void tu_case_abort(void);
 
+extern tu_post_test_fn_t *tu_case_post_test_cb;
+extern void *tu_case_post_test_cb_arg;
+
 #endif
diff --git a/libs/util/include/util/mem.h b/libs/util/include/util/mem.h
new file mode 100644
index 00000000..52ddce4d
--- /dev/null
+++ b/libs/util/include/util/mem.h
@@ -0,0 +1,38 @@
+/**
+ * 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 H_UTIL_MEM_
+#define H_UTIL_MEM_
+
+struct os_mempool;
+struct os_mbuf_pool;
+
+int mem_malloc_mempool(struct os_mempool *mempool, int num_blocks,
+                       int block_size, char *name, void **out_buf);
+
+int mem_malloc_mbuf_pool(struct os_mempool *mempool,
+                         struct os_mbuf_pool *mbuf_pool, int num_blocks,
+                         int block_size, char *name,
+                         void **out_buf);
+int mem_malloc_mbufpkt_pool(struct os_mempool *mempool,
+                            struct os_mbuf_pool *mbuf_pool, int num_blocks,
+                            int block_size, char *name,
+                            void **out_buf);
+
+#endif
diff --git a/libs/util/src/mem.c b/libs/util/src/mem.c
new file mode 100644
index 00000000..faf82345
--- /dev/null
+++ b/libs/util/src/mem.c
@@ -0,0 +1,146 @@
+/**
+ * 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 "os/os.h"
+
+/**
+ * Mallocs a block of memory and initializes a mempool to use it.
+ *
+ * @param mempool               The mempool to initialize.
+ * @param num_blocks            The total number of memory blocks in the
+ *                                  mempool.
+ * @param block_size            The size of each mempool entry.
+ * @param name                  The name to give the mempool.
+ * @param out_buf               On success, this points to the malloced memory.
+ *                                  Pass NULL if you don't need this
+ *                                  information.
+ *
+ * @return                      0 on success;
+ *                              OS_ENOMEM on malloc failure;
+ *                              Other OS code on unexpected error.
+ */
+int
+mem_malloc_mempool(struct os_mempool *mempool, int num_blocks, int block_size,
+                   char *name, void **out_buf)
+{
+    void *buf;
+    int rc;
+
+    block_size = OS_ALIGN(block_size, OS_ALIGNMENT);
+
+    if (num_blocks > 0) {
+        buf = malloc(OS_MEMPOOL_BYTES(num_blocks, block_size));
+        if (buf == NULL) {
+            return OS_ENOMEM;
+        }
+    } else {
+        buf = NULL;
+    }
+
+    rc = os_mempool_init(mempool, num_blocks, block_size, buf, name);
+    if (rc != 0) {
+        free(buf);
+        return rc;
+    }
+
+    if (out_buf != NULL) {
+        *out_buf = buf;
+    }
+
+    return 0;
+}
+
+/**
+ * Mallocs a block of memory and initializes an mbuf pool to use it.  The
+ * specified block_size indicates the size of an mbuf acquired from the pool if
+ * it does not contain a pkthdr.
+ *
+ * @param mempool               The mempool to initialize.
+ * @param mbuf_pool             The mbuf pool to initialize.
+ * @param num_blocks            The total number of mbufs in the pool.
+ * @param block_size            The size of each mbuf.
+ * @param name                  The name to give the mempool.
+ * @param out_buf               On success, this points to the malloced memory.
+ *                                  Pass NULL if you don't need this
+ *                                  information.
+ *
+ * @return                      0 on success;
+ *                              OS_ENOMEM on malloc failure;
+ *                              Other OS code on unexpected error.
+ */
+int
+mem_malloc_mbuf_pool(struct os_mempool *mempool,
+                     struct os_mbuf_pool *mbuf_pool, int num_blocks,
+                     int block_size, char *name,
+                     void **out_buf)
+{
+    void *buf;
+    int rc;
+
+    block_size = OS_ALIGN(block_size + sizeof (struct os_mbuf), OS_ALIGNMENT);
+
+    rc = mem_malloc_mempool(mempool, num_blocks, block_size, name, &buf);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = os_mbuf_pool_init(mbuf_pool, mempool, block_size, num_blocks);
+    if (rc != 0) {
+        free(buf);
+        return rc;
+    }
+
+    if (out_buf != NULL) {
+        *out_buf = buf;
+    }
+
+    return 0;
+}
+
+/**
+ * Mallocs a block of memory and initializes an mbuf pool to use it.  The
+ * specified block_size indicates the size of an mbuf acquired from the pool if
+ * it contains a pkthdr.
+ *
+ * @param mempool               The mempool to initialize.
+ * @param mbuf_pool             The mbuf pool to initialize.
+ * @param num_blocks            The total number of mbufs in the pool.
+ * @param block_size            The size of each mbuf.
+ * @param name                  The name to give the mempool.
+ * @param out_buf               On success, this points to the malloced memory.
+ *                                  Pass NULL if you don't need this
+ *                                  information.
+ *
+ * @return                      0 on success;
+ *                              OS_ENOMEM on malloc failure;
+ *                              Other OS code on unexpected error.
+ */
+int
+mem_malloc_mbufpkt_pool(struct os_mempool *mempool,
+                        struct os_mbuf_pool *mbuf_pool, int num_blocks,
+                        int block_size, char *name,
+                        void **out_buf)
+{
+    int rc;
+
+    rc = mem_malloc_mbuf_pool(mempool, mbuf_pool, num_blocks,
+                              block_size + sizeof (struct os_mbuf_pkthdr),
+                              name, out_buf);
+    return rc;
+}
diff --git a/libs/wifi_mgmt/include/wifi_mgmt/wifi_mgmt.h b/libs/wifi_mgmt/include/wifi_mgmt/wifi_mgmt.h
new file mode 100644
index 00000000..411921a8
--- /dev/null
+++ b/libs/wifi_mgmt/include/wifi_mgmt/wifi_mgmt.h
@@ -0,0 +1,82 @@
+/**
+ * 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 __WIFI_MGMT_H__
+#define __WIFI_MGMT_H__
+
+/*
+ * Wi-Fi interface abstraction.
+ */
+#define WIFI_SSID_MAX           32	/* max SSID name length */
+#define WIFI_BSSID_LEN           6      /* MAC address len */
+#define WIFI_SCAN_CNT_MAX       20
+#define WIFI_KEY_MAX            64      /* max key length */
+#define WIFI_SSID_EMPTY(ssid)   (ssid)[0] == '\0'
+
+/*
+ * Info about an access point.
+ */
+struct wifi_ap {
+    char wa_ssid[WIFI_SSID_MAX + 1];
+    char wa_bssid[WIFI_BSSID_LEN + 1];
+    int8_t wa_rssi;
+    uint8_t wa_key_type;
+    uint8_t wa_channel;
+};
+
+struct wifi_if_ops;
+
+/*
+ * Wifi interface
+ */
+struct wifi_if {
+    enum  {
+        STOPPED = 0,
+        INIT,
+        CONNECTING,
+        DHCP_WAIT,
+        CONNECTED,
+        SCANNING
+    } wi_state, wi_tgt;
+    struct os_mutex wi_mtx;
+    struct os_event wi_event;
+    struct os_callout_func wi_timer;
+    const struct wifi_if_ops *wi_ops;
+
+    uint8_t wi_scan_cnt;
+    struct wifi_ap wi_scan[WIFI_SCAN_CNT_MAX];
+    char wi_ssid[WIFI_SSID_MAX + 1];
+    char wi_key[WIFI_KEY_MAX + 1];
+    uint8_t wi_myip[4];
+};
+
+/*
+ * XXX. is wifi_if_lookup() needed? It is unlikely that there's going to be
+ * multiple wifi interfaces on a system.
+ */
+struct wifi_if *wifi_if_lookup(int port);
+int wifi_if_register(struct wifi_if *, const struct wifi_if_ops *);
+
+int wifi_start(struct wifi_if *);
+int wifi_connect(struct wifi_if *);
+int wifi_stop(struct wifi_if *w);
+int wifi_scan_start(struct wifi_if *w);
+
+int wifi_task_init(uint8_t prio, os_stack_t *stack, uint16_t stack_size);
+
+#endif /* __WIFI_MGMT_H__ */
diff --git a/libs/wifi_mgmt/include/wifi_mgmt/wifi_mgmt_if.h b/libs/wifi_mgmt/include/wifi_mgmt/wifi_mgmt_if.h
new file mode 100644
index 00000000..c6e432da
--- /dev/null
+++ b/libs/wifi_mgmt/include/wifi_mgmt/wifi_mgmt_if.h
@@ -0,0 +1,47 @@
+/**
+ * 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 __WIFI_MGMT_IF_H__
+#define __WIFI_MGMT_IF_H__
+
+/*
+ * Interface between Wi-fi management and the driver.
+ */
+struct wifi_if_ops {
+    int (*wio_init)(struct wifi_if *);
+    void (*wio_deinit)(struct wifi_if *);
+    int (*wio_scan_start)(struct wifi_if *);
+    int (*wio_connect)(struct wifi_if *, struct wifi_ap *);
+    void (*wio_disconnect)(struct wifi_if *);
+};
+
+/*
+ * Exported so driver can use this for it's timers.
+ */
+extern struct os_eventq wifi_evq;
+
+/*
+ * Called by the Wi-fi driver.
+ */
+void wifi_scan_result(struct wifi_if *, struct wifi_ap *);
+void wifi_scan_done(struct wifi_if *, int status);
+void wifi_connect_done(struct wifi_if *wi, int status);
+void wifi_disconnected(struct wifi_if *wi, int status);
+void wifi_dhcp_done(struct wifi_if *wi, uint8_t *ip); /* XXX more IP info */
+
+#endif /* __WIFI_MGMT_IF_H__ */
diff --git a/libs/wifi_mgmt/pkg.yml b/libs/wifi_mgmt/pkg.yml
new file mode 100644
index 00000000..ebe582c5
--- /dev/null
+++ b/libs/wifi_mgmt/pkg.yml
@@ -0,0 +1,35 @@
+#
+# 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: libs/wifi_mgmt
+pkg.description: Wifi state management
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+pkg.deps:
+    - "@apache-mynewt-core/libs/os"
+    - "@apache-mynewt-core/libs/util"
+pkg.reqs:
+    - console
+pkg.cflags.SHELL:
+    - -DSHELL_PRESENT
+
+pkg.deps.TEST:
+   - libs/testutil
+
diff --git a/libs/wifi_mgmt/src/wifi.c b/libs/wifi_mgmt/src/wifi.c
new file mode 100644
index 00000000..b5b6a72a
--- /dev/null
+++ b/libs/wifi_mgmt/src/wifi.c
@@ -0,0 +1,348 @@
+/**
+ * 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 <stddef.h>
+#include <string.h>
+#include <assert.h>
+
+#include <os/os.h>
+#include <bsp/bsp.h>
+#include <hal/hal_gpio.h>
+#include <shell/shell.h>
+#include <console/console.h>
+
+#include "wifi_mgmt/wifi_mgmt.h"
+#include "wifi_mgmt/wifi_mgmt_if.h"
+#include "wifi_priv.h"
+
+#define WIFI_EV_STATE           OS_EVENT_T_PERUSER
+
+static struct os_task wifi_os_task;
+struct os_eventq wifi_evq;
+
+static struct wifi_ap *wifi_find_ap(struct wifi_if *wi, char *ssid);
+static void wifi_events(void *arg);
+
+static struct wifi_if *wifi_if;
+
+/*
+ * Looks up interface based on port number.
+ */
+struct wifi_if *
+wifi_if_lookup(int port)
+{
+    assert(port == 0);
+    return wifi_if;
+}
+
+/*
+ * Called by wi-fi driver to register itself.
+ */
+int
+wifi_if_register(struct wifi_if *wi, const struct wifi_if_ops *ops)
+{
+    if (wifi_if) {
+        return -1;
+    }
+    wifi_if = wi;
+
+    wi->wi_ops = ops;
+    os_mutex_init(&wi->wi_mtx);
+    os_callout_func_init(&wi->wi_timer, &wifi_evq, wifi_events, wi);
+    wi->wi_event.ev_type = WIFI_EV_STATE;
+    wi->wi_event.ev_arg = wi;
+
+    return 0;
+}
+
+/*
+ * For Wi-fi mgmt state machine, set the target state, and queue an
+ * event to do the state transition in wifi task context.
+ */
+static void
+wifi_tgt_state(struct wifi_if *wi, int state)
+{
+    wi->wi_tgt = state;
+    os_eventq_put(&wifi_evq, &wi->wi_event);
+}
+
+/*
+ * Wi-fi driver reports a response to wifi scan request.
+ * Driver reports networks one at a time.
+ */
+void
+wifi_scan_result(struct wifi_if *wi, struct wifi_ap *ap)
+{
+    if (wi->wi_scan_cnt == WIFI_SCAN_CNT_MAX) {
+        return;
+    }
+    wi->wi_scan[wi->wi_scan_cnt++] = *ap;
+}
+
+/*
+ * Wifi driver reports that scan is finished.
+ */
+void
+wifi_scan_done(struct wifi_if *wi, int status)
+{
+    struct wifi_ap *ap = NULL;
+
+    console_printf("scan_results %d: %d\n", wi->wi_scan_cnt, status);
+    if (status) {
+        wifi_tgt_state(wi, STOPPED);
+        return;
+    }
+
+    /*
+     * XXX decide what to do with scan results here.
+     */
+    if (!WIFI_SSID_EMPTY(wi->wi_ssid)) {
+        ap = wifi_find_ap(wi, wi->wi_ssid);
+    }
+    if (ap) {
+        wifi_tgt_state(wi, CONNECTING);
+    } else {
+        wifi_tgt_state(wi, INIT);
+    }
+}
+
+/*
+ * Wi-fi driver reports whether it was successful in establishing connection
+ * to an AP. XXX need status codes in wifi_mgmt.h
+ */
+void
+wifi_connect_done(struct wifi_if *wi, int status)
+{
+    console_printf("connect_done : %d\n", status);
+    if (status) {
+        wifi_tgt_state(wi, INIT);
+        return;
+    }
+    wifi_tgt_state(wi, DHCP_WAIT);
+}
+
+/*
+ * Wi-fi driver reports that there's an IP address. We can start using
+ * this interface.
+ */
+void
+wifi_dhcp_done(struct wifi_if *wi, uint8_t *ip)
+{
+    console_printf("dhcp done %d.%d.%d.%d\n", ip[0], ip[1], ip[2], ip[3]);
+    wifi_tgt_state(wi, CONNECTED);
+}
+
+/*
+ * Wi-fi driver reports that we are disconnected from an AP.
+ */
+void
+wifi_disconnected(struct wifi_if *wi, int status)
+{
+    console_printf("disconnect : %d\n", status);
+    wifi_tgt_state(wi, INIT);
+}
+
+static struct wifi_ap *
+wifi_find_ap(struct wifi_if *wi, char *ssid)
+{
+    int i;
+
+    for (i = 0; i < wi->wi_scan_cnt; i++) {
+        if (!strcmp(wi->wi_scan[i].wa_ssid, ssid)) {
+            return &wi->wi_scan[i];
+        }
+    }
+    return NULL;
+}
+
+static void
+wifi_events(void *arg)
+{
+    /*
+     * Expire connection attempts. Periodic scanning if tgt AP not visible.
+     */
+}
+
+/*
+ * Called by user to start bringing up Wi-fi interface online.
+ */
+int
+wifi_start(struct wifi_if *wi)
+{
+    if (wi->wi_state != STOPPED) {
+        return -1;
+    }
+    wifi_tgt_state(wi, INIT);
+    return 0;
+}
+
+/*
+ * Called by user to stop Wi-fi interface.
+ */
+int
+wifi_stop(struct wifi_if *wi)
+{
+    wifi_tgt_state(wi, STOPPED);
+    return 0;
+}
+
+/*
+ * Called by user to connect Wi-fi interface to AP. Will fail if no
+ * SSID name is set.
+ */
+int
+wifi_connect(struct wifi_if *wi)
+{
+    switch (wi->wi_state) {
+    case STOPPED:
+        return -1;
+    case INIT:
+        if (WIFI_SSID_EMPTY(wi->wi_ssid)) {
+            return -1;
+        }
+        wifi_tgt_state(wi, CONNECTING);
+        return 0;
+    default:
+        return -1;
+    }
+    return 0;
+}
+
+/*
+ * From user to initiate Wi-Fi scan.
+ */
+int
+wifi_scan_start(struct wifi_if *wi)
+{
+    if (wi->wi_state == INIT) {
+        wifi_tgt_state(wi, SCANNING);
+        return 0;
+    }
+    return -1;
+}
+
+/*
+ * Wi-fi mgmt state machine.
+ */
+static void
+wifi_step(struct wifi_if *wi)
+{
+    int rc;
+    struct wifi_ap *ap;
+
+    switch (wi->wi_tgt) {
+    case STOPPED:
+        if (wi->wi_state != STOPPED) {
+            if (wi->wi_state >= CONNECTING) {
+                wi->wi_ops->wio_disconnect(wi);
+            }
+            wi->wi_ops->wio_deinit(wi);
+            wi->wi_state = STOPPED;
+        }
+        break;
+    case INIT:
+        if (wi->wi_state == STOPPED) {
+            rc = wi->wi_ops->wio_init(wi);
+            console_printf("wifi_init : %d\n", rc);
+            if (!rc) {
+                wi->wi_state = INIT;
+            }
+        } else if (wi->wi_state == SCANNING) {
+            wi->wi_state = wi->wi_tgt;
+        } else if (wi->wi_state == CONNECTING) {
+            wi->wi_state = wi->wi_tgt;
+        }
+        break;
+    case SCANNING:
+        if (wi->wi_state == INIT) {
+            memset(wi->wi_scan, 0, sizeof(wi->wi_scan));
+            rc = wi->wi_ops->wio_scan_start(wi);
+            console_printf("wifi_request_scan : %d\n", rc);
+            if (rc != 0) {
+                break;
+            }
+            wi->wi_state = SCANNING;
+        } else {
+            wi->wi_tgt = wi->wi_state;
+        }
+        break;
+    case CONNECTING:
+        if (wi->wi_state == INIT || wi->wi_state == SCANNING) {
+            ap = wifi_find_ap(wi, wi->wi_ssid);
+            if (!ap) {
+                wifi_tgt_state(wi, SCANNING);
+                break;
+            }
+            rc = wi->wi_ops->wio_connect(wi, ap);
+            console_printf("wifi_connect : %d\n", rc);
+            if (rc == 0) {
+                wi->wi_state = CONNECTING;
+            } else {
+                wi->wi_tgt = STOPPED;
+            }
+        }
+        break;
+    case DHCP_WAIT:
+        wi->wi_state = wi->wi_tgt;
+        break;
+    case CONNECTED:
+        wi->wi_state = wi->wi_tgt;
+        break;
+    default:
+        console_printf("wifi_step() unknown tgt : %d\n", wi->wi_tgt);
+        wi->wi_state = wi->wi_tgt;
+        break;
+    }
+}
+
+static void
+wifi_task(void *arg)
+{
+    struct os_event *ev;
+    struct os_callout_func *cf;
+    struct wifi_if *wi;
+
+    while ((ev = os_eventq_get(&wifi_evq))) {
+        switch (ev->ev_type) {
+        case OS_EVENT_T_TIMER:
+            cf = (struct os_callout_func *)ev;
+            cf->cf_func(CF_ARG(cf));
+            break;
+        case WIFI_EV_STATE:
+            wi = (struct wifi_if *)ev->ev_arg;
+            while (wi->wi_state != wi->wi_tgt) {
+                wifi_step(wi);
+            }
+            break;
+        default:
+            break;
+        }
+    }
+}
+
+int
+wifi_task_init(uint8_t prio, os_stack_t *stack, uint16_t stack_size)
+{
+#ifdef SHELL_PRESENT
+    shell_cmd_register(&wifi_cli_cmd);
+#endif
+    os_eventq_init(&wifi_evq);
+
+    return os_task_init(&wifi_os_task, "wifi", wifi_task, NULL,
+      prio, OS_WAIT_FOREVER, stack, stack_size);
+}
diff --git a/libs/wifi_mgmt/src/wifi_cli.c b/libs/wifi_mgmt/src/wifi_cli.c
new file mode 100644
index 00000000..4cd95529
--- /dev/null
+++ b/libs/wifi_mgmt/src/wifi_cli.c
@@ -0,0 +1,89 @@
+/**
+ * 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.
+ */
+#ifdef SHELL_PRESENT
+#include <stddef.h>
+#include <string.h>
+#include <assert.h>
+
+#include <shell/shell.h>
+#include <console/console.h>
+
+#include "wifi_mgmt/wifi_mgmt.h"
+
+#include "wifi_priv.h"
+
+static int
+wifi_cli(int argc, char **argv)
+{
+    struct wifi_if *wi;
+    int i;
+
+    if (argc < 1) {
+        return 0;
+    }
+    wi = wifi_if_lookup(0);
+
+    if (!strcmp(argv[1], "start")) {
+        wifi_start(wi);
+    } else if (!strcmp(argv[1], "stop")) {
+        wifi_stop(wi);
+    } else if (!strcmp(argv[1], "scan")) {
+        wifi_scan_start(wi);
+    } else if (!strcmp(argv[1], "aps")) {
+        int i;
+        struct wifi_ap *ap;
+
+        console_printf("   %32s %4s %4s %s\n", "SSID", "RSSI", "chan", "sec");
+        for (i = 0; i < wi->wi_scan_cnt; i++) {
+            ap = (struct wifi_ap *)&wi->wi_scan[i];
+            console_printf("%2d:%32s %4d %4d %s\n",
+              i, ap->wa_ssid, ap->wa_rssi, ap->wa_channel,
+              ap->wa_key_type ? "X" : "");
+        }
+    } else if (!strcmp(argv[1], "connect")) {
+        if (argc < 2) {
+            goto conn_usage;
+        }
+        i = strlen(argv[2]);
+        if (i >= sizeof(wi->wi_ssid)) {
+            goto conn_usage;
+        }
+        if (argc > 2) {
+            i = strlen(argv[2]);
+            if (i >= sizeof(wi->wi_key)) {
+                goto conn_usage;
+            }
+            strcpy(wi->wi_key, argv[3]);
+        }
+        strcpy(wi->wi_ssid, argv[2]);
+        if (wifi_connect(wi)) {
+conn_usage:
+            console_printf("%s %s [<ssid> [<key>]]\n",
+              argv[0], argv[1]);
+        }
+    }
+    return 0;
+}
+
+struct shell_cmd wifi_cli_cmd = {
+    .sc_cmd = "wifi",
+    .sc_cmd_func = wifi_cli
+};
+
+#endif
diff --git a/net/nimble/host/src/ble_hci_util_priv.h b/libs/wifi_mgmt/src/wifi_priv.h
index f4455147..dfba81da 100644
--- a/net/nimble/host/src/ble_hci_util_priv.h
+++ b/libs/wifi_mgmt/src/wifi_priv.h
@@ -17,14 +17,11 @@
  * under the License.
  */
 
-#ifndef H_BLE_HCI_UTIL_
-#define H_BLE_HCI_UTIL_
-
-int ble_hci_util_read_adv_tx_pwr(int8_t *out_pwr);
-int ble_hci_util_rand(void *dst, int len);
-int ble_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi);
-int ble_hs_util_set_random_addr(uint8_t *addr);
-int ble_hci_util_set_data_len(uint16_t conn_handle, uint16_t tx_octets,
-                              uint16_t tx_time);
+#ifndef __WIFI_PRIV_H__
+#define __WIFI_PRIV_H__
 
+#ifdef SHELL_PRESENT
+extern struct shell_cmd wifi_cli_cmd;
 #endif
+
+#endif /* __WIFI_PRIV_H__ */
diff --git a/net/nimble/controller/include/controller/ble_ll.h b/net/nimble/controller/include/controller/ble_ll.h
index 81cba53f..7865cf89 100644
--- a/net/nimble/controller/include/controller/ble_ll.h
+++ b/net/nimble/controller/include/controller/ble_ll.h
@@ -22,6 +22,7 @@
 
 #include "stats/stats.h"
 #include "hal/hal_cputime.h"
+#include "os/os_eventq.h"
 #include "nimble/nimble_opt.h"
 
 /* Controller revision. */
@@ -85,6 +86,7 @@ STATS_SECT_START(ble_ll_stats)
     STATS_SECT_ENTRY(hci_events_sent)
     STATS_SECT_ENTRY(bad_ll_state)
     STATS_SECT_ENTRY(bad_acl_hdr)
+    STATS_SECT_ENTRY(no_bufs)
     STATS_SECT_ENTRY(rx_adv_pdu_crc_ok)
     STATS_SECT_ENTRY(rx_adv_pdu_crc_err)
     STATS_SECT_ENTRY(rx_adv_bytes_crc_ok)
@@ -311,13 +313,24 @@ int ble_ll_is_our_devaddr(uint8_t *addr, int addr_type);
  */
 void ble_ll_acl_data_in(struct os_mbuf *txpkt);
 
+/**
+ * Allocate a pdu (chain) for reception.
+ *
+ * @param len Length of PDU. This includes the PDU header as well as payload.
+ * Does not include MIC if encrypted.
+ *
+ * @return struct os_mbuf* Pointer to mbuf chain to hold received packet
+ */
+struct os_mbuf *ble_ll_rxpdu_alloc(uint16_t len);
+
 /*--- PHY interfaces ---*/
+struct ble_mbuf_hdr;
+
 /* Called by the PHY when a packet has started */
-int ble_ll_rx_start(struct os_mbuf *rxpdu, uint8_t chan);
+int ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *hdr);
 
 /* Called by the PHY when a packet reception ends */
-struct ble_mbuf_hdr;
-int ble_ll_rx_end(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *ble_hdr);
+int ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
 
 /*--- Controller API ---*/
 void ble_ll_mbuf_init(struct os_mbuf *m, uint8_t pdulen, uint8_t hdr);
@@ -331,6 +344,9 @@ uint8_t ble_ll_state_get(void);
 /* Send an event to LL task */
 void ble_ll_event_send(struct os_event *ev);
 
+/* Hand received pdu's to LL task  */
+void ble_ll_rx_pdu_in(struct os_mbuf *rxpdu);
+
 /* Set random address */
 int ble_ll_set_random_addr(uint8_t *addr);
 
@@ -382,6 +398,7 @@ int ble_ll_rand_start(void);
 #define BLE_LL_LOG_ID_CONN_END          (30)
 #define BLE_LL_LOG_ID_ADV_TXBEG         (50)
 #define BLE_LL_LOG_ID_ADV_TXDONE        (60)
+#define BLE_LL_LOG_ID_SCHED             (80)
 
 #ifdef BLE_LL_LOG
 void ble_ll_log(uint8_t id, uint8_t arg8, uint16_t arg16, uint32_t arg32);
diff --git a/net/nimble/controller/include/controller/ble_ll_hci.h b/net/nimble/controller/include/controller/ble_ll_hci.h
index 47daa765..e3a7e3e5 100644
--- a/net/nimble/controller/include/controller/ble_ll_hci.h
+++ b/net/nimble/controller/include/controller/ble_ll_hci.h
@@ -24,6 +24,9 @@
 #define BLE_LL_SUPP_CMD_LEN (36)
 extern const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN];
 
+/* The largest event the controller will send. */
+#define BLE_LL_MAX_EVT_LEN  (70)
+
 /*
  * This determines the number of outstanding commands allowed from the
  * host to the controller.
diff --git a/net/nimble/controller/include/controller/ble_ll_resolv.h b/net/nimble/controller/include/controller/ble_ll_resolv.h
index 251543b2..6c63c79f 100644
--- a/net/nimble/controller/include/controller/ble_ll_resolv.h
+++ b/net/nimble/controller/include/controller/ble_ll_resolv.h
@@ -23,15 +23,18 @@
 /*
  * An entry in the resolving list.
  *      The identity address is stored in little endian format.
+ *      The local rpa is stored in little endian format.
  *      The IRKs are stored in big endian format.
  */
 struct ble_ll_resolv_entry
 {
-    uint8_t rl_reserved;
     uint8_t rl_addr_type;
-    uint8_t rl_identity_addr[BLE_DEV_ADDR_LEN];
+    uint8_t rl_local_rpa_set;
+    uint16_t rl_reserved;
     uint8_t rl_local_irk[16];
     uint8_t rl_peer_irk[16];
+    uint8_t rl_identity_addr[BLE_DEV_ADDR_LEN];
+    uint8_t rl_local_rpa[BLE_DEV_ADDR_LEN];
 };
 
 extern struct ble_ll_resolv_entry g_ble_ll_resolv_list[];
diff --git a/net/nimble/controller/include/controller/ble_ll_scan.h b/net/nimble/controller/include/controller/ble_ll_scan.h
index 7dafcee2..3b935f43 100644
--- a/net/nimble/controller/include/controller/ble_ll_scan.h
+++ b/net/nimble/controller/include/controller/ble_ll_scan.h
@@ -96,7 +96,7 @@ void ble_ll_scan_init(void);
 void ble_ll_scan_reset(void);
 
 /* Called when Link Layer starts to receive a PDU and is in scanning state */
-int ble_ll_scan_rx_isr_start(uint8_t pdu_type, struct os_mbuf *rxpdu);
+int ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint8_t *rxflags);
 
 /* Called when Link Layer has finished receiving a PDU while scanning */
 int ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok);
diff --git a/net/nimble/controller/include/controller/ble_phy.h b/net/nimble/controller/include/controller/ble_phy.h
index 1333c192..585c2dc9 100644
--- a/net/nimble/controller/include/controller/ble_phy.h
+++ b/net/nimble/controller/include/controller/ble_phy.h
@@ -97,6 +97,9 @@ int ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans);
 /* Place the PHY into receive mode */
 int ble_phy_rx(void);
 
+/* Copies the received PHY buffer into the allocated pdu */
+void ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu);
+
 /* Get an RSSI reading */
 int ble_phy_rssi_get(void);
 
diff --git a/net/nimble/controller/pkg.yml b/net/nimble/controller/pkg.yml
index 07a9d560..8e52aba9 100644
--- a/net/nimble/controller/pkg.yml
+++ b/net/nimble/controller/pkg.yml
@@ -25,10 +25,14 @@ pkg.keywords:
     - ble
     - bluetooth
 
-pkg.req_apis: ble_driver
+pkg.req_apis:
+    - ble_driver
+    - ble_transport
+
 pkg.deps:
     - libs/os
     - sys/stats
     - net/nimble
+
 pkg.features:
     - BLE_DEVICE
diff --git a/net/nimble/controller/src/ble_ll.c b/net/nimble/controller/src/ble_ll.c
index 3a09e3e9..3beb28ea 100644
--- a/net/nimble/controller/src/ble_ll.c
+++ b/net/nimble/controller/src/ble_ll.c
@@ -26,6 +26,7 @@
 #include "nimble/ble.h"
 #include "nimble/nimble_opt.h"
 #include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
 #include "controller/ble_hw.h"
 #include "controller/ble_phy.h"
 #include "controller/ble_ll.h"
@@ -150,6 +151,7 @@ STATS_NAME_START(ble_ll_stats)
     STATS_NAME(ble_ll_stats, hci_events_sent)
     STATS_NAME(ble_ll_stats, bad_ll_state)
     STATS_NAME(ble_ll_stats, bad_acl_hdr)
+    STATS_NAME(ble_ll_stats, no_bufs)
     STATS_NAME(ble_ll_stats, rx_adv_pdu_crc_ok)
     STATS_NAME(ble_ll_stats, rx_adv_pdu_crc_err)
     STATS_NAME(ble_ll_stats, rx_adv_bytes_crc_ok)
@@ -182,6 +184,9 @@ STATS_NAME_END(ble_ll_stats)
 struct os_task g_ble_ll_task;
 os_stack_t g_ble_ll_stack[BLE_LL_STACK_SIZE];
 
+struct os_mempool g_ble_ll_hci_ev_pool;
+static void *ble_ll_hci_os_event_buf;
+
 /* XXX: temporary logging until we transition to real logging */
 #ifdef BLE_LL_LOG
 struct ble_ll_log
@@ -258,6 +263,68 @@ ble_ll_count_rx_adv_pdus(uint8_t pdu_type)
     }
 }
 
+/**
+ * Allocate a pdu (chain) for reception.
+ *
+ * @param len
+ *
+ * @return struct os_mbuf*
+ */
+struct os_mbuf *
+ble_ll_rxpdu_alloc(uint16_t len)
+{
+    uint16_t mb_bytes;
+    struct os_mbuf *m;
+    struct os_mbuf *n;
+    struct os_mbuf *p;
+    struct os_mbuf_pkthdr *pkthdr;
+
+    p = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
+    if (!p) {
+        goto rxpdu_alloc_exit;
+    }
+
+    /* Set packet length */
+    pkthdr = OS_MBUF_PKTHDR(p);
+    pkthdr->omp_len = len;
+
+    /*
+     * NOTE: first mbuf in chain will have data pre-pended to it so we adjust
+     * m_data by a word.
+     */
+    p->om_data += 4;
+    mb_bytes = (p->om_omp->omp_databuf_len - p->om_pkthdr_len - 4);
+
+    if (mb_bytes < len) {
+        n = p;
+        len -= mb_bytes;
+        while (len) {
+            m = os_msys_get(len, 0);
+            if (!m) {
+                os_mbuf_free_chain(p);
+                p = NULL;
+                goto rxpdu_alloc_exit;
+            }
+            /* Chain new mbuf to existing chain */
+            SLIST_NEXT(n, om_next) = m;
+            n = m;
+            mb_bytes = m->om_omp->omp_databuf_len;
+            if (mb_bytes >= len) {
+                len = 0;
+            } else {
+                len -= mb_bytes;
+            }
+        }
+    }
+
+
+rxpdu_alloc_exit:
+    if (!p) {
+        STATS_INC(ble_ll_stats, no_bufs);
+    }
+    return p;
+}
+
 int
 ble_ll_chk_txrx_octets(uint16_t octets)
 {
@@ -662,25 +729,21 @@ ble_ll_acl_data_in(struct os_mbuf *txpkt)
  *   > 0: Continue to receive frame and go from rx to tx when done
  */
 int
-ble_ll_rx_start(struct os_mbuf *rxpdu, uint8_t chan)
+ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *rxhdr)
 {
     int rc;
     uint8_t pdu_type;
-    uint8_t *rxbuf;
-    struct ble_mbuf_hdr *ble_hdr;
 
-    ble_ll_log(BLE_LL_LOG_ID_RX_START, chan, 0, (uint32_t)rxpdu);
+    ble_ll_log(BLE_LL_LOG_ID_RX_START, chan, 0, rxhdr->beg_cputime);
 
     /* Check channel type */
-    rxbuf = rxpdu->om_data;
     if (chan < BLE_PHY_NUM_DATA_CHANS) {
         /*
          * Data channel pdu. We should be in CONNECTION state with an
          * ongoing connection
          */
         if (g_ble_ll_data.ll_state == BLE_LL_STATE_CONNECTION) {
-            ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
-            rc = ble_ll_conn_rx_isr_start(ble_hdr, ble_phy_access_addr_get());
+            rc = ble_ll_conn_rx_isr_start(rxhdr, ble_phy_access_addr_get());
         } else {
             STATS_INC(ble_ll_stats, bad_ll_state);
             rc = 0;
@@ -704,7 +767,7 @@ ble_ll_rx_start(struct os_mbuf *rxpdu, uint8_t chan)
         }
         break;
     case BLE_LL_STATE_SCANNING:
-        rc = ble_ll_scan_rx_isr_start(pdu_type, rxpdu);
+        rc = ble_ll_scan_rx_isr_start(pdu_type, &rxhdr->rxinfo.flags);
         break;
     case BLE_LL_STATE_CONNECTION:
         /* Should not occur */
@@ -726,8 +789,8 @@ ble_ll_rx_start(struct os_mbuf *rxpdu, uint8_t chan)
  *
  * NOTE: Called from interrupt context!
  *
- * @param rxpdu Pointer to received PDU
- *        ble_hdr Pointer to BLE header of received mbuf
+ * @param rxbuf Pointer to received PDU data
+ *        rxhdr Pointer to BLE header of received mbuf
  *
  * @return int
  *       < 0: Disable the phy after reception.
@@ -735,7 +798,7 @@ ble_ll_rx_start(struct os_mbuf *rxpdu, uint8_t chan)
  *       > 0: Do not disable PHY as that has already been done.
  */
 int
-ble_ll_rx_end(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *ble_hdr)
+ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
 {
     int rc;
     int badpkt;
@@ -743,40 +806,23 @@ ble_ll_rx_end(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *ble_hdr)
     uint8_t len;
     uint8_t chan;
     uint8_t crcok;
-    uint16_t mblen;
-    uint8_t *rxbuf;
-
-    /* Set the rx buffer pointer to the start of the received data */
-    rxbuf = rxpdu->om_data;
+    struct os_mbuf *rxpdu;
 
     /* Get channel and CRC status from BLE header */
-    chan = ble_hdr->rxinfo.channel;
-    crcok = BLE_MBUF_HDR_CRC_OK(ble_hdr);
+    chan = rxhdr->rxinfo.channel;
+    crcok = BLE_MBUF_HDR_CRC_OK(rxhdr);
 
     ble_ll_log(BLE_LL_LOG_ID_RX_END, rxbuf[0],
-               ((uint16_t)ble_hdr->rxinfo.flags << 8) | rxbuf[1],
-               (BLE_MBUF_HDR_PTR(rxpdu))->beg_cputime);
+               ((uint16_t)rxhdr->rxinfo.flags << 8) | rxbuf[1],
+               rxhdr->beg_cputime);
 
     /* Check channel type */
     if (chan < BLE_PHY_NUM_DATA_CHANS) {
-        /* Set length in the received PDU */
-        mblen = rxbuf[1] + BLE_LL_PDU_HDR_LEN;
-        OS_MBUF_PKTHDR(rxpdu)->omp_len = mblen;
-        rxpdu->om_len = mblen;
-
-        /*
-         * NOTE: this looks a bit odd, and it is, but for now we place the
-         * received PDU on the Link Layer task before calling the rx end
-         * function. We do this to guarantee connection event end ordering
-         * and receive PDU processing.
-         */
-        ble_ll_rx_pdu_in(rxpdu);
-
         /*
          * Data channel pdu. We should be in CONNECTION state with an
          * ongoing connection.
          */
-        rc = ble_ll_conn_rx_isr_end(rxpdu);
+        rc = ble_ll_conn_rx_isr_end(rxbuf, rxhdr);
         return rc;
     }
 
@@ -784,14 +830,9 @@ ble_ll_rx_end(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *ble_hdr)
     pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK;
     len = rxbuf[1] & BLE_ADV_PDU_HDR_LEN_MASK;
 
-    /* Setup the mbuf lengths */
-    mblen = len + BLE_LL_PDU_HDR_LEN;
-    OS_MBUF_PKTHDR(rxpdu)->omp_len = mblen;
-    rxpdu->om_len = mblen;
-
     /* If the CRC checks, make sure lengths check! */
+    badpkt = 0;
     if (crcok) {
-        badpkt = 0;
         switch (pdu_type) {
         case BLE_ADV_PDU_TYPE_SCAN_REQ:
         case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND:
@@ -820,22 +861,32 @@ ble_ll_rx_end(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *ble_hdr)
         /* If this is a malformed packet, just kill it here */
         if (badpkt) {
             STATS_INC(ble_ll_stats, rx_adv_malformed_pkts);
-            os_mbuf_free_chain(rxpdu);
-            rxpdu = NULL;
         }
     }
 
-
     /* Hand packet to the appropriate state machine (if crc ok) */
-    switch (BLE_MBUF_HDR_RX_STATE(ble_hdr)) {
+    rxpdu = NULL;
+    switch (BLE_MBUF_HDR_RX_STATE(rxhdr)) {
     case BLE_LL_STATE_ADV:
+        if (!badpkt) {
+            rxpdu = ble_ll_rxpdu_alloc(len + BLE_LL_PDU_HDR_LEN);
+            if (rxpdu) {
+                ble_phy_rxpdu_copy(rxbuf, rxpdu);
+            }
+        }
         rc = ble_ll_adv_rx_isr_end(pdu_type, rxpdu, crcok);
         break;
     case BLE_LL_STATE_SCANNING:
+        if (!badpkt) {
+            rxpdu = ble_ll_rxpdu_alloc(len + BLE_LL_PDU_HDR_LEN);
+            if (rxpdu) {
+                ble_phy_rxpdu_copy(rxbuf, rxpdu);
+            }
+        }
         rc = ble_ll_scan_rx_isr_end(rxpdu, crcok);
         break;
     case BLE_LL_STATE_INITIATING:
-        rc = ble_ll_init_rx_isr_end(rxpdu, crcok);
+        rc = ble_ll_init_rx_isr_end(rxbuf, crcok, rxhdr);
         break;
     default:
         rc = -1;
@@ -1129,6 +1180,16 @@ ble_ll_init(uint8_t ll_task_prio, uint8_t num_acl_pkts, uint16_t acl_pkt_size)
     cputime_timer_init(&g_ble_ll_data.ll_wfr_timer, ble_ll_wfr_timer_exp,
                        NULL);
 
+    ble_ll_hci_os_event_buf = malloc(
+        OS_MEMPOOL_BYTES(16, sizeof (struct os_event)));
+    assert(ble_ll_hci_os_event_buf != NULL);
+
+    /* Create memory pool of OS events */
+    rc = os_mempool_init(&g_ble_ll_hci_ev_pool, 16,
+                         sizeof (struct os_event), ble_ll_hci_os_event_buf,
+                         "g_ble_ll_hci_ev_pool");
+    assert(rc == 0);
+
     /* Initialize LL HCI */
     ble_ll_hci_init();
 
@@ -1182,6 +1243,9 @@ ble_ll_init(uint8_t ll_task_prio, uint8_t num_acl_pkts, uint16_t acl_pkt_size)
                             STATS_SIZE_INIT_PARMS(ble_ll_stats, STATS_SIZE_32),
                             STATS_NAME_INIT_PARMS(ble_ll_stats),
                             "ble_ll");
+
+    ble_hci_trans_cfg_ll(ble_ll_hci_cmd_rx, NULL,
+                                    ble_ll_hci_acl_rx, NULL);
     return rc;
 }
 
diff --git a/net/nimble/controller/src/ble_ll_adv.c b/net/nimble/controller/src/ble_ll_adv.c
index 1f43a5d2..0c6df3b1 100644
--- a/net/nimble/controller/src/ble_ll_adv.c
+++ b/net/nimble/controller/src/ble_ll_adv.c
@@ -121,6 +121,23 @@ struct ble_ll_adv_sm g_ble_ll_adv_sm;
 
 
 #if (BLE_LL_CFG_FEAT_LL_PRIVACY == 1)
+/**
+ * Called to change advertisers ADVA and INITA (for directed advertisements)
+ * as an advertiser needs to adhere to the resolvable private address generation
+ * timer.
+ *
+ * NOTE: the resolvable private address code uses its own timer to regenerate
+ * local resolvable private addresses. The advertising code uses its own
+ * timer to reset the INITA (for directed advertisements). This code also sets
+ * the appropriate txadd and rxadd bits that will go into the advertisement.
+ *
+ * Another thing to note: it is possible that an IRK is all zeroes in the
+ * resolving list. That is why we need to check if the generated address is
+ * in fact a RPA as a resolving list entry with all zeroes will use the
+ * identity address (which may be a private address or public).
+ *
+ * @param advsm
+ */
 void
 ble_ll_adv_chk_rpa_timeout(struct ble_ll_adv_sm *advsm)
 {
diff --git a/net/nimble/controller/src/ble_ll_conn.c b/net/nimble/controller/src/ble_ll_conn.c
index ca94310a..e3c40e26 100644
--- a/net/nimble/controller/src/ble_ll_conn.c
+++ b/net/nimble/controller/src/ble_ll_conn.c
@@ -25,6 +25,7 @@
 #include "nimble/ble.h"
 #include "nimble/nimble_opt.h"
 #include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
 #include "ble/xcvr.h"
 #include "controller/ble_ll.h"
 #include "controller/ble_ll_hci.h"
@@ -103,9 +104,6 @@ extern void bletest_completed_pkt(uint16_t handle);
  *  are hosed. Well, anchor point can get really messed up!
  */
 
-/* XXX: this does not belong here! Move to transport? */
-extern int ble_hs_rx_data(struct os_mbuf *om);
-
 /*
  * The amount of time that we will wait to hear the start of a receive
  * packet after we have transmitted a packet. This time is at least
@@ -218,13 +216,13 @@ STATS_NAME_END(ble_ll_conn_stats)
  * Called to determine if the received PDU is an empty PDU or not.
  */
 static int
-ble_ll_conn_is_empty_pdu(struct os_mbuf *rxpdu)
+ble_ll_conn_is_empty_pdu(uint8_t *rxbuf)
 {
     int rc;
     uint8_t llid;
 
-    llid = rxpdu->om_data[0] & BLE_LL_DATA_HDR_LLID_MASK;
-    if ((llid == BLE_LL_LLID_DATA_FRAG) && (rxpdu->om_data[1] == 0)) {
+    llid = rxbuf[0] & BLE_LL_DATA_HDR_LLID_MASK;
+    if ((llid == BLE_LL_LLID_DATA_FRAG) && (rxbuf[1] == 0)) {
         rc = 1;
     } else {
         rc = 0;
@@ -287,7 +285,7 @@ ble_ll_conn_get_ce_end_time(void)
  *  standby and set the current state machine pointer to NULL.
  */
 static void
-ble_ll_conn_current_sm_over(void)
+ble_ll_conn_current_sm_over(struct ble_ll_conn_sm *connsm)
 {
     /* Disable the PHY */
     ble_phy_disable();
@@ -300,6 +298,15 @@ ble_ll_conn_current_sm_over(void)
 
     /* Set current LL connection to NULL */
     g_ble_ll_conn_cur_sm = NULL;
+
+    /*
+     * NOTE: the connection state machine may be NULL if we are calling
+     * this when we are ending the connection. In that case, there is no
+     * need to post to the LL the connection event end event
+     */
+    if (connsm) {
+        ble_ll_event_send(&connsm->conn_ev_end);
+    }
 }
 
 /**
@@ -462,8 +469,8 @@ ble_ll_conn_calc_access_addr(void)
         transitions = 0;
         consecutive = 0;
         mask = 0x00000001;
-        prev_bit = aa & mask;
         while (mask < 0x80000000) {
+            prev_bit = aa & mask;
             mask <<= 1;
             if (mask & aa) {
                 if (prev_bit == 0) {
@@ -573,11 +580,8 @@ ble_ll_conn_wfr_timer_exp(void)
     struct ble_ll_conn_sm *connsm;
 
     connsm = g_ble_ll_conn_cur_sm;
-    ble_ll_conn_current_sm_over();
-    if (connsm) {
-        ble_ll_event_send(&connsm->conn_ev_end);
-        STATS_INC(ble_ll_conn_stats, wfr_expirations);
-    }
+    ble_ll_conn_current_sm_over(connsm);
+    STATS_INC(ble_ll_conn_stats, wfr_expirations);
 }
 
 /**
@@ -594,10 +598,8 @@ ble_ll_conn_wait_txend(void *arg)
 {
     struct ble_ll_conn_sm *connsm;
 
-    ble_ll_conn_current_sm_over();
-
     connsm = (struct ble_ll_conn_sm *)arg;
-    ble_ll_event_send(&connsm->conn_ev_end);
+    ble_ll_conn_current_sm_over(connsm);
 }
 
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
@@ -631,8 +633,7 @@ ble_ll_conn_txend_encrypt(void *arg)
 
     connsm = (struct ble_ll_conn_sm *)arg;
     CONN_F_ENCRYPTED(connsm) = 1;
-    ble_ll_conn_current_sm_over();
-    ble_ll_event_send(&connsm->conn_ev_end);
+    ble_ll_conn_current_sm_over(connsm);
 }
 
 static void
@@ -642,8 +643,7 @@ ble_ll_conn_rxend_unencrypt(void *arg)
 
     connsm = (struct ble_ll_conn_sm *)arg;
     CONN_F_ENCRYPTED(connsm) = 0;
-    ble_ll_conn_current_sm_over();
-    ble_ll_event_send(&connsm->conn_ev_end);
+    ble_ll_conn_current_sm_over(connsm);
 }
 
 static void
@@ -1904,6 +1904,7 @@ ble_ll_conn_event_end(void *arg)
      * The way this works is that whenever the timer expires it just gets reset
      * and we send the autheticated payload timeout event. Note that this timer
      * should run even when encryption is paused.
+     * XXX: what should be here? Was there code here that got deleted?
      */
 #endif
 
@@ -2195,7 +2196,8 @@ ble_ll_init_rx_pkt_in(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr)
  *       > 0: Do not disable PHY as that has already been done.
  */
 int
-ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
+ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok,
+                       struct ble_mbuf_hdr *ble_hdr)
 {
     int rc;
     int resolved;
@@ -2208,24 +2210,18 @@ ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
     uint8_t *adv_addr;
     uint8_t *peer;
     uint8_t *init_addr;
-    uint8_t *rxbuf;
     uint8_t pyld_len;
     uint8_t inita_is_rpa;
     uint32_t endtime;
-    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf *rxpdu;
     struct ble_ll_conn_sm *connsm;
 
     /*
      * We have to restart receive if we cant hand up pdu. We return 0 so that
      * the phy does not get disabled.
      */
-    if (!rxpdu) {
-        ble_phy_disable();
-        ble_phy_rx();
-        return 0;
-    }
-
     rc = -1;
+    pyld_len = rxbuf[1] & BLE_ADV_PDU_HDR_LEN_MASK;
     if (!crcok) {
         goto init_rx_isr_exit;
     }
@@ -2234,10 +2230,7 @@ ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
     connsm = g_ble_ll_conn_create_sm;
 
     /* Only interested in ADV IND or ADV DIRECT IND */
-    rxbuf = rxpdu->om_data;
     pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK;
-    pyld_len = rxbuf[1] & BLE_ADV_PDU_HDR_LEN_MASK;
-
     inita_is_rpa = 0;
 
     switch (pdu_type) {
@@ -2285,7 +2278,6 @@ ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
         }
 
         index = -1;
-        ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
         peer = adv_addr;
         peer_addr_type = addr_type;
 
@@ -2303,7 +2295,7 @@ ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
                 resolved = 1;
             } else {
                 if (chk_wl) {
-                    return -1;
+                    goto init_rx_isr_exit;
                 }
             }
         }
@@ -2312,12 +2304,12 @@ ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
         /* Check filter policy */
         if (chk_wl) {
             if (!ble_ll_whitelist_match(peer, peer_addr_type, resolved)) {
-                return -1;
+                goto init_rx_isr_exit;
             }
         } else {
             /* Must match the connection address */
             if (!ble_ll_conn_is_peer_adv(addr_type, adv_addr, index)) {
-                return -1;
+                goto init_rx_isr_exit;
             }
         }
         ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_DEVMATCH;
@@ -2330,7 +2322,7 @@ ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
             if ((index < 0) ||
                 !ble_ll_resolv_rpa(init_addr,
                                    g_ble_ll_resolv_list[index].rl_local_irk)) {
-                return -1;
+                goto init_rx_isr_exit;
             }
         }
 
@@ -2344,6 +2336,8 @@ ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
             if (!rc) {
                 CONN_F_CONN_REQ_TXD(connsm) = 1;
                 STATS_INC(ble_ll_conn_stats, conn_req_txd);
+            } else {
+                ble_ll_sched_rmv_elem(&connsm->conn_sch);
             }
         } else {
             /* Count # of times we could not set schedule */
@@ -2352,9 +2346,24 @@ ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
     }
 
 init_rx_isr_exit:
+    /*
+     * We have to restart receive if we cant hand up pdu. We return 0 so that
+     * the phy does not get disabled.
+     */
+    rxpdu = ble_ll_rxpdu_alloc(pyld_len + BLE_LL_PDU_HDR_LEN);
+    if (rxpdu == NULL) {
+        ble_phy_disable();
+        ble_phy_rx();
+        rc = 0;
+    } else {
+        ble_phy_rxpdu_copy(rxbuf, rxpdu);
+        ble_ll_rx_pdu_in(rxpdu);
+    }
+
     if (rc) {
         ble_ll_state_set(BLE_LL_STATE_STANDBY);
     }
+
     return rc;
 }
 
@@ -2377,7 +2386,7 @@ ble_ll_conn_timeout(struct ble_ll_conn_sm *connsm, uint8_t ble_err)
     was_current = 0;
     OS_ENTER_CRITICAL(sr);
     if (g_ble_ll_conn_cur_sm == connsm) {
-        ble_ll_conn_current_sm_over();
+        ble_ll_conn_current_sm_over(NULL);
         was_current = 1;
     }
     OS_EXIT_CRITICAL(sr);
@@ -2449,7 +2458,6 @@ ble_ll_conn_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa)
             connsm->anchor_point = connsm->last_anchor_point;
         }
     }
-
     return 1;
 }
 
@@ -2569,7 +2577,7 @@ ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
                     acl_hdr = (acl_hdr << 12) | connsm->conn_handle;
                     htole16(rxbuf, acl_hdr);
                     htole16(rxbuf + 2, acl_len);
-                    ble_hs_rx_data(rxpdu);
+                    ble_hci_trans_ll_acl_tx(rxpdu);
                 }
 
                 /* NOTE: we dont free the mbuf since we handed it off! */
@@ -2601,7 +2609,7 @@ conn_rx_data_pdu_end:
  *       > 0: Do not disable PHY as that has already been done.
  */
 int
-ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu)
+ble_ll_conn_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
 {
     int rc;
     int is_ctrl;
@@ -2617,9 +2625,22 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu)
     uint32_t endtime;
     struct os_mbuf *txpdu;
     struct ble_ll_conn_sm *connsm;
-    struct ble_mbuf_hdr *rxhdr;
+    struct os_mbuf *rxpdu;
     struct ble_mbuf_hdr *txhdr;
 
+    /* Retrieve the header and payload length */
+    hdr_byte = rxbuf[0];
+    rx_pyld_len = rxbuf[1];
+
+    /*
+     * We need to attempt to allocate a buffer here. The reason we do this
+     * now is that we should not ack the packet if we have no receive
+     * buffers available. We want to free up our transmit PDU if it was
+     * acked, but we should not ack the received frame if we cant hand it up.
+     * NOTE: we hand up empty pdu's to the LL task!
+     */
+    rxpdu = ble_ll_rxpdu_alloc(rx_pyld_len + BLE_LL_PDU_HDR_LEN);
+
     /*
      * We should have a current connection state machine. If we dont, we just
      * hand the packet to the higher layer to count it.
@@ -2631,11 +2652,6 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu)
         goto conn_exit;
     }
 
-    /* Set the handle in the ble mbuf header */
-    rxhdr = BLE_MBUF_HDR_PTR(rxpdu);
-    hdr_byte = rxpdu->om_data[0];
-    rx_pyld_len = rxpdu->om_data[1];
-
     /*
      * Check the packet CRC. A connection event can continue even if the
      * received PDU does not pass the CRC check. If we receive two consecutive
@@ -2661,15 +2677,13 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu)
         /* Reset consecutively received bad crcs (since this one was good!) */
         connsm->cons_rxd_bad_crc = 0;
 
-        /* Check for valid LLID before proceeding. */
+        /*
+         * Check for valid LLID before proceeding. We have seen some weird
+         * things with the PHY where the CRC is OK but we dont have a valid
+         * LLID. This should really never happen but if it does we will just
+         * bail. An error stat will get incremented at the LL.
+         */
         if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == 0) {
-            /*
-             * XXX: for now, just exit since we dont trust the length
-             * and may erroneously adjust anchor. Once we fix the anchor
-             * point issue we need to decide what to do on bad llid. Note
-             * that an error stat gets counted at the LL
-             */
-            reply = 0;
             goto conn_exit;
         }
 
@@ -2682,10 +2696,10 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu)
          */
         hdr_sn = hdr_byte & BLE_LL_DATA_HDR_SN_MASK;
         conn_nesn = connsm->next_exp_seqnum;
-        if ((hdr_sn && conn_nesn) || (!hdr_sn && !conn_nesn)) {
+        if (rxpdu && ((hdr_sn && conn_nesn) || (!hdr_sn && !conn_nesn))) {
             connsm->next_exp_seqnum ^= 1;
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
-            if (CONN_F_ENCRYPTED(connsm) && !ble_ll_conn_is_empty_pdu(rxpdu)) {
+            if (CONN_F_ENCRYPTED(connsm) && !ble_ll_conn_is_empty_pdu(rxbuf)) {
                 ++connsm->enc_data.rx_pkt_cntr;
             }
 #endif
@@ -2778,13 +2792,13 @@ chk_rx_terminate_ind:
         is_ctrl = 0;
         if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) {
             is_ctrl = 1;
-            opcode = rxpdu->om_data[2];
+            opcode = rxbuf[2];
         }
 
         /* If we received a terminate IND, we must set some flags */
         if (is_ctrl && (opcode == BLE_LL_CTRL_TERMINATE_IND)) {
             connsm->csmflags.cfbit.terminate_ind_rxd = 1;
-            connsm->rxd_disconnect_reason = rxpdu->om_data[3];
+            connsm->rxd_disconnect_reason = rxbuf[3];
             reply = 1;
         } else if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
             reply = CONN_F_LAST_TXD_MD(connsm) || (hdr_byte & BLE_LL_DATA_HDR_MD_MASK);
@@ -2810,12 +2824,15 @@ chk_rx_terminate_ind:
     }
 
 conn_exit:
+    /* Copy the received pdu and hand it up */
+    if (rxpdu) {
+        ble_phy_rxpdu_copy(rxbuf, rxpdu);
+        ble_ll_rx_pdu_in(rxpdu);
+    }
+
     /* Send link layer a connection end event if over */
     if (rc) {
-        ble_ll_conn_current_sm_over();
-        if (connsm) {
-            ble_ll_event_send(&connsm->conn_ev_end);
-        }
+        ble_ll_conn_current_sm_over(connsm);
     }
 
     return rc;
diff --git a/net/nimble/controller/src/ble_ll_conn_hci.c b/net/nimble/controller/src/ble_ll_conn_hci.c
index e1bc978f..d7001f89 100644
--- a/net/nimble/controller/src/ble_ll_conn_hci.c
+++ b/net/nimble/controller/src/ble_ll_conn_hci.c
@@ -25,6 +25,7 @@
 #include "nimble/ble.h"
 #include "nimble/nimble_opt.h"
 #include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
 #include "controller/ble_ll.h"
 #include "controller/ble_ll_hci.h"
 #include "controller/ble_ll_conn.h"
@@ -140,7 +141,7 @@ ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status)
     enh_enabled = ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE);
 
     if (enabled || enh_enabled) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             /* Put common elements in event */
             evbuf[0] = BLE_HCI_EVCODE_LE_META;
@@ -202,22 +203,25 @@ ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status)
     }
 }
 
+
 /**
  * Called to create and send the number of completed packets event to the
  * host.
  *
- * Because of the ridiculous spec, all the connection handles are contiguous and
- * then all the completed packets are contiguous. In order to avoid multiple
- * passes through the connection list or allocating a large stack variable or
- * malloc, I just use the event buffer and place the completed packets after
- * the last possible handle. I then copy the completed packets to make it
- * contiguous with the handles.
- *
- * @param connsm
+ * Because of the ridiculous spec, all the connection handles are contiguous
+ * and then all the completed packets are contiguous. In order to avoid
+ * multiple passes through the connection list or allocating a large stack
+ * variable or malloc, I just use the event buffer and place the completed
+ * packets after the last possible handle. I then copy the completed packets
+ * to make it contiguous with the handles.
  */
 void
 ble_ll_conn_num_comp_pkts_event_send(void)
 {
+    /** The maximum number of handles that will fit in an event buffer. */
+    static const int max_handles =
+        (BLE_LL_MAX_EVT_LEN - BLE_HCI_EVENT_HDR_LEN - 1) / 4;
+
     int event_sent;
     uint8_t *evbuf;
     uint8_t *handle_ptr;
@@ -246,13 +250,13 @@ ble_ll_conn_num_comp_pkts_event_send(void)
             (connsm->completed_pkts || !STAILQ_EMPTY(&connsm->conn_txq))) {
             /* If no buffer, get one, If cant get one, leave. */
             if (!evbuf) {
-                evbuf = os_memblock_get(&g_hci_cmd_pool);
+                evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
                 if (!evbuf) {
                     break;
                 }
                 handles = 0;
                 handle_ptr = evbuf + 3;
-                comp_pkt_ptr = handle_ptr + (sizeof(uint16_t) * 60);
+                comp_pkt_ptr = handle_ptr + (sizeof(uint16_t) * max_handles);
             }
 
             /* Add handle and complete packets */
@@ -263,11 +267,8 @@ ble_ll_conn_num_comp_pkts_event_send(void)
             comp_pkt_ptr += sizeof(uint16_t);
             ++handles;
 
-            /*
-             * The event buffer should fit at least 255 bytes so this means we
-             * can fit up to 60 handles per event (a little more but who cares).
-             */
-            if (handles == 60) {
+            /* Send now if the buffer is full. */
+            if (handles == max_handles) {
                 evbuf[0] = BLE_HCI_EVCODE_NUM_COMP_PKTS;
                 evbuf[1] = (handles * 2 * sizeof(uint16_t)) + 1;
                 evbuf[2] = handles;
@@ -284,9 +285,9 @@ ble_ll_conn_num_comp_pkts_event_send(void)
         evbuf[0] = BLE_HCI_EVCODE_NUM_COMP_PKTS;
         evbuf[1] = (handles * 2 * sizeof(uint16_t)) + 1;
         evbuf[2] = handles;
-        if (handles < 60) {
+        if (handles < max_handles) {
             /* Make the pkt counts contiguous with handles */
-            memmove(handle_ptr, evbuf + 3 + (60 * 2), handles * 2);
+            memmove(handle_ptr, evbuf + 3 + (max_handles * 2), handles * 2);
         }
         ble_ll_hci_event_send(evbuf);
         event_sent = 1;
@@ -313,7 +314,7 @@ ble_ll_auth_pyld_tmo_event_send(struct ble_ll_conn_sm *connsm)
     uint8_t *evbuf;
 
     if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_AUTH_PYLD_TMO)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_AUTH_PYLD_TMO;
             evbuf[1] = sizeof(uint16_t);
@@ -338,7 +339,7 @@ ble_ll_disconn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t reason)
     uint8_t *evbuf;
 
     if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_DISCONN_CMP)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_DISCONN_CMP;
             evbuf[1] = BLE_HCI_EVENT_DISCONN_COMPLETE_LEN;
diff --git a/net/nimble/controller/src/ble_ll_conn_priv.h b/net/nimble/controller/src/ble_ll_conn_priv.h
index 218ecbc4..fc06e5f9 100644
--- a/net/nimble/controller/src/ble_ll_conn_priv.h
+++ b/net/nimble/controller/src/ble_ll_conn_priv.h
@@ -81,6 +81,9 @@ extern struct ble_ll_conn_free_list g_ble_ll_conn_free_list;
 /* Pointer to connection state machine we are trying to create */
 extern struct ble_ll_conn_sm *g_ble_ll_conn_create_sm;
 
+extern struct os_mempool g_ble_ll_hci_ev_pool;
+
+
 /* Generic interface */
 struct ble_ll_len_req;
 struct hci_create_conn;
@@ -107,10 +110,11 @@ void ble_ll_conn_event_end(void *arg);
 void ble_ll_conn_tx_pkt_in(struct os_mbuf *om, uint16_t handle, uint16_t len);
 void ble_ll_conn_spvn_timeout(void *arg);
 int ble_ll_conn_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa);
-int ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu);
+int ble_ll_conn_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
 void ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr);
 void ble_ll_init_rx_pkt_in(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr);
-int ble_ll_init_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok);
+int ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok,
+                           struct ble_mbuf_hdr *ble_hdr);
 void ble_ll_conn_wfr_timer_exp(void);
 int ble_ll_conn_is_lru(struct ble_ll_conn_sm *s1, struct ble_ll_conn_sm *s2);
 uint32_t ble_ll_conn_get_ce_end_time(void);
@@ -150,4 +154,8 @@ void ble_ll_conn_auth_pyld_timer_start(struct ble_ll_conn_sm *connsm);
 #else
 #define ble_ll_conn_auth_pyld_timer_start(x)
 #endif
+
+int ble_ll_hci_cmd_rx(uint8_t *cmd, void *arg);
+int ble_ll_hci_acl_rx(struct os_mbuf *om, void *arg);
+
 #endif /* H_BLE_LL_CONN_PRIV_ */
diff --git a/net/nimble/controller/src/ble_ll_hci.c b/net/nimble/controller/src/ble_ll_hci.c
index 1d73465d..758568b4 100644
--- a/net/nimble/controller/src/ble_ll_hci.c
+++ b/net/nimble/controller/src/ble_ll_hci.c
@@ -23,7 +23,7 @@
 #include "nimble/ble.h"
 #include "nimble/nimble_opt.h"
 #include "nimble/hci_common.h"
-#include "nimble/hci_transport.h"
+#include "nimble/ble_hci_trans.h"
 #include "controller/ble_hw.h"
 #include "controller/ble_ll_adv.h"
 #include "controller/ble_ll_scan.h"
@@ -64,11 +64,13 @@ ble_ll_hci_event_send(uint8_t *evbuf)
 {
     int rc;
 
+    assert(BLE_HCI_EVENT_HDR_LEN + evbuf[1] <= BLE_LL_MAX_EVT_LEN);
+
     /* Count number of events sent */
     STATS_INC(ble_ll_stats, hci_events_sent);
 
     /* Send the event to the host */
-    rc = ble_hci_transport_ctlr_event_send(evbuf);
+    rc = ble_hci_trans_ll_evt_tx(evbuf);
 
     return rc;
 }
@@ -86,7 +88,7 @@ ble_ll_hci_send_noop(void)
     uint8_t *evbuf;
     uint16_t opcode;
 
-    evbuf = os_memblock_get(&g_hci_cmd_pool);
+    evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
     if (evbuf) {
         /* Create a command complete event with a NO-OP opcode */
         opcode = 0;
@@ -919,7 +921,7 @@ ble_ll_hci_cmd_proc(struct os_event *ev)
     assert(cmdbuf != NULL);
 
     /* Free the event */
-    err = os_memblock_put(&g_hci_os_event_pool, ev);
+    err = os_memblock_put(&g_ble_ll_hci_ev_pool, ev);
     assert(err == OS_OK);
 
     /* Get the opcode from the command buffer */
@@ -994,12 +996,12 @@ ble_ll_hci_cmd_proc(struct os_event *ev)
  *                              BLE_ERR_MEM_CAPACITY on HCI buffer exhaustion.
  */
 int
-ble_hci_transport_host_cmd_send(uint8_t *cmd)
+ble_ll_hci_cmd_rx(uint8_t *cmd, void *arg)
 {
     struct os_event *ev;
 
     /* Get an event structure off the queue */
-    ev = (struct os_event *)os_memblock_get(&g_hci_os_event_pool);
+    ev = (struct os_event *)os_memblock_get(&g_ble_ll_hci_ev_pool);
     if (!ev) {
         return BLE_ERR_MEM_CAPACITY;
     }
@@ -1015,7 +1017,7 @@ ble_hci_transport_host_cmd_send(uint8_t *cmd)
 
 /* Send ACL data from host to contoller */
 int
-ble_hci_transport_host_acl_data_send(struct os_mbuf *om)
+ble_ll_hci_acl_rx(struct os_mbuf *om, void *arg)
 {
     ble_ll_acl_data_in(om);
     return 0;
diff --git a/net/nimble/controller/src/ble_ll_hci_ev.c b/net/nimble/controller/src/ble_ll_hci_ev.c
index 773e4e6b..2548bdc8 100644
--- a/net/nimble/controller/src/ble_ll_hci_ev.c
+++ b/net/nimble/controller/src/ble_ll_hci_ev.c
@@ -21,6 +21,7 @@
 #include <string.h>
 #include "nimble/ble.h"
 #include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
 #include "controller/ble_ll.h"
 #include "controller/ble_ll_hci.h"
 #include "controller/ble_ll_ctrl.h"
@@ -41,7 +42,7 @@ ble_ll_hci_ev_datalen_chg(struct ble_ll_conn_sm *connsm)
     uint8_t *evbuf;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_DATA_LEN_CHG)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_LE_META;
             evbuf[1] = BLE_HCI_LE_DATA_LEN_CHG_LEN;
@@ -68,7 +69,7 @@ ble_ll_hci_ev_rem_conn_parm_req(struct ble_ll_conn_sm *connsm,
     uint8_t *evbuf;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_LE_META;
             evbuf[1] = BLE_HCI_LE_REM_CONN_PARM_REQ_LEN;
@@ -95,7 +96,7 @@ ble_ll_hci_ev_conn_update(struct ble_ll_conn_sm *connsm, uint8_t status)
     uint8_t *evbuf;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_LE_META;
             evbuf[1] = BLE_HCI_LE_CONN_UPD_LEN;
@@ -127,7 +128,7 @@ ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status)
     }
 
     if (ble_ll_hci_is_event_enabled(evcode)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = evcode;
             evbuf[1] = evlen;
@@ -158,7 +159,7 @@ ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm)
     uint8_t *evbuf;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_LT_KEY_REQ)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_LE_META;
             evbuf[1] = BLE_HCI_LE_LT_KEY_REQ_LEN;
@@ -188,7 +189,7 @@ ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm, uint8_t status)
     uint8_t *evbuf;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_LE_META;
             evbuf[1] = BLE_HCI_LE_RD_REM_USED_FEAT_LEN;
@@ -208,7 +209,7 @@ ble_ll_hci_ev_rd_rem_ver(struct ble_ll_conn_sm *connsm, uint8_t status)
     uint8_t *evbuf;
 
     if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP;
             evbuf[1] = BLE_HCI_EVENT_RD_RM_VER_LEN;
diff --git a/net/nimble/controller/src/ble_ll_rand.c b/net/nimble/controller/src/ble_ll_rand.c
index c5201f9f..8d26f8de 100644
--- a/net/nimble/controller/src/ble_ll_rand.c
+++ b/net/nimble/controller/src/ble_ll_rand.c
@@ -37,7 +37,8 @@ struct ble_ll_rnum_data
 struct ble_ll_rnum_data g_ble_ll_rnum_data;
 uint8_t g_ble_ll_rnum_buf[NIMBLE_OPT_LL_RNG_BUFSIZE];
 
-#define IS_RNUM_BUF_END(x)  (x == &g_ble_ll_rnum_buf[NIMBLE_OPT_LL_RNG_BUFSIZE])
+#define IS_RNUM_BUF_END(x)  \
+    (x == &g_ble_ll_rnum_buf[NIMBLE_OPT_LL_RNG_BUFSIZE - 1])
 
 void
 ble_ll_rand_sample(uint8_t rnum)
diff --git a/net/nimble/controller/src/ble_ll_resolv.c b/net/nimble/controller/src/ble_ll_resolv.c
index 4d91685a..903c95fd 100644
--- a/net/nimble/controller/src/ble_ll_resolv.c
+++ b/net/nimble/controller/src/ble_ll_resolv.c
@@ -31,12 +31,15 @@
 #include "ble_ll_conn_priv.h"
 
 #if (BLE_LL_CFG_FEAT_LL_PRIVACY == 1)
-
-/* Flag denoting whether or not address translation is enabled. */
-uint8_t g_ble_ll_addr_res_enabled;
-uint8_t g_ble_ll_resolv_list_size;
-uint8_t g_ble_ll_resolv_list_cnt;
-uint32_t g_ble_ll_resolv_rpa_tmo;
+struct ble_ll_resolv_data
+{
+    uint8_t addr_res_enabled;
+    uint8_t rl_size;
+    uint8_t rl_cnt;
+    uint32_t rpa_tmo;
+    struct os_callout_func rpa_timer;
+};
+struct ble_ll_resolv_data g_ble_ll_resolv_data;
 
 struct ble_ll_resolv_entry g_ble_ll_resolv_list[NIMBLE_OPT_LL_RESOLV_LIST_SIZE];
 
@@ -53,9 +56,9 @@ ble_ll_resolv_list_chg_allowed(void)
 {
     int rc;
 
-    if (g_ble_ll_addr_res_enabled && (ble_ll_adv_enabled()  ||
-                                      ble_ll_scan_enabled() ||
-                                      g_ble_ll_conn_create_sm)) {
+    if (g_ble_ll_resolv_data.addr_res_enabled &&
+            (ble_ll_adv_enabled() || ble_ll_scan_enabled() ||
+             g_ble_ll_conn_create_sm)) {
         rc = 0;
     } else {
         rc = 1;
@@ -64,6 +67,32 @@ ble_ll_resolv_list_chg_allowed(void)
 }
 
 /**
+ * Called when the Resolvable private address timer expires. This timer
+ * is used to regenerate local RPA's in the resolving list.
+ *
+ * @param arg
+ */
+void
+ble_ll_resolv_rpa_timer_cb(void *arg)
+{
+    int i;
+    os_sr_t sr;
+    struct ble_ll_resolv_entry *rl;
+
+    rl = &g_ble_ll_resolv_list[0];
+    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
+        OS_ENTER_CRITICAL(sr);
+        rl->rl_local_rpa_set = 0;
+        ble_ll_resolv_gen_priv_addr(rl, 1, rl->rl_local_rpa);
+        rl->rl_local_rpa_set = 1;
+        OS_EXIT_CRITICAL(sr);
+        ++rl;
+    }
+    os_callout_reset(&g_ble_ll_resolv_data.rpa_timer.cf_c,
+                     (int32_t)g_ble_ll_resolv_data.rpa_tmo);
+}
+
+/**
  * Called to determine if the IRK is all zero.
  *
  * @param irk
@@ -102,7 +131,7 @@ ble_ll_resolv_list_clr(void)
     }
 
     /* Sets total on list to 0. Clears HW resolve list */
-    g_ble_ll_resolv_list_cnt = 0;
+    g_ble_ll_resolv_data.rl_cnt = 0;
     ble_hw_resolv_list_clear();
 
     return BLE_ERR_SUCCESS;
@@ -119,7 +148,7 @@ ble_ll_resolv_list_clr(void)
 int
 ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen)
 {
-    rspbuf[0] = g_ble_ll_resolv_list_size;
+    rspbuf[0] = g_ble_ll_resolv_data.rl_size;
     *rsplen = 1;
     return BLE_ERR_SUCCESS;
 }
@@ -141,7 +170,7 @@ ble_ll_is_on_resolv_list(uint8_t *addr, uint8_t addr_type)
     struct ble_ll_resolv_entry *rl;
 
     rl = &g_ble_ll_resolv_list[0];
-    for (i = 0; i < g_ble_ll_resolv_list_cnt; ++i) {
+    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
         if ((rl->rl_addr_type == addr_type) &&
             (!memcmp(&rl->rl_identity_addr[0], addr, BLE_DEV_ADDR_LEN))) {
             return i + 1;
@@ -167,7 +196,7 @@ ble_ll_resolv_list_find(uint8_t *addr, uint8_t addr_type)
     struct ble_ll_resolv_entry *rl;
 
     rl = &g_ble_ll_resolv_list[0];
-    for (i = 0; i < g_ble_ll_resolv_list_cnt; ++i) {
+    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
         if ((rl->rl_addr_type == addr_type) &&
             (!memcmp(&rl->rl_identity_addr[0], addr, BLE_DEV_ADDR_LEN))) {
             return rl;
@@ -197,7 +226,7 @@ ble_ll_resolv_list_add(uint8_t *cmdbuf)
     }
 
     /* Check if we have any open entries */
-    if (g_ble_ll_resolv_list_cnt >= g_ble_ll_resolv_list_size) {
+    if (g_ble_ll_resolv_data.rl_cnt >= g_ble_ll_resolv_data.rl_size) {
         return BLE_ERR_MEM_CAPACITY;
     }
 
@@ -206,14 +235,23 @@ ble_ll_resolv_list_add(uint8_t *cmdbuf)
 
     rc = BLE_ERR_SUCCESS;
     if (!ble_ll_is_on_resolv_list(ident_addr, addr_type)) {
-        rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_list_cnt];
+        rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt];
         rl->rl_addr_type = addr_type;
+        rl->rl_local_rpa_set = 0;
         memcpy(&rl->rl_identity_addr[0], ident_addr, BLE_DEV_ADDR_LEN);
         swap_buf(rl->rl_peer_irk, cmdbuf + 7, 16);
         swap_buf(rl->rl_local_irk, cmdbuf + 23, 16);
-        ++g_ble_ll_resolv_list_cnt;
 
+        /*
+         * Add peer IRK to HW resolving list. If we can add it, also
+         * generate a local RPA now to save time later.
+         */
         rc = ble_hw_resolv_list_add(rl->rl_peer_irk);
+        if (!rc) {
+            ble_ll_resolv_gen_priv_addr(rl, 1, rl->rl_local_rpa);
+            rl->rl_local_rpa_set = 1;
+        }
+        ++g_ble_ll_resolv_data.rl_cnt;
     }
 
     return rc;
@@ -243,11 +281,11 @@ ble_ll_resolv_list_rmv(uint8_t *cmdbuf)
 
     /* Remove from IRK records */
     position = ble_ll_is_on_resolv_list(ident_addr, addr_type);
-    if (position && (position < g_ble_ll_resolv_list_cnt)) {
+    if (position && (position < g_ble_ll_resolv_data.rl_cnt)) {
         memmove(&g_ble_ll_resolv_list[position - 1],
                 &g_ble_ll_resolv_list[position],
-                g_ble_ll_resolv_list_cnt - position);
-        --g_ble_ll_resolv_list_cnt;
+                g_ble_ll_resolv_data.rl_cnt - position);
+        --g_ble_ll_resolv_data.rl_cnt;
 
         /* Remove from HW list */
         ble_hw_resolv_list_rmv(position - 1);
@@ -267,13 +305,29 @@ int
 ble_ll_resolv_enable_cmd(uint8_t *cmdbuf)
 {
     int rc;
+    int32_t tmo;
+    uint8_t enabled;
 
     if (ble_ll_adv_enabled() || ble_ll_scan_enabled() ||
         g_ble_ll_conn_create_sm) {
         rc = BLE_ERR_CMD_DISALLOWED;
     } else {
-        g_ble_ll_addr_res_enabled = cmdbuf[0];
-        rc = BLE_ERR_SUCCESS;
+        enabled = cmdbuf[0];
+        if (enabled <= 1) {
+            /* If we change state, we need to disable/enable the RPA timer */
+            if ((enabled ^ g_ble_ll_resolv_data.addr_res_enabled) != 0) {
+                if (enabled) {
+                    tmo = (int32_t)g_ble_ll_resolv_data.rpa_tmo;
+                    os_callout_reset(&g_ble_ll_resolv_data.rpa_timer.cf_c, tmo);
+                } else {
+                    os_callout_stop(&g_ble_ll_resolv_data.rpa_timer.cf_c);
+                }
+                g_ble_ll_resolv_data.addr_res_enabled = enabled;
+            }
+            rc = BLE_ERR_SUCCESS;
+        } else {
+            rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        }
     }
 
     return rc;
@@ -306,7 +360,11 @@ ble_ll_resolv_set_rpa_tmo(uint8_t *cmdbuf)
 
     tmo_secs = le16toh(cmdbuf);
     if ((tmo_secs > 0) && (tmo_secs <= 0xA1B8)) {
-        g_ble_ll_resolv_rpa_tmo = tmo_secs * OS_TICKS_PER_SEC;
+        g_ble_ll_resolv_data.rpa_tmo = tmo_secs * OS_TICKS_PER_SEC;
+        if (g_ble_ll_resolv_data.addr_res_enabled) {
+            os_callout_reset(&g_ble_ll_resolv_data.rpa_timer.cf_c,
+                             (int32_t)g_ble_ll_resolv_data.rpa_tmo);
+        }
     } else {
         rc = BLE_ERR_INV_HCI_CMD_PARMS;
     }
@@ -323,12 +381,11 @@ ble_ll_resolv_set_rpa_tmo(uint8_t *cmdbuf)
 uint32_t
 ble_ll_resolv_get_rpa_tmo(void)
 {
-    return g_ble_ll_resolv_rpa_tmo;
+    return g_ble_ll_resolv_data.rpa_tmo;
 }
 
 /**
- * Called the generate a resolvable private address
- *
+ * Called to generate a resolvable private address
  *
  * @param rl
  * @param local
@@ -340,11 +397,20 @@ ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local,
 {
     uint8_t *irk;
     uint8_t *prand;
+    uint32_t *irk32;
+    uint32_t *key32;
+    uint32_t *pt32;
     struct ble_encryption_block ecb;
 
     assert(rl != NULL);
     assert(addr != NULL);
 
+    /* If the local rpa has already been generated, just copy it */
+    if (local && rl->rl_local_rpa_set) {
+        memcpy(addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
+        return;
+    }
+
     /* Get prand */
     prand = addr + 3;
     ble_ll_rand_prand_get(prand);
@@ -356,13 +422,27 @@ ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local,
         irk = rl->rl_peer_irk;
     }
 
-    memcpy(ecb.key, irk, BLE_ENC_BLOCK_SIZE);
-    memset(ecb.plain_text, 0, BLE_ENC_BLOCK_SIZE);
-    swap_buf(&ecb.plain_text[13], prand, 3);
+    irk32 = (uint32_t *)irk;
+    key32 = (uint32_t *)&ecb.key[0];
+    key32[0] = irk32[0];
+    key32[1] = irk32[1];
+    key32[2] = irk32[2];
+    key32[3] = irk32[3];
+    pt32 = (uint32_t *)&ecb.plain_text[0];
+    pt32[0] = 0;
+    pt32[1] = 0;
+    pt32[2] = 0;
+    ecb.plain_text[12] = 0;
+    ecb.plain_text[13] = prand[2];
+    ecb.plain_text[14] = prand[1];
+    ecb.plain_text[15] = prand[0];
 
     /* Calculate hash */
     ble_hw_encrypt_block(&ecb);
-    swap_buf(addr, ecb.cipher_text + 13, 3);
+
+    addr[0] = ecb.cipher_text[15];
+    addr[1] = ecb.cipher_text[14];
+    addr[2] = ecb.cipher_text[13];
 }
 
 /**
@@ -410,11 +490,29 @@ int
 ble_ll_resolv_rpa(uint8_t *rpa, uint8_t *irk)
 {
     int rc;
+    uint32_t *irk32;
+    uint32_t *key32;
+    uint32_t *pt32;
     struct ble_encryption_block ecb;
 
-    memcpy(ecb.key, irk, BLE_ENC_BLOCK_SIZE);
-    memset(ecb.plain_text, 0, BLE_ENC_BLOCK_SIZE);
-    swap_buf(&ecb.plain_text[13], rpa + 3, 3);
+    irk32 = (uint32_t *)irk;
+    key32 = (uint32_t *)&ecb.key[0];
+
+    key32[0] = irk32[0];
+    key32[1] = irk32[1];
+    key32[2] = irk32[2];
+    key32[3] = irk32[3];
+
+    pt32 = (uint32_t *)&ecb.plain_text[0];
+    pt32[0] = 0;
+    pt32[1] = 0;
+    pt32[2] = 0;
+    pt32[3] = 0;
+
+    ecb.plain_text[15] = rpa[3];
+    ecb.plain_text[14] = rpa[4];
+    ecb.plain_text[13] = rpa[5];
+
     ble_hw_encrypt_block(&ecb);
     if ((ecb.cipher_text[15] == rpa[0]) && (ecb.cipher_text[14] == rpa[1]) &&
         (ecb.cipher_text[13] == rpa[2])) {
@@ -434,7 +532,7 @@ ble_ll_resolv_rpa(uint8_t *rpa, uint8_t *irk)
 uint8_t
 ble_ll_resolv_enabled(void)
 {
-    return g_ble_ll_addr_res_enabled;
+    return g_ble_ll_resolv_data.addr_res_enabled;
 }
 
 /**
@@ -443,7 +541,8 @@ ble_ll_resolv_enabled(void)
 void
 ble_ll_resolv_list_reset(void)
 {
-    g_ble_ll_addr_res_enabled = 0;
+    g_ble_ll_resolv_data.addr_res_enabled = 0;
+    os_callout_stop(&g_ble_ll_resolv_data.rpa_timer.cf_c);
     ble_ll_resolv_list_clr();
     ble_ll_resolv_init();
 }
@@ -454,14 +553,18 @@ ble_ll_resolv_init(void)
     uint8_t hw_size;
 
     /* Default is 15 minutes */
-    g_ble_ll_resolv_rpa_tmo = 15 * 60 * OS_TICKS_PER_SEC;
+    g_ble_ll_resolv_data.rpa_tmo = 15 * 60 * OS_TICKS_PER_SEC;
 
     hw_size = ble_hw_resolv_list_size();
     if (hw_size > NIMBLE_OPT_LL_RESOLV_LIST_SIZE) {
         hw_size = NIMBLE_OPT_LL_RESOLV_LIST_SIZE;
     }
-    g_ble_ll_resolv_list_size = hw_size;
+    g_ble_ll_resolv_data.rl_size = hw_size;
 
+    os_callout_func_init(&g_ble_ll_resolv_data.rpa_timer,
+                         &g_ble_ll_data.ll_evq,
+                         ble_ll_resolv_rpa_timer_cb,
+                         NULL);
 }
 
 #endif  /* if BLE_LL_CFG_FEAT_LL_PRIVACY == 1 */
diff --git a/net/nimble/controller/src/ble_ll_scan.c b/net/nimble/controller/src/ble_ll_scan.c
index e7346baf..29193dc8 100644
--- a/net/nimble/controller/src/ble_ll_scan.c
+++ b/net/nimble/controller/src/ble_ll_scan.c
@@ -25,6 +25,7 @@
 #include "nimble/ble.h"
 #include "nimble/nimble_opt.h"
 #include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
 #include "controller/ble_phy.h"
 #include "controller/ble_hw.h"
 #include "controller/ble_ll.h"
@@ -424,7 +425,7 @@ ble_ll_hci_send_adv_report(uint8_t pdu_type, uint8_t txadd, uint8_t *rxbuf,
     }
 
     if (ble_ll_hci_is_le_event_enabled(subev)) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
         if (evbuf) {
             evbuf[0] = BLE_HCI_EVCODE_LE_META;
             evbuf[1] = event_len;
@@ -447,7 +448,7 @@ ble_ll_hci_send_adv_report(uint8_t pdu_type, uint8_t txadd, uint8_t *rxbuf,
                  * are 2 greater than the unresolved ones in the spec, so
                  * we just add 2 here.
                  */
-                addr_type += 2;
+                addr_type = g_ble_ll_resolv_list[index].rl_addr_type + 2;
             } else{
                 adv_addr = rxbuf;
             }
@@ -837,11 +838,10 @@ ble_ll_scan_event_proc(void *arg)
  *  1: we may send a response to this frame.
  */
 int
-ble_ll_scan_rx_isr_start(uint8_t pdu_type, struct os_mbuf *rxpdu)
+ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint8_t *rxflags)
 {
     int rc;
     struct ble_ll_scan_sm *scansm;
-    struct ble_mbuf_hdr *ble_hdr;
 
     rc = 0;
     scansm = &g_ble_ll_scan_sm;
@@ -863,8 +863,7 @@ ble_ll_scan_rx_isr_start(uint8_t pdu_type, struct os_mbuf *rxpdu)
          */
         if (scansm->scan_rsp_pending) {
             if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_RSP) {
-                ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
-                ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_SCAN_RSP_CHK;
+                *rxflags |= BLE_MBUF_HDR_F_SCAN_RSP_CHK;
             } else {
                 ble_ll_scan_req_backoff(scansm, 0);
             }
@@ -990,7 +989,7 @@ ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
 
     /* If whitelist enabled, check to see if device is in the white list */
     if (chk_wl && !ble_ll_whitelist_match(peer, peer_addr_type, resolved)) {
-        return -1;
+        goto scan_rx_isr_exit;
     }
     ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_DEVMATCH;
 
@@ -998,7 +997,7 @@ ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
     if (chk_send_req) {
         /* Dont send scan request if we have sent one to this advertiser */
         if (ble_ll_scan_have_rxd_scan_rsp(peer, peer_addr_type)) {
-            return -1;
+            goto scan_rx_isr_exit;
         }
 
         /* Better not be a scan response pending */
diff --git a/net/nimble/controller/src/ble_ll_sched.c b/net/nimble/controller/src/ble_ll_sched.c
index 5b792176..90cb510e 100644
--- a/net/nimble/controller/src/ble_ll_sched.c
+++ b/net/nimble/controller/src/ble_ll_sched.c
@@ -32,6 +32,10 @@
 /* XXX: this is temporary. Not sure what I want to do here */
 struct cpu_timer g_ble_ll_sched_timer;
 
+#if (BLE_LL_SCHED_DEBUG == 1)
+int32_t g_ble_ll_sched_max_late;
+#endif
+
 /* XXX: TODO:
  *  1) Add some accounting to the schedule code to see how late we are
  *  (min/max?)
@@ -645,6 +649,7 @@ ble_ll_sched_execute_item(struct ble_ll_sched_item *sch)
         } else {
             STATS_INC(ble_ll_stats, sched_state_conn_errs);
             ble_ll_conn_event_halt();
+            return -1;
         }
     }
 
@@ -663,12 +668,19 @@ ble_ll_sched_execute_item(struct ble_ll_sched_item *sch)
 void
 ble_ll_sched_run(void *arg)
 {
+    int32_t dt;
     struct ble_ll_sched_item *sch;
 
     /* Look through schedule queue */
     while ((sch = TAILQ_FIRST(&g_ble_ll_sched_q)) != NULL) {
         /* Make sure we have passed the start time of the first event */
-        if ((int32_t)(cputime_get32() - sch->start_time) >= 0) {
+        dt = (int32_t)(cputime_get32() - sch->start_time);
+        if (dt >= 0) {
+#if (BLE_LL_SCHED_DEBUG == 1)
+            if (dt > g_ble_ll_sched_max_late) {
+                g_ble_ll_sched_max_late = dt;
+            }
+#endif
             /* Remove schedule item and execute the callback */
             TAILQ_REMOVE(&g_ble_ll_sched_q, sch, link);
             sch->enqueued = 0;
diff --git a/net/nimble/drivers/native/src/ble_phy.c b/net/nimble/drivers/native/src/ble_phy.c
index e2c9f259..ff195fc6 100644
--- a/net/nimble/drivers/native/src/ble_phy.c
+++ b/net/nimble/drivers/native/src/ble_phy.c
@@ -18,24 +18,32 @@
  */
 
 #include <stdint.h>
+#include <string.h>
 #include <assert.h>
 #include "os/os.h"
-#include "nimble/ble.h"             /* XXX: needed for ble mbuf header.*/
+#include "ble/xcvr.h"
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
 #include "controller/ble_phy.h"
 #include "controller/ble_ll.h"
 
 /* BLE PHY data structure */
 struct ble_phy_obj
 {
+    uint8_t phy_stats_initialized;
     int8_t  phy_txpwr_dbm;
     uint8_t phy_chan;
     uint8_t phy_state;
     uint8_t phy_transition;
     uint8_t phy_rx_started;
+    uint8_t phy_encrypted;
     uint8_t phy_privacy;
+    uint8_t phy_tx_pyld_len;
+    uint32_t phy_aar_scratch;
     uint32_t phy_access_address;
-    struct os_mbuf *rxpdu;
+    struct ble_mbuf_hdr rxhdr;
     void *txend_arg;
+    uint8_t *rxdptr;
     ble_phy_tx_end_func txend_cb;
 };
 struct ble_phy_obj g_ble_phy_data;
@@ -75,6 +83,42 @@ static struct xcvr_data g_xcvr_data;
 #define BLE_XCVR_TX_PWR_MAX_DBM     (30)
 #define BLE_XCVR_TX_PWR_MIN_DBM     (-20)
 
+/* Statistics */
+STATS_SECT_START(ble_phy_stats)
+    STATS_SECT_ENTRY(phy_isrs)
+    STATS_SECT_ENTRY(tx_good)
+    STATS_SECT_ENTRY(tx_fail)
+    STATS_SECT_ENTRY(tx_late)
+    STATS_SECT_ENTRY(tx_bytes)
+    STATS_SECT_ENTRY(rx_starts)
+    STATS_SECT_ENTRY(rx_aborts)
+    STATS_SECT_ENTRY(rx_valid)
+    STATS_SECT_ENTRY(rx_crc_err)
+    STATS_SECT_ENTRY(rx_late)
+    STATS_SECT_ENTRY(no_bufs)
+    STATS_SECT_ENTRY(radio_state_errs)
+    STATS_SECT_ENTRY(rx_hw_err)
+    STATS_SECT_ENTRY(tx_hw_err)
+STATS_SECT_END
+STATS_SECT_DECL(ble_phy_stats) ble_phy_stats;
+
+STATS_NAME_START(ble_phy_stats)
+    STATS_NAME(ble_phy_stats, phy_isrs)
+    STATS_NAME(ble_phy_stats, tx_good)
+    STATS_NAME(ble_phy_stats, tx_fail)
+    STATS_NAME(ble_phy_stats, tx_late)
+    STATS_NAME(ble_phy_stats, tx_bytes)
+    STATS_NAME(ble_phy_stats, rx_starts)
+    STATS_NAME(ble_phy_stats, rx_aborts)
+    STATS_NAME(ble_phy_stats, rx_valid)
+    STATS_NAME(ble_phy_stats, rx_crc_err)
+    STATS_NAME(ble_phy_stats, rx_late)
+    STATS_NAME(ble_phy_stats, no_bufs)
+    STATS_NAME(ble_phy_stats, radio_state_errs)
+    STATS_NAME(ble_phy_stats, rx_hw_err)
+    STATS_NAME(ble_phy_stats, tx_hw_err)
+STATS_NAME_END(ble_phy_stats)
+
 /* XXX: TODO:
 
  * 1) Test the following to make sure it works: suppose an event is already
@@ -95,28 +139,81 @@ ble_xcvr_clear_irq(uint32_t mask)
 }
 
 /**
- * ble phy rxpdu get
+ * Copies the data from the phy receive buffer into a mbuf chain.
  *
- * Gets a mbuf for PDU reception.
+ * @param dptr Pointer to receive buffer
+ * @param rxpdu Pointer to already allocated mbuf chain
+ *
+ * NOTE: the packet header already has the total mbuf length in it. The
+ * lengths of the individual mbufs are not set prior to calling.
  *
- * @return struct os_mbuf* Pointer to retrieved mbuf or NULL if none available
  */
-static struct os_mbuf *
-ble_phy_rxpdu_get(void)
+void
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
 {
+    uint16_t rem_bytes;
+    uint16_t mb_bytes;
+    uint16_t copylen;
+    uint32_t *dst;
+    uint32_t *src;
     struct os_mbuf *m;
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf_pkthdr *pkthdr;
+
+    /* Better be aligned */
+    assert(((uint32_t)dptr & 3) == 0);
+
+    pkthdr = OS_MBUF_PKTHDR(rxpdu);
+    rem_bytes = pkthdr->omp_len;
+
+    /* Fill in the mbuf pkthdr first. */
+    dst = (uint32_t *)(rxpdu->om_data);
+    src = (uint32_t *)dptr;
+
+    mb_bytes = (rxpdu->om_omp->omp_databuf_len - rxpdu->om_pkthdr_len - 4);
+    copylen = min(mb_bytes, rem_bytes);
+    copylen &= 0xFFFC;
+    rem_bytes -= copylen;
+    mb_bytes -= copylen;
+    rxpdu->om_len = copylen;
+    while (copylen > 0) {
+        *dst = *src;
+        ++dst;
+        ++src;
+        copylen -= 4;
+    }
 
-    m = g_ble_phy_data.rxpdu;
-    if (m == NULL) {
-        m = os_msys_get_pkthdr(BLE_MBUF_PAYLOAD_SIZE, sizeof(struct ble_mbuf_hdr));
-        if (!m) {
-            ++g_ble_phy_stats.no_bufs;
-        } else {
-            g_ble_phy_data.rxpdu = m;
+    /* Copy remaining bytes */
+    m = rxpdu;
+    while (rem_bytes > 0) {
+        /* If there are enough bytes in the mbuf, copy them and leave */
+        if (rem_bytes <= mb_bytes) {
+            memcpy(m->om_data + m->om_len, src, rem_bytes);
+            m->om_len += rem_bytes;
+            break;
+        }
+
+        m = SLIST_NEXT(m, om_next);
+        assert(m != NULL);
+
+        mb_bytes = m->om_omp->omp_databuf_len;
+        copylen = min(mb_bytes, rem_bytes);
+        copylen &= 0xFFFC;
+        rem_bytes -= copylen;
+        mb_bytes -= copylen;
+        m->om_len = copylen;
+        dst = (uint32_t *)m->om_data;
+        while (copylen > 0) {
+            *dst = *src;
+            ++dst;
+            ++src;
+            copylen -= 4;
         }
     }
 
-    return m;
+    /* Copy ble header */
+    ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
+    memcpy(ble_hdr, &g_ble_phy_data.rxhdr, sizeof(struct ble_mbuf_hdr));
 }
 
 void
@@ -126,7 +223,6 @@ ble_phy_isr(void)
     uint8_t crcok;
     uint8_t transition;
     uint32_t irq_en;
-    struct os_mbuf *rxpdu;
     struct ble_mbuf_hdr *ble_hdr;
 
     /* Check for disabled event. This only happens for transmits now */
@@ -139,14 +235,9 @@ ble_phy_isr(void)
 
         transition = g_ble_phy_data.phy_transition;
         if (transition == BLE_PHY_TRANSITION_TX_RX) {
-            /* Packet pointer needs to be reset. */
-            if (g_ble_phy_data.rxpdu != NULL) {
-                g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
-            } else {
-                /* Disable the phy */
-                /* XXX: count no bufs? */
-                ble_phy_disable();
-            }
+            /* Disable the phy */
+            /* XXX: count no bufs? */
+            ble_phy_disable();
         } else {
             /* Better not be going from rx to tx! */
             assert(transition == BLE_PHY_TRANSITION_NONE);
@@ -158,11 +249,9 @@ ble_phy_isr(void)
 
         ble_xcvr_clear_irq(BLE_XCVR_IRQ_F_RX_START);
 
-        /* Better have a PDU! */
-        assert(g_ble_phy_data.rxpdu != NULL);
-
         /* Call Link Layer receive start function */
-        rc = ble_ll_rx_start(g_ble_phy_data.rxpdu, g_ble_phy_data.phy_chan);
+        rc = ble_ll_rx_start(g_ble_phy_data.rxdptr, g_ble_phy_data.phy_chan,
+                             &g_ble_phy_data.rxhdr);
         if (rc >= 0) {
             /* XXX: set rx end enable isr */
         } else {
@@ -182,7 +271,7 @@ ble_phy_isr(void)
         ble_xcvr_clear_irq(BLE_XCVR_IRQ_F_RX_END);
 
         /* Construct BLE header before handing up */
-        ble_hdr = BLE_MBUF_HDR_PTR(g_ble_phy_data.rxpdu);
+        ble_hdr = &g_ble_phy_data.rxhdr;
         ble_hdr->rxinfo.flags = 0;
         ble_hdr->rxinfo.rssi = -77;    /* XXX: dummy rssi */
         ble_hdr->rxinfo.channel = g_ble_phy_data.phy_chan;
@@ -197,9 +286,7 @@ ble_phy_isr(void)
         }
 
         /* Call Link Layer receive payload function */
-        rxpdu = g_ble_phy_data.rxpdu;
-        g_ble_phy_data.rxpdu = NULL;
-        rc = ble_ll_rx_end(rxpdu, ble_hdr);
+        rc = ble_ll_rx_end(g_ble_phy_data.rxdptr, ble_hdr);
         if (rc < 0) {
             /* Disable the PHY. */
             ble_phy_disable();
@@ -239,11 +326,6 @@ ble_phy_rx(void)
         return BLE_PHY_ERR_RADIO_STATE;
     }
 
-    /* If no pdu, get one */
-    if (ble_phy_rxpdu_get() == NULL) {
-        return BLE_PHY_ERR_NO_BUFS;
-    }
-
     g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
 
     return 0;
@@ -348,11 +430,6 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
     }
 
 
-    /* Enable shortcuts for transmit start/end. */
-    if (end_trans == BLE_PHY_TRANSITION_TX_RX) {
-        ble_phy_rxpdu_get();
-    }
-
     /* Set the PHY transition */
     g_ble_phy_data.phy_transition = end_trans;
 
diff --git a/net/nimble/drivers/nrf51/src/ble_phy.c b/net/nimble/drivers/nrf51/src/ble_phy.c
index 0c050119..a24a434e 100644
--- a/net/nimble/drivers/nrf51/src/ble_phy.c
+++ b/net/nimble/drivers/nrf51/src/ble_phy.c
@@ -28,17 +28,12 @@
 #include "controller/ble_ll.h"
 #include "mcu/nrf51_bitfields.h"
 
-/*
- * XXX: need to make the copy from mbuf into the PHY data structures 32-bit
- * copies or we are screwed.
- */
-
 /* XXX: 4) Make sure RF is higher priority interrupt than schedule */
 
 /*
  * XXX: Maximum possible transmit time is 1 msec for a 60ppm crystal
  * and 16ms for a 30ppm crystal! We need to limit PDU size based on
- * crystal accuracy
+ * crystal accuracy. Look at this in the spec.
  */
 
 /* XXX: private header file? */
@@ -82,9 +77,10 @@ struct ble_phy_obj
     uint8_t phy_encrypted;
     uint8_t phy_privacy;
     uint8_t phy_tx_pyld_len;
+    uint8_t *rxdptr;
     uint32_t phy_aar_scratch;
     uint32_t phy_access_address;
-    struct os_mbuf *rxpdu;
+    struct ble_mbuf_hdr rxhdr;
     void *txend_arg;
     ble_phy_tx_end_func txend_cb;
 };
@@ -92,7 +88,8 @@ struct ble_phy_obj g_ble_phy_data;
 
 /* XXX: if 27 byte packets desired we can make this smaller */
 /* Global transmit/receive buffer */
-static uint32_t g_ble_phy_txrx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+static uint32_t g_ble_phy_tx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+static uint32_t g_ble_phy_rx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
 
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
 /* Make sure word-aligned for faster copies */
@@ -111,7 +108,6 @@ STATS_SECT_START(ble_phy_stats)
     STATS_SECT_ENTRY(rx_valid)
     STATS_SECT_ENTRY(rx_crc_err)
     STATS_SECT_ENTRY(rx_late)
-    STATS_SECT_ENTRY(no_bufs)
     STATS_SECT_ENTRY(radio_state_errs)
     STATS_SECT_ENTRY(rx_hw_err)
     STATS_SECT_ENTRY(tx_hw_err)
@@ -129,7 +125,6 @@ STATS_NAME_START(ble_phy_stats)
     STATS_NAME(ble_phy_stats, rx_valid)
     STATS_NAME(ble_phy_stats, rx_crc_err)
     STATS_NAME(ble_phy_stats, rx_late)
-    STATS_NAME(ble_phy_stats, no_bufs)
     STATS_NAME(ble_phy_stats, radio_state_errs)
     STATS_NAME(ble_phy_stats, rx_hw_err)
     STATS_NAME(ble_phy_stats, tx_hw_err)
@@ -183,33 +178,81 @@ struct nrf_ccm_data g_nrf_ccm_data;
 #endif
 
 /**
- * ble phy rxpdu get
+ * Copies the data from the phy receive buffer into a mbuf chain.
  *
- * Gets a mbuf for PDU reception.
+ * @param dptr Pointer to receive buffer
+ * @param rxpdu Pointer to already allocated mbuf chain
+ *
+ * NOTE: the packet header already has the total mbuf length in it. The
+ * lengths of the individual mbufs are not set prior to calling.
  *
- * @return struct os_mbuf* Pointer to retrieved mbuf or NULL if none available
  */
-static struct os_mbuf *
-ble_phy_rxpdu_get(void)
+void
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
 {
+    uint16_t rem_bytes;
+    uint16_t mb_bytes;
+    uint16_t copylen;
+    uint32_t *dst;
+    uint32_t *src;
     struct os_mbuf *m;
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf_pkthdr *pkthdr;
+
+    /* Better be aligned */
+    assert(((uint32_t)dptr & 3) == 0);
+
+    pkthdr = OS_MBUF_PKTHDR(rxpdu);
+    rem_bytes = pkthdr->omp_len;
+
+    /* Fill in the mbuf pkthdr first. */
+    dst = (uint32_t *)(rxpdu->om_data);
+    src = (uint32_t *)dptr;
+
+    mb_bytes = (rxpdu->om_omp->omp_databuf_len - rxpdu->om_pkthdr_len - 4);
+    copylen = min(mb_bytes, rem_bytes);
+    copylen &= 0xFFFC;
+    rem_bytes -= copylen;
+    mb_bytes -= copylen;
+    rxpdu->om_len = copylen;
+    while (copylen > 0) {
+        *dst = *src;
+        ++dst;
+        ++src;
+        copylen -= 4;
+    }
 
-    m = g_ble_phy_data.rxpdu;
-    if (m == NULL) {
-        m = os_msys_get_pkthdr(BLE_MBUF_PAYLOAD_SIZE, sizeof(struct ble_mbuf_hdr));
-        if (!m) {
-            STATS_INC(ble_phy_stats, no_bufs);
-        } else {
-            /*
-             * NOTE: we add two bytes to the data pointer as we will prepend
-             * two bytes if we hand this received pdu up to host.
-             */
-            m->om_data += 2;
-            g_ble_phy_data.rxpdu = m;
+    /* Copy remaining bytes */
+    m = rxpdu;
+    while (rem_bytes > 0) {
+        /* If there are enough bytes in the mbuf, copy them and leave */
+        if (rem_bytes <= mb_bytes) {
+            memcpy(m->om_data + m->om_len, src, rem_bytes);
+            m->om_len += rem_bytes;
+            break;
+        }
+
+        m = SLIST_NEXT(m, om_next);
+        assert(m != NULL);
+
+        mb_bytes = m->om_omp->omp_databuf_len;
+        copylen = min(mb_bytes, rem_bytes);
+        copylen &= 0xFFFC;
+        rem_bytes -= copylen;
+        mb_bytes -= copylen;
+        m->om_len = copylen;
+        dst = (uint32_t *)m->om_data;
+        while (copylen > 0) {
+            *dst = *src;
+            ++dst;
+            ++src;
+            copylen -= 4;
         }
     }
 
-    return m;
+    /* Copy ble header */
+    ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
+    memcpy(ble_hdr, &g_ble_phy_data.rxhdr, sizeof(struct ble_mbuf_hdr));
 }
 
 /**
@@ -241,11 +284,16 @@ nrf_wait_disabled(void)
 static void
 ble_phy_rx_xcvr_setup(void)
 {
+    uint8_t *dptr;
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
     if (g_ble_phy_data.phy_encrypted) {
+        dptr += 3;
         NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_enc_buf[0];
         NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
-        NRF_CCM->OUTPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+        NRF_CCM->OUTPTR = (uint32_t)dptr;
         NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
         NRF_CCM->MODE = CCM_MODE_MODE_Decryption;
         NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
@@ -254,20 +302,22 @@ ble_phy_rx_xcvr_setup(void)
         NRF_CCM->EVENTS_ENDCRYPT = 0;
         NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk | PPI_CHEN_CH25_Msk;
     } else {
-        NRF_RADIO->PACKETPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+        NRF_RADIO->PACKETPTR = (uint32_t)dptr;
     }
 #else
-    NRF_RADIO->PACKETPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+    NRF_RADIO->PACKETPTR = (uint32_t)dptr;
 #endif
 
 #if (BLE_LL_CFG_FEAT_LL_PRIVACY == 1)
     if (g_ble_phy_data.phy_privacy) {
+        dptr += 3;
+        NRF_RADIO->PACKETPTR = (uint32_t)dptr;
         NRF_RADIO->PCNF0 = (6 << RADIO_PCNF0_LFLEN_Pos) |
                            (2 << RADIO_PCNF0_S1LEN_Pos) |
                            (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
         NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Enabled;
         NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
-        NRF_AAR->ADDRPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+        NRF_AAR->ADDRPTR = (uint32_t)dptr;
         NRF_AAR->SCRATCHPTR = (uint32_t)&g_ble_phy_data.phy_aar_scratch;
         NRF_AAR->EVENTS_END = 0;
         NRF_AAR->EVENTS_RESOLVED = 0;
@@ -289,6 +339,7 @@ ble_phy_rx_xcvr_setup(void)
     /* Reset the rx started flag. Used for the wait for response */
     g_ble_phy_data.phy_rx_started = 0;
     g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
+    g_ble_phy_data.rxdptr = dptr;
 
     /* I want to know when 1st byte received (after address) */
     NRF_RADIO->BCC = 8; /* in bits */
@@ -313,16 +364,28 @@ ble_phy_rx_xcvr_setup(void)
 static void
 ble_phy_tx_end_isr(void)
 {
+    uint8_t was_encrypted;
     uint8_t transition;
     uint8_t txlen;
     uint32_t wfr_time;
+    uint32_t txstart;
+
+    /*
+     * Read captured tx start time. This is not the actual transmit start
+     * time but it is the time at which the address event occurred
+     * (after transmission of access address)
+     */
+    txstart = NRF_TIMER0->CC[1];
+
+    /* If this transmission was encrypted we need to remember it */
+    was_encrypted = g_ble_phy_data.phy_encrypted;
 
     /* Better be in TX state! */
     assert(g_ble_phy_data.phy_state == BLE_PHY_STATE_TX);
 
     /* Log the event */
-    ble_ll_log(BLE_LL_LOG_ID_PHY_TXEND, (g_ble_phy_txrx_buf[0] >> 8) & 0xFF,
-               g_ble_phy_data.phy_encrypted, NRF_TIMER0->CC[1]);
+    ble_ll_log(BLE_LL_LOG_ID_PHY_TXEND, (g_ble_phy_tx_buf[0] >> 8) & 0xFF,
+               was_encrypted, txstart);
 
     /* Clear events and clear interrupt on disabled event */
     NRF_RADIO->EVENTS_DISABLED = 0;
@@ -335,7 +398,7 @@ ble_phy_tx_end_isr(void)
      * XXX: not sure what to do. We had a HW error during transmission.
      * For now I just count a stat but continue on like all is good.
      */
-    if (g_ble_phy_data.phy_encrypted) {
+    if (was_encrypted) {
         if (NRF_CCM->EVENTS_ERROR) {
             STATS_INC(ble_phy_stats, tx_hw_err);
             NRF_CCM->EVENTS_ERROR = 0;
@@ -343,26 +406,25 @@ ble_phy_tx_end_isr(void)
     }
 #endif
 
+    /* Call transmit end callback */
+    if (g_ble_phy_data.txend_cb) {
+        g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg);
+    }
+
     transition = g_ble_phy_data.phy_transition;
     if (transition == BLE_PHY_TRANSITION_TX_RX) {
         /* Packet pointer needs to be reset. */
-        if (g_ble_phy_data.rxpdu != NULL) {
-            ble_phy_rx_xcvr_setup();
-        } else {
-            /* Disable the phy */
-            STATS_INC(ble_phy_stats, no_bufs);
-            ble_phy_disable();
-        }
+        ble_phy_rx_xcvr_setup();
 
         /*
          * Enable the wait for response timer. Note that cc #1 on
          * timer 0 contains the transmit start time
          */
         txlen = g_ble_phy_data.phy_tx_pyld_len;
-        if (txlen && g_ble_phy_data.phy_encrypted) {
+        if (txlen && was_encrypted) {
             txlen += BLE_LL_DATA_MIC_LEN;
         }
-        wfr_time = NRF_TIMER0->CC[1] - BLE_TX_LEN_USECS_M(NRF_RX_START_OFFSET);
+        wfr_time = txstart - BLE_TX_LEN_USECS_M(NRF_RX_START_OFFSET);
         wfr_time += BLE_TX_DUR_USECS_M(txlen);
         wfr_time += cputime_usecs_to_ticks(BLE_LL_WFR_USECS);
         ble_ll_wfr_enable(wfr_time);
@@ -371,22 +433,14 @@ ble_phy_tx_end_isr(void)
         NRF_PPI->CHENCLR = PPI_CHEN_CH20_Msk;
         assert(transition == BLE_PHY_TRANSITION_NONE);
     }
-
-    /* Call transmit end callback */
-    if (g_ble_phy_data.txend_cb) {
-        g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg);
-    }
 }
 
 static void
 ble_phy_rx_end_isr(void)
 {
     int rc;
-#if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
     uint8_t *dptr;
-#endif
     uint8_t crcok;
-    struct os_mbuf *rxpdu;
     struct ble_mbuf_hdr *ble_hdr;
 
     /* Clear events and clear interrupt */
@@ -397,12 +451,12 @@ ble_phy_rx_end_isr(void)
     NRF_PPI->CHENCLR = PPI_CHEN_CH21_Msk;
 
     /* Set RSSI and CRC status flag in header */
-    ble_hdr = BLE_MBUF_HDR_PTR(g_ble_phy_data.rxpdu);
+    ble_hdr = &g_ble_phy_data.rxhdr;
     assert(NRF_RADIO->EVENTS_RSSIEND != 0);
     ble_hdr->rxinfo.rssi = -1 * NRF_RADIO->RSSISAMPLE;
-#if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
-    dptr = g_ble_phy_data.rxpdu->om_data;
-#endif
+
+    dptr = g_ble_phy_data.rxdptr;
+
     /* Count PHY crc errors and valid packets */
     crcok = (uint8_t)NRF_RADIO->CRCSTATUS;
     if (!crcok) {
@@ -441,10 +495,6 @@ ble_phy_rx_end_isr(void)
 #endif
     }
 
-    /* Call Link Layer receive payload function */
-    rxpdu = g_ble_phy_data.rxpdu;
-    g_ble_phy_data.rxpdu = NULL;
-
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1) || (BLE_LL_CFG_FEAT_LL_PRIVACY == 1)
     if (g_ble_phy_data.phy_encrypted || g_ble_phy_data.phy_privacy) {
         /*
@@ -454,10 +504,10 @@ ble_phy_rx_end_isr(void)
          */
         dptr[2] = dptr[1];
         dptr[1] = dptr[0];
-        rxpdu->om_data += 1;
+        ++dptr;
     }
 #endif
-    rc = ble_ll_rx_end(rxpdu, ble_hdr);
+    rc = ble_ll_rx_end(dptr, ble_hdr);
     if (rc < 0) {
         ble_phy_disable();
     }
@@ -474,8 +524,6 @@ ble_phy_rx_start_isr(void)
     NRF_RADIO->EVENTS_ADDRESS = 0;
     NRF_RADIO->INTENCLR = RADIO_INTENCLR_ADDRESS_Msk;
 
-    assert(g_ble_phy_data.rxpdu != NULL);
-
     /* Wait to get 1st byte of frame */
     while (1) {
         state = NRF_RADIO->STATE;
@@ -495,7 +543,7 @@ ble_phy_rx_start_isr(void)
     }
 
     /* Initialize flags, channel and state in ble header at rx start */
-    ble_hdr = BLE_MBUF_HDR_PTR(g_ble_phy_data.rxpdu);
+    ble_hdr = &g_ble_phy_data.rxhdr;
     ble_hdr->rxinfo.flags = ble_ll_state_get();
     ble_hdr->rxinfo.channel = g_ble_phy_data.phy_chan;
     ble_hdr->rxinfo.handle = 0;
@@ -503,7 +551,8 @@ ble_phy_rx_start_isr(void)
         BLE_TX_LEN_USECS_M(NRF_RX_START_OFFSET);
 
     /* Call Link Layer receive start function */
-    rc = ble_ll_rx_start(g_ble_phy_data.rxpdu, g_ble_phy_data.phy_chan);
+    rc = ble_ll_rx_start(g_ble_phy_data.rxdptr, g_ble_phy_data.phy_chan,
+                         &g_ble_phy_data.rxhdr);
     if (rc >= 0) {
         /* Set rx started flag and enable rx end ISR */
         g_ble_phy_data.phy_rx_started = 1;
@@ -672,11 +721,6 @@ ble_phy_rx(void)
         return BLE_PHY_ERR_RADIO_STATE;
     }
 
-    /* If no pdu, get one */
-    if (ble_phy_rxpdu_get() == NULL) {
-        return BLE_PHY_ERR_NO_BUFS;
-    }
-
     /* Make sure all interrupts are disabled */
     NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
 
@@ -855,7 +899,7 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
 
         NRF_CCM->SHORTS = 1;
         NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
-        NRF_CCM->OUTPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
+        NRF_CCM->OUTPTR = (uint32_t)&g_ble_phy_tx_buf[0];
         NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
         NRF_CCM->EVENTS_ERROR = 0;
         NRF_CCM->MODE = CCM_MODE_MODE_Encryption;
@@ -872,7 +916,7 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
         NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
 #endif
         /* RAM representation has S0 and LENGTH fields (2 bytes) */
-        dptr = (uint8_t *)&g_ble_phy_txrx_buf[0];
+        dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
         dptr[0] = ble_hdr->txinfo.hdr_byte;
         dptr[1] = payload_len;
         dptr += 2;
@@ -887,13 +931,13 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
 #endif
 
     /* RAM representation has S0 and LENGTH fields (2 bytes) */
-    dptr = (uint8_t *)&g_ble_phy_txrx_buf[0];
+    dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
     dptr[0] = ble_hdr->txinfo.hdr_byte;
     dptr[1] = payload_len;
     dptr += 2;
 #endif
 
-    NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
+    NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_tx_buf[0];
 
     /* Clear the ready, end and disabled events */
     NRF_RADIO->EVENTS_READY = 0;
@@ -903,13 +947,10 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
     /* Enable shortcuts for transmit start/end. */
     shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk;
     if (end_trans == BLE_PHY_TRANSITION_TX_RX) {
-        /* If we are going into receive after this, try to get a buffer. */
-        if (ble_phy_rxpdu_get()) {
-            shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk;
-        }
+        shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk;
     }
-    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
     NRF_RADIO->SHORTS = shortcuts;
+    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
 
     /* Set transmitted payload length */
     g_ble_phy_data.phy_tx_pyld_len = payload_len;
diff --git a/net/nimble/drivers/nrf52/src/ble_phy.c b/net/nimble/drivers/nrf52/src/ble_phy.c
index 541ece95..2e346699 100644
--- a/net/nimble/drivers/nrf52/src/ble_phy.c
+++ b/net/nimble/drivers/nrf52/src/ble_phy.c
@@ -29,17 +29,12 @@
 #include "controller/ble_ll.h"
 #include "mcu/nrf52_bitfields.h"
 
-/*
- * XXX: need to make the copy from mbuf into the PHY data structures 32-bit
- * copies or we are screwed.
- */
-
 /* XXX: 4) Make sure RF is higher priority interrupt than schedule */
 
 /*
  * XXX: Maximum possible transmit time is 1 msec for a 60ppm crystal
  * and 16ms for a 30ppm crystal! We need to limit PDU size based on
- * crystal accuracy
+ * crystal accuracy. Look at this in the spec.
  */
 
 /* XXX: private header file? */
@@ -79,7 +74,7 @@ struct ble_phy_obj
     uint8_t phy_tx_pyld_len;
     uint32_t phy_aar_scratch;
     uint32_t phy_access_address;
-    struct os_mbuf *rxpdu;
+    struct ble_mbuf_hdr rxhdr;
     void *txend_arg;
     ble_phy_tx_end_func txend_cb;
 };
@@ -87,7 +82,8 @@ struct ble_phy_obj g_ble_phy_data;
 
 /* XXX: if 27 byte packets desired we can make this smaller */
 /* Global transmit/receive buffer */
-static uint32_t g_ble_phy_txrx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+static uint32_t g_ble_phy_tx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+static uint32_t g_ble_phy_rx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
 
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
 /* Make sure word-aligned for faster copies */
@@ -106,7 +102,6 @@ STATS_SECT_START(ble_phy_stats)
     STATS_SECT_ENTRY(rx_valid)
     STATS_SECT_ENTRY(rx_crc_err)
     STATS_SECT_ENTRY(rx_late)
-    STATS_SECT_ENTRY(no_bufs)
     STATS_SECT_ENTRY(radio_state_errs)
     STATS_SECT_ENTRY(rx_hw_err)
     STATS_SECT_ENTRY(tx_hw_err)
@@ -124,7 +119,6 @@ STATS_NAME_START(ble_phy_stats)
     STATS_NAME(ble_phy_stats, rx_valid)
     STATS_NAME(ble_phy_stats, rx_crc_err)
     STATS_NAME(ble_phy_stats, rx_late)
-    STATS_NAME(ble_phy_stats, no_bufs)
     STATS_NAME(ble_phy_stats, radio_state_errs)
     STATS_NAME(ble_phy_stats, rx_hw_err)
     STATS_NAME(ble_phy_stats, tx_hw_err)
@@ -185,33 +179,81 @@ struct nrf_ccm_data g_nrf_ccm_data;
 #endif
 
 /**
- * ble phy rxpdu get
+ * Copies the data from the phy receive buffer into a mbuf chain.
  *
- * Gets a mbuf for PDU reception.
+ * @param dptr Pointer to receive buffer
+ * @param rxpdu Pointer to already allocated mbuf chain
+ *
+ * NOTE: the packet header already has the total mbuf length in it. The
+ * lengths of the individual mbufs are not set prior to calling.
  *
- * @return struct os_mbuf* Pointer to retrieved mbuf or NULL if none available
  */
-static struct os_mbuf *
-ble_phy_rxpdu_get(void)
+void
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
 {
+    uint16_t rem_bytes;
+    uint16_t mb_bytes;
+    uint16_t copylen;
+    uint32_t *dst;
+    uint32_t *src;
     struct os_mbuf *m;
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf_pkthdr *pkthdr;
+
+    /* Better be aligned */
+    assert(((uint32_t)dptr & 3) == 0);
+
+    pkthdr = OS_MBUF_PKTHDR(rxpdu);
+    rem_bytes = pkthdr->omp_len;
+
+    /* Fill in the mbuf pkthdr first. */
+    dst = (uint32_t *)(rxpdu->om_data);
+    src = (uint32_t *)dptr;
+
+    mb_bytes = (rxpdu->om_omp->omp_databuf_len - rxpdu->om_pkthdr_len - 4);
+    copylen = min(mb_bytes, rem_bytes);
+    copylen &= 0xFFFC;
+    rem_bytes -= copylen;
+    mb_bytes -= copylen;
+    rxpdu->om_len = copylen;
+    while (copylen > 0) {
+        *dst = *src;
+        ++dst;
+        ++src;
+        copylen -= 4;
+    }
 
-    m = g_ble_phy_data.rxpdu;
-    if (m == NULL) {
-        m = os_msys_get_pkthdr(BLE_MBUF_PAYLOAD_SIZE, sizeof(struct ble_mbuf_hdr));
-        if (!m) {
-            STATS_INC(ble_phy_stats, no_bufs);
-        } else {
-            /*
-             * NOTE: we add two bytes to the data pointer as we will prepend
-             * two bytes if we hand this received pdu up to host.
-             */
-            m->om_data += 2;
-            g_ble_phy_data.rxpdu = m;
+    /* Copy remaining bytes */
+    m = rxpdu;
+    while (rem_bytes > 0) {
+        /* If there are enough bytes in the mbuf, copy them and leave */
+        if (rem_bytes <= mb_bytes) {
+            memcpy(m->om_data + m->om_len, src, rem_bytes);
+            m->om_len += rem_bytes;
+            break;
+        }
+
+        m = SLIST_NEXT(m, om_next);
+        assert(m != NULL);
+
+        mb_bytes = m->om_omp->omp_databuf_len;
+        copylen = min(mb_bytes, rem_bytes);
+        copylen &= 0xFFFC;
+        rem_bytes -= copylen;
+        mb_bytes -= copylen;
+        m->om_len = copylen;
+        dst = (uint32_t *)m->om_data;
+        while (copylen > 0) {
+            *dst = *src;
+            ++dst;
+            ++src;
+            copylen -= 4;
         }
     }
 
-    return m;
+    /* Copy ble header */
+    ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
+    memcpy(ble_hdr, &g_ble_phy_data.rxhdr, sizeof(struct ble_mbuf_hdr));
 }
 
 /**
@@ -243,11 +285,16 @@ nrf_wait_disabled(void)
 static void
 ble_phy_rx_xcvr_setup(void)
 {
+    uint8_t *dptr;
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+    dptr += 3;
+
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
     if (g_ble_phy_data.phy_encrypted) {
         NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_enc_buf[0];
         NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
-        NRF_CCM->OUTPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+        NRF_CCM->OUTPTR = (uint32_t)dptr;
         NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
         NRF_CCM->MODE = CCM_MODE_LENGTH_Msk | CCM_MODE_MODE_Decryption;
         NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
@@ -256,17 +303,17 @@ ble_phy_rx_xcvr_setup(void)
         NRF_CCM->EVENTS_ENDCRYPT = 0;
         NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk | PPI_CHEN_CH25_Msk;
     } else {
-        NRF_RADIO->PACKETPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+        NRF_RADIO->PACKETPTR = (uint32_t)dptr;
     }
 #else
-    NRF_RADIO->PACKETPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+    NRF_RADIO->PACKETPTR = (uint32_t)dptr;
 #endif
 
 #if (BLE_LL_CFG_FEAT_LL_PRIVACY == 1)
     if (g_ble_phy_data.phy_privacy) {
         NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Enabled;
         NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
-        NRF_AAR->ADDRPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+        NRF_AAR->ADDRPTR = (uint32_t)dptr;
         NRF_AAR->SCRATCHPTR = (uint32_t)&g_ble_phy_data.phy_aar_scratch;
         NRF_AAR->EVENTS_END = 0;
         NRF_AAR->EVENTS_RESOLVED = 0;
@@ -308,16 +355,28 @@ ble_phy_rx_xcvr_setup(void)
 static void
 ble_phy_tx_end_isr(void)
 {
+    uint8_t was_encrypted;
     uint8_t transition;
     uint8_t txlen;
     uint32_t wfr_time;
+    uint32_t txstart;
+
+    /*
+     * Read captured tx start time. This is not the actual transmit start
+     * time but it is the time at which the address event occurred
+     * (after transmission of access address)
+     */
+    txstart = NRF_TIMER0->CC[1];
+
+    /* If this transmission was encrypted we need to remember it */
+    was_encrypted = g_ble_phy_data.phy_encrypted;
 
     /* Better be in TX state! */
     assert(g_ble_phy_data.phy_state == BLE_PHY_STATE_TX);
 
     /* Log the event */
-    ble_ll_log(BLE_LL_LOG_ID_PHY_TXEND, (g_ble_phy_txrx_buf[0] >> 8) & 0xFF,
-               g_ble_phy_data.phy_encrypted, NRF_TIMER0->CC[1]);
+    ble_ll_log(BLE_LL_LOG_ID_PHY_TXEND, (g_ble_phy_tx_buf[0] >> 8) & 0xFF,
+               was_encrypted, txstart);
 
     /* Clear events and clear interrupt on disabled event */
     NRF_RADIO->EVENTS_DISABLED = 0;
@@ -330,7 +389,7 @@ ble_phy_tx_end_isr(void)
      * XXX: not sure what to do. We had a HW error during transmission.
      * For now I just count a stat but continue on like all is good.
      */
-    if (g_ble_phy_data.phy_encrypted) {
+    if (was_encrypted) {
         if (NRF_CCM->EVENTS_ERROR) {
             STATS_INC(ble_phy_stats, tx_hw_err);
             NRF_CCM->EVENTS_ERROR = 0;
@@ -338,26 +397,25 @@ ble_phy_tx_end_isr(void)
     }
 #endif
 
+    /* Call transmit end callback */
+    if (g_ble_phy_data.txend_cb) {
+        g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg);
+    }
+
     transition = g_ble_phy_data.phy_transition;
     if (transition == BLE_PHY_TRANSITION_TX_RX) {
         /* Packet pointer needs to be reset. */
-        if (g_ble_phy_data.rxpdu != NULL) {
-            ble_phy_rx_xcvr_setup();
-        } else {
-            /* Disable the phy */
-            STATS_INC(ble_phy_stats, no_bufs);
-            ble_phy_disable();
-        }
+        ble_phy_rx_xcvr_setup();
 
         /*
          * Enable the wait for response timer. Note that cc #1 on
          * timer 0 contains the transmit start time
          */
         txlen = g_ble_phy_data.phy_tx_pyld_len;
-        if (txlen && g_ble_phy_data.phy_encrypted) {
+        if (txlen && was_encrypted) {
             txlen += BLE_LL_DATA_MIC_LEN;
         }
-        wfr_time = NRF_TIMER0->CC[1] - BLE_TX_LEN_USECS_M(NRF_RX_START_OFFSET);
+        wfr_time = txstart - BLE_TX_LEN_USECS_M(NRF_RX_START_OFFSET);
         wfr_time += BLE_TX_DUR_USECS_M(txlen);
         wfr_time += cputime_usecs_to_ticks(BLE_LL_WFR_USECS);
         ble_ll_wfr_enable(wfr_time);
@@ -366,11 +424,6 @@ ble_phy_tx_end_isr(void)
         NRF_PPI->CHENCLR = PPI_CHEN_CH20_Msk;
         assert(transition == BLE_PHY_TRANSITION_NONE);
     }
-
-    /* Call transmit end callback */
-    if (g_ble_phy_data.txend_cb) {
-        g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg);
-    }
 }
 
 static void
@@ -379,7 +432,6 @@ ble_phy_rx_end_isr(void)
     int rc;
     uint8_t *dptr;
     uint8_t crcok;
-    struct os_mbuf *rxpdu;
     struct ble_mbuf_hdr *ble_hdr;
 
     /* Clear events and clear interrupt */
@@ -390,11 +442,12 @@ ble_phy_rx_end_isr(void)
     NRF_PPI->CHENCLR = PPI_CHEN_CH21_Msk;
 
     /* Set RSSI and CRC status flag in header */
-    ble_hdr = BLE_MBUF_HDR_PTR(g_ble_phy_data.rxpdu);
+    ble_hdr = &g_ble_phy_data.rxhdr;
     assert(NRF_RADIO->EVENTS_RSSIEND != 0);
     ble_hdr->rxinfo.rssi = -1 * NRF_RADIO->RSSISAMPLE;
 
-    dptr = g_ble_phy_data.rxpdu->om_data;
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+    dptr += 3;
 
     /* Count PHY crc errors and valid packets */
     crcok = (uint8_t)NRF_RADIO->CRCSTATUS;
@@ -434,10 +487,6 @@ ble_phy_rx_end_isr(void)
 #endif
     }
 
-    /* Call Link Layer receive payload function */
-    rxpdu = g_ble_phy_data.rxpdu;
-    g_ble_phy_data.rxpdu = NULL;
-
     /*
      * XXX: This is a horrible ugly hack to deal with the RAM S1 byte
      * that is not sent over the air but is present here. Simply move the
@@ -445,9 +494,7 @@ ble_phy_rx_end_isr(void)
      */
     dptr[2] = dptr[1];
     dptr[1] = dptr[0];
-    rxpdu->om_data += 1;
-
-    rc = ble_ll_rx_end(rxpdu, ble_hdr);
+    rc = ble_ll_rx_end(dptr + 1, ble_hdr);
     if (rc < 0) {
         ble_phy_disable();
     }
@@ -464,8 +511,6 @@ ble_phy_rx_start_isr(void)
     NRF_RADIO->EVENTS_ADDRESS = 0;
     NRF_RADIO->INTENCLR = RADIO_INTENCLR_ADDRESS_Msk;
 
-    assert(g_ble_phy_data.rxpdu != NULL);
-
     /* Wait to get 1st byte of frame */
     while (1) {
         state = NRF_RADIO->STATE;
@@ -485,7 +530,7 @@ ble_phy_rx_start_isr(void)
     }
 
     /* Initialize flags, channel and state in ble header at rx start */
-    ble_hdr = BLE_MBUF_HDR_PTR(g_ble_phy_data.rxpdu);
+    ble_hdr = &g_ble_phy_data.rxhdr;
     ble_hdr->rxinfo.flags = ble_ll_state_get();
     ble_hdr->rxinfo.channel = g_ble_phy_data.phy_chan;
     ble_hdr->rxinfo.handle = 0;
@@ -493,7 +538,9 @@ ble_phy_rx_start_isr(void)
         BLE_TX_LEN_USECS_M(NRF_RX_START_OFFSET);
 
     /* Call Link Layer receive start function */
-    rc = ble_ll_rx_start(g_ble_phy_data.rxpdu, g_ble_phy_data.phy_chan);
+    rc = ble_ll_rx_start((uint8_t *)&g_ble_phy_rx_buf[0] + 3,
+                         g_ble_phy_data.phy_chan,
+                         &g_ble_phy_data.rxhdr);
     if (rc >= 0) {
         /* Set rx started flag and enable rx end ISR */
         g_ble_phy_data.phy_rx_started = 1;
@@ -589,6 +636,7 @@ ble_phy_init(void)
                        RADIO_PCNF0_S1INCL_Msk                       |
                        (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos)        |
                        (RADIO_PCNF0_PLEN_8bit << RADIO_PCNF0_PLEN_Pos);
+
     /* XXX: should maxlen be 251 for encryption? */
     NRF_RADIO->PCNF1 = NRF_MAXLEN |
                        (RADIO_PCNF1_ENDIAN_Little <<  RADIO_PCNF1_ENDIAN_Pos) |
@@ -664,11 +712,6 @@ ble_phy_rx(void)
         return BLE_PHY_ERR_RADIO_STATE;
     }
 
-    /* If no pdu, get one */
-    if (ble_phy_rxpdu_get() == NULL) {
-        return BLE_PHY_ERR_NO_BUFS;
-    }
-
     /* Make sure all interrupts are disabled */
     NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
 
@@ -809,6 +852,7 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
 {
     int rc;
     uint8_t *dptr;
+    uint8_t *pktptr;
     uint8_t payload_len;
     uint32_t state;
     uint32_t shortcuts;
@@ -829,9 +873,11 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
 #if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 1)
     if (g_ble_phy_data.phy_encrypted) {
         dptr = (uint8_t *)&g_ble_phy_enc_buf[0];
+        ++dptr;
+        pktptr = (uint8_t *)&g_ble_phy_tx_buf[0];
         NRF_CCM->SHORTS = 1;
-        NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
-        NRF_CCM->OUTPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
+        NRF_CCM->INPTR = (uint32_t)dptr;
+        NRF_CCM->OUTPTR = (uint32_t)pktptr;
         NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
         NRF_CCM->EVENTS_ERROR = 0;
         NRF_CCM->MODE = CCM_MODE_LENGTH_Msk;
@@ -843,13 +889,17 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
         NRF_PPI->CHENCLR = PPI_CHEN_CH23_Msk;
         NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
 #endif
-        dptr = (uint8_t *)&g_ble_phy_txrx_buf[0];
+        dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+        ++dptr;
+        pktptr = dptr;
     }
 #else
 #if (BLE_LL_CFG_FEAT_LL_PRIVACY == 1)
     NRF_PPI->CHENCLR = PPI_CHEN_CH23_Msk;
 #endif
-    dptr = (uint8_t *)&g_ble_phy_txrx_buf[0];
+    dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+    ++dptr;
+    pktptr = dptr;
 #endif
 
     /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
@@ -857,7 +907,7 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
     dptr[1] = payload_len;
     dptr[2] = 0;
     dptr += 3;
-    NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
+    NRF_RADIO->PACKETPTR = (uint32_t)pktptr;
 
     /* Clear the ready, end and disabled events */
     NRF_RADIO->EVENTS_READY = 0;
@@ -867,13 +917,10 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans)
     /* Enable shortcuts for transmit start/end. */
     shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk;
     if (end_trans == BLE_PHY_TRANSITION_TX_RX) {
-        /* If we are going into receive after this, try to get a buffer. */
-        if (ble_phy_rxpdu_get()) {
-            shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk;
-        }
+        shortcuts |= RADIO_SHORTS_DISABLED_RXEN_Msk;
     }
-    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
     NRF_RADIO->SHORTS = shortcuts;
+    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
 
     /* Set transmitted payload length */
     g_ble_phy_data.phy_tx_pyld_len = payload_len;
diff --git a/net/nimble/host/include/host/ble_att.h b/net/nimble/host/include/host/ble_att.h
index 53b4e19f..24b44961 100644
--- a/net/nimble/host/include/host/ble_att.h
+++ b/net/nimble/host/include/host/ble_att.h
@@ -21,6 +21,7 @@
 #define H_BLE_ATT_
 
 #include "os/queue.h"
+struct os_mbuf;
 
 #define BLE_ATT_UUID_PRIMARY_SERVICE        0x2800
 #define BLE_ATT_UUID_SECONDARY_SERVICE      0x2801
@@ -31,14 +32,17 @@
 #define BLE_ATT_ERR_READ_NOT_PERMITTED      0x02
 #define BLE_ATT_ERR_WRITE_NOT_PERMITTED     0x03
 #define BLE_ATT_ERR_INVALID_PDU             0x04
-#define BLE_ATT_ERR_INSUFFICIENT_AUTHENT    0x05
+#define BLE_ATT_ERR_INSUFFICIENT_AUTHEN     0x05
 #define BLE_ATT_ERR_REQ_NOT_SUPPORTED       0x06
 #define BLE_ATT_ERR_INVALID_OFFSET          0x07
+#define BLE_ATT_ERR_INSUFFICIENT_AUTHOR     0x08
 #define BLE_ATT_ERR_PREPARE_QUEUE_FULL      0x09
 #define BLE_ATT_ERR_ATTR_NOT_FOUND          0x0a
 #define BLE_ATT_ERR_ATTR_NOT_LONG           0x0b
+#define BLE_ATT_ERR_INSUFFICIENT_KEY_SZ     0x0c
 #define BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN  0x0d
 #define BLE_ATT_ERR_UNLIKELY                0x0e
+#define BLE_ATT_ERR_INSUFFICIENT_ENC        0x0f
 #define BLE_ATT_ERR_UNSUPPORTED_GROUP       0x10
 #define BLE_ATT_ERR_INSUFFICIENT_RES        0x11
 
@@ -86,44 +90,15 @@
 #define BLE_ATT_ACCESS_OP_READ              1
 #define BLE_ATT_ACCESS_OP_WRITE             2
 
-struct ble_att_svr_access_ctxt {
-    void *attr_data;
-    uint16_t data_len;
-    uint16_t offset; /* Only used for read-blob requests. */
-};
+#define BLE_ATT_MTU_DFLT                23  /* Also the minimum. */
+#define BLE_ATT_MTU_MAX                 240
+#define BLE_ATT_MTU_PREFERRED_DFLT      240
 
-/**
- * Handles a host attribute request.
- *
- * @param entry                 The host attribute being requested.
- * @param op                    The operation being performed on the attribute.
- * @param arg                   The request data associated with that host
- *                                  attribute.
- *
- * @return                      0 on success;
- *                              One of the BLE_ATT_ERR_[...] codes on
- *                                  failure.
- */
-typedef int ble_att_svr_access_fn(uint16_t conn_handle, uint16_t attr_handle,
-                                  uint8_t *uuid128, uint8_t op,
-                                  struct ble_att_svr_access_ctxt *ctxt,
-                                  void *arg);
-
-int ble_att_svr_register(uint8_t *uuid, uint8_t flags, uint16_t *handle_id,
-                         ble_att_svr_access_fn *cb, void *cb_arg);
-int ble_att_svr_register_uuid16(uint16_t uuid16, uint8_t flags,
-                                uint16_t *handle_id, ble_att_svr_access_fn *cb,
-                                void *cb_arg);
-
-typedef int ble_att_svr_notify_fn(uint16_t conn_handle, uint16_t attr_handle,
-                                  uint8_t *attr_val, uint16_t attr_len,
-                                  void *arg);
-
-int ble_att_svr_read_local(uint16_t attr_handle, void **out_data,
-                           uint16_t *out_attr_len);
-int ble_att_svr_write_local(uint16_t attr_handle, void *data,
-                            uint16_t data_len);
+int ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om);
+int ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om);
 
+uint16_t ble_att_mtu(uint16_t conn_handle);
+uint16_t ble_att_preferred_mtu(void);
 int ble_att_set_preferred_mtu(uint16_t mtu);
 
 #endif
diff --git a/net/nimble/host/include/host/ble_gap.h b/net/nimble/host/include/host/ble_gap.h
index 23b8f462..960ad7b2 100644
--- a/net/nimble/host/include/host/ble_gap.h
+++ b/net/nimble/host/include/host/ble_gap.h
@@ -24,7 +24,6 @@
 #include "host/ble_hs.h"
 struct hci_le_conn_complete;
 struct hci_conn_update;
-struct hci_adv_params;
 
 /** 30 ms. */
 #define BLE_GAP_ADV_FAST_INTERVAL1_MIN      (30 * 1000 / BLE_HCI_ADV_ITVL)
@@ -44,6 +43,12 @@ struct hci_adv_params;
 /** 60 ms; active scanning. */
 #define BLE_GAP_SCAN_FAST_INTERVAL_MAX      (60 * 1000 / BLE_HCI_ADV_ITVL)
 
+/** 11.25 ms; limited discovery interval. */
+#define BLE_GAP_LIM_DISC_SCAN_INT           (11.25 * 1000 / BLE_HCI_SCAN_ITVL)
+
+/** 11.25 ms; limited discovery window (not from the spec). */
+#define BLE_GAP_LIM_DISC_SCAN_WINDOW        (11.25 * 1000 / BLE_HCI_SCAN_ITVL)
+
 /** 30 ms; active scanning. */
 #define BLE_GAP_SCAN_FAST_WINDOW            (30 * 1000 / BLE_HCI_SCAN_ITVL)
 
@@ -57,7 +62,10 @@ struct hci_adv_params;
 #define BLE_GAP_SCAN_SLOW_WINDOW1           (11.25 * 1000 / BLE_HCI_SCAN_ITVL)
 
 /** 10.24 seconds. */
-#define BLE_GAP_GEN_DISC_SCAN_MIN           (10.24 * 1000)
+#define BLE_GAP_DISC_DUR_DFLT               (10.24 * 1000)
+
+/** 30 seconds (not from the spec). */
+#define BLE_GAP_CONN_DUR_DFLT               (30 * 1000)
 
 /** 1 second. */
 #define BLE_GAP_CONN_PAUSE_CENTRAL          (1 * 1000)
@@ -71,21 +79,18 @@ struct hci_adv_params;
 /* 50 ms. */
 #define BLE_GAP_INITIAL_CONN_ITVL_MAX       (50 * 1000 / BLE_HCI_CONN_ITVL)
 
+#define BLE_GAP_ADV_DFLT_CHANNEL_MAP        0x07 /* All three channels. */
+
 #define BLE_GAP_INITIAL_CONN_LATENCY        0
 #define BLE_GAP_INITIAL_SUPERVISION_TIMEOUT 0x0100
 #define BLE_GAP_INITIAL_CONN_MIN_CE_LEN     0x0010
 #define BLE_GAP_INITIAL_CONN_MAX_CE_LEN     0x0300
 
-#define BLE_GAP_SVC_UUID16                              0x1800
-#define BLE_GAP_CHR_UUID16_DEVICE_NAME                  0x2a00
-#define BLE_GAP_CHR_UUID16_APPEARANCE                   0x2a01
-#define BLE_GAP_CHR_UUID16_PERIPH_PRIV_FLAG             0x2a02
-#define BLE_GAP_CHR_UUID16_RECONNECT_ADDR               0x2a03
-#define BLE_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS      0x2a04
-
-#define BLE_GAP_APPEARANCE_GEN_COMPUTER                 128
-
 #define BLE_GAP_ADDR_TYPE_WL                0xff
+#define BLE_GAP_ADDR_TYPE_NONE              0xfe
+
+#define BLE_GAP_ROLE_MASTER                 0
+#define BLE_GAP_ROLE_SLAVE                  1
 
 #define BLE_GAP_EVENT_CONNECT               0
 #define BLE_GAP_EVENT_DISCONNECT            1
@@ -94,12 +99,29 @@ struct hci_adv_params;
 #define BLE_GAP_EVENT_CONN_UPDATE_REQ       4
 #define BLE_GAP_EVENT_L2CAP_UPDATE_REQ      5
 #define BLE_GAP_EVENT_TERM_FAILURE          6
-#define BLE_GAP_EVENT_DISC_SUCCESS          7
+#define BLE_GAP_EVENT_DISC                  7
 #define BLE_GAP_EVENT_DISC_COMPLETE         8
 #define BLE_GAP_EVENT_ADV_COMPLETE          9
 #define BLE_GAP_EVENT_ENC_CHANGE            10
 #define BLE_GAP_EVENT_PASSKEY_ACTION        11
-#define BLE_GAP_EVENT_NOTIFY                12
+#define BLE_GAP_EVENT_NOTIFY_RX             12
+#define BLE_GAP_EVENT_NOTIFY_TX             13
+#define BLE_GAP_EVENT_SUBSCRIBE             14
+#define BLE_GAP_EVENT_MTU                   15
+
+/*** Reason codes for the subscribe GAP event. */
+
+/** Peer's CCCD subscription state changed due to a descriptor write. */
+#define BLE_GAP_SUBSCRIBE_REASON_WRITE      1
+
+/** Peer's CCCD subscription state cleared due to connection termination. */
+#define BLE_GAP_SUBSCRIBE_REASON_TERM       2
+
+/**
+ * Peer's CCCD subscription state changed due to restore from persistence
+ * (bonding restored).
+ */
+#define BLE_GAP_SUBSCRIBE_REASON_RESTORE    3
 
 struct ble_gap_sec_state {
     unsigned encrypted:1;
@@ -107,13 +129,33 @@ struct ble_gap_sec_state {
     unsigned bonded:1;
 };
 
+/**
+ * @param discoverable_mode     One of the following constants:
+ *                                  o BLE_GAP_DISC_MODE_NON
+ *                                      (non-discoverable; 3.C.9.2.2).
+ *                                  o BLE_GAP_DISC_MODE_LTD
+ *                                      (limited-discoverable; 3.C.9.2.3).
+ *                                  o BLE_GAP_DISC_MODE_GEN
+ *                                      (general-discoverable; 3.C.9.2.4).
+ * @param connectable_mode      One of the following constants:
+ *                                  o BLE_GAP_CONN_MODE_NON
+ *                                      (non-connectable; 3.C.9.3.2).
+ *                                  o BLE_GAP_CONN_MODE_DIR
+ *                                      (directed-connectable; 3.C.9.3.3).
+ *                                  o BLE_GAP_CONN_MODE_UND
+ *                                      (undirected-connectable; 3.C.9.3.4).
+ */
 struct ble_gap_adv_params {
-    uint8_t adv_type;
-    uint8_t adv_channel_map;
-    uint8_t own_addr_type;
-    uint8_t adv_filter_policy;
-    uint16_t adv_itvl_min;
-    uint16_t adv_itvl_max;
+    /*** Mandatory fields. */
+    uint8_t conn_mode;
+    uint8_t disc_mode;
+
+    /*** Optional fields; assign 0 to make the stack calculate them. */
+    uint16_t itvl_min;
+    uint16_t itvl_max;
+    uint8_t channel_map;
+    uint8_t filter_policy;
+    uint8_t high_duty_cycle:1;
 };
 
 struct ble_gap_conn_desc {
@@ -130,12 +172,13 @@ struct ble_gap_conn_desc {
     uint8_t peer_id_addr_type;
     uint8_t our_id_addr_type;
     uint8_t our_ota_addr_type;
+    uint8_t role;
+    uint8_t master_clock_accuracy;
 };
 
-struct ble_gap_crt_params {
-    uint16_t scan_window;
+struct ble_gap_conn_params {
     uint16_t scan_itvl;
-    uint8_t our_addr_type;
+    uint16_t scan_window;
     uint16_t itvl_min;
     uint16_t itvl_max;
     uint16_t latency;
@@ -144,6 +187,15 @@ struct ble_gap_crt_params {
     uint16_t max_ce_len;
 };
 
+struct ble_gap_disc_params {
+    uint16_t itvl;
+    uint16_t window;
+    uint8_t filter_policy;
+    uint8_t limited:1;
+    uint8_t passive:1;
+    uint8_t filter_duplicates:1;
+};
+
 struct ble_gap_upd_params {
     uint16_t itvl_min;
     uint16_t itvl_max;
@@ -153,85 +205,315 @@ struct ble_gap_upd_params {
     uint16_t max_ce_len;
 };
 
-struct ble_gap_notify_params {
-    uint16_t attr_handle;
-    void *attr_data;
-    uint16_t attr_len;
-
-    unsigned indication:1;
-};
-
-struct ble_gap_enhanced_conn {
-    uint8_t peer_rpa[6];
-    uint8_t local_rpa[6];
-};
-
-struct ble_gap_passkey_action {
+struct ble_gap_passkey_params {
     uint8_t action;
     uint32_t numcmp;
 };
 
-struct ble_gap_conn_ctxt {
-    struct ble_gap_conn_desc *desc;
+struct ble_gap_disc_desc {
+    /*** Common fields. */
+    uint8_t event_type;
+    uint8_t addr_type;
+    uint8_t length_data;
+    int8_t rssi;
+    uint8_t addr[6];
+
+    /*** LE advertising report fields; both null if no data present. */
+    uint8_t *data;
+    struct ble_hs_adv_fields *fields;
+
+    /***
+     * LE direct advertising report fields; direct_addr_type is
+     * BLE_GAP_ADDR_TYPE_NONE if direct address fields are not present.
+     */
+    uint8_t direct_addr_type;
+    uint8_t direct_addr[6];
+};
 
+/**
+ * Represents a GAP-related event.  When such an event occurs, the host
+ * notifies the application by passing an instance of this structure to an
+ * application-specified callback.
+ */
+struct ble_gap_event {
+    /**
+     * Indicates the type of GAP event that occurred.  This is one of the
+     * BLE_GAP_EVENT codes.
+     */
+    uint8_t type;
+
+    /**
+     * A discriminated union containing additional details concerning the GAP
+     * event.  The 'type' field indicates which member of the union is valid.
+     */
     union {
+        /**
+         * Represents a connection attempt.  Valid for the following event
+         * types:
+         *     o BLE_GAP_EVENT_CONNECT
+         */
         struct {
+            /**
+             * The status of the connection attempt;
+             *     o 0: the connection was successfully established.
+             *     o BLE host error code: the connection attempt failed for
+             *       the specified reason.
+             */
             int status;
-            struct ble_gap_enhanced_conn *enhanced_conn;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
         } connect;
 
+        /**
+         * Represents a terminated connection.  Valid for the following event
+         * types:
+         *     o BLE_GAP_EVENT_DISCONNECT
+         */
         struct {
+            /**
+             * A BLE host return code indicating the reason for the
+             * disconnect.
+             */
             int reason;
+
+            /** Information about the connection prior to termination. */
+            struct ble_gap_conn_desc conn;
         } disconnect;
 
+        /**
+         * Represents an advertising report received during a discovery
+         * procedure.  Valid for the following event types:
+         *     o BLE_GAP_EVENT_DISC
+         */
+        struct ble_gap_disc_desc disc;
+
+        /**
+         * Represents an attempt to update a connection's parameters.  If the
+         * attempt was successful, the connection's descriptor reflects the
+         * updated parameters.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_CONN_UPDATE
+         */
         struct {
+            /**
+             * The result of the connection update attempt;
+             *     o 0: the connection was successfully updated.
+             *     o BLE host error code: the connection update attempt failed
+             *       for the specified reason.
+             */
             int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
         } conn_update;
 
+        /**
+         * Represents a peer's request to update the connection parameters.
+         * This event is generated when a peer performs any of the following
+         * procedures:
+         *     o L2CAP Connection Parameter Update Procedure
+         *     o Link-Layer Connection Parameters Request Procedure
+         *
+         * To reject the request, return a non-zero HCI error code.  The value
+         * returned is the reject reason given to the controller.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_L2CAP_UPDATE_REQ
+         *     o BLE_GAP_EVENT_CONN_UPDATE_REQ
+         */
         struct {
+            /**
+             * Indicates the connection parameters that the peer would like to
+             * use.
+             */
+            const struct ble_gap_upd_params *peer_params;
+
+            /**
+             * Indicates the connection parameters that the local device would
+             * like to use.  The application callback should fill this in.  By
+             * default, this struct contains the requested parameters (i.e.,
+             * it is a copy of 'peer_params').
+             */
             struct ble_gap_upd_params *self_params;
-            struct ble_gap_upd_params *peer_params;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
         } conn_update_req;
 
+        /**
+         * Represents a failed attempt to terminate an established connection.
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_TERM_FAILURE
+         */
         struct {
+            /**
+             * A BLE host return code indicating the reason for the failure.
+             */
             int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
         } term_failure;
 
+        /**
+         * Represents an attempt to change the encrypted state of a
+         * connection.  If the attempt was successful, the connection
+         * descriptor reflects the updated encrypted state.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_ENC_CHANGE
+         */
         struct {
+            /**
+             * Indicates the result of the encryption state change attempt;
+             *     o 0: the encrypted state was successfully updated;
+             *     o BLE host error code: the encryption state change attempt
+             *       failed for the specified reason.
+             */
             int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
         } enc_change;
 
-        struct ble_gap_passkey_action passkey_action;
+        /**
+         * Represents a passkey query needed to complete a pairing procedure.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_PASSKEY_ACTION
+         */
+        struct {
+            /** Contains details about the passkey query. */
+            struct ble_gap_passkey_params params;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+        } passkey;
+
+        /**
+         * Represents a received ATT notification or indication.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_NOTIFY_RX
+         */
+        struct {
+            /**
+             * The contents of the notification or indication.  If the
+             * application wishes to retain this mbuf for later use, it must
+             * set this pointer to NULL to prevent the stack from freeing it.
+             */
+            struct os_mbuf *om;
+
+            /** The handle of the relevant ATT attribute. */
+            uint16_t attr_handle;
 
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+
+            /**
+             * Whether the received command is a notification or an
+             * indication;
+             *     o 0: Notification;
+             *     o 1: Indication.
+             */
+            uint8_t indication:1;
+        } notify_rx;
+
+        /**
+         * Represents a transmitted ATT notification or indication, or a
+         * completed indication transaction.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_NOTIFY_TX
+         */
         struct {
+            /**
+             * The status of the notification or indication transaction;
+             *     o 0:                 Command successfully sent;
+             *     o BLE_HS_EDONE:      Confirmation (indication ack) received;
+             *     o BLE_HS_ETIMEOUT:   Confirmation (indication ack) never
+             *                              received;
+             *     o Other return code: Error.
+             */
+            int status;
+
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+
+            /** The handle of the relevant characterstic value. */
             uint16_t attr_handle;
-            void *attr_data;
-            uint16_t attr_len;
 
-            unsigned indication:1;
-        } notify;
+            /**
+             * Whether the transmitted command is a notification or an
+             * indication;
+             *     o 0: Notification;
+             *     o 1: Indication.
+             */
+            uint8_t indication:1;
+        } notify_tx;
+
+        /**
+         * Represents a state change in a peer's subscription status.  In this
+         * comment, the term "update" is used to refer to either a notification
+         * or an indication.  This event is triggered by any of the following
+         * occurrences:
+         *     o Peer enables or disables updates via a CCCD write.
+         *     o Connection is about to be terminated and the peer is
+         *       subscribed to updates.
+         *     o Peer is now subscribed to updates after its state was restored
+         *       from persistence.  This happens when bonding is restored.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_SUBSCRIBE
+         */
+        struct {
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
 
-        struct ble_gap_ltk_params *ltk_params;
-    };
-};
+            /** The value handle of the relevant characteristic. */
+            uint16_t attr_handle;
 
-typedef int ble_gap_event_fn(int event, struct ble_gap_conn_ctxt *ctxt,
-                             void *arg);
+            /** One of the BLE_GAP_SUBSCRIBE_REASON codes. */
+            uint8_t reason;
 
-struct ble_gap_disc_desc {
-    uint8_t event_type;
-    uint8_t addr_type;
-    uint8_t length_data;
-    int8_t rssi;
-    uint8_t addr[6];
-    uint8_t *data;
-    struct ble_hs_adv_fields *fields;
-};
+            /** Whether the peer was previously subscribed to notifications. */
+            uint8_t prev_notify:1;
+
+            /** Whether the peer is currently subscribed to notifications. */
+            uint8_t cur_notify:1;
 
-typedef void ble_gap_disc_fn(int event, int status,
-                             struct ble_gap_disc_desc *desc, void *arg);
+            /** Whether the peer was previously subscribed to indications. */
+            uint8_t prev_indicate:1;
 
-typedef void ble_gap_wl_fn(int status, void *arg);
+            /** Whether the peer is currently subscribed to indications. */
+            uint8_t cur_indicate:1;
+        } subscribe;
+
+        /**
+         * Represents a change in an L2CAP channel's MTU.
+         *
+         * Valid for the following event types:
+         *     o BLE_GAP_EVENT_MTU
+         */
+        struct {
+            /** The handle of the relevant connection. */
+            uint16_t conn_handle;
+
+            /**
+             * Indicates the channel whose MTU has been updated; either
+             * BLE_L2CAP_CID_ATT or the ID of a connection-oriented channel.
+             */
+            uint16_t channel_id;
+
+            /* The channel's new MTU. */
+            uint16_t value;
+        } mtu;
+    };
+};
+
+typedef int ble_gap_event_fn(struct ble_gap_event *event, void *arg);
 
 #define BLE_GAP_CONN_MODE_NON               0
 #define BLE_GAP_CONN_MODE_DIR               1
@@ -246,35 +528,37 @@ struct ble_gap_white_entry {
     uint8_t addr[6];
 };
 
-int ble_gap_find_conn(uint16_t handle, struct ble_gap_conn_desc *out_desc);
+int ble_gap_conn_find(uint16_t handle, struct ble_gap_conn_desc *out_desc);
 
-int ble_gap_adv_start(uint8_t discoverable_mode, uint8_t connectable_mode,
-                      uint8_t *peer_addr, uint8_t peer_addr_type,
+int ble_gap_adv_start(uint8_t own_addr_type, uint8_t peer_addr_type,
+                      const uint8_t *peer_addr, int32_t duration_ms,
                       const struct ble_gap_adv_params *adv_params,
                       ble_gap_event_fn *cb, void *cb_arg);
-
 int ble_gap_adv_stop(void);
-int ble_gap_adv_set_fields(struct ble_hs_adv_fields *adv_fields);
-int ble_gap_adv_rsp_set_fields(struct ble_hs_adv_fields *rsp_fields);
-int ble_gap_disc(uint32_t duration_ms, uint8_t discovery_mode,
-                      uint8_t scan_type, uint8_t filter_policy,
-                      uint8_t addr_mode,
-                      ble_gap_disc_fn *cb, void *cb_arg);
+int ble_gap_adv_active(void);
+int ble_gap_adv_set_fields(const struct ble_hs_adv_fields *adv_fields);
+int ble_gap_adv_rsp_set_fields(const struct ble_hs_adv_fields *rsp_fields);
+int ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms,
+                 const struct ble_gap_disc_params *disc_params,
+                 ble_gap_event_fn *cb, void *cb_arg);
 int ble_gap_disc_cancel(void);
-int ble_gap_conn_initiate(int addr_type, uint8_t *addr,
-                          struct ble_gap_crt_params *params,
-                          ble_gap_event_fn *cb, void *cb_arg);
-int ble_gap_terminate(uint16_t handle);
-int ble_gap_cancel(void);
-int ble_gap_wl_set(struct ble_gap_white_entry *white_list,
+int ble_gap_disc_active(void);
+int ble_gap_connect(uint8_t own_addr_type,
+                    uint8_t peer_addr_type, const uint8_t *peer_addr,
+                    int32_t duration_ms,
+                    const struct ble_gap_conn_params *params,
+                    ble_gap_event_fn *cb, void *cb_arg);
+int ble_gap_conn_cancel(void);
+int ble_gap_conn_active(void);
+int ble_gap_terminate(uint16_t conn_handle, uint8_t hci_reason);
+int ble_gap_wl_set(const struct ble_gap_white_entry *white_list,
                    uint8_t white_list_count);
 int ble_gap_update_params(uint16_t conn_handle,
-                          struct ble_gap_upd_params *params);
+                          const struct ble_gap_upd_params *params);
 int ble_gap_security_initiate(uint16_t conn_handle);
 int ble_gap_pair_initiate(uint16_t conn_handle);
-int ble_gap_encryption_initiate(uint16_t conn_handle, uint8_t *ltk,
+int ble_gap_encryption_initiate(uint16_t conn_handle, const uint8_t *ltk,
                                 uint16_t ediv, uint64_t rand_val, int auth);
-int ble_gap_provide_ltk(uint16_t conn_handle, uint8_t *ltk);
-void ble_gap_init_identity_addr(uint8_t *addr);
+int ble_gap_conn_rssi(uint16_t conn_handle, int8_t *out_rssi);
 
 #endif
diff --git a/net/nimble/host/include/host/ble_gatt.h b/net/nimble/host/include/host/ble_gatt.h
index 194ebf31..fbc32a2f 100644
--- a/net/nimble/host/include/host/ble_gatt.h
+++ b/net/nimble/host/include/host/ble_gatt.h
@@ -24,9 +24,48 @@
 #include "host/ble_att.h"
 struct ble_hs_conn;
 struct ble_att_error_rsp;
+struct ble_hs_cfg;
+
+#define BLE_GATT_REGISTER_OP_SVC                        1
+#define BLE_GATT_REGISTER_OP_CHR                        2
+#define BLE_GATT_REGISTER_OP_DSC                        3
 
 #define BLE_GATT_SVC_UUID16                             0x1801
-#define BLE_GATT_CHR_SERVICE_CHANGED_UUID16             0x2a05
+#define BLE_GATT_DSC_CLT_CFG_UUID16                     0x2902
+
+#define BLE_GATT_CHR_PROP_BROADCAST                     0x01
+#define BLE_GATT_CHR_PROP_READ                          0x02
+#define BLE_GATT_CHR_PROP_WRITE_NO_RSP                  0x04
+#define BLE_GATT_CHR_PROP_WRITE                         0x08
+#define BLE_GATT_CHR_PROP_NOTIFY                        0x10
+#define BLE_GATT_CHR_PROP_INDICATE                      0x20
+#define BLE_GATT_CHR_PROP_AUTH_SIGN_WRITE               0x40
+#define BLE_GATT_CHR_PROP_EXTENDED                      0x80
+
+#define BLE_GATT_ACCESS_OP_READ_CHR                     0
+#define BLE_GATT_ACCESS_OP_WRITE_CHR                    1
+#define BLE_GATT_ACCESS_OP_READ_DSC                     2
+#define BLE_GATT_ACCESS_OP_WRITE_DSC                    3
+
+#define BLE_GATT_CHR_F_BROADCAST                        0x0001
+#define BLE_GATT_CHR_F_READ                             0x0002
+#define BLE_GATT_CHR_F_WRITE_NO_RSP                     0x0004
+#define BLE_GATT_CHR_F_WRITE                            0x0008
+#define BLE_GATT_CHR_F_NOTIFY                           0x0010
+#define BLE_GATT_CHR_F_INDICATE                         0x0020
+#define BLE_GATT_CHR_F_AUTH_SIGN_WRITE                  0x0040
+#define BLE_GATT_CHR_F_RELIABLE_WRITE                   0x0080
+#define BLE_GATT_CHR_F_AUX_WRITE                        0x0100
+#define BLE_GATT_CHR_F_READ_ENC                         0x0200
+#define BLE_GATT_CHR_F_READ_AUTHEN                      0x0400
+#define BLE_GATT_CHR_F_READ_AUTHOR                      0x0800
+#define BLE_GATT_CHR_F_WRITE_ENC                        0x1000
+#define BLE_GATT_CHR_F_WRITE_AUTHEN                     0x2000
+#define BLE_GATT_CHR_F_WRITE_AUTHOR                     0x4000
+
+#define BLE_GATT_SVC_TYPE_END                           0
+#define BLE_GATT_SVC_TYPE_PRIMARY                       1
+#define BLE_GATT_SVC_TYPE_SECONDARY                     2
 
 /*** @client. */
 struct ble_gatt_error {
@@ -43,8 +82,7 @@ struct ble_gatt_svc {
 struct ble_gatt_attr {
     uint16_t handle;
     uint16_t offset;
-    uint16_t value_len;
-    void *value;
+    struct os_mbuf *om;
 };
 
 struct ble_gatt_chr {
@@ -59,32 +97,49 @@ struct ble_gatt_dsc {
     uint8_t uuid128[16];
 };
 
-typedef int ble_gatt_mtu_fn(uint16_t conn_handle, struct ble_gatt_error *error,
+typedef int ble_gatt_mtu_fn(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
                             uint16_t mtu, void *arg);
 typedef int ble_gatt_disc_svc_fn(uint16_t conn_handle,
-                                 struct ble_gatt_error *error,
-                                 struct ble_gatt_svc *service,
+                                 const struct ble_gatt_error *error,
+                                 const struct ble_gatt_svc *service,
                                  void *arg);
+
+/**
+ * The host will free the attribute mbuf automatically after the callback is
+ * executed.  The application can take ownership of the mbuf and prevent it
+ * from being freed by assigning NULL to attr->om.
+ */
 typedef int ble_gatt_attr_fn(uint16_t conn_handle,
-                             struct ble_gatt_error *error,
-                             struct ble_gatt_attr *attr, void *arg);
+                             const struct ble_gatt_error *error,
+                             struct ble_gatt_attr *attr,
+                             void *arg);
+
+/**
+ * The host will free the attribute mbufs automatically after the callback is
+ * executed.  The application can take ownership of the mbufs and prevent them
+ * from being freed by assigning NULL to each attribute's om field.
+ */
 typedef int ble_gatt_reliable_attr_fn(uint16_t conn_handle,
-                                      struct ble_gatt_error *error,
+                                      const struct ble_gatt_error *error,
                                       struct ble_gatt_attr *attrs,
                                       uint8_t num_attrs, void *arg);
 
-typedef int ble_gatt_chr_fn(uint16_t conn_handle, struct ble_gatt_error *error,
-                            struct ble_gatt_chr *chr, void *arg);
+typedef int ble_gatt_chr_fn(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            const struct ble_gatt_chr *chr, void *arg);
 
-typedef int ble_gatt_dsc_fn(uint16_t conn_handle, struct ble_gatt_error *error,
-                            uint16_t chr_def_handle, struct ble_gatt_dsc *dsc,
+typedef int ble_gatt_dsc_fn(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            uint16_t chr_def_handle,
+                            const struct ble_gatt_dsc *dsc,
                             void *arg);
 
 int ble_gattc_exchange_mtu(uint16_t conn_handle,
                            ble_gatt_mtu_fn *cb, void *cb_arg);
 int ble_gattc_disc_all_svcs(uint16_t conn_handle,
                             ble_gatt_disc_svc_fn *cb, void *cb_arg);
-int ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, void *service_uuid128,
+int ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, const void *svc_uuid128,
                                ble_gatt_disc_svc_fn *cb, void *cb_arg);
 int ble_gattc_find_inc_svcs(uint16_t conn_handle, uint16_t start_handle,
                             uint16_t end_handle,
@@ -93,7 +148,7 @@ int ble_gattc_disc_all_chrs(uint16_t conn_handle, uint16_t start_handle,
                             uint16_t end_handle, ble_gatt_chr_fn *cb,
                             void *cb_arg);
 int ble_gattc_disc_chrs_by_uuid(uint16_t conn_handle, uint16_t start_handle,
-                               uint16_t end_handle, void *uuid128,
+                               uint16_t end_handle, const void *uuid128,
                                ble_gatt_chr_fn *cb, void *cb_arg);
 int ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t chr_val_handle,
                             uint16_t chr_end_handle,
@@ -101,150 +156,295 @@ int ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t chr_val_handle,
 int ble_gattc_read(uint16_t conn_handle, uint16_t attr_handle,
                    ble_gatt_attr_fn *cb, void *cb_arg);
 int ble_gattc_read_by_uuid(uint16_t conn_handle, uint16_t start_handle,
-                           uint16_t end_handle, void *uuid128,
+                           uint16_t end_handle, const void *uuid128,
                            ble_gatt_attr_fn *cb, void *cb_arg);
 int ble_gattc_read_long(uint16_t conn_handle, uint16_t handle,
                         ble_gatt_attr_fn *cb, void *cb_arg);
-int ble_gattc_read_mult(uint16_t conn_handle, uint16_t *handles,
+int ble_gattc_read_mult(uint16_t conn_handle, const uint16_t *handles,
                         uint8_t num_handles, ble_gatt_attr_fn *cb,
                         void *cb_arg);
 int ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle,
-                           void *value, uint16_t value_len);
-int ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle, void *value,
-                    uint16_t value_len, ble_gatt_attr_fn *cb, void *cb_arg);
+                           struct os_mbuf *om);
+int ble_gattc_write_no_rsp_flat(uint16_t conn_handle, uint16_t attr_handle,
+                                const void *data, uint16_t data_len);
+int ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle,
+                    struct os_mbuf *om,
+                    ble_gatt_attr_fn *cb, void *cb_arg);
+int ble_gattc_write_flat(uint16_t conn_handle, uint16_t attr_handle,
+                         const void *data, uint16_t data_len,
+                         ble_gatt_attr_fn *cb, void *cb_arg);
 int ble_gattc_write_long(uint16_t conn_handle, uint16_t attr_handle,
-                         void *value, uint16_t value_len, ble_gatt_attr_fn *cb,
-                         void *cb_arg);
-int ble_gattc_write_reliable(uint16_t conn_handle, struct ble_gatt_attr *attrs,
+                         struct os_mbuf *om,
+                         ble_gatt_attr_fn *cb, void *cb_arg);
+int ble_gattc_write_reliable(uint16_t conn_handle,
+                             struct ble_gatt_attr *attrs,
                              int num_attrs, ble_gatt_reliable_attr_fn *cb,
                              void *cb_arg);
-int ble_gattc_read_dsc(uint16_t conn_handle, uint16_t attr_handle,
-                       ble_gatt_attr_fn *cb, void *cb_arg);
-int ble_gattc_read_long_dsc(uint16_t conn_handle, uint16_t attr_handle,
-                            ble_gatt_attr_fn *cb, void *cb_arg);
-int ble_gattc_write_dsc(uint16_t conn_handle, uint16_t attr_handle,
-                        void *value, uint16_t value_len,
-                        ble_gatt_attr_fn *cb, void *cb_arg);
-int ble_gattc_write_long_dsc(uint16_t conn_handle, uint16_t attr_handle,
-                             void *value, uint16_t value_len,
-                             ble_gatt_attr_fn *cb, void *cb_arg);
-int ble_gattc_notify(uint16_t conn_handle, uint16_t chr_val_handle);
 int ble_gattc_notify_custom(uint16_t conn_handle, uint16_t att_handle,
-                            void *attr_data, uint16_t attr_data_len);
+                            struct os_mbuf *om);
+int ble_gattc_notify(uint16_t conn_handle, uint16_t chr_val_handle);
+int ble_gattc_indicate(uint16_t conn_handle, uint16_t chr_val_handle);
 
 int ble_gattc_init(void);
 
 /*** @server. */
 
-#define BLE_GATT_CHR_PROP_BROADCAST         0x01
-#define BLE_GATT_CHR_PROP_READ              0x02
-#define BLE_GATT_CHR_PROP_WRITE_NO_RSP      0x04
-#define BLE_GATT_CHR_PROP_WRITE             0x08
-#define BLE_GATT_CHR_PROP_NOTIFY            0x10
-#define BLE_GATT_CHR_PROP_INDICATE          0x20
-#define BLE_GATT_CHR_PROP_AUTH_SIGN_WRITE   0x40
-#define BLE_GATT_CHR_PROP_EXTENDED          0x80
-
-#define BLE_GATT_ACCESS_OP_READ_CHR         0
-#define BLE_GATT_ACCESS_OP_WRITE_CHR        1
-#define BLE_GATT_ACCESS_OP_READ_DSC         2
-#define BLE_GATT_ACCESS_OP_WRITE_DSC        3
-
-union ble_gatt_access_ctxt;
+struct ble_gatt_access_ctxt;
 typedef int ble_gatt_access_fn(uint16_t conn_handle, uint16_t attr_handle,
-                               uint8_t op, union ble_gatt_access_ctxt *ctxt,
-                               void *arg);
+                               struct ble_gatt_access_ctxt *ctxt, void *arg);
 
 typedef uint16_t ble_gatt_chr_flags;
 
-#define BLE_GATT_CHR_F_BROADCAST            0x0001
-#define BLE_GATT_CHR_F_READ                 0x0002
-#define BLE_GATT_CHR_F_WRITE_NO_RSP         0x0004
-#define BLE_GATT_CHR_F_WRITE                0x0008
-#define BLE_GATT_CHR_F_NOTIFY               0x0010
-#define BLE_GATT_CHR_F_INDICATE             0x0020
-#define BLE_GATT_CHR_F_AUTH_SIGN_WRITE      0x0040
-#define BLE_GATT_CHR_F_RELIABLE_WRITE       0x0080
-#define BLE_GATT_CHR_F_AUX_WRITE            0x0100
-#define BLE_GATT_CHR_F_READ_ENC             0x0200
-#define BLE_GATT_CHR_F_READ_AUTHEN          0x0400
-#define BLE_GATT_CHR_F_READ_AUTHOR          0x0800
-#define BLE_GATT_CHR_F_WRITE_ENC            0x1000
-#define BLE_GATT_CHR_F_WRITE_AUTHEN         0x2000
-#define BLE_GATT_CHR_F_WRITE_AUTHOR         0x4000
-
 struct ble_gatt_chr_def {
-    uint8_t *uuid128;   /* NULL if no more characteristics. */
+    /**
+     * Pointer to first element in a uint8_t[16]; use the BLE_UUID16 macro for
+     * 16-bit UUIDs; NULL if there are no more characteristics in the service.
+     */
+    const uint8_t *uuid128;
+
+    /**
+     * Callback that gets executed when this characteristic is read or
+     * written.
+     */
     ble_gatt_access_fn *access_cb;
+
+    /** Optional argument for callback. */
     void *arg;
+
+    /**
+     * Array of this characteristic's descriptors.  NULL if no descriptors.
+     * Do not include CCCD; it gets added automatically if this
+     * characteristic's notify or indicate flag is set.
+     */
     struct ble_gatt_dsc_def *descriptors;
+
+    /** Specifies the set of permitted operations for this characteristic. */
     ble_gatt_chr_flags flags;
-};
 
-#define BLE_GATT_SVC_TYPE_END       0
-#define BLE_GATT_SVC_TYPE_PRIMARY   1
-#define BLE_GATT_SVC_TYPE_SECONDARY 2
+    /** 
+     * At registration time, this is filled in with the characteristic's value
+     * attribute handle.
+     */
+    uint16_t * const val_handle;
+};
 
 struct ble_gatt_svc_def {
+    /**
+     * One of the following:
+     *     o BLE_GATT_SVC_TYPE_PRIMARY - primary service
+     *     o BLE_GATT_SVC_TYPE_SECONDARY - secondary service
+     *     o 0 - No more services in this array.
+     */
     uint8_t type;
-    uint8_t *uuid128;
-    const struct ble_gatt_svc_def **includes; /* Terminated with null. */
-    struct ble_gatt_chr_def *characteristics;
-};
-
-union ble_gatt_access_ctxt {
-    struct {
-        const struct ble_gatt_chr_def *chr;
-        void *data;
-        int len;
-    } chr_access;
 
-    struct {
-        const struct ble_gatt_dsc_def *dsc;
-        void *data;
-        int len;
-    } dsc_access;
+    /**
+     * Pointer to first element in a uint8_t[16]; use the BLE_UUID16 macro for
+     * 16-bit UUIDs.
+     */
+    const uint8_t *uuid128;
+
+    /**
+     * Array of pointers to other service definitions.  These services are
+     * reported as "included services" during service discovery.  Terminate the
+     * array with NULL.
+     */
+    const struct ble_gatt_svc_def **includes;
+
+    /**
+     * Array of characteristic definitions corresponding to characteristics
+     * belonging to this service.
+     */
+    const struct ble_gatt_chr_def *characteristics;
 };
 
 struct ble_gatt_dsc_def {
+    /**
+     * The first element in a uint8_t[16]; use the BLE_UUID16 macro for 16-bit
+     * UUIDs; NULL if there are no more descriptors in the characteristic.
+     */
     uint8_t *uuid128;
+
+    /** Specifies the set of permitted operations for this descriptor. */
     uint8_t att_flags;
+
+    /** Callback that gets executed when the descriptor is read or written. */
     ble_gatt_access_fn *access_cb;
+
+    /** Optional argument for callback. */
     void *arg;
 };
 
-#define BLE_GATT_REGISTER_OP_SVC    1
-#define BLE_GATT_REGISTER_OP_CHR    2
-#define BLE_GATT_REGISTER_OP_DSC    3
+/**
+ * Context for an access to a GATT characteristic or descriptor.  When a client
+ * reads or writes a locally registered characteristic or descriptor, an
+ * instance of this struct gets passed to the application callback.
+ */
+struct ble_gatt_access_ctxt {
+    /**
+     * Indicates the gatt operation being performed.  This is equal to one of
+     * the following values:
+     *     o  BLE_GATT_ACCESS_OP_READ_CHR
+     *     o  BLE_GATT_ACCESS_OP_WRITE_CHR
+     *     o  BLE_GATT_ACCESS_OP_READ_DSC
+     *     o  BLE_GATT_ACCESS_OP_WRITE_DSC
+     */
+    uint8_t op;
+
+    /**
+     * A container for the GATT access data.
+     *     o For reads: The application populates this with the value of the
+     *       characteristic or descriptor being read.
+     *     o For writes: This is already populated with the value being written
+     *       by the peer.  If the application wishes to retain this mbuf for
+     *       later use, the access callback must set this pointer to NULL to
+     *       prevent the stack from freeing it.
+     */
+    struct os_mbuf *om;
+
+    /**
+     * The GATT operation being performed dictates which field in this union is
+     * valid.  If a characteristic is being accessed, the chr field is valid.
+     * Otherwise a descriptor is being accessed, in which case the dsc field
+     * is valid.
+     */
+    union {
+        /**
+         * The characteristic definition corresponding to the characteristic
+         * being accessed.  This is what the app registered at startup.
+         */
+        const struct ble_gatt_chr_def *chr;
+
+        /**
+         * The descriptor definition corresponding to the descriptor being
+         * accessed.  This is what the app registered at startup.
+         */
+        const struct ble_gatt_dsc_def *dsc;
+    };
+};
+
+/**
+ * Context passed to the registration callback; represents the GATT service,
+ * characteristic, or descriptor being registered.
+ */
+struct ble_gatt_register_ctxt {
+    /**
+     * Indicates the gatt registration operation just performed.  This is
+     * equal to one of the following values:
+     *     o BLE_GATT_REGISTER_OP_SVC
+     *     o BLE_GATT_REGISTER_OP_CHR
+     *     o BLE_GATT_REGISTER_OP_DSC
+     */
+    uint8_t op;
+
+    /**
+     * The value of the op field determines which field in this union is valid.
+     */
+    union {
+        /** Service; valid if op == BLE_GATT_REGISTER_OP_SVC. */
+        struct {
+            /** The ATT handle of the service definition attribute. */
+            uint16_t handle;
+
+            /**
+             * The service definition representing the service being
+             * registered.
+             */
+            const struct ble_gatt_svc_def *svc_def;
+        } svc;
+
+        /** Characteristic; valid if op == BLE_GATT_REGISTER_OP_CHR. */
+        struct {
+            /** The ATT handle of the characteristic definition attribute. */
+            uint16_t def_handle;
+
+            /** The ATT handle of the characteristic value attribute. */
+            uint16_t val_handle;
+
+            /**
+             * The characteristic definition representing the characteristic
+             * being registered.
+             */
+            const struct ble_gatt_chr_def *chr_def;
+
+            /**
+             * The service definition corresponding to the characteristic's
+             * parent service.
+             */
+            const struct ble_gatt_svc_def *svc_def;
+        } chr;
+
+        /** Descriptor; valid if op == BLE_GATT_REGISTER_OP_DSC. */
+        struct {
+            /** The ATT handle of the descriptor definition attribute. */
+            uint16_t handle;
+
+            /**
+             * The descriptor definition corresponding to the descriptor being
+             * registered.
+             */
+            const struct ble_gatt_dsc_def *dsc_def;
+
+            /**
+             * The characteristic definition corresponding to the descriptor's
+             * parent characteristic.
+             */
+            const struct ble_gatt_chr_def *chr_def;
+
+            /**
+             * The service definition corresponding to the descriptor's
+             * grandparent service
+             */
+            const struct ble_gatt_svc_def *svc_def;
+        } dsc;
+    };
+};
+
+/**
+ * Contains counts of resources required by the GATT server.  The contents of
+ * this struct are generally used to populate a configuration struct before
+ * the host is initialized.
+ */
+struct ble_gatt_resources {
+    /** Number of services. */
+    uint16_t svcs;
+
+    /** Number of included services. */
+    uint16_t incs;
 
-union ble_gatt_register_ctxt;
-typedef void ble_gatt_register_fn(uint8_t op,
-                                  union ble_gatt_register_ctxt *ctxt,
+    /** Number of characteristics. */
+    uint16_t chrs;
+
+    /** Number of descriptors. */
+    uint16_t dscs;
+
+    /**
+     * Number of client characteristic configuration descriptors.  Each of
+     * these also contributes to the total descriptor count.
+     */
+    uint16_t cccds;
+
+    /** Total number of ATT attributes. */
+    uint16_t attrs;
+};
+
+typedef void ble_gatt_register_fn(struct ble_gatt_register_ctxt *ctxt,
                                   void *arg);
 
 int ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
                             ble_gatt_register_fn *register_cb,
                             void *cb_arg);
-void ble_gatts_chr_updated(uint16_t chr_def_handle);
+int ble_gatts_add_svcs(const struct ble_gatt_svc_def *svcs);
+int ble_gatts_count_resources(const struct ble_gatt_svc_def *svcs,
+                              struct ble_gatt_resources *res);
+int ble_gatts_count_cfg(const struct ble_gatt_svc_def *defs,
+                        struct ble_hs_cfg *cfg);
 
-union ble_gatt_register_ctxt {
-    struct {
-        uint16_t handle;
-        const struct ble_gatt_svc_def *svc;
-    } svc_reg;
-
-    struct {
-        uint16_t def_handle;
-        uint16_t val_handle;
-        const struct ble_gatt_chr_def *chr;
-    } chr_reg;
+void ble_gatts_chr_updated(uint16_t chr_def_handle);
 
-    struct {
-        uint16_t dsc_handle;
-        const struct ble_gatt_dsc_def *dsc;
-        uint16_t chr_def_handle;
-        const struct ble_gatt_chr_def *chr;
-    } dsc_reg;
-};
+int ble_gatts_find_svc(const void *uuid128, uint16_t *out_handle);
+int ble_gatts_find_chr(const void *svc_uuid128, const void *chr_uuid128,
+                       uint16_t *out_def_handle, uint16_t *out_val_handle);
+int ble_gatts_find_dsc(const void *svc_uuid128, const void *chr_uuid128,
+                       const void *dsc_uuid128, uint16_t *out_dsc_handle);
 
 #endif
diff --git a/net/nimble/host/include/host/ble_hs.h b/net/nimble/host/include/host/ble_hs.h
index e493723f..8d850097 100644
--- a/net/nimble/host/include/host/ble_hs.h
+++ b/net/nimble/host/include/host/ble_hs.h
@@ -21,17 +21,24 @@
 #define H_BLE_HS_
 
 #include <inttypes.h>
+#include "nimble/hci_common.h"
 #include "host/ble_att.h"
 #include "host/ble_gap.h"
 #include "host/ble_gatt.h"
 #include "host/ble_hs.h"
+#include "host/ble_hs_adv.h"
+#include "host/ble_hs_id.h"
+#include "host/ble_hs_log.h"
 #include "host/ble_hs_test.h"
-#include "host/ble_uuid.h"
+#include "host/ble_hs_mbuf.h"
+#include "host/ble_sm.h"
 #include "host/ble_store.h"
-#include "host/host_hci.h"
+#include "host/ble_uuid.h"
 struct os_eventq;
 struct os_event;
 
+#define BLE_HS_FOREVER              INT32_MAX
+
 #define BLE_HS_CONN_HANDLE_NONE     0xffff
 
 #define BLE_HS_EAGAIN               1
@@ -45,17 +52,17 @@ struct os_event;
 #define BLE_HS_EAPP                 9
 #define BLE_HS_EBADDATA             10
 #define BLE_HS_EOS                  11
-#define BLE_HS_ECONGESTED           12
-#define BLE_HS_ECONTROLLER          13
-#define BLE_HS_ETIMEOUT             14
-#define BLE_HS_EDONE                15
-#define BLE_HS_EBUSY                16
-#define BLE_HS_EREJECT              17
-#define BLE_HS_EUNKNOWN             18
-#define BLE_HS_EROLE                19
-#define BLE_HS_ETIMEOUT_HCI         20
-#define BLE_HS_ENOMEM_HCI           21
-#define BLE_HS_ENOMEM_EVT           22
+#define BLE_HS_ECONTROLLER          12
+#define BLE_HS_ETIMEOUT             13
+#define BLE_HS_EDONE                14
+#define BLE_HS_EBUSY                15
+#define BLE_HS_EREJECT              16
+#define BLE_HS_EUNKNOWN             17
+#define BLE_HS_EROLE                18
+#define BLE_HS_ETIMEOUT_HCI         19
+#define BLE_HS_ENOMEM_EVT           20
+#define BLE_HS_ENOADDR              21
+#define BLE_HS_ENOTSYNCED           22
 
 #define BLE_HS_ERR_ATT_BASE         0x100   /* 256 */
 #define BLE_HS_ATT_ERR(x)           ((x) ? BLE_HS_ERR_ATT_BASE + (x) : 0)
@@ -69,39 +76,31 @@ struct os_event;
 #define BLE_HS_ERR_SM_US_BASE       0x400   /* 1024 */
 #define BLE_HS_SM_US_ERR(x)         ((x) ? BLE_HS_ERR_SM_US_BASE + (x) : 0)
 
-#define BLE_HS_ERR_SM_THEM_BASE     0x500   /* 1280 */
-#define BLE_HS_SM_THEM_ERR(x)       ((x) ? BLE_HS_ERR_SM_THEM_BASE + (x) : 0)
+#define BLE_HS_ERR_SM_PEER_BASE     0x500   /* 1280 */
+#define BLE_HS_SM_PEER_ERR(x)       ((x) ? BLE_HS_ERR_SM_PEER_BASE + (x) : 0)
 
-/* defines the input output (io) capabilities for the host device */
+/* Note: A hardware error of 0 is not success. */
+#define BLE_HS_ERR_HW_BASE          0x600   /* 1536 */
+#define BLE_HS_HW_ERR(x)            (BLE_HS_ERR_HW_BASE + (x))
+
+/* Defines the IO capabilities for the local device. */
 #define BLE_HS_IO_DISPLAY_ONLY              0x00
 #define BLE_HS_IO_DISPLAY_YESNO             0x01
 #define BLE_HS_IO_KEYBOARD_ONLY             0x02
 #define BLE_HS_IO_NO_INPUT_OUTPUT           0x03
 #define BLE_HS_IO_KEYBOARD_DISPLAY          0x04
 
-#define BLE_HS_PRIVACY_MODE_NONE            0
-#define BLE_HS_PRIVACY_MODE_RANDOM_STATIC   1
-#define BLE_HS_PRIVACY_MODE_RESOLV_RAND     2
+typedef void ble_hs_reset_fn(int reason);
+typedef void ble_hs_sync_fn(void);
 
 struct ble_hs_cfg {
-    /*** HCI settings. */
     /**
-     * An HCI buffer is a "flat" 260-byte buffer.  HCI buffers are used by the
-     * controller to send unsolicited events to the host.
-     *
-     * HCI buffers can get tied up when the controller sends lots of
-     * asynchronous / unsolicited events (i.e., non-acks).  When the controller
-     * needs to send one of these events, it allocates an HCI buffer, fills it
-     * with the event payload, and puts it on a host queue.  If the controller
-     * sends a quick burst of these events, the buffer pool may be exhausted,
-     * preventing the host from sending an HCI command to the controller.
-     *
      * Every time the controller sends a non-ack HCI event to the host, it also
      * allocates an OS event (it is unfortunate that these are both called
      * "events").  The OS event is put on the host-parent-task's event queue;
      * it is what wakes up the host-parent-task and indicates that an HCI event
-     * needs to be processsed.  The pool of OS events is allocated with the
-     * same number of elements as the HCI buffer pool.
+     * needs to be processsed.  This setting should be equal to the total
+     * number of HCI event buffers that the transport is configured to use.
      */
     uint8_t max_hci_bufs;
 
@@ -132,6 +131,18 @@ struct ble_hs_cfg {
      */
     uint16_t max_client_configs;
 
+    /**
+     * An optional callback that gets executed upon registration of each GATT
+     * resource (service, characteristic, or descriptor).
+     */
+    ble_gatt_register_fn *gatts_register_cb;
+
+    /**
+     * An optional argument that gets passed to the GATT registration
+     * callback.
+     */
+    void *gatts_register_arg;
+
     /*** GATT client settings. */
     /**
      * The maximum number of concurrent GATT client procedures.  When you
@@ -197,6 +208,19 @@ struct ble_hs_cfg {
     uint8_t sm_our_key_dist;
     uint8_t sm_their_key_dist;
 
+    /*** HCI settings */
+    /**
+     * This callback is executed when the host resets itself and the controller
+     * due to fatal error.
+     */
+    ble_hs_reset_fn *reset_cb;
+
+    /**
+     * This callback is executed when the host and controller become synced.
+     * This happens at startup and after a reset.
+     */
+    ble_hs_sync_fn *sync_cb;
+
     /*** Store settings. */
     /**
      * These function callbacks handle persistence of sercurity material
@@ -206,7 +230,7 @@ struct ble_hs_cfg {
     ble_store_write_fn *store_write_cb;
     ble_store_delete_fn *store_delete_cb;
 
-    /*** privacy settings */
+    /*** Privacy settings. */
     /**
      * The frequency at which new resovlable private addresses are generated.
      * Units are seconds.
@@ -216,9 +240,8 @@ struct ble_hs_cfg {
 
 extern const struct ble_hs_cfg ble_hs_cfg_dflt;
 
+int ble_hs_synced(void);
 int ble_hs_start(void);
-void ble_hs_event_enqueue(struct os_event *ev);
-int ble_ibeacon_set_adv_data(void *uuid128, uint16_t major, uint16_t minor);
 int ble_hs_init(struct os_eventq *app_evq, struct ble_hs_cfg *cfg);
 
 #endif
diff --git a/net/nimble/host/include/host/ble_hs_adv.h b/net/nimble/host/include/host/ble_hs_adv.h
index 0377d1d9..8ae2b00b 100644
--- a/net/nimble/host/include/host/ble_hs_adv.h
+++ b/net/nimble/host/include/host/ble_hs_adv.h
@@ -22,18 +22,21 @@
 
 #include <inttypes.h>
 
+/** Max field payload size (account for 2-byte header). */
+#define BLE_HS_ADV_MAX_FIELD_SZ     (BLE_HCI_MAX_ADV_DATA_LEN - 2)
+
 struct ble_hs_adv_fields {
     /*** 0x01 - Flags. */
     uint8_t flags;
     unsigned flags_is_present:1;
 
     /*** 0x02,0x03 - 16-bit service class UUIDs. */
-    void *uuids16;
+    uint16_t *uuids16;
     uint8_t num_uuids16;
     unsigned uuids16_is_complete:1;
 
     /*** 0x04,0x05 - 32-bit service class UUIDs. */
-    void *uuids32;
+    uint32_t *uuids32;
     uint8_t num_uuids32;
     unsigned uuids32_is_complete:1;
 
@@ -48,7 +51,7 @@ struct ble_hs_adv_fields {
     unsigned name_is_complete:1;
 
     /*** 0x0a - Tx power level. */
-    uint8_t tx_pwr_lvl;
+    int8_t tx_pwr_lvl;
     unsigned tx_pwr_lvl_is_present:1;
 
     /*** 0x0d - Class of device. */
@@ -141,6 +144,12 @@ struct ble_hs_adv_fields {
 
 #define BLE_HS_ADV_TX_PWR_LVL_LEN               1
 
+/**
+ * Set the tx_pwr_lvl field to this if you want the stack to fill in the tx
+ * power level field.
+ */
+#define BLE_HS_ADV_TX_PWR_LVL_AUTO              (-128)
+
 #define BLE_HS_ADV_DEVICE_CLASS_LEN             3
 
 #define BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN         4
@@ -165,10 +174,4 @@ struct ble_hs_adv_fields {
 
 #define BLE_HS_ADV_SVC_DATA_UUID128_MIN_LEN     16
 
-int ble_hs_adv_set_flat(uint8_t type, int data_len, void *data,
-                        uint8_t *dst, uint8_t *dst_len, uint8_t max_len);
-int ble_hs_adv_set_fields(struct ble_hs_adv_fields *adv_fields,
-                          uint8_t *dst, uint8_t *dst_len, uint8_t max_len);
-int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields, uint8_t *src,
-                            uint8_t src_len);
 #endif
diff --git a/net/nimble/host/include/host/ble_hs_id.h b/net/nimble/host/include/host/ble_hs_id.h
new file mode 100644
index 00000000..749524b8
--- /dev/null
+++ b/net/nimble/host/include/host/ble_hs_id.h
@@ -0,0 +1,30 @@
+/**
+ * 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 H_BLE_HS_ID_
+#define H_BLE_HS_ID_
+
+#include <inttypes.h>
+
+int ble_hs_id_gen_rnd(int nrpa, uint8_t *out_addr);
+int ble_hs_id_set_rnd(const uint8_t *rnd_addr);
+int ble_hs_id_copy_addr(uint8_t id_addr_type, uint8_t *out_id_addr,
+                        int *out_is_nrpa);
+
+#endif
diff --git a/net/nimble/host/include/host/ble_hs_log.h b/net/nimble/host/include/host/ble_hs_log.h
new file mode 100644
index 00000000..fd10ddba
--- /dev/null
+++ b/net/nimble/host/include/host/ble_hs_log.h
@@ -0,0 +1,39 @@
+/**
+ * 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 H_BLE_HS_LOG_
+#define H_BLE_HS_LOG_
+
+#include "log/log.h"
+struct os_mbuf;
+
+extern struct log ble_hs_log;
+
+#define BLE_HS_LOG(lvl, ...) \
+    LOG_ ## lvl(&ble_hs_log, LOG_MODULE_NIMBLE_HOST, __VA_ARGS__)
+
+#define BLE_HS_LOG_ADDR(lvl, addr)                      \
+    BLE_HS_LOG(lvl, "%02x:%02x:%02x:%02x:%02x:%02x",    \
+               (addr)[5], (addr)[4], (addr)[3],         \
+               (addr)[2], (addr)[1], (addr)[0])
+
+void ble_hs_log_mbuf(const struct os_mbuf *om);
+void ble_hs_log_flat_buf(const void *data, int len);
+
+#endif
diff --git a/net/nimble/host/include/host/ble_hs_mbuf.h b/net/nimble/host/include/host/ble_hs_mbuf.h
new file mode 100644
index 00000000..d3606f2e
--- /dev/null
+++ b/net/nimble/host/include/host/ble_hs_mbuf.h
@@ -0,0 +1,31 @@
+/**
+ * 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 H_BLE_HS_MBUF_
+#define H_BLE_HS_MBUF_
+
+#include <inttypes.h>
+struct os_mbuf;
+
+struct os_mbuf *ble_hs_mbuf_att_pkt(void);
+struct os_mbuf *ble_hs_mbuf_from_flat(const void *buf, uint16_t len);
+int ble_hs_mbuf_to_flat(const struct os_mbuf *om, void *flat, uint16_t max_len,
+                        uint16_t *out_copy_len);
+
+#endif
diff --git a/net/nimble/host/include/host/ble_hs_test.h b/net/nimble/host/include/host/ble_hs_test.h
index 787ec41e..3247b59d 100644
--- a/net/nimble/host/include/host/ble_hs_test.h
+++ b/net/nimble/host/include/host/ble_hs_test.h
@@ -23,9 +23,6 @@
 #include <inttypes.h>
 struct os_mbuf;
 
-void ble_hs_test_pkt_txed(struct os_mbuf *om);
-void ble_hs_test_hci_txed(uint8_t *cmdbuf);
-
 int ble_att_clt_test_all(void);
 int ble_att_svr_test_all(void);
 int ble_gap_test_all(void);
@@ -37,16 +34,16 @@ int ble_gatt_find_s_test_all(void);
 int ble_gatt_read_test_all(void);
 int ble_gatt_write_test_all(void);
 int ble_gatts_notify_test_all(void);
+int ble_gatts_read_test_suite(void);
 int ble_gatts_reg_test_all(void);
-int ble_host_hci_test_all(void);
+int ble_hs_hci_test_all(void);
 int ble_hs_adv_test_all(void);
 int ble_hs_conn_test_all(void);
 int ble_l2cap_test_all(void);
 int ble_os_test_all(void);
-int ble_sm_test_all(void);
-int ble_uuid_test_all(void);
-
 int ble_sm_lgcy_test_suite(void);
 int ble_sm_sc_test_suite(void);
+int ble_sm_test_all(void);
+int ble_uuid_test_all(void);
 
 #endif
diff --git a/net/nimble/host/src/host_dbg_priv.h b/net/nimble/host/include/host/ble_ibeacon.h
index 8d548cab..112f52b3 100644
--- a/net/nimble/host/src/host_dbg_priv.h
+++ b/net/nimble/host/include/host/ble_ibeacon.h
@@ -6,7 +6,7 @@
  * 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,
@@ -17,9 +17,9 @@
  * under the License.
  */
 
-#ifndef H_HOST_DBG_
-#define H_HOST_DBG_
+#ifndef H_BLE_IBEACON_
+#define H_BLE_IBEACON_
 
-void host_hci_dbg_event_disp(uint8_t *evbuf);
+int ble_ibeacon_set_adv_data(void *uuid128, uint16_t major, uint16_t minor);
 
-#endif /* H_HOST_DBG_ */
+#endif
diff --git a/net/nimble/host/include/host/ble_store.h b/net/nimble/host/include/host/ble_store.h
index 9a7efab3..a5331594 100644
--- a/net/nimble/host/include/host/ble_store.h
+++ b/net/nimble/host/include/host/ble_store.h
@@ -28,6 +28,12 @@
 
 #define BLE_STORE_ADDR_TYPE_NONE        0xff
 
+/**
+ * Used as a key for lookups of security material.  This struct corresponds to
+ * the following store object types:
+ *     o BLE_STORE_OBJ_TYPE_OUR_SEC
+ *     o BLE_STORE_OBJ_TYPE_PEER_SEC
+ */
 struct ble_store_key_sec {
     /**
      * Key by peer identity address;
@@ -52,6 +58,12 @@ struct ble_store_key_sec {
     uint8_t idx;
 };
 
+/**
+ * Represents stored security material.  This struct corresponds to the
+ * following store object types:
+ *     o BLE_STORE_OBJ_TYPE_OUR_SEC
+ *     o BLE_STORE_OBJ_TYPE_PEER_SEC
+ */
 struct ble_store_value_sec {
     uint8_t peer_addr[6];
     uint8_t peer_addr_type;
@@ -71,6 +83,11 @@ struct ble_store_value_sec {
     unsigned sc:1;
 };
 
+/**
+ * Used as a key for lookups of stored client characteristic configuration
+ * descriptors (CCCDs).  This struct corresponds to the BLE_STORE_OBJ_TYPE_CCCD
+ * store object type.
+ */
 struct ble_store_key_cccd {
     /**
      * Key by peer identity address;
@@ -89,6 +106,10 @@ struct ble_store_key_cccd {
     uint8_t idx;
 };
 
+/**
+ * Represents a stored client characteristic configuration descriptor (CCCD).
+ * This struct corresponds to the BLE_STORE_OBJ_TYPE_CCCD store object type.
+ */
 struct ble_store_value_cccd {
     uint8_t peer_addr[6];
     uint8_t peer_addr_type;
@@ -97,21 +118,72 @@ struct ble_store_value_cccd {
     unsigned value_changed:1;
 };
 
+/**
+ * Used as a key for store lookups.  This union must be accompanied by an
+ * object type code to indicate which field is valid.
+ */
 union ble_store_key {
     struct ble_store_key_sec sec;
     struct ble_store_key_cccd cccd;
 };
 
+/**
+ * Represents stored data.  This union must be accompanied by an object type
+ * code to indicate which field is valid.
+ */
 union ble_store_value {
     struct ble_store_value_sec sec;
     struct ble_store_value_cccd cccd;
 };
 
+/**
+ * Searches the store for an object matching the specified criteria.  If a
+ * match is found, it is read from the store and the dst parameter is populated
+ * with the retrieved object.
+ *
+ * @param obj_type              The type of object to search for; one of the
+ *                                  BLE_STORE_OBJ_TYPE_[...] codes.
+ * @param key                   Specifies properties of the object to search
+ *                                  for.  An object is retrieved if it matches
+ *                                  these criteria.
+ * @param dst                   On success, this is populated with the
+ *                                  retrieved object.
+ *
+ * @return                      0 if an object was successfully retreived;
+ *                              BLE_HS_ENOENT if no matching object was found;
+ *                              Other nonzero on error.
+ */
 typedef int ble_store_read_fn(int obj_type, union ble_store_key *key,
                               union ble_store_value *dst);
 
+/**
+ * Writes the specified object to the store.  If an object with the same
+ * identity is already in the store, it is replaced.  If the store lacks
+ * sufficient capacity to write the object, this function may remove previously
+ * stored values to make room.
+ *
+ * @param obj_type              The type of object being written; one of the
+ *                                  BLE_STORE_OBJ_TYPE_[...] codes.
+ * @param val                   The object to persist.
+ *
+ * @return                      0 if the object was successfully written;
+ *                              Other nonzero on error.
+ */
 typedef int ble_store_write_fn(int obj_type, union ble_store_value *val);
 
+/**
+ * Searches the store for the first object matching the specified criteria.  If
+ * a match is found, it is deleted from the store.
+ *
+ * @param obj_type              The type of object to delete; one of the
+ *                                  BLE_STORE_OBJ_TYPE_[...] codes.
+ * @param key                   Specifies properties of the object to search
+ *                                  for.  An object is deleted if it matches
+ *                                  these criteria.
+ * @return                      0 if an object was successfully retreived;
+ *                              BLE_HS_ENOENT if no matching object was found;
+ *                              Other nonzero on error.
+ */
 typedef int ble_store_delete_fn(int obj_type, union ble_store_key *key);
 
 int ble_store_read(int obj_type, union ble_store_key *key,
diff --git a/net/nimble/host/include/host/ble_uuid.h b/net/nimble/host/include/host/ble_uuid.h
index a092dd66..89d9df07 100644
--- a/net/nimble/host/include/host/ble_uuid.h
+++ b/net/nimble/host/include/host/ble_uuid.h
@@ -23,10 +23,8 @@
 #include <inttypes.h>
 struct os_mbuf;
 
-uint16_t ble_uuid_128_to_16(void *uuid128);
+uint16_t ble_uuid_128_to_16(const void *uuid128);
 int ble_uuid_16_to_128(uint16_t uuid16, void *dst);
-int ble_uuid_append(struct os_mbuf *om, void *uuid128);
-int ble_uuid_extract(struct os_mbuf *om, int off, void *uuid128);
 
 #define BLE_UUID16_ARR(uuid16) {                                            \
     0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,                         \
diff --git a/net/nimble/host/include/host/host_hci.h b/net/nimble/host/include/host/host_hci.h
deleted file mode 100644
index c33cb390..00000000
--- a/net/nimble/host/include/host/host_hci.h
+++ /dev/null
@@ -1,133 +0,0 @@
-/**
- * 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 H_HOST_HCI_
-#define H_HOST_HCI_
-
-#include "nimble/hci_common.h"
-struct ble_hs_conn;
-struct os_mbuf;
-
-#define HCI_CMD_BUF_SIZE        260
-
-extern uint8_t host_hci_cmd_buf[HCI_CMD_BUF_SIZE];
-
-int host_hci_os_event_proc(struct os_event *ev);
-int host_hci_event_rx(uint8_t *data);
-uint16_t host_hci_opcode_join(uint8_t ogf, uint16_t ocf);
-void host_hci_write_hdr(uint8_t ogf, uint8_t ocf, uint8_t len, void *buf);
-int host_hci_cmd_send(uint8_t ogf, uint8_t ocf, uint8_t len, void *cmddata);
-int host_hci_cmd_send_buf(void *cmddata);
-void host_hci_cmd_build_set_event_mask(uint64_t event_mask,
-                                       uint8_t *dst, int dst_len);
-void host_hci_cmd_build_set_event_mask2(uint64_t event_mask, uint8_t *dst,
-                                        int dst_len);
-void host_hci_cmd_build_disconnect(uint16_t handle, uint8_t reason,
-                                   uint8_t *dst, int dst_len);
-int host_hci_cmd_disconnect(uint16_t handle, uint8_t reason);
-void host_hci_cmd_build_read_rssi(uint16_t handle, uint8_t *dst, int dst_len);
-int host_hci_cmd_read_rssi(uint16_t handle);
-int host_hci_cmd_build_le_set_scan_rsp_data(uint8_t *data, uint8_t len,
-                                            uint8_t *dst, int dst_len);
-int host_hci_cmd_build_le_set_adv_data(uint8_t *data, uint8_t len,
-                                       uint8_t *dst, int dst_len);
-int host_hci_cmd_build_le_set_adv_params(struct hci_adv_params *adv,
-                                         uint8_t *dst, int dst_len);
-void host_hci_cmd_build_le_set_event_mask(uint64_t event_mask,
-                                          uint8_t *dst, int dst_len);
-void host_hci_cmd_build_le_read_buffer_size(uint8_t *dst, int dst_len);
-int host_hci_cmd_le_read_buffer_size(void);
-void host_hci_cmd_build_le_read_loc_supp_feat(uint8_t *dst, uint8_t dst_len);
-void host_hci_cmd_build_le_set_adv_enable(uint8_t enable, uint8_t *dst,
-                                          int dst_len);
-int host_hci_cmd_le_set_adv_enable(uint8_t enable);
-int host_hci_cmd_build_le_set_scan_params(uint8_t scan_type,
-                                          uint16_t scan_itvl,
-                                          uint16_t scan_window,
-                                          uint8_t own_addr_type,
-                                          uint8_t filter_policy,
-                                          uint8_t *cmd, int cmd_len);
-void host_hci_cmd_build_le_set_scan_enable(uint8_t enable,
-                                           uint8_t filter_dups,
-                                           uint8_t *dst, uint8_t dst_len);
-int host_hci_cmd_le_set_scan_enable(uint8_t enable, uint8_t filter_dups);
-int host_hci_cmd_build_le_create_connection(struct hci_create_conn *hcc,
-                                            uint8_t *cmd, int cmd_len);
-int host_hci_cmd_le_create_connection(struct hci_create_conn *hcc);
-void host_hci_cmd_build_le_clear_whitelist(uint8_t *dst, int dst_len);
-int host_hci_cmd_build_le_add_to_whitelist(uint8_t *addr, uint8_t addr_type,
-                                           uint8_t *dst, int dst_len);
-void host_hci_cmd_build_reset(uint8_t *dst, int dst_len);
-int host_hci_cmd_reset(void);
-void host_hci_cmd_build_read_adv_pwr(uint8_t *dst, int dst_len);
-int host_hci_cmd_read_adv_pwr(void);
-void host_hci_cmd_build_le_create_conn_cancel(uint8_t *dst, int dst_len);
-int host_hci_cmd_le_create_conn_cancel(void);
-int host_hci_cmd_build_le_conn_update(struct hci_conn_update *hcu,
-                                      uint8_t *dst, int dst_len);
-int host_hci_cmd_le_conn_update(struct hci_conn_update *hcu);
-void host_hci_cmd_build_le_lt_key_req_reply(struct hci_lt_key_req_reply *hkr,
-                                            uint8_t *dst, int dst_len);
-void host_hci_cmd_build_le_lt_key_req_neg_reply(uint16_t conn_handle,
-                                                uint8_t *dst, int dst_len);
-void host_hci_cmd_build_le_conn_param_reply(struct hci_conn_param_reply *hcr,
-                                            uint8_t *dst, int dst_len);
-int host_hci_cmd_le_conn_param_reply(struct hci_conn_param_reply *hcr);
-void host_hci_cmd_build_le_conn_param_neg_reply(
-    struct hci_conn_param_neg_reply *hcn, uint8_t *dst, int dst_len);
-int host_hci_cmd_le_conn_param_neg_reply(struct hci_conn_param_neg_reply *hcn);
-void host_hci_cmd_build_le_rand(uint8_t *dst, int dst_len);
-void host_hci_cmd_build_le_start_encrypt(struct hci_start_encrypt *cmd,
-                                         uint8_t *dst, int dst_len);
-int host_hci_set_buf_size(uint16_t pktlen, uint8_t max_pkts);
-
-uint16_t host_hci_handle_pb_bc_join(uint16_t handle, uint8_t pb, uint8_t bc);
-
-int host_hci_data_rx(struct os_mbuf *om);
-int host_hci_data_tx(struct ble_hs_conn *connection, struct os_mbuf *om);
-
-int host_hci_cmd_build_set_data_len(uint16_t connection_handle,
-                                    uint16_t tx_octets, uint16_t tx_time,
-                                    uint8_t *dst, int dst_len);
-int host_hci_cmd_build_add_to_resolv_list(
-                                    struct hci_add_dev_to_resolving_list *padd,
-                                    uint8_t *dst, int dst_len);
-int host_hci_cmd_build_remove_from_resolv_list(uint8_t addr_type,
-                                            uint8_t *addr, uint8_t *dst,
-                                            int dst_len);
-int host_hci_cmd_build_read_resolv_list_size(uint8_t *dst, int dst_len);
-int
-host_hci_cmd_build_clear_resolv_list(uint8_t *dst, int dst_len);
-int host_hci_cmd_build_read_peer_resolv_addr(uint8_t peer_identity_addr_type,
-                                          uint8_t *peer_identity_addr,
-                                          uint8_t *dst, int dst_len);
-int host_hci_cmd_build_read_lcl_resolv_addr(uint8_t local_identity_addr_type,
-                                        uint8_t *local_identity_addr,
-                                        uint8_t *dst, int dst_len);
-int host_hci_cmd_build_set_addr_res_en(uint8_t enable,
-                                          uint8_t *dst, int dst_len);
-int host_hci_cmd_build_set_resolv_priv_addr_timeout(uint16_t timeout,
-                                                            uint8_t *dst,
-                                                            int dst_len);
-
-void host_hci_timer_set(void);
-
-int host_hci_cmd_build_set_random_addr(uint8_t *addr, uint8_t *dst, int dst_len);
-
-#endif /* H_HOST_HCI_ */
diff --git a/net/nimble/host/pkg.yml b/net/nimble/host/pkg.yml
index f597f344..a45ba1e5 100644
--- a/net/nimble/host/pkg.yml
+++ b/net/nimble/host/pkg.yml
@@ -36,17 +36,23 @@ pkg.deps:
     # Tinycrypt is only required when secure connections (NIMBPLE_OPT_SM_SC)
     # is enabled.  It always gets built as a dependency, but not is not
     # included by the linker unless SC is enabled.  XXX: We should not build
-    # this library if it is not requiresd.
+    # this library if it is not required.
     - libs/tinycrypt
 
 pkg.req_apis:
+    - ble_transport
     - console
 
+pkg.features:
+    - BLE_HOST
+
 # Satisfy capability dependencies for the self-contained test executable.
-pkg.deps.SELFTEST: libs/console/stub
+pkg.deps.SELFTEST:
+    - libs/console/stub
+    - net/nimble/transport/ram
+
 pkg.cflags.SELFTEST:
-    - -DPHONY_TRANSPORT=1
-    - -DPHONY_HCI_ACKS=1
-    - -DNIMBLE_OPT_SM=1
-    - -DNIMBLE_OPT_SM_SC=1
+    - "-DPHONY_HCI_ACKS=1"
+    - "-DNIMBLE_OPT_SM=1"
+    - "-DNIMBLE_OPT_SM_SC=1"
 pkg.cflags.TEST: -DBLE_HS_DEBUG
diff --git a/net/nimble/host/services/gap/include/services/gap/ble_svc_gap.h b/net/nimble/host/services/gap/include/services/gap/ble_svc_gap.h
new file mode 100644
index 00000000..95d4226c
--- /dev/null
+++ b/net/nimble/host/services/gap/include/services/gap/ble_svc_gap.h
@@ -0,0 +1,39 @@
+/**
+ * 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 H_BLE_SVC_GAP_
+#define H_BLE_SVC_GAP_
+
+struct ble_hs_cfg;
+
+#define BLE_SVC_GAP_UUID16                                  0x1800
+#define BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME                  0x2a00
+#define BLE_SVC_GAP_CHR_UUID16_APPEARANCE                   0x2a01
+#define BLE_SVC_GAP_CHR_UUID16_PERIPH_PRIV_FLAG             0x2a02
+#define BLE_SVC_GAP_CHR_UUID16_RECONNECT_ADDR               0x2a03
+#define BLE_SVC_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS      0x2a04
+
+#define BLE_SVC_GAP_APPEARANCE_GEN_COMPUTER                 128
+
+const char *ble_svc_gap_device_name(void);
+int ble_svc_gap_device_name_set(const char *name);
+
+int ble_svc_gap_init(struct ble_hs_cfg *cfg);
+
+#endif
diff --git a/net/nimble/host/services/gap/pkg.yml b/net/nimble/host/services/gap/pkg.yml
new file mode 100644
index 00000000..b84e8161
--- /dev/null
+++ b/net/nimble/host/services/gap/pkg.yml
@@ -0,0 +1,31 @@
+#
+# 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: net/nimble/host/services/gap
+pkg.description: Implements the GAP Service. 
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - nimble
+    - gap
+
+pkg.deps:
+    - net/nimble/host
diff --git a/net/nimble/host/services/gap/src/ble_svc_gap.c b/net/nimble/host/services/gap/src/ble_svc_gap.c
new file mode 100644
index 00000000..1a2e8a48
--- /dev/null
+++ b/net/nimble/host/services/gap/src/ble_svc_gap.c
@@ -0,0 +1,167 @@
+/**
+ * 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 <assert.h>
+#include <string.h>
+#include "host/ble_hs.h"
+#include "services/gap/ble_svc_gap.h"
+
+/* XXX: This should be configurable. */
+#define BLE_SVC_GAP_NAME_MAX_LEN    31
+
+static char ble_svc_gap_name[BLE_SVC_GAP_NAME_MAX_LEN + 1] = "nimble";
+static uint16_t ble_svc_gap_appearance;
+static uint8_t ble_svc_gap_privacy_flag;
+static uint8_t ble_svc_gap_reconnect_addr[6];
+static uint8_t ble_svc_gap_pref_conn_params[8];
+
+static int
+ble_svc_gap_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_gap_defs[] = {
+    {
+        /*** Service: GAP. */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid128 = BLE_UUID16(BLE_SVC_GAP_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            /*** Characteristic: Device Name. */
+            .uuid128 = BLE_UUID16(BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            /*** Characteristic: Appearance. */
+            .uuid128 = BLE_UUID16(BLE_SVC_GAP_CHR_UUID16_APPEARANCE),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            /*** Characteristic: Peripheral Privacy Flag. */
+            .uuid128 = BLE_UUID16(BLE_SVC_GAP_CHR_UUID16_PERIPH_PRIV_FLAG),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            /*** Characteristic: Reconnection Address. */
+            .uuid128 = BLE_UUID16(BLE_SVC_GAP_CHR_UUID16_RECONNECT_ADDR),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_WRITE,
+        }, {
+            /*** Characteristic: Peripheral Preferred Connection Parameters. */
+            .uuid128 =
+                BLE_UUID16(BLE_SVC_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS),
+            .access_cb = ble_svc_gap_access,
+            .flags = BLE_GATT_CHR_F_READ,
+        }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+static int
+ble_svc_gap_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    uint16_t uuid16;
+    int rc;
+
+    uuid16 = ble_uuid_128_to_16(ctxt->chr->uuid128);
+    assert(uuid16 != 0);
+
+    switch (uuid16) {
+    case BLE_SVC_GAP_CHR_UUID16_DEVICE_NAME:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, ble_svc_gap_name,
+                            strlen(ble_svc_gap_name));
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    case BLE_SVC_GAP_CHR_UUID16_APPEARANCE:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &ble_svc_gap_appearance,
+                            sizeof ble_svc_gap_appearance);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    case BLE_SVC_GAP_CHR_UUID16_PERIPH_PRIV_FLAG:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &ble_svc_gap_privacy_flag,
+                            sizeof ble_svc_gap_privacy_flag);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    case BLE_SVC_GAP_CHR_UUID16_RECONNECT_ADDR:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR);
+        if (OS_MBUF_PKTLEN(ctxt->om) != sizeof ble_svc_gap_reconnect_addr) {
+            return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+        }
+        ble_hs_mbuf_to_flat(ctxt->om, ble_svc_gap_reconnect_addr,
+                            sizeof ble_svc_gap_reconnect_addr, NULL);
+        return 0;
+
+    case BLE_SVC_GAP_CHR_UUID16_PERIPH_PREF_CONN_PARAMS:
+        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+        rc = os_mbuf_append(ctxt->om, &ble_svc_gap_pref_conn_params,
+                            sizeof ble_svc_gap_pref_conn_params);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+}
+
+const char *
+ble_svc_gap_device_name(void)
+{
+    return ble_svc_gap_name;
+}
+
+int
+ble_svc_gap_device_name_set(const char *name)
+{
+    int len;
+
+    len = strlen(name);
+    if (len > BLE_SVC_GAP_NAME_MAX_LEN) {
+        return BLE_HS_EINVAL;
+    }
+
+    memcpy(ble_svc_gap_name, name, len);
+    ble_svc_gap_name[len] = '\0';
+
+    return 0;
+}
+
+int
+ble_svc_gap_init(struct ble_hs_cfg *cfg)
+{
+    int rc;
+
+    rc = ble_gatts_count_cfg(ble_svc_gap_defs, cfg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_gatts_add_svcs(ble_svc_gap_defs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
diff --git a/net/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h b/net/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h
new file mode 100644
index 00000000..320a3ff1
--- /dev/null
+++ b/net/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h
@@ -0,0 +1,29 @@
+/**
+ * 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 H_BLE_SVC_GATT_
+#define H_BLE_SVC_GATT_
+
+struct ble_hs_cfg;
+
+#define BLE_SVC_GATT_CHR_SERVICE_CHANGED_UUID16     0x2a05
+
+int ble_svc_gatt_init(struct ble_hs_cfg *cfg);
+
+#endif
diff --git a/net/nimble/host/services/gatt/pkg.yml b/net/nimble/host/services/gatt/pkg.yml
new file mode 100644
index 00000000..b8fdabe1
--- /dev/null
+++ b/net/nimble/host/services/gatt/pkg.yml
@@ -0,0 +1,31 @@
+#
+# 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: net/nimble/host/services/gatt
+pkg.description: Implements the GATT service.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - nimble
+    - gatt
+
+pkg.deps:
+    - net/nimble/host
diff --git a/net/nimble/host/services/gatt/src/ble_svc_gatt.c b/net/nimble/host/services/gatt/src/ble_svc_gatt.c
new file mode 100644
index 00000000..74d3ac97
--- /dev/null
+++ b/net/nimble/host/services/gatt/src/ble_svc_gatt.c
@@ -0,0 +1,90 @@
+/**
+ * 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 <assert.h>
+
+#include "host/ble_hs.h"
+#include "services/gatt/ble_svc_gatt.h"
+
+static int
+ble_svc_gatt_access(uint16_t conn_handle, uint16_t attr_handle,
+                    struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_gatt_defs[] = {
+    {
+        /*** Service: GATT */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid128 = BLE_UUID16(BLE_GATT_SVC_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid128 = BLE_UUID16(BLE_SVC_GATT_CHR_SERVICE_CHANGED_UUID16),
+            .access_cb = ble_svc_gatt_access,
+            .flags = BLE_GATT_CHR_F_INDICATE,
+        }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+static int
+ble_svc_gatt_access(uint16_t conn_handle, uint16_t attr_handle,
+                    struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    uint8_t *u8;
+
+    /* The only operation allowed for this characteristic is indicate.  This
+     * access callback gets called by the stack when it needs to read the
+     * characteristic value to populate the outgoing indication command.
+     * Therefore, this callback should only get called during an attempt to
+     * read the characteristic.
+     */
+    assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+    assert(ctxt->chr == &ble_svc_gatt_defs[0].characteristics[0]);
+
+    /* XXX: For now, always respond with 0 (unchanged). */
+    u8 = os_mbuf_extend(ctxt->om, 1);
+    if (u8 == NULL) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+
+    *u8 = 0;
+
+    return 0;
+}
+
+int
+ble_svc_gatt_init(struct ble_hs_cfg *cfg)
+{
+    int rc;
+
+    rc = ble_gatts_count_cfg(ble_svc_gatt_defs, cfg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_gatts_add_svcs(ble_svc_gatt_defs);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
diff --git a/net/nimble/host/services/lls/include/services/lls/ble_svc_lls.h b/net/nimble/host/services/lls/include/services/lls/ble_svc_lls.h
new file mode 100644
index 00000000..8dee7796
--- /dev/null
+++ b/net/nimble/host/services/lls/include/services/lls/ble_svc_lls.h
@@ -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.
+ */
+
+#ifndef H_BLE_SVC_LLS_
+#define H_BLE_SVC_LLS_
+
+struct ble_hs_cfg;
+
+#define BLE_SVC_LLS_UUID16                                  0x1803
+#define BLE_SVC_LLS_CHR_UUID16_ALERT_LEVEL                  0x2a06
+
+/* Alert level definitions */
+#define BLE_SVC_LLS_ALERT_LEVEL_NO_ALERT                    0
+#define BLE_SVC_LLS_ALERT_LEVEL_MILD_ALERT                  1
+#define BLE_SVC_LLS_ALERT_LEVEL_HIGH_ALERT                  2
+
+typedef int ble_svc_lls_event_fn(uint8_t alert_level); 
+
+uint8_t ble_svc_lls_alert_level_get(void);
+int ble_svc_lls_alert_level_set(uint8_t alert_level);
+void ble_svc_lls_on_gap_disconnect(int reason); 
+
+int ble_svc_lls_init(struct ble_hs_cfg *cfg, 
+                     uint8_t initial_alert_level,
+                     ble_svc_lls_event_fn *cb);
+
+#endif
+
diff --git a/net/nimble/host/services/lls/pkg.yml b/net/nimble/host/services/lls/pkg.yml
new file mode 100644
index 00000000..d45c49bb
--- /dev/null
+++ b/net/nimble/host/services/lls/pkg.yml
@@ -0,0 +1,31 @@
+#
+# 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: net/nimble/host/services/lls
+pkg.description: Link Loss Service Implementation.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble 
+    - bluetooth
+    - lls
+    - nimble 
+
+pkg.deps:
+    - net/nimble/host
diff --git a/net/nimble/host/services/lls/src/ble_svc_lls.c b/net/nimble/host/services/lls/src/ble_svc_lls.c
new file mode 100644
index 00000000..3371fda8
--- /dev/null
+++ b/net/nimble/host/services/lls/src/ble_svc_lls.c
@@ -0,0 +1,201 @@
+/**
+ * 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 <assert.h>
+#include <string.h>
+#include "host/ble_hs.h"
+#include "services/lls/ble_svc_lls.h"
+
+/* Callback function */
+static ble_svc_lls_event_fn *cb_fn; 
+/* Alert level */
+static uint8_t ble_svc_lls_alert_level;
+
+/* Write characteristic function */
+static int
+ble_svc_lls_chr_write(struct os_mbuf *om, uint16_t min_len, 
+                      uint16_t max_len, void *dst, 
+                      uint16_t *len);
+
+/* Access function */
+static int
+ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg);
+
+static const struct ble_gatt_svc_def ble_svc_lls_defs[] = {
+    {
+        /*** Service: Link Loss Service (LLS). */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid128 = BLE_UUID16(BLE_SVC_LLS_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            /*** Characteristic: Alert Level. */
+            .uuid128 = BLE_UUID16(BLE_SVC_LLS_CHR_UUID16_ALERT_LEVEL),
+            .access_cb = ble_svc_lls_access,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE,
+        }, {
+            0, /* No more characteristics in this service. */
+        } },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+/**
+ * Writes the received value from a characteristic write to 
+ * the given destination.
+ */
+static int
+ble_svc_lls_chr_write(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;
+    }   
+
+    return 0;
+}
+
+/**
+ * Simple read/write access callback for the alert level
+ * characteristic.
+ */
+static int
+ble_svc_lls_access(uint16_t conn_handle, uint16_t attr_handle,
+                   struct ble_gatt_access_ctxt *ctxt, void *arg)
+{
+    assert(ctxt->chr == &ble_svc_lls_defs[0].characteristics[0]);
+    int rc;
+    switch (ctxt->op) {
+    case BLE_GATT_ACCESS_OP_READ_CHR:
+        rc = os_mbuf_append(ctxt->om, &ble_svc_lls_alert_level,
+                            sizeof ble_svc_lls_alert_level);
+        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+
+    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+        rc = ble_svc_lls_chr_write(ctxt->om, 
+                                   sizeof ble_svc_lls_alert_level,
+                                   sizeof ble_svc_lls_alert_level,
+                                   &ble_svc_lls_alert_level, NULL);
+        return rc; 
+
+    default:
+        assert(0);
+        return BLE_ATT_ERR_UNLIKELY;
+    }
+
+    return 0;
+}
+
+/**
+ * This function is the crux of the link loss service. The application
+ * developer must call this function inside the gap event callback
+ * function when a BLE_GAP_EVENT_DISCONNECT event is received and
+ * pass the disconnect reason into this function. 
+ * 
+ * Here, we then check if the disconnect reason is due to a timout, and if
+ * so, we call the ble_svc_lls_event_fn callback with the current 
+ * alert level. The actual alert implementation is left up to the 
+ * developer.
+ *
+ * @param reason            The reason attatched to the GAP disconnect
+ *                              event.
+ */
+void
+ble_svc_lls_on_gap_disconnect(int reason)
+{
+    if (reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_SPVN_TMO)) {
+            cb_fn(ble_svc_lls_alert_level);
+    } 
+}
+
+/**
+ * Gets the current alert level.
+ *
+ * @return The current alert level
+ */
+uint8_t
+ble_svc_lls_alert_level_get(void)
+{
+    return ble_svc_lls_alert_level;
+}
+
+/**
+ * Sets the current alert level. 
+ *
+ * @return 0 on success, BLE_HS_EINVAL if the given alert level is not valid.
+ */
+int
+ble_svc_lls_alert_level_set(uint8_t alert_level)
+{
+    if (alert_level > BLE_SVC_LLS_ALERT_LEVEL_HIGH_ALERT) {
+        return BLE_HS_EINVAL;
+    }
+    
+    memcpy(&ble_svc_lls_alert_level, &alert_level, 
+           sizeof alert_level);
+
+    return 0;
+}
+
+/**
+ * Initialize the LLS. The developer must specify the event function
+ * callback for the LLS to function properly.
+ *
+ * @param initial_alert_level       The initial alert value to set
+ * @param cb                        The callback function to call when 
+ *                                      connection has been lost due to 
+ *                                      link loss
+ */
+int
+ble_svc_lls_init(struct ble_hs_cfg *cfg, uint8_t initial_alert_level,
+                 ble_svc_lls_event_fn *cb)
+{
+    int rc;
+
+    if (!cb) {
+        return BLE_HS_EINVAL;
+    }
+    
+    ble_svc_lls_alert_level = initial_alert_level;
+    cb_fn = cb;
+
+    rc = ble_gatts_count_cfg(ble_svc_lls_defs, cfg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    rc = ble_gatts_add_svcs(ble_svc_lls_defs);
+    if (rc != 0) {
+        return rc;
+    }
+    
+    return 0;
+}
diff --git a/net/nimble/host/src/ble_att.c b/net/nimble/host/src/ble_att.c
index 68e1c7b2..a6d39b82 100644
--- a/net/nimble/host/src/ble_att.c
+++ b/net/nimble/host/src/ble_att.c
@@ -22,9 +22,7 @@
 #include "bsp/bsp.h"
 #include "ble_hs_priv.h"
 
-static bssnz_t uint8_t ble_att_flat_buf[BLE_ATT_ATTR_MAX_LEN];
-
-static uint16_t ble_att_preferred_mtu;
+static uint16_t ble_att_preferred_mtu_val;
 
 /** Dispatch table for incoming ATT requests.  Sorted by op code. */
 typedef int ble_att_rx_fn(uint16_t conn_handle, struct os_mbuf **om);
@@ -145,15 +143,12 @@ ble_att_rx_dispatch_entry_find(uint8_t op)
     return NULL;
 }
 
-int
+void
 ble_att_conn_chan_find(uint16_t conn_handle, struct ble_hs_conn **out_conn,
                        struct ble_l2cap_chan **out_chan)
 {
-    int rc;
-
-    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_ATT,
-                                         out_conn, out_chan);
-    return rc;
+    ble_hs_misc_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT,
+                               out_conn, out_chan);
 }
 
 void
@@ -390,17 +385,29 @@ ble_att_inc_rx_stat(uint8_t att_op)
     }
 }
 
-/**
- * Retrieves a pointer to the global ATT flat buffer.  This buffer is only used
- * by the host parent task, so users can assume exclusive access.
- */
-uint8_t *
-ble_att_get_flat_buf(void)
+void
+ble_att_truncate_to_mtu(const struct ble_l2cap_chan *att_chan,
+                        struct os_mbuf *txom)
 {
-    BLE_HS_DBG_ASSERT(ble_hs_is_parent_task());
-    return ble_att_flat_buf;
+    int32_t extra_len;
+    uint16_t mtu;
+
+    mtu = ble_l2cap_chan_mtu(att_chan);
+    extra_len = OS_MBUF_PKTLEN(txom) - mtu;
+    if (extra_len > 0) {
+        os_mbuf_adj(txom, -extra_len);
+    }
 }
 
+/**
+ * Retrieves the ATT MTU of the specified connection.  If an MTU exchange for
+ * this connection has occurred, the MTU is the lower of the two peers'
+ * preferred values.  Otherwise, the MTU is the default value of 23.
+ *
+ * @param conn_handle           The handle of the connection to query.
+ *
+ * @return                      The specified connection's ATT MTU.
+ */
 uint16_t
 ble_att_mtu(uint16_t conn_handle)
 {
@@ -422,6 +429,16 @@ ble_att_mtu(uint16_t conn_handle)
     return mtu;
 }
 
+void
+ble_att_set_peer_mtu(struct ble_l2cap_chan *chan, uint16_t peer_mtu)
+{
+    if (peer_mtu < BLE_ATT_MTU_DFLT) {
+        peer_mtu = BLE_ATT_MTU_DFLT;
+    }
+
+    chan->blc_peer_mtu = peer_mtu;
+}
+
 static int
 ble_att_rx(uint16_t conn_handle, struct os_mbuf **om)
 {
@@ -449,19 +466,40 @@ ble_att_rx(uint16_t conn_handle, struct os_mbuf **om)
     return 0;
 }
 
-void
-ble_att_set_peer_mtu(struct ble_l2cap_chan *chan, uint16_t peer_mtu)
+/**
+ * Retrieves the preferred ATT MTU.
+ *
+ * @return                      The preferred ATT MTU.
+ */
+uint16_t
+ble_att_preferred_mtu(void)
 {
-    if (peer_mtu < BLE_ATT_MTU_DFLT) {
-        peer_mtu = BLE_ATT_MTU_DFLT;
-    }
-
-    chan->blc_peer_mtu = peer_mtu;
+    return ble_att_preferred_mtu_val;
 }
 
+/**
+ * Sets the preferred ATT MTU; the device will indicate this value in all
+ * subseqeunt ATT MTU exchanges.  The ATT MTU of a connection is equal to the
+ * lower of the two peers' preferred MTU values.  The ATT MTU is what dictates
+ * the maximum size of any message sent during a GATT procedure.
+ *
+ * The specified MTU must be within the following range: [23, BLE_ATT_MTU_MAX].
+ * 23 is a minimum imposed by the Bluetooth specification; BLE_ATT_MTU_MAX is a
+ * NimBLE compile-time setting.
+ *
+ * @param mtu                   The preferred ATT MTU.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the specifeid value is not
+ *                                  within the allowed range.
+ */
 int
 ble_att_set_preferred_mtu(uint16_t mtu)
 {
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
+    int i;
+
     if (mtu < BLE_ATT_MTU_DFLT) {
         return BLE_HS_EINVAL;
     }
@@ -469,9 +507,24 @@ ble_att_set_preferred_mtu(uint16_t mtu)
         return BLE_HS_EINVAL;
     }
 
-    ble_att_preferred_mtu = mtu;
+    ble_att_preferred_mtu_val = mtu;
+
+    /* Set my_mtu for established connections that haven't exchanged. */
+    ble_hs_lock();
 
-    /* XXX: Set my_mtu for established connections that haven't exchanged. */
+    i = 0;
+    while ((conn = ble_hs_conn_find_by_idx(i)) != NULL) {
+        chan = ble_hs_conn_chan_find(conn, BLE_L2CAP_CID_ATT);
+        BLE_HS_DBG_ASSERT(chan != NULL);
+
+        if (!(chan->blc_flags & BLE_L2CAP_CHAN_F_TXED_MTU)) {
+            chan->blc_my_mtu = mtu;
+        }
+
+        i++;
+    }
+
+    ble_hs_unlock();
 
     return 0;
 }
@@ -487,7 +540,7 @@ ble_att_create_chan(void)
     }
 
     chan->blc_cid = BLE_L2CAP_CID_ATT;
-    chan->blc_my_mtu = ble_att_preferred_mtu;
+    chan->blc_my_mtu = ble_att_preferred_mtu_val;
     chan->blc_default_mtu = BLE_ATT_MTU_DFLT;
     chan->blc_rx_fn = ble_att_rx;
 
@@ -499,7 +552,7 @@ ble_att_init(void)
 {
     int rc;
 
-    ble_att_preferred_mtu = BLE_ATT_MTU_PREFERRED_DFLT;
+    ble_att_preferred_mtu_val = BLE_ATT_MTU_PREFERRED_DFLT;
 
     rc = stats_init_and_reg(
         STATS_HDR(ble_att_stats), STATS_SIZE_INIT_PARMS(ble_att_stats,
diff --git a/net/nimble/host/src/ble_att_clt.c b/net/nimble/host/src/ble_att_clt.c
index dba3527b..2e7352ba 100644
--- a/net/nimble/host/src/ble_att_clt.c
+++ b/net/nimble/host/src/ble_att_clt.c
@@ -29,84 +29,40 @@
 static int
 ble_att_clt_init_req(uint16_t initial_sz, struct os_mbuf **out_txom)
 {
+    struct os_mbuf *om;
     void *buf;
     int rc;
 
-    *out_txom = ble_hs_misc_pkthdr();
-    if (*out_txom == NULL) {
+    *out_txom = NULL;
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    if (om == NULL) {
         rc = BLE_HS_ENOMEM;
         goto err;
     }
 
-    buf = os_mbuf_extend(*out_txom, initial_sz);
+    buf = os_mbuf_extend(om, initial_sz);
     if (buf == NULL) {
         rc = BLE_HS_ENOMEM;
         goto err;
     }
 
     /* The caller expects the initial buffer to be at the start of the mbuf. */
-    BLE_HS_DBG_ASSERT(buf == (*out_txom)->om_data);
+    BLE_HS_DBG_ASSERT(buf == om->om_data);
 
+    *out_txom = om;
     return 0;
 
 err:
-    os_mbuf_free_chain(*out_txom);
-    *out_txom = NULL;
+    os_mbuf_free_chain(om);
     return rc;
 }
 
 static int
-ble_att_clt_append_blob(uint16_t conn_handle, struct os_mbuf *txom,
-                        void *blob, int blob_len)
-{
-    int rc;
-
-    if (blob_len < 0) {
-        return BLE_HS_EINVAL;
-    }
-    if (blob_len == 0) {
-        return 0;
-    }
-
-    rc = os_mbuf_append(txom, blob, blob_len);
-    if (rc != 0) {
-        return rc;
-    }
-
-    return 0;
-}
-
-static int
-ble_att_clt_copy_attr_to_flatbuf(struct os_mbuf *om, void **out_attr_val,
-                                 uint16_t *out_attr_len)
-{
-    uint8_t *flat_buf;
-    uint16_t attr_len;
-
-    /* Make sure the attribute value isn't too big. */
-    attr_len = OS_MBUF_PKTLEN(om);
-    if (attr_len > BLE_ATT_ATTR_MAX_LEN) {
-        *out_attr_len = 0;
-        *out_attr_val = NULL;
-        return BLE_HS_EBADDATA;
-    }
-
-    /* Copy the attribute data into the global ATT flat buffer. */
-    flat_buf = ble_att_get_flat_buf();
-    os_mbuf_copydata(om, 0, attr_len, flat_buf);
-    *out_attr_val = flat_buf;
-    *out_attr_len = attr_len;
-    return 0;
-}
-
-static int
 ble_att_clt_tx_req(uint16_t conn_handle, struct os_mbuf *txom)
 {
     struct ble_l2cap_chan *chan;
     struct ble_hs_conn *conn;
-    uint16_t total_len;
-    uint16_t mtu;
-    int extra_len;
     int rc;
 
     BLE_HS_DBG_ASSERT_EVAL(txom->om_len >= 1);
@@ -114,25 +70,16 @@ ble_att_clt_tx_req(uint16_t conn_handle, struct os_mbuf *txom)
 
     ble_hs_lock();
 
-    rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
-    if (rc == 0) {
-        /* Reduce the size of the transmission to fit the connection's ATT
-         * MTU.
-         */
-        total_len = OS_MBUF_PKTLEN(txom);
-        mtu = ble_l2cap_chan_mtu(chan);
-        extra_len = total_len - mtu;
-        if (extra_len > 0) {
-            os_mbuf_adj(txom, -extra_len);
-        }
-
-        rc = ble_l2cap_tx(conn, chan, txom);
-        txom = NULL;
-    }
+    ble_att_conn_chan_find(conn_handle, &conn, &chan);
+    ble_att_truncate_to_mtu(chan, txom);
+    rc = ble_l2cap_tx(conn, chan, txom);
 
     ble_hs_unlock();
 
-    os_mbuf_free_chain(txom);
+    if (rc != 0) {
+        os_mbuf_free_chain(txom);
+    }
+
     return rc;
 }
 
@@ -141,17 +88,17 @@ ble_att_clt_tx_req(uint16_t conn_handle, struct os_mbuf *txom)
  *****************************************************************************/
 
 int
-ble_att_clt_rx_error(uint16_t conn_handle, struct os_mbuf **om)
+ble_att_clt_rx_error(uint16_t conn_handle, struct os_mbuf **rxom)
 {
     struct ble_att_error_rsp rsp;
     int rc;
 
-    rc = ble_hs_misc_pullup_base(om, BLE_ATT_ERROR_RSP_SZ);
+    rc = ble_hs_mbuf_pullup_base(rxom, BLE_ATT_ERROR_RSP_SZ);
     if (rc != 0) {
         return rc;
     }
 
-    ble_att_error_rsp_parse((*om)->om_data, (*om)->om_len, &rsp);
+    ble_att_error_rsp_parse((*rxom)->om_data, (*rxom)->om_len, &rsp);
     BLE_ATT_LOG_CMD(0, "error rsp", conn_handle, ble_att_error_rsp_log, &rsp);
 
     ble_gattc_rx_err(conn_handle, &rsp);
@@ -163,62 +110,51 @@ ble_att_clt_rx_error(uint16_t conn_handle, struct os_mbuf **om)
  * $mtu exchange                                                             *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_mtu_req(struct ble_att_mtu_cmd *req,
-                          struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_MTU_CMD_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_mtu_req_write(txom->om_data, txom->om_len, req);
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
-ble_att_clt_tx_mtu(uint16_t conn_handle, struct ble_att_mtu_cmd *req)
+ble_att_clt_tx_mtu(uint16_t conn_handle, const struct ble_att_mtu_cmd *req)
 {
     struct ble_l2cap_chan *chan;
     struct ble_hs_conn *conn;
     struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "mtu req", conn_handle, ble_att_mtu_cmd_log, req);
-
     if (req->bamc_mtu < BLE_ATT_MTU_DFLT) {
         return BLE_HS_EINVAL;
     }
 
-    rc = ble_att_clt_build_mtu_req(req, &txom);
+    ble_hs_lock();
+
+    ble_att_conn_chan_find(conn_handle, &conn, &chan);
+    if (chan == NULL) {
+        rc = BLE_HS_ENOTCONN;
+    } else if (chan->blc_flags & BLE_L2CAP_CHAN_F_TXED_MTU) {
+        rc = BLE_HS_EALREADY;
+    } else {
+        rc = 0;
+    }
+    ble_hs_unlock();
+
     if (rc != 0) {
         return rc;
     }
 
+    rc = ble_att_clt_init_req(BLE_ATT_MTU_CMD_SZ, &txom);
+    if (rc != 0) {
+        return rc;
+    }
+    ble_att_mtu_req_write(txom->om_data, txom->om_len, req);
+
     rc = ble_att_clt_tx_req(conn_handle, txom);
     if (rc != 0) {
         return rc;
     }
 
+    BLE_ATT_LOG_CMD(1, "mtu req", conn_handle, ble_att_mtu_cmd_log, req);
+
     ble_hs_lock();
 
-    rc = ble_att_conn_chan_find(conn_handle, &conn, &chan);
-    if (rc == 0) {
-        chan->blc_flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
-    }
+    ble_att_conn_chan_find(conn_handle, &conn, &chan);
+    chan->blc_flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
 
     ble_hs_unlock();
 
@@ -226,7 +162,7 @@ ble_att_clt_tx_mtu(uint16_t conn_handle, struct ble_att_mtu_cmd *req)
 }
 
 int
-ble_att_clt_rx_mtu(uint16_t conn_handle, struct os_mbuf **om)
+ble_att_clt_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom)
 {
     struct ble_att_mtu_cmd cmd;
     struct ble_l2cap_chan *chan;
@@ -235,20 +171,20 @@ ble_att_clt_rx_mtu(uint16_t conn_handle, struct os_mbuf **om)
 
     mtu = 0;
 
-    rc = ble_hs_misc_pullup_base(om, BLE_ATT_MTU_CMD_SZ);
+    rc = ble_hs_mbuf_pullup_base(rxom, BLE_ATT_MTU_CMD_SZ);
     if (rc == 0) {
-        ble_att_mtu_cmd_parse((*om)->om_data, (*om)->om_len, &cmd);
+        ble_att_mtu_rsp_parse((*rxom)->om_data, (*rxom)->om_len, &cmd);
         BLE_ATT_LOG_CMD(0, "mtu rsp", conn_handle, ble_att_mtu_cmd_log, &cmd);
 
         ble_hs_lock();
 
-        rc = ble_att_conn_chan_find(conn_handle, NULL, &chan);
-        if (rc == 0) {
-            ble_att_set_peer_mtu(chan, cmd.bamc_mtu);
-            mtu = ble_l2cap_chan_mtu(chan);
-        }
+        ble_att_conn_chan_find(conn_handle, NULL, &chan);
+        ble_att_set_peer_mtu(chan, cmd.bamc_mtu);
+        mtu = ble_l2cap_chan_mtu(chan);
 
         ble_hs_unlock();
+
+        ble_gap_mtu_event(conn_handle, BLE_L2CAP_CID_ATT, mtu);
     }
 
     ble_gattc_rx_mtu(conn_handle, rc, mtu);
@@ -259,27 +195,9 @@ ble_att_clt_rx_mtu(uint16_t conn_handle, struct os_mbuf **om)
  * $find information                                                         *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_find_info_req(struct ble_att_find_info_req *req,
-                                struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    rc = ble_att_clt_init_req(BLE_ATT_FIND_INFO_REQ_SZ, &txom);
-    if (rc != 0) {
-        return rc;
-    }
-
-    ble_att_find_info_req_write(txom->om_data, txom->om_len, req);
-    *out_txom = txom;
-
-    return 0;
-}
-
 int
 ble_att_clt_tx_find_info(uint16_t conn_handle,
-                         struct ble_att_find_info_req *req)
+                         const struct ble_att_find_info_req *req)
 {
 #if !NIMBLE_OPT(ATT_CLT_FIND_INFO)
     return BLE_HS_ENOTSUP;
@@ -288,25 +206,26 @@ ble_att_clt_tx_find_info(uint16_t conn_handle,
     struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "find info req", conn_handle,
-                    ble_att_find_info_req_log, req);
-
     if (req->bafq_start_handle == 0 ||
         req->bafq_start_handle > req->bafq_end_handle) {
 
         return BLE_HS_EINVAL;
     }
 
-    rc = ble_att_clt_build_find_info_req(req, &txom);
+    rc = ble_att_clt_init_req(BLE_ATT_FIND_INFO_REQ_SZ, &txom);
     if (rc != 0) {
         return rc;
     }
+    ble_att_find_info_req_write(txom->om_data, txom->om_len, req);
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
     if (rc != 0) {
         return rc;
     }
 
+    BLE_ATT_LOG_CMD(1, "find info req", conn_handle,
+                    ble_att_find_info_req_log, req);
+
     return 0;
 }
 
@@ -331,7 +250,7 @@ ble_att_clt_parse_find_info_entry(struct os_mbuf **rxom, uint8_t rsp_format,
         return BLE_HS_EBADDATA;
     }
 
-    rc = ble_hs_misc_pullup_base(rxom, entry_len);
+    rc = ble_hs_mbuf_pullup_base(rxom, entry_len);
     if (rc != 0) {
         return rc;
     }
@@ -374,7 +293,7 @@ ble_att_clt_rx_find_info(uint16_t conn_handle, struct os_mbuf **om)
     struct ble_att_find_info_rsp rsp;
     int rc;
 
-    rc = ble_hs_misc_pullup_base(om, BLE_ATT_FIND_INFO_RSP_BASE_SZ);
+    rc = ble_hs_mbuf_pullup_base(om, BLE_ATT_FIND_INFO_RSP_BASE_SZ);
     if (rc != 0) {
         goto done;
     }
@@ -408,63 +327,53 @@ done:
  * $find by type value                                                       *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_find_type_value_req(struct ble_att_find_type_value_req *req,
-                                      void *attribute_value, int value_len,
-                                      struct os_mbuf **out_txom)
-{
-    int rc;
-
-    rc = ble_att_clt_init_req(BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, out_txom);
-    if (rc != 0) {
-        return rc;
-    }
-
-    ble_att_find_type_value_req_write((*out_txom)->om_data,
-                                      (*out_txom)->om_len,
-                                      req);
-    rc = os_mbuf_append(*out_txom, attribute_value, value_len);
-    if (rc != 0) {
-        os_mbuf_free_chain(*out_txom);
-        return BLE_HS_ENOMEM;
-    }
-
-    return 0;
-}
-
 int
 ble_att_clt_tx_find_type_value(uint16_t conn_handle,
-                               struct ble_att_find_type_value_req *req,
-                               void *attribute_value, int value_len)
+                               const struct ble_att_find_type_value_req *req,
+                               const void *attribute_value, int value_len)
 {
 #if !NIMBLE_OPT(ATT_CLT_FIND_TYPE)
     return BLE_HS_ENOTSUP;
 #endif
 
-    BLE_ATT_LOG_CMD(1, "find type value req", conn_handle,
-                    ble_att_find_type_value_req_log, req);
-
     struct os_mbuf *txom;
     int rc;
 
+    txom = NULL;
+
     if (req->bavq_start_handle == 0 ||
         req->bavq_start_handle > req->bavq_end_handle) {
 
-        return BLE_HS_EINVAL;
+        rc = BLE_HS_EINVAL;
+        goto err;
     }
 
-    rc = ble_att_clt_build_find_type_value_req(req, attribute_value, value_len,
-                                               &txom);
+    rc = ble_att_clt_init_req(BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, &txom);
     if (rc != 0) {
-        return rc;
+        goto err;
+    }
+
+    ble_att_find_type_value_req_write(txom->om_data, txom->om_len, req);
+    rc = os_mbuf_append(txom, attribute_value, value_len);
+    if (rc != 0) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
     }
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
+    txom = NULL;
     if (rc != 0) {
-        return rc;
+        goto err;
     }
 
+    BLE_ATT_LOG_CMD(1, "find type value req", conn_handle,
+                    ble_att_find_type_value_req_log, req);
+
     return 0;
+
+err:
+    os_mbuf_free_chain(txom);
+    return rc;
 }
 
 static int
@@ -524,42 +433,10 @@ ble_att_clt_rx_find_type_value(uint16_t conn_handle, struct os_mbuf **rxom)
  * $read by type                                                             *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_read_type_req(struct ble_att_read_type_req *req,
-                                void *uuid128, struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_READ_TYPE_REQ_BASE_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_read_type_req_write(txom->om_data, txom->om_len, req);
-
-    rc = ble_uuid_append(txom, uuid128);
-    if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
-    }
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
 ble_att_clt_tx_read_type(uint16_t conn_handle,
-                         struct ble_att_read_type_req *req,
-                         void *uuid128)
+                         const struct ble_att_read_type_req *req,
+                         const void *uuid128)
 {
 #if !NIMBLE_OPT(ATT_CLT_READ_TYPE)
     return BLE_HS_ENOTSUP;
@@ -568,25 +445,40 @@ ble_att_clt_tx_read_type(uint16_t conn_handle,
     struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "read type req", conn_handle,
-                    ble_att_read_type_req_log, req);
+    txom = NULL;
 
     if (req->batq_start_handle == 0 ||
         req->batq_start_handle > req->batq_end_handle) {
 
-        return BLE_HS_EINVAL;
+        rc = BLE_HS_EINVAL;
+        goto err;
     }
 
-    rc = ble_att_clt_build_read_type_req(req, uuid128, &txom);
+    rc = ble_att_clt_init_req(BLE_ATT_READ_TYPE_REQ_BASE_SZ, &txom);
     if (rc != 0) {
-        return rc;
+        goto err;
+    }
+
+    ble_att_read_type_req_write(txom->om_data, txom->om_len, req);
+    rc = ble_uuid_append(txom, uuid128);
+    if (rc != 0) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
     }
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
+    txom = NULL;
     if (rc != 0) {
-        return rc;
+        goto err;
     }
 
+    BLE_ATT_LOG_CMD(1, "read type req", conn_handle,
+                    ble_att_read_type_req_log, req);
+
+    return 0;
+
+err:
+    os_mbuf_free_chain(txom);
     return 0;
 }
 
@@ -596,7 +488,7 @@ ble_att_clt_parse_read_type_adata(struct os_mbuf **om, int data_len,
 {
     int rc;
 
-    rc = ble_hs_misc_pullup_base(om, data_len);
+    rc = ble_hs_mbuf_pullup_base(om, data_len);
     if (rc != 0) {
         return rc;
     }
@@ -619,7 +511,7 @@ ble_att_clt_rx_read_type(uint16_t conn_handle, struct os_mbuf **rxom)
     struct ble_att_read_type_rsp rsp;
     int rc;
 
-    rc = ble_hs_misc_pullup_base(rxom, BLE_ATT_READ_TYPE_RSP_BASE_SZ);
+    rc = ble_hs_mbuf_pullup_base(rxom, BLE_ATT_READ_TYPE_RSP_BASE_SZ);
     if (rc != 0) {
         goto done;
     }
@@ -653,34 +545,8 @@ done:
  * $read                                                                     *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_read_req(struct ble_att_read_req *req,
-                           struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_READ_REQ_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_read_req_write(txom->om_data, txom->om_len, req);
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
-ble_att_clt_tx_read(uint16_t conn_handle, struct ble_att_read_req *req)
+ble_att_clt_tx_read(uint16_t conn_handle, const struct ble_att_read_req *req)
 {
 #if !NIMBLE_OPT(ATT_CLT_READ)
     return BLE_HS_ENOTSUP;
@@ -689,23 +555,23 @@ ble_att_clt_tx_read(uint16_t conn_handle, struct ble_att_read_req *req)
     struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "read req", conn_handle,
-                    ble_att_read_req_log, req);
-
     if (req->barq_handle == 0) {
         return BLE_HS_EINVAL;
     }
 
-    rc = ble_att_clt_build_read_req(req, &txom);
+    rc = ble_att_clt_init_req(BLE_ATT_READ_REQ_SZ, &txom);
     if (rc != 0) {
         return rc;
     }
+    ble_att_read_req_write(txom->om_data, txom->om_len, req);
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
     if (rc != 0) {
         return rc;
     }
 
+    BLE_ATT_LOG_CMD(1, "read req", conn_handle, ble_att_read_req_log, req);
+
     return 0;
 }
 
@@ -716,10 +582,6 @@ ble_att_clt_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
     return BLE_HS_ENOTSUP;
 #endif
 
-    uint16_t value_len;
-    void *value;
-    int rc;
-
     BLE_ATT_LOG_EMPTY_CMD(0, "read rsp", conn_handle);
 
     /* Reponse consists of a one-byte opcode (already verified) and a variable
@@ -727,47 +589,18 @@ ble_att_clt_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
      */
     os_mbuf_adj(*rxom, BLE_ATT_READ_RSP_BASE_SZ);
 
-    /* Copy the attribute data into the global ATT flat buffer. */
-    rc = ble_att_clt_copy_attr_to_flatbuf(*rxom, &value, &value_len);
-
     /* Pass the Attribute Value field to GATT. */
-    ble_gattc_rx_read_rsp(conn_handle, rc, value, value_len);
-    return rc;
+    ble_gattc_rx_read_rsp(conn_handle, 0, rxom);
+    return 0;
 }
 
 /*****************************************************************************
  * $read blob                                                                *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_read_blob_req(struct ble_att_read_blob_req *req,
-                                struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_READ_BLOB_REQ_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_read_blob_req_write(txom->om_data, txom->om_len, req);
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
 ble_att_clt_tx_read_blob(uint16_t conn_handle,
-                         struct ble_att_read_blob_req *req)
+                         const struct ble_att_read_blob_req *req)
 {
 #if !NIMBLE_OPT(ATT_CLT_READ_BLOB)
     return BLE_HS_ENOTSUP;
@@ -776,23 +609,24 @@ ble_att_clt_tx_read_blob(uint16_t conn_handle,
     struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "read blob req", conn_handle,
-                    ble_att_read_blob_req_log, req);
-
     if (req->babq_handle == 0) {
         return BLE_HS_EINVAL;
     }
 
-    rc = ble_att_clt_build_read_blob_req(req, &txom);
+    rc = ble_att_clt_init_req(BLE_ATT_READ_BLOB_REQ_SZ, &txom);
     if (rc != 0) {
         return rc;
     }
+    ble_att_read_blob_req_write(txom->om_data, txom->om_len, req);
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
     if (rc != 0) {
         return rc;
     }
 
+    BLE_ATT_LOG_CMD(1, "read blob req", conn_handle,
+                    ble_att_read_blob_req_log, req);
+
     return 0;
 }
 
@@ -803,10 +637,6 @@ ble_att_clt_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
     return BLE_HS_ENOTSUP;
 #endif
 
-    uint16_t value_len;
-    void *value;
-    int rc;
-
     BLE_ATT_LOG_EMPTY_CMD(0, "read blob rsp", conn_handle);
 
     /* Reponse consists of a one-byte opcode (already verified) and a variable
@@ -814,12 +644,9 @@ ble_att_clt_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
      */
     os_mbuf_adj(*rxom, BLE_ATT_READ_BLOB_RSP_BASE_SZ);
 
-    /* Copy the attribute data into the global ATT flat buffer. */
-    rc = ble_att_clt_copy_attr_to_flatbuf(*rxom, &value, &value_len);
-
     /* Pass the Attribute Value field to GATT. */
-    ble_gattc_rx_read_blob_rsp(conn_handle, rc, value, value_len);
-    return rc;
+    ble_gattc_rx_read_blob_rsp(conn_handle, 0, rxom);
+    return 0;
 }
 
 /*****************************************************************************
@@ -827,7 +654,8 @@ ble_att_clt_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
  *****************************************************************************/
 
 static int
-ble_att_clt_build_read_mult_req(uint16_t *att_handles, int num_att_handles,
+ble_att_clt_build_read_mult_req(const uint16_t *att_handles,
+                                int num_att_handles,
                                 struct os_mbuf **out_txom)
 {
     struct os_mbuf *txom;
@@ -835,39 +663,34 @@ ble_att_clt_build_read_mult_req(uint16_t *att_handles, int num_att_handles,
     int rc;
     int i;
 
-    txom = NULL;
+    *out_txom = NULL;
 
     rc = ble_att_clt_init_req(BLE_ATT_READ_MULT_REQ_BASE_SZ, &txom);
     if (rc != 0) {
-        goto done;
+        goto err;
     }
-
     ble_att_read_mult_req_write(txom->om_data, txom->om_len);
 
     for (i = 0; i < num_att_handles; i++) {
         buf = os_mbuf_extend(txom, 2);
         if (buf == NULL) {
             rc = BLE_HS_ENOMEM;
-            goto done;
+            goto err;
         }
 
         htole16(buf, att_handles[i]);
     }
 
-    rc = 0;
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
     *out_txom = txom;
+    return 0;
+
+err:
+    os_mbuf_free_chain(txom);
     return rc;
 }
 
 int
-ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t *att_handles,
+ble_att_clt_tx_read_mult(uint16_t conn_handle, const uint16_t *att_handles,
                          int num_att_handles)
 {
 #if !NIMBLE_OPT(ATT_CLT_READ_MULT)
@@ -903,10 +726,6 @@ ble_att_clt_rx_read_mult(uint16_t conn_handle, struct os_mbuf **rxom)
     return BLE_HS_ENOTSUP;
 #endif
 
-    uint16_t value_len;
-    void *value;
-    int rc;
-
     BLE_ATT_LOG_EMPTY_CMD(0, "read mult rsp", conn_handle);
 
     /* Reponse consists of a one-byte opcode (already verified) and a variable
@@ -914,53 +733,19 @@ ble_att_clt_rx_read_mult(uint16_t conn_handle, struct os_mbuf **rxom)
      */
     os_mbuf_adj(*rxom, BLE_ATT_READ_MULT_RSP_BASE_SZ);
 
-    /* Copy the attribute data into the global ATT flat buffer. */
-    rc = ble_att_clt_copy_attr_to_flatbuf(*rxom, &value, &value_len);
-
     /* Pass the Attribute Value field to GATT. */
-    ble_gattc_rx_read_mult_rsp(conn_handle, rc, value, value_len);
-    return rc;
+    ble_gattc_rx_read_mult_rsp(conn_handle, 0, rxom);
+    return 0;
 }
 
 /*****************************************************************************
  * $read by group type                                                       *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_read_group_type_req(struct ble_att_read_group_type_req *req,
-                                      void *uuid128, struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_read_group_type_req_write(txom->om_data, txom->om_len, req);
-
-    rc = ble_uuid_append(txom, uuid128);
-    if (rc != 0) {
-        goto done;
-    }
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
 ble_att_clt_tx_read_group_type(uint16_t conn_handle,
-                               struct ble_att_read_group_type_req *req,
-                               void *uuid128)
+                               const struct ble_att_read_group_type_req *req,
+                               const void *uuid128)
 {
 #if !NIMBLE_OPT(ATT_CLT_READ_GROUP_TYPE)
     return BLE_HS_ENOTSUP;
@@ -971,26 +756,38 @@ ble_att_clt_tx_read_group_type(uint16_t conn_handle,
 
     txom = NULL;
 
-    BLE_ATT_LOG_CMD(1, "read group type req", conn_handle,
-                    ble_att_read_group_type_req_log, req);
-
     if (req->bagq_start_handle == 0 ||
         req->bagq_start_handle > req->bagq_end_handle) {
 
-        return BLE_HS_EINVAL;
+        rc = BLE_HS_EINVAL;
+        goto err;
     }
 
-    rc = ble_att_clt_build_read_group_type_req(req, uuid128, &txom);
+    rc = ble_att_clt_init_req(BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, &txom);
     if (rc != 0) {
-        return rc;
+        goto err;
+    }
+    ble_att_read_group_type_req_write(txom->om_data, txom->om_len, req);
+
+    rc = ble_uuid_append(txom, uuid128);
+    if (rc != 0) {
+        goto err;
     }
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
+    txom = NULL;
     if (rc != 0) {
-        return rc;
+        goto err;
     }
 
+    BLE_ATT_LOG_CMD(1, "read group type req", conn_handle,
+                    ble_att_read_group_type_req_log, req);
+
     return 0;
+
+err:
+    os_mbuf_free_chain(txom);
+    return rc;
 }
 
 static int
@@ -1004,7 +801,7 @@ ble_att_clt_parse_read_group_type_adata(
         return BLE_HS_EMSGSIZE;
     }
 
-    rc = ble_hs_misc_pullup_base(om, data_len);
+    rc = ble_hs_mbuf_pullup_base(om, data_len);
     if (rc != 0) {
         return rc;
     }
@@ -1028,7 +825,7 @@ ble_att_clt_rx_read_group_type(uint16_t conn_handle, struct os_mbuf **rxom)
     struct ble_att_read_group_type_rsp rsp;
     int rc;
 
-    rc = ble_hs_misc_pullup_base(rxom, BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ);
+    rc = ble_hs_mbuf_pullup_base(rxom, BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ);
     if (rc != 0) {
         goto done;
     }
@@ -1063,55 +860,21 @@ done:
  *****************************************************************************/
 
 static int
-ble_att_clt_build_write_req_or_cmd(uint16_t conn_handle,
-                                   struct ble_att_write_req *req,
-                                   void *value, uint16_t value_len, int is_req,
-                                   struct os_mbuf **out_txom)
+ble_att_clt_tx_write_req_or_cmd(uint16_t conn_handle,
+                                const struct ble_att_write_req *req,
+                                struct os_mbuf *txom, int is_req)
 {
-    struct os_mbuf *txom;
     int rc;
 
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_WRITE_REQ_BASE_SZ, &txom);
-    if (rc != 0) {
-        goto done;
+    txom = os_mbuf_prepend_pullup(txom, BLE_ATT_WRITE_REQ_BASE_SZ);
+    if (txom == NULL) {
+        return BLE_HS_ENOMEM;
     }
 
     if (is_req) {
-        ble_att_write_req_write(txom->om_data, txom->om_len, req);
+        ble_att_write_req_write(txom->om_data, BLE_ATT_WRITE_REQ_BASE_SZ, req);
     } else {
-        ble_att_write_cmd_write(txom->om_data, txom->om_len, req);
-    }
-
-    rc = ble_att_clt_append_blob(conn_handle, txom, value, value_len);
-    if (rc != 0) {
-        goto done;
-    }
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
-static int
-ble_att_clt_tx_write_req_or_cmd(uint16_t conn_handle,
-                                struct ble_att_write_req *req,
-                                void *value, uint16_t value_len,
-                                int is_req)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    rc = ble_att_clt_build_write_req_or_cmd(conn_handle, req, value, value_len,
-                                            is_req, &txom);
-    if (rc != 0) {
-        return rc;
+        ble_att_write_cmd_write(txom->om_data, BLE_ATT_WRITE_REQ_BASE_SZ, req);
     }
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
@@ -1123,8 +886,9 @@ ble_att_clt_tx_write_req_or_cmd(uint16_t conn_handle,
 }
 
 int
-ble_att_clt_tx_write_req(uint16_t conn_handle, struct ble_att_write_req *req,
-                         void *value, uint16_t value_len)
+ble_att_clt_tx_write_req(uint16_t conn_handle,
+                         const struct ble_att_write_req *req,
+                         struct os_mbuf *txom)
 {
 #if !NIMBLE_OPT(ATT_CLT_WRITE)
     return BLE_HS_ENOTSUP;
@@ -1132,17 +896,20 @@ ble_att_clt_tx_write_req(uint16_t conn_handle, struct ble_att_write_req *req,
 
     int rc;
 
+    rc = ble_att_clt_tx_write_req_or_cmd(conn_handle, req, txom, 1);
+    if (rc != 0) {
+        return rc;
+    }
+
     BLE_ATT_LOG_CMD(1, "write req", conn_handle, ble_att_write_cmd_log, req);
 
-    rc = ble_att_clt_tx_write_req_or_cmd(conn_handle, req, value, value_len,
-                                         1);
-    return rc;
+    return 0;
 }
 
 int
 ble_att_clt_tx_write_cmd(uint16_t conn_handle,
-                         struct ble_att_write_req *req,
-                         void *value, uint16_t value_len)
+                         const struct ble_att_write_req *req,
+                         struct os_mbuf *txom)
 {
 #if !NIMBLE_OPT(ATT_CLT_WRITE_NO_RSP)
     return BLE_HS_ENOTSUP;
@@ -1150,11 +917,14 @@ ble_att_clt_tx_write_cmd(uint16_t conn_handle,
 
     int rc;
 
+    rc = ble_att_clt_tx_write_req_or_cmd(conn_handle, req, txom, 0);
+    if (rc != 0) {
+        return rc;
+    }
+
     BLE_ATT_LOG_CMD(1, "write cmd", conn_handle, ble_att_write_cmd_log, req);
 
-    rc = ble_att_clt_tx_write_req_or_cmd(conn_handle, req, value, value_len,
-                                         0);
-    return rc;
+    return 0;
 }
 
 int
@@ -1175,80 +945,57 @@ ble_att_clt_rx_write(uint16_t conn_handle, struct os_mbuf **rxom)
  * $prepare write request                                                    *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_prep_write_req(uint16_t conn_handle,
-                                 struct ble_att_prep_write_cmd *req,
-                                 void *value, uint16_t value_len,
-                                 struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_PREP_WRITE_CMD_BASE_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_prep_write_req_write(txom->om_data, txom->om_len, req);
-
-    rc = ble_att_clt_append_blob(conn_handle, txom, value, value_len);
-    if (rc != 0) {
-        goto done;
-    }
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
 ble_att_clt_tx_prep_write(uint16_t conn_handle,
-                          struct ble_att_prep_write_cmd *req,
-                          void *value, uint16_t value_len)
+                          const struct ble_att_prep_write_cmd *req,
+                          struct os_mbuf *txom)
 {
 #if !NIMBLE_OPT(ATT_CLT_PREP_WRITE)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "prep write req", conn_handle,
-                    ble_att_prep_write_cmd_log, req);
-
     if (req->bapc_handle == 0) {
-        return BLE_HS_EINVAL;
+        rc = BLE_HS_EINVAL;
+        goto err;
     }
 
-    if (req->bapc_offset + value_len > BLE_ATT_ATTR_MAX_LEN) {
-        return BLE_HS_EINVAL;
+    if (req->bapc_offset + OS_MBUF_PKTLEN(txom) > BLE_ATT_ATTR_MAX_LEN) {
+        rc = BLE_HS_EINVAL;
+        goto err;
     }
 
-    if (value_len >
+    if (OS_MBUF_PKTLEN(txom) >
         ble_att_mtu(conn_handle) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ) {
 
-        return BLE_HS_EINVAL;
+        rc = BLE_HS_EINVAL;
+        goto err;
     }
 
-    rc = ble_att_clt_build_prep_write_req(conn_handle, req, value, value_len,
-                                          &txom);
-    if (rc != 0) {
-        return rc;
+    txom = os_mbuf_prepend_pullup(txom, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+    if (txom == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
     }
 
+    ble_att_prep_write_req_write(txom->om_data, BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
+                                 req);
+
     rc = ble_att_clt_tx_req(conn_handle, txom);
+    txom = NULL;
     if (rc != 0) {
-        return rc;
+        goto err;
     }
 
+    BLE_ATT_LOG_CMD(1, "prep write req", conn_handle,
+                    ble_att_prep_write_cmd_log, req);
+
     return 0;
+
+err:
+    os_mbuf_free_chain(txom);
+    return rc;
 }
 
 int
@@ -1259,16 +1006,12 @@ ble_att_clt_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom)
 #endif
 
     struct ble_att_prep_write_cmd rsp;
-    uint16_t value_len;
-    void *value;
     int rc;
 
     /* Initialize some values in case of early error. */
     memset(&rsp, 0, sizeof rsp);
-    value = NULL;
-    value_len = 0;
 
-    rc = ble_hs_misc_pullup_base(rxom, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+    rc = ble_hs_mbuf_pullup_base(rxom, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
     if (rc != 0) {
         goto done;
     }
@@ -1280,12 +1023,9 @@ ble_att_clt_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom)
     /* Strip the base from the front of the response. */
     os_mbuf_adj(*rxom, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
 
-    /* Copy the attribute data into the global ATT flat buffer. */
-    rc = ble_att_clt_copy_attr_to_flatbuf(*rxom, &value, &value_len);
-
 done:
     /* Notify GATT client that the full response has been parsed. */
-    ble_gattc_rx_prep_write_rsp(conn_handle, rc, &rsp, value, value_len);
+    ble_gattc_rx_prep_write_rsp(conn_handle, rc, &rsp, rxom);
     return rc;
 }
 
@@ -1293,35 +1033,9 @@ done:
  * $execute write request                                                    *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_exec_write_req(struct ble_att_exec_write_req *req,
-                                 struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_EXEC_WRITE_REQ_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_exec_write_req_write(txom->om_data, txom->om_len, req);
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
 ble_att_clt_tx_exec_write(uint16_t conn_handle,
-                          struct ble_att_exec_write_req *req)
+                          const struct ble_att_exec_write_req *req)
 {
 #if !NIMBLE_OPT(ATT_CLT_EXEC_WRITE)
     return BLE_HS_ENOTSUP;
@@ -1330,23 +1044,24 @@ ble_att_clt_tx_exec_write(uint16_t conn_handle,
     struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "exec write req", conn_handle,
-                    ble_att_exec_write_req_log, req);
-
     if ((req->baeq_flags & BLE_ATT_EXEC_WRITE_F_RESERVED) != 0) {
         return BLE_HS_EINVAL;
     }
 
-    rc = ble_att_clt_build_exec_write_req(req, &txom);
+    rc = ble_att_clt_init_req(BLE_ATT_EXEC_WRITE_REQ_SZ, &txom);
     if (rc != 0) {
         return rc;
     }
+    ble_att_exec_write_req_write(txom->om_data, txom->om_len, req);
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
     if (rc != 0) {
         return rc;
     }
 
+    BLE_ATT_LOG_CMD(1, "exec write req", conn_handle,
+                    ble_att_exec_write_req_log, req);
+
     return 0;
 }
 
@@ -1361,7 +1076,7 @@ ble_att_clt_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom)
 
     BLE_ATT_LOG_EMPTY_CMD(0, "exec write rsp", conn_handle);
 
-    rc = ble_hs_misc_pullup_base(rxom, BLE_ATT_EXEC_WRITE_RSP_SZ);
+    rc = ble_hs_mbuf_pullup_base(rxom, BLE_ATT_EXEC_WRITE_RSP_SZ);
     if (rc == 0) {
         ble_att_exec_write_rsp_parse((*rxom)->om_data, (*rxom)->om_len);
     }
@@ -1374,138 +1089,87 @@ ble_att_clt_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom)
  * $handle value notification                                                *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_notify_req(uint16_t conn_handle,
-                             struct ble_att_notify_req *req,
-                             void *value, uint16_t value_len,
-                             struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_NOTIFY_REQ_BASE_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_notify_req_write(txom->om_data, txom->om_len, req);
-
-    rc = ble_att_clt_append_blob(conn_handle, txom, value, value_len);
-    if (rc != 0) {
-        goto done;
-    }
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
-ble_att_clt_tx_notify(uint16_t conn_handle, struct ble_att_notify_req *req,
-                      void *value, uint16_t value_len)
+ble_att_clt_tx_notify(uint16_t conn_handle,
+                      const struct ble_att_notify_req *req,
+                      struct os_mbuf *txom)
 {
 #if !NIMBLE_OPT(ATT_CLT_NOTIFY)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "notify req", conn_handle, ble_att_notify_req_log, req);
-
     if (req->banq_handle == 0) {
-        return BLE_HS_EINVAL;
+        rc = BLE_HS_EINVAL;
+        goto err;
     }
 
-    rc = ble_att_clt_build_notify_req(conn_handle, req, value, value_len,
-                                      &txom);
-    if (rc != 0) {
-        return rc;
+    txom = os_mbuf_prepend_pullup(txom, BLE_ATT_NOTIFY_REQ_BASE_SZ);
+    if (txom == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
     }
+    ble_att_notify_req_write(txom->om_data, BLE_ATT_NOTIFY_REQ_BASE_SZ, req);
 
     rc = ble_att_clt_tx_req(conn_handle, txom);
+    txom = NULL;
     if (rc != 0) {
-        return rc;
+        goto err;
     }
 
+    BLE_ATT_LOG_CMD(1, "notify req", conn_handle, ble_att_notify_req_log, req);
+
     return 0;
+
+err:
+    os_mbuf_free_chain(txom);
+    return rc;
 }
 
 /*****************************************************************************
  * $handle value indication                                                  *
  *****************************************************************************/
 
-static int
-ble_att_clt_build_indicate_req(uint16_t conn_handle,
-                               struct ble_att_indicate_req *req,
-                               void *value, uint16_t value_len,
-                               struct os_mbuf **out_txom)
-{
-    struct os_mbuf *txom;
-    int rc;
-
-    txom = NULL;
-
-    rc = ble_att_clt_init_req(BLE_ATT_INDICATE_REQ_BASE_SZ, &txom);
-    if (rc != 0) {
-        goto done;
-    }
-
-    ble_att_indicate_req_write(txom->om_data, txom->om_len, req);
-
-    rc = ble_att_clt_append_blob(conn_handle, txom, value, value_len);
-    if (rc != 0) {
-        goto done;
-    }
-
-done:
-    if (rc != 0) {
-        os_mbuf_free_chain(txom);
-        txom = NULL;
-    }
-
-    *out_txom = txom;
-    return rc;
-}
-
 int
 ble_att_clt_tx_indicate(uint16_t conn_handle,
-                        struct ble_att_indicate_req *req,
-                        void *value, uint16_t value_len)
+                        const struct ble_att_indicate_req *req,
+                        struct os_mbuf *txom)
 {
 #if !NIMBLE_OPT(ATT_CLT_INDICATE)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct os_mbuf *txom;
     int rc;
 
-    BLE_ATT_LOG_CMD(1, "indicate req", conn_handle, ble_att_indicate_req_log,
-                    req);
-
     if (req->baiq_handle == 0) {
-        return BLE_HS_EINVAL;
+        rc = BLE_HS_EINVAL;
+        goto err;
     }
 
-    rc = ble_att_clt_build_indicate_req(conn_handle, req, value, value_len,
-                                        &txom);
-    if (rc != 0) {
-        return rc;
+    txom = os_mbuf_prepend_pullup(txom, BLE_ATT_INDICATE_REQ_BASE_SZ);
+    if (txom == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
     }
 
+    ble_att_indicate_req_write(txom->om_data, BLE_ATT_INDICATE_REQ_BASE_SZ,
+                               req);
+
     rc = ble_att_clt_tx_req(conn_handle, txom);
+    txom = NULL;
     if (rc != 0) {
-        return rc;
+        goto err;
     }
 
+    BLE_ATT_LOG_CMD(1, "indicate req", conn_handle, ble_att_indicate_req_log,
+                    req);
+
     return 0;
+
+err:
+    os_mbuf_free_chain(txom);
+    return rc;
 }
 
 int
diff --git a/net/nimble/host/src/ble_att_cmd.c b/net/nimble/host/src/ble_att_cmd.c
index 5df08466..05c68749 100644
--- a/net/nimble/host/src/ble_att_cmd.c
+++ b/net/nimble/host/src/ble_att_cmd.c
@@ -26,10 +26,11 @@
 #include "host/ble_uuid.h"
 #include "ble_hs_priv.h"
 
-static void *
-ble_att_init_parse(uint8_t op, void *payload, int min_len, int actual_len)
+static const void *
+ble_att_init_parse(uint8_t op, const void *payload,
+                   int min_len, int actual_len)
 {
-    uint8_t *u8ptr;
+    const uint8_t *u8ptr;
 
     BLE_HS_DBG_ASSERT(actual_len >= min_len);
 
@@ -40,20 +41,6 @@ ble_att_init_parse(uint8_t op, void *payload, int min_len, int actual_len)
 }
 
 static void *
-ble_att_init_parse_2op(uint8_t op1, uint8_t op2, void *payload,
-                       int min_len, int actual_len)
-{
-    uint8_t *u8ptr;
-
-    BLE_HS_DBG_ASSERT(actual_len >= min_len);
-
-    u8ptr = payload;
-    BLE_HS_DBG_ASSERT(u8ptr[0] == op1 || u8ptr[0] == op2);
-
-    return u8ptr + 1;
-}
-
-static void *
 ble_att_init_write(uint8_t op, void *payload, int min_len, int actual_len)
 {
     uint8_t *u8ptr;
@@ -68,7 +55,7 @@ ble_att_init_write(uint8_t op, void *payload, int min_len, int actual_len)
 
 static void
 ble_att_error_rsp_swap(struct ble_att_error_rsp *dst,
-                       struct ble_att_error_rsp *src)
+                       const struct ble_att_error_rsp *src)
 {
     dst->baep_req_op = src->baep_req_op;
     dst->baep_handle = TOFROMLE16(src->baep_handle);
@@ -76,9 +63,10 @@ ble_att_error_rsp_swap(struct ble_att_error_rsp *dst,
 }
 
 void
-ble_att_error_rsp_parse(void *payload, int len, struct ble_att_error_rsp *dst)
+ble_att_error_rsp_parse(const void *payload, int len,
+                        struct ble_att_error_rsp *dst)
 {
-    struct ble_att_error_rsp *src;
+    const struct ble_att_error_rsp *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_ERROR_RSP, payload,
                              BLE_ATT_ERROR_RSP_SZ, len);
@@ -86,7 +74,8 @@ ble_att_error_rsp_parse(void *payload, int len, struct ble_att_error_rsp *dst)
 }
 
 void
-ble_att_error_rsp_write(void *payload, int len, struct ble_att_error_rsp *src)
+ble_att_error_rsp_write(void *payload, int len,
+                        const struct ble_att_error_rsp *src)
 {
     struct ble_att_error_rsp *dst;
 
@@ -96,30 +85,44 @@ ble_att_error_rsp_write(void *payload, int len, struct ble_att_error_rsp *src)
 }
 
 void
-ble_att_error_rsp_log(struct ble_att_error_rsp *cmd)
+ble_att_error_rsp_log(const struct ble_att_error_rsp *cmd)
 {
     BLE_HS_LOG(DEBUG, "req_op=%d handle=0x%04x error_code=%d",
                cmd->baep_req_op, cmd->baep_handle, cmd->baep_error_code);
 }
 
 static void
-ble_att_mtu_cmd_swap(struct ble_att_mtu_cmd *dst, struct ble_att_mtu_cmd *src)
+ble_att_mtu_cmd_swap(struct ble_att_mtu_cmd *dst,
+                     const struct ble_att_mtu_cmd *src)
 {
     dst->bamc_mtu = TOFROMLE16(src->bamc_mtu);
 }
 
 void
-ble_att_mtu_cmd_parse(void *payload, int len, struct ble_att_mtu_cmd *dst)
+ble_att_mtu_req_parse(const void *payload, int len,
+                      struct ble_att_mtu_cmd *dst)
+{
+    const struct ble_att_mtu_cmd *src;
+
+    src = ble_att_init_parse(BLE_ATT_OP_MTU_REQ, payload, BLE_ATT_MTU_CMD_SZ,
+                             len);
+    ble_att_mtu_cmd_swap(dst, src);
+}
+
+void
+ble_att_mtu_rsp_parse(const void *payload, int len,
+                      struct ble_att_mtu_cmd *dst)
 {
-    struct ble_att_mtu_cmd *src;
+    const struct ble_att_mtu_cmd *src;
 
-    src = ble_att_init_parse_2op(BLE_ATT_OP_MTU_REQ, BLE_ATT_OP_MTU_RSP,
-                                 payload, BLE_ATT_MTU_CMD_SZ, len);
+    src = ble_att_init_parse(BLE_ATT_OP_MTU_RSP, payload, BLE_ATT_MTU_CMD_SZ,
+                             len);
     ble_att_mtu_cmd_swap(dst, src);
 }
 
 void
-ble_att_mtu_req_write(void *payload, int len, struct ble_att_mtu_cmd *src)
+ble_att_mtu_req_write(void *payload, int len,
+                      const struct ble_att_mtu_cmd *src)
 {
     struct ble_att_mtu_cmd *dst;
 
@@ -129,7 +132,8 @@ ble_att_mtu_req_write(void *payload, int len, struct ble_att_mtu_cmd *src)
 }
 
 void
-ble_att_mtu_rsp_write(void *payload, int len, struct ble_att_mtu_cmd *src)
+ble_att_mtu_rsp_write(void *payload, int len,
+                      const struct ble_att_mtu_cmd *src)
 {
     struct ble_att_mtu_cmd *dst;
 
@@ -139,24 +143,24 @@ ble_att_mtu_rsp_write(void *payload, int len, struct ble_att_mtu_cmd *src)
 }
 
 void
-ble_att_mtu_cmd_log(struct ble_att_mtu_cmd *cmd)
+ble_att_mtu_cmd_log(const struct ble_att_mtu_cmd *cmd)
 {
     BLE_HS_LOG(DEBUG, "mtu=%d", cmd->bamc_mtu);
 }
 
 static void
 ble_att_find_info_req_swap(struct ble_att_find_info_req *dst,
-                           struct ble_att_find_info_req *src)
+                           const struct ble_att_find_info_req *src)
 {
     dst->bafq_start_handle = TOFROMLE16(src->bafq_start_handle);
     dst->bafq_end_handle = TOFROMLE16(src->bafq_end_handle);
 }
 
 void
-ble_att_find_info_req_parse(void *payload, int len,
+ble_att_find_info_req_parse(const void *payload, int len,
                             struct ble_att_find_info_req *dst)
 {
-    struct ble_att_find_info_req *src;
+    const struct ble_att_find_info_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_REQ, payload,
                              BLE_ATT_FIND_INFO_REQ_SZ, len);
@@ -165,7 +169,7 @@ ble_att_find_info_req_parse(void *payload, int len,
 
 void
 ble_att_find_info_req_write(void *payload, int len,
-                            struct ble_att_find_info_req *src)
+                            const struct ble_att_find_info_req *src)
 {
     struct ble_att_find_info_req *dst;
 
@@ -175,7 +179,7 @@ ble_att_find_info_req_write(void *payload, int len,
 }
 
 void
-ble_att_find_info_req_log(struct ble_att_find_info_req *cmd)
+ble_att_find_info_req_log(const struct ble_att_find_info_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "start_handle=0x%04x end_handle=0x%04x",
                cmd->bafq_start_handle, cmd->bafq_end_handle);
@@ -183,16 +187,16 @@ ble_att_find_info_req_log(struct ble_att_find_info_req *cmd)
 
 static void
 ble_att_find_info_rsp_swap(struct ble_att_find_info_rsp *dst,
-                           struct ble_att_find_info_rsp *src)
+                           const struct ble_att_find_info_rsp *src)
 {
     dst->bafp_format = src->bafp_format;
 }
 
 void
-ble_att_find_info_rsp_parse(void *payload, int len,
+ble_att_find_info_rsp_parse(const void *payload, int len,
                             struct ble_att_find_info_rsp *dst)
 {
-    struct ble_att_find_info_rsp *src;
+    const struct ble_att_find_info_rsp *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_FIND_INFO_RSP, payload,
                              BLE_ATT_FIND_INFO_RSP_BASE_SZ, len);
@@ -201,7 +205,7 @@ ble_att_find_info_rsp_parse(void *payload, int len,
 
 void
 ble_att_find_info_rsp_write(void *payload, int len,
-                            struct ble_att_find_info_rsp *src)
+                            const struct ble_att_find_info_rsp *src)
 {
     struct ble_att_find_info_rsp *dst;
 
@@ -211,14 +215,14 @@ ble_att_find_info_rsp_write(void *payload, int len,
 }
 
 void
-ble_att_find_info_rsp_log(struct ble_att_find_info_rsp *cmd)
+ble_att_find_info_rsp_log(const struct ble_att_find_info_rsp *cmd)
 {
     BLE_HS_LOG(DEBUG, "format=%d", cmd->bafp_format);
 }
 
 static void
 ble_att_find_type_value_req_swap(struct ble_att_find_type_value_req *dst,
-                                 struct ble_att_find_type_value_req *src)
+                                 const struct ble_att_find_type_value_req *src)
 {
     dst->bavq_start_handle = TOFROMLE16(src->bavq_start_handle);
     dst->bavq_end_handle = TOFROMLE16(src->bavq_end_handle);
@@ -226,10 +230,10 @@ ble_att_find_type_value_req_swap(struct ble_att_find_type_value_req *dst,
 }
 
 void
-ble_att_find_type_value_req_parse(void *payload, int len,
+ble_att_find_type_value_req_parse(const void *payload, int len,
                                   struct ble_att_find_type_value_req *dst)
 {
-    struct ble_att_find_type_value_req *src;
+    const struct ble_att_find_type_value_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_FIND_TYPE_VALUE_REQ, payload,
                              BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ, len);
@@ -237,8 +241,8 @@ ble_att_find_type_value_req_parse(void *payload, int len,
 }
 
 void
-ble_att_find_type_value_req_write(void *payload, int len,
-                                  struct ble_att_find_type_value_req *src)
+ble_att_find_type_value_req_write(
+    void *payload, int len, const struct ble_att_find_type_value_req *src)
 {
     struct ble_att_find_type_value_req *dst;
 
@@ -248,7 +252,7 @@ ble_att_find_type_value_req_write(void *payload, int len,
 }
 
 void
-ble_att_find_type_value_req_log(struct ble_att_find_type_value_req *cmd)
+ble_att_find_type_value_req_log(const struct ble_att_find_type_value_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "start_handle=0x%04x end_handle=0x%04x attr_type=%d",
                cmd->bavq_start_handle, cmd->bavq_end_handle,
@@ -257,17 +261,17 @@ ble_att_find_type_value_req_log(struct ble_att_find_type_value_req *cmd)
 
 static void
 ble_att_read_type_req_swap(struct ble_att_read_type_req *dst,
-                           struct ble_att_read_type_req *src)
+                           const struct ble_att_read_type_req *src)
 {
     dst->batq_start_handle = TOFROMLE16(src->batq_start_handle);
     dst->batq_end_handle = TOFROMLE16(src->batq_end_handle);
 }
 
 void
-ble_att_read_type_req_parse(void *payload, int len,
+ble_att_read_type_req_parse(const void *payload, int len,
                             struct ble_att_read_type_req *dst)
 {
-    struct ble_att_read_type_req *src;
+    const struct ble_att_read_type_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_REQ, payload,
                              BLE_ATT_READ_TYPE_REQ_BASE_SZ, len);
@@ -276,7 +280,7 @@ ble_att_read_type_req_parse(void *payload, int len,
 
 void
 ble_att_read_type_req_write(void *payload, int len,
-                            struct ble_att_read_type_req *src)
+                            const struct ble_att_read_type_req *src)
 {
     struct ble_att_read_type_req *dst;
 
@@ -286,7 +290,7 @@ ble_att_read_type_req_write(void *payload, int len,
 }
 
 void
-ble_att_read_type_req_log(struct ble_att_read_type_req *cmd)
+ble_att_read_type_req_log(const struct ble_att_read_type_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "start_handle=0x%04x end_handle=0x%04x",
                cmd->batq_start_handle, cmd->batq_end_handle);
@@ -294,16 +298,16 @@ ble_att_read_type_req_log(struct ble_att_read_type_req *cmd)
 
 static void
 ble_att_read_type_rsp_swap(struct ble_att_read_type_rsp *dst,
-                           struct ble_att_read_type_rsp *src)
+                           const struct ble_att_read_type_rsp *src)
 {
     dst->batp_length = src->batp_length;
 }
 
 void
-ble_att_read_type_rsp_parse(void *payload, int len,
+ble_att_read_type_rsp_parse(const void *payload, int len,
                             struct ble_att_read_type_rsp *dst)
 {
-    struct ble_att_read_type_rsp *src;
+    const struct ble_att_read_type_rsp *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_TYPE_RSP, payload,
                              BLE_ATT_READ_TYPE_RSP_BASE_SZ, len);
@@ -312,7 +316,7 @@ ble_att_read_type_rsp_parse(void *payload, int len,
 
 void
 ble_att_read_type_rsp_write(void *payload, int len,
-                            struct ble_att_read_type_rsp *src)
+                            const struct ble_att_read_type_rsp *src)
 {
     struct ble_att_read_type_rsp *dst;
 
@@ -322,22 +326,23 @@ ble_att_read_type_rsp_write(void *payload, int len,
 }
 
 void
-ble_att_read_type_rsp_log(struct ble_att_read_type_rsp *cmd)
+ble_att_read_type_rsp_log(const struct ble_att_read_type_rsp *cmd)
 {
     BLE_HS_LOG(DEBUG, "length=%d", cmd->batp_length);
 }
 
 static void
 ble_att_read_req_swap(struct ble_att_read_req *dst,
-                      struct ble_att_read_req *src)
+                      const struct ble_att_read_req *src)
 {
     dst->barq_handle = TOFROMLE16(src->barq_handle);
 }
 
 void
-ble_att_read_req_parse(void *payload, int len, struct ble_att_read_req *dst)
+ble_att_read_req_parse(const void *payload, int len,
+                       struct ble_att_read_req *dst)
 {
-    struct ble_att_read_req *src;
+    const struct ble_att_read_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_REQ, payload,
                              BLE_ATT_READ_REQ_SZ, len);
@@ -345,7 +350,8 @@ ble_att_read_req_parse(void *payload, int len, struct ble_att_read_req *dst)
 }
 
 void
-ble_att_read_req_write(void *payload, int len, struct ble_att_read_req *src)
+ble_att_read_req_write(void *payload, int len,
+                       const struct ble_att_read_req *src)
 {
     struct ble_att_read_req *dst;
 
@@ -355,24 +361,24 @@ ble_att_read_req_write(void *payload, int len, struct ble_att_read_req *src)
 }
 
 void
-ble_att_read_req_log(struct ble_att_read_req *cmd)
+ble_att_read_req_log(const struct ble_att_read_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "handle=0x%04x", cmd->barq_handle);
 }
 
 static void
 ble_att_read_blob_req_swap(struct ble_att_read_blob_req *dst,
-                           struct ble_att_read_blob_req *src)
+                           const struct ble_att_read_blob_req *src)
 {
     dst->babq_handle = TOFROMLE16(src->babq_handle);
     dst->babq_offset = TOFROMLE16(src->babq_offset);
 }
 
 void
-ble_att_read_blob_req_parse(void *payload, int len,
+ble_att_read_blob_req_parse(const void *payload, int len,
                             struct ble_att_read_blob_req *dst)
 {
-    struct ble_att_read_blob_req *src;
+    const struct ble_att_read_blob_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_BLOB_REQ, payload,
                              BLE_ATT_READ_BLOB_REQ_SZ, len);
@@ -381,7 +387,7 @@ ble_att_read_blob_req_parse(void *payload, int len,
 
 void
 ble_att_read_blob_req_write(void *payload, int len,
-                            struct ble_att_read_blob_req *src)
+                            const struct ble_att_read_blob_req *src)
 {
     struct ble_att_read_blob_req *dst;
 
@@ -391,14 +397,14 @@ ble_att_read_blob_req_write(void *payload, int len,
 }
 
 void
-ble_att_read_blob_req_log(struct ble_att_read_blob_req *cmd)
+ble_att_read_blob_req_log(const struct ble_att_read_blob_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "handle=0x%04x offset=%d", cmd->babq_handle,
                cmd->babq_offset);
 }
 
 void
-ble_att_read_mult_req_parse(void *payload, int len)
+ble_att_read_mult_req_parse(const void *payload, int len)
 {
     ble_att_init_parse(BLE_ATT_OP_READ_MULT_REQ, payload,
                        BLE_ATT_READ_MULT_REQ_BASE_SZ, len);
@@ -412,7 +418,7 @@ ble_att_read_mult_req_write(void *payload, int len)
 }
 
 void
-ble_att_read_mult_rsp_parse(void *payload, int len)
+ble_att_read_mult_rsp_parse(const void *payload, int len)
 {
     ble_att_init_parse(BLE_ATT_OP_READ_MULT_RSP, payload,
                        BLE_ATT_READ_MULT_RSP_BASE_SZ, len);
@@ -427,17 +433,17 @@ ble_att_read_mult_rsp_write(void *payload, int len)
 
 static void
 ble_att_read_group_type_req_swap(struct ble_att_read_group_type_req *dst,
-                                 struct ble_att_read_group_type_req *src)
+                                 const struct ble_att_read_group_type_req *src)
 {
     dst->bagq_start_handle = TOFROMLE16(src->bagq_start_handle);
     dst->bagq_end_handle = TOFROMLE16(src->bagq_end_handle);
 }
 
 void
-ble_att_read_group_type_req_parse(void *payload, int len,
+ble_att_read_group_type_req_parse(const void *payload, int len,
                                   struct ble_att_read_group_type_req *dst)
 {
-    struct ble_att_read_group_type_req *src;
+    const struct ble_att_read_group_type_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_REQ, payload,
                              BLE_ATT_READ_GROUP_TYPE_REQ_BASE_SZ, len);
@@ -445,8 +451,8 @@ ble_att_read_group_type_req_parse(void *payload, int len,
 }
 
 void
-ble_att_read_group_type_req_write(void *payload, int len,
-                                  struct ble_att_read_group_type_req *src)
+ble_att_read_group_type_req_write(
+    void *payload, int len, const struct ble_att_read_group_type_req *src)
 {
     struct ble_att_read_group_type_req *dst;
 
@@ -456,7 +462,7 @@ ble_att_read_group_type_req_write(void *payload, int len,
 }
 
 void
-ble_att_read_group_type_req_log(struct ble_att_read_group_type_req *cmd)
+ble_att_read_group_type_req_log(const struct ble_att_read_group_type_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "start_handle=0x%04x end_handle=0x%04x",
                cmd->bagq_start_handle, cmd->bagq_end_handle);
@@ -464,16 +470,16 @@ ble_att_read_group_type_req_log(struct ble_att_read_group_type_req *cmd)
 
 static void
 ble_att_read_group_type_rsp_swap(struct ble_att_read_group_type_rsp *dst,
-                                 struct ble_att_read_group_type_rsp *src)
+                                 const struct ble_att_read_group_type_rsp *src)
 {
     dst->bagp_length = src->bagp_length;
 }
 
 void
-ble_att_read_group_type_rsp_parse(void *payload, int len,
+ble_att_read_group_type_rsp_parse(const void *payload, int len,
                                   struct ble_att_read_group_type_rsp *dst)
 {
-    struct ble_att_read_group_type_rsp *src;
+    const struct ble_att_read_group_type_rsp *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_READ_GROUP_TYPE_RSP, payload,
                              BLE_ATT_READ_GROUP_TYPE_RSP_BASE_SZ, len);
@@ -481,8 +487,8 @@ ble_att_read_group_type_rsp_parse(void *payload, int len,
 }
 
 void
-ble_att_read_group_type_rsp_write(void *payload, int len,
-                                  struct ble_att_read_group_type_rsp *src)
+ble_att_read_group_type_rsp_write(
+    void *payload, int len, const struct ble_att_read_group_type_rsp *src)
 {
     struct ble_att_read_group_type_rsp *dst;
 
@@ -492,22 +498,23 @@ ble_att_read_group_type_rsp_write(void *payload, int len,
 }
 
 void
-ble_att_read_group_type_rsp_log(struct ble_att_read_group_type_rsp *cmd)
+ble_att_read_group_type_rsp_log(const struct ble_att_read_group_type_rsp *cmd)
 {
     BLE_HS_LOG(DEBUG, "length=%d", cmd->bagp_length);
 }
 
 static void
 ble_att_write_req_swap(struct ble_att_write_req *dst,
-                       struct ble_att_write_req *src)
+                       const struct ble_att_write_req *src)
 {
     dst->bawq_handle = TOFROMLE16(src->bawq_handle);
 }
 
 void
-ble_att_write_req_parse(void *payload, int len, struct ble_att_write_req *dst)
+ble_att_write_req_parse(const void *payload, int len,
+                        struct ble_att_write_req *dst)
 {
-    struct ble_att_write_req *src;
+    const struct ble_att_write_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_WRITE_REQ, payload,
                              BLE_ATT_WRITE_REQ_BASE_SZ, len);
@@ -515,9 +522,10 @@ ble_att_write_req_parse(void *payload, int len, struct ble_att_write_req *dst)
 }
 
 void
-ble_att_write_cmd_parse(void *payload, int len, struct ble_att_write_req *dst)
+ble_att_write_cmd_parse(const void *payload, int len,
+                        struct ble_att_write_req *dst)
 {
-    struct ble_att_write_req *src;
+    const struct ble_att_write_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_WRITE_CMD, payload,
                              BLE_ATT_WRITE_REQ_BASE_SZ, len);
@@ -525,7 +533,8 @@ ble_att_write_cmd_parse(void *payload, int len, struct ble_att_write_req *dst)
 }
 
 void
-ble_att_write_req_write(void *payload, int len, struct ble_att_write_req *src)
+ble_att_write_req_write(void *payload, int len,
+                        const struct ble_att_write_req *src)
 {
     struct ble_att_write_req *dst;
 
@@ -535,7 +544,8 @@ ble_att_write_req_write(void *payload, int len, struct ble_att_write_req *src)
 }
 
 void
-ble_att_write_cmd_write(void *payload, int len, struct ble_att_write_req *src)
+ble_att_write_cmd_write(void *payload, int len,
+                        const struct ble_att_write_req *src)
 {
     struct ble_att_write_req *dst;
 
@@ -545,24 +555,24 @@ ble_att_write_cmd_write(void *payload, int len, struct ble_att_write_req *src)
 }
 
 void
-ble_att_write_cmd_log(struct ble_att_write_req *cmd)
+ble_att_write_cmd_log(const struct ble_att_write_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "handle=0x%04x", cmd->bawq_handle);
 }
 
 static void
 ble_att_prep_write_cmd_swap(struct ble_att_prep_write_cmd *dst,
-                            struct ble_att_prep_write_cmd *src)
+                            const struct ble_att_prep_write_cmd *src)
 {
     dst->bapc_handle = TOFROMLE16(src->bapc_handle);
     dst->bapc_offset = TOFROMLE16(src->bapc_offset);
 }
 
 void
-ble_att_prep_write_req_parse(void *payload, int len,
+ble_att_prep_write_req_parse(const void *payload, int len,
                              struct ble_att_prep_write_cmd *dst)
 {
-    struct ble_att_prep_write_cmd *src;
+    const struct ble_att_prep_write_cmd *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_REQ, payload,
                              BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len);
@@ -571,7 +581,7 @@ ble_att_prep_write_req_parse(void *payload, int len,
 
 void
 ble_att_prep_write_req_write(void *payload, int len,
-                             struct ble_att_prep_write_cmd *src)
+                             const struct ble_att_prep_write_cmd *src)
 {
     struct ble_att_prep_write_cmd *dst;
 
@@ -581,10 +591,10 @@ ble_att_prep_write_req_write(void *payload, int len,
 }
 
 void
-ble_att_prep_write_rsp_parse(void *payload, int len,
+ble_att_prep_write_rsp_parse(const void *payload, int len,
                              struct ble_att_prep_write_cmd *dst)
 {
-    struct ble_att_prep_write_cmd *src;
+    const struct ble_att_prep_write_cmd *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_PREP_WRITE_RSP, payload,
                              BLE_ATT_PREP_WRITE_CMD_BASE_SZ, len);
@@ -593,7 +603,7 @@ ble_att_prep_write_rsp_parse(void *payload, int len,
 
 void
 ble_att_prep_write_rsp_write(void *payload, int len,
-                             struct ble_att_prep_write_cmd *src)
+                             const struct ble_att_prep_write_cmd *src)
 {
     struct ble_att_prep_write_cmd *dst;
 
@@ -603,7 +613,7 @@ ble_att_prep_write_rsp_write(void *payload, int len,
 }
 
 void
-ble_att_prep_write_cmd_log(struct ble_att_prep_write_cmd *cmd)
+ble_att_prep_write_cmd_log(const struct ble_att_prep_write_cmd *cmd)
 {
     BLE_HS_LOG(DEBUG, "handle=0x%04x offset=%d", cmd->bapc_handle,
                cmd->bapc_offset);
@@ -611,16 +621,16 @@ ble_att_prep_write_cmd_log(struct ble_att_prep_write_cmd *cmd)
 
 static void
 ble_att_exec_write_req_swap(struct ble_att_exec_write_req *dst,
-                            struct ble_att_exec_write_req *src)
+                            const struct ble_att_exec_write_req *src)
 {
     dst->baeq_flags = src->baeq_flags;
 }
 
 void
-ble_att_exec_write_req_parse(void *payload, int len,
+ble_att_exec_write_req_parse(const void *payload, int len,
                              struct ble_att_exec_write_req *dst)
 {
-    struct ble_att_exec_write_req *src;
+    const struct ble_att_exec_write_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_EXEC_WRITE_REQ, payload,
                              BLE_ATT_EXEC_WRITE_REQ_SZ, len);
@@ -629,7 +639,7 @@ ble_att_exec_write_req_parse(void *payload, int len,
 
 void
 ble_att_exec_write_req_write(void *payload, int len,
-                             struct ble_att_exec_write_req *src)
+                             const struct ble_att_exec_write_req *src)
 {
     struct ble_att_exec_write_req *dst;
 
@@ -639,13 +649,13 @@ ble_att_exec_write_req_write(void *payload, int len,
 }
 
 void
-ble_att_exec_write_req_log(struct ble_att_exec_write_req *cmd)
+ble_att_exec_write_req_log(const struct ble_att_exec_write_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "flags=0x%02x", cmd->baeq_flags);
 }
 
 void
-ble_att_exec_write_rsp_parse(void *payload, int len)
+ble_att_exec_write_rsp_parse(const void *payload, int len)
 {
     ble_att_init_parse(BLE_ATT_OP_EXEC_WRITE_RSP, payload,
                        BLE_ATT_EXEC_WRITE_RSP_SZ, len);
@@ -660,16 +670,16 @@ ble_att_exec_write_rsp_write(void *payload, int len)
 
 static void
 ble_att_notify_req_swap(struct ble_att_notify_req *dst,
-                        struct ble_att_notify_req *src)
+                        const struct ble_att_notify_req *src)
 {
     dst->banq_handle = TOFROMLE16(src->banq_handle);
 }
 
 void
-ble_att_notify_req_parse(void *payload, int len,
+ble_att_notify_req_parse(const void *payload, int len,
                          struct ble_att_notify_req *dst)
 {
-    struct ble_att_notify_req *src;
+    const struct ble_att_notify_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_NOTIFY_REQ, payload,
                              BLE_ATT_NOTIFY_REQ_BASE_SZ, len);
@@ -678,7 +688,7 @@ ble_att_notify_req_parse(void *payload, int len,
 
 void
 ble_att_notify_req_write(void *payload, int len,
-                         struct ble_att_notify_req *src)
+                         const struct ble_att_notify_req *src)
 {
     struct ble_att_notify_req *dst;
 
@@ -688,23 +698,23 @@ ble_att_notify_req_write(void *payload, int len,
 }
 
 void
-ble_att_notify_req_log(struct ble_att_notify_req *cmd)
+ble_att_notify_req_log(const struct ble_att_notify_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "handle=0x%04x", cmd->banq_handle);
 }
 
 static void
 ble_att_indicate_req_swap(struct ble_att_indicate_req *dst,
-                          struct ble_att_indicate_req *src)
+                          const struct ble_att_indicate_req *src)
 {
     dst->baiq_handle = TOFROMLE16(src->baiq_handle);
 }
 
 void
-ble_att_indicate_req_parse(void *payload, int len,
+ble_att_indicate_req_parse(const void *payload, int len,
                            struct ble_att_indicate_req *dst)
 {
-    struct ble_att_indicate_req *src;
+    const struct ble_att_indicate_req *src;
 
     src = ble_att_init_parse(BLE_ATT_OP_INDICATE_REQ, payload,
                              BLE_ATT_INDICATE_REQ_BASE_SZ, len);
@@ -713,7 +723,7 @@ ble_att_indicate_req_parse(void *payload, int len,
 
 void
 ble_att_indicate_req_write(void *payload, int len,
-                           struct ble_att_indicate_req *src)
+                           const struct ble_att_indicate_req *src)
 {
     struct ble_att_indicate_req *dst;
 
@@ -723,13 +733,13 @@ ble_att_indicate_req_write(void *payload, int len,
 }
 
 void
-ble_att_indicate_req_log(struct ble_att_indicate_req *cmd)
+ble_att_indicate_req_log(const struct ble_att_indicate_req *cmd)
 {
     BLE_HS_LOG(DEBUG, "handle=0x%04x", cmd->baiq_handle);
 }
 
 void
-ble_att_indicate_rsp_parse(void *payload, int len)
+ble_att_indicate_rsp_parse(const void *payload, int len)
 {
     ble_att_init_parse(BLE_ATT_OP_INDICATE_RSP, payload,
                        BLE_ATT_INDICATE_RSP_SZ, len);
diff --git a/net/nimble/host/src/ble_att_cmd_priv.h b/net/nimble/host/src/ble_att_cmd_priv.h
index 38af5f2e..bfeb28f1 100644
--- a/net/nimble/host/src/ble_att_cmd_priv.h
+++ b/net/nimble/host/src/ble_att_cmd_priv.h
@@ -304,103 +304,108 @@ struct ble_att_indicate_req {
  */
 #define BLE_ATT_INDICATE_RSP_SZ         1
 
-void ble_att_error_rsp_parse(void *payload, int len,
-                              struct ble_att_error_rsp *rsp);
+void ble_att_error_rsp_parse(const void *payload, int len,
+                             struct ble_att_error_rsp *rsp);
 void ble_att_error_rsp_write(void *payload, int len,
-                              struct ble_att_error_rsp *rsp);
-void ble_att_error_rsp_log(struct ble_att_error_rsp *cmd);
-void ble_att_mtu_cmd_parse(void *payload, int len,
-                            struct ble_att_mtu_cmd *cmd);
+                             const struct ble_att_error_rsp *rsp);
+void ble_att_error_rsp_log(const struct ble_att_error_rsp *cmd);
+void ble_att_mtu_req_parse(const void *payload, int len,
+                           struct ble_att_mtu_cmd *cmd);
 void ble_att_mtu_req_write(void *payload, int len,
-                            struct ble_att_mtu_cmd *cmd);
-void ble_att_mtu_rsp_write(void *payload, int len,
+                           const struct ble_att_mtu_cmd *cmd);
+void ble_att_mtu_rsp_parse(const void *payload, int len,
                            struct ble_att_mtu_cmd *cmd);
-void ble_att_mtu_cmd_log(struct ble_att_mtu_cmd *cmd);
-void ble_att_find_info_req_parse(void *payload, int len,
+void ble_att_mtu_rsp_write(void *payload, int len,
+                           const struct ble_att_mtu_cmd *cmd);
+void ble_att_mtu_cmd_log(const struct ble_att_mtu_cmd *cmd);
+void ble_att_find_info_req_parse(const void *payload, int len,
                                  struct ble_att_find_info_req *req);
 void ble_att_find_info_req_write(void *payload, int len,
-                                 struct ble_att_find_info_req *req);
-void ble_att_find_info_req_log(struct ble_att_find_info_req *cmd);
-void ble_att_find_info_rsp_parse(void *payload, int len,
+                                 const struct ble_att_find_info_req *req);
+void ble_att_find_info_req_log(const struct ble_att_find_info_req *cmd);
+void ble_att_find_info_rsp_parse(const void *payload, int len,
                                  struct ble_att_find_info_rsp *rsp);
 void ble_att_find_info_rsp_write(void *payload, int len,
-                                 struct ble_att_find_info_rsp *rsp);
-void ble_att_find_info_rsp_log(struct ble_att_find_info_rsp *cmd);
-void ble_att_find_type_value_req_parse(void *payload, int len,
-                                       struct ble_att_find_type_value_req *req);
-void ble_att_find_type_value_req_write(void *payload, int len,
-                                       struct ble_att_find_type_value_req *req);
-void ble_att_find_type_value_req_log(struct ble_att_find_type_value_req *cmd);
-void ble_att_read_type_req_parse(void *payload, int len,
+                                 const struct ble_att_find_info_rsp *rsp);
+void ble_att_find_info_rsp_log(const struct ble_att_find_info_rsp *cmd);
+void ble_att_find_type_value_req_parse(
+    const void *payload, int len, struct ble_att_find_type_value_req *req);
+void ble_att_find_type_value_req_write(
+    void *payload, int len, const struct ble_att_find_type_value_req *req);
+void ble_att_find_type_value_req_log(
+    const struct ble_att_find_type_value_req *cmd);
+void ble_att_read_type_req_parse(const void *payload, int len,
                                  struct ble_att_read_type_req *req);
 void ble_att_read_type_req_write(void *payload, int len,
-                                 struct ble_att_read_type_req *req);
-void ble_att_read_type_req_log(struct ble_att_read_type_req *cmd);
-void ble_att_read_type_rsp_parse(void *payload, int len,
+                                 const struct ble_att_read_type_req *req);
+void ble_att_read_type_req_log(const struct ble_att_read_type_req *cmd);
+void ble_att_read_type_rsp_parse(const void *payload, int len,
                                  struct ble_att_read_type_rsp *rsp);
 void ble_att_read_type_rsp_write(void *payload, int len,
-                                 struct ble_att_read_type_rsp *rsp);
-void ble_att_read_type_rsp_log(struct ble_att_read_type_rsp *cmd);
-void ble_att_read_req_parse(void *payload, int len,
+                                 const struct ble_att_read_type_rsp *rsp);
+void ble_att_read_type_rsp_log(const struct ble_att_read_type_rsp *cmd);
+void ble_att_read_req_parse(const void *payload, int len,
                             struct ble_att_read_req *req);
 void ble_att_read_req_write(void *payload, int len,
-                            struct ble_att_read_req *req);
-void ble_att_read_req_log(struct ble_att_read_req *cmd);
-void ble_att_read_blob_req_parse(void *payload, int len,
+                            const struct ble_att_read_req *req);
+void ble_att_read_req_log(const struct ble_att_read_req *cmd);
+void ble_att_read_blob_req_parse(const void *payload, int len,
                                  struct ble_att_read_blob_req *req);
 void ble_att_read_blob_req_write(void *payload, int len,
-                                 struct ble_att_read_blob_req *req);
-void ble_att_read_blob_req_log(struct ble_att_read_blob_req *cmd);
-void ble_att_read_mult_req_parse(void *payload, int len);
+                                 const struct ble_att_read_blob_req *req);
+void ble_att_read_blob_req_log(const struct ble_att_read_blob_req *cmd);
+void ble_att_read_mult_req_parse(const void *payload, int len);
 void ble_att_read_mult_req_write(void *payload, int len);
-void ble_att_read_mult_rsp_parse(void *payload, int len);
+void ble_att_read_mult_rsp_parse(const void *payload, int len);
 void ble_att_read_mult_rsp_write(void *payload, int len);
 void ble_att_read_group_type_req_parse(
-    void *payload, int len, struct ble_att_read_group_type_req *req);
+    const void *payload, int len, struct ble_att_read_group_type_req *req);
 void ble_att_read_group_type_req_write(
-    void *payload, int len, struct ble_att_read_group_type_req *req);
-void ble_att_read_group_type_req_log(struct ble_att_read_group_type_req *cmd);
+    void *payload, int len, const struct ble_att_read_group_type_req *req);
+void ble_att_read_group_type_req_log(
+    const struct ble_att_read_group_type_req *cmd);
 void ble_att_read_group_type_rsp_parse(
-    void *payload, int len, struct ble_att_read_group_type_rsp *rsp);
+    const void *payload, int len, struct ble_att_read_group_type_rsp *rsp);
 void ble_att_read_group_type_rsp_write(
-    void *payload, int len, struct ble_att_read_group_type_rsp *rsp);
-void ble_att_read_group_type_rsp_log(struct ble_att_read_group_type_rsp *cmd);
-void ble_att_write_req_parse(void *payload, int len,
+    void *payload, int len, const struct ble_att_read_group_type_rsp *rsp);
+void ble_att_read_group_type_rsp_log(
+    const struct ble_att_read_group_type_rsp *cmd);
+void ble_att_write_req_parse(const void *payload, int len,
                              struct ble_att_write_req *req);
 void ble_att_write_req_write(void *payload, int len,
-                             struct ble_att_write_req *req);
-void ble_att_write_cmd_parse(void *payload, int len,
+                             const struct ble_att_write_req *req);
+void ble_att_write_cmd_parse(const void *payload, int len,
                              struct ble_att_write_req *req);
 void ble_att_write_cmd_write(void *payload, int len,
-                             struct ble_att_write_req *req);
-void ble_att_write_cmd_log(struct ble_att_write_req *cmd);
-void ble_att_prep_write_req_parse(void *payload, int len,
+                             const struct ble_att_write_req *req);
+void ble_att_write_cmd_log(const struct ble_att_write_req *cmd);
+void ble_att_prep_write_req_parse(const void *payload, int len,
                                   struct ble_att_prep_write_cmd *cmd);
 void ble_att_prep_write_req_write(void *payload, int len,
-                                  struct ble_att_prep_write_cmd *cmd);
-void ble_att_prep_write_cmd_log(struct ble_att_prep_write_cmd *cmd);
-void ble_att_prep_write_rsp_parse(void *payload, int len,
+                                  const struct ble_att_prep_write_cmd *cmd);
+void ble_att_prep_write_cmd_log(const struct ble_att_prep_write_cmd *cmd);
+void ble_att_prep_write_rsp_parse(const void *payload, int len,
                                   struct ble_att_prep_write_cmd *cmd);
 void ble_att_prep_write_rsp_write(void *payload, int len,
-                                  struct ble_att_prep_write_cmd *cmd);
-void ble_att_exec_write_req_parse(void *payload, int len,
+                                  const struct ble_att_prep_write_cmd *cmd);
+void ble_att_exec_write_req_parse(const void *payload, int len,
                                   struct ble_att_exec_write_req *req);
-void ble_att_exec_write_req_log(struct ble_att_exec_write_req *cmd);
+void ble_att_exec_write_req_log(const struct ble_att_exec_write_req *cmd);
 void ble_att_exec_write_req_write(void *payload, int len,
-                                  struct ble_att_exec_write_req *req);
-void ble_att_exec_write_rsp_parse(void *payload, int len);
+                                  const struct ble_att_exec_write_req *req);
+void ble_att_exec_write_rsp_parse(const void *payload, int len);
 void ble_att_exec_write_rsp_write(void *payload, int len);
-void ble_att_notify_req_parse(void *payload, int len,
+void ble_att_notify_req_parse(const void *payload, int len,
                               struct ble_att_notify_req *req);
 void ble_att_notify_req_write(void *payload, int len,
-                              struct ble_att_notify_req *req);
-void ble_att_notify_req_log(struct ble_att_notify_req *cmd);
-void ble_att_indicate_req_parse(void *payload, int len,
+                              const struct ble_att_notify_req *req);
+void ble_att_notify_req_log(const struct ble_att_notify_req *cmd);
+void ble_att_indicate_req_parse(const void *payload, int len,
                                 struct ble_att_indicate_req *req);
 void ble_att_indicate_req_write(void *payload, int len,
-                                struct ble_att_indicate_req *req);
-void ble_att_indicate_rsp_parse(void *payload, int len);
+                                const struct ble_att_indicate_req *req);
+void ble_att_indicate_rsp_parse(const void *payload, int len);
 void ble_att_indicate_rsp_write(void *payload, int len);
-void ble_att_indicate_req_log(struct ble_att_indicate_req *cmd);
+void ble_att_indicate_req_log(const struct ble_att_indicate_req *cmd);
 
 #endif
diff --git a/net/nimble/host/src/ble_att_priv.h b/net/nimble/host/src/ble_att_priv.h
index f908b9c1..ee9a9e98 100644
--- a/net/nimble/host/src/ble_att_priv.h
+++ b/net/nimble/host/src/ble_att_priv.h
@@ -100,10 +100,6 @@ STATS_SECT_START(ble_att_stats)
 STATS_SECT_END
 extern STATS_SECT_DECL(ble_att_stats) ble_att_stats;
 
-#define BLE_ATT_MTU_DFLT                23  /* Also the minimum. */
-#define BLE_ATT_MTU_MAX                 240
-#define BLE_ATT_MTU_PREFERRED_DFLT      240
-
 struct ble_att_prep_entry {
     SLIST_ENTRY(ble_att_prep_entry) bape_next;
     uint16_t bape_handle;
@@ -123,6 +119,29 @@ struct ble_att_svr_conn {
     uint32_t basc_prep_write_rx_time;
 };
 
+/**
+ * Handles a host attribute request.
+ *
+ * @param entry                 The host attribute being requested.
+ * @param op                    The operation being performed on the attribute.
+ * @param arg                   The request data associated with that host
+ *                                  attribute.
+ *
+ * @return                      0 on success;
+ *                              One of the BLE_ATT_ERR_[...] codes on
+ *                                  failure.
+ */
+typedef int ble_att_svr_access_fn(uint16_t conn_handle, uint16_t attr_handle,
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg);
+
+int ble_att_svr_register(const uint8_t *uuid, uint8_t flags,
+                         uint16_t *handle_id,
+                         ble_att_svr_access_fn *cb, void *cb_arg);
+int ble_att_svr_register_uuid16(uint16_t uuid16, uint8_t flags,
+                                uint16_t *handle_id, ble_att_svr_access_fn *cb,
+                                void *cb_arg);
+
 struct ble_att_svr_entry {
     STAILQ_ENTRY(ble_att_svr_entry) ha_next;
 
@@ -139,11 +158,11 @@ SLIST_HEAD(ble_att_clt_entry_list, ble_att_clt_entry);
 /*** @gen */
 
 struct ble_l2cap_chan *ble_att_create_chan(void);
-int ble_att_conn_chan_find(uint16_t conn_handle, struct ble_hs_conn **out_conn,
-                           struct ble_l2cap_chan **out_chan);
+void ble_att_conn_chan_find(uint16_t conn_handle, struct ble_hs_conn **out_conn,
+                            struct ble_l2cap_chan **out_chan);
 void ble_att_inc_tx_stat(uint8_t att_op);
-uint8_t *ble_att_get_flat_buf(void);
-uint16_t ble_att_mtu(uint16_t conn_handle);
+void ble_att_truncate_to_mtu(const struct ble_l2cap_chan *att_chan,
+                             struct os_mbuf *txom);
 void ble_att_set_peer_mtu(struct ble_l2cap_chan *chan, uint16_t peer_mtu);
 int ble_att_init(void);
 
@@ -156,9 +175,12 @@ int ble_att_init(void);
 /*** @svr */
 
 struct ble_att_svr_entry *
-ble_att_svr_find_by_uuid(struct ble_att_svr_entry *start_at, uint8_t *uuid);
+ble_att_svr_find_by_uuid(struct ble_att_svr_entry *start_at,
+                         const uint8_t *uuid,
+                         uint16_t end_handle);
 uint16_t ble_att_svr_prev_handle(void);
-int ble_att_svr_rx_mtu(uint16_t conn_handle, struct os_mbuf **om);
+int ble_att_svr_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom);
+struct ble_att_svr_entry *ble_att_svr_find_by_handle(uint16_t handle_id);
 int ble_att_svr_rx_find_info(uint16_t conn_handle, struct os_mbuf **rxom);
 int ble_att_svr_rx_find_type_value(uint16_t conn_handle,
                                    struct os_mbuf **rxom);
@@ -185,7 +207,7 @@ int ble_att_svr_rx_indicate(uint16_t conn_handle,
                             struct os_mbuf **rxom);
 void ble_att_svr_prep_clear(struct ble_att_prep_entry_list *prep_list);
 int ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
-                            struct ble_att_svr_access_ctxt *ctxt,
+                            uint16_t offset, struct os_mbuf *om,
                             uint8_t *out_att_err);
 int ble_att_svr_init(void);
 
@@ -209,6 +231,7 @@ struct ble_att_read_type_adata {
     uint16_t att_handle;
     int value_len;
     uint8_t *value;
+
 };
 
 /** An attribute-data entry in a read by group type response. */
@@ -220,53 +243,55 @@ struct ble_att_read_group_type_adata {
 };
 
 int ble_att_clt_rx_error(uint16_t conn_handle, struct os_mbuf **rxom);
-int ble_att_clt_tx_mtu(uint16_t conn_handle, struct ble_att_mtu_cmd *req);
+int ble_att_clt_tx_mtu(uint16_t conn_handle,
+                       const struct ble_att_mtu_cmd *req);
 int ble_att_clt_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom);
-int ble_att_clt_tx_read(uint16_t conn_handle, struct ble_att_read_req *req);
+int ble_att_clt_tx_read(uint16_t conn_handle,
+                        const struct ble_att_read_req *req);
 int ble_att_clt_rx_read(uint16_t conn_handle, struct os_mbuf **rxom);
 int ble_att_clt_tx_read_blob(uint16_t conn_handle,
-                             struct ble_att_read_blob_req *req);
+                             const struct ble_att_read_blob_req *req);
 int ble_att_clt_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom);
 int ble_att_clt_tx_read_mult(uint16_t conn_handle,
-                             uint16_t *handles, int num_handles);
+                             const uint16_t *handles, int num_handles);
 int ble_att_clt_rx_read_mult(uint16_t conn_handle, struct os_mbuf **rxom);
 int ble_att_clt_tx_read_type(uint16_t conn_handle,
-                             struct ble_att_read_type_req *req,
-                             void *uuid128);
+                             const struct ble_att_read_type_req *req,
+                             const void *uuid128);
 int ble_att_clt_rx_read_type(uint16_t conn_handle, struct os_mbuf **rxom);
-int ble_att_clt_tx_read_group_type(uint16_t conn_handle,
-                                   struct ble_att_read_group_type_req *req,
-                                   void *uuid128);
+int ble_att_clt_tx_read_group_type(
+    uint16_t conn_handle, const struct ble_att_read_group_type_req *req,
+    const void *uuid128);
 int ble_att_clt_rx_read_group_type(uint16_t conn_handle,
                                    struct os_mbuf **rxom);
 int ble_att_clt_tx_find_info(uint16_t conn_handle,
-                             struct ble_att_find_info_req *req);
-int ble_att_clt_rx_find_info(uint16_t conn_handle, struct os_mbuf **om);
-int ble_att_clt_tx_find_type_value(uint16_t conn_handle,
-                                   struct ble_att_find_type_value_req *req,
-                                   void *attribute_value, int value_len);
+                             const struct ble_att_find_info_req *req);
+int ble_att_clt_rx_find_info(uint16_t conn_handle, struct os_mbuf **rxom);
+int ble_att_clt_tx_find_type_value(
+    uint16_t conn_handle, const struct ble_att_find_type_value_req *req,
+    const void *attribute_value, int value_len);
 int ble_att_clt_rx_find_type_value(uint16_t conn_handle,
                                    struct os_mbuf **rxom);
 int ble_att_clt_tx_write_req(uint16_t conn_handle,
-                             struct ble_att_write_req *req,
-                             void *value, uint16_t value_len);
+                             const struct ble_att_write_req *req,
+                             struct os_mbuf *txom);
 int ble_att_clt_tx_write_cmd(uint16_t conn_handle,
-                             struct ble_att_write_req *req,
-                             void *value, uint16_t value_len);
+                             const struct ble_att_write_req *req,
+                             struct os_mbuf *txom);
 int ble_att_clt_tx_prep_write(uint16_t conn_handle,
-                              struct ble_att_prep_write_cmd *req,
-                              void *value, uint16_t value_len);
+                              const struct ble_att_prep_write_cmd *req,
+                              struct os_mbuf *txom);
 int ble_att_clt_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom);
 int ble_att_clt_tx_exec_write(uint16_t conn_handle,
-                              struct ble_att_exec_write_req *req);
+                              const struct ble_att_exec_write_req *req);
 int ble_att_clt_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom);
 int ble_att_clt_rx_write(uint16_t conn_handle, struct os_mbuf **rxom);
 int ble_att_clt_tx_notify(uint16_t conn_handle,
-                          struct ble_att_notify_req *req,
-                          void *value, uint16_t value_len);
+                          const struct ble_att_notify_req *req,
+                          struct os_mbuf *txom);
 int ble_att_clt_tx_indicate(uint16_t conn_handle,
-                            struct ble_att_indicate_req *req,
-                            void *value, uint16_t value_len);
+                            const struct ble_att_indicate_req *req,
+                            struct os_mbuf *txom);
 int ble_att_clt_rx_indicate(uint16_t conn_handle, struct os_mbuf **rxom);
 
 #endif
diff --git a/net/nimble/host/src/ble_att_svr.c b/net/nimble/host/src/ble_att_svr.c
index 1017fc8e..5d64c25b 100644
--- a/net/nimble/host/src/ble_att_svr.c
+++ b/net/nimble/host/src/ble_att_svr.c
@@ -25,7 +25,9 @@
 #include "host/ble_uuid.h"
 #include "ble_hs_priv.h"
 
-static STAILQ_HEAD(, ble_att_svr_entry) ble_att_svr_list;
+STAILQ_HEAD(ble_att_svr_entry_list, ble_att_svr_entry);
+static struct ble_att_svr_entry_list ble_att_svr_list;
+
 static uint16_t ble_att_svr_id;
 
 static void *ble_att_svr_entry_mem;
@@ -72,7 +74,7 @@ ble_att_svr_next_id(void)
  * @return 0 on success, non-zero error code on failure.
  */
 int
-ble_att_svr_register(uint8_t *uuid, uint8_t flags, uint16_t *handle_id,
+ble_att_svr_register(const uint8_t *uuid, uint8_t flags, uint16_t *handle_id,
                      ble_att_svr_access_fn *cb, void *cb_arg)
 {
     struct ble_att_svr_entry *entry;
@@ -159,7 +161,7 @@ ble_att_svr_find_by_handle(uint16_t handle_id)
  * Find a host attribute by UUID.
  *
  * @param uuid                  The ble_uuid_t to search for
- * @param ha_ptr                On input: Indicates the starting point of the
+ * @param prev                  On input: Indicates the starting point of the
  *                                  walk; null means start at the beginning of
  *                                  the list, non-null means start at the
  *                                  following entry.
@@ -170,7 +172,8 @@ ble_att_svr_find_by_handle(uint16_t handle_id)
  * @return                      0 on success; BLE_HS_ENOENT on not found.
  */
 struct ble_att_svr_entry *
-ble_att_svr_find_by_uuid(struct ble_att_svr_entry *prev, uint8_t *uuid)
+ble_att_svr_find_by_uuid(struct ble_att_svr_entry *prev, const uint8_t *uuid,
+                         uint16_t end_handle)
 {
     struct ble_att_svr_entry *entry;
 
@@ -180,7 +183,10 @@ ble_att_svr_find_by_uuid(struct ble_att_svr_entry *prev, uint8_t *uuid)
         entry = STAILQ_NEXT(prev, ha_next);
     }
 
-    for (; entry != NULL; entry = STAILQ_NEXT(entry, ha_next)) {
+    for (;
+         entry != NULL && entry->ha_handle_id <= end_handle;
+         entry = STAILQ_NEXT(entry, ha_next)) {
+
         if (memcmp(entry->ha_uuid, uuid, sizeof entry->ha_uuid) == 0) {
             return entry;
         }
@@ -196,7 +202,7 @@ ble_att_svr_pullup_req_base(struct os_mbuf **om, int base_len,
     uint8_t att_err;
     int rc;
 
-    rc = ble_hs_misc_pullup_base(om, base_len);
+    rc = ble_hs_mbuf_pullup_base(om, base_len);
     if (rc == BLE_HS_ENOMEM) {
         att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
     } else {
@@ -210,42 +216,45 @@ ble_att_svr_pullup_req_base(struct os_mbuf **om, int base_len,
     return rc;
 }
 
-static int
+static void
 ble_att_svr_get_sec_state(uint16_t conn_handle,
                           struct ble_gap_sec_state *out_sec_state)
 {
     struct ble_hs_conn *conn;
 
     ble_hs_lock();
-    conn = ble_hs_conn_find(conn_handle);
-    if (conn != NULL) {
-        *out_sec_state = conn->bhc_sec_state;
-    }
-    ble_hs_unlock();
 
-    if (conn == NULL) {
-        return BLE_HS_ENOTCONN;
-    } else {
-        return 0;
-    }
+    conn = ble_hs_conn_find_assert(conn_handle);
+    *out_sec_state = conn->bhc_sec_state;
+
+    ble_hs_unlock();
 }
 
 static int
-ble_att_svr_check_security(uint16_t conn_handle, int is_read,
-                           struct ble_att_svr_entry *entry,
-                           uint8_t *out_att_err)
+ble_att_svr_check_perms(uint16_t conn_handle, int is_read,
+                        struct ble_att_svr_entry *entry,
+                        uint8_t *out_att_err)
 {
     struct ble_gap_sec_state sec_state;
     int author;
     int authen;
     int enc;
-    int rc;
 
     if (is_read) {
+        if (!(entry->ha_flags & BLE_ATT_F_READ)) {
+            *out_att_err = BLE_ATT_ERR_READ_NOT_PERMITTED;
+            return BLE_HS_ENOTSUP;
+        }
+
         enc = entry->ha_flags & BLE_ATT_F_READ_ENC;
         authen = entry->ha_flags & BLE_ATT_F_READ_AUTHEN;
         author = entry->ha_flags & BLE_ATT_F_READ_AUTHOR;
     } else {
+        if (!(entry->ha_flags & BLE_ATT_F_WRITE)) {
+            *out_att_err = BLE_ATT_ERR_WRITE_NOT_PERMITTED;
+            return BLE_HS_ENOTSUP;
+        }
+
         enc = entry->ha_flags & BLE_ATT_F_WRITE_ENC;
         authen = entry->ha_flags & BLE_ATT_F_WRITE_AUTHEN;
         author = entry->ha_flags & BLE_ATT_F_WRITE_AUTHOR;
@@ -256,21 +265,17 @@ ble_att_svr_check_security(uint16_t conn_handle, int is_read,
         return 0;
     }
 
-    rc = ble_att_svr_get_sec_state(conn_handle, &sec_state);
-    if (rc != 0) {
-        return rc;
-    }
-
+    ble_att_svr_get_sec_state(conn_handle, &sec_state);
     if (enc && !sec_state.encrypted) {
         /* XXX: Check security database; if required key present, respond with
          * insufficient encryption error code.
          */
-        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHENT;
+        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
         return BLE_HS_ATT_ERR(*out_att_err);
     }
 
     if (authen && !sec_state.authenticated) {
-        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHENT;
+        *out_att_err = BLE_ATT_ERR_INSUFFICIENT_AUTHEN;
         return BLE_HS_ATT_ERR(*out_att_err);
     }
 
@@ -284,7 +289,8 @@ ble_att_svr_check_security(uint16_t conn_handle, int is_read,
 static int
 ble_att_svr_read(uint16_t conn_handle,
                  struct ble_att_svr_entry *entry,
-                 struct ble_att_svr_access_ctxt *ctxt,
+                 uint16_t offset,
+                 struct os_mbuf *om,
                  uint8_t *out_att_err)
 {
     uint8_t att_err;
@@ -293,13 +299,7 @@ ble_att_svr_read(uint16_t conn_handle,
     att_err = 0;    /* Silence gcc warning. */
 
     if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
-        if (!(entry->ha_flags & BLE_ATT_F_READ)) {
-            att_err = BLE_ATT_ERR_READ_NOT_PERMITTED;
-            rc = BLE_HS_ENOTSUP;
-            goto err;
-        }
-
-        rc = ble_att_svr_check_security(conn_handle, 1, entry, &att_err);
+        rc = ble_att_svr_check_perms(conn_handle, 1, entry, &att_err);
         if (rc != 0) {
             goto err;
         }
@@ -307,8 +307,7 @@ ble_att_svr_read(uint16_t conn_handle,
 
     BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
     rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
-                      entry->ha_uuid, BLE_ATT_ACCESS_OP_READ, ctxt,
-                      entry->ha_cb_arg);
+                      BLE_ATT_ACCESS_OP_READ, offset, &om, entry->ha_cb_arg);
     if (rc != 0) {
         att_err = rc;
         rc = BLE_HS_EAPP;
@@ -324,9 +323,51 @@ err:
     return rc;
 }
 
+static int
+ble_att_svr_read_flat(uint16_t conn_handle, 
+                      struct ble_att_svr_entry *entry,
+                      uint16_t offset,
+                      uint16_t max_len,
+                      void *dst,
+                      uint16_t *out_len,
+                      uint8_t *out_att_err)
+{
+    struct os_mbuf *om;
+    uint16_t len;
+    int rc;
+
+    om = ble_hs_mbuf_l2cap_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
+    if (rc != 0) {
+        goto done;
+    }
+
+    len = OS_MBUF_PKTLEN(om);
+    if (len > max_len) {
+        rc = BLE_HS_EMSGSIZE;
+        *out_att_err = BLE_ATT_ERR_UNLIKELY;
+        goto done;
+    }
+
+    rc = os_mbuf_copydata(om, 0, len, dst);
+    BLE_HS_DBG_ASSERT(rc == 0);
+
+    *out_len = len;
+    rc = 0;
+
+done:
+    os_mbuf_free_chain(om);
+    return rc;
+}
+
 int
 ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
-                        struct ble_att_svr_access_ctxt *ctxt,
+                        uint16_t offset, struct os_mbuf *om,
                         uint8_t *out_att_err)
 {
     struct ble_att_svr_entry *entry;
@@ -340,7 +381,7 @@ ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
         return BLE_HS_ENOENT;
     }
 
-    rc = ble_att_svr_read(conn_handle, entry, ctxt, out_att_err);
+    rc = ble_att_svr_read(conn_handle, entry, offset, om, out_att_err);
     if (rc != 0) {
         return rc;
     }
@@ -348,30 +389,51 @@ ble_att_svr_read_handle(uint16_t conn_handle, uint16_t attr_handle,
     return 0;
 }
 
+/**
+ * Reads a locally registered attribute.  If the specified attribute handle
+ * coresponds to a GATT characteristic value or descriptor, the read is
+ * performed by calling the registered GATT access callback.
+ *
+ * @param attr_handle           The 16-bit handle of the attribute to read.
+ * @param out_om                On success, this is made to point to a
+ *                                  newly-allocated mbuf containing the
+ *                                  attribute data read.
+ *
+ * @return                      0 on success;
+ *                              NimBLE host ATT return code if the attribute
+ *                                  access callback reports failure;
+ *                              NimBLE host core return code on unexpected
+ *                                  error.
+ */
 int
-ble_att_svr_read_local(uint16_t attr_handle, void **out_data,
-                       uint16_t *out_attr_len)
+ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om)
 {
-    struct ble_att_svr_access_ctxt ctxt;
+    struct os_mbuf *om;
     int rc;
 
-    ctxt.offset = 0;
+    om = ble_hs_mbuf_bare_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
 
-    rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, &ctxt,
+    rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0, om,
                                  NULL);
     if (rc != 0) {
-        return rc;
+        goto err;
     }
 
-    *out_attr_len = ctxt.data_len;
-    *out_data = ctxt.attr_data;
-
+    *out_om = om;
     return 0;
+
+err:
+    os_mbuf_free_chain(om);
+    return rc;
 }
 
 static int
 ble_att_svr_write(uint16_t conn_handle, struct ble_att_svr_entry *entry,
-                  struct ble_att_svr_access_ctxt *ctxt, uint8_t *out_att_err)
+                  uint16_t offset, struct os_mbuf **om, uint8_t *out_att_err)
 {
     uint8_t att_err;
     int rc;
@@ -379,31 +441,22 @@ ble_att_svr_write(uint16_t conn_handle, struct ble_att_svr_entry *entry,
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
 
     if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
-        if (!(entry->ha_flags & BLE_ATT_F_WRITE)) {
-            att_err = BLE_ATT_ERR_WRITE_NOT_PERMITTED;
-            rc = BLE_HS_ENOTSUP;
-            goto err;
-        }
-
-        rc = ble_att_svr_check_security(conn_handle, 0, entry, &att_err);
+        rc = ble_att_svr_check_perms(conn_handle, 0, entry, &att_err);
         if (rc != 0) {
-            goto err;
+            goto done;
         }
     }
 
     BLE_HS_DBG_ASSERT(entry->ha_cb != NULL);
     rc = entry->ha_cb(conn_handle, entry->ha_handle_id,
-                      entry->ha_uuid, BLE_ATT_ACCESS_OP_WRITE, ctxt,
-                      entry->ha_cb_arg);
+                      BLE_ATT_ACCESS_OP_WRITE, offset, om, entry->ha_cb_arg);
     if (rc != 0) {
         att_err = rc;
         rc = BLE_HS_EAPP;
-        goto err;
+        goto done;
     }
 
-    return 0;
-
-err:
+done:
     if (out_att_err != NULL) {
         *out_att_err = att_err;
     }
@@ -412,7 +465,7 @@ err:
 
 static int
 ble_att_svr_write_handle(uint16_t conn_handle, uint16_t attr_handle,
-                         struct ble_att_svr_access_ctxt *ctxt,
+                         uint16_t offset, struct os_mbuf **om,
                          uint8_t *out_att_err)
 {
     struct ble_att_svr_entry *entry;
@@ -420,11 +473,13 @@ ble_att_svr_write_handle(uint16_t conn_handle, uint16_t attr_handle,
 
     entry = ble_att_svr_find_by_handle(attr_handle);
     if (entry == NULL) {
-        *out_att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        if (out_att_err != NULL) {
+            *out_att_err = BLE_ATT_ERR_INVALID_HANDLE;
+        }
         return BLE_HS_ENOENT;
     }
 
-    rc = ble_att_svr_write(conn_handle, entry, ctxt, out_att_err);
+    rc = ble_att_svr_write(conn_handle, entry, offset, om, out_att_err);
     if (rc != 0) {
         return rc;
     }
@@ -434,20 +489,15 @@ ble_att_svr_write_handle(uint16_t conn_handle, uint16_t attr_handle,
 
 static int
 ble_att_svr_tx_error_rsp(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
-                         uint8_t req_op, uint16_t handle, uint8_t error_code)
+                         struct os_mbuf *txom, uint8_t req_op,
+                         uint16_t handle, uint8_t error_code)
 {
     struct ble_att_error_rsp rsp;
-    struct os_mbuf *txom;
     void *dst;
     int rc;
 
     BLE_HS_DBG_ASSERT(error_code != 0);
-
-    txom = ble_hs_misc_pkthdr();
-    if (txom == NULL) {
-        rc = BLE_HS_ENOMEM;
-        goto err;
-    }
+    BLE_HS_DBG_ASSERT(OS_MBUF_PKTLEN(txom) == 0);
 
     dst = os_mbuf_extend(txom, BLE_ATT_ERROR_RSP_SZ);
     if (dst == NULL) {
@@ -460,8 +510,6 @@ ble_att_svr_tx_error_rsp(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
     rsp.baep_error_code = error_code;
 
     ble_att_error_rsp_write(dst, BLE_ATT_ERROR_RSP_SZ, &rsp);
-    BLE_ATT_LOG_CMD(1, "error rsp", conn->bhc_handle,
-                    ble_att_error_rsp_log, &rsp);
 
     rc = ble_l2cap_tx(conn, chan, txom);
     txom = NULL;
@@ -469,6 +517,9 @@ ble_att_svr_tx_error_rsp(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
         goto err;
     }
 
+    BLE_ATT_LOG_CMD(1, "error rsp", conn->bhc_handle,
+                    ble_att_error_rsp_log, &rsp);
+
     return 0;
 
 err:
@@ -479,10 +530,10 @@ err:
 /**
  * Transmits a response or error message over the specified connection.
  *
- * The specified rc value controls what gets sent as follows:
+ * The specified rc and err_status values control what gets sent as follows:
  *     o If rc == 0: tx an affirmative response.
- *     o If rc == BLE_HS_ENOTCONN: tx nothing.
- *     o Else: tx an error response.
+ *     o Else if err_status != 0: tx an error response.
+ *     o Else: tx nothing.
  *
  * In addition, if transmission of an affirmative response fails, an error is
  * sent instead.
@@ -500,17 +551,14 @@ err:
  *                                  field.
  */
 static int
-ble_att_svr_tx_rsp(uint16_t conn_handle, int rc, struct os_mbuf *txom,
+ble_att_svr_tx_rsp(uint16_t conn_handle, int rc, struct os_mbuf *om,
                    uint8_t att_op, uint8_t err_status, uint16_t err_handle)
 {
     struct ble_l2cap_chan *chan;
     struct ble_hs_conn *conn;
     int do_tx;
 
-    if (rc == BLE_HS_ENOTCONN) {
-        /* No connection; tx is not possible. */
-        do_tx = 0;
-    } else if (rc != 0 && err_status == 0) {
+    if (rc != 0 && err_status == 0) {
         /* Processing failed, but err_status of 0 means don't send error. */
         do_tx = 0;
     } else {
@@ -521,30 +569,41 @@ ble_att_svr_tx_rsp(uint16_t conn_handle, int rc, struct os_mbuf *txom,
         ble_hs_lock();
 
         ble_att_conn_chan_find(conn_handle, &conn, &chan);
-        if (chan == NULL) {
-            rc = BLE_HS_ENOTCONN;
-        } else {
-            if (rc == 0) {
-                BLE_HS_DBG_ASSERT(txom != NULL);
-                ble_att_inc_tx_stat(txom->om_data[0]);
-                rc = ble_l2cap_tx(conn, chan, txom);
-                txom = NULL;
-                if (rc != 0) {
-                    err_status = BLE_ATT_ERR_UNLIKELY;
-                }
-            }
+        BLE_HS_DBG_ASSERT(chan != NULL);
 
+        if (rc == 0) {
+            BLE_HS_DBG_ASSERT(om != NULL);
+
+            ble_att_inc_tx_stat(om->om_data[0]);
+            ble_att_truncate_to_mtu(chan, om);
+            rc = ble_l2cap_tx(conn, chan, om);
+            om = NULL;
             if (rc != 0) {
-                STATS_INC(ble_att_stats, error_rsp_tx);
-                ble_att_svr_tx_error_rsp(conn, chan, att_op,
+                err_status = BLE_ATT_ERR_UNLIKELY;
+            }
+        }
+
+        if (rc != 0) {
+            STATS_INC(ble_att_stats, error_rsp_tx);
+
+            /* Reuse om for error response. */
+            if (om == NULL) {
+                om = ble_hs_mbuf_l2cap_pkt();
+            } else {
+                os_mbuf_adj(om, OS_MBUF_PKTLEN(om));
+            }
+            if (om != NULL) {
+                ble_att_svr_tx_error_rsp(conn, chan, om, att_op,
                                          err_handle, err_status);
+                om = NULL;
             }
         }
 
         ble_hs_unlock();
     }
 
-    os_mbuf_free_chain(txom);
+    /* Free mbuf if it was not consumed (i.e., if the send failed). */
+    os_mbuf_free_chain(om);
 
     return rc;
 }
@@ -564,17 +623,11 @@ ble_att_svr_build_mtu_rsp(uint16_t conn_handle, struct os_mbuf **out_txom,
     txom = NULL;
 
     ble_hs_lock();
-    rc = ble_att_conn_chan_find(conn_handle, NULL, &chan);
-    if (rc == 0) {
-        mtu = chan->blc_my_mtu;
-    }
+    ble_att_conn_chan_find(conn_handle, NULL, &chan);
+    mtu = chan->blc_my_mtu;
     ble_hs_unlock();
 
-    if (rc != 0) {
-        goto done;
-    }
-
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
@@ -601,23 +654,24 @@ done:
 }
 
 int
-ble_att_svr_rx_mtu(uint16_t conn_handle, struct os_mbuf **om)
+ble_att_svr_rx_mtu(uint16_t conn_handle, struct os_mbuf **rxom)
 {
     struct ble_att_mtu_cmd cmd;
     struct ble_l2cap_chan *chan;
     struct ble_hs_conn *conn;
     struct os_mbuf *txom;
+    uint16_t mtu;
     uint8_t att_err;
     int rc;
 
     txom = NULL;
 
-    rc = ble_att_svr_pullup_req_base(om, BLE_ATT_MTU_CMD_SZ, &att_err);
+    rc = ble_att_svr_pullup_req_base(rxom, BLE_ATT_MTU_CMD_SZ, &att_err);
     if (rc != 0) {
         goto done;
     }
 
-    ble_att_mtu_cmd_parse((*om)->om_data, (*om)->om_len, &cmd);
+    ble_att_mtu_req_parse((*rxom)->om_data, (*rxom)->om_len, &cmd);
     BLE_ATT_LOG_CMD(0, "mtu req", conn_handle, ble_att_mtu_cmd_log, &cmd);
 
     rc = ble_att_svr_build_mtu_rsp(conn_handle, &txom, &att_err);
@@ -632,12 +686,15 @@ done:
                             att_err, 0);
     if (rc == 0) {
         ble_hs_lock();
+
         ble_att_conn_chan_find(conn_handle, &conn, &chan);
-        if (chan != NULL) {
-            ble_att_set_peer_mtu(chan, cmd.bamc_mtu);
-            chan->blc_flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
-        }
+        ble_att_set_peer_mtu(chan, cmd.bamc_mtu);
+        chan->blc_flags |= BLE_L2CAP_CHAN_F_TXED_MTU;
+        mtu = ble_l2cap_chan_mtu(chan);
+
         ble_hs_unlock();
+
+        ble_gap_mtu_event(conn_handle, BLE_L2CAP_CID_ATT, mtu);
     }
     return rc;
 }
@@ -754,12 +811,8 @@ ble_att_svr_build_find_info_rsp(uint16_t conn_handle,
     txom = NULL;
 
     mtu = ble_att_mtu(conn_handle);
-    if (mtu == 0) {
-        rc = BLE_HS_ENOTCONN;
-        goto done;
-    }
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
@@ -777,8 +830,6 @@ ble_att_svr_build_find_info_rsp(uint16_t conn_handle,
     }
 
     ble_att_find_info_rsp_write(buf, BLE_ATT_FIND_INFO_RSP_BASE_SZ, &rsp);
-    BLE_ATT_LOG_CMD(1, "find info rsp", conn_handle, ble_att_find_info_rsp_log,
-                    &rsp);
 
     /* Write the variable length Information Data field, populating the format
      * field as appropriate.
@@ -790,6 +841,9 @@ ble_att_svr_build_find_info_rsp(uint16_t conn_handle,
         goto done;
     }
 
+    BLE_ATT_LOG_CMD(1, "find info rsp", conn_handle, ble_att_find_info_rsp_log,
+                    &rsp);
+
     rc = 0;
 
 done:
@@ -986,8 +1040,9 @@ ble_att_svr_fill_type_value(uint16_t conn_handle,
                             struct os_mbuf *rxom, struct os_mbuf *txom,
                             uint16_t mtu, uint8_t *out_att_err)
 {
-    struct ble_att_svr_access_ctxt ctxt;
     struct ble_att_svr_entry *ha;
+    uint8_t buf[16];
+    uint16_t attr_len;
     uint16_t uuid16;
     uint16_t first;
     uint16_t prev;
@@ -1016,13 +1071,13 @@ ble_att_svr_fill_type_value(uint16_t conn_handle,
              */
             uuid16 = ble_uuid_128_to_16(ha->ha_uuid);
             if (uuid16 == req->bavq_attr_type) {
-                ctxt.offset = 0;
-                rc = ble_att_svr_read(conn_handle, ha, &ctxt, out_att_err);
+                rc = ble_att_svr_read_flat(conn_handle, ha, 0, sizeof buf, buf,
+                                           &attr_len, out_att_err);
                 if (rc != 0) {
                     goto done;
                 }
-                rc = os_mbuf_memcmp(rxom, BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ,
-                                    ctxt.attr_data, ctxt.data_len);
+                rc = os_mbuf_cmpf(rxom, BLE_ATT_FIND_TYPE_VALUE_REQ_BASE_SZ,
+                                  buf, attr_len);
                 if (rc == 0) {
                     match = 1;
                 }
@@ -1075,7 +1130,7 @@ ble_att_svr_build_find_type_value_rsp(uint16_t conn_handle,
     uint8_t *buf;
     int rc;
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         *out_att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
@@ -1093,10 +1148,6 @@ ble_att_svr_build_find_type_value_rsp(uint16_t conn_handle,
 
     /* Write the variable length Information Data field. */
     mtu = ble_att_mtu(conn_handle);
-    if (mtu == 0) {
-        rc = BLE_HS_ENOTCONN;
-        goto done;
-    }
 
     rc = ble_att_svr_fill_type_value(conn_handle, req, rxom, txom, mtu,
                                      out_att_err);
@@ -1179,15 +1230,15 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle,
                                 uint16_t *err_handle)
 {
     struct ble_att_read_type_rsp rsp;
-    struct ble_att_svr_access_ctxt ctxt;
     struct ble_att_svr_entry *entry;
     struct os_mbuf *txom;
+    uint16_t attr_len;
     uint16_t mtu;
+    uint8_t buf[19];
     uint8_t *dptr;
     int entry_written;
     int txomlen;
     int prev_attr_len;
-    int attr_len;
     int rc;
 
     *att_err = 0;    /* Silence unnecessary warning. */
@@ -1197,11 +1248,8 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle,
     prev_attr_len = 0;
 
     mtu = ble_att_mtu(conn_handle);
-    if (mtu == 0) {
-        return BLE_HS_ENOTCONN;
-    }
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         *err_handle = 0;
@@ -1223,28 +1271,22 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle,
     /* Find all matching attributes, writing a record for each. */
     entry = NULL;
     while (1) {
-        entry = ble_att_svr_find_by_uuid(entry, uuid128);
+        entry = ble_att_svr_find_by_uuid(entry, uuid128, req->batq_end_handle);
         if (entry == NULL) {
             rc = BLE_HS_ENOENT;
             break;
         }
 
-        if (entry->ha_handle_id > req->batq_end_handle) {
-            break;
-        }
-
         if (entry->ha_handle_id >= req->batq_start_handle) {
-            ctxt.offset = 0;
-            rc = ble_att_svr_read(conn_handle, entry, &ctxt, att_err);
+            rc = ble_att_svr_read_flat(conn_handle, entry, 0, sizeof buf, buf,
+                                       &attr_len, att_err);
             if (rc != 0) {
                 *err_handle = entry->ha_handle_id;
                 goto done;
             }
 
-            if (ctxt.data_len > mtu - 4) {
+            if (attr_len > mtu - 4) {
                 attr_len = mtu - 4;
-            } else {
-                attr_len = ctxt.data_len;
             }
 
             if (prev_attr_len == 0) {
@@ -1267,7 +1309,7 @@ ble_att_svr_build_read_type_rsp(uint16_t conn_handle,
             }
 
             htole16(dptr + 0, entry->ha_handle_id);
-            memcpy(dptr + 2, ctxt.attr_data, attr_len);
+            memcpy(dptr + 2, buf, attr_len);
             entry_written = 1;
         }
     }
@@ -1335,6 +1377,7 @@ ble_att_svr_rx_read_type(uint16_t conn_handle, struct os_mbuf **rxom)
     BLE_ATT_LOG_CMD(0, "read type req", conn_handle, ble_att_read_type_req_log,
                     &req);
 
+
     if (req.batq_start_handle > req.batq_end_handle ||
         req.batq_start_handle == 0) {
 
@@ -1381,64 +1424,6 @@ done:
     return rc;
 }
 
-/**
- * @return                      0 on success; nonzero on failure.
- */
-static int
-ble_att_svr_build_read_rsp(uint16_t conn_handle, void *attr_data, int attr_len,
-                           struct os_mbuf **out_txom, uint8_t *att_err)
-{
-    struct os_mbuf *txom;
-    uint16_t data_len;
-    uint16_t mtu;
-    uint8_t op;
-    int rc;
-
-    txom = NULL;
-
-    mtu = ble_att_mtu(conn_handle);
-    if (mtu == 0) {
-        rc = BLE_HS_ENOTCONN;
-        goto done;
-    }
-
-    txom = ble_hs_misc_pkthdr();
-    if (txom == NULL) {
-        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
-        rc = BLE_HS_ENOMEM;
-        goto done;
-    }
-
-    op = BLE_ATT_OP_READ_RSP;
-    rc = os_mbuf_append(txom, &op, 1);
-    if (rc != 0) {
-        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
-        rc = BLE_HS_ENOMEM;
-        goto done;
-    }
-
-    /* Vol. 3, part F, 3.2.9; don't send more than ATT_MTU-1 bytes of data. */
-    if (attr_len > mtu - 1) {
-        data_len = mtu - 1;
-    } else {
-        data_len = attr_len;
-    }
-
-    rc = os_mbuf_append(txom, attr_data, data_len);
-    if (rc != 0) {
-        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
-        rc = BLE_HS_ENOMEM;
-        goto done;
-    }
-
-    BLE_ATT_LOG_EMPTY_CMD(1, "read rsp", conn_handle);
-    rc = 0;
-
-done:
-    *out_txom = txom;
-    return rc;
-}
-
 int
 ble_att_svr_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
 {
@@ -1446,11 +1431,11 @@ ble_att_svr_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct ble_att_svr_access_ctxt ctxt;
     struct ble_att_read_req req;
     struct os_mbuf *txom;
     uint16_t err_handle;
     uint8_t att_err;
+    uint8_t *dptr;
     int rc;
 
     /* Initialize some values in case of early error. */
@@ -1467,74 +1452,31 @@ ble_att_svr_rx_read(uint16_t conn_handle, struct os_mbuf **rxom)
     ble_att_read_req_parse((*rxom)->om_data, (*rxom)->om_len, &req);
     BLE_ATT_LOG_CMD(0, "read req", conn_handle, ble_att_read_req_log, &req);
 
-    ctxt.offset = 0;
-    rc = ble_att_svr_read_handle(conn_handle, req.barq_handle, &ctxt,
-                                 &att_err);
-    if (rc != 0) {
-        err_handle = req.barq_handle;
-        goto done;
-    }
-
-    rc = ble_att_svr_build_read_rsp(conn_handle, ctxt.attr_data, ctxt.data_len,
-                                    &txom, &att_err);
-    if (rc != 0) {
-        err_handle = req.barq_handle;
-        goto done;
-    }
-
-    rc = 0;
-
-done:
-    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_REQ,
-                            att_err, err_handle);
-    return rc;
-}
-
-/**
- * @return                      0 on success; nonzero on failure.
- */
-static int
-ble_att_svr_build_read_blob_rsp(void *attr_data, int attr_len, uint16_t mtu,
-                                struct os_mbuf **out_txom, uint8_t *att_err)
-{
-    struct os_mbuf *txom;
-    uint16_t data_len;
-    uint8_t op;
-    int rc;
-
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
-        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
         goto done;
     }
 
-    op = BLE_ATT_OP_READ_BLOB_RSP;
-    rc = os_mbuf_append(txom, &op, 1);
-    if (rc != 0) {
-        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+    dptr = os_mbuf_extend(txom, 1);
+    if (dptr == NULL) {
+        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
         goto done;
     }
+    *dptr = BLE_ATT_OP_READ_RSP;
 
-    /* Vol. 3, part F, 3.2.9; don't send more than ATT_MTU-1 bytes of data. */
-    if (attr_len > mtu - 1) {
-        data_len = mtu - 1;
-    } else {
-        data_len = attr_len;
-    }
-
-    rc = os_mbuf_append(txom, attr_data, data_len);
+    rc = ble_att_svr_read_handle(conn_handle, req.barq_handle, 0, txom,
+                                 &att_err);
     if (rc != 0) {
-        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
-        rc = BLE_HS_ENOMEM;
+        err_handle = req.barq_handle;
         goto done;
     }
 
-    rc = 0;
-
 done:
-    *out_txom = txom;
+    rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_READ_REQ,
+                            att_err, err_handle);
     return rc;
 }
 
@@ -1545,11 +1487,10 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct ble_att_svr_access_ctxt ctxt;
     struct ble_att_read_blob_req req;
     struct os_mbuf *txom;
     uint16_t err_handle;
-    uint16_t mtu;
+    uint8_t *dptr;
     uint8_t att_err;
     int rc;
 
@@ -1558,12 +1499,6 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
     att_err = 0;
     err_handle = 0;
 
-    mtu = ble_att_mtu(conn_handle);
-    if (mtu == 0) {
-        rc = BLE_HS_ENOTCONN;
-        goto done;
-    }
-
     rc = ble_att_svr_pullup_req_base(rxom, BLE_ATT_READ_BLOB_REQ_SZ, &att_err);
     if (rc != 0) {
         err_handle = 0;
@@ -1574,27 +1509,28 @@ ble_att_svr_rx_read_blob(uint16_t conn_handle, struct os_mbuf **rxom)
     BLE_ATT_LOG_CMD(0, "read blob req", conn_handle, ble_att_read_blob_req_log,
                     &req);
 
-    ctxt.offset = req.babq_offset;
-    rc = ble_att_svr_read_handle(conn_handle, req.babq_handle, &ctxt,
-                                 &att_err);
-    if (rc != 0) {
-        err_handle = req.babq_handle;
+    txom = ble_hs_mbuf_l2cap_pkt();
+    if (txom == NULL) {
+        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
         goto done;
     }
 
-    if (ctxt.offset + ctxt.data_len <= mtu - 3) {
-        att_err = BLE_ATT_ERR_ATTR_NOT_LONG;
-        err_handle = req.babq_handle;
-        rc = BLE_HS_ENOTSUP;
+    dptr = os_mbuf_extend(txom, 1);
+    if (dptr == NULL) {
+        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        rc = BLE_HS_ENOMEM;
         goto done;
     }
+    *dptr = BLE_ATT_OP_READ_BLOB_RSP;
 
-    rc = ble_att_svr_build_read_blob_rsp(ctxt.attr_data, ctxt.data_len, mtu,
-                                         &txom, &att_err);
+    rc = ble_att_svr_read_handle(conn_handle, req.babq_handle, req.babq_offset,
+                                 txom, &att_err);
     if (rc != 0) {
         err_handle = req.babq_handle;
         goto done;
     }
+
     BLE_ATT_LOG_EMPTY_CMD(1, "read blob rsp", conn_handle);
 
     rc = 0;
@@ -1612,24 +1548,15 @@ ble_att_svr_build_read_mult_rsp(uint16_t conn_handle,
                                 uint8_t *att_err,
                                 uint16_t *err_handle)
 {
-    struct ble_att_svr_access_ctxt ctxt;
     struct os_mbuf *txom;
-    uint16_t chunk_sz;
-    uint16_t tx_space;
     uint16_t handle;
     uint16_t mtu;
     uint8_t *dptr;
     int rc;
 
-    txom = NULL;
-
     mtu = ble_att_mtu(conn_handle);
-    if (mtu == 0) {
-        rc = BLE_HS_ENOTCONN;
-        goto done;
-    }
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         *err_handle = 0;
@@ -1646,13 +1573,11 @@ ble_att_svr_build_read_mult_rsp(uint16_t conn_handle,
     }
     ble_att_read_mult_rsp_write(dptr, BLE_ATT_READ_MULT_RSP_BASE_SZ);
 
-    tx_space = mtu - OS_MBUF_PKTLEN(txom);
-
     /* Iterate through requested handles, reading the corresponding attribute
      * for each.  Stop when there are no more handles to process, or the
      * response is full.
      */
-    while (OS_MBUF_PKTLEN(*rxom) >= 2 && tx_space > 0) {
+    while (OS_MBUF_PKTLEN(*rxom) >= 2 && OS_MBUF_PKTLEN(txom) < mtu) {
         /* Ensure the full 16-bit handle is contiguous at the start of the
          * mbuf.
          */
@@ -1668,28 +1593,11 @@ ble_att_svr_build_read_mult_rsp(uint16_t conn_handle,
         handle = le16toh((*rxom)->om_data);
         os_mbuf_adj(*rxom, 2);
 
-        ctxt.offset = 0;
-        rc = ble_att_svr_read_handle(conn_handle, handle, &ctxt, att_err);
+        rc = ble_att_svr_read_handle(conn_handle, handle, 0, txom, att_err);
         if (rc != 0) {
             *err_handle = handle;
             goto done;
         }
-
-        if (ctxt.data_len > tx_space) {
-            chunk_sz = tx_space;
-        } else {
-            chunk_sz = ctxt.data_len;
-        }
-
-        rc = os_mbuf_append(txom, ctxt.attr_data, chunk_sz);
-        if (rc != 0) {
-            *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
-            *err_handle = handle;
-            rc = BLE_HS_ENOMEM;
-            goto done;
-        }
-
-        tx_space -= chunk_sz;
     }
 
     BLE_ATT_LOG_EMPTY_CMD(1, "read mult rsp", conn_handle);
@@ -1760,23 +1668,22 @@ static int
 ble_att_svr_service_uuid(struct ble_att_svr_entry *entry, uint16_t *uuid16,
                          uint8_t *uuid128)
 {
-    struct ble_att_svr_access_ctxt ctxt;
+    uint16_t attr_len;
     int rc;
 
-    ctxt.offset = 0;
-    rc = ble_att_svr_read(BLE_HS_CONN_HANDLE_NONE, entry, &ctxt, NULL);
+    rc = ble_att_svr_read_flat(BLE_HS_CONN_HANDLE_NONE, entry, 0, 16, uuid128,
+                               &attr_len, NULL);
     if (rc != 0) {
         return rc;
     }
 
-    switch (ctxt.data_len) {
+    switch (attr_len) {
     case 16:
         *uuid16 = 0;
-        memcpy(uuid128, ctxt.attr_data, 16);
         return 0;
 
     case 2:
-        *uuid16 = le16toh(ctxt.attr_data);
+        *uuid16 = le16toh(uuid128);
         if (*uuid16 == 0) {
             return BLE_HS_EINVAL;
         }
@@ -1852,15 +1759,9 @@ ble_att_svr_build_read_group_type_rsp(uint16_t conn_handle,
     *att_err = 0;
     *err_handle = req->bagq_start_handle;
 
-    txom = NULL;
-
     mtu = ble_att_mtu(conn_handle);
-    if (mtu == 0) {
-        rc = BLE_HS_ENOTCONN;
-        goto done;
-    }
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
@@ -2091,7 +1992,7 @@ ble_att_svr_build_write_rsp(struct os_mbuf **out_txom, uint8_t *att_err)
     uint8_t *dst;
     int rc;
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
@@ -2121,7 +2022,6 @@ ble_att_svr_rx_write(uint16_t conn_handle, struct os_mbuf **rxom)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct ble_att_svr_access_ctxt ctxt;
     struct ble_att_write_req req;
     struct os_mbuf *txom;
     uint16_t err_handle;
@@ -2147,10 +2047,7 @@ ble_att_svr_rx_write(uint16_t conn_handle, struct os_mbuf **rxom)
     /* Strip the request base from the front of the mbuf. */
     os_mbuf_adj(*rxom, BLE_ATT_WRITE_REQ_BASE_SZ);
 
-    ctxt.attr_data = ble_att_get_flat_buf();
-    ctxt.data_len = OS_MBUF_PKTLEN(*rxom);
-    os_mbuf_copydata(*rxom, 0, ctxt.data_len, ctxt.attr_data);
-    rc = ble_att_svr_write_handle(conn_handle, req.bawq_handle, &ctxt,
+    rc = ble_att_svr_write_handle(conn_handle, req.bawq_handle, 0, rxom,
                                   &att_err);
     if (rc != 0) {
         err_handle = req.bawq_handle;
@@ -2179,7 +2076,6 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, struct os_mbuf **rxom)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct ble_att_svr_access_ctxt ctxt;
     struct ble_att_write_req req;
     uint8_t att_err;
     int rc;
@@ -2197,10 +2093,7 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, struct os_mbuf **rxom)
     /* Strip the request base from the front of the mbuf. */
     os_mbuf_adj(*rxom, BLE_ATT_WRITE_REQ_BASE_SZ);
 
-    ctxt.attr_data = ble_att_get_flat_buf();
-    ctxt.data_len = OS_MBUF_PKTLEN(*rxom);
-    os_mbuf_copydata(*rxom, 0, ctxt.data_len, ctxt.attr_data);
-    rc = ble_att_svr_write_handle(conn_handle, req.bawq_handle, &ctxt,
+    rc = ble_att_svr_write_handle(conn_handle, req.bawq_handle, 0, rxom,
                                   &att_err);
     if (rc != 0) {
         return rc;
@@ -2209,18 +2102,31 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, struct os_mbuf **rxom)
     return 0;
 }
 
+/**
+ * Writes a locally registered attribute.  This function consumes the supplied
+ * mbuf regardless of the outcome.  If the specified attribute handle
+ * coresponds to a GATT characteristic value or descriptor, the write is
+ * performed by calling the registered GATT access callback.
+ *
+ * @param attr_handle           The 16-bit handle of the attribute to write.
+ * @param om                    The value to write to the attribute.
+ *
+ * @return                      0 on success;
+ *                              NimBLE host ATT return code if the attribute
+ *                                  access callback reports failure;
+ *                              NimBLE host core return code on unexpected
+ *                                  error.
+ */
 int
-ble_att_svr_write_local(uint16_t attr_handle, void *data, uint16_t data_len)
+ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om)
 {
-    struct ble_att_svr_access_ctxt ctxt;
     int rc;
 
-    ctxt.attr_data = data;
-    ctxt.data_len = data_len;
-    ctxt.offset = 0;
+    rc = ble_att_svr_write_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, 0,
+                                  &om, NULL);
 
-    rc = ble_att_svr_write_handle(BLE_HS_CONN_HANDLE_NONE, attr_handle, &ctxt,
-                                  NULL);
+    /* Free the mbuf if it wasn't relinquished to the application. */
+    os_mbuf_free_chain(om);
 
     return rc;
 }
@@ -2228,8 +2134,10 @@ ble_att_svr_write_local(uint16_t attr_handle, void *data, uint16_t data_len)
 static void
 ble_att_svr_prep_free(struct ble_att_prep_entry *entry)
 {
-    os_mbuf_free_chain(entry->bape_value);
-    os_memblock_put(&ble_att_svr_prep_entry_pool, entry);
+    if (entry != NULL) {
+        os_mbuf_free_chain(entry->bape_value);
+        os_memblock_put(&ble_att_svr_prep_entry_pool, entry);
+    }
 }
 
 static struct ble_att_prep_entry *
@@ -2243,7 +2151,7 @@ ble_att_svr_prep_alloc(void)
     }
 
     memset(entry, 0, sizeof *entry);
-    entry->bape_value = ble_hs_misc_pkthdr();
+    entry->bape_value = ble_hs_mbuf_l2cap_pkt();
     if (entry->bape_value == NULL) {
         ble_att_svr_prep_free(entry);
         return NULL;
@@ -2327,6 +2235,42 @@ ble_att_svr_prep_validate(struct ble_att_prep_entry_list *prep_list,
     return 0;
 }
 
+static void
+ble_att_svr_prep_extract(struct ble_att_prep_entry_list *prep_list,
+                         uint16_t *out_attr_handle,
+                         struct os_mbuf **out_om)
+{
+    struct ble_att_prep_entry *entry;
+    struct ble_att_prep_entry *first;
+    struct os_mbuf *om;
+    uint16_t attr_handle;
+
+    BLE_HS_DBG_ASSERT(!SLIST_EMPTY(prep_list));
+
+    first = SLIST_FIRST(prep_list);
+    attr_handle = first->bape_handle;
+    om = NULL;
+
+    while ((entry = SLIST_FIRST(prep_list)) != NULL) {
+        if (entry->bape_handle != attr_handle) {
+            break;
+        }
+
+        if (om == NULL) {
+            om = entry->bape_value;
+        } else {
+            os_mbuf_concat(om, entry->bape_value);
+        }
+        entry->bape_value = NULL;
+
+        SLIST_REMOVE_HEAD(prep_list, bape_next);
+        ble_att_svr_prep_free(entry);
+    }
+
+    *out_attr_handle = attr_handle;
+    *out_om = om;
+}
+
 /**
  * @return                      0 on success; ATT error code on failure.
  */
@@ -2335,13 +2279,10 @@ ble_att_svr_prep_write(uint16_t conn_handle,
                        struct ble_att_prep_entry_list *prep_list,
                        uint16_t *err_handle)
 {
-    struct ble_att_svr_access_ctxt ctxt;
-    struct ble_att_prep_entry *entry;
-    struct ble_att_prep_entry *next;
     struct ble_att_svr_entry *attr;
-    uint8_t *flat_buf;
+    struct os_mbuf *om;
+    uint16_t attr_handle;
     uint8_t att_err;
-    int buf_off;
     int rc;
 
     *err_handle = 0; /* Silence unnecessary warning. */
@@ -2352,40 +2293,68 @@ ble_att_svr_prep_write(uint16_t conn_handle,
         return rc;
     }
 
-    flat_buf = ble_att_get_flat_buf();
-
     /* Contents are valid; perform the writes. */
-    buf_off = 0;
-    entry = SLIST_FIRST(prep_list);
-    while (entry != NULL) {
-        next = SLIST_NEXT(entry, bape_next);
-
-        rc = os_mbuf_copydata(entry->bape_value, 0,
-                              OS_MBUF_PKTLEN(entry->bape_value),
-                              flat_buf + buf_off);
-        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
-        buf_off += OS_MBUF_PKTLEN(entry->bape_value);
-
-        /* If this is the last entry for this attribute, perform the write. */
-        if (next == NULL || entry->bape_handle != next->bape_handle) {
-            attr = ble_att_svr_find_by_handle(entry->bape_handle);
-            if (attr == NULL) {
-                *err_handle = entry->bape_handle;
-                return BLE_ATT_ERR_INVALID_HANDLE;
-            }
+    while (!SLIST_EMPTY(prep_list)) {
+        ble_att_svr_prep_extract(prep_list, &attr_handle, &om);
 
-            ctxt.attr_data = flat_buf;
-            ctxt.data_len = buf_off;
-            rc = ble_att_svr_write(conn_handle, attr, &ctxt, &att_err);
-            if (rc != 0) {
-                *err_handle = entry->bape_handle;
-                return att_err;
-            }
+        /* Attribute existence was verified during prepare-write request
+         * processing.
+         */
+        attr = ble_att_svr_find_by_handle(attr_handle);
+        BLE_HS_DBG_ASSERT(attr != NULL);
 
-            buf_off = 0;
+        rc = ble_att_svr_write(conn_handle, attr, 0, &om, &att_err);
+        os_mbuf_free_chain(om);
+        if (rc != 0) {
+            *err_handle = attr_handle;
+            return att_err;
         }
+    }
+
+    return 0;
+}
 
-        entry = next;
+static int
+ble_att_svr_insert_prep_entry(uint16_t conn_handle,
+                              const struct ble_att_prep_write_cmd *req,
+                              const struct os_mbuf *rxom,
+                              uint8_t *out_att_err)
+{
+    struct ble_att_prep_entry *prep_entry;
+    struct ble_att_prep_entry *prep_prev;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    conn = ble_hs_conn_find_assert(conn_handle);
+
+    prep_entry = ble_att_svr_prep_alloc();
+    if (prep_entry == NULL) {
+        *out_att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
+        return BLE_HS_ENOMEM;
+    }
+    prep_entry->bape_handle = req->bapc_handle;
+    prep_entry->bape_offset = req->bapc_offset;
+
+    /* Append attribute value from request onto prep mbuf. */
+    rc = os_mbuf_appendfrom(
+        prep_entry->bape_value,
+        rxom,
+        BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
+        OS_MBUF_PKTLEN(rxom) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+    if (rc != 0) {
+        ble_att_svr_prep_free(prep_entry);
+        *out_att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
+        return rc;
+    }
+
+    prep_prev = ble_att_svr_prep_find_prev(&conn->bhc_att_svr,
+                                           req->bapc_handle,
+                                           req->bapc_offset);
+    if (prep_prev == NULL) {
+        SLIST_INSERT_HEAD(&conn->bhc_att_svr.basc_prep_list, prep_entry,
+                          bape_next);
+    } else {
+        SLIST_INSERT_AFTER(prep_prev, prep_entry, bape_next);
     }
 
     return 0;
@@ -2399,18 +2368,13 @@ ble_att_svr_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom)
 #endif
 
     struct ble_att_prep_write_cmd req;
-    struct ble_att_prep_entry *prep_entry;
-    struct ble_att_prep_entry *prep_prev;
     struct ble_att_svr_entry *attr_entry;
-    struct ble_hs_conn *conn;
-    struct os_mbuf *srcom;
     struct os_mbuf *txom;
     uint16_t err_handle;
     uint8_t att_err;
     int rc;
 
     /* Initialize some values in case of early error. */
-    prep_entry = NULL;
     txom = NULL;
     att_err = 0;
     err_handle = 0;
@@ -2425,115 +2389,55 @@ ble_att_svr_rx_prep_write(uint16_t conn_handle, struct os_mbuf **rxom)
     ble_att_prep_write_req_parse((*rxom)->om_data, (*rxom)->om_len, &req);
     BLE_ATT_LOG_CMD(0, "prep write req", conn_handle,
                     ble_att_prep_write_cmd_log, &req);
-
-    /* Strip the request base from the front of the mbuf. */
-    os_mbuf_adj(*rxom, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+    err_handle = req.bapc_handle;
 
     attr_entry = ble_att_svr_find_by_handle(req.bapc_handle);
+
+    /* A prepare write request gets rejected for the following reasons:
+     * 1. Insufficient authorization.
+     * 2. Insufficient authentication.
+     * 3. Insufficient encryption key size (XXX: Not checked).
+     * 4. Insufficient encryption (XXX: Not checked).
+     * 5. Invalid handle.
+     * 6. Write not permitted.
+     */
+
+    /* <5> */
     if (attr_entry == NULL) {
         rc = BLE_HS_ENOENT;
         att_err = BLE_ATT_ERR_INVALID_HANDLE;
-        err_handle = req.bapc_handle;
         goto done;
     }
 
-    prep_entry = ble_att_svr_prep_alloc();
-    if (prep_entry == NULL) {
-        att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
-        err_handle = req.bapc_handle;
-        rc = BLE_HS_ENOMEM;
+    /* <1>, <2>, <4>, <6> */
+    rc = ble_att_svr_check_perms(conn_handle, 0, attr_entry, &att_err);
+    if (rc != 0) {
         goto done;
     }
-    prep_entry->bape_handle = req.bapc_handle;
-    prep_entry->bape_offset = req.bapc_offset;
 
     ble_hs_lock();
-
-    conn = ble_hs_conn_find(conn_handle);
-    if (conn == NULL) {
-        rc = BLE_HS_ENOTCONN;
-    } else {
-        prep_prev = ble_att_svr_prep_find_prev(&conn->bhc_att_svr,
-                                               req.bapc_handle,
-                                               req.bapc_offset);
-        if (prep_prev == NULL) {
-            SLIST_INSERT_HEAD(&conn->bhc_att_svr.basc_prep_list, prep_entry,
-                              bape_next);
-        } else {
-            SLIST_INSERT_AFTER(prep_prev, prep_entry, bape_next);
-        }
-
-        /* Append attribute value from request onto prep mbuf. */
-        for (srcom = *rxom;
-             srcom != NULL;
-             srcom = SLIST_NEXT(srcom, om_next)) {
-
-            rc = os_mbuf_append(prep_entry->bape_value, srcom->om_data,
-                                srcom->om_len);
-            if (rc != 0) {
-                att_err = BLE_ATT_ERR_PREPARE_QUEUE_FULL;
-                err_handle = req.bapc_handle;
-                break;
-            }
-        }
-    }
-
+    rc = ble_att_svr_insert_prep_entry(conn_handle, &req, *rxom, &att_err);
     ble_hs_unlock();
 
     if (rc != 0) {
         goto done;
     }
 
-    /* The receive buffer now contains the attribute value.  Repurpose this
-     * buffer for the response.  Prepend a response header.
+    /* Reuse rxom for response.  Response is identical to request except for
+     * op code.
      */
-    *rxom = os_mbuf_prepend(*rxom, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
-    if (*rxom == NULL) {
-        att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
-        err_handle = req.bapc_handle;
-        rc = BLE_HS_ENOMEM;
-        goto done;
-    }
     txom = *rxom;
+    *rxom = NULL;
+    txom->om_data[0] = BLE_ATT_OP_PREP_WRITE_RSP;
 
-    ble_att_prep_write_rsp_write(txom->om_data, BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
-                                 &req);
     BLE_ATT_LOG_CMD(1, "prep write rsp", conn_handle,
                     ble_att_prep_write_cmd_log, &req);
 
     rc = 0;
 
 done:
-    if (rc != 0 && rc != BLE_HS_ENOTCONN) {
-        ble_hs_lock();
-
-        conn = ble_hs_conn_find(conn_handle);
-        if (conn == NULL) {
-            rc = BLE_HS_ENOTCONN;
-        } else {
-            if (prep_entry != NULL) {
-                if (prep_prev == NULL) {
-                    SLIST_REMOVE_HEAD(&conn->bhc_att_svr.basc_prep_list,
-                                      bape_next);
-                } else {
-                    SLIST_NEXT(prep_prev, bape_next) =
-                        SLIST_NEXT(prep_entry, bape_next);
-                }
-
-                ble_att_svr_prep_free(prep_entry);
-            }
-        }
-
-        ble_hs_unlock();
-    }
-
     rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_PREP_WRITE_REQ,
                             att_err, err_handle);
-
-    /* Make sure the receive buffer doesn't get freed since we are using it for
-     * the response.
-     */
-    *rxom = NULL;
     return rc;
 }
 
@@ -2547,7 +2451,7 @@ ble_att_svr_build_exec_write_rsp(struct os_mbuf **out_txom, uint8_t *att_err)
     uint8_t *dst;
     int rc;
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
         rc = BLE_HS_ENOMEM;
@@ -2610,33 +2514,28 @@ ble_att_svr_rx_exec_write(uint16_t conn_handle, struct os_mbuf **rxom)
 done:
     if (rc == 0) {
         ble_hs_lock();
-        conn = ble_hs_conn_find(conn_handle);
-        if (conn == NULL) {
-            rc = BLE_HS_ENOTCONN;
-        } else {
-            /* Extract the list of prepared writes from the connection so
-             * that they can be processed after the mutex is unlocked.  They
-             * aren't processed now because attribute writes involve executing
-             * an application callback.
-             */
-            prep_list = conn->bhc_att_svr.basc_prep_list;
-            SLIST_INIT(&conn->bhc_att_svr.basc_prep_list);
-        }
+        conn = ble_hs_conn_find_assert(conn_handle);
+
+        /* Extract the list of prepared writes from the connection so
+         * that they can be processed after the mutex is unlocked.  They
+         * aren't processed now because attribute writes involve executing
+         * an application callback.
+         */
+        prep_list = conn->bhc_att_svr.basc_prep_list;
+        SLIST_INIT(&conn->bhc_att_svr.basc_prep_list);
         ble_hs_unlock();
 
-        if (conn != NULL) {
-            if (req.baeq_flags & BLE_ATT_EXEC_WRITE_F_CONFIRM) {
-                /* Perform attribute writes. */
-                att_err = ble_att_svr_prep_write(conn_handle, &prep_list,
-                                                 &err_handle);
-                if (att_err != 0) {
-                    rc = BLE_HS_EAPP;
-                }
+        if (req.baeq_flags & BLE_ATT_EXEC_WRITE_F_CONFIRM) {
+            /* Perform attribute writes. */
+            att_err = ble_att_svr_prep_write(conn_handle, &prep_list,
+                                             &err_handle);
+            if (att_err != 0) {
+                rc = BLE_HS_EAPP;
             }
-
-            /* Free the prep entries. */
-            ble_att_svr_prep_clear(&prep_list);
         }
+
+        /* Free the prep entries. */
+        ble_att_svr_prep_clear(&prep_list);
     }
 
     rc = ble_att_svr_tx_rsp(conn_handle, rc, txom, BLE_ATT_OP_EXEC_WRITE_REQ,
@@ -2652,8 +2551,6 @@ ble_att_svr_rx_notify(uint16_t conn_handle, struct os_mbuf **rxom)
 #endif
 
     struct ble_att_notify_req req;
-    uint16_t attr_len;
-    void *attr_data;
     int rc;
 
     if (OS_MBUF_PKTLEN(*rxom) < BLE_ATT_NOTIFY_REQ_BASE_SZ) {
@@ -2676,11 +2573,8 @@ ble_att_svr_rx_notify(uint16_t conn_handle, struct os_mbuf **rxom)
     /* Strip the request base from the front of the mbuf. */
     os_mbuf_adj(*rxom, BLE_ATT_NOTIFY_REQ_BASE_SZ);
 
-    attr_data = ble_att_get_flat_buf();
-    attr_len = OS_MBUF_PKTLEN(*rxom);
-    os_mbuf_copydata(*rxom, 0, attr_len, attr_data);
-
-    ble_gap_notify_event(conn_handle, req.banq_handle, attr_data, attr_len, 0);
+    ble_gap_notify_rx_event(conn_handle, req.banq_handle, *rxom, 0);
+    *rxom = NULL;
 
     return 0;
 }
@@ -2695,7 +2589,7 @@ ble_att_svr_build_indicate_rsp(struct os_mbuf **out_txom)
     uint8_t *dst;
     int rc;
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
         rc = BLE_HS_ENOMEM;
         goto done;
@@ -2725,8 +2619,6 @@ ble_att_svr_rx_indicate(uint16_t conn_handle, struct os_mbuf **rxom)
 
     struct ble_att_indicate_req req;
     struct os_mbuf *txom;
-    uint16_t attr_len;
-    void *attr_data;
     int rc;
 
     /* Initialize some values in case of early error. */
@@ -2754,11 +2646,8 @@ ble_att_svr_rx_indicate(uint16_t conn_handle, struct os_mbuf **rxom)
     /* Strip the request base from the front of the mbuf. */
     os_mbuf_adj(*rxom, BLE_ATT_INDICATE_REQ_BASE_SZ);
 
-    attr_data = ble_att_get_flat_buf();
-    attr_len = OS_MBUF_PKTLEN(*rxom);
-    os_mbuf_copydata(*rxom, 0, attr_len, attr_data);
-
-    ble_gap_notify_event(conn_handle, req.baiq_handle, attr_data, attr_len, 1);
+    ble_gap_notify_rx_event(conn_handle, req.baiq_handle, *rxom, 1);
+    *rxom = NULL;
 
     rc = ble_att_svr_build_indicate_rsp(&txom);
     if (rc != 0) {
diff --git a/net/nimble/host/src/ble_eddystone.c b/net/nimble/host/src/ble_eddystone.c
index dbd8a716..2ca496f6 100644
--- a/net/nimble/host/src/ble_eddystone.c
+++ b/net/nimble/host/src/ble_eddystone.c
@@ -19,6 +19,7 @@
 
 #include <string.h>
 #include "host/ble_eddystone.h"
+#include "host/ble_hs_adv.h"
 #include "ble_hs_priv.h"
 
 #define BLE_EDDYSTONE_MAX_SVC_DATA_LEN  23
@@ -112,7 +113,11 @@ ble_eddystone_set_adv_data_gen(struct ble_hs_adv_fields *adv_fields,
  *                                  this struct before calling this function.
  * @param uid                   The 16-byte UID to advertise.
  *
- * @return                      0 on success; BLE_HS_E... on failure.
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if advertising is in progress;
+ *                              BLE_HS_EMSGSIZE if the specified data is too
+ *                                  large to fit in an advertisement;
+ *                              Other nonzero on failure.
  */
 int
 ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields, void *uid)
@@ -151,7 +156,11 @@ ble_eddystone_set_adv_data_uid(struct ble_hs_adv_fields *adv_fields, void *uid)
  *                                  BLE_EDDYSTONE_URL_SUFFIX_NONE if the suffix
  *                                  is embedded in the body argument.
  *
- * @return                      0 on success; BLE_HS_E... on failure.
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if advertising is in progress;
+ *                              BLE_HS_EMSGSIZE if the specified data is too
+ *                                  large to fit in an advertisement;
+ *                              Other nonzero on failure.
  */
 int
 ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields,
@@ -177,7 +186,7 @@ ble_eddystone_set_adv_data_url(struct ble_hs_adv_fields *adv_fields,
 
     svc_data = ble_eddystone_set_svc_data_base(BLE_EDDYSTONE_FRAME_TYPE_URL);
     
-    rc = ble_hci_util_read_adv_tx_pwr(&tx_pwr);
+    rc = ble_hs_hci_util_read_adv_tx_pwr(&tx_pwr);
     if (rc != 0) {
         return rc;
     }
diff --git a/net/nimble/host/src/ble_gap.c b/net/nimble/host/src/ble_gap.c
index 309b7cf1..f53a7385 100644
--- a/net/nimble/host/src/ble_gap.c
+++ b/net/nimble/host/src/ble_gap.c
@@ -23,7 +23,7 @@
 #include "bsp/bsp.h"
 #include "os/os.h"
 #include "nimble/nimble_opt.h"
-#include "host/host_hci.h"
+#include "host/ble_hs_adv.h"
 #include "ble_hs_priv.h"
 
 /**
@@ -65,18 +65,20 @@
 #define BLE_GAP_OP_S_ADV                                1
 
 /**
+ * If an attempt to cancel an active procedure fails, the attempt is retried
+ * at this rate (ms).
+ */
+#define BLE_GAP_CANCEL_RETRY_RATE                              100 /* ms */
+
+/**
  * The maximum amount of user data that can be put into the advertising data.
- * The stack may automatically insert some fields on its own, limiting the
- * maximum amount of user data.  The following fields are automatically
- * inserted:
- *     o Flags (3 bytes)
- *     o Tx-power-level (3 bytes) - Only if the application specified a
- *       tx_pwr_llvl_present value of 1 in a call to ble_gap_set_adv_data().
+ * The stack will automatically insert the flags field on its own if requested
+ * by the application, limiting the maximum amount of user data.
  */
-#define BLE_GAP_ADV_DATA_LIMIT_PWR      (BLE_HCI_MAX_ADV_DATA_LEN - 6)
-#define BLE_GAP_ADV_DATA_LIMIT_NO_PWR   (BLE_HCI_MAX_ADV_DATA_LEN - 3)
+#define BLE_GAP_ADV_DATA_LIMIT_FLAGS    (BLE_HCI_MAX_ADV_DATA_LEN - 3)
+#define BLE_GAP_ADV_DATA_LIMIT_NO_FLAGS BLE_HCI_MAX_ADV_DATA_LEN
 
-static const struct ble_gap_crt_params ble_gap_params_dflt = {
+static const struct ble_gap_conn_params ble_gap_conn_params_dflt = {
     .scan_itvl = 0x0010,
     .scan_window = 0x0010,
     .itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN,
@@ -87,41 +89,33 @@ static const struct ble_gap_crt_params ble_gap_params_dflt = {
     .max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN,
 };
 
-static const struct ble_gap_adv_params ble_gap_adv_params_dflt = {
-    .adv_itvl_min = 0,
-    .adv_itvl_max = 0,
-    .adv_type = BLE_HCI_ADV_TYPE_ADV_IND,
-    .own_addr_type = BLE_HCI_ADV_OWN_ADDR_PUBLIC,
-    .adv_channel_map = BLE_HCI_ADV_CHANMASK_DEF,
-    .adv_filter_policy = BLE_HCI_ADV_FILT_DEF,
-};
-
 /**
  * The state of the in-progress master connection.  If no master connection is
  * currently in progress, then the op field is set to BLE_GAP_OP_NULL.
  */
-static bssnz_t struct {
+struct ble_gap_master_state {
     uint8_t op;
 
-    unsigned exp_set:1;
+    uint8_t exp_set:1;
     os_time_t exp_os_ticks;
 
+    ble_gap_event_fn *cb;
+    void *cb_arg;
+
     union {
-        struct {
-            ble_gap_event_fn *cb;
-            void *cb_arg;
 
-            unsigned using_wl:1;
-            unsigned our_addr_type:2;
+        struct {
+            uint8_t using_wl:1;
+            uint8_t our_addr_type:2;
+            uint8_t cancel:1;
         } conn;
 
         struct {
-            uint8_t disc_mode;
-            ble_gap_disc_fn *cb;
-            void *cb_arg;
+            uint8_t limited:1;
         } disc;
     };
-} ble_gap_master;
+};
+static bssnz_t struct ble_gap_master_state ble_gap_master;
 
 /**
  * The state of the in-progress slave connection.  If no slave connection is
@@ -130,6 +124,9 @@ static bssnz_t struct {
 static bssnz_t struct {
     uint8_t op;
 
+    unsigned exp_set:1;
+    os_time_t exp_os_ticks;
+
     uint8_t conn_mode;
     uint8_t disc_mode;
     unsigned our_addr_type:2;
@@ -140,15 +137,16 @@ static bssnz_t struct {
     uint8_t rsp_data[BLE_HCI_MAX_ADV_DATA_LEN];
     uint8_t adv_data_len;
     uint8_t rsp_data_len;
-    int8_t tx_pwr_lvl;
 
-    unsigned adv_pwr_lvl:1;
+    unsigned adv_auto_flags:1;
 } ble_gap_slave;
 
-static int ble_gap_disc_tx_disable(void);
+static int ble_gap_adv_enable_tx(int enable);
+static int ble_gap_conn_cancel_tx(void);
+static int ble_gap_disc_enable_tx(int enable, int filter_duplicates);
 
 struct ble_gap_snapshot {
-    struct ble_gap_conn_desc desc;
+    struct ble_gap_conn_desc *desc;
     ble_gap_event_fn *cb;
     void *cb_arg;
 };
@@ -184,6 +182,8 @@ STATS_NAME_START(ble_gap_stats)
     STATS_NAME(ble_gap_stats, rx_conn_complete)
     STATS_NAME(ble_gap_stats, discover_cancel)
     STATS_NAME(ble_gap_stats, discover_cancel_fail)
+    STATS_NAME(ble_gap_stats, security_initiate)
+    STATS_NAME(ble_gap_stats, security_initiate_fail)
 STATS_NAME_END(ble_gap_stats)
 
 /*****************************************************************************
@@ -191,34 +191,49 @@ STATS_NAME_END(ble_gap_stats)
  *****************************************************************************/
 
 static void
-ble_gap_log_conn(uint8_t addr_type, uint8_t *addr,
-                 struct ble_gap_crt_params *params)
+ble_gap_log_duration(int32_t duration_ms)
+{
+    if (duration_ms == BLE_HS_FOREVER) {
+        BLE_HS_LOG(INFO, "duration=forever");
+    } else {
+        BLE_HS_LOG(INFO, "duration=%dms", duration_ms);
+    }
+}
+
+static void
+ble_gap_log_conn(uint8_t own_addr_type,
+                 uint8_t peer_addr_type, const uint8_t *peer_addr,
+                 const struct ble_gap_conn_params *params)
 {
-    BLE_HS_LOG(INFO, "addr_type=%d addr=", addr_type);
-    if (addr == NULL) {
+    BLE_HS_LOG(INFO, "peer_addr_type=%d peer_addr=", peer_addr_type);
+    if (peer_addr == NULL) {
         BLE_HS_LOG(INFO, "N/A");
     } else {
-        BLE_HS_LOG_ADDR(INFO, addr);
+        BLE_HS_LOG_ADDR(INFO, peer_addr);
     }
 
     BLE_HS_LOG(INFO, " scan_itvl=%d scan_window=%d itvl_min=%d itvl_max=%d "
                      "latency=%d supervision_timeout=%d min_ce_len=%d "
-                     "max_ce_len=%d our_addr_type=%d",
+                     "max_ce_len=%d own_addr_type=%d",
                params->scan_itvl, params->scan_window, params->itvl_min,
                params->itvl_max, params->latency, params->supervision_timeout,
-               params->min_ce_len, params->max_ce_len, params->our_addr_type);
+               params->min_ce_len, params->max_ce_len, own_addr_type);
 }
 
 static void
-ble_gap_log_disc(uint8_t scan_type, uint8_t filter_policy, uint8_t addr_mode)
+ble_gap_log_disc(uint8_t own_addr_type, int32_t duration_ms,
+                 const struct ble_gap_disc_params *disc_params)
 {
-    BLE_HS_LOG(INFO, "disc_mode=%d filter_policy=%d scan_type=%d addr_node %d",
-               ble_gap_master.disc.disc_mode,
-               filter_policy, scan_type, addr_mode);
+    BLE_HS_LOG(INFO, "own_addr_type=%d filter_policy=%d passive=%d limited=%d "
+                     "filter_duplicates=%d ",
+               own_addr_type, disc_params->filter_policy, disc_params->passive,
+               disc_params->limited, disc_params->filter_duplicates);
+    ble_gap_log_duration(duration_ms);
 }
 
 static void
-ble_gap_log_update(uint16_t conn_handle, struct ble_gap_upd_params *params)
+ble_gap_log_update(uint16_t conn_handle,
+                   const struct ble_gap_upd_params *params)
 {
     BLE_HS_LOG(INFO, "connection parameter update; "
                      "conn_handle=%d itvl_min=%d itvl_max=%d latency=%d "
@@ -229,10 +244,10 @@ ble_gap_log_update(uint16_t conn_handle, struct ble_gap_upd_params *params)
 }
 
 static void
-ble_gap_log_wl(struct ble_gap_white_entry *white_list,
+ble_gap_log_wl(const struct ble_gap_white_entry *white_list,
                uint8_t white_list_count)
 {
-    struct ble_gap_white_entry *entry;
+    const struct ble_gap_white_entry *entry;
     int i;
 
     BLE_HS_LOG(INFO, "count=%d ", white_list_count);
@@ -247,25 +262,25 @@ ble_gap_log_wl(struct ble_gap_white_entry *white_list,
 }
 
 static void
-ble_gap_log_adv(const struct ble_gap_adv_params *adv_params,
-                uint8_t *peer_addr, uint8_t peer_addr_type)
+ble_gap_log_adv(uint8_t own_addr_type, uint8_t peer_addr_type,
+                const uint8_t *peer_addr,
+                const struct ble_gap_adv_params *adv_params)
 {
-    BLE_HS_LOG(INFO, "disc_mode=%d addr_type=%d addr=",
+    BLE_HS_LOG(INFO, "disc_mode=%d peer_addr_type=%d peer_addr=",
                ble_gap_slave.disc_mode, peer_addr_type);
     if(peer_addr) {
         BLE_HS_LOG_ADDR(INFO, peer_addr);
     } else {
         BLE_HS_LOG(INFO, "none");
     }
-    BLE_HS_LOG(INFO, " adv_type=%d adv_channel_map=%d own_addr_type=%d "
+    BLE_HS_LOG(INFO, " adv_channel_map=%d own_addr_type=%d "
                      "adv_filter_policy=%d adv_itvl_min=%d adv_itvl_max=%d "
                      "adv_data_len=%d",
-               adv_params->adv_type,
-               adv_params->adv_channel_map,
-               adv_params->own_addr_type,
-               adv_params->adv_filter_policy,
-               adv_params->adv_itvl_min,
-               adv_params->adv_itvl_max,
+               adv_params->channel_map,
+               own_addr_type,
+               adv_params->filter_policy,
+               adv_params->itvl_min,
+               adv_params->itvl_max,
                ble_gap_slave.adv_data_len);
 }
 
@@ -294,14 +309,21 @@ ble_gap_fill_conn_desc(struct ble_hs_conn *conn,
     desc->conn_itvl = conn->bhc_itvl;
     desc->conn_latency = conn->bhc_latency;
     desc->supervision_timeout = conn->bhc_supervision_timeout;
+    desc->master_clock_accuracy = conn->bhc_master_clock_accuracy;
     desc->sec_state = conn->bhc_sec_state;
+
+    if (conn->bhc_flags & BLE_HS_CONN_F_MASTER) {
+        desc->role = BLE_GAP_ROLE_MASTER;
+    } else {
+        desc->role = BLE_GAP_ROLE_SLAVE;
+    }
 }
 
 static void
 ble_gap_conn_to_snapshot(struct ble_hs_conn *conn,
                          struct ble_gap_snapshot *snap)
 {
-    ble_gap_fill_conn_desc(conn, &snap->desc);
+    ble_gap_fill_conn_desc(conn, snap->desc);
     snap->cb = conn->bhc_cb;
     snap->cb_arg = conn->bhc_cb_arg;
 }
@@ -341,7 +363,7 @@ ble_gap_find_snapshot(uint16_t handle, struct ble_gap_snapshot *snap)
  *                                  connection was found.
  */
 int
-ble_gap_find_conn(uint16_t handle, struct ble_gap_conn_desc *out_desc)
+ble_gap_conn_find(uint16_t handle, struct ble_gap_conn_desc *out_desc)
 {
     struct ble_hs_conn *conn;
 
@@ -361,12 +383,40 @@ ble_gap_find_conn(uint16_t handle, struct ble_gap_conn_desc *out_desc)
     }
 }
 
+static int
+ble_gap_extract_conn_cb(uint16_t conn_handle,
+                        ble_gap_event_fn **out_cb, void **out_cb_arg)
+{
+    const struct ble_hs_conn *conn;
+
+    BLE_HS_DBG_ASSERT(conn_handle != 0);
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        *out_cb = conn->bhc_cb;
+        *out_cb_arg = conn->bhc_cb_arg;
+    } else {
+        *out_cb = NULL;
+        *out_cb_arg = NULL;
+    }
+
+    ble_hs_unlock();
+
+    if (conn == NULL) {
+        return BLE_HS_ENOTCONN;
+    } else {
+        return 0;
+    }
+}
+
 /*****************************************************************************
  * $misc                                                                     *
  *****************************************************************************/
 
 static int
-ble_gap_call_event_cb(int event, struct ble_gap_conn_ctxt *ctxt,
+ble_gap_call_event_cb(struct ble_gap_event *event,
                       ble_gap_event_fn *cb, void *cb_arg)
 {
     int rc;
@@ -374,12 +424,12 @@ ble_gap_call_event_cb(int event, struct ble_gap_conn_ctxt *ctxt,
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
 
     if (cb != NULL) {
-        rc = cb(event, ctxt, cb_arg);
+        rc = cb(event, cb_arg);
     } else {
-        if (event == BLE_GAP_EVENT_CONN_UPDATE_REQ) {
+        if (event->type == BLE_GAP_EVENT_CONN_UPDATE_REQ) {
             /* Just copy peer parameters back into the reply. */
-            *ctxt->conn_update_req.self_params =
-                *ctxt->conn_update_req.peer_params;
+            *event->conn_update_req.self_params =
+                *event->conn_update_req.peer_params;
         }
         rc = 0;
     }
@@ -387,34 +437,25 @@ ble_gap_call_event_cb(int event, struct ble_gap_conn_ctxt *ctxt,
     return rc;
 }
 
-static void
-ble_gap_slave_extract_cb(ble_gap_event_fn **out_cb, void **out_cb_arg)
-{
-    ble_hs_lock();
-
-    *out_cb = ble_gap_slave.cb;
-    *out_cb_arg = ble_gap_slave.cb_arg;
-    ble_gap_slave.op = BLE_GAP_OP_NULL;
 
-    ble_hs_unlock();
-}
-
-static void
-ble_gap_adv_finished(int event)
+static int
+ble_gap_call_conn_event_cb(struct ble_gap_event *event, uint16_t conn_handle)
 {
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_conn_desc desc;
     ble_gap_event_fn *cb;
     void *cb_arg;
+    int rc;
 
-    ble_gap_slave_extract_cb(&cb, &cb_arg);
-    if (cb != NULL) {
-        memset(&ctxt, 0, sizeof ctxt);
-        desc.conn_handle = BLE_HS_CONN_HANDLE_NONE;
-        ctxt.desc = &desc;
+    rc = ble_gap_extract_conn_cb(conn_handle, &cb, &cb_arg);
+    if (rc != 0) {
+        return rc;
+    }
 
-        cb(event, &ctxt, cb_arg);
+    rc = ble_gap_call_event_cb(event, cb, cb_arg);
+    if (rc != 0) {
+        return rc;
     }
+
+    return 0;
 }
 
 static void
@@ -422,63 +463,73 @@ ble_gap_master_reset_state(void)
 {
     ble_gap_master.op = BLE_GAP_OP_NULL;
     ble_gap_master.exp_set = 0;
+    ble_gap_master.conn.cancel = 0;
+}
+
+static void
+ble_gap_slave_reset_state(void)
+{
+    ble_gap_slave.op = BLE_GAP_OP_NULL;
+    ble_gap_slave.exp_set = 0;
 }
 
 static void
-ble_gap_master_extract_cb(ble_gap_event_fn **out_cb, void **out_cb_arg)
+ble_gap_master_extract_state(struct ble_gap_master_state *out_state,
+                             int reset_state)
 {
     ble_hs_lock();
 
-    *out_cb = ble_gap_master.conn.cb;
-    *out_cb_arg = ble_gap_master.conn.cb_arg;
-    ble_gap_master_reset_state();
+    *out_state = ble_gap_master;
+
+    if (reset_state) {
+        ble_gap_master_reset_state();
+    }
 
     ble_hs_unlock();
 }
 
-static int
-ble_gap_master_connect_failure(int status)
+static void
+ble_gap_slave_extract_cb(ble_gap_event_fn **out_cb, void **out_cb_arg)
 {
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_conn_desc desc;
+    ble_hs_lock();
+
+    *out_cb = ble_gap_slave.cb;
+    *out_cb_arg = ble_gap_slave.cb_arg;
+    ble_gap_slave_reset_state();
+
+    ble_hs_unlock();
+}
+
+static void
+ble_gap_adv_finished(void)
+{
+    struct ble_gap_event event;
     ble_gap_event_fn *cb;
     void *cb_arg;
-    int rc;
-
-    memset(&desc, 0, sizeof ctxt);
 
-    ble_gap_master_extract_cb(&cb, &cb_arg);
+    ble_gap_slave_extract_cb(&cb, &cb_arg);
     if (cb != NULL) {
-        memset(&ctxt, 0, sizeof ctxt);
-        desc.conn_handle = BLE_HS_CONN_HANDLE_NONE;
-        ctxt.desc = &desc;
-        ctxt.connect.status = status;
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_ADV_COMPLETE;
 
-        rc = cb(BLE_GAP_EVENT_CONNECT, &ctxt, cb_arg);
-    } else {
-        rc = 0;
+        cb(&event, cb_arg);
     }
-
-    return rc;
 }
 
 static int
-ble_gap_master_connect_cancel(void)
+ble_gap_master_connect_failure(int status)
 {
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_conn_desc desc;
-    ble_gap_event_fn *cb;
-    void *cb_arg;
+    struct ble_gap_master_state state;
+    struct ble_gap_event event;
     int rc;
 
-    memset(&desc, 0, sizeof ctxt);
-    desc.conn_handle = BLE_HS_CONN_HANDLE_NONE;
+    ble_gap_master_extract_state(&state, 1);
+    if (state.cb != NULL) {
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_CONNECT;
+        event.connect.status = status;
 
-    ble_gap_master_extract_cb(&cb, &cb_arg);
-    if (cb != NULL) {
-        memset(&ctxt, 0, sizeof ctxt);
-        ctxt.desc = &desc;
-        rc = cb(BLE_GAP_EVENT_CONN_CANCEL, &ctxt, cb_arg);
+        rc = state.cb(&event, state.cb_arg);
     } else {
         rc = 0;
     }
@@ -487,58 +538,128 @@ ble_gap_master_connect_cancel(void)
 }
 
 static void
-ble_gap_call_master_disc_cb(int event, int status, struct ble_hs_adv *adv,
-                            struct ble_hs_adv_fields *fields, int reset_state)
+ble_gap_master_connect_cancelled(void)
 {
-    struct ble_gap_disc_desc desc;
-    ble_gap_disc_fn *cb;
-    void *cb_arg;
+    struct ble_gap_master_state state;
+    struct ble_gap_event event;
 
-    ble_hs_lock();
+    ble_gap_master_extract_state(&state, 1);
+    if (state.cb != NULL) {
+        /* The GAP event type depends on whether 1) the application manually
+         * cancelled the connect procedure or 2) the connect procedure timed
+         * out.
+         */
+        memset(&event, 0, sizeof event);
+        if (state.conn.cancel) {
+            event.type = BLE_GAP_EVENT_CONN_CANCEL;
+        } else {
+            event.type = BLE_GAP_EVENT_CONNECT;
+            event.connect.status = BLE_HS_ETIMEOUT;
+            event.connect.conn_handle = BLE_HS_CONN_HANDLE_NONE;
 
-    if (adv != NULL) {
-        desc.event_type = adv->event_type;
-        desc.addr_type = adv->addr_type;
-        desc.length_data = adv->length_data;
-        desc.rssi = adv->rssi;
-        memcpy(desc.addr, adv->addr, sizeof adv->addr);
-        desc.data = adv->data;
-        desc.fields = fields;
-    } else {
-        memset(&desc, 0, sizeof desc);
+        }
+        state.cb(&event, state.cb_arg);
     }
+}
 
-    cb = ble_gap_master.disc.cb;
-    cb_arg = ble_gap_master.disc.cb_arg;
+static void
+ble_gap_disc_report(struct ble_gap_disc_desc *desc)
+{
+    struct ble_gap_master_state state;
+    struct ble_gap_event event;
 
-    if (reset_state) {
-        ble_gap_master_reset_state();
+    ble_gap_master_extract_state(&state, 0);
+
+    if (state.cb != NULL) {
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_DISC;
+        event.disc = *desc;
+
+        state.cb(&event, state.cb_arg);
     }
+}
 
-    ble_hs_unlock();
+static void
+ble_gap_disc_complete(void)
+{
+    struct ble_gap_master_state state;
+    struct ble_gap_event event;
 
-    if (cb != NULL) {
-        cb(event, status, &desc, cb_arg);
+    ble_gap_master_extract_state(&state, 1);
+
+    if (state.cb != NULL) {
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_DISC_COMPLETE;
+
+        ble_gap_call_event_cb(&event, state.cb, state.cb_arg);
     }
 }
 
 static void
 ble_gap_update_notify(uint16_t conn_handle, int status)
 {
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
-    int rc;
+    struct ble_gap_event event;
 
-    rc = ble_gap_find_snapshot(conn_handle, &snap);
-    if (rc != 0) {
-        return;
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_CONN_UPDATE;
+    event.conn_update.conn_handle = conn_handle;
+    event.conn_update.status = status;
+
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+}
+
+static uint32_t
+ble_gap_master_ticks_until_exp(void)
+{
+    int32_t ticks;
+
+    if (ble_gap_master.op == BLE_GAP_OP_NULL || !ble_gap_master.exp_set) {
+        /* Timer not set; infinity ticks until next event. */
+        return BLE_HS_FOREVER;
     }
 
-    memset(&ctxt, 0, sizeof ctxt);
-    ctxt.desc = &snap.desc;
-    ctxt.conn_update.status = status;
-    ble_gap_call_event_cb(BLE_GAP_EVENT_CONN_UPDATE, &ctxt,
-                          snap.cb, snap.cb_arg);
+    ticks = ble_gap_master.exp_os_ticks - os_time_get();
+    if (ticks > 0) {
+        /* Timer not expired yet. */
+        return ticks;
+    }
+
+    /* Timer just expired. */
+    return 0;
+}
+
+static uint32_t
+ble_gap_slave_ticks_until_exp(void)
+{
+    int32_t ticks;
+
+    if (ble_gap_slave.op == BLE_GAP_OP_NULL || !ble_gap_slave.exp_set) {
+        /* Timer not set; infinity ticks until next event. */
+        return BLE_HS_FOREVER;
+    }
+
+    ticks = ble_gap_slave.exp_os_ticks - os_time_get();
+    if (ticks > 0) {
+        /* Timer not expired yet. */
+        return ticks;
+    }
+
+    /* Timer just expired. */
+    return 0;
+}
+
+static void
+ble_gap_heartbeat_sched(void)
+{
+    int32_t mst_ticks;
+    int32_t slv_ticks;
+    int32_t ticks;
+
+    mst_ticks = ble_gap_master_ticks_until_exp();
+    slv_ticks = ble_gap_slave_ticks_until_exp();
+    ticks = min(mst_ticks, slv_ticks);
+
+    ble_hs_heartbeat_sched(ticks);
 }
 
 static void
@@ -546,29 +667,34 @@ ble_gap_master_set_timer(uint32_t ticks_from_now)
 {
     ble_gap_master.exp_os_ticks = os_time_get() + ticks_from_now;
     ble_gap_master.exp_set = 1;
+
+    ble_gap_heartbeat_sched();
+}
+
+static void
+ble_gap_slave_set_timer(uint32_t ticks_from_now)
+{
+    ble_gap_slave.exp_os_ticks = os_time_get() + ticks_from_now;
+    ble_gap_slave.exp_set = 1;
+
+    ble_gap_heartbeat_sched();
 }
 
 /**
  * Called when an error is encountered while the master-connection-fsm is
- * active.  Resets the state machine, clears the HCI ack callback, and notifies
- * the host task that the next hci_batch item can be processed.
+ * active.
  */
 static void
 ble_gap_master_failed(int status)
 {
     switch (ble_gap_master.op) {
-    case BLE_GAP_OP_M_DISC:
-        STATS_INC(ble_gap_stats, discover_fail);
-        ble_gap_call_master_disc_cb(BLE_GAP_EVENT_DISC_COMPLETE, status,
-                                    NULL, NULL, 1);
-        break;
-
     case BLE_GAP_OP_M_CONN:
         STATS_INC(ble_gap_stats, initiate_fail);
         ble_gap_master_connect_failure(status);
         break;
 
     default:
+        BLE_HS_DBG_ASSERT(0);
         break;
     }
 }
@@ -581,25 +707,35 @@ ble_gap_update_failed(uint16_t conn_handle, int status)
     ble_gap_update_notify(conn_handle, status);
 }
 
-static void
-ble_gap_conn_broken(struct ble_gap_snapshot *snap, int reason)
+void
+ble_gap_conn_broken(uint16_t conn_handle, int reason)
 {
-    struct ble_gap_conn_ctxt ctxt;
+    struct ble_gap_snapshot snap;
+    struct ble_gap_event event;
+    int rc;
 
-    /* XXX: Consider removing the connection from the list and handing it to
-     * each fo the "connection_broken" functions below.
-     */
+    memset(&event, 0, sizeof event);
+    snap.desc = &event.disconnect.conn;
+
+    rc = ble_gap_find_snapshot(conn_handle, &snap);
+    if (rc != 0) {
+        /* No longer connected. */
+        return;
+    }
 
-    ble_sm_connection_broken(snap->desc.conn_handle);
-    ble_gattc_connection_broken(snap->desc.conn_handle);
+    /* Indicate the connection termination to each module.  The order matters
+     * here: gatts must come before gattc to ensure the application does not
+     * get informed of spurious notify-tx events.
+     */
+    ble_sm_connection_broken(conn_handle);
+    ble_gatts_connection_broken(conn_handle);
+    ble_gattc_connection_broken(conn_handle);
 
-    ble_hs_atomic_conn_delete(snap->desc.conn_handle);
+    ble_hs_atomic_conn_delete(conn_handle);
 
-    memset(&ctxt, 0, sizeof ctxt);
-    ctxt.desc = &snap->desc;
-    ctxt.disconnect.reason = reason;
-    ble_gap_call_event_cb(BLE_GAP_EVENT_DISCONNECT, &ctxt,
-                          snap->cb, snap->cb_arg);
+    event.type = BLE_GAP_EVENT_DISCONNECT;
+    event.disconnect.reason = reason;
+    ble_gap_call_event_cb(&event, snap.cb, snap.cb_arg);
 
     STATS_INC(ble_gap_stats, disconnect);
 }
@@ -611,28 +747,19 @@ ble_gap_rx_disconn_complete(struct hci_disconn_complete *evt)
     return;
 #endif
 
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
-    int status;
-    int rc;
+    struct ble_gap_event event;
 
     STATS_INC(ble_gap_stats, rx_disconnect);
 
-    rc = ble_gap_find_snapshot(evt->connection_handle, &snap);
-    if (rc != 0) {
-        /* No longer connected. */
-        return;
-    }
-
     if (evt->status == 0) {
-        status = BLE_HS_HCI_ERR(evt->reason);
-        ble_gap_conn_broken(&snap, status);
+        ble_gap_conn_broken(evt->connection_handle,
+                            BLE_HS_HCI_ERR(evt->reason));
     } else {
-        memset(&ctxt, 0, sizeof ctxt);
-        ctxt.desc = &snap.desc;
-        ctxt.term_failure.status = BLE_HS_HCI_ERR(evt->status);
-        ble_gap_call_event_cb(BLE_GAP_EVENT_TERM_FAILURE, &ctxt,
-                              snap.cb, snap.cb_arg);
+        memset(&event, 0, sizeof event);
+        event.type = BLE_GAP_EVENT_TERM_FAILURE;
+        event.term_failure.conn_handle = evt->connection_handle;
+        event.term_failure.status = BLE_HS_HCI_ERR(evt->status);
+        ble_gap_call_conn_event_cb(&event, evt->connection_handle);
     }
 }
 
@@ -643,12 +770,13 @@ ble_gap_rx_update_complete(struct hci_le_conn_upd_complete *evt)
     return;
 #endif
 
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
+    struct ble_gap_event event;
     struct ble_hs_conn *conn;
 
     STATS_INC(ble_gap_stats, rx_update_complete);
 
+    memset(&event, 0, sizeof event);
+
     ble_hs_lock();
 
     conn = ble_hs_conn_find(evt->connection_handle);
@@ -658,25 +786,20 @@ ble_gap_rx_update_complete(struct hci_le_conn_upd_complete *evt)
             conn->bhc_latency = evt->conn_latency;
             conn->bhc_supervision_timeout = evt->supervision_timeout;
         }
-
-        ble_gap_conn_to_snapshot(conn, &snap);
     }
 
     conn->bhc_flags &= ~BLE_HS_CONN_F_UPDATE;
 
     ble_hs_unlock();
 
-    if (conn != NULL) {
-        memset(&ctxt, 0, sizeof ctxt);
-        ctxt.desc = &snap.desc;
-        ctxt.conn_update.status = BLE_HS_HCI_ERR(evt->status);
-        ble_gap_call_event_cb(BLE_GAP_EVENT_CONN_UPDATE, &ctxt,
-                              snap.cb, snap.cb_arg);
-    }
+    event.type = BLE_GAP_EVENT_CONN_UPDATE;
+    event.conn_update.conn_handle = evt->connection_handle;
+    event.conn_update.status = BLE_HS_HCI_ERR(evt->status);
+    ble_gap_call_conn_event_cb(&event, evt->connection_handle);
 }
 
 /**
- * Tells you if the BLE host is in the process of creating a master connection.
+ * Tells you if there is an active central GAP procedure (connect or discover).
  */
 int
 ble_gap_master_in_progress(void)
@@ -685,21 +808,6 @@ ble_gap_master_in_progress(void)
 }
 
 /**
- * Tells you if the BLE host is in the process of creating a slave connection.
- */
-int
-ble_gap_slave_in_progress(void)
-{
-    return ble_gap_slave.op != BLE_GAP_OP_NULL;
-}
-
-static int
-ble_gap_currently_advertising(void)
-{
-    return ble_gap_slave.op == BLE_GAP_OP_S_ADV;
-}
-
-/**
  * Attempts to complete the master connection process in response to a
  * "connection complete" event from the controller.  If the master connection
  * FSM is in a state that can accept this event, and the peer device address is
@@ -764,7 +872,7 @@ ble_gap_accept_slave_conn(uint8_t addr_type, uint8_t *addr)
 {
     int rc;
 
-    if (!ble_gap_currently_advertising()) {
+    if (!ble_gap_adv_active()) {
         rc = BLE_HS_ENOENT;
     } else {
         switch (ble_gap_slave.conn_mode) {
@@ -795,7 +903,7 @@ ble_gap_accept_slave_conn(uint8_t addr_type, uint8_t *addr)
 }
 
 void
-ble_gap_rx_adv_report(struct ble_hs_adv *adv)
+ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc)
 {
 #if !NIMBLE_OPT(ROLE_OBSERVER)
     return;
@@ -810,20 +918,23 @@ ble_gap_rx_adv_report(struct ble_hs_adv *adv)
         return;
     }
 
-    rc = ble_hs_adv_parse_fields(&fields, adv->data, adv->length_data);
+    rc = ble_hs_adv_parse_fields(&fields, desc->data, desc->length_data);
     if (rc != 0) {
         /* XXX: Increment stat. */
         return;
     }
 
-    if (ble_gap_master.disc.disc_mode == BLE_GAP_DISC_MODE_LTD &&
+    /* If a limited discovery procedure is active, discard non-limited
+     * advertisements.
+     */
+    if (ble_gap_master.disc.limited &&
         !(fields.flags & BLE_HS_ADV_F_DISC_LTD)) {
 
         return;
     }
 
-    ble_gap_call_master_disc_cb(BLE_GAP_EVENT_DISC_SUCCESS, 0, adv,
-                                &fields, 0);
+    desc->fields = &fields;
+    ble_gap_disc_report(desc);
 }
 
 /**
@@ -836,25 +947,21 @@ ble_gap_rx_conn_complete(struct hci_le_conn_complete *evt)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
+    struct ble_gap_event event;
     struct ble_hs_conn *conn;
-    struct ble_gap_enhanced_conn enhanced_conn;
     int rc;
 
     STATS_INC(ble_gap_stats, rx_conn_complete);
 
-    /* Determine if this event refers to a completed connection or a connection
-     * in progress.
-     */
-    rc = ble_gap_find_snapshot(evt->connection_handle, &snap);
-
     /* Apply the event to the existing connection if it exists. */
-    if (rc == 0) {
+    if (evt->status != BLE_ERR_UNK_CONN_ID &&
+        ble_hs_atomic_conn_flags(evt->connection_handle, NULL) == 0) {
+
         /* XXX: Does this ever happen? */
 
         if (evt->status != 0) {
-            ble_gap_conn_broken(&snap, BLE_HS_HCI_ERR(evt->status));
+            ble_gap_conn_broken(evt->connection_handle,
+                                BLE_HS_HCI_ERR(evt->status));
         }
         return 0;
     }
@@ -865,8 +972,8 @@ ble_gap_rx_conn_complete(struct hci_le_conn_complete *evt)
         /* Determine the role from the status code. */
         switch (evt->status) {
         case BLE_ERR_DIR_ADV_TMO:
-            if (ble_gap_slave_in_progress()) {
-                ble_gap_adv_finished(BLE_GAP_EVENT_ADV_COMPLETE);
+            if (ble_gap_adv_active()) {
+                ble_gap_adv_finished();
             }
             break;
 
@@ -874,7 +981,7 @@ ble_gap_rx_conn_complete(struct hci_le_conn_complete *evt)
             if (ble_gap_master_in_progress()) {
                 if (evt->status == BLE_ERR_UNK_CONN_ID) {
                     /* Connect procedure successfully cancelled. */
-                    ble_gap_master_connect_cancel();
+                    ble_gap_master_connect_cancelled();
                 } else {
                     ble_gap_master_failed(BLE_HS_HCI_ERR(evt->status));
                 }
@@ -910,40 +1017,43 @@ ble_gap_rx_conn_complete(struct hci_le_conn_complete *evt)
     BLE_HS_DBG_ASSERT(conn != NULL);
 
     conn->bhc_handle = evt->connection_handle;
-    memcpy(conn->bhc_addr, evt->peer_addr, sizeof conn->bhc_addr);
-    conn->bhc_addr_type = evt->peer_addr_type;
-    memcpy(conn->our_rpa_addr, evt->local_rpa, sizeof(conn->our_rpa_addr));
-    memcpy(conn->peer_rpa_addr, evt->peer_rpa, sizeof(conn->peer_rpa_addr));
+    memcpy(conn->bhc_peer_addr, evt->peer_addr, sizeof conn->bhc_peer_addr);
+    conn->bhc_peer_addr_type = evt->peer_addr_type;
+    memcpy(conn->bhc_our_rpa_addr, evt->local_rpa,
+           sizeof conn->bhc_our_rpa_addr);
+    memcpy(conn->bhc_peer_rpa_addr, evt->peer_rpa,
+           sizeof conn->bhc_peer_rpa_addr);
     conn->bhc_itvl = evt->conn_itvl;
     conn->bhc_latency = evt->conn_latency;
     conn->bhc_supervision_timeout = evt->supervision_timeout;
+    conn->bhc_master_clock_accuracy = evt->master_clk_acc;
     if (evt->role == BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER) {
+        conn->bhc_cb = ble_gap_master.cb;
+        conn->bhc_cb_arg = ble_gap_master.cb_arg;
         conn->bhc_flags |= BLE_HS_CONN_F_MASTER;
-        conn->bhc_cb = ble_gap_master.conn.cb;
-        conn->our_addr_type = ble_gap_master.conn.our_addr_type;
-        conn->bhc_cb_arg = ble_gap_master.conn.cb_arg;
+        conn->bhc_our_addr_type = ble_gap_master.conn.our_addr_type;
         ble_gap_master_reset_state();
     } else {
         conn->bhc_cb = ble_gap_slave.cb;
         conn->bhc_cb_arg = ble_gap_slave.cb_arg;
-        conn->our_addr_type = ble_gap_slave.our_addr_type;
-        ble_gap_slave.op = BLE_GAP_OP_NULL;
+        conn->bhc_our_addr_type = ble_gap_slave.our_addr_type;
+        ble_gap_slave_reset_state();
     }
 
-    memcpy(conn->our_rpa_addr, evt->local_rpa, 6);
-    memcpy(conn->peer_rpa_addr, evt->peer_rpa, 6);
+    memcpy(conn->bhc_our_rpa_addr, evt->local_rpa, 6);
+    memcpy(conn->bhc_peer_rpa_addr, evt->peer_rpa, 6);
 
-    ble_gap_conn_to_snapshot(conn, &snap);
+    ble_hs_lock();
+
+    memset(&event, 0, sizeof event);
+    ble_hs_conn_insert(conn);
 
-    ble_hs_atomic_conn_insert(conn);
+    ble_hs_unlock();
 
-    memset(&ctxt, 0, sizeof ctxt);
-    ctxt.desc = &snap.desc;
-    memcpy(enhanced_conn.local_rpa, evt->local_rpa,6);
-    memcpy(enhanced_conn.peer_rpa, evt->peer_rpa,6);
-    ctxt.connect.enhanced_conn = &enhanced_conn;
-    ctxt.connect.status = 0;
-    ble_gap_call_event_cb(BLE_GAP_EVENT_CONNECT, &ctxt, snap.cb, snap.cb_arg);
+    event.type = BLE_GAP_EVENT_CONNECT;
+    event.connect.conn_handle = evt->connection_handle;
+    event.connect.status = 0;
+    ble_gap_call_conn_event_cb(&event, evt->connection_handle);
 
     return 0;
 }
@@ -952,62 +1062,77 @@ int
 ble_gap_rx_l2cap_update_req(uint16_t conn_handle,
                             struct ble_gap_upd_params *params)
 {
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
+    struct ble_gap_event event;
     int rc;
 
-    rc = ble_gap_find_snapshot(conn_handle, &snap);
-    if (rc != 0) {
-        return rc;
-    }
-
-    if (snap.cb != NULL) {
-        memset(&ctxt, 0, sizeof ctxt);
-        ctxt.desc = &snap.desc;
-        ctxt.conn_update_req.peer_params = params;
-        rc = snap.cb(BLE_GAP_EVENT_L2CAP_UPDATE_REQ, &ctxt, snap.cb_arg);
-    } else {
-        rc = 0;
-    }
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_L2CAP_UPDATE_REQ;
+    event.conn_update_req.conn_handle = conn_handle;
+    event.conn_update_req.peer_params = params;
 
+    rc = ble_gap_call_conn_event_cb(&event, conn_handle);
     return rc;
 }
 
-static uint32_t
-ble_gap_master_ticks_until_exp(void)
+static int32_t
+ble_gap_master_heartbeat(void)
 {
-    int32_t ticks;
+    uint32_t ticks_until_exp;
+    int rc;
 
-    if (ble_gap_master.op == BLE_GAP_OP_NULL || !ble_gap_master.exp_set) {
-        /* Timer not set; infinity ticks until next event. */
-        return UINT32_MAX;
+    ticks_until_exp = ble_gap_master_ticks_until_exp();
+    if (ticks_until_exp != 0) {
+        /* Timer not expired yet. */
+        return ticks_until_exp;
     }
 
-    ticks = ble_gap_master.exp_os_ticks - os_time_get();
-    if (ticks > 0) {
-        /* Timer not expired yet. */
-        return ticks;
+    /*** Timer expired; process event. */
+
+    switch (ble_gap_master.op) {
+    case BLE_GAP_OP_M_CONN:
+        rc = ble_gap_conn_cancel_tx();
+        if (rc != 0) {
+            /* Failed to stop connecting; try again in 100 ms. */
+            return BLE_GAP_CANCEL_RETRY_RATE;
+        } else {
+            /* Stop the timer now that the cancel command has been acked. */
+            ble_gap_master.exp_set = 0;
+
+            /* Timeout gets reported when we receive a connection complete
+             * event indicating the connect procedure has been cancelled.
+             */
+            /* XXX: Set a timer to reset the controller if a connection
+             * complete event isn't received within a reasonable interval.
+             */
+        }
+        break;
+
+    case BLE_GAP_OP_M_DISC:
+        /* When a discovery procedure times out, it is not a failure. */
+        rc = ble_gap_disc_enable_tx(0, 0);
+        if (rc != 0) {
+            /* Failed to stop discovery; try again in 100 ms. */
+            return BLE_GAP_CANCEL_RETRY_RATE;
+        }
+
+        ble_gap_disc_complete();
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
     }
 
-    /* Timer just expired. */
-    return 0;
+    return BLE_HS_FOREVER;
 }
 
-/**
- * Handles timed-out master procedures.
- *
- * Called by the heartbeat timer; executed at least once a second.
- *
- * @return                      The number of ticks until this function should
- *                                  be called again.
- */
-uint32_t
-ble_gap_heartbeat(void)
+static int32_t
+ble_gap_slave_heartbeat(void)
 {
     uint32_t ticks_until_exp;
     int rc;
 
-    ticks_until_exp = ble_gap_master_ticks_until_exp();
+    ticks_until_exp = ble_gap_slave_ticks_until_exp();
     if (ticks_until_exp != 0) {
         /* Timer not expired yet. */
         return ticks_until_exp;
@@ -1015,23 +1140,40 @@ ble_gap_heartbeat(void)
 
     /*** Timer expired; process event. */
 
-    /* Clear the timer. */
-    ble_gap_master.exp_set = 0;
+    /* Stop advertising. */
+    rc = ble_gap_adv_enable_tx(0);
+    if (rc != 0) {
+        /* Failed to stop advertising; try again in 100 ms. */
+        return 100;
+    }
 
-    switch (ble_gap_master.op) {
-    case BLE_GAP_OP_M_DISC:
-        /* When a discovery procedure times out, it is not a failure. */
-        rc = ble_gap_disc_tx_disable();
-        ble_gap_call_master_disc_cb(BLE_GAP_EVENT_DISC_COMPLETE, rc,
-                                    NULL, NULL, 1);
-        break;
+    /* Clear the timer and cancel the current procedure. */
+    ble_gap_slave_reset_state();
 
-    default:
-        ble_gap_master_failed(BLE_HS_ETIMEOUT);
-        break;
-    }
+    /* Indicate to application that advertising has stopped. */
+    ble_gap_adv_finished();
+
+    return BLE_HS_FOREVER;
+}
 
-    return UINT32_MAX;
+/**
+ * Handles timed-out master procedures.
+ *
+ * Called by the heartbeat timer; executed at least once a second.
+ *
+ * @return                      The number of ticks until this function should
+ *                                  be called again.
+ */
+int32_t
+ble_gap_heartbeat(void)
+{
+    int32_t master_ticks;
+    int32_t slave_ticks;
+
+    master_ticks = ble_gap_master_heartbeat();
+    slave_ticks = ble_gap_slave_heartbeat();
+
+    return min(master_ticks, slave_ticks);
 }
 
 /*****************************************************************************
@@ -1053,18 +1195,18 @@ ble_gap_wl_busy(void)
 }
 
 static int
-ble_gap_wl_tx_add(struct ble_gap_white_entry *entry)
+ble_gap_wl_tx_add(const struct ble_gap_white_entry *entry)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_CHG_WHITE_LIST_LEN];
     int rc;
 
-    rc = host_hci_cmd_build_le_add_to_whitelist(entry->addr, entry->addr_type,
-                                                buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_add_to_whitelist(
+        entry->addr, entry->addr_type, buf, sizeof buf);
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1078,8 +1220,8 @@ ble_gap_wl_tx_clear(void)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN];
     int rc;
 
-    host_hci_cmd_build_le_clear_whitelist(buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_clear_whitelist(buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1087,8 +1229,16 @@ ble_gap_wl_tx_clear(void)
     return 0;
 }
 
+/**
+ * Overwrites the controller's white list with the specified contents.
+ *
+ * @param white_list            The entries to write to the white list.
+ * @param white_list_count      The number of entries in the white list.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
 int
-ble_gap_wl_set(struct ble_gap_white_entry *white_list,
+ble_gap_wl_set(const struct ble_gap_white_entry *white_list,
                uint8_t white_list_count)
 {
 #if !NIMBLE_OPT(WHITELIST)
@@ -1100,9 +1250,11 @@ ble_gap_wl_set(struct ble_gap_white_entry *white_list,
 
     STATS_INC(ble_gap_stats, wl_set);
 
+    ble_hs_lock();
+
     if (white_list_count == 0) {
         rc = BLE_HS_EINVAL;
-        goto err;
+        goto done;
     }
 
     for (i = 0; i < white_list_count; i++) {
@@ -1110,13 +1262,13 @@ ble_gap_wl_set(struct ble_gap_white_entry *white_list,
             white_list[i].addr_type != BLE_ADDR_TYPE_RANDOM) {
 
             rc = BLE_HS_EINVAL;
-            goto err;
+            goto done;
         }
     }
 
     if (ble_gap_wl_busy()) {
         rc = BLE_HS_EBUSY;
-        goto err;
+        goto done;
     }
 
     BLE_HS_LOG(INFO, "GAP procedure initiated: set whitelist; ");
@@ -1125,20 +1277,24 @@ ble_gap_wl_set(struct ble_gap_white_entry *white_list,
 
     rc = ble_gap_wl_tx_clear();
     if (rc != 0) {
-        goto err;
+        goto done;
     }
 
     for (i = 0; i < white_list_count; i++) {
         rc = ble_gap_wl_tx_add(white_list + i);
         if (rc != 0) {
-            goto err;
+            goto done;
         }
     }
 
-    return 0;
+    rc = 0;
 
-err:
-    STATS_INC(ble_gap_stats, wl_set_fail);
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, wl_set_fail);
+    }
     return rc;
 }
 
@@ -1147,13 +1303,13 @@ err:
  *****************************************************************************/
 
 static int
-ble_gap_adv_disable_tx(void)
+ble_gap_adv_enable_tx(int enable)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_ENABLE_LEN];
     int rc;
 
-    host_hci_cmd_build_le_set_adv_enable(0, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_set_adv_enable(!!enable, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1161,6 +1317,16 @@ ble_gap_adv_disable_tx(void)
     return 0;
 }
 
+/**
+ * Stops the currently-active advertising procedure.  A success return
+ * code indicates that advertising has been fully aborted; a new advertising
+ * procedure can be initiated immediately.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EALREADY if there is no active
+ *                                  advertising procedure;
+ *                              Other nonzero on error.
+ */
 int
 ble_gap_adv_stop(void)
 {
@@ -1172,25 +1338,31 @@ ble_gap_adv_stop(void)
 
     STATS_INC(ble_gap_stats, adv_stop);
 
+    ble_hs_lock();
+
     /* Do nothing if advertising is already disabled. */
-    if (!ble_gap_currently_advertising()) {
+    if (!ble_gap_adv_active()) {
         rc = BLE_HS_EALREADY;
-        goto err;
+        goto done;
     }
 
     BLE_HS_LOG(INFO, "GAP procedure initiated: stop advertising.\n");
 
-    rc = ble_gap_adv_disable_tx();
+    rc = ble_gap_adv_enable_tx(0);
     if (rc != 0) {
-        goto err;
+        goto done;
     }
 
-    ble_gap_slave.op = BLE_GAP_OP_NULL;
+    ble_gap_slave_reset_state();
 
-    return 0;
+    rc = 0;
 
-err:
-    STATS_INC(ble_gap_stats, adv_set_fields_fail);
+done:
+    ble_hs_unlock();
+
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, adv_set_fields_fail);
+    }
     return rc;
 }
 
@@ -1198,62 +1370,20 @@ err:
  * $advertise                                                                *
  *****************************************************************************/
 
-static void
-ble_gap_adv_itvls(uint8_t disc_mode, uint8_t conn_mode,
-                  uint16_t *out_itvl_min, uint16_t *out_itvl_max)
-{
-    switch (conn_mode) {
-    case BLE_GAP_CONN_MODE_NON:
-        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL2_MIN;
-        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL2_MAX;
-        break;
-
-    case BLE_GAP_CONN_MODE_UND:
-        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
-        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
-        break;
-
-    case BLE_GAP_CONN_MODE_DIR:
-        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
-        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
-        break;
-
-    default:
-        BLE_HS_DBG_ASSERT(0);
-        break;
-    }
-}
-
-static int
-ble_gap_adv_enable_tx(void)
-{
-    uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_PARAM_LEN];
-    int rc;
-
-    host_hci_cmd_build_le_set_adv_enable(1, buf, sizeof buf);
-
-    rc = ble_hci_cmd_tx_empty_ack(buf);
-    if (rc != 0) {
-        return rc;
-    }
-
-    return 0;
-}
-
 static int
 ble_gap_adv_rsp_data_tx(void)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_RSP_DATA_LEN];
     int rc;
 
-    rc = host_hci_cmd_build_le_set_scan_rsp_data(ble_gap_slave.rsp_data,
-                                                 ble_gap_slave.rsp_data_len,
-                                                 buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_set_scan_rsp_data(ble_gap_slave.rsp_data,
+                                                   ble_gap_slave.rsp_data_len,
+                                                   buf, sizeof buf);
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1261,11 +1391,9 @@ ble_gap_adv_rsp_data_tx(void)
     return 0;
 }
 
-static int
-ble_gap_adv_data_tx(void)
+static void
+ble_gap_adv_data_set_flags(void)
 {
-    uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_DATA_LEN];
-    uint8_t adv_data_len;
     uint8_t flags;
     int rc;
 
@@ -1290,31 +1418,38 @@ ble_gap_adv_data_tx(void)
 
     flags |= BLE_HS_ADV_F_BREDR_UNSUP;
 
-    /* Encode the flags AD field if it is nonzero. */
-    adv_data_len = ble_gap_slave.adv_data_len;
     if (flags != 0) {
         rc = ble_hs_adv_set_flat(BLE_HS_ADV_TYPE_FLAGS, 1, &flags,
-                                 ble_gap_slave.adv_data, &adv_data_len,
+                                 ble_gap_slave.adv_data,
+                                 &ble_gap_slave.adv_data_len,
                                  BLE_HCI_MAX_ADV_DATA_LEN);
-        BLE_HS_DBG_ASSERT(rc == 0);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
     }
+}
 
-    /* Encode the transmit power AD field. */
-    if (ble_gap_slave.adv_pwr_lvl) {
-        rc = ble_hs_adv_set_flat(BLE_HS_ADV_TYPE_TX_PWR_LVL, 1,
-                                 &ble_gap_slave.tx_pwr_lvl,
-                                 ble_gap_slave.adv_data,
-                                 &adv_data_len, BLE_HCI_MAX_ADV_DATA_LEN);
-        BLE_HS_DBG_ASSERT(rc == 0);
+static int
+ble_gap_adv_data_tx(void)
+{
+    uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_DATA_LEN];
+    int rc;
+
+    /* Calculate the flags AD field if requested by application.  Clear the
+     * auto flag after encoding the flags so that we don't get repeated flags
+     * fields on subsequent advertising procedures.
+     */
+    if (ble_gap_slave.adv_auto_flags) {
+        ble_gap_adv_data_set_flags();
+        ble_gap_slave.adv_auto_flags = 0;
     }
 
-    rc = host_hci_cmd_build_le_set_adv_data(ble_gap_slave.adv_data,
-                                            adv_data_len, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_set_adv_data(ble_gap_slave.adv_data,
+                                              ble_gap_slave.adv_data_len,
+                                              buf, sizeof buf);
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1323,41 +1458,103 @@ ble_gap_adv_data_tx(void)
 }
 
 static int
-ble_gap_adv_params_tx(const struct ble_gap_adv_params *adv_params,
-                      uint8_t *peer_addr, uint8_t peer_addr_type)
+ble_gap_adv_type(const struct ble_gap_adv_params *adv_params)
+{
+    switch (adv_params->conn_mode) {
+    case BLE_GAP_CONN_MODE_NON:
+        if (adv_params->disc_mode == BLE_GAP_DISC_MODE_NON) {
+            return BLE_HCI_ADV_TYPE_ADV_NONCONN_IND;
+        } else {
+            return BLE_HCI_ADV_TYPE_ADV_SCAN_IND;
+        }
+
+    case BLE_GAP_CONN_MODE_UND:
+        return BLE_HCI_ADV_TYPE_ADV_IND;
+
+    case BLE_GAP_CONN_MODE_DIR:
+        if (adv_params->high_duty_cycle) {
+            return BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD;
+        } else {
+            return BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD;
+        }
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_HCI_ADV_TYPE_ADV_IND;
+    }
+}
+
+static void
+ble_gap_adv_dflt_itvls(uint8_t conn_mode,
+                       uint16_t *out_itvl_min, uint16_t *out_itvl_max)
+{
+    switch (conn_mode) {
+    case BLE_GAP_CONN_MODE_NON:
+        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL2_MIN;
+        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL2_MAX;
+        break;
+
+    case BLE_GAP_CONN_MODE_UND:
+        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
+        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
+        break;
+
+    case BLE_GAP_CONN_MODE_DIR:
+        *out_itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
+        *out_itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        break;
+    }
+}
+
+static int
+ble_gap_adv_params_tx(uint8_t own_addr_type,
+                      uint8_t peer_addr_type, const uint8_t *peer_addr,
+                      const struct ble_gap_adv_params *adv_params)
+
 {
     struct hci_adv_params hci_adv_params;
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_PARAM_LEN];
     int rc;
-    uint8_t peer[6];
 
-    if(peer_addr) {
-        memcpy(peer, peer_addr, 6);
-    } else {
-        memset(peer, 0, 6);
+    if (peer_addr == NULL) {
+        peer_addr = ble_hs_misc_null_addr;
     }
 
-    hci_adv_params.adv_channel_map = adv_params->adv_channel_map;
-    hci_adv_params.own_addr_type = adv_params->own_addr_type;
-    hci_adv_params.adv_filter_policy = adv_params->adv_filter_policy;
-    hci_adv_params.adv_itvl_min = adv_params->adv_itvl_min;
-    hci_adv_params.adv_itvl_max = adv_params->adv_itvl_max;
+    hci_adv_params.own_addr_type = own_addr_type;
     hci_adv_params.peer_addr_type = peer_addr_type;
-    hci_adv_params.adv_type = adv_params->adv_type;
-
-    if ((ble_gap_slave.conn_mode == BLE_GAP_CONN_MODE_DIR) ||
-        (adv_params->own_addr_type == BLE_ADDR_TYPE_RPA_PUB_DEFAULT) ||
-        (adv_params->own_addr_type == BLE_ADDR_TYPE_RPA_RND_DEFAULT)) {
-            memcpy(hci_adv_params.peer_addr,peer,
-            sizeof(hci_adv_params.peer_addr));
+    memcpy(hci_adv_params.peer_addr, peer_addr,
+           sizeof hci_adv_params.peer_addr);
+
+    /* Fill optional fields if application did not specify them. */
+    if (adv_params->itvl_min == 0 && adv_params->itvl_max == 0) {
+        ble_gap_adv_dflt_itvls(adv_params->conn_mode,
+                               &hci_adv_params.adv_itvl_min,
+                               &hci_adv_params.adv_itvl_max);
+    } else {
+        hci_adv_params.adv_itvl_min = adv_params->itvl_min;
+        hci_adv_params.adv_itvl_max = adv_params->itvl_max;
     }
+    if (adv_params->channel_map == 0) {
+        hci_adv_params.adv_channel_map = BLE_GAP_ADV_DFLT_CHANNEL_MAP;
+    } else {
+        hci_adv_params.adv_channel_map = adv_params->channel_map;
+    }
+
+    /* Zero is the default value for filter policy and high duty cycle */
+    hci_adv_params.adv_filter_policy = adv_params->filter_policy;
 
-    rc = host_hci_cmd_build_le_set_adv_params(&hci_adv_params, buf, sizeof buf);
+    hci_adv_params.adv_type = ble_gap_adv_type(adv_params);
+    rc = ble_hs_hci_cmd_build_le_set_adv_params(&hci_adv_params,
+                                                buf, sizeof buf);
     if (rc != 0) {
-        return rc;
+        return BLE_HS_EINVAL;
     }
 
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1365,67 +1562,47 @@ ble_gap_adv_params_tx(const struct ble_gap_adv_params *adv_params,
     return 0;
 }
 
-/**
- * Enables the specified discoverable mode and connectable mode, and initiates
- * the advertising process.
- *
- * @param discoverable_mode     One of the following constants:
- *                                  o BLE_GAP_DISC_MODE_NON
- *                                      (non-discoverable; 3.C.9.2.2).
- *                                  o BLE_GAP_DISC_MODE_LTD
- *                                      (limited-discoverable; 3.C.9.2.3).
- *                                  o BLE_GAP_DISC_MODE_GEN
- *                                      (general-discoverable; 3.C.9.2.4).
- * @param connectable_mode      One of the following constants:
- *                                  o BLE_GAP_CONN_MODE_NON
- *                                      (non-connectable; 3.C.9.3.2).
- *                                  o BLE_GAP_CONN_MODE_DIR
- *                                      (directed-connectable; 3.C.9.3.3).
- *                                  o BLE_GAP_CONN_MODE_UND
- *                                      (undirected-connectable; 3.C.9.3.4).
- *
- * @return                      0 on success; nonzero on failure.
- */
-int
-ble_gap_adv_start(uint8_t discoverable_mode, uint8_t connectable_mode,
-                  uint8_t *peer_addr, uint8_t peer_addr_type,
-                  const struct ble_gap_adv_params *adv_params,
-                  ble_gap_event_fn *cb, void *cb_arg)
+static int
+ble_gap_adv_validate(uint8_t own_addr_type, uint8_t peer_addr_type,
+                     const uint8_t *peer_addr,
+                     const struct ble_gap_adv_params *adv_params)
 {
-#if !NIMBLE_OPT(ADVERTISE)
-    return BLE_HS_ENOTSUP;
-#endif
-
-    struct ble_gap_adv_params gap_adv_params;
-    int rc;
-
-    ble_hs_lock();
-
-    STATS_INC(ble_gap_stats, adv_start);
-
-    if (ble_gap_slave.op != BLE_GAP_OP_NULL) {
-        rc = BLE_HS_EALREADY;
-        goto done;
+    if (adv_params == NULL) {
+        return BLE_HS_EINVAL;
     }
 
-    if (discoverable_mode >= BLE_GAP_DISC_MODE_MAX) {
-        rc = BLE_HS_EINVAL;
-        goto done;
+    if (own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_HS_EINVAL;
     }
 
-    /* Don't initiate a connection procedure if we won't be able to allocate a
-     * connection object on completion.
-     */
-    if (connectable_mode != BLE_GAP_CONN_MODE_NON &&
-        !ble_hs_conn_can_alloc()) {
+    if (adv_params->disc_mode >= BLE_GAP_DISC_MODE_MAX) {
+        return BLE_HS_EINVAL;
+    }
 
-        rc = BLE_HS_ENOMEM;
-        goto done;
+    if (ble_gap_slave.op != BLE_GAP_OP_NULL) {
+        return BLE_HS_EALREADY;
     }
 
-    switch (connectable_mode) {
+    switch (adv_params->conn_mode) {
     case BLE_GAP_CONN_MODE_NON:
+        /* High duty cycle only allowed for directed advertising. */
+        if (adv_params->high_duty_cycle) {
+            return BLE_HS_EINVAL;
+        }
+        break;
+
     case BLE_GAP_CONN_MODE_UND:
+        /* High duty cycle only allowed for directed advertising. */
+        if (adv_params->high_duty_cycle) {
+            return BLE_HS_EINVAL;
+        }
+
+        /* Don't allow connectable advertising if we won't be able to allocate
+         * a new connection.
+         */
+        if (!ble_hs_conn_can_alloc()) {
+            return BLE_HS_ENOMEM;
+        }
         break;
 
     case BLE_GAP_CONN_MODE_DIR:
@@ -1434,76 +1611,117 @@ ble_gap_adv_start(uint8_t discoverable_mode, uint8_t connectable_mode,
             peer_addr_type != BLE_ADDR_TYPE_RPA_PUB_DEFAULT &&
             peer_addr_type != BLE_ADDR_TYPE_RPA_RND_DEFAULT) {
 
-            rc = BLE_HS_EINVAL;
-            goto done;
+            return BLE_HS_EINVAL;
+        }
+        if (peer_addr == NULL) {
+            return BLE_HS_EINVAL;
+        }
+
+        /* Don't allow connectable advertising if we won't be able to allocate
+         * a new connection.
+         */
+        if (!ble_hs_conn_can_alloc()) {
+            return BLE_HS_ENOMEM;
         }
         break;
 
     default:
-        rc = BLE_HS_EINVAL;
-        goto done;
-    }
-
-    if(adv_params == NULL) {
-        gap_adv_params = ble_gap_adv_params_dflt;
-    } else {
-        gap_adv_params = *adv_params;
+        return BLE_HS_EINVAL;
     }
 
-    if(gap_adv_params.own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
-        rc = BLE_HS_EINVAL;
-        goto done;
-    }
+    return 0;
+}
 
-    ble_gap_slave.cb = cb;
-    ble_gap_slave.cb_arg = cb_arg;
-    ble_gap_slave.conn_mode = connectable_mode;
-    ble_gap_slave.disc_mode = discoverable_mode;
-    ble_gap_slave.our_addr_type = gap_adv_params.own_addr_type;
+/**
+ * Initiates advertising.
+ *
+ * @param own_addr_type         The type of address the stack should use for
+ *                                  itself.  Valid values are:
+ *                                      o BLE_ADDR_TYPE_PUBLIC
+ *                                      o BLE_ADDR_TYPE_RANDOM
+ *                                      o BLE_ADDR_TYPE_RPA_PUB_DEFAULT
+ *                                      o BLE_ADDR_TYPE_RPA_RND_DEFAULT
+ * @param peer_addr_type        Address type of the peer's identity address.
+ *                                  Valid values are:
+ *                                      o BLE_ADDR_TYPE_PUBLIC
+ *                                      o BLE_ADDR_TYPE_RANDOM
+ *                                  This parameter is ignored unless directed
+ *                                  advertising is being used.
+ * @param peer_addr             The peer's six-byte identity address.
+ *                                  This parameter is ignored unless directed
+ *                                  advertising is being used.
+ * @param duration_ms           The duration of the advertisement procedure.
+ *                                  On expiration, the procedure ends and a
+ *                                  BLE_GAP_EVENT_ADV_COMPLETE event is
+ *                                  reported.  Units are milliseconds.  Specify
+ *                                  BLE_HS_FOREVER for no expiration.
+ * @param adv_params            Additional arguments specifying the particulars
+ *                                  of the advertising procedure.
+ * @param cb                    The callback to associate with this advertising
+ *                                  procedure.  If advertising ends, the event
+ *                                  is reported through this callback.  If
+ *                                  advertising results in a connection, the
+ *                                  connection inherits this callback as its
+ *                                  event-reporting mechanism.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int
+ble_gap_adv_start(uint8_t own_addr_type, uint8_t peer_addr_type,
+                  const uint8_t *peer_addr, int32_t duration_ms,
+                  const struct ble_gap_adv_params *adv_params,
+                  ble_gap_event_fn *cb, void *cb_arg)
+{
+#if !NIMBLE_OPT(ADVERTISE)
+    return BLE_HS_ENOTSUP;
+#endif
 
-    ble_gap_adv_itvls(discoverable_mode, connectable_mode,
-                      &gap_adv_params.adv_itvl_min,
-                      &gap_adv_params.adv_itvl_max);
+    uint32_t duration_ticks;
+    int rc;
 
-    if (gap_adv_params.own_addr_type == BLE_HCI_ADV_OWN_ADDR_RANDOM) {
-        ble_hs_pvcy_set_our_nrpa();
-    }
+    STATS_INC(ble_gap_stats, adv_start);
 
-    switch (connectable_mode) {
-    case BLE_GAP_CONN_MODE_NON:
-        gap_adv_params.adv_type = BLE_HCI_ADV_TYPE_ADV_NONCONN_IND;
-        break;
+    ble_hs_lock();
 
-    case BLE_GAP_CONN_MODE_DIR:
-        gap_adv_params.adv_type = BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD;
-        break;
+    rc = ble_gap_adv_validate(own_addr_type, peer_addr_type, peer_addr,
+                              adv_params);
+    if (rc != 0) {
+        goto done;
+    }
 
-    case BLE_GAP_CONN_MODE_UND:
-        gap_adv_params.adv_type = BLE_HCI_ADV_TYPE_ADV_IND;
-        break;
+    if (duration_ms != BLE_HS_FOREVER) {
+        rc = os_time_ms_to_ticks(duration_ms, &duration_ticks);
+        if (rc != 0) {
+            /* Duration too great. */
+            rc = BLE_HS_EINVAL;
+            goto done;
+        }
+    }
 
-    default:
-        BLE_HS_DBG_ASSERT(0);
-        break;
+    rc = ble_hs_id_use_addr(own_addr_type);
+    if (rc != 0) {
+        return rc;
     }
 
     BLE_HS_LOG(INFO, "GAP procedure initiated: advertise; ");
-    ble_gap_log_adv(&gap_adv_params, peer_addr, peer_addr_type);
+    ble_gap_log_adv(own_addr_type, peer_addr_type, peer_addr, adv_params);
     BLE_HS_LOG(INFO, "\n");
 
-    rc = ble_gap_adv_params_tx(&gap_adv_params, peer_addr, peer_addr_type);
+    ble_gap_slave.cb = cb;
+    ble_gap_slave.cb_arg = cb_arg;
+    ble_gap_slave.conn_mode = adv_params->conn_mode;
+    ble_gap_slave.disc_mode = adv_params->disc_mode;
+    ble_gap_slave.our_addr_type = own_addr_type;
+
+    rc = ble_gap_adv_params_tx(own_addr_type, peer_addr_type, peer_addr,
+                               adv_params);
     if (rc != 0) {
         goto done;
     }
 
-    if (ble_gap_slave.adv_pwr_lvl) {
-        rc = ble_hci_util_read_adv_tx_pwr(&ble_gap_slave.tx_pwr_lvl);
-        if (rc != 0) {
-            goto done;
-        }
-    }
-
-    if (ble_gap_slave.conn_mode != BLE_GAP_CONN_MODE_DIR) {
+    if (adv_params->conn_mode != BLE_GAP_CONN_MODE_DIR) {
         rc = ble_gap_adv_data_tx();
         if (rc != 0) {
             goto done;
@@ -1515,27 +1733,42 @@ ble_gap_adv_start(uint8_t discoverable_mode, uint8_t connectable_mode,
         }
     }
 
-    rc = ble_gap_adv_enable_tx();
+    ble_gap_slave.op = BLE_GAP_OP_S_ADV;
+
+    rc = ble_gap_adv_enable_tx(1);
     if (rc != 0) {
+        ble_gap_slave_reset_state();
         goto done;
     }
 
-    ble_gap_slave.op = BLE_GAP_OP_S_ADV;
+    if (duration_ms != BLE_HS_FOREVER) {
+        ble_gap_slave_set_timer(duration_ticks);
+    }
 
     rc = 0;
 
 done:
+    ble_hs_unlock();
+
     if (rc != 0) {
         STATS_INC(ble_gap_stats, adv_start_fail);
     }
-
-    ble_hs_unlock();
-
     return rc;
 }
 
+/**
+ * Configures the data to include in subsequent advertisements.
+ *
+ * @param adv_fields            Specifies the advertisement data.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if advertising is in progress;
+ *                              BLE_HS_EMSGSIZE if the specified data is too
+ *                                  large to fit in an advertisement;
+ *                              Other nonzero on failure.
+ */
 int
-ble_gap_adv_set_fields(struct ble_hs_adv_fields *adv_fields)
+ble_gap_adv_set_fields(const struct ble_hs_adv_fields *adv_fields)
 {
 #if !NIMBLE_OPT(ADVERTISE)
     return BLE_HS_ENOTSUP;
@@ -1544,31 +1777,55 @@ ble_gap_adv_set_fields(struct ble_hs_adv_fields *adv_fields)
     int max_sz;
     int rc;
 
+    STATS_INC(ble_gap_stats, adv_set_fields);
+
     ble_hs_lock();
 
-    STATS_INC(ble_gap_stats, adv_set_fields);
+    /* Don't allow advertising fields to be set while advertising is active. */
+    if (ble_gap_slave.op != BLE_GAP_OP_NULL) {
+        rc = BLE_HS_EBUSY;
+        goto done;
+    }
 
-    if (adv_fields->tx_pwr_lvl_is_present) {
-        max_sz = BLE_GAP_ADV_DATA_LIMIT_PWR;
+    /* If application has requested the stack to calculate the flags field
+     * automatically (flags == 0), there is less room for user data.
+     */
+    if (adv_fields->flags_is_present && adv_fields->flags == 0) {
+        max_sz = BLE_GAP_ADV_DATA_LIMIT_FLAGS;
+        ble_gap_slave.adv_auto_flags = 1;
     } else {
-        max_sz = BLE_GAP_ADV_DATA_LIMIT_NO_PWR;
+        max_sz = BLE_GAP_ADV_DATA_LIMIT_NO_FLAGS;
+        ble_gap_slave.adv_auto_flags = 0;
     }
 
     rc = ble_hs_adv_set_fields(adv_fields, ble_gap_slave.adv_data,
                                &ble_gap_slave.adv_data_len, max_sz);
-    if (rc == 0) {
-        ble_gap_slave.adv_pwr_lvl = adv_fields->tx_pwr_lvl_is_present;
-    } else {
-        STATS_INC(ble_gap_stats, adv_set_fields_fail);
+    if (rc != 0) {
+        goto done;
     }
 
+done:
     ble_hs_unlock();
 
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, adv_set_fields_fail);
+    }
     return rc;
 }
 
+/**
+ * Configures the data to include in subsequent scan responses.
+ *
+ * @param adv_fields            Specifies the scan response data.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if advertising is in progress;
+ *                              BLE_HS_EMSGSIZE if the specified data is too
+ *                                  large to fit in an advertisement;
+ *                              Other nonzero on failure.
+ */
 int
-ble_gap_adv_rsp_set_fields(struct ble_hs_adv_fields *rsp_fields)
+ble_gap_adv_rsp_set_fields(const struct ble_hs_adv_fields *rsp_fields)
 {
 #if !NIMBLE_OPT(ADVERTISE)
     return BLE_HS_ENOTSUP;
@@ -1576,34 +1833,58 @@ ble_gap_adv_rsp_set_fields(struct ble_hs_adv_fields *rsp_fields)
 
     int rc;
 
+    STATS_INC(ble_gap_stats, adv_rsp_set_fields);
+
     ble_hs_lock();
 
-    STATS_INC(ble_gap_stats, adv_rsp_set_fields);
+    /* Don't allow response fields to be set while advertising is active. */
+    if (ble_gap_slave.op != BLE_GAP_OP_NULL) {
+        rc = BLE_HS_EBUSY;
+        goto done;
+    }
 
     rc = ble_hs_adv_set_fields(rsp_fields, ble_gap_slave.rsp_data,
                                &ble_gap_slave.rsp_data_len,
                                BLE_HCI_MAX_ADV_DATA_LEN);
     if (rc != 0) {
-        STATS_INC(ble_gap_stats, adv_rsp_set_fields_fail);
+        goto done;
     }
 
+done:
     ble_hs_unlock();
 
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, adv_rsp_set_fields_fail);
+    }
     return rc;
 }
 
+/**
+ * Indicates whether an advertisement procedure is currently in progress.
+ *
+ * @return                      0: No advertisement procedure in progress;
+ *                              1: Advertisement procedure in progress.
+ */
+int
+ble_gap_adv_active(void)
+{
+    /* Assume read is atomic; mutex not necessary. */
+    return ble_gap_slave.op == BLE_GAP_OP_S_ADV;
+}
+
 /*****************************************************************************
  * $discovery procedures                                                     *
  *****************************************************************************/
 
 static int
-ble_gap_disc_tx_disable(void)
+ble_gap_disc_enable_tx(int enable, int filter_duplicates)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_ENABLE_LEN];
     int rc;
 
-    host_hci_cmd_build_le_set_scan_enable(0, 0, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_set_scan_enable(!!enable, !!filter_duplicates,
+                                            buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1612,37 +1893,30 @@ ble_gap_disc_tx_disable(void)
 }
 
 static int
-ble_gap_disc_tx_enable(void)
+ble_gap_disc_tx_params(uint8_t own_addr_type,
+                       const struct ble_gap_disc_params *disc_params)
 {
-    uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_ENABLE_LEN];
+    uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_PARAM_LEN];
+    uint8_t scan_type;
     int rc;
 
-    host_hci_cmd_build_le_set_scan_enable(1, 0, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
-    if (rc != 0) {
-        return rc;
+    if (disc_params->passive) {
+        scan_type = BLE_HCI_SCAN_TYPE_PASSIVE;
+    } else {
+        scan_type = BLE_HCI_SCAN_TYPE_ACTIVE;
     }
 
-    return 0;
-}
-
-static int
-ble_gap_disc_tx_params(uint8_t scan_type, uint8_t filter_policy,
-                       uint8_t addr_mode)
-{
-    uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_PARAM_LEN];
-    int rc;
-
-    rc = host_hci_cmd_build_le_set_scan_params(
-        scan_type,
-        BLE_GAP_SCAN_FAST_INTERVAL_MIN,
-        BLE_GAP_SCAN_FAST_WINDOW,
-        addr_mode,
-        filter_policy,
-        buf, sizeof buf);
-    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    rc = ble_hs_hci_cmd_build_le_set_scan_params(scan_type,
+                                                 disc_params->itvl,
+                                                 disc_params->window,
+                                                 own_addr_type,
+                                                 disc_params->filter_policy,
+                                                 buf, sizeof buf);
+    if (rc != 0) {
+        return BLE_HS_EINVAL;
+    }
 
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1665,16 +1939,16 @@ ble_gap_disc_cancel(void)
 {
     int rc;
 
-    ble_hs_lock();
-
     STATS_INC(ble_gap_stats, discover_cancel);
 
-    if (ble_gap_master.op != BLE_GAP_OP_M_DISC) {
+    ble_hs_lock();
+
+    if (!ble_gap_disc_active()) {
         rc = BLE_HS_EALREADY;
         goto done;
     }
 
-    rc = ble_gap_disc_tx_disable();
+    rc = ble_gap_disc_enable_tx(0, 0);
     if (rc != 0) {
         goto done;
     }
@@ -1682,123 +1956,190 @@ ble_gap_disc_cancel(void)
     ble_gap_master_reset_state();
 
 done:
+    ble_hs_unlock();
+
     if (rc != 0) {
         STATS_INC(ble_gap_stats, discover_cancel_fail);
     }
-    ble_hs_unlock();
-
     return rc;
 }
 
+static void
+ble_gap_disc_fill_dflts(struct ble_gap_disc_params *disc_params)
+{
+   if (disc_params->itvl == 0) {
+        if (disc_params->limited) {
+            disc_params->itvl = BLE_GAP_LIM_DISC_SCAN_INT;
+        } else {
+            disc_params->itvl = BLE_GAP_SCAN_FAST_INTERVAL_MIN;
+        }
+    }
+
+    if (disc_params->window == 0) {
+        if (disc_params->limited) {
+            disc_params->window = BLE_GAP_LIM_DISC_SCAN_WINDOW;
+        } else {
+            disc_params->window = BLE_GAP_SCAN_FAST_WINDOW;
+        }
+    }
+}
+
+static int
+ble_gap_disc_validate(uint8_t own_addr_type,
+                      const struct ble_gap_disc_params *disc_params)
+{
+    if (disc_params == NULL) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) {
+        return BLE_HS_EINVAL;
+    }
+
+    if (ble_gap_conn_active()) {
+        return BLE_HS_EBUSY;
+    }
+
+    if (ble_gap_disc_active()) {
+        return BLE_HS_EALREADY;
+    }
+
+    return 0;
+}
+
 /**
- * Performs the Limited or General Discovery Procedures, as described in
- * vol. 3, part C, section 9.2.5 / 9.2.6.
+ * Performs the Limited or General Discovery Procedures.
+ *
+ * @param own_addr_type         The type of address the stack should use for
+ *                                  itself when sending scan requests.  Valid
+ *                                  values are:
+ *                                      o BLE_ADDR_TYPE_PUBLIC
+ *                                      o BLE_ADDR_TYPE_RANDOM
+ *                                      o BLE_ADDR_TYPE_RPA_PUB_DEFAULT
+ *                                      o BLE_ADDR_TYPE_RPA_RND_DEFAULT
+ *                                  This parameter is ignored unless active
+ *                                  scanning is being used.
+ * @param duration_ms           The duration of the discovery procedure.
+ *                                  On expiration, the procedure ends and a
+ *                                  BLE_GAP_EVENT_DISC_COMPLETE event is
+ *                                  reported.  Units are milliseconds.  Specify
+ *                                  BLE_HS_FOREVER for no expiration.
+ * @param disc_params           Additional arguments specifying the particulars
+ *                                  of the discovery procedure.
+ * @param cb                    The callback to associate with this discovery
+ *                                  procedure.  Advertising reports and
+ *                                  discovery termination events are reported
+ *                                  through this callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
  *
  * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gap_disc(uint32_t duration_ms, uint8_t discovery_mode,
-             uint8_t scan_type, uint8_t filter_policy, uint8_t addr_mode,
-             ble_gap_disc_fn *cb, void *cb_arg)
+ble_gap_disc(uint8_t own_addr_type, int32_t duration_ms,
+             const struct ble_gap_disc_params *disc_params,
+             ble_gap_event_fn *cb, void *cb_arg)
 {
 #if !NIMBLE_OPT(ROLE_OBSERVER)
     return BLE_HS_ENOTSUP;
 #endif
 
+    struct ble_gap_disc_params params;
     uint32_t duration_ticks;
     int rc;
 
-    ble_hs_lock();
-
-    if (ble_gap_master.op != BLE_GAP_OP_NULL) {
-        rc = BLE_HS_EALREADY;
-        goto done;
-    }
-
     STATS_INC(ble_gap_stats, discover);
 
-    if (discovery_mode != BLE_GAP_DISC_MODE_LTD &&
-        discovery_mode != BLE_GAP_DISC_MODE_GEN) {
-
-        rc = BLE_HS_EINVAL;
-        goto done;
-    }
+    ble_hs_lock();
 
-    if (scan_type != BLE_HCI_SCAN_TYPE_PASSIVE &&
-        scan_type != BLE_HCI_SCAN_TYPE_ACTIVE) {
+    /* Make a copy of the parameter strcuture and fill unspecified values with
+     * defaults.
+     */
+    params = *disc_params;
+    ble_gap_disc_fill_dflts(&params);
 
-        rc = BLE_HS_EINVAL;
+    rc = ble_gap_disc_validate(own_addr_type, &params);
+    if (rc != 0) {
         goto done;
     }
 
-    if((addr_mode != BLE_HCI_ADV_OWN_ADDR_PUBLIC) &&
-       (addr_mode != BLE_HCI_ADV_OWN_ADDR_RANDOM) &&
-       (addr_mode != BLE_HCI_ADV_OWN_ADDR_PRIV_PUB) &&
-       (addr_mode != BLE_HCI_ADV_OWN_ADDR_PRIV_RAND)) {
-          rc = BLE_HS_EINVAL;
-          goto done;
-      }
-
-    if (filter_policy > BLE_HCI_SCAN_FILT_MAX) {
-        rc = BLE_HS_EINVAL;
-        goto done;
+    if (duration_ms == 0) {
+        duration_ms = BLE_GAP_DISC_DUR_DFLT;
     }
 
-    if (duration_ms == 0) {
-        duration_ms = BLE_GAP_GEN_DISC_SCAN_MIN;
+    if (duration_ms != BLE_HS_FOREVER) {
+        rc = os_time_ms_to_ticks(duration_ms, &duration_ticks);
+        if (rc != 0) {
+            /* Duration too great. */
+            rc = BLE_HS_EINVAL;
+            goto done;
+        }
     }
 
-    rc = os_time_ms_to_ticks(duration_ms, &duration_ticks);
-    if (rc != 0) {
-        /* Duration too great. */
-        rc = BLE_HS_EINVAL;
-        goto done;
+    if (!params.passive) {
+        rc = ble_hs_id_use_addr(own_addr_type);
+        if (rc != 0) {
+            return rc;
+        }
     }
 
-    ble_gap_master.disc.disc_mode = discovery_mode;
-    ble_gap_master.disc.cb = cb;
-    ble_gap_master.disc.cb_arg = cb_arg;
+    ble_gap_master.disc.limited = params.limited;
+    ble_gap_master.cb = cb;
+    ble_gap_master.cb_arg = cb_arg;
 
     BLE_HS_LOG(INFO, "GAP procedure initiated: discovery; ");
-    ble_gap_log_disc(scan_type, filter_policy, addr_mode);
+    ble_gap_log_disc(own_addr_type, duration_ms, &params);
     BLE_HS_LOG(INFO, "\n");
 
-    if (addr_mode == BLE_HCI_ADV_OWN_ADDR_RANDOM) {
-        ble_hs_pvcy_set_our_nrpa();
-    }
-
-    rc = ble_gap_disc_tx_params(scan_type, filter_policy, addr_mode);
+    rc = ble_gap_disc_tx_params(own_addr_type, &params);
     if (rc != 0) {
         goto done;
     }
 
-    rc = ble_gap_disc_tx_enable();
+    ble_gap_master.op = BLE_GAP_OP_M_DISC;
+
+    rc = ble_gap_disc_enable_tx(1, params.filter_duplicates);
     if (rc != 0) {
+        ble_gap_master_reset_state();
         goto done;
     }
 
-    ble_gap_master_set_timer(duration_ticks);
-    ble_gap_master.op = BLE_GAP_OP_M_DISC;
+    if (duration_ms != BLE_HS_FOREVER) {
+        ble_gap_master_set_timer(duration_ticks);
+    }
 
     rc = 0;
 
 done:
+    ble_hs_unlock();
+
     if (rc != 0) {
         STATS_INC(ble_gap_stats, discover_fail);
     }
-
-    ble_hs_unlock();
-
     return rc;
 }
 
+/**
+ * Indicates whether a discovery procedure is currently in progress.
+ *
+ * @return                      0: No discovery procedure in progress;
+ *                              1: Discovery procedure in progress.
+ */
+int
+ble_gap_disc_active(void)
+{
+    /* Assume read is atomic; mutex not necessary. */
+    return ble_gap_master.op == BLE_GAP_OP_M_DISC;
+}
+
 /*****************************************************************************
  * $connection establishment procedures                                      *
  *****************************************************************************/
 
 static int
-ble_gap_conn_create_tx(int addr_type, uint8_t *addr,
-                       struct ble_gap_crt_params *params)
+ble_gap_conn_create_tx(uint8_t own_addr_type,
+                       uint8_t peer_addr_type, const uint8_t *peer_addr,
+                       const struct ble_gap_conn_params *params)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_CREATE_CONN_LEN];
     struct hci_create_conn hcc;
@@ -1807,18 +2148,21 @@ ble_gap_conn_create_tx(int addr_type, uint8_t *addr,
     hcc.scan_itvl = params->scan_itvl;
     hcc.scan_window = params->scan_window;
 
-    if (addr_type == BLE_GAP_ADDR_TYPE_WL) {
+    if (peer_addr_type == BLE_GAP_ADDR_TYPE_WL) {
+        /* Application wants to connect to any device in the white list.  The
+         * peer address type and peer address fields are ignored by the
+         * controller; fill them with dummy values.
+         */
         hcc.filter_policy = BLE_HCI_CONN_FILT_USE_WL;
-        hcc.peer_addr_type = BLE_HCI_ADV_PEER_ADDR_PUBLIC;
+        hcc.peer_addr_type = 0;
         memset(hcc.peer_addr, 0, sizeof hcc.peer_addr);
     } else {
         hcc.filter_policy = BLE_HCI_CONN_FILT_NO_WL;
-        hcc.peer_addr_type = addr_type;
-        memcpy(hcc.peer_addr, addr, sizeof hcc.peer_addr);
+        hcc.peer_addr_type = peer_addr_type;
+        memcpy(hcc.peer_addr, peer_addr, sizeof hcc.peer_addr);
     }
-    /* TODO error check our_addr_type */
-    hcc.own_addr_type = params->our_addr_type;
 
+    hcc.own_addr_type = own_addr_type;
     hcc.conn_itvl_min = params->itvl_min;
     hcc.conn_itvl_max = params->itvl_max;
     hcc.conn_latency = params->latency;
@@ -1826,12 +2170,12 @@ ble_gap_conn_create_tx(int addr_type, uint8_t *addr,
     hcc.min_ce_len = params->min_ce_len;
     hcc.max_ce_len = params->max_ce_len;
 
-    rc = host_hci_cmd_build_le_create_connection(&hcc, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_create_connection(&hcc, buf, sizeof buf);
     if (rc != 0) {
         return BLE_HS_EUNKNOWN;
     }
 
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1840,108 +2184,193 @@ ble_gap_conn_create_tx(int addr_type, uint8_t *addr,
 }
 
 /**
- * Performs the Direct Connection Establishment Procedure, as described in
- * vol. 3, part C, section 9.3.8.
+ * Initiates a connect procedure.
  *
- * @param addr_type             The peer's address type; one of:
- *                                  o BLE_HCI_CONN_PEER_ADDR_PUBLIC
- *                                  o BLE_HCI_CONN_PEER_ADDR_RANDOM
- *                                  o BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT
- *                                  o BLE_HCI_CONN_PEER_ADDR_RANDOM_IDENT
- *                                  o BLE_GAP_ADDR_TYPE_WL
- * @param addr                  The address of the peer to connect to.
+ * @param own_addr_type         The type of address the stack should use for
+ *                                  itself during connection establishment.
+ *                                      o BLE_ADDR_TYPE_PUBLIC
+ *                                      o BLE_ADDR_TYPE_RANDOM
+ *                                      o BLE_ADDR_TYPE_RPA_PUB_DEFAULT
+ *                                      o BLE_ADDR_TYPE_RPA_RND_DEFAULT
+ * @param peer_addr_type        The peer's address type.  One of:
+ *                                      o BLE_HCI_CONN_PEER_ADDR_PUBLIC
+ *                                      o BLE_HCI_CONN_PEER_ADDR_RANDOM
+ *                                      o BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT
+ *                                      o BLE_HCI_CONN_PEER_ADDR_RANDOM_IDENT
+ *                                      o BLE_GAP_ADDR_TYPE_WL
+ * @param peer_addr             The identity address of the peer to connect to.
+ *                                  This parameter is ignored when the white
+ *                                  list is used.
+ * @param duration_ms           The duration of the discovery procedure.
+ *                                  On expiration, the procedure ends and a
+ *                                  BLE_GAP_EVENT_DISC_COMPLETE event is
+ *                                  reported.  Units are milliseconds.
+ * @param conn_params           Additional arguments specifying the particulars
+ *                                  of the connect procedure.  Specify null for
+ *                                  default values.
+ * @param cb                    The callback to associate with this connect
+ *                                  procedure.  When the connect procedure
+ *                                  completes, the result is reported through
+ *                                  this callback.  If the connect procedure
+ *                                  succeeds, the connection inherits this
+ *                                  callback as its event-reporting mechanism.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
  *
  * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gap_conn_initiate(int addr_type, uint8_t *addr,
-                      struct ble_gap_crt_params *params,
-                      ble_gap_event_fn *cb, void *cb_arg)
+ble_gap_connect(uint8_t own_addr_type,
+                uint8_t peer_addr_type, const uint8_t *peer_addr,
+                int32_t duration_ms,
+                const struct ble_gap_conn_params *conn_params,
+                ble_gap_event_fn *cb, void *cb_arg)
 {
 #if !NIMBLE_OPT(ROLE_CENTRAL)
     return BLE_HS_ENOTSUP;
 #endif
 
+    uint32_t duration_ticks;
     int rc;
 
+    STATS_INC(ble_gap_stats, initiate);
+
     ble_hs_lock();
 
-    if (ble_gap_master.op != BLE_GAP_OP_NULL) {
+    if (ble_gap_conn_active()) {
         rc = BLE_HS_EALREADY;
         goto done;
     }
 
-    STATS_INC(ble_gap_stats, initiate);
+    if (ble_gap_disc_active()) {
+        rc = BLE_HS_EBUSY;
+        goto done;
+    }
 
-    if (addr_type != BLE_HCI_CONN_PEER_ADDR_PUBLIC &&
-        addr_type != BLE_HCI_CONN_PEER_ADDR_RANDOM &&
-        addr_type != BLE_HCI_CONN_PEER_ADDR_PUB_ID &&
-        addr_type != BLE_HCI_CONN_PEER_ADDR_RAND_ID &&
-        addr_type != BLE_GAP_ADDR_TYPE_WL) {
+    if (!ble_hs_conn_can_alloc()) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    if (peer_addr_type != BLE_HCI_CONN_PEER_ADDR_PUBLIC &&
+        peer_addr_type != BLE_HCI_CONN_PEER_ADDR_RANDOM &&
+        peer_addr_type != BLE_HCI_CONN_PEER_ADDR_PUB_ID &&
+        peer_addr_type != BLE_HCI_CONN_PEER_ADDR_RAND_ID &&
+        peer_addr_type != BLE_GAP_ADDR_TYPE_WL) {
 
         rc = BLE_HS_EINVAL;
         goto done;
     }
 
-    if (params == NULL) {
-        params = (void *)&ble_gap_params_dflt;
+    if (conn_params == NULL) {
+        conn_params = &ble_gap_conn_params_dflt;
+    }
+
+    if (duration_ms == 0) {
+        duration_ms = BLE_GAP_CONN_DUR_DFLT;
+    }
+
+    if (duration_ms != BLE_HS_FOREVER) {
+        rc = os_time_ms_to_ticks(duration_ms, &duration_ticks);
+        if (rc != 0) {
+            /* Duration too great. */
+            rc = BLE_HS_EINVAL;
+            goto done;
+        }
     }
 
-    /* XXX: Verify params. */
+    /* XXX: Verify conn_params. */
+
+    rc = ble_hs_id_use_addr(own_addr_type);
+    if (rc != 0) {
+        return rc;
+    }
 
     BLE_HS_LOG(INFO, "GAP procedure initiated: connect; ");
-    ble_gap_log_conn(addr_type, addr, params);
+    ble_gap_log_conn(own_addr_type, peer_addr_type, peer_addr, conn_params);
     BLE_HS_LOG(INFO, "\n");
 
-    ble_gap_master.conn.cb = cb;
-    ble_gap_master.conn.cb_arg = cb_arg;
-    ble_gap_master.conn.using_wl = addr_type == BLE_GAP_ADDR_TYPE_WL;
-    ble_gap_master.conn.our_addr_type = params->our_addr_type;
+    ble_gap_master.cb = cb;
+    ble_gap_master.cb_arg = cb_arg;
+    ble_gap_master.conn.using_wl = peer_addr_type == BLE_GAP_ADDR_TYPE_WL;
+    ble_gap_master.conn.our_addr_type = own_addr_type;
 
-    rc = ble_gap_conn_create_tx(addr_type, addr, params);
+    ble_gap_master.op = BLE_GAP_OP_M_CONN;
+
+    rc = ble_gap_conn_create_tx(own_addr_type, peer_addr_type, peer_addr,
+                                conn_params);
     if (rc != 0) {
+        ble_gap_master_reset_state();
         goto done;
     }
 
-    ble_gap_master.op = BLE_GAP_OP_M_CONN;
+    if (duration_ms != BLE_HS_FOREVER) {
+        ble_gap_master_set_timer(duration_ticks);
+    }
 
     rc = 0;
 
 done:
+    ble_hs_unlock();
+
     if (rc != 0) {
         STATS_INC(ble_gap_stats, initiate_fail);
     }
-
-    ble_hs_unlock();
-
     return rc;
 }
 
+/**
+ * Indicates whether a connect procedure is currently in progress.
+ *
+ * @return                      0: No connect procedure in progress;
+ *                              1: Connect procedure in progress.
+ */
+int
+ble_gap_conn_active(void)
+{
+    /* Assume read is atomic; mutex not necessary. */
+    return ble_gap_master.op == BLE_GAP_OP_M_CONN;
+}
+
 /*****************************************************************************
  * $terminate connection procedure                                           *
  *****************************************************************************/
 
+/**
+ * Terminates an established connection.
+ *
+ * @param conn_handle           The handle corresponding to the connection to
+ *                                  terminate.
+ * @param hci_reason            The HCI error code to indicate as the reason
+ *                                  for termination.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOTCONN if there is no connection with
+ *                                  the specified handle;
+ *                              Other nonzero on failure.
+ */
 int
-ble_gap_terminate(uint16_t conn_handle)
+ble_gap_terminate(uint16_t conn_handle, uint8_t hci_reason)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_DISCONNECT_CMD_LEN];
     int rc;
 
-    ble_hs_lock();
-
     STATS_INC(ble_gap_stats, terminate);
 
+    ble_hs_lock();
+
     if (!ble_hs_conn_exists(conn_handle)) {
         rc = BLE_HS_ENOTCONN;
         goto done;
     }
 
     BLE_HS_LOG(INFO, "GAP procedure initiated: terminate connection; "
-                     "conn_handle=%d\n", conn_handle);
+                     "conn_handle=%d hci_reason=%d\n",
+               conn_handle, hci_reason);
 
-    host_hci_cmd_build_disconnect(conn_handle, BLE_ERR_REM_USER_CONN_TERM,
-                                  buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_disconnect(conn_handle, hci_reason,
+                                    buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         goto done;
     }
@@ -1949,12 +2378,11 @@ ble_gap_terminate(uint16_t conn_handle)
     rc = 0;
 
 done:
+    ble_hs_unlock();
+
     if (rc != 0) {
         STATS_INC(ble_gap_stats, terminate_fail);
     }
-
-    ble_hs_unlock();
-
     return rc;
 }
 
@@ -1962,38 +2390,59 @@ done:
  * $cancel                                                                   *
  *****************************************************************************/
 
-int
-ble_gap_cancel(void)
+static int
+ble_gap_conn_cancel_tx(void)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN];
     int rc;
 
-    ble_hs_lock();
+    ble_hs_hci_cmd_build_le_create_conn_cancel(buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Aborts a connect procedure in progress.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EALREADY if there is no active connect
+ *                                  procedure.
+ *                              Other nonzero on error.
+ */
+int
+ble_gap_conn_cancel(void)
+{
+    int rc;
 
     STATS_INC(ble_gap_stats, cancel);
 
-    if (!ble_gap_master_in_progress()) {
-        rc = BLE_HS_ENOENT;
+    ble_hs_lock();
+
+    if (!ble_gap_conn_active()) {
+        rc = BLE_HS_EALREADY;
         goto done;
     }
 
     BLE_HS_LOG(INFO, "GAP procedure initiated: cancel connection\n");
 
-    host_hci_cmd_build_le_create_conn_cancel(buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_gap_conn_cancel_tx();
     if (rc != 0) {
         goto done;
     }
 
+    ble_gap_master.conn.cancel = 1;
     rc = 0;
 
 done:
+    ble_hs_unlock();
+
     if (rc != 0) {
         STATS_INC(ble_gap_stats, cancel_fail);
     }
-
-    ble_hs_unlock();
-
     return rc;
 }
 
@@ -2017,8 +2466,8 @@ ble_gap_tx_param_pos_reply(uint16_t conn_handle,
     pos_reply.min_ce_len = params->min_ce_len;
     pos_reply.max_ce_len = params->max_ce_len;
 
-    host_hci_cmd_build_le_conn_param_reply(&pos_reply, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_conn_param_reply(&pos_reply, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -2036,8 +2485,8 @@ ble_gap_tx_param_neg_reply(uint16_t conn_handle, uint8_t reject_reason)
     neg_reply.handle = conn_handle;
     neg_reply.reason = reject_reason;
 
-    host_hci_cmd_build_le_conn_param_neg_reply(&neg_reply, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_conn_param_neg_reply(&neg_reply, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -2054,18 +2503,13 @@ ble_gap_rx_param_req(struct hci_le_conn_param_req *evt)
 
     struct ble_gap_upd_params peer_params;
     struct ble_gap_upd_params self_params;
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
+    struct ble_gap_event event;
     uint8_t reject_reason;
     int rc;
 
     reject_reason = 0; /* Silence warning. */
 
-    rc = ble_gap_find_snapshot(evt->connection_handle, &snap);
-    if (rc != 0) {
-        /* We are not connected to the sender. */
-        return;
-    }
+    memset(&event, 0, sizeof event);
 
     peer_params.itvl_min = evt->itvl_min;
     peer_params.itvl_max = evt->itvl_max;
@@ -2080,12 +2524,12 @@ ble_gap_rx_param_req(struct hci_le_conn_param_req *evt)
      */
     self_params = peer_params;
 
-    memset(&ctxt, 0, sizeof ctxt);
-    ctxt.desc = &snap.desc;
-    ctxt.conn_update_req.self_params = &self_params;
-    ctxt.conn_update_req.peer_params = &peer_params;
-    rc = ble_gap_call_event_cb(BLE_GAP_EVENT_CONN_UPDATE_REQ, &ctxt,
-                               snap.cb, snap.cb_arg);
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_CONN_UPDATE_REQ;
+    event.conn_update_req.conn_handle = evt->connection_handle;
+    event.conn_update_req.self_params = &self_params;
+    event.conn_update_req.peer_params = &peer_params;
+    rc = ble_gap_call_conn_event_cb(&event, evt->connection_handle);
     if (rc != 0) {
         reject_reason = rc;
     }
@@ -2104,7 +2548,8 @@ ble_gap_rx_param_req(struct hci_le_conn_param_req *evt)
 }
 
 static int
-ble_gap_update_tx(uint16_t conn_handle, struct ble_gap_upd_params *params)
+ble_gap_update_tx(uint16_t conn_handle,
+                  const struct ble_gap_upd_params *params)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_CONN_UPDATE_LEN];
     struct hci_conn_update cmd;
@@ -2118,12 +2563,12 @@ ble_gap_update_tx(uint16_t conn_handle, struct ble_gap_upd_params *params)
     cmd.min_ce_len = params->min_ce_len;
     cmd.max_ce_len = params->max_ce_len;
 
-    rc = host_hci_cmd_build_le_conn_update(&cmd, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_build_le_conn_update(&cmd, buf, sizeof buf);
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -2131,8 +2576,25 @@ ble_gap_update_tx(uint16_t conn_handle, struct ble_gap_upd_params *params)
     return 0;
 }
 
+/**
+ * Initiates a connection parameter update procedure.
+ *
+ * @param conn_handle           The handle corresponding to the connection to
+ *                                  update.
+ * @param params                The connection parameters to attempt to update
+ *                                  to.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOTCONN if the there is no connection
+ *                                  with the specified handle;
+ *                              BLE_HS_EALREADY if a connection update
+ *                                  procedure for this connection is already in
+ *                                  progress;
+ *                              Other nonzero on error.
+ */
 int
-ble_gap_update_params(uint16_t conn_handle, struct ble_gap_upd_params *params)
+ble_gap_update_params(uint16_t conn_handle,
+                      const struct ble_gap_upd_params *params)
 {
 #if !NIMBLE_OPT(CONNECT)
     return BLE_HS_ENOTSUP;
@@ -2141,10 +2603,10 @@ ble_gap_update_params(uint16_t conn_handle, struct ble_gap_upd_params *params)
     struct ble_hs_conn *conn;
     int rc;
 
-    ble_hs_lock();
-
     STATS_INC(ble_gap_stats, update);
 
+    ble_hs_lock();
+
     conn = ble_hs_conn_find(conn_handle);
     if (conn == NULL) {
         rc = BLE_HS_ENOTCONN;
@@ -2168,12 +2630,11 @@ ble_gap_update_params(uint16_t conn_handle, struct ble_gap_upd_params *params)
     conn->bhc_flags |= BLE_HS_CONN_F_UPDATE;
 
 done:
+    ble_hs_unlock();
+
     if (rc != 0) {
         STATS_INC(ble_gap_stats, update_fail);
     }
-
-    ble_hs_unlock();
-
     return rc;
 }
 
@@ -2181,6 +2642,19 @@ done:
  * $security                                                                 *
  *****************************************************************************/
 
+/**
+ * Initiates the GAP encryption procedure.
+ *
+ * @param conn_handle           The handle corresponding to the connection to
+ *                                  encrypt.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOTCONN if the there is no connection
+ *                                  with the specified handle;
+ *                              BLE_HS_EALREADY if an encrpytion procedure for
+ *                                  this connection is already in progress;
+ *                              Other nonzero on error.
+ */
 int
 ble_gap_security_initiate(uint16_t conn_handle)
 {
@@ -2195,6 +2669,8 @@ ble_gap_security_initiate(uint16_t conn_handle)
     struct ble_hs_conn *conn;
     int rc;
 
+    STATS_INC(ble_gap_stats, security_initiate);
+
     ble_hs_lock();
     conn = ble_hs_conn_find(conn_handle);
     if (conn != NULL) {
@@ -2208,7 +2684,8 @@ ble_gap_security_initiate(uint16_t conn_handle)
     ble_hs_unlock();
 
     if (conn == NULL) {
-        return BLE_HS_ENOTCONN;
+        rc = BLE_HS_ENOTCONN;
+        goto done;
     }
 
     if (conn_flags & BLE_HS_CONN_F_MASTER) {
@@ -2221,11 +2698,27 @@ ble_gap_security_initiate(uint16_t conn_handle)
             rc = ble_sm_enc_initiate(conn_handle, value_sec.ltk,
                                      value_sec.ediv, value_sec.rand_num,
                                      value_sec.authenticated);
+            if (rc != 0) {
+                goto done;
+            }
         } else {
             rc = ble_sm_pair_initiate(conn_handle);
+            if (rc != 0) {
+                goto done;
+            }
         }
     } else {
         rc = ble_sm_slave_initiate(conn_handle);
+        if (rc != 0) {
+            goto done;
+        }
+    }
+
+    rc = 0;
+
+done:
+    if (rc != 0) {
+        STATS_INC(ble_gap_stats, security_initiate_fail);
     }
 
     return rc;
@@ -2243,7 +2736,7 @@ ble_gap_pair_initiate(uint16_t conn_handle)
 
 int
 ble_gap_encryption_initiate(uint16_t conn_handle,
-                            uint8_t *ltk,
+                            const uint8_t *ltk,
                             uint16_t ediv,
                             uint64_t rand_val,
                             int auth)
@@ -2270,38 +2763,22 @@ ble_gap_encryption_initiate(uint16_t conn_handle,
 
 void
 ble_gap_passkey_event(uint16_t conn_handle,
-                      struct ble_gap_passkey_action *passkey_action)
+                      struct ble_gap_passkey_params *passkey_params)
 {
 #if !NIMBLE_OPT(SM)
     return;
 #endif
 
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
-    struct ble_hs_conn *conn;
-
-    ble_hs_lock();
-
-    conn = ble_hs_conn_find(conn_handle);
-    if (conn != NULL) {
-        ble_gap_conn_to_snapshot(conn, &snap);
-    }
-
-    ble_hs_unlock();
-
-    if (conn == NULL) {
-        /* No longer connected. */
-        return;
-    }
+    struct ble_gap_event event;
 
     BLE_HS_LOG(DEBUG, "send passkey action request %d\n",
-               passkey_action->action);
+               passkey_params->action);
 
-    memset(&ctxt, 0, sizeof ctxt);
-    ctxt.desc = &snap.desc;
-    ctxt.passkey_action = *passkey_action;
-    ble_gap_call_event_cb(BLE_GAP_EVENT_PASSKEY_ACTION, &ctxt,
-                          snap.cb, snap.cb_arg);
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_PASSKEY_ACTION;
+    event.passkey.conn_handle = conn_handle;
+    event.passkey.params = *passkey_params;
+    ble_gap_call_conn_event_cb(&event, conn_handle);
 }
 
 void
@@ -2311,59 +2788,134 @@ ble_gap_enc_event(uint16_t conn_handle, int status, int security_restored)
     return;
 #endif
 
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
-    int rc;
-
-    rc = ble_gap_find_snapshot(conn_handle, &snap);
-    if (rc != 0) {
-        /* No longer connected. */
-        return;
-    }
+    struct ble_gap_event event;
 
-    memset(&ctxt, 0, sizeof ctxt);
-    ctxt.desc = &snap.desc;
-    ctxt.enc_change.status = status;
-    ble_gap_call_event_cb(BLE_GAP_EVENT_ENC_CHANGE, &ctxt,
-                          snap.cb, snap.cb_arg);
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_ENC_CHANGE;
+    event.enc_change.conn_handle = conn_handle;
+    event.enc_change.status = status;
+    ble_gap_call_conn_event_cb(&event, conn_handle);
 
     if (status == 0 && security_restored) {
-        BLE_HS_DBG_ASSERT(snap.desc.sec_state.bonded);
         ble_gatts_bonding_restored(conn_handle);
     }
 }
 
 /*****************************************************************************
+ * $rssi                                                                     *
+ *****************************************************************************/
+
+/**
+ * Retrieves the most-recently measured RSSI for the specified connection.  A
+ * connection's RSSI is updated whenever a data channel PDU is received.
+ *
+ * @param conn_handle           Specifies the connection to query.
+ * @param out_rssi              On success, the retrieved RSSI is written here.
+ *
+ * @return                      0 on success;
+ *                              A BLE host HCI return code if the controller
+ *                                  rejected the request;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int
+ble_gap_conn_rssi(uint16_t conn_handle, int8_t *out_rssi)
+{
+    int rc;
+
+    rc = ble_hs_hci_util_read_rssi(conn_handle, out_rssi);
+    return rc;
+}
+
+/*****************************************************************************
  * $notify                                                                   *
  *****************************************************************************/
 
 void
-ble_gap_notify_event(uint16_t conn_handle, uint16_t attr_handle,
-                     void *attr_data, uint16_t attr_len, int is_indication)
+ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle,
+                        struct os_mbuf *om, int is_indication)
 {
-    /* XXX: Early return if notifications and indications disabled. */
-    struct ble_gap_conn_ctxt ctxt;
-    struct ble_gap_snapshot snap;
-    int rc;
+#if !NIMBLE_OPT(GATT_NOTIFY) && !NIMBLE_OPT(GATT_INDICATE)
+    return;
+#endif
 
-    rc = ble_gap_find_snapshot(conn_handle, &snap);
-    if (rc != 0) {
-        /* No longer connected. */
-        return;
-    }
+    struct ble_gap_event event;
 
-    memset(&ctxt, 0, sizeof ctxt);
-    ctxt.desc = &snap.desc;
-    ctxt.notify.attr_handle = attr_handle;
-    ctxt.notify.attr_data = attr_data;
-    ctxt.notify.attr_len = attr_len;
-    ctxt.notify.indication = is_indication;
-    ble_gap_call_event_cb(BLE_GAP_EVENT_NOTIFY, &ctxt, snap.cb, snap.cb_arg);
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_NOTIFY_RX;
+    event.notify_rx.conn_handle = conn_handle;
+    event.notify_rx.attr_handle = attr_handle;
+    event.notify_rx.om = om;
+    event.notify_rx.indication = is_indication;
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+
+    os_mbuf_free_chain(event.notify_rx.om);
 }
 
-void ble_gap_init_identity_addr(uint8_t *addr)
+void
+ble_gap_notify_tx_event(int status, uint16_t conn_handle, uint16_t attr_handle,
+                        int is_indication)
 {
-    ble_hs_pvcy_set_our_id_addr(addr);
+#if !NIMBLE_OPT(GATT_NOTIFY) && !NIMBLE_OPT(GATT_INDICATE)
+    return;
+#endif
+
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_NOTIFY_TX;
+    event.notify_tx.conn_handle = conn_handle;
+    event.notify_tx.status = status;
+    event.notify_tx.attr_handle = attr_handle;
+    event.notify_tx.indication = is_indication;
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+}
+
+/*****************************************************************************
+ * $subscribe                                                                *
+ *****************************************************************************/
+
+void
+ble_gap_subscribe_event(uint16_t conn_handle, uint16_t attr_handle,
+                        uint8_t reason,
+                        uint8_t prev_notify, uint8_t cur_notify,
+                        uint8_t prev_indicate, uint8_t cur_indicate)
+{
+    struct ble_gap_event event;
+
+    BLE_HS_DBG_ASSERT(prev_notify != cur_notify ||
+                      prev_indicate != cur_indicate);
+    BLE_HS_DBG_ASSERT(reason == BLE_GAP_SUBSCRIBE_REASON_WRITE ||
+                      reason == BLE_GAP_SUBSCRIBE_REASON_TERM  ||
+                      reason == BLE_GAP_SUBSCRIBE_REASON_RESTORE);
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_SUBSCRIBE;
+    event.subscribe.conn_handle = conn_handle;
+    event.subscribe.attr_handle = attr_handle;
+    event.subscribe.reason = reason;
+    event.subscribe.prev_notify = !!prev_notify;
+    event.subscribe.cur_notify = !!cur_notify;
+    event.subscribe.prev_indicate = !!prev_indicate;
+    event.subscribe.cur_indicate = !!cur_indicate;
+    ble_gap_call_conn_event_cb(&event, conn_handle);
+}
+
+/*****************************************************************************
+ * $mtu                                                                      *
+ *****************************************************************************/
+
+void
+ble_gap_mtu_event(uint16_t conn_handle, uint16_t cid, uint16_t mtu)
+{
+    struct ble_gap_event event;
+
+    memset(&event, 0, sizeof event);
+    event.type = BLE_GAP_EVENT_MTU;
+    event.mtu.conn_handle = conn_handle;
+    event.mtu.channel_id = cid;
+    event.mtu.value = mtu;
+    ble_gap_call_conn_event_cb(&event, conn_handle);
 }
 
 /*****************************************************************************
diff --git a/net/nimble/host/src/ble_gap_priv.h b/net/nimble/host/src/ble_gap_priv.h
index 547dffb7..eed5a18f 100644
--- a/net/nimble/host/src/ble_gap_priv.h
+++ b/net/nimble/host/src/ble_gap_priv.h
@@ -28,7 +28,7 @@ struct hci_le_conn_param_req;
 struct hci_le_conn_complete;
 struct hci_disconn_complete;
 struct hci_encrypt_change;
-struct ble_hci_ack;
+struct ble_hs_hci_ack;
 struct ble_hs_adv;
 
 STATS_SECT_START(ble_gap_stats)
@@ -61,6 +61,8 @@ STATS_SECT_START(ble_gap_stats)
     STATS_SECT_ENTRY(rx_conn_complete)
     STATS_SECT_ENTRY(discover_cancel)
     STATS_SECT_ENTRY(discover_cancel_fail)
+    STATS_SECT_ENTRY(security_initiate)
+    STATS_SECT_ENTRY(security_initiate_fail)
 STATS_SECT_END
 
 extern STATS_SECT_DECL(ble_gap_stats) ble_gap_stats;
@@ -68,8 +70,7 @@ extern STATS_SECT_DECL(ble_gap_stats) ble_gap_stats;
 #define BLE_GAP_CONN_MODE_MAX               3
 #define BLE_GAP_DISC_MODE_MAX               3
 
-int ble_gap_locked_by_cur_task(void);
-void ble_gap_rx_adv_report(struct ble_hs_adv *adv);
+void ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc);
 int ble_gap_rx_conn_complete(struct hci_le_conn_complete *evt);
 void ble_gap_rx_disconn_complete(struct hci_disconn_complete *evt);
 void ble_gap_rx_update_complete(struct hci_le_conn_upd_complete *evt);
@@ -79,14 +80,20 @@ int ble_gap_rx_l2cap_update_req(uint16_t conn_handle,
 void ble_gap_enc_event(uint16_t conn_handle, int status,
                        int security_restored);
 void ble_gap_passkey_event(uint16_t conn_handle,
-                           struct ble_gap_passkey_action *passkey_action);
-void ble_gap_notify_event(uint16_t conn_handle, uint16_t attr_handle,
-                          void *attr_data, uint16_t attr_len,
-                          int is_indication);
+                           struct ble_gap_passkey_params *passkey_params);
+void ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle,
+                             struct os_mbuf *om, int is_indication);
+void ble_gap_notify_tx_event(int status, uint16_t conn_handle,
+                             uint16_t attr_handle, int is_indication);
+void ble_gap_subscribe_event(uint16_t conn_handle, uint16_t attr_handle,
+                             uint8_t reason,
+                             uint8_t prev_notify, uint8_t cur_notify,
+                             uint8_t prev_indicate, uint8_t cur_indicate);
+void ble_gap_mtu_event(uint16_t conn_handle, uint16_t cid, uint16_t mtu);
 int ble_gap_master_in_progress(void);
-int ble_gap_slave_in_progress(void);
 
-uint32_t ble_gap_heartbeat(void);
+void ble_gap_conn_broken(uint16_t conn_handle, int reason);
+int32_t ble_gap_heartbeat(void);
 
 int ble_gap_init(void);
 
diff --git a/net/nimble/host/src/ble_gatt_priv.h b/net/nimble/host/src/ble_gatt_priv.h
index 5955b913..7c0f020a 100644
--- a/net/nimble/host/src/ble_gatt_priv.h
+++ b/net/nimble/host/src/ble_gatt_priv.h
@@ -84,8 +84,6 @@ extern STATS_SECT_DECL(ble_gatts_stats) ble_gatts_stats;
 #define BLE_GATT_CHR_DECL_SZ_16     5
 #define BLE_GATT_CHR_DECL_SZ_128    19
 
-#define BLE_GATT_DSC_CLT_CFG_UUID16 0x2902
-
 typedef uint8_t ble_gatts_conn_flags;
 
 struct ble_gatts_conn {
@@ -97,22 +95,19 @@ struct ble_gatts_conn {
 
 /*** @client. */
 int ble_gattc_locked_by_cur_task(void);
-int ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle, void *value,
-                    uint16_t value_len, ble_gatt_attr_fn *cb, void *cb_arg);
-int ble_gattc_indicate(uint16_t conn_handle, uint16_t chr_val_handle,
-                       ble_gatt_attr_fn *cb, void *cb_arg);
+void ble_gatts_indicate_fail_notconn(uint16_t conn_handle);
 
 void ble_gattc_rx_err(uint16_t conn_handle, struct ble_att_error_rsp *rsp);
 void ble_gattc_rx_mtu(uint16_t conn_handle, int status, uint16_t chan_mtu);
 void ble_gattc_rx_read_type_adata(uint16_t conn_handle,
                                   struct ble_att_read_type_adata *adata);
 void ble_gattc_rx_read_type_complete(uint16_t conn_handle, int status);
-void ble_gattc_rx_read_rsp(uint16_t conn_handle, int status, void *value,
-                           int value_len);
+void ble_gattc_rx_read_rsp(uint16_t conn_handle, int status,
+                           struct os_mbuf **rxom);
 void ble_gattc_rx_read_blob_rsp(uint16_t conn_handle, int status,
-                                void *value, int value_len);
+                                struct os_mbuf **rxom);
 void ble_gattc_rx_read_mult_rsp(uint16_t conn_handle, int status,
-                                void *value, int value_len);
+                                struct os_mbuf **rxom);
 void ble_gattc_rx_read_group_type_adata(
     uint16_t conn_handle, struct ble_att_read_group_type_adata *adata);
 void ble_gattc_rx_read_group_type_complete(uint16_t conn_handle, int rc);
@@ -122,7 +117,7 @@ void ble_gattc_rx_find_type_value_complete(uint16_t conn_handle, int status);
 void ble_gattc_rx_write_rsp(uint16_t conn_handle);
 void ble_gattc_rx_prep_write_rsp(uint16_t conn_handle, int status,
                                  struct ble_att_prep_write_cmd *rsp,
-                                 void *attr_data, uint16_t attr_data_len);
+                                 struct os_mbuf **rxom);
 void ble_gattc_rx_exec_write_rsp(uint16_t conn_handle, int status);
 void ble_gattc_rx_indicate_rsp(uint16_t conn_handle);
 void ble_gattc_rx_find_info_idata(uint16_t conn_handle,
@@ -130,7 +125,7 @@ void ble_gattc_rx_find_info_idata(uint16_t conn_handle,
 void ble_gattc_rx_find_info_complete(uint16_t conn_handle, int status);
 void ble_gattc_connection_txable(uint16_t conn_handle);
 void ble_gattc_connection_broken(uint16_t conn_handle);
-uint32_t ble_gattc_heartbeat(void);
+int32_t ble_gattc_heartbeat(void);
 
 int ble_gattc_any_jobs(void);
 int ble_gattc_init(void);
@@ -138,7 +133,7 @@ int ble_gattc_init(void);
 /*** @server. */
 #define BLE_GATTS_CLT_CFG_F_NOTIFY              0x0001
 #define BLE_GATTS_CLT_CFG_F_INDICATE            0x0002
-#define BLE_GATTS_CLT_CFG_F_INDICATE_PENDING    0x0080 /* Internal only. */
+#define BLE_GATTS_CLT_CFG_F_MODIFIED            0x0080 /* Internal only. */
 #define BLE_GATTS_CLT_CFG_F_RESERVED            0xfffc
 
 #define BLE_GATTS_INC_SVC_LEN_NO_UUID           4
@@ -146,10 +141,11 @@ int ble_gattc_init(void);
 
 int ble_gatts_rx_indicate_ack(uint16_t conn_handle, uint16_t chr_val_handle);
 int ble_gatts_send_next_indicate(uint16_t conn_handle);
+void ble_gatts_tx_notifications(void);
 void ble_gatts_bonding_restored(uint16_t conn_handle);
+void ble_gatts_connection_broken(uint16_t conn_handle);
 
 /*** @misc. */
-void ble_gatts_conn_deinit(struct ble_gatts_conn *gatts_conn);
 int ble_gatts_conn_can_alloc(void);
 int ble_gatts_conn_init(struct ble_gatts_conn *gatts_conn);
 int ble_gatts_start(void);
diff --git a/net/nimble/host/src/ble_gattc.c b/net/nimble/host/src/ble_gattc.c
index 29e6b4ef..3dee6799 100644
--- a/net/nimble/host/src/ble_gattc.c
+++ b/net/nimble/host/src/ble_gattc.c
@@ -138,7 +138,7 @@ struct ble_gattc_proc {
         } disc_chr_uuid;
 
         struct {
-            uint16_t chr_def_handle;
+            uint16_t chr_val_handle;
             uint16_t prev_handle;
             uint16_t end_handle;
             ble_gatt_dsc_fn *cb;
@@ -182,17 +182,16 @@ struct ble_gattc_proc {
         } write_long;
 
         struct {
-            struct ble_gatt_attr *attrs;
-            int num_attrs;
-            int cur_attr;
+            struct ble_gatt_attr attrs[NIMBLE_OPT(GATT_WRITE_MAX_ATTRS)];
+            uint8_t num_attrs;
+            uint8_t cur_attr;
+            uint16_t length;
             ble_gatt_reliable_attr_fn *cb;
             void *cb_arg;
         } write_reliable;
 
         struct {
-            ble_gatt_attr_fn *cb;
             uint16_t chr_val_handle;
-            void *cb_arg;
         } indicate;
     };
 };
@@ -248,10 +247,10 @@ typedef int ble_gattc_rx_adata_fn(struct ble_gattc_proc *proc,
 
 typedef int ble_gattc_rx_prep_fn(struct ble_gattc_proc *proc, int status,
                                  struct ble_att_prep_write_cmd *rsp,
-                                 void *attr_data, uint16_t attr_len);
+                                 struct os_mbuf **om);
 
 typedef int ble_gattc_rx_attr_fn(struct ble_gattc_proc *proc, int status,
-                                 void *value, int value_len);
+                                 struct os_mbuf **om);
 
 typedef int ble_gattc_rx_complete_fn(struct ble_gattc_proc *proc, int status);
 typedef int ble_gattc_rx_exec_fn(struct ble_gattc_proc *proc, int status);
@@ -393,9 +392,9 @@ ble_gattc_log_proc_init(char *name)
 }
 
 static void
-ble_gattc_log_uuid(void *uuid128)
+ble_gattc_log_uuid(const void *uuid128)
 {
-    uint8_t *u8p;
+    const uint8_t *u8p;
 
     u8p = uuid128;
     BLE_HS_LOG(INFO, "%02x%02x%02x%02x-%02x%02x-%02x%02x-"
@@ -447,8 +446,8 @@ static void
 ble_gattc_log_disc_all_dscs(struct ble_gattc_proc *proc)
 {
     ble_gattc_log_proc_init("discover all descriptors; ");
-    BLE_HS_LOG(INFO, "chr_def_handle=%d end_handle=%d\n",
-               proc->disc_all_dscs.chr_def_handle,
+    BLE_HS_LOG(INFO, "chr_val_handle=%d end_handle=%d\n",
+               proc->disc_all_dscs.chr_val_handle,
                proc->disc_all_dscs.end_handle);
 }
 
@@ -461,7 +460,7 @@ ble_gattc_log_read(uint16_t att_handle)
 
 static void
 ble_gattc_log_read_uuid(uint16_t start_handle, uint16_t end_handle,
-                        uint8_t *uuid128)
+                        const uint8_t *uuid128)
 {
     uint16_t uuid16;
 
@@ -486,7 +485,7 @@ ble_gattc_log_read_long(struct ble_gattc_proc *proc)
 }
 
 static void
-ble_gattc_log_read_mult(uint16_t *handles, uint8_t num_handles)
+ble_gattc_log_read_mult(const uint16_t *handles, uint8_t num_handles)
 {
     int i;
 
@@ -518,7 +517,8 @@ ble_gattc_log_write_long(struct ble_gattc_proc *proc)
 {
     ble_gattc_log_proc_init("write long; ");
     BLE_HS_LOG(INFO, "att_handle=%d len=%d\n",
-               proc->write_long.attr.handle, proc->write_long.attr.value_len);
+               proc->write_long.attr.handle,
+               OS_MBUF_PKTLEN(proc->write_long.attr.om));
 }
 
 static void
@@ -603,10 +603,26 @@ static void
 ble_gattc_proc_free(struct ble_gattc_proc *proc)
 {
     int rc;
+    int i;
 
     if (proc != NULL) {
         ble_gattc_dbg_assert_proc_not_inserted(proc);
 
+        switch (proc->op) {
+        case BLE_GATT_OP_WRITE_LONG:
+            os_mbuf_free_chain(proc->write_long.attr.om);
+            break;
+
+        case BLE_GATT_OP_WRITE_RELIABLE:
+            for (i = 0; i < proc->write_reliable.num_attrs; i++) {
+                os_mbuf_free_chain(proc->write_reliable.attrs[i].om);
+            }
+            break;
+
+        default:
+            break;
+        }
+
         rc = os_memblock_put(&ble_gattc_proc_pool, proc);
         BLE_HS_DBG_ASSERT_EVAL(rc == 0);
     }
@@ -618,7 +634,7 @@ ble_gattc_proc_insert(struct ble_gattc_proc *proc)
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
     ble_hs_lock();
-    STAILQ_INSERT_HEAD(&ble_gattc_procs, proc, next);
+    STAILQ_INSERT_TAIL(&ble_gattc_procs, proc, next);
     ble_hs_unlock();
 }
 
@@ -707,9 +723,26 @@ ble_gattc_extract(uint16_t conn_handle, uint8_t op)
     return proc;
 }
 
+static int
+ble_gattc_conn_op_matches(struct ble_gattc_proc *proc, uint16_t conn_handle,
+                          uint8_t op)
+{
+    if (conn_handle != BLE_HS_CONN_HANDLE_NONE &&
+        conn_handle != proc->conn_handle) {
+
+        return 0;
+    }
+
+    if (op != BLE_GATT_OP_NONE && op != proc->op) {
+        return 0;
+    }
+
+    return 1;
+}
+
 static void
-ble_gattc_extract_by_conn(uint16_t conn_handle,
-                          struct ble_gattc_proc_list *dst_list)
+ble_gattc_extract_by_conn_op(uint16_t conn_handle, uint8_t op,
+                             struct ble_gattc_proc_list *dst_list)
 {
     struct ble_gattc_proc *proc;
     struct ble_gattc_proc *prev;
@@ -727,7 +760,7 @@ ble_gattc_extract_by_conn(uint16_t conn_handle,
     while (proc != NULL) {
         next = STAILQ_NEXT(proc, next);
 
-        if (proc->conn_handle == conn_handle) {
+        if (ble_gattc_conn_op_matches(proc, conn_handle, op)) {
             if (prev == NULL) {
                 STAILQ_REMOVE_HEAD(&ble_gattc_procs, next);
             } else {
@@ -842,6 +875,35 @@ ble_gattc_extract_with_rx_entry(uint16_t conn_handle,
         sizeof (rx_entries) / sizeof (rx_entries)[0],                         \
         (const void **)(out_rx_entry))
 
+
+/**
+ * Causes all GATT procedures matching the specified criteria to fail with the
+ * specified status code.
+ */
+static void
+ble_gattc_fail_procs(uint16_t conn_handle, uint8_t op, int status)
+{
+    struct ble_gattc_proc_list temp_list;
+    struct ble_gattc_proc *proc;
+    ble_gattc_err_fn *err_cb;
+
+    /* Remove all procs with the specified conn handle-op-pair and insert them
+     * into the temporary list.
+     */
+    ble_gattc_extract_by_conn_op(conn_handle, op, &temp_list);
+
+    /* Notify application of failed procedures and free the corresponding proc
+     * entries.
+     */
+    while ((proc = STAILQ_FIRST(&temp_list)) != NULL) {
+        err_cb = ble_gattc_err_dispatch_get(proc->op);
+        err_cb(proc, status, 0);
+
+        STAILQ_REMOVE_HEAD(&temp_list, next);
+        ble_gattc_proc_free(proc);
+    }
+}
+
 /**
  * Applies periodic checks and actions to all active procedures.
  *
@@ -854,7 +916,7 @@ ble_gattc_extract_with_rx_entry(uint16_t conn_handle,
  *                                  be called again; currently always
  *                                  UINT32_MAX.
  */
-uint32_t
+int32_t
 ble_gattc_heartbeat(void)
 {
     struct ble_gattc_proc_list exp_list;
@@ -869,13 +931,13 @@ ble_gattc_heartbeat(void)
     /* Terminate the connection associated with each timed-out procedure. */
     while ((proc = STAILQ_FIRST(&exp_list)) != NULL) {
         STATS_INC(ble_gattc_stats, proc_timeout);
-        ble_gap_terminate(proc->conn_handle);
+        ble_gap_terminate(proc->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
 
         STAILQ_REMOVE_HEAD(&exp_list, next);
         ble_gattc_proc_free(proc);
     }
 
-    return UINT32_MAX;
+    return BLE_HS_FOREVER;
 }
 
 /**
@@ -883,13 +945,14 @@ ble_gattc_heartbeat(void)
  * returned object is statically allocated, so this function is not reentrant.
  * This function should only ever be called by the ble_hs task.
  */
-struct ble_gatt_error *
+static struct ble_gatt_error *
 ble_gattc_error(int status, uint16_t att_handle)
 {
     static struct ble_gatt_error error;
 
-    if (status == 0) {
-        return NULL;
+    /* For consistency, always indicate a handle of 0 on success. */
+    if (status == 0 || status == BLE_HS_EDONE) {
+        att_handle = 0;
     }
 
     error.status = status;
@@ -917,7 +980,7 @@ ble_gattc_mtu_cb(struct ble_gattc_proc *proc, int status, uint16_t att_handle,
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, mtu_fail);
     }
 
@@ -949,7 +1012,10 @@ ble_gattc_mtu_err(struct ble_gattc_proc *proc, int status, uint16_t att_handle)
  *                                  procedure.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
 ble_gattc_exchange_mtu(uint16_t conn_handle, ble_gatt_mtu_fn *cb, void *cb_arg)
@@ -976,16 +1042,10 @@ ble_gattc_exchange_mtu(uint16_t conn_handle, ble_gatt_mtu_fn *cb, void *cb_arg)
     ble_gattc_log_proc_init("exchange mtu\n");
 
     ble_hs_lock();
-    rc = ble_att_conn_chan_find(proc->conn_handle, &conn, &chan);
-    if (rc == 0) {
-        req.bamc_mtu = chan->blc_my_mtu;
-    }
+    ble_att_conn_chan_find(proc->conn_handle, &conn, &chan);
+    req.bamc_mtu = chan->blc_my_mtu;
     ble_hs_unlock();
 
-    if (rc != 0) {
-        goto done;
-    }
-
     rc = ble_att_clt_tx_mtu(proc->conn_handle, &req);
     if (rc != 0) {
         goto done;
@@ -1019,9 +1079,10 @@ ble_gattc_disc_all_svcs_cb(struct ble_gattc_proc *proc,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(service != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, disc_all_svcs_fail);
     }
 
@@ -1077,7 +1138,7 @@ ble_gattc_disc_all_svcs_err(struct ble_gattc_proc *proc, int status,
 
     if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
         /* Discovery is complete. */
-        status = 0;
+        status = BLE_HS_EDONE;
     }
 
     ble_gattc_disc_all_svcs_cb(proc, status, att_handle, NULL);
@@ -1149,19 +1210,24 @@ ble_gattc_disc_all_svcs_rx_complete(struct ble_gattc_proc *proc, int status)
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0 || proc->disc_all_svcs.prev_handle == 0xffff) {
-        /* Error or all svcs discovered. */
+    if (status != 0) {
         ble_gattc_disc_all_svcs_cb(proc, status, 0, NULL);
         return BLE_HS_EDONE;
-    } else {
-        /* Send follow-up request. */
-        rc = ble_gattc_disc_all_svcs_go(proc, 1);
-        if (rc != 0) {
-            return BLE_HS_EDONE;
-        }
+    }
 
-        return 0;
+    if (proc->disc_all_svcs.prev_handle == 0xffff) {
+        /* Service discovery complete. */
+        ble_gattc_disc_all_svcs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
     }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_all_svcs_go(proc, 1);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    return 0;
 }
 
 /**
@@ -1171,7 +1237,8 @@ ble_gattc_disc_all_svcs_rx_complete(struct ble_gattc_proc *proc, int status)
  *                                  procedure.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
  */
 int
 ble_gattc_disc_all_svcs(uint16_t conn_handle, ble_gatt_disc_svc_fn *cb,
@@ -1233,9 +1300,10 @@ ble_gattc_disc_svc_uuid_cb(struct ble_gattc_proc *proc, int status,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(service != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, disc_svc_uuid_fail);
     }
 
@@ -1289,7 +1357,7 @@ ble_gattc_disc_svc_uuid_err(struct ble_gattc_proc *proc, int status,
 
     if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
         /* Discovery is complete. */
-        status = 0;
+        status = BLE_HS_EDONE;
     }
 
     ble_gattc_disc_svc_uuid_cb(proc, status, att_handle, NULL);
@@ -1343,18 +1411,24 @@ ble_gattc_disc_svc_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0 || proc->disc_svc_uuid.prev_handle == 0xffff) {
-        /* Error or all svcs discovered. */
+    if (status != 0) {
         ble_gattc_disc_svc_uuid_cb(proc, status, 0, NULL);
         return BLE_HS_EDONE;
-    } else {
-        /* Send follow-up request. */
-        rc = ble_gattc_disc_svc_uuid_go(proc, 1);
-        if (rc != 0) {
-            return BLE_HS_EDONE;
-        }
-        return 0;
     }
+
+    if (proc->disc_svc_uuid.prev_handle == 0xffff) {
+        /* Service discovery complete. */
+        ble_gattc_disc_svc_uuid_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_svc_uuid_go(proc, 1);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    return 0;
 }
 
 /**
@@ -1365,10 +1439,13 @@ ble_gattc_disc_svc_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
  * @param service_uuid128       The 128-bit UUID of the service to discover.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, void *service_uuid128,
+ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, const void *svc_uuid128,
                            ble_gatt_disc_svc_fn *cb, void *cb_arg)
 {
 #if !NIMBLE_OPT(GATT_DISC_SVC_UUID)
@@ -1388,7 +1465,7 @@ ble_gattc_disc_svc_by_uuid(uint16_t conn_handle, void *service_uuid128,
 
     proc->op = BLE_GATT_OP_DISC_SVC_UUID;
     proc->conn_handle = conn_handle;
-    memcpy(proc->disc_svc_uuid.service_uuid, service_uuid128, 16);
+    memcpy(proc->disc_svc_uuid.service_uuid, svc_uuid128, 16);
     proc->disc_svc_uuid.prev_handle = 0x0000;
     proc->disc_svc_uuid.cb = cb;
     proc->disc_svc_uuid.cb_arg = cb_arg;
@@ -1428,9 +1505,10 @@ ble_gattc_find_inc_svcs_cb(struct ble_gattc_proc *proc, int status,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(service != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, find_inc_svcs_fail);
     }
 
@@ -1499,7 +1577,7 @@ ble_gattc_find_inc_svcs_err(struct ble_gattc_proc *proc, int status,
         status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
 
         /* Discovery is complete. */
-        status = 0;
+        status = BLE_HS_EDONE;
     }
 
     ble_gattc_find_inc_svcs_cb(proc, status, att_handle, NULL);
@@ -1511,52 +1589,57 @@ ble_gattc_find_inc_svcs_err(struct ble_gattc_proc *proc, int status,
  */
 static int
 ble_gattc_find_inc_svcs_rx_read_rsp(struct ble_gattc_proc *proc, int status,
-                                    void *value, int value_len)
+                                    struct os_mbuf **om)
 {
     struct ble_gatt_svc service;
-    int cbrc;
+    uint16_t om_len;
     int rc;
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
+    rc = ble_hs_mbuf_to_flat(*om, service.uuid128, 16, &om_len);
+    os_mbuf_free_chain(*om);
+    *om = NULL;
+
+    if (rc != 0 || om_len != 16) {
+        /* Invalid UUID. */
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
+
     if (proc->find_inc_svcs.cur_start == 0) {
         /* Unexpected read response; terminate procedure. */
         rc = BLE_HS_EBADDATA;
-        goto done;
+        goto err;
     }
 
     if (status != 0) {
         rc = status;
-        goto done;
-    }
-
-    if (value_len != 16) {
-        /* Invalid UUID. */
-        rc = BLE_HS_EBADDATA;
-        goto done;
+        goto err;
     }
 
+    /* Report discovered service to application. */
     service.start_handle = proc->find_inc_svcs.cur_start;
     service.end_handle = proc->find_inc_svcs.cur_end;
-    memcpy(service.uuid128, value, 16);
+    rc = ble_gattc_find_inc_svcs_cb(proc, 0, 0, &service);
+    if (rc != 0) {
+        /* Application has indicated that the procedure should be aborted. */
+        return BLE_HS_EDONE;
+    }
 
-    /* We are done with this service; proceed to the next. */
+    /* Proceed to the next service. */
     proc->find_inc_svcs.cur_start = 0;
     proc->find_inc_svcs.cur_end = 0;
     rc = ble_gattc_find_inc_svcs_go(proc, 1);
     if (rc != 0) {
-        goto done;
+        goto err;
     }
 
-    rc = 0;
+    return 0;
 
-done:
-    cbrc = ble_gattc_find_inc_svcs_cb(proc, rc, 0, &service);
-    if (rc != 0 || cbrc != 0) {
-        return BLE_HS_EDONE;
-    } else {
-        return 0;
-    }
+err:
+    ble_gattc_find_inc_svcs_cb(proc, rc, 0, NULL);
+    return BLE_HS_EDONE;
 }
 
 /**
@@ -1646,18 +1729,23 @@ ble_gattc_find_inc_svcs_rx_complete(struct ble_gattc_proc *proc, int status)
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0 || proc->find_inc_svcs.prev_handle == 0xffff) {
-        /* Error or all svcs discovered. */
+    if (status != 0) {
         ble_gattc_find_inc_svcs_cb(proc, status, 0, NULL);
         return BLE_HS_EDONE;
-    } else {
-        /* Send follow-up request. */
-        rc = ble_gattc_find_inc_svcs_go(proc, 1);
-        if (rc != 0) {
-            return BLE_HS_EDONE;
-        }
-        return 0;
     }
+
+    if (proc->find_inc_svcs.prev_handle == 0xffff) {
+        /* Procedure complete. */
+        ble_gattc_find_inc_svcs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_find_inc_svcs_go(proc, 1);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+    return 0;
 }
 
 /**
@@ -1671,7 +1759,10 @@ ble_gattc_find_inc_svcs_rx_complete(struct ble_gattc_proc *proc, int status)
  *                                  last handle in the service).
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
 ble_gattc_find_inc_svcs(uint16_t conn_handle, uint16_t start_handle,
@@ -1734,9 +1825,10 @@ ble_gattc_disc_all_chrs_cb(struct ble_gattc_proc *proc, int status,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(chr != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, disc_all_chrs_fail);
     }
 
@@ -1794,7 +1886,7 @@ ble_gattc_disc_all_chrs_err(struct ble_gattc_proc *proc, int status,
 
     if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
         /* Discovery is complete. */
-        status = 0;
+        status = BLE_HS_EDONE;
     }
 
     ble_gattc_disc_all_chrs_cb(proc, status, att_handle, NULL);
@@ -1869,20 +1961,23 @@ ble_gattc_disc_all_chrs_rx_complete(struct ble_gattc_proc *proc, int status)
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0 ||
-        proc->disc_all_chrs.prev_handle == proc->disc_all_chrs.end_handle) {
-
-        /* Error or all svcs discovered. */
+    if (status != 0) {
         ble_gattc_disc_all_chrs_cb(proc, status, 0, NULL);
         return BLE_HS_EDONE;
-    } else {
-        /* Send follow-up request. */
-        rc = ble_gattc_disc_all_chrs_go(proc, 1);
-        if (rc != 0) {
-            return BLE_HS_EDONE;
-        }
-        return 0;
     }
+
+    if (proc->disc_all_chrs.prev_handle == proc->disc_all_chrs.end_handle) {
+        /* Characteristic discovery complete. */
+        ble_gattc_disc_all_chrs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_all_chrs_go(proc, 1);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+    return 0;
 }
 
 /**
@@ -1896,7 +1991,10 @@ ble_gattc_disc_all_chrs_rx_complete(struct ble_gattc_proc *proc, int status)
  *                                  last handle in the service).
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
 ble_gattc_disc_all_chrs(uint16_t conn_handle, uint16_t start_handle,
@@ -1959,9 +2057,10 @@ ble_gattc_disc_chr_uuid_cb(struct ble_gattc_proc *proc, int status,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(chr != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, disc_chrs_uuid_fail);
     }
 
@@ -2019,7 +2118,7 @@ ble_gattc_disc_chr_uuid_err(struct ble_gattc_proc *proc, int status,
 
     if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
         /* Discovery is complete. */
-        status = 0;
+        status = BLE_HS_EDONE;
     }
 
     ble_gattc_disc_chr_uuid_cb(proc, status, att_handle, NULL);
@@ -2076,11 +2175,14 @@ ble_gattc_disc_chr_uuid_rx_adata(struct ble_gattc_proc *proc,
     rc = 0;
 
 done:
-    if (rc != 0 ||
-        memcmp(chr.uuid128, proc->disc_chr_uuid.chr_uuid, 16) == 0) {
-
-        cbrc = ble_gattc_disc_chr_uuid_cb(proc, rc, 0, &chr);
+    if (rc != 0) {
+        /* Failure. */
+        cbrc = ble_gattc_disc_chr_uuid_cb(proc, rc, 0, NULL);
+    } else if (memcmp(chr.uuid128, proc->disc_chr_uuid.chr_uuid, 16) == 0) {
+        /* Requested characteristic discovered. */
+        cbrc = ble_gattc_disc_chr_uuid_cb(proc, 0, 0, &chr);
     } else {
+        /* Uninteresting characteristic; ignore. */
         cbrc = 0;
     }
 
@@ -2102,20 +2204,23 @@ ble_gattc_disc_chr_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0 ||
-        proc->disc_chr_uuid.prev_handle == proc->disc_chr_uuid.end_handle) {
-
-        /* Error or all svcs discovered. */
+    if (status != 0) {
         ble_gattc_disc_chr_uuid_cb(proc, status, 0, NULL);
         return BLE_HS_EDONE;
-    } else {
-        /* Send follow-up request. */
-        rc = ble_gattc_disc_chr_uuid_go(proc, 1);
-        if (rc != 0) {
-            return BLE_HS_EDONE;
-        }
-        return 0;
     }
+
+    if (proc->disc_chr_uuid.prev_handle == proc->disc_chr_uuid.end_handle) {
+        /* Characteristic discovery complete. */
+        ble_gattc_disc_chr_uuid_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_chr_uuid_go(proc, 1);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+    return 0;
 }
 
 /**
@@ -2131,11 +2236,14 @@ ble_gattc_disc_chr_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
  *                                  discover.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
 ble_gattc_disc_chrs_by_uuid(uint16_t conn_handle, uint16_t start_handle,
-                            uint16_t end_handle, void *uuid128,
+                            uint16_t end_handle, const void *uuid128,
                             ble_gatt_chr_fn *cb, void *cb_arg)
 {
 #if !NIMBLE_OPT(GATT_DISC_CHR_UUID)
@@ -2195,9 +2303,10 @@ ble_gattc_disc_all_dscs_cb(struct ble_gattc_proc *proc, int status,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(dsc != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, disc_all_dscs_fail);
     }
 
@@ -2206,7 +2315,7 @@ ble_gattc_disc_all_dscs_cb(struct ble_gattc_proc *proc, int status,
     } else {
         rc = proc->disc_all_dscs.cb(proc->conn_handle,
                                     ble_gattc_error(status, att_handle),
-                                    proc->disc_all_dscs.chr_def_handle,
+                                    proc->disc_all_dscs.chr_val_handle,
                                     dsc, proc->disc_all_dscs.cb_arg);
     }
 
@@ -2250,7 +2359,7 @@ ble_gattc_disc_all_dscs_err(struct ble_gattc_proc *proc, int status,
 
     if (status == BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_FOUND)) {
         /* Discovery is complete. */
-        status = 0;
+        status = BLE_HS_EDONE;
     }
 
     ble_gattc_disc_all_dscs_cb(proc, status, att_handle, NULL);
@@ -2302,20 +2411,24 @@ ble_gattc_disc_all_dscs_rx_complete(struct ble_gattc_proc *proc, int status)
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0 ||
-        proc->disc_all_dscs.prev_handle == proc->disc_all_dscs.end_handle) {
-
-        /* Error or all descriptors discovered. */
+    if (status != 0) {
         ble_gattc_disc_all_dscs_cb(proc, status, 0, NULL);
         return BLE_HS_EDONE;
-    } else {
-        /* Send follow-up request. */
-        rc = ble_gattc_disc_all_dscs_go(proc, 1);
-        if (rc != 0) {
-            return BLE_HS_EDONE;
-        }
-        return 0;
     }
+
+    if (proc->disc_all_dscs.prev_handle == proc->disc_all_dscs.end_handle) {
+        /* All descriptors discovered. */
+        ble_gattc_disc_all_dscs_cb(proc, BLE_HS_EDONE, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* Send follow-up request. */
+    rc = ble_gattc_disc_all_dscs_go(proc, 1);
+    if (rc != 0) {
+        return BLE_HS_EDONE;
+    }
+
+    return 0;
 }
 
 /**
@@ -2323,16 +2436,19 @@ ble_gattc_disc_all_dscs_rx_complete(struct ble_gattc_proc *proc, int status)
  *
  * @param conn_handle           The connection over which to execute the
  *                                  procedure.
- * @param chr_def_handle        The handle of the characteristic definition
+ * @param chr_val_handle        The handle of the characteristic value
  *                                  attribute.
  * @param chr_end_handle        The last handle in the characteristic
  *                                  definition.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t chr_def_handle,
+ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t chr_val_handle,
                         uint16_t chr_end_handle,
                         ble_gatt_dsc_fn *cb, void *cb_arg)
 {
@@ -2353,8 +2469,8 @@ ble_gattc_disc_all_dscs(uint16_t conn_handle, uint16_t chr_def_handle,
 
     proc->op = BLE_GATT_OP_DISC_ALL_DSCS;
     proc->conn_handle = conn_handle;
-    proc->disc_all_dscs.chr_def_handle = chr_def_handle;
-    proc->disc_all_dscs.prev_handle = chr_def_handle + 1;
+    proc->disc_all_dscs.chr_val_handle = chr_val_handle;
+    proc->disc_all_dscs.prev_handle = chr_val_handle;
     proc->disc_all_dscs.end_handle = chr_end_handle;
     proc->disc_all_dscs.cb = cb;
     proc->disc_all_dscs.cb_arg = cb_arg;
@@ -2393,9 +2509,10 @@ ble_gattc_read_cb(struct ble_gattc_proc *proc, int status,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(attr != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, read_fail);
     }
 
@@ -2428,7 +2545,7 @@ ble_gattc_read_err(struct ble_gattc_proc *proc, int status,
  */
 static int
 ble_gattc_read_rx_read_rsp(struct ble_gattc_proc *proc, int status,
-                           void *value, int value_len)
+                           struct os_mbuf **om)
 {
     struct ble_gatt_attr attr;
 
@@ -2436,11 +2553,13 @@ ble_gattc_read_rx_read_rsp(struct ble_gattc_proc *proc, int status,
 
     attr.handle = proc->read.handle;
     attr.offset = 0;
-    attr.value_len = value_len;
-    attr.value = value;
+    attr.om = *om;
 
     ble_gattc_read_cb(proc, status, 0, &attr);
 
+    /* Indicate to the caller whether the application consumed the mbuf. */
+    *om = attr.om;
+
     /* The read operation only has a single request / response exchange. */
     return BLE_HS_EDONE;
 }
@@ -2453,7 +2572,10 @@ ble_gattc_read_rx_read_rsp(struct ble_gattc_proc *proc, int status,
  * @param attr_handle           The handle of the characteristic value to read.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
 ble_gattc_read(uint16_t conn_handle, uint16_t attr_handle,
@@ -2516,9 +2638,10 @@ ble_gattc_read_uuid_cb(struct ble_gattc_proc *proc, int status,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(attr != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, read_uuid_fail);
     }
 
@@ -2565,10 +2688,18 @@ ble_gattc_read_uuid_rx_adata(struct ble_gattc_proc *proc,
 
     attr.handle = adata->att_handle;
     attr.offset = 0;
-    attr.value_len = adata->value_len;
-    attr.value = adata->value;
+    attr.om = ble_hs_mbuf_from_flat(adata->value, adata->value_len);
+    if (attr.om == NULL) {
+        rc = BLE_HS_ENOMEM;
+    } else {
+        rc = 0;
+    }
+
+    rc = ble_gattc_read_uuid_cb(proc, rc, 0, &attr);
+
+    /* Free the attribute mbuf if the application has not consumed it. */
+    os_mbuf_free_chain(attr.om);
 
-    rc = ble_gattc_read_uuid_cb(proc, 0, 0, &attr);
     if (rc != 0) {
         return BLE_HS_EDONE;
     }
@@ -2584,7 +2715,16 @@ static int
 ble_gattc_read_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
 {
     ble_gattc_dbg_assert_proc_not_inserted(proc);
-    ble_gattc_read_uuid_cb(proc, status, 0, NULL);
+
+    if (status != 0) {
+        ble_gattc_read_uuid_cb(proc, status, 0, NULL);
+        return BLE_HS_EDONE;
+    }
+
+    /* XXX: We may need to send a follow-up request to address the possibility
+     * of multiple characteristics with identical UUIDs.
+     */
+    ble_gattc_read_uuid_cb(proc, BLE_HS_EDONE, 0, NULL);
     return BLE_HS_EDONE;
 }
 
@@ -2599,11 +2739,14 @@ ble_gattc_read_uuid_rx_complete(struct ble_gattc_proc *proc, int status)
  *                                  last handle in the service definition).
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
 ble_gattc_read_by_uuid(uint16_t conn_handle, uint16_t start_handle,
-                       uint16_t end_handle, void *uuid128,
+                       uint16_t end_handle, const void *uuid128,
                        ble_gatt_attr_fn *cb, void *cb_arg)
 {
 #if !NIMBLE_OPT(GATT_READ_UUID)
@@ -2663,9 +2806,10 @@ ble_gattc_read_long_cb(struct ble_gattc_proc *proc, int status,
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(attr != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, read_long_fail);
     }
 
@@ -2729,21 +2873,27 @@ ble_gattc_read_long_err(struct ble_gattc_proc *proc, int status,
  */
 static int
 ble_gattc_read_long_rx_read_rsp(struct ble_gattc_proc *proc, int status,
-                                void *value, int value_len)
+                                struct os_mbuf **om)
 {
     struct ble_gatt_attr attr;
+    uint16_t data_len;
     uint16_t mtu;
     int rc;
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
+    data_len = OS_MBUF_PKTLEN(*om);
+
     attr.handle = proc->read_long.handle;
     attr.offset = proc->read_long.offset;
-    attr.value_len = value_len;
-    attr.value = value;
+    attr.om = *om;
 
     /* Report partial payload to application. */
     rc = ble_gattc_read_long_cb(proc, status, 0, &attr);
+
+    /* Indicate to the caller whether the application consumed the mbuf. */
+    *om = attr.om;
+
     if (rc != 0 || status != 0) {
         return BLE_HS_EDONE;
     }
@@ -2755,13 +2905,14 @@ ble_gattc_read_long_rx_read_rsp(struct ble_gattc_proc *proc, int status,
         return BLE_HS_EDONE;
     }
 
-    if (value_len < mtu - 1) {
-        ble_gattc_read_long_cb(proc, 0, 0, NULL);
+    if (data_len < mtu - 1) {
+        /* Response shorter than maximum allowed; read complete. */
+        ble_gattc_read_long_cb(proc, BLE_HS_EDONE, 0, NULL);
         return BLE_HS_EDONE;
     }
 
     /* Send follow-up request. */
-    proc->read_long.offset += value_len;
+    proc->read_long.offset += data_len;
     rc = ble_gattc_read_long_go(proc, 1);
     if (rc != 0) {
         return BLE_HS_EDONE;
@@ -2778,7 +2929,10 @@ ble_gattc_read_long_rx_read_rsp(struct ble_gattc_proc *proc, int status,
  * @param handle                The handle of the characteristic value to read.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
 ble_gattc_read_long(uint16_t conn_handle, uint16_t handle,
@@ -2835,38 +2989,40 @@ done:
  */
 static int
 ble_gattc_read_mult_cb(struct ble_gattc_proc *proc, int status,
-                       uint16_t att_handle, uint8_t *attr_data,
-                       uint16_t attr_data_len)
+                       uint16_t att_handle, struct os_mbuf **om)
 {
-    struct ble_gatt_attr *attrp;
     struct ble_gatt_attr attr;
     int rc;
 
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
+    BLE_HS_DBG_ASSERT(om != NULL || status != 0);
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, read_mult_fail);
     }
 
+    attr.handle = 0;
+    attr.offset = 0;
+    if (om == NULL) {
+        attr.om = NULL;
+    } else {
+        attr.om = *om;
+    }
+
     if (proc->read_mult.cb == NULL) {
         rc = 0;
     } else {
-        if (status != 0) {
-            attrp = NULL;
-        } else {
-            attrp = &attr;
-            attr.handle = 0;
-            attr.offset = 0;
-            attr.value_len = attr_data_len;
-            attr.value = attr_data;
-        }
-
         rc = proc->read_mult.cb(proc->conn_handle,
-                                ble_gattc_error(status, att_handle), attrp,
+                                ble_gattc_error(status, att_handle), &attr,
                                 proc->read_mult.cb_arg);
     }
 
+    /* Indicate to the caller whether the application consumed the mbuf. */
+    if (om != NULL) {
+        *om = attr.om;
+    }
+
     return rc;
 }
 
@@ -2879,7 +3035,7 @@ ble_gattc_read_mult_err(struct ble_gattc_proc *proc, int status,
                         uint16_t att_handle)
 {
     ble_gattc_dbg_assert_proc_not_inserted(proc);
-    ble_gattc_read_mult_cb(proc, status, att_handle, NULL, 0);
+    ble_gattc_read_mult_cb(proc, status, att_handle, NULL);
 }
 
 /**
@@ -2891,10 +3047,13 @@ ble_gattc_read_mult_err(struct ble_gattc_proc *proc, int status,
  * @param num_handles           The number of entries in the "handles" array.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gattc_read_mult(uint16_t conn_handle, uint16_t *handles,
+ble_gattc_read_mult(uint16_t conn_handle, const uint16_t *handles,
                     uint8_t num_handles, ble_gatt_attr_fn *cb,
                     void *cb_arg)
 {
@@ -2939,21 +3098,20 @@ done:
  *****************************************************************************/
 
 /**
- * Initiates GATT procedure: Write Without Response.
+ * Initiates GATT procedure: Write Without Response.  This function consumes
+ * the supplied mbuf regardless of the outcome.
  *
  * @param conn_handle           The connection over which to execute the
  *                                  procedure.
  * @param attr_handle           The handle of the characteristic value to write
  *                                  to.
- * @param value                 The value to write to the characteristic.
- * @param value_len             The number of bytes to write.
- * @param cb                    The function to call to report procedure status
- *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param txom                  The value to write to the characteristic.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle, void *value,
-                       uint16_t value_len)
+ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle,
+                       struct os_mbuf *txom)
 {
 #if !NIMBLE_OPT(GATT_WRITE_NO_RSP)
     return BLE_HS_ENOTSUP;
@@ -2964,10 +3122,10 @@ ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle, void *value,
 
     STATS_INC(ble_gattc_stats, write_no_rsp);
 
-    ble_gattc_log_write(attr_handle, value_len, 0);
+    ble_gattc_log_write(attr_handle, OS_MBUF_PKTLEN(txom), 0);
 
     req.bawq_handle = attr_handle;
-    rc = ble_att_clt_tx_write_cmd(conn_handle, &req, value, value_len);
+    rc = ble_att_clt_tx_write_cmd(conn_handle, &req, txom);
     if (rc != 0) {
         STATS_INC(ble_gattc_stats, write);
     }
@@ -2975,6 +3133,39 @@ ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle, void *value,
     return rc;
 }
 
+/**
+ * Initiates GATT procedure: Write Without Response.  This function consumes
+ * the supplied mbuf regardless of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attr_handle           The handle of the characteristic value to write
+ *                                  to.
+ * @param value                 The value to write to the characteristic.
+ * @param value_len             The number of bytes to write.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int
+ble_gattc_write_no_rsp_flat(uint16_t conn_handle, uint16_t attr_handle,
+                            const void *data, uint16_t data_len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_mbuf_from_flat(data, data_len);
+    if (om == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    rc = ble_gattc_write_no_rsp(conn_handle, attr_handle, om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
 /*****************************************************************************
  * $write                                                                    *
  *****************************************************************************/
@@ -2996,7 +3187,7 @@ ble_gattc_write_cb(struct ble_gattc_proc *proc, int status,
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, write_fail);
     }
 
@@ -3026,21 +3217,24 @@ ble_gattc_write_err(struct ble_gattc_proc *proc, int status,
 }
 
 /**
- * Initiates GATT procedure: Write Characteristic Value.
+ * Initiates GATT procedure: Write Characteristic Value.  This function
+ * consumes the supplied mbuf regardless of the outcome.
  *
  * @param conn_handle           The connection over which to execute the
  *                                  procedure.
  * @param attr_handle           The handle of the characteristic value to write
  *                                  to.
- * @param value                 The value to write to the characteristic.
- * @param value_len             The number of bytes to write.
+ * @param txom                  The value to write to the characteristic.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle, void *value,
-                uint16_t value_len, ble_gatt_attr_fn *cb, void *cb_arg)
+ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle,
+                struct os_mbuf *txom, ble_gatt_attr_fn *cb, void *cb_arg)
 {
 #if !NIMBLE_OPT(GATT_WRITE)
     return BLE_HS_ENOTSUP;
@@ -3064,10 +3258,11 @@ ble_gattc_write(uint16_t conn_handle, uint16_t attr_handle, void *value,
     proc->write.cb = cb;
     proc->write.cb_arg = cb_arg;
 
-    ble_gattc_log_write(attr_handle, value_len, 1);
+    ble_gattc_log_write(attr_handle, OS_MBUF_PKTLEN(txom), 1);
 
     req.bawq_handle = attr_handle;
-    rc = ble_att_clt_tx_write_req(conn_handle, &req, value, value_len);
+    rc = ble_att_clt_tx_write_req(conn_handle, &req, txom);
+    txom = NULL;
     if (rc != 0) {
         goto done;
     }
@@ -3077,10 +3272,50 @@ done:
         STATS_INC(ble_gattc_stats, write_fail);
     }
 
+    /* Free the mbuf in case the send failed. */
+    os_mbuf_free_chain(txom);
+
     ble_gattc_process_status(proc, rc);
     return rc;
 }
 
+/**
+ * Initiates GATT procedure: Write Characteristic Value (flat buffer version).
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param attr_handle           The handle of the characteristic value to write
+ *                                  to.
+ * @param value                 The value to write to the characteristic.
+ * @param value_len             The number of bytes to write.
+ * @param cb                    The function to call to report procedure status
+ *                                  updates; null for no callback.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int
+ble_gattc_write_flat(uint16_t conn_handle, uint16_t attr_handle,
+                     const void *data, uint16_t data_len,
+                     ble_gatt_attr_fn *cb, void *cb_arg)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_mbuf_from_flat(data, data_len);
+    if (om == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    rc = ble_gattc_write(conn_handle, attr_handle, om, cb, cb_arg);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
 /*****************************************************************************
  * $write long                                                               *
  *****************************************************************************/
@@ -3101,7 +3336,7 @@ ble_gattc_write_long_cb(struct ble_gattc_proc *proc, int status,
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, write_long_fail);
     }
 
@@ -3126,48 +3361,65 @@ ble_gattc_write_long_go(struct ble_gattc_proc *proc, int cb_on_err)
 {
     struct ble_att_prep_write_cmd prep_req;
     struct ble_att_exec_write_req exec_req;
-    void *value;
+    struct os_mbuf *om;
+    int write_len;
     int max_sz;
     int rc;
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (proc->write_long.attr.offset < proc->write_long.attr.value_len) {
-        max_sz = ble_att_mtu(proc->conn_handle) -
-                 BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
-        if (max_sz == 0) {
-            /* Not connected. */
-            rc = BLE_HS_ENOTCONN;
-        } else {
-            if (proc->write_long.attr.offset + max_sz >
-                proc->write_long.attr.value_len) {
+    om = NULL;
 
-                proc->write_long.length = proc->write_long.attr.value_len -
-                                          proc->write_long.attr.offset;
-            } else {
-                proc->write_long.length = max_sz;
-            }
+    max_sz = ble_att_mtu(proc->conn_handle) -
+             BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+    if (max_sz <= 0) {
+        /* Not connected. */
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
 
-            prep_req.bapc_handle = proc->write_long.attr.handle;
-            prep_req.bapc_offset = proc->write_long.attr.offset;
-            value = proc->write_long.attr.value + proc->write_long.attr.offset;
-            rc = ble_att_clt_tx_prep_write(proc->conn_handle,
-                                           &prep_req, value,
-                                           proc->write_long.length);
-        }
-    } else {
+    write_len = min(max_sz,
+                    OS_MBUF_PKTLEN(proc->write_long.attr.om) -
+                        proc->write_long.attr.offset);
+
+    if (write_len <= 0) {
         exec_req.baeq_flags = BLE_ATT_EXEC_WRITE_F_CONFIRM;
         rc = ble_att_clt_tx_exec_write(proc->conn_handle, &exec_req);
+        goto done;
+    }
+
+    proc->write_long.length = write_len;
+    om = ble_hs_mbuf_att_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
     }
 
+    rc = os_mbuf_appendfrom(om, proc->write_long.attr.om,
+                            proc->write_long.attr.offset,
+                            proc->write_long.length);
     if (rc != 0) {
-        if (cb_on_err) {
-            ble_gattc_write_long_cb(proc, rc, 0);
-        }
-        return rc;
+        rc = BLE_HS_ENOMEM;
+        goto done;
     }
 
-    return 0;
+    prep_req.bapc_handle = proc->write_long.attr.handle;
+    prep_req.bapc_offset = proc->write_long.attr.offset;
+
+    rc = ble_att_clt_tx_prep_write(proc->conn_handle, &prep_req, om);
+    om = NULL;
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    os_mbuf_free_chain(om);
+
+    if (rc != 0 && cb_on_err) {
+        ble_gattc_write_long_cb(proc, rc, 0);
+    }
+
+    return rc;
 }
 
 /**
@@ -3181,17 +3433,20 @@ ble_gattc_write_long_err(struct ble_gattc_proc *proc, int status,
     struct ble_att_exec_write_req exec_req;
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
-    ble_gattc_write_long_cb(proc, status, att_handle);
 
     /* If we have successfully queued any data, and the failure occurred before
      * we could send the execute write command, then erase all queued data.
      */
     if (proc->write_long.attr.offset > 0 &&
-        proc->write_long.attr.offset < proc->write_long.attr.value_len) {
+        proc->write_long.attr.offset <
+            OS_MBUF_PKTLEN(proc->write_long.attr.om)) {
 
         exec_req.baeq_flags = 0;
         ble_att_clt_tx_exec_write(proc->conn_handle, &exec_req);
     }
+
+    /* Report failure. */
+    ble_gattc_write_long_cb(proc, status, att_handle);
 }
 
 /**
@@ -3201,18 +3456,31 @@ ble_gattc_write_long_err(struct ble_gattc_proc *proc, int status,
 static int
 ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc,
                              int status, struct ble_att_prep_write_cmd *rsp,
-                             void *attr_data, uint16_t attr_len)
+                             struct os_mbuf **rxom)
 {
+    struct os_mbuf *om;
     int rc;
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
+    /* Let the caller free the mbuf. */
+    om = *rxom;
+
     if (status != 0) {
         rc = status;
         goto err;
     }
 
     /* Verify the response. */
+    if (proc->write_long.attr.offset >=
+        OS_MBUF_PKTLEN(proc->write_long.attr.om)) {
+
+        /* Expecting a prepare write response, not an execute write
+         * response.
+         */
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
     if (rsp->bapc_handle != proc->write_long.attr.handle) {
         rc = BLE_HS_EBADDATA;
         goto err;
@@ -3221,22 +3489,26 @@ ble_gattc_write_long_rx_prep(struct ble_gattc_proc *proc,
         rc = BLE_HS_EBADDATA;
         goto err;
     }
-    if (rsp->bapc_offset + attr_len > proc->write_long.attr.value_len) {
+    if (rsp->bapc_offset + OS_MBUF_PKTLEN(om) >
+        OS_MBUF_PKTLEN(proc->write_long.attr.om)) {
+
         rc = BLE_HS_EBADDATA;
         goto err;
     }
-    if (attr_len != proc->write_long.length) {
+    if (OS_MBUF_PKTLEN(om) != proc->write_long.length) {
         rc = BLE_HS_EBADDATA;
         goto err;
     }
-    if (memcmp(attr_data, proc->write_long.attr.value + rsp->bapc_offset,
-               attr_len) != 0) {
+    if (os_mbuf_cmpm(om, 0,
+                     proc->write_long.attr.om, rsp->bapc_offset,
+                     proc->write_long.length) != 0) {
+
         rc = BLE_HS_EBADDATA;
         goto err;
     }
 
     /* Send follow-up request. */
-    proc->write_long.attr.offset += attr_len;
+    proc->write_long.attr.offset += OS_MBUF_PKTLEN(om);
     rc = ble_gattc_write_long_go(proc, 1);
     if (rc != 0) {
         goto err;
@@ -3258,26 +3530,39 @@ static int
 ble_gattc_write_long_rx_exec(struct ble_gattc_proc *proc, int status)
 {
     ble_gattc_dbg_assert_proc_not_inserted(proc);
+
+    if (proc->write_long.attr.offset <
+        OS_MBUF_PKTLEN(proc->write_long.attr.om)) {
+
+        /* Expecting an execute write response, not a prepare write
+         * response.
+         */
+        return BLE_HS_EBADDATA;
+    }
+
     ble_gattc_write_long_cb(proc, status, 0);
     return BLE_HS_EDONE;
 }
 
 /**
- * Initiates GATT procedure: Write Long Characteristic Values.
+ * Initiates GATT procedure: Write Long Characteristic Values.  This function
+ * consumes the supplied mbuf regardless of the outcome.
  *
  * @param conn_handle           The connection over which to execute the
  *                                  procedure.
  * @param attr_handle           The handle of the characteristic value to write
  *                                  to.
- * @param value                 The value to write to the characteristic.
- * @param value_len             The number of bytes to write.
+ * @param txom                  The value to write to the characteristic.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gattc_write_long(uint16_t conn_handle, uint16_t attr_handle, void *value,
-                     uint16_t value_len, ble_gatt_attr_fn *cb, void *cb_arg)
+ble_gattc_write_long(uint16_t conn_handle, uint16_t attr_handle,
+                     struct os_mbuf *txom, ble_gatt_attr_fn *cb, void *cb_arg)
 {
 #if !NIMBLE_OPT(GATT_WRITE_LONG)
     return BLE_HS_ENOTSUP;
@@ -3298,11 +3583,13 @@ ble_gattc_write_long(uint16_t conn_handle, uint16_t attr_handle, void *value,
     proc->conn_handle = conn_handle;
     proc->write_long.attr.handle = attr_handle;
     proc->write_long.attr.offset = 0;
-    proc->write_long.attr.value = value;
-    proc->write_long.attr.value_len = value_len;
+    proc->write_long.attr.om = txom;
     proc->write_long.cb = cb;
     proc->write_long.cb_arg = cb_arg;
 
+    /* The mbuf is consumed by the procedure. */
+    txom = NULL;
+
     ble_gattc_log_write_long(proc);
 
     rc = ble_gattc_write_long_go(proc, 0);
@@ -3315,6 +3602,9 @@ done:
         STATS_INC(ble_gattc_stats, write_long_fail);
     }
 
+    /* Free the mbuf in case of failure. */
+    os_mbuf_free_chain(txom);
+
     ble_gattc_process_status(proc, rc);
     return rc;
 }
@@ -3339,7 +3629,7 @@ ble_gattc_write_reliable_cb(struct ble_gattc_proc *proc, int status,
     BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
+    if (status != 0 && status != BLE_HS_EDONE) {
         STATS_INC(ble_gattc_stats, write_reliable_fail);
     }
 
@@ -3366,31 +3656,65 @@ ble_gattc_write_reliable_go(struct ble_gattc_proc *proc, int cb_on_err)
     struct ble_att_prep_write_cmd prep_req;
     struct ble_att_exec_write_req exec_req;
     struct ble_gatt_attr *attr;
+    struct os_mbuf *om;
+    uint16_t max_sz;
     int attr_idx;
     int rc;
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
+    om = NULL;
+
     attr_idx = proc->write_reliable.cur_attr;
-    if (attr_idx < proc->write_reliable.num_attrs) {
-        attr = proc->write_reliable.attrs + attr_idx;
-        prep_req.bapc_handle = attr->handle;
-        prep_req.bapc_offset = 0;
-        rc = ble_att_clt_tx_prep_write(proc->conn_handle, &prep_req,
-                                       attr->value, attr->value_len);
-    } else {
+
+    if (attr_idx >= proc->write_reliable.num_attrs) {
         exec_req.baeq_flags = BLE_ATT_EXEC_WRITE_F_CONFIRM;
         rc = ble_att_clt_tx_exec_write(proc->conn_handle, &exec_req);
+        goto done;
+    }
+
+    attr = proc->write_reliable.attrs + attr_idx;
+
+    max_sz = ble_att_mtu(proc->conn_handle) - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+    if (max_sz <= 0) {
+        /* Not connected. */
+        rc = BLE_HS_ENOTCONN;
+        goto done;
+    }
+
+    proc->write_reliable.length =
+        min(max_sz, OS_MBUF_PKTLEN(attr->om) - attr->offset);
+
+    om = ble_hs_mbuf_att_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
     }
 
+    rc = os_mbuf_appendfrom(om, attr->om, attr->offset,
+                            proc->write_reliable.length);
     if (rc != 0) {
-        if (cb_on_err) {
-            ble_gattc_write_reliable_cb(proc, rc, 0);
-        }
-        return rc;
+        rc = BLE_HS_ENOMEM;
+        goto done;
     }
 
-    return 0;
+    prep_req.bapc_handle = attr->handle;
+    prep_req.bapc_offset = attr->offset;
+
+    rc = ble_att_clt_tx_prep_write(proc->conn_handle, &prep_req, om);
+    om = NULL;
+    if (rc != 0) {
+        goto done;
+    }
+
+done:
+    os_mbuf_free_chain(om);
+
+    if (rc != 0 && cb_on_err) {
+        ble_gattc_write_reliable_cb(proc, rc, 0);
+    }
+
+    return rc;
 }
 
 /**
@@ -3425,19 +3749,26 @@ static int
 ble_gattc_write_reliable_rx_prep(struct ble_gattc_proc *proc,
                                  int status,
                                  struct ble_att_prep_write_cmd *rsp,
-                                 void *attr_data, uint16_t attr_len)
+                                 struct os_mbuf **rxom)
 {
     struct ble_gatt_attr *attr;
+    struct os_mbuf *om;
     int rc;
 
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
+    /* Let the caller free the mbuf. */
+    om = *rxom;
+
     if (status != 0) {
         rc = status;
         goto err;
     }
 
     if (proc->write_reliable.cur_attr >= proc->write_reliable.num_attrs) {
+        /* Expecting an execute write response, not a prepare write
+         * response.
+         */
         rc = BLE_HS_EBADDATA;
         goto err;
     }
@@ -3448,21 +3779,23 @@ ble_gattc_write_reliable_rx_prep(struct ble_gattc_proc *proc,
         rc = BLE_HS_EBADDATA;
         goto err;
     }
-    if (rsp->bapc_offset != 0) {
+    if (rsp->bapc_offset != attr->offset) {
         rc = BLE_HS_EBADDATA;
         goto err;
     }
-    if (attr_len != attr->value_len) {
-        rc = BLE_HS_EBADDATA;
-        goto err;
-    }
-    if (memcmp(attr_data, attr->value, attr_len) != 0) {
+    if (os_mbuf_cmpm(attr->om, rsp->bapc_offset, om, 0,
+                     proc->write_reliable.length) != 0) {
+
         rc = BLE_HS_EBADDATA;
         goto err;
     }
 
     /* Send follow-up request. */
-    proc->write_reliable.cur_attr++;
+    attr->offset += proc->write_reliable.length;
+    if (attr->offset >= OS_MBUF_PKTLEN(attr->om)) {
+        attr->offset = 0;
+        proc->write_reliable.cur_attr++;
+    }
     rc = ble_gattc_write_reliable_go(proc, 1);
     if (rc != 0) {
         goto err;
@@ -3489,22 +3822,29 @@ ble_gattc_write_reliable_rx_exec(struct ble_gattc_proc *proc, int status)
 }
 
 /**
- * Initiates GATT procedure: Write Long Characteristic Values.
+ * Initiates GATT procedure: Reliable Writes.  This function consumes the
+ * supplied mbufs regardless of the outcome.
  *
  * @param conn_handle           The connection over which to execute the
  *                                  procedure.
- * @param attr_handle           The handle of the characteristic value to write
- *                                  to.
- * @param value                 The value to write to the characteristic.
- * @param value_len             The number of bytes to write.
+ * @param attrs                 An array of attribute descriptors; specifies
+ *                                  which characteristics to write to and what
+ *                                  data to write to them.  The mbuf pointer in
+ *                                  each attribute is set to NULL by this
+ *                                  function.
+ * @param num_attrs             The number of characteristics to write; equal
+ *                                  to the number of elements in the 'attrs'
+ *                                  array.
  * @param cb                    The function to call to report procedure status
  *                                  updates; null for no callback.
- * @param cb_arg                The argument to pass to the callback function.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
  */
 int
-ble_gattc_write_reliable(uint16_t conn_handle, struct ble_gatt_attr *attrs,
-                         int num_attrs, ble_gatt_reliable_attr_fn *cb,
-                         void *cb_arg)
+ble_gattc_write_reliable(uint16_t conn_handle,
+                         struct ble_gatt_attr *attrs,
+                         int num_attrs,
+                         ble_gatt_reliable_attr_fn *cb, void *cb_arg)
 {
 #if !NIMBLE_OPT(GATT_WRITE_RELIABLE)
     return BLE_HS_ENOTSUP;
@@ -3512,9 +3852,17 @@ ble_gattc_write_reliable(uint16_t conn_handle, struct ble_gatt_attr *attrs,
 
     struct ble_gattc_proc *proc;
     int rc;
+    int i;
+
+    proc = NULL;
 
     STATS_INC(ble_gattc_stats, write_reliable);
 
+    if (num_attrs > NIMBLE_OPT(GATT_WRITE_MAX_ATTRS)) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
     proc = ble_gattc_proc_alloc();
     if (proc == NULL) {
         rc = BLE_HS_ENOMEM;
@@ -3523,12 +3871,19 @@ ble_gattc_write_reliable(uint16_t conn_handle, struct ble_gatt_attr *attrs,
 
     proc->op = BLE_GATT_OP_WRITE_RELIABLE;
     proc->conn_handle = conn_handle;
-    proc->write_reliable.attrs = attrs;
     proc->write_reliable.num_attrs = num_attrs;
     proc->write_reliable.cur_attr = 0;
     proc->write_reliable.cb = cb;
     proc->write_reliable.cb_arg = cb_arg;
 
+    for (i = 0; i < num_attrs; i++) {
+        proc->write_reliable.attrs[i] = attrs[i];
+        proc->write_reliable.attrs[i].offset = 0;
+
+        /* Consume mbuf from caller. */
+        attrs[i].om = NULL;
+    }
+
     ble_gattc_log_write_reliable(proc);
 
     rc = ble_gattc_write_reliable_go(proc, 1);
@@ -3541,6 +3896,12 @@ done:
         STATS_INC(ble_gattc_stats, write_reliable_fail);
     }
 
+    /* Free supplied mbufs in case something failed. */
+    for (i = 0; i < num_attrs; i++) {
+        os_mbuf_free_chain(attrs[i].om);
+        attrs[i].om = NULL;
+    }
+
     ble_gattc_process_status(proc, rc);
     return rc;
 }
@@ -3550,45 +3911,53 @@ done:
  *****************************************************************************/
 
 /**
- * Sends an attribute notification.  The content of the message is specified
- * in the attr parameter.
+ * Sends a "free-form" characteristic notification.  This function consumes the
+ * supplied mbuf regardless of the outcome.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param chr_val_handle        The attribute handle to indicate in the
+ *                                  outgoing notification.
+ * @param txom                  The value to write to the characteristic.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gattc_notify_custom(uint16_t conn_handle, uint16_t att_handle,
-                        void *attr_data, uint16_t attr_data_len)
+ble_gattc_notify_custom(uint16_t conn_handle, uint16_t chr_val_handle,
+                        struct os_mbuf *txom)
 {
 #if !NIMBLE_OPT(GATT_NOTIFY)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct ble_att_svr_access_ctxt ctxt;
     struct ble_att_notify_req req;
     int rc;
 
     STATS_INC(ble_gattc_stats, notify);
 
-    ble_gattc_log_notify(att_handle);
+    ble_gattc_log_notify(chr_val_handle);
 
-    if (attr_data == NULL) {
+    if (txom == NULL) {
         /* No custom attribute data; read the value from the specified
          * attribute.
          */
+        txom = ble_hs_mbuf_att_pkt();
+        if (txom == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
         rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE,
-                                     att_handle, &ctxt, NULL);
+                                     chr_val_handle, 0, txom, NULL);
         if (rc != 0) {
             /* Fatal error; application disallowed attribute read. */
             rc = BLE_HS_EAPP;
             goto err;
         }
-    } else {
-        ctxt.attr_data = attr_data;
-        ctxt.data_len = attr_data_len;
-        ctxt.offset = 0;
     }
 
-    req.banq_handle = att_handle;
-    rc = ble_att_clt_tx_notify(conn_handle, &req,
-                               ctxt.attr_data, ctxt.data_len);
+    req.banq_handle = chr_val_handle;
+    rc = ble_att_clt_tx_notify(conn_handle, &req, txom);
+    txom = NULL;
     if (rc != 0) {
         goto err;
     }
@@ -3596,13 +3965,26 @@ ble_gattc_notify_custom(uint16_t conn_handle, uint16_t att_handle,
     return 0;
 
 err:
-    STATS_INC(ble_gattc_stats, notify_fail);
+    if (rc != 0) {
+        STATS_INC(ble_gattc_stats, notify_fail);
+    }
+
+    os_mbuf_free_chain(txom);
+
     return rc;
 }
 
 /**
- * Sends an attribute notification.  The content of the message is read from
- * the specified characteristic.
+ * Sends a characteristic notification.  The content of the message is read
+ * from the specified characteristic.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param chr_val_handle        The value attribute handle of the
+ *                                  characteristic to include in the outgoing
+ *                                  notification.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
 ble_gattc_notify(uint16_t conn_handle, uint16_t chr_val_handle)
@@ -3613,12 +3995,12 @@ ble_gattc_notify(uint16_t conn_handle, uint16_t chr_val_handle)
 
     int rc;
 
-    rc = ble_gattc_notify_custom(conn_handle, chr_val_handle, NULL, 0);
-    if (rc != 0) {
-        return rc;
-    }
+    rc = ble_gattc_notify_custom(conn_handle, chr_val_handle, NULL);
 
-    return 0;
+    /* Tell the application that a notification transmission was attempted. */
+    ble_gap_notify_tx_event(rc, conn_handle, chr_val_handle, 0);
+
+    return rc;
 }
 
 /*****************************************************************************
@@ -3626,48 +4008,33 @@ ble_gattc_notify(uint16_t conn_handle, uint16_t chr_val_handle)
  *****************************************************************************/
 
 /**
- * Calls an indication proc's callback with the specified parameters.  If the
- * proc has no callback, this function is a no-op.
- *
- * @return                      The return code of the callback (or 0 if there
- *                                  is no callback).
+ * Handles an incoming ATT error response for the specified indication proc.
+ * A device should never send an error in response to an indication.  If this
+ * happens, we treat it like a confirmation (indication ack), but report the
+ * error status to the application.
  */
-static int
-ble_gattc_indicate_cb(struct ble_gattc_proc *proc, int status,
-                      uint16_t att_handle)
+static void
+ble_gattc_indicate_err(struct ble_gattc_proc *proc, int status,
+                       uint16_t att_handle)
 {
-    struct ble_gatt_attr attr;
     int rc;
 
-    BLE_HS_DBG_ASSERT(!ble_hs_locked_by_cur_task());
     ble_gattc_dbg_assert_proc_not_inserted(proc);
 
-    if (status != 0) {
-        STATS_INC(ble_gattc_stats, indicate_fail);
-    }
-
-    if (proc->indicate.cb == NULL) {
-        rc = 0;
-    } else {
-        memset(&attr, 0, sizeof attr);
-        attr.handle = proc->indicate.chr_val_handle;
-        rc = proc->indicate.cb(proc->conn_handle,
-                               ble_gattc_error(status, att_handle),
-                               &attr, proc->indicate.cb_arg);
+    if (status != BLE_HS_ENOTCONN) {
+        rc = ble_gatts_rx_indicate_ack(proc->conn_handle,
+                                       proc->indicate.chr_val_handle);
+        if (rc != 0) {
+            return;
+        }
     }
 
-    return rc;
-}
+    /* Tell the application about the received acknowledgment. */
+    ble_gap_notify_tx_event(status, proc->conn_handle,
+                            proc->indicate.chr_val_handle, 1);
 
-/**
- * Handles an incoming ATT error response for the specified indication proc.
- */
-static void
-ble_gattc_indicate_err(struct ble_gattc_proc *proc, int status,
-                       uint16_t att_handle)
-{
-    ble_gattc_dbg_assert_proc_not_inserted(proc);
-    ble_gattc_indicate_cb(proc, status, att_handle);
+    /* Send the next indication if one is pending. */
+    ble_gatts_send_next_indicate(proc->conn_handle);
 }
 
 /**
@@ -3683,33 +4050,57 @@ ble_gattc_indicate_rx_rsp(struct ble_gattc_proc *proc)
 
     rc = ble_gatts_rx_indicate_ack(proc->conn_handle,
                                    proc->indicate.chr_val_handle);
-    if (rc != BLE_HS_ENOTCONN && rc != BLE_HS_ENOENT) {
-        ble_gattc_indicate_cb(proc, rc, 0);
+    if (rc != 0) {
+        return;
     }
 
+    /* Tell the application about the received acknowledgment. */
+    ble_gap_notify_tx_event(BLE_HS_EDONE, proc->conn_handle,
+                            proc->indicate.chr_val_handle, 1);
+
     /* Send the next indication if one is pending. */
     ble_gatts_send_next_indicate(proc->conn_handle);
 }
 
 /**
- * Sends an attribute indication.
+ * Causes the indication in progress for the specified connection (if any) to
+ * fail with a status code of BLE_HS_ENOTCONN;
+ */
+void
+ble_gatts_indicate_fail_notconn(uint16_t conn_handle)
+{
+    ble_gattc_fail_procs(conn_handle, BLE_GATT_OP_INDICATE, BLE_HS_ENOTCONN);
+}
+
+/**
+ * Sends a characteristic indication.  The content of the message is read from
+ * the specified characteristic.
+ *
+ * @param conn_handle           The connection over which to execute the
+ *                                  procedure.
+ * @param chr_val_handle        The value attribute handle of the
+ *                                  characteristic to include in the outgoing
+ *                                  indication.
+ *
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_gattc_indicate(uint16_t conn_handle, uint16_t chr_val_handle,
-                   ble_gatt_attr_fn *cb, void *cb_arg)
+ble_gattc_indicate(uint16_t conn_handle, uint16_t chr_val_handle)
 {
 #if !NIMBLE_OPT(GATT_INDICATE)
     return BLE_HS_ENOTSUP;
 #endif
 
-    struct ble_att_svr_access_ctxt ctxt;
     struct ble_att_indicate_req req;
     struct ble_gattc_proc *proc;
     struct ble_hs_conn *conn;
+    struct os_mbuf *om;
     int rc;
 
     STATS_INC(ble_gattc_stats, indicate);
 
+    om = NULL;
+
     proc = ble_gattc_proc_alloc();
     if (proc == NULL) {
         rc = BLE_HS_ENOMEM;
@@ -3719,13 +4110,17 @@ ble_gattc_indicate(uint16_t conn_handle, uint16_t chr_val_handle,
     proc->op = BLE_GATT_OP_INDICATE;
     proc->conn_handle = conn_handle;
     proc->indicate.chr_val_handle = chr_val_handle;
-    proc->indicate.cb = cb;
-    proc->indicate.cb_arg = cb_arg;
 
     ble_gattc_log_indicate(chr_val_handle);
 
-    rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, chr_val_handle,
-                                 &ctxt, NULL);
+    om = ble_hs_mbuf_att_pkt();
+    if (om == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    rc = ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, chr_val_handle, 0,
+                                 om, NULL);
     if (rc != 0) {
         /* Fatal error; application disallowed attribute read. */
         BLE_HS_DBG_ASSERT(0);
@@ -3734,9 +4129,8 @@ ble_gattc_indicate(uint16_t conn_handle, uint16_t chr_val_handle,
     }
 
     req.baiq_handle = chr_val_handle;
-    rc = ble_att_clt_tx_indicate(conn_handle, &req,
-                                 ctxt.attr_data, ctxt.data_len);
-
+    rc = ble_att_clt_tx_indicate(conn_handle, &req, om);
+    om = NULL;
     if (rc != 0) {
         goto done;
     }
@@ -3755,7 +4149,11 @@ done:
         STATS_INC(ble_gattc_stats, indicate_fail);
     }
 
+    /* Tell the application that an indication transmission was attempted. */
+    ble_gap_notify_tx_event(rc, conn_handle, chr_val_handle, 1);
+
     ble_gattc_process_status(proc, rc);
+    os_mbuf_free_chain(om);
     return rc;
 }
 
@@ -3983,8 +4381,7 @@ ble_gattc_rx_read_group_type_complete(uint16_t conn_handle, int status)
  * procedure.
  */
 void
-ble_gattc_rx_read_rsp(uint16_t conn_handle, int status, void *value,
-                      int value_len)
+ble_gattc_rx_read_rsp(uint16_t conn_handle, int status, struct os_mbuf **om)
 {
 #if !NIMBLE_OPT(ATT_CLT_READ)
     return;
@@ -3998,7 +4395,7 @@ ble_gattc_rx_read_rsp(uint16_t conn_handle, int status, void *value,
                                          ble_gattc_rx_read_rsp_entries,
                                          &rx_entry);
     if (proc != NULL) {
-        rc = rx_entry->cb(proc, status, value, value_len);
+        rc = rx_entry->cb(proc, status, om);
         ble_gattc_process_status(proc, rc);
     }
 }
@@ -4009,7 +4406,7 @@ ble_gattc_rx_read_rsp(uint16_t conn_handle, int status, void *value,
  */
 void
 ble_gattc_rx_read_blob_rsp(uint16_t conn_handle, int status,
-                           void *value, int value_len)
+                           struct os_mbuf **om)
 {
 #if !NIMBLE_OPT(ATT_CLT_READ_BLOB)
     return;
@@ -4020,7 +4417,7 @@ ble_gattc_rx_read_blob_rsp(uint16_t conn_handle, int status,
 
     proc = ble_gattc_extract(conn_handle, BLE_GATT_OP_READ_LONG);
     if (proc != NULL) {
-        rc = ble_gattc_read_long_rx_read_rsp(proc, status, value, value_len);
+        rc = ble_gattc_read_long_rx_read_rsp(proc, status, om);
         ble_gattc_process_status(proc, rc);
     }
 }
@@ -4031,7 +4428,7 @@ ble_gattc_rx_read_blob_rsp(uint16_t conn_handle, int status,
  */
 void
 ble_gattc_rx_read_mult_rsp(uint16_t conn_handle, int status,
-                           void *value, int value_len)
+                           struct os_mbuf **om)
 {
 #if !NIMBLE_OPT(ATT_CLT_READ_MULT)
     return;
@@ -4041,7 +4438,7 @@ ble_gattc_rx_read_mult_rsp(uint16_t conn_handle, int status,
 
     proc = ble_gattc_extract(conn_handle, BLE_GATT_OP_READ_MULT);
     if (proc != NULL) {
-        ble_gattc_read_mult_cb(proc, status, 0, value, value_len);
+        ble_gattc_read_mult_cb(proc, status, 0, om);
         ble_gattc_process_status(proc, BLE_HS_EDONE);
     }
 }
@@ -4073,7 +4470,7 @@ ble_gattc_rx_write_rsp(uint16_t conn_handle)
 void
 ble_gattc_rx_prep_write_rsp(uint16_t conn_handle, int status,
                             struct ble_att_prep_write_cmd *rsp,
-                            void *attr_data, uint16_t attr_data_len)
+                            struct os_mbuf **om)
 {
 #if !NIMBLE_OPT(ATT_CLT_PREP_WRITE)
     return;
@@ -4087,7 +4484,7 @@ ble_gattc_rx_prep_write_rsp(uint16_t conn_handle, int status,
                                          ble_gattc_rx_prep_entries,
                                          &rx_entry);
     if (proc != NULL) {
-        rc = rx_entry->cb(proc, status, rsp, attr_data, attr_data_len);
+        rc = rx_entry->cb(proc, status, rsp, om);
         ble_gattc_process_status(proc, rc);
     }
 }
@@ -4150,25 +4547,7 @@ ble_gattc_rx_indicate_rsp(uint16_t conn_handle)
 void
 ble_gattc_connection_broken(uint16_t conn_handle)
 {
-    struct ble_gattc_proc_list temp_list;
-    struct ble_gattc_proc *proc;
-    ble_gattc_err_fn *err_cb;
-
-    /* Remove all procs with the specified conn handle and insert them into the
-     * temporary list.
-     */
-    ble_gattc_extract_by_conn(conn_handle, &temp_list);
-
-    /* Notify application of failed procedures and free the corresponding proc
-     * entries.
-     */
-    while ((proc = STAILQ_FIRST(&temp_list)) != NULL) {
-        err_cb = ble_gattc_err_dispatch_get(proc->op);
-        err_cb(proc, BLE_HS_ENOTCONN, 0);
-
-        STAILQ_REMOVE_HEAD(&temp_list, next);
-        ble_gattc_proc_free(proc);
-    }
+    ble_gattc_fail_procs(conn_handle, BLE_GATT_OP_NONE, BLE_HS_ENOTCONN);
 }
 
 /**
diff --git a/net/nimble/host/src/ble_gatts.c b/net/nimble/host/src/ble_gatts.c
index 3f73e015..e6ff2031 100644
--- a/net/nimble/host/src/ble_gatts.c
+++ b/net/nimble/host/src/ble_gatts.c
@@ -28,6 +28,9 @@
 #define BLE_GATTS_INCLUDE_SZ    6
 #define BLE_GATTS_CHR_MAX_SZ    19
 
+static const struct ble_gatt_svc_def **ble_gatts_svc_defs;
+static int ble_gatts_num_svc_defs;
+
 struct ble_gatts_svc_entry {
     const struct ble_gatt_svc_def *svc;
     uint16_t handle;            /* 0 means unregistered. */
@@ -35,7 +38,7 @@ struct ble_gatts_svc_entry {
 };
 
 static struct ble_gatts_svc_entry *ble_gatts_svc_entries;
-static int ble_gatts_num_svc_entries;
+static uint16_t ble_gatts_num_svc_entries;
 
 static os_membuf_t *ble_gatts_clt_cfg_mem;
 static struct os_mempool ble_gatts_clt_cfg_pool;
@@ -66,11 +69,13 @@ STATS_NAME_END(ble_gatts_stats)
 
 static int
 ble_gatts_svc_access(uint16_t conn_handle, uint16_t attr_handle,
-                     uint8_t *uuid128, uint8_t op,
-                     struct ble_att_svr_access_ctxt *ctxt, void *arg)
+                     uint8_t op, uint16_t offset, struct os_mbuf **om,
+                     void *arg)
 {
     const struct ble_gatt_svc_def *svc;
-    static uint16_t uuid16;
+    uint16_t uuid16;
+    uint8_t *buf;
+    int rc;
 
     STATS_INC(ble_gatts_stats, svc_def_reads);
 
@@ -80,12 +85,16 @@ ble_gatts_svc_access(uint16_t conn_handle, uint16_t attr_handle,
 
     uuid16 = ble_uuid_128_to_16(svc->uuid128);
     if (uuid16 != 0) {
-        htole16(&uuid16, uuid16);
-        ctxt->attr_data = &uuid16;
-        ctxt->data_len = 2;
+        buf = os_mbuf_extend(*om, 2);
+        if (buf == NULL) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        htole16(buf, uuid16);
     } else {
-        ctxt->attr_data = svc->uuid128;
-        ctxt->data_len = 16;
+        rc = os_mbuf_append(*om, svc->uuid128, 16);
+        if (rc != 0) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
     }
 
     return 0;
@@ -93,13 +102,12 @@ ble_gatts_svc_access(uint16_t conn_handle, uint16_t attr_handle,
 
 static int
 ble_gatts_inc_access(uint16_t conn_handle, uint16_t attr_handle,
-                     uint8_t *uuid128, uint8_t op,
-                     struct ble_att_svr_access_ctxt *ctxt, void *arg)
+                     uint8_t op, uint16_t offset, struct os_mbuf **om,
+                     void *arg)
 {
-    static uint8_t buf[BLE_GATTS_INCLUDE_SZ];
-
     const struct ble_gatts_svc_entry *entry;
     uint16_t uuid16;
+    uint8_t *buf;
 
     STATS_INC(ble_gatts_stats, svc_inc_reads);
 
@@ -107,18 +115,22 @@ ble_gatts_inc_access(uint16_t conn_handle, uint16_t attr_handle,
 
     entry = arg;
 
+    buf = os_mbuf_extend(*om, 4);
+    if (buf == NULL) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
     htole16(buf + 0, entry->handle);
     htole16(buf + 2, entry->end_group_handle);
 
     /* Only include the service UUID if it has a 16-bit representation. */
     uuid16 = ble_uuid_128_to_16(entry->svc->uuid128);
     if (uuid16 != 0) {
-        htole16(buf + 4, uuid16);
-        ctxt->data_len = 6;
-    } else {
-        ctxt->data_len = 4;
+        buf = os_mbuf_extend(*om, 2);
+        if (buf == NULL) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        htole16(buf, uuid16);
     }
-    ctxt->attr_data = buf;
 
     return 0;
 }
@@ -212,12 +224,12 @@ ble_gatts_chr_properties(const struct ble_gatt_chr_def *chr)
 
 static int
 ble_gatts_chr_def_access(uint16_t conn_handle, uint16_t attr_handle,
-                         uint8_t *uuid128, uint8_t op,
-                         struct ble_att_svr_access_ctxt *ctxt, void *arg)
+                         uint8_t op, uint16_t offset, struct os_mbuf **om,
+                         void *arg)
 {
-    static uint8_t buf[BLE_GATTS_CHR_MAX_SZ];
     const struct ble_gatt_chr_def *chr;
     uint16_t uuid16;
+    uint8_t *buf;
 
     STATS_INC(ble_gatts_stats, chr_def_reads);
 
@@ -225,6 +237,11 @@ ble_gatts_chr_def_access(uint16_t conn_handle, uint16_t attr_handle,
 
     chr = arg;
 
+    buf = os_mbuf_extend(*om, 3);
+    if (buf == NULL) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+
     buf[0] = ble_gatts_chr_properties(chr);
 
     /* The value attribute is always immediately after the declaration. */
@@ -232,13 +249,18 @@ ble_gatts_chr_def_access(uint16_t conn_handle, uint16_t attr_handle,
 
     uuid16 = ble_uuid_128_to_16(chr->uuid128);
     if (uuid16 != 0) {
-        htole16(buf + 3, uuid16);
-        ctxt->data_len = 5;
+        buf = os_mbuf_extend(*om, 2);
+        if (buf == NULL) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        htole16(buf, uuid16);
     } else {
-        memcpy(buf + 3, chr->uuid128, 16);
-        ctxt->data_len = 19;
+        buf = os_mbuf_extend(*om, 16);
+        if (buf == NULL) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        memcpy(buf, chr->uuid128, 16);
     }
-    ctxt->attr_data = buf;
 
     return 0;
 }
@@ -293,45 +315,70 @@ ble_gatts_chr_inc_val_stat(uint8_t gatt_op)
 }
 
 static int
-ble_gatts_chr_val_access(uint16_t conn_handle, uint16_t attr_handle,
-                         uint8_t *uuid128, uint8_t att_op,
-                         struct ble_att_svr_access_ctxt *att_ctxt, void *arg)
+ble_gatts_val_access(uint16_t conn_handle, uint16_t attr_handle,
+                     uint16_t offset, struct ble_gatt_access_ctxt *gatt_ctxt,
+                     struct os_mbuf **om, ble_gatt_access_fn *access_cb,
+                     void *cb_arg)
 {
-    const struct ble_gatt_chr_def *chr;
-    union ble_gatt_access_ctxt gatt_ctxt;
-    uint8_t gatt_op;
+    int attr_len;
     int rc;
 
-    chr = arg;
-    BLE_HS_DBG_ASSERT(chr != NULL && chr->access_cb != NULL);
+    switch (gatt_ctxt->op) {
+    case BLE_GATT_ACCESS_OP_READ_CHR:
+    case BLE_GATT_ACCESS_OP_READ_DSC:
+        gatt_ctxt->om = os_msys_get_pkthdr(0, 0);
+        if (gatt_ctxt->om == NULL) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
 
-    gatt_op = ble_gatts_chr_op(att_op);
-    gatt_ctxt.chr_access.chr = chr;
-    gatt_ctxt.chr_access.data = att_ctxt->attr_data;
-    gatt_ctxt.chr_access.len = att_ctxt->data_len;
+        rc = access_cb(conn_handle, attr_handle, gatt_ctxt, cb_arg);
+        if (rc == 0) {
+            attr_len = OS_MBUF_PKTLEN(gatt_ctxt->om) - offset;
+            if (attr_len > 0) {
+                os_mbuf_appendfrom(*om, gatt_ctxt->om, offset, attr_len);
+            }
+        }
 
-    ble_gatts_chr_inc_val_stat(gatt_op);
+        os_mbuf_free_chain(gatt_ctxt->om);
+        return rc;
 
-    rc = chr->access_cb(conn_handle, attr_handle, gatt_op, &gatt_ctxt,
-                        chr->arg);
-    if (rc != 0) {
+    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+    case BLE_GATT_ACCESS_OP_WRITE_DSC:
+        gatt_ctxt->om = *om;
+        rc = access_cb(conn_handle, attr_handle, gatt_ctxt, cb_arg);
+        *om = gatt_ctxt->om;
         return rc;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_ATT_ERR_UNLIKELY;
     }
+}
+
+static int
+ble_gatts_chr_val_access(uint16_t conn_handle, uint16_t attr_handle,
+                         uint8_t att_op, uint16_t offset,
+                         struct os_mbuf **om, void *arg)
+{
+    const struct ble_gatt_chr_def *chr_def;
+    struct ble_gatt_access_ctxt gatt_ctxt;
+    int rc;
 
-    if (att_op == BLE_ATT_ACCESS_OP_WRITE &&
-        ble_gatts_chr_clt_cfg_allowed(chr)) {
+    chr_def = arg;
+    BLE_HS_DBG_ASSERT(chr_def != NULL && chr_def->access_cb != NULL);
 
-        ble_gatts_chr_updated(attr_handle - 1);
-    }
+    gatt_ctxt.op = ble_gatts_chr_op(att_op);
+    gatt_ctxt.chr = chr_def;
 
-    att_ctxt->attr_data = gatt_ctxt.chr_access.data;
-    att_ctxt->data_len = gatt_ctxt.chr_access.len;
+    ble_gatts_chr_inc_val_stat(gatt_ctxt.op);
+    rc = ble_gatts_val_access(conn_handle, attr_handle, offset, &gatt_ctxt, om,
+                              chr_def->access_cb, chr_def->arg);
 
-    return 0;
+    return rc;
 }
 
 static int
-ble_gatts_find_svc(const struct ble_gatt_svc_def *svc)
+ble_gatts_find_svc_entry_idx(const struct ble_gatt_svc_def *svc)
 {
     int i;
 
@@ -356,7 +403,7 @@ ble_gatts_svc_incs_satisfied(const struct ble_gatt_svc_def *svc)
     }
 
     for (i = 0; svc->includes[i] != NULL; i++) {
-        idx = ble_gatts_find_svc(svc->includes[i]);
+        idx = ble_gatts_find_svc_entry_idx(svc->includes[i]);
         if (idx == -1 || ble_gatts_svc_entries[idx].handle == 0) {
             return 0;
         }
@@ -418,34 +465,24 @@ ble_gatts_dsc_inc_stat(uint8_t gatt_op)
 
 static int
 ble_gatts_dsc_access(uint16_t conn_handle, uint16_t attr_handle,
-                     uint8_t *uuid128, uint8_t att_op,
-                     struct ble_att_svr_access_ctxt *att_ctxt, void *arg)
+                     uint8_t att_op, uint16_t offset, struct os_mbuf **om,
+                     void *arg)
 {
-    const struct ble_gatt_dsc_def *dsc;
-    union ble_gatt_access_ctxt gatt_ctxt;
-    uint8_t gatt_op;
+    const struct ble_gatt_dsc_def *dsc_def;
+    struct ble_gatt_access_ctxt gatt_ctxt;
     int rc;
 
-    dsc = arg;
-    BLE_HS_DBG_ASSERT(dsc != NULL && dsc->access_cb != NULL);
+    dsc_def = arg;
+    BLE_HS_DBG_ASSERT(dsc_def != NULL && dsc_def->access_cb != NULL);
 
-    gatt_op = ble_gatts_dsc_op(att_op);
-    gatt_ctxt.dsc_access.dsc = dsc;
-    gatt_ctxt.dsc_access.data = att_ctxt->attr_data;
-    gatt_ctxt.dsc_access.len = att_ctxt->data_len;
-
-    ble_gatts_dsc_inc_stat(gatt_op);
-
-    rc = dsc->access_cb(conn_handle, attr_handle, gatt_op, &gatt_ctxt,
-                        dsc->arg);
-    if (rc != 0) {
-        return rc;
-    }
+    gatt_ctxt.op = ble_gatts_dsc_op(att_op);
+    gatt_ctxt.dsc = dsc_def;
 
-    att_ctxt->attr_data = gatt_ctxt.dsc_access.data;
-    att_ctxt->data_len = gatt_ctxt.dsc_access.len;
+    ble_gatts_dsc_inc_stat(gatt_ctxt.op);
+    rc = ble_gatts_val_access(conn_handle, attr_handle, offset, &gatt_ctxt, om,
+                              dsc_def->access_cb, dsc_def->arg);
 
-    return 0;
+    return rc;
 }
 
 static int
@@ -463,12 +500,13 @@ ble_gatts_dsc_is_sane(const struct ble_gatt_dsc_def *dsc)
 }
 
 static int
-ble_gatts_register_dsc(const struct ble_gatt_dsc_def *dsc,
+ble_gatts_register_dsc(const struct ble_gatt_svc_def *svc,
                        const struct ble_gatt_chr_def *chr,
+                       const struct ble_gatt_dsc_def *dsc,
                        uint16_t chr_def_handle,
                        ble_gatt_register_fn *register_cb, void *cb_arg)
 {
-    union ble_gatt_register_ctxt register_ctxt;
+    struct ble_gatt_register_ctxt register_ctxt;
     uint16_t dsc_handle;
     int rc;
 
@@ -483,11 +521,12 @@ ble_gatts_register_dsc(const struct ble_gatt_dsc_def *dsc,
     }
 
     if (register_cb != NULL) {
-        register_ctxt.dsc_reg.dsc_handle = dsc_handle;
-        register_ctxt.dsc_reg.dsc = dsc;
-        register_ctxt.dsc_reg.chr_def_handle = chr_def_handle;
-        register_ctxt.dsc_reg.chr = chr;
-        register_cb(BLE_GATT_REGISTER_OP_DSC, &register_ctxt, cb_arg);
+        register_ctxt.op = BLE_GATT_REGISTER_OP_DSC;
+        register_ctxt.dsc.handle = dsc_handle;
+        register_ctxt.dsc.svc_def = svc;
+        register_ctxt.dsc.chr_def = chr;
+        register_ctxt.dsc.dsc_def = dsc;
+        register_cb(&register_ctxt, cb_arg);
     }
 
     STATS_INC(ble_gatts_stats, dscs);
@@ -527,6 +566,22 @@ ble_gatts_clt_cfg_find(struct ble_gatts_clt_cfg *cfgs,
     }
 }
 
+static void
+ble_gatts_subscribe_event(uint16_t conn_handle, uint16_t attr_handle,
+                          uint8_t reason,
+                          uint8_t prev_flags, uint8_t cur_flags)
+{
+    if (prev_flags != cur_flags) {
+        ble_gap_subscribe_event(conn_handle,
+                                attr_handle,
+                                reason,
+                                prev_flags  & BLE_GATTS_CLT_CFG_F_NOTIFY,
+                                cur_flags   & BLE_GATTS_CLT_CFG_F_NOTIFY,
+                                prev_flags  & BLE_GATTS_CLT_CFG_F_INDICATE,
+                                cur_flags   & BLE_GATTS_CLT_CFG_F_INDICATE);
+    }
+}
+
 /**
  * Performs a read or write access on a client characteritic configuration
  * descriptor (CCCD).
@@ -549,16 +604,17 @@ ble_gatts_clt_cfg_find(struct ble_gatts_clt_cfg *cfgs,
  */
 static int
 ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle,
-                                uint8_t att_op,
-                                struct ble_att_svr_access_ctxt *ctxt,
-                                struct ble_store_value_cccd *out_cccd)
+                                uint8_t att_op, uint16_t offset,
+                                struct os_mbuf *om,
+                                struct ble_store_value_cccd *out_cccd,
+                                uint8_t *out_prev_clt_cfg_flags,
+                                uint8_t *out_cur_clt_cfg_flags)
 {
     struct ble_gatts_clt_cfg *clt_cfg;
     uint16_t chr_val_handle;
     uint16_t flags;
     uint8_t gatt_op;
-
-    static uint8_t buf[2];
+    uint8_t *buf;
 
     /* Assume nothing needs to be persisted. */
     out_cccd->chr_val_handle = 0;
@@ -578,37 +634,49 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle,
         return BLE_ATT_ERR_UNLIKELY;
     }
 
+    /* Assume no change in flags. */
+    *out_prev_clt_cfg_flags = clt_cfg->flags;
+    *out_cur_clt_cfg_flags = clt_cfg->flags;
+
     gatt_op = ble_gatts_dsc_op(att_op);
     ble_gatts_dsc_inc_stat(gatt_op);
 
     switch (gatt_op) {
     case BLE_GATT_ACCESS_OP_READ_DSC:
         STATS_INC(ble_gatts_stats, dsc_reads);
+        buf = os_mbuf_extend(om, 2);
+        if (buf == NULL) {
+            return BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
         htole16(buf, clt_cfg->flags & ~BLE_GATTS_CLT_CFG_F_RESERVED);
-        ctxt->attr_data = buf;
-        ctxt->data_len = sizeof buf;
         break;
 
     case BLE_GATT_ACCESS_OP_WRITE_DSC:
         STATS_INC(ble_gatts_stats, dsc_writes);
-        if (ctxt->data_len != 2) {
+        if (OS_MBUF_PKTLEN(om) != 2) {
             return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
         }
 
-        flags = le16toh(ctxt->attr_data);
+        om = os_mbuf_pullup(om, 2);
+        BLE_HS_DBG_ASSERT(om != NULL);
+
+        flags = le16toh(om->om_data);
         if ((flags & ~clt_cfg->allowed) != 0) {
-            return BLE_ATT_ERR_WRITE_NOT_PERMITTED;
+            return BLE_ATT_ERR_REQ_NOT_SUPPORTED;
         }
 
-        clt_cfg->flags = flags;
-
-        /* Successful writes get persisted for bonded connections. */
-        if (conn->bhc_sec_state.bonded) {
-            out_cccd->peer_addr_type = conn->bhc_addr_type;
-            memcpy(out_cccd->peer_addr, conn->bhc_addr, 6);
-            out_cccd->chr_val_handle = chr_val_handle;
-            out_cccd->flags = clt_cfg->flags;
-            out_cccd->value_changed = 0;
+        if (clt_cfg->flags != flags) {
+            clt_cfg->flags = flags;
+            *out_cur_clt_cfg_flags = flags;
+
+            /* Successful writes get persisted for bonded connections. */
+            if (conn->bhc_sec_state.bonded) {
+                out_cccd->peer_addr_type = conn->bhc_peer_addr_type;
+                memcpy(out_cccd->peer_addr, conn->bhc_peer_addr, 6);
+                out_cccd->chr_val_handle = chr_val_handle;
+                out_cccd->flags = clt_cfg->flags;
+                out_cccd->value_changed = 0;
+            }
         }
         break;
 
@@ -622,13 +690,15 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle,
 
 static int
 ble_gatts_clt_cfg_access(uint16_t conn_handle, uint16_t attr_handle,
-                         uint8_t *uuid128, uint8_t op,
-                         struct ble_att_svr_access_ctxt *ctxt,
+                         uint8_t op, uint16_t offset, struct os_mbuf **om,
                          void *arg)
 {
     struct ble_store_value_cccd cccd_value;
     struct ble_store_key_cccd cccd_key;
     struct ble_hs_conn *conn;
+    uint16_t chr_val_handle;
+    uint8_t prev_flags;
+    uint8_t cur_flags;
     int rc;
 
     ble_hs_lock();
@@ -637,14 +707,27 @@ ble_gatts_clt_cfg_access(uint16_t conn_handle, uint16_t attr_handle,
     if (conn == NULL) {
         rc = BLE_ATT_ERR_UNLIKELY;
     } else {
-        rc = ble_gatts_clt_cfg_access_locked(conn, attr_handle, op, ctxt,
-                                             &cccd_value);
+        rc = ble_gatts_clt_cfg_access_locked(conn, attr_handle, op, offset,
+                                             *om, &cccd_value, &prev_flags,
+                                             &cur_flags);
     }
 
     ble_hs_unlock();
 
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* The value attribute is always immediately after the declaration. */
+    chr_val_handle = attr_handle - 1;
+
+    /* Tell the application if the peer changed its subscription state. */
+    ble_gatts_subscribe_event(conn_handle, chr_val_handle,
+                              BLE_GAP_SUBSCRIBE_REASON_WRITE,
+                              prev_flags, cur_flags);
+
     /* Persist the CCCD if required. */
-    if (rc == 0 && cccd_value.chr_val_handle != 0) {
+    if (cccd_value.chr_val_handle != 0) {
         if (cccd_value.flags == 0) {
             ble_store_key_from_value_cccd(&cccd_key, &cccd_value);
             rc = ble_store_delete_cccd(&cccd_key);
@@ -679,10 +762,11 @@ ble_gatts_register_clt_cfg_dsc(uint16_t *att_handle)
 }
 
 static int
-ble_gatts_register_chr(const struct ble_gatt_chr_def *chr,
+ble_gatts_register_chr(const struct ble_gatt_svc_def *svc,
+                       const struct ble_gatt_chr_def *chr,
                        ble_gatt_register_fn *register_cb, void *cb_arg)
 {
-    union ble_gatt_register_ctxt register_ctxt;
+    struct ble_gatt_register_ctxt register_ctxt;
     struct ble_gatt_dsc_def *dsc;
     uint16_t def_handle;
     uint16_t val_handle;
@@ -694,7 +778,14 @@ ble_gatts_register_chr(const struct ble_gatt_chr_def *chr,
         return BLE_HS_EINVAL;
     }
 
-    /* Register characteristic declaration attribute (cast away const on
+    if (ble_gatts_chr_clt_cfg_allowed(chr) != 0) {
+        if (ble_gatts_num_cfgable_chrs > ble_hs_cfg.max_client_configs) {
+            return BLE_HS_ENOMEM;
+        }
+        ble_gatts_num_cfgable_chrs++;
+    }
+
+    /* Register characteristic definition attribute (cast away const on
      * callback arg).
      */
     rc = ble_att_svr_register_uuid16(BLE_ATT_UUID_CHARACTERISTIC,
@@ -715,11 +806,17 @@ ble_gatts_register_chr(const struct ble_gatt_chr_def *chr,
     }
     BLE_HS_DBG_ASSERT(val_handle == def_handle + 1);
 
+    if (chr->val_handle != NULL) {
+        *chr->val_handle = val_handle;
+    }
+
     if (register_cb != NULL) {
-        register_ctxt.chr_reg.def_handle = def_handle;
-        register_ctxt.chr_reg.val_handle = val_handle;
-        register_ctxt.chr_reg.chr = chr;
-        register_cb(BLE_GATT_REGISTER_OP_CHR, &register_ctxt, cb_arg);
+        register_ctxt.op = BLE_GATT_REGISTER_OP_CHR;
+        register_ctxt.chr.def_handle = def_handle;
+        register_ctxt.chr.val_handle = val_handle;
+        register_ctxt.chr.svc_def = svc;
+        register_ctxt.chr.chr_def = chr;
+        register_cb(&register_ctxt, cb_arg);
     }
 
     if (ble_gatts_chr_clt_cfg_allowed(chr) != 0) {
@@ -733,7 +830,7 @@ ble_gatts_register_chr(const struct ble_gatt_chr_def *chr,
     /* Register each descriptor. */
     if (chr->descriptors != NULL) {
         for (dsc = chr->descriptors; dsc->uuid128 != NULL; dsc++) {
-            rc = ble_gatts_register_dsc(dsc, chr, def_handle, register_cb,
+            rc = ble_gatts_register_dsc(svc, chr, dsc, def_handle, register_cb,
                                         cb_arg);
             if (rc != 0) {
                 return rc;
@@ -785,7 +882,7 @@ ble_gatts_register_svc(const struct ble_gatt_svc_def *svc,
                        ble_gatt_register_fn *register_cb, void *cb_arg)
 {
     const struct ble_gatt_chr_def *chr;
-    union ble_gatt_register_ctxt register_ctxt;
+    struct ble_gatt_register_ctxt register_ctxt;
     uint16_t uuid16;
     int idx;
     int rc;
@@ -815,15 +912,16 @@ ble_gatts_register_svc(const struct ble_gatt_svc_def *svc,
     }
 
     if (register_cb != NULL) {
-        register_ctxt.svc_reg.handle = *out_handle;
-        register_ctxt.svc_reg.svc = svc;
-        register_cb(BLE_GATT_REGISTER_OP_SVC, &register_ctxt, cb_arg);
+        register_ctxt.op = BLE_GATT_REGISTER_OP_SVC;
+        register_ctxt.svc.handle = *out_handle;
+        register_ctxt.svc.svc_def = svc;
+        register_cb(&register_ctxt, cb_arg);
     }
 
     /* Register each include. */
     if (svc->includes != NULL) {
         for (i = 0; svc->includes[i] != NULL; i++) {
-            idx = ble_gatts_find_svc(svc->includes[i]);
+            idx = ble_gatts_find_svc_entry_idx(svc->includes[i]);
             BLE_HS_DBG_ASSERT_EVAL(idx != -1);
 
             rc = ble_gatts_register_inc(ble_gatts_svc_entries + idx);
@@ -836,7 +934,7 @@ ble_gatts_register_svc(const struct ble_gatt_svc_def *svc,
     /* Register each characteristic. */
     if (svc->characteristics != NULL) {
         for (chr = svc->characteristics; chr->uuid128 != NULL; chr++) {
-            rc = ble_gatts_register_chr(chr, register_cb, cb_arg);
+            rc = ble_gatts_register_chr(svc, chr, register_cb, cb_arg);
             if (rc != 0) {
                 return rc;
             }
@@ -873,7 +971,7 @@ ble_gatts_register_round(int *out_num_registered, ble_gatt_register_fn *cb,
 
             case BLE_HS_EAGAIN:
                 /* Service could not be registered due to unsatisfied includes.
-                 * Try again on the next itereation.
+                 * Try again on the next iteration.
                  */
                 break;
 
@@ -891,28 +989,50 @@ ble_gatts_register_round(int *out_num_registered, ble_gatt_register_fn *cb,
     return 0;
 }
 
+/**
+ * Registers a set of services, characteristics, and descriptors to be accessed
+ * by GATT clients.
+ *
+ * @param svcs                  A table of the service definitions to be
+ *                                  registered.
+ * @param cb                    The function to call for each service,
+ *                                  characteristic, and descriptor that gets
+ *                                  registered.
+ * @param cb_arg                The optional argument to pass to the callback
+ *                                  function.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOMEM if registration failed due to
+ *                                  resource exhaustion;
+ *                              BLE_HS_EINVAL if the service definition table
+ *                                  contains an invalid element.
+ */
 int
 ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
                         ble_gatt_register_fn *cb, void *cb_arg)
 {
     int total_registered;
     int cur_registered;
+    int num_svcs;
+    int idx;
     int rc;
     int i;
 
     for (i = 0; svcs[i].type != BLE_GATT_SVC_TYPE_END; i++) {
-        ble_gatts_svc_entries[i].svc = svcs + i;
-        ble_gatts_svc_entries[i].handle = 0;
-        ble_gatts_svc_entries[i].end_group_handle = 0xffff;
-    }
-    if (i > ble_hs_cfg.max_services) {
-        return BLE_HS_ENOMEM;
-    }
+        idx = ble_gatts_num_svc_entries + i;
+        if (idx >= ble_hs_cfg.max_services) {
+            return BLE_HS_ENOMEM;
+        }
 
-    ble_gatts_num_svc_entries = i;
+        ble_gatts_svc_entries[idx].svc = svcs + i;
+        ble_gatts_svc_entries[idx].handle = 0;
+        ble_gatts_svc_entries[idx].end_group_handle = 0xffff;
+    }
+    num_svcs = i;
+    ble_gatts_num_svc_entries += num_svcs;
 
     total_registered = 0;
-    while (total_registered < ble_gatts_num_svc_entries) {
+    while (total_registered < num_svcs) {
         rc = ble_gatts_register_round(&cur_registered, cb, cb_arg);
         if (rc != 0) {
             return rc;
@@ -923,23 +1043,71 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs,
     return 0;
 }
 
+static int
+ble_gatts_clt_cfg_size(void)
+{
+    return ble_gatts_num_cfgable_chrs * sizeof (struct ble_gatts_clt_cfg);
+}
+
+/**
+ * Handles GATT server clean up for a terminated connection:
+ *     o Informs the application that the peer is no longer subscribed to any
+ *       characteristic updates.
+ *     o Frees GATT server resources consumed by the connection (CCCDs).
+ */
 void
-ble_gatts_conn_deinit(struct ble_gatts_conn *gatts_conn)
+ble_gatts_connection_broken(uint16_t conn_handle)
 {
+    struct ble_gatts_clt_cfg *clt_cfgs;
+    struct ble_hs_conn *conn;
+    int num_clt_cfgs;
     int rc;
+    int i;
 
-    if (gatts_conn->clt_cfgs != NULL) {
-        rc = os_memblock_put(&ble_gatts_clt_cfg_pool, gatts_conn->clt_cfgs);
-        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    /* Find the specified connection and extract its CCCD entries.  Extracting
+     * the clt_cfg pointer and setting the original to null is done for two
+     * reasons:
+     *     1. So that the CCCD entries can be safely processed after unlocking
+     *        the mutex.
+     *     2. To ensure a subsequent indicate procedure for this peer is not
+     *        attempted, as the connection is about to be terminated.  This
+     *        avoids a spurious notify-tx GAP event callback to the
+     *        application.  By setting the clt_cfg pointer to null, it is
+     *        assured that the connection has no pending indications to send.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    if (conn != NULL) {
+        clt_cfgs = conn->bhc_gatt_svr.clt_cfgs;
+        num_clt_cfgs = conn->bhc_gatt_svr.num_clt_cfgs;
 
-        gatts_conn->clt_cfgs = NULL;
+        conn->bhc_gatt_svr.clt_cfgs = NULL;
+        conn->bhc_gatt_svr.num_clt_cfgs = 0;
     }
-}
+    ble_hs_unlock();
 
-static int
-ble_gatts_clt_cfg_size(void)
-{
-    return ble_gatts_num_cfgable_chrs * sizeof (struct ble_gatts_clt_cfg);
+    if (conn == NULL) {
+        return;
+    }
+
+    /* If there is an indicate procedure in progress for this connection,
+     * inform the application that it has failed.
+     */
+    ble_gatts_indicate_fail_notconn(conn_handle);
+
+    /* Now that the mutex is unlocked, inform the application that the peer is
+     * no longer subscribed to any characteristic updates.
+     */
+    if (clt_cfgs != NULL) {
+        for (i = 0; i < num_clt_cfgs; i++) {
+            ble_gatts_subscribe_event(conn_handle, clt_cfgs[i].chr_val_handle,
+                                      BLE_GAP_SUBSCRIBE_REASON_TERM,
+                                      clt_cfgs[i].flags, 0);
+        }
+
+        rc = os_memblock_put(&ble_gatts_clt_cfg_pool, clt_cfgs);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+    }
 }
 
 int
@@ -953,26 +1121,10 @@ ble_gatts_start(void)
     int idx;
     int rc;
 
-    rc = ble_uuid_16_to_128(BLE_ATT_UUID_CHARACTERISTIC, uuid128);
-    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
-
-    /* Count the number of client-configurable characteristics. */
-    ble_gatts_num_cfgable_chrs = 0;
-    ha = NULL;
-    while ((ha = ble_att_svr_find_by_uuid(ha, uuid128)) != NULL) {
-        chr = ha->ha_cb_arg;
-        if (ble_gatts_chr_clt_cfg_allowed(chr) != 0) {
-            ble_gatts_num_cfgable_chrs++;
-        }
-    }
     if (ble_gatts_num_cfgable_chrs == 0) {
         return 0;
     }
 
-    if (ble_gatts_num_cfgable_chrs > ble_hs_cfg.max_client_configs) {
-        return BLE_HS_ENOMEM;
-    }
-
     /* Initialize client-configuration memory pool. */
     num_elems = ble_hs_cfg.max_client_configs / ble_gatts_num_cfgable_chrs;
     rc = os_mempool_init(&ble_gatts_clt_cfg_pool, num_elems,
@@ -991,9 +1143,11 @@ ble_gatts_start(void)
     }
 
     /* Fill the cache. */
+    rc = ble_uuid_16_to_128(BLE_ATT_UUID_CHARACTERISTIC, uuid128);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
     idx = 0;
     ha = NULL;
-    while ((ha = ble_att_svr_find_by_uuid(ha, uuid128)) != NULL) {
+    while ((ha = ble_att_svr_find_by_uuid(ha, uuid128, 0xffff)) != NULL) {
         chr = ha->ha_cb_arg;
         allowed_flags = ble_gatts_chr_clt_cfg_allowed(chr);
         if (allowed_flags != 0) {
@@ -1020,16 +1174,19 @@ int
 ble_gatts_conn_init(struct ble_gatts_conn *gatts_conn)
 {
     if (ble_gatts_num_cfgable_chrs > 0) {
-        ble_gatts_conn_deinit(gatts_conn);
         gatts_conn->clt_cfgs = os_memblock_get(&ble_gatts_clt_cfg_pool);
         if (gatts_conn->clt_cfgs == NULL) {
             return BLE_HS_ENOMEM;
         }
-    }
 
-    /* Initialize the client configuration with a copy of the cache. */
-    memcpy(gatts_conn->clt_cfgs, ble_gatts_clt_cfgs, ble_gatts_clt_cfg_size());
-    gatts_conn->num_clt_cfgs = ble_gatts_num_cfgable_chrs;
+        /* Initialize the client configuration with a copy of the cache. */
+        memcpy(gatts_conn->clt_cfgs, ble_gatts_clt_cfgs,
+               ble_gatts_clt_cfg_size());
+        gatts_conn->num_clt_cfgs = ble_gatts_num_cfgable_chrs;
+    } else {
+        gatts_conn->clt_cfgs = NULL;
+        gatts_conn->num_clt_cfgs = 0;
+    }
 
     return 0;
 }
@@ -1037,9 +1194,7 @@ ble_gatts_conn_init(struct ble_gatts_conn *gatts_conn)
 
 /**
  * Schedules a notification or indication for the specified peer-CCCD pair.  If
- * the udpate should be sent immediately, it is indicated in the return code.
- * If the update can only be sent in the future, the appropriate flags are set
- * to ensure this happens.
+ * the update should be sent immediately, it is indicated in the return code.
  *
  * @param conn                  The connection to schedule the update for.
  * @param clt_cfg               The client config entry corresponding to the
@@ -1054,7 +1209,10 @@ ble_gatts_schedule_update(struct ble_hs_conn *conn,
 {
     uint8_t att_op;
 
-    if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_NOTIFY) {
+    if (!(clt_cfg->flags & BLE_GATTS_CLT_CFG_F_MODIFIED)) {
+        /* Characteristic not modified.  Nothing to send. */
+        att_op = 0;
+    } else if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_NOTIFY) {
         /* Notifications always get sent immediately. */
         att_op = BLE_ATT_OP_NOTIFY_REQ;
     } else if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_INDICATE) {
@@ -1064,16 +1222,24 @@ ble_gatts_schedule_update(struct ble_hs_conn *conn,
          * If there isn't an outstanding indication, send this one now.
          */
         if (conn->bhc_gatt_svr.indicate_val_handle != 0) {
-            clt_cfg->flags |= BLE_GATTS_CLT_CFG_F_INDICATE_PENDING;
             att_op = 0;
         } else {
             att_op = BLE_ATT_OP_INDICATE_REQ;
         }
     } else {
-        BLE_HS_DBG_ASSERT(0);
+        /* Peer isn't subscribed to notifications or indications.  Nothing to
+         * send.
+         */
         att_op = 0;
     }
 
+    /* If we will be sending an update, clear the modified flag so that we
+     * don't double-send.
+     */
+    if (att_op != 0) {
+        clt_cfg->flags &= ~BLE_GATTS_CLT_CFG_F_MODIFIED;
+    }
+
     return att_op;
 }
 
@@ -1095,14 +1261,14 @@ ble_gatts_send_next_indicate(uint16_t conn_handle)
     if (conn != NULL) {
         for (i = 0; i < conn->bhc_gatt_svr.num_clt_cfgs; i++) {
             clt_cfg = conn->bhc_gatt_svr.clt_cfgs + i;
-            if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_INDICATE_PENDING) {
+            if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_MODIFIED) {
                 BLE_HS_DBG_ASSERT(clt_cfg->flags &
                                   BLE_GATTS_CLT_CFG_F_INDICATE);
 
                 chr_val_handle = clt_cfg->chr_val_handle;
 
                 /* Clear pending flag in anticipation of indication tx. */
-                clt_cfg->flags &= ~BLE_GATTS_CLT_CFG_F_INDICATE_PENDING;
+                clt_cfg->flags &= ~BLE_GATTS_CLT_CFG_F_MODIFIED;
                 break;
             }
         }
@@ -1118,7 +1284,7 @@ ble_gatts_send_next_indicate(uint16_t conn_handle)
         return BLE_HS_ENOENT;
     }
 
-    rc = ble_gattc_indicate(conn_handle, chr_val_handle, NULL, NULL);
+    rc = ble_gattc_indicate(conn_handle, chr_val_handle);
     if (rc != 0) {
         return rc;
     }
@@ -1170,16 +1336,16 @@ ble_gatts_rx_indicate_ack(uint16_t conn_handle, uint16_t chr_val_handle)
         BLE_HS_DBG_ASSERT(clt_cfg->chr_val_handle == chr_val_handle);
 
         persist = conn->bhc_sec_state.bonded &&
-                  !(clt_cfg->flags & BLE_GATTS_CLT_CFG_F_INDICATE_PENDING);
+                  !(clt_cfg->flags & BLE_GATTS_CLT_CFG_F_MODIFIED);
         if (persist) {
-            cccd_value.peer_addr_type = conn->bhc_addr_type;
-            memcpy(cccd_value.peer_addr, conn->bhc_addr, 6);
+            cccd_value.peer_addr_type = conn->bhc_peer_addr_type;
+            memcpy(cccd_value.peer_addr, conn->bhc_peer_addr, 6);
             cccd_value.chr_val_handle = chr_val_handle;
             cccd_value.flags = clt_cfg->flags;
             cccd_value.value_changed = 0;
         }
     } else {
-        /* This acknowledgement doesn't correspnod to the outstanding
+        /* This acknowledgement doesn't correspond to the outstanding
          * indication; ignore it.
          */
         rc = BLE_HS_ENOENT;
@@ -1194,7 +1360,7 @@ ble_gatts_rx_indicate_ack(uint16_t conn_handle, uint16_t chr_val_handle)
     if (persist) {
         rc = ble_store_write_cccd(&cccd_value);
         if (rc != 0) {
-            return rc;
+            /* XXX: How should this error get reported? */
         }
     }
 
@@ -1208,15 +1374,14 @@ ble_gatts_chr_updated(uint16_t chr_val_handle)
     struct ble_store_key_cccd cccd_key;
     struct ble_gatts_clt_cfg *clt_cfg;
     struct ble_hs_conn *conn;
-    uint16_t conn_handle;
-    uint8_t att_op;
+    int new_notifications;
     int clt_cfg_idx;
     int persist;
     int rc;
     int i;
 
-    /* Determine if notifications / indications are enabled for this
-     * characteristic.
+    /* Determine if notifications or indications are allowed for this
+     * characteristic.  If not, return immediately.
      */
     clt_cfg_idx = ble_gatts_clt_cfg_find_idx(ble_gatts_clt_cfgs,
                                              chr_val_handle);
@@ -1224,47 +1389,31 @@ ble_gatts_chr_updated(uint16_t chr_val_handle)
         return;
     }
 
-    /* Handle the connected devices. */
-    for (i = 0; ; i++) {
-        ble_hs_lock();
+    /*** Send notifications and indications to connected devices. */
 
+    ble_hs_lock();
+    for (i = 0; ; i++) {
+        /* XXX: This is inefficient when there are a lot of connections.
+         * Consider using a "foreach" function to walk the connection list.
+         */
         conn = ble_hs_conn_find_by_idx(i);
-        if (conn != NULL) {
-            BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs >
-                                   clt_cfg_idx);
-            clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx;
-            BLE_HS_DBG_ASSERT_EVAL(clt_cfg->chr_val_handle == chr_val_handle);
-
-            /* Determine what kind of update should get sent immediately (if
-             * any).
-             */
-            att_op = ble_gatts_schedule_update(conn, clt_cfg);
-            conn_handle = conn->bhc_handle;
-        } else {
-            /* Silence some spurious gcc warnings. */
-            att_op = 0;
-            conn_handle = BLE_HS_CONN_HANDLE_NONE;
-        }
-        ble_hs_unlock();
-
         if (conn == NULL) {
-            /* No more connected devices. */
             break;
         }
 
-        switch (att_op) {
-        case 0:
-            break;
-        case BLE_ATT_OP_NOTIFY_REQ:
-            ble_gattc_notify(conn_handle, chr_val_handle);
-            break;
-        case BLE_ATT_OP_INDICATE_REQ:
-            ble_gattc_indicate(conn_handle, chr_val_handle, NULL, NULL);
-            break;
-        default:
-            BLE_HS_DBG_ASSERT(0);
-            break;
-        }
+        BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs >
+                               clt_cfg_idx);
+        clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx;
+        BLE_HS_DBG_ASSERT_EVAL(clt_cfg->chr_val_handle == chr_val_handle);
+
+        /* Mark the CCCD entry as modified. */
+        clt_cfg->flags |= BLE_GATTS_CLT_CFG_F_MODIFIED;
+        new_notifications = 1;
+    }
+    ble_hs_unlock();
+
+    if (new_notifications) {
+        ble_hs_notifications_sched();
     }
 
     /*** Persist updated flag for unconnected and not-yet-bonded devices. */
@@ -1319,6 +1468,92 @@ ble_gatts_chr_updated(uint16_t chr_val_handle)
 }
 
 /**
+ * Sends notifications or indications for the specified characteristic to all
+ * connected devices.  The bluetooth spec does not allow more than one
+ * concurrent indication for a single peer, so this function will hold off on
+ * sending such indications.
+ */
+static void
+ble_gatts_tx_notifications_one_chr(uint16_t chr_val_handle)
+{
+    struct ble_gatts_clt_cfg *clt_cfg;
+    struct ble_hs_conn *conn;
+    uint16_t conn_handle;
+    uint8_t att_op;
+    int clt_cfg_idx;
+    int i;
+
+    /* Determine if notifications / indications are enabled for this
+     * characteristic.
+     */
+    clt_cfg_idx = ble_gatts_clt_cfg_find_idx(ble_gatts_clt_cfgs,
+                                             chr_val_handle);
+    if (clt_cfg_idx == -1) {
+        return;
+    }
+
+    for (i = 0; ; i++) {
+        ble_hs_lock();
+
+        conn = ble_hs_conn_find_by_idx(i);
+        if (conn != NULL) {
+            BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs >
+                                   clt_cfg_idx);
+            clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx;
+            BLE_HS_DBG_ASSERT_EVAL(clt_cfg->chr_val_handle == chr_val_handle);
+
+            /* Determine what type of command should get sent, if any. */
+            att_op = ble_gatts_schedule_update(conn, clt_cfg);
+            conn_handle = conn->bhc_handle;
+        } else {
+            /* Silence some spurious gcc warnings. */
+            att_op = 0;
+            conn_handle = BLE_HS_CONN_HANDLE_NONE;
+        }
+        ble_hs_unlock();
+
+        if (conn == NULL) {
+            /* No more connected devices. */
+            break;
+        }
+
+        switch (att_op) {
+        case 0:
+            break;
+
+        case BLE_ATT_OP_NOTIFY_REQ:
+            ble_gattc_notify(conn_handle, chr_val_handle);
+            break;
+
+        case BLE_ATT_OP_INDICATE_REQ:
+            ble_gattc_indicate(conn_handle, chr_val_handle);
+            break;
+
+        default:
+            BLE_HS_DBG_ASSERT(0);
+            break;
+        }
+    }
+}
+
+/**
+ * Sends all pending notifications and indications.  The bluetooth spec does
+ * not allow more than one concurrent indication for a single peer, so this
+ * function will hold off on sending such indications.
+ */
+void
+ble_gatts_tx_notifications(void)
+{
+    uint16_t chr_val_handle;
+    int i;
+
+    for (i = 0; i < ble_gatts_num_cfgable_chrs; i++) {
+        chr_val_handle = ble_gatts_clt_cfgs[i].chr_val_handle;
+        ble_gatts_tx_notifications_one_chr(chr_val_handle);
+    }
+}
+
+/**
  * Called when bonding has been restored via the encryption procedure.  This
  * function:
  *     o Restores persisted CCCD entries for the connected peer.
@@ -1342,8 +1577,8 @@ ble_gatts_bonding_restored(uint16_t conn_handle)
     BLE_HS_DBG_ASSERT(conn != NULL);
     BLE_HS_DBG_ASSERT(conn->bhc_sec_state.bonded);
 
-    cccd_key.peer_addr_type = conn->bhc_addr_type;
-    memcpy(cccd_key.peer_addr, conn->bhc_addr, 6);
+    cccd_key.peer_addr_type = conn->bhc_peer_addr_type;
+    memcpy(cccd_key.peer_addr, conn->bhc_peer_addr, 6);
     cccd_key.chr_val_handle = 0;
     cccd_key.idx = 0;
 
@@ -1355,7 +1590,7 @@ ble_gatts_bonding_restored(uint16_t conn_handle)
             break;
         }
 
-        /* Assume no immediate send for this characteristic. */
+        /* Assume no notification or indication will get sent. */
         att_op = 0;
 
         ble_hs_lock();
@@ -1367,16 +1602,30 @@ ble_gatts_bonding_restored(uint16_t conn_handle)
                                          cccd_value.chr_val_handle);
         if (clt_cfg != NULL) {
             clt_cfg->flags = cccd_value.flags;
+
             if (cccd_value.value_changed) {
+                /* The characteristic's value changed while the device was
+                 * disconnected or unbonded.  Schedule the notification or
+                 * indication now.
+                 */
+                clt_cfg->flags |= BLE_GATTS_CLT_CFG_F_MODIFIED;
                 att_op = ble_gatts_schedule_update(conn, clt_cfg);
             }
         }
 
         ble_hs_unlock();
 
+        /* Tell the application if the peer changed its subscription state
+         * when it was restored from persistence.
+         */
+        ble_gatts_subscribe_event(conn_handle, cccd_value.chr_val_handle,
+                                  BLE_GAP_SUBSCRIBE_REASON_RESTORE,
+                                  0, cccd_value.flags);
+
         switch (att_op) {
         case 0:
             break;
+
         case BLE_ATT_OP_NOTIFY_REQ:
             rc = ble_gattc_notify(conn_handle, cccd_value.chr_val_handle);
             if (rc == 0) {
@@ -1384,10 +1633,11 @@ ble_gatts_bonding_restored(uint16_t conn_handle)
                 ble_store_write_cccd(&cccd_value);
             }
             break;
+
         case BLE_ATT_OP_INDICATE_REQ:
-            ble_gattc_indicate(conn_handle, cccd_value.chr_val_handle,
-                               NULL, NULL);
+            ble_gattc_indicate(conn_handle, cccd_value.chr_val_handle);
             break;
+
         default:
             BLE_HS_DBG_ASSERT(0);
             break;
@@ -1397,6 +1647,384 @@ ble_gatts_bonding_restored(uint16_t conn_handle)
     }
 }
 
+static struct ble_gatts_svc_entry *
+ble_gatts_find_svc_entry(const void *uuid128)
+{
+    struct ble_gatts_svc_entry *entry;
+    int i;
+
+    for (i = 0; i < ble_gatts_num_svc_entries; i++) {
+        entry = ble_gatts_svc_entries + i;
+        if (memcmp(uuid128, entry->svc->uuid128, 16) == 0) {
+            return entry;
+        }
+    }
+
+    return NULL;
+}
+
+static int
+ble_gatts_find_svc_chr_attr(const void *svc_uuid128, const void *chr_uuid128,
+                            struct ble_gatts_svc_entry **out_svc_entry,
+                            struct ble_att_svr_entry **out_att_chr)
+{
+    struct ble_gatts_svc_entry *svc_entry;
+    struct ble_att_svr_entry *att_svc;
+    struct ble_att_svr_entry *next;
+    struct ble_att_svr_entry *cur;
+    uint16_t uuid16;
+
+    svc_entry = ble_gatts_find_svc_entry(svc_uuid128);
+    if (svc_entry == NULL) {
+        return BLE_HS_ENOENT;
+    }
+
+    att_svc = ble_att_svr_find_by_handle(svc_entry->handle);
+    if (att_svc == NULL) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    cur = STAILQ_NEXT(att_svc, ha_next);
+    while (1) {
+        if (cur == NULL) {
+            /* Reached end of attribute list without a match. */
+            return BLE_HS_ENOENT;
+        }
+        next = STAILQ_NEXT(cur, ha_next);
+
+        if (cur->ha_handle_id == svc_entry->end_group_handle) {
+            /* Reached end of service without a match. */
+            return BLE_HS_ENOENT;
+        }
+
+        uuid16 = ble_uuid_128_to_16(cur->ha_uuid);
+        if (uuid16 == BLE_ATT_UUID_CHARACTERISTIC &&
+            next != NULL &&
+            memcmp(next->ha_uuid, chr_uuid128, 16) == 0) {
+
+            if (out_svc_entry != NULL) {
+                *out_svc_entry = svc_entry;
+            }
+            if (out_att_chr != NULL) {
+                *out_att_chr = next;
+            }
+            return 0;
+        }
+
+        cur = next;
+    }
+}
+
+/**
+ * Retrieves the attribute handle associated with a local GATT service.
+ *
+ * @param uuid128               The UUID of the service to look up.
+ * @param out_handle            On success, populated with the handle of the
+ *                                  service attribute.  Pass null if you don't
+ *                                  need this value.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOENT if the specified service could
+ *                                  not be found.
+ */
+int
+ble_gatts_find_svc(const void *uuid128, uint16_t *out_handle)
+{
+    struct ble_gatts_svc_entry *entry;
+
+    entry = ble_gatts_find_svc_entry(uuid128);
+    if (entry == NULL) {
+        return BLE_HS_ENOENT;
+    }
+
+    if (out_handle != NULL) {
+        *out_handle = entry->handle;
+    }
+    return 0;
+}
+
+/**
+ * Retrieves the pair of attribute handles associated with a local GATT
+ * characteristic.
+ *
+ * @param svc_uuid128           The UUID of the parent service.
+ * @param chr_uuid128           The UUID of the characteristic to look up.
+ * @param out_def_handle        On success, populated with the handle
+ *                                  of the characteristic definition attribute.
+ *                                  Pass null if you don't need this value.
+ * @param out_val_handle        On success, populated with the handle
+ *                                  of the characteristic value attribute.
+ *                                  Pass null if you don't need this value.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOENT if the specified service or
+ *                                  characteristic could not be found.
+ */
+int
+ble_gatts_find_chr(const void *svc_uuid128, const void *chr_uuid128,
+                   uint16_t *out_def_handle, uint16_t *out_val_handle)
+{
+    struct ble_att_svr_entry *att_chr;
+    int rc;
+
+    rc = ble_gatts_find_svc_chr_attr(svc_uuid128, chr_uuid128, NULL, &att_chr);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (out_def_handle) {
+        *out_def_handle = att_chr->ha_handle_id - 1;
+    }
+    if (out_val_handle) {
+        *out_val_handle = att_chr->ha_handle_id;
+    }
+    return 0;
+}
+
+/**
+ * Retrieves the attribute handle associated with a local GATT descriptor.
+ *
+ * @param svc_uuid128           The UUID of the grandparent service.
+ * @param chr_uuid128           The UUID of the parent characteristic.
+ * @param dsc_uuid128           The UUID of the descriptor ro look up.
+ * @param out_handle            On success, populated with the handle
+ *                                  of the descripytor attribute.  Pass null if
+ *                                  you don't need this value.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOENT if the specified service,
+ *                                  characteristic, or descriptor could not be
+ *                                  found.
+ */
+int
+ble_gatts_find_dsc(const void *svc_uuid128, const void *chr_uuid128,
+                   const void *dsc_uuid128, uint16_t *out_handle)
+{
+    struct ble_gatts_svc_entry *svc_entry;
+    struct ble_att_svr_entry *att_chr;
+    struct ble_att_svr_entry *cur;
+    uint16_t uuid16;
+    int rc;
+
+    rc = ble_gatts_find_svc_chr_attr(svc_uuid128, chr_uuid128, &svc_entry,
+                                     &att_chr);
+    if (rc != 0) {
+        return rc;
+    }
+
+    cur = STAILQ_NEXT(att_chr, ha_next);
+    while (1) {
+        if (cur == NULL) {
+            /* Reached end of attribute list without a match. */
+            return BLE_HS_ENOENT;
+        }
+
+        if (cur->ha_handle_id == svc_entry->end_group_handle) {
+            /* Reached end of service without a match. */
+            return BLE_HS_ENOENT;
+        }
+
+        uuid16 = ble_uuid_128_to_16(cur->ha_uuid);
+        if (uuid16 == BLE_ATT_UUID_CHARACTERISTIC) {
+            /* Reached end of characteristic without a match. */
+            return BLE_HS_ENOENT;
+        }
+
+        if (memcmp(cur->ha_uuid, dsc_uuid128, 16) == 0) {
+            if (out_handle != NULL) {
+                *out_handle = cur->ha_handle_id;
+                return 0;
+            }
+        }
+        cur = STAILQ_NEXT(cur, ha_next);
+    }
+}
+
+/**
+ * Queues a set of service definitions for registration.  All services queued
+ * in this manner get registered when ble_hs_init() is called.
+ *
+ * @param svcs                  An array of service definitions to queue for
+ *                                  registration.  This array must be
+ *                                  terminated with an entry whose 'type'
+ *                                  equals 0.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOMEM on heap exhaustion.
+ */
+int
+ble_gatts_add_svcs(const struct ble_gatt_svc_def *svcs)
+{
+    void *p;
+
+    p = realloc(ble_gatts_svc_defs,
+                (ble_gatts_num_svc_defs + 1) * sizeof *ble_gatts_svc_defs);
+    if (p == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    ble_gatts_svc_defs = p;
+    ble_gatts_svc_defs[ble_gatts_num_svc_defs] = svcs;
+    ble_gatts_num_svc_defs++;
+
+    return 0;
+}
+
+/**
+ * Accumulates counts of each resource type required by the specified service
+ * definition array.  This function is generally used to calculate some host
+ * configuration values prior to initialization.  This function adds the counts
+ * to the appropriate fields in the supplied ble_gatt_resources object without
+ * clearing them first, so it can be called repeatedly with different inputs to
+ * calculate totals.  Be sure to zero the resource struct prior to the first
+ * call to this function.
+ *
+ * @param svcs                  The service array containing the resource
+ *                                  definitions to be counted.
+ * @param res                   The resource counts are accumulated in this
+ *                                  struct.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the svcs array contains an
+ *                                  invalid resource definition.
+ */
+int
+ble_gatts_count_resources(const struct ble_gatt_svc_def *svcs,
+                          struct ble_gatt_resources *res)
+{
+    const struct ble_gatt_svc_def *svc;
+    const struct ble_gatt_chr_def *chr;
+    int s;
+    int i;
+    int c;
+    int d;
+
+    for (s = 0; svcs[s].type != BLE_GATT_SVC_TYPE_END; s++) {
+        svc = svcs + s;
+
+        if (!ble_gatts_svc_is_sane(svc)) {
+            BLE_HS_DBG_ASSERT(0);
+            return BLE_HS_EINVAL;
+        }
+
+        /* Each service requires:
+         *     o 1 service
+         *     o 1 attribute
+         */
+        res->svcs++;
+        res->attrs++;
+
+        if (svc->includes != NULL) {
+            for (i = 0; svc->includes[i] != NULL; i++) {
+                /* Each include requires:
+                 *     o 1 include
+                 *     o 1 attribute
+                 */
+                res->incs++;
+                res->attrs++;
+            }
+        }
+
+        if (svc->characteristics != NULL) {
+            for (c = 0; svc->characteristics[c].uuid128 != NULL; c++) {
+                chr = svc->characteristics + c;
+
+                if (!ble_gatts_chr_is_sane(chr)) {
+                    BLE_HS_DBG_ASSERT(0);
+                    return BLE_HS_EINVAL;
+                }
+
+                /* Each characteristic requires:
+                 *     o 1 characteristic
+                 *     o 2 attributes
+                 */
+                res->chrs++;
+                res->attrs += 2;
+
+                /* If the characteristic permits notifications or indications,
+                 * it has a CCCD.
+                 */
+                if (chr->flags & BLE_GATT_CHR_F_NOTIFY ||
+                    chr->flags & BLE_GATT_CHR_F_INDICATE) {
+
+                    /* Each CCCD requires:
+                     *     o 1 descriptor
+                     *     o 1 CCCD
+                     *     o 1 attribute
+                     */
+                    res->dscs++;
+                    res->cccds++;
+                    res->attrs++;
+                }
+
+                if (chr->descriptors != NULL) {
+                    for (d = 0; chr->descriptors[d].uuid128 != NULL; d++) {
+                        if (!ble_gatts_dsc_is_sane(chr->descriptors + d)) {
+                            BLE_HS_DBG_ASSERT(0);
+                            return BLE_HS_EINVAL;
+                        }
+
+                        /* Each descriptor requires:
+                         *     o 1 descriptor
+                         *     o 1 attribute
+                         */
+                        res->dscs++;
+                        res->attrs++;
+                    }
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * Adjusts a host configuration object's settings to accommodate the specified
+ * service definition array.  This function adds the counts to the appropriate
+ * fields in the supplied configuration object without clearing them first, so
+ * it can be called repeatedly with different inputs to calculate totals.  Be
+ * sure to zero the GATT server settings prior to the first call to this
+ * function.
+ *
+ * @param defs                  The service array containing the resource
+ *                                  definitions to be counted.
+ * @param cfg                   The resource counts are accumulated in this
+ *                                  configuration object.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the svcs array contains an
+ *                                  invalid resource definition.
+ */
+int
+ble_gatts_count_cfg(const struct ble_gatt_svc_def *defs,
+                    struct ble_hs_cfg *cfg)
+{
+    struct ble_gatt_resources res = { 0 };
+    int rc;
+
+    rc = ble_gatts_count_resources(defs, &res);
+    if (rc != 0) {
+        return rc;
+    }
+
+    cfg->max_services += res.svcs;
+    cfg->max_attrs += res.attrs;
+
+    /* Reserve an extra CCCD for the cache. */
+    cfg->max_client_configs += res.cccds * (cfg->max_connections + 1);
+
+    return 0;
+}
+
+static void
+ble_gatts_free_svc_defs(void)
+{
+    free(ble_gatts_svc_defs);
+    ble_gatts_svc_defs = NULL;
+    ble_gatts_num_svc_defs = 0;
+}
+
 static void
 ble_gatts_free_mem(void)
 {
@@ -1411,6 +2039,7 @@ int
 ble_gatts_init(void)
 {
     int rc;
+    int i;
 
     ble_gatts_free_mem();
     ble_gatts_num_cfgable_chrs = 0;
@@ -1435,6 +2064,17 @@ ble_gatts_init(void)
         }
     }
 
+    ble_gatts_num_svc_entries = 0;
+    for (i = 0; i < ble_gatts_num_svc_defs; i++) {
+        rc = ble_gatts_register_svcs(ble_gatts_svc_defs[i],
+                                     ble_hs_cfg.gatts_register_cb,
+                                     ble_hs_cfg.gatts_register_arg);
+        if (rc != 0) {
+            goto err;
+        }
+    }
+    ble_gatts_free_svc_defs();
+
     rc = stats_init_and_reg(
         STATS_HDR(ble_gatts_stats), STATS_SIZE_INIT_PARMS(ble_gatts_stats,
         STATS_SIZE_32), STATS_NAME_INIT_PARMS(ble_gatts_stats), "ble_gatts");
@@ -1447,5 +2087,6 @@ ble_gatts_init(void)
 
 err:
     ble_gatts_free_mem();
+    ble_gatts_free_svc_defs();
     return rc;
 }
diff --git a/net/nimble/host/src/ble_hci_cmd.c b/net/nimble/host/src/ble_hci_cmd.c
deleted file mode 100644
index 23eaba37..00000000
--- a/net/nimble/host/src/ble_hci_cmd.c
+++ /dev/null
@@ -1,298 +0,0 @@
-/**
- * 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 <stdint.h>
-#include <string.h>
-#include <errno.h>
-#include <stdio.h>
-#include "os/os.h"
-#include "ble_hs_priv.h"
-#include "host_dbg_priv.h"
-
-#define BLE_HCI_CMD_TIMEOUT     (OS_TICKS_PER_SEC)
-
-static struct os_mutex ble_hci_cmd_mutex;
-static struct os_sem ble_hci_cmd_sem;
-
-#if PHONY_HCI_ACKS
-static ble_hci_cmd_phony_ack_fn *ble_hci_cmd_phony_ack_cb;
-#endif
-
-#if PHONY_HCI_ACKS
-void
-ble_hci_set_phony_ack_cb(ble_hci_cmd_phony_ack_fn *cb)
-{
-    ble_hci_cmd_phony_ack_cb = cb;
-}
-#endif
-
-static void
-ble_hci_cmd_lock(void)
-{
-    int rc;
-
-    rc = os_mutex_pend(&ble_hci_cmd_mutex, 0xffffffff);
-    BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
-}
-
-static void
-ble_hci_cmd_unlock(void)
-{
-    int rc;
-
-    rc = os_mutex_release(&ble_hci_cmd_mutex);
-    BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
-}
-
-static int
-ble_hci_cmd_rx_cmd_complete(uint8_t event_code, uint8_t *data, int len,
-                            struct ble_hci_ack *out_ack)
-{
-    uint16_t opcode;
-    uint8_t *params;
-    uint8_t params_len;
-    uint8_t num_pkts;
-
-    if (len < BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN) {
-        /* XXX: Increment stat. */
-        return BLE_HS_ECONTROLLER;
-    }
-
-    num_pkts = data[2];
-    opcode = le16toh(data + 3);
-    params = data + 5;
-
-    /* XXX: Process num_pkts field. */
-    (void)num_pkts;
-
-    out_ack->bha_opcode = opcode;
-
-    params_len = len - BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN;
-    if (params_len > 0) {
-        out_ack->bha_status = BLE_HS_HCI_ERR(params[0]);
-    } else if (opcode == BLE_HCI_OPCODE_NOP) {
-        out_ack->bha_status = 0;
-    } else {
-        out_ack->bha_status = BLE_HS_ECONTROLLER;
-    }
-
-    /* Don't include the status byte in the parameters blob. */
-    if (params_len > 1) {
-        out_ack->bha_params = params + 1;
-        out_ack->bha_params_len = params_len - 1;
-    } else {
-        out_ack->bha_params = NULL;
-        out_ack->bha_params_len = 0;
-    }
-
-    return 0;
-}
-
-static int
-ble_hci_cmd_rx_cmd_status(uint8_t event_code, uint8_t *data, int len,
-                          struct ble_hci_ack *out_ack)
-{
-    uint16_t opcode;
-    uint8_t num_pkts;
-    uint8_t status;
-
-    if (len < BLE_HCI_EVENT_CMD_STATUS_LEN) {
-        /* XXX: Increment stat. */
-        return BLE_HS_ECONTROLLER;
-    }
-
-    status = data[2];
-    num_pkts = data[3];
-    opcode = le16toh(data + 4);
-
-    /* XXX: Process num_pkts field. */
-    (void)num_pkts;
-
-    out_ack->bha_opcode = opcode;
-    out_ack->bha_params = NULL;
-    out_ack->bha_params_len = 0;
-    out_ack->bha_status = BLE_HS_HCI_ERR(status);
-
-    return 0;
-}
-
-static int
-ble_hci_cmd_process_ack(uint8_t *params_buf, uint8_t params_buf_len,
-                        struct ble_hci_ack *out_ack)
-{
-    uint8_t event_code;
-    uint8_t param_len;
-    uint8_t event_len;
-    int rc;
-
-    /***
-     * The controller always reuses the command buffer for its acknowledgement
-     * events.  This function processes the acknowledgement event contained in
-     * the command buffer.
-     */
-
-    /* Count events received */
-    STATS_INC(ble_hs_stats, hci_event);
-
-    /* Display to console */
-    host_hci_dbg_event_disp(host_hci_cmd_buf);
-
-    event_code = host_hci_cmd_buf[0];
-    param_len = host_hci_cmd_buf[1];
-    event_len = param_len + 2;
-
-    /* Clear ack fields up front to silence spurious gcc warnings. */
-    memset(out_ack, 0, sizeof *out_ack);
-
-    switch (event_code) {
-    case BLE_HCI_EVCODE_COMMAND_COMPLETE:
-        rc = ble_hci_cmd_rx_cmd_complete(event_code, host_hci_cmd_buf,
-                                         event_len, out_ack);
-        break;
-
-    case BLE_HCI_EVCODE_COMMAND_STATUS:
-        rc = ble_hci_cmd_rx_cmd_status(event_code, host_hci_cmd_buf,
-                                       event_len, out_ack);
-        break;
-
-    default:
-        BLE_HS_DBG_ASSERT(0);
-        rc = BLE_HS_EUNKNOWN;
-        break;
-    }
-
-    if (rc == 0) {
-        if (params_buf == NULL) {
-            out_ack->bha_params_len = 0;
-        } else {
-            if (out_ack->bha_params_len > params_buf_len) {
-                out_ack->bha_params_len = params_buf_len;
-                rc = BLE_HS_ECONTROLLER;
-            }
-            memcpy(params_buf, out_ack->bha_params, out_ack->bha_params_len);
-        }
-        out_ack->bha_params = params_buf;
-    }
-
-    return rc;
-}
-
-static int
-ble_hci_cmd_wait_for_ack(void)
-{
-    int rc;
-
-#if PHONY_HCI_ACKS
-    if (ble_hci_cmd_phony_ack_cb == NULL) {
-        rc = BLE_HS_ETIMEOUT_HCI;
-    } else {
-        rc = ble_hci_cmd_phony_ack_cb(host_hci_cmd_buf, 260);
-    }
-#else
-    rc = os_sem_pend(&ble_hci_cmd_sem, BLE_HCI_CMD_TIMEOUT);
-    switch (rc) {
-    case 0:
-        break;
-    case OS_TIMEOUT:
-        rc = BLE_HS_ETIMEOUT_HCI;
-        break;
-    default:
-        rc = BLE_HS_EOS;
-        break;
-    }
-#endif
-
-    return rc;
-}
-
-int
-ble_hci_cmd_tx(void *cmd, void *evt_buf, uint8_t evt_buf_len,
-               uint8_t *out_evt_buf_len)
-{
-    struct ble_hci_ack ack;
-    int rc;
-
-    ble_hci_cmd_lock();
-
-    rc = host_hci_cmd_send_buf(cmd);
-    if (rc != 0) {
-        goto done;
-    }
-
-    rc = ble_hci_cmd_wait_for_ack();
-    if (rc != 0) {
-        goto done;
-    }
-
-    rc = ble_hci_cmd_process_ack(evt_buf, evt_buf_len, &ack);
-    if (rc != 0) {
-        goto done;
-    }
-
-    if (out_evt_buf_len != NULL) {
-        *out_evt_buf_len = ack.bha_params_len;
-    }
-
-    rc = ack.bha_status;
-
-done:
-    ble_hci_cmd_unlock();
-    return rc;
-}
-
-int
-ble_hci_cmd_tx_empty_ack(void *cmd)
-{
-    int rc;
-
-    rc = ble_hci_cmd_tx(cmd, NULL, 0, NULL);
-    if (rc != 0) {
-        return rc;
-    }
-
-    return 0;
-}
-
-void
-ble_hci_cmd_rx_ack(uint8_t *ack_ev)
-{
-    /* The controller should always reuse the command buffer for its acks. */
-    BLE_HS_DBG_ASSERT(ack_ev == host_hci_cmd_buf);
-
-    if (ble_hci_cmd_sem.sem_tokens != 0) {
-        /* This ack is unexpected; ignore it. */
-        return;
-    }
-
-    /* Unblock the application now that the HCI command buffer is populated
-     * with the acknowledgement.
-     */
-    os_sem_release(&ble_hci_cmd_sem);
-}
-
-void
-ble_hci_cmd_init(void)
-{
-    int rc;
-
-    rc = os_sem_init(&ble_hci_cmd_sem, 0);
-    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
-
-    rc = os_mutex_init(&ble_hci_cmd_mutex);
-    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
-}
diff --git a/net/nimble/host/src/ble_hs.c b/net/nimble/host/src/ble_hs.c
index bd092173..8d4b0473 100644
--- a/net/nimble/host/src/ble_hs.c
+++ b/net/nimble/host/src/ble_hs.c
@@ -23,38 +23,44 @@
 #include "stats/stats.h"
 #include "util/tpq.h"
 #include "os/os.h"
-#include "nimble/hci_transport.h"
-#include "host/host_hci.h"
+#include "nimble/ble_hci_trans.h"
 #include "ble_hs_priv.h"
-#ifdef PHONY_TRANSPORT
-#include "host/ble_hs_test.h"
-#endif
 
-#ifdef ARCH_sim
-#define BLE_HS_STACK_SIZE   (1024)
-#else
-#define BLE_HS_STACK_SIZE   (512)//(250)
-#endif
+/**
+ * The maximum number of events the host will process in a row before returning
+ * control to the parent task.
+ */
+#define BLE_HS_MAX_EVS_IN_A_ROW 2
 
 static struct log_handler ble_hs_log_console_handler;
-struct log ble_hs_log;
 
-struct os_mempool g_hci_cmd_pool;
-static void *ble_hs_hci_cmd_buf;
+struct os_mempool ble_hs_hci_ev_pool;
+static void *ble_hs_hci_os_event_buf;
 
-/* XXX: this might be transport layer */
-#define HCI_OS_EVENT_BUF_SIZE   (sizeof(struct os_event))
+/** OS event - triggers tx of pending notifications and indications. */
+static struct os_event ble_hs_event_tx_notifications = {
+    .ev_type = BLE_HS_EVENT_TX_NOTIFICATIONS,
+    .ev_arg = NULL,
+};
 
-struct os_mempool g_hci_os_event_pool;
-static void *ble_hs_hci_os_event_buf;
+/** OS event - triggers a full reset. */
+static struct os_event ble_hs_event_reset = {
+    .ev_type = BLE_HS_EVENT_RESET,
+    .ev_arg = NULL,
+};
+
+uint8_t ble_hs_sync_state;
+static int ble_hs_reset_reason;
 
 #if MYNEWT_SELFTEST
 /** Use a higher frequency timer to allow tests to run faster. */
-#define BLE_HS_HEARTBEAT_OS_TICKS         (OS_TICKS_PER_SEC / 10)
+#define BLE_HS_HEARTBEAT_OS_TICKS       (OS_TICKS_PER_SEC / 10)
 #else
-#define BLE_HS_HEARTBEAT_OS_TICKS         OS_TICKS_PER_SEC
+#define BLE_HS_HEARTBEAT_OS_TICKS       OS_TICKS_PER_SEC
 #endif
 
+#define BLE_HS_SYNC_RETRY_RATE          (OS_TICKS_PER_SEC / 10)    
+
 /**
  * Handles unresponsive timeouts and periodic retries in case of resource
  * shortage.
@@ -86,6 +92,9 @@ STATS_NAME_START(ble_hs_stats)
     STATS_NAME(ble_hs_stats, hci_event)
     STATS_NAME(ble_hs_stats, hci_invalid_ack)
     STATS_NAME(ble_hs_stats, hci_unknown_event)
+    STATS_NAME(ble_hs_stats, hci_timeout)
+    STATS_NAME(ble_hs_stats, reset)
+    STATS_NAME(ble_hs_stats, sync)
 STATS_NAME_END(ble_hs_stats)
 
 int
@@ -153,24 +162,31 @@ ble_hs_process_tx_data_queue(void)
     struct os_mbuf *om;
 
     while ((om = os_mqueue_get(&ble_hs_tx_q)) != NULL) {
-#ifdef PHONY_TRANSPORT
-        ble_hs_test_pkt_txed(om);
-#else
-        ble_hci_transport_host_acl_data_send(om);
-#endif
+        ble_hci_trans_hs_acl_tx(om);
     }
 }
 
-static void
+void
 ble_hs_process_rx_data_queue(void)
 {
     struct os_mbuf *om;
 
     while ((om = os_mqueue_get(&ble_hs_rx_q)) != NULL) {
-        host_hci_data_rx(om);
+        ble_hs_hci_evt_acl_process(om);
+    }
+}
+
+static void
+ble_hs_clear_data_queue(struct os_mqueue *mqueue)
+{
+    struct os_mbuf *om;
+
+    while ((om = os_mqueue_get(mqueue)) != NULL) {
+        os_mbuf_free_chain(om);
     }
 }
 
+
 static void
 ble_hs_heartbeat_timer_reset(uint32_t ticks)
 {
@@ -180,6 +196,102 @@ ble_hs_heartbeat_timer_reset(uint32_t ticks)
     BLE_HS_DBG_ASSERT_EVAL(rc == 0);
 }
 
+void
+ble_hs_heartbeat_sched(int32_t ticks_from_now)
+{
+    if (ticks_from_now == BLE_HS_FOREVER) {
+        return;
+    }
+
+    /* Reset heartbeat timer if it is not currently scheduled or if the
+     * specified time is sooner than the current expiration time.
+     */
+    if (!os_callout_queued(&ble_hs_heartbeat_timer.cf_c) ||
+        OS_TIME_TICK_LT(ticks_from_now, ble_hs_heartbeat_timer.cf_c.c_ticks)) {
+
+        ble_hs_heartbeat_timer_reset(ticks_from_now);
+    }
+}
+
+/**
+ * Indicates whether the host has synchronized with the controller.
+ * Synchronization must occur before any host procedures can be performed.
+ *
+ * @return                      1 if the host and controller are in sync;
+ *                              0 if the host and controller our out of sync.
+ */
+int
+ble_hs_synced(void)
+{
+    return ble_hs_sync_state == BLE_HS_SYNC_STATE_GOOD;
+}
+
+static int
+ble_hs_sync(void)
+{
+    int rc;
+
+    /* Set the sync state to "bringup."  This allows the parent task to send
+     * the startup sequence to the controller.  No other tasks are allowed to
+     * send any commands.
+     */
+    ble_hs_sync_state = BLE_HS_SYNC_STATE_BRINGUP;
+
+    rc = ble_hs_startup_go();
+    if (rc == 0) {
+        ble_hs_sync_state = BLE_HS_SYNC_STATE_GOOD;
+        if (ble_hs_cfg.sync_cb != NULL) {
+            ble_hs_cfg.sync_cb();
+        }
+    } else {
+        ble_hs_sync_state = BLE_HS_SYNC_STATE_BAD;
+    }
+
+    ble_hs_heartbeat_sched(BLE_HS_SYNC_RETRY_RATE);
+
+    if (rc == 0) {
+        STATS_INC(ble_hs_stats, sync);
+    }
+
+    return rc;
+}
+
+static int
+ble_hs_reset(void)
+{
+    uint16_t conn_handle;
+    int rc;
+
+    STATS_INC(ble_hs_stats, reset);
+
+    ble_hs_sync_state = 0;
+
+    rc = ble_hci_trans_reset();
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_hs_clear_data_queue(&ble_hs_tx_q);
+    ble_hs_clear_data_queue(&ble_hs_rx_q);
+
+    while (1) {
+        conn_handle = ble_hs_atomic_first_conn_handle();
+        if (conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+            break;
+        }
+
+        ble_gap_conn_broken(conn_handle, ble_hs_reset_reason);
+    }
+
+    if (ble_hs_cfg.reset_cb != NULL && ble_hs_reset_reason != 0) {
+        ble_hs_cfg.reset_cb(ble_hs_reset_reason);
+    }
+    ble_hs_reset_reason = 0;
+
+    rc = ble_hs_sync();
+    return rc;
+}
+
 /**
  * Called once a second by the ble_hs heartbeat timer.  Handles unresponsive
  * timeouts and periodic retries in case of resource shortage.
@@ -187,43 +299,62 @@ ble_hs_heartbeat_timer_reset(uint32_t ticks)
 static void
 ble_hs_heartbeat(void *unused)
 {
-    uint32_t lcl_ticks_until_next;
-    uint32_t ticks_until_next;
+    int32_t ticks_until_next;
 
-    ticks_until_next = BLE_HS_HEARTBEAT_OS_TICKS;
+    if (!ble_hs_sync_state) {
+        ble_hs_reset();
+        return;
+    }
 
-    lcl_ticks_until_next = ble_gattc_heartbeat();
-    ticks_until_next = min(ticks_until_next, lcl_ticks_until_next);
+    /* Ensure the timer expires at least once in the next second.
+     * XXX: This is not very power efficient.  We will need separate timers for
+     * each module.
+     */
+    ticks_until_next = BLE_HS_HEARTBEAT_OS_TICKS;
+    ble_hs_heartbeat_sched(ticks_until_next);
 
-    lcl_ticks_until_next = ble_gap_heartbeat();
-    ticks_until_next = min(ticks_until_next, lcl_ticks_until_next);
+    ticks_until_next = ble_gattc_heartbeat();
+    ble_hs_heartbeat_sched(ticks_until_next);
 
-    lcl_ticks_until_next = ble_l2cap_sig_heartbeat();
-    ticks_until_next = min(ticks_until_next, lcl_ticks_until_next);
+    ticks_until_next = ble_gap_heartbeat();
+    ble_hs_heartbeat_sched(ticks_until_next);
 
-    lcl_ticks_until_next = ble_sm_heartbeat();
-    ticks_until_next = min(ticks_until_next, lcl_ticks_until_next);
+    ticks_until_next = ble_l2cap_sig_heartbeat();
+    ble_hs_heartbeat_sched(ticks_until_next);
 
-    ble_hs_heartbeat_timer_reset(ticks_until_next);
+    ticks_until_next = ble_sm_heartbeat();
+    ble_hs_heartbeat_sched(ticks_until_next);
 }
 
 static void
 ble_hs_event_handle(void *unused)
 {
     struct os_callout_func *cf;
+    struct os_eventq *evqp;
     struct os_event *ev;
-    os_sr_t sr;
+    uint8_t *hci_evt;
+    int rc;
+    int i;
 
+    evqp = &ble_hs_evq;
+
+    i = 0;
     while (1) {
-        OS_ENTER_CRITICAL(sr);
-        ev = STAILQ_FIRST(&ble_hs_evq.evq_list);
-        OS_EXIT_CRITICAL(sr);
+        /* If the host has already processed several consecutive events, stop
+         * and return control to the parent task.  Put an event on the parent
+         * task's eventq to indicate that more host events are enqueued.
+         */
+        if (i >= BLE_HS_MAX_EVS_IN_A_ROW) {
+            os_eventq_put(ble_hs_parent_evq, &ble_hs_event_co.cf_c.c_ev);
+            break;
+        }
+        i++;
 
+        ev = os_eventq_poll(&evqp, 1, 0);
         if (ev == NULL) {
             break;
         }
 
-        ev = os_eventq_get(&ble_hs_evq);
         switch (ev->ev_type) {
         case OS_EVENT_T_TIMER:
             cf = (struct os_callout_func *)ev;
@@ -232,8 +363,16 @@ ble_hs_event_handle(void *unused)
             break;
 
         case BLE_HOST_HCI_EVENT_CTLR_EVENT:
-            /* Process HCI event from controller */
-            host_hci_os_event_proc(ev);
+            hci_evt = ev->ev_arg;
+            rc = os_memblock_put(&ble_hs_hci_ev_pool, ev);
+            BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+            ble_hs_hci_evt_process(hci_evt);
+            break;
+
+        case BLE_HS_EVENT_TX_NOTIFICATIONS:
+            BLE_HS_DBG_ASSERT(ev == &ble_hs_event_tx_notifications);
+            ble_gatts_tx_notifications();
             break;
 
         case OS_EVENT_T_MQUEUE_DATA:
@@ -241,6 +380,11 @@ ble_hs_event_handle(void *unused)
             ble_hs_process_rx_data_queue();
             break;
 
+        case BLE_HS_EVENT_RESET:
+            BLE_HS_DBG_ASSERT(ev == &ble_hs_event_reset);
+            ble_hs_reset();
+            break;
+
         default:
             BLE_HS_DBG_ASSERT(0);
             break;
@@ -255,6 +399,67 @@ ble_hs_event_enqueue(struct os_event *ev)
     os_eventq_put(ble_hs_parent_evq, &ble_hs_event_co.cf_c.c_ev);
 }
 
+void
+ble_hs_enqueue_hci_event(uint8_t *hci_evt)
+{
+    struct os_event *ev;
+
+    ev = os_memblock_get(&ble_hs_hci_ev_pool);
+    if (ev == NULL) {
+        ble_hci_trans_buf_free(hci_evt);
+    } else {
+        ev->ev_queued = 0;
+        ev->ev_type = BLE_HOST_HCI_EVENT_CTLR_EVENT;
+        ev->ev_arg = hci_evt;
+        ble_hs_event_enqueue(ev);
+    }
+}
+
+/**
+ * Schedules for all pending notifications and indications to be sent in the
+ * host parent task.
+ */
+void
+ble_hs_notifications_sched(void)
+{
+#if MYNEWT_SELFTEST
+    if (!os_started()) {
+        ble_gatts_tx_notifications();
+        return;
+    }
+#endif
+
+    ble_hs_event_enqueue(&ble_hs_event_tx_notifications);
+}
+
+void
+ble_hs_sched_reset(int reason)
+{
+    BLE_HS_DBG_ASSERT(ble_hs_reset_reason == 0);
+
+    ble_hs_reset_reason = reason;
+    ble_hs_event_enqueue(&ble_hs_event_reset);
+}
+
+void
+ble_hs_hw_error(uint8_t hw_code)
+{
+    ble_hs_sched_reset(BLE_HS_HW_ERR(hw_code));
+}
+
+/**
+ * Synchronizes the host with the controller by sending a sequence of HCI
+ * commands.  This function must be called before any other host functionality
+ * is used, but it must be called after both the host and controller are
+ * initialized.  Typically, the host-parent-task calls this function at the top
+ * of its task routine.
+ *
+ * If the host fails to synchronize with the controller (if the controller is
+ * not fully booted, for example), the host will attempt to resynchronize every
+ * 100 ms.  For this reason, an error return code is not necessarily fatal.
+ *
+ * @return                      0 on success; nonzero on error.
+ */
 int
 ble_hs_start(void)
 {
@@ -262,11 +467,9 @@ ble_hs_start(void)
 
     ble_hs_parent_task = os_sched_get_current_task();
 
-    ble_hs_heartbeat_timer_reset(BLE_HS_HEARTBEAT_OS_TICKS);
-
     ble_gatts_start();
 
-    rc = ble_hs_startup_go();
+    rc = ble_hs_sync();
     return rc;
 }
 
@@ -280,19 +483,29 @@ ble_hs_start(void)
  * @return                      0 on success; nonzero on failure.
  */
 int
-ble_hs_rx_data(struct os_mbuf *om)
+ble_hs_rx_data(struct os_mbuf *om, void *arg)
 {
     int rc;
 
     rc = os_mqueue_put(&ble_hs_rx_q, &ble_hs_evq, om);
-    if (rc != 0) {
-        return BLE_HS_EOS;
+    if (rc == 0) {
+        os_eventq_put(ble_hs_parent_evq, &ble_hs_event_co.cf_c.c_ev);
+    } else {
+        os_mbuf_free_chain(om);
+        rc = BLE_HS_EOS;
     }
-    os_eventq_put(ble_hs_parent_evq, &ble_hs_event_co.cf_c.c_ev);
-
-    return 0;
+    return rc;
 }
 
+/**
+ * Enqueues an ACL data packet for transmission.  This function consumes the
+ * supplied mbuf, regardless of the outcome.
+ *
+ * @param om                    The outgoing data packet, beginning with the
+ *                                  HCI ACL data header.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
 int
 ble_hs_tx_data(struct os_mbuf *om)
 {
@@ -300,6 +513,7 @@ ble_hs_tx_data(struct os_mbuf *om)
 
     rc = os_mqueue_put(&ble_hs_tx_q, &ble_hs_evq, om);
     if (rc != 0) {
+        os_mbuf_free_chain(om);
         return BLE_HS_EOS;
     }
     os_eventq_put(ble_hs_parent_evq, &ble_hs_event_co.cf_c.c_ev);
@@ -310,15 +524,26 @@ ble_hs_tx_data(struct os_mbuf *om)
 static void
 ble_hs_free_mem(void)
 {
-    free(ble_hs_hci_cmd_buf);
-    ble_hs_hci_cmd_buf = NULL;
-
     free(ble_hs_hci_os_event_buf);
     ble_hs_hci_os_event_buf = NULL;
 }
 
 /**
- * Initializes the host portion of the BLE stack.
+ * Initializes the NimBLE host.  This function must be called before the OS is
+ * started.  The NimBLE stack requires an application task to function.  One
+ * application task in particular is designated as the "host parent task".  In
+ * addition to application-specific work, the host parent task does work for
+ * NimBLE by processing events generated by the host.
+ *
+ * @param app_evq               The event queue associated with the host parent
+ *                                  task.
+ * @param cfg                   The set of configuration settings to initialize
+ *                                  the host with.  Specify null for defaults.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_ENOMEM if initialization failed due to
+ *                                  resource exhaustion.
+ *                              Other nonzero on error.
  */
 int
 ble_hs_init(struct os_eventq *app_evq, struct ble_hs_cfg *cfg)
@@ -339,30 +564,17 @@ ble_hs_init(struct os_eventq *app_evq, struct ble_hs_cfg *cfg)
     log_console_handler_init(&ble_hs_log_console_handler);
     log_register("ble_hs", &ble_hs_log, &ble_hs_log_console_handler);
 
-    ble_hs_hci_cmd_buf = malloc(OS_MEMPOOL_BYTES(ble_hs_cfg.max_hci_bufs,
-                                                 HCI_CMD_BUF_SIZE));
-    if (ble_hs_hci_cmd_buf == NULL) {
-        rc = BLE_HS_ENOMEM;
-        goto err;
-    }
-
-    /* Create memory pool of command buffers */
-    rc = os_mempool_init(&g_hci_cmd_pool, ble_hs_cfg.max_hci_bufs,
-                         HCI_CMD_BUF_SIZE, ble_hs_hci_cmd_buf,
-                         "HCICmdPool");
-    assert(rc == 0);
-
-    ble_hs_hci_os_event_buf = malloc(OS_MEMPOOL_BYTES(ble_hs_cfg.max_hci_bufs,
-                                                      HCI_OS_EVENT_BUF_SIZE));
+    ble_hs_hci_os_event_buf = malloc(
+        OS_MEMPOOL_BYTES(ble_hs_cfg.max_hci_bufs, sizeof (struct os_event)));
     if (ble_hs_hci_os_event_buf == NULL) {
         rc = BLE_HS_ENOMEM;
         goto err;
     }
 
     /* Create memory pool of OS events */
-    rc = os_mempool_init(&g_hci_os_event_pool, ble_hs_cfg.max_hci_bufs,
-                         HCI_OS_EVENT_BUF_SIZE, ble_hs_hci_os_event_buf,
-                         "HCIOsEventPool");
+    rc = os_mempool_init(&ble_hs_hci_ev_pool, ble_hs_cfg.max_hci_bufs,
+                         sizeof (struct os_event), ble_hs_hci_os_event_buf,
+                         "ble_hs_hci_ev_pool");
     assert(rc == 0);
 
     /* Initialize eventq */
@@ -375,7 +587,7 @@ ble_hs_init(struct os_eventq *app_evq, struct ble_hs_cfg *cfg)
         goto err;
     }
 
-    ble_hci_cmd_init();
+    ble_hs_hci_init();
 
     rc = ble_hs_conn_init();
     if (rc != 0) {
@@ -437,6 +649,9 @@ ble_hs_init(struct os_eventq *app_evq, struct ble_hs_cfg *cfg)
     ble_hs_dbg_mutex_locked = 0;
 #endif
 
+    /* Configure the HCI transport to communicate with a host. */
+    ble_hci_trans_cfg_hs(ble_hs_hci_rx_evt, NULL, ble_hs_rx_data, NULL);
+
     return 0;
 
 err:
diff --git a/net/nimble/host/src/ble_hs_adv.c b/net/nimble/host/src/ble_hs_adv.c
index c56bb194..fd7bfafb 100644
--- a/net/nimble/host/src/ble_hs_adv.c
+++ b/net/nimble/host/src/ble_hs_adv.c
@@ -20,8 +20,12 @@
 #include <string.h>
 #include <errno.h>
 #include "nimble/ble.h"
+#include "host/ble_hs_adv.h"
 #include "ble_hs_priv.h"
 
+static uint16_t ble_hs_adv_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2];
+static uint32_t ble_hs_adv_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4];
+
 static int
 ble_hs_adv_set_hdr(uint8_t type, uint8_t data_len, uint8_t max_len,
                    uint8_t *dst, uint8_t *dst_len)
@@ -39,7 +43,7 @@ ble_hs_adv_set_hdr(uint8_t type, uint8_t data_len, uint8_t max_len,
 }
 
 int
-ble_hs_adv_set_flat(uint8_t type, int data_len, void *data,
+ble_hs_adv_set_flat(uint8_t type, int data_len, const void *data,
                     uint8_t *dst, uint8_t *dst_len, uint8_t max_len)
 {
 #if !NIMBLE_OPT(ADVERTISE)
@@ -62,7 +66,7 @@ ble_hs_adv_set_flat(uint8_t type, int data_len, void *data,
 }
 
 static int
-ble_hs_adv_set_array16(uint8_t type, uint8_t num_elems, uint16_t *elems,
+ble_hs_adv_set_array16(uint8_t type, uint8_t num_elems, const uint16_t *elems,
                        uint8_t *dst, uint8_t *dst_len, uint8_t max_len)
 {
     int rc;
@@ -83,7 +87,7 @@ ble_hs_adv_set_array16(uint8_t type, uint8_t num_elems, uint16_t *elems,
 }
 
 static int
-ble_hs_adv_set_array32(uint8_t type, uint8_t num_elems, uint32_t *elems,
+ble_hs_adv_set_array32(uint8_t type, uint8_t num_elems, const uint32_t *elems,
                        uint8_t *dst, uint8_t *dst_len, uint8_t max_len)
 {
     int rc;
@@ -106,10 +110,10 @@ ble_hs_adv_set_array32(uint8_t type, uint8_t num_elems, uint32_t *elems,
 /**
  * Sets the significant part of the data in outgoing advertisements.
  *
- * @return                      0 on success;  on failure.
+ * @return                      0 on success; nonzero on failure.
  */
 int
-ble_hs_adv_set_fields(struct ble_hs_adv_fields *adv_fields,
+ble_hs_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
                       uint8_t *dst, uint8_t *dst_len, uint8_t max_len)
 {
 #if !NIMBLE_OPT(ADVERTISE)
@@ -117,11 +121,28 @@ ble_hs_adv_set_fields(struct ble_hs_adv_fields *adv_fields,
 #endif
 
     uint8_t type;
+    int8_t tx_pwr_lvl;
     int rc;
 
     *dst_len = 0;
 
-    /*** 0x01 - Flags (written automatically by GAP). */
+    /*** 0x01 - Flags. */
+    /* The application has three options concerning the flags field:
+     * 1. Don't include it in advertisements (!flags_is_present).
+     * 2. Explicitly specify the value (flags_is_present && flags != 0).
+     * 3. Let stack calculate the value (flags_is_present && flags == 0).
+     *
+     * For option 3, the calculation is delayed until advertising is enabled.
+     * The delay is necessary because the flags value depends on the type of
+     * advertising being performed which is not known at this time.
+     *
+     * Note: The CSS prohibits advertising a flags value of 0, so this method
+     * of specifying option 2 vs. 3 is sound.
+     */
+    if (adv_fields->flags_is_present && adv_fields->flags != 0) {
+        rc = ble_hs_adv_set_flat(BLE_HS_ADV_TYPE_FLAGS, 1, &adv_fields->flags,
+                                 dst, dst_len, max_len);
+    }
 
     /*** 0x02,0x03 - 16-bit service class UUIDs. */
     if (adv_fields->uuids16 != NULL && adv_fields->num_uuids16) {
@@ -185,7 +206,26 @@ ble_hs_adv_set_fields(struct ble_hs_adv_fields *adv_fields,
         }
     }
 
-    /*** 0x0a - Tx power level (written automatically by GAP). */
+    /*** 0x0a - Tx power level. */
+    if (adv_fields->tx_pwr_lvl_is_present) {
+        /* Read the power level from the controller if requested; otherwise use
+         * the explicitly specified value.
+         */
+        if (adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO) {
+            rc = ble_hs_hci_util_read_adv_tx_pwr(&tx_pwr_lvl);
+            if (rc != 0) {
+                return rc;
+            }
+        } else {
+            tx_pwr_lvl = adv_fields->tx_pwr_lvl;
+        }
+
+        rc = ble_hs_adv_set_flat(BLE_HS_ADV_TYPE_TX_PWR_LVL, 1, &tx_pwr_lvl,
+                                 dst, dst_len, max_len);
+        if (rc != 0) {
+            return rc;
+        }
+    }
 
     /*** 0x0d - Class of device. */
     if (adv_fields->device_class != NULL) {
@@ -321,12 +361,53 @@ ble_hs_adv_set_fields(struct ble_hs_adv_fields *adv_fields,
 }
 
 static int
+ble_hs_adv_parse_uuids16(struct ble_hs_adv_fields *adv_fields,
+                         const uint8_t *data, uint8_t data_len)
+{
+    int i;
+
+    if (data_len % 2 != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    adv_fields->uuids16 = ble_hs_adv_uuids16;
+    adv_fields->num_uuids16 = data_len / 2;
+
+    for (i = 0; i < adv_fields->num_uuids16; i++) {
+        adv_fields->uuids16[i] = le16toh(data + i * 2);
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_adv_parse_uuids32(struct ble_hs_adv_fields *adv_fields,
+                         const uint8_t *data, uint8_t data_len)
+{
+    int i;
+
+    if (data_len % 4 != 0) {
+        return BLE_HS_EBADDATA;
+    }
+
+    adv_fields->uuids32 = ble_hs_adv_uuids32;
+    adv_fields->num_uuids32 = data_len / 4;
+
+    for (i = 0; i < adv_fields->num_uuids32; i++) {
+        adv_fields->uuids32[i] = le32toh(data + i * 4);
+    }
+
+    return 0;
+}
+
+static int
 ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
                            uint8_t *total_len, uint8_t *src, uint8_t src_len)
 {
     uint8_t data_len;
     uint8_t type;
     uint8_t *data;
+    int rc;
 
     if (src_len < 1) {
         return BLE_HS_EMSGSIZE;
@@ -341,6 +422,10 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
     data = src + 2;
     data_len = *total_len - 2;
 
+    if (data_len > BLE_HS_ADV_MAX_FIELD_SZ) {
+        return BLE_HS_EBADDATA;
+    }
+
     switch (type) {
     case BLE_HS_ADV_TYPE_FLAGS:
         if (data_len != BLE_HS_ADV_FLAGS_LEN) {
@@ -351,39 +436,35 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
         break;
 
     case BLE_HS_ADV_TYPE_INCOMP_UUIDS16:
-        if (data_len % 2 != 0) {
-            return BLE_HS_EBADDATA;
+        rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
         }
-        adv_fields->uuids16 = data;
-        adv_fields->num_uuids16 = data_len / 2;
         adv_fields->uuids16_is_complete = 0;
         break;
 
     case BLE_HS_ADV_TYPE_COMP_UUIDS16:
-        if (data_len % 2 != 0) {
-            return BLE_HS_EBADDATA;
+        rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
         }
-        adv_fields->uuids16 = data;
-        adv_fields->num_uuids16 = data_len / 2;
         adv_fields->uuids16_is_complete = 1;
         break;
 
     case BLE_HS_ADV_TYPE_INCOMP_UUIDS32:
-        if (data_len % 4 != 0) {
-            return BLE_HS_EBADDATA;
+        rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
         }
-        adv_fields->uuids32 = data;
-        adv_fields->num_uuids32 = data_len / 4;
-        adv_fields->uuids32_is_complete = 0;
+        adv_fields->uuids16_is_complete = 0;
         break;
 
     case BLE_HS_ADV_TYPE_COMP_UUIDS32:
-        if (data_len % 4 != 0) {
-            return BLE_HS_EBADDATA;
+        rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len);
+        if (rc != 0) {
+            return rc;
         }
-        adv_fields->uuids32 = data;
-        adv_fields->num_uuids32 = data_len / 4;
-        adv_fields->uuids32_is_complete = 1;
+        adv_fields->uuids16_is_complete = 1;
         break;
 
     case BLE_HS_ADV_TYPE_INCOMP_UUIDS128:
@@ -443,6 +524,7 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
             return BLE_HS_EBADDATA;
         }
         adv_fields->svc_data_uuid16 = data;
+        adv_fields->svc_data_uuid16_len = data_len;
         break;
 
     case BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR:
@@ -490,6 +572,7 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
             return BLE_HS_EBADDATA;
         }
         adv_fields->svc_data_uuid32 = data;
+        adv_fields->svc_data_uuid32_len = data_len;
         break;
 
     case BLE_HS_ADV_TYPE_SVC_DATA_UUID128:
@@ -497,6 +580,7 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
             return BLE_HS_EBADDATA;
         }
         adv_fields->svc_data_uuid128 = data;
+        adv_fields->svc_data_uuid128_len = data_len;
         break;
 
     case BLE_HS_ADV_TYPE_URI:
diff --git a/net/nimble/host/src/ble_hs_adv_priv.h b/net/nimble/host/src/ble_hs_adv_priv.h
index 5b853740..3ad4f49c 100644
--- a/net/nimble/host/src/ble_hs_adv_priv.h
+++ b/net/nimble/host/src/ble_hs_adv_priv.h
@@ -6,7 +6,7 @@
  * 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,
@@ -20,16 +20,11 @@
 #ifndef H_BLE_HS_ADV_PRIV_
 #define H_BLE_HS_ADV_PRIV_
 
-#include "host/ble_hs_adv.h"
-
-struct ble_hs_adv {
-    uint8_t event_type;
-    uint8_t addr_type;
-    uint8_t length_data;
-    int8_t rssi;
-    uint8_t addr[6];
-    uint8_t *data;
-    struct ble_hs_adv_fields *fields;
-};
+int ble_hs_adv_set_flat(uint8_t type, int data_len, const void *data,
+                        uint8_t *dst, uint8_t *dst_len, uint8_t max_len);
+int ble_hs_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
+                          uint8_t *dst, uint8_t *dst_len, uint8_t max_len);
+int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields, uint8_t *src,
+                            uint8_t src_len);
 
 #endif
diff --git a/net/nimble/host/src/ble_hs_atomic.c b/net/nimble/host/src/ble_hs_atomic.c
index 36a603dd..9c933fce 100644
--- a/net/nimble/host/src/ble_hs_atomic.c
+++ b/net/nimble/host/src/ble_hs_atomic.c
@@ -93,3 +93,23 @@ ble_hs_atomic_conn_set_flags(uint16_t conn_handle, ble_hs_conn_flags_t flags,
 
     return rc;
 }
+
+uint16_t
+ble_hs_atomic_first_conn_handle(void)
+{
+    const struct ble_hs_conn *conn;
+    uint16_t conn_handle;
+
+    ble_hs_lock();
+
+    conn = ble_hs_conn_first();
+    if (conn != NULL) {
+        conn_handle = conn->bhc_handle;
+    } else {
+        conn_handle = BLE_HS_CONN_HANDLE_NONE;
+    }
+
+    ble_hs_unlock();
+
+    return conn_handle;
+}
diff --git a/net/nimble/host/src/ble_hs_atomic_priv.h b/net/nimble/host/src/ble_hs_atomic_priv.h
index a08b7ca7..d82eeabc 100644
--- a/net/nimble/host/src/ble_hs_atomic_priv.h
+++ b/net/nimble/host/src/ble_hs_atomic_priv.h
@@ -28,5 +28,6 @@ int ble_hs_atomic_conn_flags(uint16_t conn_handle,
                              ble_hs_conn_flags_t *out_flags);
 int ble_hs_atomic_conn_set_flags(uint16_t conn_handle,
                                  ble_hs_conn_flags_t flags, int on);
+uint16_t ble_hs_atomic_first_conn_handle(void);
 
 #endif
diff --git a/net/nimble/host/src/ble_hs_cfg.c b/net/nimble/host/src/ble_hs_cfg.c
index 6fe02365..eca8407e 100644
--- a/net/nimble/host/src/ble_hs_cfg.c
+++ b/net/nimble/host/src/ble_hs_cfg.c
@@ -19,31 +19,38 @@
 
 #include "ble_hs_priv.h"
 
+#if NIMBLE_OPT(CONNECT)
+#define BLE_HS_CFG_MAX_CONNECTIONS NIMBLE_OPT(MAX_CONNECTIONS)
+#else
+#define BLE_HS_CFG_MAX_CONNECTIONS 0
+#endif
+
 const struct ble_hs_cfg ble_hs_cfg_dflt = {
     /** HCI settings. */
-    .max_hci_bufs = 8,
+    .max_hci_bufs = 14,
 
     /** Connection settings. */
-#if NIMBLE_OPT(CONNECT)
-    .max_connections = NIMBLE_OPT(MAX_CONNECTIONS),
-#else
-    .max_connections = 0,
-#endif
+    .max_connections = BLE_HS_CFG_MAX_CONNECTIONS,
 
     /** GATT server settings. */
-    .max_services = 16,
-    .max_client_configs = 32,
+    /* These get set to zero with the expectation that they will be increased
+     * as needed when each supported GATT service is initialized.
+     */
+    .max_services = 0,
+    .max_client_configs = 0,
 
     /** GATT client settings. */
-    .max_gattc_procs = 16,
+    .max_gattc_procs = 4,
 
     /** ATT server settings. */
-    .max_attrs = 64,
+    /* This is set to 0; see note above re: GATT server settings. */
+    .max_attrs = 0,
     .max_prep_entries = 6,
 
     /** L2CAP settings. */
-    .max_l2cap_chans = 16,
-    .max_l2cap_sig_procs = 8,
+    /* Three channels per connection (sig, att, and sm). */
+    .max_l2cap_chans = 3 * BLE_HS_CFG_MAX_CONNECTIONS,
+    .max_l2cap_sig_procs = 1,
     .max_l2cap_sm_procs = 1,
 
     /** Security manager settings. */
@@ -55,7 +62,8 @@ const struct ble_hs_cfg ble_hs_cfg_dflt = {
     .sm_keypress = 0,
     .sm_our_key_dist = 0,
     .sm_their_key_dist = 0,
-    /** privacy info */
+
+    /** Privacy settings. */
     .rpa_timeout = 300,
 };
 
diff --git a/net/nimble/host/src/ble_hs_conn.c b/net/nimble/host/src/ble_hs_conn.c
index a4b0f16b..34d95903 100644
--- a/net/nimble/host/src/ble_hs_conn.c
+++ b/net/nimble/host/src/ble_hs_conn.c
@@ -20,7 +20,7 @@
 #include <string.h>
 #include <errno.h>
 #include "os/os.h"
-#include "host/host_hci.h"
+#include "host/ble_hs_id.h"
 #include "ble_hs_priv.h"
 
 /** At least three channels required per connection (sig, att, sm). */
@@ -187,8 +187,6 @@ ble_hs_conn_free(struct ble_hs_conn *conn)
         return;
     }
 
-    ble_gatts_conn_deinit(&conn->bhc_gatt_svr);
-
     ble_att_svr_prep_clear(&conn->bhc_att_svr.basc_prep_list);
 
     while ((chan = SLIST_FIRST(&conn->bhc_channels)) != NULL) {
@@ -247,6 +245,17 @@ ble_hs_conn_find(uint16_t conn_handle)
 }
 
 struct ble_hs_conn *
+ble_hs_conn_find_assert(uint16_t conn_handle)
+{
+    struct ble_hs_conn *conn;
+
+    conn = ble_hs_conn_find(conn_handle);
+    BLE_HS_DBG_ASSERT(conn != NULL);
+
+    return conn;
+}
+
+struct ble_hs_conn *
 ble_hs_conn_find_by_addr(uint8_t addr_type, uint8_t *addr)
 {
 #if !NIMBLE_OPT(CONNECT)
@@ -258,8 +267,8 @@ ble_hs_conn_find_by_addr(uint8_t addr_type, uint8_t *addr)
     BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
 
     SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) {
-        if (conn->bhc_addr_type == addr_type &&
-            memcmp(conn->bhc_addr, addr, 6) == 0) {
+        if (conn->bhc_peer_addr_type == addr_type &&
+            memcmp(conn->bhc_peer_addr, addr, 6) == 0) {
 
             return conn;
         }
@@ -316,59 +325,52 @@ ble_hs_conn_first(void)
 }
 
 void
-ble_hs_conn_addrs(struct ble_hs_conn *conn,
+ble_hs_conn_addrs(const struct ble_hs_conn *conn,
                   struct ble_hs_conn_addrs *addrs)
 {
+    int rc;
+
     /* Determine our address information. */
-    addrs->our_id_addr =
-        ble_hs_pvcy_our_id_addr(&addrs->our_id_addr_type);
-    if (memcmp(conn->our_rpa_addr, ble_hs_conn_null_addr, 6) == 0) {
+    addrs->our_id_addr_type =
+        ble_hs_misc_addr_type_to_id(conn->bhc_our_addr_type);
+    rc = ble_hs_id_addr(addrs->our_id_addr_type, &addrs->our_id_addr, NULL);
+    assert(rc == 0);
+
+    if (memcmp(conn->bhc_our_rpa_addr, ble_hs_conn_null_addr, 6) == 0) {
         addrs->our_ota_addr_type = addrs->our_id_addr_type;
         addrs->our_ota_addr = addrs->our_id_addr;
     } else {
-        switch (addrs->our_id_addr_type) {
-        case BLE_ADDR_TYPE_PUBLIC:
-            addrs->our_ota_addr_type = BLE_ADDR_TYPE_RPA_PUB_DEFAULT;
-            break;
-
-        case BLE_ADDR_TYPE_RANDOM:
-            addrs->our_ota_addr_type = BLE_ADDR_TYPE_RPA_RND_DEFAULT;
-            break;
-
-        default:
-            BLE_HS_DBG_ASSERT(0);
-        }
-
-        addrs->our_ota_addr = conn->our_rpa_addr;
+        addrs->our_ota_addr_type = conn->bhc_our_addr_type;
+        addrs->our_ota_addr = conn->bhc_our_rpa_addr;
     }
 
     /* Determine peer address information. */
-    addrs->peer_ota_addr_type = conn->bhc_addr_type;
-    addrs->peer_id_addr = conn->bhc_addr;
-    switch (conn->bhc_addr_type) {
+    addrs->peer_ota_addr_type = conn->bhc_peer_addr_type;
+    addrs->peer_id_addr = conn->bhc_peer_addr;
+    switch (conn->bhc_peer_addr_type) {
     case BLE_ADDR_TYPE_PUBLIC:
         addrs->peer_id_addr_type = BLE_ADDR_TYPE_PUBLIC;
-        addrs->peer_ota_addr = conn->bhc_addr;
+        addrs->peer_ota_addr = conn->bhc_peer_addr;
         break;
 
     case BLE_ADDR_TYPE_RANDOM:
         addrs->peer_id_addr_type = BLE_ADDR_TYPE_RANDOM;
-        addrs->peer_ota_addr = conn->bhc_addr;
+        addrs->peer_ota_addr = conn->bhc_peer_addr;
         break;
 
     case BLE_ADDR_TYPE_RPA_PUB_DEFAULT:
         addrs->peer_id_addr_type = BLE_ADDR_TYPE_PUBLIC;
-        addrs->peer_ota_addr = conn->peer_rpa_addr;
+        addrs->peer_ota_addr = conn->bhc_peer_rpa_addr;
         break;
 
     case BLE_ADDR_TYPE_RPA_RND_DEFAULT:
         addrs->peer_id_addr_type = BLE_ADDR_TYPE_RANDOM;
-        addrs->peer_ota_addr = conn->peer_rpa_addr;
+        addrs->peer_ota_addr = conn->bhc_peer_rpa_addr;
         break;
 
     default:
         BLE_HS_DBG_ASSERT(0);
-        return;
+        break;
     }
 }
 
diff --git a/net/nimble/host/src/ble_hs_conn_priv.h b/net/nimble/host/src/ble_hs_conn_priv.h
index 36b483dc..da8d2fc4 100644
--- a/net/nimble/host/src/ble_hs_conn_priv.h
+++ b/net/nimble/host/src/ble_hs_conn_priv.h
@@ -36,15 +36,16 @@ typedef uint8_t ble_hs_conn_flags_t;
 struct ble_hs_conn {
     SLIST_ENTRY(ble_hs_conn) bhc_next;
     uint16_t bhc_handle;
-    uint8_t bhc_addr_type;
-    uint8_t our_addr_type;
-    uint8_t bhc_addr[6];
-    uint8_t our_rpa_addr[6];
-    uint8_t peer_rpa_addr[6];
+    uint8_t bhc_peer_addr_type;
+    uint8_t bhc_our_addr_type;
+    uint8_t bhc_peer_addr[6];
+    uint8_t bhc_our_rpa_addr[6];
+    uint8_t bhc_peer_rpa_addr[6];
 
     uint16_t bhc_itvl;
     uint16_t bhc_latency;
     uint16_t bhc_supervision_timeout;
+    uint8_t bhc_master_clock_accuracy;
 
     ble_hs_conn_flags_t bhc_flags;
 
@@ -66,10 +67,10 @@ struct ble_hs_conn_addrs {
     uint8_t our_id_addr_type;
     uint8_t peer_ota_addr_type;
     uint8_t peer_id_addr_type;
-    uint8_t *our_ota_addr;
-    uint8_t *our_id_addr;
-    uint8_t *peer_ota_addr;
-    uint8_t *peer_id_addr;
+    const uint8_t *our_ota_addr;
+    const uint8_t *our_id_addr;
+    const uint8_t *peer_ota_addr;
+    const uint8_t *peer_id_addr;
 };
 
 int ble_hs_conn_can_alloc(void);
@@ -78,6 +79,7 @@ void ble_hs_conn_free(struct ble_hs_conn *conn);
 void ble_hs_conn_insert(struct ble_hs_conn *conn);
 void ble_hs_conn_remove(struct ble_hs_conn *conn);
 struct ble_hs_conn *ble_hs_conn_find(uint16_t conn_handle);
+struct ble_hs_conn *ble_hs_conn_find_assert(uint16_t conn_handle);
 struct ble_hs_conn *ble_hs_conn_find_by_addr(uint8_t addr_type, uint8_t *addr);
 struct ble_hs_conn *ble_hs_conn_find_by_idx(int idx);
 int ble_hs_conn_exists(uint16_t conn_handle);
@@ -86,7 +88,7 @@ struct ble_l2cap_chan *ble_hs_conn_chan_find(struct ble_hs_conn *conn,
                                              uint16_t cid);
 int ble_hs_conn_chan_insert(struct ble_hs_conn *conn,
                             struct ble_l2cap_chan *chan);
-void ble_hs_conn_addrs(struct ble_hs_conn *conn,
+void ble_hs_conn_addrs(const struct ble_hs_conn *conn,
                        struct ble_hs_conn_addrs *addrs);
 
 int ble_hs_conn_init(void);
diff --git a/net/nimble/host/src/host_dbg.c b/net/nimble/host/src/ble_hs_dbg.c
index 6b58624d..bb6ebdf9 100644
--- a/net/nimble/host/src/host_dbg.c
+++ b/net/nimble/host/src/ble_hs_dbg.c
@@ -23,11 +23,11 @@
 #include "os/os.h"
 #include "console/console.h"
 #include "nimble/hci_common.h"
-#include "nimble/hci_transport.h"
+#include "nimble/ble_hci_trans.h"
 #include "ble_hs_priv.h"
 
 static void
-host_hci_dbg_le_event_disp(uint8_t subev, uint8_t len, uint8_t *evdata)
+ble_hs_dbg_le_event_disp(uint8_t subev, uint8_t len, uint8_t *evdata)
 {
     int8_t rssi;
     uint8_t advlen;
@@ -158,7 +158,7 @@ host_hci_dbg_le_event_disp(uint8_t subev, uint8_t len, uint8_t *evdata)
  * @param len
  */
 static void
-host_hci_dbg_disconn_comp_disp(uint8_t *evdata, uint8_t len)
+ble_hs_dbg_disconn_comp_disp(uint8_t *evdata, uint8_t len)
 {
     uint8_t status;
     uint8_t reason;
@@ -183,7 +183,7 @@ host_hci_dbg_disconn_comp_disp(uint8_t *evdata, uint8_t len)
  * @param len
  */
 static void
-host_hci_dbg_encrypt_chg_disp(uint8_t *evdata, uint8_t len)
+ble_hs_dbg_encrypt_chg_disp(uint8_t *evdata, uint8_t len)
 {
     uint8_t status;
     uint8_t enabled;
@@ -209,7 +209,7 @@ host_hci_dbg_encrypt_chg_disp(uint8_t *evdata, uint8_t len)
  * @param len
  */
 static void
-host_hci_dbg_encrypt_refresh_disp(uint8_t *evdata, uint8_t len)
+ble_hs_dbg_encrypt_refresh_disp(uint8_t *evdata, uint8_t len)
 {
     uint8_t status;
     uint16_t handle;
@@ -228,7 +228,7 @@ host_hci_dbg_encrypt_refresh_disp(uint8_t *evdata, uint8_t len)
  * @param len
  */
 static void
-host_hci_dbg_rd_rem_ver_disp(uint8_t *evdata, uint8_t len)
+ble_hs_dbg_rd_rem_ver_disp(uint8_t *evdata, uint8_t len)
 {
     BLE_HS_LOG(DEBUG, "Remote Version Info: status=%u handle=%u vers_nr=%u "
                       "compid=%u subver=%u\n",
@@ -243,7 +243,7 @@ host_hci_dbg_rd_rem_ver_disp(uint8_t *evdata, uint8_t len)
  * @param len
  */
 static void
-host_hci_dbg_num_comp_pkts_disp(uint8_t *evdata, uint8_t len)
+ble_hs_dbg_num_comp_pkts_disp(uint8_t *evdata, uint8_t len)
 {
     uint8_t handles;
     uint8_t *handle_ptr;
@@ -282,7 +282,7 @@ host_hci_dbg_num_comp_pkts_disp(uint8_t *evdata, uint8_t len)
  * @param len
  */
 static void
-host_hci_dbg_auth_pyld_tmo_disp(uint8_t *evdata, uint8_t len)
+ble_hs_dbg_auth_pyld_tmo_disp(uint8_t *evdata, uint8_t len)
 {
     uint16_t handle;
 
@@ -298,7 +298,7 @@ host_hci_dbg_auth_pyld_tmo_disp(uint8_t *evdata, uint8_t len)
 
 
 static void
-host_hci_dbg_cmd_comp_info_params(uint8_t status, uint8_t ocf, uint8_t *evdata)
+ble_hs_dbg_cmd_comp_info_params(uint8_t status, uint8_t ocf, uint8_t *evdata)
 {
     int i;
     uint8_t *dptr;
@@ -339,7 +339,7 @@ host_hci_dbg_cmd_comp_info_params(uint8_t status, uint8_t ocf, uint8_t *evdata)
 }
 
 static void
-host_hci_dbg_cmd_complete_disp(uint8_t *evdata, uint8_t len)
+ble_hs_dbg_cmd_complete_disp(uint8_t *evdata, uint8_t len)
 {
     uint8_t cmd_pkts;
     uint8_t ogf;
@@ -374,7 +374,7 @@ host_hci_dbg_cmd_complete_disp(uint8_t *evdata, uint8_t len)
     /* Display parameters based on command. */
     switch (ogf) {
     case BLE_HCI_OGF_INFO_PARAMS:
-        host_hci_dbg_cmd_comp_info_params(status, ocf, evdata);
+        ble_hs_dbg_cmd_comp_info_params(status, ocf, evdata);
         break;
     case BLE_HCI_OGF_STATUS_PARAMS:
         switch (ocf) {
@@ -438,7 +438,7 @@ done:
 }
 
 static void
-host_hci_dbg_cmd_status_disp(uint8_t *evdata, uint8_t len)
+ble_hs_dbg_cmd_status_disp(uint8_t *evdata, uint8_t len)
 {
     uint8_t ogf;
     uint8_t ocf;
@@ -453,7 +453,7 @@ host_hci_dbg_cmd_status_disp(uint8_t *evdata, uint8_t len)
 }
 
 void
-host_hci_dbg_event_disp(uint8_t *evbuf)
+ble_hs_dbg_event_disp(uint8_t *evbuf)
 {
 #if LOG_LEVEL > LOG_LEVEL_DEBUG
     return;
@@ -470,34 +470,40 @@ host_hci_dbg_event_disp(uint8_t *evbuf)
 
     switch (evcode) {
     case BLE_HCI_EVCODE_DISCONN_CMP:
-        host_hci_dbg_disconn_comp_disp(evdata, len);
+        ble_hs_dbg_disconn_comp_disp(evdata, len);
         break;
     case BLE_HCI_EVCODE_ENC_KEY_REFRESH:
-        host_hci_dbg_encrypt_refresh_disp(evdata, len);
+        ble_hs_dbg_encrypt_refresh_disp(evdata, len);
         break;
     case BLE_HCI_EVCODE_ENCRYPT_CHG:
-        host_hci_dbg_encrypt_chg_disp(evdata, len);
+        ble_hs_dbg_encrypt_chg_disp(evdata, len);
         break;
     case BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP:
-        host_hci_dbg_rd_rem_ver_disp(evdata, len);
+        ble_hs_dbg_rd_rem_ver_disp(evdata, len);
         break;
     case BLE_HCI_EVCODE_COMMAND_COMPLETE:
-        host_hci_dbg_cmd_complete_disp(evdata, len);
+        ble_hs_dbg_cmd_complete_disp(evdata, len);
         break;
     case BLE_HCI_EVCODE_COMMAND_STATUS:
-        host_hci_dbg_cmd_status_disp(evdata, len);
+        ble_hs_dbg_cmd_status_disp(evdata, len);
         break;
     case BLE_HCI_EVCODE_NUM_COMP_PKTS:
-        host_hci_dbg_num_comp_pkts_disp(evdata, len);
+        ble_hs_dbg_num_comp_pkts_disp(evdata, len);
         break;
     case BLE_HCI_EVCODE_LE_META:
-        host_hci_dbg_le_event_disp(evdata[0], len, evdata + 1);
+        ble_hs_dbg_le_event_disp(evdata[0], len, evdata + 1);
         break;
     case BLE_HCI_EVCODE_AUTH_PYLD_TMO:
-        host_hci_dbg_auth_pyld_tmo_disp(evdata, len);
+        ble_hs_dbg_auth_pyld_tmo_disp(evdata, len);
         break;
     default:
         BLE_HS_LOG(DEBUG, "Unknown event 0x%x len=%u\n", evcode, len);
         break;
     }
 }
+
+void
+ble_hs_dbg_set_sync_state(uint8_t sync_state)
+{
+    ble_hs_sync_state = sync_state;
+}
diff --git a/net/nimble/host/src/ble_hs_dbg_priv.h b/net/nimble/host/src/ble_hs_dbg_priv.h
new file mode 100644
index 00000000..cf8d2039
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_dbg_priv.h
@@ -0,0 +1,26 @@
+/**
+ * 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 H_BLE_HS_DBG_PRIV_
+#define H_BLE_HS_DBG_PRIV_
+
+void ble_hs_dbg_event_disp(uint8_t *evbuf);
+void ble_hs_dbg_set_sync_state(uint8_t sync_state);
+
+#endif /* H_HOST_DBG_ */
diff --git a/net/nimble/host/src/ble_hs_hci.c b/net/nimble/host/src/ble_hs_hci.c
new file mode 100644
index 00000000..433c2e09
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_hci.c
@@ -0,0 +1,518 @@
+/**
+ * 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 <stdint.h>
+#include <string.h>
+#include <errno.h>
+#include <stdio.h>
+#include "os/os.h"
+#include "nimble/ble_hci_trans.h"
+#include "ble_hs_priv.h"
+#include "ble_hs_dbg_priv.h"
+
+#define BLE_HCI_CMD_TIMEOUT     (OS_TICKS_PER_SEC)
+
+static struct os_mutex ble_hs_hci_mutex;
+static struct os_sem ble_hs_hci_sem;
+
+static uint8_t *ble_hs_hci_ack;
+static uint16_t ble_hs_hci_buf_sz;
+static uint8_t ble_hs_hci_max_pkts;
+
+#if PHONY_HCI_ACKS
+static ble_hs_hci_phony_ack_fn *ble_hs_hci_phony_ack_cb;
+#endif
+
+#if PHONY_HCI_ACKS
+void
+ble_hs_hci_set_phony_ack_cb(ble_hs_hci_phony_ack_fn *cb)
+{
+    ble_hs_hci_phony_ack_cb = cb;
+}
+#endif
+
+static void
+ble_hs_hci_lock(void)
+{
+    int rc;
+
+    rc = os_mutex_pend(&ble_hs_hci_mutex, 0xffffffff);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
+}
+
+static void
+ble_hs_hci_unlock(void)
+{
+    int rc;
+
+    rc = os_mutex_release(&ble_hs_hci_mutex);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
+}
+
+int
+ble_hs_hci_set_buf_sz(uint16_t pktlen, uint8_t max_pkts)
+{
+    if (pktlen == 0 || max_pkts == 0) {
+        return BLE_HS_EINVAL;
+    }
+
+    ble_hs_hci_buf_sz = pktlen;
+    ble_hs_hci_max_pkts = max_pkts;
+
+    return 0;
+}
+
+static int
+ble_hs_hci_rx_cmd_complete(uint8_t event_code, uint8_t *data, int len,
+                           struct ble_hs_hci_ack *out_ack)
+{
+    uint16_t opcode;
+    uint8_t *params;
+    uint8_t params_len;
+    uint8_t num_pkts;
+
+    if (len < BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    num_pkts = data[2];
+    opcode = le16toh(data + 3);
+    params = data + 5;
+
+    /* XXX: Process num_pkts field. */
+    (void)num_pkts;
+
+    out_ack->bha_opcode = opcode;
+
+    params_len = len - BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN;
+    if (params_len > 0) {
+        out_ack->bha_status = BLE_HS_HCI_ERR(params[0]);
+    } else if (opcode == BLE_HCI_OPCODE_NOP) {
+        out_ack->bha_status = 0;
+    } else {
+        out_ack->bha_status = BLE_HS_ECONTROLLER;
+    }
+
+    /* Don't include the status byte in the parameters blob. */
+    if (params_len > 1) {
+        out_ack->bha_params = params + 1;
+        out_ack->bha_params_len = params_len - 1;
+    } else {
+        out_ack->bha_params = NULL;
+        out_ack->bha_params_len = 0;
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_hci_rx_cmd_status(uint8_t event_code, uint8_t *data, int len,
+                         struct ble_hs_hci_ack *out_ack)
+{
+    uint16_t opcode;
+    uint8_t num_pkts;
+    uint8_t status;
+
+    if (len < BLE_HCI_EVENT_CMD_STATUS_LEN) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    status = data[2];
+    num_pkts = data[3];
+    opcode = le16toh(data + 4);
+
+    /* XXX: Process num_pkts field. */
+    (void)num_pkts;
+
+    out_ack->bha_opcode = opcode;
+    out_ack->bha_params = NULL;
+    out_ack->bha_params_len = 0;
+    out_ack->bha_status = BLE_HS_HCI_ERR(status);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_process_ack(uint16_t expected_opcode,
+                       uint8_t *params_buf, uint8_t params_buf_len,
+                       struct ble_hs_hci_ack *out_ack)
+{
+    uint8_t event_code;
+    uint8_t param_len;
+    uint8_t event_len;
+    int rc;
+
+    BLE_HS_DBG_ASSERT(ble_hs_hci_ack != NULL);
+
+    /* Count events received */
+    STATS_INC(ble_hs_stats, hci_event);
+
+    /* Display to console */
+    ble_hs_dbg_event_disp(ble_hs_hci_ack);
+
+    event_code = ble_hs_hci_ack[0];
+    param_len = ble_hs_hci_ack[1];
+    event_len = param_len + 2;
+
+    /* Clear ack fields up front to silence spurious gcc warnings. */
+    memset(out_ack, 0, sizeof *out_ack);
+
+    switch (event_code) {
+    case BLE_HCI_EVCODE_COMMAND_COMPLETE:
+        rc = ble_hs_hci_rx_cmd_complete(event_code, ble_hs_hci_ack,
+                                         event_len, out_ack);
+        break;
+
+    case BLE_HCI_EVCODE_COMMAND_STATUS:
+        rc = ble_hs_hci_rx_cmd_status(event_code, ble_hs_hci_ack,
+                                       event_len, out_ack);
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        rc = BLE_HS_EUNKNOWN;
+        break;
+    }
+
+    if (rc == 0) {
+        if (params_buf == NULL) {
+            out_ack->bha_params_len = 0;
+        } else {
+            if (out_ack->bha_params_len > params_buf_len) {
+                out_ack->bha_params_len = params_buf_len;
+                rc = BLE_HS_ECONTROLLER;
+            }
+            memcpy(params_buf, out_ack->bha_params, out_ack->bha_params_len);
+        }
+        out_ack->bha_params = params_buf;
+
+        if (out_ack->bha_opcode != expected_opcode) {
+            rc = BLE_HS_ECONTROLLER;
+        }
+    }
+
+    if (rc != 0) {
+        STATS_INC(ble_hs_stats, hci_invalid_ack);
+    }
+
+    return rc;
+}
+
+static int
+ble_hs_hci_wait_for_ack(void)
+{
+    int rc;
+
+#if PHONY_HCI_ACKS
+    if (ble_hs_hci_phony_ack_cb == NULL) {
+        rc = BLE_HS_ETIMEOUT_HCI;
+    } else {
+        ble_hs_hci_ack =
+            ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+        BLE_HS_DBG_ASSERT(ble_hs_hci_ack != NULL);
+        rc = ble_hs_hci_phony_ack_cb(ble_hs_hci_ack, 260);
+    }
+#else
+    rc = os_sem_pend(&ble_hs_hci_sem, BLE_HCI_CMD_TIMEOUT);
+    switch (rc) {
+    case 0:
+        BLE_HS_DBG_ASSERT(ble_hs_hci_ack != NULL);
+        break;
+    case OS_TIMEOUT:
+        rc = BLE_HS_ETIMEOUT_HCI;
+        STATS_INC(ble_hs_stats, hci_timeout);
+        break;
+    default:
+        rc = BLE_HS_EOS;
+        break;
+    }
+#endif
+
+    return rc;
+}
+
+int
+ble_hs_hci_cmd_tx(void *cmd, void *evt_buf, uint8_t evt_buf_len,
+                  uint8_t *out_evt_buf_len)
+{
+    struct ble_hs_hci_ack ack;
+    uint16_t opcode;
+    int rc;
+
+    opcode = le16toh(cmd);
+
+    BLE_HS_DBG_ASSERT(ble_hs_hci_ack == NULL);
+    ble_hs_hci_lock();
+
+    rc = ble_hs_hci_cmd_send_buf(cmd);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = ble_hs_hci_wait_for_ack();
+    if (rc != 0) {
+        ble_hs_sched_reset(rc);
+        goto done;
+    }
+
+    rc = ble_hs_hci_process_ack(opcode, evt_buf, evt_buf_len, &ack);
+    if (rc != 0) {
+        ble_hs_sched_reset(rc);
+        goto done;
+    }
+
+    if (out_evt_buf_len != NULL) {
+        *out_evt_buf_len = ack.bha_params_len;
+    }
+
+    rc = ack.bha_status;
+
+done:
+    if (ble_hs_hci_ack != NULL) {
+        ble_hci_trans_buf_free(ble_hs_hci_ack);
+        ble_hs_hci_ack = NULL;
+    }
+
+    ble_hs_hci_unlock();
+    return rc;
+}
+
+int
+ble_hs_hci_cmd_tx_empty_ack(void *cmd)
+{
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(cmd, NULL, 0, NULL);
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+void
+ble_hs_hci_rx_ack(uint8_t *ack_ev)
+{
+    if (ble_hs_hci_sem.sem_tokens != 0) {
+        /* This ack is unexpected; ignore it. */
+        ble_hci_trans_buf_free(ack_ev);
+        return;
+    }
+    BLE_HS_DBG_ASSERT(ble_hs_hci_ack == NULL);
+
+    /* Unblock the application now that the HCI command buffer is populated
+     * with the acknowledgement.
+     */
+    ble_hs_hci_ack = ack_ev;
+    os_sem_release(&ble_hs_hci_sem);
+}
+
+int
+ble_hs_hci_rx_evt(uint8_t *hci_ev, void *arg)
+{
+    int enqueue;
+
+    BLE_HS_DBG_ASSERT(hci_ev != NULL);
+
+    switch (hci_ev[0]) {
+    case BLE_HCI_EVCODE_COMMAND_COMPLETE:
+    case BLE_HCI_EVCODE_COMMAND_STATUS:
+        if (hci_ev[3] == 0 && hci_ev[4] == 0) {
+            enqueue = 1;
+        } else {
+            ble_hs_hci_rx_ack(hci_ev);
+            enqueue = 0;
+        }
+        break;
+
+    default:
+        enqueue = 1;
+        break;
+    }
+
+    if (enqueue) {
+        ble_hs_enqueue_hci_event(hci_ev);
+    }
+
+    return 0;
+}
+
+
+/**
+ * Splits an appropriately-sized fragment from the front of an outgoing ACL
+ * data packet, if necessary.  If the packet size is within the controller's
+ * buffer size requirements, no splitting is performed.  The fragment data is
+ * removed from the data packet mbuf.
+ *
+ * @param om                    The ACL data packet.  If this constitutes a
+ *                                  single fragment, it gets set to NULL on
+ *                                  success.
+ * @param out_frag              On success, this points to the fragment to
+ *                                  send.  If the entire packet can fit within
+ *                                  a single fragment, this will point to the
+ *                                  ACL data packet itself ('om').
+ *
+ * @return                      0 on success;
+ *                              BLE host core return code on error.
+ */
+static int
+ble_hs_hci_split_frag(struct os_mbuf **om, struct os_mbuf **out_frag)
+{
+    struct os_mbuf *frag;
+    int rc;
+
+    /* Assume failure. */
+    *out_frag = NULL;
+
+    if (OS_MBUF_PKTLEN(*om) <= ble_hs_hci_buf_sz) {
+        /* Final fragment. */
+        *out_frag = *om;
+        *om = NULL;
+        return 0;
+    }
+
+    frag = ble_hs_mbuf_acm_pkt();
+    if (frag == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+
+    /* Move data from the front of the packet into the fragment mbuf. */
+    rc = os_mbuf_appendfrom(frag, *om, 0, ble_hs_hci_buf_sz);
+    if (rc != 0) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+    os_mbuf_adj(*om, ble_hs_hci_buf_sz);
+
+    /* More fragments to follow. */
+    *out_frag = frag;
+    return 0;
+
+err:
+    os_mbuf_free_chain(frag);
+    return rc;
+}
+
+static struct os_mbuf *
+ble_hs_hci_acl_hdr_prepend(struct os_mbuf *om, uint16_t handle,
+                           uint8_t pb_flag)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om2;
+
+    hci_hdr.hdh_handle_pb_bc =
+        ble_hs_hci_util_handle_pb_bc_join(handle, pb_flag, 0);
+    htole16(&hci_hdr.hdh_len, OS_MBUF_PKTHDR(om)->omp_len);
+
+    om2 = os_mbuf_prepend(om, sizeof hci_hdr);
+    if (om2 == NULL) {
+        return NULL;
+    }
+
+    om = om2;
+    om = os_mbuf_pullup(om, sizeof hci_hdr);
+    if (om == NULL) {
+        return NULL;
+    }
+
+    memcpy(om->om_data, &hci_hdr, sizeof hci_hdr);
+
+    BLE_HS_LOG(DEBUG, "host tx hci data; handle=%d length=%d\n", handle,
+               le16toh(&hci_hdr.hdh_len));
+
+    return om;
+}
+
+/**
+ * Transmits an HCI ACL data packet.  This function consumes the supplied mbuf,
+ * regardless of the outcome.
+ *
+ * XXX: Ensure the controller has sufficient buffer capacity for the outgoing
+ * fragments.
+ */
+int
+ble_hs_hci_acl_tx(struct ble_hs_conn *connection, struct os_mbuf *txom)
+{
+    struct os_mbuf *frag;
+    uint8_t pb;
+    int rc;
+
+    /* The first fragment uses the first-non-flush packet boundary value.
+     * After sending the first fragment, pb gets set appropriately for all
+     * subsequent fragments in this packet.
+     */
+    pb = BLE_HCI_PB_FIRST_NON_FLUSH;
+
+    /* Send fragments until the entire packet has been sent. */
+    while (txom != NULL) {
+        rc = ble_hs_hci_split_frag(&txom, &frag);
+        if (rc != 0) {
+            goto err;
+        }
+
+        frag = ble_hs_hci_acl_hdr_prepend(frag, connection->bhc_handle, pb);
+        if (frag == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+        pb = BLE_HCI_PB_MIDDLE;
+
+        BLE_HS_LOG(DEBUG, "ble_hs_hci_acl_tx(): ");
+        ble_hs_log_mbuf(frag);
+        BLE_HS_LOG(DEBUG, "\n");
+
+        /* XXX: Try to pullup the entire fragment.  The controller currently
+         * requires the entire fragment to fit in a single buffer.  When this
+         * restriction is removed from the controller, this operation can be
+         * removed.
+         */
+        frag = os_mbuf_pullup(frag, OS_MBUF_PKTLEN(frag));
+        if (frag == NULL) {
+            rc = BLE_HS_ENOMEM;
+            goto err;
+        }
+
+        rc = ble_hs_tx_data(frag);
+        if (rc != 0) {
+            goto err;
+        }
+
+        connection->bhc_outstanding_pkts++;
+    }
+
+    return 0;
+
+err:
+    BLE_HS_DBG_ASSERT(rc != 0);
+
+    os_mbuf_free_chain(txom);
+    return rc;
+}
+
+void
+ble_hs_hci_init(void)
+{
+    int rc;
+
+    rc = os_sem_init(&ble_hs_hci_sem, 0);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+
+    rc = os_mutex_init(&ble_hs_hci_mutex);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+}
diff --git a/net/nimble/host/src/host_hci_cmd.c b/net/nimble/host/src/ble_hs_hci_cmd.c
index baebed3e..82b442dd 100644
--- a/net/nimble/host/src/host_hci_cmd.c
+++ b/net/nimble/host/src/ble_hs_hci_cmd.c
@@ -24,26 +24,16 @@
 #include "os/os.h"
 #include "console/console.h"
 #include "nimble/hci_common.h"
-#include "nimble/hci_transport.h"
-#include "host/host_hci.h"
-#include "host_dbg_priv.h"
+#include "nimble/ble_hci_trans.h"
+#include "ble_hs_dbg_priv.h"
 #include "ble_hs_priv.h"
-#ifdef PHONY_TRANSPORT
-#include "host/ble_hs_test.h"
-#endif
-
-uint8_t host_hci_cmd_buf[HCI_CMD_BUF_SIZE];
 
 static int
-host_hci_cmd_transport(uint8_t *cmdbuf)
+ble_hs_hci_cmd_transport(uint8_t *cmdbuf)
 {
-#ifdef PHONY_TRANSPORT
-    ble_hs_test_hci_txed(cmdbuf);
-    return 0;
-#else
     int rc;
 
-    rc = ble_hci_transport_host_cmd_send(cmdbuf);
+    rc = ble_hci_trans_hs_cmd_tx(cmdbuf);
     switch (rc) {
     case 0:
         return 0;
@@ -54,11 +44,10 @@ host_hci_cmd_transport(uint8_t *cmdbuf)
     default:
         return BLE_HS_EUNKNOWN;
     }
-#endif
 }
 
 void
-host_hci_write_hdr(uint8_t ogf, uint8_t ocf, uint8_t len, void *buf)
+ble_hs_hci_cmd_write_hdr(uint8_t ogf, uint8_t ocf, uint8_t len, void *buf)
 {
     uint16_t opcode;
     uint8_t *u8ptr;
@@ -71,44 +60,68 @@ host_hci_write_hdr(uint8_t ogf, uint8_t ocf, uint8_t len, void *buf)
 }
 
 int
-host_hci_cmd_send(uint8_t ogf, uint8_t ocf, uint8_t len, void *cmddata)
+ble_hs_hci_cmd_send(uint8_t ogf, uint8_t ocf, uint8_t len, const void *cmddata)
 {
+    uint8_t *buf;
     int rc;
 
-    htole16(host_hci_cmd_buf, ogf << 10 | ocf);
-    host_hci_cmd_buf[2] = len;
+    buf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+    BLE_HS_DBG_ASSERT(buf != NULL);
+
+    htole16(buf, ogf << 10 | ocf);
+    buf[2] = len;
     if (len != 0) {
-        memcpy(host_hci_cmd_buf + BLE_HCI_CMD_HDR_LEN, cmddata, len);
+        memcpy(buf + BLE_HCI_CMD_HDR_LEN, cmddata, len);
     }
 
-    rc = host_hci_cmd_transport(host_hci_cmd_buf);
-    BLE_HS_LOG(DEBUG, "host_hci_cmd_send: ogf=0x%02x ocf=0x%02x len=%d "
-                      "rc=%d\n", ogf, ocf, len, rc);
-    ble_hs_misc_log_flat_buf(host_hci_cmd_buf, len + BLE_HCI_CMD_HDR_LEN);
+    BLE_HS_LOG(DEBUG, "ble_hs_hci_cmd_send: ogf=0x%02x ocf=0x%02x len=%d\n",
+               ogf, ocf, len);
+    ble_hs_log_flat_buf(buf, len + BLE_HCI_CMD_HDR_LEN);
     BLE_HS_LOG(DEBUG, "\n");
+    rc = ble_hs_hci_cmd_transport(buf);
 
     if (rc == 0) {
         STATS_INC(ble_hs_stats, hci_cmd);
+    } else {
+        BLE_HS_LOG(DEBUG, "ble_hs_hci_cmd_send failure; rc=%d\n", rc);
     }
 
     return rc;
 }
 
 int
-host_hci_cmd_send_buf(void *buf)
+ble_hs_hci_cmd_send_buf(void *buf)
 {
     uint16_t opcode;
     uint8_t *u8ptr;
     uint8_t len;
     int rc;
 
+    switch (ble_hs_sync_state) {
+    case BLE_HS_SYNC_STATE_BAD:
+        return BLE_HS_ENOTSYNCED;
+
+    case BLE_HS_SYNC_STATE_BRINGUP:
+        if (!ble_hs_is_parent_task()) {
+            return BLE_HS_ENOTSYNCED;
+        }
+        break;
+
+    case BLE_HS_SYNC_STATE_GOOD:
+        break;
+
+    default:
+        BLE_HS_DBG_ASSERT(0);
+        return BLE_HS_EUNKNOWN;
+    }
+
     u8ptr = buf;
 
     opcode = le16toh(u8ptr + 0);
     len = u8ptr[2];
 
-    rc = host_hci_cmd_send(BLE_HCI_OGF(opcode), BLE_HCI_OCF(opcode), len,
-                           u8ptr + BLE_HCI_CMD_HDR_LEN);
+    rc = ble_hs_hci_cmd_send(BLE_HCI_OGF(opcode), BLE_HCI_OCF(opcode), len,
+                             u8ptr + BLE_HCI_CMD_HDR_LEN);
     return rc;
 }
 
@@ -123,16 +136,31 @@ host_hci_cmd_send_buf(void *buf)
  * @return int
  */
 static int
-host_hci_le_cmd_send(uint16_t ocf, uint8_t len, void *cmddata)
+ble_hs_hci_cmd_le_send(uint16_t ocf, uint8_t len, void *cmddata)
 {
     int rc;
-    rc = host_hci_cmd_send(BLE_HCI_OGF_LE, ocf, len, cmddata);
+    rc = ble_hs_hci_cmd_send(BLE_HCI_OGF_LE, ocf, len, cmddata);
     return rc;
 }
 
+/**
+ * Read BD_ADDR
+ *
+ * OGF = 0x04 (Informational parameters)
+ * OCF = 0x0009
+ */
+void
+ble_hs_hci_cmd_build_read_bd_addr(uint8_t *dst, int dst_len)
+{
+    BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_INFO_PARAMS,
+                             BLE_HCI_OCF_IP_RD_BD_ADDR,
+                             0, dst);
+}
+
 static int
-host_hci_cmd_body_le_whitelist_chg(uint8_t *addr, uint8_t addr_type,
-                                   uint8_t *dst)
+ble_hs_hci_cmd_body_le_whitelist_chg(const uint8_t *addr, uint8_t addr_type,
+                                     uint8_t *dst)
 {
     if (addr_type > BLE_ADDR_TYPE_RANDOM) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -145,7 +173,8 @@ host_hci_cmd_body_le_whitelist_chg(uint8_t *addr, uint8_t addr_type,
 }
 
 static int
-host_hci_cmd_body_le_set_adv_params(struct hci_adv_params *adv, uint8_t *dst)
+ble_hs_hci_cmd_body_le_set_adv_params(const struct hci_adv_params *adv,
+                                      uint8_t *dst)
 {
     uint16_t itvl;
 
@@ -192,19 +221,19 @@ host_hci_cmd_body_le_set_adv_params(struct hci_adv_params *adv, uint8_t *dst)
 }
 
 int
-host_hci_cmd_build_le_set_adv_params(struct hci_adv_params *adv, uint8_t *dst,
-                                     int dst_len)
+ble_hs_hci_cmd_build_le_set_adv_params(const struct hci_adv_params *adv,
+                                       uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_PARAM_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_PARAMS,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_PARAMS,
                        BLE_HCI_SET_ADV_PARAM_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    rc = host_hci_cmd_body_le_set_adv_params(adv, dst);
+    rc = ble_hs_hci_cmd_body_le_set_adv_params(adv, dst);
     if (rc != 0) {
         return rc;
     }
@@ -225,7 +254,8 @@ host_hci_cmd_build_le_set_adv_params(struct hci_adv_params *adv, uint8_t *dst,
  * @return int
  */
 static int
-host_hci_cmd_body_le_set_adv_data(uint8_t *data, uint8_t len, uint8_t *dst)
+ble_hs_hci_cmd_body_le_set_adv_data(const uint8_t *data, uint8_t len,
+                                    uint8_t *dst)
 {
     /* Check for valid parameters */
     if (((data == NULL) && (len != 0)) || (len > BLE_HCI_MAX_ADV_DATA_LEN)) {
@@ -252,19 +282,19 @@ host_hci_cmd_body_le_set_adv_data(uint8_t *data, uint8_t len, uint8_t *dst)
  * @return int
  */
 int
-host_hci_cmd_build_le_set_adv_data(uint8_t *data, uint8_t len, uint8_t *dst,
-                                   int dst_len)
+ble_hs_hci_cmd_build_le_set_adv_data(const uint8_t *data, uint8_t len,
+                                     uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_DATA_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_DATA,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_DATA,
                        BLE_HCI_SET_ADV_DATA_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    rc = host_hci_cmd_body_le_set_adv_data(data, len, dst);
+    rc = ble_hs_hci_cmd_body_le_set_adv_data(data, len, dst);
     if (rc != 0) {
         return rc;
     }
@@ -273,8 +303,8 @@ host_hci_cmd_build_le_set_adv_data(uint8_t *data, uint8_t len, uint8_t *dst,
 }
 
 static int
-host_hci_cmd_body_le_set_scan_rsp_data(uint8_t *data, uint8_t len,
-                                       uint8_t *dst)
+ble_hs_hci_cmd_body_le_set_scan_rsp_data(const uint8_t *data, uint8_t len,
+                                         uint8_t *dst)
 {
     /* Check for valid parameters */
     if (((data == NULL) && (len != 0)) ||
@@ -290,19 +320,19 @@ host_hci_cmd_body_le_set_scan_rsp_data(uint8_t *data, uint8_t len,
 }
 
 int
-host_hci_cmd_build_le_set_scan_rsp_data(uint8_t *data, uint8_t len,
-                                        uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_set_scan_rsp_data(const uint8_t *data, uint8_t len,
+                                          uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_RSP_DATA_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA,
                        BLE_HCI_SET_SCAN_RSP_DATA_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    rc = host_hci_cmd_body_le_set_scan_rsp_data(data, len, dst);
+    rc = ble_hs_hci_cmd_body_le_set_scan_rsp_data(data, len, dst);
     if (rc != 0) {
         return rc;
     }
@@ -311,70 +341,70 @@ host_hci_cmd_build_le_set_scan_rsp_data(uint8_t *data, uint8_t len,
 }
 
 static void
-host_hci_cmd_body_set_event_mask(uint64_t event_mask, uint8_t *dst)
+ble_hs_hci_cmd_body_set_event_mask(uint64_t event_mask, uint8_t *dst)
 {
     htole64(dst, event_mask);
 }
 
 void
-host_hci_cmd_build_set_event_mask(uint64_t event_mask,
-                                  uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_set_event_mask(uint64_t event_mask,
+                                    uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_EVENT_MASK_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_CTLR_BASEBAND,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_CTLR_BASEBAND,
                        BLE_HCI_OCF_CB_SET_EVENT_MASK,
                        BLE_HCI_SET_EVENT_MASK_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_set_event_mask(event_mask, dst);
+    ble_hs_hci_cmd_body_set_event_mask(event_mask, dst);
 }
 
 void
-host_hci_cmd_build_set_event_mask2(uint64_t event_mask,
-                                  uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_set_event_mask2(uint64_t event_mask,
+                                     uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_EVENT_MASK_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_CTLR_BASEBAND,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_CTLR_BASEBAND,
                        BLE_HCI_OCF_CB_SET_EVENT_MASK2,
                        BLE_HCI_SET_EVENT_MASK_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_set_event_mask(event_mask, dst);
+    ble_hs_hci_cmd_body_set_event_mask(event_mask, dst);
 }
 
 static void
-host_hci_cmd_body_disconnect(uint16_t handle, uint8_t reason, uint8_t *dst)
+ble_hs_hci_cmd_body_disconnect(uint16_t handle, uint8_t reason, uint8_t *dst)
 {
     htole16(dst + 0, handle);
     dst[2] = reason;
 }
 
 void
-host_hci_cmd_build_disconnect(uint16_t handle, uint8_t reason,
-                              uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_disconnect(uint16_t handle, uint8_t reason,
+                                uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_DISCONNECT_CMD_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LINK_CTRL, BLE_HCI_OCF_DISCONNECT_CMD,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LINK_CTRL, BLE_HCI_OCF_DISCONNECT_CMD,
                        BLE_HCI_DISCONNECT_CMD_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_disconnect(handle, reason, dst);
+    ble_hs_hci_cmd_body_disconnect(handle, reason, dst);
 }
 
 int
-host_hci_cmd_disconnect(uint16_t handle, uint8_t reason)
+ble_hs_hci_cmd_disconnect(uint16_t handle, uint8_t reason)
 {
     uint8_t cmd[BLE_HCI_DISCONNECT_CMD_LEN];
     int rc;
 
-    host_hci_cmd_body_disconnect(handle, reason, cmd);
-    rc = host_hci_cmd_send(BLE_HCI_OGF_LINK_CTRL,
+    ble_hs_hci_cmd_body_disconnect(handle, reason, cmd);
+    rc = ble_hs_hci_cmd_send(BLE_HCI_OGF_LINK_CTRL,
                            BLE_HCI_OCF_DISCONNECT_CMD,
                            BLE_HCI_DISCONNECT_CMD_LEN,
                            cmd);
@@ -382,24 +412,24 @@ host_hci_cmd_disconnect(uint16_t handle, uint8_t reason)
 }
 
 static void
-host_hci_cmd_body_le_set_event_mask(uint64_t event_mask, uint8_t *dst)
+ble_hs_hci_cmd_body_le_set_event_mask(uint64_t event_mask, uint8_t *dst)
 {
     htole64(dst, event_mask);
 }
 
 void
-host_hci_cmd_build_le_set_event_mask(uint64_t event_mask,
-                                     uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_set_event_mask(uint64_t event_mask,
+                                       uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_LE_EVENT_MASK_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE,
                        BLE_HCI_OCF_LE_SET_EVENT_MASK,
                        BLE_HCI_SET_LE_EVENT_MASK_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_le_set_event_mask(event_mask, dst);
+    ble_hs_hci_cmd_body_le_set_event_mask(event_mask, dst);
 }
 
 /**
@@ -411,10 +441,11 @@ host_hci_cmd_build_le_set_event_mask(uint64_t event_mask,
  * @return int
  */
 void
-host_hci_cmd_build_le_read_buffer_size(uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_read_buffer_size(uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_BUF_SIZE, 0, dst);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_BUF_SIZE,
+                             0, dst);
 }
 
 /**
@@ -426,11 +457,11 @@ host_hci_cmd_build_le_read_buffer_size(uint8_t *dst, int dst_len)
  * @return int
  */
 int
-host_hci_cmd_le_read_buffer_size(void)
+ble_hs_hci_cmd_le_read_buffer_size(void)
 {
     int rc;
 
-    rc = host_hci_le_cmd_send(BLE_HCI_OCF_LE_RD_BUF_SIZE, 0, NULL);
+    rc = ble_hs_hci_cmd_le_send(BLE_HCI_OCF_LE_RD_BUF_SIZE, 0, NULL);
     return rc;
 }
 
@@ -438,35 +469,35 @@ host_hci_cmd_le_read_buffer_size(void)
  * OGF=LE, OCF=0x0003
  */
 void
-host_hci_cmd_build_le_read_loc_supp_feat(uint8_t *dst, uint8_t dst_len)
+ble_hs_hci_cmd_build_le_read_loc_supp_feat(uint8_t *dst, uint8_t dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT,
                        0, dst);
 }
 
 static void
-host_hci_cmd_body_le_set_adv_enable(uint8_t enable, uint8_t *dst)
+ble_hs_hci_cmd_body_le_set_adv_enable(uint8_t enable, uint8_t *dst)
 {
     dst[0] = enable;
 }
 
 void
-host_hci_cmd_build_le_set_adv_enable(uint8_t enable, uint8_t *dst,
-                                     int dst_len)
+ble_hs_hci_cmd_build_le_set_adv_enable(uint8_t enable, uint8_t *dst,
+                                       int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADV_ENABLE_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_ENABLE,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADV_ENABLE,
                        BLE_HCI_SET_ADV_ENABLE_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_le_set_adv_enable(enable, dst);
+    ble_hs_hci_cmd_body_le_set_adv_enable(enable, dst);
 }
 
 static int
-host_hci_cmd_body_le_set_scan_params(
+ble_hs_hci_cmd_body_le_set_scan_params(
     uint8_t scan_type, uint16_t scan_itvl, uint16_t scan_window,
     uint8_t own_addr_type, uint8_t filter_policy, uint8_t *dst) {
 
@@ -505,24 +536,25 @@ host_hci_cmd_body_le_set_scan_params(
 }
 
 int
-host_hci_cmd_build_le_set_scan_params(uint8_t scan_type, uint16_t scan_itvl,
-                                      uint16_t scan_window,
-                                      uint8_t own_addr_type,
-                                      uint8_t filter_policy,
-                                      uint8_t *cmd, int cmd_len)
+ble_hs_hci_cmd_build_le_set_scan_params(uint8_t scan_type,
+                                        uint16_t scan_itvl,
+                                        uint16_t scan_window,
+                                        uint8_t own_addr_type,
+                                        uint8_t filter_policy,
+                                        uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
-        cmd_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_PARAM_LEN);
+        dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_PARAM_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_PARAMS,
-                       BLE_HCI_SET_SCAN_PARAM_LEN, cmd);
-    cmd += BLE_HCI_CMD_HDR_LEN;
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_PARAMS,
+                       BLE_HCI_SET_SCAN_PARAM_LEN, dst);
+    dst += BLE_HCI_CMD_HDR_LEN;
 
-    rc = host_hci_cmd_body_le_set_scan_params(scan_type, scan_itvl,
+    rc = ble_hs_hci_cmd_body_le_set_scan_params(scan_type, scan_itvl,
                                               scan_window, own_addr_type,
-                                              filter_policy, cmd);
+                                              filter_policy, dst);
     if (rc != 0) {
         return rc;
     }
@@ -531,30 +563,30 @@ host_hci_cmd_build_le_set_scan_params(uint8_t scan_type, uint16_t scan_itvl,
 }
 
 static void
-host_hci_cmd_body_le_set_scan_enable(uint8_t enable, uint8_t filter_dups,
-                                     uint8_t *dst)
+ble_hs_hci_cmd_body_le_set_scan_enable(uint8_t enable, uint8_t filter_dups,
+                                       uint8_t *dst)
 {
     dst[0] = enable;
     dst[1] = filter_dups;
 }
 
 void
-host_hci_cmd_build_le_set_scan_enable(uint8_t enable, uint8_t filter_dups,
-                                      uint8_t *dst, uint8_t dst_len)
+ble_hs_hci_cmd_build_le_set_scan_enable(uint8_t enable, uint8_t filter_dups,
+                                        uint8_t *dst, uint8_t dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_SCAN_ENABLE_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_ENABLE,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_ENABLE,
                        BLE_HCI_SET_SCAN_ENABLE_LEN, dst);
 
-    host_hci_cmd_body_le_set_scan_enable(enable, filter_dups,
+    ble_hs_hci_cmd_body_le_set_scan_enable(enable, filter_dups,
                                          dst + BLE_HCI_CMD_HDR_LEN);
 }
 
 static int
-host_hci_cmd_body_le_create_connection(struct hci_create_conn *hcc,
-                                       uint8_t *cmd)
+ble_hs_hci_cmd_body_le_create_connection(const struct hci_create_conn *hcc,
+                                         uint8_t *cmd)
 {
     /* Check scan interval and scan window */
     if ((hcc->scan_itvl < BLE_HCI_SCAN_ITVL_MIN) ||
@@ -627,18 +659,18 @@ host_hci_cmd_body_le_create_connection(struct hci_create_conn *hcc,
 }
 
 int
-host_hci_cmd_build_le_create_connection(struct hci_create_conn *hcc,
-                                        uint8_t *cmd, int cmd_len)
+ble_hs_hci_cmd_build_le_create_connection(const struct hci_create_conn *hcc,
+                                          uint8_t *cmd, int cmd_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         cmd_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_CREATE_CONN_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CREATE_CONN,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CREATE_CONN,
                        BLE_HCI_CREATE_CONN_LEN, cmd);
 
-    rc = host_hci_cmd_body_le_create_connection(hcc,
+    rc = ble_hs_hci_cmd_body_le_create_connection(hcc,
                                                 cmd + BLE_HCI_CMD_HDR_LEN);
     if (rc != 0) {
         return rc;
@@ -648,27 +680,28 @@ host_hci_cmd_build_le_create_connection(struct hci_create_conn *hcc,
 }
 
 void
-host_hci_cmd_build_le_clear_whitelist(uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_clear_whitelist(uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLEAR_WHITE_LIST,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLEAR_WHITE_LIST,
                        0, dst);
 }
 
 int
-host_hci_cmd_build_le_add_to_whitelist(uint8_t *addr, uint8_t addr_type,
-                                       uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_add_to_whitelist(const uint8_t *addr,
+                                         uint8_t addr_type,
+                                         uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_CHG_WHITE_LIST_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_WHITE_LIST,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_WHITE_LIST,
                        BLE_HCI_CHG_WHITE_LIST_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    rc = host_hci_cmd_body_le_whitelist_chg(addr, addr_type, dst);
+    rc = ble_hs_hci_cmd_body_le_whitelist_chg(addr, addr_type, dst);
     if (rc != 0) {
         return rc;
     }
@@ -677,10 +710,10 @@ host_hci_cmd_build_le_add_to_whitelist(uint8_t *addr, uint8_t addr_type,
 }
 
 void
-host_hci_cmd_build_reset(uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_reset(uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
-    host_hci_write_hdr(BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_RESET,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_RESET,
                        0, dst);
 }
 
@@ -690,21 +723,21 @@ host_hci_cmd_build_reset(uint8_t *dst, int dst_len)
  * @return int
  */
 int
-host_hci_cmd_reset(void)
+ble_hs_hci_cmd_reset(void)
 {
     int rc;
 
-    rc = host_hci_cmd_send(BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_RESET, 0,
-                           NULL);
+    rc = ble_hs_hci_cmd_send(BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_RESET,
+                             0, NULL);
     return rc;
 }
 
 void
-host_hci_cmd_build_read_adv_pwr(uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_read_adv_pwr(uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR,
-                       0, dst);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR,
+                             0, dst);
 }
 
 /**
@@ -713,35 +746,36 @@ host_hci_cmd_build_read_adv_pwr(uint8_t *dst, int dst_len)
  * @return int
  */
 int
-host_hci_cmd_read_adv_pwr(void)
+ble_hs_hci_cmd_read_adv_pwr(void)
 {
     int rc;
 
-    rc = host_hci_cmd_send(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR, 0,
-                           NULL);
+    rc = ble_hs_hci_cmd_send(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR,
+                             0, NULL);
     return rc;
 }
 
 void
-host_hci_cmd_build_le_create_conn_cancel(uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_create_conn_cancel(uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CREATE_CONN_CANCEL,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CREATE_CONN_CANCEL,
                        0, dst);
 }
 
 int
-host_hci_cmd_le_create_conn_cancel(void)
+ble_hs_hci_cmd_le_create_conn_cancel(void)
 {
     int rc;
 
-    rc = host_hci_cmd_send(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CREATE_CONN_CANCEL,
+    rc = ble_hs_hci_cmd_send(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CREATE_CONN_CANCEL,
                            0, NULL);
     return rc;
 }
 
 static int
-host_hci_cmd_body_le_conn_update(struct hci_conn_update *hcu, uint8_t *dst)
+ble_hs_hci_cmd_body_le_conn_update(const struct hci_conn_update *hcu,
+                                   uint8_t *dst)
 {
     /* XXX: add parameter checking later */
     htole16(dst + 0, hcu->handle);
@@ -756,19 +790,19 @@ host_hci_cmd_body_le_conn_update(struct hci_conn_update *hcu, uint8_t *dst)
 }
 
 int
-host_hci_cmd_build_le_conn_update(struct hci_conn_update *hcu, uint8_t *dst,
-                                  int dst_len)
+ble_hs_hci_cmd_build_le_conn_update(const struct hci_conn_update *hcu,
+                                    uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_CONN_UPDATE_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CONN_UPDATE,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CONN_UPDATE,
                        BLE_HCI_CONN_UPDATE_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    rc = host_hci_cmd_body_le_conn_update(hcu, dst);
+    rc = ble_hs_hci_cmd_body_le_conn_update(hcu, dst);
     if (rc != 0) {
         return rc;
     }
@@ -777,17 +811,17 @@ host_hci_cmd_build_le_conn_update(struct hci_conn_update *hcu, uint8_t *dst,
 }
 
 int
-host_hci_cmd_le_conn_update(struct hci_conn_update *hcu)
+ble_hs_hci_cmd_le_conn_update(const struct hci_conn_update *hcu)
 {
     uint8_t cmd[BLE_HCI_CONN_UPDATE_LEN];
     int rc;
 
-    rc = host_hci_cmd_body_le_conn_update(hcu, cmd);
+    rc = ble_hs_hci_cmd_body_le_conn_update(hcu, cmd);
     if (rc != 0) {
         return rc;
     }
 
-    rc = host_hci_le_cmd_send(BLE_HCI_OCF_LE_CONN_UPDATE,
+    rc = ble_hs_hci_cmd_le_send(BLE_HCI_OCF_LE_CONN_UPDATE,
                               BLE_HCI_CONN_UPDATE_LEN, cmd);
     if (rc != 0) {
         return rc;
@@ -797,8 +831,8 @@ host_hci_cmd_le_conn_update(struct hci_conn_update *hcu)
 }
 
 static void
-host_hci_cmd_body_le_lt_key_req_reply(struct hci_lt_key_req_reply *hkr,
-                                      uint8_t *dst)
+ble_hs_hci_cmd_body_le_lt_key_req_reply(const struct hci_lt_key_req_reply *hkr,
+                                        uint8_t *dst)
 {
     htole16(dst + 0, hkr->conn_handle);
     memcpy(dst + 2, hkr->long_term_key, sizeof hkr->long_term_key);
@@ -819,36 +853,37 @@ host_hci_cmd_body_le_lt_key_req_reply(struct hci_lt_key_req_reply *hkr,
  * @return int
  */
 void
-host_hci_cmd_build_le_lt_key_req_reply(struct hci_lt_key_req_reply *hkr,
-                                       uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_lt_key_req_reply(
+    const struct hci_lt_key_req_reply *hkr, uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_LT_KEY_REQ_REPLY_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY,
                        BLE_HCI_LT_KEY_REQ_REPLY_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_le_lt_key_req_reply(hkr, dst);
+    ble_hs_hci_cmd_body_le_lt_key_req_reply(hkr, dst);
 }
 
 void
-host_hci_cmd_build_le_lt_key_req_neg_reply(uint16_t conn_handle,
-                                           uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_lt_key_req_neg_reply(uint16_t conn_handle,
+                                             uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_LT_KEY_REQ_NEG_REPLY_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY,
-                       BLE_HCI_LT_KEY_REQ_NEG_REPLY_LEN, dst);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY,
+                             BLE_HCI_LT_KEY_REQ_NEG_REPLY_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
     htole16(dst + 0, conn_handle);
 }
 
 static void
-host_hci_cmd_body_le_conn_param_reply(struct hci_conn_param_reply *hcr,
-                                      uint8_t *dst)
+ble_hs_hci_cmd_body_le_conn_param_reply(const struct hci_conn_param_reply *hcr,
+                                        uint8_t *dst)
 {
     htole16(dst + 0, hcr->handle);
     htole16(dst + 2, hcr->conn_itvl_min);
@@ -860,21 +895,21 @@ host_hci_cmd_body_le_conn_param_reply(struct hci_conn_param_reply *hcr,
 }
 
 void
-host_hci_cmd_build_le_conn_param_reply(struct hci_conn_param_reply *hcr,
-                                       uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_conn_param_reply(
+    const struct hci_conn_param_reply *hcr, uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_CONN_PARAM_REPLY_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REM_CONN_PARAM_RR,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REM_CONN_PARAM_RR,
                        BLE_HCI_CONN_PARAM_REPLY_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_le_conn_param_reply(hcr, dst);
+    ble_hs_hci_cmd_body_le_conn_param_reply(hcr, dst);
 }
 
 int
-host_hci_cmd_le_conn_param_reply(struct hci_conn_param_reply *hcr)
+ble_hs_hci_cmd_le_conn_param_reply(const struct hci_conn_param_reply *hcr)
 {
     uint8_t cmd[BLE_HCI_CONN_PARAM_REPLY_LEN];
     int rc;
@@ -887,14 +922,14 @@ host_hci_cmd_le_conn_param_reply(struct hci_conn_param_reply *hcr)
     htole16(cmd + 10, hcr->min_ce_len);
     htole16(cmd + 12, hcr->max_ce_len);
 
-    rc = host_hci_le_cmd_send(BLE_HCI_OCF_LE_REM_CONN_PARAM_RR,
+    rc = ble_hs_hci_cmd_le_send(BLE_HCI_OCF_LE_REM_CONN_PARAM_RR,
                               BLE_HCI_CONN_PARAM_REPLY_LEN, cmd);
     return rc;
 }
 
 static void
-host_hci_cmd_body_le_conn_param_neg_reply(struct hci_conn_param_neg_reply *hcn,
-                                          uint8_t *dst)
+ble_hs_hci_cmd_body_le_conn_param_neg_reply(
+    const struct hci_conn_param_neg_reply *hcn, uint8_t *dst)
 {
     htole16(dst + 0, hcn->handle);
     dst[2] = hcn->reason;
@@ -902,28 +937,29 @@ host_hci_cmd_body_le_conn_param_neg_reply(struct hci_conn_param_neg_reply *hcn,
 
 
 void
-host_hci_cmd_build_le_conn_param_neg_reply(
-    struct hci_conn_param_neg_reply *hcn, uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_conn_param_neg_reply(
+    const struct hci_conn_param_neg_reply *hcn, uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_CONN_PARAM_NEG_REPLY_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR,
                        BLE_HCI_CONN_PARAM_NEG_REPLY_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_le_conn_param_neg_reply(hcn, dst);
+    ble_hs_hci_cmd_body_le_conn_param_neg_reply(hcn, dst);
 }
 
 int
-host_hci_cmd_le_conn_param_neg_reply(struct hci_conn_param_neg_reply *hcn)
+ble_hs_hci_cmd_le_conn_param_neg_reply(
+    const struct hci_conn_param_neg_reply *hcn)
 {
     uint8_t cmd[BLE_HCI_CONN_PARAM_NEG_REPLY_LEN];
     int rc;
 
-    host_hci_cmd_body_le_conn_param_neg_reply(hcn, cmd);
+    ble_hs_hci_cmd_body_le_conn_param_neg_reply(hcn, cmd);
 
-    rc = host_hci_le_cmd_send(BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR,
+    rc = ble_hs_hci_cmd_le_send(BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR,
                               BLE_HCI_CONN_PARAM_NEG_REPLY_LEN, cmd);
     return rc;
 }
@@ -937,14 +973,15 @@ host_hci_cmd_le_conn_param_neg_reply(struct hci_conn_param_neg_reply *hcn)
  * @return int
  */
 void
-host_hci_cmd_build_le_rand(uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_rand(uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RAND, 0, dst);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RAND, 0, dst);
 }
 
 static void
-host_hci_cmd_body_le_start_encrypt(struct hci_start_encrypt *cmd, uint8_t *dst)
+ble_hs_hci_cmd_body_le_start_encrypt(const struct hci_start_encrypt *cmd,
+                                     uint8_t *dst)
 {
     htole16(dst + 0, cmd->connection_handle);
     htole64(dst + 2, cmd->random_number);
@@ -956,17 +993,17 @@ host_hci_cmd_body_le_start_encrypt(struct hci_start_encrypt *cmd, uint8_t *dst)
  * OGF=0x08 OCF=0x0019
  */
 void
-host_hci_cmd_build_le_start_encrypt(struct hci_start_encrypt *cmd,
-                                    uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_le_start_encrypt(const struct hci_start_encrypt *cmd,
+                                      uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_LE_START_ENCRYPT_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_START_ENCRYPT,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_START_ENCRYPT,
                        BLE_HCI_LE_START_ENCRYPT_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_le_start_encrypt(cmd, dst);
+    ble_hs_hci_cmd_body_le_start_encrypt(cmd, dst);
 }
 
 /**
@@ -979,27 +1016,28 @@ host_hci_cmd_build_le_start_encrypt(struct hci_start_encrypt *cmd,
  * @return int
  */
 static void
-host_hci_cmd_body_read_rssi(uint16_t handle, uint8_t *dst)
+ble_hs_hci_cmd_body_read_rssi(uint16_t handle, uint8_t *dst)
 {
     htole16(dst, handle);
 }
 
 void
-host_hci_cmd_build_read_rssi(uint16_t handle, uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_read_rssi(uint16_t handle, uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_READ_RSSI_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_STATUS_PARAMS, BLE_HCI_OCF_RD_RSSI,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_STATUS_PARAMS, BLE_HCI_OCF_RD_RSSI,
                        BLE_HCI_READ_RSSI_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    host_hci_cmd_body_read_rssi(handle, dst);
+    ble_hs_hci_cmd_body_read_rssi(handle, dst);
 }
 
 static int
-host_hci_cmd_body_set_data_len(uint16_t connection_handle, uint16_t tx_octets,
-                               uint16_t tx_time, uint8_t *dst)
+ble_hs_hci_cmd_body_set_data_len(uint16_t connection_handle,
+                                 uint16_t tx_octets,
+                                 uint16_t tx_time, uint8_t *dst)
 {
 
     if (tx_octets < BLE_HCI_SET_DATALEN_TX_OCTETS_MIN ||
@@ -1025,21 +1063,21 @@ host_hci_cmd_body_set_data_len(uint16_t connection_handle, uint16_t tx_octets,
  * OGF=0x08 OCF=0x0022
  */
 int
-host_hci_cmd_build_set_data_len(uint16_t connection_handle,
-                                uint16_t tx_octets, uint16_t tx_time,
-                                uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_set_data_len(uint16_t connection_handle,
+                                  uint16_t tx_octets, uint16_t tx_time,
+                                  uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_DATALEN_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_DATA_LEN,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_DATA_LEN,
                        BLE_HCI_SET_DATALEN_LEN, dst);
     dst += BLE_HCI_CMD_HDR_LEN;
 
-    rc = host_hci_cmd_body_set_data_len(connection_handle, tx_octets, tx_time,
-                                        dst);
+    rc = ble_hs_hci_cmd_body_set_data_len(connection_handle, tx_octets,
+                                          tx_time, dst);
     if (rc != 0) {
         return rc;
     }
@@ -1051,9 +1089,10 @@ host_hci_cmd_build_set_data_len(uint16_t connection_handle,
  * IRKs are in little endian.
  */
 static int
-host_hci_cmd_body_add_to_resolv_list(uint8_t addr_type, uint8_t *addr,
-                                     uint8_t *peer_irk, uint8_t *local_irk,
-                                     uint8_t *dst)
+ble_hs_hci_cmd_body_add_to_resolv_list(uint8_t addr_type, const uint8_t *addr,
+                                       const uint8_t *peer_irk,
+                                       const uint8_t *local_irk,
+                                       uint8_t *dst)
 {
     if (addr_type > BLE_ADDR_TYPE_RANDOM) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -1073,8 +1112,8 @@ host_hci_cmd_body_add_to_resolv_list(uint8_t addr_type, uint8_t *addr,
  * IRKs are in little endian.
  */
 int
-host_hci_cmd_build_add_to_resolv_list(
-    struct hci_add_dev_to_resolving_list *padd,
+ble_hs_hci_cmd_build_add_to_resolv_list(
+    const struct hci_add_dev_to_resolving_list *padd,
     uint8_t *dst,
     int dst_len)
 {
@@ -1083,10 +1122,10 @@ host_hci_cmd_build_add_to_resolv_list(
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_ADD_TO_RESOLV_LIST_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_RESOLV_LIST,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_RESOLV_LIST,
                        BLE_HCI_ADD_TO_RESOLV_LIST_LEN, dst);
 
-    rc = host_hci_cmd_body_add_to_resolv_list(
+    rc = ble_hs_hci_cmd_body_add_to_resolv_list(
         padd->addr_type, padd->addr, padd->peer_irk, padd->local_irk,
         dst + BLE_HCI_CMD_HDR_LEN);
     if (rc != 0) {
@@ -1097,8 +1136,9 @@ host_hci_cmd_build_add_to_resolv_list(
 }
 
 static int
-host_hci_cmd_body_remove_from_resolv_list(uint8_t addr_type, uint8_t *addr,
-                                          uint8_t *dst)
+ble_hs_hci_cmd_body_remove_from_resolv_list(uint8_t addr_type,
+                                            const uint8_t *addr,
+                                            uint8_t *dst)
 {
     if (addr_type > BLE_ADDR_TYPE_RANDOM) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -1111,18 +1151,19 @@ host_hci_cmd_body_remove_from_resolv_list(uint8_t addr_type, uint8_t *addr,
 
 
 int
-host_hci_cmd_build_remove_from_resolv_list(uint8_t addr_type, uint8_t *addr,
-                                           uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_remove_from_resolv_list(uint8_t addr_type,
+                                             const uint8_t *addr,
+                                             uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_RMV_FROM_RESOLV_LIST_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RMV_RESOLV_LIST,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RMV_RESOLV_LIST,
                        BLE_HCI_RMV_FROM_RESOLV_LIST_LEN, dst);
 
-    rc = host_hci_cmd_body_remove_from_resolv_list(addr_type, addr,
+    rc = ble_hs_hci_cmd_body_remove_from_resolv_list(addr_type, addr,
                                                 dst + BLE_HCI_CMD_HDR_LEN);
     if (rc != 0) {
         return rc;
@@ -1131,31 +1172,32 @@ host_hci_cmd_build_remove_from_resolv_list(uint8_t addr_type, uint8_t *addr,
 }
 
 int
-host_hci_cmd_build_clear_resolv_list(uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_clear_resolv_list(uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLR_RESOLV_LIST,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLR_RESOLV_LIST,
                        0, dst);
 
     return 0;
 }
 
 int
-host_hci_cmd_build_read_resolv_list_size(uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_read_resolv_list_size(uint8_t *dst, int dst_len)
 {
     BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_CMD_HDR_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_RESOLV_LIST_SIZE,
-                       0, dst);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_RESOLV_LIST_SIZE,
+                             0, dst);
 
     return 0;
 }
 
 static int
-host_hci_cmd_body_read_peer_resolv_addr(uint8_t peer_identity_addr_type,
-                                        uint8_t *peer_identity_addr,
-                                        uint8_t *dst)
+ble_hs_hci_cmd_body_read_peer_resolv_addr(uint8_t peer_identity_addr_type,
+                                          const uint8_t *peer_identity_addr,
+                                          uint8_t *dst)
 {
     if (peer_identity_addr_type > BLE_ADDR_TYPE_RANDOM) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -1167,22 +1209,23 @@ host_hci_cmd_body_read_peer_resolv_addr(uint8_t peer_identity_addr_type,
 }
 
 int
-host_hci_cmd_build_read_peer_resolv_addr(uint8_t peer_identity_addr_type,
-                                         uint8_t *peer_identity_addr,
-                                         uint8_t *dst,
-                                         int dst_len)
+ble_hs_hci_cmd_build_read_peer_resolv_addr(uint8_t peer_identity_addr_type,
+                                           const uint8_t *peer_identity_addr,
+                                           uint8_t *dst,
+                                           int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_RD_PEER_RESOLV_ADDR_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_PEER_RESOLV_ADDR,
-                       BLE_HCI_RD_PEER_RESOLV_ADDR_LEN, dst);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_PEER_RESOLV_ADDR,
+                             BLE_HCI_RD_PEER_RESOLV_ADDR_LEN, dst);
 
-    rc = host_hci_cmd_body_read_peer_resolv_addr(peer_identity_addr_type,
-                                                peer_identity_addr,
-                                                dst + BLE_HCI_CMD_HDR_LEN);
+    rc = ble_hs_hci_cmd_body_read_peer_resolv_addr(peer_identity_addr_type,
+                                                   peer_identity_addr,
+                                                   dst + BLE_HCI_CMD_HDR_LEN);
     if (rc != 0) {
         return rc;
     }
@@ -1190,9 +1233,9 @@ host_hci_cmd_build_read_peer_resolv_addr(uint8_t peer_identity_addr_type,
 }
 
 static int
-host_hci_cmd_body_read_lcl_resolv_addr(
+ble_hs_hci_cmd_body_read_lcl_resolv_addr(
     uint8_t local_identity_addr_type,
-    uint8_t *local_identity_addr,
+    const uint8_t *local_identity_addr,
     uint8_t *dst)
 {
     if (local_identity_addr_type > BLE_ADDR_TYPE_RANDOM) {
@@ -1208,22 +1251,23 @@ host_hci_cmd_body_read_lcl_resolv_addr(
  * OGF=0x08 OCF=0x002c
  */
 int
-host_hci_cmd_build_read_lcl_resolv_addr(uint8_t local_identity_addr_type,
-                                        uint8_t *local_identity_addr,
-                                        uint8_t *dst,
-                                        int dst_len)
+ble_hs_hci_cmd_build_read_lcl_resolv_addr(uint8_t local_identity_addr_type,
+                                          const uint8_t *local_identity_addr,
+                                          uint8_t *dst,
+                                          int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_RD_LOC_RESOLV_ADDR_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_LOCAL_RESOLV_ADDR,
-                       BLE_HCI_RD_LOC_RESOLV_ADDR_LEN, dst);
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE,
+                             BLE_HCI_OCF_LE_RD_LOCAL_RESOLV_ADDR,
+                             BLE_HCI_RD_LOC_RESOLV_ADDR_LEN, dst);
 
-    rc = host_hci_cmd_body_read_lcl_resolv_addr(local_identity_addr_type,
-                                                local_identity_addr,
-                                                dst + BLE_HCI_CMD_HDR_LEN);
+    rc = ble_hs_hci_cmd_body_read_lcl_resolv_addr(local_identity_addr_type,
+                                                  local_identity_addr,
+                                                  dst + BLE_HCI_CMD_HDR_LEN);
     if (rc != 0) {
         return rc;
     }
@@ -1231,7 +1275,7 @@ host_hci_cmd_build_read_lcl_resolv_addr(uint8_t local_identity_addr_type,
 }
 
 static int
-host_hci_cmd_body_set_addr_res_en(uint8_t enable, uint8_t *dst)
+ble_hs_hci_cmd_body_set_addr_res_en(uint8_t enable, uint8_t *dst)
 {
     if (enable > 1) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -1245,17 +1289,18 @@ host_hci_cmd_body_set_addr_res_en(uint8_t enable, uint8_t *dst)
  * OGF=0x08 OCF=0x002d
  */
 int
-host_hci_cmd_build_set_addr_res_en(uint8_t enable, uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_set_addr_res_en(uint8_t enable, uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADDR_RESOL_ENA_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN,
                        BLE_HCI_SET_ADDR_RESOL_ENA_LEN, dst);
 
-    rc = host_hci_cmd_body_set_addr_res_en(enable, dst + BLE_HCI_CMD_HDR_LEN);
+    rc = ble_hs_hci_cmd_body_set_addr_res_en(enable,
+                                             dst + BLE_HCI_CMD_HDR_LEN);
     if (rc != 0) {
         return rc;
     }
@@ -1263,7 +1308,8 @@ host_hci_cmd_build_set_addr_res_en(uint8_t enable, uint8_t *dst, int dst_len)
 }
 
 static int
-host_hci_cmd_body_set_resolv_priv_addr_timeout(uint16_t timeout, uint8_t *dst)
+ble_hs_hci_cmd_body_set_resolv_priv_addr_timeout(uint16_t timeout,
+                                                 uint8_t *dst)
 {
     if (timeout == 0 || timeout > 0xA1B8) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -1277,18 +1323,18 @@ host_hci_cmd_body_set_resolv_priv_addr_timeout(uint16_t timeout, uint8_t *dst)
  * OGF=0x08 OCF=0x002e
  */
 int
-host_hci_cmd_build_set_resolv_priv_addr_timeout(uint16_t timeout, uint8_t *dst,
-                                                int dst_len)
+ble_hs_hci_cmd_build_set_resolv_priv_addr_timeout(
+    uint16_t timeout, uint8_t *dst, int dst_len)
 {
     int rc;
 
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_RESOLV_PRIV_ADDR_TO_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_RPA_TMO,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_RPA_TMO,
                        BLE_HCI_SET_RESOLV_PRIV_ADDR_TO_LEN, dst);
 
-    rc = host_hci_cmd_body_set_resolv_priv_addr_timeout(
+    rc = ble_hs_hci_cmd_body_set_resolv_priv_addr_timeout(
         timeout, dst + BLE_HCI_CMD_HDR_LEN);
     if (rc != 0) {
         return rc;
@@ -1297,14 +1343,16 @@ host_hci_cmd_build_set_resolv_priv_addr_timeout(uint16_t timeout, uint8_t *dst,
 }
 
 static int
-host_hci_cmd_body_set_random_addr(struct hci_rand_addr *paddr, uint8_t *dst)
+ble_hs_hci_cmd_body_set_random_addr(const struct hci_rand_addr *paddr,
+                                    uint8_t *dst)
 {
     memcpy(dst, paddr->addr, BLE_DEV_ADDR_LEN);
     return 0;
 }
 
 int
-host_hci_cmd_build_set_random_addr(uint8_t *addr, uint8_t *dst, int dst_len)
+ble_hs_hci_cmd_build_set_random_addr(const uint8_t *addr,
+                                     uint8_t *dst, int dst_len)
 {
     struct hci_rand_addr r_addr;
     int rc;
@@ -1314,10 +1362,11 @@ host_hci_cmd_build_set_random_addr(uint8_t *addr, uint8_t *dst, int dst_len)
     BLE_HS_DBG_ASSERT(
         dst_len >= BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_RAND_ADDR_LEN);
 
-    host_hci_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_RAND_ADDR,
+    ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_RAND_ADDR,
                        BLE_HCI_SET_RAND_ADDR_LEN, dst);
 
-    rc = host_hci_cmd_body_set_random_addr(&r_addr, dst + BLE_HCI_CMD_HDR_LEN);
+    rc = ble_hs_hci_cmd_body_set_random_addr(&r_addr,
+                                             dst + BLE_HCI_CMD_HDR_LEN);
     if (rc != 0) {
         return rc;
     }
diff --git a/net/nimble/host/src/host_hci.c b/net/nimble/host/src/ble_hs_hci_evt.c
index 4452dcc3..3d382dc1 100644
--- a/net/nimble/host/src/host_hci.c
+++ b/net/nimble/host/src/ble_hs_hci_evt.c
@@ -23,30 +23,29 @@
 #include "os/os.h"
 #include "console/console.h"
 #include "nimble/hci_common.h"
-#include "nimble/hci_transport.h"
-#include "host/host_hci.h"
+#include "nimble/ble_hci_trans.h"
+#include "host/ble_gap.h"
 #include "ble_hs_priv.h"
-#include "host_dbg_priv.h"
+#include "ble_hs_dbg_priv.h"
 
 _Static_assert(sizeof (struct hci_data_hdr) == BLE_HCI_DATA_HDR_SZ,
                "struct hci_data_hdr must be 4 bytes");
 
-typedef int host_hci_event_fn(uint8_t event_code, uint8_t *data, int len);
-static host_hci_event_fn host_hci_rx_disconn_complete;
-static host_hci_event_fn host_hci_rx_encrypt_change;
-static host_hci_event_fn host_hci_rx_num_completed_pkts;
-static host_hci_event_fn host_hci_rx_enc_key_refresh;
-static host_hci_event_fn host_hci_rx_le_meta;
-
-typedef int host_hci_le_event_fn(uint8_t subevent, uint8_t *data, int len);
-static host_hci_le_event_fn host_hci_rx_le_conn_complete;
-static host_hci_le_event_fn host_hci_rx_le_adv_rpt;
-static host_hci_le_event_fn host_hci_rx_le_conn_upd_complete;
-static host_hci_le_event_fn host_hci_rx_le_lt_key_req;
-static host_hci_le_event_fn host_hci_rx_le_conn_parm_req;
-
-static uint16_t host_hci_buffer_sz;
-static uint8_t host_hci_max_pkts;
+typedef int ble_hs_hci_evt_fn(uint8_t event_code, uint8_t *data, int len);
+static ble_hs_hci_evt_fn ble_hs_hci_evt_disconn_complete;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_encrypt_change;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_hw_error;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_num_completed_pkts;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_enc_key_refresh;
+static ble_hs_hci_evt_fn ble_hs_hci_evt_le_meta;
+
+typedef int ble_hs_hci_evt_le_fn(uint8_t subevent, uint8_t *data, int len);
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_conn_complete;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_adv_rpt;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_conn_upd_complete;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_lt_key_req;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_conn_parm_req;
+static ble_hs_hci_evt_le_fn ble_hs_hci_evt_le_dir_adv_rpt;
 
 /* Statistics */
 struct host_hci_stats
@@ -57,71 +56,56 @@ struct host_hci_stats
     uint32_t unknown_events_rxd;
 };
 
-#define HOST_HCI_TIMEOUT        50      /* Milliseconds. */
+#define BLE_HS_HCI_EVT_TIMEOUT        50      /* Milliseconds. */
 
 /** Dispatch table for incoming HCI events.  Sorted by event code field. */
-struct host_hci_event_dispatch_entry {
-    uint8_t hed_event_code;
-    host_hci_event_fn *hed_fn;
+struct ble_hs_hci_evt_dispatch_entry {
+    uint8_t event_code;
+    ble_hs_hci_evt_fn *cb;
 };
 
-static const struct host_hci_event_dispatch_entry host_hci_event_dispatch[] = {
-    { BLE_HCI_EVCODE_DISCONN_CMP, host_hci_rx_disconn_complete },
-    { BLE_HCI_EVCODE_ENCRYPT_CHG, host_hci_rx_encrypt_change },
-    { BLE_HCI_EVCODE_NUM_COMP_PKTS, host_hci_rx_num_completed_pkts },
-    { BLE_HCI_EVCODE_ENC_KEY_REFRESH, host_hci_rx_enc_key_refresh },
-    { BLE_HCI_EVCODE_LE_META, host_hci_rx_le_meta },
+static const struct ble_hs_hci_evt_dispatch_entry ble_hs_hci_evt_dispatch[] = {
+    { BLE_HCI_EVCODE_DISCONN_CMP, ble_hs_hci_evt_disconn_complete },
+    { BLE_HCI_EVCODE_ENCRYPT_CHG, ble_hs_hci_evt_encrypt_change },
+    { BLE_HCI_EVCODE_HW_ERROR, ble_hs_hci_evt_hw_error },
+    { BLE_HCI_EVCODE_NUM_COMP_PKTS, ble_hs_hci_evt_num_completed_pkts },
+    { BLE_HCI_EVCODE_ENC_KEY_REFRESH, ble_hs_hci_evt_enc_key_refresh },
+    { BLE_HCI_EVCODE_LE_META, ble_hs_hci_evt_le_meta },
 };
 
-#define HOST_HCI_EVENT_DISPATCH_SZ \
-    (sizeof host_hci_event_dispatch / sizeof host_hci_event_dispatch[0])
+#define BLE_HS_HCI_EVT_DISPATCH_SZ \
+    (sizeof ble_hs_hci_evt_dispatch / sizeof ble_hs_hci_evt_dispatch[0])
 
 /** Dispatch table for incoming LE meta events.  Sorted by subevent field. */
-struct host_hci_le_event_dispatch_entry {
-    uint8_t hmd_subevent;
-    host_hci_le_event_fn *hmd_fn;
+struct ble_hs_hci_evt_le_dispatch_entry {
+    uint8_t subevent;
+    ble_hs_hci_evt_le_fn *cb;
 };
 
-static const struct host_hci_le_event_dispatch_entry
-        host_hci_le_event_dispatch[] = {
-    { BLE_HCI_LE_SUBEV_CONN_COMPLETE, host_hci_rx_le_conn_complete },
-    { BLE_HCI_LE_SUBEV_ADV_RPT, host_hci_rx_le_adv_rpt },
-    { BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE, host_hci_rx_le_conn_upd_complete },
-    { BLE_HCI_LE_SUBEV_LT_KEY_REQ, host_hci_rx_le_lt_key_req },
-    { BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ, host_hci_rx_le_conn_parm_req },
-    { BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE, host_hci_rx_le_conn_complete },
+static const struct ble_hs_hci_evt_le_dispatch_entry
+        ble_hs_hci_evt_le_dispatch[] = {
+    { BLE_HCI_LE_SUBEV_CONN_COMPLETE, ble_hs_hci_evt_le_conn_complete },
+    { BLE_HCI_LE_SUBEV_ADV_RPT, ble_hs_hci_evt_le_adv_rpt },
+    { BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE,
+          ble_hs_hci_evt_le_conn_upd_complete },
+    { BLE_HCI_LE_SUBEV_LT_KEY_REQ, ble_hs_hci_evt_le_lt_key_req },
+    { BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ, ble_hs_hci_evt_le_conn_parm_req },
+    { BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE, ble_hs_hci_evt_le_conn_complete },
+    { BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT, ble_hs_hci_evt_le_dir_adv_rpt },
 };
 
-#define HOST_HCI_LE_EVENT_DISPATCH_SZ \
-    (sizeof host_hci_le_event_dispatch / sizeof host_hci_le_event_dispatch[0])
-
-uint16_t
-host_hci_opcode_join(uint8_t ogf, uint16_t ocf)
-{
-    return (ogf << 10) | ocf;
-}
-
-uint16_t
-host_hci_handle_pb_bc_join(uint16_t handle, uint8_t pb, uint8_t bc)
-{
-    BLE_HS_DBG_ASSERT(handle <= 0x0fff);
-    BLE_HS_DBG_ASSERT(pb <= 0x03);
-    BLE_HS_DBG_ASSERT(bc <= 0x03);
-
-    return (handle  << 0)   |
-           (pb      << 12)  |
-           (bc      << 14);
-}
+#define BLE_HS_HCI_EVT_LE_DISPATCH_SZ \
+    (sizeof ble_hs_hci_evt_le_dispatch / sizeof ble_hs_hci_evt_le_dispatch[0])
 
-static const struct host_hci_event_dispatch_entry *
-host_hci_dispatch_entry_find(uint8_t event_code)
+static const struct ble_hs_hci_evt_dispatch_entry *
+ble_hs_hci_evt_dispatch_find(uint8_t event_code)
 {
-    const struct host_hci_event_dispatch_entry *entry;
+    const struct ble_hs_hci_evt_dispatch_entry *entry;
     int i;
 
-    for (i = 0; i < HOST_HCI_EVENT_DISPATCH_SZ; i++) {
-        entry = host_hci_event_dispatch + i;
-        if (entry->hed_event_code == event_code) {
+    for (i = 0; i < BLE_HS_HCI_EVT_DISPATCH_SZ; i++) {
+        entry = ble_hs_hci_evt_dispatch + i;
+        if (entry->event_code == event_code) {
             return entry;
         }
     }
@@ -129,15 +113,15 @@ host_hci_dispatch_entry_find(uint8_t event_code)
     return NULL;
 }
 
-static const struct host_hci_le_event_dispatch_entry *
-host_hci_le_dispatch_entry_find(uint8_t event_code)
+static const struct ble_hs_hci_evt_le_dispatch_entry *
+ble_hs_hci_evt_le_dispatch_find(uint8_t event_code)
 {
-    const struct host_hci_le_event_dispatch_entry *entry;
+    const struct ble_hs_hci_evt_le_dispatch_entry *entry;
     int i;
 
-    for (i = 0; i < HOST_HCI_LE_EVENT_DISPATCH_SZ; i++) {
-        entry = host_hci_le_event_dispatch + i;
-        if (entry->hmd_subevent == event_code) {
+    for (i = 0; i < BLE_HS_HCI_EVT_LE_DISPATCH_SZ; i++) {
+        entry = ble_hs_hci_evt_le_dispatch + i;
+        if (entry->subevent == event_code) {
             return entry;
         }
     }
@@ -146,7 +130,7 @@ host_hci_le_dispatch_entry_find(uint8_t event_code)
 }
 
 static int
-host_hci_rx_disconn_complete(uint8_t event_code, uint8_t *data, int len)
+ble_hs_hci_evt_disconn_complete(uint8_t event_code, uint8_t *data, int len)
 {
     struct hci_disconn_complete evt;
 
@@ -164,7 +148,7 @@ host_hci_rx_disconn_complete(uint8_t event_code, uint8_t *data, int len)
 }
 
 static int
-host_hci_rx_encrypt_change(uint8_t event_code, uint8_t *data, int len)
+ble_hs_hci_evt_encrypt_change(uint8_t event_code, uint8_t *data, int len)
 {
     struct hci_encrypt_change evt;
 
@@ -182,7 +166,22 @@ host_hci_rx_encrypt_change(uint8_t event_code, uint8_t *data, int len)
 }
 
 static int
-host_hci_rx_enc_key_refresh(uint8_t event_code, uint8_t *data, int len)
+ble_hs_hci_evt_hw_error(uint8_t event_code, uint8_t *data, int len)
+{
+    uint8_t hw_code;
+
+    if (len < BLE_HCI_EVENT_HW_ERROR_LEN) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    hw_code = data[0];
+    ble_hs_hw_error(hw_code);
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_enc_key_refresh(uint8_t event_code, uint8_t *data, int len)
 {
     struct hci_encrypt_key_refresh evt;
 
@@ -199,7 +198,7 @@ host_hci_rx_enc_key_refresh(uint8_t event_code, uint8_t *data, int len)
 }
 
 static int
-host_hci_rx_num_completed_pkts(uint8_t event_code, uint8_t *data, int len)
+ble_hs_hci_evt_num_completed_pkts(uint8_t event_code, uint8_t *data, int len)
 {
     uint16_t num_pkts;
     uint16_t handle;
@@ -232,21 +231,20 @@ host_hci_rx_num_completed_pkts(uint8_t event_code, uint8_t *data, int len)
 }
 
 static int
-host_hci_rx_le_meta(uint8_t event_code, uint8_t *data, int len)
+ble_hs_hci_evt_le_meta(uint8_t event_code, uint8_t *data, int len)
 {
-    const struct host_hci_le_event_dispatch_entry *entry;
+    const struct ble_hs_hci_evt_le_dispatch_entry *entry;
     uint8_t subevent;
     int rc;
 
     if (len < BLE_HCI_EVENT_HDR_LEN + BLE_HCI_LE_MIN_LEN) {
-        /* XXX: Increment stat. */
         return BLE_HS_ECONTROLLER;
     }
 
     subevent = data[2];
-    entry = host_hci_le_dispatch_entry_find(subevent);
+    entry = ble_hs_hci_evt_le_dispatch_find(subevent);
     if (entry != NULL) {
-        rc = entry->hmd_fn(subevent, data + BLE_HCI_EVENT_HDR_LEN,
+        rc = entry->cb(subevent, data + BLE_HCI_EVENT_HDR_LEN,
                            len - BLE_HCI_EVENT_HDR_LEN);
         if (rc != 0) {
             return rc;
@@ -257,7 +255,7 @@ host_hci_rx_le_meta(uint8_t event_code, uint8_t *data, int len)
 }
 
 static int
-host_hci_rx_le_conn_complete(uint8_t subevent, uint8_t *data, int len)
+ble_hs_hci_evt_le_conn_complete(uint8_t subevent, uint8_t *data, int len)
 {
     struct hci_le_conn_complete evt;
     int extended_offset = 0;
@@ -313,8 +311,9 @@ host_hci_rx_le_conn_complete(uint8_t subevent, uint8_t *data, int len)
 }
 
 static int
-host_hci_le_adv_rpt_first_pass(uint8_t *data, int len,
-                               uint8_t *out_num_reports, int *out_rssi_off)
+ble_hs_hci_evt_le_adv_rpt_first_pass(uint8_t *data, int len,
+                                     uint8_t *out_num_reports,
+                                     int *out_rssi_off)
 {
     uint8_t num_reports;
     int data_len;
@@ -361,50 +360,117 @@ host_hci_le_adv_rpt_first_pass(uint8_t *data, int len,
 }
 
 static int
-host_hci_rx_le_adv_rpt(uint8_t subevent, uint8_t *data, int len)
+ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, uint8_t *data, int len)
 {
-    struct ble_hs_adv adv;
+    struct ble_gap_disc_desc desc;
     uint8_t num_reports;
     int rssi_off;
     int data_off;
+    int suboff;
     int off;
     int rc;
     int i;
 
-    rc = host_hci_le_adv_rpt_first_pass(data, len, &num_reports, &rssi_off);
+    rc = ble_hs_hci_evt_le_adv_rpt_first_pass(data, len, &num_reports,
+                                             &rssi_off);
     if (rc != 0) {
         return rc;
     }
 
+    /* Direct address fields not present in a standard advertising report. */
+    desc.direct_addr_type = BLE_GAP_ADDR_TYPE_NONE;
+    memset(desc.direct_addr, 0, sizeof desc.direct_addr);
+
     data_off = 0;
     for (i = 0; i < num_reports; i++) {
-        off = 2 + 0 * num_reports + i;
-        adv.event_type = data[2 + 0 * num_reports + i];
+        suboff = 0;
+
+        off = 2 + suboff * num_reports + i;
+        desc.event_type = data[off];
+        suboff++;
 
-        off = 2 + 1 * num_reports + i;
-        adv.addr_type = data[2 + 1 * num_reports + i];
+        off = 2 + suboff * num_reports + i;
+        desc.addr_type = data[off];
+        suboff++;
 
-        off = 2 + 2 * num_reports + i * 6;
-        memcpy(adv.addr, data + off, 6);
+        off = 2 + suboff * num_reports + i * 6;
+        memcpy(desc.addr, data + off, 6);
+        suboff += 6;
 
-        off = 2 + 8 * num_reports + i;
-        adv.length_data = data[off];
+        off = 2 + suboff * num_reports + i;
+        desc.length_data = data[off];
+        suboff++;
 
-        off = 2 + 9 * num_reports + data_off;
-        adv.data = data + off;
-        data_off += adv.length_data;
+        off = 2 + suboff * num_reports + data_off;
+        desc.data = data + off;
+        data_off += desc.length_data;
 
         off = rssi_off + 1 * i;
-        adv.rssi = data[off];
+        desc.rssi = data[off];
 
-        ble_gap_rx_adv_report(&adv);
+        ble_gap_rx_adv_report(&desc);
     }
 
     return 0;
 }
 
 static int
-host_hci_rx_le_conn_upd_complete(uint8_t subevent, uint8_t *data, int len)
+ble_hs_hci_evt_le_dir_adv_rpt(uint8_t subevent, uint8_t *data, int len)
+{
+    struct ble_gap_disc_desc desc;
+    uint8_t num_reports;
+    int suboff;
+    int off;
+    int i;
+
+    if (len < BLE_HCI_LE_ADV_DIRECT_RPT_LEN) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    num_reports = data[1];
+    if (len != 2 + num_reports * BLE_HCI_LE_ADV_DIRECT_RPT_SUB_LEN) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    /* Data fields not present in a direct advertising report. */
+    desc.data = NULL;
+    desc.fields = NULL;
+
+    for (i = 0; i < num_reports; i++) {
+        suboff = 0;
+
+        off = 2 + suboff * num_reports + i;
+        desc.event_type = data[off];
+        suboff++;
+
+        off = 2 + suboff * num_reports + i;
+        desc.addr_type = data[off];
+        suboff++;
+
+        off = 2 + suboff * num_reports + i * 6;
+        memcpy(desc.addr, data + off, 6);
+        suboff += 6;
+
+        off = 2 + suboff * num_reports + i;
+        desc.direct_addr_type = data[off];
+        suboff++;
+
+        off = 2 + suboff * num_reports + i * 6;
+        memcpy(desc.direct_addr, data + off, 6);
+        suboff += 6;
+
+        off = 2 + suboff * num_reports + i;
+        desc.rssi = data[off];
+        suboff++;
+
+        ble_gap_rx_adv_report(&desc);
+    }
+
+    return 0;
+}
+
+static int
+ble_hs_hci_evt_le_conn_upd_complete(uint8_t subevent, uint8_t *data, int len)
 {
     struct hci_le_conn_upd_complete evt;
 
@@ -443,7 +509,7 @@ host_hci_rx_le_conn_upd_complete(uint8_t subevent, uint8_t *data, int len)
 }
 
 static int
-host_hci_rx_le_lt_key_req(uint8_t subevent, uint8_t *data, int len)
+ble_hs_hci_evt_le_lt_key_req(uint8_t subevent, uint8_t *data, int len)
 {
     struct hci_le_lt_key_req evt;
 
@@ -462,7 +528,7 @@ host_hci_rx_le_lt_key_req(uint8_t subevent, uint8_t *data, int len)
 }
 
 static int
-host_hci_rx_le_conn_parm_req(uint8_t subevent, uint8_t *data, int len)
+ble_hs_hci_evt_le_conn_parm_req(uint8_t subevent, uint8_t *data, int len)
 {
     struct hci_le_conn_param_req evt;
 
@@ -500,22 +566,9 @@ host_hci_rx_le_conn_parm_req(uint8_t subevent, uint8_t *data, int len)
 }
 
 int
-host_hci_set_buf_size(uint16_t pktlen, uint8_t max_pkts)
-{
-    if (pktlen == 0 || max_pkts == 0) {
-        return BLE_HS_EINVAL;
-    }
-
-    host_hci_buffer_sz = pktlen;
-    host_hci_max_pkts = max_pkts;
-
-    return 0;
-}
-
-int
-host_hci_event_rx(uint8_t *data)
+ble_hs_hci_evt_process(uint8_t *data)
 {
-    const struct host_hci_event_dispatch_entry *entry;
+    const struct ble_hs_hci_evt_dispatch_entry *entry;
     uint8_t event_code;
     uint8_t param_len;
     int event_len;
@@ -525,7 +578,7 @@ host_hci_event_rx(uint8_t *data)
     STATS_INC(ble_hs_stats, hci_event);
 
     /* Display to console */
-    host_hci_dbg_event_disp(data);
+    ble_hs_dbg_event_disp(data);
 
     /* Process the event */
     event_code = data[0];
@@ -533,100 +586,19 @@ host_hci_event_rx(uint8_t *data)
 
     event_len = param_len + 2;
 
-    entry = host_hci_dispatch_entry_find(event_code);
+    entry = ble_hs_hci_evt_dispatch_find(event_code);
     if (entry == NULL) {
-        STATS_INC(ble_hs_stats, hci_invalid_ack);
+        STATS_INC(ble_hs_stats, hci_unknown_event);
         rc = BLE_HS_ENOTSUP;
     } else {
-        rc = entry->hed_fn(event_code, data, event_len);
+        rc = entry->cb(event_code, data, event_len);
     }
 
-    return rc;
-}
-
-int
-host_hci_os_event_proc(struct os_event *ev)
-{
-    os_error_t err;
-    int rc;
-
-    rc = host_hci_event_rx(ev->ev_arg);
-
-    /* Free the command buffer */
-    err = os_memblock_put(&g_hci_cmd_pool, ev->ev_arg);
-    BLE_HS_DBG_ASSERT_EVAL(err == OS_OK);
-
-    /* Free the event */
-    err = os_memblock_put(&g_hci_os_event_pool, ev);
-    BLE_HS_DBG_ASSERT_EVAL(err == OS_OK);
+    ble_hci_trans_buf_free(data);
 
     return rc;
 }
 
-/* XXX: For now, put this here */
-int
-ble_hci_transport_ctlr_event_send(uint8_t *hci_ev)
-{
-    struct os_event *ev;
-    os_error_t err;
-    int enqueue;
-
-    BLE_HS_DBG_ASSERT(hci_ev != NULL);
-
-    switch (hci_ev[0]) {
-    case BLE_HCI_EVCODE_COMMAND_COMPLETE:
-    case BLE_HCI_EVCODE_COMMAND_STATUS:
-        if (hci_ev[3] == 0 && hci_ev[4] == 0) {
-            enqueue = 1;
-        } else {
-            ble_hci_cmd_rx_ack(hci_ev);
-            enqueue = 0;
-        }
-        break;
-
-    default:
-        enqueue = 1;
-        break;
-    }
-
-    if (enqueue) {
-        /* Get an event structure off the queue */
-        ev = (struct os_event *)os_memblock_get(&g_hci_os_event_pool);
-        if (!ev) {
-            err = os_memblock_put(&g_hci_cmd_pool, hci_ev);
-            BLE_HS_DBG_ASSERT_EVAL(err == OS_OK);
-            return -1;
-        }
-
-        /* Fill out the event and post to host task. */
-        ev->ev_queued = 0;
-        ev->ev_type = BLE_HOST_HCI_EVENT_CTLR_EVENT;
-        ev->ev_arg = hci_ev;
-        ble_hs_event_enqueue(ev);
-    }
-
-    return 0;
-}
-
-static int
-host_hci_data_hdr_strip(struct os_mbuf *om, struct hci_data_hdr *hdr)
-{
-    int rc;
-
-    rc = os_mbuf_copydata(om, 0, BLE_HCI_DATA_HDR_SZ, hdr);
-    if (rc != 0) {
-        return BLE_HS_ECONTROLLER;
-    }
-
-    /* Strip HCI ACL data header from the front of the packet. */
-    os_mbuf_adj(om, BLE_HCI_DATA_HDR_SZ);
-
-    hdr->hdh_handle_pb_bc = le16toh(&hdr->hdh_handle_pb_bc);
-    hdr->hdh_len = le16toh(&hdr->hdh_len);
-
-    return 0;
-}
-
 /**
  * Called when a data packet is received from the controller.  This function
  * consumes the supplied mbuf, regardless of the outcome.
@@ -637,7 +609,7 @@ host_hci_data_hdr_strip(struct os_mbuf *om, struct hci_data_hdr *hdr)
  * @return                      0 on success; nonzero on failure.
  */
 int
-host_hci_data_rx(struct os_mbuf *om)
+ble_hs_hci_evt_acl_process(struct os_mbuf *om)
 {
     struct hci_data_hdr hci_hdr;
     struct ble_hs_conn *conn;
@@ -646,100 +618,60 @@ host_hci_data_rx(struct os_mbuf *om)
     uint16_t handle;
     int rc;
 
-    rc = host_hci_data_hdr_strip(om, &hci_hdr);
-    if (rc == 0) {
+    rc = ble_hs_hci_util_data_hdr_strip(om, &hci_hdr);
+    if (rc != 0) {
+        goto err;
+    }
+
 #if (BLETEST_THROUGHPUT_TEST == 0)
-        BLE_HS_LOG(DEBUG, "host_hci_data_rx(): handle=%u pb=%x len=%u data=",
-                   BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc), 
-                   BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc), 
-                   hci_hdr.hdh_len);
-        ble_hs_misc_log_mbuf(om);
-        BLE_HS_LOG(DEBUG, "\n");
+    BLE_HS_LOG(DEBUG, "ble_hs_hci_evt_acl_process(): handle=%u pb=%x len=%u "
+                      "data=",
+               BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc), 
+               BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc), 
+               hci_hdr.hdh_len);
+    ble_hs_log_mbuf(om);
+    BLE_HS_LOG(DEBUG, "\n");
 #endif
 
-        if (hci_hdr.hdh_len != OS_MBUF_PKTHDR(om)->omp_len) {
-            rc = BLE_HS_EBADDATA;
-        } else {
-            handle = BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc);
+    if (hci_hdr.hdh_len != OS_MBUF_PKTHDR(om)->omp_len) {
+        rc = BLE_HS_EBADDATA;
+        goto err;
+    }
 
-            ble_hs_lock();
+    handle = BLE_HCI_DATA_HANDLE(hci_hdr.hdh_handle_pb_bc);
 
-            conn = ble_hs_conn_find(handle);
-            if (conn == NULL) {
-                rc = BLE_HS_ENOTCONN;
-            } else {
-                rc = ble_l2cap_rx(conn, &hci_hdr, om, &rx_cb, &rx_buf);
-                om = NULL;
-            }
+    ble_hs_lock();
 
-            ble_hs_unlock();
-        }
+    conn = ble_hs_conn_find(handle);
+    if (conn == NULL) {
+        rc = BLE_HS_ENOTCONN;
+    } else {
+        rc = ble_l2cap_rx(conn, &hci_hdr, om, &rx_cb, &rx_buf);
+        om = NULL;
     }
 
-    os_mbuf_free_chain(om);
+    ble_hs_unlock();
 
-    if (rc == 0) {
+    switch (rc) {
+    case 0:
+        /* Final fragment received. */
         BLE_HS_DBG_ASSERT(rx_cb != NULL);
         BLE_HS_DBG_ASSERT(rx_buf != NULL);
         rc = rx_cb(handle, &rx_buf);
         os_mbuf_free_chain(rx_buf);
-    } else if (rc == BLE_HS_EAGAIN) {
-        /* More fragments on the way. */
-        rc = 0;
-    }
-
-    return rc;
-}
-
-static struct os_mbuf *
-host_hci_data_hdr_prepend(struct os_mbuf *om, uint16_t handle, uint8_t pb_flag)
-{
-    struct hci_data_hdr hci_hdr;
-
-    hci_hdr.hdh_handle_pb_bc = host_hci_handle_pb_bc_join(handle, pb_flag, 0);
-    htole16(&hci_hdr.hdh_len, OS_MBUF_PKTHDR(om)->omp_len);
-
-    om = os_mbuf_prepend(om, sizeof hci_hdr);
-    if (om == NULL) {
-        return NULL;
-    }
-
-    memcpy(om->om_data, &hci_hdr, sizeof hci_hdr);
-
-    BLE_HS_LOG(DEBUG, "host tx hci data; handle=%d length=%d\n", handle,
-               le16toh(&hci_hdr.hdh_len));
-
-    return om;
-}
-
-/**
- * Transmits an HCI ACL data packet.  This function consumes the supplied mbuf,
- * regardless of the outcome.
- */
-int
-host_hci_data_tx(struct ble_hs_conn *connection, struct os_mbuf *om)
-{
-    int rc;
-
-    /* XXX: Different transport mechanisms have different fragmentation
-     * requirements.  For now, never fragment.
-     */
-    om = host_hci_data_hdr_prepend(om, connection->bhc_handle,
-                                   BLE_HCI_PB_FIRST_NON_FLUSH);
-    if (om == NULL) {
-        return BLE_HS_ENOMEM;
-    }
+        break;
 
-    BLE_HS_LOG(DEBUG, "host_hci_data_tx(): ");
-    ble_hs_misc_log_mbuf(om);
-    BLE_HS_LOG(DEBUG, "\n");
+    case BLE_HS_EAGAIN:
+        /* More fragments on the way. */
+        break;
 
-    rc = ble_hs_tx_data(om);
-    if (rc != 0) {
-        return rc;
+    default:
+        goto err;
     }
 
-    connection->bhc_outstanding_pkts++;
-
     return 0;
+
+err:
+    os_mbuf_free_chain(om);
+    return rc;
 }
diff --git a/net/nimble/host/src/ble_hs_hci_priv.h b/net/nimble/host/src/ble_hs_hci_priv.h
new file mode 100644
index 00000000..7d2fb120
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_hci_priv.h
@@ -0,0 +1,158 @@
+/**
+ * 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 H_BLE_HS_HCI_PRIV_
+#define H_BLE_HS_HCI_PRIV_
+
+#include "nimble/hci_common.h"
+struct ble_hs_conn;
+struct os_mbuf;
+
+struct ble_hs_hci_ack {
+    int bha_status;         /* A BLE_HS_E<...> error; NOT a naked HCI code. */
+    uint8_t *bha_params;
+    int bha_params_len;
+    uint16_t bha_opcode;
+    uint8_t bha_hci_handle;
+};
+
+int ble_hs_hci_cmd_tx(void *cmd, void *evt_buf, uint8_t evt_buf_len,
+                      uint8_t *out_evt_buf_len);
+int ble_hs_hci_cmd_tx_empty_ack(void *cmd);
+void ble_hs_hci_rx_ack(uint8_t *ack_ev);
+void ble_hs_hci_init(void);
+
+#if PHONY_HCI_ACKS
+typedef int ble_hs_hci_phony_ack_fn(uint8_t *ack, int ack_buf_len);
+void ble_hs_hci_set_phony_ack_cb(ble_hs_hci_phony_ack_fn *cb);
+#endif
+
+int ble_hs_hci_util_read_adv_tx_pwr(int8_t *out_pwr);
+int ble_hs_hci_util_rand(void *dst, int len);
+int ble_hs_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi);
+int ble_hs_hci_util_set_random_addr(const uint8_t *addr);
+int ble_hs_hci_util_set_data_len(uint16_t conn_handle, uint16_t tx_octets,
+                                 uint16_t tx_time);
+int ble_hs_hci_util_data_hdr_strip(struct os_mbuf *om,
+                                   struct hci_data_hdr *out_hdr);
+
+int ble_hs_hci_evt_process(uint8_t *data);
+uint16_t ble_hs_hci_util_opcode_join(uint8_t ogf, uint16_t ocf);
+void ble_hs_hci_cmd_write_hdr(uint8_t ogf, uint8_t ocf, uint8_t len,
+                              void *buf);
+int ble_hs_hci_cmd_send(uint8_t ogf, uint8_t ocf, uint8_t len,
+                        const void *cmddata);
+int ble_hs_hci_cmd_send_buf(void *cmddata);
+void ble_hs_hci_cmd_build_read_bd_addr(uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_set_event_mask(uint64_t event_mask,
+                                         uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_set_event_mask2(uint64_t event_mask, uint8_t *dst,
+                                          int dst_len);
+void ble_hs_hci_cmd_build_disconnect(uint16_t handle, uint8_t reason,
+                                     uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_disconnect(uint16_t handle, uint8_t reason);
+void ble_hs_hci_cmd_build_read_rssi(uint16_t handle, uint8_t *dst,
+                                    int dst_len);
+int ble_hs_hci_cmd_read_rssi(uint16_t handle);
+int ble_hs_hci_cmd_build_le_set_scan_rsp_data(const uint8_t *data, uint8_t len,
+                                              uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_le_set_adv_data(const uint8_t *data, uint8_t len,
+                                         uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_le_set_adv_params(const struct hci_adv_params *adv,
+                                           uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_le_set_event_mask(uint64_t event_mask,
+                                            uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_le_read_buffer_size(uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_le_read_buffer_size(void);
+void ble_hs_hci_cmd_build_le_read_loc_supp_feat(uint8_t *dst, uint8_t dst_len);
+void ble_hs_hci_cmd_build_le_set_adv_enable(uint8_t enable, uint8_t *dst,
+                                            int dst_len);
+int ble_hs_hci_cmd_le_set_adv_enable(uint8_t enable);
+int ble_hs_hci_cmd_build_le_set_scan_params(uint8_t scan_type,
+                                            uint16_t scan_itvl,
+                                            uint16_t scan_window,
+                                            uint8_t own_addr_type,
+                                            uint8_t filter_policy,
+                                            uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_le_set_scan_enable(uint8_t enable,
+                                             uint8_t filter_dups,
+                                             uint8_t *dst, uint8_t dst_len);
+int ble_hs_hci_cmd_le_set_scan_enable(uint8_t enable, uint8_t filter_dups);
+int ble_hs_hci_cmd_build_le_create_connection(
+    const struct hci_create_conn *hcc, uint8_t *cmd, int cmd_len);
+int ble_hs_hci_cmd_le_create_connection(const struct hci_create_conn *hcc);
+void ble_hs_hci_cmd_build_le_clear_whitelist(uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_le_add_to_whitelist(const uint8_t *addr,
+                                             uint8_t addr_type,
+                                             uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_reset(uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_reset(void);
+void ble_hs_hci_cmd_build_read_adv_pwr(uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_read_adv_pwr(void);
+void ble_hs_hci_cmd_build_le_create_conn_cancel(uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_le_create_conn_cancel(void);
+int ble_hs_hci_cmd_build_le_conn_update(const struct hci_conn_update *hcu,
+                                        uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_le_conn_update(const struct hci_conn_update *hcu);
+void ble_hs_hci_cmd_build_le_lt_key_req_reply(
+    const struct hci_lt_key_req_reply *hkr, uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_le_lt_key_req_neg_reply(uint16_t conn_handle,
+                                                  uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_le_conn_param_reply(
+    const struct hci_conn_param_reply *hcr, uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_le_conn_param_reply(const struct hci_conn_param_reply *hcr);
+void ble_hs_hci_cmd_build_le_conn_param_neg_reply(
+    const struct hci_conn_param_neg_reply *hcn, uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_le_conn_param_neg_reply(
+    const struct hci_conn_param_neg_reply *hcn);
+void ble_hs_hci_cmd_build_le_rand(uint8_t *dst, int dst_len);
+void ble_hs_hci_cmd_build_le_start_encrypt(const struct hci_start_encrypt *cmd,
+                                           uint8_t *dst, int dst_len);
+int ble_hs_hci_set_buf_sz(uint16_t pktlen, uint8_t max_pkts);
+
+uint16_t ble_hs_hci_util_handle_pb_bc_join(uint16_t handle, uint8_t pb,
+                                           uint8_t bc);
+
+int ble_hs_hci_acl_tx(struct ble_hs_conn *connection, struct os_mbuf *txom);
+
+int ble_hs_hci_cmd_build_set_data_len(uint16_t connection_handle,
+                                      uint16_t tx_octets, uint16_t tx_time,
+                                      uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_add_to_resolv_list(
+    const struct hci_add_dev_to_resolving_list *padd,
+    uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_remove_from_resolv_list(
+    uint8_t addr_type, const uint8_t *addr, uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_read_resolv_list_size(uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_clear_resolv_list(uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_read_peer_resolv_addr(
+    uint8_t peer_identity_addr_type, const uint8_t *peer_identity_addr,
+    uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_read_lcl_resolv_addr(
+    uint8_t local_identity_addr_type, const uint8_t *local_identity_addr,
+    uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_set_addr_res_en(
+    uint8_t enable, uint8_t *dst, int dst_len);
+int ble_hs_hci_cmd_build_set_resolv_priv_addr_timeout(
+    uint16_t timeout, uint8_t *dst, int dst_len);
+
+int ble_hs_hci_cmd_build_set_random_addr(const uint8_t *addr,
+                                         uint8_t *dst, int dst_len);
+
+#endif
diff --git a/net/nimble/host/src/ble_hci_util.c b/net/nimble/host/src/ble_hs_hci_util.c
index f9ed2c52..0d9f625e 100644
--- a/net/nimble/host/src/ble_hci_util.c
+++ b/net/nimble/host/src/ble_hs_hci_util.c
@@ -19,18 +19,35 @@
 
 #include <string.h>
 #include "nimble/hci_common.h"
-#include "host/host_hci.h"
 #include "ble_hs_priv.h"
 
+uint16_t
+ble_hs_hci_util_opcode_join(uint8_t ogf, uint16_t ocf)
+{
+    return (ogf << 10) | ocf;
+}
+
+uint16_t
+ble_hs_hci_util_handle_pb_bc_join(uint16_t handle, uint8_t pb, uint8_t bc)
+{
+    BLE_HS_DBG_ASSERT(handle <= 0x0fff);
+    BLE_HS_DBG_ASSERT(pb <= 0x03);
+    BLE_HS_DBG_ASSERT(bc <= 0x03);
+
+    return (handle  << 0)   |
+           (pb      << 12)  |
+           (bc      << 14);
+}
+
 int
-ble_hci_util_read_adv_tx_pwr(int8_t *out_tx_pwr)
+ble_hs_hci_util_read_adv_tx_pwr(int8_t *out_tx_pwr)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN];
     uint8_t params_len;
     int rc;
 
-    host_hci_cmd_build_read_adv_pwr(buf, sizeof buf);
-    rc = ble_hci_cmd_tx(buf, out_tx_pwr, 1, &params_len);
+    ble_hs_hci_cmd_build_read_adv_pwr(buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx(buf, out_tx_pwr, 1, &params_len);
     if (rc != 0) {
         return rc;
     }
@@ -46,7 +63,7 @@ ble_hci_util_read_adv_tx_pwr(int8_t *out_tx_pwr)
 }
 
 int
-ble_hci_util_rand(void *dst, int len)
+ble_hs_hci_util_rand(void *dst, int len)
 {
     uint8_t rsp_buf[BLE_HCI_LE_RAND_LEN];
     uint8_t req_buf[BLE_HCI_CMD_HDR_LEN];
@@ -55,11 +72,11 @@ ble_hci_util_rand(void *dst, int len)
     int chunk_sz;
     int rc;
 
-    host_hci_cmd_build_le_rand(req_buf, sizeof req_buf);
+    ble_hs_hci_cmd_build_le_rand(req_buf, sizeof req_buf);
 
     u8ptr = dst;
     while (len > 0) {
-        rc = ble_hci_cmd_tx(req_buf, rsp_buf, sizeof rsp_buf, &params_len);
+        rc = ble_hs_hci_cmd_tx(req_buf, rsp_buf, sizeof rsp_buf, &params_len);
         if (rc != 0) {
             return rc;
         }
@@ -78,7 +95,7 @@ ble_hci_util_rand(void *dst, int len)
 }
 
 int
-ble_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi)
+ble_hs_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_READ_RSSI_LEN];
     uint8_t params[BLE_HCI_READ_RSSI_ACK_PARAM_LEN];
@@ -86,8 +103,8 @@ ble_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi)
     uint8_t params_len;
     int rc;
 
-    host_hci_cmd_build_read_rssi(conn_handle, buf, sizeof buf);
-    rc = ble_hci_cmd_tx(buf, params, sizeof params, &params_len);
+    ble_hs_hci_cmd_build_read_rssi(conn_handle, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx(buf, params, sizeof params, &params_len);
     if (rc != 0) {
         return rc;
     }
@@ -107,25 +124,25 @@ ble_hci_util_read_rssi(uint16_t conn_handle, int8_t *out_rssi)
 }
 
 int
-ble_hs_util_set_random_addr(uint8_t *addr)
+ble_hs_hci_util_set_random_addr(const uint8_t *addr)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_RAND_ADDR_LEN];
     int rc;
 
     /* set the address in the controller */
 
-    rc = host_hci_cmd_build_set_random_addr(addr, buf, sizeof(buf));
+    rc = ble_hs_hci_cmd_build_set_random_addr(addr, buf, sizeof(buf));
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     return rc;
 }
 
 int
-ble_hci_util_set_data_len(uint16_t conn_handle, uint16_t tx_octets,
-                          uint16_t tx_time)
+ble_hs_hci_util_set_data_len(uint16_t conn_handle, uint16_t tx_octets,
+                             uint16_t tx_time)
 {
 
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_DATALEN_LEN];
@@ -134,14 +151,14 @@ ble_hci_util_set_data_len(uint16_t conn_handle, uint16_t tx_octets,
     uint8_t params_len;
     int rc;
 
-    rc = host_hci_cmd_build_set_data_len(conn_handle, tx_octets, tx_time, buf,
-                                         sizeof buf);
+    rc = ble_hs_hci_cmd_build_set_data_len(conn_handle, tx_octets, tx_time,
+                                           buf, sizeof buf);
     if (rc != 0) {
         return BLE_HS_HCI_ERR(rc);
     }
 
-    rc = ble_hci_cmd_tx(buf, params, BLE_HCI_SET_DATALEN_ACK_PARAM_LEN,
-                        &params_len);
+    rc = ble_hs_hci_cmd_tx(buf, params, BLE_HCI_SET_DATALEN_ACK_PARAM_LEN,
+                           &params_len);
     if (rc != 0) {
         return rc;
     }
@@ -157,3 +174,23 @@ ble_hci_util_set_data_len(uint16_t conn_handle, uint16_t tx_octets,
 
     return 0;
 }
+
+int
+ble_hs_hci_util_data_hdr_strip(struct os_mbuf *om,
+                               struct hci_data_hdr *out_hdr)
+{
+    int rc;
+
+    rc = os_mbuf_copydata(om, 0, BLE_HCI_DATA_HDR_SZ, out_hdr);
+    if (rc != 0) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    /* Strip HCI ACL data header from the front of the packet. */
+    os_mbuf_adj(om, BLE_HCI_DATA_HDR_SZ);
+
+    out_hdr->hdh_handle_pb_bc = le16toh(&out_hdr->hdh_handle_pb_bc);
+    out_hdr->hdh_len = le16toh(&out_hdr->hdh_len);
+
+    return 0;
+}
diff --git a/net/nimble/host/src/ble_hs_id.c b/net/nimble/host/src/ble_hs_id.c
new file mode 100644
index 00000000..9ef384b9
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_id.c
@@ -0,0 +1,248 @@
+/**
+ * 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 <string.h>
+#include "host/ble_hs_id.h"
+#include "ble_hs_priv.h"
+
+static uint8_t ble_hs_id_pub[6];
+static uint8_t ble_hs_id_rnd[6];
+
+void
+ble_hs_id_set_pub(const uint8_t *pub_addr)
+{
+    ble_hs_lock();
+    memcpy(ble_hs_id_pub, pub_addr, 6);
+    ble_hs_unlock();
+}
+
+/**
+ * Generates a new random address.  This function does not configure the device
+ * with the new address; the caller can use the address in subsequent
+ * operations.
+ *
+ * @param nrpa                  The type of random address to generate:
+ *                                  0: static
+ *                                  1: non-resolvable private
+ * @param out_addr              On success, the generated address gets written
+ *                                  here.
+ *
+ * @return                      0 on success; nonzero on failure.
+ */
+int
+ble_hs_id_gen_rnd(int nrpa, uint8_t *out_addr)
+{
+    int rc;
+
+    rc = ble_hs_hci_util_rand(out_addr, 6);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (nrpa) {
+        out_addr[5] &= ~0xc0;
+    } else {
+        out_addr[5] |= 0xc0;
+    }
+
+    return 0;
+}
+
+/**
+ * Sets the device's random address.  The address type (static vs.
+ * non-resolvable private) is inferred from the most-significant byte of the
+ * address.  The address is specified in host byte order (little-endian!).
+ *
+ * @param rnd_addr              The random address to set.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the specified address is not a
+ *                                  valid static random or non-resolvable
+ *                                  private address.
+ *                              Other nonzero on error.
+ */
+int
+ble_hs_id_set_rnd(const uint8_t *rnd_addr)
+{
+    uint8_t addr_type_byte;
+    int rc;
+
+    ble_hs_lock();
+
+    addr_type_byte = rnd_addr[5] & 0xc0;
+    if (addr_type_byte != 0x00 && addr_type_byte != 0xc0) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+
+    rc = ble_hs_hci_util_set_random_addr(rnd_addr);
+    if (rc != 0) {
+        goto done;
+    }
+
+    memcpy(ble_hs_id_rnd, rnd_addr, 6);
+
+    rc = 0;
+
+done:
+    ble_hs_unlock();
+    return rc;
+}
+
+/**
+ * Retrieves one of the device's identity addresses.  The device can have two
+ * identity addresses: one public and one random.  The id_addr_type argument
+ * specifies which of these two addresses to retrieve.
+ *
+ * @param id_addr_type          The type of identity address to retrieve.
+ *                                  Valid values are:
+ *                                      o BLE_ADDR_TYPE_PUBLIC
+ *                                      o BLE_ADDR_TYPE_RANDOM
+ * @param out_id_addr           On success, this is reseated to point to the
+ *                                  retrieved 6-byte identity address.
+ * @param out_is_nrpa           On success, the pointed-to value indicates
+ *                                  whether the retrieved address is a
+ *                                  non-resolvable private address.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if an invalid address type was
+ *                                  specified;
+ *                              BLE_HS_ENOADDR if the device does not have an
+ *                                  identity address of the requested type;
+ *                              Other BLE host core code on error.
+ */
+int
+ble_hs_id_addr(uint8_t id_addr_type, const uint8_t **out_id_addr,
+               int *out_is_nrpa)
+{
+    const uint8_t *id_addr;
+    int nrpa;
+
+    BLE_HS_DBG_ASSERT(ble_hs_locked_by_cur_task());
+
+    switch (id_addr_type) {
+    case BLE_ADDR_TYPE_PUBLIC:
+        id_addr = ble_hs_id_pub;
+        nrpa = 0;
+        break;
+
+    case BLE_ADDR_TYPE_RANDOM:
+        id_addr = ble_hs_id_rnd;
+        nrpa = (ble_hs_id_rnd[5] & 0xc0) == 0;
+        break;
+
+    default:
+        return BLE_HS_EINVAL;
+    }
+
+    if (memcmp(id_addr, ble_hs_misc_null_addr, 6) == 0) {
+        return BLE_HS_ENOADDR;
+    }
+
+    if (out_id_addr != NULL) {
+        *out_id_addr = id_addr;
+    }
+    if (out_is_nrpa != NULL) {
+        *out_is_nrpa = nrpa;
+    }
+
+    return 0;
+}
+
+/**
+ * Retrieves one of the device's identity addresses.  The device can have two
+ * identity addresses: one public and one random.  The id_addr_type argument
+ * specifies which of these two addresses to retrieve.
+ *
+ * @param id_addr_type          The type of identity address to retrieve.
+ *                                  Valid values are:
+ *                                      o BLE_ADDR_TYPE_PUBLIC
+ *                                      o BLE_ADDR_TYPE_RANDOM
+ * @param out_id_addr           On success, the requested identity address is
+ *                                  copied into this buffer.  The buffer must
+ *                                  be at least six bytes in size.
+ * @param out_is_nrpa           On success, the pointed-to value indicates
+ *                                  whether the retrieved address is a
+ *                                  non-resolvable private address.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if an invalid address type was
+ *                                  specified;
+ *                              BLE_HS_ENOADDR if the device does not have an
+ *                                  identity address of the requested type;
+ *                              Other BLE host core code on error.
+ */
+int
+ble_hs_id_copy_addr(uint8_t id_addr_type, uint8_t *out_id_addr,
+                    int *out_is_nrpa)
+{
+    const uint8_t *addr;
+    int rc;
+
+    ble_hs_lock();
+
+    rc = ble_hs_id_addr(id_addr_type, &addr, out_is_nrpa);
+    if (rc == 0) {
+        memcpy(out_id_addr, addr, 6);
+    }
+
+    ble_hs_unlock();
+
+    return rc;
+}
+
+int
+ble_hs_id_use_addr(uint8_t addr_type)
+{
+    uint8_t id_addr_type;
+    int nrpa;
+    int rc;
+
+    switch (addr_type) {
+    case BLE_ADDR_TYPE_PUBLIC:
+    case BLE_ADDR_TYPE_RANDOM:
+        rc = ble_hs_id_addr(addr_type, NULL, NULL);
+        if (rc != 0) {
+            return rc;
+        }
+        break;
+
+    case BLE_ADDR_TYPE_RPA_PUB_DEFAULT:
+    case BLE_ADDR_TYPE_RPA_RND_DEFAULT:
+        id_addr_type = ble_hs_misc_addr_type_to_id(addr_type);
+        rc = ble_hs_id_addr(id_addr_type, NULL, &nrpa);
+        if (rc != 0) {
+            return rc;
+        }
+        if (nrpa) {
+            return BLE_HS_ENOADDR;
+        }
+
+        rc = ble_hs_pvcy_ensure_started();
+        if (rc != 0) {
+            return rc;
+        }
+        break;
+
+    default:
+        return BLE_HS_EINVAL;
+    }
+
+    return 0;
+}
diff --git a/net/nimble/host/src/ble_hs_id_priv.h b/net/nimble/host/src/ble_hs_id_priv.h
new file mode 100644
index 00000000..5c0728c5
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_id_priv.h
@@ -0,0 +1,30 @@
+/**
+ * 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 H_BLE_HS_ID_PRIV_
+#define H_BLE_HS_ID_PRIV_
+
+#include <inttypes.h>
+
+void ble_hs_id_set_pub(const uint8_t *pub_addr);
+int ble_hs_id_addr(uint8_t id_addr_type, const uint8_t **out_id_addr,
+                   int *out_is_nrpa);
+int ble_hs_id_use_addr(uint8_t addr_type);
+
+#endif
diff --git a/net/nimble/host/src/ble_hs_log.c b/net/nimble/host/src/ble_hs_log.c
new file mode 100644
index 00000000..39294830
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_log.c
@@ -0,0 +1,47 @@
+/**
+ * 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 "os/os.h"
+#include "host/ble_hs.h"
+
+struct log ble_hs_log;
+
+void
+ble_hs_log_mbuf(const struct os_mbuf *om)
+{
+    uint8_t u8;
+    int i;
+
+    for (i = 0; i < OS_MBUF_PKTLEN(om); i++) {
+        os_mbuf_copydata(om, i, 1, &u8);
+        BLE_HS_LOG(DEBUG, "0x%02x ", u8);
+    }
+}
+
+void
+ble_hs_log_flat_buf(const void *data, int len)
+{
+    const uint8_t *u8ptr;
+    int i;
+
+    u8ptr = data;
+    for (i = 0; i < len; i++) {
+        BLE_HS_LOG(DEBUG, "0x%02x ", u8ptr[i]);
+    }
+}
diff --git a/net/nimble/host/src/ble_hs_mbuf.c b/net/nimble/host/src/ble_hs_mbuf.c
new file mode 100644
index 00000000..870c88bd
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_mbuf.c
@@ -0,0 +1,198 @@
+/**
+ * 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 "host/ble_hs.h"
+#include "ble_hs_priv.h"
+
+/**
+ * Allocates an mbuf for use by the nimble host.
+ */
+static struct os_mbuf *
+ble_hs_mbuf_gen_pkt(uint16_t leading_space)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = os_msys_get_pkthdr(0, 0);
+    if (om == NULL) {
+        return NULL;
+    }
+
+    if (om->om_omp->omp_databuf_len < leading_space) {
+        rc = os_mbuf_free_chain(om);
+        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
+        return NULL;
+    }
+
+    om->om_data += leading_space;
+
+    return om;
+}
+
+/**
+ * Allocates an mbuf with no leading space.
+ *
+ * @return                  An empty mbuf on success; null on memory
+ *                              exhaustion.
+ */
+struct os_mbuf *
+ble_hs_mbuf_bare_pkt(void)
+{
+    return ble_hs_mbuf_gen_pkt(0);
+}
+
+/**
+ * Allocates an mbuf suitable for an HCI ACM data packet.
+ *
+ * @return                  An empty mbuf on success; null on memory
+ *                              exhaustion.
+ */
+struct os_mbuf *
+ble_hs_mbuf_acm_pkt(void)
+{
+    return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ);
+}
+
+/**
+ * Allocates an mbuf suitable for an L2CAP data packet.  The resulting packet
+ * has sufficient leading space for:
+ *     o ACM data header
+ *     o L2CAP B-frame header
+ *
+ * @return                  An empty mbuf on success; null on memory
+ *                              exhaustion.
+ */
+struct os_mbuf *
+ble_hs_mbuf_l2cap_pkt(void)
+{
+    return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ + BLE_L2CAP_HDR_SZ);
+}
+
+/**
+ * Allocates an mbuf suitable for an ATT command packet.  The resulting packet
+ * has sufficient leading space for:
+ *     o ACM data header
+ *     o L2CAP B-frame header
+ *     o Largest ATT command base (prepare write request / response).
+ *
+ * @return                  An empty mbuf on success; null on memory
+ *                              exhaustion.
+ */
+struct os_mbuf *
+ble_hs_mbuf_att_pkt(void)
+{
+    /* Prepare write request and response are the larget ATT commands which
+     * contain attribute data.
+     */
+     return ble_hs_mbuf_gen_pkt(BLE_HCI_DATA_HDR_SZ +
+                                BLE_L2CAP_HDR_SZ +
+                                BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+}
+
+/**
+ * Allocates a an mbuf and fills it with the contents of the specified flat
+ * buffer.
+ *
+ * @param buf                   The flat buffer to copy from.
+ * @param len                   The length of the flat buffer.
+ *
+ * @return                      A newly-allocated mbuf on success;
+ *                              NULL on memory exhaustion.
+ */
+struct os_mbuf *
+ble_hs_mbuf_from_flat(const void *buf, uint16_t len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_mbuf_att_pkt();
+    if (om == NULL) {
+        return NULL;
+    }
+
+    rc = os_mbuf_copyinto(om, 0, buf, len);
+    if (rc != 0) {
+        os_mbuf_free_chain(om);
+        return NULL;
+    }
+
+    return om;
+}
+
+/**
+ * Copies the contents of an mbuf into the specified flat buffer.  If the flat
+ * buffer is too small to contain the mbuf's contents, it is filled to capacity
+ * and BLE_HS_EMSGSIZE is returned.
+ *
+ * @param om                    The mbuf to copy from.
+ * @param flat                  The destination flat buffer.
+ * @param max_len               The size of the flat buffer.
+ * @param out_copy_len          The number of bytes actually copied gets
+ *                                  written here.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EMSGSIZE if the flat buffer is too small
+ *                                  to contain the mbuf's contents;
+ *                              A BLE host core return code on unexpected
+ *                                  error.
+ */
+int
+ble_hs_mbuf_to_flat(const struct os_mbuf *om, void *flat, uint16_t max_len,
+                    uint16_t *out_copy_len)
+{
+    uint16_t copy_len;
+    int rc;
+
+    if (OS_MBUF_PKTLEN(om) <= max_len) {
+        copy_len = OS_MBUF_PKTLEN(om);
+    } else {
+        copy_len = max_len;
+    }
+
+    rc = os_mbuf_copydata(om, 0, copy_len, flat);
+    if (rc != 0) {
+        return BLE_HS_EUNKNOWN;
+    }
+
+    if (copy_len > max_len) {
+        rc = BLE_HS_EMSGSIZE;
+    } else {
+        rc = 0;
+    }
+
+    if (out_copy_len != NULL) {
+        *out_copy_len = copy_len;
+    }
+    return rc;
+}
+
+int
+ble_hs_mbuf_pullup_base(struct os_mbuf **om, int base_len)
+{
+    if (OS_MBUF_PKTLEN(*om) < base_len) {
+        return BLE_HS_EBADDATA;
+    }
+
+    *om = os_mbuf_pullup(*om, base_len);
+    if (*om == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+
+    return 0;
+}
diff --git a/net/nimble/host/src/ble_hs_mbuf_priv.h b/net/nimble/host/src/ble_hs_mbuf_priv.h
new file mode 100644
index 00000000..f0ea9b52
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_mbuf_priv.h
@@ -0,0 +1,11 @@
+#ifndef H_BLE_HS_MBUF_PRIV_
+#define H_BLE_HS_MBUF_PRIV_
+
+struct os_mbuf;
+
+struct os_mbuf *ble_hs_mbuf_bare_pkt(void);
+struct os_mbuf *ble_hs_mbuf_acm_pkt(void);
+struct os_mbuf *ble_hs_mbuf_l2cap_pkt(void);
+int ble_hs_mbuf_pullup_base(struct os_mbuf **om, int base_len);
+
+#endif
diff --git a/net/nimble/host/src/ble_hs_misc.c b/net/nimble/host/src/ble_hs_misc.c
index 9575ae39..649fc05e 100644
--- a/net/nimble/host/src/ble_hs_misc.c
+++ b/net/nimble/host/src/ble_hs_misc.c
@@ -23,6 +23,8 @@
 #include "console/console.h"
 #include "ble_hs_priv.h"
 
+const uint8_t ble_hs_misc_null_addr[6];
+
 int
 ble_hs_misc_malloc_mempool(void **mem, struct os_mempool *pool,
                            int num_entries, int entry_size, char *name)
@@ -43,71 +45,6 @@ ble_hs_misc_malloc_mempool(void **mem, struct os_mempool *pool,
     return 0;
 }
 
-void
-ble_hs_misc_log_mbuf(struct os_mbuf *om)
-{
-    uint8_t u8;
-    int i;
-
-    for (i = 0; i < OS_MBUF_PKTLEN(om); i++) {
-        os_mbuf_copydata(om, i, 1, &u8);
-        BLE_HS_LOG(DEBUG, "0x%02x ", u8);
-    }
-}
-
-void
-ble_hs_misc_log_flat_buf(void *data, int len)
-{
-    uint8_t *u8ptr;
-    int i;
-
-    u8ptr = data;
-    for (i = 0; i < len; i++) {
-        BLE_HS_LOG(DEBUG, "0x%02x ", u8ptr[i]);
-    }
-}
-
-/**
- * Allocates an mbuf for use by the nimble host.
- */
-struct os_mbuf *
-ble_hs_misc_pkthdr(void)
-{
-    struct os_mbuf *om;
-    int rc;
-
-    om = os_msys_get_pkthdr(0, 0);
-    if (om == NULL) {
-        return NULL;
-    }
-
-    /* Make room in the buffer for various headers.  XXX Check this number. */
-    if (om->om_omp->omp_databuf_len < 8) {
-        rc = os_mbuf_free_chain(om);
-        BLE_HS_DBG_ASSERT_EVAL(rc == 0);
-        return NULL;
-    }
-
-    om->om_data += 8;
-
-    return om;
-}
-
-int
-ble_hs_misc_pullup_base(struct os_mbuf **om, int base_len)
-{
-    if (OS_MBUF_PKTLEN(*om) < base_len) {
-        return BLE_HS_EBADDATA;
-    }
-
-    *om = os_mbuf_pullup(*om, base_len);
-    if (*om == NULL) {
-        return BLE_HS_ENOMEM;
-    }
-
-    return 0;
-}
-
 int
 ble_hs_misc_conn_chan_find(uint16_t conn_handle, uint16_t cid,
                            struct ble_hs_conn **out_conn,
@@ -140,7 +77,7 @@ ble_hs_misc_conn_chan_find(uint16_t conn_handle, uint16_t cid,
     return rc;
 }
 
-int
+void
 ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
                                 struct ble_hs_conn **out_conn,
                                 struct ble_l2cap_chan **out_chan)
@@ -150,7 +87,7 @@ ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
     int rc;
 
     rc = ble_hs_misc_conn_chan_find(conn_handle, cid, &conn, &chan);
-    BLE_HS_DBG_ASSERT(conn == NULL || chan != NULL);
+    BLE_HS_DBG_ASSERT_EVAL(rc == 0);
 
     if (out_conn != NULL) {
         *out_conn = conn;
@@ -158,8 +95,6 @@ ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
     if (out_chan != NULL) {
         *out_chan = chan;
     }
-
-    return rc;
 }
 
 uint8_t
diff --git a/net/nimble/host/src/ble_hs_priv.h b/net/nimble/host/src/ble_hs_priv.h
index 1be119a1..6bebec9c 100644
--- a/net/nimble/host/src/ble_hs_priv.h
+++ b/net/nimble/host/src/ble_hs_priv.h
@@ -26,25 +26,37 @@
 #include "ble_att_priv.h"
 #include "ble_gap_priv.h"
 #include "ble_gatt_priv.h"
-#include "ble_hci_util_priv.h"
-#include "ble_hs_adv_priv.h"
+#include "ble_hs_dbg_priv.h"
+#include "ble_hs_hci_priv.h"
 #include "ble_hs_atomic_priv.h"
 #include "ble_hs_conn_priv.h"
+#include "ble_hs_atomic_priv.h"
 #include "ble_hs_endian_priv.h"
+#include "ble_hs_mbuf_priv.h"
 #include "ble_hs_startup_priv.h"
 #include "ble_l2cap_priv.h"
 #include "ble_l2cap_sig_priv.h"
 #include "ble_sm_priv.h"
+#include "ble_hs_adv_priv.h"
+#include "ble_hs_pvcy_priv.h"
+#include "ble_hs_id_priv.h"
+#include "ble_uuid_priv.h"
 #include "host/ble_hs.h"
-#include "log/log.h"
 #include "nimble/nimble_opt.h"
 #include "stats/stats.h"
 struct ble_hs_conn;
 struct ble_l2cap_chan;
 struct os_mbuf;
 struct os_mempool;
+struct os_event;
 
 #define BLE_HOST_HCI_EVENT_CTLR_EVENT   (OS_EVENT_T_PERUSER + 0)
+#define BLE_HS_EVENT_TX_NOTIFICATIONS   (OS_EVENT_T_PERUSER + 1)
+#define BLE_HS_EVENT_RESET              (OS_EVENT_T_PERUSER + 2)
+
+#define BLE_HS_SYNC_STATE_BAD           0
+#define BLE_HS_SYNC_STATE_BRINGUP       1
+#define BLE_HS_SYNC_STATE_GOOD          2
 
 STATS_SECT_START(ble_hs_stats)
     STATS_SECT_ENTRY(conn_create)
@@ -53,38 +65,35 @@ STATS_SECT_START(ble_hs_stats)
     STATS_SECT_ENTRY(hci_event)
     STATS_SECT_ENTRY(hci_invalid_ack)
     STATS_SECT_ENTRY(hci_unknown_event)
+    STATS_SECT_ENTRY(hci_timeout)
+    STATS_SECT_ENTRY(reset)
+    STATS_SECT_ENTRY(sync)
 STATS_SECT_END
 extern STATS_SECT_DECL(ble_hs_stats) ble_hs_stats;
 
-struct ble_hci_ack {
-    int bha_status;         /* A BLE_HS_E<...> error; NOT a naked HCI code. */
-    uint8_t *bha_params;
-    int bha_params_len;
-    uint16_t bha_opcode;
-    uint8_t bha_hci_handle;
-};
-
-extern struct ble_hs_dev ble_hs_our_dev;
 extern struct ble_hs_cfg ble_hs_cfg;
-
 extern struct os_mbuf_pool ble_hs_mbuf_pool;
+extern uint8_t ble_hs_sync_state;
 
-extern struct log ble_hs_log;
+extern const uint8_t ble_hs_misc_null_addr[6];
 
 void ble_hs_process_tx_data_queue(void);
-int ble_hs_rx_data(struct os_mbuf *om);
+void ble_hs_process_rx_data_queue(void);
 int ble_hs_tx_data(struct os_mbuf *om);
+void ble_hs_enqueue_hci_event(uint8_t *hci_evt);
+void ble_hs_event_enqueue(struct os_event *ev);
+
+int ble_hs_hci_rx_evt(uint8_t *hci_ev, void *arg);
+int ble_hs_hci_evt_acl_process(struct os_mbuf *om);
 
 int ble_hs_misc_malloc_mempool(void **mem, struct os_mempool *pool,
                                int num_entries, int entry_size, char *name);
-void ble_hs_misc_log_mbuf(struct os_mbuf *om);
-void ble_hs_misc_log_flat_buf(void *data, int len);
 int ble_hs_misc_conn_chan_find(uint16_t conn_handle, uint16_t cid,
                                struct ble_hs_conn **out_conn,
                                struct ble_l2cap_chan **out_chan);
-int ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
-                                    struct ble_hs_conn **out_conn,
-                                    struct ble_l2cap_chan **out_chan);
+void ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
+                                     struct ble_hs_conn **out_conn,
+                                     struct ble_l2cap_chan **out_chan);
 uint8_t ble_hs_misc_addr_type_to_id(uint8_t addr_type);
 
 void ble_hs_cfg_init(struct ble_hs_cfg *cfg);
@@ -93,37 +102,10 @@ int ble_hs_locked_by_cur_task(void);
 int ble_hs_is_parent_task(void);
 void ble_hs_lock(void);
 void ble_hs_unlock(void);
-
-struct os_mbuf *ble_hs_misc_pkthdr(void);
-
-int ble_hs_misc_pullup_base(struct os_mbuf **om, int base_len);
-
-int ble_hci_cmd_tx(void *cmd, void *evt_buf, uint8_t evt_buf_len,
-                   uint8_t *out_evt_buf_len);
-int ble_hci_cmd_tx_empty_ack(void *cmd);
-void ble_hci_cmd_rx_ack(uint8_t *ack_ev);
-void ble_hci_cmd_init(void);
-int ble_hs_pvcy_set_our_nrpa(void);
-void ble_hs_pvcy_our_nrpa(uint8_t *addr);
-void ble_hs_pvcy_set_our_id_addr(uint8_t *addr);
-void ble_hs_pvcy_set_our_irk(uint8_t *irk);
-uint8_t *ble_hs_pvcy_our_id_addr(uint8_t *type);
-uint8_t *ble_hs_pvcy_our_irk(void);
-int ble_hs_pvcy_remove_entry(uint8_t addr_type, uint8_t *addr);
-int ble_hs_pvcy_add_entry(uint8_t *addr, uint8_t addrtype, uint8_t *irk);
-#if PHONY_HCI_ACKS
-typedef int ble_hci_cmd_phony_ack_fn(uint8_t *ack, int ack_buf_len);
-
-void ble_hci_set_phony_ack_cb(ble_hci_cmd_phony_ack_fn *cb);
-#endif
-
-#define BLE_HS_LOG(lvl, ...) \
-    LOG_ ## lvl(&ble_hs_log, LOG_MODULE_NIMBLE_HOST, __VA_ARGS__)
-
-#define BLE_HS_LOG_ADDR(lvl, addr)                      \
-    BLE_HS_LOG(lvl, "%02x:%02x:%02x:%02x:%02x:%02x",    \
-               (addr)[5], (addr)[4], (addr)[3],         \
-               (addr)[2], (addr)[1], (addr)[0])
+void ble_hs_sched_reset(int reason);
+void ble_hs_hw_error(uint8_t hw_code);
+void ble_hs_heartbeat_sched(int32_t ticks);
+void ble_hs_notifications_sched(void);
 
 #if LOG_LEVEL <= LOG_LEVEL_DEBUG
 
diff --git a/net/nimble/host/src/ble_hs_pvcy.c b/net/nimble/host/src/ble_hs_pvcy.c
index 1ea2f918..ad3fa0c3 100644
--- a/net/nimble/host/src/ble_hs_pvcy.c
+++ b/net/nimble/host/src/ble_hs_pvcy.c
@@ -21,13 +21,9 @@
 #include <string.h>
 #include "ble_hs_priv.h"
 
-static int ble_hs_pvcy_initialized;
-static uint8_t ble_hs_pvcy_id_addr[6];
-static uint8_t ble_hs_pvcy_id_addr_type;
-static uint8_t ble_hs_pvcy_nrpa[6];
+static uint8_t ble_hs_pvcy_started;
 uint8_t ble_hs_pvcy_irk[16];
 
-
 /** Use this as a default IRK if none gets set. */
 const uint8_t default_irk[16] = {
     0xef, 0x8d, 0xe2, 0x16, 0x4f, 0xec, 0x43, 0x0d,
@@ -35,24 +31,12 @@ const uint8_t default_irk[16] = {
 };
 
 static int
-ble_hs_pvcy_gen_static_random_addr(uint8_t *addr)
-{
-    int rc;
-
-    rc = ble_hci_util_rand(addr, 6);
-    /* TODO -- set bits properly */
-    addr[5] |= 0xc0;
-
-    return rc;
-}
-
-static int
 ble_hs_pvcy_set_addr_timeout(uint16_t timeout)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_RESOLV_PRIV_ADDR_TO_LEN];
     int rc;
 
-    rc = host_hci_cmd_build_set_resolv_priv_addr_timeout(
+    rc = ble_hs_hci_cmd_build_set_resolv_priv_addr_timeout(
             timeout, buf, sizeof(buf));
 
     return rc;
@@ -64,12 +48,12 @@ ble_hs_pvcy_set_resolve_enabled(int enable)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_ADDR_RESOL_ENA_LEN];
     int rc;
 
-    rc = host_hci_cmd_build_set_addr_res_en(enable, buf, sizeof(buf));
+    rc = ble_hs_hci_cmd_build_set_addr_res_en(enable, buf, sizeof(buf));
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    rc = ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
     if (rc != 0) {
         return rc;
     }
@@ -83,13 +67,13 @@ ble_hs_pvcy_remove_entry(uint8_t addr_type, uint8_t *addr)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_RMV_FROM_RESOLV_LIST_LEN];
     int rc;
 
-    rc = host_hci_cmd_build_remove_from_resolv_list(
+    rc = ble_hs_hci_cmd_build_remove_from_resolv_list(
         addr_type, addr, buf, sizeof(buf));
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    rc = ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
     if (rc != 0) {
         return rc;
     }
@@ -103,12 +87,12 @@ ble_hs_pvcy_clear_entries(void)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN ];
     int rc;
 
-    rc = host_hci_cmd_build_clear_resolv_list(buf, sizeof(buf));
+    rc = ble_hs_hci_cmd_build_clear_resolv_list(buf, sizeof(buf));
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    rc = ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
     if (rc != 0) {
         return rc;
     }
@@ -125,15 +109,15 @@ ble_hs_pvcy_add_entry(uint8_t *addr, uint8_t addr_type, uint8_t *irk)
 
     add.addr_type = addr_type;
     memcpy(add.addr, addr, 6);
-    memcpy(add.local_irk, ble_hs_pvcy_our_irk(), 16);
+    memcpy(add.local_irk, ble_hs_pvcy_irk, 16);
     memcpy(add.peer_irk, irk, 16);
 
-    rc = host_hci_cmd_build_add_to_resolv_list(&add, buf, sizeof(buf));
+    rc = ble_hs_hci_cmd_build_add_to_resolv_list(&add, buf, sizeof(buf));
     if (rc != 0) {
         return rc;
     }
 
-    rc = ble_hci_cmd_tx(buf, NULL, 0, NULL);
+    rc = ble_hs_hci_cmd_tx(buf, NULL, 0, NULL);
     if (rc != 0) {
         return rc;
     }
@@ -141,73 +125,32 @@ ble_hs_pvcy_add_entry(uint8_t *addr, uint8_t addr_type, uint8_t *irk)
     return 0;
 }
 
-void
-ble_hs_pvcy_our_nrpa(uint8_t *addr)
-{
-    memcpy(addr, ble_hs_pvcy_nrpa, 6);
-}
-
-int 
-ble_hs_pvcy_set_our_nrpa(void)
+int
+ble_hs_pvcy_ensure_started(void)
 {
     int rc;
-    uint8_t addr[6];
-
-    rc = ble_hci_util_rand(addr, 6);
-    assert(rc == 0);
-    addr[5] &= ~(0xc0);
-    memcpy(ble_hs_pvcy_nrpa, addr, 6);
-
-    return ble_hs_util_set_random_addr(addr);
-}
 
-uint8_t *
-ble_hs_pvcy_our_id_addr(uint8_t *type)
-{
-    if (!ble_hs_pvcy_initialized) {
-        ble_hs_pvcy_set_our_id_addr(NULL);
+    if (ble_hs_pvcy_started) {
+        return 0;
     }
 
-    if (type != NULL) {
-        *type = ble_hs_pvcy_id_addr_type;
-    }
-
-    return ble_hs_pvcy_id_addr;
-}
-
-void
-ble_hs_pvcy_set_our_id_addr(uint8_t *addr)
-{
-    uint8_t random_addr[6];
-    int rc;
-
-    if (!ble_hs_pvcy_initialized) {
-        /* Set up the periodic change of our RPA. */
-        rc = ble_hs_pvcy_set_addr_timeout(ble_hs_cfg.rpa_timeout);
-        assert(rc == 0);
+    /* Set up the periodic change of our RPA. */
+    rc = ble_hs_pvcy_set_addr_timeout(ble_hs_cfg.rpa_timeout);
+    if (rc != 0) {
+        return rc;
     }
 
-    if (addr != NULL) {
-        memcpy(ble_hs_pvcy_id_addr, addr, 6);
-        ble_hs_pvcy_id_addr_type = BLE_HCI_ADV_OWN_ADDR_PUBLIC;
-    } else {
-        /* Generate a new static random address. */
-        ble_hs_pvcy_gen_static_random_addr(random_addr);
-        rc = ble_hs_util_set_random_addr(random_addr);
-        assert(rc == 0);
+    ble_hs_pvcy_started = 1;
 
-        ble_hs_pvcy_id_addr_type = BLE_HCI_ADV_OWN_ADDR_RANDOM;
-        memcpy(ble_hs_pvcy_id_addr, random_addr, 6);
-    }
-
-    ble_hs_pvcy_initialized = 1;
+    return 0;
 }
 
-void
-ble_hs_pvcy_set_our_irk(uint8_t *irk)
+int
+ble_hs_pvcy_set_our_irk(const uint8_t *irk)
 {
     uint8_t tmp_addr[6];
     uint8_t new_irk[16];
+    int rc;
 
     memset(new_irk, 0, sizeof(new_irk));
 
@@ -221,25 +164,40 @@ ble_hs_pvcy_set_our_irk(uint8_t *irk)
     if (memcmp(ble_hs_pvcy_irk, new_irk, 16) != 0) {
         memcpy(ble_hs_pvcy_irk, new_irk, 16);
 
-        ble_hs_pvcy_set_resolve_enabled(0);
-        ble_hs_pvcy_clear_entries();
-        ble_hs_pvcy_set_resolve_enabled(1);
+        rc = ble_hs_pvcy_set_resolve_enabled(0);
+        if (rc != 0) {
+            return rc;
+        }
+
+        rc = ble_hs_pvcy_clear_entries();
+        if (rc != 0) {
+            return rc;
+        }
 
-        /* Push our identity to the controller as a keycache entry with a null
-         * MAC address. The controller uses this entry to generate an RPA when
-         * we do advertising with own-addr-type = rpa.
+        rc = ble_hs_pvcy_set_resolve_enabled(1);
+        if (rc != 0) {
+            return rc;
+        }
+
+        /* Push a null address identity to the controller.  The controller uses
+         * this entry to generate an RPA when we do advertising with
+         * own-addr-type = rpa.
          */
         memset(tmp_addr, 0, 6);
-        ble_hs_pvcy_add_entry(tmp_addr, 0, ble_hs_pvcy_irk);
+        rc = ble_hs_pvcy_add_entry(tmp_addr, 0, ble_hs_pvcy_irk);
+        if (rc != 0) {
+            return rc;
+        }
     }
+
+    return 0;
 }
 
-uint8_t *
-ble_hs_pvcy_our_irk(void)
+int
+ble_hs_pvcy_our_irk(const uint8_t **out_irk)
 {
-    if (!ble_hs_pvcy_initialized) {
-        ble_hs_pvcy_set_our_id_addr(NULL);
-    }
+    /* XXX: Return error if privacy not supported. */
 
-    return ble_hs_pvcy_irk;
+    *out_irk = ble_hs_pvcy_irk;
+    return 0;
 }
diff --git a/net/nimble/host/src/ble_hs_pvcy_priv.h b/net/nimble/host/src/ble_hs_pvcy_priv.h
new file mode 100644
index 00000000..16ded359
--- /dev/null
+++ b/net/nimble/host/src/ble_hs_pvcy_priv.h
@@ -0,0 +1,31 @@
+/**
+ * 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 H_BLE_HS_PVCY_PRIV_
+#define H_BLE_HS_PVCY_PRIV_
+
+#include <inttypes.h>
+
+int ble_hs_pvcy_set_our_irk(const uint8_t *irk);
+int ble_hs_pvcy_our_irk(const uint8_t **out_irk);
+int ble_hs_pvcy_remove_entry(uint8_t addr_type, uint8_t *addr);
+int ble_hs_pvcy_add_entry(uint8_t *addr, uint8_t addrtype, uint8_t *irk);
+int ble_hs_pvcy_ensure_started(void);
+
+#endif
diff --git a/net/nimble/host/src/ble_hs_startup.c b/net/nimble/host/src/ble_hs_startup.c
index 1973cf9a..89cbd700 100644
--- a/net/nimble/host/src/ble_hs_startup.c
+++ b/net/nimble/host/src/ble_hs_startup.c
@@ -19,7 +19,6 @@
 
 #include <stddef.h>
 #include <string.h>
-#include "host/host_hci.h"
 #include "host/ble_hs.h"
 #include "ble_hs_priv.h"
 
@@ -31,8 +30,9 @@ ble_hs_startup_le_read_sup_f_tx(void)
     uint8_t ack_params_len;
     int rc;
 
-    host_hci_cmd_build_le_read_loc_supp_feat(buf, sizeof buf);
-    rc = ble_hci_cmd_tx(buf, ack_params, sizeof ack_params, &ack_params_len);
+    ble_hs_hci_cmd_build_le_read_loc_supp_feat(buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx(buf, ack_params, sizeof ack_params,
+                           &ack_params_len);
     if (rc != 0) {
         return rc;
     }
@@ -56,8 +56,9 @@ ble_hs_startup_le_read_buf_sz_tx(void)
     uint8_t max_pkts;
     int rc;
 
-    host_hci_cmd_build_le_read_buffer_size(buf, sizeof buf);
-    rc = ble_hci_cmd_tx(buf, ack_params, sizeof ack_params, &ack_params_len);
+    ble_hs_hci_cmd_build_le_read_buffer_size(buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx(buf, ack_params, sizeof ack_params,
+                           &ack_params_len);
     if (rc != 0) {
         return rc;
     }
@@ -69,7 +70,7 @@ ble_hs_startup_le_read_buf_sz_tx(void)
     pktlen = le16toh(ack_params + 0);
     max_pkts = ack_params[2];
 
-    rc = host_hci_set_buf_size(pktlen, max_pkts);
+    rc = ble_hs_hci_set_buf_sz(pktlen, max_pkts);
     if (rc != 0) {
         return rc;
     }
@@ -78,6 +79,29 @@ ble_hs_startup_le_read_buf_sz_tx(void)
 }
 
 static int
+ble_hs_startup_read_bd_addr(void)
+{
+    uint8_t ack_params[BLE_HCI_IP_RD_BD_ADDR_ACK_PARAM_LEN];
+    uint8_t buf[BLE_HCI_CMD_HDR_LEN];
+    uint8_t ack_params_len;
+    int rc;
+
+    ble_hs_hci_cmd_build_read_bd_addr(buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx(buf, ack_params, sizeof ack_params,
+                           &ack_params_len);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (ack_params_len != sizeof ack_params) {
+        return BLE_HS_ECONTROLLER;
+    }
+
+    ble_hs_id_set_pub(ack_params);
+    return 0;
+}
+
+static int
 ble_hs_startup_le_set_evmask_tx(void)
 {
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_SET_LE_EVENT_MASK_LEN];
@@ -94,8 +118,9 @@ ble_hs_startup_le_set_evmask_tx(void)
      *     0x0000000000000040 LE Data Length Change Event
      *     0x0000000000000200 LE Enhanced Connection Complete Event
      */
-    host_hci_cmd_build_le_set_event_mask(0x000000000000027f, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_set_event_mask(0x000000000000027f,
+                                           buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -151,8 +176,8 @@ ble_hs_startup_set_evmask_tx(void)
      *     0x0000800000000000 Encryption Key Refresh Complete Event
      *     0x2000000000000000 LE Meta-Event
      */
-    host_hci_cmd_build_set_event_mask(0x20009fffffffffff, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_set_event_mask(0x20009fffffffffff, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -161,8 +186,8 @@ ble_hs_startup_set_evmask_tx(void)
      * Enable the following events:
      *     0x0000000000800000 Authenticated Payload Timeout Event
      */
-    host_hci_cmd_build_set_event_mask2(0x0000000000800000, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_set_event_mask2(0x0000000000800000, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -176,8 +201,8 @@ ble_hs_startup_reset_tx(void)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN];
     int rc;
 
-    host_hci_cmd_build_reset(buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_reset(buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -200,19 +225,16 @@ ble_hs_startup_go(void)
 
     rc = ble_hs_startup_set_evmask_tx();
     if (rc != 0) {
-        assert(0);
         return rc;
     }
 
     rc = ble_hs_startup_le_set_evmask_tx();
     if (rc != 0) {
-        assert(0);
         return rc;
     }
 
     rc = ble_hs_startup_le_read_buf_sz_tx();
     if (rc != 0) {
-        assert(0);
         return rc;
     }
 
@@ -220,13 +242,15 @@ ble_hs_startup_go(void)
 
     rc = ble_hs_startup_le_read_sup_f_tx();
     if (rc != 0) {
-        assert(0);
         return rc;
     }
 
-    /* XXX: Read BD_ADDR. */
-    ble_hs_pvcy_set_our_id_addr(g_dev_addr);
+    rc = ble_hs_startup_read_bd_addr();
+    if (rc != 0) {
+        return rc;
+    }
+
     ble_hs_pvcy_set_our_irk(NULL);
 
-    return rc;
+    return 0;
 }
diff --git a/net/nimble/host/src/ble_ibeacon.c b/net/nimble/host/src/ble_ibeacon.c
index db34dfbd..5bdb99d7 100644
--- a/net/nimble/host/src/ble_ibeacon.c
+++ b/net/nimble/host/src/ble_ibeacon.c
@@ -18,10 +18,24 @@
  */
 
 #include <string.h>
+#include "host/ble_hs_adv.h"
 #include "ble_hs_priv.h"
 
 #define BLE_IBEACON_MFG_DATA_SIZE       25
 
+/**
+ * Configures the device to advertise iBeacons.
+ *
+ * @param uuid                  The 128-bit UUID to advertise.
+ * @param major                 The major version number to include in
+ *                                  iBeacons.
+ * @param minor                 The minor version number to include in
+ *                                  iBeacons.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EBUSY if advertising is in progress;
+ *                              Other nonzero on failure.
+ */
 int
 ble_ibeacon_set_adv_data(void *uuid128, uint16_t major, uint16_t minor)
 {
@@ -47,7 +61,7 @@ ble_ibeacon_set_adv_data(void *uuid128, uint16_t major, uint16_t minor)
 
     /** Last byte (tx power level) filled in after HCI exchange. */
 
-    rc = ble_hci_util_read_adv_tx_pwr(&tx_pwr);
+    rc = ble_hs_hci_util_read_adv_tx_pwr(&tx_pwr);
     if (rc != 0) {
         return rc;
     }
diff --git a/net/nimble/host/src/ble_l2cap.c b/net/nimble/host/src/ble_l2cap.c
index da66bf1b..43ccd6f7 100644
--- a/net/nimble/host/src/ble_l2cap.c
+++ b/net/nimble/host/src/ble_l2cap.c
@@ -22,7 +22,6 @@
 #include "os/os.h"
 #include "nimble/ble.h"
 #include "nimble/hci_common.h"
-#include "host/host_hci.h"
 #include "ble_hs_priv.h"
 
 _Static_assert(sizeof (struct ble_l2cap_hdr) == BLE_L2CAP_HDR_SZ,
@@ -79,7 +78,7 @@ ble_l2cap_chan_free(struct ble_l2cap_chan *chan)
 }
 
 uint16_t
-ble_l2cap_chan_mtu(struct ble_l2cap_chan *chan)
+ble_l2cap_chan_mtu(const struct ble_l2cap_chan *chan)
 {
     uint16_t mtu;
 
@@ -124,7 +123,7 @@ ble_l2cap_prepend_hdr(struct os_mbuf *om, uint16_t cid, uint16_t len)
     htole16(&hdr.blh_len, len);
     htole16(&hdr.blh_cid, cid);
 
-    om = os_mbuf_prepend(om, sizeof hdr);
+    om = os_mbuf_prepend_pullup(om, sizeof hdr);
     if (om == NULL) {
         return NULL;
     }
@@ -270,33 +269,27 @@ err:
  * mbuf is consumed, regardless of the outcome of the function call.
  * 
  * @param chan                  The L2CAP channel to transmit over.
- * @param om                    The data to transmit.
+ * @param txom                  The data to transmit.
  *
  * @return                      0 on success; nonzero on error.
  */
 int
 ble_l2cap_tx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
-             struct os_mbuf *om)
+             struct os_mbuf *txom)
 {
     int rc;
 
-    om = ble_l2cap_prepend_hdr(om, chan->blc_cid, OS_MBUF_PKTLEN(om));
-    if (om == NULL) {
-        rc = BLE_HS_ENOMEM;
-        goto err;
+    txom = ble_l2cap_prepend_hdr(txom, chan->blc_cid, OS_MBUF_PKTLEN(txom));
+    if (txom == NULL) {
+        return BLE_HS_ENOMEM;
     }
 
-    rc = host_hci_data_tx(conn, om);
-    om = NULL;
+    rc = ble_hs_hci_acl_tx(conn, txom);
     if (rc != 0) {
-        goto err;
+        return rc;
     }
 
     return 0;
-
-err:
-    os_mbuf_free_chain(om);
-    return rc;
 }
 
 static void
diff --git a/net/nimble/host/src/ble_l2cap_priv.h b/net/nimble/host/src/ble_l2cap_priv.h
index 9cbf8791..ce19218e 100644
--- a/net/nimble/host/src/ble_l2cap_priv.h
+++ b/net/nimble/host/src/ble_l2cap_priv.h
@@ -57,8 +57,7 @@ extern struct os_mempool ble_l2cap_chan_pool;
 
 typedef uint8_t ble_l2cap_chan_flags;
 
-typedef int ble_l2cap_rx_fn(uint16_t conn_handle,
-                            struct os_mbuf **om);
+typedef int ble_l2cap_rx_fn(uint16_t conn_handle, struct os_mbuf **rxom);
 
 struct ble_l2cap_chan {
     SLIST_ENTRY(ble_l2cap_chan) blc_next;
@@ -94,7 +93,7 @@ struct os_mbuf *ble_l2cap_prepend_hdr(struct os_mbuf *om, uint16_t cid,
 struct ble_l2cap_chan *ble_l2cap_chan_alloc(void);
 void ble_l2cap_chan_free(struct ble_l2cap_chan *chan);
 
-uint16_t ble_l2cap_chan_mtu(struct ble_l2cap_chan *chan);
+uint16_t ble_l2cap_chan_mtu(const struct ble_l2cap_chan *chan);
 
 
 int ble_l2cap_rx(struct ble_hs_conn *conn,
@@ -103,7 +102,7 @@ int ble_l2cap_rx(struct ble_hs_conn *conn,
                  ble_l2cap_rx_fn **out_rx_cb,
                  struct os_mbuf **out_rx_buf);
 int ble_l2cap_tx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
-                 struct os_mbuf *om);
+                 struct os_mbuf *txom);
 
 int ble_l2cap_init(void);
 
diff --git a/net/nimble/host/src/ble_l2cap_sig.c b/net/nimble/host/src/ble_l2cap_sig.c
index 93ba2e23..a382b097 100644
--- a/net/nimble/host/src/ble_l2cap_sig.c
+++ b/net/nimble/host/src/ble_l2cap_sig.c
@@ -300,7 +300,7 @@ ble_l2cap_sig_update_req_rx(uint16_t conn_handle,
 
     l2cap_result = 0; /* Silence spurious gcc warning. */
 
-    rc = ble_hs_misc_pullup_base(om, BLE_L2CAP_SIG_UPDATE_REQ_SZ);
+    rc = ble_hs_mbuf_pullup_base(om, BLE_L2CAP_SIG_UPDATE_REQ_SZ);
     if (rc != 0) {
         return rc;
     }
@@ -338,18 +338,16 @@ ble_l2cap_sig_update_req_rx(uint16_t conn_handle,
 
     /* Send L2CAP response. */
     ble_hs_lock();
-    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
-                                         &conn, &chan);
-    if (rc == 0) {
-        if (!sig_err) {
-            rc = ble_l2cap_sig_update_rsp_tx(conn, chan, hdr->identifier,
-                                             l2cap_result);
-        } else {
-            ble_l2cap_sig_reject_tx(conn, chan, hdr->identifier,
-                                    BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD,
-                                    NULL, 0);
-            rc = BLE_HS_L2C_ERR(BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD);
-        }
+    ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                    &conn, &chan);
+    if (!sig_err) {
+        rc = ble_l2cap_sig_update_rsp_tx(conn, chan, hdr->identifier,
+                                         l2cap_result);
+    } else {
+        ble_l2cap_sig_reject_tx(conn, chan, hdr->identifier,
+                                BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD,
+                                NULL, 0);
+        rc = BLE_HS_L2C_ERR(BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD);
     }
     ble_hs_unlock();
 
@@ -373,7 +371,7 @@ ble_l2cap_sig_update_rsp_rx(uint16_t conn_handle,
         return BLE_HS_ENOENT;
     }
 
-    rc = ble_hs_misc_pullup_base(om, BLE_L2CAP_SIG_UPDATE_RSP_SZ);
+    rc = ble_hs_mbuf_pullup_base(om, BLE_L2CAP_SIG_UPDATE_RSP_SZ);
     if (rc != 0) {
         cb_status = rc;
         goto done;
@@ -421,11 +419,8 @@ ble_l2cap_sig_update(uint16_t conn_handle,
 
     ble_hs_lock();
 
-    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
-                                         &conn, &chan);
-    if (rc != 0) {
-        goto done;
-    }
+    ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                    &conn, &chan);
     if (conn->bhc_flags & BLE_HS_CONN_F_MASTER) {
         /* Only the slave can initiate the L2CAP connection update
          * procedure.
@@ -479,10 +474,10 @@ ble_l2cap_sig_rx(uint16_t conn_handle, struct os_mbuf **om)
 
     STATS_INC(ble_l2cap_stats, sig_rx);
     BLE_HS_LOG(DEBUG, "L2CAP - rxed signalling msg: ");
-    ble_hs_misc_log_mbuf(*om);
+    ble_hs_log_mbuf(*om);
     BLE_HS_LOG(DEBUG, "\n");
 
-    rc = ble_hs_misc_pullup_base(om, BLE_L2CAP_SIG_HDR_SZ);
+    rc = ble_hs_mbuf_pullup_base(om, BLE_L2CAP_SIG_HDR_SZ);
     if (rc != 0) {
         return rc;
     }
@@ -499,16 +494,12 @@ ble_l2cap_sig_rx(uint16_t conn_handle, struct os_mbuf **om)
     rx_cb = ble_l2cap_sig_dispatch_get(hdr.op);
     if (rx_cb == NULL) {
         ble_hs_lock();
-        rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
-                                             &conn, &chan);
-        if (rc == 0) {
-            ble_l2cap_sig_reject_tx(conn, chan, hdr.identifier,
-                                    BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD,
-                                    NULL, 0);
-            rc = BLE_HS_L2C_ERR(BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD);
-        } else {
-            rc = BLE_HS_ENOTCONN;
-        }
+        ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                        &conn, &chan);
+        ble_l2cap_sig_reject_tx(conn, chan, hdr.identifier,
+                                BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD,
+                                NULL, 0);
+        rc = BLE_HS_L2C_ERR(BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD);
         ble_hs_unlock();
     } else {
         rc = rx_cb(conn_handle, &hdr, om);
@@ -582,7 +573,7 @@ ble_l2cap_sig_extract_expired(struct ble_l2cap_sig_proc_list *dst_list)
  *                                  be called again; currently always
  *                                  UINT32_MAX.
  */
-uint32_t
+int32_t
 ble_l2cap_sig_heartbeat(void)
 {
     struct ble_l2cap_sig_proc_list temp_list;
@@ -596,10 +587,10 @@ ble_l2cap_sig_heartbeat(void)
     /* Terminate the connection associated with each timed-out procedure. */
     STAILQ_FOREACH(proc, &temp_list, next) {
         STATS_INC(ble_l2cap_stats, proc_timeout);
-        ble_gap_terminate(proc->conn_handle);
+        ble_gap_terminate(proc->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
     }
 
-    return UINT32_MAX;
+    return BLE_HS_FOREVER;
 }
 
 int
diff --git a/net/nimble/host/src/ble_l2cap_sig_cmd.c b/net/nimble/host/src/ble_l2cap_sig_cmd.c
index a93833c6..d8220322 100644
--- a/net/nimble/host/src/ble_l2cap_sig_cmd.c
+++ b/net/nimble/host/src/ble_l2cap_sig_cmd.c
@@ -27,18 +27,21 @@ ble_l2cap_sig_init_cmd(uint8_t op, uint8_t id, uint8_t payload_len,
     struct ble_l2cap_sig_hdr hdr;
     struct os_mbuf *txom;
     void *v;
+    int rc;
 
     *out_om = NULL;
     *out_payload_buf = NULL;
 
-    txom = ble_hs_misc_pkthdr();
+    txom = ble_hs_mbuf_l2cap_pkt();
     if (txom == NULL) {
-        return BLE_HS_ENOMEM;
+        rc = BLE_HS_ENOMEM;
+        goto err;
     }
 
     v = os_mbuf_extend(txom, BLE_L2CAP_SIG_HDR_SZ + payload_len);
     if (v == NULL) {
-        return BLE_HS_ENOMEM;
+        rc = BLE_HS_ENOMEM;
+        goto err;
     }
 
     hdr.op = op;
@@ -51,6 +54,10 @@ ble_l2cap_sig_init_cmd(uint8_t op, uint8_t id, uint8_t payload_len,
     *out_payload_buf = (uint8_t *)v + BLE_L2CAP_SIG_HDR_SZ;
 
     return 0;
+
+err:
+    os_mbuf_free(txom);
+    return rc;
 }
 
 static void
@@ -113,6 +120,9 @@ ble_l2cap_sig_reject_tx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
     rc = ble_l2cap_sig_init_cmd(BLE_L2CAP_SIG_OP_REJECT, id,
                                 BLE_L2CAP_SIG_REJECT_MIN_SZ + data_len, &txom,
                                 &payload_buf);
+    if (rc != 0) {
+        return rc;
+    }
 
     cmd.reason = reason;
     ble_l2cap_sig_reject_write(payload_buf, txom->om_len, &cmd,
@@ -120,7 +130,11 @@ ble_l2cap_sig_reject_tx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
 
     STATS_INC(ble_l2cap_stats, sig_rx);
     rc = ble_l2cap_tx(conn, chan, txom);
-    return rc;
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
 }
 
 int
diff --git a/net/nimble/host/src/ble_l2cap_sig_priv.h b/net/nimble/host/src/ble_l2cap_sig_priv.h
index a561a82a..3477ee13 100644
--- a/net/nimble/host/src/ble_l2cap_sig_priv.h
+++ b/net/nimble/host/src/ble_l2cap_sig_priv.h
@@ -80,7 +80,7 @@ int ble_l2cap_sig_reject_invalid_cid_tx(struct ble_hs_conn *conn,
                                         uint8_t id,
                                         uint16_t src_cid, uint16_t dst_cid);
 
-uint32_t ble_l2cap_sig_heartbeat(void);
+int32_t ble_l2cap_sig_heartbeat(void);
 struct ble_l2cap_chan *ble_l2cap_sig_create_chan(void);
 int ble_l2cap_sig_init(void);
 
diff --git a/net/nimble/host/src/ble_sm.c b/net/nimble/host/src/ble_sm.c
index ab411731..8e07cdb0 100644
--- a/net/nimble/host/src/ble_sm.c
+++ b/net/nimble/host/src/ble_sm.c
@@ -259,7 +259,7 @@ ble_sm_gen_pair_rand(uint8_t *pair_rand)
     }
 #endif
 
-    rc = ble_hci_util_rand(pair_rand, 16);
+    rc = ble_hs_hci_util_rand(pair_rand, 16);
     if (rc != 0) {
         return rc;
     }
@@ -280,7 +280,7 @@ ble_sm_gen_ediv(uint16_t *ediv)
     }
 #endif
 
-    rc = ble_hci_util_rand(ediv, sizeof *ediv);
+    rc = ble_hs_hci_util_rand(ediv, sizeof *ediv);
     if (rc != 0) {
         return rc;
     }
@@ -301,7 +301,7 @@ ble_sm_gen_master_id_rand(uint64_t *master_id_rand)
     }
 #endif
 
-    rc = ble_hci_util_rand(master_id_rand, sizeof *master_id_rand);
+    rc = ble_hs_hci_util_rand(master_id_rand, sizeof *master_id_rand);
     if (rc != 0) {
         return rc;
     }
@@ -323,7 +323,7 @@ ble_sm_gen_ltk(struct ble_sm_proc *proc, uint8_t *ltk)
     }
 #endif
 
-    rc = ble_hci_util_rand(ltk, 16);
+    rc = ble_hs_hci_util_rand(ltk, 16);
     if (rc != 0) {
         return rc;
     }
@@ -345,7 +345,7 @@ ble_sm_gen_csrk(struct ble_sm_proc *proc, uint8_t *csrk)
     }
 #endif
 
-    rc = ble_hci_util_rand(csrk, 16);
+    rc = ble_hs_hci_util_rand(csrk, 16);
     if (rc != 0) {
         return rc;
     }
@@ -494,24 +494,7 @@ ble_sm_fill_store_value(uint8_t peer_addr_type, uint8_t *peer_addr,
     }
 }
 
-int
-ble_sm_peer_addr(struct ble_sm_proc *proc,
-                 uint8_t *out_type, uint8_t **out_addr)
-{
-    struct ble_hs_conn *conn;
-
-    conn = ble_hs_conn_find(proc->conn_handle);
-    if (conn == NULL) {
-        return BLE_HS_ENOTCONN;
-    }
-
-    *out_type = conn->bhc_addr_type;
-    *out_addr = conn->bhc_addr;
-
-    return 0;
-}
-
-int
+void
 ble_sm_ia_ra(struct ble_sm_proc *proc,
              uint8_t *out_iat, uint8_t *out_ia,
              uint8_t *out_rat, uint8_t *out_ra)
@@ -519,10 +502,7 @@ ble_sm_ia_ra(struct ble_sm_proc *proc,
     struct ble_hs_conn_addrs addrs;
     struct ble_hs_conn *conn;
 
-    conn = ble_hs_conn_find(proc->conn_handle);
-    if (conn == NULL) {
-        return BLE_HS_ENOTCONN;
-    }
+    conn = ble_hs_conn_find_assert(proc->conn_handle);
 
     ble_hs_conn_addrs(conn, &addrs);
 
@@ -539,8 +519,6 @@ ble_sm_ia_ra(struct ble_sm_proc *proc,
         *out_rat = addrs.our_id_addr_type;
         memcpy(out_ra, addrs.our_ota_addr, 6);
     }
-
-    return 0;
 }
 
 static void
@@ -562,8 +540,8 @@ ble_sm_persist_keys(struct ble_sm_proc *proc)
         peer_addr_type = proc->peer_keys.addr_type;
         memcpy(peer_addr, proc->peer_keys.addr, sizeof peer_addr);
     } else {
-        peer_addr_type = ble_hs_misc_addr_type_to_id(conn->bhc_addr_type);
-        memcpy(peer_addr, conn->bhc_addr, sizeof peer_addr);
+        peer_addr_type = ble_hs_misc_addr_type_to_id(conn->bhc_peer_addr_type);
+        memcpy(peer_addr, conn->bhc_peer_addr, sizeof peer_addr);
     }
 
     ble_hs_unlock();
@@ -765,13 +743,16 @@ ble_sm_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, void *arg)
 {
     ble_sm_state_fn *cb;
 
-    BLE_HS_DBG_ASSERT(proc->state < BLE_SM_PROC_STATE_CNT);
-    cb = ble_sm_state_dispatch[proc->state];
-    BLE_HS_DBG_ASSERT(cb != NULL);
-
     memset(res, 0, sizeof *res);
 
-    cb(proc, res, arg);
+    if (!ble_hs_conn_exists(proc->conn_handle)) {
+        res->app_status = BLE_HS_ENOTCONN;
+    } else {
+        BLE_HS_DBG_ASSERT(proc->state < BLE_SM_PROC_STATE_CNT);
+        cb = ble_sm_state_dispatch[proc->state];
+        BLE_HS_DBG_ASSERT(cb != NULL);
+        cb(proc, res, arg);
+    }
 }
 
 void
@@ -824,9 +805,9 @@ ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res)
         }
 
         if (res->app_status == 0 &&
-            res->passkey_action.action != BLE_SM_IOACT_NONE) {
+            res->passkey_params.action != BLE_SM_IOACT_NONE) {
 
-            ble_gap_passkey_event(conn_handle, &res->passkey_action);
+            ble_gap_passkey_event(conn_handle, &res->passkey_params);
         }
 
         /* Persist keys if bonding has successfully completed. */
@@ -875,8 +856,8 @@ ble_sm_start_encrypt_tx(struct hci_start_encrypt *cmd)
     uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_LE_START_ENCRYPT_LEN];
     int rc;
 
-    host_hci_cmd_build_le_start_encrypt(cmd, buf, sizeof buf);
-    rc = ble_hci_cmd_tx_empty_ack(buf);
+    ble_hs_hci_cmd_build_le_start_encrypt(cmd, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx_empty_ack(buf);
     if (rc != 0) {
         return rc;
     }
@@ -1059,9 +1040,9 @@ ble_sm_ltk_req_reply_tx(uint16_t conn_handle, uint8_t *ltk)
     cmd.conn_handle = conn_handle;
     memcpy(cmd.long_term_key, ltk, 16);
 
-    host_hci_cmd_build_le_lt_key_req_reply(&cmd, buf, sizeof buf);
-    rc = ble_hci_cmd_tx(buf, &ack_conn_handle, sizeof ack_conn_handle,
-                        &ack_params_len);
+    ble_hs_hci_cmd_build_le_lt_key_req_reply(&cmd, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx(buf, &ack_conn_handle, sizeof ack_conn_handle,
+                           &ack_params_len);
     if (rc != 0) {
         return rc;
     }
@@ -1085,9 +1066,9 @@ ble_sm_ltk_req_neg_reply_tx(uint16_t conn_handle)
     uint8_t ack_params_len;
     int rc;
 
-    host_hci_cmd_build_le_lt_key_req_neg_reply(conn_handle, buf, sizeof buf);
-    rc = ble_hci_cmd_tx(buf, &ack_conn_handle, sizeof ack_conn_handle,
-                        &ack_params_len);
+    ble_hs_hci_cmd_build_le_lt_key_req_neg_reply(conn_handle, buf, sizeof buf);
+    rc = ble_hs_hci_cmd_tx(buf, &ack_conn_handle, sizeof ack_conn_handle,
+                           &ack_params_len);
     if (rc != 0) {
         return rc;
     }
@@ -1208,13 +1189,9 @@ ble_sm_ltk_req_rx(struct hci_le_lt_key_req *evt)
     }
 
     if (restore) {
-        conn = ble_hs_conn_find(evt->connection_handle);
-        if (conn == NULL) {
-            res.app_status = BLE_HS_ENOTCONN;
-        } else {
-            ble_hs_conn_addrs(conn, &addrs);
-            memcpy(peer_id_addr, addrs.peer_id_addr, 6);
-        }
+        conn = ble_hs_conn_find_assert(evt->connection_handle);
+        ble_hs_conn_addrs(conn, &addrs);
+        memcpy(peer_id_addr, addrs.peer_id_addr, 6);
     }
 
     ble_hs_unlock();
@@ -1285,7 +1262,7 @@ ble_sm_random_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *proc;
     struct ble_sm_proc *prev;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_PAIR_RANDOM_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_PAIR_RANDOM_SZ);
     if (res->app_status != 0) {
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
@@ -1335,7 +1312,7 @@ ble_sm_confirm_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *prev;
     uint8_t ioact;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_PAIR_CONFIRM_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_PAIR_CONFIRM_SZ);
     if (res->app_status != 0) {
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
@@ -1466,7 +1443,7 @@ ble_sm_pair_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
 
         ioact = ble_sm_io_action(proc);
         if (ble_sm_ioact_state(ioact) == proc->state) {
-            res->passkey_action.action = ioact;
+            res->passkey_params.action = ioact;
         }
     }
 
@@ -1496,7 +1473,7 @@ ble_sm_pair_req_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *prev;
     struct ble_hs_conn *conn;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_PAIR_CMD_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_PAIR_CMD_SZ);
     if (res->app_status != 0) {
         return;
     }
@@ -1526,11 +1503,8 @@ ble_sm_pair_req_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
 
         proc->pair_req = req;
 
-        conn = ble_hs_conn_find(proc->conn_handle);
-        if (conn == NULL) {
-            res->sm_err = BLE_SM_ERR_UNSPECIFIED;
-            res->app_status = BLE_HS_ENOTCONN;
-        } else if (conn->bhc_flags & BLE_HS_CONN_F_MASTER) {
+        conn = ble_hs_conn_find_assert(proc->conn_handle);
+        if (conn->bhc_flags & BLE_HS_CONN_F_MASTER) {
             res->sm_err = BLE_SM_ERR_CMD_NOT_SUPP;
             res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CMD_NOT_SUPP);
         } else if (!ble_sm_pair_cmd_is_valid(&req)) {
@@ -1553,7 +1527,7 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *prev;
     uint8_t ioact;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_PAIR_CMD_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_PAIR_CMD_SZ);
     if (res->app_status != 0) {
         res->enc_cb = 1;
         return;
@@ -1575,7 +1549,7 @@ ble_sm_pair_rsp_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
             proc->state = ble_sm_state_after_pair(proc);
             ioact = ble_sm_io_action(proc);
             if (ble_sm_ioact_state(ioact) == proc->state) {
-                res->passkey_action.action = ioact;
+                res->passkey_params.action = ioact;
             }
             if (ble_sm_proc_can_advance(proc)) {
                 res->execute = 1;
@@ -1616,7 +1590,7 @@ ble_sm_sec_req_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_hs_conn *conn;
     int authreq_mitm;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_SEC_REQ_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_SEC_REQ_SZ);
     if (res->app_status != 0) {
         return;
     }
@@ -1630,10 +1604,8 @@ ble_sm_sec_req_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
 
     ble_hs_lock();
 
-    conn = ble_hs_conn_find(conn_handle);
-    if (conn == NULL) {
-        res->app_status = BLE_HS_ENOTCONN;
-    } else if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) {
+    conn = ble_hs_conn_find_assert(conn_handle);
+    if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) {
         res->app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_CMD_NOT_SUPP);
         res->sm_err = BLE_SM_ERR_CMD_NOT_SUPP;
     } else {
@@ -1703,15 +1675,16 @@ ble_sm_key_exch_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
                      void *arg)
 {
     struct ble_sm_id_addr_info addr_info;
+    struct ble_hs_conn_addrs addrs;
     struct ble_sm_sign_info sign_info;
     struct ble_sm_master_id master_id;
     struct ble_sm_enc_info enc_info;
     struct ble_sm_id_info id_info;
+    struct ble_hs_conn *conn;
     uint8_t init_key_dist;
     uint8_t resp_key_dist;
     uint8_t our_key_dist;
-    uint8_t *our_id_addr;
-    uint8_t *irk;
+    const uint8_t *irk;
     int rc;
 
     ble_sm_key_dist(proc, &init_key_dist, &resp_key_dist);
@@ -1754,7 +1727,10 @@ ble_sm_key_exch_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
 
     if (our_key_dist & BLE_SM_PAIR_KEY_DIST_ID) {
         /* Send identity information. */
-        irk = ble_hs_pvcy_our_irk();
+        rc = ble_hs_pvcy_our_irk(&irk);
+        if (rc != 0) {
+            goto err;
+        }
 
         memcpy(id_info.irk, irk, 16);
 
@@ -1765,8 +1741,11 @@ ble_sm_key_exch_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
         proc->our_keys.irk_valid = 1;
 
         /* Send identity address information. */
-        our_id_addr = ble_hs_pvcy_our_id_addr(&addr_info.addr_type);
-        memcpy(addr_info.bd_addr, our_id_addr, 6);
+        conn = ble_hs_conn_find_assert(proc->conn_handle);
+
+        ble_hs_conn_addrs(conn, &addrs);
+        addr_info.addr_type = addrs.our_id_addr_type;
+        memcpy(addr_info.bd_addr, addrs.our_id_addr, 6);
         rc = ble_sm_id_addr_info_tx(proc->conn_handle, &addr_info);
         if (rc != 0) {
             goto err;
@@ -1830,7 +1809,7 @@ ble_sm_enc_info_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *proc;
     struct ble_sm_proc *prev;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_ENC_INFO_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_ENC_INFO_SZ);
     if (res->app_status != 0) {
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
@@ -1866,7 +1845,7 @@ ble_sm_master_id_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *proc;
     struct ble_sm_proc *prev;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_MASTER_ID_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_MASTER_ID_SZ);
     if (res->app_status != 0) {
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
@@ -1903,7 +1882,7 @@ ble_sm_id_info_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *proc;
     struct ble_sm_proc *prev;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_ID_INFO_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_ID_INFO_SZ);
     if (res->app_status != 0) {
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
@@ -1940,7 +1919,7 @@ ble_sm_id_addr_info_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *proc;
     struct ble_sm_proc *prev;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_ID_ADDR_INFO_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_ID_ADDR_INFO_SZ);
     if (res->app_status != 0) {
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
@@ -1978,7 +1957,7 @@ ble_sm_sign_info_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *proc;
     struct ble_sm_proc *prev;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_SIGN_INFO_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_SIGN_INFO_SZ);
     if (res->app_status != 0) {
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
@@ -2019,12 +1998,12 @@ ble_sm_fail_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
 
     res->enc_cb = 1;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_PAIR_FAIL_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_PAIR_FAIL_SZ);
     if (res->app_status == 0) {
         ble_sm_pair_fail_parse((*om)->om_data, (*om)->om_len, &cmd);
         BLE_SM_LOG_CMD(0, "fail", conn_handle, ble_sm_pair_fail_log, &cmd);
 
-        res->app_status = BLE_HS_SM_THEM_ERR(cmd.reason);
+        res->app_status = BLE_HS_SM_PEER_ERR(cmd.reason);
     }
 }
 
@@ -2044,7 +2023,7 @@ ble_sm_fail_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
  *                                  be called again; currently always
  *                                  UINT32_MAX.
  */
-uint32_t
+int32_t
 ble_sm_heartbeat(void)
 {
     struct ble_sm_proc_list exp_list;
@@ -2067,7 +2046,7 @@ ble_sm_heartbeat(void)
         ble_sm_proc_free(proc);
     }
 
-    return UINT32_MAX;
+    return BLE_HS_FOREVER;
 }
 
 /**
@@ -2153,7 +2132,7 @@ ble_sm_slave_initiate(uint16_t conn_handle)
  * Initiates the encryption procedure for the specified connection.
  */
 int
-ble_sm_enc_initiate(uint16_t conn_handle, uint8_t *ltk, uint16_t ediv,
+ble_sm_enc_initiate(uint16_t conn_handle, const uint8_t *ltk, uint16_t ediv,
                     uint64_t rand_val, int auth)
 {
     struct ble_sm_result res;
@@ -2281,17 +2260,12 @@ ble_sm_inject_io(uint16_t conn_handle, struct ble_sm_io *pkey)
 
         switch (pkey->action) {
         case BLE_SM_IOACT_OOB:
-            if (pkey->oob == NULL) {
-                res.app_status = BLE_HS_SM_US_ERR(BLE_SM_ERR_OOB);
-                res.sm_err = BLE_SM_ERR_OOB;
-            } else {
-                proc->flags |= BLE_SM_PROC_F_IO_INJECTED;
-                memcpy(proc->tk, pkey->oob, 16);
-                if ((proc->flags & BLE_SM_PROC_F_INITIATOR) ||
-                    (proc->flags & BLE_SM_PROC_F_ADVANCE_ON_IO)) {
+            proc->flags |= BLE_SM_PROC_F_IO_INJECTED;
+            memcpy(proc->tk, pkey->oob, 16);
+            if ((proc->flags & BLE_SM_PROC_F_INITIATOR) ||
+                (proc->flags & BLE_SM_PROC_F_ADVANCE_ON_IO)) {
 
-                    res.execute = 1;
-                }
+                res.execute = 1;
             }
             break;
 
diff --git a/net/nimble/host/src/ble_sm_alg.c b/net/nimble/host/src/ble_sm_alg.c
index 1885d098..66f86dc6 100644
--- a/net/nimble/host/src/ble_sm_alg.c
+++ b/net/nimble/host/src/ble_sm_alg.c
@@ -40,39 +40,11 @@ static const uint32_t ble_sm_alg_dbg_priv_key[8] = {
     0xa3c55f38, 0x3f49f6d4
 };
 
-static const uint8_t ble_sm_alg_dbg_pub_key[64] = {
-    0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc,
-    0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef,
-    0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e,
-    0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20,
-    0x8b, 0xd2, 0x89, 0x15, 0xd0, 0x8e, 0x1c, 0x74,
-    0x24, 0x30, 0xed, 0x8f, 0xc2, 0x45, 0x63, 0x76,
-    0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63,
-    0x6d, 0xeb, 0x2a, 0x65, 0x49, 0x9c, 0x80, 0xdc
-};
-
-static const uint8_t ble_sm_alg_dbg_f4[16] = {
-    0x2d, 0x87, 0x74, 0xa9, 0xbe, 0xa1, 0xed, 0xf1,
-    0x1c, 0xbd, 0xa9, 0x07, 0xf1, 0x16, 0xc9, 0xf2,
-};
-
-static const uint8_t ble_sm_alg_dbg_f5[32] = {
-    0x20, 0x6e, 0x63, 0xce, 0x20, 0x6a, 0x3f, 0xfd,
-    0x02, 0x4a, 0x08, 0xa1, 0x76, 0xf1, 0x65, 0x29,
-    0x38, 0x0a, 0x75, 0x94, 0xb5, 0x22, 0x05, 0x98,
-    0x23, 0xcd, 0xd7, 0x69, 0x11, 0x79, 0x86, 0x69,
-};
-
-static const uint8_t ble_sm_alg_dbg_f6[16] = {
-    0x61, 0x8f, 0x95, 0xda, 0x09, 0x0b, 0x6c, 0xd2,
-    0xc5, 0xe8, 0xd0, 0x9c, 0x98, 0x73, 0xc4, 0xe3,
-};
-
 static void
-ble_sm_alg_log_buf(char *name, uint8_t *buf, int len)
+ble_sm_alg_log_buf(const char *name, const uint8_t *buf, int len)
 {
     BLE_HS_LOG(DEBUG, "    %s=", name);
-    ble_hs_misc_log_flat_buf(buf, len);
+    ble_hs_log_flat_buf(buf, len);
     BLE_HS_LOG(DEBUG, "\n");
 }
 
@@ -166,13 +138,13 @@ ble_sm_alg_s1(uint8_t *k, uint8_t *r1, uint8_t *r2, uint8_t *out)
     }
 
     BLE_HS_LOG(DEBUG, "ble_sm_alg_s1()\n    k=");
-    ble_hs_misc_log_flat_buf(k, 16);
+    ble_hs_log_flat_buf(k, 16);
     BLE_HS_LOG(DEBUG, "\n    r1=");
-    ble_hs_misc_log_flat_buf(r1, 16);
+    ble_hs_log_flat_buf(r1, 16);
     BLE_HS_LOG(DEBUG, "\n    r2=");
-    ble_hs_misc_log_flat_buf(r2, 16);
+    ble_hs_log_flat_buf(r2, 16);
     BLE_HS_LOG(DEBUG, "\n    out=");
-    ble_hs_misc_log_flat_buf(out, 16);
+    ble_hs_log_flat_buf(out, 16);
     BLE_HS_LOG(DEBUG, "\n");
 
     return 0;
@@ -189,18 +161,18 @@ ble_sm_alg_c1(uint8_t *k, uint8_t *r,
     int rc;
 
     BLE_HS_LOG(DEBUG, "ble_sm_alg_c1()\n    k=");
-    ble_hs_misc_log_flat_buf(k, 16);
+    ble_hs_log_flat_buf(k, 16);
     BLE_HS_LOG(DEBUG, "\n    r=");
-    ble_hs_misc_log_flat_buf(r, 16);
+    ble_hs_log_flat_buf(r, 16);
     BLE_HS_LOG(DEBUG, "\n    iat=%d rat=%d", iat, rat);
     BLE_HS_LOG(DEBUG, "\n    ia=");
-    ble_hs_misc_log_flat_buf(ia, 6);
+    ble_hs_log_flat_buf(ia, 6);
     BLE_HS_LOG(DEBUG, "\n    ra=");
-    ble_hs_misc_log_flat_buf(ra, 6);
+    ble_hs_log_flat_buf(ra, 6);
     BLE_HS_LOG(DEBUG, "\n    preq=");
-    ble_hs_misc_log_flat_buf(preq, 7);
+    ble_hs_log_flat_buf(preq, 7);
     BLE_HS_LOG(DEBUG, "\n    pres=");
-    ble_hs_misc_log_flat_buf(pres, 7);
+    ble_hs_log_flat_buf(pres, 7);
 
     /* pres, preq, rat and iat are concatenated to generate p1 */
     p1[0] = iat;
@@ -209,7 +181,7 @@ ble_sm_alg_c1(uint8_t *k, uint8_t *r,
     memcpy(p1 + 9, pres, 7);
 
     BLE_HS_LOG(DEBUG, "\n    p1=");
-    ble_hs_misc_log_flat_buf(p1, sizeof p1);
+    ble_hs_log_flat_buf(p1, sizeof p1);
 
     /* c1 = e(k, e(k, r XOR p1) XOR p2) */
 
@@ -228,7 +200,7 @@ ble_sm_alg_c1(uint8_t *k, uint8_t *r,
     memset(p2 + 12, 0, 4);
 
     BLE_HS_LOG(DEBUG, "\n    p2=");
-    ble_hs_misc_log_flat_buf(p2, sizeof p2);
+    ble_hs_log_flat_buf(p2, sizeof p2);
 
     ble_sm_alg_xor_128(out_enc_data, p2, out_enc_data);
 
@@ -239,7 +211,7 @@ ble_sm_alg_c1(uint8_t *k, uint8_t *r,
     }
 
     BLE_HS_LOG(DEBUG, "\n    out_enc_data=");
-    ble_hs_misc_log_flat_buf(out_enc_data, 16);
+    ble_hs_log_flat_buf(out_enc_data, 16);
 
     rc = 0;
 
@@ -257,11 +229,11 @@ ble_sm_alg_f4(uint8_t *u, uint8_t *v, uint8_t *x, uint8_t z,
     int rc;
 
     BLE_HS_LOG(DEBUG, "ble_sm_alg_f4()\n    u=");
-    ble_hs_misc_log_flat_buf(u, 32);
+    ble_hs_log_flat_buf(u, 32);
     BLE_HS_LOG(DEBUG, "\n    v=");
-    ble_hs_misc_log_flat_buf(v, 32);
+    ble_hs_log_flat_buf(v, 32);
     BLE_HS_LOG(DEBUG, "\n    x=");
-    ble_hs_misc_log_flat_buf(x, 16);
+    ble_hs_log_flat_buf(x, 16);
     BLE_HS_LOG(DEBUG, "\n    z=0x%02x\n", z);
 
     /*
@@ -287,7 +259,7 @@ ble_sm_alg_f4(uint8_t *u, uint8_t *v, uint8_t *x, uint8_t z,
     swap_in_place(out_enc_data, 16);
 
     BLE_HS_LOG(DEBUG, "    out_enc_data=");
-    ble_hs_misc_log_flat_buf(out_enc_data, 16);
+    ble_hs_log_flat_buf(out_enc_data, 16);
     BLE_HS_LOG(DEBUG, "\n");
 
     return 0;
@@ -362,9 +334,10 @@ ble_sm_alg_f5(uint8_t *w, uint8_t *n1, uint8_t *n2, uint8_t a1t,
 }
 
 int
-ble_sm_alg_f6(uint8_t *w, uint8_t *n1, uint8_t *n2, uint8_t *r,
-              uint8_t *iocap, uint8_t a1t, uint8_t *a1,
-              uint8_t a2t, uint8_t *a2, uint8_t *check)
+ble_sm_alg_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2,
+              const uint8_t *r, const uint8_t *iocap, uint8_t a1t,
+              const uint8_t *a1, uint8_t a2t, const uint8_t *a2,
+              uint8_t *check)
 {
     uint8_t ws[16];
     uint8_t m[65];
@@ -409,7 +382,8 @@ ble_sm_alg_f6(uint8_t *w, uint8_t *n1, uint8_t *n2, uint8_t *r,
 }
 
 int
-ble_sm_alg_g2(uint8_t *u, uint8_t *v, uint8_t *x, uint8_t *y, uint32_t *passkey)
+ble_sm_alg_g2(uint8_t *u, uint8_t *v, uint8_t *x, uint8_t *y,
+              uint32_t *passkey)
 {
     uint8_t m[80], xs[16];
     int rc;
@@ -475,7 +449,7 @@ ble_sm_alg_gen_key_pair(void *pub, uint32_t *priv)
     int rc;
 
     do {
-        rc = ble_hci_util_rand(random, sizeof random);
+        rc = ble_hs_hci_util_rand(random, sizeof random);
         if (rc != 0) {
             return rc;
         }
diff --git a/net/nimble/host/src/ble_sm_cmd.c b/net/nimble/host/src/ble_sm_cmd.c
index 3d506b24..02831374 100644
--- a/net/nimble/host/src/ble_sm_cmd.c
+++ b/net/nimble/host/src/ble_sm_cmd.c
@@ -38,13 +38,14 @@ ble_sm_tx(uint16_t conn_handle, struct os_mbuf *txom)
 
     STATS_INC(ble_l2cap_stats, sm_tx);
 
-    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SM,
-                                         &conn, &chan);
-    if (rc == 0) {
-        rc = ble_l2cap_tx(conn, chan, txom);
+    ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SM,
+                                    &conn, &chan);
+    rc = ble_l2cap_tx(conn, chan, txom);
+    if (rc != 0) {
+        return rc;
     }
 
-    return rc;
+    return 0;
 }
 
 static int
@@ -53,7 +54,7 @@ ble_sm_init_req(uint16_t initial_sz, struct os_mbuf **out_txom)
     void *buf;
     int rc;
 
-    *out_txom = ble_hs_misc_pkthdr();
+    *out_txom = ble_hs_mbuf_l2cap_pkt();
     if (*out_txom == NULL) {
         rc = BLE_HS_ENOMEM;
         goto err;
@@ -148,8 +149,7 @@ ble_sm_pair_cmd_tx(uint16_t conn_handle, int is_req,
 
     rc = ble_sm_init_req(BLE_SM_PAIR_CMD_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     ble_sm_pair_cmd_write(txom->om_data, txom->om_len, is_req, cmd);
@@ -158,11 +158,11 @@ ble_sm_pair_cmd_tx(uint16_t conn_handle, int is_req,
     BLE_HS_DBG_ASSERT(ble_sm_pair_cmd_is_valid(cmd));
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
@@ -204,29 +204,27 @@ ble_sm_pair_confirm_tx(uint16_t conn_handle, struct ble_sm_pair_confirm *cmd)
     struct os_mbuf *txom;
     int rc;
 
-
     rc = ble_sm_init_req(BLE_SM_PAIR_CONFIRM_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     ble_sm_pair_confirm_write(txom->om_data, txom->om_len, cmd);
     BLE_SM_LOG_CMD(1, "confirm", conn_handle, ble_sm_pair_confirm_log, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
 ble_sm_pair_confirm_log(struct ble_sm_pair_confirm *cmd)
 {
     BLE_HS_LOG(DEBUG, "value=");
-    ble_hs_misc_log_flat_buf(cmd->value, sizeof cmd->value);
+    ble_hs_log_flat_buf(cmd->value, sizeof cmd->value);
 }
 
 void
@@ -260,26 +258,25 @@ ble_sm_pair_random_tx(uint16_t conn_handle, struct ble_sm_pair_random *cmd)
 
     rc = ble_sm_init_req(BLE_SM_PAIR_RANDOM_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     ble_sm_pair_random_write(txom->om_data, txom->om_len, cmd);
     BLE_SM_LOG_CMD(1, "random", conn_handle, ble_sm_pair_random_log, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
 ble_sm_pair_random_log(struct ble_sm_pair_random *cmd)
 {
     BLE_HS_LOG(DEBUG, "value=");
-    ble_hs_misc_log_flat_buf(cmd->value, sizeof cmd->value);
+    ble_hs_log_flat_buf(cmd->value, sizeof cmd->value);
 }
 
 void
@@ -318,8 +315,7 @@ ble_sm_pair_fail_tx(uint16_t conn_handle, uint8_t reason)
 
     rc = ble_sm_init_req(BLE_SM_PAIR_FAIL_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     cmd.reason = reason;
@@ -328,11 +324,11 @@ ble_sm_pair_fail_tx(uint16_t conn_handle, uint8_t reason)
     BLE_SM_LOG_CMD(1, "fail", conn_handle, ble_sm_pair_fail_log, &cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
@@ -368,8 +364,7 @@ ble_sm_enc_info_tx(uint16_t conn_handle, struct ble_sm_enc_info *cmd)
 
     rc = ble_sm_init_req(BLE_SM_ENC_INFO_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     ble_sm_enc_info_write(txom->om_data, txom->om_len, cmd);
@@ -377,18 +372,18 @@ ble_sm_enc_info_tx(uint16_t conn_handle, struct ble_sm_enc_info *cmd)
     BLE_SM_LOG_CMD(1, "enc info", conn_handle, ble_sm_enc_info_log, cmd);
     
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
 ble_sm_enc_info_log(struct ble_sm_enc_info *cmd)
 {
     BLE_HS_LOG(DEBUG, "ltk=");
-    ble_hs_misc_log_flat_buf(cmd->ltk, sizeof cmd->ltk);
+    ble_hs_log_flat_buf(cmd->ltk, sizeof cmd->ltk);
 }
 
 void
@@ -424,8 +419,7 @@ ble_sm_master_id_tx(uint16_t conn_handle, struct ble_sm_master_id *cmd)
 
     rc = ble_sm_init_req(BLE_SM_MASTER_ID_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     txom->om_data[0] = BLE_SM_OP_MASTER_ID;
@@ -435,11 +429,11 @@ ble_sm_master_id_tx(uint16_t conn_handle, struct ble_sm_master_id *cmd)
     BLE_SM_LOG_CMD(1, "master id", conn_handle, ble_sm_master_id_log, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
@@ -481,25 +475,24 @@ ble_sm_id_info_tx(uint16_t conn_handle, struct ble_sm_id_info *cmd)
 
     rc = ble_sm_init_req(BLE_SM_ID_INFO_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     ble_sm_id_info_write(txom->om_data, txom->om_len, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
 ble_sm_id_info_log(struct ble_sm_id_info *cmd)
 {
     BLE_HS_LOG(DEBUG, "irk=");
-    ble_hs_misc_log_flat_buf(cmd->irk, sizeof cmd->irk);
+    ble_hs_log_flat_buf(cmd->irk, sizeof cmd->irk);
 }
 
 void
@@ -537,18 +530,17 @@ ble_sm_id_addr_info_tx(uint16_t conn_handle, struct ble_sm_id_addr_info *cmd)
 
     rc = ble_sm_init_req(BLE_SM_ID_ADDR_INFO_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     ble_sm_id_addr_info_write(txom->om_data, txom->om_len, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
@@ -587,25 +579,24 @@ ble_sm_sign_info_tx(uint16_t conn_handle, struct ble_sm_sign_info *cmd)
 
     rc = ble_sm_init_req(BLE_SM_SIGN_INFO_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     ble_sm_sign_info_write(txom->om_data, txom->om_len, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
 ble_sm_sign_info_log(struct ble_sm_sign_info *cmd)
 {
     BLE_HS_LOG(DEBUG, "sig_key=");
-    ble_hs_misc_log_flat_buf(cmd->sig_key, sizeof cmd->sig_key);
+    ble_hs_log_flat_buf(cmd->sig_key, sizeof cmd->sig_key);
 }
 
 void
@@ -640,8 +631,7 @@ ble_sm_sec_req_tx(uint16_t conn_handle, struct ble_sm_sec_req *cmd)
 
     rc = ble_sm_init_req(BLE_SM_SEC_REQ_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     ble_sm_sec_req_write(txom->om_data, txom->om_len, cmd);
@@ -649,11 +639,11 @@ ble_sm_sec_req_tx(uint16_t conn_handle, struct ble_sm_sec_req *cmd)
     BLE_SM_LOG_CMD(1, "sec req", conn_handle, ble_sm_sec_req_log, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
@@ -704,8 +694,7 @@ ble_sm_public_key_tx(uint16_t conn_handle, struct ble_sm_public_key *cmd)
 
     rc = ble_sm_init_req(BLE_SM_PUBLIC_KEY_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     rc = ble_sm_public_key_write(txom->om_data, txom->om_len, cmd);
@@ -714,20 +703,20 @@ ble_sm_public_key_tx(uint16_t conn_handle, struct ble_sm_public_key *cmd)
     BLE_SM_LOG_CMD(1, "public key", conn_handle, ble_sm_public_key_log, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
 ble_sm_public_key_log(struct ble_sm_public_key *cmd)
 {
     BLE_HS_LOG(DEBUG, "x=");
-    ble_hs_misc_log_flat_buf(cmd->x, sizeof cmd->x);
+    ble_hs_log_flat_buf(cmd->x, sizeof cmd->x);
     BLE_HS_LOG(DEBUG, "y=");
-    ble_hs_misc_log_flat_buf(cmd->y, sizeof cmd->y);
+    ble_hs_log_flat_buf(cmd->y, sizeof cmd->y);
 }
 
 void
@@ -765,8 +754,7 @@ ble_sm_dhkey_check_tx(uint16_t conn_handle, struct ble_sm_dhkey_check *cmd)
 
     rc = ble_sm_init_req(BLE_SM_DHKEY_CHECK_SZ, &txom);
     if (rc != 0) {
-        rc = BLE_HS_ENOMEM;
-        goto done;
+        return BLE_HS_ENOMEM;
     }
 
     rc = ble_sm_dhkey_check_write(txom->om_data, txom->om_len, cmd);
@@ -775,18 +763,18 @@ ble_sm_dhkey_check_tx(uint16_t conn_handle, struct ble_sm_dhkey_check *cmd)
     BLE_SM_LOG_CMD(1, "dhkey check", conn_handle, ble_sm_dhkey_check_log, cmd);
 
     rc = ble_sm_tx(conn_handle, txom);
-    txom = NULL;
+    if (rc != 0) {
+        return rc;
+    }
 
-done:
-    os_mbuf_free_chain(txom);
-    return rc;
+    return 0;
 }
 
 void
 ble_sm_dhkey_check_log(struct ble_sm_dhkey_check *cmd)
 {
     BLE_HS_LOG(DEBUG, "value=");
-    ble_hs_misc_log_flat_buf(cmd->value, sizeof cmd->value);
+    ble_hs_log_flat_buf(cmd->value, sizeof cmd->value);
 }
 
 #endif
diff --git a/net/nimble/host/src/ble_sm_lgcy.c b/net/nimble/host/src/ble_sm_lgcy.c
index d844a6f1..7b1673c7 100644
--- a/net/nimble/host/src/ble_sm_lgcy.c
+++ b/net/nimble/host/src/ble_sm_lgcy.c
@@ -109,12 +109,7 @@ ble_sm_lgcy_confirm_prepare_args(struct ble_sm_proc *proc,
                                  uint8_t *iat, uint8_t *rat,
                                  uint8_t *ia, uint8_t *ra)
 {
-    int rc;
-
-    rc = ble_sm_ia_ra(proc, iat, ia, rat, ra);
-    if (rc != 0) {
-        return rc;
-    }
+    ble_sm_ia_ra(proc, iat, ia, rat, ra);
 
     memcpy(k, proc->tk, sizeof proc->tk);
 
diff --git a/net/nimble/host/src/ble_sm_priv.h b/net/nimble/host/src/ble_sm_priv.h
index 2543514f..d15afbc2 100644
--- a/net/nimble/host/src/ble_sm_priv.h
+++ b/net/nimble/host/src/ble_sm_priv.h
@@ -276,7 +276,7 @@ struct ble_sm_proc {
 struct ble_sm_result {
     int app_status;
     uint8_t sm_err;
-    struct ble_gap_passkey_action passkey_action;
+    struct ble_gap_passkey_params passkey_params;
     void *state_arg;
     unsigned execute:1;
     unsigned enc_cb:1;
@@ -386,9 +386,10 @@ int ble_sm_alg_g2(uint8_t *u, uint8_t *v, uint8_t *x, uint8_t *y,
 int ble_sm_alg_f5(uint8_t *w, uint8_t *n1, uint8_t *n2, uint8_t a1t,
                   uint8_t *a1, uint8_t a2t, uint8_t *a2,
                   uint8_t *mackey, uint8_t *ltk);
-int ble_sm_alg_f6(uint8_t *w, uint8_t *n1, uint8_t *n2, uint8_t *r,
-                  uint8_t *iocap, uint8_t a1t, uint8_t *a1,
-                  uint8_t a2t, uint8_t *a2, uint8_t *check);
+int ble_sm_alg_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2,
+                  const uint8_t *r, const uint8_t *iocap, uint8_t a1t,
+                  const uint8_t *a1, uint8_t a2t, const uint8_t *a2,
+                  uint8_t *check);
 int ble_sm_alg_gen_dhkey(uint8_t *peer_pub_key_x, uint8_t *peer_pub_key_y,
                          uint32_t *our_priv_key, void *out_dhkey);
 int ble_sm_alg_gen_key_pair(void *pub, uint32_t *priv);
@@ -416,11 +417,12 @@ void ble_sm_sc_public_key_exec(struct ble_sm_proc *proc,
                                struct ble_sm_result *res,
                                void *arg);
 void ble_sm_sc_public_key_rx(uint16_t conn_handle, uint8_t op,
-                             struct os_mbuf **om, struct ble_sm_result *res);
+                             struct os_mbuf **rxom, struct ble_sm_result *res);
 void ble_sm_sc_dhkey_check_exec(struct ble_sm_proc *proc,
                                 struct ble_sm_result *res, void *arg);
 void ble_sm_sc_dhkey_check_rx(uint16_t conn_handle, uint8_t op,
-                              struct os_mbuf **om, struct ble_sm_result *res);
+                              struct os_mbuf **rxom,
+                              struct ble_sm_result *res);
 void ble_sm_sc_init(void);
 #else
 #define ble_sm_sc_io_action(proc) (BLE_SM_IOACT_NONE)
@@ -446,17 +448,15 @@ int ble_sm_ioact_state(uint8_t action);
 int ble_sm_proc_can_advance(struct ble_sm_proc *proc);
 void ble_sm_process_result(uint16_t conn_handle, struct ble_sm_result *res);
 void ble_sm_confirm_advance(struct ble_sm_proc *proc);
-int ble_sm_peer_addr(struct ble_sm_proc *proc,
-                     uint8_t *out_type, uint8_t **out_addr);
-int ble_sm_ia_ra(struct ble_sm_proc *proc,
-                 uint8_t *out_iat, uint8_t *out_ia,
-                 uint8_t *out_rat, uint8_t *out_ra);
+void ble_sm_ia_ra(struct ble_sm_proc *proc,
+                  uint8_t *out_iat, uint8_t *out_ia,
+                  uint8_t *out_rat, uint8_t *out_ra);
 
-uint32_t ble_sm_heartbeat(void);
+int32_t ble_sm_heartbeat(void);
 void ble_sm_connection_broken(uint16_t conn_handle);
 int ble_sm_pair_initiate(uint16_t conn_handle);
 int ble_sm_slave_initiate(uint16_t conn_handle);
-int ble_sm_enc_initiate(uint16_t conn_handle, uint8_t *ltk,
+int ble_sm_enc_initiate(uint16_t conn_handle, const uint8_t *ltk,
                         uint16_t ediv, uint64_t rand_val, int auth);
 int ble_sm_init(void);
 
@@ -471,7 +471,7 @@ int ble_sm_init(void);
 #define ble_sm_ltk_req_rx(evt) ((void)(evt))
 #define ble_sm_enc_key_refresh_rx(evt) ((void)(evt))
 
-#define ble_sm_heartbeat() UINT32_MAX
+#define ble_sm_heartbeat() BLE_HS_FOREVER
 #define ble_sm_connection_broken(conn_handle)
 #define ble_sm_pair_initiate(conn_handle)   BLE_HS_ENOTSUP
 #define ble_sm_slave_initiate(conn_handle)  BLE_HS_ENOTSUP
diff --git a/net/nimble/host/src/ble_sm_sc.c b/net/nimble/host/src/ble_sm_sc.c
index fc568044..e82ba72f 100644
--- a/net/nimble/host/src/ble_sm_sc.c
+++ b/net/nimble/host/src/ble_sm_sc.c
@@ -153,10 +153,10 @@ ble_sm_sc_ensure_keys_generated(void)
     }
 
     BLE_HS_LOG(DEBUG, "our pubkey=");
-    ble_hs_misc_log_flat_buf(&ble_sm_sc_pub_key, 64);
+    ble_hs_log_flat_buf(&ble_sm_sc_pub_key, 64);
     BLE_HS_LOG(DEBUG, "\n");
     BLE_HS_LOG(DEBUG, "our privkey=");
-    ble_hs_misc_log_flat_buf(&ble_sm_sc_priv_key, 32);
+    ble_hs_log_flat_buf(&ble_sm_sc_priv_key, 32);
     BLE_HS_LOG(DEBUG, "\n");
 
     return 0;
@@ -221,7 +221,7 @@ ble_sm_sc_gen_ri(struct ble_sm_proc *proc)
         return 0;
 
     case BLE_SM_PAIR_ALG_OOB:
-        rc = ble_hci_util_rand(&proc->ri, 1);
+        rc = ble_hs_hci_util_rand(&proc->ri, 1);
         return rc;
 
     default:
@@ -280,7 +280,7 @@ ble_sm_sc_gen_numcmp(struct ble_sm_proc *proc, struct ble_sm_result *res)
         pkb = ble_sm_sc_pub_key.u8;
     }
     res->app_status = ble_sm_alg_g2(pka, pkb, proc->randm, proc->rands,
-                                    &res->passkey_action.numcmp);
+                                    &res->passkey_params.numcmp);
     if (res->app_status != 0) {
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
         res->enc_cb = 1;
@@ -341,7 +341,7 @@ ble_sm_sc_random_exec(struct ble_sm_proc *proc, struct ble_sm_result *res)
         if (ble_sm_ioact_state(ioact) == proc->state &&
             !(proc->flags & BLE_SM_PROC_F_IO_INJECTED)) {
 
-            res->passkey_action.action = ioact;
+            res->passkey_params.action = ioact;
             BLE_HS_DBG_ASSERT(ioact == BLE_SM_IOACT_NUMCMP);
             ble_sm_sc_gen_numcmp(proc, res);
         }
@@ -363,7 +363,7 @@ ble_sm_sc_random_rx(struct ble_sm_proc *proc, struct ble_sm_result *res)
         ble_sm_sc_responder_verifies_random(proc)) {
 
         BLE_HS_LOG(DEBUG, "tk=");
-        ble_hs_misc_log_flat_buf(proc->tk, 32);
+        ble_hs_log_flat_buf(proc->tk, 32);
         BLE_HS_LOG(DEBUG, "\n");
 
         rc = ble_sm_alg_f4(proc->pub_key_peer.x, ble_sm_sc_pub_key.u8,
@@ -386,14 +386,7 @@ ble_sm_sc_random_rx(struct ble_sm_proc *proc, struct ble_sm_result *res)
     }
 
     /* Calculate the mac key and ltk. */
-    rc = ble_sm_ia_ra(proc, &iat, ia, &rat, ra);
-    if (rc != 0) {
-        res->app_status = rc;
-        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
-        res->enc_cb = 1;
-        return;
-    }
-
+    ble_sm_ia_ra(proc, &iat, ia, &rat, ra);
     rc = ble_sm_alg_f5(proc->dhkey, proc->randm, proc->rands,
                        iat, ia, rat, ra, proc->mackey, proc->ltk);
     if (rc != 0) {
@@ -423,7 +416,7 @@ ble_sm_sc_random_rx(struct ble_sm_proc *proc, struct ble_sm_result *res)
         if (ble_sm_ioact_state(ioact) == proc->state &&
             !(proc->flags & BLE_SM_PROC_F_IO_INJECTED)) {
 
-            res->passkey_action.action = ioact;
+            res->passkey_params.action = ioact;
             BLE_HS_DBG_ASSERT(ioact == BLE_SM_IOACT_NUMCMP);
             ble_sm_sc_gen_numcmp(proc, res);
         } else {
@@ -457,14 +450,14 @@ ble_sm_sc_public_key_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
         return;
     }
 
-    ioact = ble_sm_sc_io_action(proc);
-    if (ble_sm_ioact_state(ioact) == BLE_SM_PROC_STATE_CONFIRM) {
-        res->passkey_action.action = ioact;
-    }
-
     if (!(proc->flags & BLE_SM_PROC_F_INITIATOR)) {
         proc->state = BLE_SM_PROC_STATE_CONFIRM;
 
+        ioact = ble_sm_sc_io_action(proc);
+        if (ble_sm_ioact_state(ioact) == proc->state) {
+            res->passkey_params.action = ioact;
+        }
+
         if (ble_sm_proc_can_advance(proc) &&
             !ble_sm_sc_initiator_txes_confirm(proc)) {
 
@@ -480,9 +473,10 @@ ble_sm_sc_public_key_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_public_key cmd;
     struct ble_sm_proc *proc;
     struct ble_sm_proc *prev;
+    uint8_t ioact;
     int rc;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_PUBLIC_KEY_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_PUBLIC_KEY_SZ);
     if (res->app_status != 0) {
         res->enc_cb = 1;
         return;
@@ -518,6 +512,11 @@ ble_sm_sc_public_key_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
             if (proc->flags & BLE_SM_PROC_F_INITIATOR) {
                 proc->state = BLE_SM_PROC_STATE_CONFIRM;
 
+                ioact = ble_sm_sc_io_action(proc);
+                if (ble_sm_ioact_state(ioact) == proc->state) {
+                    res->passkey_params.action = ioact;
+                }
+
                 if (ble_sm_proc_can_advance(proc) &&
                     ble_sm_sc_initiator_txes_confirm(proc)) {
 
@@ -531,28 +530,23 @@ ble_sm_sc_public_key_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     ble_hs_unlock();
 }
 
-static int
+static void
 ble_sm_sc_dhkey_addrs(struct ble_sm_proc *proc,
                       uint8_t *out_our_id_addr_type,
-                      uint8_t **out_our_ota_addr,
+                      const uint8_t **out_our_ota_addr,
                       uint8_t *out_peer_id_addr_type,
-                      uint8_t **out_peer_ota_addr)
+                      const uint8_t **out_peer_ota_addr)
 {
     struct ble_hs_conn_addrs addrs;
     struct ble_hs_conn *conn;
 
-    conn = ble_hs_conn_find(proc->conn_handle);
-    if (conn == NULL) {
-        return BLE_HS_ENOTCONN;
-    }
+    conn = ble_hs_conn_find_assert(proc->conn_handle);
 
     ble_hs_conn_addrs(conn, &addrs);
     *out_our_id_addr_type = addrs.our_id_addr_type;
     *out_our_ota_addr = addrs.our_ota_addr;
     *out_peer_id_addr_type = addrs.peer_id_addr_type;
     *out_peer_ota_addr = addrs.peer_ota_addr;
-
-    return 0;
 }
 
 static void
@@ -569,8 +563,8 @@ ble_sm_sc_dhkey_check_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
                            void *arg)
 {
     struct ble_sm_dhkey_check cmd;
-    uint8_t *our_ota_addr;
-    uint8_t *peer_ota_addr;
+    const uint8_t *our_ota_addr;
+    const uint8_t *peer_ota_addr;
     uint8_t peer_id_addr_type;
     uint8_t our_id_addr_type;
     uint8_t iocap[3];
@@ -582,12 +576,9 @@ ble_sm_sc_dhkey_check_exec(struct ble_sm_proc *proc, struct ble_sm_result *res,
         ble_sm_sc_dhkey_check_iocap(&proc->pair_rsp, iocap);
     }
 
-    rc = ble_sm_sc_dhkey_addrs(proc,
-                               &our_id_addr_type, &our_ota_addr,
-                               &peer_id_addr_type, &peer_ota_addr);
-    if (rc != 0) {
-        goto err;
-    }
+    ble_sm_sc_dhkey_addrs(proc,
+                          &our_id_addr_type, &our_ota_addr,
+                          &peer_id_addr_type, &peer_ota_addr);
 
     rc = ble_sm_alg_f6(proc->mackey, ble_sm_our_pair_rand(proc),
                        ble_sm_peer_pair_rand(proc), proc->tk, iocap,
@@ -621,8 +612,8 @@ ble_sm_dhkey_check_process(struct ble_sm_proc *proc,
                            struct ble_sm_result *res)
 {
     uint8_t exp_value[16];
-    uint8_t *peer_ota_addr;
-    uint8_t *our_ota_addr;
+    const uint8_t *peer_ota_addr;
+    const uint8_t *our_ota_addr;
     uint8_t peer_id_addr_type;
     uint8_t our_id_addr_type;
     uint8_t iocap[3];
@@ -634,19 +625,13 @@ ble_sm_dhkey_check_process(struct ble_sm_proc *proc,
         ble_sm_sc_dhkey_check_iocap(&proc->pair_req, iocap);
     }
 
-    res->app_status = ble_sm_sc_dhkey_addrs(proc,
-                                            &our_id_addr_type,
-                                            &our_ota_addr,
-                                            &peer_id_addr_type,
-                                            &peer_ota_addr);
-    if (res->app_status != 0) {
-        res->sm_err = BLE_SM_ERR_UNSPECIFIED;
-        res->enc_cb = 1;
-        return;
-    }
-
+    ble_sm_sc_dhkey_addrs(proc,
+                          &our_id_addr_type,
+                          &our_ota_addr,
+                          &peer_id_addr_type,
+                          &peer_ota_addr);
     BLE_HS_LOG(DEBUG, "tk=");
-    ble_hs_misc_log_flat_buf(proc->tk, 32);
+    ble_hs_log_flat_buf(proc->tk, 32);
     BLE_HS_LOG(DEBUG, "\n");
 
     res->app_status = ble_sm_alg_f6(proc->mackey,
@@ -693,7 +678,7 @@ ble_sm_sc_dhkey_check_rx(uint16_t conn_handle, uint8_t op, struct os_mbuf **om,
     struct ble_sm_proc *proc;
     struct ble_sm_proc *prev;
 
-    res->app_status = ble_hs_misc_pullup_base(om, BLE_SM_DHKEY_CHECK_SZ);
+    res->app_status = ble_hs_mbuf_pullup_base(om, BLE_SM_DHKEY_CHECK_SZ);
     if (res->app_status != 0) {
         res->enc_cb = 1;
         res->sm_err = BLE_SM_ERR_UNSPECIFIED;
diff --git a/net/nimble/host/src/ble_uuid.c b/net/nimble/host/src/ble_uuid.c
index ec015286..805cf497 100644
--- a/net/nimble/host/src/ble_uuid.c
+++ b/net/nimble/host/src/ble_uuid.c
@@ -34,15 +34,19 @@ static uint8_t ble_uuid_base[16] = {
  * Attempts to convert the supplied 128-bit UUID into its shortened 16-bit
  * form.
  *
- * @return                          Positive 16-bit unsigned integer on
+ * @param uuid128                   The 128-bit UUID to attempt to convert.
+ *                                      This must point to 16 contiguous bytes.
+ *
+ * @return                          A positive 16-bit unsigned integer on
  *                                      success;
- *                                  0 if the UUID could not be converted.
+ *                                  0 if the UUID cannot be represented in 16
+ *                                      bits.
  */
 uint16_t
-ble_uuid_128_to_16(void *uuid128)
+ble_uuid_128_to_16(const void *uuid128)
 {
+    const uint8_t *u8ptr;
     uint16_t uuid16;
-    uint8_t *u8ptr;
     int rc;
 
     u8ptr = uuid128;
@@ -68,8 +72,19 @@ ble_uuid_128_to_16(void *uuid128)
     return uuid16;
 }
 
+/**
+ * Expands a 16-bit UUID into its 128-bit form.
+ *
+ * @param uuid16                The 16-bit UUID to convert.
+ * @param out_uuid128           On success, the resulting 128-bit UUID gets
+ *                                  written here.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if uuid16 is not a valid 16-bit
+ *                                  UUID.
+ */
 int
-ble_uuid_16_to_128(uint16_t uuid16, void *uuid128)
+ble_uuid_16_to_128(uint16_t uuid16, void *out_uuid128)
 {
     uint8_t *u8ptr;
 
@@ -77,7 +92,7 @@ ble_uuid_16_to_128(uint16_t uuid16, void *uuid128)
         return BLE_HS_EINVAL;
     }
 
-    u8ptr = uuid128;
+    u8ptr = out_uuid128;
 
     memcpy(u8ptr, ble_uuid_base, 16);
     htole16(u8ptr + 12, uuid16);
@@ -86,7 +101,7 @@ ble_uuid_16_to_128(uint16_t uuid16, void *uuid128)
 }
 
 int
-ble_uuid_append(struct os_mbuf *om, void *uuid128)
+ble_uuid_append(struct os_mbuf *om, const void *uuid128)
 {
     uint16_t uuid16;
     void *buf;
diff --git a/net/nimble/host/src/ble_uuid_priv.h b/net/nimble/host/src/ble_uuid_priv.h
new file mode 100644
index 00000000..a1f1bd49
--- /dev/null
+++ b/net/nimble/host/src/ble_uuid_priv.h
@@ -0,0 +1,28 @@
+/**
+ * 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 H_BLE_UUID_PRIV_
+#define H_BLE_UUID_PRIV_
+
+struct os_mbuf;
+
+int ble_uuid_append(struct os_mbuf *om, const void *uuid128);
+int ble_uuid_extract(struct os_mbuf *om, int off, void *uuid128);
+
+#endif
diff --git a/net/nimble/host/src/test/ble_att_clt_test.c b/net/nimble/host/src/test/ble_att_clt_test.c
index dcb402e6..14d66f62 100644
--- a/net/nimble/host/src/test/ble_att_clt_test.c
+++ b/net/nimble/host/src/test/ble_att_clt_test.c
@@ -63,12 +63,14 @@ ble_att_clt_test_tx_write_req_or_cmd(uint16_t conn_handle,
                                      struct ble_att_write_req *req,
                                      void *value, int value_len, int is_req)
 {
+    struct os_mbuf *om;
     int rc;
 
+    om = ble_hs_test_util_om_from_flat(value, value_len);
     if (is_req) {
-        rc = ble_att_clt_tx_write_req(conn_handle, req, value, value_len);
+        rc = ble_att_clt_tx_write_req(conn_handle, req, om);
     } else {
-        rc = ble_att_clt_tx_write_cmd(conn_handle, req, value, value_len);
+        rc = ble_att_clt_tx_write_cmd(conn_handle, req, om);
     }
     TEST_ASSERT(rc == 0);
 }
@@ -212,8 +214,8 @@ ble_att_clt_test_misc_prep_good(uint16_t handle, uint16_t offset,
 
     req.bapc_handle = handle;
     req.bapc_offset = offset;
-    rc = ble_att_clt_tx_prep_write(conn_handle, &req, attr_data,
-                                   attr_data_len);
+    om = ble_hs_test_util_om_from_flat(attr_data, attr_data_len);
+    rc = ble_att_clt_tx_prep_write(conn_handle, &req, om);
     TEST_ASSERT(rc == 0);
 
     ble_hs_test_util_tx_all();
@@ -259,18 +261,36 @@ ble_att_clt_test_misc_prep_bad(uint16_t handle, uint16_t offset,
                                int status)
 {
     struct ble_att_prep_write_cmd req;
+    struct os_mbuf *om;
     uint16_t conn_handle;
     int rc;
 
     conn_handle = ble_att_clt_test_misc_init();
 
+    om = ble_hs_test_util_om_from_flat(attr_data, attr_data_len);
+
     req.bapc_handle = handle;
     req.bapc_offset = offset;
-    rc = ble_att_clt_tx_prep_write(conn_handle, &req, attr_data,
-                                   attr_data_len);
+    rc = ble_att_clt_tx_prep_write(conn_handle, &req, om);
     TEST_ASSERT(rc == status);
 }
 
+static void
+ble_att_clt_test_misc_tx_mtu(uint16_t conn_handle, uint16_t mtu, int status)
+{
+    struct ble_att_mtu_cmd req;
+    int rc;
+
+    req.bamc_mtu = mtu;
+    rc = ble_att_clt_tx_mtu(conn_handle, &req);
+    TEST_ASSERT(rc == status);
+
+    if (rc == 0) {
+        ble_hs_test_util_verify_tx_mtu_cmd(1, mtu);
+    }
+}
+
+
 TEST_CASE(ble_att_clt_test_tx_write)
 {
     ble_att_clt_test_case_tx_write_req_or_cmd(0);
@@ -408,7 +428,8 @@ TEST_CASE(ble_att_clt_test_rx_read_mult)
     htole16(buf + BLE_ATT_READ_MULT_RSP_BASE_SZ + 0, 12);
 
     rc = ble_hs_test_util_l2cap_rx_payload_flat(
-        conn_handle, BLE_L2CAP_CID_ATT, buf, BLE_ATT_READ_MULT_RSP_BASE_SZ + 2);
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_READ_MULT_RSP_BASE_SZ + 2);
     TEST_ASSERT(rc == 0);
 
     /*** Larger response. */
@@ -416,12 +437,14 @@ TEST_CASE(ble_att_clt_test_rx_read_mult)
     htole16(buf + BLE_ATT_READ_MULT_RSP_BASE_SZ + 2, 43);
     htole16(buf + BLE_ATT_READ_MULT_RSP_BASE_SZ + 4, 91);
     rc = ble_hs_test_util_l2cap_rx_payload_flat(
-        conn_handle, BLE_L2CAP_CID_ATT, buf, BLE_ATT_READ_MULT_RSP_BASE_SZ + 6);
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_READ_MULT_RSP_BASE_SZ + 6);
     TEST_ASSERT(rc == 0);
 
     /*** Zero-length response. */
     rc = ble_hs_test_util_l2cap_rx_payload_flat(
-        conn_handle, BLE_L2CAP_CID_ATT, buf, BLE_ATT_READ_MULT_RSP_BASE_SZ + 0);
+        conn_handle, BLE_L2CAP_CID_ATT, buf,
+        BLE_ATT_READ_MULT_RSP_BASE_SZ + 0);
     TEST_ASSERT(rc == 0);
 }
 
@@ -502,8 +525,24 @@ TEST_CASE(ble_att_clt_test_tx_exec_write)
     TEST_ASSERT(rc == BLE_HS_EINVAL);
 }
 
+TEST_CASE(ble_att_clt_test_tx_mtu)
+{
+    uint16_t conn_handle;
+
+    conn_handle = ble_att_clt_test_misc_init();
+
+    /*** Success. */
+    ble_att_clt_test_misc_tx_mtu(conn_handle, 50, 0);
+
+    /*** Error: repeated sends. */
+    ble_att_clt_test_misc_tx_mtu(conn_handle, 50, BLE_HS_EALREADY);
+    ble_att_clt_test_misc_tx_mtu(conn_handle, 60, BLE_HS_EALREADY);
+}
+
 TEST_SUITE(ble_att_clt_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_att_clt_test_tx_find_info();
     ble_att_clt_test_rx_find_info();
     ble_att_clt_test_tx_read();
@@ -516,6 +555,7 @@ TEST_SUITE(ble_att_clt_suite)
     ble_att_clt_test_tx_prep_write();
     ble_att_clt_test_rx_prep_write();
     ble_att_clt_test_tx_exec_write();
+    ble_att_clt_test_tx_mtu();
 }
 
 int
diff --git a/net/nimble/host/src/test/ble_att_svr_test.c b/net/nimble/host/src/test/ble_att_svr_test.c
index e6f8bdbd..52a56694 100644
--- a/net/nimble/host/src/test/ble_att_svr_test.c
+++ b/net/nimble/host/src/test/ble_att_svr_test.c
@@ -27,33 +27,32 @@
 #include "ble_hs_test_util.h"
 
 static uint8_t *ble_att_svr_test_attr_r_1;
-static int ble_att_svr_test_attr_r_1_len;
+static uint16_t ble_att_svr_test_attr_r_1_len;
 static uint8_t *ble_att_svr_test_attr_r_2;
-static int ble_att_svr_test_attr_r_2_len;
+static uint16_t ble_att_svr_test_attr_r_2_len;
 
 static uint8_t ble_att_svr_test_attr_w_1[1024];
-static int ble_att_svr_test_attr_w_1_len;
+static uint16_t ble_att_svr_test_attr_w_1_len;
 static uint8_t ble_att_svr_test_attr_w_2[1024];
-static int ble_att_svr_test_attr_w_2_len;
+static uint16_t ble_att_svr_test_attr_w_2_len;
 
 static uint16_t ble_att_svr_test_n_conn_handle;
 static uint16_t ble_att_svr_test_n_attr_handle;
 static uint8_t ble_att_svr_test_attr_n[1024];
-static int ble_att_svr_test_attr_n_len;
+static uint16_t ble_att_svr_test_attr_n_len;
 
 static int
-ble_att_svr_test_misc_gap_cb(int event,
-                             struct ble_gap_conn_ctxt *ctxt, void *arg)
+ble_att_svr_test_misc_gap_cb(struct ble_gap_event *event, void *arg)
 {
-    switch (event) {
-    case BLE_GAP_EVENT_NOTIFY:
-        ble_att_svr_test_n_conn_handle = ctxt->desc->conn_handle;
-        ble_att_svr_test_n_attr_handle = ctxt->notify.attr_handle;
-        TEST_ASSERT_FATAL(ctxt->notify.attr_len <=
+    switch (event->type) {
+    case BLE_GAP_EVENT_NOTIFY_RX:
+        ble_att_svr_test_n_conn_handle = event->notify_rx.conn_handle;
+        ble_att_svr_test_n_attr_handle = event->notify_rx.attr_handle;
+        TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(event->notify_rx.om) <=
                           sizeof ble_att_svr_test_attr_n);
-        ble_att_svr_test_attr_n_len = ctxt->notify.attr_len;
-        memcpy(ble_att_svr_test_attr_n, ctxt->notify.attr_data,
-               ctxt->notify.attr_len);
+        ble_att_svr_test_attr_n_len = OS_MBUF_PKTLEN(event->notify_rx.om);
+        os_mbuf_copydata(event->notify_rx.om, 0, ble_att_svr_test_attr_n_len,
+                         ble_att_svr_test_attr_n);
         break;
 
     default:
@@ -100,18 +99,17 @@ ble_att_svr_test_misc_init(uint16_t mtu)
 
 static int
 ble_att_svr_test_misc_attr_fn_r_1(uint16_t conn_handle, uint16_t attr_handle,
-                                  uint8_t *uuid128, uint8_t op,
-                                  struct ble_att_svr_access_ctxt *ctxt,
-                                  void *arg)
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg)
 {
-    if (ctxt->offset > ble_att_svr_test_attr_r_1_len) {
-        return BLE_ATT_ERR_INVALID_OFFSET;
-    }
-
     switch (op) {
     case BLE_ATT_ACCESS_OP_READ:
-        ctxt->attr_data = ble_att_svr_test_attr_r_1 + ctxt->offset;
-        ctxt->data_len = ble_att_svr_test_attr_r_1_len - ctxt->offset;
+        if (offset > ble_att_svr_test_attr_r_1_len) {
+            return BLE_ATT_ERR_INVALID_OFFSET;
+        }
+
+        os_mbuf_append(*om, ble_att_svr_test_attr_r_1 + offset,
+                       ble_att_svr_test_attr_r_1_len - offset);
         return 0;
 
     default:
@@ -121,18 +119,18 @@ ble_att_svr_test_misc_attr_fn_r_1(uint16_t conn_handle, uint16_t attr_handle,
 
 static int
 ble_att_svr_test_misc_attr_fn_r_2(uint16_t conn_handle, uint16_t attr_handle,
-                                  uint8_t *uuid128, uint8_t op,
-                                  struct ble_att_svr_access_ctxt *ctxt,
-                                  void *arg)
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg)
 {
-    if (ctxt->offset > ble_att_svr_test_attr_r_2_len) {
-        return BLE_ATT_ERR_INVALID_OFFSET;
-    }
 
     switch (op) {
     case BLE_ATT_ACCESS_OP_READ:
-        ctxt->attr_data = ble_att_svr_test_attr_r_2 + ctxt->offset;
-        ctxt->data_len = ble_att_svr_test_attr_r_2_len - ctxt->offset;
+        if (offset > ble_att_svr_test_attr_r_2_len) {
+            return BLE_ATT_ERR_INVALID_OFFSET;
+        }
+
+        os_mbuf_append(*om, ble_att_svr_test_attr_r_2 + offset,
+                       ble_att_svr_test_attr_r_2_len - offset);
         return 0;
 
     default:
@@ -145,11 +143,15 @@ ble_att_svr_test_misc_attr_fn_r_2(uint16_t conn_handle, uint16_t attr_handle,
 
 static int
 ble_att_svr_test_misc_attr_fn_r_group(uint16_t conn_handle,
-                                      uint16_t attr_handle, uint8_t *uuid128,
+                                      uint16_t attr_handle,
                                       uint8_t op,
-                                      struct ble_att_svr_access_ctxt *ctxt,
+                                      uint16_t offset,
+                                      struct os_mbuf **om,
                                       void *arg)
 {
+    uint8_t *src;
+    int rc;
+
     /* Service 0x1122 from 1 to 5 */
     /* Service 0x2233 from 6 to 10 */
     /* Service 010203...0f from 11 to 24 */
@@ -190,11 +192,14 @@ ble_att_svr_test_misc_attr_fn_r_group(uint16_t conn_handle,
     TEST_ASSERT_FATAL(attr_handle >= 1 &&
                       attr_handle <= BLE_ATT_SVR_TEST_LAST_ATTR);
 
-    ctxt->attr_data = vals + attr_handle;
-    if (memcmp(ctxt->attr_data + 2, zeros, 14) == 0) {
-        ctxt->data_len = 2;
+    src = &vals[attr_handle][0];
+    if (memcmp(src + 2, zeros, 14) == 0) {
+        rc = os_mbuf_append(*om, src, 2);
     } else {
-        ctxt->data_len = 16;
+        rc = os_mbuf_append(*om, src, 16);
+    }
+    if (rc != 0) {
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
     }
 
     return 0;
@@ -282,14 +287,14 @@ ble_att_svr_test_misc_register_group_attrs(void)
 
 static int
 ble_att_svr_test_misc_attr_fn_w_1(uint16_t conn_handle, uint16_t attr_handle,
-                                  uint8_t *uuid128, uint8_t op,
-                                  struct ble_att_svr_access_ctxt *ctxt,
-                                  void *arg)
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg)
 {
     switch (op) {
     case BLE_ATT_ACCESS_OP_WRITE:
-        memcpy(ble_att_svr_test_attr_w_1, ctxt->attr_data, ctxt->data_len);
-        ble_att_svr_test_attr_w_1_len = ctxt->data_len;
+        os_mbuf_copydata(*om, 0, OS_MBUF_PKTLEN(*om),
+                         ble_att_svr_test_attr_w_1);
+        ble_att_svr_test_attr_w_1_len = OS_MBUF_PKTLEN(*om);
         return 0;
 
     default:
@@ -299,14 +304,14 @@ ble_att_svr_test_misc_attr_fn_w_1(uint16_t conn_handle, uint16_t attr_handle,
 
 static int
 ble_att_svr_test_misc_attr_fn_w_2(uint16_t conn_handle, uint16_t attr_handle,
-                                  uint8_t *uuid128, uint8_t op,
-                                  struct ble_att_svr_access_ctxt *ctxt,
-                                  void *arg)
+                                  uint8_t op, uint16_t offset,
+                                  struct os_mbuf **om, void *arg)
 {
     switch (op) {
     case BLE_ATT_ACCESS_OP_WRITE:
-        memcpy(ble_att_svr_test_attr_w_2, ctxt->attr_data, ctxt->data_len);
-        ble_att_svr_test_attr_w_2_len = ctxt->data_len;
+        os_mbuf_copydata(*om, 0, OS_MBUF_PKTLEN(*om),
+                         ble_att_svr_test_attr_w_2);
+        ble_att_svr_test_attr_w_2_len = OS_MBUF_PKTLEN(*om);
         return 0;
 
     default:
@@ -314,6 +319,15 @@ ble_att_svr_test_misc_attr_fn_w_2(uint16_t conn_handle, uint16_t attr_handle,
     }
 }
 
+static int
+ble_att_svr_test_misc_attr_fn_w_fail(uint16_t conn_handle,
+                                     uint16_t attr_handle,
+                                     uint8_t op, uint16_t offset,
+                                     struct os_mbuf **om, void *arg)
+{
+    return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+}
+
 static void
 ble_att_svr_test_misc_verify_w_1(void *data, int data_len)
 {
@@ -329,55 +343,6 @@ ble_att_svr_test_misc_verify_w_2(void *data, int data_len)
 }
 
 static void
-ble_att_svr_test_misc_verify_tx_err_rsp(uint8_t req_op, uint16_t handle,
-                                        uint8_t error_code)
-{
-    struct ble_att_error_rsp rsp;
-    struct os_mbuf *om;
-    uint8_t buf[BLE_ATT_ERROR_RSP_SZ];
-    int rc;
-
-    ble_hs_test_util_tx_all();
-
-    om = ble_hs_test_util_prev_tx_dequeue();
-
-    rc = os_mbuf_copydata(om, 0, sizeof buf, buf);
-    TEST_ASSERT(rc == 0);
-
-    ble_att_error_rsp_parse(buf, sizeof buf, &rsp);
-
-    TEST_ASSERT(rsp.baep_req_op == req_op);
-    TEST_ASSERT(rsp.baep_handle == handle);
-    TEST_ASSERT(rsp.baep_error_code == error_code);
-}
-
-static void
-ble_att_svr_test_misc_verify_tx_read_rsp(uint8_t *attr_data, int attr_len)
-{
-    struct os_mbuf *om;
-    uint8_t u8;
-    int rc;
-    int i;
-
-    ble_hs_test_util_tx_all();
-
-    om = ble_hs_test_util_prev_tx_dequeue();
-
-    rc = os_mbuf_copydata(om, 0, 1, &u8);
-    TEST_ASSERT(rc == 0);
-    TEST_ASSERT(u8 == BLE_ATT_OP_READ_RSP);
-
-    for (i = 0; i < attr_len; i++) {
-        rc = os_mbuf_copydata(om, i + 1, 1, &u8);
-        TEST_ASSERT(rc == 0);
-        TEST_ASSERT(u8 == attr_data[i]);
-    }
-
-    rc = os_mbuf_copydata(om, i + 1, 1, &u8);
-    TEST_ASSERT(rc != 0);
-}
-
-static void
 ble_att_svr_test_misc_verify_tx_read_blob_rsp(uint8_t *attr_data, int attr_len)
 {
     struct os_mbuf *om;
@@ -431,18 +396,17 @@ ble_att_svr_test_misc_rx_read_mult_req(uint16_t conn_handle,
 }
 
 static void
-ble_att_svr_test_misc_verify_tx_read_mult_rsp(uint16_t conn_handle,
-                                              struct ble_gatt_attr *attrs,
-                                              int num_attrs)
+ble_att_svr_test_misc_verify_tx_read_mult_rsp(
+    uint16_t conn_handle, struct ble_hs_test_util_flat_attr *attrs,
+    int num_attrs)
 {
     struct ble_l2cap_chan *chan;
     struct os_mbuf *om;
+    uint16_t attr_len;
     uint16_t mtu;
-    uint8_t *attr_value;
     uint8_t u8;
     int rc;
     int off;
-    int ii;
     int i;
 
     ble_hs_test_util_tx_all();
@@ -464,25 +428,21 @@ ble_att_svr_test_misc_verify_tx_read_mult_rsp(uint16_t conn_handle,
 
     off = 1;
     for (i = 0; i < num_attrs; i++) {
-        attr_value = attrs[i].value;
+        attr_len = min(attrs[i].value_len, mtu - off);
 
-        for (ii = 0; ii < attrs[i].value_len && off < mtu; ii++) {
-            rc = os_mbuf_copydata(om, off, 1, &u8);
-            TEST_ASSERT(rc == 0);
-            TEST_ASSERT(u8 == attr_value[ii]);
+        rc = os_mbuf_cmpf(om, off, attrs[i].value, attr_len);
+        TEST_ASSERT(rc == 0);
 
-            off++;
-        }
+        off += attr_len;
     }
 
-    rc = os_mbuf_copydata(om, off, 1, &u8);
-    TEST_ASSERT(rc != 0);
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) == off);
 }
 
 static void
-ble_att_svr_test_misc_verify_all_read_mult(uint16_t conn_handle,
-                                           struct ble_gatt_attr *attrs,
-                                           int num_attrs)
+ble_att_svr_test_misc_verify_all_read_mult(
+    uint16_t conn_handle, struct ble_hs_test_util_flat_attr *attrs,
+    int num_attrs)
 {
     uint16_t handles[256];
     int i;
@@ -498,23 +458,6 @@ ble_att_svr_test_misc_verify_all_read_mult(uint16_t conn_handle,
                                                   attrs, num_attrs);
 }
 
-
-static void
-ble_att_svr_test_misc_verify_tx_write_rsp(void)
-{
-    struct os_mbuf *om;
-    uint8_t u8;
-    int rc;
-
-    ble_hs_test_util_tx_all();
-
-    om = ble_hs_test_util_prev_tx_dequeue();
-
-    rc = os_mbuf_copydata(om, 0, 1, &u8);
-    TEST_ASSERT(rc == 0);
-    TEST_ASSERT(u8 == BLE_ATT_OP_WRITE_RSP);
-}
-
 static void
 ble_att_svr_test_misc_verify_tx_mtu_rsp(uint16_t conn_handle)
 {
@@ -532,7 +475,7 @@ ble_att_svr_test_misc_verify_tx_mtu_rsp(uint16_t conn_handle)
     rc = os_mbuf_copydata(om, 0, sizeof buf, buf);
     TEST_ASSERT(rc == 0);
 
-    ble_att_mtu_cmd_parse(buf, sizeof buf, &rsp);
+    ble_att_mtu_rsp_parse(buf, sizeof buf, &rsp);
 
     ble_hs_lock();
     rc = ble_hs_misc_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT,
@@ -836,6 +779,7 @@ ble_att_svr_test_misc_mtu_exchange(uint16_t my_mtu, uint16_t peer_sent,
     TEST_ASSERT(chan->blc_peer_mtu == peer_actual);
     TEST_ASSERT(ble_l2cap_chan_mtu(chan) == chan_mtu);
     ble_hs_unlock();
+
 }
 
 static void
@@ -861,7 +805,7 @@ ble_att_svr_test_misc_prep_write(uint16_t conn_handle, uint16_t attr_handle,
                                                        data, data_len);
     } else {
         TEST_ASSERT(rc != 0);
-        ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_PREP_WRITE_REQ,
+        ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_PREP_WRITE_REQ,
                                                 attr_handle, error_code);
     }
 }
@@ -885,7 +829,7 @@ ble_att_svr_test_misc_exec_write(uint16_t conn_handle, uint8_t flags,
         ble_att_svr_test_misc_verify_tx_exec_write_rsp();
     } else {
         TEST_ASSERT(rc != 0);
-        ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_EXEC_WRITE_REQ,
+        ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_EXEC_WRITE_REQ,
                                                 error_handle, error_code);
     }
 }
@@ -1022,12 +966,11 @@ TEST_CASE(ble_att_svr_test_read)
 {
     struct ble_att_read_req req;
     struct ble_hs_conn *conn;
+    struct os_mbuf *om;
     uint16_t conn_handle;
-    uint16_t attr_len;
     uint8_t buf[BLE_ATT_READ_REQ_SZ];
     uint8_t uuid_sec[16] = {1};
     uint8_t uuid[16] = {0};
-    void *attr_data;
     int rc;
 
     conn_handle = ble_att_svr_test_misc_init(0);
@@ -1039,7 +982,7 @@ TEST_CASE(ble_att_svr_test_read)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_READ_REQ, 0,
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_REQ, 0,
                                             BLE_ATT_ERR_INVALID_HANDLE);
 
     /*** Successful read. */
@@ -1054,7 +997,7 @@ TEST_CASE(ble_att_svr_test_read)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc == 0);
-    ble_att_svr_test_misc_verify_tx_read_rsp(
+    ble_hs_test_util_verify_tx_read_rsp(
         ble_att_svr_test_attr_r_1, ble_att_svr_test_attr_r_1_len);
 
     /*** Partial read. */
@@ -1068,8 +1011,8 @@ TEST_CASE(ble_att_svr_test_read)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc == 0);
-    ble_att_svr_test_misc_verify_tx_read_rsp(ble_att_svr_test_attr_r_1,
-                                             BLE_ATT_MTU_DFLT - 1);
+    ble_hs_test_util_verify_tx_read_rsp(ble_att_svr_test_attr_r_1,
+                                        BLE_ATT_MTU_DFLT - 1);
 
     /*** Read requires encryption. */
     /* Insufficient authentication. */
@@ -1082,16 +1025,18 @@ TEST_CASE(ble_att_svr_test_read)
 
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
-    TEST_ASSERT(rc == BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHENT));
-    ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_READ_REQ,
+    TEST_ASSERT(rc == BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHEN));
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_REQ,
                                             req.barq_handle,
-                                            BLE_ATT_ERR_INSUFFICIENT_AUTHENT);
+                                            BLE_ATT_ERR_INSUFFICIENT_AUTHEN);
 
     /* Security check bypassed for local reads. */
-    rc = ble_att_svr_read_local(req.barq_handle, &attr_data, &attr_len);
-    TEST_ASSERT(rc == 0);
-    TEST_ASSERT(attr_len == ble_att_svr_test_attr_r_1_len);
-    TEST_ASSERT(attr_data == ble_att_svr_test_attr_r_1);
+    rc = ble_att_svr_read_local(req.barq_handle, &om);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) == ble_att_svr_test_attr_r_1_len);
+    TEST_ASSERT(os_mbuf_cmpf(om, 0, ble_att_svr_test_attr_r_1,
+                               ble_att_svr_test_attr_r_1_len) == 0);
+    os_mbuf_free_chain(om);
 
     /* Ensure no response got sent. */
     ble_hs_test_util_tx_all();
@@ -1106,8 +1051,9 @@ TEST_CASE(ble_att_svr_test_read)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc == 0);
-    ble_att_svr_test_misc_verify_tx_read_rsp(ble_att_svr_test_attr_r_1,
-                                             BLE_ATT_MTU_DFLT - 1);
+    ble_hs_test_util_verify_tx_read_rsp(ble_att_svr_test_attr_r_1,
+                                        BLE_ATT_MTU_DFLT - 1);
+
 }
 
 TEST_CASE(ble_att_svr_test_read_blob)
@@ -1128,14 +1074,15 @@ TEST_CASE(ble_att_svr_test_read_blob)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_READ_BLOB_REQ, 0,
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_BLOB_REQ, 0,
                                             BLE_ATT_ERR_INVALID_HANDLE);
 
-    /*** Short read failure. */
+
+    /*** Successful partial read. */
     ble_att_svr_test_attr_r_1 =
         (uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,
                     22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39};
-    ble_att_svr_test_attr_r_1_len = BLE_ATT_MTU_DFLT - 3;
+    ble_att_svr_test_attr_r_1_len = 40;
     rc = ble_att_svr_register(uuid, HA_FLAG_PERM_RW, &req.babq_handle,
                               ble_att_svr_test_misc_attr_fn_r_1, NULL);
     TEST_ASSERT(rc == 0);
@@ -1144,18 +1091,6 @@ TEST_CASE(ble_att_svr_test_read_blob)
 
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
-    TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_READ_BLOB_REQ,
-                                            req.babq_handle,
-                                            BLE_ATT_ERR_ATTR_NOT_LONG);
-
-    /*** Successful partial read. */
-    ble_att_svr_test_attr_r_1_len = 40;
-
-    ble_att_read_blob_req_write(buf, sizeof buf, &req);
-
-    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
-                                                buf, sizeof buf);
     TEST_ASSERT(rc == 0);
     ble_att_svr_test_misc_verify_tx_read_blob_rsp(ble_att_svr_test_attr_r_1,
                                                   BLE_ATT_MTU_DFLT - 1);
@@ -1180,73 +1115,84 @@ TEST_CASE(ble_att_svr_test_read_blob)
     TEST_ASSERT(rc == 0);
     ble_att_svr_test_misc_verify_tx_read_blob_rsp(ble_att_svr_test_attr_r_1,
                                                   0);
+
 }
 
 TEST_CASE(ble_att_svr_test_read_mult)
 {
-    struct ble_gatt_attr attr1;
-    struct ble_gatt_attr attr2;
     uint16_t conn_handle;
     int rc;
 
     conn_handle = ble_att_svr_test_misc_init(0);
 
-    attr1.value = (uint8_t[]){ 1, 2, 3, 4 };
-    attr1.value_len = 4;
-    ble_att_svr_test_attr_r_1 = attr1.value;
-    ble_att_svr_test_attr_r_1_len = attr1.value_len;
+    struct ble_hs_test_util_flat_attr attrs[2] = {
+        {
+            .handle = 0,
+            .offset = 0,
+            .value = { 1, 2, 3, 4 },
+            .value_len = 4,
+        },
+        {
+            .handle = 0,
+            .offset = 0,
+            .value = { 2, 3, 4, 5, 6 },
+            .value_len = 5,
+        },
+    };
+
+    ble_att_svr_test_attr_r_1 = attrs[0].value;
+    ble_att_svr_test_attr_r_1_len = attrs[0].value_len;
+    ble_att_svr_test_attr_r_2 = attrs[1].value;
+    ble_att_svr_test_attr_r_2_len = attrs[1].value_len;
+
     rc = ble_att_svr_register(BLE_UUID16(0x1111), HA_FLAG_PERM_RW,
-                              &attr1.handle,
+                              &attrs[0].handle,
                               ble_att_svr_test_misc_attr_fn_r_1, NULL);
     TEST_ASSERT(rc == 0);
 
-    attr2.value = (uint8_t[]){ 2, 3, 4, 5, 6 };
-    attr2.value_len = 5;
-    ble_att_svr_test_attr_r_2 = attr2.value;
-    ble_att_svr_test_attr_r_2_len = attr2.value_len;
     rc = ble_att_svr_register(BLE_UUID16(0x2222), HA_FLAG_PERM_RW,
-                              &attr2.handle,
+                              &attrs[1].handle,
                               ble_att_svr_test_misc_attr_fn_r_2, NULL);
     TEST_ASSERT(rc == 0);
 
     /*** Single nonexistent attribute. */
     ble_att_svr_test_misc_rx_read_mult_req(
         conn_handle, ((uint16_t[]){ 100 }), 1, 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_READ_MULT_REQ,
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_MULT_REQ,
                                             100, BLE_ATT_ERR_INVALID_HANDLE);
 
     /*** Single attribute. */
-    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, &attr1, 1);
+    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, &attrs[0], 1);
 
     /*** Two attributes. */
-    ble_att_svr_test_misc_verify_all_read_mult(
-        conn_handle, ((struct ble_gatt_attr[]) { attr1, attr2 }), 2);
+    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, attrs, 2);
 
     /*** Reverse order. */
-    ble_att_svr_test_misc_verify_all_read_mult(
-        conn_handle, ((struct ble_gatt_attr[]) { attr2, attr1 }), 2);
+    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, attrs, 2);
 
     /*** Second attribute nonexistent; verify only error txed. */
     ble_att_svr_test_misc_rx_read_mult_req(
-        conn_handle, ((uint16_t[]){ attr1.handle, 100 }), 2, 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_READ_MULT_REQ,
+        conn_handle, ((uint16_t[]){ attrs[0].handle, 100 }), 2, 0);
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_READ_MULT_REQ,
                                             100, BLE_ATT_ERR_INVALID_HANDLE);
 
     /*** Response too long; verify only MTU bytes sent. */
-    attr1.value =
-        (uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19};
-    attr1.value_len = 20;
-    ble_att_svr_test_attr_r_1 = attr1.value;
-    ble_att_svr_test_attr_r_1_len = attr1.value_len;
-
-    attr2.value =
-        (uint8_t[]){22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39};
-    attr2.value_len = 20;
-    ble_att_svr_test_attr_r_2 = attr2.value;
-    ble_att_svr_test_attr_r_2_len = attr2.value_len;
-
-    ble_att_svr_test_misc_verify_all_read_mult(
-        conn_handle, ((struct ble_gatt_attr[]) { attr1, attr2 }), 2);
+    attrs[0].value_len = 20;
+    memcpy(attrs[0].value,
+           ((uint8_t[]){0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19}),
+           attrs[0].value_len);
+    ble_att_svr_test_attr_r_1_len = attrs[0].value_len;
+
+    attrs[1].value_len = 20;
+    memcpy(attrs[1].value,
+           ((uint8_t[]){
+                22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39
+           }),
+           attrs[1].value_len);
+    ble_att_svr_test_attr_r_2_len = attrs[1].value_len;
+
+    ble_att_svr_test_misc_verify_all_read_mult(conn_handle, attrs, 2);
+
 }
 
 TEST_CASE(ble_att_svr_test_write)
@@ -1270,7 +1216,7 @@ TEST_CASE(ble_att_svr_test_write)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_WRITE_REQ, 0, BLE_ATT_ERR_INVALID_HANDLE);
 
     /*** Write not permitted if non-local. */
@@ -1286,12 +1232,12 @@ TEST_CASE(ble_att_svr_test_write)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc == BLE_HS_ENOTSUP);
-    ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ,
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ,
                                             req.bawq_handle,
                                             BLE_ATT_ERR_WRITE_NOT_PERMITTED);
 
     /* Local write (success). */
-    rc = ble_att_svr_write_local(req.bawq_handle, buf, sizeof buf);
+    rc = ble_hs_test_util_write_local_flat(req.bawq_handle, buf, sizeof buf);
     TEST_ASSERT(rc == 0);
 
     /* Ensure no response got sent. */
@@ -1310,7 +1256,7 @@ TEST_CASE(ble_att_svr_test_write)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc == 0);
-    ble_att_svr_test_misc_verify_tx_write_rsp();
+    ble_hs_test_util_verify_tx_write_rsp();
 
     /*** Write requires encryption. */
     /* Insufficient authentication. */
@@ -1325,13 +1271,13 @@ TEST_CASE(ble_att_svr_test_write)
 
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
-    TEST_ASSERT(rc == BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHENT));
-    ble_att_svr_test_misc_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ,
+    TEST_ASSERT(rc == BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHEN));
+    ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ,
                                             req.bawq_handle,
-                                            BLE_ATT_ERR_INSUFFICIENT_AUTHENT);
+                                            BLE_ATT_ERR_INSUFFICIENT_AUTHEN);
 
     /* Security check bypassed for local writes. */
-    rc = ble_att_svr_write_local(req.bawq_handle, buf, sizeof buf);
+    rc = ble_hs_test_util_write_local_flat(req.bawq_handle, buf, sizeof buf);
     TEST_ASSERT(rc == 0);
 
     /* Ensure no response got sent. */
@@ -1347,7 +1293,7 @@ TEST_CASE(ble_att_svr_test_write)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc == 0);
-    ble_att_svr_test_misc_verify_tx_write_rsp();
+    ble_hs_test_util_verify_tx_write_rsp();
 }
 
 TEST_CASE(ble_att_svr_test_find_info)
@@ -1377,7 +1323,7 @@ TEST_CASE(ble_att_svr_test_find_info)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_FIND_INFO_REQ, 0, BLE_ATT_ERR_INVALID_HANDLE);
 
     /*** Start handle > end handle. */
@@ -1389,7 +1335,7 @@ TEST_CASE(ble_att_svr_test_find_info)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_FIND_INFO_REQ, 101, BLE_ATT_ERR_INVALID_HANDLE);
 
     /*** No attributes. */
@@ -1401,7 +1347,7 @@ TEST_CASE(ble_att_svr_test_find_info)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_FIND_INFO_REQ, 200, BLE_ATT_ERR_ATTR_NOT_FOUND);
 
     /*** Range too late. */
@@ -1417,7 +1363,7 @@ TEST_CASE(ble_att_svr_test_find_info)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_FIND_INFO_REQ, 200, BLE_ATT_ERR_ATTR_NOT_FOUND);
 
     /*** One 128-bit entry. */
@@ -1501,6 +1447,7 @@ TEST_CASE(ble_att_svr_test_find_info)
         }, {
             .handle = 0,
         } }));
+
 }
 
 TEST_CASE(ble_att_svr_test_find_type_value)
@@ -1543,7 +1490,7 @@ TEST_CASE(ble_att_svr_test_find_type_value)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 0,
         BLE_ATT_ERR_INVALID_HANDLE);
 
@@ -1556,7 +1503,7 @@ TEST_CASE(ble_att_svr_test_find_type_value)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 101,
         BLE_ATT_ERR_INVALID_HANDLE);
 
@@ -1569,7 +1516,7 @@ TEST_CASE(ble_att_svr_test_find_type_value)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 200,
         BLE_ATT_ERR_ATTR_NOT_FOUND);
 
@@ -1586,7 +1533,7 @@ TEST_CASE(ble_att_svr_test_find_type_value)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_FIND_TYPE_VALUE_REQ, 200,
         BLE_ATT_ERR_ATTR_NOT_FOUND);
 
@@ -1701,6 +1648,7 @@ TEST_CASE(ble_att_svr_test_find_type_value)
         }, {
             .first = 0,
         } }));
+
 }
 
 static void
@@ -1724,7 +1672,7 @@ ble_att_svr_test_misc_read_type(uint16_t mtu)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_TYPE_REQ, 0,
         BLE_ATT_ERR_INVALID_HANDLE);
 
@@ -1739,7 +1687,7 @@ ble_att_svr_test_misc_read_type(uint16_t mtu)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_TYPE_REQ, 101,
         BLE_ATT_ERR_INVALID_HANDLE);
 
@@ -1754,7 +1702,7 @@ ble_att_svr_test_misc_read_type(uint16_t mtu)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_TYPE_REQ, 1,
         BLE_ATT_ERR_ATTR_NOT_FOUND);
 
@@ -1770,7 +1718,7 @@ ble_att_svr_test_misc_read_type(uint16_t mtu)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_TYPE_REQ, 200,
         BLE_ATT_ERR_ATTR_NOT_FOUND);
 
@@ -1907,6 +1855,7 @@ ble_att_svr_test_misc_read_type(uint16_t mtu)
         }, {
             .handle = 0,
         } }));
+
 }
 
 TEST_CASE(ble_att_svr_test_read_type)
@@ -1935,7 +1884,7 @@ TEST_CASE(ble_att_svr_test_read_group_type)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_GROUP_TYPE_REQ, 0,
         BLE_ATT_ERR_INVALID_HANDLE);
 
@@ -1950,7 +1899,7 @@ TEST_CASE(ble_att_svr_test_read_group_type)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_GROUP_TYPE_REQ, 101,
         BLE_ATT_ERR_INVALID_HANDLE);
 
@@ -1964,7 +1913,7 @@ TEST_CASE(ble_att_svr_test_read_group_type)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_GROUP_TYPE_REQ, 110,
         BLE_ATT_ERR_UNSUPPORTED_GROUP);
 
@@ -1979,7 +1928,7 @@ TEST_CASE(ble_att_svr_test_read_group_type)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_GROUP_TYPE_REQ, 1,
         BLE_ATT_ERR_ATTR_NOT_FOUND);
 
@@ -1995,7 +1944,7 @@ TEST_CASE(ble_att_svr_test_read_group_type)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc != 0);
-    ble_att_svr_test_misc_verify_tx_err_rsp(
+    ble_hs_test_util_verify_tx_err_rsp(
         BLE_ATT_OP_READ_GROUP_TYPE_REQ, 200,
         BLE_ATT_ERR_ATTR_NOT_FOUND);
 
@@ -2086,28 +2035,47 @@ TEST_CASE(ble_att_svr_test_read_group_type)
         }, {
             .start_handle = 0,
         } }));
+
 }
 
 TEST_CASE(ble_att_svr_test_prep_write)
 {
+    struct ble_hs_conn *conn;
     uint16_t conn_handle;
     int i;
 
     static uint8_t data[1024];
 
-    conn_handle = ble_att_svr_test_misc_init(200);
+    conn_handle = ble_att_svr_test_misc_init(205);
 
     /* Initialize some attribute data. */
     for (i = 0; i < sizeof data; i++) {
         data[i] = i;
     }
 
-    /* Register two attributes. */
+    /* Register two writable attributes. */
     ble_att_svr_test_misc_register_uuid16(0x1234, HA_FLAG_PERM_RW, 1,
                                           ble_att_svr_test_misc_attr_fn_w_1);
     ble_att_svr_test_misc_register_uuid16(0x8989, HA_FLAG_PERM_RW, 2,
                                           ble_att_svr_test_misc_attr_fn_w_2);
 
+    /* 3: not writable. */
+    ble_att_svr_test_misc_register_uuid16(0xabab, BLE_ATT_F_READ, 3,
+                                          ble_att_svr_test_misc_attr_fn_r_1);
+    /* 4: Encryption required. */
+    ble_att_svr_test_misc_register_uuid16(
+        0xabac, BLE_ATT_F_WRITE | BLE_ATT_F_WRITE_ENC, 4,
+        ble_att_svr_test_misc_attr_fn_w_1);
+
+    /* 5: Encryption+authentication required. */
+    ble_att_svr_test_misc_register_uuid16(
+        0xabad, BLE_ATT_F_WRITE | BLE_ATT_F_WRITE_ENC | BLE_ATT_F_WRITE_AUTHEN,
+        5, ble_att_svr_test_misc_attr_fn_w_1);
+
+    /* 6: Write callback always fails. */
+    ble_att_svr_test_misc_register_uuid16(
+        0xabae, BLE_ATT_F_WRITE, 6, ble_att_svr_test_misc_attr_fn_w_fail);
+
     /*** Empty write succeeds. */
     ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_CONFIRM,
                                      0, 0);
@@ -2119,6 +2087,28 @@ TEST_CASE(ble_att_svr_test_prep_write)
     ble_att_svr_test_misc_prep_write(conn_handle, 53525, 0, data, 10,
                                      BLE_ATT_ERR_INVALID_HANDLE);
 
+    /*** Failure due to write-not-permitted. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 3, 0, data, 35,
+                                     BLE_ATT_ERR_WRITE_NOT_PERMITTED);
+
+    /*** Failure due to insufficient authentication (encryption required). */
+    ble_att_svr_test_misc_prep_write(conn_handle, 4, 0, data, 1,
+                                     BLE_ATT_ERR_INSUFFICIENT_AUTHEN);
+
+    /*** Encrypt connection; ensure previous prep write now succeeds. */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_sec_state.encrypted = 1;
+    ble_hs_unlock();
+
+    ble_att_svr_test_misc_prep_write(conn_handle, 4, 0, data, 1, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, 0, 0, 0);
+
+    /*** Failure due to insufficient authentication (not authenticated). */
+    ble_att_svr_test_misc_prep_write(conn_handle, 5, 0, data, 35,
+                                     BLE_ATT_ERR_INSUFFICIENT_AUTHEN);
+
     /*** Failure for write starting at nonzero offset. */
     ble_att_svr_test_misc_prep_write(conn_handle, 1, 1, data, 10, 0);
     ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_CONFIRM,
@@ -2195,6 +2185,13 @@ TEST_CASE(ble_att_svr_test_prep_write)
                                      0, 0);
     ble_att_svr_test_misc_verify_w_1(data, 12);
     ble_att_svr_test_misc_verify_w_2(data, 61);
+
+    /*** Fail due to attribute callback error. */
+    ble_att_svr_test_misc_prep_write(conn_handle, 6, 0, data, 35, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 6, 35, data + 35, 43, 0);
+    ble_att_svr_test_misc_prep_write(conn_handle, 6, 78, data + 78, 1, 0);
+    ble_att_svr_test_misc_exec_write(conn_handle, BLE_ATT_EXEC_WRITE_F_CONFIRM,
+                                     BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN, 6);
 }
 
 TEST_CASE(ble_att_svr_test_notify)
@@ -2217,6 +2214,7 @@ TEST_CASE(ble_att_svr_test_notify)
     /* Attribute handle of 0. */
     ble_att_svr_test_misc_verify_notify(conn_handle, 0,
                                         (uint8_t[]) { 1, 2, 3 }, 3, 0);
+
 }
 
 TEST_CASE(ble_att_svr_test_indicate)
@@ -2239,10 +2237,20 @@ TEST_CASE(ble_att_svr_test_indicate)
     /* Attribute handle of 0. */
     ble_att_svr_test_misc_verify_indicate(conn_handle, 0,
                                           (uint8_t[]) { 1, 2, 3 }, 3, 0);
+
 }
 
 TEST_SUITE(ble_att_svr_suite)
 {
+    /* When checking for mbuf leaks, ensure no stale prep entries. */
+    static struct ble_hs_test_util_mbuf_params mbuf_params = {
+        .prev_tx = 1,
+        .rx_queue = 1,
+        .prep_list = 0,
+    };
+
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, &mbuf_params);
+
     ble_att_svr_test_mtu();
     ble_att_svr_test_read();
     ble_att_svr_test_read_blob();
diff --git a/net/nimble/host/src/test/ble_gap_test.c b/net/nimble/host/src/test/ble_gap_test.c
index fcaba400..5dd6532b 100644
--- a/net/nimble/host/src/test/ble_gap_test.c
+++ b/net/nimble/host/src/test/ble_gap_test.c
@@ -6,7 +6,7 @@
  * 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,
@@ -22,18 +22,18 @@
 #include "testutil/testutil.h"
 #include "nimble/ble.h"
 #include "nimble/hci_common.h"
+#include "host/ble_hs_adv.h"
 #include "host/ble_hs_test.h"
 #include "ble_hs_test_util.h"
 
-static int ble_gap_test_conn_event;
+static struct ble_gap_event ble_gap_test_event;
 static int ble_gap_test_conn_status;
 static struct ble_gap_conn_desc ble_gap_test_conn_desc;
 static void *ble_gap_test_conn_arg;
 static struct ble_gap_upd_params ble_gap_test_conn_peer_params;
 static struct ble_gap_upd_params ble_gap_test_conn_self_params;
 
-static int ble_gap_test_disc_event;
-static int ble_gap_test_disc_status;
+static int ble_gap_test_disc_event_type;
 static struct ble_gap_disc_desc ble_gap_test_disc_desc;
 static void *ble_gap_test_disc_arg;
 
@@ -54,13 +54,12 @@ ble_gap_test_util_update_in_progress(uint16_t conn_handle)
 static void
 ble_gap_test_util_reset_cb_info(void)
 {
-    ble_gap_test_conn_event = -1;
+    memset(&ble_gap_test_event, 0xff, sizeof ble_gap_test_event);
     ble_gap_test_conn_status = -1;
     memset(&ble_gap_test_conn_desc, 0xff, sizeof ble_gap_test_conn_desc);
     ble_gap_test_conn_arg = (void *)-1;
 
-    ble_gap_test_disc_event = -1;
-    ble_gap_test_disc_status = -1;
+    ble_gap_test_disc_event_type = -1;
     memset(&ble_gap_test_disc_desc, 0xff, sizeof ble_gap_test_disc_desc);
     ble_gap_test_disc_arg = (void *)-1;
 }
@@ -69,47 +68,55 @@ static void
 ble_gap_test_util_init(void)
 {
     ble_hs_test_util_init();
+    ble_hs_test_util_set_static_rnd_addr();
     ble_gap_test_util_reset_cb_info();
 }
 
-static void
-ble_gap_test_util_disc_cb(int event, int status,
-                          struct ble_gap_disc_desc *desc, void *arg)
+static int
+ble_gap_test_util_disc_cb(struct ble_gap_event *event, void *arg)
 {
-    ble_gap_test_disc_event = event;
-    ble_gap_test_disc_status = status;
-    ble_gap_test_disc_desc = *desc;
+    ble_gap_test_disc_event_type = event->type;
     ble_gap_test_disc_arg = arg;
+
+    if (event->type == BLE_GAP_EVENT_DISC) {
+        ble_gap_test_disc_desc = event->disc;
+    }
+
+    return 0;
 }
 
 static int
-ble_gap_test_util_connect_cb(int event, struct ble_gap_conn_ctxt *ctxt,
-                             void *arg)
+ble_gap_test_util_connect_cb(struct ble_gap_event *event, void *arg)
 {
     int *fail_reason;
 
-    ble_gap_test_conn_event = event;
-    ble_gap_test_conn_desc = *ctxt->desc;
+    ble_gap_test_event = *event;
     ble_gap_test_conn_arg = arg;
 
-    switch (event) {
+    switch (event->type) {
     case BLE_GAP_EVENT_CONNECT:
-        ble_gap_test_conn_status = ctxt->connect.status;
+        ble_gap_test_conn_status = event->connect.status;
+        ble_gap_conn_find(event->connect.conn_handle, &ble_gap_test_conn_desc);
         break;
 
     case BLE_GAP_EVENT_DISCONNECT:
-        ble_gap_test_conn_status = ctxt->disconnect.reason;
+        ble_gap_test_conn_status = event->disconnect.reason;
+        ble_gap_test_conn_desc = event->disconnect.conn;
         break;
 
     case BLE_GAP_EVENT_CONN_UPDATE:
-        ble_gap_test_conn_status = ctxt->conn_update.status;
+        ble_gap_test_conn_status = event->conn_update.status;
+        ble_gap_conn_find(event->conn_update.conn_handle,
+                          &ble_gap_test_conn_desc);
         break;
 
     case BLE_GAP_EVENT_CONN_CANCEL:
         break;
 
     case BLE_GAP_EVENT_TERM_FAILURE:
-        ble_gap_test_conn_status = ctxt->term_failure.status;
+        ble_gap_test_conn_status = event->term_failure.status;
+        ble_gap_conn_find(event->term_failure.conn_handle,
+                          &ble_gap_test_conn_desc);
         break;
 
     case BLE_GAP_EVENT_ADV_COMPLETE:
@@ -117,8 +124,10 @@ ble_gap_test_util_connect_cb(int event, struct ble_gap_conn_ctxt *ctxt,
         break;
 
     case BLE_GAP_EVENT_CONN_UPDATE_REQ:
-        ble_gap_test_conn_peer_params = *ctxt->conn_update_req.peer_params;
-        *ctxt->conn_update_req.self_params = ble_gap_test_conn_self_params;
+        ble_gap_test_conn_peer_params = *event->conn_update_req.peer_params;
+        *event->conn_update_req.self_params = ble_gap_test_conn_self_params;
+        ble_gap_conn_find(event->conn_update_req.conn_handle,
+                          &ble_gap_test_conn_desc);
 
         fail_reason = arg;
         if (fail_reason == NULL) {
@@ -128,6 +137,9 @@ ble_gap_test_util_connect_cb(int event, struct ble_gap_conn_ctxt *ctxt,
         }
         break;
 
+    case BLE_GAP_EVENT_MTU:
+        break;
+
     default:
         TEST_ASSERT_FATAL(0);
         break;
@@ -165,7 +177,9 @@ ble_gap_test_util_verify_tx_add_wl(struct ble_gap_white_entry *entry)
 }
 
 static void
-ble_gap_test_util_verify_tx_set_scan_params(uint16_t itvl,
+ble_gap_test_util_verify_tx_set_scan_params(uint8_t own_addr_type,
+                                            uint8_t scan_type,
+                                            uint16_t itvl,
                                             uint16_t scan_window,
                                             uint8_t filter_policy)
 {
@@ -176,15 +190,16 @@ ble_gap_test_util_verify_tx_set_scan_params(uint16_t itvl,
                                            BLE_HCI_OCF_LE_SET_SCAN_PARAMS,
                                            &param_len);
     TEST_ASSERT(param_len == BLE_HCI_SET_SCAN_PARAM_LEN);
-    TEST_ASSERT(param[0] == BLE_HCI_SCAN_TYPE_ACTIVE);
+    TEST_ASSERT(param[0] == scan_type);
     TEST_ASSERT(le16toh(param + 1) == itvl);
     TEST_ASSERT(le16toh(param + 3) == scan_window);
-    TEST_ASSERT(param[5] == BLE_HCI_ADV_OWN_ADDR_PUBLIC);
+    TEST_ASSERT(param[5] == own_addr_type);
     TEST_ASSERT(param[6] == filter_policy);
 }
 
 static void
-ble_gap_test_util_verify_tx_scan_enable(uint8_t enable)
+ble_gap_test_util_verify_tx_scan_enable(uint8_t enable,
+                                        uint8_t filter_duplicates)
 {
     uint8_t param_len;
     uint8_t *param;
@@ -194,26 +209,11 @@ ble_gap_test_util_verify_tx_scan_enable(uint8_t enable)
                                            &param_len);
     TEST_ASSERT(param_len == BLE_HCI_SET_SCAN_ENABLE_LEN);
     TEST_ASSERT(param[0] == enable);
+    TEST_ASSERT(param[1] == filter_duplicates);
 }
 
 static void
-ble_gap_test_util_verify_tx_create_conn(uint8_t filter_policy)
-{
-    uint8_t param_len;
-    uint8_t *param;
-
-    param = ble_hs_test_util_verify_tx_hci(BLE_HCI_OGF_LE,
-                                           BLE_HCI_OCF_LE_CREATE_CONN,
-                                           &param_len);
-    TEST_ASSERT(param_len == BLE_HCI_CREATE_CONN_LEN);
-
-    TEST_ASSERT(param[4] == filter_policy);
-
-    /* XXX: Verify other fields. */
-}
-
-static void
-ble_gap_test_util_verify_tx_create_conn_cancel(void)
+ble_hs_test_util_verify_tx_create_conn_cancel(void)
 {
     uint8_t param_len;
 
@@ -251,17 +251,6 @@ ble_gap_test_util_verify_tx_adv_params(void)
 }
 
 static void
-ble_gap_test_util_verify_tx_rd_pwr(void)
-{
-    uint8_t param_len;
-
-    ble_hs_test_util_verify_tx_hci(BLE_HCI_OGF_LE,
-                                   BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR,
-                                   &param_len);
-    TEST_ASSERT(param_len == 0);
-}
-
-static void
 ble_gap_test_util_verify_tx_adv_data(void)
 {
     uint8_t param_len;
@@ -384,13 +373,13 @@ ble_gap_test_util_rx_param_req(struct ble_gap_upd_params *params, int pos,
     evt.itvl_max = params->itvl_max;
     evt.latency = params->latency;
     evt.timeout = params->supervision_timeout;
- 
+
     if (pos) {
-        opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                      BLE_HCI_OCF_LE_REM_CONN_PARAM_RR);
+        opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REM_CONN_PARAM_RR);
     } else {
-        opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                      BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR);
+        opcode = ble_hs_hci_util_opcode_join(
+            BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR);
     }
     if (*cmd_idx == cmd_fail_idx) {
         hci_status = fail_status;
@@ -442,7 +431,7 @@ ble_gap_test_util_wl_set(struct ble_gap_white_entry *white_list,
     }
 }
 
-TEST_CASE(ble_gap_test_case_conn_wl_bad_args)
+TEST_CASE(ble_gap_test_case_wl_bad_args)
 {
     int rc;
 
@@ -461,8 +450,9 @@ TEST_CASE(ble_gap_test_case_conn_wl_bad_args)
     TEST_ASSERT(rc == BLE_HS_EINVAL);
 
     /*** White-list-using connection in progress. */
-    rc = ble_hs_test_util_conn_initiate(BLE_GAP_ADDR_TYPE_WL, NULL, NULL,
-                                        ble_gap_test_util_connect_cb, NULL, 0);
+    rc = ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC,
+                                  BLE_GAP_ADDR_TYPE_WL, NULL, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
     TEST_ASSERT(rc == 0);
 
     rc = ble_hs_test_util_wl_set(
@@ -473,7 +463,7 @@ TEST_CASE(ble_gap_test_case_conn_wl_bad_args)
     TEST_ASSERT(rc == BLE_HS_EBUSY);
 }
 
-TEST_CASE(ble_gap_test_case_conn_wl_ctlr_fail)
+TEST_CASE(ble_gap_test_case_wl_ctlr_fail)
 {
     int i;
 
@@ -491,7 +481,7 @@ TEST_CASE(ble_gap_test_case_conn_wl_ctlr_fail)
     }
 }
 
-TEST_CASE(ble_gap_test_case_conn_wl_good)
+TEST_CASE(ble_gap_test_case_wl_good)
 {
     struct ble_gap_white_entry white_list[] = {
         { BLE_ADDR_TYPE_PUBLIC, { 1, 2, 3, 4, 5, 6 } },
@@ -504,11 +494,13 @@ TEST_CASE(ble_gap_test_case_conn_wl_good)
     ble_gap_test_util_wl_set(white_list, white_list_count, 0, 0);
 }
 
-TEST_SUITE(ble_gap_test_suite_conn_wl)
+TEST_SUITE(ble_gap_test_suite_wl)
 {
-    ble_gap_test_case_conn_wl_good();
-    ble_gap_test_case_conn_wl_bad_args();
-    ble_gap_test_case_conn_wl_ctlr_fail();
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gap_test_case_wl_good();
+    ble_gap_test_case_wl_bad_args();
+    ble_gap_test_case_wl_ctlr_fail();
 }
 
 /*****************************************************************************
@@ -516,73 +508,85 @@ TEST_SUITE(ble_gap_test_suite_conn_wl)
  *****************************************************************************/
 
 static int
-ble_gap_test_util_disc(uint8_t disc_mode, uint8_t *peer_addr,
-                       struct ble_hs_adv *adv, int cmd_fail_idx,
+ble_gap_test_util_disc(uint8_t own_addr_type,
+                       const struct ble_gap_disc_params *disc_params,
+                       struct ble_gap_disc_desc *desc, int cmd_fail_idx,
                        uint8_t fail_status)
 {
     int rc;
 
     ble_gap_test_util_init();
 
+    TEST_ASSERT(!ble_gap_disc_active());
+
     /* Begin the discovery procedure. */
-    rc = ble_hs_test_util_disc(0, disc_mode, BLE_HCI_SCAN_TYPE_ACTIVE,
-                               BLE_HCI_SCAN_FILT_NO_WL,
+    rc = ble_hs_test_util_disc(own_addr_type, BLE_HS_FOREVER, disc_params,
                                ble_gap_test_util_disc_cb, NULL, cmd_fail_idx,
                                fail_status);
     TEST_ASSERT(rc == BLE_HS_HCI_ERR(fail_status));
     if (rc == 0) {
         TEST_ASSERT(ble_gap_master_in_progress());
-        ble_gap_rx_adv_report(adv);
+        ble_gap_rx_adv_report(desc);
     } else {
-        TEST_ASSERT(ble_gap_test_disc_status == -1);
+        TEST_ASSERT(ble_gap_test_disc_event_type == -1);
     }
 
     if (cmd_fail_idx > 0) {
         /* Verify tx of set scan parameters command. */
         ble_gap_test_util_verify_tx_set_scan_params(
-            30 * 1000 / BLE_HCI_ADV_ITVL,
-            30 * 1000 / BLE_HCI_SCAN_ITVL,
-            BLE_HCI_SCAN_FILT_NO_WL);
+            own_addr_type,
+            disc_params->passive ?
+                BLE_HCI_SCAN_TYPE_PASSIVE :
+                BLE_HCI_SCAN_TYPE_ACTIVE,
+            disc_params->itvl,
+            disc_params->window,
+            disc_params->filter_policy);
     }
 
     if (cmd_fail_idx > 1) {
         /* Verify tx of scan enable command. */
-        ble_gap_test_util_verify_tx_scan_enable(1);
+        ble_gap_test_util_verify_tx_scan_enable(
+            1, disc_params->filter_duplicates);
+    }
+
+    if (rc == 0) {
+        TEST_ASSERT(ble_gap_disc_active());
     }
 
     return rc;
 }
 
-TEST_CASE(ble_gap_test_case_conn_disc_bad_args)
+TEST_CASE(ble_gap_test_case_disc_bad_args)
 {
+    struct ble_gap_disc_params params;
     int rc;
 
+    params.itvl = 0;
+    params.window = 0;
+    params.filter_policy = BLE_HCI_SCAN_FILT_NO_WL;
+    params.limited = 0;
+    params.passive = 0;
+    params.filter_duplicates = 0;
+
     ble_gap_test_util_init();
 
-    /*** Invalid discovery mode. */
-    rc = ble_gap_disc(0, BLE_GAP_DISC_MODE_NON, BLE_HCI_SCAN_TYPE_ACTIVE,
-                      BLE_HCI_SCAN_FILT_NO_WL, BLE_ADDR_TYPE_PUBLIC, ble_gap_test_util_disc_cb,
-                      NULL);
+    /*** Invalid filter policy. */
+    params.filter_policy = 6;
+    rc = ble_gap_disc(BLE_ADDR_TYPE_PUBLIC, 0, &params,
+                      ble_gap_test_util_disc_cb, NULL);
     TEST_ASSERT(rc == BLE_HS_EINVAL);
-
-    /*** Master operation already in progress. */
-    rc = ble_hs_test_util_conn_initiate(BLE_GAP_ADDR_TYPE_WL, NULL, NULL,
-                                        ble_gap_test_util_connect_cb, NULL, 0);
-    rc = ble_gap_disc(0, BLE_GAP_DISC_MODE_GEN, BLE_HCI_SCAN_TYPE_ACTIVE,
-                      BLE_HCI_SCAN_FILT_NO_WL, BLE_ADDR_TYPE_PUBLIC, ble_gap_test_util_disc_cb,
-                      NULL);
-    TEST_ASSERT(rc == BLE_HS_EALREADY);
 }
 
-TEST_CASE(ble_gap_test_case_conn_disc_good)
+TEST_CASE(ble_gap_test_case_disc_good)
 {
     uint8_t adv_data[32];
     uint8_t flags;
+    uint8_t own_addr_type;
+    int passive;
+    int limited;
     int rc;
-    int d;
 
-    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
-    struct ble_hs_adv adv = {
+    struct ble_gap_disc_desc desc = {
         .event_type = BLE_HCI_ADV_TYPE_ADV_IND,
         .addr_type = BLE_ADDR_TYPE_PUBLIC,
         .length_data = 0,
@@ -590,56 +594,101 @@ TEST_CASE(ble_gap_test_case_conn_disc_good)
         .addr = { 1, 2, 3, 4, 5, 6 },
         .data = adv_data,
     };
+    struct ble_gap_disc_params disc_params = {
+        .itvl = BLE_GAP_SCAN_SLOW_INTERVAL1,
+        .window = BLE_GAP_SCAN_SLOW_WINDOW1,
+        .filter_policy = BLE_HCI_CONN_FILT_NO_WL,
+        .limited = 0,
+        .passive = 0,
+        .filter_duplicates = 0,
+    };
 
     flags = BLE_HS_ADV_F_DISC_LTD;
     rc = ble_hs_adv_set_flat(BLE_HS_ADV_TYPE_FLAGS, 1, &flags,
-                             adv.data, &adv.length_data,
+                             desc.data, &desc.length_data,
                              sizeof adv_data);
     TEST_ASSERT_FATAL(rc == 0);
 
-    for (d = BLE_GAP_DISC_MODE_LTD; d < BLE_GAP_DISC_MODE_MAX; d++) {
-        ble_gap_test_util_disc(d, peer_addr, &adv, -1, 0);
+    for (own_addr_type = 0;
+         own_addr_type <= BLE_ADDR_TYPE_RPA_RND_DEFAULT;
+         own_addr_type++)
+    for (passive = 0; passive <= 1; passive++)
+    for (limited = 0; limited <= 1; limited++) {
+        disc_params.passive = passive;
+        disc_params.limited = limited;
+        ble_gap_test_util_disc(own_addr_type, &disc_params, &desc, -1, 0);
 
         TEST_ASSERT(ble_gap_master_in_progress());
-        TEST_ASSERT(ble_gap_test_disc_event == BLE_GAP_EVENT_DISC_SUCCESS);
-        TEST_ASSERT(ble_gap_test_disc_status == 0);
+        TEST_ASSERT(ble_gap_test_disc_event_type == BLE_GAP_EVENT_DISC);
         TEST_ASSERT(ble_gap_test_disc_desc.event_type ==
                     BLE_HCI_ADV_TYPE_ADV_IND);
-        TEST_ASSERT(ble_gap_test_disc_desc.addr_type == BLE_ADDR_TYPE_PUBLIC);
+        TEST_ASSERT(ble_gap_test_disc_desc.addr_type ==
+                    BLE_ADDR_TYPE_PUBLIC);
         TEST_ASSERT(ble_gap_test_disc_desc.length_data == 3);
         TEST_ASSERT(ble_gap_test_disc_desc.rssi == 0);
-        TEST_ASSERT(memcmp(ble_gap_test_disc_desc.addr, adv.addr, 6) == 0);
+        TEST_ASSERT(memcmp(ble_gap_test_disc_desc.addr, desc.addr, 6) == 0);
         TEST_ASSERT(ble_gap_test_disc_arg == NULL);
+
     }
 }
 
-TEST_CASE(ble_gap_test_case_conn_disc_bad_flags)
+TEST_CASE(ble_gap_test_case_disc_ltd_mismatch)
 {
-    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
-    struct ble_hs_adv adv = {
+    int rc;
+    struct ble_gap_disc_desc desc = {
         .event_type = BLE_HCI_ADV_TYPE_ADV_IND,
         .addr_type = BLE_ADDR_TYPE_PUBLIC,
         .length_data = 0,
         .rssi = 0,
         .addr = { 1, 2, 3, 4, 5, 6 },
-        .data = NULL,
+        .data = (uint8_t[BLE_HCI_MAX_ADV_DATA_LEN]){
+            2, 
+            BLE_HS_ADV_TYPE_FLAGS,
+            BLE_HS_ADV_F_DISC_GEN,
+        },
+    };
+    struct ble_gap_disc_params disc_params = {
+        .itvl = BLE_GAP_SCAN_SLOW_INTERVAL1,
+        .window = BLE_GAP_SCAN_SLOW_WINDOW1,
+        .filter_policy = BLE_HCI_CONN_FILT_NO_WL,
+        .limited = 1,
+        .passive = 0,
+        .filter_duplicates = 0,
     };
 
-    ble_gap_test_util_disc(BLE_GAP_DISC_MODE_LTD, peer_addr, &adv, -1, 0);
+    rc = ble_gap_test_util_disc(BLE_ADDR_TYPE_PUBLIC, &disc_params, &desc,
+                                -1, 0);
+    TEST_ASSERT(rc == 0);
     TEST_ASSERT(ble_gap_master_in_progress());
 
-    /* Verify that the report was ignored becuase of a mismatched LTD flag. */
-    TEST_ASSERT(ble_gap_test_disc_event == -1);
+    /* Verify that the report was ignored because of a mismatched LTD flag. */
+    TEST_ASSERT(ble_gap_test_disc_event_type == -1);
+
+    /* Stop the scan and swap the flags. */
+    rc = ble_hs_test_util_disc_cancel(0);
+    TEST_ASSERT(rc == 0);
+
+    desc.data[2] = BLE_HS_ADV_F_DISC_LTD;
+    disc_params.limited = 0;
+    rc = ble_gap_test_util_disc(BLE_ADDR_TYPE_PUBLIC, &disc_params, &desc,
+                                -1, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(ble_gap_master_in_progress());
+
+    /* This time we should have reported the advertisement; general discovery
+     * hears everything.
+     */
+    TEST_ASSERT(ble_gap_test_disc_event_type == BLE_GAP_EVENT_DISC);
+
 }
 
-TEST_CASE(ble_gap_test_case_conn_disc_hci_fail)
+TEST_CASE(ble_gap_test_case_disc_hci_fail)
 {
     int fail_idx;
+    int limited;
     int rc;
-    int d;
 
-    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
-    struct ble_hs_adv adv = {
+    struct ble_gap_disc_desc desc = {
         .event_type = BLE_HCI_ADV_TYPE_ADV_IND,
         .addr_type = BLE_ADDR_TYPE_PUBLIC,
         .length_data = 0,
@@ -647,32 +696,127 @@ TEST_CASE(ble_gap_test_case_conn_disc_hci_fail)
         .addr = { 1, 2, 3, 4, 5, 6 },
         .data = NULL,
     };
+    struct ble_gap_disc_params disc_params = {
+        .itvl = BLE_GAP_SCAN_SLOW_INTERVAL1,
+        .window = BLE_GAP_SCAN_SLOW_WINDOW1,
+        .filter_policy = BLE_HCI_CONN_FILT_NO_WL,
+        .limited = 0,
+        .passive = 0,
+        .filter_duplicates = 0,
+    };
+
+    for (limited = 0; limited <= 1; limited++) {
+        disc_params.limited = limited;
 
-    for (d = BLE_GAP_DISC_MODE_LTD; d < BLE_GAP_DISC_MODE_MAX; d++) {
         for (fail_idx = 0; fail_idx < 2; fail_idx++) {
-            rc = ble_gap_test_util_disc(d, peer_addr, &adv, fail_idx,
-                                        BLE_ERR_UNSUPPORTED);
+            rc = ble_gap_test_util_disc(BLE_ADDR_TYPE_PUBLIC, &disc_params,
+                                        &desc, fail_idx, BLE_ERR_UNSUPPORTED);
             TEST_ASSERT(rc == BLE_HS_HCI_ERR(BLE_ERR_UNSUPPORTED));
             TEST_ASSERT(!ble_gap_master_in_progress());
         }
     }
 }
 
-TEST_SUITE(ble_gap_test_suite_conn_disc)
+static void
+ble_gap_test_util_disc_dflts_once(int limited)
+{
+    struct ble_gap_disc_params params;
+    uint16_t exp_window;
+    uint16_t exp_itvl;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    memset(&params, 0, sizeof params);
+    params.limited = limited;
+
+    rc = ble_hs_test_util_disc(BLE_ADDR_TYPE_PUBLIC, 0, &params,
+                               ble_gap_test_util_disc_cb, NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    if (limited) {
+        exp_itvl = BLE_GAP_LIM_DISC_SCAN_INT;
+        exp_window = BLE_GAP_LIM_DISC_SCAN_WINDOW;
+    } else {
+        exp_itvl = BLE_GAP_SCAN_FAST_INTERVAL_MIN;
+        exp_window = BLE_GAP_SCAN_FAST_WINDOW;
+    }
+    ble_gap_test_util_verify_tx_set_scan_params(
+        BLE_ADDR_TYPE_PUBLIC,
+        BLE_HCI_SCAN_TYPE_ACTIVE,
+        exp_itvl,
+        exp_window,
+        BLE_HCI_SCAN_FILT_NO_WL);
+
+    ble_gap_test_util_verify_tx_scan_enable(1, 0);
+}
+
+TEST_CASE(ble_gap_test_case_disc_dflts)
+{
+    ble_gap_test_util_disc_dflts_once(0);
+    ble_gap_test_util_disc_dflts_once(1);
+}
+
+TEST_CASE(ble_gap_test_case_disc_already)
 {
-    ble_gap_test_case_conn_disc_bad_args();
-    ble_gap_test_case_conn_disc_good();
-    ble_gap_test_case_conn_disc_bad_flags();
-    ble_gap_test_case_conn_disc_hci_fail();
+    static const struct ble_gap_disc_params disc_params = { 0 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a discovery procedure. */
+    rc = ble_hs_test_util_disc(BLE_ADDR_TYPE_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, ble_gap_test_util_disc_cb,
+                               NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure host indicates BLE_HS_EALREADY if we try to discover. */
+    rc = ble_gap_disc(BLE_ADDR_TYPE_PUBLIC, BLE_HS_FOREVER, &disc_params,
+                               ble_gap_test_util_disc_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EALREADY);
+}
+
+TEST_CASE(ble_gap_test_case_disc_busy)
+{
+    static const struct ble_gap_disc_params disc_params = { 0 };
+    static const uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a connect procedure. */
+    rc = ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                  peer_addr, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure host indicates BLE_HS_EBUSY if we try to discover. */
+    rc = ble_gap_disc(BLE_ADDR_TYPE_PUBLIC, BLE_HS_FOREVER, &disc_params,
+                               ble_gap_test_util_disc_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EBUSY);
+}
+
+TEST_SUITE(ble_gap_test_suite_disc)
+{
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gap_test_case_disc_bad_args();
+    ble_gap_test_case_disc_good();
+    ble_gap_test_case_disc_ltd_mismatch();
+    ble_gap_test_case_disc_hci_fail();
+    ble_gap_test_case_disc_dflts();
+    ble_gap_test_case_disc_already();
+    ble_gap_test_case_disc_busy();
 }
 
 /*****************************************************************************
  * $direct connect                                                           *
  *****************************************************************************/
 
-TEST_CASE(ble_gap_test_case_conn_dir_good)
+TEST_CASE(ble_gap_test_case_conn_gen_good)
 {
     struct hci_le_conn_complete evt;
+    struct ble_gap_conn_params params;
     int rc;
 
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
@@ -680,15 +824,25 @@ TEST_CASE(ble_gap_test_case_conn_dir_good)
     ble_gap_test_util_init();
 
     TEST_ASSERT(!ble_gap_master_in_progress());
-
-    rc = ble_hs_test_util_conn_initiate(BLE_ADDR_TYPE_PUBLIC, peer_addr, NULL,
-                                        ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT(!ble_gap_conn_active());
+
+    params.scan_itvl = 0x12;
+    params.scan_window = 0x11;
+    params.itvl_min = 25;
+    params.itvl_max = 26;
+    params.latency = 1;
+    params.supervision_timeout = 20;
+    params.min_ce_len = 3;
+    params.max_ce_len = 4;
+
+    rc = ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC,
+                                  BLE_ADDR_TYPE_PUBLIC, peer_addr, 0, &params,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
     TEST_ASSERT(rc == 0);
 
     TEST_ASSERT(ble_gap_master_in_progress());
+    TEST_ASSERT(ble_gap_conn_active());
 
-    /* Verify tx of create connection command. */
-    ble_gap_test_util_verify_tx_create_conn(BLE_HCI_CONN_FILT_NO_WL);
     TEST_ASSERT(ble_gap_master_in_progress());
     TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == BLE_HS_ENOTCONN);
 
@@ -704,14 +858,15 @@ TEST_CASE(ble_gap_test_case_conn_dir_good)
 
     TEST_ASSERT(!ble_gap_master_in_progress());
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
 
     TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == 0);
 }
 
-TEST_CASE(ble_gap_test_case_conn_dir_bad_args)
+TEST_CASE(ble_gap_test_case_conn_gen_bad_args)
 {
     int rc;
 
@@ -720,29 +875,92 @@ TEST_CASE(ble_gap_test_case_conn_dir_bad_args)
     TEST_ASSERT(!ble_gap_master_in_progress());
 
     /*** Invalid address type. */
-    rc = ble_gap_conn_initiate(5, ((uint8_t[]){ 1, 2, 3, 4, 5, 6 }), NULL,
-                               ble_gap_test_util_connect_cb, NULL);
+    rc = ble_gap_connect(BLE_ADDR_TYPE_PUBLIC, 5,
+                         ((uint8_t[]){ 1, 2, 3, 4, 5, 6 }), 0, NULL,
+                         ble_gap_test_util_connect_cb, NULL);
     TEST_ASSERT(rc == BLE_HS_EINVAL);
     TEST_ASSERT(!ble_gap_master_in_progress());
 
     /*** Connection already in progress. */
-    rc = ble_hs_test_util_conn_initiate(BLE_ADDR_TYPE_PUBLIC,
-                                        ((uint8_t[]){ 1, 2, 3, 4, 5, 6 }),
-                                        NULL, ble_gap_test_util_connect_cb,
-                                        NULL, 0);
+    rc = ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC,
+                                  BLE_ADDR_TYPE_PUBLIC,
+                                  ((uint8_t[]){ 1, 2, 3, 4, 5, 6 }), 0,
+                                  NULL, ble_gap_test_util_connect_cb,
+                                  NULL, 0);
     TEST_ASSERT(rc == 0);
     TEST_ASSERT(ble_gap_master_in_progress());
 
-    rc = ble_gap_conn_initiate(BLE_ADDR_TYPE_PUBLIC,
-                               ((uint8_t[]){ 2, 3, 4, 5, 6, 7 }), NULL,
-                               ble_gap_test_util_connect_cb, NULL);
+    rc = ble_gap_connect(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                         ((uint8_t[]){ 1, 2, 3, 4, 5, 6 }), 0, NULL,
+                         ble_gap_test_util_connect_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EALREADY);
+}
+
+TEST_CASE(ble_gap_test_case_conn_gen_dflt_params)
+{
+    static const uint8_t peer_addr[6] = { 2, 3, 8, 6, 6, 1 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    rc = ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC,
+                                  BLE_ADDR_TYPE_PUBLIC, peer_addr, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT(rc == 0);
+}
+
+TEST_CASE(ble_gap_test_case_conn_gen_already)
+{
+    static const struct ble_gap_conn_params conn_params = { 0 };
+    static const uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a connect procedure. */
+    rc = ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                  peer_addr, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure host indicates BLE_HS_EALREADY if we try to connect. */
+    rc = ble_gap_connect(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                         peer_addr, BLE_HS_FOREVER, &conn_params,
+                         ble_gap_test_util_connect_cb, NULL);
     TEST_ASSERT(rc == BLE_HS_EALREADY);
 }
 
-TEST_SUITE(ble_gap_test_suite_conn_dir)
+TEST_CASE(ble_gap_test_case_conn_gen_busy)
 {
-    ble_gap_test_case_conn_dir_good();
-    ble_gap_test_case_conn_dir_bad_args();
+    static const struct ble_gap_disc_params disc_params = { 0 };
+    static const struct ble_gap_conn_params conn_params = { 0 };
+    static const uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Start a discovery procedure. */
+    rc = ble_hs_test_util_disc(BLE_ADDR_TYPE_PUBLIC, BLE_HS_FOREVER,
+                               &disc_params, ble_gap_test_util_disc_cb,
+                               NULL, -1, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure host indicates BLE_HS_EBUSY if we try to connect. */
+    rc = ble_gap_connect(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                         peer_addr, BLE_HS_FOREVER, &conn_params,
+                         ble_gap_test_util_connect_cb, NULL);
+    TEST_ASSERT(rc == BLE_HS_EBUSY);
+}
+
+TEST_SUITE(ble_gap_test_suite_conn_gen)
+{
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gap_test_case_conn_gen_good();
+    ble_gap_test_case_conn_gen_bad_args();
+    ble_gap_test_case_conn_gen_dflt_params();
+    ble_gap_test_case_conn_gen_already();
+    ble_gap_test_case_conn_gen_busy();
 }
 
 /*****************************************************************************
@@ -750,26 +968,17 @@ TEST_SUITE(ble_gap_test_suite_conn_dir)
  *****************************************************************************/
 
 static void
-ble_gap_test_util_conn_cancel(uint8_t *peer_addr, uint8_t hci_status)
+ble_gap_test_util_conn_cancel(uint8_t hci_status)
 {
     struct hci_le_conn_complete evt;
     int rc;
 
-    ble_gap_test_util_init();
-
-    /* Begin creating a connection. */
-    rc = ble_hs_test_util_conn_initiate(BLE_ADDR_TYPE_PUBLIC, peer_addr, NULL,
-                                        ble_gap_test_util_connect_cb, NULL, 0);
-    TEST_ASSERT(rc == 0);
-    TEST_ASSERT(ble_gap_master_in_progress());
-    ble_gap_test_util_verify_tx_create_conn(BLE_HCI_CONN_FILT_NO_WL);
-
     /* Initiate cancel procedure. */
     rc = ble_hs_test_util_conn_cancel(hci_status);
     TEST_ASSERT(rc == BLE_HS_HCI_ERR(hci_status));
 
     /* Verify tx of cancel create connection command. */
-    ble_gap_test_util_verify_tx_create_conn_cancel();
+    ble_hs_test_util_verify_tx_create_conn_cancel();
     if (rc != 0) {
         return;
     }
@@ -779,12 +988,29 @@ ble_gap_test_util_conn_cancel(uint8_t *peer_addr, uint8_t hci_status)
     memset(&evt, 0, sizeof evt);
     evt.subevent_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE;
     evt.status = BLE_ERR_UNK_CONN_ID;
-    evt.connection_handle = 2;
-    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER;
-    memcpy(evt.peer_addr, peer_addr, 6);
     rc = ble_gap_rx_conn_complete(&evt);
     TEST_ASSERT(rc == 0);
     TEST_ASSERT(!ble_gap_master_in_progress());
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_CANCEL);
+}
+
+static void
+ble_gap_test_util_conn_and_cancel(uint8_t *peer_addr, uint8_t hci_status)
+{
+    int rc;
+
+    ble_gap_test_util_init();
+
+    /* Begin creating a connection. */
+    rc = ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC,
+                                  BLE_ADDR_TYPE_PUBLIC, peer_addr, 0, NULL,
+                                  ble_gap_test_util_connect_cb, NULL, 0);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(ble_gap_master_in_progress());
+
+    /* Initiate cancel procedure. */
+    ble_gap_test_util_conn_cancel(hci_status);
     TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == BLE_HS_ENOTCONN);
 }
 
@@ -797,16 +1023,16 @@ TEST_CASE(ble_gap_test_case_conn_cancel_bad_args)
     /* Initiate cancel procedure with no connection in progress. */
     TEST_ASSERT(!ble_gap_master_in_progress());
     rc = ble_hs_test_util_conn_cancel(0);
-    TEST_ASSERT(rc == BLE_HS_ENOENT);
+    TEST_ASSERT(rc == BLE_HS_EALREADY);
 }
 
 TEST_CASE(ble_gap_test_case_conn_cancel_good)
 {
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
 
-    ble_gap_test_util_conn_cancel(peer_addr, 0);
+    ble_gap_test_util_conn_and_cancel(peer_addr, 0);
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_CANCEL);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_CANCEL);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == BLE_HS_CONN_HANDLE_NONE);
 }
 
@@ -817,12 +1043,12 @@ TEST_CASE(ble_gap_test_case_conn_cancel_ctlr_fail)
 
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
 
-    ble_gap_test_util_conn_cancel(peer_addr, BLE_ERR_REPEATED_ATTEMPTS);
+    ble_gap_test_util_conn_and_cancel(peer_addr, BLE_ERR_REPEATED_ATTEMPTS);
 
     /* Make sure the host didn't invoke the application callback.  The cancel
      * failure was indicated via the return code from the gap call.
      */
-    TEST_ASSERT(ble_gap_test_conn_event == -1);
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
 
     /* Allow connection complete to succeed. */
     memset(&evt, 0, sizeof evt);
@@ -836,7 +1062,7 @@ TEST_CASE(ble_gap_test_case_conn_cancel_ctlr_fail)
 
     TEST_ASSERT(!ble_gap_master_in_progress());
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
     TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
                        peer_addr, 6) == 0);
@@ -846,6 +1072,8 @@ TEST_CASE(ble_gap_test_case_conn_cancel_ctlr_fail)
 
 TEST_SUITE(ble_gap_test_suite_conn_cancel)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gap_test_case_conn_cancel_good();
     ble_gap_test_case_conn_cancel_bad_args();
     ble_gap_test_case_conn_cancel_ctlr_fail();
@@ -870,7 +1098,7 @@ ble_gap_test_util_terminate(uint8_t *peer_addr, uint8_t hci_status)
     /* Reset the callback event code; we don't care about the successful
      * connection in this test.
      */
-    ble_gap_test_conn_event = -1;
+    ble_gap_test_event.type = -1;
 
     /* Terminate the connection. */
     rc = ble_hs_test_util_conn_terminate(2, hci_status);
@@ -906,12 +1134,14 @@ TEST_CASE(ble_gap_test_case_conn_terminate_good)
 
     ble_gap_test_util_terminate(peer_addr, 0);
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_DISCONNECT);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_DISCONNECT);
     TEST_ASSERT(ble_gap_test_conn_status ==
                 BLE_HS_HCI_ERR(BLE_ERR_CONN_TERM_LOCAL));
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(ble_gap_test_conn_desc.peer_id_addr_type == BLE_ADDR_TYPE_PUBLIC);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.peer_id_addr_type ==
+                BLE_ADDR_TYPE_PUBLIC);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_arg == NULL);
 
     TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == BLE_HS_ENOTCONN);
@@ -945,12 +1175,14 @@ TEST_CASE(ble_gap_test_case_conn_terminate_ctlr_fail)
     evt.reason = 0;
     ble_gap_rx_disconn_complete(&evt);
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_TERM_FAILURE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_TERM_FAILURE);
     TEST_ASSERT(ble_gap_test_conn_status ==
                 BLE_HS_HCI_ERR(BLE_ERR_UNSUPPORTED));
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(ble_gap_test_conn_desc.peer_id_addr_type == BLE_ADDR_TYPE_PUBLIC);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(ble_gap_test_conn_desc.peer_id_addr_type ==
+                BLE_ADDR_TYPE_PUBLIC);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_arg == NULL);
 
     TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == 0);
@@ -963,13 +1195,15 @@ TEST_CASE(ble_gap_test_case_conn_terminate_hci_fail)
 
     ble_gap_test_util_terminate(peer_addr, BLE_ERR_REPEATED_ATTEMPTS);
 
-    TEST_ASSERT(ble_gap_test_conn_event == -1);
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
     TEST_ASSERT(ble_hs_atomic_conn_flags(2, NULL) == 0);
     TEST_ASSERT(!ble_gap_master_in_progress());
 }
 
 TEST_SUITE(ble_gap_test_suite_conn_terminate)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gap_test_case_conn_terminate_bad_args();
     ble_gap_test_case_conn_terminate_good();
     ble_gap_test_case_conn_terminate_ctlr_fail();
@@ -977,38 +1211,170 @@ TEST_SUITE(ble_gap_test_suite_conn_terminate)
 }
 
 /*****************************************************************************
+ * $conn find                                                                *
+ *****************************************************************************/
+
+TEST_CASE(ble_gap_test_case_conn_find)
+{
+
+    struct ble_gap_conn_desc desc;
+    struct ble_hs_conn *conn;
+    uint8_t pub_addr[6];
+    int rc;
+
+    /*** We are master; public addresses. */
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_rpa_conn(8,
+                                     BLE_ADDR_TYPE_PUBLIC,
+                                     ((uint8_t[6]){0,0,0,0,0,0}),
+                                     BLE_ADDR_TYPE_PUBLIC,
+                                     ((uint8_t[6]){2,3,4,5,6,7}),
+                                     ((uint8_t[6]){0,0,0,0,0,0}),
+                                     ble_gap_test_util_connect_cb,
+                                     NULL);
+
+
+    rc = ble_hs_id_copy_addr(BLE_ADDR_TYPE_PUBLIC, pub_addr, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    rc = ble_gap_conn_find(8, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.conn_handle == 8);
+    TEST_ASSERT(desc.our_id_addr_type == BLE_ADDR_TYPE_PUBLIC);
+    TEST_ASSERT(desc.our_ota_addr_type == BLE_ADDR_TYPE_PUBLIC);
+    TEST_ASSERT(desc.peer_ota_addr_type == BLE_ADDR_TYPE_PUBLIC);
+    TEST_ASSERT(desc.role == BLE_GAP_ROLE_MASTER);
+    TEST_ASSERT(memcmp(desc.our_ota_addr, pub_addr, 6) == 0);
+    TEST_ASSERT(memcmp(desc.our_id_addr, pub_addr, 6) == 0);
+    TEST_ASSERT(memcmp(desc.peer_ota_addr,
+                       ((uint8_t[6]){2,3,4,5,6,7}), 6) == 0);
+    TEST_ASSERT(memcmp(desc.peer_id_addr,
+                       ((uint8_t[6]){2,3,4,5,6,7}), 6) == 0);
+    TEST_ASSERT(desc.conn_itvl == BLE_GAP_INITIAL_CONN_ITVL_MAX);
+    TEST_ASSERT(desc.conn_latency == BLE_GAP_INITIAL_CONN_LATENCY);
+    TEST_ASSERT(desc.supervision_timeout ==
+                BLE_GAP_INITIAL_SUPERVISION_TIMEOUT);
+    TEST_ASSERT(desc.master_clock_accuracy == 0);
+    TEST_ASSERT(!desc.sec_state.encrypted);
+    TEST_ASSERT(!desc.sec_state.authenticated);
+    TEST_ASSERT(!desc.sec_state.bonded);
+
+    /*** Swap roles. */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(8);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    rc = ble_gap_conn_find(8, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.role == BLE_GAP_ROLE_SLAVE);
+
+    /*** We are master; RPAs. */
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_rpa_conn(54,
+                                     BLE_ADDR_TYPE_RPA_PUB_DEFAULT,
+                                     ((uint8_t[6]){0x40,1,2,3,4,5}),
+                                     BLE_ADDR_TYPE_RPA_RND_DEFAULT,
+                                     ((uint8_t[6]){3,4,5,6,7,8}),
+                                     ((uint8_t[6]){0x50,1,2,3,4,5}),
+                                     ble_gap_test_util_connect_cb,
+                                     NULL);
+
+    rc = ble_gap_conn_find(54, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.conn_handle == 54);
+    TEST_ASSERT(desc.our_id_addr_type == BLE_ADDR_TYPE_PUBLIC);
+    TEST_ASSERT(desc.our_ota_addr_type == BLE_ADDR_TYPE_RPA_PUB_DEFAULT);
+    TEST_ASSERT(desc.peer_ota_addr_type == BLE_ADDR_TYPE_RPA_RND_DEFAULT);
+    TEST_ASSERT(desc.role == BLE_GAP_ROLE_MASTER);
+    TEST_ASSERT(memcmp(desc.our_ota_addr,
+                       ((uint8_t[6]){0x40,1,2,3,4,5}), 6) == 0);
+    TEST_ASSERT(memcmp(desc.our_id_addr, pub_addr, 6) == 0);
+    TEST_ASSERT(memcmp(desc.peer_ota_addr,
+                       ((uint8_t[6]){0x50,1,2,3,4,5}), 6) == 0);
+    TEST_ASSERT(memcmp(desc.peer_id_addr,
+                       ((uint8_t[6]){3,4,5,6,7,8}), 6) == 0);
+    TEST_ASSERT(desc.conn_itvl == BLE_GAP_INITIAL_CONN_ITVL_MAX);
+    TEST_ASSERT(desc.conn_latency == BLE_GAP_INITIAL_CONN_LATENCY);
+    TEST_ASSERT(desc.supervision_timeout ==
+                BLE_GAP_INITIAL_SUPERVISION_TIMEOUT);
+    TEST_ASSERT(desc.master_clock_accuracy == 0);
+    TEST_ASSERT(!desc.sec_state.encrypted);
+    TEST_ASSERT(!desc.sec_state.authenticated);
+    TEST_ASSERT(!desc.sec_state.bonded);
+
+    /*** Swap roles. */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(54);
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+    ble_hs_unlock();
+
+    rc = ble_gap_conn_find(54, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.role == BLE_GAP_ROLE_SLAVE);
+}
+
+TEST_SUITE(ble_gap_test_suite_conn_find)
+{
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gap_test_case_conn_find();
+}
+
+/*****************************************************************************
  * $advertise                                                                *
  *****************************************************************************/
 
 static void
-ble_gap_test_util_adv(uint8_t disc_mode, uint8_t conn_mode,
-                      uint8_t *peer_addr, uint8_t peer_addr_type,
-                      int connect_status,
+ble_gap_test_util_adv(uint8_t own_addr_type, uint8_t peer_addr_type,
+                      const uint8_t *peer_addr, uint8_t conn_mode,
+                      uint8_t disc_mode, int connect_status,
                       int cmd_fail_idx, uint8_t fail_status)
 {
     struct hci_le_conn_complete evt;
+    struct ble_gap_adv_params adv_params;
     struct ble_hs_adv_fields adv_fields;
+    uint8_t hci_status;
     int cmd_idx;
     int rc;
 
     ble_gap_test_util_init();
 
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = conn_mode;
+    adv_params.disc_mode = disc_mode;
+
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    cmd_idx = 0;
 
     if (conn_mode != BLE_GAP_CONN_MODE_DIR) {
         memset(&adv_fields, 0, sizeof adv_fields);
         adv_fields.tx_pwr_lvl_is_present = 1;
-        rc = ble_gap_adv_set_fields(&adv_fields);
-        TEST_ASSERT_FATAL(rc == 0);
+        adv_fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
+
+        hci_status = ble_hs_test_util_exp_hci_status(cmd_idx, cmd_fail_idx,
+                                                     fail_status);
+        rc = ble_hs_test_util_adv_set_fields(&adv_fields, hci_status);
+
+        if (adv_fields.tx_pwr_lvl_is_present &&
+            adv_fields.tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO) {
+
+            TEST_ASSERT_FATAL(rc == BLE_HS_HCI_ERR(hci_status));
+            cmd_idx++;
+        }
     }
 
-    rc = ble_hs_test_util_adv_start(disc_mode, conn_mode, peer_addr,
-                                    peer_addr_type, NULL,
-                                    ble_gap_test_util_connect_cb, NULL,
-                                    cmd_fail_idx, fail_status);
-    TEST_ASSERT(rc == BLE_HS_HCI_ERR(fail_status));
+    if (fail_status == 0 || cmd_fail_idx >= cmd_idx) {
+        rc = ble_hs_test_util_adv_start(own_addr_type, peer_addr_type,
+                                        peer_addr, &adv_params,
+                                        ble_gap_test_util_connect_cb, NULL,
+                                        cmd_fail_idx - cmd_idx, fail_status);
 
-    cmd_idx = 0;
+        TEST_ASSERT(rc == BLE_HS_HCI_ERR(fail_status));
+    }
 
     if (fail_status == 0 || cmd_fail_idx >= cmd_idx) {
         /* Verify tx of set advertising params command. */
@@ -1018,12 +1384,6 @@ ble_gap_test_util_adv(uint8_t disc_mode, uint8_t conn_mode,
 
     if (conn_mode != BLE_GAP_CONN_MODE_DIR) {
         if (fail_status == 0 || cmd_fail_idx >= cmd_idx) {
-            /* Verify tx of read tx power command. */
-            ble_gap_test_util_verify_tx_rd_pwr();
-        }
-        cmd_idx++;
-
-        if (fail_status == 0 || cmd_fail_idx >= cmd_idx) {
             /* Verify tx of set advertise data command. */
             ble_gap_test_util_verify_tx_adv_data();
         }
@@ -1042,8 +1402,10 @@ ble_gap_test_util_adv(uint8_t disc_mode, uint8_t conn_mode,
     }
     cmd_idx++;
 
-    if (fail_status == 0 || cmd_fail_idx >= cmd_idx) {
-        TEST_ASSERT(ble_gap_slave_in_progress());
+    if (connect_status != -1 &&
+        (fail_status == 0 || cmd_fail_idx >= cmd_idx)) {
+
+        TEST_ASSERT(ble_gap_adv_active());
 
         /* Receive a connection complete event. */
         if (conn_mode != BLE_GAP_CONN_MODE_NON) {
@@ -1059,74 +1421,164 @@ ble_gap_test_util_adv(uint8_t disc_mode, uint8_t conn_mode,
             if (connect_status == 0 ||
                 connect_status == BLE_ERR_DIR_ADV_TMO) {
 
-                TEST_ASSERT(!ble_gap_slave_in_progress());
+                TEST_ASSERT(!ble_gap_adv_active());
             } else {
-                TEST_ASSERT(ble_gap_slave_in_progress());
+                TEST_ASSERT(ble_gap_adv_active());
             }
         }
     }
 }
 
-TEST_CASE(ble_gap_test_case_conn_adv_bad_args)
+TEST_CASE(ble_gap_test_case_adv_bad_args)
 {
+    struct ble_gap_adv_params adv_params;
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
     int rc;
 
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
 
     /*** Invalid discoverable mode. */
-    rc = ble_hs_test_util_adv_start(-1, BLE_GAP_CONN_MODE_DIR, peer_addr,
-                                    BLE_ADDR_TYPE_PUBLIC, NULL,
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.disc_mode = 43;
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                    peer_addr, &adv_params,
                                     ble_gap_test_util_connect_cb, NULL, 0, 0);
     TEST_ASSERT(rc == BLE_HS_EINVAL);
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
 
     /*** Invalid connectable mode. */
-    rc = ble_hs_test_util_adv_start(BLE_GAP_DISC_MODE_GEN, -1, peer_addr,
-                                    BLE_ADDR_TYPE_PUBLIC, NULL,
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = 27;
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                    peer_addr, &adv_params,
                                     ble_gap_test_util_connect_cb, NULL, 0, 0);
     TEST_ASSERT(rc == BLE_HS_EINVAL);
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
 
     /*** Invalid peer address type with directed advertisable mode. */
-    rc = ble_hs_test_util_adv_start(BLE_GAP_DISC_MODE_GEN,
-                                    BLE_GAP_CONN_MODE_DIR, peer_addr, -1,
-                                    NULL, ble_gap_test_util_connect_cb, NULL,
-                                    0, 0);
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_DIR;
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, 12,
+                                    peer_addr, &adv_params,
+                                    ble_gap_test_util_connect_cb, NULL, 0, 0);
     TEST_ASSERT(rc == BLE_HS_EINVAL);
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
 
     /*** Advertising already in progress. */
-    rc = ble_hs_test_util_adv_start(BLE_GAP_DISC_MODE_GEN,
-                                    BLE_GAP_CONN_MODE_DIR,
-                                    peer_addr, BLE_ADDR_TYPE_PUBLIC, NULL,
+    adv_params = ble_hs_test_util_adv_params;
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                    peer_addr, &adv_params,
                                     ble_gap_test_util_connect_cb, NULL, 0, 0);
     TEST_ASSERT(rc == 0);
-    TEST_ASSERT(ble_gap_slave_in_progress());
+    TEST_ASSERT(ble_gap_adv_active());
 
-    rc = ble_hs_test_util_adv_start(BLE_GAP_DISC_MODE_GEN,
-                                    BLE_GAP_CONN_MODE_DIR,
-                                    peer_addr, BLE_ADDR_TYPE_PUBLIC, NULL,
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                    peer_addr, &adv_params,
                                     ble_gap_test_util_connect_cb, NULL, 0, 0);
     TEST_ASSERT(rc == BLE_HS_EALREADY);
-    TEST_ASSERT(ble_gap_slave_in_progress());
+    TEST_ASSERT(ble_gap_adv_active());
+}
+
+static void
+ble_gap_test_util_adv_verify_dflt_params(uint8_t own_addr_type,
+                                         uint8_t peer_addr_type,
+                                         const uint8_t *peer_addr,
+                                         uint8_t conn_mode,
+                                         uint8_t disc_mode)
+{
+    struct ble_gap_adv_params adv_params;
+    struct hci_adv_params hci_cmd;
+    uint8_t *hci_buf;
+    uint8_t hci_param_len;
+    int rc;
+
+    ble_gap_test_util_init();
+
+    TEST_ASSERT(!ble_gap_adv_active());
+
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = conn_mode;
+    adv_params.disc_mode = disc_mode;
+
+    /* Let stack calculate all default parameters. */
+    adv_params.itvl_min = 0;
+    adv_params.itvl_max = 0;
+    adv_params.channel_map = 0;
+    adv_params.filter_policy = 0;
+    adv_params.high_duty_cycle = 0;
+
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                    peer_addr, &adv_params,
+                                    ble_gap_test_util_connect_cb, NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure default parameters properly filled in. */
+    hci_buf = ble_hs_test_util_verify_tx_hci(BLE_HCI_OGF_LE,
+                                             BLE_HCI_OCF_LE_SET_ADV_PARAMS,
+                                             &hci_param_len);
+    TEST_ASSERT_FATAL(hci_buf != NULL);
+    TEST_ASSERT_FATAL(hci_param_len == BLE_HCI_SET_ADV_PARAM_LEN);
+
+    hci_cmd.adv_itvl_min = le16toh(hci_buf + 0);
+    hci_cmd.adv_itvl_max = le16toh(hci_buf + 2);
+    hci_cmd.adv_type = hci_buf[4];
+    hci_cmd.own_addr_type = hci_buf[5];
+    hci_cmd.peer_addr_type = hci_buf[6];
+    memcpy(hci_cmd.peer_addr, hci_buf + 7, 6);
+    hci_cmd.adv_channel_map = hci_buf[13];
+    hci_cmd.adv_filter_policy = hci_buf[14];
+
+    if (conn_mode == BLE_GAP_CONN_MODE_NON) {
+        TEST_ASSERT(hci_cmd.adv_itvl_min == BLE_GAP_ADV_FAST_INTERVAL2_MIN);
+        TEST_ASSERT(hci_cmd.adv_itvl_max == BLE_GAP_ADV_FAST_INTERVAL2_MAX);
+    } else {
+        TEST_ASSERT(hci_cmd.adv_itvl_min == BLE_GAP_ADV_FAST_INTERVAL1_MIN);
+        TEST_ASSERT(hci_cmd.adv_itvl_max == BLE_GAP_ADV_FAST_INTERVAL1_MAX);
+    }
+
+    if (conn_mode == BLE_GAP_CONN_MODE_NON) {
+        if (disc_mode == BLE_GAP_DISC_MODE_NON) {
+            TEST_ASSERT(hci_cmd.adv_type == BLE_HCI_ADV_TYPE_ADV_NONCONN_IND);
+        } else {
+            TEST_ASSERT(hci_cmd.adv_type == BLE_HCI_ADV_TYPE_ADV_SCAN_IND);
+        }
+    } else if (conn_mode == BLE_GAP_CONN_MODE_UND) {
+        TEST_ASSERT(hci_cmd.adv_type == BLE_HCI_ADV_TYPE_ADV_IND);
+    } else {
+        TEST_ASSERT(hci_cmd.adv_type == BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD);
+    }
 }
 
-TEST_CASE(ble_gap_test_case_conn_adv_good)
+TEST_CASE(ble_gap_test_case_adv_dflt_params)
 {
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+
     int d;
     int c;
 
     for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
         for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
-            ble_gap_test_util_adv(d, c, peer_addr, BLE_ADDR_TYPE_PUBLIC,
-                                  BLE_ERR_SUCCESS, -1, 0);
+            ble_gap_test_util_adv_verify_dflt_params(
+                BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC, peer_addr, c, d);
+        }
+    }
+}
+
+TEST_CASE(ble_gap_test_case_adv_good)
+{
+    uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
+    int d;
+    int c;
+
+    for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
+        for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
+            ble_gap_test_util_adv(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                  peer_addr, c, d, BLE_ERR_SUCCESS, -1, 0);
 
             if (c != BLE_GAP_CONN_MODE_NON) {
-                TEST_ASSERT(!ble_gap_slave_in_progress());
-                TEST_ASSERT(ble_gap_test_conn_event ==
-                            BLE_GAP_EVENT_CONNECT);
+                TEST_ASSERT(!ble_gap_adv_active());
+                TEST_ASSERT(ble_gap_test_event.type ==
+                                BLE_GAP_EVENT_CONNECT);
                 TEST_ASSERT(ble_gap_test_conn_status == 0);
                 TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
                 TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
@@ -1137,19 +1589,19 @@ TEST_CASE(ble_gap_test_case_conn_adv_good)
     }
 }
 
-TEST_CASE(ble_gap_test_case_conn_adv_ctlr_fail)
+TEST_CASE(ble_gap_test_case_adv_ctlr_fail)
 {
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
     int d;
     int c;
 
-    for (c = BLE_GAP_CONN_MODE_DIR; c < BLE_GAP_CONN_MODE_MAX; c++) {
+    for (c = BLE_GAP_CONN_MODE_NON + 1; c < BLE_GAP_CONN_MODE_MAX; c++) {
         for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
-            ble_gap_test_util_adv(d, c, peer_addr, BLE_ADDR_TYPE_PUBLIC,
-                                  BLE_ERR_DIR_ADV_TMO, -1, 0);
+            ble_gap_test_util_adv(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                  peer_addr, c, d, BLE_ERR_DIR_ADV_TMO, -1, 0);
 
-            TEST_ASSERT(!ble_gap_slave_in_progress());
-            TEST_ASSERT(ble_gap_test_conn_event ==
+            TEST_ASSERT(!ble_gap_adv_active());
+            TEST_ASSERT(ble_gap_test_event.type ==
                         BLE_GAP_EVENT_ADV_COMPLETE);
             TEST_ASSERT(ble_gap_test_conn_desc.conn_handle ==
                         BLE_HS_CONN_HANDLE_NONE);
@@ -1158,7 +1610,7 @@ TEST_CASE(ble_gap_test_case_conn_adv_ctlr_fail)
     }
 }
 
-TEST_CASE(ble_gap_test_case_conn_adv_hci_fail)
+TEST_CASE(ble_gap_test_case_adv_hci_fail)
 {
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
     int num_hci_cmds;
@@ -1175,21 +1627,26 @@ TEST_CASE(ble_gap_test_case_conn_adv_hci_fail)
 
         for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
             for (fail_idx = 0; fail_idx < num_hci_cmds; fail_idx++) {
-                ble_gap_test_util_adv(d, c, peer_addr, BLE_ADDR_TYPE_PUBLIC,
-                                      0, fail_idx, BLE_ERR_UNSUPPORTED);
-                TEST_ASSERT(!ble_gap_slave_in_progress());
-                TEST_ASSERT(ble_gap_test_conn_event == -1);
+                ble_gap_test_util_adv(BLE_ADDR_TYPE_PUBLIC,
+                                      BLE_ADDR_TYPE_PUBLIC, peer_addr,
+                                      c, d, 0, fail_idx, BLE_ERR_UNSUPPORTED);
+
+                TEST_ASSERT(!ble_gap_adv_active());
+                TEST_ASSERT(ble_gap_test_event.type == 0xff);
             }
         }
     }
 }
 
-TEST_SUITE(ble_gap_test_suite_conn_adv)
+TEST_SUITE(ble_gap_test_suite_adv)
 {
-    ble_gap_test_case_conn_adv_bad_args();
-    ble_gap_test_case_conn_adv_good();
-    ble_gap_test_case_conn_adv_ctlr_fail();
-    ble_gap_test_case_conn_adv_hci_fail();
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gap_test_case_adv_bad_args();
+    ble_gap_test_case_adv_dflt_params();
+    ble_gap_test_case_adv_good();
+    ble_gap_test_case_adv_ctlr_fail();
+    ble_gap_test_case_adv_hci_fail();
 }
 
 /*****************************************************************************
@@ -1197,8 +1654,8 @@ TEST_SUITE(ble_gap_test_suite_conn_adv)
  *****************************************************************************/
 
 static void
-ble_gap_test_util_stop_adv(uint8_t disc_mode, uint8_t conn_mode,
-                           uint8_t *peer_addr, uint8_t peer_addr_type,
+ble_gap_test_util_stop_adv(uint8_t peer_addr_type, const uint8_t *peer_addr,
+                           uint8_t conn_mode, uint8_t disc_mode,
                            int cmd_fail_idx, uint8_t fail_status)
 {
     uint8_t hci_status;
@@ -1207,15 +1664,10 @@ ble_gap_test_util_stop_adv(uint8_t disc_mode, uint8_t conn_mode,
     ble_gap_test_util_init();
 
     /* Start advertising; don't rx a successful connection event. */
-    ble_gap_test_util_adv(disc_mode, conn_mode, peer_addr,
-                          BLE_ADDR_TYPE_PUBLIC, BLE_ERR_UNSUPPORTED, 0, 0);
+    ble_gap_test_util_adv(BLE_ADDR_TYPE_PUBLIC, peer_addr_type, peer_addr,
+                          conn_mode, disc_mode, -1, -1, 0);
 
-    TEST_ASSERT(ble_gap_slave_in_progress());
-
-    /* Clear the advertising event that the host sent; we only care about what
-     * happens after advertising is stopped.
-     */
-    ble_gap_test_util_reset_cb_info();
+    TEST_ASSERT(ble_gap_adv_active());
 
     /* Stop advertising. */
     hci_status = cmd_fail_idx == 0 ? fail_status : 0;
@@ -1227,7 +1679,7 @@ ble_gap_test_util_stop_adv(uint8_t disc_mode, uint8_t conn_mode,
     ble_gap_test_util_verify_tx_adv_enable(0);
 }
 
-TEST_CASE(ble_gap_test_case_conn_stop_adv_good)
+TEST_CASE(ble_gap_test_case_stop_adv_good)
 {
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
     int d;
@@ -1235,10 +1687,10 @@ TEST_CASE(ble_gap_test_case_conn_stop_adv_good)
 
     for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
         for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
-            ble_gap_test_util_stop_adv(d, c, peer_addr, BLE_ADDR_TYPE_PUBLIC,
+            ble_gap_test_util_stop_adv(BLE_ADDR_TYPE_PUBLIC, peer_addr, c, d,
                                        -1, 0);
-            TEST_ASSERT(!ble_gap_slave_in_progress());
-            TEST_ASSERT(ble_gap_test_conn_event == -1);
+            TEST_ASSERT(!ble_gap_adv_active());
+            TEST_ASSERT(ble_gap_test_event.type == 0xff);
             TEST_ASSERT(ble_gap_test_conn_status == -1);
             TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == (uint16_t)-1);
             TEST_ASSERT(ble_gap_test_conn_arg == (void *)-1);
@@ -1246,7 +1698,7 @@ TEST_CASE(ble_gap_test_case_conn_stop_adv_good)
     }
 }
 
-TEST_CASE(ble_gap_test_case_conn_stop_adv_hci_fail)
+TEST_CASE(ble_gap_test_case_stop_adv_hci_fail)
 {
     uint8_t peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
     int d;
@@ -1254,10 +1706,10 @@ TEST_CASE(ble_gap_test_case_conn_stop_adv_hci_fail)
 
     for (c = BLE_GAP_CONN_MODE_NON; c < BLE_GAP_CONN_MODE_MAX; c++) {
         for (d = BLE_GAP_DISC_MODE_NON; d < BLE_GAP_DISC_MODE_MAX; d++) {
-            ble_gap_test_util_stop_adv(d, c, peer_addr, BLE_ADDR_TYPE_PUBLIC,
+            ble_gap_test_util_stop_adv(BLE_ADDR_TYPE_PUBLIC, peer_addr, c, d,
                                        0, BLE_ERR_UNSUPPORTED);
-            TEST_ASSERT(ble_gap_slave_in_progress());
-            TEST_ASSERT(ble_gap_test_conn_event == -1);
+            TEST_ASSERT(ble_gap_adv_active());
+            TEST_ASSERT(ble_gap_test_event.type == 0xff);
             TEST_ASSERT(ble_gap_test_conn_status == -1);
             TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == (uint16_t)-1);
             TEST_ASSERT(ble_gap_test_conn_arg == (void *)-1);
@@ -1265,10 +1717,12 @@ TEST_CASE(ble_gap_test_case_conn_stop_adv_hci_fail)
     }
 }
 
-TEST_SUITE(ble_gap_test_suite_conn_stop_adv)
+TEST_SUITE(ble_gap_test_suite_stop_adv)
 {
-    ble_gap_test_case_conn_stop_adv_good();
-    ble_gap_test_case_conn_stop_adv_hci_fail();
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gap_test_case_stop_adv_good();
+    ble_gap_test_case_stop_adv_hci_fail();
 }
 
 /*****************************************************************************
@@ -1316,10 +1770,11 @@ ble_gap_test_util_update(struct ble_gap_upd_params *params,
 
     TEST_ASSERT(!ble_gap_master_in_progress());
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
     TEST_ASSERT(ble_gap_test_conn_status == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl == params->itvl_max);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_latency == params->latency);
     TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
@@ -1330,10 +1785,11 @@ ble_gap_test_util_update(struct ble_gap_upd_params *params,
     return;
 
 fail:
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
     TEST_ASSERT(ble_gap_test_conn_status == status);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl ==
                 BLE_GAP_INITIAL_CONN_ITVL_MAX);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_latency ==
@@ -1361,7 +1817,7 @@ ble_gap_test_util_update_peer(uint8_t status,
 
     TEST_ASSERT(!ble_gap_master_in_progress());
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
     TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(status));
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
     TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
@@ -1416,10 +1872,11 @@ ble_gap_test_util_update_req_pos(struct ble_gap_upd_params *peer_params,
     TEST_ASSERT(!ble_gap_master_in_progress());
     TEST_ASSERT(!ble_gap_test_util_update_in_progress(2));
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
     TEST_ASSERT(ble_gap_test_conn_status == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl == self_params->itvl_max);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_latency == self_params->latency);
     TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
@@ -1428,10 +1885,11 @@ ble_gap_test_util_update_req_pos(struct ble_gap_upd_params *peer_params,
     return;
 
 hci_fail:
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
     TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(hci_status));
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl ==
                 BLE_GAP_INITIAL_CONN_ITVL_MAX);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_latency ==
@@ -1477,10 +1935,11 @@ ble_gap_test_util_update_req_neg(struct ble_gap_upd_params *peer_params,
     return;
 
 hci_fail:
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
     TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(hci_status));
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl ==
                 BLE_GAP_INITIAL_CONN_ITVL_MAX);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_latency ==
@@ -1552,10 +2011,11 @@ ble_gap_test_util_update_req_concurrent(
     TEST_ASSERT(!ble_gap_master_in_progress());
     TEST_ASSERT(!ble_gap_test_util_update_in_progress(2));
 
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
     TEST_ASSERT(ble_gap_test_conn_status == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl == self_params->itvl_max);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_latency == self_params->latency);
     TEST_ASSERT(ble_gap_test_conn_desc.supervision_timeout ==
@@ -1564,10 +2024,11 @@ ble_gap_test_util_update_req_concurrent(
     return;
 
 hci_fail:
-    TEST_ASSERT(ble_gap_test_conn_event == BLE_GAP_EVENT_CONN_UPDATE);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONN_UPDATE);
     TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_HCI_ERR(fail_status));
     TEST_ASSERT(ble_gap_test_conn_desc.conn_handle == 2);
-    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr, peer_addr, 6) == 0);
+    TEST_ASSERT(memcmp(ble_gap_test_conn_desc.peer_id_addr,
+                       peer_addr, 6) == 0);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_itvl ==
                 BLE_GAP_INITIAL_CONN_ITVL_MAX);
     TEST_ASSERT(ble_gap_test_conn_desc.conn_latency ==
@@ -1831,6 +2292,8 @@ TEST_CASE(ble_gap_test_case_update_concurrent_hci_fail)
 
 TEST_SUITE(ble_gap_test_suite_update_conn)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gap_test_case_update_conn_good();
     ble_gap_test_case_update_conn_bad();
     ble_gap_test_case_update_conn_hci_fail();
@@ -1843,20 +2306,328 @@ TEST_SUITE(ble_gap_test_suite_update_conn)
 }
 
 /*****************************************************************************
+ * $timeout                                                                  *
+ *****************************************************************************/
+
+static void
+ble_gap_test_util_conn_forever(void)
+{
+    int32_t ticks_from_now;
+
+    /* Initiate a connect procedure with no timeout. */
+    ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC,
+                             BLE_ADDR_TYPE_PUBLIC,
+                             ((uint8_t[]){ 1, 2, 3, 4, 5, 6 }), BLE_HS_FOREVER,
+                             NULL, ble_gap_test_util_connect_cb,
+                             NULL, 0);
+
+    /* Ensure no pending GAP event. */
+    ticks_from_now = ble_gap_heartbeat();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Advance 100 seconds; ensure no timeout reported. */
+    os_time_advance(100 * OS_TICKS_PER_SEC);
+    ble_gap_heartbeat();
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
+    TEST_ASSERT(ble_gap_conn_active());
+}
+
+static void
+ble_gap_test_util_conn_timeout(int32_t duration_ms)
+{
+    struct hci_le_conn_complete evt;
+    uint32_t duration_ticks;
+    int32_t ticks_from_now;
+    int rc;
+
+    TEST_ASSERT_FATAL(duration_ms != BLE_HS_FOREVER);
+
+    /* Initiate a connect procedure with the specified timeout. */
+    ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC,
+                             BLE_ADDR_TYPE_PUBLIC,
+                             ((uint8_t[]){ 1, 2, 3, 4, 5, 6 }), duration_ms,
+                             NULL, ble_gap_test_util_connect_cb,
+                             NULL, 0);
+
+    /* Ensure next GAP event is at the expected time. */
+    rc = os_time_ms_to_ticks(duration_ms, &duration_ticks);
+    TEST_ASSERT_FATAL(rc == 0);
+    ticks_from_now = ble_gap_heartbeat();
+    TEST_ASSERT(ticks_from_now == duration_ticks);
+
+    /* Advance duration ms; ensure timeout event does not get reported before
+     * connection complete event rxed.
+     */
+    os_time_advance(duration_ms);
+
+    ble_hs_test_util_set_ack(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_CREATE_CONN_CANCEL),
+        0);
+
+    TEST_ASSERT(ble_gap_test_event.type == 0xff);
+
+    ticks_from_now = ble_gap_heartbeat();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Ensure cancel create connection command was sent. */
+    ble_hs_test_util_verify_tx_create_conn_cancel();
+
+    /* Ensure timer has been stopped. */
+    ticks_from_now = ble_gap_heartbeat();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Receive the connection complete event indicating a successful cancel. */
+    memset(&evt, 0, sizeof evt);
+    evt.subevent_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE;
+    evt.status = BLE_ERR_UNK_CONN_ID;
+    rc = ble_gap_rx_conn_complete(&evt);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Ensure the GAP event was triggered. */
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(ble_gap_test_conn_status == BLE_HS_ETIMEOUT);
+
+    /* Clear GAP event for remainder of test. */
+    ble_gap_test_util_reset_cb_info();
+}
+
+static void
+ble_gap_test_util_disc_forever(void)
+{
+    struct ble_gap_disc_params params;
+    int32_t ticks_from_now;
+
+    memset(&params, 0, sizeof params);
+
+    /* Initiate a discovery procedure with no timeout. */
+    ble_hs_test_util_disc(BLE_ADDR_TYPE_PUBLIC,
+                          BLE_HS_FOREVER, &params, ble_gap_test_util_disc_cb,
+                          NULL, -1, 0);
+
+    /* Ensure no pending GAP event. */
+    ticks_from_now = ble_gap_heartbeat();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    /* Advance 100 seconds; ensure no timeout reported. */
+    os_time_advance(100 * OS_TICKS_PER_SEC);
+    TEST_ASSERT(ble_gap_test_disc_event_type == -1);
+    TEST_ASSERT(ble_gap_disc_active());
+}
+
+static void
+ble_gap_test_util_disc_timeout(int32_t duration_ms)
+{
+    struct ble_gap_disc_params params;
+    uint32_t duration_ticks;
+    int32_t ticks_from_now;
+    int rc;
+
+    TEST_ASSERT_FATAL(duration_ms != BLE_HS_FOREVER);
+
+    memset(&params, 0, sizeof params);
+
+    /* Initiate a discovery procedure with the specified timeout. */
+    ble_hs_test_util_disc(BLE_ADDR_TYPE_PUBLIC,
+                          duration_ms, &params, ble_gap_test_util_disc_cb,
+                          NULL, -1, 0);
+
+    /* Ensure next GAP event is at the expected time. */
+    rc = os_time_ms_to_ticks(duration_ms, &duration_ticks);
+    TEST_ASSERT_FATAL(rc == 0);
+    ticks_from_now = ble_gap_heartbeat();
+    TEST_ASSERT(ticks_from_now == duration_ticks);
+
+    /* Advance duration ms; ensure timeout event was reported. */
+    os_time_advance(duration_ms);
+
+    ble_hs_test_util_set_ack(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+        0);
+    ticks_from_now = ble_gap_heartbeat();
+    TEST_ASSERT(ticks_from_now == BLE_HS_FOREVER);
+
+    TEST_ASSERT(ble_gap_test_disc_event_type == BLE_GAP_EVENT_DISC_COMPLETE);
+
+    /* Clear GAP event for remainder of test. */
+    ble_gap_test_util_reset_cb_info();
+}
+
+TEST_CASE(ble_gap_test_case_conn_timeout_conn_forever)
+{
+    ble_gap_test_util_init();
+
+    /* 3 ms. */
+    ble_gap_test_util_conn_timeout(3);
+
+    /* No timeout. */
+    ble_gap_test_util_conn_forever();
+
+}
+
+TEST_CASE(ble_gap_test_case_conn_timeout_conn_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* 30 ms. */
+    ble_gap_test_util_conn_timeout(30);
+
+    /* 5 ms. */
+    ble_gap_test_util_conn_timeout(5);
+
+}
+
+TEST_CASE(ble_gap_test_case_conn_forever_conn_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* No timeout. */
+    ble_gap_test_util_conn_forever();
+
+    /* Cancel connect procedure manually. */
+    ble_gap_test_util_conn_cancel(0);
+
+    /* Clear GAP event for remainder of test. */
+    ble_gap_test_util_reset_cb_info();
+
+    /* 3 ms. */
+    ble_gap_test_util_conn_timeout(3);
+}
+
+TEST_CASE(ble_gap_test_case_disc_timeout_disc_forever)
+{
+    ble_gap_test_util_init();
+
+    /* 3 ms. */
+    ble_gap_test_util_disc_timeout(3);
+
+    /* No timeout. */
+    ble_gap_test_util_disc_forever();
+
+}
+
+TEST_CASE(ble_gap_test_case_disc_timeout_disc_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* 30 ms. */
+    ble_gap_test_util_disc_timeout(30);
+
+    /* 5 ms. */
+    ble_gap_test_util_disc_timeout(5);
+
+}
+
+TEST_CASE(ble_gap_test_case_disc_forever_disc_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* No timeout. */
+    ble_gap_test_util_disc_forever();
+
+    /* Cancel discovery procedure manually. */
+    ble_hs_test_util_disc_cancel(0);
+
+    /* 3 ms. */
+    ble_gap_test_util_disc_timeout(3);
+}
+
+TEST_CASE(ble_gap_test_case_conn_timeout_disc_timeout)
+{
+    ble_gap_test_util_init();
+
+    /* 15 seconds. */
+    ble_gap_test_util_conn_timeout(15000);
+
+    /* 1285 ms. */
+    ble_gap_test_util_disc_timeout(1285);
+}
+
+TEST_SUITE(ble_gap_test_suite_timeout)
+{
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gap_test_case_conn_timeout_conn_forever();
+    ble_gap_test_case_conn_timeout_conn_timeout();
+    ble_gap_test_case_conn_forever_conn_timeout();
+
+    ble_gap_test_case_disc_timeout_disc_forever();
+    ble_gap_test_case_disc_timeout_disc_timeout();
+    ble_gap_test_case_disc_forever_disc_timeout();
+
+    ble_gap_test_case_conn_timeout_disc_timeout();
+}
+
+TEST_CASE(ble_gap_test_case_mtu_us)
+{
+    const uint8_t peer_addr[6] = { 1,2,3,4,5,6 };
+    int rc;
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    ble_att_set_preferred_mtu(200);
+
+    rc = ble_gattc_exchange_mtu(2, NULL, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_hs_test_util_verify_tx_mtu_cmd(1, 200);
+
+    ble_hs_test_util_rx_att_mtu_cmd(2, 0, 123);
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_MTU);
+    TEST_ASSERT(ble_gap_test_event.mtu.conn_handle == 2);
+    TEST_ASSERT(ble_gap_test_event.mtu.channel_id == BLE_L2CAP_CID_ATT);
+    TEST_ASSERT(ble_gap_test_event.mtu.value == 123);
+}
+
+TEST_CASE(ble_gap_test_case_mtu_peer)
+{
+    const uint8_t peer_addr[6] = { 1,2,3,4,5,6 };
+
+    ble_gap_test_util_init();
+
+    ble_hs_test_util_create_conn(2, peer_addr, ble_gap_test_util_connect_cb,
+                                 NULL);
+
+    ble_att_set_preferred_mtu(200);
+
+    ble_hs_test_util_rx_att_mtu_cmd(2, 1, 123);
+    ble_hs_test_util_verify_tx_mtu_cmd(0, 200);
+
+    TEST_ASSERT(ble_gap_test_event.type == BLE_GAP_EVENT_MTU);
+    TEST_ASSERT(ble_gap_test_event.mtu.conn_handle == 2);
+    TEST_ASSERT(ble_gap_test_event.mtu.channel_id == BLE_L2CAP_CID_ATT);
+    TEST_ASSERT(ble_gap_test_event.mtu.value == 123);
+}
+
+TEST_SUITE(ble_gap_test_suite_mtu)
+{
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gap_test_case_mtu_us();
+    ble_gap_test_case_mtu_peer();
+}
+
+/*****************************************************************************
  * $all                                                                      *
  *****************************************************************************/
 
 int
 ble_gap_test_all(void)
 {
-    ble_gap_test_suite_conn_wl();
-    ble_gap_test_suite_conn_disc();
-    ble_gap_test_suite_conn_dir();
+    ble_gap_test_suite_wl();
+    ble_gap_test_suite_disc();
+    ble_gap_test_suite_conn_gen();
     ble_gap_test_suite_conn_cancel();
     ble_gap_test_suite_conn_terminate();
-    ble_gap_test_suite_conn_adv();
-    ble_gap_test_suite_conn_stop_adv();
+    ble_gap_test_suite_conn_find();
+    ble_gap_test_suite_adv();
+    ble_gap_test_suite_stop_adv();
     ble_gap_test_suite_update_conn();
+    ble_gap_test_suite_timeout();
+    ble_gap_test_suite_mtu();
 
     return tu_any_failed;
 }
diff --git a/net/nimble/host/src/test/ble_gatt_conn_test.c b/net/nimble/host/src/test/ble_gatt_conn_test.c
index 6f3a4a59..be4a46d3 100644
--- a/net/nimble/host/src/test/ble_gatt_conn_test.c
+++ b/net/nimble/host/src/test/ble_gatt_conn_test.c
@@ -36,16 +36,16 @@ struct ble_gatt_conn_test_cb_arg {
 
 static int
 ble_gatt_conn_test_attr_cb(uint16_t conn_handle, uint16_t attr_handle,
-                           uint8_t *uuid128, uint8_t op,
-                           struct ble_att_svr_access_ctxt *ctxt,
+                           uint8_t op, uint16_t offset, struct os_mbuf **om,
                            void *arg)
 {
-    static uint8_t data = 1;
+    uint8_t *buf;
 
     switch (op) {
     case BLE_ATT_ACCESS_OP_READ:
-        ctxt->attr_data = &data;
-        ctxt->data_len = 1;
+        buf = os_mbuf_extend(*om, 1);
+        TEST_ASSERT_FATAL(buf != NULL);
+        *buf = 1;
         return 0;
 
     default:
@@ -54,7 +54,8 @@ ble_gatt_conn_test_attr_cb(uint16_t conn_handle, uint16_t attr_handle,
 }
 
 static int
-ble_gatt_conn_test_mtu_cb(uint16_t conn_handle, struct ble_gatt_error *error,
+ble_gatt_conn_test_mtu_cb(uint16_t conn_handle,
+                          const struct ble_gatt_error *error,
                           uint16_t mtu, void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -74,9 +75,9 @@ ble_gatt_conn_test_mtu_cb(uint16_t conn_handle, struct ble_gatt_error *error,
 
 static int
 ble_gatt_conn_test_disc_all_svcs_cb(uint16_t conn_handle,
-                                   struct ble_gatt_error *error,
-                                   struct ble_gatt_svc *service,
-                                   void *arg)
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_svc *service,
+                                    void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
 
@@ -95,8 +96,8 @@ ble_gatt_conn_test_disc_all_svcs_cb(uint16_t conn_handle,
 
 static int
 ble_gatt_conn_test_disc_svc_uuid_cb(uint16_t conn_handle,
-                                    struct ble_gatt_error *error,
-                                    struct ble_gatt_svc *service,
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_svc *service,
                                     void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -116,8 +117,8 @@ ble_gatt_conn_test_disc_svc_uuid_cb(uint16_t conn_handle,
 
 static int
 ble_gatt_conn_test_find_inc_svcs_cb(uint16_t conn_handle,
-                                    struct ble_gatt_error *error,
-                                    struct ble_gatt_svc *service,
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_svc *service,
                                     void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -137,8 +138,8 @@ ble_gatt_conn_test_find_inc_svcs_cb(uint16_t conn_handle,
 
 static int
 ble_gatt_conn_test_disc_all_chrs_cb(uint16_t conn_handle,
-                                    struct ble_gatt_error *error,
-                                    struct ble_gatt_chr *chr, void *arg)
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_chr *chr, void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
 
@@ -157,8 +158,8 @@ ble_gatt_conn_test_disc_all_chrs_cb(uint16_t conn_handle,
 
 static int
 ble_gatt_conn_test_disc_chr_uuid_cb(uint16_t conn_handle,
-                                    struct ble_gatt_error *error,
-                                    struct ble_gatt_chr *chr, void *arg)
+                                    const struct ble_gatt_error *error,
+                                    const struct ble_gatt_chr *chr, void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
 
@@ -177,9 +178,9 @@ ble_gatt_conn_test_disc_chr_uuid_cb(uint16_t conn_handle,
 
 static int
 ble_gatt_conn_test_disc_all_dscs_cb(uint16_t conn_handle,
-                                    struct ble_gatt_error *error,
+                                    const struct ble_gatt_error *error,
                                     uint16_t chr_def_handle,
-                                    struct ble_gatt_dsc *dsc,
+                                    const struct ble_gatt_dsc *dsc,
                                     void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -198,7 +199,8 @@ ble_gatt_conn_test_disc_all_dscs_cb(uint16_t conn_handle,
 }
 
 static int
-ble_gatt_conn_test_read_cb(uint16_t conn_handle, struct ble_gatt_error *error,
+ble_gatt_conn_test_read_cb(uint16_t conn_handle,
+                           const struct ble_gatt_error *error,
                            struct ble_gatt_attr *attr, void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -218,7 +220,7 @@ ble_gatt_conn_test_read_cb(uint16_t conn_handle, struct ble_gatt_error *error,
 
 static int
 ble_gatt_conn_test_read_uuid_cb(uint16_t conn_handle,
-                                struct ble_gatt_error *error,
+                                const struct ble_gatt_error *error,
                                 struct ble_gatt_attr *attr, void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -238,7 +240,7 @@ ble_gatt_conn_test_read_uuid_cb(uint16_t conn_handle,
 
 static int
 ble_gatt_conn_test_read_long_cb(uint16_t conn_handle,
-                                struct ble_gatt_error *error,
+                                const struct ble_gatt_error *error,
                                 struct ble_gatt_attr *attr, void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -257,7 +259,7 @@ ble_gatt_conn_test_read_long_cb(uint16_t conn_handle,
 }
 static int
 ble_gatt_conn_test_read_mult_cb(uint16_t conn_handle,
-                                struct ble_gatt_error *error,
+                                const struct ble_gatt_error *error,
                                 struct ble_gatt_attr *attr, void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -268,7 +270,7 @@ ble_gatt_conn_test_read_mult_cb(uint16_t conn_handle,
     TEST_ASSERT(!cb_arg->called);
     TEST_ASSERT_FATAL(error != NULL);
     TEST_ASSERT(error->status == BLE_HS_ENOTCONN);
-    TEST_ASSERT(attr == NULL);
+    TEST_ASSERT(attr->om == NULL);
 
     cb_arg->called++;
 
@@ -276,8 +278,10 @@ ble_gatt_conn_test_read_mult_cb(uint16_t conn_handle,
 }
 
 static int
-ble_gatt_conn_test_write_cb(uint16_t conn_handle, struct ble_gatt_error *error,
-                            struct ble_gatt_attr *attr, void *arg)
+ble_gatt_conn_test_write_cb(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            struct ble_gatt_attr *attr,
+                            void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
 
@@ -297,7 +301,7 @@ ble_gatt_conn_test_write_cb(uint16_t conn_handle, struct ble_gatt_error *error,
 
 static int
 ble_gatt_conn_test_write_long_cb(uint16_t conn_handle,
-                                 struct ble_gatt_error *error,
+                                 const struct ble_gatt_error *error,
                                  struct ble_gatt_attr *attr, void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -318,8 +322,9 @@ ble_gatt_conn_test_write_long_cb(uint16_t conn_handle,
 
 static int
 ble_gatt_conn_test_write_rel_cb(uint16_t conn_handle,
-                                struct ble_gatt_error *error,
-                                struct ble_gatt_attr *attrs, uint8_t num_attrs,
+                                const struct ble_gatt_error *error,
+                                struct ble_gatt_attr *attrs,
+                                uint8_t num_attrs,
                                 void *arg)
 {
     struct ble_gatt_conn_test_cb_arg *cb_arg;
@@ -337,26 +342,6 @@ ble_gatt_conn_test_write_rel_cb(uint16_t conn_handle,
     return 0;
 }
 
-static int
-ble_gatt_conn_test_indicate_cb(uint16_t conn_handle,
-                               struct ble_gatt_error *error,
-                               struct ble_gatt_attr *attr, void *arg)
-{
-    struct ble_gatt_conn_test_cb_arg *cb_arg;
-
-    cb_arg = arg;
-
-    TEST_ASSERT(cb_arg->exp_conn_handle == conn_handle);
-    TEST_ASSERT(!cb_arg->called);
-    TEST_ASSERT_FATAL(error != NULL);
-    TEST_ASSERT(error->status == BLE_HS_ENOTCONN);
-    TEST_ASSERT(attr != NULL);
-
-    cb_arg->called++;
-
-    return 0;
-}
-
 TEST_CASE(ble_gatt_conn_test_disconnect)
 {
     struct ble_gatt_conn_test_cb_arg mtu_arg            = { 0 };
@@ -373,7 +358,7 @@ TEST_CASE(ble_gatt_conn_test_disconnect)
     struct ble_gatt_conn_test_cb_arg write_arg          = { 0 };
     struct ble_gatt_conn_test_cb_arg write_long_arg     = { 0 };
     struct ble_gatt_conn_test_cb_arg write_rel_arg      = { 0 };
-    struct ble_gatt_conn_test_cb_arg indicate_arg       = { 0 };
+    struct ble_gatt_attr attr;
     uint16_t attr_handle;
     int rc;
 
@@ -423,12 +408,12 @@ TEST_CASE(ble_gatt_conn_test_disconnect)
 
     /* Connection 2. */
     disc_all_dscs_arg.exp_conn_handle = 2;
-    rc = ble_gattc_disc_all_dscs(2, 1, 0xffff,
+    rc = ble_gattc_disc_all_dscs(2, 3, 0xffff,
                                  ble_gatt_conn_test_disc_all_dscs_cb,
                                  &disc_all_dscs_arg);
 
     disc_chr_uuid_arg.exp_conn_handle = 2;
-    rc = ble_gattc_disc_chrs_by_uuid(2, 1, 0xffff, BLE_UUID16(0x2222),
+    rc = ble_gattc_disc_chrs_by_uuid(2, 2, 0xffff, BLE_UUID16(0x2222),
                                      ble_gatt_conn_test_disc_chr_uuid_cb,
                                      &disc_chr_uuid_arg);
 
@@ -455,31 +440,25 @@ TEST_CASE(ble_gatt_conn_test_disconnect)
     TEST_ASSERT_FATAL(rc == 0);
 
     write_arg.exp_conn_handle = 3;
-    rc = ble_gattc_write(3, BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE,
-                         ble_gatt_conn_test_write_value,
-                         sizeof ble_gatt_conn_test_write_value,
-                         ble_gatt_conn_test_write_cb, &write_arg);
+    rc = ble_hs_test_util_gatt_write_flat(
+        3, BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE,
+        ble_gatt_conn_test_write_value, sizeof ble_gatt_conn_test_write_value,
+        ble_gatt_conn_test_write_cb, &write_arg);
     TEST_ASSERT_FATAL(rc == 0);
 
     write_long_arg.exp_conn_handle = 3;
-    rc = ble_gattc_write_long(3, BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE,
-                              ble_gatt_conn_test_write_value,
-                              sizeof ble_gatt_conn_test_write_value,
-                              ble_gatt_conn_test_write_long_cb,
-                              &write_long_arg);
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        3, BLE_GATT_BREAK_TEST_WRITE_ATTR_HANDLE,
+        ble_gatt_conn_test_write_value, sizeof ble_gatt_conn_test_write_value,
+        ble_gatt_conn_test_write_long_cb, &write_long_arg);
     TEST_ASSERT_FATAL(rc == 0);
 
+    attr.handle = 8;
+    attr.offset = 0;
+    attr.om = os_msys_get_pkthdr(0, 0);
     write_rel_arg.exp_conn_handle = 3;
-    rc = ble_gattc_write_reliable(3,
-                                  ((struct ble_gatt_attr[]){{8, 0, 0, NULL}}),
-                                  1, ble_gatt_conn_test_write_rel_cb,
-                                  &write_rel_arg);
-    TEST_ASSERT_FATAL(rc == 0);
-
-    indicate_arg.exp_conn_handle = 3;
-    rc = ble_gattc_indicate(3, attr_handle,
-                            ble_gatt_conn_test_indicate_cb,
-                            &indicate_arg);
+    rc = ble_gattc_write_reliable(
+        3, &attr, 1, ble_gatt_conn_test_write_rel_cb, &write_rel_arg);
     TEST_ASSERT_FATAL(rc == 0);
 
     /*** Start the procedures. */
@@ -502,7 +481,6 @@ TEST_CASE(ble_gatt_conn_test_disconnect)
     TEST_ASSERT(write_arg.called == 0);
     TEST_ASSERT(write_long_arg.called == 0);
     TEST_ASSERT(write_rel_arg.called == 0);
-    TEST_ASSERT(indicate_arg.called == 0);
 
     /* Connection 2. */
     ble_gattc_connection_broken(2);
@@ -520,7 +498,6 @@ TEST_CASE(ble_gatt_conn_test_disconnect)
     TEST_ASSERT(write_arg.called == 0);
     TEST_ASSERT(write_long_arg.called == 0);
     TEST_ASSERT(write_rel_arg.called == 0);
-    TEST_ASSERT(indicate_arg.called == 0);
 
     /* Connection 3. */
     ble_gattc_connection_broken(3);
@@ -538,11 +515,12 @@ TEST_CASE(ble_gatt_conn_test_disconnect)
     TEST_ASSERT(write_arg.called == 1);
     TEST_ASSERT(write_long_arg.called == 1);
     TEST_ASSERT(write_rel_arg.called == 1);
-    TEST_ASSERT(indicate_arg.called == 1);
 }
 
 TEST_SUITE(ble_gatt_break_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatt_conn_test_disconnect();
 }
 
diff --git a/net/nimble/host/src/test/ble_gatt_disc_c_test.c b/net/nimble/host/src/test/ble_gatt_disc_c_test.c
index 6325926f..a4eb67bf 100644
--- a/net/nimble/host/src/test/ble_gatt_disc_c_test.c
+++ b/net/nimble/host/src/test/ble_gatt_disc_c_test.c
@@ -169,25 +169,33 @@ ble_gatt_disc_c_test_misc_verify_chars(struct ble_gatt_disc_c_test_char *chars,
 
 static int
 ble_gatt_disc_c_test_misc_cb(uint16_t conn_handle,
-                             struct ble_gatt_error *error,
-                             struct ble_gatt_chr *chr, void *arg)
+                             const struct ble_gatt_error *error,
+                             const struct ble_gatt_chr *chr, void *arg)
 {
     struct ble_gatt_chr *dst;
     int *stop_after;
 
-    TEST_ASSERT(error == NULL);
+    TEST_ASSERT(error != NULL);
     TEST_ASSERT(!ble_gatt_disc_c_test_rx_complete);
 
     stop_after = arg;
 
-    if (chr == NULL) {
-        ble_gatt_disc_c_test_rx_complete = 1;
-    } else {
+    switch (error->status) {
+    case 0:
         TEST_ASSERT_FATAL(ble_gatt_disc_c_test_num_chars <
                           BLE_GATT_DISC_C_TEST_MAX_CHARS);
 
         dst = ble_gatt_disc_c_test_chars + ble_gatt_disc_c_test_num_chars++;
         *dst = *chr;
+        break;
+
+    case BLE_HS_EDONE:
+        ble_gatt_disc_c_test_rx_complete = 1;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
     }
 
     if (*stop_after > 0) {
@@ -524,6 +532,8 @@ TEST_CASE(ble_gatt_disc_c_test_disc_uuid)
 
 TEST_SUITE(ble_gatt_disc_c_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatt_disc_c_test_disc_all();
     ble_gatt_disc_c_test_disc_uuid();
 }
diff --git a/net/nimble/host/src/test/ble_gatt_disc_d_test.c b/net/nimble/host/src/test/ble_gatt_disc_d_test.c
index 6b0a8077..7e021e2e 100644
--- a/net/nimble/host/src/test/ble_gatt_disc_d_test.c
+++ b/net/nimble/host/src/test/ble_gatt_disc_d_test.c
@@ -28,7 +28,7 @@
 #include "ble_hs_test_util.h"
 
 struct ble_gatt_disc_d_test_dsc {
-    uint16_t chr_def_handle; /* 0 if last entry. */
+    uint16_t chr_val_handle; /* 0 if last entry. */
     uint16_t dsc_handle;
     uint8_t dsc_uuid128[16];
 };
@@ -74,7 +74,7 @@ ble_gatt_disc_d_test_misc_rx_rsp_once(
 
     off = BLE_ATT_FIND_INFO_RSP_BASE_SZ;
     for (i = 0; ; i++) {
-        if (dscs[i].chr_def_handle == 0) {
+        if (dscs[i].chr_val_handle == 0) {
             /* No more descriptors. */
             break;
         }
@@ -113,7 +113,7 @@ ble_gatt_disc_d_test_misc_rx_rsp(uint16_t conn_handle,
     int idx;
 
     idx = 0;
-    while (dscs[idx].chr_def_handle != 0) {
+    while (dscs[idx].chr_val_handle != 0) {
         count = ble_gatt_disc_d_test_misc_rx_rsp_once(conn_handle, dscs + idx);
         if (count == 0) {
             break;
@@ -140,9 +140,9 @@ ble_gatt_disc_d_test_misc_verify_dscs(struct ble_gatt_disc_d_test_dsc *dscs,
         stop_after = INT_MAX;
     }
 
-    for (i = 0; i < stop_after && dscs[i].chr_def_handle != 0; i++) {
-        TEST_ASSERT(dscs[i].chr_def_handle ==
-                    ble_gatt_disc_d_test_dscs[i].chr_def_handle);
+    for (i = 0; i < stop_after && dscs[i].chr_val_handle != 0; i++) {
+        TEST_ASSERT(dscs[i].chr_val_handle ==
+                    ble_gatt_disc_d_test_dscs[i].chr_val_handle);
         TEST_ASSERT(dscs[i].dsc_handle ==
                     ble_gatt_disc_d_test_dscs[i].dsc_handle);
         TEST_ASSERT(memcmp(dscs[i].dsc_uuid128,
@@ -156,28 +156,37 @@ ble_gatt_disc_d_test_misc_verify_dscs(struct ble_gatt_disc_d_test_dsc *dscs,
 
 static int
 ble_gatt_disc_d_test_misc_cb(uint16_t conn_handle,
-                             struct ble_gatt_error *error,
-                             uint16_t chr_def_handle, struct ble_gatt_dsc *dsc,
+                             const struct ble_gatt_error *error,
+                             uint16_t chr_val_handle,
+                             const struct ble_gatt_dsc *dsc,
                              void *arg)
 {
     struct ble_gatt_disc_d_test_dsc *dst;
     int *stop_after;
 
-    TEST_ASSERT(error == NULL);
+    TEST_ASSERT(error != NULL);
     TEST_ASSERT(!ble_gatt_disc_d_test_rx_complete);
 
     stop_after = arg;
 
-    if (dsc == NULL) {
-        ble_gatt_disc_d_test_rx_complete = 1;
-    } else {
+    switch (error->status) {
+    case 0:
         TEST_ASSERT_FATAL(ble_gatt_disc_d_test_num_dscs <
                           BLE_GATT_DISC_D_TEST_MAX_DSCS);
 
         dst = ble_gatt_disc_d_test_dscs + ble_gatt_disc_d_test_num_dscs++;
-        dst->chr_def_handle = chr_def_handle;
+        dst->chr_val_handle = chr_val_handle;
         dst->dsc_handle = dsc->handle;
         memcpy(dst->dsc_uuid128, dsc->uuid128, 16);
+        break;
+
+    case BLE_HS_EDONE:
+        ble_gatt_disc_d_test_rx_complete = 1;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
     }
 
     if (*stop_after > 0) {
@@ -192,7 +201,7 @@ ble_gatt_disc_d_test_misc_cb(uint16_t conn_handle,
 }
 
 static void
-ble_gatt_disc_d_test_misc_all(uint16_t chr_def_handle, uint16_t end_handle,
+ble_gatt_disc_d_test_misc_all(uint16_t chr_val_handle, uint16_t end_handle,
                               int stop_after,
                               struct ble_gatt_disc_d_test_dsc *dscs)
 {
@@ -205,7 +214,7 @@ ble_gatt_disc_d_test_misc_all(uint16_t chr_def_handle, uint16_t end_handle,
                                  NULL, NULL);
 
     num_left = stop_after;
-    rc = ble_gattc_disc_all_dscs(2, chr_def_handle, end_handle,
+    rc = ble_gattc_disc_all_dscs(2, chr_val_handle, end_handle,
                                  ble_gatt_disc_d_test_misc_cb, &num_left);
     TEST_ASSERT(rc == 0);
 
@@ -216,9 +225,9 @@ ble_gatt_disc_d_test_misc_all(uint16_t chr_def_handle, uint16_t end_handle,
 TEST_CASE(ble_gatt_disc_d_test_1)
 {
     /*** One 16-bit descriptor. */
-    ble_gatt_disc_d_test_misc_all(4, 10, 0,
+    ble_gatt_disc_d_test_misc_all(5, 10, 0,
         ((struct ble_gatt_disc_d_test_dsc[]) { {
-            .chr_def_handle = 4,
+            .chr_val_handle = 5,
             .dsc_handle = 6,
             .dsc_uuid128 = BLE_UUID16_ARR(0x1234),
         }, {
@@ -227,13 +236,13 @@ TEST_CASE(ble_gatt_disc_d_test_1)
     );
 
     /*** Two 16-bit descriptors. */
-    ble_gatt_disc_d_test_misc_all(49, 100, 0,
+    ble_gatt_disc_d_test_misc_all(50, 100, 0,
         ((struct ble_gatt_disc_d_test_dsc[]) { {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 51,
             .dsc_uuid128 = BLE_UUID16_ARR(0x1111),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 52,
             .dsc_uuid128 = BLE_UUID16_ARR(0x2222),
         }, {
@@ -242,25 +251,25 @@ TEST_CASE(ble_gatt_disc_d_test_1)
     );
 
     /*** Five 16-bit descriptors. */
-    ble_gatt_disc_d_test_misc_all(49, 100, 0,
+    ble_gatt_disc_d_test_misc_all(50, 100, 0,
         ((struct ble_gatt_disc_d_test_dsc[]) { {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 51,
             .dsc_uuid128 = BLE_UUID16_ARR(0x1111),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 52,
             .dsc_uuid128 = BLE_UUID16_ARR(0x2222),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 53,
             .dsc_uuid128 = BLE_UUID16_ARR(0x3333),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 54,
             .dsc_uuid128 = BLE_UUID16_ARR(0x4444),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 55,
             .dsc_uuid128 = BLE_UUID16_ARR(0x5555),
         }, {
@@ -269,25 +278,25 @@ TEST_CASE(ble_gatt_disc_d_test_1)
     );
 
     /*** Interleaved 16-bit and 128-bit descriptors. */
-    ble_gatt_disc_d_test_misc_all(49, 100, 0,
+    ble_gatt_disc_d_test_misc_all(50, 100, 0,
         ((struct ble_gatt_disc_d_test_dsc[]) { {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 51,
             .dsc_uuid128 = BLE_UUID16_ARR(0x1111),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 52,
             .dsc_uuid128 = { 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 },
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 53,
             .dsc_uuid128 = BLE_UUID16_ARR(0x3333),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 54,
             .dsc_uuid128 = { 1,0,4,0,6,9,17,7,8,43,7,4,12,43,19,35 },
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 55,
             .dsc_uuid128 = BLE_UUID16_ARR(0x5555),
         }, {
@@ -296,13 +305,13 @@ TEST_CASE(ble_gatt_disc_d_test_1)
     );
 
     /*** Ends with final handle ID. */
-    ble_gatt_disc_d_test_misc_all(49, 52, 0,
+    ble_gatt_disc_d_test_misc_all(50, 52, 0,
         ((struct ble_gatt_disc_d_test_dsc[]) { {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 51,
             .dsc_uuid128 = BLE_UUID16_ARR(0x1111),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 52,
             .dsc_uuid128 = BLE_UUID16_ARR(0x2222),
         }, {
@@ -311,25 +320,25 @@ TEST_CASE(ble_gatt_disc_d_test_1)
     );
 
     /*** Stop after two descriptors. */
-    ble_gatt_disc_d_test_misc_all(49, 100, 2,
+    ble_gatt_disc_d_test_misc_all(50, 100, 2,
         ((struct ble_gatt_disc_d_test_dsc[]) { {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 51,
             .dsc_uuid128 = BLE_UUID16_ARR(0x1111),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 52,
             .dsc_uuid128 = BLE_UUID16_ARR(0x2222),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 53,
             .dsc_uuid128 = BLE_UUID16_ARR(0x3333),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 54,
             .dsc_uuid128 = BLE_UUID16_ARR(0x4444),
         }, {
-            .chr_def_handle = 49,
+            .chr_val_handle = 50,
             .dsc_handle = 55,
             .dsc_uuid128 = BLE_UUID16_ARR(0x5555),
         }, {
@@ -340,6 +349,8 @@ TEST_CASE(ble_gatt_disc_d_test_1)
 
 TEST_SUITE(ble_gatt_disc_d_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatt_disc_d_test_1();
 }
 
diff --git a/net/nimble/host/src/test/ble_gatt_disc_s_test.c b/net/nimble/host/src/test/ble_gatt_disc_s_test.c
index d7572b2c..2e278d6e 100644
--- a/net/nimble/host/src/test/ble_gatt_disc_s_test.c
+++ b/net/nimble/host/src/test/ble_gatt_disc_s_test.c
@@ -219,18 +219,28 @@ ble_gatt_disc_s_test_misc_verify_services(
 
 static int
 ble_gatt_disc_s_test_misc_disc_cb(uint16_t conn_handle,
-                                  struct ble_gatt_error *error,
-                                  struct ble_gatt_svc *service, void *arg)
+                                  const struct ble_gatt_error *error,
+                                  const struct ble_gatt_svc *service,
+                                  void *arg)
 {
-    TEST_ASSERT(error == NULL);
+    TEST_ASSERT(error != NULL);
     TEST_ASSERT(!ble_gatt_disc_s_test_rx_complete);
 
-    if (service == NULL) {
-        ble_gatt_disc_s_test_rx_complete = 1;
-    } else {
+    switch (error->status) {
+    case 0:
+        TEST_ASSERT(service != NULL);
         TEST_ASSERT_FATAL(ble_gatt_disc_s_test_num_svcs <
                           BLE_GATT_DISC_S_TEST_MAX_SERVICES);
         ble_gatt_disc_s_test_svcs[ble_gatt_disc_s_test_num_svcs++] = *service;
+        break;
+
+    case BLE_HS_EDONE:
+        TEST_ASSERT(service == NULL);
+        ble_gatt_disc_s_test_rx_complete = 1;
+        break;
+
+    default:
+        TEST_ASSERT(0);
     }
 
     return 0;
@@ -381,6 +391,8 @@ TEST_CASE(ble_gatt_disc_s_test_disc_service_uuid)
 
 TEST_SUITE(ble_gatt_disc_s_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatt_disc_s_test_disc_all();
     ble_gatt_disc_s_test_disc_service_uuid();
 }
diff --git a/net/nimble/host/src/test/ble_gatt_find_s_test.c b/net/nimble/host/src/test/ble_gatt_find_s_test.c
index dca1f367..c3ab93df 100644
--- a/net/nimble/host/src/test/ble_gatt_find_s_test.c
+++ b/net/nimble/host/src/test/ble_gatt_find_s_test.c
@@ -46,17 +46,25 @@ ble_gatt_find_s_test_misc_init(void)
 
 static int
 ble_gatt_find_s_test_misc_cb(uint16_t conn_handle,
-                             struct ble_gatt_error *error,
-                             struct ble_gatt_svc *service,
+                             const struct ble_gatt_error *error,
+                             const struct ble_gatt_svc *service,
                              void *arg)
 {
     TEST_ASSERT(!ble_gatt_find_s_test_proc_complete);
-    TEST_ASSERT(error == NULL);
+    TEST_ASSERT(error != NULL);
 
-    if (service == NULL) {
-        ble_gatt_find_s_test_proc_complete = 1;
-    } else {
+    switch (error->status) {
+    case 0:
         ble_gatt_find_s_test_svcs[ble_gatt_find_s_test_num_svcs++] = *service;
+        break;
+
+    case BLE_HS_EDONE:
+        ble_gatt_find_s_test_proc_complete = 1;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
     }
 
     return 0;
@@ -320,6 +328,8 @@ TEST_CASE(ble_gatt_find_s_test_1)
 
 TEST_SUITE(ble_gatt_find_s_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatt_find_s_test_1();
 }
 
diff --git a/net/nimble/host/src/test/ble_gatt_read_test.c b/net/nimble/host/src/test/ble_gatt_read_test.c
index 03df56ec..822de5c7 100644
--- a/net/nimble/host/src/test/ble_gatt_read_test.c
+++ b/net/nimble/host/src/test/ble_gatt_read_test.c
@@ -57,7 +57,7 @@ ble_gatt_read_test_misc_init(void)
 }
 
 static int
-ble_gatt_read_test_cb(uint16_t conn_handle, struct ble_gatt_error *error,
+ble_gatt_read_test_cb(uint16_t conn_handle, const struct ble_gatt_error *error,
                       struct ble_gatt_attr *attr, void *arg)
 {
     struct ble_gatt_read_test_attr *dst;
@@ -65,7 +65,9 @@ ble_gatt_read_test_cb(uint16_t conn_handle, struct ble_gatt_error *error,
 
     stop_after = arg;
 
-    if (error != NULL) {
+    TEST_ASSERT_FATAL(error != NULL);
+
+    if (error->status != 0) {
         ble_gatt_read_test_bad_conn_handle = conn_handle;
         ble_gatt_read_test_bad_status = error->status;
         ble_gatt_read_test_complete = 1;
@@ -81,12 +83,12 @@ ble_gatt_read_test_cb(uint16_t conn_handle, struct ble_gatt_error *error,
                       BLE_GATT_READ_TEST_MAX_ATTRS);
     dst = ble_gatt_read_test_attrs + ble_gatt_read_test_num_attrs++;
 
-    TEST_ASSERT_FATAL(attr->value_len <= sizeof dst->value);
+    TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(attr->om) <= sizeof dst->value);
 
     dst->conn_handle = conn_handle;
     dst->handle = attr->handle;
-    dst->value_len = attr->value_len;
-    memcpy(dst->value, attr->value, attr->value_len);
+    dst->value_len = OS_MBUF_PKTLEN(attr->om);
+    os_mbuf_copydata(attr->om, 0, OS_MBUF_PKTLEN(attr->om), dst->value);
 
     if (stop_after != NULL && *stop_after > 0) {
         (*stop_after)--;
@@ -102,7 +104,8 @@ ble_gatt_read_test_cb(uint16_t conn_handle, struct ble_gatt_error *error,
 }
 
 static int
-ble_gatt_read_test_long_cb(uint16_t conn_handle, struct ble_gatt_error *error,
+ble_gatt_read_test_long_cb(uint16_t conn_handle,
+                           const struct ble_gatt_error *error,
                            struct ble_gatt_attr *attr, void *arg)
 {
     struct ble_gatt_read_test_attr *dst;
@@ -110,7 +113,9 @@ ble_gatt_read_test_long_cb(uint16_t conn_handle, struct ble_gatt_error *error,
 
     reads_left = arg;
 
-    if (error != NULL) {
+    TEST_ASSERT_FATAL(error != NULL);
+
+    if (error->status != 0) {
         ble_gatt_read_test_bad_conn_handle = conn_handle;
         ble_gatt_read_test_bad_status = error->status;
         ble_gatt_read_test_complete = 1;
@@ -124,7 +129,8 @@ ble_gatt_read_test_long_cb(uint16_t conn_handle, struct ble_gatt_error *error,
 
     dst = ble_gatt_read_test_attrs + 0;
 
-    TEST_ASSERT_FATAL(attr->value_len <= dst->value_len + sizeof dst->value);
+    TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(attr->om) <=
+        dst->value_len + sizeof dst->value);
     TEST_ASSERT(attr->offset == dst->value_len);
 
     if (attr->offset == 0) {
@@ -134,8 +140,9 @@ ble_gatt_read_test_long_cb(uint16_t conn_handle, struct ble_gatt_error *error,
         TEST_ASSERT(conn_handle == dst->conn_handle);
         TEST_ASSERT(attr->handle == dst->handle);
     }
-    memcpy(dst->value + dst->value_len, attr->value, attr->value_len);
-    dst->value_len += attr->value_len;
+    os_mbuf_copydata(attr->om, 0, OS_MBUF_PKTLEN(attr->om),
+                     dst->value + dst->value_len);
+    dst->value_len += OS_MBUF_PKTLEN(attr->om);
 
     if (reads_left != NULL && *reads_left > 0) {
         (*reads_left)--;
@@ -151,7 +158,7 @@ ble_gatt_read_test_long_cb(uint16_t conn_handle, struct ble_gatt_error *error,
 static void
 ble_gatt_read_test_misc_rx_rsp_good_raw(uint16_t conn_handle,
                                         uint8_t att_op,
-                                        void *data, int data_len)
+                                        const void *data, int data_len)
 {
     uint8_t buf[1024];
     int rc;
@@ -171,7 +178,7 @@ ble_gatt_read_test_misc_rx_rsp_good_raw(uint16_t conn_handle,
 
 static void
 ble_gatt_read_test_misc_rx_rsp_good(uint16_t conn_handle,
-                                    struct ble_gatt_attr *attr)
+                                    struct ble_hs_test_util_flat_attr *attr)
 {
     ble_gatt_read_test_misc_rx_rsp_good_raw(conn_handle, BLE_ATT_OP_READ_RSP,
                                             attr->value,
@@ -190,8 +197,8 @@ ble_gatt_read_test_misc_rx_rsp_bad(uint16_t conn_handle,
 }
 
 static int
-ble_gatt_read_test_misc_uuid_rx_rsp_good(uint16_t conn_handle,
-                                         struct ble_gatt_attr *attrs)
+ble_gatt_read_test_misc_uuid_rx_rsp_good(
+    uint16_t conn_handle, struct ble_hs_test_util_flat_attr *attrs)
 {
     struct ble_att_read_type_rsp rsp;
     uint8_t buf[1024];
@@ -233,7 +240,7 @@ ble_gatt_read_test_misc_uuid_rx_rsp_good(uint16_t conn_handle,
 }
 
 static void
-ble_gatt_read_test_misc_verify_good(struct ble_gatt_attr *attr)
+ble_gatt_read_test_misc_verify_good(struct ble_hs_test_util_flat_attr *attr)
 {
     int rc;
 
@@ -256,7 +263,7 @@ ble_gatt_read_test_misc_verify_good(struct ble_gatt_attr *attr)
 
 static void
 ble_gatt_read_test_misc_verify_bad(uint8_t att_status,
-                                   struct ble_gatt_attr *attr)
+                                   struct ble_hs_test_util_flat_attr *attr)
 {
     int rc;
 
@@ -277,10 +284,9 @@ ble_gatt_read_test_misc_verify_bad(uint8_t att_status,
 }
 
 static void
-ble_gatt_read_test_misc_uuid_verify_good(uint16_t start_handle,
-                                         uint16_t end_handle, void *uuid128,
-                                         int stop_after,
-                                         struct ble_gatt_attr *attrs)
+ble_gatt_read_test_misc_uuid_verify_good(
+    uint16_t start_handle, uint16_t end_handle, void *uuid128,
+    int stop_after, struct ble_hs_test_util_flat_attr *attrs)
 {
     int num_read;
     int idx;
@@ -324,8 +330,8 @@ ble_gatt_read_test_misc_uuid_verify_good(uint16_t start_handle,
 }
 
 static void
-ble_gatt_read_test_misc_long_verify_good(int max_reads,
-                                         struct ble_gatt_attr *attr)
+ble_gatt_read_test_misc_long_verify_good(
+    int max_reads, struct ble_hs_test_util_flat_attr *attr)
 {
     int reads_left;
     int chunk_sz;
@@ -377,8 +383,8 @@ ble_gatt_read_test_misc_long_verify_good(int max_reads,
 }
 
 static void
-ble_gatt_read_test_misc_long_verify_bad(uint8_t att_status,
-                                        struct ble_gatt_attr *attr)
+ble_gatt_read_test_misc_long_verify_bad(
+    uint8_t att_status, struct ble_hs_test_util_flat_attr *attr)
 {
     int rc;
 
@@ -400,8 +406,8 @@ ble_gatt_read_test_misc_long_verify_bad(uint8_t att_status,
 }
 
 static int
-ble_gatt_read_test_misc_extract_handles(struct ble_gatt_attr *attrs,
-                                        uint16_t *handles)
+ble_gatt_read_test_misc_extract_handles(
+    struct ble_hs_test_util_flat_attr *attrs, uint16_t *handles)
 {
     int i;
 
@@ -412,7 +418,8 @@ ble_gatt_read_test_misc_extract_handles(struct ble_gatt_attr *attrs,
 }
 
 static void
-ble_gatt_read_test_misc_mult_verify_good(struct ble_gatt_attr *attrs)
+ble_gatt_read_test_misc_mult_verify_good(
+    struct ble_hs_test_util_flat_attr *attrs)
 {
     uint8_t expected_value[512];
     uint16_t handles[256];
@@ -458,9 +465,9 @@ ble_gatt_read_test_misc_mult_verify_good(struct ble_gatt_attr *attrs)
 }
 
 static void
-ble_gatt_read_test_misc_mult_verify_bad(uint8_t att_status,
-                                        uint16_t err_handle,
-                                        struct ble_gatt_attr *attrs)
+ble_gatt_read_test_misc_mult_verify_bad(
+    uint8_t att_status, uint16_t err_handle,
+    struct ble_hs_test_util_flat_attr *attrs)
 {
     uint16_t handles[256];
     int num_attrs;
@@ -488,39 +495,42 @@ ble_gatt_read_test_misc_mult_verify_bad(uint8_t att_status,
 TEST_CASE(ble_gatt_read_test_by_handle)
 {
     /* Read a seven-byte attribute. */
-    ble_gatt_read_test_misc_verify_good((struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 43,
-        .value = (uint8_t[]){ 1,2,3,4,5,6,7 },
+        .value = { 1,2,3,4,5,6,7 },
         .value_len = 7
     } });
 
     /* Read a one-byte attribute. */
-    ble_gatt_read_test_misc_verify_good((struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 0x5432,
-        .value = (uint8_t[]){ 0xff },
+        .value = { 0xff },
         .value_len = 1
     } });
 
     /* Read a 200-byte attribute. */
-    ble_gatt_read_test_misc_verify_good((struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 815,
-        .value = (uint8_t[200]){ 0 },
+        .value = { 0 },
         .value_len = 200,
     } });
 
     /* Fail due to attribute not found. */
     ble_gatt_read_test_misc_verify_bad(BLE_ATT_ERR_ATTR_NOT_FOUND,
-        (struct ble_gatt_attr[]) { {
+        (struct ble_hs_test_util_flat_attr[]) { {
             .handle = 719,
-            .value = (uint8_t[]){ 1,2,3,4,5,6,7 },
+            .value = { 1,2,3,4,5,6,7 },
             .value_len = 7
         } });
 
     /* Fail due to invalid PDU. */
     ble_gatt_read_test_misc_verify_bad(BLE_ATT_ERR_INVALID_PDU,
-        (struct ble_gatt_attr[]) { {
+        (struct ble_hs_test_util_flat_attr[]) { {
             .handle = 65,
-            .value = (uint8_t[]){ 0xfa, 0x4c },
+            .value = { 0xfa, 0x4c },
             .value_len = 2
         } });
 }
@@ -529,9 +539,9 @@ TEST_CASE(ble_gatt_read_test_by_uuid)
 {
     /* Read a single seven-byte attribute. */
     ble_gatt_read_test_misc_uuid_verify_good(1, 100, BLE_UUID16(0x1234), 0,
-        (struct ble_gatt_attr[]) { {
+        (struct ble_hs_test_util_flat_attr[]) { {
             .handle = 43,
-            .value = (uint8_t[]){ 1,2,3,4,5,6,7 },
+            .value = { 1,2,3,4,5,6,7 },
             .value_len = 7
         }, {
             0,
@@ -539,13 +549,13 @@ TEST_CASE(ble_gatt_read_test_by_uuid)
 
     /* Read two seven-byte attributes; one response. */
     ble_gatt_read_test_misc_uuid_verify_good(1, 100, BLE_UUID16(0x1234), 0,
-        (struct ble_gatt_attr[]) { {
+        (struct ble_hs_test_util_flat_attr[]) { {
             .handle = 43,
-            .value = (uint8_t[]){ 1,2,3,4,5,6,7 },
+            .value = { 1,2,3,4,5,6,7 },
             .value_len = 7
         }, {
             .handle = 44,
-            .value = (uint8_t[]){ 2,3,4,5,6,7,8 },
+            .value = { 2,3,4,5,6,7,8 },
             .value_len = 7
         }, {
             0,
@@ -553,13 +563,13 @@ TEST_CASE(ble_gatt_read_test_by_uuid)
 
     /* Read two attributes; two responses. */
     ble_gatt_read_test_misc_uuid_verify_good(1, 100, BLE_UUID16(0x1234), 0,
-        (struct ble_gatt_attr[]) { {
+        (struct ble_hs_test_util_flat_attr[]) { {
             .handle = 43,
-            .value = (uint8_t[]){ 1,2,3,4,5,6,7 },
+            .value = { 1,2,3,4,5,6,7 },
             .value_len = 7
         }, {
             .handle = 44,
-            .value = (uint8_t[]){ 2,3,4 },
+            .value = { 2,3,4 },
             .value_len = 3
         }, {
             0,
@@ -567,25 +577,25 @@ TEST_CASE(ble_gatt_read_test_by_uuid)
 
     /* Stop after three reads. */
     ble_gatt_read_test_misc_uuid_verify_good(1, 100, BLE_UUID16(0x1234), 3,
-        (struct ble_gatt_attr[]) { {
+        (struct ble_hs_test_util_flat_attr[]) { {
             .handle = 43,
-            .value = (uint8_t[]){ 1,2,3,4,5,6,7 },
+            .value = { 1,2,3,4,5,6,7 },
             .value_len = 7
         }, {
             .handle = 44,
-            .value = (uint8_t[]){ 2,3,4 },
+            .value = { 2,3,4 },
             .value_len = 3
         }, {
             .handle = 45,
-            .value = (uint8_t[]){ 2,3,4 },
+            .value = { 2,3,4 },
             .value_len = 3
         }, {
             .handle = 46,
-            .value = (uint8_t[]){ 3,4,5,6 },
+            .value = { 3,4,5,6 },
             .value_len = 4
         }, {
             .handle = 47,
-            .value = (uint8_t[]){ 2,3,4 },
+            .value = { 2,3,4 },
             .value_len = 3
         }, {
             0,
@@ -602,38 +612,52 @@ TEST_CASE(ble_gatt_read_test_long)
     }
 
     /* Read a seven-byte attribute. */
-    ble_gatt_read_test_misc_long_verify_good(0, (struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_long_verify_good(0,
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 43,
-        .value = data512,
+        .value = { 1,2,3,4,5,6,7 },
         .value_len = 7
     } });
 
     /* Read a zero-byte attribute. */
-    ble_gatt_read_test_misc_long_verify_good(0, (struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_long_verify_good(0,
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 43,
-        .value = NULL,
+        .value = { 0 },
         .value_len = 0
     } });
 
     /* Read a 60-byte attribute; three requests. */
-    ble_gatt_read_test_misc_long_verify_good(0, (struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_long_verify_good(0,
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 34,
-        .value = data512,
+        .value = {
+            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+            17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
+            33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
+            49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60
+        },
         .value_len = 60
     } });
 
     /* Stop after two reads. */
-    ble_gatt_read_test_misc_long_verify_good(2, (struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_long_verify_good(2,
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 34,
-        .value = data512,
+        .value = {
+            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+            17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
+            33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
+            49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60
+        },
         .value_len = 60
     } });
 
     /* Fail due to attribute not found. */
     ble_gatt_read_test_misc_long_verify_bad(BLE_ATT_ERR_ATTR_NOT_FOUND,
-        (struct ble_gatt_attr[]) { {
+        (struct ble_hs_test_util_flat_attr[]) { {
             .handle = 719,
-            .value = (uint8_t[]){ 1,2,3,4,5,6,7 },
+            .value = { 1, 2, 3, 4, 5, 6, 7 }, 
             .value_len = 7
         } });
 }
@@ -648,60 +672,64 @@ TEST_CASE(ble_gatt_read_test_mult)
     }
 
     /* Read one attribute. */
-    ble_gatt_read_test_misc_mult_verify_good((struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_mult_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 43,
-        .value = data512,
+        .value = { 0, 1, 2, 3, 4, 5, 6, 7 },
         .value_len = 7
     }, {
         0
     } });
 
     /* Read two attributes. */
-    ble_gatt_read_test_misc_mult_verify_good((struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_mult_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 43,
-        .value = data512,
+        .value = { 0, 1, 2, 3, 4, 5, 6, 7 },
         .value_len = 7,
     }, {
         .handle = 44,
-        .value = data512 + 7,
+        .value = { 8, 9, 10, 11 },
         .value_len = 4,
     }, {
         0
     } });
 
     /* Read two attributes (swap order). */
-    ble_gatt_read_test_misc_mult_verify_good((struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_mult_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 44,
-        .value = data512 + 7,
+        .value = { 8, 9, 10, 11 },
         .value_len = 4,
     }, {
         .handle = 43,
-        .value = data512,
+        .value = { 0, 1, 2, 3, 4, 5, 6, 7 },
         .value_len = 7,
     }, {
         0
     } });
 
     /* Read five attributes. */
-    ble_gatt_read_test_misc_mult_verify_good((struct ble_gatt_attr[]) { {
+    ble_gatt_read_test_misc_mult_verify_good(
+        (struct ble_hs_test_util_flat_attr[]) { {
         .handle = 43,
-        .value = data512,
+        .value = { 0, 1, 2, 3, 4, 5, 6, 7 },
         .value_len = 7,
     }, {
         .handle = 44,
-        .value = data512 + 7,
+        .value = { 8, 9, 10, 11 },
         .value_len = 4,
     }, {
         .handle = 145,
-        .value = data512 + 11,
+        .value = { 12, 13 },
         .value_len = 2,
     }, {
         .handle = 191,
-        .value = data512 + 13,
+        .value = { 14, 15, 16 },
         .value_len = 3,
     }, {
         .handle = 352,
-        .value = data512 + 16,
+        .value = { 17, 18, 19, 20 },
         .value_len = 4,
     }, {
         0
@@ -709,21 +737,81 @@ TEST_CASE(ble_gatt_read_test_mult)
 
     /* Fail due to attribute not found. */
     ble_gatt_read_test_misc_mult_verify_bad(BLE_ATT_ERR_ATTR_NOT_FOUND, 719,
-        (struct ble_gatt_attr[]) { {
+        (struct ble_hs_test_util_flat_attr[]) { {
             .handle = 719,
-            .value = (uint8_t[]){ 1,2,3,4,5,6,7 },
+            .value = { 1,2,3,4,5,6,7 },
             .value_len = 7
         }, {
             0
         } });
 }
 
+TEST_CASE(ble_gatt_read_test_concurrent)
+{
+    int rc;
+    int i;
+
+    ble_gatt_read_test_misc_init();
+    ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
+                                 NULL, NULL);
+
+    /***
+     * Perform three concurrent reads.  Assert that each response is correctly
+     * matched up with its corresponding GATT procedure.
+     */
+
+    struct ble_hs_test_util_flat_attr attrs[3] = {
+        {
+            .handle = 1,
+            .offset = 0,
+            .value_len = 3,
+            .value = { 1, 2, 3 },
+        },
+        {
+            .handle = 2,
+            .offset = 0,
+            .value_len = 4,
+            .value = { 2, 3, 4, 5 },
+        },
+        {
+            .handle = 3,
+            .offset = 0,
+            .value_len = 5,
+            .value = { 3, 4, 5, 6, 7 },
+        },
+    };
+
+    rc = ble_gattc_read(2, attrs[0].handle, ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    rc = ble_gattc_read(2, attrs[1].handle, ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    rc = ble_gattc_read(2, attrs[2].handle, ble_gatt_read_test_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_gatt_read_test_misc_rx_rsp_good(2, attrs + 0);
+    ble_gatt_read_test_misc_rx_rsp_good(2, attrs + 1);
+    ble_gatt_read_test_misc_rx_rsp_good(2, attrs + 2);
+
+    TEST_ASSERT(ble_gatt_read_test_num_attrs == 3);
+
+    for (i = 0; i < 3; i++) {
+        TEST_ASSERT(ble_gatt_read_test_attrs[i].handle == attrs[i].handle);
+        TEST_ASSERT(ble_gatt_read_test_attrs[i].value_len ==
+                    attrs[i].value_len);
+        TEST_ASSERT(memcmp(ble_gatt_read_test_attrs[i].value, attrs[i].value,
+                           attrs[i].value_len) == 0);
+    }
+}
+
 TEST_SUITE(ble_gatt_read_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatt_read_test_by_handle();
     ble_gatt_read_test_by_uuid();
     ble_gatt_read_test_long();
     ble_gatt_read_test_mult();
+    ble_gatt_read_test_concurrent();
 }
 
 int
diff --git a/net/nimble/host/src/test/ble_gatt_write_test.c b/net/nimble/host/src/test/ble_gatt_write_test.c
index 56e88d36..f5481980 100644
--- a/net/nimble/host/src/test/ble_gatt_write_test.c
+++ b/net/nimble/host/src/test/ble_gatt_write_test.c
@@ -33,7 +33,7 @@ static int ble_gatt_write_test_cb_called;
 static uint8_t ble_gatt_write_test_attr_value[BLE_ATT_ATTR_MAX_LEN];
 static struct ble_gatt_error ble_gatt_write_test_error;
 
-static struct ble_gatt_attr *
+static struct ble_hs_test_util_flat_attr
 ble_gatt_write_test_attrs[BLE_GATT_WRITE_TEST_MAX_ATTRS];
 static int ble_gatt_write_test_num_attrs;
 
@@ -52,20 +52,22 @@ ble_gatt_write_test_init(void)
 }
 
 static int
-ble_gatt_write_test_cb_good(uint16_t conn_handle, struct ble_gatt_error *error,
+ble_gatt_write_test_cb_good(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
                             struct ble_gatt_attr *attr, void *arg)
 {
     int *attr_len;
 
     attr_len = arg;
 
+    TEST_ASSERT(error != NULL);
     TEST_ASSERT(conn_handle == 2);
+
+    ble_gatt_write_test_error = *error;
+
     if (attr_len != NULL) {
-        TEST_ASSERT(error == NULL);
+        TEST_ASSERT(error->status == 0);
         TEST_ASSERT(attr->handle == 100);
-    } else {
-        TEST_ASSERT(error != NULL);
-        ble_gatt_write_test_error = *error;
     }
 
     ble_gatt_write_test_cb_called = 1;
@@ -88,7 +90,7 @@ ble_gatt_write_test_rx_rsp(uint16_t conn_handle)
 static void
 ble_gatt_write_test_rx_prep_rsp(uint16_t conn_handle, uint16_t attr_handle,
                                 uint16_t offset,
-                                void *attr_data, uint16_t attr_data_len)
+                                const void *attr_data, uint16_t attr_data_len)
 {
     struct ble_att_prep_write_cmd rsp;
     uint8_t buf[512];
@@ -121,6 +123,7 @@ ble_gatt_write_test_rx_exec_rsp(uint16_t conn_handle)
 static void
 ble_gatt_write_test_misc_long_good(int attr_len)
 {
+    uint16_t mtu;
     int off;
     int len;
     int rc;
@@ -130,24 +133,27 @@ ble_gatt_write_test_misc_long_good(int attr_len)
     ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
                                  NULL, NULL);
 
-    rc = ble_gattc_write_long(2, 100, ble_gatt_write_test_attr_value,
-                              attr_len, ble_gatt_write_test_cb_good,
-                              &attr_len);
+    mtu = ble_att_mtu(2);
+
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        2, 100, ble_gatt_write_test_attr_value, attr_len,
+        ble_gatt_write_test_cb_good, &attr_len);
     TEST_ASSERT(rc == 0);
 
     off = 0;
     while (off < attr_len) {
-        /* Send the pending ATT Prep Write Command. */
-        ble_hs_test_util_tx_all();
-
-        /* Receive Prep Write response. */
-        len = BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+        len = mtu - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
         if (off + len > attr_len) {
             len = attr_len - off;
         }
-        ble_gatt_write_test_rx_prep_rsp(2, 100, off,
-                                        ble_gatt_write_test_attr_value + off,
-                                        len);
+
+        /* Send the pending ATT Prep Write Command. */
+        ble_hs_test_util_verify_tx_prep_write(
+            100, off, ble_gatt_write_test_attr_value + off, len);
+
+        /* Receive Prep Write response. */
+        ble_gatt_write_test_rx_prep_rsp(
+            2, 100, off, ble_gatt_write_test_attr_value + off, len);
 
         /* Verify callback hasn't gotten called. */
         TEST_ASSERT(!ble_gatt_write_test_cb_called);
@@ -155,6 +161,9 @@ ble_gatt_write_test_misc_long_good(int attr_len)
         off += len;
     }
 
+    /* Verify execute write request sent. */
+    ble_hs_test_util_verify_tx_exec_write(BLE_ATT_EXEC_WRITE_F_CONFIRM);
+
     /* Receive Exec Write response. */
     ble_hs_test_util_tx_all();
     ble_gatt_write_test_rx_exec_rsp(2);
@@ -170,6 +179,7 @@ static void
 ble_gatt_write_test_misc_long_bad(int attr_len,
                                   ble_gatt_write_test_long_fail_fn *cb)
 {
+    uint16_t mtu;
     int fail_now;
     int off;
     int len;
@@ -179,17 +189,24 @@ ble_gatt_write_test_misc_long_bad(int attr_len,
 
     ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
                                  NULL, NULL);
+    mtu = ble_att_mtu(2);
 
-    rc = ble_gattc_write_long(2, 100, ble_gatt_write_test_attr_value,
-                              attr_len, ble_gatt_write_test_cb_good, NULL);
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        2, 100, ble_gatt_write_test_attr_value, attr_len,
+        ble_gatt_write_test_cb_good, NULL);
     TEST_ASSERT(rc == 0);
 
     fail_now = 0;
     off = 0;
     while (off < attr_len) {
+        len = mtu - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+        if (off + len > attr_len) {
+            len = attr_len - off;
+        }
+
         /* Send the pending ATT Prep Write Command. */
-        ble_hs_test_util_tx_all();
-        TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() != NULL);
+        ble_hs_test_util_verify_tx_prep_write(
+            100, off, ble_gatt_write_test_attr_value + off, len);
 
         /* Receive Prep Write response. */
         len = BLE_ATT_MTU_DFLT - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
@@ -255,7 +272,7 @@ ble_gatt_write_test_misc_long_fail_length(uint16_t conn_handle,
 
 static int
 ble_gatt_write_test_reliable_cb_good(uint16_t conn_handle,
-                                     struct ble_gatt_error *error,
+                                     const struct ble_gatt_error *error,
                                      struct ble_gatt_attr *attrs,
                                      uint8_t num_attrs, void *arg)
 {
@@ -267,7 +284,8 @@ ble_gatt_write_test_reliable_cb_good(uint16_t conn_handle,
 
     ble_gatt_write_test_num_attrs = num_attrs;
     for (i = 0; i < num_attrs; i++) {
-        ble_gatt_write_test_attrs[i] = attrs + i;
+        ble_hs_test_util_attr_to_flat(ble_gatt_write_test_attrs + i,
+                                      attrs + i);
     }
 
     ble_gatt_write_test_cb_called = 1;
@@ -276,38 +294,66 @@ ble_gatt_write_test_reliable_cb_good(uint16_t conn_handle,
 }
 
 static void
-ble_gatt_write_test_misc_reliable_good(struct ble_gatt_attr *attrs)
+ble_gatt_write_test_misc_reliable_good(
+    struct ble_hs_test_util_flat_attr *flat_attrs)
 {
+    const struct ble_hs_test_util_flat_attr *attr;
+    struct ble_gatt_attr attrs[16];
+    uint16_t mtu;
     int num_attrs;
     int attr_idx;
+    int len;
+    int off;
     int rc;
     int i;
 
     ble_gatt_write_test_init();
 
-    for (num_attrs = 0; attrs[num_attrs].handle != 0; num_attrs++)
-        ;
+    for (num_attrs = 0; flat_attrs[num_attrs].handle != 0; num_attrs++) {
+        TEST_ASSERT_FATAL(num_attrs < sizeof attrs / sizeof attrs[0]);
+        ble_hs_test_util_attr_from_flat(attrs + num_attrs,
+                                        flat_attrs + num_attrs);
+    }
 
     ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
                                  NULL, NULL);
+    mtu = ble_att_mtu(2);
 
     rc = ble_gattc_write_reliable(2, attrs, num_attrs,
                                   ble_gatt_write_test_reliable_cb_good, NULL);
     TEST_ASSERT(rc == 0);
 
-    for (attr_idx = 0; attr_idx < num_attrs; attr_idx++) {
+    attr_idx = 0;
+    off = 0;
+    while (attr_idx < num_attrs) {
+        attr = flat_attrs + attr_idx;
+
+        len = mtu - BLE_ATT_PREP_WRITE_CMD_BASE_SZ;
+        if (off + len > attr->value_len) {
+            len = attr->value_len - off;
+        }
+
         /* Send the pending ATT Prep Write Command. */
-        ble_hs_test_util_tx_all();
+        ble_hs_test_util_verify_tx_prep_write(attr->handle, off,
+                                              attr->value + off, len);
 
         /* Receive Prep Write response. */
-        ble_gatt_write_test_rx_prep_rsp(2, attrs[attr_idx].handle, 0,
-                                        attrs[attr_idx].value,
-                                        attrs[attr_idx].value_len);
+        ble_gatt_write_test_rx_prep_rsp(2, attr->handle, off,
+                                        attr->value + off, len);
 
         /* Verify callback hasn't gotten called. */
         TEST_ASSERT(!ble_gatt_write_test_cb_called);
+
+        off += len;
+        if (off >= attr->value_len) {
+            attr_idx++;
+            off = 0;
+        }
     }
 
+    /* Verify execute write request sent. */
+    ble_hs_test_util_verify_tx_exec_write(BLE_ATT_EXEC_WRITE_F_CONFIRM);
+
     /* Receive Exec Write response. */
     ble_hs_test_util_tx_all();
     ble_gatt_write_test_rx_exec_rsp(2);
@@ -316,7 +362,9 @@ ble_gatt_write_test_misc_reliable_good(struct ble_gatt_attr *attrs)
     TEST_ASSERT(ble_gatt_write_test_cb_called);
     TEST_ASSERT(ble_gatt_write_test_num_attrs == num_attrs);
     for (i = 0; i < num_attrs; i++) {
-        TEST_ASSERT(ble_gatt_write_test_attrs[i] == attrs + i);
+        rc = ble_hs_test_util_flat_attr_cmp(
+            ble_gatt_write_test_attrs + i, flat_attrs + i);
+        TEST_ASSERT(rc == 0);
     }
 }
 
@@ -331,8 +379,8 @@ TEST_CASE(ble_gatt_write_test_no_rsp)
                                  NULL, NULL);
 
     attr_len = 4;
-    rc = ble_gattc_write_no_rsp(2, 100, ble_gatt_write_test_attr_value,
-                                attr_len);
+    rc = ble_hs_test_util_gatt_write_no_rsp_flat(
+        2, 100, ble_gatt_write_test_attr_value, attr_len);
     TEST_ASSERT(rc == 0);
 
     /* Send the pending ATT Write Command. */
@@ -352,8 +400,9 @@ TEST_CASE(ble_gatt_write_test_rsp)
                                  NULL, NULL);
 
     attr_len = 4;
-    ble_gattc_write(2, 100, ble_gatt_write_test_attr_value, attr_len,
-                    ble_gatt_write_test_cb_good, &attr_len);
+    ble_hs_test_util_gatt_write_flat(2, 100, ble_gatt_write_test_attr_value,
+                                     attr_len, ble_gatt_write_test_cb_good,
+                                     &attr_len);
 
     /* Send the pending ATT Write Command. */
     ble_hs_test_util_tx_all();
@@ -458,42 +507,57 @@ TEST_CASE(ble_gatt_write_test_reliable_good)
 {
     /*** 1 attribute. */
     ble_gatt_write_test_misc_reliable_good(
-        ((struct ble_gatt_attr[]) { {
+        ((struct ble_hs_test_util_flat_attr[]) { {
             .handle = 100,
             .value_len = 2,
-            .value = (uint8_t[]){ 1, 2 },
+            .value = { 1, 2 },
         }, {
             0
         } }));
 
     /*** 2 attributes. */
     ble_gatt_write_test_misc_reliable_good(
-        ((struct ble_gatt_attr[]) { {
+        ((struct ble_hs_test_util_flat_attr[]) { {
             .handle = 100,
             .value_len = 2,
-            .value = (uint8_t[]){ 1,2 },
+            .value = { 1,2 },
         }, {
             .handle = 113,
             .value_len = 6,
-            .value = (uint8_t[]){ 5,6,7,8,9,10 },
+            .value = { 5,6,7,8,9,10 },
         }, {
             0
         } }));
 
     /*** 3 attributes. */
     ble_gatt_write_test_misc_reliable_good(
-        ((struct ble_gatt_attr[]) { {
+        ((struct ble_hs_test_util_flat_attr[]) { {
             .handle = 100,
             .value_len = 2,
-            .value = (uint8_t[]){ 1,2 },
+            .value = { 1,2 },
         }, {
             .handle = 113,
             .value_len = 6,
-            .value = (uint8_t[]){ 5,6,7,8,9,10 },
+            .value = { 5,6,7,8,9,10 },
         }, {
             .handle = 144,
             .value_len = 1,
-            .value = (uint8_t[]){ 0xff },
+            .value = { 0xff },
+        }, {
+            0
+        } }));
+
+    /*** Long attributes. */
+    ble_gatt_write_test_misc_reliable_good(
+        ((struct ble_hs_test_util_flat_attr[]) { {
+            .handle = 100,
+            .value_len = 20,
+            .value = { 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20 },
+        }, {
+            .handle = 144,
+            .value_len = 20,
+            .value = { 11,12,13,14,15,16,17,18,19,110,
+                       111,112,113,114,115,116,117,118,119,120 },
         }, {
             0
         } }));
@@ -511,8 +575,9 @@ TEST_CASE(ble_gatt_write_test_long_queue_full)
     ble_hs_test_util_create_conn(2, ((uint8_t[]){2,3,4,5,6,7,8,9}),
                                  NULL, NULL);
 
-    rc = ble_gattc_write_long(2, 100, ble_gatt_write_test_attr_value,
-                              128, ble_gatt_write_test_cb_good, NULL);
+    rc = ble_hs_test_util_gatt_write_long_flat(
+        2, 100, ble_gatt_write_test_attr_value, 128,
+        ble_gatt_write_test_cb_good, NULL);
     TEST_ASSERT(rc == 0);
 
     off = 0;
@@ -552,6 +617,8 @@ TEST_CASE(ble_gatt_write_test_long_queue_full)
 
 TEST_SUITE(ble_gatt_write_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatt_write_test_no_rsp();
     ble_gatt_write_test_rsp();
     ble_gatt_write_test_long_good();
diff --git a/net/nimble/host/src/test/ble_gatts_notify_test.c b/net/nimble/host/src/test/ble_gatts_notify_test.c
index 6a0f2188..4e2d587c 100644
--- a/net/nimble/host/src/test/ble_gatts_notify_test.c
+++ b/net/nimble/host/src/test/ble_gatts_notify_test.c
@@ -29,16 +29,17 @@
 #define BLE_GATTS_NOTIFY_TEST_CHR_1_UUID    0x1111
 #define BLE_GATTS_NOTIFY_TEST_CHR_2_UUID    0x2222
 
-static uint8_t ble_gatts_peer_addr[6] = {2,3,4,5,6,7};
+#define BLE_GATTS_NOTIFY_TEST_MAX_EVENTS    16
+
+static uint8_t ble_gatts_notify_test_peer_addr[6] = {2,3,4,5,6,7};
 
 static int
 ble_gatts_notify_test_misc_access(uint16_t conn_handle,
-                                  uint16_t attr_handle, uint8_t op,
-                                  union ble_gatt_access_ctxt *ctxt,
+                                  uint16_t attr_handle, 
+                                  struct ble_gatt_access_ctxt *ctxt,
                                   void *arg);
 static void
-ble_gatts_notify_test_misc_reg_cb(uint8_t op,
-                                  union ble_gatt_register_ctxt *ctxt,
+ble_gatts_notify_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt,
                                   void *arg);
 
 static const struct ble_gatt_svc_def ble_gatts_notify_test_svcs[] = { {
@@ -69,10 +70,34 @@ static uint16_t ble_gatts_notify_test_chr_2_def_handle;
 static uint8_t ble_gatts_notify_test_chr_2_val[1024];
 static int ble_gatts_notify_test_chr_2_len;
 
+static struct ble_gap_event
+ble_gatts_notify_test_events[BLE_GATTS_NOTIFY_TEST_MAX_EVENTS];
+
+static int ble_gatts_notify_test_num_events;
+
 typedef int ble_store_write_fn(int obj_type, union ble_store_value *val);
 
 typedef int ble_store_delete_fn(int obj_type, union ble_store_key *key);
 
+static int
+ble_gatts_notify_test_util_gap_event(struct ble_gap_event *event, void *arg)
+{
+    switch (event->type) {
+    case BLE_GAP_EVENT_NOTIFY_TX:
+    case BLE_GAP_EVENT_SUBSCRIBE:
+        TEST_ASSERT_FATAL(ble_gatts_notify_test_num_events <
+                          BLE_GATTS_NOTIFY_TEST_MAX_EVENTS);
+
+        ble_gatts_notify_test_events[ble_gatts_notify_test_num_events++] =
+            *event;
+
+    default:
+        break;
+    }
+
+    return 0;
+}
+
 static uint16_t
 ble_gatts_notify_test_misc_read_notify(uint16_t conn_handle,
                                        uint16_t chr_def_handle)
@@ -102,9 +127,9 @@ ble_gatts_notify_test_misc_read_notify(uint16_t conn_handle,
 }
 
 static void
-ble_gatts_notify_test_misc_enable_notify(uint16_t conn_handle,
-                                         uint16_t chr_def_handle,
-                                         uint16_t flags)
+ble_gatts_notify_test_misc_try_enable_notify(uint16_t conn_handle,
+                                             uint16_t chr_def_handle,
+                                             uint16_t flags, int fail)
 {
     struct ble_att_write_req req;
     uint8_t buf[BLE_ATT_WRITE_REQ_BASE_SZ + 2];
@@ -116,7 +141,86 @@ ble_gatts_notify_test_misc_enable_notify(uint16_t conn_handle,
     htole16(buf + BLE_ATT_WRITE_REQ_BASE_SZ, flags);
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
-    TEST_ASSERT(rc == 0);
+    if (fail) {
+        TEST_ASSERT_FATAL(rc != 0);
+        ble_hs_test_util_verify_tx_err_rsp(BLE_ATT_OP_WRITE_REQ,
+                                           req.bawq_handle,
+                                           BLE_ATT_ERR_REQ_NOT_SUPPORTED);
+    } else {
+        TEST_ASSERT_FATAL(rc == 0);
+        ble_hs_test_util_verify_tx_write_rsp();
+    }
+}
+
+static void
+ble_gatts_notify_test_misc_enable_notify(uint16_t conn_handle,
+                                         uint16_t chr_def_handle,
+                                         uint16_t flags)
+{
+    ble_gatts_notify_test_misc_try_enable_notify(conn_handle,
+                                                 chr_def_handle,
+                                                 flags, 0);
+}
+
+static void
+ble_gatts_notify_test_util_next_event(struct ble_gap_event *event)
+{
+    TEST_ASSERT_FATAL(ble_gatts_notify_test_num_events > 0);
+
+    *event = *ble_gatts_notify_test_events;
+
+    ble_gatts_notify_test_num_events--;
+    if (ble_gatts_notify_test_num_events > 0) {
+        memmove(ble_gatts_notify_test_events + 0,
+                ble_gatts_notify_test_events + 1,
+                ble_gatts_notify_test_num_events * sizeof *event);
+    }
+}
+
+static void
+ble_gatts_notify_test_util_verify_sub_event(uint16_t conn_handle,
+                                            uint8_t attr_handle,
+                                            uint8_t reason,
+                                            uint8_t prevn, uint8_t curn,
+                                            uint8_t previ, uint8_t curi)
+{
+    struct ble_gap_event event;
+
+    ble_gatts_notify_test_util_next_event(&event);
+
+    TEST_ASSERT(event.type == BLE_GAP_EVENT_SUBSCRIBE);
+    TEST_ASSERT(event.subscribe.conn_handle == conn_handle);
+    TEST_ASSERT(event.subscribe.attr_handle == attr_handle);
+    TEST_ASSERT(event.subscribe.reason == reason);
+    TEST_ASSERT(event.subscribe.prev_notify == prevn);
+    TEST_ASSERT(event.subscribe.cur_notify == curn);
+    TEST_ASSERT(event.subscribe.prev_indicate == previ);
+    TEST_ASSERT(event.subscribe.cur_indicate == curi);
+}
+
+static void
+ble_gatts_notify_test_util_verify_tx_event(uint16_t conn_handle,
+                                           uint8_t attr_handle,
+                                           int status,
+                                           int indication)
+{
+    struct ble_gap_event event;
+
+    ble_gatts_notify_test_util_next_event(&event);
+
+    TEST_ASSERT(event.type == BLE_GAP_EVENT_NOTIFY_TX);
+    TEST_ASSERT(event.notify_tx.status == status);
+    TEST_ASSERT(event.notify_tx.conn_handle == conn_handle);
+    TEST_ASSERT(event.notify_tx.attr_handle == attr_handle);
+    TEST_ASSERT(event.notify_tx.indication == indication);
+}
+
+static void
+ble_gatts_notify_test_util_verify_ack_event(uint16_t conn_handle,
+                                            uint8_t attr_handle)
+{
+    ble_gatts_notify_test_util_verify_tx_event(conn_handle, attr_handle,
+                                               BLE_HS_EDONE, 1);
 }
 
 static void
@@ -130,6 +234,8 @@ ble_gatts_notify_test_misc_init(uint16_t *out_conn_handle, int bonding,
 
     ble_hs_test_util_init();
 
+    ble_gatts_notify_test_num_events = 0;
+
     ble_hs_test_util_store_init(10, 10, 10);
     ble_hs_cfg.store_read_cb = ble_hs_test_util_store_read;
     ble_hs_cfg.store_write_cb = ble_hs_test_util_store_write;
@@ -142,7 +248,8 @@ ble_gatts_notify_test_misc_init(uint16_t *out_conn_handle, int bonding,
 
     ble_gatts_start();
 
-    ble_hs_test_util_create_conn(2, ble_gatts_peer_addr, NULL, NULL);
+    ble_hs_test_util_create_conn(2, ble_gatts_notify_test_peer_addr,
+                                 ble_gatts_notify_test_util_gap_event, NULL);
     *out_conn_handle = 2;
 
     if (bonding) {
@@ -163,16 +270,35 @@ ble_gatts_notify_test_misc_init(uint16_t *out_conn_handle, int bonding,
         2, ble_gatts_notify_test_chr_2_def_handle);
     TEST_ASSERT(flags == 0);
 
-    /* Set initial notification / indication state. */
+    /* Set initial notification / indication state and verify that subscription
+     * callback gets executed.
+     */
     if (chr1_flags != 0) {
         ble_gatts_notify_test_misc_enable_notify(
             2, ble_gatts_notify_test_chr_1_def_handle, chr1_flags);
+
+        ble_gatts_notify_test_util_verify_sub_event(
+            *out_conn_handle,
+            ble_gatts_notify_test_chr_1_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_WRITE,
+            0, chr1_flags == BLE_GATTS_CLT_CFG_F_NOTIFY,
+            0, chr1_flags == BLE_GATTS_CLT_CFG_F_INDICATE);
     }
     if (chr2_flags != 0) {
         ble_gatts_notify_test_misc_enable_notify(
             2, ble_gatts_notify_test_chr_2_def_handle, chr2_flags);
+
+        ble_gatts_notify_test_util_verify_sub_event(
+            *out_conn_handle,
+            ble_gatts_notify_test_chr_2_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_WRITE,
+            0, chr2_flags == BLE_GATTS_CLT_CFG_F_NOTIFY,
+            0, chr2_flags == BLE_GATTS_CLT_CFG_F_INDICATE);
     }
 
+    /* Ensure no extraneous subscription callbacks were executed. */
+    TEST_ASSERT(ble_gatts_notify_test_num_events == 0);
+
     /* Toss both write responses. */
     ble_hs_test_util_prev_tx_queue_clear();
 
@@ -185,42 +311,76 @@ ble_gatts_notify_test_misc_init(uint16_t *out_conn_handle, int bonding,
     TEST_ASSERT(flags == chr2_flags);
 
     /* Ensure both CCCDs still persisted. */
-    exp_num_cccds = (chr1_flags != 0) + (chr2_flags != 0);
+    if (bonding) {
+        exp_num_cccds = (chr1_flags != 0) + (chr2_flags != 0);
+    } else {
+        exp_num_cccds = 0;
+    }
     TEST_ASSERT(ble_hs_test_util_store_num_cccds == exp_num_cccds);
 }
 
 static void
-ble_gatts_restore_bonding(uint16_t conn_handle)
+ble_gatts_notify_test_disconnect(uint16_t conn_handle,
+                                 uint8_t chr1_flags,
+                                 uint8_t chr1_indicate_in_progress,
+                                 uint8_t chr2_flags,
+                                 uint8_t chr2_indicate_in_progress)
 {
-    struct ble_hs_conn *conn;
+    ble_hs_test_util_conn_disconnect(conn_handle);
 
-    ble_hs_lock();
-    conn = ble_hs_conn_find(conn_handle);
-    TEST_ASSERT_FATAL(conn != NULL);
-    conn->bhc_sec_state.encrypted = 1;
-    conn->bhc_sec_state.authenticated = 1;
-    conn->bhc_sec_state.bonded = 1;
-    ble_hs_unlock();
+    if (chr1_indicate_in_progress) {
+        ble_gatts_notify_test_util_verify_tx_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_1_def_handle + 1,
+            BLE_HS_ENOTCONN,
+            1);
+    }
 
-    ble_gatts_bonding_restored(conn_handle);
+    /* Verify subscription callback executed for each subscribed
+     * characteristic.
+     */
+    if (chr1_flags != 0) {
+        ble_gatts_notify_test_util_verify_sub_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_1_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_TERM,
+            chr1_flags == BLE_GATTS_CLT_CFG_F_NOTIFY, 0,
+            chr1_flags == BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+    }
+
+    if (chr2_indicate_in_progress) {
+        ble_gatts_notify_test_util_verify_tx_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_2_def_handle + 1,
+            BLE_HS_ENOTCONN,
+            1);
+    }
+
+    if (chr2_flags != 0) {
+        ble_gatts_notify_test_util_verify_sub_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_2_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_TERM,
+            chr2_flags == BLE_GATTS_CLT_CFG_F_NOTIFY, 0,
+            chr2_flags == BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+    }
 }
 
 static void
-ble_gatts_notify_test_misc_reg_cb(uint8_t op,
-                                  union ble_gatt_register_ctxt *ctxt,
+ble_gatts_notify_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt,
                                   void *arg)
 {
     uint16_t uuid16;
 
-    if (op == BLE_GATT_REGISTER_OP_CHR) {
-        uuid16 = ble_uuid_128_to_16(ctxt->chr_reg.chr->uuid128);
+    if (ctxt->op == BLE_GATT_REGISTER_OP_CHR) {
+        uuid16 = ble_uuid_128_to_16(ctxt->chr.chr_def->uuid128);
         switch (uuid16) {
         case BLE_GATTS_NOTIFY_TEST_CHR_1_UUID:
-            ble_gatts_notify_test_chr_1_def_handle = ctxt->chr_reg.def_handle;
+            ble_gatts_notify_test_chr_1_def_handle = ctxt->chr.def_handle;
             break;
 
         case BLE_GATTS_NOTIFY_TEST_CHR_2_UUID:
-            ble_gatts_notify_test_chr_2_def_handle = ctxt->chr_reg.def_handle;
+            ble_gatts_notify_test_chr_2_def_handle = ctxt->chr.def_handle;
             break;
 
         default:
@@ -232,32 +392,37 @@ ble_gatts_notify_test_misc_reg_cb(uint8_t op,
 
 static int
 ble_gatts_notify_test_misc_access(uint16_t conn_handle,
-                                  uint16_t attr_handle, uint8_t op,
-                                  union ble_gatt_access_ctxt *ctxt,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
                                   void *arg)
 {
-    TEST_ASSERT_FATAL(op == BLE_GATT_ACCESS_OP_READ_CHR);
+    int rc;
+
+    TEST_ASSERT_FATAL(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
     TEST_ASSERT(conn_handle == 0xffff);
 
     if (attr_handle == ble_gatts_notify_test_chr_1_def_handle + 1) {
-        TEST_ASSERT(ctxt->chr_access.chr ==
+        TEST_ASSERT(ctxt->chr ==
                     &ble_gatts_notify_test_svcs[0].characteristics[0]);
-        ctxt->chr_access.data = ble_gatts_notify_test_chr_1_val;
-        ctxt->chr_access.len = ble_gatts_notify_test_chr_1_len;
+        rc = os_mbuf_copyinto(ctxt->om, 0, ble_gatts_notify_test_chr_1_val,
+                              ble_gatts_notify_test_chr_1_len);
+        TEST_ASSERT_FATAL(rc == 0);
     } else if (attr_handle == ble_gatts_notify_test_chr_2_def_handle + 1) {
-        TEST_ASSERT(ctxt->chr_access.chr ==
+        TEST_ASSERT(ctxt->chr ==
                     &ble_gatts_notify_test_svcs[0].characteristics[1]);
-        ctxt->chr_access.data = ble_gatts_notify_test_chr_2_val;
-        ctxt->chr_access.len = ble_gatts_notify_test_chr_2_len;
+        rc = os_mbuf_copyinto(ctxt->om, 0, ble_gatts_notify_test_chr_2_val,
+                              ble_gatts_notify_test_chr_2_len);
+        TEST_ASSERT_FATAL(rc == 0);
     } else {
-        TEST_ASSERT(0);
+        TEST_ASSERT_FATAL(0);
     }
 
     return 0;
 }
 
 static void
-ble_gatts_notify_test_misc_rx_indicate_rsp(uint16_t conn_handle)
+ble_gatts_notify_test_misc_rx_indicate_rsp(uint16_t conn_handle,
+                                           uint16_t attr_handle)
 {
     uint8_t buf[BLE_ATT_INDICATE_RSP_SZ];
     int rc;
@@ -267,11 +432,15 @@ ble_gatts_notify_test_misc_rx_indicate_rsp(uint16_t conn_handle)
     rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
                                                 buf, sizeof buf);
     TEST_ASSERT(rc == 0);
+
+    ble_gatts_notify_test_util_verify_ack_event(conn_handle, attr_handle);
 }
 
 
 static void
-ble_gatts_notify_test_misc_verify_tx_n(uint8_t *attr_data, int attr_len)
+ble_gatts_notify_test_misc_verify_tx_n(uint16_t conn_handle,
+                                       uint16_t attr_handle,
+                                       uint8_t *attr_data, int attr_len)
 {
     struct ble_att_notify_req req;
     struct os_mbuf *om;
@@ -283,15 +452,20 @@ ble_gatts_notify_test_misc_verify_tx_n(uint8_t *attr_data, int attr_len)
     TEST_ASSERT_FATAL(om != NULL);
 
     ble_att_notify_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.banq_handle == attr_handle);
 
     for (i = 0; i < attr_len; i++) {
         TEST_ASSERT(om->om_data[BLE_ATT_NOTIFY_REQ_BASE_SZ + i] ==
                     attr_data[i]);
     }
+
+    ble_gatts_notify_test_util_verify_tx_event(conn_handle, attr_handle, 0, 0);
 }
 
 static void
-ble_gatts_notify_test_misc_verify_tx_i(uint8_t *attr_data, int attr_len)
+ble_gatts_notify_test_misc_verify_tx_i(uint16_t conn_handle,
+                                       uint16_t attr_handle,
+                                       uint8_t *attr_data, int attr_len)
 {
     struct ble_att_indicate_req req;
     struct os_mbuf *om;
@@ -303,33 +477,117 @@ ble_gatts_notify_test_misc_verify_tx_i(uint8_t *attr_data, int attr_len)
     TEST_ASSERT_FATAL(om != NULL);
 
     ble_att_indicate_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.baiq_handle == attr_handle);
 
     for (i = 0; i < attr_len; i++) {
         TEST_ASSERT(om->om_data[BLE_ATT_INDICATE_REQ_BASE_SZ + i] ==
                     attr_data[i]);
     }
+
+    ble_gatts_notify_test_util_verify_tx_event(conn_handle, attr_handle, 0, 1);
 }
 
-TEST_CASE(ble_gatts_notify_test_n)
+static void
+ble_gatts_notify_test_misc_verify_tx_gen(uint16_t conn_handle, int attr_idx,
+                                         uint8_t chr_flags)
 {
-    uint16_t conn_handle;
-    uint16_t flags;
+    uint16_t attr_handle;
+    uint16_t attr_len;
+    void *attr_val;
+
+    switch (attr_idx) {
+    case 1:
+        attr_handle = ble_gatts_notify_test_chr_1_def_handle + 1;
+        attr_len = ble_gatts_notify_test_chr_1_len;
+        attr_val = ble_gatts_notify_test_chr_1_val;
+        break;
+
+    case 2:
+        attr_handle = ble_gatts_notify_test_chr_2_def_handle + 1;
+        attr_len = ble_gatts_notify_test_chr_2_len;
+        attr_val = ble_gatts_notify_test_chr_2_val;
+        break;
+
+    default:
+        TEST_ASSERT_FATAL(0);
+        break;
+    }
 
-    ble_gatts_notify_test_misc_init(&conn_handle, 0, 0, 0);
+    switch (chr_flags) {
+    case 0:
+        break;
 
-    /* Enable notifications on both characteristics. */
-    ble_gatts_notify_test_misc_enable_notify(
-        conn_handle, ble_gatts_notify_test_chr_1_def_handle,
-        BLE_GATTS_CLT_CFG_F_NOTIFY);
-    ble_gatts_notify_test_misc_enable_notify(
-        conn_handle, ble_gatts_notify_test_chr_2_def_handle,
-        BLE_GATTS_CLT_CFG_F_NOTIFY);
+    case BLE_GATTS_CLT_CFG_F_NOTIFY:
+        ble_gatts_notify_test_misc_verify_tx_n(conn_handle, attr_handle,
+                                               attr_val, attr_len);
+        break;
 
-    /* Toss both write responses. */
-    ble_hs_test_util_prev_tx_queue_clear();
+    case BLE_GATTS_CLT_CFG_F_INDICATE:
+        ble_gatts_notify_test_misc_verify_tx_i(conn_handle, attr_handle,
+                                               attr_val, attr_len);
+        break;
+
+    default:
+        TEST_ASSERT_FATAL(0);
+        break;
+    }
+}
+
+static void
+ble_gatts_notify_test_restore_bonding(uint16_t conn_handle,
+                                      uint8_t chr1_flags, uint8_t chr1_tx,
+                                      uint8_t chr2_flags, uint8_t chr2_tx)
+{
+    struct ble_hs_conn *conn;
+
+    ble_hs_lock();
+    conn = ble_hs_conn_find(conn_handle);
+    TEST_ASSERT_FATAL(conn != NULL);
+    conn->bhc_sec_state.encrypted = 1;
+    conn->bhc_sec_state.authenticated = 1;
+    conn->bhc_sec_state.bonded = 1;
+    ble_hs_unlock();
+
+    ble_gatts_bonding_restored(conn_handle);
+
+    /* Verify subscription callback executed for each subscribed
+     * characteristic.
+     */
+    if (chr1_flags != 0) {
+        ble_gatts_notify_test_util_verify_sub_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_1_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_RESTORE,
+            0, chr1_flags == BLE_GATTS_CLT_CFG_F_NOTIFY,
+            0, chr1_flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+
+    }
+    if (chr1_tx) {
+        ble_gatts_notify_test_misc_verify_tx_gen(conn_handle, 1, chr1_flags);
+    }
 
-    /* Ensure nothing got persisted since peer is not bonded. */
-    TEST_ASSERT(ble_hs_test_util_store_num_cccds == 0);
+
+    if (chr2_flags != 0) {
+        ble_gatts_notify_test_util_verify_sub_event(
+            conn_handle,
+            ble_gatts_notify_test_chr_2_def_handle + 1,
+            BLE_GAP_SUBSCRIBE_REASON_RESTORE,
+            0, chr2_flags == BLE_GATTS_CLT_CFG_F_NOTIFY,
+            0, chr2_flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+    }
+    if (chr2_tx) {
+        ble_gatts_notify_test_misc_verify_tx_gen(conn_handle, 2, chr2_flags);
+    }
+}
+
+TEST_CASE(ble_gatts_notify_test_n)
+{
+    uint16_t conn_handle;
+    uint16_t flags;
+
+    ble_gatts_notify_test_misc_init(&conn_handle, 0,
+                                    BLE_GATTS_CLT_CFG_F_NOTIFY,
+                                    BLE_GATTS_CLT_CFG_F_NOTIFY);
 
     /* Ensure notifications read back as enabled. */
     flags = ble_gatts_notify_test_misc_read_notify(
@@ -345,8 +603,11 @@ TEST_CASE(ble_gatts_notify_test_n)
     ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
 
     /* Verify notification sent properly. */
-    ble_gatts_notify_test_misc_verify_tx_n(ble_gatts_notify_test_chr_1_val,
-                                           ble_gatts_notify_test_chr_1_len);
+    ble_gatts_notify_test_misc_verify_tx_n(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1,
+        ble_gatts_notify_test_chr_1_val,
+        ble_gatts_notify_test_chr_1_len);
 
     /* Update characteristic 2's value. */
     ble_gatts_notify_test_chr_2_len = 16;
@@ -355,15 +616,20 @@ TEST_CASE(ble_gatts_notify_test_n)
     ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
 
     /* Verify notification sent properly. */
-    ble_gatts_notify_test_misc_verify_tx_n(ble_gatts_notify_test_chr_2_val,
-                                           ble_gatts_notify_test_chr_2_len);
+    ble_gatts_notify_test_misc_verify_tx_n(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1,
+        ble_gatts_notify_test_chr_2_val,
+        ble_gatts_notify_test_chr_2_len);
 
     /***
      * Disconnect, modify characteristic values, and reconnect.  Ensure
      * notifications are not sent and are no longer enabled.
      */
 
-    ble_hs_test_util_conn_disconnect(conn_handle);
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_NOTIFY, 0,
+                                     BLE_GATTS_CLT_CFG_F_NOTIFY, 0);
 
     /* Update characteristic 1's value. */
     ble_gatts_notify_test_chr_1_len = 1;
@@ -377,7 +643,7 @@ TEST_CASE(ble_gatts_notify_test_n)
     ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
 
     ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
-                                 NULL, NULL);
+                                 ble_gatts_notify_test_util_gap_event, NULL);
 
     /* Ensure no notifications sent. */
     TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
@@ -396,29 +662,9 @@ TEST_CASE(ble_gatts_notify_test_i)
     uint16_t conn_handle;
     uint16_t flags;
 
-    ble_gatts_notify_test_misc_init(&conn_handle, 0, 0, 0);
-
-    /* Enable indications on both characteristics. */
-    ble_gatts_notify_test_misc_enable_notify(
-        conn_handle, ble_gatts_notify_test_chr_1_def_handle,
-        BLE_GATTS_CLT_CFG_F_INDICATE);
-    ble_gatts_notify_test_misc_enable_notify(
-        conn_handle, ble_gatts_notify_test_chr_2_def_handle,
-        BLE_GATTS_CLT_CFG_F_INDICATE);
-
-    /* Toss both write responses. */
-    ble_hs_test_util_prev_tx_queue_clear();
-
-    /* Ensure nothing got persisted since peer is not bonded. */
-    TEST_ASSERT(ble_hs_test_util_store_num_cccds == 0);
-
-    /* Ensure indications read back as enabled. */
-    flags = ble_gatts_notify_test_misc_read_notify(
-        conn_handle, ble_gatts_notify_test_chr_1_def_handle);
-    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_INDICATE);
-    flags = ble_gatts_notify_test_misc_read_notify(
-        conn_handle, ble_gatts_notify_test_chr_2_def_handle);
-    TEST_ASSERT(flags == BLE_GATTS_CLT_CFG_F_INDICATE);
+    ble_gatts_notify_test_misc_init(&conn_handle, 0,
+                                    BLE_GATTS_CLT_CFG_F_INDICATE,
+                                    BLE_GATTS_CLT_CFG_F_INDICATE);
 
     /* Update characteristic 1's value. */
     ble_gatts_notify_test_chr_1_len = 1;
@@ -426,8 +672,11 @@ TEST_CASE(ble_gatts_notify_test_i)
     ble_gatts_chr_updated(ble_gatts_notify_test_chr_1_def_handle + 1);
 
     /* Verify indication sent properly. */
-    ble_gatts_notify_test_misc_verify_tx_i(ble_gatts_notify_test_chr_1_val,
-                                           ble_gatts_notify_test_chr_1_len);
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1,
+        ble_gatts_notify_test_chr_1_val,
+        ble_gatts_notify_test_chr_1_len);
 
     /* Update characteristic 2's value. */
     ble_gatts_notify_test_chr_2_len = 16;
@@ -442,15 +691,22 @@ TEST_CASE(ble_gatts_notify_test_i)
     TEST_ASSERT(ble_hs_test_util_prev_tx_queue_sz() == 0);
 
     /* Receive the confirmation for the first indication. */
-    ble_gatts_notify_test_misc_rx_indicate_rsp(conn_handle);
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1);
 
     /* Verify indication sent properly. */
     ble_hs_test_util_tx_all();
-    ble_gatts_notify_test_misc_verify_tx_i(ble_gatts_notify_test_chr_2_val,
-                                           ble_gatts_notify_test_chr_2_len);
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1,
+        ble_gatts_notify_test_chr_2_val,
+        ble_gatts_notify_test_chr_2_len);
 
     /* Receive the confirmation for the second indication. */
-    ble_gatts_notify_test_misc_rx_indicate_rsp(conn_handle);
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1);
 
     /* Verify no pending GATT jobs. */
     TEST_ASSERT(!ble_gattc_any_jobs());
@@ -460,7 +716,9 @@ TEST_CASE(ble_gatts_notify_test_i)
      * indications are not sent and are no longer enabled.
      */
 
-    ble_hs_test_util_conn_disconnect(conn_handle);
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 0,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 0);
 
     /* Update characteristic 1's value. */
     ble_gatts_notify_test_chr_1_len = 1;
@@ -474,7 +732,7 @@ TEST_CASE(ble_gatts_notify_test_i)
     ble_gatts_chr_updated(ble_gatts_notify_test_chr_2_def_handle + 1);
 
     ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
-                                 NULL, NULL);
+                                 ble_gatts_notify_test_util_gap_event, NULL);
 
     /* Ensure no indications sent. */
     TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
@@ -498,7 +756,9 @@ TEST_CASE(ble_gatts_notify_test_bonded_n)
                                     BLE_GATTS_CLT_CFG_F_NOTIFY);
 
     /* Disconnect. */
-    ble_hs_test_util_conn_disconnect(conn_handle);
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_NOTIFY, 0,
+                                     BLE_GATTS_CLT_CFG_F_NOTIFY, 0);
 
     /* Ensure both CCCDs still persisted. */
     TEST_ASSERT(ble_hs_test_util_store_num_cccds == 2);
@@ -519,8 +779,9 @@ TEST_CASE(ble_gatts_notify_test_bonded_n)
      */
 
     ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
-                                 NULL, NULL);
+                                 ble_gatts_notify_test_util_gap_event, NULL);
 
+    ble_gatts_notify_test_num_events = 0;
     /* Ensure no notifications sent. */
     TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
 
@@ -533,13 +794,9 @@ TEST_CASE(ble_gatts_notify_test_bonded_n)
     TEST_ASSERT(flags == 0);
 
     /* Simulate a successful encryption procedure (bonding restoration). */
-    ble_gatts_restore_bonding(conn_handle);
-
-    /* Verify notifications sent properly. */
-    ble_gatts_notify_test_misc_verify_tx_n(ble_gatts_notify_test_chr_1_val,
-                                           ble_gatts_notify_test_chr_1_len);
-    ble_gatts_notify_test_misc_verify_tx_n(ble_gatts_notify_test_chr_2_val,
-                                           ble_gatts_notify_test_chr_2_len);
+    ble_gatts_notify_test_restore_bonding(conn_handle,
+                                          BLE_GATTS_CLT_CFG_F_NOTIFY, 1,
+                                          BLE_GATTS_CLT_CFG_F_NOTIFY, 1);
 
     /* Ensure notifications enabled. */
     flags = ble_gatts_notify_test_misc_read_notify(
@@ -563,7 +820,9 @@ TEST_CASE(ble_gatts_notify_test_bonded_i)
                                     BLE_GATTS_CLT_CFG_F_INDICATE);
 
     /* Disconnect. */
-    ble_hs_test_util_conn_disconnect(conn_handle);
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 0,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 0);
 
     /* Ensure both CCCDs still persisted. */
     TEST_ASSERT(ble_hs_test_util_store_num_cccds == 2);
@@ -584,7 +843,7 @@ TEST_CASE(ble_gatts_notify_test_bonded_i)
      */
 
     ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
-                                 NULL, NULL);
+                                 ble_gatts_notify_test_util_gap_event, NULL);
 
     /* Ensure no indications sent. */
     TEST_ASSERT(ble_hs_test_util_prev_tx_dequeue() == NULL);
@@ -598,11 +857,9 @@ TEST_CASE(ble_gatts_notify_test_bonded_i)
     TEST_ASSERT(flags == 0);
 
     /* Simulate a successful encryption procedure (bonding restoration). */
-    ble_gatts_restore_bonding(conn_handle);
-
-    /* Verify first indication sent properly. */
-    ble_gatts_notify_test_misc_verify_tx_i(ble_gatts_notify_test_chr_1_val,
-                                           ble_gatts_notify_test_chr_1_len);
+    ble_gatts_notify_test_restore_bonding(conn_handle,
+                                          BLE_GATTS_CLT_CFG_F_INDICATE, 1,
+                                          BLE_GATTS_CLT_CFG_F_INDICATE, 0);
 
     /* Verify the second indication doesn't get sent until the first is
      * confirmed.
@@ -611,15 +868,22 @@ TEST_CASE(ble_gatts_notify_test_bonded_i)
     TEST_ASSERT(ble_hs_test_util_prev_tx_queue_sz() == 0);
 
     /* Receive the confirmation for the first indication. */
-    ble_gatts_notify_test_misc_rx_indicate_rsp(conn_handle);
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1);
 
     /* Verify indication sent properly. */
     ble_hs_test_util_tx_all();
-    ble_gatts_notify_test_misc_verify_tx_i(ble_gatts_notify_test_chr_2_val,
-                                           ble_gatts_notify_test_chr_2_len);
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1,
+        ble_gatts_notify_test_chr_2_val,
+        ble_gatts_notify_test_chr_2_len);
 
     /* Receive the confirmation for the second indication. */
-    ble_gatts_notify_test_misc_rx_indicate_rsp(conn_handle);
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_2_def_handle + 1);
 
     /* Verify no pending GATT jobs. */
     TEST_ASSERT(!ble_gattc_any_jobs());
@@ -654,8 +918,11 @@ TEST_CASE(ble_gatts_notify_test_bonded_i_no_ack)
 
     /* Verify indication sent properly. */
     ble_hs_test_util_tx_all();
-    ble_gatts_notify_test_misc_verify_tx_i(ble_gatts_notify_test_chr_1_val,
-                                           ble_gatts_notify_test_chr_1_len);
+    ble_gatts_notify_test_misc_verify_tx_i(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1,
+        ble_gatts_notify_test_chr_1_val,
+        ble_gatts_notify_test_chr_1_len);
 
     /* Verify 'updated' state is still persisted. */
     key_cccd.peer_addr_type = BLE_STORE_ADDR_TYPE_NONE;
@@ -667,24 +934,25 @@ TEST_CASE(ble_gatts_notify_test_bonded_i_no_ack)
     TEST_ASSERT(value_cccd.value_changed);
 
     /* Disconnect. */
-    ble_hs_test_util_conn_disconnect(conn_handle);
+    ble_gatts_notify_test_disconnect(conn_handle,
+                                     BLE_GATTS_CLT_CFG_F_INDICATE, 1, 0, 0);
 
     /* Ensure CCCD still persisted. */
     TEST_ASSERT(ble_hs_test_util_store_num_cccds == 1);
 
     /* Reconnect. */
     ble_hs_test_util_create_conn(conn_handle, ((uint8_t[]){2,3,4,5,6,7,8,9}),
-                                 NULL, NULL);
+                                 ble_gatts_notify_test_util_gap_event, NULL);
 
     /* Simulate a successful encryption procedure (bonding restoration). */
-    ble_gatts_restore_bonding(conn_handle);
-
-    /* Verify indication sent properly. */
-    ble_gatts_notify_test_misc_verify_tx_i(ble_gatts_notify_test_chr_1_val,
-                                           ble_gatts_notify_test_chr_1_len);
+    ble_gatts_notify_test_restore_bonding(conn_handle,
+                                          BLE_GATTS_CLT_CFG_F_INDICATE, 1,
+                                          0, 0);
 
     /* Receive the confirmation for the indication. */
-    ble_gatts_notify_test_misc_rx_indicate_rsp(conn_handle);
+    ble_gatts_notify_test_misc_rx_indicate_rsp(
+        conn_handle,
+        ble_gatts_notify_test_chr_1_def_handle + 1);
 
     /* Verify no pending GATT jobs. */
     TEST_ASSERT(!ble_gattc_any_jobs());
@@ -706,8 +974,81 @@ TEST_CASE(ble_gatts_notify_test_bonded_i_no_ack)
     TEST_ASSERT(!value_cccd.value_changed);
 }
 
+TEST_CASE(ble_gatts_notify_test_disallowed)
+{
+    uint16_t chr1_val_handle;
+    uint16_t chr2_val_handle;
+    uint16_t chr3_val_handle;
+    int rc;
+
+    const struct ble_gatt_svc_def svcs[] = { {
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid128 = BLE_UUID16(0x1234),
+        .characteristics = (struct ble_gatt_chr_def[]) { {
+            .uuid128 = BLE_UUID16(1),
+            .access_cb = ble_gatts_notify_test_misc_access,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
+            .val_handle = &chr1_val_handle,
+        }, {
+            .uuid128 = BLE_UUID16(2),
+            .access_cb = ble_gatts_notify_test_misc_access,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_INDICATE,
+            .val_handle = &chr2_val_handle,
+        }, {
+            .uuid128 = BLE_UUID16(3),
+            .access_cb = ble_gatts_notify_test_misc_access,
+            .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY |
+                     BLE_GATT_CHR_F_INDICATE,
+            .val_handle = &chr3_val_handle,
+        }, {
+            0
+        } },
+    }, {
+        0
+    } };
+
+    ble_hs_test_util_init();
+
+    rc = ble_gatts_register_svcs(svcs, NULL, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(chr1_val_handle != 0);
+    TEST_ASSERT_FATAL(chr2_val_handle != 0);
+    TEST_ASSERT_FATAL(chr3_val_handle != 0);
+
+    ble_gatts_start();
+
+    ble_hs_test_util_create_conn(2, ble_gatts_notify_test_peer_addr,
+                                 ble_gatts_notify_test_util_gap_event, NULL);
+
+    /* Attempt to enable notifications on chr1 should succeed. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr1_val_handle - 1, BLE_GATTS_CLT_CFG_F_NOTIFY, 0);
+
+    /* Attempt to enable indications on chr1 should fail. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr1_val_handle - 1, BLE_GATTS_CLT_CFG_F_INDICATE, 1);
+
+    /* Attempt to enable notifications on chr2 should fail. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr2_val_handle - 1, BLE_GATTS_CLT_CFG_F_NOTIFY, 1);
+
+    /* Attempt to enable indications on chr2 should succeed. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr2_val_handle - 1, BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+
+    /* Attempt to enable notifications on chr3 should succeed. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr3_val_handle - 1, BLE_GATTS_CLT_CFG_F_NOTIFY, 0);
+
+    /* Attempt to enable indications on chr3 should succeed. */
+    ble_gatts_notify_test_misc_try_enable_notify(
+        2, chr3_val_handle - 1, BLE_GATTS_CLT_CFG_F_INDICATE, 0);
+}
+
 TEST_SUITE(ble_gatts_notify_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatts_notify_test_n();
     ble_gatts_notify_test_i();
 
@@ -716,6 +1057,8 @@ TEST_SUITE(ble_gatts_notify_suite)
 
     ble_gatts_notify_test_bonded_i_no_ack();
 
+    ble_gatts_notify_test_disallowed();
+
     /* XXX: Test corner cases:
      *     o Bonding after CCCD configuration.
      *     o Disconnect prior to rx of indicate ack.
diff --git a/net/nimble/host/src/test/ble_gatts_read_test.c b/net/nimble/host/src/test/ble_gatts_read_test.c
new file mode 100644
index 00000000..cef9f927
--- /dev/null
+++ b/net/nimble/host/src/test/ble_gatts_read_test.c
@@ -0,0 +1,261 @@
+/**
+ * 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 <string.h>
+#include <errno.h>
+#include "testutil/testutil.h"
+#include "host/ble_uuid.h"
+#include "host/ble_hs_test.h"
+#include "ble_hs_test_util.h"
+
+#define BLE_GATTS_READ_TEST_CHR_1_UUID    0x1111
+#define BLE_GATTS_READ_TEST_CHR_2_UUID    0x2222
+
+static uint8_t ble_gatts_read_test_peer_addr[6] = {2,3,4,5,6,7};
+
+static int
+ble_gatts_read_test_util_access_1(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg);
+
+static int
+ble_gatts_read_test_util_access_2(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg);
+static void
+ble_gatts_read_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt,
+                                void *arg);
+
+static const struct ble_gatt_svc_def ble_gatts_read_test_svcs[] = { {
+    .type = BLE_GATT_SVC_TYPE_PRIMARY,
+    .uuid128 = BLE_UUID16(0x1234),
+    .characteristics = (struct ble_gatt_chr_def[]) { {
+        .uuid128 = BLE_UUID16(BLE_GATTS_READ_TEST_CHR_1_UUID),
+        .access_cb = ble_gatts_read_test_util_access_1,
+        .flags = BLE_GATT_CHR_F_READ
+    }, {
+        .uuid128 = BLE_UUID16(BLE_GATTS_READ_TEST_CHR_2_UUID),
+        .access_cb = ble_gatts_read_test_util_access_2,
+        .flags = BLE_GATT_CHR_F_READ
+    }, {
+        0
+    } },
+}, {
+    0
+} };
+
+
+static uint16_t ble_gatts_read_test_chr_1_def_handle;
+static uint16_t ble_gatts_read_test_chr_1_val_handle;
+static uint8_t ble_gatts_read_test_chr_1_val[1024];
+static int ble_gatts_read_test_chr_1_len;
+static uint16_t ble_gatts_read_test_chr_2_def_handle;
+static uint16_t ble_gatts_read_test_chr_2_val_handle;
+
+static void
+ble_gatts_read_test_misc_init(uint16_t *out_conn_handle)
+{
+    int rc;
+
+    ble_hs_test_util_init();
+
+    rc = ble_gatts_register_svcs(ble_gatts_read_test_svcs,
+                                 ble_gatts_read_test_misc_reg_cb, NULL);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(ble_gatts_read_test_chr_1_def_handle != 0);
+    TEST_ASSERT_FATAL(ble_gatts_read_test_chr_1_val_handle ==
+                      ble_gatts_read_test_chr_1_def_handle + 1);
+    TEST_ASSERT_FATAL(ble_gatts_read_test_chr_2_def_handle != 0);
+    TEST_ASSERT_FATAL(ble_gatts_read_test_chr_2_val_handle ==
+                      ble_gatts_read_test_chr_2_def_handle + 1);
+
+    ble_gatts_start();
+
+    ble_hs_test_util_create_conn(2, ble_gatts_read_test_peer_addr, NULL, NULL);
+
+    if (out_conn_handle != NULL) {
+        *out_conn_handle = 2;
+    }
+}
+
+static void
+ble_gatts_read_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt,
+                                void *arg)
+{
+    uint16_t uuid16;
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_CHR) {
+        uuid16 = ble_uuid_128_to_16(ctxt->chr.chr_def->uuid128);
+        switch (uuid16) {
+        case BLE_GATTS_READ_TEST_CHR_1_UUID:
+            ble_gatts_read_test_chr_1_def_handle = ctxt->chr.def_handle;
+            ble_gatts_read_test_chr_1_val_handle = ctxt->chr.val_handle;
+            break;
+
+        case BLE_GATTS_READ_TEST_CHR_2_UUID:
+            ble_gatts_read_test_chr_2_def_handle = ctxt->chr.def_handle;
+            ble_gatts_read_test_chr_2_val_handle = ctxt->chr.val_handle;
+            break;
+
+        default:
+            TEST_ASSERT_FATAL(0);
+            break;
+        }
+    }
+}
+
+static int
+ble_gatts_read_test_util_access_1(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg)
+{
+    int rc;
+
+    TEST_ASSERT_FATAL(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+    TEST_ASSERT_FATAL(attr_handle == ble_gatts_read_test_chr_1_val_handle);
+
+    TEST_ASSERT(ctxt->chr ==
+                &ble_gatts_read_test_svcs[0].characteristics[0]);
+
+    rc = os_mbuf_append(ctxt->om, ble_gatts_read_test_chr_1_val,
+                        ble_gatts_read_test_chr_1_len);
+    TEST_ASSERT(rc == 0);
+
+    return 0;
+}
+
+static int
+ble_gatts_read_test_util_access_2(uint16_t conn_handle,
+                                  uint16_t attr_handle,
+                                  struct ble_gatt_access_ctxt *ctxt,
+                                  void *arg)
+{
+    uint8_t *buf;
+
+    TEST_ASSERT_FATAL(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
+    TEST_ASSERT_FATAL(attr_handle == ble_gatts_read_test_chr_2_def_handle + 1);
+
+    TEST_ASSERT(ctxt->chr ==
+                &ble_gatts_read_test_svcs[0].characteristics[1]);
+
+    buf = os_mbuf_extend(ctxt->om, 6);
+    TEST_ASSERT_FATAL(buf != NULL);
+
+    buf[0] = 0;
+    buf[1] = 10;
+    buf[2] = 20;
+    buf[3] = 30;
+    buf[4] = 40;
+    buf[5] = 50;
+
+    return 0;
+}
+
+static void
+ble_gatts_read_test_once(uint16_t conn_handle, uint16_t attr_id,
+                         void *expected_value, uint16_t expected_len)
+{
+    struct ble_att_read_req read_req;
+    uint8_t buf[BLE_ATT_READ_REQ_SZ];
+    int rc;
+
+    read_req.barq_handle = attr_id;
+    ble_att_read_req_write(buf, sizeof buf, &read_req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_verify_tx_read_rsp(expected_value, expected_len);
+}
+
+TEST_CASE(ble_gatts_read_test_case_basic)
+{
+    uint16_t conn_handle;
+
+    ble_gatts_read_test_misc_init(&conn_handle);
+
+    /*** Application points attribute at static data. */
+    ble_gatts_read_test_chr_1_val[0] = 1;
+    ble_gatts_read_test_chr_1_val[1] = 2;
+    ble_gatts_read_test_chr_1_val[2] = 3;
+    ble_gatts_read_test_chr_1_len = 3;
+    ble_gatts_read_test_once(conn_handle,
+                             ble_gatts_read_test_chr_1_val_handle,
+                             ble_gatts_read_test_chr_1_val,
+                             ble_gatts_read_test_chr_1_len);
+
+    /*** Application uses stack-provided buffer for dynamic attribute. */
+    ble_gatts_read_test_once(conn_handle,
+                             ble_gatts_read_test_chr_2_def_handle + 1,
+                             ((uint8_t[6]){0,10,20,30,40,50}), 6);
+
+}
+
+TEST_CASE(ble_gatts_read_test_case_long)
+{
+    struct ble_att_read_blob_req read_blob_req;
+    struct ble_att_read_req read_req;
+    uint8_t buf[max(BLE_ATT_READ_REQ_SZ, BLE_ATT_READ_BLOB_REQ_SZ)];
+    uint16_t conn_handle;
+    int rc;
+    int i;
+
+    ble_gatts_read_test_misc_init(&conn_handle);
+
+    /*** Prepare characteristic value. */
+    ble_gatts_read_test_chr_1_len = 40;
+    for (i = 0; i < ble_gatts_read_test_chr_1_len; i++) {
+        ble_gatts_read_test_chr_1_val[i] = i;
+    }
+
+    /* Receive first read request. */
+    read_req.barq_handle = ble_gatts_read_test_chr_1_val_handle;
+    ble_att_read_req_write(buf, sizeof buf, &read_req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_hs_test_util_verify_tx_read_rsp(ble_gatts_read_test_chr_1_val, 22);
+
+    /* Receive follow-up read blob request. */
+    read_blob_req.babq_handle = ble_gatts_read_test_chr_1_val_handle;
+    read_blob_req.babq_offset = 22;
+    ble_att_read_blob_req_write(buf, sizeof buf, &read_blob_req);
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+
+    /* Ensure response starts at appropriate offset (22). */
+    ble_hs_test_util_verify_tx_read_blob_rsp(
+        ble_gatts_read_test_chr_1_val + 22, 18);
+}
+
+TEST_SUITE(ble_gatts_read_test_suite)
+{
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_gatts_read_test_case_basic();
+    ble_gatts_read_test_case_long();
+}
diff --git a/net/nimble/host/src/test/ble_gatts_reg_test.c b/net/nimble/host/src/test/ble_gatts_reg_test.c
index a198d997..ad5f18fa 100644
--- a/net/nimble/host/src/test/ble_gatts_reg_test.c
+++ b/net/nimble/host/src/test/ble_gatts_reg_test.c
@@ -30,6 +30,12 @@
 struct ble_gatts_reg_test_entry {
     uint8_t op;
     uint8_t uuid128[16];
+    uint16_t handle;
+    uint16_t val_handle; /* If a characteristic. */
+
+    const struct ble_gatt_svc_def *svc;
+    const struct ble_gatt_chr_def *chr;
+    const struct ble_gatt_dsc_def *dsc;
 };
 
 static struct ble_gatts_reg_test_entry
@@ -45,8 +51,7 @@ ble_gatts_reg_test_init(void)
 }
 
 static void
-ble_gatts_reg_test_misc_reg_cb(uint8_t op, union ble_gatt_register_ctxt *ctxt,
-                               void *arg)
+ble_gatts_reg_test_misc_reg_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
 {
     struct ble_gatts_reg_test_entry *entry;
 
@@ -54,19 +59,65 @@ ble_gatts_reg_test_misc_reg_cb(uint8_t op, union ble_gatt_register_ctxt *ctxt,
                       BLE_GATTS_REG_TEST_MAX_ENTRIES);
 
     entry = ble_gatts_reg_test_entries + ble_gatts_reg_test_num_entries++;
+    memset(entry, 0, sizeof *entry);
+
+    entry->op = ctxt->op;
+    switch (ctxt->op) {
+    case BLE_GATT_REGISTER_OP_SVC:
+        memcpy(entry->uuid128, ctxt->svc.svc_def->uuid128, 16);
+        entry->handle = ctxt->svc.handle;
+        entry->svc = ctxt->svc.svc_def;
+        break;
+
+    case BLE_GATT_REGISTER_OP_CHR:
+        memcpy(entry->uuid128, ctxt->chr.chr_def->uuid128, 16);
+        entry->handle = ctxt->chr.def_handle;
+        entry->val_handle = ctxt->chr.val_handle;
+        entry->svc = ctxt->chr.svc_def;
+        entry->chr = ctxt->chr.chr_def;
+        break;
+
+    case BLE_GATT_REGISTER_OP_DSC:
+        memcpy(entry->uuid128, ctxt->dsc.dsc_def->uuid128, 16);
+        entry->handle = ctxt->dsc.handle;
+        entry->svc = ctxt->dsc.svc_def;
+        entry->chr = ctxt->dsc.chr_def;
+        entry->dsc = ctxt->dsc.dsc_def;
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
+    }
+}
 
-    entry->op = op;
-    switch (op) {
+static void
+ble_gatts_reg_test_misc_lookup_good(struct ble_gatts_reg_test_entry *entry)
+{
+    uint16_t chr_def_handle;
+    uint16_t chr_val_handle;
+    uint16_t svc_handle;
+    uint16_t dsc_handle;
+    int rc;
+
+    switch (entry->op) {
     case BLE_GATT_REGISTER_OP_SVC:
-        memcpy(entry->uuid128, ctxt->svc_reg.svc->uuid128, 16);
+        rc = ble_gatts_find_svc(entry->uuid128, &svc_handle);
+        TEST_ASSERT_FATAL(rc == 0);
+        TEST_ASSERT(svc_handle == entry->handle);
         break;
 
     case BLE_GATT_REGISTER_OP_CHR:
-        memcpy(entry->uuid128, ctxt->chr_reg.chr->uuid128, 16);
+        rc = ble_gatts_find_chr(entry->svc->uuid128, entry->chr->uuid128,
+                                &chr_def_handle, &chr_val_handle);
+        TEST_ASSERT_FATAL(rc == 0);
+        TEST_ASSERT(chr_def_handle == entry->handle);
+        TEST_ASSERT(chr_val_handle == entry->val_handle);
         break;
 
     case BLE_GATT_REGISTER_OP_DSC:
-        memcpy(entry->uuid128, ctxt->dsc_reg.dsc->uuid128, 16);
+        rc = ble_gatts_find_dsc(entry->svc->uuid128, entry->chr->uuid128,
+                                entry->dsc->uuid128, &dsc_handle);
         break;
 
     default:
@@ -76,7 +127,139 @@ ble_gatts_reg_test_misc_reg_cb(uint8_t op, union ble_gatt_register_ctxt *ctxt,
 }
 
 static void
-ble_gatts_reg_test_misc_verify_entry(uint8_t op, uint8_t *uuid128)
+ble_gatts_reg_test_misc_lookup_bad(struct ble_gatts_reg_test_entry *entry)
+{
+    struct ble_gatts_reg_test_entry *cur;
+    uint8_t wrong_uuid[16];
+    int rc;
+    int i;
+
+    switch (entry->op) {
+    case BLE_GATT_REGISTER_OP_SVC:
+        /* Wrong service UUID. */
+        memcpy(wrong_uuid, entry->svc->uuid128, 16);
+        wrong_uuid[15]++;
+        rc = ble_gatts_find_svc(wrong_uuid, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+        break;
+
+    case BLE_GATT_REGISTER_OP_CHR:
+        /* Correct service UUID, wrong characteristic UUID. */
+        memcpy(wrong_uuid, entry->chr->uuid128, 16);
+        wrong_uuid[15]++;
+        rc = ble_gatts_find_chr(entry->svc->uuid128, wrong_uuid, NULL, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+        /* Incorrect service UUID, correct characteristic UUID. */
+        memcpy(wrong_uuid, entry->svc->uuid128, 16);
+        wrong_uuid[15]++;
+        rc = ble_gatts_find_chr(wrong_uuid, entry->chr->uuid128, NULL, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+        /* Existing (but wrong) service, correct characteristic UUID. */
+        for (i = 0; i < ble_gatts_reg_test_num_entries; i++) {
+            cur = ble_gatts_reg_test_entries + i;
+            switch (cur->op) {
+            case BLE_GATT_REGISTER_OP_SVC:
+                if (cur->svc != entry->svc) {
+                    rc = ble_gatts_find_chr(cur->svc->uuid128,
+                                            entry->chr->uuid128,
+                                            NULL, NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            case BLE_GATT_REGISTER_OP_CHR:
+                /* Characteristic that isn't in this service. */
+                if (cur->svc != entry->svc) {
+                    rc = ble_gatts_find_chr(entry->svc->uuid128,
+                                            cur->chr->uuid128,
+                                            NULL, NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            case BLE_GATT_REGISTER_OP_DSC:
+                /* Use descriptor UUID instead of characteristic UUID. */
+                rc = ble_gatts_find_chr(entry->svc->uuid128,
+                                        cur->dsc->uuid128,
+                                        NULL, NULL);
+                TEST_ASSERT(rc == BLE_HS_ENOENT);
+                break;
+
+            default:
+                TEST_ASSERT(0);
+                break;
+            }
+        }
+        break;
+
+    case BLE_GATT_REGISTER_OP_DSC:
+        /* Correct svc/chr UUID, wrong dsc UUID. */
+        memcpy(wrong_uuid, entry->dsc->uuid128, 16);
+        wrong_uuid[15]++;
+        rc = ble_gatts_find_dsc(entry->svc->uuid128, entry->chr->uuid128,
+                                wrong_uuid, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+        /* Incorrect svc UUID, correct chr/dsc UUID. */
+        memcpy(wrong_uuid, entry->svc->uuid128, 16);
+        wrong_uuid[15]++;
+        rc = ble_gatts_find_dsc(wrong_uuid, entry->chr->uuid128,
+                                entry->dsc->uuid128, NULL);
+        TEST_ASSERT(rc == BLE_HS_ENOENT);
+
+        for (i = 0; i < ble_gatts_reg_test_num_entries; i++) {
+            cur = ble_gatts_reg_test_entries + i;
+            switch (cur->op) {
+            case BLE_GATT_REGISTER_OP_SVC:
+                /* Existing (but wrong) svc, correct chr/dsc UUID. */
+                if (cur->svc != entry->svc) {
+                    rc = ble_gatts_find_dsc(cur->svc->uuid128,
+                                            entry->chr->uuid128,
+                                            entry->dsc->uuid128,
+                                            NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            case BLE_GATT_REGISTER_OP_CHR:
+                /* Existing (but wrong) svc/chr, correct dsc UUID. */
+                if (cur->chr != entry->chr) {
+                    rc = ble_gatts_find_dsc(cur->svc->uuid128,
+                                            cur->chr->uuid128,
+                                            entry->dsc->uuid128,
+                                            NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            case BLE_GATT_REGISTER_OP_DSC:
+                /* Descriptor that isn't in this characteristic. */
+                if (cur->chr != entry->chr) {
+                    rc = ble_gatts_find_dsc(cur->svc->uuid128,
+                                            cur->chr->uuid128,
+                                            entry->dsc->uuid128,
+                                            NULL);
+                    TEST_ASSERT(rc == BLE_HS_ENOENT);
+                }
+                break;
+
+            default:
+                TEST_ASSERT(0);
+                break;
+            }
+        }
+        break;
+
+    default:
+        TEST_ASSERT(0);
+        break;
+    }
+}
+
+static void
+ble_gatts_reg_test_misc_verify_entry(uint8_t op, const uint8_t *uuid128)
 {
     struct ble_gatts_reg_test_entry *entry;
     int i;
@@ -84,17 +267,31 @@ ble_gatts_reg_test_misc_verify_entry(uint8_t op, uint8_t *uuid128)
     for (i = 0; i < ble_gatts_reg_test_num_entries; i++) {
         entry = ble_gatts_reg_test_entries + i;
         if (entry->op == op && memcmp(entry->uuid128, uuid128, 16) == 0) {
-            return;
+            break;
         }
     }
+    TEST_ASSERT_FATAL(entry != NULL);
 
-    TEST_ASSERT(0);
+    /* Verify that characteristic value handle was properly assigned at
+     * registration.
+     */
+    if (op == BLE_GATT_REGISTER_OP_CHR) {
+        TEST_ASSERT(*entry->chr->val_handle == entry->val_handle);
+    }
+
+    /* Verify that the entry can be looked up. */
+    ble_gatts_reg_test_misc_lookup_good(entry);
+
+    /* Verify that "barely incorrect" UUID information doesn't retrieve any
+     * handles.
+     */
+    ble_gatts_reg_test_misc_lookup_bad(entry);
 }
 
 static int
 ble_gatts_reg_test_misc_dummy_access(uint16_t conn_handle,
-                                     uint16_t attr_handle, uint8_t op,
-                                     union ble_gatt_access_ctxt *ctxt,
+                                     uint16_t attr_handle,
+                                     struct ble_gatt_access_ctxt *ctxt,
                                      void *arg)
 {
     return 0;
@@ -104,9 +301,8 @@ TEST_CASE(ble_gatts_reg_test_svc_return)
 {
     int rc;
 
-    ble_gatts_reg_test_init();
-
     /*** Missing UUID. */
+    ble_gatts_reg_test_init();
     struct ble_gatt_svc_def svcs_no_uuid[] = { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
     }, {
@@ -117,6 +313,7 @@ TEST_CASE(ble_gatts_reg_test_svc_return)
     TEST_ASSERT(rc == BLE_HS_EINVAL);
 
     /*** Circular dependency. */
+    ble_gatts_reg_test_init();
     struct ble_gatt_svc_def svcs_circ[] = { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(0x1234),
@@ -133,6 +330,7 @@ TEST_CASE(ble_gatts_reg_test_svc_return)
     TEST_ASSERT(rc == BLE_HS_EINVAL);
 
     /*** Success. */
+    ble_gatts_reg_test_init();
     struct ble_gatt_svc_def svcs_good[] = { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(0x1234),
@@ -152,9 +350,8 @@ TEST_CASE(ble_gatts_reg_test_chr_return)
 {
     int rc;
 
-    ble_gatts_reg_test_init();
-
     /*** Missing callback. */
+    ble_gatts_reg_test_init();
     struct ble_gatt_svc_def svcs_no_chr_cb[] = { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(0x1234),
@@ -172,6 +369,7 @@ TEST_CASE(ble_gatts_reg_test_chr_return)
     TEST_ASSERT(rc == BLE_HS_EINVAL);
 
     /*** Success. */
+    ble_gatts_reg_test_init();
     struct ble_gatt_svc_def svcs_good[] = { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(0x1234),
@@ -194,9 +392,8 @@ TEST_CASE(ble_gatts_reg_test_dsc_return)
 {
     int rc;
 
-    ble_gatts_reg_test_init();
-
     /*** Missing callback. */
+    ble_gatts_reg_test_init();
     struct ble_gatt_svc_def svcs_no_dsc_cb[] = { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(0x1234),
@@ -221,6 +418,7 @@ TEST_CASE(ble_gatts_reg_test_dsc_return)
     TEST_ASSERT(rc == BLE_HS_EINVAL);
 
     /*** Success. */
+    ble_gatts_reg_test_init();
     struct ble_gatt_svc_def svcs_good[] = { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(0x1234),
@@ -249,9 +447,9 @@ TEST_CASE(ble_gatts_reg_test_dsc_return)
 static void
 ble_gatts_reg_test_misc_svcs(struct ble_gatt_svc_def *svcs)
 {
-    struct ble_gatt_svc_def *svc;
-    struct ble_gatt_chr_def *chr;
-    struct ble_gatt_dsc_def *dsc;
+    const struct ble_gatt_svc_def *svc;
+    const struct ble_gatt_chr_def *chr;
+    const struct ble_gatt_dsc_def *dsc;
     int rc;
 
     ble_gatts_reg_test_init();
@@ -337,6 +535,8 @@ TEST_CASE(ble_gatts_reg_test_svc_cb)
 
 TEST_CASE(ble_gatts_reg_test_chr_cb)
 {
+    uint16_t val_handles[16];
+
     /*** 1 characteristic. */
     ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
@@ -345,6 +545,7 @@ TEST_CASE(ble_gatts_reg_test_chr_cb)
             .uuid128 = BLE_UUID16(0x1111),
             .access_cb = ble_gatts_reg_test_misc_dummy_access,
             .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 0,
         }, {
             0
         } },
@@ -360,10 +561,12 @@ TEST_CASE(ble_gatts_reg_test_chr_cb)
             .uuid128 = BLE_UUID16(0x1111),
             .access_cb = ble_gatts_reg_test_misc_dummy_access,
             .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 0,
         }, {
             .uuid128 = BLE_UUID16(0x2222),
             .access_cb = ble_gatts_reg_test_misc_dummy_access,
             .flags = BLE_GATT_CHR_F_WRITE,
+            .val_handle = val_handles + 1,
         }, {
             0
         } },
@@ -374,6 +577,7 @@ TEST_CASE(ble_gatts_reg_test_chr_cb)
             .uuid128 = BLE_UUID16(0x3333),
             .access_cb = ble_gatts_reg_test_misc_dummy_access,
             .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 2,
         }, {
             0
         } },
@@ -384,6 +588,8 @@ TEST_CASE(ble_gatts_reg_test_chr_cb)
 
 TEST_CASE(ble_gatts_reg_test_dsc_cb)
 {
+    uint16_t val_handles[16];
+
     /*** 1 descriptor. */
     ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
@@ -392,8 +598,9 @@ TEST_CASE(ble_gatts_reg_test_dsc_cb)
             .uuid128 = BLE_UUID16(0x1111),
             .access_cb = ble_gatts_reg_test_misc_dummy_access,
             .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 0,
             .descriptors = (struct ble_gatt_dsc_def[]) { {
-                .uuid128 = BLE_UUID16(0xaaaa),
+                .uuid128 = BLE_UUID16(0x111a),
                 .att_flags = 5,
                 .access_cb = ble_gatts_reg_test_misc_dummy_access,
             }, {
@@ -406,7 +613,7 @@ TEST_CASE(ble_gatts_reg_test_dsc_cb)
         0
     } });
 
-    /*** 5 descriptors. */
+    /*** 5+ descriptors. */
     ble_gatts_reg_test_misc_svcs((struct ble_gatt_svc_def[]) { {
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
         .uuid128 = BLE_UUID16(0x1234),
@@ -414,8 +621,9 @@ TEST_CASE(ble_gatts_reg_test_dsc_cb)
             .uuid128 = BLE_UUID16(0x1111),
             .access_cb = ble_gatts_reg_test_misc_dummy_access,
             .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 0,
             .descriptors = (struct ble_gatt_dsc_def[]) { {
-                .uuid128 = BLE_UUID16(0xaaaa),
+                .uuid128 = BLE_UUID16(0x111a),
                 .att_flags = 5,
                 .access_cb = ble_gatts_reg_test_misc_dummy_access,
             }, {
@@ -425,6 +633,7 @@ TEST_CASE(ble_gatts_reg_test_dsc_cb)
             .uuid128 = BLE_UUID16(0x2222),
             .access_cb = ble_gatts_reg_test_misc_dummy_access,
             .flags = BLE_GATT_CHR_F_WRITE,
+            .val_handle = val_handles + 1,
         }, {
             0
         } },
@@ -435,20 +644,45 @@ TEST_CASE(ble_gatts_reg_test_dsc_cb)
             .uuid128 = BLE_UUID16(0x3333),
             .access_cb = ble_gatts_reg_test_misc_dummy_access,
             .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 2,
             .descriptors = (struct ble_gatt_dsc_def[]) { {
-                .uuid128 = BLE_UUID16(0xaaab),
+                .uuid128 = BLE_UUID16(0x333a),
                 .att_flags = 5,
                 .access_cb = ble_gatts_reg_test_misc_dummy_access,
             }, {
-                .uuid128 = BLE_UUID16(0xaaac),
+                .uuid128 = BLE_UUID16(0x333b),
                 .att_flags = 5,
                 .access_cb = ble_gatts_reg_test_misc_dummy_access,
             }, {
-                .uuid128 = BLE_UUID16(0xaaad),
+                .uuid128 = BLE_UUID16(0x333c),
                 .att_flags = 5,
                 .access_cb = ble_gatts_reg_test_misc_dummy_access,
             }, {
-                .uuid128 = BLE_UUID16(0xaaae),
+                .uuid128 = BLE_UUID16(0x333e),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                0
+            } },
+        }, {
+            .uuid128 = BLE_UUID16(0x4444),
+            .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            .flags = BLE_GATT_CHR_F_READ,
+            .val_handle = val_handles + 3,
+            .descriptors = (struct ble_gatt_dsc_def[]) { {
+                .uuid128 = BLE_UUID16(0x444a),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid128 = BLE_UUID16(0x444b),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid128 = BLE_UUID16(0x444c),
+                .att_flags = 5,
+                .access_cb = ble_gatts_reg_test_misc_dummy_access,
+            }, {
+                .uuid128 = BLE_UUID16(0x444e),
                 .att_flags = 5,
                 .access_cb = ble_gatts_reg_test_misc_dummy_access,
             }, {
@@ -464,6 +698,8 @@ TEST_CASE(ble_gatts_reg_test_dsc_cb)
 
 TEST_SUITE(ble_gatts_reg_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_gatts_reg_test_svc_return();
     ble_gatts_reg_test_chr_return();
     ble_gatts_reg_test_dsc_return();
diff --git a/net/nimble/host/src/test/ble_hs_adv_test.c b/net/nimble/host/src/test/ble_hs_adv_test.c
index 9673cc10..e99fd5b4 100644
--- a/net/nimble/host/src/test/ble_hs_adv_test.c
+++ b/net/nimble/host/src/test/ble_hs_adv_test.c
@@ -22,8 +22,8 @@
 #include <string.h>
 #include "testutil/testutil.h"
 #include "nimble/hci_common.h"
+#include "host/ble_hs_adv.h"
 #include "host/ble_hs_test.h"
-#include "host/host_hci.h"
 #include "ble_hs_test_util.h"
 
 #define BLE_ADV_TEST_DATA_OFF   4
@@ -140,18 +140,36 @@ ble_hs_adv_test_misc_tx_and_verify_data(
     struct ble_hs_adv_fields *rsp_fields,
     struct ble_hs_adv_test_field *test_rsp_fields)
 {
+    struct ble_gap_adv_params adv_params;
     int rc;
 
     ble_hs_test_util_init();
 
-    rc = ble_gap_adv_set_fields(adv_fields);
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.disc_mode = disc_mode;
+
+    rc = ble_hs_test_util_adv_set_fields(adv_fields, 0);
     TEST_ASSERT_FATAL(rc == 0);
 
     rc = ble_gap_adv_rsp_set_fields(rsp_fields);
     TEST_ASSERT_FATAL(rc == 0);
 
-    rc = ble_hs_test_util_adv_start(disc_mode, BLE_GAP_CONN_MODE_UND, NULL, 0,
-                                    NULL, NULL, NULL, 0, 0);
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, 0, NULL, &adv_params,
+                                    NULL, NULL, 0, 0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    /* Discard the adv-enable command. */
+    ble_hs_test_util_get_last_hci_tx();
+
+    ble_hs_adv_test_misc_verify_tx_rsp_data(test_rsp_fields);
+    ble_hs_adv_test_misc_verify_tx_adv_data(test_adv_fields);
+
+    /* Ensure the same data gets sent on repeated advertise procedures. */
+    rc = ble_hs_test_util_adv_stop(0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, 0, NULL, &adv_params,
+                                    NULL, NULL, 0, 0);
     TEST_ASSERT_FATAL(rc == 0);
 
     /* Discard the adv-enable command. */
@@ -170,18 +188,20 @@ TEST_CASE(ble_hs_adv_test_case_flags)
     memset(&rsp_fields, 0, sizeof rsp_fields);
 
     /* Default flags. */
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON,
         &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -191,15 +211,16 @@ TEST_CASE(ble_hs_adv_test_case_flags)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_LTD, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_FLAGS,
                 .val = (uint8_t[]) {
                     BLE_HS_ADV_F_DISC_LTD | BLE_HS_ADV_F_BREDR_UNSUP
                  },
                 .val_len = 1,
-            }, {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
             },
             { 0 },
         }, &rsp_fields, NULL);
@@ -208,15 +229,16 @@ TEST_CASE(ble_hs_adv_test_case_flags)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_GEN, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_FLAGS,
                 .val = (uint8_t[]) {
                     BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP
                  },
                 .val_len = 1,
-            }, {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
             },
             { 0 },
         }, &rsp_fields, NULL);
@@ -231,7 +253,9 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Complete 16-bit service class UUIDs. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
+    adv_fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
     adv_fields.uuids16 = (uint16_t[]) { 0x0001, 0x1234, 0x54ab };
     adv_fields.num_uuids16 = 3;
     adv_fields.uuids16_is_complete = 1;
@@ -244,13 +268,13 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val_len = 6,
             },
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -258,6 +282,7 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Incomplete 16-bit service class UUIDs. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.uuids16 = (uint16_t[]) { 0x0001, 0x1234, 0x54ab };
     adv_fields.num_uuids16 = 3;
@@ -271,13 +296,13 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val_len = 6,
             },
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -285,6 +310,7 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Complete 32-bit service class UUIDs. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.uuids32 = (uint32_t[]) { 0x12345678, 0xabacadae };
     adv_fields.num_uuids32 = 2;
@@ -298,13 +324,13 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val_len = 8,
             },
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -312,6 +338,7 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Incomplete 32-bit service class UUIDs. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.uuids32 = (uint32_t[]) { 0x12345678, 0xabacadae };
     adv_fields.num_uuids32 = 2;
@@ -325,13 +352,13 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val_len = 8,
             },
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -339,6 +366,7 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Complete 128-bit service class UUIDs. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.uuids128 = (uint8_t[]) {
         0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
@@ -358,13 +386,13 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val_len = 16,
             },
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -372,6 +400,7 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Incomplete 128-bit service class UUIDs. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.uuids128 = (uint8_t[]) {
         0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
@@ -391,13 +420,13 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val_len = 16,
             },
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -405,6 +434,7 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Complete name. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.name = (uint8_t *)"myname";
     adv_fields.name_len = 6;
@@ -418,13 +448,13 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val_len = 6,
             },
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -432,6 +462,7 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Incomplete name. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.name = (uint8_t *)"myname";
     adv_fields.name_len = 6;
@@ -445,13 +476,13 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val_len = 6,
             },
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -459,12 +490,18 @@ TEST_CASE(ble_hs_adv_test_case_user)
 
     /*** Class of device. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.device_class = (uint8_t[]){ 1,2,3 };
 
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_DEVICE_CLASS,
                 .val = (uint8_t[]) { 1,2,3 },
                 .val_len = BLE_HS_ADV_DEVICE_CLASS_LEN,
@@ -474,22 +511,23 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** Slave interval range. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.slave_itvl_range = (uint8_t[]){ 1,2,3,4 };
 
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE,
                 .val = (uint8_t[]) { 1,2,3,4 },
                 .val_len = BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN,
@@ -499,16 +537,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x16 - Service data - 16-bit UUID. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.svc_data_uuid16 = (uint8_t[]){ 1,2,3,4 };
     adv_fields.svc_data_uuid16_len = 4;
@@ -516,6 +550,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID16,
                 .val = (uint8_t[]) { 1,2,3,4 },
                 .val_len = 4,
@@ -525,16 +564,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x17 - Public target address. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.public_tgt_addr = (uint8_t[]){ 1,2,3,4,5,6, 6,5,4,3,2,1 };
     adv_fields.num_public_tgt_addrs = 2;
@@ -542,6 +577,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR,
                 .val = (uint8_t[]){ 1,2,3,4,5,6, 6,5,4,3,2,1 },
                 .val_len = 2 * BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN,
@@ -551,16 +591,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x19 - Appearance. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.appearance = 0x1234;
     adv_fields.appearance_is_present = 1;
@@ -568,6 +604,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_APPEARANCE,
                 .val = (uint8_t[]){ 0x34, 0x12 },
                 .val_len = BLE_HS_ADV_APPEARANCE_LEN,
@@ -577,16 +618,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x1a - Advertising interval. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.adv_itvl = 0x1234;
     adv_fields.adv_itvl_is_present = 1;
@@ -594,6 +631,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_ADV_ITVL,
                 .val = (uint8_t[]){ 0x34, 0x12 },
                 .val_len = BLE_HS_ADV_ADV_ITVL_LEN,
@@ -603,22 +645,23 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x1b - LE bluetooth device address. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.le_addr = (uint8_t[]){ 1,2,3,4,5,6,7 };
 
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_LE_ADDR,
                 .val = (uint8_t[]) { 1,2,3,4,5,6,7 },
                 .val_len = BLE_HS_ADV_LE_ADDR_LEN,
@@ -628,16 +671,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x1c - LE role. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.le_role = BLE_HS_ADV_LE_ROLE_BOTH_PERIPH_PREF;
     adv_fields.le_role_is_present = 1;
@@ -645,6 +684,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_LE_ROLE,
                 .val = (uint8_t[]) { BLE_HS_ADV_LE_ROLE_BOTH_PERIPH_PREF },
                 .val_len = 1,
@@ -654,16 +698,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x20 - Service data - 32-bit UUID. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.svc_data_uuid32 = (uint8_t[]){ 1,2,3,4,5 };
     adv_fields.svc_data_uuid32_len = 5;
@@ -671,6 +711,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID32,
                 .val = (uint8_t[]) { 1,2,3,4,5 },
                 .val_len = 5,
@@ -680,16 +725,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x21 - Service data - 128-bit UUID. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.svc_data_uuid128 =
         (uint8_t[]){ 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18 };
@@ -698,6 +739,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_SVC_DATA_UUID128,
                 .val = (uint8_t[]){ 1,2,3,4,5,6,7,8,9,10,
                                     11,12,13,14,15,16,17,18 },
@@ -708,16 +754,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0x24 - URI. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.uri = (uint8_t[]){ 1,2,3,4 };
     adv_fields.uri_len = 4;
@@ -725,6 +767,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_URI,
                 .val = (uint8_t[]) { 1,2,3,4 },
                 .val_len = 4,
@@ -734,16 +781,12 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 
     /*** 0xff - Manufacturer specific data. */
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
     adv_fields.mfg_data = (uint8_t[]){ 1,2,3,4 };
     adv_fields.mfg_data_len = 4;
@@ -751,6 +794,11 @@ TEST_CASE(ble_hs_adv_test_case_user)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
+                .val_len = 1,
+            },
+            {
                 .type = BLE_HS_ADV_TYPE_MFG_DATA,
                 .val = (uint8_t[]) { 1,2,3,4 },
                 .val_len = 4,
@@ -760,11 +808,6 @@ TEST_CASE(ble_hs_adv_test_case_user)
                 .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
-            {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
-                .val_len = 1,
-            },
             { 0 },
         }, &rsp_fields, NULL);
 }
@@ -775,6 +818,7 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     struct ble_hs_adv_fields adv_fields;
 
     memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.flags_is_present = 1;
     adv_fields.tx_pwr_lvl_is_present = 1;
 
     /*** Complete 16-bit service class UUIDs. */
@@ -786,13 +830,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -816,13 +860,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -846,13 +890,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -876,13 +920,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -909,13 +953,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -945,13 +989,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -978,13 +1022,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1008,13 +1052,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1036,13 +1080,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1064,13 +1108,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1093,13 +1137,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1122,13 +1166,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1151,13 +1195,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1180,13 +1224,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1208,13 +1252,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1237,13 +1281,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1266,13 +1310,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1296,13 +1340,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1326,13 +1370,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1355,13 +1399,13 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
     ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
         (struct ble_hs_adv_test_field[]) {
             {
-                .type = BLE_HS_ADV_TYPE_FLAGS,
-                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
+                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
+                .val = (uint8_t[]){ 0 },
                 .val_len = 1,
             },
             {
-                .type = BLE_HS_ADV_TYPE_TX_PWR_LVL,
-                .val = (uint8_t[]){ 0x00 },
+                .type = BLE_HS_ADV_TYPE_FLAGS,
+                .val = (uint8_t[]){ BLE_HS_ADV_F_BREDR_UNSUP },
                 .val_len = 1,
             },
             { 0 },
@@ -1377,11 +1421,59 @@ TEST_CASE(ble_hs_adv_test_case_user_rsp)
         });
 }
 
+TEST_CASE(ble_hs_adv_test_case_user_full_payload)
+{
+    /* Intentionally allocate an extra byte. */
+    static const uint8_t mfg_data[30] = {
+        0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+        0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x10,
+        0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18,
+        0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e,
+    };
+
+    struct ble_hs_adv_fields adv_fields;
+    struct ble_hs_adv_fields rsp_fields;
+    int rc;
+
+    ble_hs_test_util_init();
+
+    memset(&rsp_fields, 0, sizeof rsp_fields);
+
+    /***
+     * An advertisement should allow 31 bytes of user data.  Each field has a
+     * two-byte header, leaving 29 bytes of payload.
+     */
+    memset(&adv_fields, 0, sizeof adv_fields);
+    adv_fields.mfg_data = (void *)mfg_data;
+    adv_fields.mfg_data_len = 29;
+
+    ble_hs_adv_test_misc_tx_and_verify_data(BLE_GAP_DISC_MODE_NON, &adv_fields,
+        (struct ble_hs_adv_test_field[]) {
+            {
+                .type = BLE_HS_ADV_TYPE_MFG_DATA,
+                .val = (void *)mfg_data,
+                .val_len = 29,
+            },
+            { 0 },
+        }, &rsp_fields, NULL);
+
+    /*** Fail with 30 bytes. */
+    rc = ble_hs_test_util_adv_stop(0);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    adv_fields.mfg_data_len = 30;
+    rc = ble_gap_adv_set_fields(&adv_fields);
+    TEST_ASSERT(rc == BLE_HS_EMSGSIZE);
+}
+
 TEST_SUITE(ble_hs_adv_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_hs_adv_test_case_flags();
     ble_hs_adv_test_case_user();
     ble_hs_adv_test_case_user_rsp();
+    ble_hs_adv_test_case_user_full_payload();
 }
 
 int
diff --git a/net/nimble/host/src/test/ble_hs_conn_test.c b/net/nimble/host/src/test/ble_hs_conn_test.c
index 5f86ad1b..c9574460 100644
--- a/net/nimble/host/src/test/ble_hs_conn_test.c
+++ b/net/nimble/host/src/test/ble_hs_conn_test.c
@@ -22,8 +22,8 @@
 #include <string.h>
 #include "testutil/testutil.h"
 #include "nimble/hci_common.h"
+#include "host/ble_hs_adv.h"
 #include "host/ble_hs_test.h"
-#include "host/host_hci.h"
 #include "ble_hs_test_util.h"
 
 static int
@@ -53,7 +53,9 @@ TEST_CASE(ble_hs_conn_test_direct_connect_success)
     TEST_ASSERT(!ble_hs_conn_test_util_any());
 
     /* Initiate connection. */
-    rc = ble_hs_test_util_conn_initiate(0, addr, NULL, NULL, NULL, 0);
+    rc = ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC,
+                                        BLE_ADDR_TYPE_PUBLIC,
+                                        addr, 0, NULL, NULL, NULL, 0);
     TEST_ASSERT(rc == 0);
 
     TEST_ASSERT(ble_gap_master_in_progress());
@@ -74,7 +76,7 @@ TEST_CASE(ble_hs_conn_test_direct_connect_success)
     conn = ble_hs_conn_first();
     TEST_ASSERT_FATAL(conn != NULL);
     TEST_ASSERT(conn->bhc_handle == 2);
-    TEST_ASSERT(memcmp(conn->bhc_addr, addr, 6) == 0);
+    TEST_ASSERT(memcmp(conn->bhc_peer_addr, addr, 6) == 0);
 
     chan = ble_hs_conn_chan_find(conn, BLE_L2CAP_CID_ATT);
     TEST_ASSERT_FATAL(chan != NULL);
@@ -85,28 +87,10 @@ TEST_CASE(ble_hs_conn_test_direct_connect_success)
     ble_hs_unlock();
 }
 
-TEST_CASE(ble_hs_conn_test_direct_connect_hci_errors)
-{
-    uint8_t addr[6] = { 1, 2, 3, 4, 5, 6 };
-    int rc;
-
-    ble_hs_test_util_init();
-
-    /* Ensure no current or pending connections. */
-    TEST_ASSERT(!ble_gap_master_in_progress());
-    TEST_ASSERT(!ble_hs_conn_test_util_any());
-
-    /* Initiate connection; receive no HCI ack. */
-    rc = ble_gap_conn_initiate(0, addr, NULL, NULL, NULL);
-    TEST_ASSERT(rc == BLE_HS_ETIMEOUT_HCI);
-
-    TEST_ASSERT(!ble_gap_master_in_progress());
-    TEST_ASSERT(!ble_hs_conn_test_util_any());
-}
-
 TEST_CASE(ble_hs_conn_test_direct_connectable_success)
 {
     struct hci_le_conn_complete evt;
+    struct ble_gap_adv_params adv_params;
     struct ble_l2cap_chan *chan;
     struct ble_hs_conn *conn;
     uint8_t addr[6] = { 1, 2, 3, 4, 5, 6 };
@@ -116,18 +100,18 @@ TEST_CASE(ble_hs_conn_test_direct_connectable_success)
 
     /* Ensure no current or pending connections. */
     TEST_ASSERT(!ble_gap_master_in_progress());
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
     TEST_ASSERT(!ble_hs_conn_test_util_any());
 
     /* Initiate advertising. */
-    rc = ble_hs_test_util_adv_start(BLE_GAP_DISC_MODE_NON,
-                                    BLE_GAP_CONN_MODE_DIR, addr,
-                                    BLE_HCI_ADV_PEER_ADDR_PUBLIC, NULL, NULL,
-                                    NULL, 0, 0);
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_DIR;
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                    addr, &adv_params, NULL, NULL, 0, 0);
     TEST_ASSERT(rc == 0);
 
     TEST_ASSERT(!ble_gap_master_in_progress());
-    TEST_ASSERT(ble_gap_slave_in_progress());
+    TEST_ASSERT(ble_gap_adv_active());
 
     /* Receive successful connection complete event. */
     memset(&evt, 0, sizeof evt);
@@ -139,14 +123,14 @@ TEST_CASE(ble_hs_conn_test_direct_connectable_success)
     rc = ble_gap_rx_conn_complete(&evt);
     TEST_ASSERT(rc == 0);
     TEST_ASSERT(!ble_gap_master_in_progress());
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
 
     ble_hs_lock();
 
     conn = ble_hs_conn_first();
     TEST_ASSERT_FATAL(conn != NULL);
     TEST_ASSERT(conn->bhc_handle == 2);
-    TEST_ASSERT(memcmp(conn->bhc_addr, addr, 6) == 0);
+    TEST_ASSERT(memcmp(conn->bhc_peer_addr, addr, 6) == 0);
 
     chan = ble_hs_conn_chan_find(conn, BLE_L2CAP_CID_ATT);
     TEST_ASSERT_FATAL(chan != NULL);
@@ -157,38 +141,11 @@ TEST_CASE(ble_hs_conn_test_direct_connectable_success)
     ble_hs_unlock();
 }
 
-TEST_CASE(ble_hs_conn_test_direct_connectable_hci_errors)
-{
-    struct hci_le_conn_complete evt;
-    uint8_t addr[6] = { 1, 2, 3, 4, 5, 6 };
-    int rc;
-
-    ble_hs_test_util_init();
-
-    /* Ensure no current or pending connections. */
-    TEST_ASSERT(!ble_gap_slave_in_progress());
-    TEST_ASSERT(!ble_hs_conn_test_util_any());
-
-    /* Initiate connection. */
-    rc = ble_hs_test_util_adv_start(BLE_GAP_DISC_MODE_NON,
-                                    BLE_GAP_CONN_MODE_DIR, addr,
-                                    BLE_HCI_ADV_PEER_ADDR_PUBLIC, NULL, NULL,
-                                    NULL, 0, 0);
-    TEST_ASSERT(rc == 0);
-    TEST_ASSERT(ble_gap_slave_in_progress());
-
-    /* Receive failure connection complete event. */
-    evt.status = BLE_ERR_UNSPECIFIED;
-    rc = ble_gap_rx_conn_complete(&evt);
-    TEST_ASSERT(rc == 0);
-    TEST_ASSERT(ble_gap_slave_in_progress());
-    TEST_ASSERT(!ble_hs_conn_test_util_any());
-}
-
 TEST_CASE(ble_hs_conn_test_undirect_connectable_success)
 {
     struct ble_hs_adv_fields adv_fields;
     struct hci_le_conn_complete evt;
+    struct ble_gap_adv_params adv_params;
     struct ble_l2cap_chan *chan;
     struct ble_hs_conn *conn;
     uint8_t addr[6] = { 1, 2, 3, 4, 5, 6 };
@@ -198,7 +155,7 @@ TEST_CASE(ble_hs_conn_test_undirect_connectable_success)
 
     /* Ensure no current or pending connections. */
     TEST_ASSERT(!ble_gap_master_in_progress());
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
     TEST_ASSERT(!ble_hs_conn_test_util_any());
 
     /* Initiate advertising. */
@@ -207,13 +164,14 @@ TEST_CASE(ble_hs_conn_test_undirect_connectable_success)
     rc = ble_gap_adv_set_fields(&adv_fields);
     TEST_ASSERT_FATAL(rc == 0);
 
-    rc = ble_hs_test_util_adv_start(BLE_GAP_DISC_MODE_NON,
-                                    BLE_GAP_CONN_MODE_UND, NULL, 0, NULL,
-                                    NULL, NULL, 0, 0);
+    adv_params = ble_hs_test_util_adv_params;
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_UND;
+    rc = ble_hs_test_util_adv_start(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                                    addr, &adv_params, NULL, NULL, 0, 0);
     TEST_ASSERT(rc == 0);
 
     TEST_ASSERT(!ble_gap_master_in_progress());
-    TEST_ASSERT(ble_gap_slave_in_progress());
+    TEST_ASSERT(ble_gap_adv_active());
 
     /* Receive successful connection complete event. */
     memset(&evt, 0, sizeof evt);
@@ -225,14 +183,14 @@ TEST_CASE(ble_hs_conn_test_undirect_connectable_success)
     rc = ble_gap_rx_conn_complete(&evt);
     TEST_ASSERT(rc == 0);
     TEST_ASSERT(!ble_gap_master_in_progress());
-    TEST_ASSERT(!ble_gap_slave_in_progress());
+    TEST_ASSERT(!ble_gap_adv_active());
 
     ble_hs_lock();
 
     conn = ble_hs_conn_first();
     TEST_ASSERT_FATAL(conn != NULL);
     TEST_ASSERT(conn->bhc_handle == 2);
-    TEST_ASSERT(memcmp(conn->bhc_addr, addr, 6) == 0);
+    TEST_ASSERT(memcmp(conn->bhc_peer_addr, addr, 6) == 0);
 
     chan = ble_hs_conn_chan_find(conn, BLE_L2CAP_CID_ATT);
     TEST_ASSERT_FATAL(chan != NULL);
@@ -245,10 +203,10 @@ TEST_CASE(ble_hs_conn_test_undirect_connectable_success)
 
 TEST_SUITE(conn_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_hs_conn_test_direct_connect_success();
-    ble_hs_conn_test_direct_connect_hci_errors();
     ble_hs_conn_test_direct_connectable_success();
-    ble_hs_conn_test_direct_connectable_hci_errors();
     ble_hs_conn_test_undirect_connectable_success();
 }
 
diff --git a/net/nimble/host/src/test/ble_host_hci_test.c b/net/nimble/host/src/test/ble_hs_hci_test.c
index 52837bd3..21184b8a 100644
--- a/net/nimble/host/src/test/ble_host_hci_test.c
+++ b/net/nimble/host/src/test/ble_hs_hci_test.c
@@ -21,31 +21,34 @@
 #include <errno.h>
 #include <string.h>
 #include "nimble/hci_common.h"
-#include "host/host_hci.h"
+#include "nimble/ble_hci_trans.h"
 #include "host/ble_hs_test.h"
 #include "testutil/testutil.h"
 #include "ble_hs_test_util.h"
 
-TEST_CASE(ble_host_hci_test_event_bad)
+TEST_CASE(ble_hs_hci_test_event_bad)
 {
-    uint8_t buf[2];
+    uint8_t *buf;
     int rc;
 
     /*** Invalid event code. */
+    buf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    TEST_ASSERT_FATAL(buf != NULL);
+
     buf[0] = 0xff;
     buf[1] = 0;
-    rc = host_hci_event_rx(buf);
+    rc = ble_hs_hci_evt_process(buf);
     TEST_ASSERT(rc == BLE_HS_ENOTSUP);
 }
 
-TEST_CASE(ble_host_hci_test_rssi)
+TEST_CASE(ble_hs_hci_test_rssi)
 {
     uint8_t params[BLE_HCI_READ_RSSI_ACK_PARAM_LEN];
     uint16_t opcode;
     int8_t rssi;
     int rc;
 
-    opcode = host_hci_opcode_join(BLE_HCI_OGF_STATUS_PARAMS,
+    opcode = ble_hs_hci_util_opcode_join(BLE_HCI_OGF_STATUS_PARAMS,
                                   BLE_HCI_OCF_RD_RSSI);
 
     /*** Success. */
@@ -57,7 +60,7 @@ TEST_CASE(ble_host_hci_test_rssi)
 
     ble_hs_test_util_set_ack_params(opcode, 0, params, sizeof params);
 
-    rc = ble_hci_util_read_rssi(1, &rssi);
+    rc = ble_hs_hci_util_read_rssi(1, &rssi);
     TEST_ASSERT_FATAL(rc == 0);
     TEST_ASSERT(rssi == -8);
 
@@ -66,29 +69,31 @@ TEST_CASE(ble_host_hci_test_rssi)
 
     ble_hs_test_util_set_ack_params(opcode, 0, params, sizeof params);
 
-    rc = ble_hci_util_read_rssi(1, &rssi);
+    rc = ble_hs_hci_util_read_rssi(1, &rssi);
     TEST_ASSERT(rc == BLE_HS_ECONTROLLER);
 
     /*** Failure: params too short. */
     ble_hs_test_util_set_ack_params(opcode, 0, params, sizeof params - 1);
-    rc = ble_hci_util_read_rssi(1, &rssi);
+    rc = ble_hs_hci_util_read_rssi(1, &rssi);
     TEST_ASSERT(rc == BLE_HS_ECONTROLLER);
 
     /*** Failure: params too long. */
     ble_hs_test_util_set_ack_params(opcode, 0, params, sizeof params + 1);
-    rc = ble_hci_util_read_rssi(1, &rssi);
+    rc = ble_hs_hci_util_read_rssi(1, &rssi);
     TEST_ASSERT(rc == BLE_HS_ECONTROLLER);
 }
 
-TEST_SUITE(ble_host_hci_suite)
+TEST_SUITE(ble_hs_hci_suite)
 {
-    ble_host_hci_test_event_bad();
-    ble_host_hci_test_rssi();
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_hs_hci_test_event_bad();
+    ble_hs_hci_test_rssi();
 }
 
 int
-ble_host_hci_test_all(void)
+ble_hs_hci_test_all(void)
 {
-    ble_host_hci_suite();
+    ble_hs_hci_suite();
     return tu_any_failed;
 }
diff --git a/net/nimble/host/src/test/ble_hs_test.c b/net/nimble/host/src/test/ble_hs_test.c
index 88a2c0cd..3bc468ec 100644
--- a/net/nimble/host/src/test/ble_hs_test.c
+++ b/net/nimble/host/src/test/ble_hs_test.c
@@ -23,24 +23,6 @@
 #include "testutil/testutil.h"
 #include "ble_hs_test_util.h"
 
-/* Our global device address. */
-uint8_t g_dev_addr[BLE_DEV_ADDR_LEN];
-
-void
-ble_hs_test_pkt_txed(struct os_mbuf *om)
-{
-    /* XXX: For now, just strip the HCI ACL data and L2CAP headers. */
-    os_mbuf_adj(om, BLE_HCI_DATA_HDR_SZ + BLE_L2CAP_HDR_SZ);
-    ble_hs_test_util_prev_tx_enqueue(om);
-}
-
-void
-ble_hs_test_hci_txed(uint8_t *cmdbuf)
-{
-    ble_hs_test_util_enqueue_hci_tx(cmdbuf);
-    os_memblock_put(&g_hci_cmd_pool, cmdbuf);
-}
-
 #ifdef MYNEWT_SELFTEST
 
 int
@@ -62,8 +44,9 @@ main(int argc, char **argv)
     ble_gatt_read_test_all();
     ble_gatt_write_test_all();
     ble_gatts_notify_test_all();
+    ble_gatts_read_test_suite();
     ble_gatts_reg_test_all();
-    ble_host_hci_test_all();
+    ble_hs_hci_test_all();
     ble_hs_adv_test_all();
     ble_hs_conn_test_all();
     ble_l2cap_test_all();
diff --git a/net/nimble/host/src/test/ble_hs_test_util.c b/net/nimble/host/src/test/ble_hs_test_util.c
index 90448da9..e1acb81c 100644
--- a/net/nimble/host/src/test/ble_hs_test_util.c
+++ b/net/nimble/host/src/test/ble_hs_test_util.c
@@ -23,10 +23,17 @@
 #include "testutil/testutil.h"
 #include "nimble/ble.h"
 #include "nimble/hci_common.h"
-#include "nimble/hci_transport.h"
-#include "host/host_hci.h"
+#include "nimble/ble_hci_trans.h"
+#include "host/ble_hs_adv.h"
+#include "host/ble_hs_id.h"
+#include "transport/ram/ble_hci_ram.h"
 #include "ble_hs_test_util.h"
 
+/* Our global device address. */
+uint8_t g_dev_addr[BLE_DEV_ADDR_LEN];
+
+#define BLE_HS_TEST_UTIL_PUB_ADDR_VAL { 0x0a, 0x54, 0xab, 0x49, 0x7f, 0x06 }
+
 /** Use lots of small mbufs to ensure correct mbuf usage. */
 #define BLE_HS_TEST_UTIL_NUM_MBUFS      (100)
 #define BLE_HS_TEST_UTIL_BUF_SIZE       OS_ALIGN(100, 4)
@@ -36,7 +43,7 @@
     OS_MEMPOOL_SIZE(BLE_HS_TEST_UTIL_NUM_MBUFS, BLE_HS_TEST_UTIL_MEMBLOCK_SIZE)
 
 #define BLE_HS_TEST_UTIL_LE_OPCODE(ocf) \
-    host_hci_opcode_join(BLE_HCI_OGF_LE, (ocf))
+    ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE, (ocf))
 
 struct os_eventq ble_hs_test_util_evq;
 
@@ -54,6 +61,17 @@ int ble_hs_test_util_num_prev_hci_txes;
 
 uint8_t ble_hs_test_util_cur_hci_tx[260];
 
+const struct ble_gap_adv_params ble_hs_test_util_adv_params = {
+    .conn_mode = BLE_GAP_CONN_MODE_UND,
+    .disc_mode = BLE_GAP_DISC_MODE_GEN,
+
+    .itvl_min = 0,
+    .itvl_max = 0,
+    .channel_map = 0,
+    .filter_policy = 0,
+    .high_duty_cycle = 0,
+};
+
 void
 ble_hs_test_util_prev_tx_enqueue(struct os_mbuf *om)
 {
@@ -69,20 +87,65 @@ ble_hs_test_util_prev_tx_enqueue(struct os_mbuf *om)
     }
 }
 
+static struct os_mbuf *
+ble_hs_test_util_prev_tx_dequeue_once(struct hci_data_hdr *out_hci_hdr)
+{
+    struct os_mbuf_pkthdr *omp;
+    struct os_mbuf *om;
+    int rc;
+
+    omp = STAILQ_FIRST(&ble_hs_test_util_prev_tx_queue);
+    if (omp == NULL) {
+        return NULL;
+    }
+    STAILQ_REMOVE_HEAD(&ble_hs_test_util_prev_tx_queue, omp_next);
+
+    om = OS_MBUF_PKTHDR_TO_MBUF(omp);
+
+    rc = ble_hs_hci_util_data_hdr_strip(om, out_hci_hdr);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT_FATAL(out_hci_hdr->hdh_len == OS_MBUF_PKTLEN(om));
+
+    return om;
+}
+
 struct os_mbuf *
 ble_hs_test_util_prev_tx_dequeue(void)
 {
-    struct os_mbuf_pkthdr *omp;
+    struct ble_l2cap_hdr l2cap_hdr;
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    uint8_t pb;
+    int rc;
 
     os_mbuf_free_chain(ble_hs_test_util_prev_tx_cur);
 
-    omp = STAILQ_FIRST(&ble_hs_test_util_prev_tx_queue);
-    if (omp != NULL) {
-        STAILQ_REMOVE_HEAD(&ble_hs_test_util_prev_tx_queue, omp_next);
-        ble_hs_test_util_prev_tx_cur = OS_MBUF_PKTHDR_TO_MBUF(omp);
+    om = ble_hs_test_util_prev_tx_dequeue_once(&hci_hdr);
+    if (om != NULL) {
+        pb = BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc);
+        TEST_ASSERT_FATAL(pb == BLE_HCI_PB_FIRST_NON_FLUSH);
+
+        rc = ble_l2cap_parse_hdr(om, 0, &l2cap_hdr);
+        TEST_ASSERT_FATAL(rc == 0);
+
+        os_mbuf_adj(om, BLE_L2CAP_HDR_SZ);
+
+        ble_hs_test_util_prev_tx_cur = om;
+        while (OS_MBUF_PKTLEN(ble_hs_test_util_prev_tx_cur) <
+               l2cap_hdr.blh_len) {
+
+            om = ble_hs_test_util_prev_tx_dequeue_once(&hci_hdr);
+            TEST_ASSERT_FATAL(om != NULL);
+
+            pb = BLE_HCI_DATA_PB(hci_hdr.hdh_handle_pb_bc);
+            TEST_ASSERT_FATAL(pb == BLE_HCI_PB_MIDDLE);
+
+            os_mbuf_concat(ble_hs_test_util_prev_tx_cur, om);
+        }
     } else {
         ble_hs_test_util_prev_tx_cur = NULL;
     }
+
     return ble_hs_test_util_prev_tx_cur;
 }
 
@@ -188,13 +251,14 @@ ble_hs_test_util_rx_hci_evt(uint8_t *evt)
     TEST_ASSERT_FATAL(totlen <= UINT8_MAX + BLE_HCI_EVENT_HDR_LEN);
 
     if (os_started()) {
-        evbuf = os_memblock_get(&g_hci_cmd_pool);
+        evbuf = ble_hci_trans_buf_alloc(
+            BLE_HCI_TRANS_BUF_EVT_LO);
         TEST_ASSERT_FATAL(evbuf != NULL);
 
         memcpy(evbuf, evt, totlen);
-        rc = ble_hci_transport_ctlr_event_send(evbuf);
+        rc = ble_hci_trans_ll_evt_tx(evbuf);
     } else {
-        rc = host_hci_event_rx(evt);
+        rc = ble_hs_hci_evt_process(evt);
     }
 
     TEST_ASSERT_FATAL(rc == 0);
@@ -284,7 +348,7 @@ ble_hs_test_util_set_ack_params(uint16_t opcode, uint8_t status, void *params,
     }
     ble_hs_test_util_num_phony_acks = 1;
 
-    ble_hci_set_phony_ack_cb(ble_hs_test_util_phony_ack_cb);
+    ble_hs_hci_set_phony_ack_cb(ble_hs_test_util_phony_ack_cb);
 }
 
 void
@@ -303,22 +367,22 @@ ble_hs_test_util_set_ack_seq(struct ble_hs_test_util_phony_ack *acks)
     }
     ble_hs_test_util_num_phony_acks = i;
 
-    ble_hci_set_phony_ack_cb(ble_hs_test_util_phony_ack_cb);
+    ble_hs_hci_set_phony_ack_cb(ble_hs_test_util_phony_ack_cb);
 }
 
 void
-ble_hs_test_util_create_rpa_conn(uint16_t handle, uint8_t *our_rpa,
-                                 uint8_t peer_addr_type, uint8_t *peer_id_addr,
-                                 uint8_t *peer_rpa,
+ble_hs_test_util_create_rpa_conn(uint16_t handle, uint8_t own_addr_type,
+                                 const uint8_t *our_rpa,
+                                 uint8_t peer_addr_type,
+                                 const uint8_t *peer_id_addr,
+                                 const uint8_t *peer_rpa,
                                  ble_gap_event_fn *cb, void *cb_arg)
 {
     struct hci_le_conn_complete evt;
     int rc;
 
-    ble_hs_test_util_set_ack(
-        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CREATE_CONN), 0);
-    rc = ble_gap_conn_initiate(peer_addr_type, peer_id_addr, NULL, cb, cb_arg);
-    TEST_ASSERT(rc == 0);
+    ble_hs_test_util_connect(own_addr_type, peer_addr_type,
+                             peer_id_addr, 0, NULL, cb, cb_arg, 0);
 
     memset(&evt, 0, sizeof evt);
     evt.subevent_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE;
@@ -340,28 +404,116 @@ ble_hs_test_util_create_rpa_conn(uint16_t handle, uint8_t *our_rpa,
 }
 
 void
-ble_hs_test_util_create_conn(uint16_t handle, uint8_t *peer_id_addr,
+ble_hs_test_util_create_conn(uint16_t handle, const uint8_t *peer_id_addr,
                              ble_gap_event_fn *cb, void *cb_arg)
 {
     static uint8_t null_addr[6];
 
-    ble_hs_test_util_create_rpa_conn(handle, null_addr, BLE_ADDR_TYPE_PUBLIC,
-                                     peer_id_addr, null_addr, cb, cb_arg);
+    ble_hs_test_util_create_rpa_conn(handle, BLE_ADDR_TYPE_PUBLIC, null_addr,
+                                     BLE_ADDR_TYPE_PUBLIC, peer_id_addr,
+                                     null_addr, cb, cb_arg);
+}
+
+static void
+ble_hs_test_util_conn_params_dflt(struct ble_gap_conn_params *conn_params)
+{
+    conn_params->scan_itvl = 0x0010;
+    conn_params->scan_window = 0x0010;
+    conn_params->itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN;
+    conn_params->itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX;
+    conn_params->latency = BLE_GAP_INITIAL_CONN_LATENCY;
+    conn_params->supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT;
+    conn_params->min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;
+    conn_params->max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;
+}
+
+static void
+ble_hs_test_util_hcc_from_conn_params(
+    struct hci_create_conn *hcc, uint8_t own_addr_type, uint8_t peer_addr_type,
+    const uint8_t *peer_addr, const struct ble_gap_conn_params *conn_params)
+{
+    hcc->scan_itvl = conn_params->scan_itvl;
+    hcc->scan_window = conn_params->scan_window;
+
+    if (peer_addr_type == BLE_GAP_ADDR_TYPE_WL) {
+        hcc->filter_policy = BLE_HCI_CONN_FILT_USE_WL;
+        hcc->peer_addr_type = 0;
+        memset(hcc->peer_addr, 0, 6);
+    } else {
+        hcc->filter_policy = BLE_HCI_CONN_FILT_NO_WL;
+        hcc->peer_addr_type = peer_addr_type;
+        memcpy(hcc->peer_addr, peer_addr, 6);
+    }
+    hcc->own_addr_type = own_addr_type;
+    hcc->conn_itvl_min = conn_params->itvl_min;
+    hcc->conn_itvl_max = conn_params->itvl_max;
+    hcc->conn_latency = conn_params->latency;
+    hcc->supervision_timeout = conn_params->supervision_timeout;
+    hcc->min_ce_len = conn_params->min_ce_len;
+    hcc->max_ce_len = conn_params->max_ce_len;
+}
+
+void
+ble_hs_test_util_verify_tx_create_conn(const struct hci_create_conn *exp)
+{
+    uint8_t param_len;
+    uint8_t *param;
+
+    param = ble_hs_test_util_verify_tx_hci(BLE_HCI_OGF_LE,
+                                           BLE_HCI_OCF_LE_CREATE_CONN,
+                                           &param_len);
+    TEST_ASSERT(param_len == BLE_HCI_CREATE_CONN_LEN);
+
+    TEST_ASSERT(le16toh(param + 0) == exp->scan_itvl);
+    TEST_ASSERT(le16toh(param + 2) == exp->scan_window);
+    TEST_ASSERT(param[4] == exp->filter_policy);
+    TEST_ASSERT(param[5] == exp->peer_addr_type);
+    TEST_ASSERT(memcmp(param + 6, exp->peer_addr, 6) == 0);
+    TEST_ASSERT(param[12] == exp->own_addr_type);
+    TEST_ASSERT(le16toh(param + 13) == exp->conn_itvl_min);
+    TEST_ASSERT(le16toh(param + 15) == exp->conn_itvl_max);
+    TEST_ASSERT(le16toh(param + 17) == exp->conn_latency);
+    TEST_ASSERT(le16toh(param + 19) == exp->supervision_timeout);
+    TEST_ASSERT(le16toh(param + 21) == exp->min_ce_len);
+    TEST_ASSERT(le16toh(param + 23) == exp->max_ce_len);
 }
 
 int
-ble_hs_test_util_conn_initiate(int addr_type, uint8_t *addr,
-                               struct ble_gap_crt_params *params,
-                               ble_gap_event_fn *cb, void *cb_arg,
-                               uint8_t ack_status)
+ble_hs_test_util_connect(uint8_t own_addr_type, uint8_t peer_addr_type,
+                         const uint8_t *peer_addr, int32_t duration_ms,
+                         const struct ble_gap_conn_params *params,
+                         ble_gap_event_fn *cb, void *cb_arg,
+                         uint8_t ack_status)
 {
+    struct ble_gap_conn_params dflt_params;
+    struct hci_create_conn hcc;
     int rc;
 
+    /* This function ensures the most recently sent HCI command is the expected
+     * create connection command.  If the current test case has unverified HCI
+     * commands, assume we are not interested in them and clear the queue.
+     */
+    ble_hs_test_util_prev_hci_tx_clear();
+
     ble_hs_test_util_set_ack(
-        host_hci_opcode_join(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CREATE_CONN),
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_CREATE_CONN),
         ack_status);
 
-    rc = ble_gap_conn_initiate(addr_type, addr, params, cb, cb_arg);
+    rc = ble_gap_connect(own_addr_type, peer_addr_type, peer_addr, duration_ms,
+                         params, cb, cb_arg);
+
+    TEST_ASSERT(rc == BLE_HS_HCI_ERR(ack_status));
+
+    if (params == NULL) {
+        ble_hs_test_util_conn_params_dflt(&dflt_params);
+        params = &dflt_params;
+    }
+
+    ble_hs_test_util_hcc_from_conn_params(&hcc, own_addr_type,
+                                          peer_addr_type, peer_addr, params);
+    ble_hs_test_util_verify_tx_create_conn(&hcc);
+
     return rc;
 }
 
@@ -371,25 +523,42 @@ ble_hs_test_util_conn_cancel(uint8_t ack_status)
     int rc;
 
     ble_hs_test_util_set_ack(
-        host_hci_opcode_join(BLE_HCI_OGF_LE,
-                             BLE_HCI_OCF_LE_CREATE_CONN_CANCEL),
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_CREATE_CONN_CANCEL),
         ack_status);
 
-    rc = ble_gap_cancel();
+    rc = ble_gap_conn_cancel();
     return rc;
 }
 
+void
+ble_hs_test_util_conn_cancel_full(void)
+{
+    struct hci_le_conn_complete evt;
+    int rc;
+
+    ble_hs_test_util_conn_cancel(0);
+
+    memset(&evt, 0, sizeof evt);
+    evt.subevent_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE;
+    evt.status = BLE_ERR_UNK_CONN_ID;
+    evt.role = BLE_HCI_LE_CONN_COMPLETE_ROLE_MASTER;
+
+    rc = ble_gap_rx_conn_complete(&evt);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
 int
 ble_hs_test_util_conn_terminate(uint16_t conn_handle, uint8_t hci_status)
 {
     int rc;
 
     ble_hs_test_util_set_ack(
-        host_hci_opcode_join(BLE_HCI_OGF_LINK_CTRL,
-                             BLE_HCI_OCF_DISCONNECT_CMD),
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LINK_CTRL,
+                                    BLE_HCI_OCF_DISCONNECT_CMD),
         hci_status);
 
-    rc = ble_gap_terminate(conn_handle);
+    rc = ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
     return rc;
 }
 
@@ -410,9 +579,19 @@ ble_hs_test_util_conn_disconnect(uint16_t conn_handle)
 }
 
 int
-ble_hs_test_util_disc(uint32_t duration_ms, uint8_t discovery_mode,
-                      uint8_t scan_type, uint8_t filter_policy,
-                      ble_gap_disc_fn *cb, void *cb_arg, int fail_idx,
+ble_hs_test_util_exp_hci_status(int cmd_idx, int fail_idx, uint8_t fail_status)
+{
+    if (cmd_idx == fail_idx) {
+        return BLE_HS_HCI_ERR(fail_status);
+    } else {
+        return 0;
+    }
+}
+
+int
+ble_hs_test_util_disc(uint8_t own_addr_type, int32_t duration_ms,
+                      const struct ble_gap_disc_params *disc_params,
+                      ble_gap_event_fn *cb, void *cb_arg, int fail_idx,
                       uint8_t fail_status)
 {
     int rc;
@@ -420,27 +599,77 @@ ble_hs_test_util_disc(uint32_t duration_ms, uint8_t discovery_mode,
     ble_hs_test_util_set_ack_seq(((struct ble_hs_test_util_phony_ack[]) {
         {
             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_PARAMS),
-            fail_idx == 0 ? fail_status : 0,
+            ble_hs_test_util_exp_hci_status(0, fail_idx, fail_status),
         },
         {
             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
-            fail_idx == 1 ? fail_status : 0,
+            ble_hs_test_util_exp_hci_status(1, fail_idx, fail_status),
         },
 
         { 0 }
     }));
 
-    rc = ble_gap_disc(duration_ms, discovery_mode, scan_type, filter_policy, 
-                      BLE_ADDR_TYPE_PUBLIC,
+    rc = ble_gap_disc(own_addr_type, duration_ms, disc_params,
                       cb, cb_arg);
     return rc;
 }
 
 int
-ble_hs_test_util_adv_start(uint8_t discoverable_mode,
-                           uint8_t connectable_mode,
-                           uint8_t *peer_addr, uint8_t peer_addr_type,
-                           struct ble_gap_adv_params *adv_params,
+ble_hs_test_util_disc_cancel(uint8_t ack_status)
+{
+    int rc;
+
+    ble_hs_test_util_set_ack(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+        ack_status);
+
+    rc = ble_gap_disc_cancel();
+    return rc;
+}
+
+static void
+ble_hs_test_util_verify_tx_rd_pwr(void)
+{
+    uint8_t param_len;
+
+    ble_hs_test_util_verify_tx_hci(BLE_HCI_OGF_LE,
+                                   BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR,
+                                   &param_len);
+    TEST_ASSERT(param_len == 0);
+}
+
+int
+ble_hs_test_util_adv_set_fields(struct ble_hs_adv_fields *adv_fields,
+                                uint8_t hci_status)
+{
+    int auto_pwr;
+    int rc;
+
+    auto_pwr = adv_fields->tx_pwr_lvl_is_present &&
+               adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO;
+
+    if (auto_pwr) {
+        ble_hs_test_util_set_ack_params(
+            ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR),
+            hci_status,
+            ((uint8_t[1]){0}), 1);
+    }
+
+    rc = ble_gap_adv_set_fields(adv_fields);
+    if (rc == 0 && auto_pwr) {
+        /* Verify tx of set advertising params command. */
+        ble_hs_test_util_verify_tx_rd_pwr();
+    }
+
+    return rc;
+}
+
+int
+ble_hs_test_util_adv_start(uint8_t own_addr_type,
+                           uint8_t peer_addr_type, const uint8_t *peer_addr, 
+                           const struct ble_gap_adv_params *adv_params,
                            ble_gap_event_fn *cb, void *cb_arg,
                            int fail_idx, uint8_t fail_status)
 {
@@ -456,31 +685,23 @@ ble_hs_test_util_adv_start(uint8_t discoverable_mode,
     };
     i++;
 
-    if (connectable_mode != BLE_GAP_CONN_MODE_DIR) {
-        acks[i] = (struct ble_hs_test_util_phony_ack) {
-            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR),
-            fail_idx == i ? fail_status : 0,
-            { 0 },
-            1,
-        };
-        i++;
-
+    if (adv_params->conn_mode != BLE_GAP_CONN_MODE_DIR) {
         acks[i] = (struct ble_hs_test_util_phony_ack) {
             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_DATA),
-            fail_idx == i ? fail_status : 0,
+            ble_hs_test_util_exp_hci_status(i, fail_idx, fail_status),
         };
         i++;
 
         acks[i] = (struct ble_hs_test_util_phony_ack) {
             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA),
-            fail_idx == i ? fail_status : 0,
+            ble_hs_test_util_exp_hci_status(i, fail_idx, fail_status),
         };
         i++;
     }
 
     acks[i] = (struct ble_hs_test_util_phony_ack) {
         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADV_ENABLE),
-        fail_idx == i ? fail_status : 0,
+        ble_hs_test_util_exp_hci_status(i, fail_idx, fail_status),
     };
     i++;
 
@@ -488,9 +709,8 @@ ble_hs_test_util_adv_start(uint8_t discoverable_mode,
 
     ble_hs_test_util_set_ack_seq(acks);
     
-    rc = ble_gap_adv_start(discoverable_mode, connectable_mode, 
-                        peer_addr, peer_addr_type,
-                        adv_params, cb, cb_arg);
+    rc = ble_gap_adv_start(own_addr_type, peer_addr_type, peer_addr, 
+                           BLE_HS_FOREVER, adv_params, cb, cb_arg);
 
     return rc;
 }
@@ -523,14 +743,14 @@ ble_hs_test_util_wl_set(struct ble_gap_white_entry *white_list,
     cmd_idx = 0;
     acks[cmd_idx] = (struct ble_hs_test_util_phony_ack) {
         BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CLEAR_WHITE_LIST),
-        fail_idx == cmd_idx ? fail_status : 0,
+        ble_hs_test_util_exp_hci_status(cmd_idx, fail_idx, fail_status),
     };
     cmd_idx++;
 
     for (i = 0; i < white_list_count; i++) {
         acks[cmd_idx] = (struct ble_hs_test_util_phony_ack) {
             BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_ADD_WHITE_LIST),
-            fail_idx == cmd_idx ? fail_status : 0,
+            ble_hs_test_util_exp_hci_status(cmd_idx, fail_idx, fail_status),
         };
 
         cmd_idx++;
@@ -557,6 +777,38 @@ ble_hs_test_util_conn_update(uint16_t conn_handle,
 }
 
 int
+ble_hs_test_util_set_our_irk(const uint8_t *irk, int fail_idx,
+                             uint8_t hci_status)
+{
+    int rc;
+
+    ble_hs_test_util_set_ack_seq(((struct ble_hs_test_util_phony_ack[]) {
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+            ble_hs_test_util_exp_hci_status(0, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_CLR_RESOLV_LIST),
+            ble_hs_test_util_exp_hci_status(1, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+            ble_hs_test_util_exp_hci_status(2, fail_idx, hci_status),
+        },
+        {
+            BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_ADD_RESOLV_LIST),
+            ble_hs_test_util_exp_hci_status(3, fail_idx, hci_status),
+        },
+        {
+            0
+        }
+    }));
+
+    rc = ble_hs_pvcy_set_our_irk(irk);
+    return rc;
+}
+
+int
 ble_hs_test_util_security_initiate(uint16_t conn_handle, uint8_t hci_status)
 {
     int rc;
@@ -597,6 +849,8 @@ ble_hs_test_util_l2cap_rx(uint16_t conn_handle,
     conn = ble_hs_conn_find(conn_handle);
     if (conn != NULL) {
         rc = ble_l2cap_rx(conn, hci_hdr, om, &rx_cb, &rx_buf);
+    } else {
+        os_mbuf_free_chain(om);
     }
 
     ble_hs_unlock();
@@ -624,15 +878,15 @@ ble_hs_test_util_l2cap_rx_payload_flat(uint16_t conn_handle, uint16_t cid,
     struct os_mbuf *om;
     int rc;
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     rc = os_mbuf_append(om, data, len);
     TEST_ASSERT_FATAL(rc == 0);
 
     hci_hdr.hdh_handle_pb_bc =
-        host_hci_handle_pb_bc_join(conn_handle,
-                                   BLE_HCI_PB_FIRST_FLUSH, 0);
+        ble_hs_hci_util_handle_pb_bc_join(conn_handle,
+                                          BLE_HCI_PB_FIRST_FLUSH, 0);
     hci_hdr.hdh_len = OS_MBUF_PKTHDR(om)->omp_len;
 
     rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, cid, &hci_hdr, om);
@@ -640,6 +894,26 @@ ble_hs_test_util_l2cap_rx_payload_flat(uint16_t conn_handle, uint16_t cid,
 }
 
 void
+ble_hs_test_util_rx_att_mtu_cmd(uint16_t conn_handle, int is_req, uint16_t mtu)
+{
+    struct ble_att_mtu_cmd cmd;
+    uint8_t buf[BLE_ATT_MTU_CMD_SZ];
+    int rc;
+
+    cmd.bamc_mtu = mtu;
+
+    if (is_req) {
+        ble_att_mtu_req_write(buf, sizeof buf, &cmd);
+    } else {
+        ble_att_mtu_rsp_write(buf, sizeof buf, &cmd);
+    }
+
+    rc = ble_hs_test_util_l2cap_rx_payload_flat(conn_handle, BLE_L2CAP_CID_ATT,
+                                                buf, sizeof buf);
+    TEST_ASSERT(rc == 0);
+}
+
+void
 ble_hs_test_util_rx_att_err_rsp(uint16_t conn_handle, uint8_t req_op,
                                 uint8_t error_code, uint16_t err_handle)
 {
@@ -666,48 +940,55 @@ ble_hs_test_util_set_startup_acks(void)
      */
     ble_hs_test_util_set_ack_seq(((struct ble_hs_test_util_phony_ack[]) {
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_CTLR_BASEBAND,
-                                           BLE_HCI_OCF_CB_RESET),
+            .opcode = ble_hs_hci_util_opcode_join(BLE_HCI_OGF_CTLR_BASEBAND,
+                                                  BLE_HCI_OCF_CB_RESET),
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_CTLR_BASEBAND,
-                                           BLE_HCI_OCF_CB_SET_EVENT_MASK),
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_SET_EVENT_MASK),
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_CTLR_BASEBAND,
-                                           BLE_HCI_OCF_CB_SET_EVENT_MASK2),
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_CTLR_BASEBAND, BLE_HCI_OCF_CB_SET_EVENT_MASK2),
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                           BLE_HCI_OCF_LE_SET_EVENT_MASK),
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_EVENT_MASK),
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                           BLE_HCI_OCF_LE_RD_BUF_SIZE),
-            .evt_params = { 0xff, 0xff, 1 },
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_BUF_SIZE),
+            /* Use a very low buffer size (16) to test fragmentation. */
+            .evt_params = { 0x10, 0x00, 0x20 },
             .evt_params_len = 3,
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                           BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT),
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT),
             .evt_params = { 0 },
             .evt_params_len = 8,
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                           BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_BD_ADDR),
+            .evt_params = BLE_HS_TEST_UTIL_PUB_ADDR_VAL,
+            .evt_params_len = 6,
+        },
+        {
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                           BLE_HCI_OCF_LE_CLR_RESOLV_LIST),
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CLR_RESOLV_LIST),
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                           BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_ADDR_RES_EN),
         },
         {
-            .opcode = host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                           BLE_HCI_OCF_LE_ADD_RESOLV_LIST),
+            .opcode = ble_hs_hci_util_opcode_join(
+                BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_RESOLV_LIST),
         },
         { 0 }
     }));
@@ -790,9 +1071,26 @@ ble_hs_test_util_tx_all(void)
 }
 
 void
-ble_hs_test_util_set_public_addr(uint8_t *addr)
+ble_hs_test_util_verify_tx_prep_write(uint16_t attr_handle, uint16_t offset,
+                                      const void *data, int data_len)
 {
-    ble_hs_pvcy_set_our_id_addr(addr);
+    struct ble_att_prep_write_cmd req;
+    struct os_mbuf *om;
+
+    ble_hs_test_util_tx_all();
+    om = ble_hs_test_util_prev_tx_dequeue();
+    TEST_ASSERT_FATAL(om != NULL);
+    TEST_ASSERT(OS_MBUF_PKTLEN(om) ==
+                BLE_ATT_PREP_WRITE_CMD_BASE_SZ + data_len);
+
+    om = os_mbuf_pullup(om, BLE_ATT_PREP_WRITE_CMD_BASE_SZ);
+    TEST_ASSERT_FATAL(om != NULL);
+
+    ble_att_prep_write_req_parse(om->om_data, om->om_len, &req);
+    TEST_ASSERT(req.bapc_handle == attr_handle);
+    TEST_ASSERT(req.bapc_offset == offset);
+    TEST_ASSERT(os_mbuf_cmpf(om, BLE_ATT_PREP_WRITE_CMD_BASE_SZ,
+                             data, data_len) == 0);
 }
 
 void
@@ -811,8 +1109,354 @@ ble_hs_test_util_verify_tx_exec_write(uint8_t expected_flags)
 }
 
 void
+ble_hs_test_util_verify_tx_read_rsp_gen(uint8_t att_op,
+                                        uint8_t *attr_data, int attr_len)
+{
+    struct os_mbuf *om;
+    uint8_t u8;
+    int rc;
+    int i;
+
+    ble_hs_test_util_tx_all();
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, 1, &u8);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(u8 == att_op);
+
+    for (i = 0; i < attr_len; i++) {
+        rc = os_mbuf_copydata(om, i + 1, 1, &u8);
+        TEST_ASSERT(rc == 0);
+        TEST_ASSERT(u8 == attr_data[i]);
+    }
+
+    rc = os_mbuf_copydata(om, i + 1, 1, &u8);
+    TEST_ASSERT(rc != 0);
+}
+
+void
+ble_hs_test_util_verify_tx_read_rsp(uint8_t *attr_data, int attr_len)
+{
+    ble_hs_test_util_verify_tx_read_rsp_gen(BLE_ATT_OP_READ_RSP,
+                                            attr_data, attr_len);
+}
+
+void
+ble_hs_test_util_verify_tx_read_blob_rsp(uint8_t *attr_data, int attr_len)
+{
+    ble_hs_test_util_verify_tx_read_rsp_gen(BLE_ATT_OP_READ_BLOB_RSP,
+                                            attr_data, attr_len);
+}
+
+void
+ble_hs_test_util_verify_tx_write_rsp(void)
+{
+    struct os_mbuf *om;
+    uint8_t u8;
+    int rc;
+
+    ble_hs_test_util_tx_all();
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, 1, &u8);
+    TEST_ASSERT(rc == 0);
+    TEST_ASSERT(u8 == BLE_ATT_OP_WRITE_RSP);
+}
+
+void
+ble_hs_test_util_verify_tx_mtu_cmd(int is_req, uint16_t mtu)
+{
+    struct ble_att_mtu_cmd cmd;
+    struct os_mbuf *om;
+
+    ble_hs_test_util_tx_all();
+
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    if (is_req) {
+        ble_att_mtu_req_parse(om->om_data, om->om_len, &cmd);
+    } else {
+        ble_att_mtu_rsp_parse(om->om_data, om->om_len, &cmd);
+    }
+
+    TEST_ASSERT(cmd.bamc_mtu == mtu);
+}
+
+void
+ble_hs_test_util_verify_tx_err_rsp(uint8_t req_op, uint16_t handle,
+                                   uint8_t error_code)
+{
+    struct ble_att_error_rsp rsp;
+    struct os_mbuf *om;
+    uint8_t buf[BLE_ATT_ERROR_RSP_SZ];
+    int rc;
+
+    ble_hs_test_util_tx_all();
+
+    om = ble_hs_test_util_prev_tx_dequeue();
+
+    rc = os_mbuf_copydata(om, 0, sizeof buf, buf);
+    TEST_ASSERT(rc == 0);
+
+    ble_att_error_rsp_parse(buf, sizeof buf, &rsp);
+
+    TEST_ASSERT(rsp.baep_req_op == req_op);
+    TEST_ASSERT(rsp.baep_handle == handle);
+    TEST_ASSERT(rsp.baep_error_code == error_code);
+}
+
+void
+ble_hs_test_util_set_static_rnd_addr(void)
+{
+    uint8_t addr[6] = { 1, 2, 3, 4, 5, 0xc1 };
+    int rc;
+
+    ble_hs_test_util_set_ack(
+        BLE_HS_TEST_UTIL_LE_OPCODE(BLE_HCI_OCF_LE_SET_RAND_ADDR), 0);
+
+    rc = ble_hs_id_set_rnd(addr);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_get_first_hci_tx();
+}
+
+struct os_mbuf *
+ble_hs_test_util_om_from_flat(const void *buf, uint16_t len)
+{
+    struct os_mbuf *om;
+
+    om = ble_hs_mbuf_from_flat(buf, len);
+    TEST_ASSERT_FATAL(om != NULL);
+
+    return om;
+}
+
+int
+ble_hs_test_util_flat_attr_cmp(const struct ble_hs_test_util_flat_attr *a,
+                               const struct ble_hs_test_util_flat_attr *b)
+{
+    if (a->handle != b->handle) {
+        return -1;
+    }
+    if (a->offset != b->offset) {
+        return -1;
+    }
+    if (a->value_len != b->value_len) {
+        return -1;
+    }
+    return memcmp(a->value, b->value, a->value_len);
+}
+
+void
+ble_hs_test_util_attr_to_flat(struct ble_hs_test_util_flat_attr *flat,
+                              const struct ble_gatt_attr *attr)
+{
+    int rc;
+
+    flat->handle = attr->handle;
+    flat->offset = attr->offset;
+    rc = ble_hs_mbuf_to_flat(attr->om, flat->value, sizeof flat->value,
+                           &flat->value_len);
+    TEST_ASSERT_FATAL(rc == 0);
+}
+
+void
+ble_hs_test_util_attr_from_flat(struct ble_gatt_attr *attr,
+                                const struct ble_hs_test_util_flat_attr *flat)
+{
+    attr->handle = flat->handle;
+    attr->offset = flat->offset;
+    attr->om = ble_hs_test_util_om_from_flat(flat->value, flat->value_len);
+}
+
+int
+ble_hs_test_util_read_local_flat(uint16_t attr_handle, uint16_t max_len,
+                                 void *buf, uint16_t *out_len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    rc = ble_att_svr_read_local(attr_handle, &om);
+    if (rc != 0) {
+        return rc;
+    }
+
+    TEST_ASSERT_FATAL(OS_MBUF_PKTLEN(om) <= max_len);
+
+    rc = os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), buf);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    *out_len = OS_MBUF_PKTLEN(om);
+
+    os_mbuf_free_chain(om);
+    return 0;
+}
+
+int
+ble_hs_test_util_write_local_flat(uint16_t attr_handle,
+                                  const void *buf, uint16_t buf_len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(buf, buf_len);
+    rc = ble_att_svr_write_local(attr_handle, om);
+    return rc;
+}
+
+int
+ble_hs_test_util_gatt_write_flat(uint16_t conn_handle, uint16_t attr_handle,
+                                 const void *data, uint16_t data_len,
+                                 ble_gatt_attr_fn *cb, void *cb_arg)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(data, data_len);
+    rc = ble_gattc_write(conn_handle, attr_handle, om, cb, cb_arg);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_gatt_write_no_rsp_flat(uint16_t conn_handle,
+                                        uint16_t attr_handle,
+                                        const void *data, uint16_t data_len)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(data, data_len);
+    rc = ble_gattc_write_no_rsp(conn_handle, attr_handle, om);
+
+    return rc;
+}
+
+int
+ble_hs_test_util_gatt_write_long_flat(uint16_t conn_handle,
+                                      uint16_t attr_handle,
+                                      const void *data, uint16_t data_len,
+                                      ble_gatt_attr_fn *cb, void *cb_arg)
+{
+    struct os_mbuf *om;
+    int rc;
+
+    om = ble_hs_test_util_om_from_flat(data, data_len);
+    rc = ble_gattc_write_long(conn_handle, attr_handle, om, cb, cb_arg);
+
+    return rc;
+}
+
+static int
+ble_hs_test_util_mbuf_chain_len(const struct os_mbuf *om)
+{
+    int count;
+
+    count = 0;
+    while (om != NULL) {
+        count++;
+        om = SLIST_NEXT(om, om_next);
+    }
+
+    return count;
+}
+
+int
+ble_hs_test_util_mbuf_count(const struct ble_hs_test_util_mbuf_params *params)
+{
+    const struct ble_att_prep_entry *prep;
+    const struct os_mbuf_pkthdr *omp;
+    const struct ble_l2cap_chan *chan;
+    const struct ble_hs_conn *conn;
+    const struct os_mbuf *om;
+    int count;
+    int i;
+
+    ble_hs_process_tx_data_queue();
+    ble_hs_process_rx_data_queue();
+
+    count = ble_hs_test_util_mbuf_mpool.mp_num_free;
+
+    if (params->prev_tx) {
+        count += ble_hs_test_util_mbuf_chain_len(ble_hs_test_util_prev_tx_cur);
+        STAILQ_FOREACH(omp, &ble_hs_test_util_prev_tx_queue, omp_next) {
+            om = OS_MBUF_PKTHDR_TO_MBUF(omp);
+            count += ble_hs_test_util_mbuf_chain_len(om);
+        }
+    }
+
+    ble_hs_lock();
+    for (i = 0; ; i++) {
+        conn = ble_hs_conn_find_by_idx(i);
+        if (conn == NULL) {
+            break;
+        }
+
+        if (params->rx_queue) {
+            SLIST_FOREACH(chan, &conn->bhc_channels, blc_next) {
+                count += ble_hs_test_util_mbuf_chain_len(chan->blc_rx_buf);
+            }
+        }
+
+        if (params->prep_list) {
+            SLIST_FOREACH(prep, &conn->bhc_att_svr.basc_prep_list, bape_next) {
+                count += ble_hs_test_util_mbuf_chain_len(prep->bape_value);
+            }
+        }
+    }
+    ble_hs_unlock();
+
+    return count;
+}
+
+void
+ble_hs_test_util_assert_mbufs_freed(
+    const struct ble_hs_test_util_mbuf_params *params)
+{
+    static const struct ble_hs_test_util_mbuf_params dflt = {
+        .prev_tx = 1,
+        .rx_queue = 1,
+        .prep_list = 1,
+    };
+
+    int count;
+
+    if (params == NULL) {
+        params = &dflt;
+    }
+
+    count = ble_hs_test_util_mbuf_count(params);
+    TEST_ASSERT(count == ble_hs_test_util_mbuf_mpool.mp_num_blocks);
+}
+
+void
+ble_hs_test_util_post_test(void *arg)
+{
+    ble_hs_test_util_assert_mbufs_freed(arg);
+}
+
+static int
+ble_hs_test_util_pkt_txed(struct os_mbuf *om, void *arg)
+{
+    ble_hs_test_util_prev_tx_enqueue(om);
+    return 0;
+}
+
+static int
+ble_hs_test_util_hci_txed(uint8_t *cmdbuf, void *arg)
+{
+    ble_hs_test_util_enqueue_hci_tx(cmdbuf);
+    ble_hci_trans_buf_free(cmdbuf);
+    return 0;
+}
+
+void
 ble_hs_test_util_init(void)
 {
+    struct ble_hci_ram_cfg hci_cfg;
     struct ble_hs_cfg cfg;
     int rc;
 
@@ -827,6 +1471,12 @@ ble_hs_test_util_init(void)
 
     cfg = ble_hs_cfg_dflt;
     cfg.max_connections = 8;
+    cfg.max_l2cap_chans = 3 * cfg.max_connections;
+    cfg.max_services = 16;
+    cfg.max_client_configs = 32;
+    cfg.max_attrs = 64;
+    cfg.max_gattc_procs = 16;
+
     rc = ble_hs_init(&ble_hs_test_util_evq, &cfg);
     TEST_ASSERT_FATAL(rc == 0);
 
@@ -846,9 +1496,19 @@ ble_hs_test_util_init(void)
     rc = os_msys_register(&ble_hs_test_util_mbuf_pool);
     TEST_ASSERT_FATAL(rc == 0);
 
-    ble_hci_set_phony_ack_cb(NULL);
+    ble_hs_hci_set_phony_ack_cb(NULL);
 
-    ble_hs_test_util_prev_hci_tx_clear();
+    ble_hci_trans_cfg_ll(ble_hs_test_util_hci_txed, NULL,
+                         ble_hs_test_util_pkt_txed, NULL);
 
-    ble_hs_test_util_set_public_addr(g_dev_addr);
+    hci_cfg = ble_hci_ram_cfg_dflt;
+    rc = ble_hci_ram_init(&hci_cfg);
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_set_startup_acks();
+
+    rc = ble_hs_start();
+    TEST_ASSERT_FATAL(rc == 0);
+
+    ble_hs_test_util_prev_hci_tx_clear();
 }
diff --git a/net/nimble/host/src/test/ble_hs_test_util.h b/net/nimble/host/src/test/ble_hs_test_util.h
index 92032526..7780d4ee 100644
--- a/net/nimble/host/src/test/ble_hs_test_util.h
+++ b/net/nimble/host/src/test/ble_hs_test_util.h
@@ -6,7 +6,7 @@
  * 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,
@@ -27,14 +27,29 @@
 struct ble_hs_conn;
 struct ble_l2cap_chan;
 struct hci_disconn_complete;
+struct hci_create_conn;
 
-struct os_eventq ble_hs_test_util_evq;
+extern struct os_eventq ble_hs_test_util_evq;
+extern const struct ble_gap_adv_params ble_hs_test_util_adv_params;
 
 struct ble_hs_test_util_num_completed_pkts_entry {
     uint16_t handle_id; /* 0 for terminating entry in array. */
     uint16_t num_pkts;
 };
 
+struct ble_hs_test_util_flat_attr {
+    uint16_t handle;
+    uint16_t offset;
+    uint8_t value[BLE_ATT_ATTR_MAX_LEN];
+    uint16_t value_len;
+};
+
+struct ble_hs_test_util_mbuf_params {
+    unsigned prev_tx:1;
+    unsigned rx_queue:1;
+    unsigned prep_list:1;
+};
+
 void ble_hs_test_util_prev_tx_enqueue(struct os_mbuf *om);
 struct os_mbuf *ble_hs_test_util_prev_tx_dequeue(void);
 struct os_mbuf *ble_hs_test_util_prev_tx_dequeue_pullup(void);
@@ -54,28 +69,40 @@ void ble_hs_test_util_build_cmd_complete(uint8_t *dst, int len,
 void ble_hs_test_util_build_cmd_status(uint8_t *dst, int len,
                                        uint8_t status, uint8_t num_pkts,
                                        uint16_t opcode);
-void ble_hs_test_util_create_rpa_conn(uint16_t handle, uint8_t *our_rpa,
+void ble_hs_test_util_create_rpa_conn(uint16_t handle, uint8_t own_addr_type,
+                                      const uint8_t *our_rpa,
                                       uint8_t peer_addr_type,
-                                      uint8_t *peer_id_addr,
-                                      uint8_t *peer_rpa,
+                                      const uint8_t *peer_id_addr,
+                                      const uint8_t *peer_rpa,
                                       ble_gap_event_fn *cb, void *cb_arg);
-void ble_hs_test_util_create_conn(uint16_t handle, uint8_t *addr,
+void ble_hs_test_util_create_conn(uint16_t handle, const uint8_t *addr,
                                   ble_gap_event_fn *cb, void *cb_arg);
-int ble_hs_test_util_conn_initiate(int addr_type, uint8_t *addr,
-                                   struct ble_gap_crt_params *params,
-                                   ble_gap_event_fn *cb, void *cb_arg,
+int ble_hs_test_util_connect(uint8_t own_addr_type,
+                                   uint8_t peer_addr_type,
+                                   const uint8_t *peer_addr,
+                                   int32_t duration_ms,
+                                   const struct ble_gap_conn_params *params,
+                                   ble_gap_event_fn *cb,
+                                   void *cb_arg,
                                    uint8_t ack_status);
 int ble_hs_test_util_conn_cancel(uint8_t ack_status);
+void ble_hs_test_util_conn_cancel_full(void);
 int ble_hs_test_util_conn_terminate(uint16_t conn_handle, uint8_t hci_status);
 void ble_hs_test_util_conn_disconnect(uint16_t conn_handle);
-int ble_hs_test_util_disc(uint32_t duration_ms, uint8_t discovery_mode,
-                          uint8_t scan_type, uint8_t filter_policy,
-                          ble_gap_disc_fn *cb, void *cb_arg, int fail_idx,
+int ble_hs_test_util_exp_hci_status(int cmd_idx, int fail_idx,
+                                    uint8_t fail_status);
+int ble_hs_test_util_disc(uint8_t own_addr_type, int32_t duration_ms,
+                          const struct ble_gap_disc_params *disc_params,
+                          ble_gap_event_fn *cb, void *cb_arg, int fail_idx,
                           uint8_t fail_status);
-int ble_hs_test_util_adv_start(uint8_t discoverable_mode,
-                               uint8_t connectable_mode,
-                               uint8_t *peer_addr, uint8_t peer_addr_type,
-                               struct ble_gap_adv_params *adv_params,
+int ble_hs_test_util_disc_cancel(uint8_t ack_status);
+void ble_hs_test_util_verify_tx_create_conn(const struct hci_create_conn *exp);
+int ble_hs_test_util_adv_set_fields(struct ble_hs_adv_fields *adv_fields,
+                                    uint8_t hci_status);
+int ble_hs_test_util_adv_start(uint8_t own_addr_type,
+                               uint8_t peer_addr_type,
+                               const uint8_t *peer_addr,
+                               const struct ble_gap_adv_params *adv_params,
                                ble_gap_event_fn *cb, void *cb_arg,
                                int fail_idx, uint8_t fail_status);
 int ble_hs_test_util_adv_stop(uint8_t hci_status);
@@ -85,6 +112,8 @@ int ble_hs_test_util_wl_set(struct ble_gap_white_entry *white_list,
 int ble_hs_test_util_conn_update(uint16_t conn_handle,
                                  struct ble_gap_upd_params *params,
                                  uint8_t hci_status);
+int ble_hs_test_util_set_our_irk(const uint8_t *irk, int fail_idx,
+                                 uint8_t hci_status);
 int ble_hs_test_util_security_initiate(uint16_t conn_handle,
                                        uint8_t hci_status);
 int ble_hs_test_util_l2cap_rx_first_frag(uint16_t conn_handle, uint16_t cid,
@@ -96,6 +125,8 @@ int ble_hs_test_util_l2cap_rx(uint16_t conn_handle,
 int ble_hs_test_util_l2cap_rx_payload_flat(uint16_t conn_handle, uint16_t cid,
                                            const void *data, int len);
 void ble_hs_test_util_rx_hci_buf_size_ack(uint16_t buf_size);
+void ble_hs_test_util_rx_att_mtu_cmd(uint16_t conn_handle, int is_req,
+                                     uint16_t mtu);
 void ble_hs_test_util_rx_att_err_rsp(uint16_t conn_handle, uint8_t req_op,
                                      uint8_t error_code, uint16_t err_handle);
 void ble_hs_test_util_set_startup_acks(void);
@@ -106,8 +137,46 @@ void ble_hs_test_util_rx_disconn_complete_event(
 uint8_t *ble_hs_test_util_verify_tx_hci(uint8_t ogf, uint16_t ocf,
                                         uint8_t *out_param_len);
 void ble_hs_test_util_tx_all(void);
-void ble_hs_test_util_set_public_addr(uint8_t *addr);
+void ble_hs_test_util_verify_tx_prep_write(uint16_t attr_handle,
+                                           uint16_t offset,
+                                           const void *data, int data_len);
 void ble_hs_test_util_verify_tx_exec_write(uint8_t expected_flags);
+void ble_hs_test_util_verify_tx_read_rsp(uint8_t *attr_data, int attr_len);
+void ble_hs_test_util_verify_tx_read_blob_rsp(uint8_t *attr_data,
+                                              int attr_len);
+void ble_hs_test_util_verify_tx_write_rsp(void);
+void ble_hs_test_util_verify_tx_mtu_cmd(int is_req, uint16_t mtu);
+void ble_hs_test_util_verify_tx_err_rsp(uint8_t req_op, uint16_t handle,
+                                        uint8_t error_code);
+void ble_hs_test_util_set_static_rnd_addr(void);
+struct os_mbuf *ble_hs_test_util_om_from_flat(const void *buf, uint16_t len);
+int ble_hs_test_util_flat_attr_cmp(const struct ble_hs_test_util_flat_attr *a,
+                                   const struct ble_hs_test_util_flat_attr *b);
+void ble_hs_test_util_attr_to_flat(struct ble_hs_test_util_flat_attr *flat,
+                                   const struct ble_gatt_attr *attr);
+void ble_hs_test_util_attr_from_flat(
+    struct ble_gatt_attr *attr, const struct ble_hs_test_util_flat_attr *flat);
+int ble_hs_test_util_read_local_flat(uint16_t attr_handle, uint16_t max_len,
+                                     void *buf, uint16_t *out_len);
+int ble_hs_test_util_write_local_flat(uint16_t attr_handle,
+                                      const void *buf, uint16_t buf_len);
+int ble_hs_test_util_gatt_write_flat(uint16_t conn_handle,
+                                     uint16_t attr_handle,
+                                     const void *data, uint16_t data_len,
+                                     ble_gatt_attr_fn *cb, void *cb_arg);
+int ble_hs_test_util_gatt_write_no_rsp_flat(uint16_t conn_handle,
+                                            uint16_t attr_handle,
+                                            const void *data,
+                                            uint16_t data_len);
+int ble_hs_test_util_gatt_write_long_flat(uint16_t conn_handle,
+                                          uint16_t attr_handle,
+                                          const void *data, uint16_t data_len,
+                                          ble_gatt_attr_fn *cb, void *cb_arg);
+int ble_hs_test_util_mbuf_count(
+    const struct ble_hs_test_util_mbuf_params *params);
+void ble_hs_test_util_assert_mbufs_freed(
+    const struct ble_hs_test_util_mbuf_params *params);
+void ble_hs_test_util_post_test(void *arg);
 void ble_hs_test_util_init(void);
 
 #endif
diff --git a/net/nimble/host/src/test/ble_l2cap_test.c b/net/nimble/host/src/test/ble_l2cap_test.c
index 161bd55c..69db2f87 100644
--- a/net/nimble/host/src/test/ble_l2cap_test.c
+++ b/net/nimble/host/src/test/ble_l2cap_test.c
@@ -21,7 +21,6 @@
 #include <errno.h>
 #include "testutil/testutil.h"
 #include "nimble/hci_common.h"
-#include "host/host_hci.h"
 #include "host/ble_hs_test.h"
 #include "ble_hs_test_util.h"
 
@@ -74,7 +73,8 @@ ble_l2cap_test_util_rx_update_req(uint16_t conn_handle, uint8_t id,
     ble_l2cap_sig_update_req_write(v, BLE_L2CAP_SIG_UPDATE_REQ_SZ, &req);
 
     ble_hs_test_util_set_ack(
-        host_hci_opcode_join(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CONN_UPDATE), 0);
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_CONN_UPDATE), 0);
     rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SIG,
                                               &hci_hdr, om);
     TEST_ASSERT_FATAL(rc == 0);
@@ -242,7 +242,7 @@ ble_l2cap_test_util_rx_first_frag(uint16_t conn_handle,
     void *v;
     int rc;
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     v = os_mbuf_extend(om, l2cap_frag_len);
@@ -266,7 +266,7 @@ ble_l2cap_test_util_rx_next_frag(uint16_t conn_handle, uint16_t hci_len)
     void *v;
     int rc;
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     v = os_mbuf_extend(om, hci_len);
@@ -374,7 +374,7 @@ TEST_CASE(ble_l2cap_test_case_frag_single)
     /*** HCI header specifies middle fragment without start. */
     hci_hdr = BLE_L2CAP_TEST_UTIL_HCI_HDR(2, BLE_HCI_PB_MIDDLE, 10);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     om = ble_l2cap_prepend_hdr(om, 0, 5);
@@ -480,12 +480,11 @@ TEST_CASE(ble_l2cap_test_case_sig_unsol_rsp)
  *****************************************************************************/
 
 static int
-ble_l2cap_test_util_conn_cb(int event, struct ble_gap_conn_ctxt *ctxt,
-                            void *arg)
+ble_l2cap_test_util_conn_cb(struct ble_gap_event *event, void *arg)
 {
     int *accept;
 
-    switch (event) {
+    switch (event->type) {
     case BLE_GAP_EVENT_L2CAP_UPDATE_REQ:
         accept = arg;
         return !*accept;
@@ -667,6 +666,8 @@ TEST_CASE(ble_l2cap_test_case_sig_update_init_fail_bad_id)
 
 TEST_SUITE(ble_l2cap_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_l2cap_test_case_bad_header();
     ble_l2cap_test_case_frag_single();
     ble_l2cap_test_case_frag_multiple();
diff --git a/net/nimble/host/src/test/ble_os_test.c b/net/nimble/host/src/test/ble_os_test.c
index ee7ffd65..a9c28ea9 100644
--- a/net/nimble/host/src/test/ble_os_test.c
+++ b/net/nimble/host/src/test/ble_os_test.c
@@ -21,18 +21,13 @@
 #include "os/os.h"
 #include "testutil/testutil.h"
 #include "nimble/hci_common.h"
-#include "nimble/hci_transport.h"
+#include "nimble/ble_hci_trans.h"
 #include "host/ble_hs_test.h"
 #include "host/ble_gap.h"
 #include "ble_hs_test_util.h"
 
-#ifdef ARCH_sim
-#define BLE_OS_TEST_STACK_SIZE      1024
-#define BLE_OS_TEST_APP_STACK_SIZE  1024
-#else
 #define BLE_OS_TEST_STACK_SIZE      256
 #define BLE_OS_TEST_APP_STACK_SIZE  256
-#endif
 
 #define BLE_OS_TEST_APP_PRIO         9
 #define BLE_OS_TEST_TASK_PRIO        10
@@ -48,7 +43,7 @@ static uint8_t ble_os_test_peer_addr[6] = { 1, 2, 3, 4, 5, 6 };
 
 static void ble_os_test_app_task_handler(void *arg);
 
-static int ble_os_test_gap_event;
+static int ble_os_test_gap_event_type;
 
 static void
 ble_os_test_init_app_task(void)
@@ -96,20 +91,23 @@ ble_os_test_misc_conn_exists(uint16_t conn_handle)
 }
 
 static int
-ble_gap_direct_connect_test_connect_cb(int event,
-                                       struct ble_gap_conn_ctxt *ctxt,
-                                       void *arg)
+ble_gap_direct_connect_test_connect_cb(struct ble_gap_event *event, void *arg)
 {
+    struct ble_gap_conn_desc desc;
     int *cb_called;
+    int rc;
 
     cb_called = arg;
     *cb_called = 1;
 
-    TEST_ASSERT(event == BLE_GAP_EVENT_CONNECT);
-    TEST_ASSERT(ctxt->connect.status == 0);
-    TEST_ASSERT(ctxt->desc->conn_handle == 2);
-    TEST_ASSERT(ctxt->desc->peer_id_addr_type == BLE_ADDR_TYPE_PUBLIC);
-    TEST_ASSERT(memcmp(ctxt->desc->peer_id_addr, ble_os_test_peer_addr, 6) == 0);
+    TEST_ASSERT(event->type == BLE_GAP_EVENT_CONNECT);
+    TEST_ASSERT(event->connect.status == 0);
+    TEST_ASSERT(event->connect.conn_handle == 2);
+
+    rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
+    TEST_ASSERT_FATAL(rc == 0);
+    TEST_ASSERT(desc.peer_id_addr_type == BLE_ADDR_TYPE_PUBLIC);
+    TEST_ASSERT(memcmp(desc.peer_id_addr, ble_os_test_peer_addr, 6) == 0);
 
     return 0;
 }
@@ -133,9 +131,10 @@ ble_gap_direct_connect_test_task_handler(void *arg)
     TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
 
     /* Initiate a direct connection. */
-    ble_hs_test_util_conn_initiate(0, addr, NULL,
-                                   ble_gap_direct_connect_test_connect_cb,
-                                   &cb_called, 0);
+    ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                             addr, 0, NULL,
+                             ble_gap_direct_connect_test_connect_cb,
+                             &cb_called, 0);
     TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
     TEST_ASSERT(!cb_called);
 
@@ -168,22 +167,23 @@ TEST_CASE(ble_gap_direct_connect_test_case)
     os_start();
 }
 
-static void
-ble_gap_gen_disc_test_connect_cb(int event, int status,
-                                 struct ble_gap_disc_desc *desc, void *arg)
+static int
+ble_os_disc_test_cb(struct ble_gap_event *event, void *arg)
 {
     int *cb_called;
 
     cb_called = arg;
     *cb_called = 1;
 
-    TEST_ASSERT(event == BLE_GAP_EVENT_DISC_COMPLETE);
-    TEST_ASSERT(status == 0);
+    TEST_ASSERT(event->type == BLE_GAP_EVENT_DISC_COMPLETE);
+
+    return 0;
 }
 
 static void
-ble_gap_gen_disc_test_task_handler(void *arg)
+ble_os_disc_test_task_handler(void *arg)
 {
+    struct ble_gap_disc_params disc_params;
     int cb_called;
     int rc;
 
@@ -203,11 +203,10 @@ ble_gap_gen_disc_test_task_handler(void *arg)
     TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
     TEST_ASSERT(!ble_gap_master_in_progress());
 
-    /* Initiate the general discovery procedure with a 200 ms timeout. */
-    rc = ble_hs_test_util_disc(300, BLE_GAP_DISC_MODE_GEN,
-                               BLE_HCI_SCAN_TYPE_ACTIVE,
-                               BLE_HCI_SCAN_FILT_NO_WL,
-                               ble_gap_gen_disc_test_connect_cb,
+    /* Initiate the general discovery procedure with a 300 ms timeout. */
+    memset(&disc_params, 0, sizeof disc_params);
+    rc = ble_hs_test_util_disc(BLE_ADDR_TYPE_PUBLIC, 300, &disc_params,
+                               ble_os_disc_test_cb,
                                &cb_called, 0, 0);
     TEST_ASSERT(rc == 0);
     TEST_ASSERT(!ble_os_test_misc_conn_exists(BLE_HS_CONN_HANDLE_NONE));
@@ -226,7 +225,8 @@ ble_gap_gen_disc_test_task_handler(void *arg)
     TEST_ASSERT(!cb_called);
 
     ble_hs_test_util_set_ack(
-        host_hci_opcode_join(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_SET_SCAN_ENABLE),
         0);
 
     /* Wait 250 more ms; verify scan completed. */
@@ -238,13 +238,13 @@ ble_gap_gen_disc_test_task_handler(void *arg)
     tu_restart();
 }
 
-TEST_CASE(ble_gap_gen_disc_test_case)
+TEST_CASE(ble_os_disc_test_case)
 {
     ble_os_test_misc_init();
 
     os_task_init(&ble_os_test_task,
-                 "ble_gap_gen_disc_test_task",
-                 ble_gap_gen_disc_test_task_handler, NULL,
+                 "ble_os_disc_test_task",
+                 ble_os_disc_test_task_handler, NULL,
                  BLE_OS_TEST_TASK_PRIO, OS_WAIT_FOREVER, ble_os_test_stack,
                  OS_STACK_ALIGN(BLE_OS_TEST_STACK_SIZE));
 
@@ -252,15 +252,15 @@ TEST_CASE(ble_gap_gen_disc_test_case)
 }
 
 static int
-ble_gap_terminate_cb(int event, struct ble_gap_conn_ctxt *ctxt, void *arg)
+ble_gap_terminate_cb(struct ble_gap_event *event, void *arg)
 {
     int *disconn_handle;
 
-    ble_os_test_gap_event = event;
+    ble_os_test_gap_event_type = event->type;
 
-    if (event == BLE_GAP_EVENT_DISCONNECT) {
+    if (event->type == BLE_GAP_EVENT_DISCONNECT) {
         disconn_handle = arg;
-        *disconn_handle = ctxt->desc->conn_handle;
+        *disconn_handle = event->disconnect.conn.conn_handle;
     }
 
     return 0;
@@ -294,8 +294,9 @@ ble_gap_terminate_test_task_handler(void *arg)
     TEST_ASSERT(!ble_gap_master_in_progress());
 
     /* Create two direct connections. */
-    ble_hs_test_util_conn_initiate(0, addr1, NULL, ble_gap_terminate_cb,
-                                   &disconn_handle, 0);
+    ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                             addr1, 0, NULL, ble_gap_terminate_cb,
+                             &disconn_handle, 0);
     memset(&conn_evt, 0, sizeof conn_evt);
     conn_evt.subevent_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE;
     conn_evt.status = BLE_ERR_SUCCESS;
@@ -304,8 +305,9 @@ ble_gap_terminate_test_task_handler(void *arg)
     rc = ble_gap_rx_conn_complete(&conn_evt);
     TEST_ASSERT(rc == 0);
 
-    ble_hs_test_util_conn_initiate(0, addr2, NULL, ble_gap_terminate_cb,
-                                   &disconn_handle, 0);
+    ble_hs_test_util_connect(BLE_ADDR_TYPE_PUBLIC, BLE_ADDR_TYPE_PUBLIC,
+                             addr2, 0, NULL, ble_gap_terminate_cb,
+                             &disconn_handle, 0);
     memset(&conn_evt, 0, sizeof conn_evt);
     conn_evt.subevent_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE;
     conn_evt.status = BLE_ERR_SUCCESS;
@@ -324,7 +326,7 @@ ble_gap_terminate_test_task_handler(void *arg)
     disconn_evt.status = 0;
     disconn_evt.reason = BLE_ERR_REM_USER_CONN_TERM;
     ble_hs_test_util_rx_disconn_complete_event(&disconn_evt);
-    TEST_ASSERT(ble_os_test_gap_event == BLE_GAP_EVENT_DISCONNECT);
+    TEST_ASSERT(ble_os_test_gap_event_type == BLE_GAP_EVENT_DISCONNECT);
     TEST_ASSERT(disconn_handle == 1);
     TEST_ASSERT_FATAL(!ble_os_test_misc_conn_exists(1));
     TEST_ASSERT_FATAL(ble_os_test_misc_conn_exists(2));
@@ -336,7 +338,7 @@ ble_gap_terminate_test_task_handler(void *arg)
     disconn_evt.status = 0;
     disconn_evt.reason = BLE_ERR_REM_USER_CONN_TERM;
     ble_hs_test_util_rx_disconn_complete_event(&disconn_evt);
-    TEST_ASSERT(ble_os_test_gap_event == BLE_GAP_EVENT_DISCONNECT);
+    TEST_ASSERT(ble_os_test_gap_event_type == BLE_GAP_EVENT_DISCONNECT);
     TEST_ASSERT(disconn_handle == 2);
     TEST_ASSERT_FATAL(!ble_os_test_misc_conn_exists(1));
     TEST_ASSERT_FATAL(!ble_os_test_misc_conn_exists(2));
@@ -384,7 +386,9 @@ TEST_CASE(ble_gap_terminate_test_case)
 
 TEST_SUITE(ble_os_test_suite)
 {
-    ble_gap_gen_disc_test_case();
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
+    ble_os_disc_test_case();
     ble_gap_direct_connect_test_case();
     ble_gap_terminate_test_case();
 }
diff --git a/net/nimble/host/src/test/ble_sm_lgcy_test.c b/net/nimble/host/src/test/ble_sm_lgcy_test.c
index 59dceab4..6e451ca9 100644
--- a/net/nimble/host/src/test/ble_sm_lgcy_test.c
+++ b/net/nimble/host/src/test/ble_sm_lgcy_test.c
@@ -23,7 +23,6 @@
 #include "testutil/testutil.h"
 #include "nimble/hci_common.h"
 #include "nimble/nimble_opt.h"
-#include "host/host_hci.h"
 #include "host/ble_sm.h"
 #include "host/ble_hs_test.h"
 #include "ble_hs_test_util.h"
@@ -822,6 +821,8 @@ TEST_CASE(ble_sm_lgcy_peer_pk_iio4_rio4_b1_iat0_rat0_ik7_rk7)
 
 TEST_SUITE(ble_sm_lgcy_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     /*** No privacy. */
 
     /* Peer as initiator. */
diff --git a/net/nimble/host/src/test/ble_sm_sc_test.c b/net/nimble/host/src/test/ble_sm_sc_test.c
index 4d60f0c5..518720c9 100644
--- a/net/nimble/host/src/test/ble_sm_sc_test.c
+++ b/net/nimble/host/src/test/ble_sm_sc_test.c
@@ -23,7 +23,6 @@
 #include "testutil/testutil.h"
 #include "nimble/hci_common.h"
 #include "nimble/nimble_opt.h"
-#include "host/host_hci.h"
 #include "host/ble_sm.h"
 #include "host/ble_hs_test.h"
 #include "ble_hs_test_util.h"
@@ -4879,6 +4878,8 @@ TEST_CASE(ble_sm_sc_us_pk_iio2_rio0_b1_iat2_rat2_ik7_rk3)
 
 TEST_SUITE(ble_sm_sc_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     /*** No privacy. */
 
     /* Peer as initiator. */
diff --git a/net/nimble/host/src/test/ble_sm_test.c b/net/nimble/host/src/test/ble_sm_test.c
index d8909965..f139ddb7 100644
--- a/net/nimble/host/src/test/ble_sm_test.c
+++ b/net/nimble/host/src/test/ble_sm_test.c
@@ -23,7 +23,6 @@
 #include "testutil/testutil.h"
 #include "nimble/hci_common.h"
 #include "nimble/nimble_opt.h"
-#include "host/host_hci.h"
 #include "host/ble_sm.h"
 #include "host/ble_hs_test.h"
 #include "ble_hs_test_util.h"
@@ -648,6 +647,8 @@ TEST_CASE(ble_sm_test_case_us_fail_inval)
 
 TEST_SUITE(ble_sm_gen_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_sm_test_case_f4();
     ble_sm_test_case_f5();
     ble_sm_test_case_f6();
diff --git a/net/nimble/host/src/test/ble_sm_test_util.c b/net/nimble/host/src/test/ble_sm_test_util.c
index 2225f30e..9edaa04c 100644
--- a/net/nimble/host/src/test/ble_sm_test_util.c
+++ b/net/nimble/host/src/test/ble_sm_test_util.c
@@ -23,15 +23,16 @@
 #include "testutil/testutil.h"
 #include "nimble/hci_common.h"
 #include "nimble/nimble_opt.h"
-#include "host/host_hci.h"
 #include "host/ble_sm.h"
 #include "host/ble_hs_test.h"
+#include "host/ble_hs_id.h"
 #include "ble_hs_test_util.h"
 #include "ble_sm_test_util.h"
 
-int ble_sm_test_gap_event;
+int ble_sm_test_gap_event_type;
 int ble_sm_test_gap_status;
 struct ble_gap_sec_state ble_sm_test_sec_state;
+static struct ble_gap_passkey_params ble_sm_test_ioact;
 
 int ble_sm_test_store_obj_type;
 union ble_store_key ble_sm_test_store_key;
@@ -102,9 +103,10 @@ ble_sm_test_util_init(void)
     ble_hs_cfg.store_write_cb = ble_sm_test_util_store_write;
 
     ble_sm_test_store_obj_type = -1;
-    ble_sm_test_gap_event = -1;
+    ble_sm_test_gap_event_type = -1;
     ble_sm_test_gap_status = -1;
 
+    memset(&ble_sm_test_ioact, 0, sizeof ble_sm_test_ioact);
     memset(&ble_sm_test_sec_state, 0xff, sizeof ble_sm_test_sec_state);
 }
 
@@ -223,19 +225,19 @@ ble_sm_test_util_init_good(struct ble_sm_test_params *params,
         ble_hs_cfg.sm_their_key_dist = out_us->pair_cmd->init_key_dist;
     }
 
-    ble_hs_test_util_set_public_addr(out_us->id_addr);
+    ble_hs_id_set_pub(out_us->id_addr);
     ble_sm_dbg_set_next_pair_rand(out_us->randoms[0].value);
     ble_sm_dbg_set_next_ediv(out_us->ediv);
     ble_sm_dbg_set_next_master_id_rand(out_us->rand_num);
     ble_sm_dbg_set_next_ltk(out_us->ltk);
-    ble_hs_pvcy_set_our_irk(out_us->id_info->irk);
+    ble_hs_test_util_set_our_irk(out_us->id_info->irk, 0, 0);
     ble_sm_dbg_set_next_csrk(out_us->sign_info->sig_key);
 
     if (out_us->public_key != NULL) {
         ble_sm_dbg_set_sc_keys(out_us->public_key->x, params->our_priv_key);
     }
 
-    ble_hs_test_util_create_rpa_conn(2, out_us->rpa,
+    ble_hs_test_util_create_rpa_conn(2, out_us->addr_type, out_us->rpa,
                                      out_peer->addr_type,
                                      out_peer->id_addr, out_peer->rpa,
                                      ble_sm_test_util_conn_cb,
@@ -260,31 +262,32 @@ ble_sm_test_util_init_good(struct ble_sm_test_params *params,
     }
 }
 
-struct ble_gap_passkey_action ble_sm_test_ioact;
-
 int
-ble_sm_test_util_conn_cb(int event, struct ble_gap_conn_ctxt *ctxt, void *arg)
+ble_sm_test_util_conn_cb(struct ble_gap_event *event, void *arg)
 {
+    struct ble_gap_conn_desc desc;
     int rc;
 
-    switch (event) {
+    switch (event->type) {
     case BLE_GAP_EVENT_ENC_CHANGE:
-        ble_sm_test_gap_status = ctxt->enc_change.status;
-        ble_sm_test_sec_state = ctxt->desc->sec_state;
-        rc = 0;
+        ble_sm_test_gap_status = event->enc_change.status;
+
+        rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
+        TEST_ASSERT_FATAL(rc == 0);
+        ble_sm_test_sec_state = desc.sec_state;
         break;
 
     case BLE_GAP_EVENT_PASSKEY_ACTION:
-        ble_sm_test_ioact = ctxt->passkey_action;
+        ble_sm_test_ioact = event->passkey.params;
         break;
 
     default:
         return 0;
     }
 
-    ble_sm_test_gap_event = event;
+    ble_sm_test_gap_event_type = event->type;
 
-    return rc;
+    return 0;
 }
 
 static void
@@ -302,7 +305,7 @@ ble_sm_test_util_rx_pair_cmd(uint16_t conn_handle, uint8_t op,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_PAIR_CMD_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_PAIR_CMD_SZ;
@@ -349,7 +352,7 @@ ble_sm_test_util_rx_confirm(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_PAIR_CONFIRM_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_PAIR_CONFIRM_SZ;
@@ -379,7 +382,7 @@ ble_sm_test_util_rx_random(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_PAIR_RANDOM_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_PAIR_RANDOM_SZ;
@@ -408,7 +411,7 @@ ble_sm_test_util_rx_sec_req(uint16_t conn_handle, struct ble_sm_sec_req *cmd,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_SEC_REQ_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_SEC_REQ_SZ;
@@ -437,7 +440,7 @@ ble_sm_test_util_rx_public_key(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_PUBLIC_KEY_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_PUBLIC_KEY_SZ;
@@ -467,7 +470,7 @@ ble_sm_test_util_rx_dhkey_check(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_DHKEY_CHECK_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_DHKEY_CHECK_SZ;
@@ -497,7 +500,7 @@ ble_sm_test_util_rx_enc_info(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_ENC_INFO_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_ENC_INFO_SZ;
@@ -527,7 +530,7 @@ ble_sm_test_util_rx_master_id(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_MASTER_ID_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_MASTER_ID_SZ;
@@ -557,7 +560,7 @@ ble_sm_test_util_rx_id_info(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_ID_INFO_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_ID_INFO_SZ;
@@ -587,7 +590,7 @@ ble_sm_test_util_rx_id_addr_info(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_ID_ADDR_INFO_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_ID_ADDR_INFO_SZ;
@@ -617,7 +620,7 @@ ble_sm_test_util_rx_sign_info(uint16_t conn_handle,
         2, BLE_HCI_PB_FIRST_FLUSH,
         BLE_L2CAP_HDR_SZ + BLE_SM_HDR_SZ + BLE_SM_SIGN_INFO_SZ);
 
-    om = ble_hs_misc_pkthdr();
+    om = ble_hs_mbuf_l2cap_pkt();
     TEST_ASSERT_FATAL(om != NULL);
 
     payload_len = BLE_SM_HDR_SZ + BLE_SM_SIGN_INFO_SZ;
@@ -637,7 +640,7 @@ ble_sm_test_util_verify_tx_hdr(uint8_t sm_op, uint16_t payload_len)
 {
     struct os_mbuf *om;
 
-    om = ble_hs_test_util_prev_tx_dequeue();
+    om = ble_hs_test_util_prev_tx_dequeue_pullup();
     TEST_ASSERT_FATAL(om != NULL);
 
     TEST_ASSERT(OS_MBUF_PKTLEN(om) == BLE_SM_HDR_SZ + payload_len);
@@ -797,10 +800,14 @@ ble_sm_test_util_verify_tx_id_addr_info(struct ble_sm_id_addr_info *exp_cmd)
 {
     struct ble_sm_id_addr_info cmd;
     struct os_mbuf *om;
-    uint8_t *our_id_addr;
-    uint8_t our_id_addr_type;
+    const uint8_t *our_id_addr;
+    int rc;
 
-    our_id_addr = ble_hs_pvcy_our_id_addr(&our_id_addr_type);
+    ble_hs_lock();
+    rc = ble_hs_id_addr(exp_cmd->addr_type, &our_id_addr, NULL);
+    ble_hs_unlock();
+
+    TEST_ASSERT_FATAL(rc == 0);
 
     ble_hs_test_util_tx_all();
     om = ble_sm_test_util_verify_tx_hdr(BLE_SM_OP_IDENTITY_ADDR_INFO,
@@ -809,8 +816,6 @@ ble_sm_test_util_verify_tx_id_addr_info(struct ble_sm_id_addr_info *exp_cmd)
 
     TEST_ASSERT(cmd.addr_type == exp_cmd->addr_type);
     TEST_ASSERT(memcmp(cmd.bd_addr, exp_cmd->bd_addr, 6) == 0);
-
-    TEST_ASSERT(cmd.addr_type == our_id_addr_type);
     TEST_ASSERT(memcmp(cmd.bd_addr, our_id_addr, 6) == 0);
 }
 
@@ -902,14 +907,27 @@ ble_sm_test_util_verify_tx_lt_key_req_neg_reply(uint16_t conn_handle)
 }
 
 static void
-ble_sm_test_util_set_lt_key_req_reply_ack(uint8_t status,
-                                                uint16_t conn_handle)
+ble_sm_test_util_set_lt_key_req_neg_reply_ack(uint8_t status,
+                                              uint16_t conn_handle)
+{
+    static uint8_t params[BLE_HCI_LT_KEY_REQ_NEG_REPLY_ACK_PARAM_LEN];
+
+    htole16(params, conn_handle);
+    ble_hs_test_util_set_ack_params(
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY),
+        status, params, sizeof params);
+}
+
+static void
+ble_sm_test_util_set_lt_key_req_reply_ack(uint8_t status, uint16_t conn_handle)
 {
     static uint8_t params[BLE_HCI_LT_KEY_REQ_REPLY_ACK_PARAM_LEN];
 
     htole16(params, conn_handle);
     ble_hs_test_util_set_ack_params(
-        host_hci_opcode_join(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY),
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY),
         status, params, sizeof params);
 }
 
@@ -975,15 +993,20 @@ ble_sm_test_util_io_inject(struct ble_sm_test_passkey_info *passkey_info,
 
     io_sm_state = ble_sm_ioact_state(passkey_info->passkey.action);
     if (io_sm_state != cur_sm_state) {
+        TEST_ASSERT(ble_sm_test_ioact.action == BLE_SM_IOACT_NONE);
         return;
     }
 
+    TEST_ASSERT(ble_sm_test_ioact.action == passkey_info->passkey.action);
+
     if (passkey_info->passkey.action == BLE_SM_IOACT_NUMCMP) {
         TEST_ASSERT(ble_sm_test_ioact.numcmp == passkey_info->exp_numcmp);
     }
 
     rc = ble_sm_inject_io(2, &passkey_info->passkey);
     TEST_ASSERT(rc == 0);
+
+    ble_sm_test_ioact.action = BLE_SM_IOACT_NONE;
 }
 
 void
@@ -1193,6 +1216,7 @@ ble_sm_test_util_verify_persist(struct ble_sm_test_params *params,
 
 static void
 ble_sm_test_util_peer_bonding_good(int send_enc_req,
+                                   uint8_t our_addr_type,
                                    uint8_t *our_rpa,
                                    uint8_t peer_addr_type,
                                    uint8_t *peer_id_addr,
@@ -1203,8 +1227,9 @@ ble_sm_test_util_peer_bonding_good(int send_enc_req,
     struct ble_hs_conn *conn;
     int rc;
 
-    ble_hs_test_util_create_rpa_conn(2, our_rpa, peer_addr_type, peer_id_addr,
-                                     peer_rpa, ble_sm_test_util_conn_cb, NULL);
+    ble_hs_test_util_create_rpa_conn(2, our_addr_type, our_rpa, peer_addr_type,
+                                     peer_id_addr, peer_rpa,
+                                     ble_sm_test_util_conn_cb, NULL);
 
     /* This test inspects and modifies the connection object after unlocking
      * the host mutex.  It is not OK for real code to do this, but this test
@@ -1255,7 +1280,7 @@ ble_sm_test_util_peer_bonding_good(int send_enc_req,
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
     TEST_ASSERT(ble_sm_test_gap_status == 0);
     TEST_ASSERT(ble_sm_test_sec_state.encrypted);
     TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
@@ -1294,7 +1319,7 @@ ble_sm_test_util_peer_bonding_bad(uint16_t ediv, uint64_t rand_num)
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Receive a long term key request from the controller. */
-    ble_sm_test_util_set_lt_key_req_reply_ack(0, 2);
+    ble_sm_test_util_set_lt_key_req_neg_reply_ack(0, 2);
     ble_sm_test_util_rx_lt_key_req(2, rand_num, ediv);
     TEST_ASSERT(!conn->bhc_sec_state.encrypted);
 
@@ -1323,7 +1348,8 @@ ble_sm_test_util_peer_bonding_bad(uint16_t ediv, uint64_t rand_num)
  *                                  0: No security request; we initiate.
  */
 static void
-ble_sm_test_util_us_bonding_good(int send_enc_req, uint8_t *our_rpa,
+ble_sm_test_util_us_bonding_good(int send_enc_req, uint8_t our_addr_type,
+                                 uint8_t *our_rpa,
                                  uint8_t peer_addr_type,
                                  uint8_t *peer_id_addr, uint8_t *peer_rpa,
                                  uint8_t *ltk, int authenticated,
@@ -1332,7 +1358,8 @@ ble_sm_test_util_us_bonding_good(int send_enc_req, uint8_t *our_rpa,
     struct ble_sm_sec_req sec_req;
     struct ble_hs_conn *conn;
 
-    ble_hs_test_util_create_rpa_conn(2, our_rpa, peer_addr_type, peer_id_addr,
+    ble_hs_test_util_create_rpa_conn(2, our_addr_type, our_rpa,
+                                     peer_addr_type, peer_id_addr,
                                      peer_rpa, ble_sm_test_util_conn_cb, NULL);
 
     /* This test inspects and modifies the connection object after unlocking
@@ -1348,7 +1375,8 @@ ble_sm_test_util_us_bonding_good(int send_enc_req, uint8_t *our_rpa,
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     ble_hs_test_util_set_ack(
-        host_hci_opcode_join(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_START_ENCRYPT),
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_START_ENCRYPT),
         0);
 
     if (send_enc_req) {
@@ -1376,7 +1404,7 @@ ble_sm_test_util_us_bonding_good(int send_enc_req, uint8_t *our_rpa,
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
     TEST_ASSERT(ble_sm_test_gap_status == 0);
     TEST_ASSERT(ble_sm_test_sec_state.encrypted);
     TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
@@ -1401,7 +1429,7 @@ ble_sm_test_util_peer_fail_inval(
     struct ble_hs_conn *conn;
 
     ble_sm_test_util_init();
-    ble_hs_test_util_set_public_addr(resp_addr);
+    ble_hs_id_set_pub(resp_addr);
 
     ble_hs_test_util_create_conn(2, init_id_addr, ble_sm_test_util_conn_cb,
                                  NULL);
@@ -1435,7 +1463,7 @@ ble_sm_test_util_peer_fail_inval(
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was not executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == -1);
+    TEST_ASSERT(ble_sm_test_gap_event_type == -1);
     TEST_ASSERT(ble_sm_test_gap_status == -1);
 
     /* Verify that connection has correct security state. */
@@ -1458,7 +1486,7 @@ ble_sm_test_util_peer_lgcy_fail_confirm(
     struct ble_hs_conn *conn;
 
     ble_sm_test_util_init();
-    ble_hs_test_util_set_public_addr(resp_addr);
+    ble_hs_id_set_pub(resp_addr);
     ble_sm_dbg_set_next_pair_rand(random_rsp->value);
 
     ble_hs_test_util_create_conn(2, init_id_addr, ble_sm_test_util_conn_cb,
@@ -1512,7 +1540,7 @@ ble_sm_test_util_peer_lgcy_fail_confirm(
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
     TEST_ASSERT(ble_sm_test_gap_status ==
                 BLE_HS_SM_US_ERR(BLE_SM_ERR_CONFIRM_MISMATCH));
     TEST_ASSERT(!ble_sm_test_sec_state.encrypted);
@@ -1548,7 +1576,8 @@ ble_sm_test_util_bonding_all(struct ble_sm_test_params *params,
 
     if (sc || peer_entity.key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
         /* We are master; we initiate procedure. */
-        ble_sm_test_util_us_bonding_good(0, our_entity.rpa,
+        ble_sm_test_util_us_bonding_good(0, our_entity.addr_type,
+                                         our_entity.rpa,
                                          peer_entity.addr_type,
                                          peer_entity.id_addr,
                                          peer_entity.rpa,
@@ -1558,7 +1587,8 @@ ble_sm_test_util_bonding_all(struct ble_sm_test_params *params,
                                          peer_entity.rand_num);
 
         /* We are master; peer initiates procedure via security request. */
-        ble_sm_test_util_us_bonding_good(1, our_entity.rpa,
+        ble_sm_test_util_us_bonding_good(1, our_entity.addr_type,
+                                         our_entity.rpa,
                                          peer_entity.addr_type,
                                          peer_entity.id_addr,
                                          peer_entity.rpa,
@@ -1570,7 +1600,8 @@ ble_sm_test_util_bonding_all(struct ble_sm_test_params *params,
 
     if (sc || our_entity.key_dist & BLE_SM_PAIR_KEY_DIST_ENC) {
         /* Peer is master; peer initiates procedure. */
-        ble_sm_test_util_peer_bonding_good(0, our_entity.rpa,
+        ble_sm_test_util_peer_bonding_good(0, our_entity.addr_type,
+                                           our_entity.rpa,
                                            peer_entity.addr_type,
                                            peer_entity.id_addr,
                                            peer_entity.rpa,
@@ -1580,7 +1611,8 @@ ble_sm_test_util_bonding_all(struct ble_sm_test_params *params,
                                            our_entity.rand_num);
 
         /* Peer is master; we initiate procedure via security request. */
-        ble_sm_test_util_peer_bonding_good(1, our_entity.rpa,
+        ble_sm_test_util_peer_bonding_good(1, our_entity.addr_type,
+                                           our_entity.rpa,
                                            peer_entity.addr_type,
                                            peer_entity.id_addr,
                                            peer_entity.rpa,
@@ -1629,8 +1661,8 @@ ble_sm_test_util_rx_keys(struct ble_sm_test_params *params,
     }
     if (peer_key_dist & BLE_SM_PAIR_KEY_DIST_ID) {
         ble_hs_test_util_set_ack(
-            host_hci_opcode_join(BLE_HCI_OGF_LE,
-                                 BLE_HCI_OCF_LE_ADD_RESOLV_LIST), 0);
+            ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_ADD_RESOLV_LIST), 0);
         ble_sm_test_util_rx_id_info(2, peer_id_info, 0);
         ble_sm_test_util_rx_id_addr_info(2, peer_id_addr_info, 0);
     }
@@ -1697,7 +1729,8 @@ ble_sm_test_util_us_lgcy_good_once(struct ble_sm_test_params *params)
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     ble_hs_test_util_set_ack(
-        host_hci_opcode_join(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_START_ENCRYPT), 0);
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_START_ENCRYPT), 0);
     if (params->sec_req.authreq != 0) {
         ble_sm_test_util_rx_sec_req(2, &params->sec_req, 0);
     } else {
@@ -1768,7 +1801,7 @@ ble_sm_test_util_us_lgcy_good_once(struct ble_sm_test_params *params)
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
     TEST_ASSERT(ble_sm_test_gap_status == 0);
     TEST_ASSERT(ble_sm_test_sec_state.encrypted);
     TEST_ASSERT(ble_sm_test_sec_state.authenticated == params->authenticated);
@@ -1894,7 +1927,7 @@ ble_sm_test_util_peer_lgcy_good_once(struct ble_sm_test_params *params)
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
     TEST_ASSERT(ble_sm_test_gap_status == 0);
     TEST_ASSERT(ble_sm_test_sec_state.encrypted);
     TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
@@ -1957,7 +1990,8 @@ ble_sm_test_util_us_sc_good_once(struct ble_sm_test_params *params)
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     ble_hs_test_util_set_ack(
-        host_hci_opcode_join(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_START_ENCRYPT), 0);
+        ble_hs_hci_util_opcode_join(BLE_HCI_OGF_LE,
+                                    BLE_HCI_OCF_LE_START_ENCRYPT), 0);
     if (params->sec_req.authreq != 0) {
         ble_sm_test_util_rx_sec_req(2, &params->sec_req, 0);
     } else {
@@ -2079,7 +2113,7 @@ ble_sm_test_util_us_sc_good_once(struct ble_sm_test_params *params)
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
     TEST_ASSERT(ble_sm_test_gap_status == 0);
     TEST_ASSERT(ble_sm_test_sec_state.encrypted);
     TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
@@ -2267,7 +2301,7 @@ ble_sm_test_util_peer_sc_good_once(struct ble_sm_test_params *params)
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == BLE_GAP_EVENT_ENC_CHANGE);
+    TEST_ASSERT(ble_sm_test_gap_event_type == BLE_GAP_EVENT_ENC_CHANGE);
     TEST_ASSERT(ble_sm_test_gap_status == 0);
     TEST_ASSERT(ble_sm_test_sec_state.encrypted);
     TEST_ASSERT(ble_sm_test_sec_state.authenticated ==
@@ -2321,7 +2355,7 @@ ble_sm_test_util_us_fail_inval(struct ble_sm_test_params *params)
     int rc;
 
     ble_sm_test_util_init();
-    ble_hs_test_util_set_public_addr(params->resp_id_addr);
+    ble_hs_id_set_pub(params->resp_id_addr);
 
     ble_sm_dbg_set_next_pair_rand(((uint8_t[16]){0}));
 
@@ -2365,7 +2399,7 @@ ble_sm_test_util_us_fail_inval(struct ble_sm_test_params *params)
     TEST_ASSERT(ble_sm_dbg_num_procs() == 0);
 
     /* Verify that security callback was not executed. */
-    TEST_ASSERT(ble_sm_test_gap_event == -1);
+    TEST_ASSERT(ble_sm_test_gap_event_type == -1);
     TEST_ASSERT(ble_sm_test_gap_status == -1);
 
     /* Verify that connection has correct security state. */
diff --git a/net/nimble/host/src/test/ble_sm_test_util.h b/net/nimble/host/src/test/ble_sm_test_util.h
index 3b0688b3..3323be69 100644
--- a/net/nimble/host/src/test/ble_sm_test_util.h
+++ b/net/nimble/host/src/test/ble_sm_test_util.h
@@ -78,8 +78,7 @@ extern union ble_store_key ble_sm_test_store_key;
 extern union ble_store_value ble_sm_test_store_value;
 
 void ble_sm_test_util_init(void);
-int ble_sm_test_util_conn_cb(int event, struct ble_gap_conn_ctxt *ctxt,
-                             void *arg);
+int ble_sm_test_util_conn_cb(struct ble_gap_event *ctxt, void *arg);
 void ble_sm_test_util_io_inject(struct ble_sm_test_passkey_info *passkey_info,
                                 uint8_t cur_sm_state);
 void ble_sm_test_util_io_inject_bad(uint16_t conn_handle,
diff --git a/net/nimble/host/src/test/ble_uuid_test.c b/net/nimble/host/src/test/ble_uuid_test.c
index ecb3505f..10113034 100644
--- a/net/nimble/host/src/test/ble_uuid_test.c
+++ b/net/nimble/host/src/test/ble_uuid_test.c
@@ -19,9 +19,10 @@
 
 #include <stddef.h>
 #include <string.h>
-#include "host/ble_hs_test.h"
 #include "testutil/testutil.h"
+#include "host/ble_hs_test.h"
 #include "host/ble_uuid.h"
+#include "ble_hs_test_util.h"
 
 TEST_CASE(ble_uuid_test_128_to_16)
 {
@@ -78,6 +79,8 @@ TEST_CASE(ble_uuid_test_128_to_16)
 
 TEST_SUITE(ble_uuid_test_suite)
 {
+    tu_suite_set_post_test_cb(ble_hs_test_util_post_test, NULL);
+
     ble_uuid_test_128_to_16();
 }
 
diff --git a/net/nimble/host/store/ram/include/store/ram/ble_store_ram.h b/net/nimble/host/store/ram/include/store/ram/ble_store_ram.h
new file mode 100644
index 00000000..b9529c5c
--- /dev/null
+++ b/net/nimble/host/store/ram/include/store/ram/ble_store_ram.h
@@ -0,0 +1,30 @@
+/**
+ * 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 H_BLE_STORE_RAM_
+#define H_BLE_STORE_RAM_
+
+union ble_store_key;
+union ble_store_value;
+
+int ble_store_ram_read(int obj_type, union ble_store_key *key,
+                       union ble_store_value *value);
+int ble_store_ram_write(int obj_type, union ble_store_value *val);
+
+#endif
diff --git a/net/nimble/host/store/ram/pkg.yml b/net/nimble/host/store/ram/pkg.yml
new file mode 100644
index 00000000..f43e7a55
--- /dev/null
+++ b/net/nimble/host/store/ram/pkg.yml
@@ -0,0 +1,31 @@
+#
+# 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: net/nimble/host/store/ram
+pkg.description: RAM-based persistence layer for the NimBLE host.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+    - nimble
+    - persistence
+
+pkg.deps:
+    - net/nimble/host
diff --git a/apps/bleprph/src/store.c b/net/nimble/host/store/ram/src/ble_store_ram.c
index 5b3ae133..7528f03a 100644
--- a/apps/bleprph/src/store.c
+++ b/net/nimble/host/store/ram/src/ble_store_ram.c
@@ -6,7 +6,7 @@
  * 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,
@@ -18,81 +18,76 @@
  */
 
 /**
- * This file implements a simple in-RAM key database for long-term keys.  A key
- * is inserted into the database immediately after a successful pairing
- * procedure.  A key is retrieved from the database when the central performs
- * the encryption procedure (bonding).
- *
- * As this database is only stored in RAM, its contents are lost if bleprph is
- * restarted.
+ * This file implements a simple in-RAM key database for BLE host security
+ * material and CCCDs.  As this database is only ble_store_ramd in RAM, its
+ * contents are lost when the application terminates.
  */
 
 #include <inttypes.h>
 #include <string.h>
 
-#include "console/console.h"
 #include "host/ble_hs.h"
+#include "store/ram/ble_store_ram.h"
 
-#include "bleprph.h"
-
+/* XXX: This should be configurable. */
 #define STORE_MAX_SLV_LTKS   4
 #define STORE_MAX_MST_LTKS   4
 #define STORE_MAX_CCCDS      16
 
-static struct ble_store_value_sec store_our_secs[STORE_MAX_SLV_LTKS];
-static int store_num_our_secs;
+static struct ble_store_value_sec ble_store_ram_our_secs[STORE_MAX_SLV_LTKS];
+static int ble_store_ram_num_our_secs;
 
-static struct ble_store_value_sec store_peer_secs[STORE_MAX_MST_LTKS];
-static int store_num_peer_secs;
+static struct ble_store_value_sec ble_store_ram_peer_secs[STORE_MAX_MST_LTKS];
+static int ble_store_ram_num_peer_secs;
 
-static struct ble_store_value_cccd store_cccds[STORE_MAX_CCCDS];
-static int store_num_cccds;
+static struct ble_store_value_cccd ble_store_ram_cccds[STORE_MAX_CCCDS];
+static int ble_store_ram_num_cccds;
 
 /*****************************************************************************
  * $sec                                                                      *
  *****************************************************************************/
 
 static void
-store_print_value_sec(struct ble_store_value_sec *sec)
+ble_store_ram_print_value_sec(struct ble_store_value_sec *sec)
 {
     if (sec->ltk_present) {
-        BLEPRPH_LOG(INFO, "ediv=%u rand=%llu authenticated=%d ltk=",
+        BLE_HS_LOG(DEBUG, "ediv=%u rand=%llu authenticated=%d ltk=",
                        sec->ediv, sec->rand_num, sec->authenticated);
-        print_bytes(sec->ltk, 16);
-        BLEPRPH_LOG(INFO, " ");
+        ble_hs_log_flat_buf(sec->ltk, 16);
+        BLE_HS_LOG(DEBUG, " ");
     }
     if (sec->irk_present) {
-        BLEPRPH_LOG(INFO, "irk=");
-        print_bytes(sec->irk, 16);
-        BLEPRPH_LOG(INFO, " ");
+        BLE_HS_LOG(DEBUG, "irk=");
+        ble_hs_log_flat_buf(sec->irk, 16);
+        BLE_HS_LOG(DEBUG, " ");
     }
     if (sec->csrk_present) {
-        BLEPRPH_LOG(INFO, "csrk=");
-        print_bytes(sec->csrk, 16);
-        BLEPRPH_LOG(INFO, " ");
+        BLE_HS_LOG(DEBUG, "csrk=");
+        ble_hs_log_flat_buf(sec->csrk, 16);
+        BLE_HS_LOG(DEBUG, " ");
     }
 
-    BLEPRPH_LOG(INFO, "\n");
+    BLE_HS_LOG(DEBUG, "\n");
 }
 
 static void
-store_print_key_sec(struct ble_store_key_sec *key_sec)
+ble_store_ram_print_key_sec(struct ble_store_key_sec *key_sec)
 {
     if (key_sec->peer_addr_type != BLE_STORE_ADDR_TYPE_NONE) {
-        BLEPRPH_LOG(INFO, "peer_addr_type=%d peer_addr=",
+        BLE_HS_LOG(DEBUG, "peer_addr_type=%d peer_addr=",
                        key_sec->peer_addr_type);
-        print_bytes(key_sec->peer_addr, 6);
-        BLEPRPH_LOG(INFO, " ");
+        ble_hs_log_flat_buf(key_sec->peer_addr, 6);
+        BLE_HS_LOG(DEBUG, " ");
     }
     if (key_sec->ediv_rand_present) {
-        BLEPRPH_LOG(INFO, "ediv=0x%02x rand=0x%llx ",
+        BLE_HS_LOG(DEBUG, "ediv=0x%02x rand=0x%llx ",
                        key_sec->ediv, key_sec->rand_num);
     }
 }
 
 static int
-store_find_sec(struct ble_store_key_sec *key_sec,
-               struct ble_store_value_sec *value_secs, int num_value_secs)
+ble_store_ram_find_sec(struct ble_store_key_sec *key_sec,
+                   struct ble_store_value_sec *value_secs, int num_value_secs)
 {
     struct ble_store_value_sec *cur;
     int skipped;
@@ -136,84 +131,88 @@ store_find_sec(struct ble_store_key_sec *key_sec,
 }
 
 static int
-store_read_our_sec(struct ble_store_key_sec *key_sec,
-                   struct ble_store_value_sec *value_sec)
+ble_store_ram_read_our_sec(struct ble_store_key_sec *key_sec,
+                       struct ble_store_value_sec *value_sec)
 {
     int idx;
 
-    idx = store_find_sec(key_sec, store_our_secs, store_num_our_secs);
+    idx = ble_store_ram_find_sec(key_sec, ble_store_ram_our_secs,
+                                 ble_store_ram_num_our_secs);
     if (idx == -1) {
         return BLE_HS_ENOENT;
     }
 
-    *value_sec = store_our_secs[idx];
+    *value_sec = ble_store_ram_our_secs[idx];
     return 0;
 }
 
 static int
-store_write_our_sec(struct ble_store_value_sec *value_sec)
+ble_store_ram_write_our_sec(struct ble_store_value_sec *value_sec)
 {
     struct ble_store_key_sec key_sec;
     int idx;
 
-    BLEPRPH_LOG(INFO, "persisting our sec; ");
-    store_print_value_sec(value_sec);
+    BLE_HS_LOG(DEBUG, "persisting our sec; ");
+    ble_store_ram_print_value_sec(value_sec);
 
     ble_store_key_from_value_sec(&key_sec, value_sec);
-    idx = store_find_sec(&key_sec, store_our_secs, store_num_our_secs);
+    idx = ble_store_ram_find_sec(&key_sec, ble_store_ram_our_secs,
+                             ble_store_ram_num_our_secs);
     if (idx == -1) {
-        if (store_num_our_secs >= STORE_MAX_SLV_LTKS) {
-            BLEPRPH_LOG(INFO, "error persisting our sec; too many entries "
-                              "(%d)\n", store_num_our_secs);
+        if (ble_store_ram_num_our_secs >= STORE_MAX_SLV_LTKS) {
+            BLE_HS_LOG(DEBUG, "error persisting our sec; too many entries "
+                              "(%d)\n", ble_store_ram_num_our_secs);
             return BLE_HS_ENOMEM;
         }
 
-        idx = store_num_our_secs;
-        store_num_our_secs++;
+        idx = ble_store_ram_num_our_secs;
+        ble_store_ram_num_our_secs++;
     }
 
-    store_our_secs[idx] = *value_sec;
+    ble_store_ram_our_secs[idx] = *value_sec;
     return 0;
 }
 
 static int
-store_read_peer_sec(struct ble_store_key_sec *key_sec,
-                   struct ble_store_value_sec *value_sec)
+ble_store_ram_read_peer_sec(struct ble_store_key_sec *key_sec,
+                        struct ble_store_value_sec *value_sec)
 {
     int idx;
 
-    idx = store_find_sec(key_sec, store_peer_secs, store_num_peer_secs);
+    idx = ble_store_ram_find_sec(key_sec, ble_store_ram_peer_secs,
+                             ble_store_ram_num_peer_secs);
     if (idx == -1) {
         return BLE_HS_ENOENT;
     }
 
-    *value_sec = store_peer_secs[idx];
+    *value_sec = ble_store_ram_peer_secs[idx];
     return 0;
 }
 
 static int
-store_write_peer_sec(struct ble_store_value_sec *value_sec)
+ble_store_ram_write_peer_sec(struct ble_store_value_sec *value_sec)
 {
     struct ble_store_key_sec key_sec;
     int idx;
 
-    BLEPRPH_LOG(INFO, "persisting peer sec; ");
-    store_print_value_sec(value_sec);
+    BLE_HS_LOG(DEBUG, "persisting peer sec; ");
+    ble_store_ram_print_value_sec(value_sec);
 
     ble_store_key_from_value_sec(&key_sec, value_sec);
-    idx = store_find_sec(&key_sec, store_peer_secs, store_num_peer_secs);
+    idx = ble_store_ram_find_sec(&key_sec, ble_store_ram_peer_secs,
+                             ble_store_ram_num_peer_secs);
     if (idx == -1) {
-        if (store_num_peer_secs >= STORE_MAX_MST_LTKS) {
-            BLEPRPH_LOG(INFO, "error persisting peer sec; too many entries "
-                           "(%d)\n", store_num_peer_secs);
+        if (ble_store_ram_num_peer_secs >= STORE_MAX_MST_LTKS) {
+            BLE_HS_LOG(DEBUG, "error persisting peer sec; too many entries "
+                             "(%d)\n", ble_store_ram_num_peer_secs);
             return BLE_HS_ENOMEM;
         }
 
-        idx = store_num_peer_secs;
-        store_num_peer_secs++;
+        idx = ble_store_ram_num_peer_secs;
+        ble_store_ram_num_peer_secs++;
     }
 
-    store_peer_secs[idx] = *value_sec;
+    ble_store_ram_peer_secs[idx] = *value_sec;
     return 0;
 }
 
@@ -222,15 +221,15 @@ store_write_peer_sec(struct ble_store_value_sec *value_sec)
  *****************************************************************************/
 
 static int
-store_find_cccd(struct ble_store_key_cccd *key)
+ble_store_ram_find_cccd(struct ble_store_key_cccd *key)
 {
     struct ble_store_value_cccd *cccd;
     int skipped;
     int i;
 
     skipped = 0;
-    for (i = 0; i < store_num_cccds; i++) {
-        cccd = store_cccds + i;
+    for (i = 0; i < ble_store_ram_num_cccds; i++) {
+        cccd = ble_store_ram_cccds + i;
 
         if (key->peer_addr_type != BLE_STORE_ADDR_TYPE_NONE) {
             if (cccd->peer_addr_type != key->peer_addr_type) {
@@ -260,40 +259,40 @@ store_find_cccd(struct ble_store_key_cccd *key)
 }
 
 static int
-store_read_cccd(struct ble_store_key_cccd *key_cccd,
+ble_store_ram_read_cccd(struct ble_store_key_cccd *key_cccd,
                 struct ble_store_value_cccd *value_cccd)
 {
     int idx;
 
-    idx = store_find_cccd(key_cccd);
+    idx = ble_store_ram_find_cccd(key_cccd);
     if (idx == -1) {
         return BLE_HS_ENOENT;
     }
 
-    *value_cccd = store_cccds[idx];
+    *value_cccd = ble_store_ram_cccds[idx];
     return 0;
 }
 
 static int
-store_write_cccd(struct ble_store_value_cccd *value_cccd)
+ble_store_ram_write_cccd(struct ble_store_value_cccd *value_cccd)
 {
     struct ble_store_key_cccd key_cccd;
     int idx;
 
     ble_store_key_from_value_cccd(&key_cccd, value_cccd);
-    idx = store_find_cccd(&key_cccd);
+    idx = ble_store_ram_find_cccd(&key_cccd);
     if (idx == -1) {
-        if (store_num_cccds >= STORE_MAX_SLV_LTKS) {
-            BLEPRPH_LOG(INFO, "error persisting cccd; too many entries (%d)\n",
-                        store_num_cccds);
+        if (ble_store_ram_num_cccds >= STORE_MAX_SLV_LTKS) {
+            BLE_HS_LOG(DEBUG, "error persisting cccd; too many entries (%d)\n",
+                       ble_store_ram_num_cccds);
             return BLE_HS_ENOMEM;
         }
 
-        idx = store_num_cccds;
-        store_num_cccds++;
+        idx = ble_store_ram_num_cccds;
+        ble_store_ram_num_cccds++;
     }
 
-    store_cccds[idx] = *value_cccd;
+    ble_store_ram_cccds[idx] = *value_cccd;
     return 0;
 }
 
@@ -307,8 +306,8 @@ store_write_cccd(struct ble_store_value_cccd *value_cccd)
  * @return                      0 if a key was found; else BLE_HS_ENOENT.
  */
 int
-store_read(int obj_type, union ble_store_key *key,
-           union ble_store_value *value)
+ble_store_ram_read(int obj_type, union ble_store_key *key,
+               union ble_store_value *value)
 {
     int rc;
 
@@ -322,21 +321,21 @@ store_read(int obj_type, union ble_store_key *key,
          * result.  The nimble stack will use this key if this function returns
          * success.
          */
-        BLEPRPH_LOG(INFO, "looking up peer sec; ");
-        store_print_key_sec(&key->sec);
-        BLEPRPH_LOG(INFO, "\n");
-        rc = store_read_peer_sec(&key->sec, &value->sec);
+        BLE_HS_LOG(DEBUG, "looking up peer sec; ");
+        ble_store_ram_print_key_sec(&key->sec);
+        BLE_HS_LOG(DEBUG, "\n");
+        rc = ble_store_ram_read_peer_sec(&key->sec, &value->sec);
         return rc;
 
     case BLE_STORE_OBJ_TYPE_OUR_SEC:
-        BLEPRPH_LOG(INFO, "looking up our sec; ");
-        store_print_key_sec(&key->sec);
-        BLEPRPH_LOG(INFO, "\n");
-        rc = store_read_our_sec(&key->sec, &value->sec);
+        BLE_HS_LOG(DEBUG, "looking up our sec; ");
+        ble_store_ram_print_key_sec(&key->sec);
+        BLE_HS_LOG(DEBUG, "\n");
+        rc = ble_store_ram_read_our_sec(&key->sec, &value->sec);
         return rc;
 
     case BLE_STORE_OBJ_TYPE_CCCD:
-        rc = store_read_cccd(&key->cccd, &value->cccd);
+        rc = ble_store_ram_read_cccd(&key->cccd, &value->cccd);
         return rc;
 
     default:
@@ -351,21 +350,21 @@ store_read(int obj_type, union ble_store_key *key,
  *                                  full.
  */
 int
-store_write(int obj_type, union ble_store_value *val)
+ble_store_ram_write(int obj_type, union ble_store_value *val)
 {
     int rc;
 
     switch (obj_type) {
     case BLE_STORE_OBJ_TYPE_PEER_SEC:
-        rc = store_write_peer_sec(&val->sec);
+        rc = ble_store_ram_write_peer_sec(&val->sec);
         return rc;
 
     case BLE_STORE_OBJ_TYPE_OUR_SEC:
-        rc = store_write_our_sec(&val->sec);
+        rc = ble_store_ram_write_our_sec(&val->sec);
         return rc;
 
     case BLE_STORE_OBJ_TYPE_CCCD:
-        rc = store_write_cccd(&val->cccd);
+        rc = ble_store_ram_write_cccd(&val->cccd);
         return rc;
 
     default:
diff --git a/net/nimble/include/nimble/ble.h b/net/nimble/include/nimble/ble.h
index 04ea21cb..6c4c90ee 100644
--- a/net/nimble/include/nimble/ble.h
+++ b/net/nimble/include/nimble/ble.h
@@ -34,10 +34,6 @@ struct ble_encryption_block
     uint8_t     cipher_text[BLE_ENC_BLOCK_SIZE];
 };
 
-/* Shared command pool for transort between host and controller */
-extern struct os_mempool g_hci_cmd_pool;
-extern struct os_mempool g_hci_os_event_pool;
-
 /*
  * BLE MBUF structure:
  *
@@ -140,17 +136,17 @@ extern uint8_t g_random_addr[BLE_DEV_ADDR_LEN];
 void htole16(void *buf, uint16_t x);
 void htole32(void *buf, uint32_t x);
 void htole64(void *buf, uint64_t x);
-uint16_t le16toh(void *buf);
-uint32_t le32toh(void *buf);
-uint64_t le64toh(void *buf);
+uint16_t le16toh(const void *buf);
+uint32_t le32toh(const void *buf);
+uint64_t le64toh(const void *buf);
 void htobe16(void *buf, uint16_t x);
 void htobe32(void *buf, uint32_t x);
 void htobe64(void *buf, uint64_t x);
-uint16_t be16toh(void *buf);
-uint32_t be32toh(void *buf);
-uint64_t be64toh(void *buf);
+uint16_t be16toh(const void *buf);
+uint32_t be32toh(const void *buf);
+uint64_t be64toh(const void *buf);
 void swap_in_place(void *buf, int len);
-void swap_buf(uint8_t *dst, uint8_t *src, int len);
+void swap_buf(uint8_t *dst, const uint8_t *src, int len);
 /* XXX */
 
 /* BLE Error Codes (Core v4.2 Vol 2 part D) */
@@ -222,7 +218,6 @@ enum ble_error_codes
     BLE_ERR_CONN_ESTABLISHMENT  = 62,
     BLE_ERR_MAC_CONN_FAIL       = 63,
     BLE_ERR_COARSE_CLK_ADJ      = 64,
-    BLE_ERR_ATTR_NOT_FOUND      = 65,
     BLE_ERR_MAX                 = 255
 };
 
@@ -232,4 +227,6 @@ enum ble_error_codes
 #define BLE_ADDR_TYPE_RPA_PUB_DEFAULT   (2)
 #define BLE_ADDR_TYPE_RPA_RND_DEFAULT   (3)
 
+int ble_err_from_os(int os_err);
+
 #endif /* H_BLE_ */
diff --git a/net/nimble/include/nimble/ble_hci_trans.h b/net/nimble/include/nimble/ble_hci_trans.h
new file mode 100644
index 00000000..3e2ec7a8
--- /dev/null
+++ b/net/nimble/include/nimble/ble_hci_trans.h
@@ -0,0 +1,169 @@
+/**
+ * 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 H_HCI_TRANSPORT_
+#define H_HCI_TRANSPORT_
+
+#include <inttypes.h>
+struct os_mbuf;
+
+#define BLE_HCI_TRANS_CMD_SZ        260
+
+/*** Type of buffers for holding commands and events. */
+/**
+ * Controller-to-host event buffers.  Events have one of two priorities:
+ * o Low-priority   (BLE_HCI_TRANS_BUF_EVT_LO)
+ * o High-priority  (BLE_HCI_TRANS_BUF_EVT_HI)
+ *
+ * Low-priority event buffers are only used for advertising reports.  If there
+ * are no free low-priority event buffers, then an incoming advertising report
+ * will get dropped.
+ *
+ * High-priority event buffers are for everything except advertising reports.
+ * If there are no free high-priority event buffers, a request to allocate one
+ * will try to allocate a low-priority buffer instead.
+ *
+ * If you want all events to be given equal treatment, then you should allocate
+ * low-priority events only.
+ *
+ * Event priorities solve the problem of critical events getting dropped due to
+ * a flood of advertising reports.  This solution is likely temporary: when
+ * HCI flow control is added, event priorities may become obsolete.
+ *
+ * Not all transports distinguish between low and high priority events.  If the
+ * transport does not have separate settings for low and high buffer counts,
+ * then it treats all events with equal priority.
+ */
+#define BLE_HCI_TRANS_BUF_EVT_LO    1
+#define BLE_HCI_TRANS_BUF_EVT_HI    2
+
+/* Host-to-controller command. */
+#define BLE_HCI_TRANS_BUF_CMD       3
+
+/** Callback function types; executed when HCI packets are received. */
+typedef int ble_hci_trans_rx_cmd_fn(uint8_t *cmd, void *arg);
+typedef int ble_hci_trans_rx_acl_fn(struct os_mbuf *om, void *arg);
+
+/**
+ * Sends an HCI event from the controller to the host.
+ *
+ * @param cmd                   The HCI event to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_ll_evt_tx(uint8_t *hci_ev);
+
+/**
+ * Sends ACL data from controller to host.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_ll_acl_tx(struct os_mbuf *om);
+
+/**
+ * Sends an HCI command from the host to the controller.
+ *
+ * @param cmd                   The HCI command to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_hs_cmd_tx(uint8_t *cmd);
+
+/**
+ * Sends ACL data from host to controller.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_hs_acl_tx(struct os_mbuf *om);
+
+/**
+ * Allocates a flat buffer of the specified type.
+ *
+ * @param type                  The type of buffer to allocate; one of the
+ *                                  BLE_HCI_TRANS_BUF_[...] constants.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+uint8_t *ble_hci_trans_buf_alloc(int type);
+
+/**
+ * Frees the specified flat buffer.  The buffer must have been allocated via
+ * ble_hci_trans_buf_alloc().
+ *
+ * @param buf                   The buffer to free.
+ */
+void ble_hci_trans_buf_free(uint8_t *buf);
+
+/**
+ * Configures the HCI transport to operate with a controller.  The transport
+ * will execute specified callbacks upon receiving HCI packets from the host.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  command.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void ble_hci_trans_cfg_ll(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                          void *cmd_arg,
+                          ble_hci_trans_rx_acl_fn *acl_cb,
+                          void *acl_arg);
+
+/**
+ * Configures the HCI transport to operate with a host.  The transport will
+ * execute specified callbacks upon receiving HCI packets from the controller.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                          void *cmd_arg,
+                          ble_hci_trans_rx_acl_fn *acl_cb,
+                          void *acl_arg);
+
+/**
+ * Resets the HCI module to a clean state.  Frees all buffers and reinitializes
+ * the underlying transport.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int ble_hci_trans_reset(void);
+
+#endif /* H_HCI_TRANSPORT_ */
diff --git a/net/nimble/include/nimble/hci_common.h b/net/nimble/include/nimble/hci_common.h
index 0292be52..f094cc7d 100644
--- a/net/nimble/include/nimble/hci_common.h
+++ b/net/nimble/include/nimble/hci_common.h
@@ -131,6 +131,9 @@
 /* --- Set event mask (OGF 0x03, OCF 0x0001 --- */
 #define BLE_HCI_SET_EVENT_MASK_LEN          (8)
 
+/* --- Read BD_ADDR (OGF 0x04, OCF 0x0009 --- */
+#define BLE_HCI_IP_RD_BD_ADDR_ACK_PARAM_LEN (6)
+
 /* --- Read/Write authenticated payload timeout (ocf 0x007B/0x007C) */
 #define BLE_HCI_RD_AUTH_PYLD_TMO_LEN        (4)
 #define BLE_HCI_WR_AUTH_PYLD_TMO_LEN        (2)
@@ -477,6 +480,9 @@
 /* Event encryption change (code=0x08) */
 #define BLE_HCI_EVENT_ENCRYPT_CHG_LEN       (4)
 
+/* Event hardware error (code=0x10) */
+#define BLE_HCI_EVENT_HW_ERROR_LEN          (1)
+
 /* Event key refresh complete (code=0x30) */
 #define BLE_HCI_EVENT_ENC_KEY_REFRESH_LEN   (3)
 
@@ -515,6 +521,9 @@
 #define BLE_HCI_LE_ADV_RPT_NUM_RPTS_MIN     (1)
 #define BLE_HCI_LE_ADV_RPT_NUM_RPTS_MAX     (0x19)
 
+/* Length of each record in an LE direct advertising report event. */
+#define BLE_HCI_LE_ADV_DIRECT_RPT_SUB_LEN   (16)
+
 /* LE connection update complete event (sub event 0x03) */
 #define BLE_HCI_LE_CONN_UPD_LEN             (10)
 
diff --git a/net/nimble/include/nimble/nimble_opt.h b/net/nimble/include/nimble/nimble_opt.h
index f8b08ccb..c82bcb5a 100644
--- a/net/nimble/include/nimble/nimble_opt.h
+++ b/net/nimble/include/nimble/nimble_opt.h
@@ -64,66 +64,70 @@
 #define NIMBLE_OPT_SM_SC                        0
 #endif
 
-/** HOST: Supported GATT procedures.  By default, all are enabled. */
+/**
+ * HOST: Supported GATT procedures.  By default:
+ *     o Notify and indicate are enabled;
+ *     o All other procedures are enabled for centrals.
+ */
 
 #ifndef NIMBLE_OPT_GATT_DISC_ALL_SVCS
-#define NIMBLE_OPT_GATT_DISC_ALL_SVCS           1
+#define NIMBLE_OPT_GATT_DISC_ALL_SVCS           NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_DISC_SVC_UUID
-#define NIMBLE_OPT_GATT_DISC_SVC_UUID           1
+#define NIMBLE_OPT_GATT_DISC_SVC_UUID           NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_FIND_INC_SVCS
-#define NIMBLE_OPT_GATT_FIND_INC_SVCS           1
+#define NIMBLE_OPT_GATT_FIND_INC_SVCS           NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_DISC_ALL_CHRS
-#define NIMBLE_OPT_GATT_DISC_ALL_CHRS           1
+#define NIMBLE_OPT_GATT_DISC_ALL_CHRS           NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_DISC_CHR_UUID
-#define NIMBLE_OPT_GATT_DISC_CHR_UUID           1
+#define NIMBLE_OPT_GATT_DISC_CHR_UUID           NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_DISC_ALL_DSCS
-#define NIMBLE_OPT_GATT_DISC_ALL_DSCS           1
+#define NIMBLE_OPT_GATT_DISC_ALL_DSCS           NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_READ
-#define NIMBLE_OPT_GATT_READ                    1
+#define NIMBLE_OPT_GATT_READ                    NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_READ_UUID
-#define NIMBLE_OPT_GATT_READ_UUID               1
+#define NIMBLE_OPT_GATT_READ_UUID               NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_READ_LONG
-#define NIMBLE_OPT_GATT_READ_LONG               1
+#define NIMBLE_OPT_GATT_READ_LONG               NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_READ_MULT
-#define NIMBLE_OPT_GATT_READ_MULT               1
+#define NIMBLE_OPT_GATT_READ_MULT               NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_WRITE_NO_RSP
-#define NIMBLE_OPT_GATT_WRITE_NO_RSP            1
+#define NIMBLE_OPT_GATT_WRITE_NO_RSP            NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_SIGNED_WRITE
-#define NIMBLE_OPT_GATT_SIGNED_WRITE            1
+#define NIMBLE_OPT_GATT_SIGNED_WRITE            NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_WRITE
-#define NIMBLE_OPT_GATT_WRITE                   1
+#define NIMBLE_OPT_GATT_WRITE                   NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_WRITE_LONG
-#define NIMBLE_OPT_GATT_WRITE_LONG              1
+#define NIMBLE_OPT_GATT_WRITE_LONG              NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_WRITE_RELIABLE
-#define NIMBLE_OPT_GATT_WRITE_RELIABLE          1
+#define NIMBLE_OPT_GATT_WRITE_RELIABLE          NIMBLE_OPT_ROLE_CENTRAL
 #endif
 
 #ifndef NIMBLE_OPT_GATT_NOTIFY
@@ -134,6 +138,14 @@
 #define NIMBLE_OPT_GATT_INDICATE                1
 #endif
 
+/** HOST: GATT options. */
+
+/* The maximum number of attributes that can be written with a single GATT
+ * Reliable Write procedure.
+ */
+#ifndef NIMBLE_OPT_GATT_WRITE_MAX_ATTRS
+#define NIMBLE_OPT_GATT_WRITE_MAX_ATTRS         4
+#endif
 
 /** HOST: Supported server ATT commands. */
 
@@ -343,13 +355,16 @@
 #endif
 
 /*
- * This option allows a controller to send/receive LE pings. Currently,
- * this feature is not implemented by the controller so turning it on or off
- * has no effect.
+ * This option allows a controller to send/receive LE pings.
  */
+#if (BLE_LL_CFG_FEAT_LE_ENCRYPTION == 0)
+#undef BLE_LL_CFG_FEAT_LE_PING
+#define BLE_LL_CFG_FEAT_LE_PING                 (0)
+#else
 #ifndef BLE_LL_CFG_FEAT_LE_PING
 #define  BLE_LL_CFG_FEAT_LE_PING                (1)
 #endif
+#endif
 
 /*
  * This option enables/disables the data length update procedure in the
diff --git a/net/nimble/src/ble_util.c b/net/nimble/src/ble_util.c
new file mode 100644
index 00000000..bfe7083e
--- /dev/null
+++ b/net/nimble/src/ble_util.c
@@ -0,0 +1,43 @@
+/**
+ * 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 "os/os.h"
+#include "nimble/ble.h"
+
+/**
+ * Converts an OS error code to its equivalent BLE_ERR code.
+ *
+ * @param os_err                The OS error code to convert.
+ *
+ * @return                      The equivalent BLE_ERR code.
+ */
+int
+ble_err_from_os(int os_err)
+{
+    switch (os_err) {
+    case 0:
+        return 0;
+
+    case OS_ENOMEM:
+        return BLE_ERR_MEM_CAPACITY;
+
+    default:
+        return BLE_ERR_UNSPECIFIED;
+    }
+}
diff --git a/net/nimble/src/util.c b/net/nimble/src/util.c
index d0056df6..9aef1103 100644
--- a/net/nimble/src/util.c
+++ b/net/nimble/src/util.c
@@ -58,10 +58,10 @@ htole64(void *buf, uint64_t x)
 }
 
 uint16_t
-le16toh(void *buf)
+le16toh(const void *buf)
 {
+    const uint8_t *u8ptr;
     uint16_t x;
-    uint8_t *u8ptr;
 
     u8ptr = buf;
     x = u8ptr[0];
@@ -71,10 +71,10 @@ le16toh(void *buf)
 }
 
 uint32_t
-le32toh(void *buf)
+le32toh(const void *buf)
 {
+    const uint8_t *u8ptr;
     uint32_t x;
-    uint8_t *u8ptr;
 
     u8ptr = buf;
     x = u8ptr[0];
@@ -86,10 +86,10 @@ le32toh(void *buf)
 }
 
 uint64_t
-le64toh(void *buf)
+le64toh(const void *buf)
 {
+    const uint8_t *u8ptr;
     uint64_t x;
-    uint8_t *u8ptr;
 
     u8ptr = buf;
     x = u8ptr[0];
@@ -143,10 +143,10 @@ htobe64(void *buf, uint64_t x)
 }
 
 uint16_t
-be16toh(void *buf)
+be16toh(const void *buf)
 {
+    const uint8_t *u8ptr;
     uint16_t x;
-    uint8_t *u8ptr;
 
     u8ptr = buf;
     x = (uint16_t)u8ptr[0] << 8;
@@ -156,10 +156,10 @@ be16toh(void *buf)
 }
 
 uint32_t
-be32toh(void *buf)
+be32toh(const void *buf)
 {
+    const uint8_t *u8ptr;
     uint32_t x;
-    uint8_t *u8ptr;
 
     u8ptr = buf;
     x = (uint32_t)u8ptr[0] << 24;
@@ -171,10 +171,10 @@ be32toh(void *buf)
 }
 
 uint64_t
-be64toh(void *buf)
+be64toh(const void *buf)
 {
+    const uint8_t *u8ptr;
     uint64_t x;
-    uint8_t *u8ptr;
 
     u8ptr = buf;
     x = (uint64_t)u8ptr[0] << 56;
@@ -208,7 +208,7 @@ swap_in_place(void *buf, int len)
 
 /* swap octets */
 void
-swap_buf(uint8_t *dst, uint8_t *src, int len)
+swap_buf(uint8_t *dst, const uint8_t *src, int len)
 {
     int i;
 
diff --git a/net/nimble/transport/ram/include/transport/ram/ble_hci_ram.h b/net/nimble/transport/ram/include/transport/ram/ble_hci_ram.h
new file mode 100644
index 00000000..9d2d672b
--- /dev/null
+++ b/net/nimble/transport/ram/include/transport/ram/ble_hci_ram.h
@@ -0,0 +1,30 @@
+#ifndef H_BLE_HCI_RAM_
+#define H_BLE_HCI_RAM_
+
+#include "nimble/ble_hci_trans.h"
+
+struct ble_hci_ram_cfg {
+    /** Number of high-priority event buffers. */
+    uint16_t num_evt_hi_bufs;
+
+    /** Number of low-priority event buffers. */
+    uint16_t num_evt_lo_bufs;
+
+    /** Size of each event buffer, in bytes. */
+    uint16_t evt_buf_sz;
+
+    /* Note: For information about high-priority vs. low-priority event
+     * buffers, see net/nimble/include/nimble/ble_hci_trans.h.
+     */
+
+    /* Note: host-to-controller command buffers are not configurable.  The RAM
+     * transport only allows one outstanding command, so it uses a single
+     * statically-allocated buffer.
+     */
+};
+
+extern const struct ble_hci_ram_cfg ble_hci_ram_cfg_dflt;
+
+int ble_hci_ram_init(const struct ble_hci_ram_cfg *cfg);
+
+#endif
diff --git a/net/nimble/transport/ram/pkg.yml b/net/nimble/transport/ram/pkg.yml
new file mode 100644
index 00000000..a3524a19
--- /dev/null
+++ b/net/nimble/transport/ram/pkg.yml
@@ -0,0 +1,33 @@
+#
+# 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: net/nimble/transport/ram
+pkg.description: XXX
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - net/nimble
+    - libs/os
+
+pkg.apis:
+    - ble_transport
diff --git a/net/nimble/transport/ram/src/ble_hci_ram.c b/net/nimble/transport/ram/src/ble_hci_ram.c
new file mode 100644
index 00000000..e3d236ea
--- /dev/null
+++ b/net/nimble/transport/ram/src/ble_hci_ram.c
@@ -0,0 +1,229 @@
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+#include "os/os.h"
+#include "util/mem.h"
+#include "nimble/ble.h"
+#include "nimble/ble_hci_trans.h"
+#include "transport/ram/ble_hci_ram.h"
+
+/** Default configuration. */
+const struct ble_hci_ram_cfg ble_hci_ram_cfg_dflt = {
+    .num_evt_hi_bufs = 2,
+    .num_evt_lo_bufs = 8,
+
+    /* The largest event the nimble controller will send is 70 bytes. */
+    .evt_buf_sz = 70,
+};
+
+static ble_hci_trans_rx_cmd_fn *ble_hci_ram_rx_cmd_hs_cb;
+static void *ble_hci_ram_rx_cmd_hs_arg;
+
+static ble_hci_trans_rx_cmd_fn *ble_hci_ram_rx_cmd_ll_cb;
+static void *ble_hci_ram_rx_cmd_ll_arg;
+
+static ble_hci_trans_rx_acl_fn *ble_hci_ram_rx_acl_hs_cb;
+static void *ble_hci_ram_rx_acl_hs_arg;
+
+static ble_hci_trans_rx_acl_fn *ble_hci_ram_rx_acl_ll_cb;
+static void *ble_hci_ram_rx_acl_ll_arg;
+
+static struct os_mempool ble_hci_ram_evt_hi_pool;
+static void *ble_hci_ram_evt_hi_buf;
+static struct os_mempool ble_hci_ram_evt_lo_pool;
+static void *ble_hci_ram_evt_lo_buf;
+
+static uint8_t *ble_hci_ram_hs_cmd_buf;
+static uint8_t ble_hci_ram_hs_cmd_buf_alloced;
+
+void
+ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_ram_rx_cmd_hs_cb = cmd_cb;
+    ble_hci_ram_rx_cmd_hs_arg = cmd_arg;
+    ble_hci_ram_rx_acl_hs_cb = acl_cb;
+    ble_hci_ram_rx_acl_hs_arg = acl_arg;
+}
+
+void
+ble_hci_trans_cfg_ll(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                     void *cmd_arg,
+                     ble_hci_trans_rx_acl_fn *acl_cb,
+                     void *acl_arg)
+{
+    ble_hci_ram_rx_cmd_ll_cb = cmd_cb;
+    ble_hci_ram_rx_cmd_ll_arg = cmd_arg;
+    ble_hci_ram_rx_acl_ll_cb = acl_cb;
+    ble_hci_ram_rx_acl_ll_arg = acl_arg;
+}
+
+int
+ble_hci_trans_hs_cmd_tx(uint8_t *cmd)
+{
+    int rc;
+
+    assert(ble_hci_ram_rx_cmd_ll_cb != NULL);
+
+    rc = ble_hci_ram_rx_cmd_ll_cb(cmd, ble_hci_ram_rx_cmd_ll_arg);
+    return rc;
+}
+
+int
+ble_hci_trans_ll_evt_tx(uint8_t *hci_ev)
+{
+    int rc;
+
+    assert(ble_hci_ram_rx_cmd_hs_cb != NULL);
+
+    rc = ble_hci_ram_rx_cmd_hs_cb(hci_ev, ble_hci_ram_rx_cmd_hs_arg);
+    return rc;
+}
+
+int
+ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
+{
+   int rc;
+
+    assert(ble_hci_ram_rx_acl_ll_cb != NULL);
+
+    rc = ble_hci_ram_rx_acl_ll_cb(om, ble_hci_ram_rx_acl_ll_arg);
+    return rc;
+}
+
+int
+ble_hci_trans_ll_acl_tx(struct os_mbuf *om)
+{
+    int rc;
+
+    assert(ble_hci_ram_rx_acl_hs_cb != NULL);
+
+    rc = ble_hci_ram_rx_acl_hs_cb(om, ble_hci_ram_rx_acl_hs_arg);
+    return rc;
+}
+
+uint8_t *
+ble_hci_trans_buf_alloc(int type)
+{
+    uint8_t *buf;
+
+    switch (type) {
+    case BLE_HCI_TRANS_BUF_EVT_HI:
+        buf = os_memblock_get(&ble_hci_ram_evt_hi_pool);
+        if (buf == NULL) {
+            /* If no high-priority event buffers remain, try to grab a
+             * low-priority one.
+             */
+            buf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+        }
+        break;
+
+    case BLE_HCI_TRANS_BUF_EVT_LO:
+        buf = os_memblock_get(&ble_hci_ram_evt_lo_pool);
+        break;
+
+    case BLE_HCI_TRANS_BUF_CMD:
+        assert(!ble_hci_ram_hs_cmd_buf_alloced);
+        ble_hci_ram_hs_cmd_buf_alloced = 1;
+        buf = ble_hci_ram_hs_cmd_buf;
+        break;
+
+    default:
+        assert(0);
+        buf = NULL;
+    }
+
+    return buf;
+}
+
+void
+ble_hci_trans_buf_free(uint8_t *buf)
+{
+    int rc;
+
+    if (buf == ble_hci_ram_hs_cmd_buf) {
+        assert(ble_hci_ram_hs_cmd_buf_alloced);
+        ble_hci_ram_hs_cmd_buf_alloced = 0;
+    } else if (os_memblock_from(&ble_hci_ram_evt_hi_pool, buf)) {
+        rc = os_memblock_put(&ble_hci_ram_evt_hi_pool, buf);
+        assert(rc == 0);
+    } else {
+        assert(os_memblock_from(&ble_hci_ram_evt_lo_pool, buf));
+        rc = os_memblock_put(&ble_hci_ram_evt_lo_pool, buf);
+        assert(rc == 0);
+    }
+}
+
+static void
+ble_hci_ram_free_mem(void)
+{
+    free(ble_hci_ram_evt_hi_buf);
+    ble_hci_ram_evt_hi_buf = NULL;
+
+    free(ble_hci_ram_evt_lo_buf);
+    ble_hci_ram_evt_lo_buf = NULL;
+
+    free(ble_hci_ram_hs_cmd_buf);
+    ble_hci_ram_hs_cmd_buf = NULL;
+    ble_hci_ram_hs_cmd_buf_alloced = 0;
+}
+
+int
+ble_hci_trans_reset(void)
+{
+    /* No work to do.  All allocated buffers are owned by the host or
+     * controller, and they will get freed by their owners.
+     */
+    return 0;
+}
+
+/**
+ * Initializes the RAM HCI transport module.
+ *
+ * @param cfg                   The settings to initialize the HCI RAM
+ *                                  transport with.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_ram_init(const struct ble_hci_ram_cfg *cfg)
+{
+    int rc;
+
+    ble_hci_ram_free_mem();
+
+    rc = mem_malloc_mempool(&ble_hci_ram_evt_hi_pool,
+                            cfg->num_evt_hi_bufs,
+                            cfg->evt_buf_sz,
+                            "ble_hci_ram_evt_hi_pool",
+                            &ble_hci_ram_evt_hi_buf);
+    if (rc != 0) {
+        rc = ble_err_from_os(rc);
+        goto err;
+    }
+
+    rc = mem_malloc_mempool(&ble_hci_ram_evt_lo_pool,
+                            cfg->num_evt_lo_bufs,
+                            cfg->evt_buf_sz,
+                            "ble_hci_ram_evt_lo_pool",
+                            &ble_hci_ram_evt_lo_buf);
+    if (rc != 0) {
+        rc = ble_err_from_os(rc);
+        goto err;
+    }
+
+    ble_hci_ram_hs_cmd_buf = malloc(BLE_HCI_TRANS_CMD_SZ);
+    if (ble_hci_ram_hs_cmd_buf == NULL) {
+        rc = BLE_ERR_MEM_CAPACITY;
+        goto err;
+    }
+
+    return 0;
+
+err:
+    ble_hci_ram_free_mem();
+    return rc;
+}
diff --git a/net/nimble/transport/uart/include/transport/uart/ble_hci_uart.h b/net/nimble/transport/uart/include/transport/uart/ble_hci_uart.h
new file mode 100644
index 00000000..1fbaa747
--- /dev/null
+++ b/net/nimble/transport/uart/include/transport/uart/ble_hci_uart.h
@@ -0,0 +1,19 @@
+#ifndef H_BLE_HCI_UART_
+#define H_BLE_HCI_UART_
+
+struct ble_hci_uart_cfg {
+    uint32_t baud;
+    uint16_t num_evt_bufs;
+    uint16_t evt_buf_sz;
+    uint8_t uart_port;
+    uint8_t flow_ctrl;
+    uint8_t data_bits;
+    uint8_t stop_bits;
+    uint8_t parity;
+};
+
+extern const struct ble_hci_uart_cfg ble_hci_uart_cfg_dflt;
+
+int ble_hci_uart_init(const struct ble_hci_uart_cfg *cfg);
+
+#endif
diff --git a/net/nimble/transport/uart/pkg.yml b/net/nimble/transport/uart/pkg.yml
new file mode 100644
index 00000000..cce429c9
--- /dev/null
+++ b/net/nimble/transport/uart/pkg.yml
@@ -0,0 +1,34 @@
+#
+# 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: net/nimble/transport/uart
+pkg.description: XXX
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.deps:
+    - hw/hal
+    - libs/os
+    - net/nimble
+
+pkg.apis:
+    - ble_transport
diff --git a/net/nimble/transport/uart/src/ble_hci_uart.c b/net/nimble/transport/uart/src/ble_hci_uart.c
new file mode 100755
index 00000000..0fa982a8
--- /dev/null
+++ b/net/nimble/transport/uart/src/ble_hci_uart.c
@@ -0,0 +1,743 @@
+/**
+ * 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 <assert.h>
+#include <string.h>
+#include <stdio.h>
+#include <errno.h>
+#include "bsp/bsp.h"
+#include "os/os.h"
+#include "util/mem.h"
+#include "hal/hal_gpio.h"
+#include "hal/hal_cputime.h"
+#include "hal/hal_uart.h"
+
+/* BLE */
+#include "nimble/ble.h"
+#include "nimble/nimble_opt.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+
+#include "transport/uart/ble_hci_uart.h"
+
+/***
+ * NOTE:
+ * The UART HCI transport doesn't use event buffer priorities.  All incoming
+ * and outgoing events and commands use buffers from the same pool.
+ */
+
+#define BLE_HCI_UART_H4_NONE        0x00
+#define BLE_HCI_UART_H4_CMD         0x01
+#define BLE_HCI_UART_H4_ACL         0x02
+#define BLE_HCI_UART_H4_SCO         0x03
+#define BLE_HCI_UART_H4_EVT         0x04
+
+/** Default configuration. */
+const struct ble_hci_uart_cfg ble_hci_uart_cfg_dflt = {
+    .uart_port = 0,
+    .baud = 1000000,
+    .flow_ctrl = HAL_UART_FLOW_CTL_RTS_CTS,
+    .data_bits = 8,
+    .stop_bits = 1,
+    .parity = HAL_UART_PARITY_NONE,
+
+    .num_evt_bufs = 8,
+    .evt_buf_sz = BLE_HCI_TRANS_CMD_SZ,
+};
+
+static ble_hci_trans_rx_cmd_fn *ble_hci_uart_rx_cmd_cb;
+static void *ble_hci_uart_rx_cmd_arg;
+
+static ble_hci_trans_rx_acl_fn *ble_hci_uart_rx_acl_cb;
+static void *ble_hci_uart_rx_acl_arg;
+
+static struct os_mempool ble_hci_uart_evt_pool;
+static void *ble_hci_uart_evt_buf;
+
+static struct os_mempool ble_hci_uart_pkt_pool;
+static void *ble_hci_uart_pkt_buf;
+
+/**
+ * An incoming or outgoing command or event.
+ */
+struct ble_hci_uart_cmd {
+    uint8_t *data;      /* Pointer to ble_hci_uart_cmd data */
+    uint16_t cur;       /* Number of bytes read/written */
+    uint16_t len;       /* Total number of bytes to read/write */
+};
+
+/**
+ * An incoming ACL data packet.
+ */
+struct ble_hci_uart_acl {
+    struct os_mbuf *buf; /* Buffer containing the data */
+    uint16_t len;        /* Target size when buf is considered complete */
+};
+
+/**
+ * A packet to be sent over the UART.  This can be a command, an event, or ACL
+ * data.
+ */
+struct ble_hci_uart_pkt {
+    STAILQ_ENTRY(ble_hci_uart_pkt) next;
+    void *data;
+    uint8_t type;
+};
+
+static struct {
+    /*** State of data received over UART. */
+    uint8_t rx_type;    /* Pending packet type. 0 means nothing pending */
+    union {
+        struct ble_hci_uart_cmd rx_cmd;
+        struct ble_hci_uart_acl rx_acl;
+    };
+
+    /*** State of data transmitted over UART. */
+    uint8_t tx_type;    /* Pending packet type. 0 means nothing pending */
+    union {
+        struct ble_hci_uart_cmd tx_cmd;
+        struct os_mbuf *tx_acl;
+    };
+    STAILQ_HEAD(, ble_hci_uart_pkt) tx_pkts; /* Packet queue to send to UART */
+} ble_hci_uart_state;
+
+static struct ble_hci_uart_cfg ble_hci_uart_cfg;
+
+static int
+ble_hci_uart_acl_tx(struct os_mbuf *om)
+{
+    struct ble_hci_uart_pkt *pkt;
+    os_sr_t sr;
+
+    pkt = os_memblock_get(&ble_hci_uart_pkt_pool);
+    if (pkt == NULL) {
+        os_mbuf_free_chain(om);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    pkt->type = BLE_HCI_UART_H4_ACL;
+    pkt->data = om;
+
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&ble_hci_uart_state.tx_pkts, pkt, next);
+    OS_EXIT_CRITICAL(sr);
+
+    hal_uart_start_tx(ble_hci_uart_cfg.uart_port);
+
+    return 0;
+}
+
+static int
+ble_hci_uart_cmdevt_tx(uint8_t *hci_ev, uint8_t h4_type)
+{
+    struct ble_hci_uart_pkt *pkt;
+    os_sr_t sr;
+
+    pkt = os_memblock_get(&ble_hci_uart_pkt_pool);
+    if (pkt == NULL) {
+        ble_hci_trans_buf_free(hci_ev);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    pkt->type = h4_type;
+    pkt->data = hci_ev;
+
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&ble_hci_uart_state.tx_pkts, pkt, next);
+    OS_EXIT_CRITICAL(sr);
+
+    hal_uart_start_tx(ble_hci_uart_cfg.uart_port);
+
+    return 0;
+}
+
+/**
+ * @return                      The packet type to transmit on success;
+ *                              -1 if there is nothing to send.
+ */
+static int
+ble_hci_uart_tx_pkt_type(void)
+{
+    struct ble_hci_uart_pkt *pkt;
+    os_sr_t sr;
+    int rc;
+
+    OS_ENTER_CRITICAL(sr);
+
+    pkt = STAILQ_FIRST(&ble_hci_uart_state.tx_pkts);
+    if (!pkt) {
+        OS_EXIT_CRITICAL(sr);
+        return -1;
+    }
+
+    STAILQ_REMOVE(&ble_hci_uart_state.tx_pkts, pkt, ble_hci_uart_pkt, next);
+
+    OS_EXIT_CRITICAL(sr);
+
+    rc = pkt->type;
+    switch (pkt->type) {
+    case BLE_HCI_UART_H4_CMD:
+        ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_CMD;
+        ble_hci_uart_state.tx_cmd.data = pkt->data;
+        ble_hci_uart_state.tx_cmd.cur = 0;
+        ble_hci_uart_state.tx_cmd.len = ble_hci_uart_state.tx_cmd.data[2] +
+                                        BLE_HCI_CMD_HDR_LEN;
+        break;
+
+    case BLE_HCI_UART_H4_EVT:
+        ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_EVT;
+        ble_hci_uart_state.tx_cmd.data = pkt->data;
+        ble_hci_uart_state.tx_cmd.cur = 0;
+        ble_hci_uart_state.tx_cmd.len = ble_hci_uart_state.tx_cmd.data[1] +
+                                        BLE_HCI_EVENT_HDR_LEN;
+        break;
+
+    case BLE_HCI_UART_H4_ACL:
+        ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_ACL;
+        ble_hci_uart_state.tx_acl = pkt->data;
+        break;
+
+    default:
+        rc = -1;
+        break;
+    }
+
+    os_memblock_put(&ble_hci_uart_pkt_pool, pkt);
+
+    return rc;
+}
+
+/**
+ * @return                      The byte to transmit on success;
+ *                              -1 if there is nothing to send.
+ */
+static int
+ble_hci_uart_tx_char(void *arg)
+{
+    int rc = -1;
+
+    switch (ble_hci_uart_state.tx_type) {
+    case BLE_HCI_UART_H4_NONE: /* No pending packet, pick one from the queue */
+        rc = ble_hci_uart_tx_pkt_type();
+        break;
+
+    case BLE_HCI_UART_H4_CMD:
+    case BLE_HCI_UART_H4_EVT:
+        rc = ble_hci_uart_state.tx_cmd.data[ble_hci_uart_state.tx_cmd.cur++];
+
+        if (ble_hci_uart_state.tx_cmd.cur == ble_hci_uart_state.tx_cmd.len) {
+            ble_hci_trans_buf_free(ble_hci_uart_state.tx_cmd.data);
+            ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_NONE;
+        }
+        break;
+
+    case BLE_HCI_UART_H4_ACL:
+        rc = *OS_MBUF_DATA(ble_hci_uart_state.tx_acl, uint8_t *);
+        os_mbuf_adj(ble_hci_uart_state.tx_acl, 1);
+        if (!OS_MBUF_PKTLEN(ble_hci_uart_state.tx_acl)) {
+            os_mbuf_free_chain(ble_hci_uart_state.tx_acl);
+            ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_NONE;
+        }
+        break;
+    }
+
+    return rc;
+}
+
+/**
+ * @return                      The type of packet to follow success;
+ *                              -1 if there is no valid packet to receive.
+ */
+static int
+ble_hci_uart_rx_pkt_type(uint8_t data)
+{
+    ble_hci_uart_state.rx_type = data;
+
+    /* XXX: For now we assert that buffer allocation succeeds.  The correct
+     * thing to do is return -1 on allocation failure so that flow control is
+     * engaged.  Then, we will need to tell the UART to start receiving again
+     * as follows:
+     *     o flat buf: when we free a buffer.
+     *     o mbuf: periodically? (which task executes the callout?)
+     */
+    switch (ble_hci_uart_state.rx_type) {
+    case BLE_HCI_UART_H4_CMD:
+        ble_hci_uart_state.rx_cmd.data =
+            ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_CMD);
+        assert(ble_hci_uart_state.rx_cmd.data != NULL);
+
+        ble_hci_uart_state.rx_cmd.len = 0;
+        ble_hci_uart_state.rx_cmd.cur = 0;
+        break;
+
+    case BLE_HCI_UART_H4_EVT:
+        ble_hci_uart_state.rx_cmd.data =
+            ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        assert(ble_hci_uart_state.rx_cmd.data != NULL);
+
+        ble_hci_uart_state.rx_cmd.len = 0;
+        ble_hci_uart_state.rx_cmd.cur = 0;
+        break;
+
+    case BLE_HCI_UART_H4_ACL:
+        ble_hci_uart_state.rx_acl.buf =
+            os_msys_get_pkthdr(BLE_HCI_DATA_HDR_SZ, 0);
+        assert(ble_hci_uart_state.rx_acl.buf != NULL);
+
+        ble_hci_uart_state.rx_acl.len = 0;
+        break;
+
+    default:
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+        return -1;
+    }
+
+    return 0;
+}
+
+static void
+ble_hci_uart_rx_cmd(uint8_t data)
+{
+    int rc;
+
+    ble_hci_uart_state.rx_cmd.data[ble_hci_uart_state.rx_cmd.cur++] = data;
+
+    if (ble_hci_uart_state.rx_cmd.cur < BLE_HCI_CMD_HDR_LEN) {
+        return;
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == BLE_HCI_CMD_HDR_LEN) {
+        ble_hci_uart_state.rx_cmd.len = ble_hci_uart_state.rx_cmd.data[2] +
+                                         BLE_HCI_CMD_HDR_LEN;
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == ble_hci_uart_state.rx_cmd.len) {
+        assert(ble_hci_uart_rx_cmd_cb != NULL);
+        rc = ble_hci_uart_rx_cmd_cb(ble_hci_uart_state.rx_cmd.data,
+                                    ble_hci_uart_rx_cmd_arg);
+        if (rc != 0) {
+            ble_hci_trans_buf_free(ble_hci_uart_state.rx_cmd.data);
+        }
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+
+static void
+ble_hci_uart_rx_evt(uint8_t data)
+{
+    int rc;
+
+    ble_hci_uart_state.rx_cmd.data[ble_hci_uart_state.rx_cmd.cur++] = data;
+
+    if (ble_hci_uart_state.rx_cmd.cur < BLE_HCI_EVENT_HDR_LEN) {
+        return;
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == BLE_HCI_EVENT_HDR_LEN) {
+        ble_hci_uart_state.rx_cmd.len = ble_hci_uart_state.rx_cmd.data[1] +
+                                        BLE_HCI_EVENT_HDR_LEN;
+    }
+
+    if (ble_hci_uart_state.rx_cmd.cur == ble_hci_uart_state.rx_cmd.len) {
+        assert(ble_hci_uart_rx_cmd_cb != NULL);
+        rc = ble_hci_uart_rx_cmd_cb(ble_hci_uart_state.rx_cmd.data,
+                                    ble_hci_uart_rx_cmd_arg);
+        if (rc != 0) {
+            ble_hci_trans_buf_free(ble_hci_uart_state.rx_cmd.data);
+        }
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+
+static void
+ble_hci_uart_rx_acl(uint8_t data)
+{
+    uint16_t pktlen;
+
+    os_mbuf_append(ble_hci_uart_state.rx_acl.buf, &data, 1);
+
+    pktlen = OS_MBUF_PKTLEN(ble_hci_uart_state.rx_acl.buf);
+
+    if (pktlen < BLE_HCI_DATA_HDR_SZ) {
+        return;
+    }
+
+    if (pktlen == BLE_HCI_DATA_HDR_SZ) {
+        os_mbuf_copydata(ble_hci_uart_state.rx_acl.buf, 2,
+                         sizeof(ble_hci_uart_state.rx_acl.len),
+                         &ble_hci_uart_state.rx_acl.len);
+        ble_hci_uart_state.rx_acl.len =
+            le16toh(&ble_hci_uart_state.rx_acl.len) + BLE_HCI_DATA_HDR_SZ;
+    }
+
+    if (pktlen == ble_hci_uart_state.rx_acl.len) {
+        assert(ble_hci_uart_rx_cmd_cb != NULL);
+        ble_hci_uart_rx_acl_cb(ble_hci_uart_state.rx_acl.buf,
+                               ble_hci_uart_rx_acl_arg);
+        ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+    }
+}
+
+static int
+ble_hci_uart_rx_char(void *arg, uint8_t data)
+{
+    switch (ble_hci_uart_state.rx_type) {
+    case BLE_HCI_UART_H4_NONE:
+        return ble_hci_uart_rx_pkt_type(data);
+    case BLE_HCI_UART_H4_CMD:
+        ble_hci_uart_rx_cmd(data);
+        return 0;
+    case BLE_HCI_UART_H4_EVT:
+        ble_hci_uart_rx_evt(data);
+        return 0;
+    case BLE_HCI_UART_H4_ACL:
+        ble_hci_uart_rx_acl(data);
+        return 0;
+    default:
+        return -1;
+    }
+}
+
+static void
+ble_hci_uart_set_rx_cbs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                        void *cmd_arg,
+                        ble_hci_trans_rx_acl_fn *acl_cb,
+                        void *acl_arg)
+{
+    ble_hci_uart_rx_cmd_cb = cmd_cb;
+    ble_hci_uart_rx_cmd_arg = cmd_arg;
+    ble_hci_uart_rx_acl_cb = acl_cb;
+    ble_hci_uart_rx_acl_arg = acl_arg;
+}
+
+static void
+ble_hci_uart_free_pkt(uint8_t type, uint8_t *cmdevt, struct os_mbuf *acl)
+{
+    switch (type) {
+    case BLE_HCI_UART_H4_NONE:
+        break;
+
+    case BLE_HCI_UART_H4_CMD:
+    case BLE_HCI_UART_H4_EVT:
+        ble_hci_trans_buf_free(cmdevt);
+        break;
+
+    case BLE_HCI_UART_H4_ACL:
+        os_mbuf_free_chain(acl);
+        break;
+
+    default:
+        assert(0);
+        break;
+    }
+}
+
+static void
+ble_hci_uart_free_mem(void)
+{
+    free(ble_hci_uart_evt_buf);
+    ble_hci_uart_evt_buf = NULL;
+
+    free(ble_hci_uart_pkt_buf);
+    ble_hci_uart_pkt_buf = NULL;
+}
+
+static int
+ble_hci_uart_config(void)
+{
+    int rc;
+
+    rc = hal_uart_init_cbs(ble_hci_uart_cfg.uart_port,
+                           ble_hci_uart_tx_char, NULL,
+                           ble_hci_uart_rx_char, NULL);
+    if (rc != 0) {
+        return BLE_ERR_UNSPECIFIED;
+    }
+
+    rc = hal_uart_config(ble_hci_uart_cfg.uart_port,
+                         ble_hci_uart_cfg.baud,
+                         ble_hci_uart_cfg.data_bits,
+                         ble_hci_uart_cfg.stop_bits,
+                         ble_hci_uart_cfg.parity,
+                         ble_hci_uart_cfg.flow_ctrl);
+    if (rc != 0) {
+        return BLE_ERR_HW_FAIL;
+    }
+
+    return 0;
+}
+
+/**
+ * Sends an HCI event from the controller to the host.
+ *
+ * @param cmd                   The HCI event to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_ll_evt_tx(uint8_t *cmd)
+{
+    int rc;
+
+    rc = ble_hci_uart_cmdevt_tx(cmd, BLE_HCI_UART_H4_EVT);
+    return rc;
+}
+
+/**
+ * Sends ACL data from controller to host.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_ll_acl_tx(struct os_mbuf *om)
+{
+    int rc;
+
+    rc = ble_hci_uart_acl_tx(om);
+    return rc;
+}
+
+/**
+ * Sends an HCI command from the host to the controller.
+ *
+ * @param cmd                   The HCI command to send.  This buffer must be
+ *                                  allocated via ble_hci_trans_buf_alloc().
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_hs_cmd_tx(uint8_t *cmd)
+{
+    int rc;
+
+    rc = ble_hci_uart_cmdevt_tx(cmd, BLE_HCI_UART_H4_CMD);
+    return rc;
+}
+
+/**
+ * Sends ACL data from host to controller.
+ *
+ * @param om                    The ACL data packet to send.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
+{
+    int rc;
+
+    rc = ble_hci_uart_acl_tx(om);
+    return rc;
+}
+
+/**
+ * Configures the HCI transport to call the specified callback upon receiving
+ * HCI packets from the controller.  This function should only be called by by
+ * host.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void
+ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                            void *cmd_arg,
+                            ble_hci_trans_rx_acl_fn *acl_cb,
+                            void *acl_arg)
+{
+    ble_hci_uart_set_rx_cbs(cmd_cb, cmd_arg, acl_cb, acl_arg);
+}
+
+/**
+ * Configures the HCI transport to operate with a host.  The transport will
+ * execute specified callbacks upon receiving HCI packets from the controller.
+ *
+ * @param cmd_cb                The callback to execute upon receiving an HCI
+ *                                  event.
+ * @param cmd_arg               Optional argument to pass to the command
+ *                                  callback.
+ * @param acl_cb                The callback to execute upon receiving ACL
+ *                                  data.
+ * @param acl_arg               Optional argument to pass to the ACL
+ *                                  callback.
+ */
+void
+ble_hci_trans_cfg_ll(ble_hci_trans_rx_cmd_fn *cmd_cb,
+                            void *cmd_arg,
+                            ble_hci_trans_rx_acl_fn *acl_cb,
+                            void *acl_arg)
+{
+    ble_hci_uart_set_rx_cbs(cmd_cb, cmd_arg, acl_cb, acl_arg);
+}
+
+/**
+ * Allocates a flat buffer of the specified type.
+ *
+ * @param type                  The type of buffer to allocate; one of the
+ *                                  BLE_HCI_TRANS_BUF_[...] constants.
+ *
+ * @return                      The allocated buffer on success;
+ *                              NULL on buffer exhaustion.
+ */
+uint8_t *
+ble_hci_trans_buf_alloc(int type)
+{
+    uint8_t *buf;
+
+    switch (type) {
+    case BLE_HCI_TRANS_BUF_CMD:
+    case BLE_HCI_TRANS_BUF_EVT_LO:
+    case BLE_HCI_TRANS_BUF_EVT_HI:
+        buf = os_memblock_get(&ble_hci_uart_evt_pool);
+        break;
+
+    default:
+        assert(0);
+        buf = NULL;
+    }
+
+    return buf;
+}
+
+/**
+ * Frees the specified flat buffer.  The buffer must have been allocated via
+ * ble_hci_trans_buf_alloc().
+ *
+ * @param buf                   The buffer to free.
+ */
+void
+ble_hci_trans_buf_free(uint8_t *buf)
+{
+    int rc;
+
+    rc = os_memblock_put(&ble_hci_uart_evt_pool, buf);
+    assert(rc == 0);
+}
+
+/**
+ * Resets the HCI UART transport to a clean state.  Frees all buffers and
+ * reconfigures the UART.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_trans_reset(void)
+{
+    struct ble_hci_uart_pkt *pkt;
+    int rc;
+
+    /* Close the UART to prevent race conditions as the buffers are freed. */
+    rc = hal_uart_close(ble_hci_uart_cfg.uart_port);
+    if (rc != 0) {
+        return BLE_ERR_HW_FAIL;
+    }
+
+    ble_hci_uart_free_pkt(ble_hci_uart_state.rx_type,
+                          ble_hci_uart_state.rx_cmd.data,
+                          ble_hci_uart_state.rx_acl.buf);
+    ble_hci_uart_state.rx_type = BLE_HCI_UART_H4_NONE;
+
+    ble_hci_uart_free_pkt(ble_hci_uart_state.tx_type,
+                          ble_hci_uart_state.tx_cmd.data,
+                          ble_hci_uart_state.tx_acl);
+    ble_hci_uart_state.tx_type = BLE_HCI_UART_H4_NONE;
+
+    while ((pkt = STAILQ_FIRST(&ble_hci_uart_state.tx_pkts)) != NULL) {
+        STAILQ_REMOVE(&ble_hci_uart_state.tx_pkts, pkt, ble_hci_uart_pkt,
+                      next);
+        ble_hci_uart_free_pkt(pkt->type, pkt->data, pkt->data);
+        os_memblock_put(&ble_hci_uart_pkt_pool, pkt);
+    }
+
+    /* Reopen the UART. */
+    rc = ble_hci_uart_config();
+    if (rc != 0) {
+        return rc;
+    }
+
+    return 0;
+}
+
+/**
+ * Initializes the UART HCI transport module.
+ *
+ * @param cfg                   The settings to initialize the HCI UART
+ *                                  transport with.
+ *
+ * @return                      0 on success;
+ *                              A BLE_ERR_[...] error code on failure.
+ */
+int
+ble_hci_uart_init(const struct ble_hci_uart_cfg *cfg)
+{
+    int rc;
+
+    ble_hci_uart_free_mem();
+
+    ble_hci_uart_cfg = *cfg;
+
+    /* Create memory pool of HCI command / event buffers */
+    rc = mem_malloc_mempool(&ble_hci_uart_evt_pool,
+                            cfg->num_evt_bufs,
+                            cfg->evt_buf_sz,
+                            "ble_hci_uart_evt_pool",
+                            &ble_hci_uart_evt_buf);
+    if (rc != 0) {
+        rc = ble_err_from_os(rc);
+        goto err;
+    }
+
+    /* Create memory pool of packet list nodes. */
+    rc = mem_malloc_mempool(&ble_hci_uart_pkt_pool,
+                            cfg->num_evt_bufs,
+                            sizeof (struct ble_hci_uart_pkt),
+                            "ble_hci_uart_pkt_pool",
+                            &ble_hci_uart_pkt_buf);
+    if (rc != 0) {
+        rc = ble_err_from_os(rc);
+        goto err;
+    }
+
+    rc = ble_hci_uart_config();
+    if (rc != 0) {
+        goto err;
+    }
+
+    memset(&ble_hci_uart_state, 0, sizeof ble_hci_uart_state);
+    STAILQ_INIT(&ble_hci_uart_state.tx_pkts);
+
+    return 0;
+
+err:
+    ble_hci_uart_free_mem();
+    return rc;
+}
diff --git a/sys/config/src/config_nmgr.c b/sys/config/src/config_nmgr.c
index ae4aa11b..b6019a76 100644
--- a/sys/config/src/config_nmgr.c
+++ b/sys/config/src/config_nmgr.c
@@ -43,34 +43,36 @@ static struct nmgr_group conf_nmgr_group = {
 static int
 conf_nmgr_read(struct nmgr_jbuf *njb)
 {
-    char tmp_str[CONF_MAX_NAME_LEN];
-    char *val_str;
+    int rc;
+    char name_str[CONF_MAX_NAME_LEN];
+    char val_str[CONF_MAX_VAL_LEN];
+    char *val;
+
     const struct json_attr_t attr[2] = {
         [0] = {
             .attribute = "name",
             .type = t_string,
-            .addr.string = tmp_str,
-            .len = sizeof(tmp_str)
+            .addr.string = name_str,
+            .len = sizeof(name_str)
         },
         [1] = {
             .attribute = NULL
         }
     };
     struct json_value jv;
-    int rc;
 
     rc = json_read_object(&njb->njb_buf, attr);
     if (rc) {
         return OS_EINVAL;
     }
 
-    val_str = conf_get_value(tmp_str, tmp_str, sizeof(tmp_str));
-    if (!val_str) {
+    val = conf_get_value(name_str, val_str, sizeof(val_str));
+    if (!val) {
         return OS_EINVAL;
     }
 
     json_encode_object_start(&njb->njb_enc);
-    JSON_VALUE_STRING(&jv, val_str);
+    JSON_VALUE_STRING(&jv, val);
     json_encode_object_entry(&njb->njb_enc, "val", &jv);
     json_encode_object_finish(&njb->njb_enc);
 
diff --git a/sys/fcb/src/fcb.c b/sys/fcb/src/fcb.c
index 27795755..fb855086 100644
--- a/sys/fcb/src/fcb.c
+++ b/sys/fcb/src/fcb.c
@@ -83,11 +83,15 @@ fcb_init(struct fcb *fcb)
     while (1) {
         rc = fcb_getnext_in_area(fcb, &fcb->f_active);
         if (rc == FCB_ERR_NOVAR) {
+            rc = FCB_OK;
+            break;
+        }
+        if (rc != 0) {
             break;
         }
     }
     os_mutex_init(&fcb->f_mtx);
-    return FCB_OK;
+    return rc;
 }
 
 int
diff --git a/sys/fcb/src/fcb_rotate.c b/sys/fcb/src/fcb_rotate.c
index 73724a95..7b6df585 100644
--- a/sys/fcb/src/fcb_rotate.c
+++ b/sys/fcb/src/fcb_rotate.c
@@ -31,6 +31,11 @@ fcb_rotate(struct fcb *fcb)
         return FCB_ERR_ARGS;
     }
 
+    rc = flash_area_erase(fcb->f_oldest, 0, fcb->f_oldest->fa_size);
+    if (rc) {
+        rc = FCB_ERR_FLASH;
+        goto out;
+    }
     if (fcb->f_oldest == fcb->f_active.fe_area) {
         /*
          * Need to create a new active area, as we're wiping the current.
@@ -44,11 +49,6 @@ fcb_rotate(struct fcb *fcb)
         fcb->f_active.fe_elem_off = sizeof(struct fcb_disk_area);
         fcb->f_active_id++;
     }
-    rc = flash_area_erase(fcb->f_oldest, 0, fcb->f_oldest->fa_size);
-    if (rc) {
-        rc = FCB_ERR_FLASH;
-        goto out;
-    }
     fcb->f_oldest = fcb_getnext_area(fcb, fcb->f_oldest);
 out:
     os_mutex_release(&fcb->f_mtx);
diff --git a/sys/log/include/log/log.h b/sys/log/include/log/log.h
index 3bd2c73c..f5772aab 100644
--- a/sys/log/include/log/log.h
+++ b/sys/log/include/log/log.h
@@ -157,6 +157,14 @@ struct log {
     STAILQ_ENTRY(log) l_next;
 };
 
+/* Newtmgr Log opcodes */
+#define LOGS_NMGR_OP_READ         (0)
+#define LOGS_NMGR_OP_CLEAR        (1)
+#define LOGS_NMGR_OP_APPEND       (2)
+#define LOGS_NMGR_OP_MODULE_LIST  (3)
+#define LOGS_NMGR_OP_LEVEL_LIST   (4)
+#define LOGS_NMGR_OP_LOGS_LIST    (5)
+
 /* Log system level functions (for all logs.) */
 int log_init(void);
 struct log *log_list_get_next(struct log *);
diff --git a/sys/log/src/log.c b/sys/log/src/log.c
index bc34c9bc..22c11595 100644
--- a/sys/log/src/log.c
+++ b/sys/log/src/log.c
@@ -104,6 +104,11 @@ log_append(struct log *log, uint16_t module, uint16_t level, void *data,
     struct os_timeval tv;
     int64_t prev_ts;
 
+    if (log->l_name == NULL || log->l_log == NULL) {
+        rc = -1;
+        goto err;
+    }
+
     ue = (struct log_entry_hdr *) data;
 
     g_log_info.li_index++;
diff --git a/sys/log/src/log_fcb.c b/sys/log/src/log_fcb.c
index 2384c832..3bfde04e 100644
--- a/sys/log/src/log_fcb.c
+++ b/sys/log/src/log_fcb.c
@@ -245,16 +245,12 @@ log_fcb_rtr_erase(struct log *log, void *arg)
     fcb_scratch.f_magic = 0x7EADBADF;
     fcb_scratch.f_version = 0;
 
+    flash_area_erase(&sector, 0, sector.fa_size);
     rc = fcb_init(&fcb_scratch);
     if (rc) {
         goto err;
     }
 
-    rc = fcb_clear(&fcb_scratch);
-    if (rc) {
-        goto err;
-    }
-
     /* Calculate offset of n-th last entry */
     rc = fcb_offset_last_n(fcb, fcb_log->fl_entries, &offset);
     if (rc) {
diff --git a/sys/log/src/log_nmgr.c b/sys/log/src/log_nmgr.c
index dcb0f6ed..b1e90a0c 100644
--- a/sys/log/src/log_nmgr.c
+++ b/sys/log/src/log_nmgr.c
@@ -41,14 +41,6 @@ static int log_nmgr_logs_list(struct nmgr_jbuf *njb);
 static struct nmgr_group log_nmgr_group;
 
 
-/* Newtmgr Log opcodes */
-#define LOGS_NMGR_OP_READ         (0)
-#define LOGS_NMGR_OP_CLEAR        (1)
-#define LOGS_NMGR_OP_APPEND       (2)
-#define LOGS_NMGR_OP_MODULE_LIST  (3)
-#define LOGS_NMGR_OP_LEVEL_LIST   (4)
-#define LOGS_NMGR_OP_LOGS_LIST    (5)
-
 /* ORDER MATTERS HERE.
  * Each element represents the command ID, referenced from newtmgr.
  */
diff --git a/sys/mn_socket/include/mn_socket/mn_socket.h b/sys/mn_socket/include/mn_socket/mn_socket.h
new file mode 100644
index 00000000..6b457be9
--- /dev/null
+++ b/sys/mn_socket/include/mn_socket/mn_socket.h
@@ -0,0 +1,146 @@
+/**
+ * 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 __SYS_MN_SOCKET_H_
+#define __SYS_MN_SOCKET_H_
+
+#include <inttypes.h>
+
+/*
+ * Address/protocol family.
+ */
+#define MN_AF_INET              4
+#define MN_PF_INET              MN_AF_INET
+#define MN_AF_INET6             6
+#define MN_PF_INET6             MN_AF_INET6
+
+/*
+ * Socket types
+ */
+#define MN_SOCK_STREAM          1
+#define MN_SOCK_DGRAM           2
+
+/*
+ * Error codes from mn_socket interface.
+ */
+#define MN_EAFNOSUPPORT         1
+#define MN_EPROTONOSUPPORT      2
+#define MN_ENOBUFS              3
+#define MN_EINVAL               4
+#define MN_ENOTCONN             5
+#define MN_ECONNABORTED         6
+#define MN_EDESTADDRREQ         7
+#define MN_EADDRINUSE           8
+#define MN_ETIMEDOUT            9
+#define MN_EAGAIN               10
+#define MN_EUNKNOWN             11
+
+struct mn_socket;
+struct mn_socket_ops;
+struct mn_sock_cb;
+struct os_mbuf;
+
+struct mn_socket {
+    const union mn_socket_cb *ms_cbs;          /* filled in by user */
+    void *ms_cb_arg;                           /* filled in by user */
+    const struct mn_socket_ops *ms_ops;        /* filled in by mn_socket */
+};
+
+/*
+ * Callbacks. Socket callbacks are for sockets which exchange
+ * data. Listen callback is for TCP listen sockets.
+ */
+union mn_socket_cb {
+    struct {
+        void (*readable)(void *cb_arg, int err);
+        void (*writable)(void *cb_arg, int err);
+    } socket;
+    struct {
+        int (*newconn)(void *cb_arg, struct mn_socket *new);
+    } listen;
+};
+
+struct mn_sockaddr {
+    uint8_t msa_len;
+    uint8_t msa_family;
+    char    msa_data[2];
+};
+
+struct mn_sockaddr_in {
+    uint8_t msin_len;
+    uint8_t msin_family;
+    uint16_t msin_port;
+    uint32_t msin_addr;
+};
+
+struct mn_sockaddr_in6 {
+    uint8_t msin6_len;
+    uint8_t msin6_family;
+    uint16_t msin6_port;
+    uint32_t msin6_flowinfo;
+    uint32_t msin6_addr[4];
+};
+
+/*
+ * Socket calls.
+ *
+ * mn_connect() for TCP is asynchronous. Once connection has been established,
+ * socket callback (*writable) will be called.
+ *
+ * mn_sendto() is asynchronous as well. If it fails due to buffer shortage,
+ * socket provider should call (*writable) when more data can be sent.
+ *
+ * mn_recvfrom() returns immediatelly if no data is available. If data arrives,
+ * the callback (*readable) will be called. Once that happens, owner of the
+ * socket should keep calling mn_recvfrom() until it has drained all the
+ * data from the socket.
+ *
+ * If remote end closes the socket, socket callback (*readable) will be
+ * called.
+ */
+int mn_socket(struct mn_socket **, uint8_t domain, uint8_t type, uint8_t proto);
+int mn_bind(struct mn_socket *, struct mn_sockaddr *);
+int mn_connect(struct mn_socket *, struct mn_sockaddr *);
+int mn_listen(struct mn_socket *, uint8_t qlen);
+
+int mn_recvfrom(struct mn_socket *, struct os_mbuf **,
+  struct mn_sockaddr *from);
+int mn_sendto(struct mn_socket *, struct os_mbuf *, struct mn_sockaddr *to);
+
+int mn_getsockopt(struct mn_socket *, uint8_t level, uint8_t optname,
+  void *optval);
+int mn_setsockopt(struct mn_socket *, uint8_t level, uint8_t optname,
+  void *optval);
+
+int mn_getsockname(struct mn_socket *, struct mn_sockaddr *);
+int mn_getpeername(struct mn_socket *, struct mn_sockaddr *);
+
+int mn_close(struct mn_socket *);
+
+#define mn_socket_set_cbs(sock, cb_arg, cbs)                            \
+    do {                                                                \
+        (sock)->ms_cbs = (cbs);                                         \
+        (sock)->ms_cb_arg = (cb_arg);                                   \
+    } while (0)
+
+/*
+ * Address conversion
+ */
+int mn_inet_pton(int af, const char *src, void *dst);
+
+#endif /* __SYS_MN_SOCKET_H_ */
diff --git a/sys/mn_socket/include/mn_socket/mn_socket_ops.h b/sys/mn_socket/include/mn_socket/mn_socket_ops.h
new file mode 100644
index 00000000..61be0540
--- /dev/null
+++ b/sys/mn_socket/include/mn_socket/mn_socket_ops.h
@@ -0,0 +1,82 @@
+/**
+ * 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 __SYS_MN_SOCKET_OPS_H_
+#define __SYS_MN_SOCKET_OPS_H_
+
+#include <inttypes.h>
+
+/*
+ * Interface for socket providers.
+ * - mso_create() creates a socket, memory allocation has to be done by
+ *   the socket provider.
+ * - mso_close() closes the socket, memory should be freed. User should not
+ *   be using the socket pointer once it has been closed.
+ */
+struct mn_socket_ops {
+    int (*mso_create)(struct mn_socket **, uint8_t domain, uint8_t type,
+      uint8_t protocol);
+    int (*mso_close)(struct mn_socket *);
+
+    int (*mso_bind)(struct mn_socket *, struct mn_sockaddr *);
+    int (*mso_connect)(struct mn_socket *, struct mn_sockaddr *);
+    int (*mso_listen)(struct mn_socket *, uint8_t qlen);
+
+    int (*mso_sendto)(struct mn_socket *, struct os_mbuf *,
+      struct mn_sockaddr *to);
+    int (*mso_recvfrom)(struct mn_socket *, struct os_mbuf **,
+      struct mn_sockaddr *from);
+
+    int (*mso_getsockopt)(struct mn_socket *, uint8_t level, uint8_t name,
+      void *val);
+    int (*mso_setsockopt)(struct mn_socket *, uint8_t level, uint8_t name,
+      void *val);
+
+    int (*mso_getsockname)(struct mn_socket *, struct mn_sockaddr *);
+    int (*mso_getpeername)(struct mn_socket *, struct mn_sockaddr *);
+};
+
+int mn_socket_ops_reg(const struct mn_socket_ops *ops);
+
+static inline void
+mn_socket_writable(struct mn_socket *s, int error)
+{
+    if (s->ms_cbs && s->ms_cbs->socket.writable) {
+        s->ms_cbs->socket.writable(s->ms_cb_arg, error);
+    }
+}
+
+static inline void
+mn_socket_readable(struct mn_socket *s, int error)
+{
+    if (s->ms_cbs && s->ms_cbs->socket.readable) {
+        s->ms_cbs->socket.readable(s->ms_cb_arg, error);
+    }
+}
+
+static inline int
+mn_socket_newconn(struct mn_socket *s, struct mn_socket *new)
+{
+    if (s->ms_cbs && s->ms_cbs->listen.newconn) {
+        return s->ms_cbs->listen.newconn(s->ms_cb_arg, new);
+    } else {
+        return -1;
+    }
+}
+
+#endif /* __SYS_MN_SOCKET_OPS_H_ */
diff --git a/sys/mn_socket/pkg.yml b/sys/mn_socket/pkg.yml
new file mode 100644
index 00000000..0c9c0647
--- /dev/null
+++ b/sys/mn_socket/pkg.yml
@@ -0,0 +1,31 @@
+#
+# 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: sys/mn_socket
+pkg.description: Socket interface for Mynewt.
+pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - socket
+    - IP
+
+pkg.deps:
+    - libs/os
+    - libs/util
+    - libs/testutil
diff --git a/sys/mn_socket/src/mn_socket.c b/sys/mn_socket/src/mn_socket.c
new file mode 100644
index 00000000..2a3602ca
--- /dev/null
+++ b/sys/mn_socket/src/mn_socket.c
@@ -0,0 +1,123 @@
+/**
+ * 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 <inttypes.h>
+#include <assert.h>
+
+#include <os/os.h>
+
+#include "mn_socket/mn_socket.h"
+#include "mn_socket/mn_socket_ops.h"
+
+/*
+ * Currently there can be just one provider of sockets.
+ */
+static const struct mn_socket_ops *mn_sock_tgt;
+
+int
+mn_socket_ops_reg(const struct mn_socket_ops *ops)
+{
+    if (mn_sock_tgt) {
+        /*
+         * XXXX for now.
+         */
+        return -1;
+    }
+    mn_sock_tgt = ops;
+    return 0;
+}
+
+int
+mn_socket(struct mn_socket **sp, uint8_t domain, uint8_t type, uint8_t proto)
+{
+    int rc;
+
+    *sp = NULL;
+    /*
+     * XXX Look up where socket should go.
+     */
+    if (!mn_sock_tgt) {
+        return MN_EINVAL;
+    }
+    rc = mn_sock_tgt->mso_create(sp, domain, type, proto);
+    if (*sp) {
+        (*sp)->ms_ops = mn_sock_tgt;
+    }
+    return rc;
+}
+
+int
+mn_bind(struct mn_socket *s, struct mn_sockaddr *addr)
+{
+    return s->ms_ops->mso_bind(s, addr);
+}
+
+int
+mn_connect(struct mn_socket *s, struct mn_sockaddr *addr)
+{
+    return s->ms_ops->mso_connect(s, addr);
+}
+
+int
+mn_listen(struct mn_socket *s, uint8_t qlen)
+{
+    return s->ms_ops->mso_listen(s, qlen);
+}
+
+int
+mn_recvfrom(struct mn_socket *s, struct os_mbuf **mp, struct mn_sockaddr *from)
+{
+    return s->ms_ops->mso_recvfrom(s, mp, from);
+}
+
+int
+mn_sendto(struct mn_socket *s, struct os_mbuf *m, struct mn_sockaddr *to)
+{
+    return s->ms_ops->mso_sendto(s, m, to);
+}
+
+int
+mn_getsockopt(struct mn_socket *s, uint8_t level, uint8_t name, void *val)
+{
+    return s->ms_ops->mso_getsockopt(s, level, name, val);
+}
+
+int
+mn_setsockopt(struct mn_socket *s, uint8_t level, uint8_t name, void *val)
+{
+    return s->ms_ops->mso_setsockopt(s, level, name, val);
+}
+
+int
+mn_getsockname(struct mn_socket *s, struct mn_sockaddr *addr)
+{
+    return s->ms_ops->mso_getsockname(s, addr);
+}
+
+int
+mn_getpeername(struct mn_socket *s, struct mn_sockaddr *addr)
+{
+    return s->ms_ops->mso_getpeername(s, addr);
+}
+
+int
+mn_close(struct mn_socket *s)
+{
+    return s->ms_ops->mso_close(s);
+}
diff --git a/sys/mn_socket/src/mn_socket_aconv.c b/sys/mn_socket/src/mn_socket_aconv.c
new file mode 100644
index 00000000..5053eac6
--- /dev/null
+++ b/sys/mn_socket/src/mn_socket_aconv.c
@@ -0,0 +1,57 @@
+/**
+ * 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 <ctype.h>
+#include <os/queue.h>
+#include "mn_socket/mn_socket.h"
+
+int
+mn_inet_pton(int af, const char *src, void *dst)
+{
+    const char *ch_src;
+    uint8_t *ch_tgt;
+    int val;
+    int cnt;
+
+    if (af == MN_PF_INET) {
+        cnt = 0;
+        ch_tgt = dst;
+        val = 0;
+        for (ch_src = src; *ch_src; ch_src++) {
+            if (cnt > 4) {
+                return 0;
+            }
+            if (isdigit(*ch_src)) {
+                val = val * 10 + *ch_src - '0';
+                if (val > 255) {
+                    return 0;
+                }
+                *ch_tgt = val;
+            } else if (*ch_src == '.') {
+                ++cnt;
+                val = 0;
+                ch_tgt++;
+            } else {
+                return 0;
+            }
+        }
+        return 1;
+    } else {
+        return MN_EAFNOSUPPORT;
+    }
+}
diff --git a/sys/mn_socket/src/test/mn_sock_test.c b/sys/mn_socket/src/test/mn_sock_test.c
new file mode 100644
index 00000000..a8d9cc20
--- /dev/null
+++ b/sys/mn_socket/src/test/mn_sock_test.c
@@ -0,0 +1,82 @@
+/**
+ * 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 <stdio.h>
+#include <string.h>
+
+#include <os/os.h>
+#include <testutil/testutil.h>
+
+#include "mn_socket/mn_socket.h"
+
+TEST_CASE(inet_pton_test)
+{
+    int rc;
+    uint8_t addr[8];
+    struct test_vec {
+        char *str;
+        uint8_t cmp[4];
+    };
+    struct test_vec ok_vec[] = {
+        { "1.1.1.1", { 1, 1, 1, 1 } },
+        { "1.2.3.4", { 1, 2, 3, 4 } },
+        { "010.001.255.255", { 10, 1, 255, 255 } },
+        { "001.002.005.006", { 1, 2, 5, 6 } }
+    };
+    struct test_vec invalid_vec[] = {
+        { "a.b.c.d" },
+        { "1a.b3.4.2" },
+        { "1.3.4.2a" },
+        { "1111.3.4.2" },
+        { "3.256.1.0" },
+    };
+    int i;
+
+    for (i = 0; i < sizeof(ok_vec) / sizeof(ok_vec[0]); i++) {
+        memset(addr, 0xa5, sizeof(addr));
+        rc = mn_inet_pton(MN_PF_INET, ok_vec[i].str, addr);
+        TEST_ASSERT(rc == 1);
+        TEST_ASSERT(!memcmp(ok_vec[i].cmp, addr, sizeof(uint32_t)));
+        TEST_ASSERT(addr[5] == 0xa5);
+    }
+    for (i = 0; i < sizeof(invalid_vec) / sizeof(invalid_vec[0]); i++) {
+        rc = mn_inet_pton(MN_PF_INET, invalid_vec[i].str, addr);
+        TEST_ASSERT(rc == 0);
+    }
+}
+
+TEST_SUITE(mn_socket_test_all)
+{
+    inet_pton_test();
+}
+
+#ifdef MYNEWT_SELFTEST
+
+int
+main(int argc, char **argv)
+{
+    tu_config.tc_print_results = 1;
+    tu_init();
+
+    mn_socket_test_all();
+
+    return tu_any_failed;
+}
+#endif
+