hci skeleton done; not working yet

3 files changed, 552 insertions, 521 deletions

diff --git a/devices/ble_hci/common-hal/_bleio/Adapter.c b/devices/ble_hci/common-hal/_bleio/Adapter.c
index c25bec5b8..5f5259f7e 100644
--- a/devices/ble_hci/common-hal/_bleio/Adapter.c
+++ b/devices/ble_hci/common-hal/_bleio/Adapter.c
@@ -31,6 +31,8 @@
 #include <stdio.h>
 #include <string.h>
 
+#include "hci.h"
+
 #include "py/gc.h"
 #include "py/mphal.h"
 #include "py/objstr.h"
@@ -178,10 +180,10 @@ char default_ble_name[] = { 'C', 'I', 'R', 'C', 'U', 'I', 'T', 'P', 'Y', 0, 0, 0
 // }
 
 void common_hal_bleio_adapter_hci_init(bleio_adapter_obj_t *self, const mcu_pin_obj_t *tx, const mcu_pin_obj_t *rx, const mcu_pin_obj_t *rts, const mcu_pin_obj_t *cts, uint32_t baudrate, uint32_t buffer_size) {
-    self->tx = tx;
-    self->rx = rx;
-    self->rts = rts;
-    self->cts = cts;
+    self->tx_pin = tx;
+    self->rx_pin = rx;
+    self->rts_pin = rts;
+    self->cts_pin = cts;
     self->baudrate = baudrate;
     self->buffer_size = buffer_size;
     self->enabled = false;
@@ -195,6 +197,35 @@ void common_hal_bleio_adapter_set_enabled(bleio_adapter_obj_t *self, bool enable
         return;
     }
 
+    if (enabled) {
+        common_hal_busio_uart_construct(
+            &self->hci_uart,
+            self->tx_pin,       // tx pin
+            self->rx_pin,       // rx pin
+            NULL,               // rts pin
+            NULL,               // cts pin
+            NULL,               // rs485 dir pin
+            false,              // rs485 invert
+            0,                  // timeout
+            self->baudrate,     // baudrate
+            8,                  // nbits
+            PARITY_NONE,        // parity
+            1,                  // stop bits
+            self->buffer_size,  // buffer size
+            NULL,               // buffer
+            false               // sigint_enabled
+            );
+        common_hal_digitalio_digitalinout_construct(&self->rts_digitalinout, self->rts_pin);
+        common_hal_digitalio_digitalinout_construct(&self->cts_digitalinout, self->cts_pin);
+
+        hci_init(self);
+    } else {
+        common_hal_busio_uart_deinit(&self->hci_uart);
+        common_hal_digitalio_digitalinout_deinit(&self->rts_digitalinout);
+        common_hal_digitalio_digitalinout_deinit(&self->cts_digitalinout);
+    }
+
+
     //FIX enable/disable HCI adapter, but don't reset it, since we don't know how.
     self->enabled = enabled;
 }
@@ -206,13 +237,14 @@ bool common_hal_bleio_adapter_get_enabled(bleio_adapter_obj_t *self) {
 bleio_address_obj_t *common_hal_bleio_adapter_get_address(bleio_adapter_obj_t *self) {
     common_hal_bleio_adapter_set_enabled(self, true);
 
-    // ble_gap_addr_t local_address;
-    // get_address(self, &local_address);
+    uint8_t addr[6];
+    hci_readBdAddr(addr);
 
     bleio_address_obj_t *address = m_new_obj(bleio_address_obj_t);
     address->base.type = &bleio_address_type;
 
-    // common_hal_bleio_address_construct(address, local_address.addr, local_address.addr_type);
+    // 0 is the type designating a public address.
+    common_hal_bleio_address_construct(address, addr, 0);
     return address;
 }
 
diff --git a/devices/ble_hci/common-hal/_bleio/Adapter.h b/devices/ble_hci/common-hal/_bleio/Adapter.h
index 38303062a..565963dc0 100644
--- a/devices/ble_hci/common-hal/_bleio/Adapter.h
+++ b/devices/ble_hci/common-hal/_bleio/Adapter.h
@@ -52,15 +52,15 @@ typedef struct {
     bleio_scanresults_obj_t* scan_results;
     mp_obj_t name;
     mp_obj_tuple_t *connection_objs;
-    const mcu_pin_obj_t* tx;
-    const mcu_pin_obj_t* rx;
-    const mcu_pin_obj_t* rts;
-    const mcu_pin_obj_t* cts;
+    const mcu_pin_obj_t* tx_pin;
+    const mcu_pin_obj_t* rx_pin;
+    const mcu_pin_obj_t* rts_pin;
+    const mcu_pin_obj_t* cts_pin;
     uint32_t baudrate;
     uint16_t buffer_size;
     busio_uart_obj_t hci_uart;
-    digitalio_digitalinout_obj_t rts_digitalio;
-    digitalio_digitalinout_obj_t cts_digitalio;
+    digitalio_digitalinout_obj_t rts_digitalinout;
+    digitalio_digitalinout_obj_t cts_digitalinout;
     bool enabled;
 } bleio_adapter_obj_t;
 
diff --git a/devices/ble_hci/common-hal/_bleio/hci.c b/devices/ble_hci/common-hal/_bleio/hci.c
index 8ff69f202..7d4add9af 100644
--- a/devices/ble_hci/common-hal/_bleio/hci.c
+++ b/devices/ble_hci/common-hal/_bleio/hci.c
@@ -1,3 +1,6 @@
+/*
+  This file is part of the ArduinoBLE library.
+  Copyright (c) 2018 Arduino SA. All rights reserved.
 
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -9,7 +12,12 @@
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
 
-#include "HCI.h"
+#include "hci.h"
+#include <string.h>
+
+#include "supervisor/shared/tick.h"
+
+#define ATT_CID       0x0004
 
 #define HCI_COMMAND_PKT 0x01
 #define HCI_ACLDATA_PKT 0x02
@@ -59,617 +67,608 @@
 
 #define HCI_OE_USER_ENDED_CONNECTION 0x13
 
-HCIClass::HCIClass() :
-  _debug(NULL),
-  _recvIndex(0),
-  _pendingPkt(0)
+
+#define RECV_BUFFER_SIZE (3 + 255)
+#define ACL_PKT_BUFFER_SIZE (255)
+
+STATIC bleio_adapter_obj_t *adapter;
+
+STATIC int recv_idx;
+STATIC uint8_t recv_buffer[RECV_BUFFER_SIZE];
+STATIC uint16_t cmd_complete_opcode;
+STATIC int cmd_complete_status;
+STATIC uint8_t cmd_response_len;
+STATIC uint8_t* cmd_response;
+
+STATIC uint8_t max_pkt;
+STATIC uint8_t pending_pkt;
+
+//FIX STATIC uint8_t acl_pkt_buffer[255];
+
+STATIC bool debug = true;
+
+typedef struct __attribute__ ((packed)) {
+    uint8_t evt;
+    uint8_t plen;
+} HCIEventHdr;
+
+STATIC void dumpPkt(const char* prefix, uint8_t plen, uint8_t pdata[])
 {
+    if (debug) {
+        mp_printf(&mp_plat_print, "%s", prefix);
+
+        for (uint8_t i = 0; i < plen; i++) {
+            mp_printf(&mp_plat_print, "%02x", pdata[i]);
+        }
+        mp_printf(&mp_plat_print, "\n");
+    }
 }
 
-HCIClass::~HCIClass()
+
+STATIC void handleAclDataPkt(uint8_t plen, uint8_t pdata[])
 {
+    // typedef struct __attribute__ ((packed)) {
+    //     uint16_t handle;
+    //     uint16_t dlen;
+    //     uint16_t len;
+    //     uint16_t cid;
+    // } HCIACLHdr;
+
+    // HCIACLHdr *aclHdr = (HCIACLHdr*)pdata;
+
+    // uint16_t aclFlags = (aclHdr->handle & 0xf000) >> 12;
+
+    // if ((aclHdr->dlen - 4) != aclHdr->len) {
+    //     // packet is fragmented
+    //     if (aclFlags != 0x01) {
+    //         // copy into ACL buffer
+    //         memcpy(acl_pkt_buffer, &recv_buffer[1], sizeof(HCIACLHdr) + aclHdr->dlen - 4);
+    //     } else {
+    //         // copy next chunk into the buffer
+    //         HCIACLHdr* aclBufferHeader = (HCIACLHdr*)acl_pkt_buffer;
+
+    //         memcpy(&acl_pkt_buffer[sizeof(HCIACLHdr) + aclBufferHeader->dlen - 4], &recv_buffer[1 + sizeof(aclHdr->handle) + sizeof(aclHdr->dlen)], aclHdr->dlen);
+
+    //         aclBufferHeader->dlen += aclHdr->dlen;
+    //         aclHdr = aclBufferHeader;
+    //     }
+    // }
+
+    // if ((aclHdr->dlen - 4) != aclHdr->len) {
+    //     // don't have the full packet yet
+    //     return;
+    // }
+
+    // if (aclHdr->cid == ATT_CID) {
+    //     if (aclFlags == 0x01) {
+    //         // use buffered packet
+    //         ATT.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &acl_pkt_buffer[sizeof(HCIACLHdr)]);
+    //     } else {
+    //         // use the recv buffer
+    //         ATT.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &recv_buffer[1 + sizeof(HCIACLHdr)]);
+    //     }
+    // } else if (aclHdr->cid == SIGNALING_CID) {
+    //     L2CAPSignaling.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &recv_buffer[1 + sizeof(HCIACLHdr)]);
+    // } else {
+    //     struct __attribute__ ((packed)) {
+    //         uint8_t op;
+    //         uint8_t id;
+    //         uint16_t length;
+    //         uint16_t reason;
+    //         uint16_t localCid;
+    //         uint16_t remoteCid;
+    //     } l2capRejectCid= { 0x01, 0x00, 0x006, 0x0002, aclHdr->cid, 0x0000 };
+
+    //     sendAclPkt(aclHdr->handle & 0x0fff, 0x0005, sizeof(l2capRejectCid), &l2capRejectCid);
+    // }
 }
 
-int HCIClass::begin()
+STATIC void handleNumCompPkts(uint16_t handle, uint16_t numPkts)
 {
-  _recvIndex = 0;
-
-  return HCITransport.begin();
+    if (numPkts && pending_pkt > numPkts) {
+        pending_pkt -= numPkts;
+    } else {
+        pending_pkt = 0;
+    }
 }
 
-void HCIClass::end()
+STATIC void handleEventPkt(uint8_t plen, uint8_t pdata[])
 {
-  HCITransport.end();
+    HCIEventHdr *eventHdr = (HCIEventHdr*)pdata;
+
+    if (eventHdr->evt == EVT_DISCONN_COMPLETE) {
+        typedef struct __attribute__ ((packed)) {
+            uint8_t status;
+            uint16_t handle;
+            uint8_t reason;
+        } DisconnComplete;
+
+        DisconnComplete *disconnComplete = (DisconnComplete*)&pdata[sizeof(HCIEventHdr)];
+        (void) disconnComplete;
+        //FIX
+        // ATT.removeConnection(disconnComplete->handle, disconnComplete->reason);
+        // L2CAPSignaling.removeConnection(disconnComplete->handle, disconnComplete->reason);
+
+        hci_leSetAdvertiseEnable(0x01);
+    } else if (eventHdr->evt == EVT_CMD_COMPLETE) {
+        typedef struct __attribute__ ((packed)) {
+            uint8_t ncmd;
+            uint16_t opcode;
+            uint8_t status;
+        } CmdComplete;
+
+        CmdComplete *cmdCompleteHeader = (CmdComplete*)&pdata[sizeof(HCIEventHdr)];
+        cmd_complete_opcode = cmdCompleteHeader->opcode;
+        cmd_complete_status = cmdCompleteHeader->status;
+        cmd_response_len = pdata[1] - sizeof(CmdComplete);
+        cmd_response = &pdata[sizeof(HCIEventHdr) + sizeof(CmdComplete)];
+
+    } else if (eventHdr->evt == EVT_CMD_STATUS) {
+        typedef struct __attribute__ ((packed)) {
+            uint8_t status;
+            uint8_t ncmd;
+            uint16_t opcode;
+        } CmdStatus;
+
+        CmdStatus *cmdStatusHeader = (CmdStatus*)&pdata[sizeof(HCIEventHdr)];
+        cmd_complete_opcode = cmdStatusHeader->opcode;
+        cmd_complete_status = cmdStatusHeader->status;
+        cmd_response_len = 0;
+    } else if (eventHdr->evt == EVT_NUM_COMP_PKTS) {
+        uint8_t numHandles = pdata[sizeof(HCIEventHdr)];
+        uint8_t* data = &pdata[sizeof(HCIEventHdr) + sizeof(numHandles)];
+
+        for (uint8_t i = 0; i < numHandles; i++) {
+            handleNumCompPkts(data[0], data[1]);
+
+            data += 2;
+        }
+    } else if (eventHdr->evt == EVT_LE_META_EVENT) {
+        typedef struct __attribute__ ((packed)) {
+            uint8_t subevent;
+        } LeMetaEventHeader;
+
+        LeMetaEventHeader *leMetaHeader = (LeMetaEventHeader*)&pdata[sizeof(HCIEventHdr)];
+        if (leMetaHeader->subevent == EVT_LE_CONN_COMPLETE) {
+            typedef struct __attribute__ ((packed)) {
+                uint8_t status;
+                uint16_t handle;
+                uint8_t role;
+                uint8_t peerBdaddrType;
+                uint8_t peerBdaddr[6];
+                uint16_t interval;
+                uint16_t latency;
+                uint16_t supervisionTimeout;
+                uint8_t masterClockAccuracy;
+            } EvtLeConnectionComplete;
+
+            EvtLeConnectionComplete *leConnectionComplete = (EvtLeConnectionComplete*)&pdata[sizeof(HCIEventHdr) + sizeof(LeMetaEventHeader)];
+
+            if (leConnectionComplete->status == 0x00) {
+                // ATT.addConnection(leConnectionComplete->handle,
+                //                   leConnectionComplete->role,
+                //                   leConnectionComplete->peerBdaddrType,
+                //                   leConnectionComplete->peerBdaddr,
+                //                   leConnectionComplete->interval,
+                //                   leConnectionComplete->latency,
+                //                   leConnectionComplete->supervisionTimeout,
+                //                   leConnectionComplete->masterClockAccuracy);
+
+                // L2CAPSignaling.addConnection(leConnectionComplete->handle,
+                //                              leConnectionComplete->role,
+                //                              leConnectionComplete->peerBdaddrType,
+                //                              leConnectionComplete->peerBdaddr,
+                //                              leConnectionComplete->interval,
+                //                              leConnectionComplete->latency,
+                //                              leConnectionComplete->supervisionTimeout,
+                //                              leConnectionComplete->masterClockAccuracy);
+            }
+        } else if (leMetaHeader->subevent == EVT_LE_ADVERTISING_REPORT) {
+            typedef struct __attribute__ ((packed)) {
+                uint8_t status;
+                uint8_t type;
+                uint8_t peerBdaddrType;
+                uint8_t peerBdaddr[6];
+                uint8_t eirLength;
+                uint8_t eirData[31];
+            } EvtLeAdvertisingReport;
+
+            EvtLeAdvertisingReport*leAdvertisingReport = (EvtLeAdvertisingReport*)&pdata[sizeof(HCIEventHdr) + sizeof(LeMetaEventHeader)];
+
+            if (leAdvertisingReport->status == 0x01) {
+                // last byte is RSSI
+                //FIX int8_t rssi = leAdvertisingReport->eirData[leAdvertisingReport->eirLength];
+
+                // GAP.handleLeAdvertisingReport(leAdvertisingReport->type,
+                //                               leAdvertisingReport->peerBdaddrType,
+                //                               leAdvertisingReport->peerBdaddr,
+                //                               leAdvertisingReport->eirLength,
+                //                               leAdvertisingReport->eirData,
+                //                               rssi);
+
+            }
+        }
+    }
 }
 
-void HCIClass::poll()
-{
-  poll(0);
+void hci_init(bleio_adapter_obj_t *adapter_in) {
+    adapter = adapter_in;
+    recv_idx = 0;
+    pending_pkt = 0;
 }
 
-void HCIClass::poll(unsigned long timeout)
-{
-#if defined(ARDUINO_AVR_UNO_WIFI_REV2) || defined(ARDUINO_METRO_M4_AIRLIFT_LITE)
-  digitalWrite(NINA_RTS, LOW);
-#endif
+void hci_poll(void) {
+    // Assert RTS low to say we're ready to read data.
+    common_hal_digitalio_digitalinout_set_value(&adapter->rts_digitalinout, false);
+
+    int errcode = 0;
+    while (common_hal_busio_uart_rx_characters_available(&adapter->hci_uart)) {
+        // Read just one character.
+        common_hal_busio_uart_read(&adapter->hci_uart, recv_buffer + recv_idx, 1, &errcode);
+        recv_idx++;
+
+        if (recv_buffer[0] == HCI_ACLDATA_PKT) {
+            if (recv_idx > 5 && recv_idx >= (5 + (recv_buffer[3] + (recv_buffer[4] << 8)))) {
+                if (debug) {
+                    dumpPkt("HCI ACLDATA RX <- ", recv_idx, recv_buffer);
+                }
+                // Hold data while processing packet.
+                common_hal_digitalio_digitalinout_set_value(&adapter->rts_digitalinout, true);
+                size_t pktLen = recv_idx - 1;
+                recv_idx = 0;
+
+                handleAclDataPkt(pktLen, &recv_buffer[1]);
+
+                common_hal_digitalio_digitalinout_set_value(&adapter->rts_digitalinout, false);
+            }
+        } else if (recv_buffer[0] == HCI_EVENT_PKT) {
+            if (recv_idx > 3 && recv_idx >= (3 + recv_buffer[2])) {
+                if (debug) {
+                    dumpPkt("HCI EVENT RX <- ", recv_idx, recv_buffer);
+                }
+                // Hold data while processing packet.
+                common_hal_digitalio_digitalinout_set_value(&adapter->rts_digitalinout, true);
+                // Received full event. Reset buffer and handle packet.
+                size_t pktLen = recv_idx - 1;
+                recv_idx = 0;
+
+                handleEventPkt(pktLen, &recv_buffer[1]);
+
+                common_hal_digitalio_digitalinout_set_value(&adapter->rts_digitalinout, false);
+            }
+        } else {
+            recv_idx = 0;
+        }
+    }
 
-  if (timeout) {
-    HCITransport.wait(timeout);
-  }
+    common_hal_digitalio_digitalinout_set_value(&adapter->rts_digitalinout, true);
+}
 
-  while (HCITransport.available()) {
-    byte b = HCITransport.read();
 
-    _recvBuffer[_recvIndex++] = b;
+int hci_sendCommand(uint8_t ogf, uint16_t ocf, uint8_t plen, void* parameters)
+{
+    uint16_t opcode = ogf << 10 | ocf;
 
-    if (_recvBuffer[0] == HCI_ACLDATA_PKT) {
-      if (_recvIndex > 5 && _recvIndex >= (5 + (_recvBuffer[3] + (_recvBuffer[4] << 8)))) {
-        if (_debug) {
-          dumpPkt("HCI ACLDATA RX <- ", _recvIndex, _recvBuffer);
-        }
-#if defined(ARDUINO_AVR_UNO_WIFI_REV2) || defined(ARDUINO_METRO_M4_AIRLIFT_LITE)
-        digitalWrite(NINA_RTS, HIGH);
-#endif
-        int pktLen = _recvIndex - 1;
-        _recvIndex = 0;
+    struct __attribute__ ((packed)) {
+        uint8_t pktType;
+        uint16_t opcode;
+        uint8_t plen;
+    } pktHdr = {HCI_COMMAND_PKT, opcode, plen};
 
-        handleAclDataPkt(pktLen, &_recvBuffer[1]);
+    uint8_t txBuffer[sizeof(pktHdr) + plen];
+    memcpy(txBuffer, &pktHdr, sizeof(pktHdr));
+    memcpy(&txBuffer[sizeof(pktHdr)], parameters, plen);
 
-#if defined(ARDUINO_AVR_UNO_WIFI_REV2) || defined(ARDUINO_METRO_M4_AIRLIFT_LITE)
-        digitalWrite(NINA_RTS, LOW);
-#endif
-      }
-    } else if (_recvBuffer[0] == HCI_EVENT_PKT) {
-      if (_recvIndex > 3 && _recvIndex >= (3 + _recvBuffer[2])) {
-        if (_debug) {
-          dumpPkt("HCI EVENT RX <- ", _recvIndex, _recvBuffer);
-        }
-#if defined(ARDUINO_AVR_UNO_WIFI_REV2) || defined(ARDUINO_METRO_M4_AIRLIFT_LITE)
-        digitalWrite(NINA_RTS, HIGH);
-#endif
-        // received full event
-        int pktLen = _recvIndex - 1;
-        _recvIndex = 0;
+    if (debug) {
+        dumpPkt("HCI COMMAND TX -> ", sizeof(pktHdr) + plen, txBuffer);
+    }
 
-        handleEventPkt(pktLen, &_recvBuffer[1]);
+    int errcode = 0;
+    common_hal_busio_uart_write(&adapter->hci_uart, txBuffer, sizeof(pktHdr) + plen, &errcode);
+    if (errcode) {
+        return -1;
+    }
 
-#if defined(ARDUINO_AVR_UNO_WIFI_REV2) || defined(ARDUINO_METRO_M4_AIRLIFT_LITE)
-        digitalWrite(NINA_RTS, LOW);
-#endif
-      }
-    } else {
-      _recvIndex = 0;
+    cmd_complete_opcode = 0xffff;
+    cmd_complete_status = -1;
 
-      if (_debug) {
-        _debug->println(b, HEX);
-      }
+    // Wait up to one second for a response.
+    for (uint64_t start = supervisor_ticks_ms64();
+         cmd_complete_opcode != opcode && supervisor_ticks_ms64() < (start + 5000);
+        ) {
+        hci_poll();
     }
-  }
 
-#if defined(ARDUINO_AVR_UNO_WIFI_REV2) || defined(ARDUINO_METRO_M4_AIRLIFT_LITE)
-  digitalWrite(NINA_RTS, HIGH);
-#endif
+    return cmd_complete_status;
 }
 
-int HCIClass::reset()
-{
-  return sendCommand(OGF_HOST_CTL << 10 | OCF_RESET);
+int hci_reset(void) {
+    return hci_sendCommand(OGF_HOST_CTL, OCF_RESET, 0, NULL);
 }
 
-int HCIClass::readLocalVersion(uint8_t& hciVer, uint16_t& hciRev, uint8_t& lmpVer, uint16_t& manufacturer, uint16_t& lmpSubVer)
-{
-  int result = sendCommand(OGF_INFO_PARAM << 10 | OCF_READ_LOCAL_VERSION);
-
-  if (result == 0) {
-    struct __attribute__ ((packed)) HCILocalVersion {
-      uint8_t hciVer;
-      uint16_t hciRev;
-      uint8_t lmpVer;
-      uint16_t manufacturer;
-      uint16_t lmpSubVer;
-    } *localVersion = (HCILocalVersion*)_cmdResponse;
-
-    hciVer = localVersion->hciVer;
-    hciRev = localVersion->hciRev;
-    lmpVer = localVersion->lmpVer;
-    manufacturer = localVersion->manufacturer;
-    lmpSubVer = localVersion->lmpSubVer;
-  }
-
-  return result;
+int hci_readLocalVersion(uint8_t *hciVer, uint16_t *hciRev, uint8_t *lmpVer, uint16_t *manufacturer, uint16_t *lmpSubVer) {
+    int result = hci_sendCommand(OGF_INFO_PARAM, OCF_READ_LOCAL_VERSION, 0, NULL);
+
+    if (result == 0) {
+        typedef struct __attribute__ ((packed)) {
+            uint8_t hciVer;
+            uint16_t hciRev;
+            uint8_t lmpVer;
+            uint16_t manufacturer;
+            uint16_t lmpSubVer;
+        } HCILocalVersion;
+
+        HCILocalVersion *localVersion = (HCILocalVersion*)cmd_response;
+        *hciVer = localVersion->hciVer;
+        *hciRev = localVersion->hciRev;
+        *lmpVer = localVersion->lmpVer;
+        *manufacturer = localVersion->manufacturer;
+        *lmpSubVer = localVersion->lmpSubVer;
+    }
+
+    return result;
 }
 
-int HCIClass::readBdAddr(uint8_t addr[6])
-{
-  int result = sendCommand(OGF_INFO_PARAM << 10 | OCF_READ_BD_ADDR);
+int hci_readBdAddr(uint8_t addr[6]) {
+    int result = hci_sendCommand(OGF_INFO_PARAM, OCF_READ_BD_ADDR, 0, NULL);
 
-  if (result == 0) {
-    memcpy(addr, _cmdResponse, 6);
-  }
+    if (result == 0) {
+        memcpy(addr, cmd_response, 6);
+    }
 
-  return result;
+    return result;
 }
 
-int HCIClass::readRssi(uint16_t handle)
-{
-  int result = sendCommand(OGF_STATUS_PARAM << 10 | OCF_READ_RSSI, sizeof(handle), &handle);
-  int rssi = 127;
+int hci_readRssi(uint16_t handle) {
+    int result = hci_sendCommand(OGF_STATUS_PARAM, OCF_READ_RSSI, sizeof(handle), &handle);
+    int rssi = 127;
 
-  if (result == 0) {
-    struct __attribute__ ((packed)) HCIReadRssi {
-      uint16_t handle;
-        int8_t rssi;
-    } *readRssi = (HCIReadRssi*)_cmdResponse;
+    if (result == 0) {
+        typedef struct __attribute__ ((packed)) {
+            uint16_t handle;
+            int8_t rssi;
+        } HCIReadRssi;
 
-    if (readRssi->handle == handle) {
-      rssi = readRssi->rssi;
+        HCIReadRssi *readRssi = (HCIReadRssi*)cmd_response;
+        if (readRssi->handle == handle) {
+            rssi = readRssi->rssi;
+        }
     }
-  }
 
-  return rssi;
+    return rssi;
 }
 
-int HCIClass::setEventMask(uint64_t eventMask)
-{
-  return sendCommand(OGF_HOST_CTL << 10 | OCF_SET_EVENT_MASK, sizeof(eventMask), &eventMask);
+int hci_setEventMask(uint64_t eventMask) {
+    return hci_sendCommand(OGF_HOST_CTL, OCF_SET_EVENT_MASK, sizeof(eventMask), &eventMask);
 }
 
-int HCIClass::readLeBufferSize(uint16_t& pktLen, uint8_t& maxPkt)
+int hci_readLeBufferSize(uint16_t *pktLen, uint8_t *maxPkt)
 {
-  int result = sendCommand(OGF_LE_CTL << 10 | OCF_LE_READ_BUFFER_SIZE);
+    int result = hci_sendCommand(OGF_LE_CTL, OCF_LE_READ_BUFFER_SIZE, 0, NULL);
 
-  if (result == 0) {
-    struct __attribute__ ((packed)) HCILeBufferSize {
-      uint16_t pktLen;
-      uint8_t maxPkt;
-    } *leBufferSize = (HCILeBufferSize*)_cmdResponse;
+    if (result == 0) {
+        typedef struct __attribute__ ((packed)) {
+            uint16_t pktLen;
+            uint8_t maxPkt;
+        } HCILeBufferSize;
 
-    pktLen = leBufferSize->pktLen;
-    _maxPkt = maxPkt = leBufferSize->maxPkt;
+        HCILeBufferSize *leBufferSize = (HCILeBufferSize*)cmd_response;
+        *pktLen = leBufferSize->pktLen;
+        *maxPkt = leBufferSize->maxPkt;
 
 #ifndef __AVR__
-    ATT.setMaxMtu(pktLen - 9); // max pkt len - ACL header size
+        // FIX (needed?) ATT.setMaxMtu(pktLen - 9); // max pkt len - ACL header size
 #endif
-  }
+    }
 
-  return result;
+    return result;
 }
 
-int HCIClass::leSetRandomAddress(uint8_t addr[6])
+int hci_leSetRandomAddress(uint8_t addr[6])
 {
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_SET_RANDOM_ADDRESS, 6, addr);
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_SET_RANDOM_ADDRESS, 6, addr);
 }
 
-int HCIClass::leSetAdvertisingParameters(uint16_t minInterval, uint16_t maxInterval,
+int hci_leSetAdvertisingParameters(uint16_t minInterval, uint16_t maxInterval,
                                          uint8_t advType, uint8_t ownBdaddrType,
                                          uint8_t directBdaddrType, uint8_t directBdaddr[6],
                                          uint8_t chanMap,
                                          uint8_t filter)
 {
-  struct __attribute__ ((packed)) HCILeAdvertisingParameters {
-    uint16_t minInterval;
-    uint16_t maxInterval;
-    uint8_t advType;
-    uint8_t ownBdaddrType;
-    uint8_t directBdaddrType;
-    uint8_t directBdaddr[6];
-    uint8_t chanMap;
-    uint8_t filter;
-  } leAdvertisingParamters;
-
-  leAdvertisingParamters.minInterval = minInterval;
-  leAdvertisingParamters.maxInterval = maxInterval;
-  leAdvertisingParamters.advType = advType;
-  leAdvertisingParamters.ownBdaddrType = ownBdaddrType;
-  leAdvertisingParamters.directBdaddrType = directBdaddrType;
-  memcpy(leAdvertisingParamters.directBdaddr, directBdaddr, 6);
-  leAdvertisingParamters.chanMap = chanMap;
-  leAdvertisingParamters.filter = filter;
-
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_SET_ADVERTISING_PARAMETERS, sizeof(leAdvertisingParamters), &leAdvertisingParamters);
+    struct __attribute__ ((packed)) HCILeAdvertisingParameters {
+        uint16_t minInterval;
+        uint16_t maxInterval;
+        uint8_t advType;
+        uint8_t ownBdaddrType;
+        uint8_t directBdaddrType;
+        uint8_t directBdaddr[6];
+        uint8_t chanMap;
+        uint8_t filter;
+    } leAdvertisingParamters;
+
+    leAdvertisingParamters.minInterval = minInterval;
+    leAdvertisingParamters.maxInterval = maxInterval;
+    leAdvertisingParamters.advType = advType;
+    leAdvertisingParamters.ownBdaddrType = ownBdaddrType;
+    leAdvertisingParamters.directBdaddrType = directBdaddrType;
+    memcpy(leAdvertisingParamters.directBdaddr, directBdaddr, 6);
+    leAdvertisingParamters.chanMap = chanMap;
+    leAdvertisingParamters.filter = filter;
+
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_SET_ADVERTISING_PARAMETERS, sizeof(leAdvertisingParamters), &leAdvertisingParamters);
 }
 
-int HCIClass::leSetAdvertisingData(uint8_t length, uint8_t data[])
+int hci_leSetAdvertisingData(uint8_t length, uint8_t data[])
 {
-  struct __attribute__ ((packed)) HCILeAdvertisingData {
-    uint8_t length;
-    uint8_t data[31];
-  } leAdvertisingData;
+    struct __attribute__ ((packed)) HCILeAdvertisingData {
+        uint8_t length;
+        uint8_t data[31];
+    } leAdvertisingData;
 
-  memset(&leAdvertisingData, 0, sizeof(leAdvertisingData));
-  leAdvertisingData.length = length;
-  memcpy(leAdvertisingData.data, data, length);
+    memset(&leAdvertisingData, 0, sizeof(leAdvertisingData));
+    leAdvertisingData.length = length;
+    memcpy(leAdvertisingData.data, data, length);
 
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_SET_ADVERTISING_DATA, sizeof(leAdvertisingData), &leAdvertisingData);
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_SET_ADVERTISING_DATA, sizeof(leAdvertisingData), &leAdvertisingData);
 }
 
-int HCIClass::leSetScanResponseData(uint8_t length, uint8_t data[])
+int hci_leSetScanResponseData(uint8_t length, uint8_t data[])
 {
-  struct __attribute__ ((packed)) HCILeScanResponseData {
-    uint8_t length;
-    uint8_t data[31];
-  } leScanResponseData;
-
-  memset(&leScanResponseData, 0, sizeof(leScanResponseData));
-  leScanResponseData.length = length;
-  memcpy(leScanResponseData.data, data, length);
-
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_SET_SCAN_RESPONSE_DATA, sizeof(leScanResponseData), &leScanResponseData);
-}
+    struct __attribute__ ((packed)) HCILeScanResponseData {
+        uint8_t length;
+        uint8_t data[31];
+    } leScanResponseData;
 
-int HCIClass::leSetAdvertiseEnable(uint8_t enable)
-{
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_SET_ADVERTISE_ENABLE, sizeof(enable), &enable);
-}
+    memset(&leScanResponseData, 0, sizeof(leScanResponseData));
+    leScanResponseData.length = length;
+    memcpy(leScanResponseData.data, data, length);
 
-int HCIClass::leSetScanParameters(uint8_t type, uint16_t interval, uint16_t window,
-                          uint8_t ownBdaddrType, uint8_t filter)
-{
-  struct __attribute__ ((packed)) HCILeSetScanParameters {
-    uint8_t type;
-    uint16_t interval;
-    uint16_t window;
-    uint8_t ownBdaddrType;
-    uint8_t filter;
-  } leScanParameters;
-
-  leScanParameters.type = type;
-  leScanParameters.interval = interval;
-  leScanParameters.window = window;
-  leScanParameters.ownBdaddrType = ownBdaddrType;
-  leScanParameters.filter = filter;
-
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_SET_SCAN_PARAMETERS, sizeof(leScanParameters), &leScanParameters);
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_SET_SCAN_RESPONSE_DATA, sizeof(leScanResponseData), &leScanResponseData);
 }
 
-int HCIClass::leSetScanEnable(uint8_t enabled, uint8_t duplicates)
+int hci_leSetAdvertiseEnable(uint8_t enable)
 {
-  struct __attribute__ ((packed)) HCILeSetScanEnableData {
-    uint8_t enabled;
-    uint8_t duplicates;
-  } leScanEnableData;
-
-  leScanEnableData.enabled = enabled;
-  leScanEnableData.duplicates = duplicates;
-
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_SET_SCAN_ENABLE, sizeof(leScanEnableData), &leScanEnableData);
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_SET_ADVERTISE_ENABLE, sizeof(enable), &enable);
 }
 
-int HCIClass::leCreateConn(uint16_t interval, uint16_t window, uint8_t initiatorFilter,
-                            uint8_t peerBdaddrType, uint8_t peerBdaddr[6], uint8_t ownBdaddrType,
-                            uint16_t minInterval, uint16_t maxInterval, uint16_t latency,
-                            uint16_t supervisionTimeout, uint16_t minCeLength, uint16_t maxCeLength)
+int hci_leSetScanParameters(uint8_t type, uint16_t interval, uint16_t window,
+                                  uint8_t ownBdaddrType, uint8_t filter)
 {
-  struct __attribute__ ((packed)) HCILeCreateConnData {
-    uint16_t interval;
-    uint16_t window;
-    uint8_t initiatorFilter;
-    uint8_t peerBdaddrType;
-    uint8_t peerBdaddr[6];
-    uint8_t ownBdaddrType;
-    uint16_t minInterval;
-    uint16_t maxInterval;
-    uint16_t latency;
-    uint16_t supervisionTimeout;
-    uint16_t minCeLength;
-    uint16_t maxCeLength;
-  } leCreateConnData;
-
-  leCreateConnData.interval = interval;
-  leCreateConnData.window = window;
-  leCreateConnData.initiatorFilter = initiatorFilter;
-  leCreateConnData.peerBdaddrType = peerBdaddrType;
-  memcpy(leCreateConnData.peerBdaddr, peerBdaddr, sizeof(leCreateConnData.peerBdaddr));
-  leCreateConnData.ownBdaddrType = ownBdaddrType;
-  leCreateConnData.minInterval = minInterval;
-  leCreateConnData.maxInterval = maxInterval;
-  leCreateConnData.latency = latency;
-  leCreateConnData.supervisionTimeout = supervisionTimeout;
-  leCreateConnData.minCeLength = minCeLength;
-  leCreateConnData.maxCeLength = maxCeLength;
-
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_CREATE_CONN, sizeof(leCreateConnData), &leCreateConnData);
+    struct __attribute__ ((packed)) HCILeSetScanParameters {
+        uint8_t type;
+        uint16_t interval;
+        uint16_t window;
+        uint8_t ownBdaddrType;
+        uint8_t filter;
+    } leScanParameters;
+
+    leScanParameters.type = type;
+    leScanParameters.interval = interval;
+    leScanParameters.window = window;
+    leScanParameters.ownBdaddrType = ownBdaddrType;
+    leScanParameters.filter = filter;
+
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_SET_SCAN_PARAMETERS, sizeof(leScanParameters), &leScanParameters);
 }
 
-int HCIClass::leCancelConn()
+int hci_leSetScanEnable(uint8_t enabled, uint8_t duplicates)
 {
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_CANCEL_CONN, 0, NULL);
-}
+    struct __attribute__ ((packed)) HCILeSetScanEnableData {
+        uint8_t enabled;
+        uint8_t duplicates;
+    } leScanEnableData;
 
-int HCIClass::leConnUpdate(uint16_t handle, uint16_t minInterval, uint16_t maxInterval,
-                          uint16_t latency, uint16_t supervisionTimeout)
-{
-  struct __attribute__ ((packed)) HCILeConnUpdateData {
-    uint16_t handle;
-    uint16_t minInterval;
-    uint16_t maxInterval;
-    uint16_t latency;
-    uint16_t supervisionTimeout;
-    uint16_t minCeLength;
-    uint16_t maxCeLength;
-  } leConnUpdateData;
-
-  leConnUpdateData.handle = handle;
-  leConnUpdateData.minInterval = minInterval;
-  leConnUpdateData.maxInterval = maxInterval;
-  leConnUpdateData.latency = latency;
-  leConnUpdateData.supervisionTimeout = supervisionTimeout;
-  leConnUpdateData.minCeLength = 0x0004;
-  leConnUpdateData.maxCeLength = 0x0006;
-
-  return sendCommand(OGF_LE_CTL << 10 | OCF_LE_CONN_UPDATE, sizeof(leConnUpdateData), &leConnUpdateData);
-}
+    leScanEnableData.enabled = enabled;
+    leScanEnableData.duplicates = duplicates;
 
-int HCIClass::sendAclPkt(uint16_t handle, uint8_t cid, uint8_t plen, void* data)
-{
-  while (_pendingPkt >= _maxPkt) {
-    poll();
-  }
-
-  struct __attribute__ ((packed)) HCIACLHdr {
-    uint8_t pktType;
-    uint16_t handle;
-    uint16_t dlen;
-    uint16_t plen;
-    uint16_t cid;
-  } aclHdr = { HCI_ACLDATA_PKT, handle, uint8_t(plen + 4), plen, cid };
-
-  uint8_t txBuffer[sizeof(aclHdr) + plen];
-  memcpy(txBuffer, &aclHdr, sizeof(aclHdr));
-  memcpy(&txBuffer[sizeof(aclHdr)], data, plen);
-
-  if (_debug) {
-    dumpPkt("HCI ACLDATA TX -> ", sizeof(aclHdr) + plen, txBuffer);
-  }
-
-  _pendingPkt++;
-  HCITransport.write(txBuffer, sizeof(aclHdr) + plen);
-
-  return 0;
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_SET_SCAN_ENABLE, sizeof(leScanEnableData), &leScanEnableData);
 }
 
-int HCIClass::disconnect(uint16_t handle)
+int hci_leCreateConn(uint16_t interval, uint16_t window, uint8_t initiatorFilter,
+                           uint8_t peerBdaddrType, uint8_t peerBdaddr[6], uint8_t ownBdaddrType,
+                           uint16_t minInterval, uint16_t maxInterval, uint16_t latency,
+                           uint16_t supervisionTimeout, uint16_t minCeLength, uint16_t maxCeLength)
 {
-    struct __attribute__ ((packed)) HCIDisconnectData {
-    uint16_t handle;
-    uint8_t reason;
-  } disconnectData = { handle, HCI_OE_USER_ENDED_CONNECTION };
-
-  return sendCommand(OGF_LINK_CTL << 10 | OCF_DISCONNECT, sizeof(disconnectData), &disconnectData);
+    struct __attribute__ ((packed)) HCILeCreateConnData {
+        uint16_t interval;
+        uint16_t window;
+        uint8_t initiatorFilter;
+        uint8_t peerBdaddrType;
+        uint8_t peerBdaddr[6];
+        uint8_t ownBdaddrType;
+        uint16_t minInterval;
+        uint16_t maxInterval;
+        uint16_t latency;
+        uint16_t supervisionTimeout;
+        uint16_t minCeLength;
+        uint16_t maxCeLength;
+    } leCreateConnData;
+
+    leCreateConnData.interval = interval;
+    leCreateConnData.window = window;
+    leCreateConnData.initiatorFilter = initiatorFilter;
+    leCreateConnData.peerBdaddrType = peerBdaddrType;
+    memcpy(leCreateConnData.peerBdaddr, peerBdaddr, sizeof(leCreateConnData.peerBdaddr));
+    leCreateConnData.ownBdaddrType = ownBdaddrType;
+    leCreateConnData.minInterval = minInterval;
+    leCreateConnData.maxInterval = maxInterval;
+    leCreateConnData.latency = latency;
+    leCreateConnData.supervisionTimeout = supervisionTimeout;
+    leCreateConnData.minCeLength = minCeLength;
+    leCreateConnData.maxCeLength = maxCeLength;
+
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_CREATE_CONN, sizeof(leCreateConnData), &leCreateConnData);
 }
 
-void HCIClass::debug(Stream& stream)
+int hci_leCancelConn()
 {
-  _debug = &stream;
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_CANCEL_CONN, 0, NULL);
 }
 
-void HCIClass::noDebug()
+int hci_leConnUpdate(uint16_t handle, uint16_t minInterval, uint16_t maxInterval,
+                           uint16_t latency, uint16_t supervisionTimeout)
 {
-  _debug = NULL;
+    struct __attribute__ ((packed)) HCILeConnUpdateData {
+        uint16_t handle;
+        uint16_t minInterval;
+        uint16_t maxInterval;
+        uint16_t latency;
+        uint16_t supervisionTimeout;
+        uint16_t minCeLength;
+        uint16_t maxCeLength;
+    } leConnUpdateData;
+
+    leConnUpdateData.handle = handle;
+    leConnUpdateData.minInterval = minInterval;
+    leConnUpdateData.maxInterval = maxInterval;
+    leConnUpdateData.latency = latency;
+    leConnUpdateData.supervisionTimeout = supervisionTimeout;
+    leConnUpdateData.minCeLength = 0x0004;
+    leConnUpdateData.maxCeLength = 0x0006;
+
+    return hci_sendCommand(OGF_LE_CTL, OCF_LE_CONN_UPDATE, sizeof(leConnUpdateData), &leConnUpdateData);
 }
 
-int HCIClass::sendCommand(uint16_t opcode, uint8_t plen, void* parameters)
-{
-  struct __attribute__ ((packed)) {
-    uint8_t pktType;
-    uint16_t opcode;
-    uint8_t plen;
-  } pktHdr = {HCI_COMMAND_PKT, opcode, plen};
-
-  uint8_t txBuffer[sizeof(pktHdr) + plen];
-  memcpy(txBuffer, &pktHdr, sizeof(pktHdr));
-  memcpy(&txBuffer[sizeof(pktHdr)], parameters, plen);
-
-  if (_debug) {
-    dumpPkt("HCI COMMAND TX -> ", sizeof(pktHdr) + plen, txBuffer);
-  }
-
-  HCITransport.write(txBuffer, sizeof(pktHdr) + plen);
-
-  _cmdCompleteOpcode = 0xffff;
-  _cmdCompleteStatus = -1;
-
-  for (unsigned long start = millis(); _cmdCompleteOpcode != opcode && millis() < (start + 1000);) {
-    poll();
-  }
+int hci_sendAclPkt(uint16_t handle, uint8_t cid, uint8_t plen, void* data) {
+    while (pending_pkt >= max_pkt) {
+        hci_poll();
+    }
 
-  return _cmdCompleteStatus;
-}
+    typedef struct __attribute__ ((packed)) {
+        uint8_t pktType;
+        uint16_t handle;
+        uint16_t dlen;
+        uint16_t plen;
+        uint16_t cid;
+    }  HCIACLHdr;
 
-void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[])
-{
-  struct __attribute__ ((packed)) HCIACLHdr {
-    uint16_t handle;
-    uint16_t dlen;
-    uint16_t len;
-    uint16_t cid;
-  } *aclHdr = (HCIACLHdr*)pdata;
-
-  uint16_t aclFlags = (aclHdr->handle & 0xf000) >> 12;
-
-  if ((aclHdr->dlen - 4) != aclHdr->len) {
-    // packet is fragmented
-    if (aclFlags != 0x01) {
-      // copy into ACL buffer
-      memcpy(_aclPktBuffer, &_recvBuffer[1], sizeof(HCIACLHdr) + aclHdr->dlen - 4);
-    } else {
-      // copy next chunk into the buffer
-      HCIACLHdr* aclBufferHeader = (HCIACLHdr*)_aclPktBuffer;
+    HCIACLHdr aclHdr = { HCI_ACLDATA_PKT, handle, (uint8_t)(plen + 4), plen, cid };
 
-      memcpy(&_aclPktBuffer[sizeof(HCIACLHdr) + aclBufferHeader->dlen - 4], &_recvBuffer[1 + sizeof(aclHdr->handle) + sizeof(aclHdr->dlen)], aclHdr->dlen);
+    uint8_t txBuffer[sizeof(aclHdr) + plen];
+    memcpy(txBuffer, &aclHdr, sizeof(aclHdr));
+    memcpy(&txBuffer[sizeof(aclHdr)], data, plen);
 
-      aclBufferHeader->dlen += aclHdr->dlen;
-      aclHdr = aclBufferHeader;
+    if (debug) {
+        dumpPkt("HCI ACLDATA TX -> ", sizeof(aclHdr) + plen, txBuffer);
     }
-  }
 
-  if ((aclHdr->dlen - 4) != aclHdr->len) {
-    // don't have the full packet yet
-    return;
-  }
+    pending_pkt++;
 
-  if (aclHdr->cid == ATT_CID) {
-    if (aclFlags == 0x01) {
-      // use buffered packet
-      ATT.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &_aclPktBuffer[sizeof(HCIACLHdr)]);
-    } else {
-      // use the recv buffer
-      ATT.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]);
+    int errcode = 0;
+    common_hal_busio_uart_write(&adapter->hci_uart, txBuffer, sizeof(aclHdr) + plen, &errcode);
+    if (errcode) {
+        return -1;
     }
-  } else if (aclHdr->cid == SIGNALING_CID) {
-    L2CAPSignaling.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]);
-  } else {
-    struct __attribute__ ((packed)) {
-      uint8_t op;
-      uint8_t id;
-      uint16_t length;
-      uint16_t reason;
-      uint16_t localCid;
-      uint16_t remoteCid;
-    } l2capRejectCid= { 0x01, 0x00, 0x006, 0x0002, aclHdr->cid, 0x0000 };
-
-    sendAclPkt(aclHdr->handle & 0x0fff, 0x0005, sizeof(l2capRejectCid), &l2capRejectCid);
-  }
-}
 
-void HCIClass::handleNumCompPkts(uint16_t /*handle*/, uint16_t numPkts)
-{
-  if (numPkts && _pendingPkt > numPkts) {
-    _pendingPkt -= numPkts;
-  } else {
-    _pendingPkt = 0;
-  }
+    return 0;
 }
 
-void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[])
+int hci_disconnect(uint16_t handle)
 {
-  struct __attribute__ ((packed)) HCIEventHdr {
-    uint8_t evt;
-    uint8_t plen;
-  } *eventHdr = (HCIEventHdr*)pdata;
-
-  if (eventHdr->evt == EVT_DISCONN_COMPLETE) {
-    struct __attribute__ ((packed)) DisconnComplete {
-      uint8_t status;
-      uint16_t handle;
-      uint8_t reason;
-    } *disconnComplete = (DisconnComplete*)&pdata[sizeof(HCIEventHdr)];
-
-    ATT.removeConnection(disconnComplete->handle, disconnComplete->reason);
-    L2CAPSignaling.removeConnection(disconnComplete->handle, disconnComplete->reason);
-
-    HCI.leSetAdvertiseEnable(0x01);
-  } else if (eventHdr->evt == EVT_CMD_COMPLETE) {
-    struct __attribute__ ((packed)) CmdComplete {
-      uint8_t ncmd;
-      uint16_t opcode;
-      uint8_t status;
-    } *cmdCompleteHeader = (CmdComplete*)&pdata[sizeof(HCIEventHdr)];
-
-    _cmdCompleteOpcode = cmdCompleteHeader->opcode;
-    _cmdCompleteStatus = cmdCompleteHeader->status;
-    _cmdResponseLen = pdata[1] - sizeof(CmdComplete);
-    _cmdResponse = &pdata[sizeof(HCIEventHdr) + sizeof(CmdComplete)];
-
-  } else if (eventHdr->evt == EVT_CMD_STATUS) {
-    struct __attribute__ ((packed)) CmdStatus {
-      uint8_t status;
-      uint8_t ncmd;
-      uint16_t opcode;
-    } *cmdStatusHeader = (CmdStatus*)&pdata[sizeof(HCIEventHdr)];
-
-    _cmdCompleteOpcode = cmdStatusHeader->opcode;
-    _cmdCompleteStatus = cmdStatusHeader->status;
-    _cmdResponseLen = 0;
-  } else if (eventHdr->evt == EVT_NUM_COMP_PKTS) {
-    uint8_t numHandles = pdata[sizeof(HCIEventHdr)];
-    uint16_t* data = (uint16_t*)&pdata[sizeof(HCIEventHdr) + sizeof(numHandles)];
-
-    for (uint8_t i = 0; i < numHandles; i++) {
-      handleNumCompPkts(data[0], data[1]);
-
-      data += 2;
-    }
-  } else if (eventHdr->evt == EVT_LE_META_EVENT) {
-    struct __attribute__ ((packed)) LeMetaEventHeader {
-      uint8_t subevent;
-    } *leMetaHeader = (LeMetaEventHeader*)&pdata[sizeof(HCIEventHdr)];
-
-    if (leMetaHeader->subevent == EVT_LE_CONN_COMPLETE) {
-      struct __attribute__ ((packed)) EvtLeConnectionComplete {
-        uint8_t status;
+    struct __attribute__ ((packed)) HCIDisconnectData {
         uint16_t handle;
-        uint8_t role;
-        uint8_t peerBdaddrType;
-        uint8_t peerBdaddr[6];
-        uint16_t interval;
-        uint16_t latency;
-        uint16_t supervisionTimeout;
-        uint8_t masterClockAccuracy;
-      } *leConnectionComplete = (EvtLeConnectionComplete*)&pdata[sizeof(HCIEventHdr) + sizeof(LeMetaEventHeader)];
-
-      if (leConnectionComplete->status == 0x00) {
-        ATT.addConnection(leConnectionComplete->handle,
-                          leConnectionComplete->role,
-                          leConnectionComplete->peerBdaddrType,
-                          leConnectionComplete->peerBdaddr,
-                          leConnectionComplete->interval,
-                          leConnectionComplete->latency,
-                          leConnectionComplete->supervisionTimeout,
-                          leConnectionComplete->masterClockAccuracy);
-
-        L2CAPSignaling.addConnection(leConnectionComplete->handle,
-                              leConnectionComplete->role,
-                              leConnectionComplete->peerBdaddrType,
-                              leConnectionComplete->peerBdaddr,
-                              leConnectionComplete->interval,
-                              leConnectionComplete->latency,
-                              leConnectionComplete->supervisionTimeout,
-                              leConnectionComplete->masterClockAccuracy);
-      }
-    } else if (leMetaHeader->subevent == EVT_LE_ADVERTISING_REPORT) {
-      struct __attribute__ ((packed)) EvtLeAdvertisingReport {
-        uint8_t status;
-        uint8_t type;
-        uint8_t peerBdaddrType;
-        uint8_t peerBdaddr[6];
-        uint8_t eirLength;
-        uint8_t eirData[31];
-      } *leAdvertisingReport = (EvtLeAdvertisingReport*)&pdata[sizeof(HCIEventHdr) + sizeof(LeMetaEventHeader)];
-
-      if (leAdvertisingReport->status == 0x01) {
-        // last byte is RSSI
-        int8_t rssi = leAdvertisingReport->eirData[leAdvertisingReport->eirLength];
-
-        GAP.handleLeAdvertisingReport(leAdvertisingReport->type,
-                                      leAdvertisingReport->peerBdaddrType,
-                                      leAdvertisingReport->peerBdaddr,
-                                      leAdvertisingReport->eirLength,
-                                      leAdvertisingReport->eirData,
-                                      rssi);
-
-      }
-    }
-  }
-}
-
-void HCIClass::dumpPkt(const char* prefix, uint8_t plen, uint8_t pdata[])
-{
-  if (_debug) {
-    _debug->print(prefix);
-
-    for (uint8_t i = 0; i < plen; i++) {
-      byte b = pdata[i];
-
-      if (b < 16) {
-        _debug->print("0");
-      }
-
-      _debug->print(b, HEX);
-    }
+        uint8_t reason;
+    } disconnectData = { handle, HCI_OE_USER_ENDED_CONNECTION };
 
-    _debug->println();
-    _debug->flush();
-  }
+    return hci_sendCommand(OGF_LINK_CTL, OCF_DISCONNECT, sizeof(disconnectData), &disconnectData);
 }
-`