Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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  // this code prevents compiler warning about a missing initializer of mpl
80  MsgPtrLen m;
81  m.len = msgLen;
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
91  return MsgQueue_INVALID_PTR;
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 
107 fail:
108 
109  tlsf_free_r(que->heap, msg);
110 
111  return MsgQueue_MAILBOX_FULL;
112 }
113 
114 int32_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) {
120  return MsgQueue_MAILBOX_FULL;
121  }
122 
123  memcpy(dst, msg, msgLen);
124  return msgqueue_send_timeout(que, dst, msgLen, urgency, timout);
125 }
126 
127 int32_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 
134 int32_t msgqueue_pop(MsgQueue *que, void **msgPtr)
135 {
136  return msgqueue_pop_timeout(que, msgPtr, TIME_INFINITE);
137 }
138 
139 int32_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 
159 static 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 
168 const 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 
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
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
const char * msgqueue_strerror(const MsgQueueStatus _errno)
debug api
Definition: msg_queue.c:168
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
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
uint8_t status
Dynamic memory allocation based on TLSF library.
void * tlsf_get_heap_addr(const tlsf_memory_heap_t *heap)
void tlsf_free_r(tlsf_memory_heap_t *heap, void *ptr)
void * tlsf_malloc_r(tlsf_memory_heap_t *heap, size_t bytes)
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78