5 files changed, 42 insertions, 2 deletions
diff --git a/hci/include/hci_packet_factory.h b/hci/include/hci_packet_factory.h
index ac6f12a04..0056817b6 100755
--- a/hci/include/hci_packet_factory.h
+++ b/hci/include/hci_packet_factory.h
@@ -41,6 +41,7 @@ typedef struct {
   BT_HDR *(*make_ble_read_suggested_default_data_length)(void);
   BT_HDR *(*make_ble_set_event_mask)(const bt_event_mask_t *event_mask);
   BT_HDR *(*make_read_local_supported_codecs)(void);
+  BT_HDR *(*make_ble_read_offload_features_support)(void);
 } hci_packet_factory_t;
 
 const hci_packet_factory_t *hci_packet_factory_get_interface();
diff --git a/hci/include/hci_packet_parser.h b/hci/include/hci_packet_parser.h
index 51d00eaec..796a5df86 100755
--- a/hci/include/hci_packet_parser.h
+++ b/hci/include/hci_packet_parser.h
@@ -96,6 +96,10 @@ typedef struct {
     BT_HDR *response,
     uint8_t *no_of_local_supported_codecs, uint8_t *local_supported_codecs);
 
+  void (*parse_ble_read_offload_features_response)(
+    BT_HDR *response,
+    bool *ble_offload_features_supported);
+
 } hci_packet_parser_t;
 
 const hci_packet_parser_t *hci_packet_parser_get_interface();
diff --git a/hci/src/hci_layer.c b/hci/src/hci_layer.c
index 94e154eed..6a8050b4b 100644
--- a/hci/src/hci_layer.c
+++ b/hci/src/hci_layer.c
@@ -750,6 +750,20 @@ static waiting_command_t *get_waiting_command(command_opcode_t opcode) {
       node = list_next(node)) {
     waiting_command_t *wait_entry = list_node(node);
 
+    if (!wait_entry || wait_entry->opcode != opcode)
+      continue;
+
+    list_remove(commands_pending_response, wait_entry);
+
+    pthread_mutex_unlock(&commands_pending_response_lock);
+    return wait_entry;
+  }
+  // look for any command complete with improper VS Opcode
+  for (const list_node_t *node = list_begin(commands_pending_response);
+      node != list_end(commands_pending_response);
+      node = list_next(node)) {
+    waiting_command_t *wait_entry = list_node(node);
+
     if (!wait_entry || wait_entry->opcode != opcode) {
         if(((wait_entry->opcode & HCI_GRP_VENDOR_SPECIFIC) == HCI_GRP_VENDOR_SPECIFIC) &&
            ((opcode & HCI_GRP_VENDOR_SPECIFIC) == HCI_GRP_VENDOR_SPECIFIC)) {
diff --git a/hci/src/hci_packet_factory.c b/hci/src/hci_packet_factory.c
index 96f578a70..d43d274bf 100755
--- a/hci/src/hci_packet_factory.c
+++ b/hci/src/hci_packet_factory.c
@@ -141,6 +141,10 @@ static BT_HDR *make_read_local_supported_codecs(void) {
     return make_command_no_params(HCI_READ_LOCAL_SUPPORTED_CODECS);
 }
 
+static BT_HDR *make_ble_read_offload_features_support(void) {
+    return make_command_no_params(HCI_BLE_VENDOR_CAP_OCF);
+}
+
 static BT_HDR *make_ble_set_event_mask(const bt_event_mask_t *event_mask) {
   uint8_t *stream;
   uint8_t parameter_size = sizeof(bt_event_mask_t);
@@ -198,7 +202,8 @@ static const hci_packet_factory_t interface = {
   make_ble_read_resolving_list_size,
   make_ble_read_suggested_default_data_length,
   make_ble_set_event_mask,
-  make_read_local_supported_codecs
+  make_read_local_supported_codecs,
+  make_ble_read_offload_features_support
 };
 
 const hci_packet_factory_t *hci_packet_factory_get_interface() {
diff --git a/hci/src/hci_packet_parser.c b/hci/src/hci_packet_parser.c
index 58633d12e..252652853 100755
--- a/hci/src/hci_packet_parser.c
+++ b/hci/src/hci_packet_parser.c
@@ -87,6 +87,21 @@ static void parse_read_local_supported_codecs_response(
   buffer_allocator->free(response);
 }
 
+
+static void parse_ble_read_offload_features_response(
+    BT_HDR *response,
+    bool *ble_offload_features_supported) {
+
+  uint8_t *stream = read_command_complete_header(response, NO_OPCODE_CHECKING, 0 /* bytes after */);
+  if(stream) {
+    *ble_offload_features_supported  = true;
+  } else {
+    *ble_offload_features_supported  = false;
+  }
+
+  buffer_allocator->free(response);
+}
+
 static void parse_read_bd_addr_response(
     BT_HDR *response,
     bt_bdaddr_t *address_ptr) {
@@ -253,7 +268,8 @@ static const hci_packet_parser_t interface = {
   parse_ble_read_local_supported_features_response,
   parse_ble_read_resolving_list_size_response,
   parse_ble_read_suggested_default_data_length_response,
-  parse_read_local_supported_codecs_response
+  parse_read_local_supported_codecs_response,
+  parse_ble_read_offload_features_response
 };
 
 const hci_packet_parser_t *hci_packet_parser_get_interface() {