nimble/host: Add internal mqueue implementation

This makes NimBLE use copy of os_mqueue instead of kernel/os
implementation. This is to allow NimBLE use its own APIs for OS
primitives (i.e. queues and events) which allows for easier porting to
other OS.

nimble/host/src/ble_hs.c

@@ -73,7 +73,7 @@ static struct os_callout ble_hs_timer_timer;
 /* Shared queue that the host uses for work items. */
 static struct os_eventq *ble_hs_evq;
 
-static struct os_mqueue ble_hs_rx_q;
+static struct ble_mqueue ble_hs_rx_q;
 
 static struct os_mutex ble_hs_mutex;
 
@@ -222,7 +222,7 @@ ble_hs_process_rx_data_queue(void)
 {
     struct os_mbuf *om;
 
-    while ((om = os_mqueue_get(&ble_hs_rx_q)) != NULL) {
+    while ((om = ble_mqueue_get(&ble_hs_rx_q)) != NULL) {
 #if BLE_MONITOR
         ble_monitor_send_om(BLE_MONITOR_OPCODE_ACL_RX_PKT, om);
 #endif
@@ -305,7 +305,7 @@ ble_hs_clear_rx_queue(void)
 {
     struct os_mbuf *om;
 
-    while ((om = os_mqueue_get(&ble_hs_rx_q)) != NULL) {
+    while ((om = ble_mqueue_get(&ble_hs_rx_q)) != NULL) {
         os_mbuf_free_chain(om);
     }
 }
@@ -618,7 +618,7 @@ ble_hs_rx_data(struct os_mbuf *om, void *arg)
      */
     ble_hs_flow_fill_acl_usrhdr(om);
 
-    rc = os_mqueue_put(&ble_hs_rx_q, ble_hs_evq, om);
+    rc = ble_mqueue_put(&ble_hs_rx_q, ble_hs_evq, om);
     if (rc != 0) {
         os_mbuf_free_chain(om);
         return BLE_HS_EOS;
@@ -710,7 +710,7 @@ ble_hs_init(void)
     rc = ble_gatts_init();
     SYSINIT_PANIC_ASSERT(rc == 0);
 
-    os_mqueue_init(&ble_hs_rx_q, ble_hs_event_rx_data, NULL);
+    ble_mqueue_init(&ble_hs_rx_q, ble_hs_event_rx_data, NULL);
 
     rc = stats_init_and_reg(
         STATS_HDR(ble_hs_stats), STATS_SIZE_INIT_PARMS(ble_hs_stats,

nimble/host/src/ble_hs_mqueue.c

@@ -0,0 +1,82 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "ble_hs_priv.h"
+
+int
+ble_mqueue_init(struct ble_mqueue *mq, ble_npl_event_fn *ev_fn, void *ev_arg)
+{
+    STAILQ_INIT(&mq->head);
+
+    ble_npl_event_init(&mq->ev, ev_fn, ev_arg);
+
+    return (0);
+}
+
+struct os_mbuf *
+ble_mqueue_get(struct ble_mqueue *mq)
+{
+    struct os_mbuf_pkthdr *mp;
+    struct os_mbuf *om;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    mp = STAILQ_FIRST(&mq->head);
+    if (mp) {
+        STAILQ_REMOVE_HEAD(&mq->head, omp_next);
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    if (mp) {
+        om = OS_MBUF_PKTHDR_TO_MBUF(mp);
+    } else {
+        om = NULL;
+    }
+
+    return (om);
+}
+
+int
+ble_mqueue_put(struct ble_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *om)
+{
+    struct os_mbuf_pkthdr *mp;
+    os_sr_t sr;
+    int rc;
+
+    /* Can only place the head of a chained mbuf on the queue. */
+    if (!OS_MBUF_IS_PKTHDR(om)) {
+        rc = OS_EINVAL;
+        goto err;
+    }
+
+    mp = OS_MBUF_PKTHDR(om);
+
+    OS_ENTER_CRITICAL(sr);
+    STAILQ_INSERT_TAIL(&mq->head, mp, omp_next);
+    OS_EXIT_CRITICAL(sr);
+
+    /* Only post an event to the queue if its specified */
+    if (evq) {
+        ble_npl_eventq_put(evq, &mq->ev);
+    }
+
+    return (0);
+err:
+    return (rc);
+}

nimble/host/src/ble_hs_priv.h

@@ -126,6 +126,15 @@ void ble_hs_timer_resched(void);
 void ble_hs_notifications_sched(void);
 struct os_eventq *ble_hs_evq_get(void);
 
+struct ble_mqueue {
+    STAILQ_HEAD(, os_mbuf_pkthdr) head;
+    struct ble_npl_event ev;
+};
+
+int ble_mqueue_init(struct ble_mqueue *mq, ble_npl_event_fn *ev_fn, void *ev_arg);
+struct os_mbuf *ble_mqueue_get(struct ble_mqueue *mq);
+int ble_mqueue_put(struct ble_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *om);
+
 #if MYNEWT_VAL(LOG_LEVEL) <= LOG_LEVEL_DEBUG && !BLE_MONITOR
 
 #define BLE_HS_LOG_CMD(is_tx, cmd_type, cmd_name, conn_handle,                \