Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
msg_queue.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2016 Gautier Hattenberger, Alexandre Bustico
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21
22/*
23 * @file modules/loggers/sdlog_chibios/msg_queue.c
24 *
25 */
28#include <string.h>
29
30#define MSGQ_HEAP HEAP_CCM
31
32
33typedef union {
34 struct {
37 };
39} MsgPtrLen;
40
41
43 msg_t *mb_buf, const cnt_t mb_size)
44{
45 chMBObjectInit(&que->mb, mb_buf, mb_size);
46 memset(mb_buf, 0, mb_size * sizeof(msg_t));
47 que->heap = heap;
48}
49
51{
52 chSysLock();
53 const bool queue_full = chMBGetFreeCountI(&que->mb) <= 0;
54 chSysUnlock();
55
56 return queue_full;
57}
58
60{
61 chSysLock();
62 bool queue_empty = (chMBGetUsedCountI(&que->mb) <= 0);
63 chSysUnlock();
64
65 return queue_empty;
66}
67
68
69int32_t msgqueue_send(MsgQueue *que, void *msg, const uint16_t msgLen,
70 const MsgQueueUrgency urgency)
71{
72 return msgqueue_send_timeout(que, msg, msgLen, urgency, TIME_IMMEDIATE);
73}
74
75
76int32_t msgqueue_send_timeout(MsgQueue *que, void *msg, const uint16_t msgLen,
77 const MsgQueueUrgency urgency, const systime_t timout)
78{
79 // this code prevents compiler warning about a missing initializer of mpl
80 MsgPtrLen m;
81 m.len = msgLen;
82 m.ptrOfst = (uint32_t) msg - (uint32_t) tlsf_get_heap_addr(&MSGQ_HEAP);
83 const MsgPtrLen mpl = m;
84
85#if CH_DBG_ENABLE_CHECKS
87 ((uint32_t) msg >= ((uint32_t) tlsf_get_heap_addr(&MSGQ_HEAP) + 65535))) {
88#if CH_DBG_ENABLE_ASSERTS
89 chSysHalt("MsgQueue_INVALID_PTR");
90#else
92#endif
93 }
94#endif
95
96 if (urgency == MsgQueue_REGULAR) {
97 if (chMBPostTimeout(&que->mb, mpl.msg_ptr_len, timout) != MSG_OK) {
98 goto fail;
99 }
100 } else {
101 if (chMBPostAheadTimeout(&que->mb, mpl.msg_ptr_len, timout) != MSG_OK) {
102 goto fail;
103 }
104 }
105 return msgLen;
106
107fail:
108
109 tlsf_free_r(que->heap, msg);
110
112}
113
114int32_t msgqueue_copy_send_timeout(MsgQueue *que, const void *msg, const uint16_t msgLen,
115 const MsgQueueUrgency urgency, const systime_t timout)
116{
117 void *dst = tlsf_malloc_r(&MSGQ_HEAP, msgLen);
118
119 if (dst == NULL) {
121 }
122
123 memcpy(dst, msg, msgLen);
124 return msgqueue_send_timeout(que, dst, msgLen, urgency, timout);
125}
126
127int32_t msgqueue_copy_send(MsgQueue *que, const void *msg, const uint16_t msgLen,
128 const MsgQueueUrgency urgency)
129{
130 return msgqueue_copy_send_timeout(que, msg, msgLen, urgency, TIME_IMMEDIATE);
131}
132
133
134int32_t msgqueue_pop(MsgQueue *que, void **msgPtr)
135{
136 return msgqueue_pop_timeout(que, msgPtr, TIME_INFINITE);
137}
138
139int32_t msgqueue_pop_timeout(MsgQueue *que, void **msgPtr, const systime_t timout)
140{
141 MsgPtrLen mpl = {.ptrOfst = 0, .len = 0};
142
143 msg_t status;
144 do {
145 status = chMBFetchTimeout(&que->mb, (msg_t *) &mpl.msg_ptr_len, timout);
146 } while (status == MSG_RESET);
147
148 if (status != MSG_OK) {
150 }
151
152 *msgPtr = (void *)(mpl.ptrOfst + (uint32_t) tlsf_get_heap_addr(&MSGQ_HEAP));
153 return mpl.len;
154}
155
156
157
158
159static const char *_strerror[] = {"MsgQueue_MAILBOX_FULL",
160 "MsgQueue_MAILBOX_FAIL",
161 "MsgQueue_MAILBOX_TIMEOUT",
162 "MsgQueue_MAILBOX_NOT_EMPTY",
163 "MsgQueue_OK",
164 "MsgQueue_INVALID_PTR",
165 "MsgQueue_INVALID_ERRNO"
166 };
167
168const char *msgqueue_strerror(const MsgQueueStatus _errno)
169{
170 size_t indice = _errno - MsgQueue_MAILBOX_FULL;
171 const size_t arrayLen = sizeof(_strerror) / sizeof(_strerror[0]);
172 indice = (indice < arrayLen) ? indice : arrayLen - 1;
173 return _strerror[indice];
174}
175
176
177
178
static uint8_t status
bool msgqueue_is_empty(MsgQueue *que)
test if queue is empty
Definition msg_queue.c:59
int32_t msgqueue_pop_timeout(MsgQueue *que, void **msgPtr, const systime_t timout)
receive message specifying timeout
Definition msg_queue.c:139
int32_t msgqueue_send(MsgQueue *que, void *msg, const uint16_t msgLen, const MsgQueueUrgency urgency)
send a buffer previously allocated by msgqueue_malloc_before_send
Definition msg_queue.c:69
int32_t msgqueue_pop(MsgQueue *que, void **msgPtr)
wait then receive message
Definition msg_queue.c:134
msg_t msg_ptr_len
Definition msg_queue.c:38
bool msgqueue_is_full(MsgQueue *que)
test if queue is full
Definition msg_queue.c:50
void msgqueue_init(MsgQueue *que, tlsf_memory_heap_t *heap, msg_t *mb_buf, const cnt_t mb_size)
initialise MsgQueue
Definition msg_queue.c:42
int32_t msgqueue_send_timeout(MsgQueue *que, void *msg, const uint16_t msgLen, const MsgQueueUrgency urgency, const systime_t timout)
send a buffer previously allocated by msgqueue_malloc_before_send
Definition msg_queue.c:76
int32_t msgqueue_copy_send_timeout(MsgQueue *que, const void *msg, const uint16_t msgLen, const MsgQueueUrgency urgency, const systime_t timout)
send a buffer NOT previously allocated
Definition msg_queue.c:114
static const char * _strerror[]
Definition msg_queue.c:159
const char * msgqueue_strerror(const MsgQueueStatus _errno)
debug api
Definition msg_queue.c:168
int32_t msgqueue_copy_send(MsgQueue *que, const void *msg, const uint16_t msgLen, const MsgQueueUrgency urgency)
send a buffer NOT previously allocated
Definition msg_queue.c:127
#define MSGQ_HEAP
Definition msg_queue.c:30
MsgQueueUrgency
Definition msg_queue.h:47
@ MsgQueue_REGULAR
Definition msg_queue.h:47
tlsf_memory_heap_t * heap
Definition msg_queue.h:227
mailbox_t mb
Definition msg_queue.h:226
MsgQueueStatus
Definition msg_queue.h:38
@ MsgQueue_MAILBOX_FULL
Definition msg_queue.h:39
@ MsgQueue_MAILBOX_TIMEOUT
Definition msg_queue.h:41
@ MsgQueue_INVALID_PTR
Definition msg_queue.h:43
Dynamic memory allocation based on TLSF library.
struct _tlsf_memory_heap_t tlsf_memory_heap_t
Definition tlsf_malloc.h:40
void tlsf_free_r(tlsf_memory_heap_t *heap, void *ptr)
void * tlsf_get_heap_addr(const tlsf_memory_heap_t *heap)
void * tlsf_malloc_r(tlsf_memory_heap_t *heap, size_t bytes)
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.