HCIHandler: Use async event mechanism for delayed replies, commands shall return immediately (1/2)

It has been observed that under certain circumstances a command like 'le_create_conn'
will timeout after having received the CMD_STATUS message while waiting for the
final completion reply LE_CONN_COMPLETE.

Such delay may even cause a disconnect in case the application is blocked
until command returns.

Here, under BlueZ Kernel, the HCI implementation issues a disconnect
and adds the device to the white-list for the adapter to complete the pending connection.
This is not desired!

Therefor, the only correct methodology is to utilize asynchronous reply
on delayed commands, i.e. commands with replies other than CMD_STATUS or CMD_COMPLETED.

Here we issue the command request and only wait for the immediate CMD_STATUS reply,
which HCIStatusCode will be returned.

The newly added MgmtEventCallback facility is utilized to forward received pending
(completion) events to added listeners.
This implementation benefits from HCIHandler's already existing reader thread.

Even in case HCIHandler won't receive the (completion) event due to
BlueZ Kernel Mgmt interception and handling,
the application will succeed as it will receive the event via callback
either from DBTManager or HCIHandler.

MgmtEventCallback has been chosen because:
- Enable existing DBTManager callback listener
- Allowing to receive the event either by DBTManager or HCIHandler
- Potential reimplementation of DBTManager using HCIHandler
- MgmtEvent types are easier to digest semantically
- MgmtEvent::Opcode value range is limited and more suitable for an array

Further details:
- HCIHandler translates supported HCIEvent to MgmtEvent.
- MgmtEvtDeviceConnected holds optional hci_conn_handle (manual creation)
- MgmtEvtDeviceConnectFailed holds optional HCIStatusCode (manual creation)
- DBTManager reader: Use mutex and reference instead of MgmtAdapterEventCallbackList copy.

4 files changed, 378 insertions, 222 deletions

diff --git a/api/direct_bt/HCIHandler.hpp b/api/direct_bt/HCIHandler.hpp
index 61c41e4f..f259bf20 100644
--- a/api/direct_bt/HCIHandler.hpp
+++ b/api/direct_bt/HCIHandler.hpp
@@ -53,6 +53,33 @@
  */
 namespace direct_bt {
 
+    class HCIHandleBDAddr {
+        public:
+            uint16_t handle;
+            EUI48 address;
+            BDAddressType addressType;
+
+        public:
+            HCIHandleBDAddr(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;
+
+            bool operator==(const HCIHandleBDAddr& rhs) const
+            { return handle == rhs.handle; }
+
+            bool operator!=(const HCIHandleBDAddr& rhs) const
+            { return !(*this == rhs); }
+
+            std::string toString() const {
+                return "HCIHandleBDAddr[handle "+uint16HexString(handle)+
+                       ", address="+address.toString()+", addressType "+getBDAddressTypeString(addressType)+"]";
+            }
+    };
+
     /**
      * A thread safe singleton handler of the HCI control channel to one controller (BT adapter)
      * <p>
@@ -75,9 +102,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:
-            const bool pass_replies_only_filter;
             const BTMode btMode;
             const uint16_t dev_id;
             POctets rbuffer;
@@ -90,9 +118,9 @@ namespace direct_bt {
             inline bool filter_test_metaev(HCIMetaEventType mec) { return 0 != test_bit_uint32(number(mec)-1, metaev_filter_mask); }
             inline void filter_put_metaevs(const uint32_t mask) { metaev_filter_mask=mask; }
 
-            inline void filter_clear_metaevs(uint32_t &mask) { mask=0; }
-            inline void filter_all_metaevs(uint32_t &mask) { mask=0xffff; }
-            inline void filter_set_metaev(HCIMetaEventType mec, uint32_t &mask) { set_bit_uint32(number(mec)-1, mask); }
+            inline static void filter_clear_metaevs(uint32_t &mask) { mask=0; }
+            inline static void filter_all_metaevs(uint32_t &mask) { mask=0xffff; }
+            inline static void filter_set_metaev(HCIMetaEventType mec, uint32_t &mask) { set_bit_uint32(number(mec)-1, mask); }
 
             LFRingbuffer<std::shared_ptr<HCIEvent>, nullptr> hciEventRing;
             std::atomic<pthread_t> hciReaderThreadId;
@@ -102,6 +130,18 @@ 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;
+
+            /** 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;
+            std::recursive_mutex mtx_callbackLists;
+            inline void checkMgmtEventCallbackListsIndex(const MgmtEvent::Opcode opc) const {
+                if( static_cast<uint16_t>(opc) >= mgmtEventCallbackLists.size() ) {
+                    throw IndexOutOfBoundsException(static_cast<uint16_t>(opc), 1, mgmtEventCallbackLists.size(), E_FILE_LINE);
+                }
+            }
+
             void hciReaderThreadImpl();
 
             bool sendCommand(HCICommand &req);
@@ -112,19 +152,23 @@ namespace direct_bt {
             template<typename hci_cmd_event_struct>
             std::shared_ptr<HCIEvent> processSimpleCommand(HCIOpcode opc, const hci_cmd_event_struct **res, HCIStatusCode *status);
 
-            template<typename hci_command_struct, typename hci_cmd_event_struct>
-            std::shared_ptr<HCIEvent> processStructCommand(HCIStructCommand<hci_command_struct> &req,
-                                                           HCIEventType evc, const hci_cmd_event_struct **res,
-                                                           HCIStatusCode *status);
+            template<typename hci_command_struct>
+            std::shared_ptr<HCIEvent> processStructCommand(HCIStructCommand<hci_command_struct> &req, HCIStatusCode *status);
 
-            template<typename hci_command_struct, typename hci_cmd_event_struct>
-            std::shared_ptr<HCIEvent> processStructMetaCmd(HCIStructCommand<hci_command_struct> &req,
-                                                           HCIMetaEventType mec, const hci_cmd_event_struct **res,
-                                                           HCIStatusCode *status);
+            template<typename hci_cmd_event_struct>
+            const hci_cmd_event_struct* getReplyStruct(std::shared_ptr<HCIEvent> event, HCIEventType evc, HCIStatusCode *status);
+
+            template<typename hci_cmd_event_struct>
+            const hci_cmd_event_struct* getMetaReplyStruct(std::shared_ptr<HCIEvent> event, HCIMetaEventType mec, HCIStatusCode *status);
 
             HCIHandler(const HCIHandler&) = delete;
             void operator=(const HCIHandler&) = delete;
 
+            bool mgmtEvDeviceDisconnectedCB(std::shared_ptr<MgmtEvent> e);
+            bool mgmtEvDeviceConnectedCB(std::shared_ptr<MgmtEvent> e);
+            bool mgmtEvConnectFailedCB(std::shared_ptr<MgmtEvent> e);
+            void sendMgmtEvent(std::shared_ptr<MgmtEvent> event);
+
         public:
             HCIHandler(const BTMode btMode, const uint16_t dev_id, const int replyTimeoutMS=Defaults::HCI_COMMAND_REPLY_TIMEOUT);
 
@@ -162,7 +206,6 @@ namespace direct_bt {
              * Set window to the same value as the interval, enables continuous scanning.
              * </p>
              *
-             * @param handle_return
              * @param peer_bdaddr
              * @param peer_mac_type
              * @param own_mac_type
@@ -174,7 +217,7 @@ namespace direct_bt {
              * @param supervision_timeout in units of 10ms, default value 1000 for 10000ms or 10s.
              * @return
              */
-            HCIStatusCode le_create_conn(uint16_t * handle_return, const EUI48 &peer_bdaddr,
+            HCIStatusCode le_create_conn(const EUI48 &peer_bdaddr,
                                         const HCIAddressType peer_mac_type=HCIAddressType::HCIADDR_LE_PUBLIC,
                                         const HCIAddressType own_mac_type=HCIAddressType::HCIADDR_LE_PUBLIC,
                                         const uint16_t le_scan_interval=48, const uint16_t le_scan_window=48,
@@ -187,7 +230,7 @@ namespace direct_bt {
              * BT Core Spec v5.2: Vol 4, Part E HCI: 7.1.5 Create Connection command
              * </p>
              */
-            HCIStatusCode create_conn(uint16_t * handle_return, const EUI48 &bdaddr,
+            HCIStatusCode create_conn(const EUI48 &bdaddr,
                                      const uint16_t pkt_type=HCI_DM1 | HCI_DM3 | HCI_DM5 | HCI_DH1 | HCI_DH3 | HCI_DH5,
                                      const uint16_t clock_offset=0x0000, const uint8_t role_switch=0x01);
 
@@ -197,7 +240,22 @@ 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 HCIStatusCode reason=HCIStatusCode::REMOTE_USER_TERMINATED_CONNECTION);
+            HCIStatusCode disconnect(const uint16_t conn_handle, const EUI48 &peer_bdaddr, const BDAddressType peer_mac_type,
+                                     const HCIStatusCode reason=HCIStatusCode::REMOTE_USER_TERMINATED_CONNECTION);
+
+            /** MgmtEventCallback handling  */
+
+            /**
+             * Appends the given MgmtEventCallback to the named MgmtEvent::Opcode list,
+             * if it is not present already (opcode + callback).
+             */
+            void addMgmtEventCallback(const MgmtEvent::Opcode opc, const MgmtEventCallback &cb);
+            /** Returns count of removed given MgmtEventCallback from the named MgmtEvent::Opcode list. */
+            int removeMgmtEventCallback(const MgmtEvent::Opcode opc, const MgmtEventCallback &cb);
+            /** Removes all MgmtEventCallbacks from the to the named MgmtEvent::Opcode list. */
+            void clearMgmtEventCallbacks(const MgmtEvent::Opcode opc);
+            /** Removes all MgmtEventCallbacks from all MgmtEvent::Opcode lists. */
+            void clearAllMgmtEventCallbacks();
     };
 
 } // namespace direct_bt
diff --git a/api/direct_bt/MgmtTypes.hpp b/api/direct_bt/MgmtTypes.hpp
index 8fa5961d..eb90d0f0 100644
--- a/api/direct_bt/MgmtTypes.hpp
+++ b/api/direct_bt/MgmtTypes.hpp
@@ -852,22 +852,36 @@ namespace direct_bt {
      */
     class MgmtEvtDeviceConnected : public MgmtEvent
     {
-        public:
+        private:
+            uint16_t hci_conn_handle;
 
         protected:
             std::string baseString() const override {
                 return MgmtEvent::baseString()+", address="+getAddress().toString()+
                        ", addressType "+getBDAddressTypeString(getAddressType())+
                        ", flags="+uint32HexString(getFlags(), true)+
-                       ", eir-size "+std::to_string(getEIRSize());
+                       ", eir-size "+std::to_string(getEIRSize())+
+                       ", hci_handle "+uint16HexString(hci_conn_handle);
             }
 
         public:
             MgmtEvtDeviceConnected(const uint8_t* buffer, const int buffer_len)
-            : MgmtEvent(buffer, buffer_len)
+            : MgmtEvent(buffer, buffer_len), hci_conn_handle(0)
             {
                 checkOpcode(getOpcode(), Opcode::DEVICE_CONNECTED);
             }
+            MgmtEvtDeviceConnected(const uint16_t dev_id, const EUI48 &address, const BDAddressType addressType, const uint16_t hci_conn_handle)
+            : MgmtEvent(Opcode::DEVICE_CONNECTED, dev_id, 6+1+4+2), hci_conn_handle(hci_conn_handle)
+            {
+                pdu.put_eui48(MGMT_HEADER_SIZE, address);
+                pdu.put_uint8(MGMT_HEADER_SIZE+6, addressType);
+                pdu.put_uint32(MGMT_HEADER_SIZE+6+1, 0); // flags
+                pdu.put_uint16(MGMT_HEADER_SIZE+6+1+4, 0); // eir-len
+            }
+
+            /** Returns the HCI connection handle, assuming creation occurred via HCIHandler */
+            uint16_t getHCIHandle() const { return hci_conn_handle; }
+
             const EUI48 getAddress() const { return EUI48(pdu.get_ptr(MGMT_HEADER_SIZE)); } // mgmt_addr_info
             BDAddressType getAddressType() const { return static_cast<BDAddressType>(pdu.get_uint8(MGMT_HEADER_SIZE+6)); } // mgmt_addr_info
 
@@ -885,26 +899,38 @@ namespace direct_bt {
      */
     class MgmtEvtDeviceConnectFailed : public MgmtEvent
     {
-        public:
+        private:
+            const HCIStatusCode hciRootStatus;
 
         protected:
             std::string baseString() const override {
                 return MgmtEvent::baseString()+", address="+getAddress().toString()+
                        ", addressType "+getBDAddressTypeString(getAddressType())+
-                       ", status "+uint8HexString(static_cast<uint8_t>(getStatus()))+" "+getMgmtStatusString(getStatus());
+                       ", status[mgmt["+uint8HexString(static_cast<uint8_t>(getStatus()))+" ("+getMgmtStatusString(getStatus())+")]"+
+                       ", hci["+uint8HexString(static_cast<uint8_t>(hciRootStatus))+" ("+getHCIStatusCodeString(hciRootStatus)+")]]";
             }
 
         public:
             MgmtEvtDeviceConnectFailed(const uint8_t* buffer, const int buffer_len)
-            : MgmtEvent(buffer, buffer_len)
+            : MgmtEvent(buffer, buffer_len), hciRootStatus(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)
+            {
+                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>(MgmtStatus::CONNECT_FAILED));
+            }
             const EUI48 getAddress() const { return EUI48(pdu.get_ptr(MGMT_HEADER_SIZE)); } // mgmt_addr_info
             BDAddressType getAddressType() const { return static_cast<BDAddressType>(pdu.get_uint8(MGMT_HEADER_SIZE+6)); } // mgmt_addr_info
 
             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; }
+
             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; }
@@ -1200,6 +1226,7 @@ namespace direct_bt {
     };
 
     typedef FunctionDef<bool, std::shared_ptr<MgmtEvent>> MgmtEventCallback;
+    typedef std::vector<MgmtEventCallback> MgmtEventCallbackList;
 
     class MgmtAdapterEventCallback {
         private:
diff --git a/src/direct_bt/DBTManager.cpp b/src/direct_bt/DBTManager.cpp
index 79255944..bc3d1204 100644
--- a/src/direct_bt/DBTManager.cpp
+++ b/src/direct_bt/DBTManager.cpp
@@ -87,8 +87,9 @@ void DBTManager::mgmtReaderThreadImpl() {
                 mgmtEventRing.putBlocking( event );
             } else {
                 // issue a callback
+                const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
                 const int dev_id = event->getDevID();
-                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() ) {
diff --git a/src/direct_bt/HCIHandler.cpp b/src/direct_bt/HCIHandler.cpp
index f4ac2d5a..cbe38a9f 100644
--- a/src/direct_bt/HCIHandler.cpp
+++ b/src/direct_bt/HCIHandler.cpp
@@ -57,6 +57,85 @@ using namespace direct_bt;
 
 const pid_t HCIHandler::pidSelf = getpid();
 
+MgmtEvent::Opcode HCIHandler::translate(HCIEventType evt, HCIMetaEventType met) {
+    if( HCIEventType::LE_META == evt ) {
+        switch( met ) {
+            case HCIMetaEventType::LE_CONN_COMPLETE: return MgmtEvent::Opcode::DEVICE_CONNECTED;
+            default: return MgmtEvent::Opcode::INVALID;
+        }
+    }
+    switch( evt ) {
+        case HCIEventType::CONN_COMPLETE: return MgmtEvent::Opcode::DEVICE_CONNECTED;
+        case HCIEventType::DISCONN_COMPLETE: return MgmtEvent::Opcode::DEVICE_DISCONNECTED;
+        case HCIEventType::CMD_COMPLETE: return MgmtEvent::Opcode::CMD_COMPLETE;
+        case HCIEventType::CMD_STATUS: return MgmtEvent::Opcode::CMD_STATUS;
+        default: return MgmtEvent::Opcode::INVALID;
+    }
+}
+
+std::shared_ptr<MgmtEvent> HCIHandler::translate(std::shared_ptr<HCIEvent> ev) {
+    const HCIEventType evt = ev->getEventType();
+    const HCIMetaEventType mevt = ev->getMetaEventType();
+
+    if( HCIEventType::LE_META == evt ) {
+        switch( mevt ) {
+            case HCIMetaEventType::LE_CONN_COMPLETE: {
+                HCIStatusCode status;
+                const hci_ev_le_conn_complete * ev_cc = getMetaReplyStruct<hci_ev_le_conn_complete>(ev, mevt, &status);
+                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) );
+                } else {
+                    return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnectFailed(dev_id, ev_cc->bdaddr, addrType, status) );
+                }
+            }
+            default:
+                return nullptr;
+        }
+    }
+    switch( evt ) {
+        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) );
+            } else {
+                return std::shared_ptr<MgmtEvent>( new MgmtEvtDeviceConnectFailed(dev_id, ev_cc->bdaddr, BDAddressType::BDADDR_BREDR, status) );
+            }
+        }
+        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) );
+                }
+                return nullptr; // unknown handle!
+            }
+            return nullptr;
+        }
+        default: return nullptr;
+    }
+}
+
 void HCIHandler::hciReaderThreadImpl() {
     {
         const std::lock_guard<std::mutex> lock(mtx_hciReaderInit); // RAII-style acquire and relinquish via destructor
@@ -86,13 +165,13 @@ void HCIHandler::hciReaderThreadImpl() {
             std::shared_ptr<HCIEvent> event( HCIEvent::getSpecialized(rbuffer.get_ptr(), len) );
             if( nullptr == event ) {
                 // not an event ...
-                ERR_PRINT("HCIHandler::reader: drop non-event %s", bytesHexString(rbuffer.get_ptr(), 0, len, true /* lsbFirst*/).c_str());
+                ERR_PRINT("HCIHandler::reader: Drop (non-event) %s", bytesHexString(rbuffer.get_ptr(), 0, len, true /* lsbFirst*/).c_str());
                 continue;
             }
             const HCIMetaEventType mec = event->getMetaEventType();
             if( HCIMetaEventType::INVALID != mec && !filter_test_metaev(mec) ) {
                 // DROP
-                DBG_PRINT("HCIHandler::reader: drop %s", event->toString().c_str());
+                DBG_PRINT("HCIHandler::reader: Drop (meta filter) %s", event->toString().c_str());
                 continue; // next packet
             }
 #ifdef SHOW_LE_ADVERTISING
@@ -106,13 +185,41 @@ void HCIHandler::hciReaderThreadImpl() {
                 continue; // next packet
             }
 #endif /* SHOW_LE_ADVERTISING */
-            if( hciEventRing.isFull() ) {
-                std::shared_ptr<HCIEvent> ev = hciEventRing.get();
-                INFO_PRINT("HCIHandler::reader: full ring, dropping oldest %s",
-                        ( nullptr != ev ) ? ev->toString().c_str() : "nil");
+            if( event->isEvent(HCIEventType::CMD_STATUS) || event->isEvent(HCIEventType::CMD_COMPLETE) )
+            {
+                if( hciEventRing.isFull() ) {
+                    std::shared_ptr<HCIEvent> ev = hciEventRing.get();
+                    INFO_PRINT("HCIHandler::reader: Drop (oldest, ring full) %s",
+                            ( nullptr != ev ) ? ev->toString().c_str() : "nil");
+                }
+                DBG_PRINT("HCIHandler::reader: CmdResult %s", event->toString().c_str());
+                hciEventRing.putBlocking( event );
+            } else {
+                // issue a callback
+                std::shared_ptr<MgmtEvent> mevent = translate(event);
+                if( nullptr != mevent ) {
+                    const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
+                    MgmtEventCallbackList & mgmtEventCallbackList = mgmtEventCallbackLists[static_cast<uint16_t>(mevent->getOpcode())];
+                    int invokeCount = 0;
+                    if( mgmtEventCallbackList.size() > 0 ) {
+                        for (auto it = mgmtEventCallbackList.begin(); it != mgmtEventCallbackList.end(); ++it) {
+                            try {
+                                it->invoke(mevent);
+                            } catch (std::exception &e) {
+                                ERR_PRINT("HCIHandler::fwdPacketReceived-CBs %d/%zd: MgmtEventCallback %s : Caught exception %s",
+                                        invokeCount+1, mgmtEventCallbackList.size(),
+                                        it->toString().c_str(), e.what());
+                            }
+                            invokeCount++;
+                        }
+                    }
+                    DBG_PRINT("HCIHandler::reader: Event %s -> %d/%zd callbacks; source %s",
+                            mevent->toString().c_str(), invokeCount, mgmtEventCallbackList.size(), event->toString().c_str());
+                    (void)invokeCount;
+                } else {
+                    DBG_PRINT("HCIHandler::reader: Drop (no translation) %s", event->toString().c_str());
+                }
             }
-            DBG_PRINT("HCIHandler::reader: got %s", event->toString().c_str());
-            hciEventRing.putBlocking( event );
         } else if( ETIMEDOUT != errno && !hciReaderShallStop ) { // expected exits
             ERR_PRINT("HCIHandler::reader: HCIComm error");
         }
@@ -159,14 +266,6 @@ std::shared_ptr<HCIEvent> HCIHandler::sendWithCmdCompleteReply(HCICommand &req,
 
     *res = nullptr;
 
-    if( pass_replies_only_filter ) {
-        hci_ufilter filter = filter_mask;
-        HCIComm::filter_set_opcode(number(req.getOpcode()), &filter);
-        if (setsockopt(comm.dd(), SOL_HCI, HCI_FILTER, &filter, sizeof(filter)) < 0) {
-            ERR_PRINT("HCIHandler::sendWithCmdCompleteReply.0: setsockopt");
-            return nullptr;
-        }
-    }
     int retryCount = 0;
     std::shared_ptr<HCIEvent> ev = nullptr;
 
@@ -203,16 +302,11 @@ std::shared_ptr<HCIEvent> HCIHandler::sendWithCmdCompleteReply(HCICommand &req,
     }
 
 exit:
-    if( pass_replies_only_filter ) {
-        if (setsockopt(comm.dd(), SOL_HCI, HCI_FILTER, &filter_mask, sizeof(filter_mask)) < 0) {
-            ERR_PRINT("HCIHandler::sendWithCmdCompleteReply.X: setsockopt");
-        }
-    }
     return ev;
 }
 
 HCIHandler::HCIHandler(const BTMode btMode, const uint16_t dev_id, const int replyTimeoutMS)
-:pass_replies_only_filter(true), btMode(btMode), dev_id(dev_id), rbuffer(HCI_MAX_MTU),
+:btMode(btMode), dev_id(dev_id), rbuffer(HCI_MAX_MTU),
  comm(dev_id, HCI_CHANNEL_RAW, Defaults::HCI_READER_THREAD_POLL_TIMEOUT), replyTimeoutMS(replyTimeoutMS),
  hciEventRing(HCI_EVT_RING_CAPACITY), hciReaderRunning(false), hciReaderShallStop(false)
 {
@@ -254,24 +348,18 @@ HCIHandler::HCIHandler(const BTMode btMode, const uint16_t dev_id, const int rep
 #endif
         HCIComm::filter_clear(&filter_mask);
         HCIComm::filter_set_ptype(number(HCIPacketType::EVENT),  &filter_mask); // only EVENTs
-        if( pass_replies_only_filter ) {
-            // Setup template filter mask, copied and adjusted for each reply
-            HCIComm::filter_set_event(number(HCIEventType::CMD_COMPLETE), &filter_mask);
-            HCIComm::filter_set_event(number(HCIEventType::CMD_STATUS), &filter_mask);
-            HCIComm::filter_set_opcode(number(HCIOpcode::RESET), &filter_mask); // listen to RESET command by default (hmm)
-        } else {
-            // Setup generic filter mask for all events
-            // HCIComm::filter_all_events(&filter_mask); // all events
-            HCIComm::filter_set_event(number(HCIEventType::CONN_COMPLETE), &filter_mask);
-            HCIComm::filter_set_event(number(HCIEventType::DISCONN_COMPLETE), &filter_mask);
-            HCIComm::filter_set_event(number(HCIEventType::CMD_COMPLETE), &filter_mask);
-            HCIComm::filter_set_event(number(HCIEventType::CMD_STATUS), &filter_mask);
-            HCIComm::filter_set_event(number(HCIEventType::HARDWARE_ERROR), &filter_mask);
-            HCIComm::filter_set_event(number(HCIEventType::LE_META), &filter_mask);
-            HCIComm::filter_set_event(number(HCIEventType::DISCONN_PHY_LINK_COMPLETE), &filter_mask);
-            HCIComm::filter_set_event(number(HCIEventType::DISCONN_LOGICAL_LINK_COMPLETE), &filter_mask);
-            HCIComm::filter_set_opcode(0, &filter_mask); // all opcode
-        }
+        // Setup generic filter mask for all events, this is also required for
+        // HCIComm::filter_all_events(&filter_mask); // all events
+        HCIComm::filter_set_event(number(HCIEventType::CONN_COMPLETE), &filter_mask);
+        HCIComm::filter_set_event(number(HCIEventType::DISCONN_COMPLETE), &filter_mask);
+        HCIComm::filter_set_event(number(HCIEventType::CMD_COMPLETE), &filter_mask);
+        HCIComm::filter_set_event(number(HCIEventType::CMD_STATUS), &filter_mask);
+        HCIComm::filter_set_event(number(HCIEventType::HARDWARE_ERROR), &filter_mask);
+        HCIComm::filter_set_event(number(HCIEventType::LE_META), &filter_mask);
+        // HCIComm::filter_set_event(number(HCIEventType::DISCONN_PHY_LINK_COMPLETE), &filter_mask);
+        // HCIComm::filter_set_event(number(HCIEventType::DISCONN_LOGICAL_LINK_COMPLETE), &filter_mask);
+        HCIComm::filter_set_opcode(0, &filter_mask); // all opcode
+
         if (setsockopt(comm.dd(), SOL_HCI, HCI_FILTER, &filter_mask, sizeof(filter_mask)) < 0) {
             ERR_PRINT("HCIHandler::ctor: setsockopt");
             goto fail;
@@ -280,15 +368,19 @@ HCIHandler::HCIHandler(const BTMode btMode, const uint16_t dev_id, const int rep
     // Mandatory own LE_META filter
     {
         uint32_t mask = 0;
-        if( !pass_replies_only_filter ) {
-            // filter_all_metaevs(mask);
-            filter_set_metaev(HCIMetaEventType::LE_CONN_COMPLETE, mask);
+        // filter_all_metaevs(mask);
+        filter_set_metaev(HCIMetaEventType::LE_CONN_COMPLETE, mask);
 #ifdef SHOW_LE_ADVERTISING
-            filter_set_metaev(HCIMetaEventType::LE_ADVERTISING_REPORT, mask);
+        filter_set_metaev(HCIMetaEventType::LE_ADVERTISING_REPORT, mask);
 #endif
-        }
         filter_put_metaevs(mask);
     }
+    // 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));
+    }
     {
         HCICommand req0(HCIOpcode::READ_LOCAL_VERSION, 0);
         const hci_rp_read_local_version * ev_lv;
@@ -299,8 +391,7 @@ HCIHandler::HCIHandler(const BTMode btMode, const uint16_t dev_id, const int rep
             ERR_PRINT("HCIHandler::ctor: failed READ_LOCAL_VERSION: 0x%x (%s)", number(status), getHCIStatusCodeString(status).c_str());
             goto fail;
         }
-        INFO_PRINT("HCIHandler: replies_only %d, LOCAL_VERSION: %d (rev %d), manuf 0x%x, lmp %d (subver %d)",
-                pass_replies_only_filter,
+        INFO_PRINT("HCIHandler: LOCAL_VERSION: %d (rev %d), manuf 0x%x, lmp %d (subver %d)",
                 ev_lv->hci_ver, le_to_cpu(ev_lv->hci_rev), le_to_cpu(ev_lv->manufacturer),
                 ev_lv->lmp_ver, le_to_cpu(ev_lv->lmp_subver));
     }
@@ -318,6 +409,8 @@ void HCIHandler::close() {
     const std::lock_guard<std::recursive_mutex> lock(mtx); // RAII-style acquire and relinquish via destructor
     DBG_PRINT("HCIHandler::close: Start");
 
+    clearAllMgmtEventCallbacks();
+
     const pthread_t tid_self = pthread_self();
     const pthread_t tid_reader = hciReaderThreadId;
     hciReaderThreadId = 0;
@@ -352,16 +445,13 @@ HCIStatusCode HCIHandler::reset() {
     return ev_cc->getReturnStatus(0);
 }
 
-HCIStatusCode HCIHandler::le_create_conn(uint16_t * handle_return, const EUI48 &peer_bdaddr,
+HCIStatusCode HCIHandler::le_create_conn(const EUI48 &peer_bdaddr,
                             const HCIAddressType peer_mac_type,
                             const HCIAddressType own_mac_type,
                             const uint16_t le_scan_interval, const uint16_t le_scan_window,
                             const uint16_t conn_interval_min, const uint16_t conn_interval_max,
                             const uint16_t conn_latency, const uint16_t supervision_timeout) {
     const std::lock_guard<std::recursive_mutex> lock(mtx); // RAII-style acquire and relinquish via destructor
-    if( nullptr != handle_return ) {
-        *handle_return = 0;
-    }
     if( !comm.isOpen() ) {
         ERR_PRINT("HCIHandler::le_create_conn: device not open");
         return HCIStatusCode::INTERNAL_FAILURE;
@@ -386,31 +476,19 @@ HCIStatusCode HCIHandler::le_create_conn(uint16_t * handle_return, const EUI48 &
     cp->min_ce_len = cpu_to_le(min_ce_length);
     cp->max_ce_len = cpu_to_le(max_ce_length);
 
-    const hci_ev_le_conn_complete * ev_cc;
     HCIStatusCode status;
-    std::shared_ptr<HCIEvent> ev = processStructMetaCmd(req0, HCIMetaEventType::LE_CONN_COMPLETE, &ev_cc, &status);
-
-    if( HCIStatusCode::SUCCESS != status ) {
-        return status;
-    }
-    if( nullptr != handle_return ) {
-        *handle_return = ev_cc->handle;
-    }
-    return HCIStatusCode::SUCCESS;
+    std::shared_ptr<HCIEvent> ev = processStructCommand(req0, &status);
+    return status;
 }
 
-HCIStatusCode HCIHandler::create_conn(uint16_t * handle_return, const EUI48 &bdaddr,
+HCIStatusCode HCIHandler::create_conn(const EUI48 &bdaddr,
                                      const uint16_t pkt_type,
                                      const uint16_t clock_offset, const uint8_t role_switch) {
     const std::lock_guard<std::recursive_mutex> lock(mtx); // RAII-style acquire and relinquish via destructor
-    if( nullptr != handle_return ) {
-        *handle_return = 0;
-    }
     if( !comm.isOpen() ) {
         ERR_PRINT("HCIHandler::create_conn: device not open");
         return HCIStatusCode::INTERNAL_FAILURE;
     }
-
     HCIStructCommand<hci_cp_create_conn> req0(HCIOpcode::CREATE_CONN);
     hci_cp_create_conn * cp = req0.getWStruct();
     bzero((void*)cp, sizeof(*cp));
@@ -421,20 +499,14 @@ HCIStatusCode HCIHandler::create_conn(uint16_t * handle_return, const EUI48 &bda
     cp->clock_offset = cpu_to_le(clock_offset);
     cp->role_switch = role_switch;
 
-    const hci_ev_conn_complete * ev_cc;
     HCIStatusCode status;
-    std::shared_ptr<HCIEvent> ev = processStructCommand(req0, HCIEventType::CONN_COMPLETE, &ev_cc, &status);
-
-    if( HCIStatusCode::SUCCESS != status ) {
-        return status;
-    }
-    if( nullptr != handle_return ) {
-        *handle_return = ev_cc->handle;
-    }
-    return HCIStatusCode::SUCCESS;
+    std::shared_ptr<HCIEvent> ev = processStructCommand(req0, &status);
+    return status;
 }
 
-HCIStatusCode HCIHandler::disconnect(const uint16_t conn_handle, const HCIStatusCode reason) {
+HCIStatusCode HCIHandler::disconnect(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
     if( !comm.isOpen() ) {
         ERR_PRINT("HCIHandler::create_conn: device not open");
@@ -443,15 +515,26 @@ HCIStatusCode HCIHandler::disconnect(const uint16_t conn_handle, const HCIStatus
     if( 0 == conn_handle ) {
         return HCIStatusCode::SUCCESS;
     }
+    {
+        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;
+            }
+        }
+        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);
 
-    const hci_ev_disconn_complete * ev_cc;
     HCIStatusCode status;
-    std::shared_ptr<HCIEvent> ev = processStructCommand(req0, HCIEventType::DISCONN_COMPLETE, &ev_cc, &status);
+    std::shared_ptr<HCIEvent> ev = processStructCommand(req0, &status);
     return status;
 }
 
@@ -496,24 +579,13 @@ std::shared_ptr<HCIEvent> HCIHandler::processSimpleCommand(HCIOpcode opc, const
     return ev;
 }
 
-template<typename hci_command_struct, typename hci_cmd_event_struct>
-std::shared_ptr<HCIEvent> HCIHandler::processStructCommand(HCIStructCommand<hci_command_struct> &req,
-                                                           HCIEventType evc, const hci_cmd_event_struct **res, HCIStatusCode *status)
+template<typename hci_command_struct>
+std::shared_ptr<HCIEvent> HCIHandler::processStructCommand(HCIStructCommand<hci_command_struct> &req, HCIStatusCode *status)
 {
     const std::lock_guard<std::recursive_mutex> lock(mtx_sendReply); // RAII-style acquire and relinquish via destructor
 
-    *res = nullptr;
     *status = HCIStatusCode::INTERNAL_FAILURE;
 
-    if( pass_replies_only_filter ) {
-        hci_ufilter filter = filter_mask;
-        HCIComm::filter_set_event(number(evc), &filter_mask);
-        HCIComm::filter_set_opcode(number(req.getOpcode()), &filter);
-        if (setsockopt(comm.dd(), SOL_HCI, HCI_FILTER, &filter, sizeof(filter)) < 0) {
-            ERR_PRINT("HCIHandler::processStructCommand.0: setsockopt");
-            return nullptr;
-        }
-    }
     int retryCount = 0;
     std::shared_ptr<HCIEvent> ev = nullptr;
 
@@ -525,21 +597,14 @@ std::shared_ptr<HCIEvent> HCIHandler::processStructCommand(HCIStructCommand<hci_
         ev = getNextReply(req, retryCount);
         if( nullptr == ev ) {
             break; // timeout, leave loop
-        } else if( ev->isEvent(evc) ) {
-            break; // gotcha, leave loop
         } else if( ev->isEvent(HCIEventType::CMD_STATUS) ) {
-            // pending command .. wait for result
             HCICommandStatusEvent * ev_cs = static_cast<HCICommandStatusEvent*>(ev.get());
-            if( HCIStatusCode::SUCCESS != ev_cs->getStatus() ) {
-                *status = ev_cs->getStatus();
-                WARN_PRINT("HCIHandler::processStructCommand %s -> %s: Status 0x%2.2X (%s), errno %d %s: res %s, req %s",
-                        getHCIOpcodeString(req.getOpcode()).c_str(), getHCIEventTypeString(evc).c_str(),
-                        number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
-                        ev_cs->toString().c_str(), req.toString().c_str());
-                goto exit; // bad status, leave
-            }
-            retryCount++;
-            continue; // next packet
+            *status = ev_cs->getStatus();
+            DBG_PRINT("HCIHandler::processStructCommand %s -> Status 0x%2.2X (%s), errno %d %s: res %s, req %s",
+                    getHCIOpcodeString(req.getOpcode()).c_str(),
+                    number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
+                    ev_cs->toString().c_str(), req.toString().c_str());
+            break; // gotcha, leave loop - pending completion result handled via callback
         } else {
             retryCount++;
             continue; // next packet
@@ -547,119 +612,124 @@ std::shared_ptr<HCIEvent> HCIHandler::processStructCommand(HCIStructCommand<hci_
     }
     if( nullptr == ev ) {
         // timeout exit
-        WARN_PRINT("HCIHandler::processStructCommand %s -> %s: Status 0x%2.2X (%s), errno %d %s: res nullptr, req %s",
-                getHCIOpcodeString(req.getOpcode()).c_str(), getHCIEventTypeString(evc).c_str(),
+        WARN_PRINT("HCIHandler::processStructCommand %s -> Status 0x%2.2X (%s), errno %d %s: res nullptr, req %s",
+                getHCIOpcodeString(req.getOpcode()).c_str(),
                 number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
                 req.toString().c_str());
-    } else {
-        typedef HCIStructCmdCompleteEvt<hci_cmd_event_struct> HCIConnCompleteEvt;
-        HCIConnCompleteEvt * ev_cc = static_cast<HCIConnCompleteEvt*>(ev.get());
-        if( ev_cc->isTypeAndSizeValid(evc) ) {
-            *status = ev_cc->getStatus();
-            *res = ev_cc->getStruct();
-            DBG_PRINT("HCIHandler::processStructCommand %s -> %s: Status 0x%2.2X (%s): res %s, req %s",
-                    getHCIOpcodeString(req.getOpcode()).c_str(), getHCIEventTypeString(evc).c_str(),
-                    number(*status), getHCIStatusCodeString(*status).c_str(),
-                    ev_cc->toString().c_str(), req.toString().c_str());
-        } else {
-            WARN_PRINT("HCIHandler::processStructCommand %s -> %s: Status 0x%2.2X (%s), errno %d %s: res %s, req %s",
-                    getHCIOpcodeString(req.getOpcode()).c_str(), getHCIEventTypeString(evc).c_str(),
-                    number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
-                    ev_cc->toString().c_str(), req.toString().c_str());
-        }
     }
 
 exit:
-    if( pass_replies_only_filter ) {
-        if (setsockopt(comm.dd(), SOL_HCI, HCI_FILTER, &filter_mask, sizeof(filter_mask)) < 0) {
-            ERR_PRINT("HCIHandler::processStructCommand.X: setsockopt");
-        }
-    }
     return ev;
 }
 
-template<typename hci_command_struct, typename hci_cmd_event_struct>
-std::shared_ptr<HCIEvent> HCIHandler::processStructMetaCmd(HCIStructCommand<hci_command_struct> &req,
-                                                           HCIMetaEventType mec, const hci_cmd_event_struct **res, HCIStatusCode *status)
+template<typename hci_cmd_event_struct>
+const hci_cmd_event_struct* HCIHandler::getReplyStruct(std::shared_ptr<HCIEvent> event, HCIEventType evc, HCIStatusCode *status)
 {
-    const std::lock_guard<std::recursive_mutex> lock(mtx_sendReply); // RAII-style acquire and relinquish via destructor
-
-    *res = nullptr;
+    const hci_cmd_event_struct* res = nullptr;
     *status = HCIStatusCode::INTERNAL_FAILURE;
 
-    if( pass_replies_only_filter ) {
-        hci_ufilter filter = filter_mask;
-        HCIComm::filter_set_event(number(HCIEventType::LE_META), &filter_mask);
-        HCIComm::filter_set_opcode(number(req.getOpcode()), &filter);
-        if (setsockopt(comm.dd(), SOL_HCI, HCI_FILTER, &filter, sizeof(filter)) < 0) {
-            ERR_PRINT("HCIHandler::processStructMetaCmd.0: setsockopt");
-            return nullptr;
-        }
-        uint32_t mask=0;
-        filter_set_metaev(mec, mask);
-        filter_put_metaevs(mask);
+    typedef HCIStructCmdCompleteEvt<hci_cmd_event_struct> HCITypeCmdCompleteEvt;
+    HCITypeCmdCompleteEvt * ev_cc = static_cast<HCITypeCmdCompleteEvt*>(event.get());
+    if( ev_cc->isTypeAndSizeValid(evc) ) {
+        *status = ev_cc->getStatus();
+        res = ev_cc->getStruct();
+        DBG_PRINT("HCIHandler::getReplyStruct: %s: Status 0x%2.2X (%s): res %s",
+                getHCIEventTypeString(evc).c_str(),
+                number(*status), getHCIStatusCodeString(*status).c_str(),
+                ev_cc->toString().c_str());
+    } else {
+        WARN_PRINT("HCIHandler::getReplyStruct: %s: Type or size mismatch: Status 0x%2.2X (%s), errno %d %s: res %s",
+                getHCIEventTypeString(evc).c_str(),
+                number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
+                ev_cc->toString().c_str());
     }
-    int retryCount = 0;
-    std::shared_ptr<HCIEvent> ev = nullptr;
+    return res;
+}
 
-    if( !sendCommand(req) ) {
-        goto exit;
+template<typename hci_cmd_event_struct>
+const hci_cmd_event_struct* HCIHandler::getMetaReplyStruct(std::shared_ptr<HCIEvent> event, HCIMetaEventType mec, HCIStatusCode *status)
+{
+    const hci_cmd_event_struct* res = nullptr;
+    *status = HCIStatusCode::INTERNAL_FAILURE;
+
+    typedef HCIStructCmdCompleteMetaEvt<hci_cmd_event_struct> HCITypeCmdCompleteMetaEvt;
+    HCITypeCmdCompleteMetaEvt * ev_cc = static_cast<HCITypeCmdCompleteMetaEvt*>(event.get());
+    if( ev_cc->isTypeAndSizeValid(mec) ) {
+        *status = ev_cc->getStatus();
+        res = ev_cc->getStruct();
+        DBG_PRINT("HCIHandler::getMetaReplyStruct: %s: Status 0x%2.2X (%s): res %s",
+                  getHCIMetaEventTypeString(mec).c_str(),
+                  number(*status), getHCIStatusCodeString(*status).c_str(),
+                  ev_cc->toString().c_str());
+    } else {
+        WARN_PRINT("HCIHandler::getMetaReplyStruct: %s: Type or size mismatch: Status 0x%2.2X (%s), errno %d %s: res %s",
+                  getHCIMetaEventTypeString(mec).c_str(),
+                  number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
+                  ev_cc->toString().c_str());
     }
+    return res;
+}
 
-    while( retryCount < HCI_READ_PACKET_MAX_RETRY ) {
-        ev = getNextReply(req, retryCount);
-        if( nullptr == ev ) {
-            break; // timeout, leave loop
-        } else if( ev->isMetaEvent(mec) ) {
-            break; // gotcha, leave loop
-        } else if( ev->isEvent(HCIEventType::CMD_STATUS) ) {
-            // pending command .. wait for result
-            HCICommandStatusEvent * ev_cs = static_cast<HCICommandStatusEvent*>(ev.get());
-            if( HCIStatusCode::SUCCESS != ev_cs->getStatus() ) {
-                *status = ev_cs->getStatus();
-                WARN_PRINT("HCIHandler::processStructMetaCmd %s -> %s: Status 0x%2.2X (%s), errno %d %s: res %s, req %s",
-                        getHCIOpcodeString(req.getOpcode()).c_str(), getHCIMetaEventTypeString(mec).c_str(),
-                        number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
-                        ev_cs->toString().c_str(), req.toString().c_str());
-                goto exit; // bad status, leave
-            }
-            retryCount++;
-            continue; // next packet
-        } else {
-            retryCount++;
-            continue; // next packet
+/***
+ *
+ * MgmtEventCallback section
+ *
+ */
+
+void HCIHandler::addMgmtEventCallback(const MgmtEvent::Opcode opc, const MgmtEventCallback &cb) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
+    checkMgmtEventCallbackListsIndex(opc);
+    MgmtEventCallbackList &l = mgmtEventCallbackLists[static_cast<uint16_t>(opc)];
+    for (auto it = l.begin(); it != l.end(); ++it) {
+        if ( *it == cb ) {
+            // already exists for given adapter
+            return;
         }
     }
-    if( nullptr == ev ) {
-        // timeout exit
-        WARN_PRINT("HCIHandler::processStructMetaCmd %s -> %s: Status 0x%2.2X (%s), errno %d %s: res nullptr, req %s",
-                getHCIOpcodeString(req.getOpcode()).c_str(), getHCIMetaEventTypeString(mec).c_str(),
-                number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
-                req.toString().c_str());
-    } else {
-        typedef HCIStructCmdCompleteMetaEvt<hci_cmd_event_struct> HCIConnCompleteMetaEvt;
-        HCIConnCompleteMetaEvt * ev_cc = static_cast<HCIConnCompleteMetaEvt*>(ev.get());
-        if( ev_cc->isTypeAndSizeValid(mec) ) {
-            *status = ev_cc->getStatus();
-            *res = ev_cc->getStruct();
-            DBG_PRINT("HCIHandler::processStructMetaCmd %s -> %s: Status 0x%2.2X (%s): res %s, req %s",
-                      getHCIOpcodeString(req.getOpcode()).c_str(), getHCIMetaEventTypeString(mec).c_str(),
-                      number(*status), getHCIStatusCodeString(*status).c_str(),
-                      ev_cc->toString().c_str(), req.toString().c_str());
+    l.push_back( cb );
+}
+int HCIHandler::removeMgmtEventCallback(const MgmtEvent::Opcode opc, const MgmtEventCallback &cb) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
+    checkMgmtEventCallbackListsIndex(opc);
+    int count = 0;
+    MgmtEventCallbackList &l = mgmtEventCallbackLists[static_cast<uint16_t>(opc)];
+    for (auto it = l.begin(); it != l.end(); ) {
+        if ( *it == cb ) {
+            it = l.erase(it);
+            count++;
         } else {
-            WARN_PRINT("HCIHandler::processStructMetaCmd %s -> %s: Type or size mismatch: Status 0x%2.2X (%s), errno %d %s: res %s, req %s",
-                      getHCIOpcodeString(req.getOpcode()).c_str(), getHCIMetaEventTypeString(mec).c_str(),
-                      number(*status), getHCIStatusCodeString(*status).c_str(), errno, strerror(errno),
-                      ev_cc->toString().c_str(), req.toString().c_str());
+            ++it;
         }
     }
-
-exit:
-    if( pass_replies_only_filter ) {
-        filter_put_metaevs(0);
-        if (setsockopt(comm.dd(), SOL_HCI, HCI_FILTER, &filter_mask, sizeof(filter_mask)) < 0) {
-            ERR_PRINT("HCIHandler::processStructMetaCmd.X: setsockopt");
-        }
+    return count;
+}
+void HCIHandler::clearMgmtEventCallbacks(const MgmtEvent::Opcode opc) {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
+    checkMgmtEventCallbackListsIndex(opc);
+    mgmtEventCallbackLists[static_cast<uint16_t>(opc)].clear();
+}
+void HCIHandler::clearAllMgmtEventCallbacks() {
+    const std::lock_guard<std::recursive_mutex> lock(mtx_callbackLists); // RAII-style acquire and relinquish via destructor
+    for(size_t i=0; i<mgmtEventCallbackLists.size(); i++) {
+        mgmtEventCallbackLists[i].clear();
     }
-    return ev;
+}
+
+bool HCIHandler::mgmtEvDeviceDisconnectedCB(std::shared_ptr<MgmtEvent> e) {
+    DBG_PRINT("HCIHandler::EventCB:DeviceDisconnected: %s", e->toString().c_str());
+    const MgmtEvtDeviceDisconnected &event = *static_cast<const MgmtEvtDeviceDisconnected *>(e.get());
+    (void)event;
+    return true;
+}
+bool HCIHandler::mgmtEvDeviceConnectedCB(std::shared_ptr<MgmtEvent> e) {
+    DBG_PRINT("HCIHandler::EventCB:DeviceConnected: %s", e->toString().c_str());
+    const MgmtEvtDeviceConnected &event = *static_cast<const MgmtEvtDeviceConnected *>(e.get());
+    (void)event;
+    return true;
+}
+bool HCIHandler::mgmtEvConnectFailedCB(std::shared_ptr<MgmtEvent> e) {
+    DBG_PRINT("HCIHandler::EventCB:ConnectFailed: %s", e->toString().c_str());
+    const MgmtEvtDeviceConnectFailed &event = *static_cast<const MgmtEvtDeviceConnectFailed *>(e.get());
+    (void)event;
+    return true;
 }