1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one
3 * or more contributor license agreements. See the NOTICE file
4 * distributed with this work for additional information
5 * regarding copyright ownership. The ASF licenses this file
6 * to you under the Apache License, Version 2.0 (the
7 * "License"); you may not use this file except in compliance
8 * with the License. You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing,
13 * software distributed under the License is distributed on an
14 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
15 * KIND, either express or implied. See the License for the
16 * specific language governing permissions and limitations
17 * under the License.
18 */
19
20 #include "ble_hs_priv.h"
21
22 int
ble_mqueue_init(struct ble_mqueue * mq,ble_npl_event_fn * ev_fn,void * ev_arg)23 ble_mqueue_init(struct ble_mqueue *mq, ble_npl_event_fn *ev_fn, void *ev_arg)
24 {
25 STAILQ_INIT(&mq->head);
26
27 ble_npl_event_init(&mq->ev, ev_fn, ev_arg);
28
29 return (0);
30 }
31
32 struct os_mbuf *
ble_mqueue_get(struct ble_mqueue * mq)33 ble_mqueue_get(struct ble_mqueue *mq)
34 {
35 struct os_mbuf_pkthdr *mp;
36 struct os_mbuf *om;
37 os_sr_t sr;
38
39 OS_ENTER_CRITICAL(sr);
40 mp = STAILQ_FIRST(&mq->head);
41 if (mp) {
42 STAILQ_REMOVE_HEAD(&mq->head, omp_next);
43 }
44 OS_EXIT_CRITICAL(sr);
45
46 if (mp) {
47 om = OS_MBUF_PKTHDR_TO_MBUF(mp);
48 } else {
49 om = NULL;
50 }
51
52 return (om);
53 }
54
55 int
ble_mqueue_put(struct ble_mqueue * mq,struct ble_npl_eventq * evq,struct os_mbuf * om)56 ble_mqueue_put(struct ble_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *om)
57 {
58 struct os_mbuf_pkthdr *mp;
59 os_sr_t sr;
60 int rc;
61
62 /* Can only place the head of a chained mbuf on the queue. */
63 if (!OS_MBUF_IS_PKTHDR(om)) {
64 rc = OS_EINVAL;
65 goto err;
66 }
67
68 mp = OS_MBUF_PKTHDR(om);
69
70 OS_ENTER_CRITICAL(sr);
71 STAILQ_INSERT_TAIL(&mq->head, mp, omp_next);
72 OS_EXIT_CRITICAL(sr);
73
74 /* Only post an event to the queue if its specified */
75 if (evq) {
76 ble_npl_eventq_put(evq, &mq->ev);
77 }
78
79 return (0);
80 err:
81 return (rc);
82 }
83