Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
33 typedef union {
34  struct {
35  uint16_t ptrOfst;
36  uint16_t len;
37  };
38  msg_t msg_ptr_len;
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 
69 int32_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 
77  const MsgQueueUrgency urgency, const systime_t timout)
78 {
79  const MsgPtrLen mpl = {.ptrOfst = (uint32_t) msg - (uint32_t) tlsf_get_heap_addr(&MSGQ_HEAP),
80  .len = msgLen
81  };
82 
83 #if CH_DBG_ENABLE_CHECKS
84  if (((uint32_t) msg < (uint32_t) tlsf_get_heap_addr(&MSGQ_HEAP)) ||
85  ((uint32_t) msg >= ((uint32_t) tlsf_get_heap_addr(&MSGQ_HEAP) + 65535))) {
86 #if CH_DBG_ENABLE_ASSERTS
87  chSysHalt("MsgQueue_INVALID_PTR");
88 #else
89  return MsgQueue_INVALID_PTR;
90 #endif
91  }
92 #endif
93 
94  if (urgency == MsgQueue_REGULAR) {
95  if (chMBPost(&que->mb, mpl.msg_ptr_len, timout) != MSG_OK) {
96  goto fail;
97  }
98  } else {
99  if (chMBPostAhead(&que->mb, mpl.msg_ptr_len, timout) != MSG_OK) {
100  goto fail;
101  }
102  }
103  return msgLen;
104 
105 fail:
106 
107  tlsf_free_r(que->heap, msg);
108 
109  return MsgQueue_MAILBOX_FULL;
110 }
111 
112 int32_t msgqueue_copy_send_timeout(MsgQueue *que, const void *msg, const uint16_t msgLen,
113  const MsgQueueUrgency urgency, const systime_t timout)
114 {
115  void *dst = tlsf_malloc_r(&MSGQ_HEAP, msgLen);
116 
117  if (dst == NULL) {
118  return MsgQueue_MAILBOX_FULL;
119  }
120 
121  memcpy(dst, msg, msgLen);
122  return msgqueue_send_timeout(que, dst, msgLen, urgency, timout);
123 }
124 
125 int32_t msgqueue_copy_send(MsgQueue *que, const void *msg, const uint16_t msgLen,
126  const MsgQueueUrgency urgency)
127 {
128  return msgqueue_copy_send_timeout(que, msg, msgLen, urgency, TIME_IMMEDIATE);
129 }
130 
131 
132 int32_t msgqueue_pop(MsgQueue *que, void **msgPtr)
133 {
134  return msgqueue_pop_timeout(que, msgPtr, TIME_INFINITE);
135 }
136 
137 int32_t msgqueue_pop_timeout(MsgQueue *que, void **msgPtr, const systime_t timout)
138 {
139  MsgPtrLen mpl = {.ptrOfst = 0, .len = 0};
140 
141  const msg_t status = chMBFetch(&que->mb, (msg_t *) &mpl.msg_ptr_len, timout);
142  if (status != MSG_OK) {
144  }
145 
146  *msgPtr = (void *)(mpl.ptrOfst + (uint32_t) tlsf_get_heap_addr(&MSGQ_HEAP));
147  return mpl.len;
148 }
149 
150 
151 
152 
153 static const char *_strerror[] = {"MsgQueue_MAILBOX_FULL",
154  "MsgQueue_MAILBOX_FAIL",
155  "MsgQueue_MAILBOX_TIMEOUT",
156  "MsgQueue_MAILBOX_NOT_EMPTY",
157  "MsgQueue_OK",
158  "MsgQueue_INVALID_PTR",
159  "MsgQueue_INVALID_ERRNO"
160  };
161 
162 const char *msgqueue_strerror(const MsgQueueStatus _errno)
163 {
164  size_t indice = _errno - MsgQueue_MAILBOX_FULL;
165  const size_t arrayLen = sizeof(_strerror) / sizeof(_strerror[0]);
166  indice = (indice < arrayLen) ? indice : arrayLen - 1;
167  return _strerror[indice];
168 }
169 
170 
171 
172 
unsigned short uint16_t
Definition: types.h:16
int32_t msgqueue_pop_timeout(MsgQueue *que, void **msgPtr, const systime_t timout)
receive message specifying timeout
Definition: msg_queue.c:137
status
Definition: anemotaxis.c:10
MsgQueueStatus
Definition: msg_queue.h:38
msg_t msg_ptr_len
Definition: msg_queue.c:38
const char * msgqueue_strerror(const MsgQueueStatus _errno)
debug api
Definition: msg_queue.c:162
void * tlsf_get_heap_addr(const tlsf_memory_heap_t *heap)
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_copy_send(MsgQueue *que, const void *msg, const uint16_t msgLen, const MsgQueueUrgency urgency)
send a buffer NOT previously allocated
Definition: msg_queue.c:125
void tlsf_free_r(tlsf_memory_heap_t *heap, void *ptr)
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:112
Dynamic memory allocation based on TLSF library.
bool msgqueue_is_full(MsgQueue *que)
test if queue is full
Definition: msg_queue.c:50
tlsf_memory_heap_t * heap
Definition: msg_queue.h:227
unsigned long uint32_t
Definition: types.h:18
static const char * _strerror[]
Definition: msg_queue.c:153
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
signed long int32_t
Definition: types.h:19
bool msgqueue_is_empty(MsgQueue *que)
test if queue is empty
Definition: msg_queue.c:59
int32_t msgqueue_pop(MsgQueue *que, void **msgPtr)
wait then receive message
Definition: msg_queue.c:132
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
#define MSGQ_HEAP
Definition: msg_queue.c:30
mailbox_t mb
Definition: msg_queue.h:226
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
MsgQueueUrgency
Definition: msg_queue.h:47
void * tlsf_malloc_r(tlsf_memory_heap_t *heap, size_t bytes)