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.

2 files changed, 271 insertions, 200 deletions

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;
 }