HCIHandler: Complete HCIConnection Tracker for full *connect* and disconnect support incl HCI connection handle

We move *connect* and disconnect from DBTManager to HCIHandler,
as we need full control of the connection tracker due to:
- expose HCI connection handle to match actual session for disconnect
- control the disconnect on IOError (no command, purge tracker element and send event)

DBTManager::disconnect:
- no more used
- still aligned the 'send event' on ioerror code path

HCIHandler, DBTManager:
- Own callbacks for verbose mode only added in VERBOSE_ON compilation

5 files changed, 264 insertions, 89 deletions

diff --git a/api/direct_bt/HCIHandler.hpp b/api/direct_bt/HCIHandler.hpp
index f259bf20..a2f70308 100644
--- a/api/direct_bt/HCIHandler.hpp
+++ b/api/direct_bt/HCIHandler.hpp
@@ -53,32 +53,33 @@
  */
 namespace direct_bt {
 
-    class HCIHandleBDAddr {
+    class HCIConnection {
         public:
             uint16_t handle;
             EUI48 address;
             BDAddressType addressType;
 
         public:
-            HCIHandleBDAddr(const uint16_t handle, const EUI48 &address, const BDAddressType addressType)
+            HCIConnection(const uint16_t handle, const EUI48 &address, const BDAddressType addressType)
             : handle(handle), address(address), addressType(addressType) {}
 
-            HCIHandleBDAddr(const HCIHandleBDAddr &o) = default;
-            HCIHandleBDAddr(HCIHandleBDAddr &&o) = default;
-            HCIHandleBDAddr& operator=(const HCIHandleBDAddr &o) = default;
-            HCIHandleBDAddr& operator=(HCIHandleBDAddr &&o) = default;
+            HCIConnection(const HCIConnection &o) = default;
+            HCIConnection(HCIConnection &&o) = default;
+            HCIConnection& operator=(const HCIConnection &o) = default;
+            HCIConnection& operator=(HCIConnection &&o) = default;
 
-            bool operator==(const HCIHandleBDAddr& rhs) const
-            { return handle == rhs.handle; }
+            bool operator==(const HCIConnection& rhs) const
+            { return handle == rhs.handle && address == rhs.address; }
 
-            bool operator!=(const HCIHandleBDAddr& rhs) const
+            bool operator!=(const HCIConnection& rhs) const
             { return !(*this == rhs); }
 
             std::string toString() const {
-                return "HCIHandleBDAddr[handle "+uint16HexString(handle)+
+                return "HCIConnection[handle "+uint16HexString(handle)+
                        ", address="+address.toString()+", addressType "+getBDAddressTypeString(addressType)+"]";
             }
     };
+    typedef std::shared_ptr<HCIConnection> HCIConnectionRef;
 
     /**
      * A thread safe singleton handler of the HCI control channel to one controller (BT adapter)
@@ -102,10 +103,10 @@ namespace direct_bt {
             };
 
             static const pid_t pidSelf;
-            static MgmtEvent::Opcode translate(HCIEventType evt, HCIMetaEventType met);
-            std::shared_ptr<MgmtEvent> translate(std::shared_ptr<HCIEvent> ev);
 
         private:
+            static MgmtEvent::Opcode translate(HCIEventType evt, HCIMetaEventType met);
+
             const BTMode btMode;
             const uint16_t dev_id;
             POctets rbuffer;
@@ -130,8 +131,14 @@ namespace direct_bt {
             std::condition_variable cv_hciReaderInit;
             std::recursive_mutex mtx_sendReply; // for sendWith*Reply, process*Command, ..
 
-            std::vector<HCIHandleBDAddr> disconnectHandleAddrList;
-            std::recursive_mutex mtx_disconnectHandleAddrList;
+            std::vector<HCIConnectionRef> connectionList;
+            std::recursive_mutex mtx_connectionList;
+            void addTrackerConnection(const EUI48 & address, BDAddressType addrType, const uint16_t handle);
+            HCIConnectionRef setTrackerConnectionHandle(const EUI48 & address, const uint16_t handle);
+            HCIConnectionRef findTrackerConnection(const EUI48 & address);
+            HCIConnectionRef findTrackerConnection(const uint16_t handle);
+            HCIConnectionRef removeTrackerConnection(const uint16_t handle);
+            bool removeTrackerConnection(const EUI48 & address);
 
             /** One MgmtAdapterEventCallbackList per event type, allowing multiple callbacks to be invoked for each event */
             std::array<MgmtEventCallbackList, static_cast<uint16_t>(MgmtEvent::Opcode::MGMT_EVENT_TYPE_COUNT)> mgmtEventCallbackLists;
@@ -141,6 +148,7 @@ namespace direct_bt {
                     throw IndexOutOfBoundsException(static_cast<uint16_t>(opc), 1, mgmtEventCallbackLists.size(), E_FILE_LINE);
                 }
             }
+            std::shared_ptr<MgmtEvent> translate(std::shared_ptr<HCIEvent> ev);
 
             void hciReaderThreadImpl();
 
@@ -240,7 +248,8 @@ namespace direct_bt {
              * BT Core Spec v5.2: Vol 4, Part E HCI: 7.1.6 Disconnect command
              * </p>
              */
-            HCIStatusCode disconnect(const uint16_t conn_handle, const EUI48 &peer_bdaddr, const BDAddressType peer_mac_type,
+            HCIStatusCode disconnect(const bool ioErrorCause,
+                                     const uint16_t conn_handle, const EUI48 &peer_bdaddr, const BDAddressType peer_mac_type,
                                      const HCIStatusCode reason=HCIStatusCode::REMOTE_USER_TERMINATED_CONNECTION);
 
             /** MgmtEventCallback handling  */
diff --git a/api/direct_bt/HCITypes.hpp b/api/direct_bt/HCITypes.hpp
index c6b23b9e..9b3e640f 100644
--- a/api/direct_bt/HCITypes.hpp
+++ b/api/direct_bt/HCITypes.hpp
@@ -659,7 +659,7 @@ namespace direct_bt {
             std::string baseString() const override {
                 return HCIEvent::baseString()+
                         ", status "+uint8HexString(static_cast<uint8_t>(getStatus()), true)+" "+getHCIStatusCodeString(getStatus())+
-                        ", handle "+std::to_string(getHandle())+
+                        ", handle "+uint16HexString(getHandle())+
                         ", reason "+uint8HexString(static_cast<uint8_t>(getReason()), true)+" "+getHCIStatusCodeString(getReason());
             }
 
diff --git a/api/direct_bt/MgmtTypes.hpp b/api/direct_bt/MgmtTypes.hpp
index eb90d0f0..10481378 100644
--- a/api/direct_bt/MgmtTypes.hpp
+++ b/api/direct_bt/MgmtTypes.hpp
@@ -866,7 +866,7 @@ namespace direct_bt {
 
         public:
             MgmtEvtDeviceConnected(const uint8_t* buffer, const int buffer_len)
-            : MgmtEvent(buffer, buffer_len), hci_conn_handle(0)
+            : MgmtEvent(buffer, buffer_len), hci_conn_handle(0xffff)
             {
                 checkOpcode(getOpcode(), Opcode::DEVICE_CONNECTED);
             }
@@ -900,24 +900,24 @@ namespace direct_bt {
     class MgmtEvtDeviceConnectFailed : public MgmtEvent
     {
         private:
-            const HCIStatusCode hciRootStatus;
+            const HCIStatusCode hciStatus;
 
         protected:
             std::string baseString() const override {
                 return MgmtEvent::baseString()+", address="+getAddress().toString()+
                        ", addressType "+getBDAddressTypeString(getAddressType())+
                        ", status[mgmt["+uint8HexString(static_cast<uint8_t>(getStatus()))+" ("+getMgmtStatusString(getStatus())+")]"+
-                       ", hci["+uint8HexString(static_cast<uint8_t>(hciRootStatus))+" ("+getHCIStatusCodeString(hciRootStatus)+")]]";
+                       ", hci["+uint8HexString(static_cast<uint8_t>(hciStatus))+" ("+getHCIStatusCodeString(hciStatus)+")]]";
             }
 
         public:
             MgmtEvtDeviceConnectFailed(const uint8_t* buffer, const int buffer_len)
-            : MgmtEvent(buffer, buffer_len), hciRootStatus(HCIStatusCode::UNKNOWN)
+            : MgmtEvent(buffer, buffer_len), hciStatus(HCIStatusCode::UNKNOWN)
             {
                 checkOpcode(getOpcode(), Opcode::CONNECT_FAILED);
             }
             MgmtEvtDeviceConnectFailed(const uint16_t dev_id, const EUI48 &address, const BDAddressType addressType, const HCIStatusCode status)
-            : MgmtEvent(Opcode::DEVICE_CONNECTED, dev_id, 6+1+1), hciRootStatus(status)
+            : MgmtEvent(Opcode::CONNECT_FAILED, dev_id, 6+1+1), hciStatus(status)
             {
                 pdu.put_eui48(MGMT_HEADER_SIZE, address);
                 pdu.put_uint8(MGMT_HEADER_SIZE+6, addressType);
@@ -929,7 +929,7 @@ namespace direct_bt {
             MgmtStatus getStatus() const { return static_cast<MgmtStatus>( pdu.get_uint8(MGMT_HEADER_SIZE+7) ); }
 
             /** Return the root reason in non reduced HCIStatusCode space, if available. Otherwise this value will be HCIStatusCode::UNKNOWN. */
-            HCIStatusCode getHCIRootStatus() const { return hciRootStatus; }
+            HCIStatusCode getHCIStatus() const { return hciStatus; }
 
             int getDataOffset() const override { return MGMT_HEADER_SIZE+8; }
             int getDataSize() const override { return getParamSize()-8; }
@@ -967,7 +967,8 @@ namespace direct_bt {
             static DisconnectReason getDisconnectReason(HCIStatusCode hciReason);
 
         private:
-            const HCIStatusCode hciRootReason;
+            const HCIStatusCode hciReason;
+            const uint16_t hci_conn_handle;
 
         protected:
             std::string baseString() const override {
@@ -976,19 +977,21 @@ namespace direct_bt {
                 return MgmtEvent::baseString()+", address="+getAddress().toString()+
                        ", addressType "+getBDAddressTypeString(getAddressType())+
                        ", reason[mgmt["+uint8HexString(static_cast<uint8_t>(reason1))+" ("+getDisconnectReasonString(reason1)+")]"+
-                       ", hci["+uint8HexString(static_cast<uint8_t>(reason2))+" ("+getHCIStatusCodeString(reason2)+")]]";
+                       ", hci["+uint8HexString(static_cast<uint8_t>(reason2))+" ("+getHCIStatusCodeString(reason2)+")]]"+
+                       ", hci_handle "+uint16HexString(hci_conn_handle);
             }
 
         public:
             MgmtEvtDeviceDisconnected(const uint8_t* buffer, const int buffer_len)
-            : MgmtEvent(buffer, buffer_len), hciRootReason(HCIStatusCode::UNKNOWN)
+            : MgmtEvent(buffer, buffer_len), hciReason(HCIStatusCode::UNKNOWN), hci_conn_handle(0xffff)
             {
                 checkOpcode(getOpcode(), Opcode::DEVICE_DISCONNECTED);
             }
-            MgmtEvtDeviceDisconnected(const uint16_t dev_id, const EUI48 &address, const BDAddressType addressType, HCIStatusCode hciRootReason)
-            : MgmtEvent(Opcode::DEVICE_DISCONNECTED, dev_id, 6+1+1), hciRootReason(hciRootReason)
+            MgmtEvtDeviceDisconnected(const uint16_t dev_id, const EUI48 &address, const BDAddressType addressType,
+                                      HCIStatusCode hciReason, const uint16_t hci_conn_handle)
+            : MgmtEvent(Opcode::DEVICE_DISCONNECTED, dev_id, 6+1+1), hciReason(hciReason), hci_conn_handle(hci_conn_handle)
             {
-                DisconnectReason disconnectReason = getDisconnectReason(hciRootReason);
+                DisconnectReason disconnectReason = getDisconnectReason(hciReason);
                 pdu.put_eui48(MGMT_HEADER_SIZE, address);
                 pdu.put_uint8(MGMT_HEADER_SIZE+6, addressType);
                 pdu.put_uint8(MGMT_HEADER_SIZE+6+1, static_cast<uint8_t>(disconnectReason));
@@ -999,17 +1002,17 @@ namespace direct_bt {
 
             DisconnectReason getReason() const { return static_cast<DisconnectReason>(pdu.get_uint8(MGMT_HEADER_SIZE+7)); }
 
-            /** Return the root reason in non reduced HCIStatusCode space, if available. Otherwise this value will be HCIStatusCode::UNKNOWN. */
-            HCIStatusCode getHCIRootReason() const { return hciRootReason; }
-
-            /** Returns either the getHCIRootReason() if not HCIStatusCode::UNKNOWN, or the translated DisconnectReason. */
+            /** Returns either the HCI reason if given, or the translated DisconnectReason. */
             HCIStatusCode getHCIReason() const {
-                if( HCIStatusCode::UNKNOWN != hciRootReason ) {
-                    return hciRootReason;
+                if( HCIStatusCode::UNKNOWN != hciReason ) {
+                    return hciReason;
                 }
                 return getHCIReason(getReason());
             }
 
+            /** Returns the disconnected HCI connection handle, assuming creation occurred via HCIHandler */
+            uint16_t getHCIHandle() const { return hci_conn_handle; }
+
             int getDataOffset() const override { return MGMT_HEADER_SIZE+8; }
             int getDataSize() const override { return getParamSize()-8; }
             const uint8_t* getData() const override { return getDataSize()>0 ? pdu.get_ptr(getDataOffset()) : nullptr; }
diff --git a/src/direct_bt/DBTManager.cpp b/src/direct_bt/DBTManager.cpp
index bc3d1204..e1dc256d 100644
--- a/src/direct_bt/DBTManager.cpp
+++ b/src/direct_bt/DBTManager.cpp
@@ -118,13 +118,20 @@ void DBTManager::mgmtReaderThreadImpl() {
 }
 
 void DBTManager::sendMgmtEvent(std::shared_ptr<MgmtEvent> event) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
     const int dev_id = event->getDevID();
     const MgmtEvent::Opcode opc = event->getOpcode();
-    MgmtAdapterEventCallbackList mgmtEventCallbackList = mgmtAdapterEventCallbackLists[static_cast<uint16_t>(opc)];
+    MgmtAdapterEventCallbackList & mgmtEventCallbackList = mgmtAdapterEventCallbackLists[static_cast<uint16_t>(opc)];
     int invokeCount = 0;
     for (auto it = mgmtEventCallbackList.begin(); it != mgmtEventCallbackList.end(); ++it) {
         if( 0 > it->getDevID() || dev_id == it->getDevID() ) {
-            it->getCallback().invoke(event);
+            try {
+                it->getCallback().invoke(event);
+            } catch (std::exception &e) {
+                ERR_PRINT("DBTManager::sendMgmtEvent-CBs %d/%zd: MgmtAdapterEventCallback %s : Caught exception %s",
+                        invokeCount+1, mgmtEventCallbackList.size(),
+                        it->toString().c_str(), e.what());
+            }
             invokeCount++;
         }
     }
@@ -372,6 +379,7 @@ next1:
     }
 
     if( ok ) {
+#ifdef VERBOSE_ON
         addMgmtEventCallback(-1, MgmtEvent::Opcode::CLASS_OF_DEV_CHANGED, bindMemberFunc(this, &DBTManager::mgmtEvClassOfDeviceChangedCB));
         addMgmtEventCallback(-1, MgmtEvent::Opcode::DISCOVERING, bindMemberFunc(this, &DBTManager::mgmtEvDeviceDiscoveringCB));
         addMgmtEventCallback(-1, MgmtEvent::Opcode::DEVICE_FOUND, bindMemberFunc(this, &DBTManager::mgmtEvDeviceFoundCB));
@@ -386,6 +394,7 @@ next1:
         addMgmtEventCallback(-1, MgmtEvent::Opcode::DEVICE_WHITELIST_REMOVED, bindMemberFunc(this, &DBTManager::mgmtEvDeviceWhilelistRemovedCB));
         addMgmtEventCallback(-1, MgmtEvent::Opcode::PIN_CODE_REQUEST, bindMemberFunc(this, &DBTManager::mgmtEvPinCodeRequestCB));
         addMgmtEventCallback(-1, MgmtEvent::Opcode::USER_PASSKEY_REQUEST, bindMemberFunc(this, &DBTManager::mgmtEvUserPasskeyRequestCB));
+#endif
         PERF_TS_TD("DBTManager::open.ok");
         return;
     }
@@ -655,10 +664,11 @@ bool DBTManager::disconnect(const bool ioErrorCause,
                 bres = true;
             }
         }
+    } else {
+        // explicit disconnected event anyways
+        MgmtEvtDeviceDisconnected *e = new MgmtEvtDeviceDisconnected(dev_id, peer_bdaddr, peer_mac_type, reason, 0xffff);
+        sendMgmtEvent(std::shared_ptr<MgmtEvent>(e));
     }
-    // explicit disconnected event anyways
-    MgmtEvtDeviceDisconnected *e = new MgmtEvtDeviceDisconnected(dev_id, peer_bdaddr, peer_mac_type, reason);
-    sendMgmtEvent(std::shared_ptr<MgmtEvent>(e));
     return bres;
 }
 
diff --git a/src/direct_bt/HCIHandler.cpp b/src/direct_bt/HCIHandler.cpp
index a37fd13b..48182e1a 100644
--- a/src/direct_bt/HCIHandler.cpp
+++ b/src/direct_bt/HCIHandler.cpp
@@ -57,6 +57,99 @@ using namespace direct_bt;
 
 const pid_t HCIHandler::pidSelf = getpid();
 
+void HCIHandler::addTrackerConnection(const EUI48 & address, BDAddressType addrType, const uint16_t handle) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_connectionList); // RAII-style acquire and relinquish via destructor
+    // remove all old entry with given address first
+    for (auto it = connectionList.begin(); it != connectionList.end(); ) {
+        HCIConnectionRef conn = *it;
+        if ( address == conn->address ) {
+            if( addrType == conn->addressType ) {
+                // reuse same entry
+                INFO_PRINT("HCIHandler::addTrackerConnection: address[%s, %s], reuse entry %s",
+                           address.toString().c_str(), getBDAddressTypeString(addrType).c_str(), conn->toString().c_str());
+                if( 0 == conn->handle && 0 != handle ) {
+                    conn->handle = handle;
+                }
+                return; // done
+            } else {
+                // delete incompatible old entry
+                WARN_PRINT("HCIHandler::addTrackerConnection: address[%s, %s], remove incompatible entry %s",
+                           address.toString().c_str(), getBDAddressTypeString(addrType).c_str(), conn->toString().c_str());
+                it = connectionList.erase(it);
+                break;
+            }
+        } else {
+            ++it;
+        }
+    }
+    connectionList.push_back( HCIConnectionRef( new HCIConnection(handle, address, addrType) ) );
+}
+
+HCIConnectionRef HCIHandler::setTrackerConnectionHandle(const EUI48 & address, const uint16_t handle) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_connectionList); // RAII-style acquire and relinquish via destructor
+    const size_t size = connectionList.size();
+    for (size_t i = 0; i < size; i++) {
+        HCIConnectionRef & e = connectionList[i];
+        if ( address == e->address ) {
+            e->handle = handle;
+            return e; // done
+        }
+    }
+    return nullptr;
+}
+
+HCIConnectionRef HCIHandler::findTrackerConnection(const EUI48 & address) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_connectionList); // RAII-style acquire and relinquish via destructor
+    const size_t size = connectionList.size();
+    for (size_t i = 0; i < size; i++) {
+        HCIConnectionRef & e = connectionList[i];
+        if ( address == e->address ) {
+            return e;
+        }
+    }
+    return nullptr;
+}
+
+HCIConnectionRef HCIHandler::findTrackerConnection(const uint16_t handle) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_connectionList); // RAII-style acquire and relinquish via destructor
+    const size_t size = connectionList.size();
+    for (size_t i = 0; i < size; i++) {
+        HCIConnectionRef & e = connectionList[i];
+        if ( handle == e->handle ) {
+            return e;
+        }
+    }
+    return nullptr;
+}
+
+HCIConnectionRef HCIHandler::removeTrackerConnection(const uint16_t handle) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_connectionList); // RAII-style acquire and relinquish via destructor
+    for (auto it = connectionList.begin(); it != connectionList.end(); ) {
+        if ( (*it)->handle == handle ) {
+            HCIConnectionRef e = *it;
+            it = connectionList.erase(it); // old entry
+            return e; // done
+        } else {
+            ++it;
+        }
+    }
+    return nullptr;
+}
+
+bool HCIHandler::removeTrackerConnection(const EUI48 & address) {
+    int count = 0;
+    const std::lock_guard<std::recursive_mutex> lock(mtx_connectionList); // RAII-style acquire and relinquish via destructor
+    for (auto it = connectionList.begin(); it != connectionList.end(); ) {
+        if ( (*it)->address == address ) {
+            it = connectionList.erase(it); // old entry
+            count++;
+        } else {
+            ++it;
+        }
+    }
+    return count>0;
+}
+
 MgmtEvent::Opcode HCIHandler::translate(HCIEventType evt, HCIMetaEventType met) {
     if( HCIEventType::LE_META == evt ) {
         switch( met ) {
@@ -82,12 +175,24 @@ std::shared_ptr<MgmtEvent> HCIHandler::translate(std::shared_ptr<HCIEvent> ev) {
             case HCIMetaEventType::LE_CONN_COMPLETE: {
                 HCIStatusCode status;
                 const hci_ev_le_conn_complete * ev_cc = getMetaReplyStruct<hci_ev_le_conn_complete>(ev, mevt, &status);
+                if( nullptr == ev_cc ) {
+                    ERR_PRINT("HCIHandler::translate(reader): LE_CONN_COMPLETE: Null reply-struct: %s", ev->toString().c_str());
+                    return nullptr;
+                }
                 const HCIAddressType hciAddrType = static_cast<HCIAddressType>(ev_cc->bdaddr_type);
                 const BDAddressType addrType = getBDAddressType(hciAddrType);
-                if( HCIStatusCode::SUCCESS == status ) {
-                    return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnected(dev_id, ev_cc->bdaddr, addrType, ev_cc->handle) );
+                HCIConnectionRef conn = setTrackerConnectionHandle(ev_cc->bdaddr, ev_cc->handle);
+                if( nullptr == conn ) {
+                    INFO_PRINT("HCIHandler::translate(reader): LE_CONN_COMPLETE: Not tracked address[%s, %s], handle %s: %s",
+                               ev_cc->bdaddr.toString().c_str(), getBDAddressTypeString(addrType).c_str(),
+                               uint16HexString(ev_cc->handle).c_str(), ev->toString().c_str());
+                    return nullptr;
                 } else {
-                    return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnectFailed(dev_id, ev_cc->bdaddr, addrType, status) );
+                    if( HCIStatusCode::SUCCESS == status ) {
+                        return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnected(dev_id, ev_cc->bdaddr, addrType, ev_cc->handle) );
+                    } else {
+                        return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnectFailed(dev_id, ev_cc->bdaddr, addrType, status) );
+                    }
                 }
             }
             default:
@@ -98,39 +203,47 @@ std::shared_ptr<MgmtEvent> HCIHandler::translate(std::shared_ptr<HCIEvent> ev) {
         case HCIEventType::CONN_COMPLETE: {
             HCIStatusCode status;
             const hci_ev_conn_complete * ev_cc = getReplyStruct<hci_ev_conn_complete>(ev, evt, &status);
-            if( HCIStatusCode::SUCCESS == status ) {
-                return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnected(dev_id, ev_cc->bdaddr, BDAddressType::BDADDR_BREDR, ev_cc->handle) );
+            if( nullptr == ev_cc ) {
+                ERR_PRINT("HCIHandler::translate(reader): CONN_COMPLETE: Null reply-struct: %s", ev->toString().c_str());
+                return nullptr;
+            }
+            HCIConnectionRef conn = setTrackerConnectionHandle(ev_cc->bdaddr, ev_cc->handle);
+            if( nullptr == conn ) {
+                INFO_PRINT("HCIHandler::translate(reader): CONN_COMPLETE: Not tracked address %s, handle %s: %s",
+                           ev_cc->bdaddr.toString().c_str(), uint16HexString(ev_cc->handle).c_str(), ev->toString().c_str());
+                return nullptr;
             } else {
-                return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnectFailed(dev_id, ev_cc->bdaddr, BDAddressType::BDADDR_BREDR, status) );
+                if( HCIStatusCode::SUCCESS == status ) {
+                    return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnected(dev_id, conn->address, conn->addressType, conn->handle) );
+                } else {
+                    std::shared_ptr<MgmtEvent> res( new MgmtEvtDeviceConnectFailed(dev_id, conn->address, conn->addressType, status) );
+                    removeTrackerConnection(conn->address);
+                    return res;
+                }
             }
         }
         case HCIEventType::DISCONN_COMPLETE: {
             HCIStatusCode status;
             const hci_ev_disconn_complete * ev_cc = getReplyStruct<hci_ev_disconn_complete>(ev, evt, &status);
-            if( HCIStatusCode::SUCCESS == status ) {
-                const HCIStatusCode hciRootReason = static_cast<HCIStatusCode>(ev_cc->reason);
-                const uint16_t handle = ev_cc->handle;
-                EUI48 address; // ANY
-                BDAddressType addrType = BDAddressType::BDADDR_UNDEFINED;
-                {
-                    const std::lock_guard<std::recursive_mutex> lock(mtx_disconnectHandleAddrList); // RAII-style acquire and relinquish via destructor
-                    for (auto it = disconnectHandleAddrList.begin(); it != disconnectHandleAddrList.end(); ) {
-                        if ( it->handle == handle ) {
-                            address = it->address;
-                            addrType = it->addressType;
-                            it = disconnectHandleAddrList.erase(it);
-                            break; // done
-                        } else {
-                            ++it;
-                        }
-                    }
-                }
-                if( BDAddressType::BDADDR_UNDEFINED != addrType ) {
-                    return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceDisconnected(dev_id, address, addrType, hciRootReason) );
+            if( nullptr == ev_cc ) {
+                ERR_PRINT("HCIHandler::translate(reader): DISCONN_COMPLETE: Null reply-struct: %s", ev->toString().c_str());
+                return nullptr;
+            }
+            HCIConnectionRef conn = removeTrackerConnection(ev_cc->handle);
+            if( nullptr == conn ) {
+                INFO_PRINT("HCIHandler::translate(reader): DISCONN_COMPLETE: Not tracked handle %s: %s",
+                           uint16HexString(ev_cc->handle).c_str(), ev->toString().c_str());
+                return nullptr;
+            } else {
+                if( HCIStatusCode::SUCCESS != status ) {
+                    // FIXME: Ever occuring? Still sending out essential disconnect event!
+                    ERR_PRINT("HCIHandler::translate(reader): DISCONN_COMPLETE: !SUCCESS[%s, %s], %s: %s",
+                            uint8HexString(static_cast<uint8_t>(status)).c_str(), getHCIStatusCodeString(status).c_str(),
+                            conn->toString().c_str(), ev->toString().c_str());
                 }
-                return nullptr; // unknown handle!
+                const HCIStatusCode hciRootReason = static_cast<HCIStatusCode>(ev_cc->reason);
+                return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceDisconnected(dev_id, conn->address, conn->addressType, hciRootReason, conn->handle) );
             }
-            return nullptr;
         }
         default: return nullptr;
     }
@@ -229,6 +342,26 @@ void HCIHandler::hciReaderThreadImpl() {
     hciEventRing.clear();
 }
 
+void HCIHandler::sendMgmtEvent(std::shared_ptr<MgmtEvent> event) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
+    const MgmtEvent::Opcode opc = event->getOpcode();
+    MgmtEventCallbackList & mgmtEventCallbackList = mgmtEventCallbackLists[static_cast<uint16_t>(opc)];
+    int invokeCount = 0;
+    for (auto it = mgmtEventCallbackList.begin(); it != mgmtEventCallbackList.end(); ++it) {
+        try {
+            it->invoke(event);
+        } catch (std::exception &e) {
+            ERR_PRINT("HCIHandler::sendMgmtEvent-CBs %d/%zd: MgmtEventCallback %s : Caught exception %s",
+                    invokeCount+1, mgmtEventCallbackList.size(),
+                    it->toString().c_str(), e.what());
+        }
+        invokeCount++;
+    }
+    DBG_PRINT("HCIHandler::sendMgmtEvent: Event %s -> %d/%zd callbacks",
+            event->toString().c_str(), invokeCount, mgmtEventCallbackList.size());
+    (void)invokeCount;
+}
+
 bool HCIHandler::sendCommand(HCICommand &req) {
     const std::lock_guard<std::recursive_mutex> lock(comm.mutex()); // RAII-style acquire and relinquish via destructor
     TROOctets & pdu = req.getPDU();
@@ -375,12 +508,12 @@ HCIHandler::HCIHandler(const BTMode btMode, const uint16_t dev_id, const int rep
 #endif
         filter_put_metaevs(mask);
     }
+#ifdef VERBOSE_ON
     // Add own callbacks
-    {
-        addMgmtEventCallback(MgmtEvent::Opcode::DEVICE_DISCONNECTED, bindMemberFunc(this, &HCIHandler::mgmtEvDeviceDisconnectedCB));
-        addMgmtEventCallback(MgmtEvent::Opcode::DEVICE_CONNECTED, bindMemberFunc(this, &HCIHandler::mgmtEvDeviceConnectedCB));
-        addMgmtEventCallback(MgmtEvent::Opcode::CONNECT_FAILED, bindMemberFunc(this, &HCIHandler::mgmtEvConnectFailedCB));
-    }
+    addMgmtEventCallback(MgmtEvent::Opcode::DEVICE_DISCONNECTED, bindMemberFunc(this, &HCIHandler::mgmtEvDeviceDisconnectedCB));
+    addMgmtEventCallback(MgmtEvent::Opcode::DEVICE_CONNECTED, bindMemberFunc(this, &HCIHandler::mgmtEvDeviceConnectedCB));
+    addMgmtEventCallback(MgmtEvent::Opcode::CONNECT_FAILED, bindMemberFunc(this, &HCIHandler::mgmtEvConnectFailedCB));
+#endif
     {
         HCICommand req0(HCIOpcode::READ_LOCAL_VERSION, 0);
         const hci_rp_read_local_version * ev_lv;
@@ -476,6 +609,7 @@ HCIStatusCode HCIHandler::le_create_conn(const EUI48 &peer_bdaddr,
     cp->min_ce_len = cpu_to_le(min_ce_length);
     cp->max_ce_len = cpu_to_le(max_ce_length);
 
+    addTrackerConnection(peer_bdaddr, getBDAddressType(peer_mac_type), 0);
     HCIStatusCode status;
     std::shared_ptr<HCIEvent> ev = processStructCommand(req0, &status);
     return status;
@@ -499,12 +633,14 @@ HCIStatusCode HCIHandler::create_conn(const EUI48 &bdaddr,
     cp->clock_offset = cpu_to_le(clock_offset);
     cp->role_switch = role_switch;
 
+    addTrackerConnection(bdaddr, BDAddressType::BDADDR_BREDR, 0);
     HCIStatusCode status;
     std::shared_ptr<HCIEvent> ev = processStructCommand(req0, &status);
     return status;
 }
 
-HCIStatusCode HCIHandler::disconnect(const uint16_t conn_handle, const EUI48 &peer_bdaddr, const BDAddressType peer_mac_type,
+HCIStatusCode HCIHandler::disconnect(const bool ioErrorCause,
+                                     const uint16_t conn_handle, const EUI48 &peer_bdaddr, const BDAddressType peer_mac_type,
                                      const HCIStatusCode reason)
 {
     const std::lock_guard<std::recursive_mutex> lock(mtx); // RAII-style acquire and relinquish via destructor
@@ -513,28 +649,45 @@ HCIStatusCode HCIHandler::disconnect(const uint16_t conn_handle, const EUI48 &pe
         return HCIStatusCode::INTERNAL_FAILURE;
     }
     if( 0 == conn_handle ) {
-        return HCIStatusCode::SUCCESS;
+        ERR_PRINT("HCIHandler::disconnect: Null conn_handle given address[%s, %s] (drop)",
+                   peer_bdaddr.toString().c_str(), getBDAddressTypeString(peer_mac_type).c_str());
+        return HCIStatusCode::INVALID_HCI_COMMAND_PARAMETERS;
     }
     {
-        const std::lock_guard<std::recursive_mutex> lock(mtx_disconnectHandleAddrList); // RAII-style acquire and relinquish via destructor
-        for (auto it = disconnectHandleAddrList.begin(); it != disconnectHandleAddrList.end(); ) {
-            if ( it->handle == conn_handle ) {
-                it = disconnectHandleAddrList.erase(it); // old entry
-                break; // done
-            } else {
-                ++it;
-            }
+        const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
+        HCIConnectionRef conn = findTrackerConnection(conn_handle);
+        if( nullptr == conn ) {
+            // disconnect called w/o being connected through this HCIHandler
+            INFO_PRINT("HCIHandler::disconnect: Not tracked address[%s, %s] (adding)",
+                       peer_bdaddr.toString().c_str(), getBDAddressTypeString(peer_mac_type).c_str());
+            addTrackerConnection(peer_bdaddr, peer_mac_type, conn_handle);
+        } else if( conn->address != peer_bdaddr || conn->addressType != peer_mac_type ) {
+            ERR_PRINT("HCIHandler::disconnect: Mismatch given address[%s, %s] and tracked %s (drop)",
+                       peer_bdaddr.toString().c_str(), getBDAddressTypeString(peer_mac_type).c_str(),
+                       conn->toString().c_str());
+            return HCIStatusCode::INVALID_HCI_COMMAND_PARAMETERS;
         }
-        disconnectHandleAddrList.push_back(HCIHandleBDAddr(conn_handle, peer_bdaddr, peer_mac_type));
     }
-    HCIStructCommand<hci_cp_disconnect> req0(HCIOpcode::DISCONNECT);
-    hci_cp_disconnect * cp = req0.getWStruct();
-    bzero(cp, sizeof(*cp));
-    cp->handle = conn_handle;
-    cp->reason = number(reason);
+    INFO_PRINT("HCIHandler::disconnect: address[%s, %s], handle %s, ioError %d",
+               peer_bdaddr.toString().c_str(), getBDAddressTypeString(peer_mac_type).c_str(),
+               uint16HexString(conn_handle).c_str(), ioErrorCause);
 
     HCIStatusCode status;
-    std::shared_ptr<HCIEvent> ev = processStructCommand(req0, &status);
+
+    if( !ioErrorCause ) {
+        HCIStructCommand<hci_cp_disconnect> req0(HCIOpcode::DISCONNECT);
+        hci_cp_disconnect * cp = req0.getWStruct();
+        bzero(cp, sizeof(*cp));
+        cp->handle = cpu_to_le(conn_handle);
+        cp->reason = number(reason);
+
+        std::shared_ptr<HCIEvent> ev = processStructCommand(req0, &status);
+    } else {
+        removeTrackerConnection(conn_handle);
+        MgmtEvtDeviceDisconnected *e = new MgmtEvtDeviceDisconnected(dev_id, peer_bdaddr, peer_mac_type, reason, conn_handle);
+        sendMgmtEvent(std::shared_ptr<MgmtEvent>(e));
+    }
+
     return status;
 }