diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c index 2d7c82f89..e936cdc84 100644 --- a/nimble/host/src/ble_hs.c +++ b/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, diff --git a/nimble/host/src/ble_hs_mqueue.c b/nimble/host/src/ble_hs_mqueue.c new file mode 100644 index 000000000..2e08efc14 --- /dev/null +++ b/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); +} diff --git a/nimble/host/src/ble_hs_priv.h b/nimble/host/src/ble_hs_priv.h index ca74f6cdd..58d2e6c02 100644 --- a/nimble/host/src/ble_hs_priv.h +++ b/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, \