1 /*
2 * Copyright 2017, NXP
3 * All rights reserved.
4 *
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9 #include <string.h>
10
11 #include <srtm_heap.h>
12
13 #include "srtm_dispatcher.h"
14 #include "srtm_dispatcher_struct.h"
15 #include "srtm_peercore.h"
16 #include "srtm_peercore_struct.h"
17 #include "srtm_service.h"
18 #include "srtm_service_struct.h"
19 #include "srtm_channel.h"
20 #include "srtm_channel_struct.h"
21 #include "srtm_message.h"
22 #include "srtm_message_struct.h"
23 #include "fsl_common.h"
24
25 /*******************************************************************************
26 * Definitions
27 ******************************************************************************/
28
29 /*******************************************************************************
30 * Prototypes
31 ******************************************************************************/
32
33 /*******************************************************************************
34 * Variables
35 ******************************************************************************/
36
37 /*******************************************************************************
38 * Code
39 ******************************************************************************/
SRTM_DumpMessage(srtm_message_t msg)40 static void SRTM_DumpMessage(srtm_message_t msg)
41 {
42 #ifdef SRTM_DEBUG_MESSAGE_FUNC
43 srtm_packet_head_t *head;
44
45 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, " -- Msg type %d, dir %d, dlen %d, chan 0x%08x, err %d, prio %d\r\n",
46 msg->type, msg->direct, msg->dataLen, msg->channel, msg->error, msg->priority);
47 if (msg->dataLen >= sizeof(struct _srtm_packet_head))
48 {
49 head = (srtm_packet_head_t *)msg->data;
50 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, " -- Data cat %d, majv %d, minv %d, type %d, cmd %d, prio %d\r\n",
51 head->category, head->majorVersion, head->minorVersion, head->type, head->command,
52 head->priority);
53 }
54 else if (msg->type == SRTM_MessageTypeProcedure)
55 {
56 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, " -- Callback 0x%08x, param 0x%x, 0x%x\r\n", msg->procMsg.cb,
57 msg->procMsg.param1, msg->procMsg.param2);
58 }
59 else
60 {
61 /* Do nothing */
62 }
63 #endif
64 }
65
SRTM_Dispatcher_InsertOrderedMessage(srtm_dispatcher_t disp,srtm_message_t msg)66 static void SRTM_Dispatcher_InsertOrderedMessage(srtm_dispatcher_t disp, srtm_message_t msg)
67 {
68 srtm_list_t *list;
69 srtm_message_t message;
70
71 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
72
73 SRTM_DumpMessage(msg);
74 /* Insert message with priority order */
75 for (list = disp->messageQ.prev; list != &disp->messageQ; list = list->prev)
76 {
77 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
78 if (message->priority >= msg->priority)
79 {
80 break;
81 }
82 }
83 SRTM_List_InsertAfter(list, &msg->node);
84 }
85
86 /* Send message to messageQ from ISR or task context */
SRTM_Dispatcher_QueueMessage(srtm_dispatcher_t disp,srtm_message_t msg)87 static void SRTM_Dispatcher_QueueMessage(srtm_dispatcher_t disp, srtm_message_t msg)
88 {
89 uint32_t primask;
90
91 assert(SRTM_List_IsEmpty(&msg->node));
92
93 primask = DisableGlobalIRQ();
94 SRTM_Dispatcher_InsertOrderedMessage(disp, msg);
95 EnableGlobalIRQ(primask);
96
97 (void)SRTM_Sem_Post(disp->queueSig);
98 }
99
100 /* Dequeue message might detach message from messageQ, peer core's pendingQ or waitingReqs */
SRTM_Dispatcher_DequeueMessage(srtm_dispatcher_t disp,srtm_message_t msg)101 static srtm_status_t SRTM_Dispatcher_DequeueMessage(srtm_dispatcher_t disp, srtm_message_t msg)
102 {
103 uint32_t primask;
104 srtm_status_t status = SRTM_Status_ListRemoveFailed;
105
106 (void)SRTM_Mutex_Lock(disp->mutex); /* Protect waitingReqs */
107 primask = DisableGlobalIRQ(); /* Protect messageQ */
108 if (!SRTM_List_IsEmpty(&msg->node))
109 {
110 SRTM_List_Remove(&msg->node);
111 status = SRTM_Status_Success;
112 }
113 EnableGlobalIRQ(primask);
114 (void)SRTM_Mutex_Unlock(disp->mutex);
115
116 return status;
117 }
118
SRTM_Dispatcher_RecvMessage(srtm_dispatcher_t disp)119 static srtm_message_t SRTM_Dispatcher_RecvMessage(srtm_dispatcher_t disp)
120 {
121 uint32_t primask;
122 srtm_list_t *list;
123 srtm_message_t message = NULL;
124
125 primask = DisableGlobalIRQ();
126 if (!SRTM_List_IsEmpty(&disp->messageQ))
127 {
128 list = disp->messageQ.next;
129 SRTM_List_Remove(list);
130 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
131 }
132 EnableGlobalIRQ(primask);
133
134 return message;
135 }
136
SRTM_Dispatcher_FreeRequest(srtm_dispatcher_t disp,srtm_request_t req,srtm_status_t error)137 static void SRTM_Dispatcher_FreeRequest(srtm_dispatcher_t disp, srtm_request_t req, srtm_status_t error)
138 {
139 req->error = error;
140 if (req->reqMsg.isSyncReq)
141 {
142 /* Synchronous request will be freed by user. Here just wakeup user's task. */
143 (void)SRTM_Sem_Post(req->reqMsg.sync.sig);
144 }
145 else
146 {
147 if (req->reqMsg.async.cb != NULL)
148 {
149 req->reqMsg.async.cb(disp, req, NULL, req->reqMsg.async.param);
150 }
151 SRTM_Request_Destroy(req);
152 }
153 }
154
SRTM_Dispatcher_FreeMessage(srtm_dispatcher_t disp,srtm_message_t msg)155 static void SRTM_Dispatcher_FreeMessage(srtm_dispatcher_t disp, srtm_message_t msg)
156 {
157 switch (msg->type)
158 {
159 case SRTM_MessageTypeProcedure:
160 SRTM_Procedure_Destroy(msg);
161 break;
162 case SRTM_MessageTypeRawData:
163 SRTM_RawData_Destroy(msg);
164 break;
165 case SRTM_MessageTypeRequest:
166 if (msg->direct == SRTM_MessageDirectRx)
167 {
168 /* Rx request will be freed after handling */
169 SRTM_Request_Destroy(msg);
170 }
171 else
172 {
173 /* Tx request finalization need to inform user and then free. */
174 SRTM_Dispatcher_FreeRequest(disp, msg, SRTM_Status_TransferNotAvail);
175 }
176 break;
177 case SRTM_MessageTypeResponse:
178 SRTM_Response_Destroy(msg);
179 break;
180 case SRTM_MessageTypeNotification:
181 SRTM_Notification_Destroy(msg);
182 break;
183 default:
184 SRTM_Message_Destroy(msg);
185 break;
186 }
187 }
188
SRTM_Dispatcher_RecycleMessage(srtm_message_t msg,void * param)189 static void SRTM_Dispatcher_RecycleMessage(srtm_message_t msg, void *param)
190 {
191 uint32_t primask;
192 srtm_dispatcher_t disp = (srtm_dispatcher_t)param;
193
194 /* Put RX message back to freeRxMsgs */
195 primask = DisableGlobalIRQ();
196 SRTM_List_AddTail(&disp->freeRxMsgs, &msg->node);
197 EnableGlobalIRQ(primask);
198 }
199
SRTM_Dispatcher_Create(void)200 srtm_dispatcher_t SRTM_Dispatcher_Create(void)
201 {
202 srtm_dispatcher_t disp = (srtm_dispatcher_t)SRTM_Heap_Malloc(sizeof(struct _srtm_dispatcher));
203 #if defined(SRTM_STATIC_API) && SRTM_STATIC_API
204 srtm_mutex_t mutex = SRTM_Mutex_Create(&disp->mutexStatic);
205 srtm_sem_t startSig = SRTM_Sem_Create(1U, 0U, &disp->startSigStatic);
206 srtm_sem_t stopSig = SRTM_Sem_Create(1U, 0U, &disp->stopSigStatic);
207 /* Assume same maximum message number of local and remote in messageQ */
208 srtm_sem_t queueSig = SRTM_Sem_Create(SRTM_DISPATCHER_CONFIG_RX_MSG_NUMBER * 2, 0U, &disp->queueSigStatic);
209 #else
210 srtm_mutex_t mutex = SRTM_Mutex_Create();
211 srtm_sem_t startSig = SRTM_Sem_Create(1U, 0U);
212 srtm_sem_t stopSig = SRTM_Sem_Create(1U, 0U);
213 /* Assume same maximum message number of local and remote in messageQ */
214 srtm_sem_t queueSig = SRTM_Sem_Create(SRTM_DISPATCHER_CONFIG_RX_MSG_NUMBER * 2U, 0U);
215 #endif
216 srtm_message_t msg;
217 uint32_t i;
218
219 assert((disp != NULL) && (mutex != NULL) && (startSig != NULL) && (stopSig != NULL) && (queueSig != NULL));
220
221 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s\r\n", __func__);
222
223 SRTM_List_Init(&disp->cores);
224 SRTM_List_Init(&disp->services);
225 SRTM_List_Init(&disp->freeRxMsgs);
226 SRTM_List_Init(&disp->messageQ);
227 SRTM_List_Init(&disp->waitingReqs);
228 disp->mutex = mutex;
229 disp->stopReq = false;
230 disp->started = false;
231 disp->startSig = startSig;
232 disp->stopSig = stopSig;
233 disp->queueSig = queueSig;
234
235 for (i = 0; i < SRTM_DISPATCHER_CONFIG_RX_MSG_NUMBER; i++)
236 {
237 msg = SRTM_Message_Create(SRTM_DISPATCHER_CONFIG_RX_MSG_MAX_LEN);
238 assert(msg);
239 SRTM_Message_SetFreeFunc(msg, SRTM_Dispatcher_RecycleMessage, disp);
240 SRTM_List_AddTail(&disp->freeRxMsgs, &msg->node);
241 }
242
243 return disp;
244 }
245
SRTM_Dispatcher_Destroy(srtm_dispatcher_t disp)246 void SRTM_Dispatcher_Destroy(srtm_dispatcher_t disp)
247 {
248 srtm_list_t *list;
249 srtm_peercore_t core;
250 srtm_service_t service;
251 srtm_message_t msg;
252
253 assert(disp);
254 assert(!disp->started);
255
256 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s\r\n", __func__);
257
258 /* Before destroy, all the messages should be well handled */
259 assert(SRTM_List_IsEmpty(&disp->messageQ));
260 /* Before destroy, all the waiting request should responded */
261 assert(SRTM_List_IsEmpty(&disp->waitingReqs));
262
263 while (!SRTM_List_IsEmpty(&disp->cores))
264 {
265 list = disp->cores.next;
266 SRTM_List_Remove(list);
267 core = SRTM_LIST_OBJ(srtm_peercore_t, node, list);
268 SRTM_PeerCore_Destroy(core);
269 }
270
271 while (!SRTM_List_IsEmpty(&disp->services))
272 {
273 list = disp->services.next;
274 SRTM_List_Remove(list);
275 service = SRTM_LIST_OBJ(srtm_service_t, node, list);
276 SRTM_Service_Destroy(service);
277 }
278
279 while (!SRTM_List_IsEmpty(&disp->freeRxMsgs))
280 {
281 list = disp->freeRxMsgs.next;
282 SRTM_List_Remove(list);
283 msg = SRTM_LIST_OBJ(srtm_message_t, node, list);
284 SRTM_Message_SetFreeFunc(msg, NULL, NULL);
285 SRTM_Message_Destroy(msg);
286 }
287
288 SRTM_Mutex_Destroy(disp->mutex);
289 SRTM_Sem_Destroy(disp->startSig);
290 SRTM_Sem_Destroy(disp->stopSig);
291 SRTM_Sem_Destroy(disp->queueSig);
292 SRTM_Heap_Free(disp);
293 }
294
SRTM_Dispatcher_Start(srtm_dispatcher_t disp)295 srtm_status_t SRTM_Dispatcher_Start(srtm_dispatcher_t disp)
296 {
297 assert(disp);
298
299 if (disp->started)
300 {
301 return SRTM_Status_InvalidState;
302 }
303
304 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s\r\n", __func__);
305
306 disp->stopReq = false;
307 disp->started = true;
308 (void)SRTM_Sem_Post(disp->startSig);
309
310 return SRTM_Status_Success;
311 }
312
SRTM_Dispatcher_Stop(srtm_dispatcher_t disp)313 srtm_status_t SRTM_Dispatcher_Stop(srtm_dispatcher_t disp)
314 {
315 if (!disp->started)
316 {
317 return SRTM_Status_InvalidState;
318 }
319
320 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s\r\n", __func__);
321
322 disp->stopReq = true;
323 /* Wakeup dispatcher to do stop operations */
324 (void)SRTM_Sem_Post(disp->queueSig);
325 /* Wait for dispatcher stopped */
326 (void)SRTM_Sem_Wait(disp->stopSig, SRTM_WAIT_FOR_EVER);
327
328 disp->started = false;
329
330 return SRTM_Status_Success;
331 }
332
SRTM_Dispatcher_Run(srtm_dispatcher_t disp)333 void SRTM_Dispatcher_Run(srtm_dispatcher_t disp)
334 {
335 srtm_list_t *list;
336 srtm_peercore_t core;
337 srtm_message_t message;
338
339 assert(disp);
340
341 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s\r\n", __func__);
342
343 while (true)
344 {
345 /* Wait for start */
346 (void)SRTM_Sem_Wait(disp->startSig, SRTM_WAIT_FOR_EVER);
347
348 /* Start peer cores */
349 for (list = disp->cores.next; list != &disp->cores; list = list->next)
350 {
351 core = SRTM_LIST_OBJ(srtm_peercore_t, node, list);
352 (void)SRTM_PeerCore_Start(core);
353 }
354
355 while (!disp->stopReq)
356 {
357 /* Wait for message putting into Q */
358 (void)SRTM_Sem_Wait(disp->queueSig, SRTM_WAIT_FOR_EVER);
359 /* Handle as many messages as possible */
360 while (!disp->stopReq)
361 {
362 message = SRTM_Dispatcher_RecvMessage(disp);
363 if (message != NULL)
364 {
365 (void)SRTM_Dispatcher_ProcessMessage(disp, message);
366 }
367 else
368 {
369 break;
370 }
371 }
372 }
373
374 /* Stop peer cores */
375 for (list = disp->cores.next; list != &disp->cores; list = list->next)
376 {
377 core = SRTM_LIST_OBJ(srtm_peercore_t, node, list);
378 (void)SRTM_PeerCore_Stop(core);
379 }
380
381 /* Signal dispatcher stopped */
382 (void)SRTM_Sem_Post(disp->stopSig);
383 }
384 }
385
SRTM_Dispatcher_AddPeerCore(srtm_dispatcher_t disp,srtm_peercore_t core)386 srtm_status_t SRTM_Dispatcher_AddPeerCore(srtm_dispatcher_t disp, srtm_peercore_t core)
387 {
388 assert(disp);
389 assert(core);
390 assert(!disp->started); /* Add core when SRTM dispatcher running is forbidden */
391
392 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s: 0x%x\r\n", __func__, core);
393
394 if (!SRTM_List_IsEmpty(&core->node))
395 {
396 return SRTM_Status_ListAddFailed;
397 }
398
399 (void)SRTM_Mutex_Lock(disp->mutex);
400 SRTM_List_AddTail(&disp->cores, &core->node);
401 (void)SRTM_Mutex_Unlock(disp->mutex);
402 core->dispatcher = disp;
403
404 return SRTM_Status_Success;
405 }
406
SRTM_Dispatcher_RemovePeerCore(srtm_dispatcher_t disp,srtm_peercore_t core)407 srtm_status_t SRTM_Dispatcher_RemovePeerCore(srtm_dispatcher_t disp, srtm_peercore_t core)
408 {
409 uint32_t primask;
410 srtm_list_t listHead;
411 srtm_list_t *list, *next;
412 srtm_message_t message;
413
414 assert(disp);
415 assert(core);
416 assert(!disp->started); /* Remove core when SRTM dispatcher running is forbidden */
417
418 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s: 0x%x\r\n", __func__, core);
419
420 if (SRTM_List_IsEmpty(&core->node))
421 {
422 return SRTM_Status_ListRemoveFailed;
423 }
424
425 (void)SRTM_Mutex_Lock(disp->mutex);
426 SRTM_List_Remove(&core->node);
427 (void)SRTM_Mutex_Unlock(disp->mutex);
428 core->dispatcher = NULL;
429
430 SRTM_List_Init(&listHead);
431
432 /* Clean up all corresponding messages for the peer core */
433 /* First clean up messages in messageQ */
434 primask = DisableGlobalIRQ();
435 for (list = disp->messageQ.next; list != &disp->messageQ; list = next)
436 {
437 next = list->next;
438 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
439 if ((message->channel != NULL) && (message->channel->core == core))
440 {
441 SRTM_List_Remove(list);
442 /* Add to temp list */
443 SRTM_List_AddTail(&listHead, list);
444 }
445 }
446 EnableGlobalIRQ(primask);
447
448 /* Next clean up messages in waitingReqs */
449 (void)SRTM_Mutex_Lock(disp->mutex);
450 for (list = disp->waitingReqs.next; list != &disp->waitingReqs; list = next)
451 {
452 next = list->next;
453 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
454 if ((message->channel != NULL) && (message->channel->core == core))
455 {
456 SRTM_List_Remove(list);
457 /* Add to temp list */
458 SRTM_List_AddTail(&listHead, list);
459 }
460 }
461 (void)SRTM_Mutex_Unlock(disp->mutex);
462
463 /* Now free all the messages on temp list */
464 while (!SRTM_List_IsEmpty(&listHead))
465 {
466 list = listHead.next;
467 SRTM_List_Remove(list);
468 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
469 SRTM_Dispatcher_FreeMessage(disp, message);
470 }
471
472 /* Last clean up messages in PeerCore's pendingQ */
473 /* pendingQ is accessed only in dispatcher context, and now dispatcher is stopped.
474 No need to lock */
475 while (!SRTM_List_IsEmpty(&core->pendingQ))
476 {
477 list = core->pendingQ.next;
478 SRTM_List_Remove(list);
479 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
480 SRTM_Dispatcher_FreeMessage(disp, message);
481 }
482
483 return SRTM_Status_Success;
484 }
485
SRTM_Dispatcher_RegisterService(srtm_dispatcher_t disp,srtm_service_t service)486 srtm_status_t SRTM_Dispatcher_RegisterService(srtm_dispatcher_t disp, srtm_service_t service)
487 {
488 assert(disp);
489 assert(service);
490 assert(!disp->started); /* Register service when SRTM dispatcher running is forbidden */
491
492 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s: %d\r\n", __func__, service->category);
493
494 if (!SRTM_List_IsEmpty(&service->node))
495 {
496 return SRTM_Status_ListAddFailed;
497 }
498
499 (void)SRTM_Mutex_Lock(disp->mutex);
500 SRTM_List_AddTail(&disp->services, &service->node);
501 (void)SRTM_Mutex_Unlock(disp->mutex);
502
503 service->dispatcher = disp;
504
505 return SRTM_Status_Success;
506 }
507
SRTM_Dispatcher_UnregisterService(srtm_dispatcher_t disp,srtm_service_t service)508 srtm_status_t SRTM_Dispatcher_UnregisterService(srtm_dispatcher_t disp, srtm_service_t service)
509 {
510 assert(disp);
511 assert(service);
512 assert(!disp->started); /* Unregister service when SRTM dispatcher running is forbidden */
513
514 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_INFO, "%s: %d\r\n", __func__, service->category);
515
516 if (SRTM_List_IsEmpty(&service->node))
517 {
518 return SRTM_Status_ListRemoveFailed;
519 }
520
521 (void)SRTM_Mutex_Lock(disp->mutex);
522 SRTM_List_Remove(&service->node);
523 (void)SRTM_Mutex_Unlock(disp->mutex);
524
525 service->dispatcher = NULL;
526
527 return SRTM_Status_Success;
528 }
529
SRTM_Dispatcher_Request(srtm_dispatcher_t disp,srtm_request_t req,srtm_response_t * pResp,uint32_t timeout)530 srtm_status_t SRTM_Dispatcher_Request(srtm_dispatcher_t disp,
531 srtm_request_t req,
532 srtm_response_t *pResp,
533 uint32_t timeout)
534 {
535 srtm_sem_t signal;
536 srtm_status_t status;
537 srtm_response_t resp;
538 #if defined(SRTM_STATIC_API) && SRTM_STATIC_API
539 srtm_sem_buf_t signalStatic;
540 #endif
541
542 assert(disp);
543 assert(req);
544
545 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
546
547 #if defined(SRTM_STATIC_API) && SRTM_STATIC_API
548 signal = SRTM_Sem_Create(1U, 0U, &signalStatic);
549 #else
550 signal = SRTM_Sem_Create(1U, 0U);
551 #endif
552 if (signal == NULL)
553 {
554 return SRTM_Status_OutOfMemory;
555 }
556
557 req->reqMsg.isSyncReq = true;
558 req->reqMsg.sync.sig = signal;
559 req->reqMsg.sync.resp = NULL;
560
561 req->error = SRTM_Status_Success;
562 SRTM_Dispatcher_QueueMessage(disp, req);
563 status = SRTM_Sem_Wait(signal, timeout);
564 /* Make sure request is no longer in messageQ or waitingReqs list */
565 if (status != SRTM_Status_Success)
566 {
567 while (SRTM_Dispatcher_DequeueMessage(disp, req) != SRTM_Status_Success)
568 {
569 /* If request is not on any list, it means SRTM dispatcher is processing or just going
570 to send the response. Wait again to make sure dispatcher has put it on the
571 waitingReqs queue or properly responded */
572 if ((status = SRTM_Sem_Wait(signal, timeout)) == SRTM_Status_Success)
573 {
574 break;
575 }
576 }
577 }
578
579 /* Clean up */
580 SRTM_Sem_Destroy(signal);
581 req->reqMsg.sync.sig = NULL;
582
583 resp = req->reqMsg.sync.resp;
584 if (pResp != NULL)
585 {
586 *pResp = resp; /* Now application gets response and will destroy it later */
587 }
588
589 if (resp == NULL) /* Failed to get response */
590 {
591 status = status == SRTM_Status_Timeout ? SRTM_Status_TransferTimeout : req->error;
592 }
593 else
594 {
595 if (pResp == NULL) /* Response not needed by application */
596 {
597 SRTM_Response_Destroy(resp);
598 }
599 status = SRTM_Status_Success;
600 }
601
602 return status;
603 }
604
SRTM_Dispatcher_DeliverRequest(srtm_dispatcher_t disp,srtm_request_t req,srtm_dispatcher_resp_cb_t callback,void * param)605 srtm_status_t SRTM_Dispatcher_DeliverRequest(srtm_dispatcher_t disp,
606 srtm_request_t req,
607 srtm_dispatcher_resp_cb_t callback,
608 void *param)
609 {
610 assert(disp);
611 assert(req);
612
613 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
614
615 req->reqMsg.isSyncReq = false;
616 req->reqMsg.async.cb = callback;
617 req->reqMsg.async.param = param;
618
619 return SRTM_Dispatcher_DeliverRawData(disp, req);
620 }
621
SRTM_Dispatcher_DeliverResponse(srtm_dispatcher_t disp,srtm_response_t resp)622 srtm_status_t SRTM_Dispatcher_DeliverResponse(srtm_dispatcher_t disp, srtm_response_t resp)
623 {
624 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
625
626 return SRTM_Dispatcher_DeliverRawData(disp, resp);
627 }
628
SRTM_Dispatcher_DeliverNotification(srtm_dispatcher_t disp,srtm_notification_t notif)629 srtm_status_t SRTM_Dispatcher_DeliverNotification(srtm_dispatcher_t disp, srtm_notification_t notif)
630 {
631 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
632
633 return SRTM_Dispatcher_DeliverRawData(disp, notif);
634 }
635
SRTM_Dispatcher_DeliverRawData(srtm_dispatcher_t disp,srtm_rawdata_t data)636 srtm_status_t SRTM_Dispatcher_DeliverRawData(srtm_dispatcher_t disp, srtm_rawdata_t data)
637 {
638 assert(disp);
639 assert(data);
640
641 SRTM_Dispatcher_QueueMessage(disp, data);
642
643 return SRTM_Status_Success;
644 }
645
SRTM_Dispatcher_DeliverMessages(srtm_dispatcher_t disp,srtm_list_t * msgs)646 srtm_status_t SRTM_Dispatcher_DeliverMessages(srtm_dispatcher_t disp, srtm_list_t *msgs)
647 {
648 uint32_t primask;
649 srtm_list_t *list;
650 srtm_message_t message;
651
652 assert(disp);
653 assert(msgs);
654
655 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
656
657 primask = DisableGlobalIRQ();
658 while (!SRTM_List_IsEmpty(msgs))
659 {
660 list = msgs->next;
661 SRTM_List_Remove(list);
662 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
663 SRTM_Dispatcher_InsertOrderedMessage(disp, message);
664 }
665 EnableGlobalIRQ(primask);
666
667 (void)SRTM_Sem_Post(disp->queueSig);
668
669 return SRTM_Status_Success;
670 }
671
SRTM_Dispatcher_CallProc(srtm_dispatcher_t disp,srtm_procedure_t proc,uint32_t timeout)672 srtm_status_t SRTM_Dispatcher_CallProc(srtm_dispatcher_t disp, srtm_procedure_t proc, uint32_t timeout)
673 {
674 srtm_sem_t signal;
675 #if defined(SRTM_STATIC_API) && SRTM_STATIC_API
676 srtm_sem_buf_t signalStatic;
677 #endif
678 srtm_status_t status;
679
680 assert(disp);
681 assert(proc);
682
683 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
684
685 #if defined(SRTM_STATIC_API) && SRTM_STATIC_API
686 signal = SRTM_Sem_Create(1U, 0U, &signalStatic);
687 #else
688 signal = SRTM_Sem_Create(1U, 0U);
689 #endif
690 if (signal == NULL)
691 {
692 return SRTM_Status_OutOfMemory;
693 }
694
695 proc->procMsg.sig = signal;
696 SRTM_Dispatcher_QueueMessage(disp, proc);
697
698 status = SRTM_Sem_Wait(signal, timeout);
699 /* Make sure proc is no longer in messageQ */
700 if (status != SRTM_Status_Success)
701 {
702 while (SRTM_Dispatcher_DequeueMessage(disp, proc) != SRTM_Status_Success)
703 {
704 /* If request is not on any list, it means SRTM dispatcher is processing.
705 Wait again to make sure dispatcher has finished calling the procedure. */
706 if ((status = SRTM_Sem_Wait(signal, timeout)) == SRTM_Status_Success)
707 {
708 break;
709 }
710 }
711 }
712
713 /* Clean up */
714 SRTM_Sem_Destroy(signal);
715 proc->procMsg.sig = NULL;
716
717 return status;
718 }
719
SRTM_Dispatcher_PostProc(srtm_dispatcher_t disp,srtm_procedure_t proc)720 srtm_status_t SRTM_Dispatcher_PostProc(srtm_dispatcher_t disp, srtm_procedure_t proc)
721 {
722 assert(disp);
723 assert(proc);
724
725 proc->procMsg.sig = NULL;
726 SRTM_Dispatcher_QueueMessage(disp, proc);
727
728 return SRTM_Status_Success;
729 }
730
SRTM_Dispatcher_PostRecvData(srtm_dispatcher_t disp,srtm_channel_t channel,void * buf,uint32_t len)731 srtm_status_t SRTM_Dispatcher_PostRecvData(srtm_dispatcher_t disp, srtm_channel_t channel, void *buf, uint32_t len)
732 {
733 srtm_list_t *list;
734 srtm_message_t message = NULL;
735 uint32_t primask;
736 srtm_packet_head_t *head;
737
738 assert(disp);
739 assert(channel);
740 assert(buf);
741
742 if (len < sizeof(struct _srtm_packet_head) || len > SRTM_DISPATCHER_CONFIG_RX_MSG_MAX_LEN)
743 {
744 return SRTM_Status_InvalidParameter;
745 }
746
747 /* Get free message to save the data */
748 primask = DisableGlobalIRQ();
749 if (!SRTM_List_IsEmpty(&disp->freeRxMsgs))
750 {
751 list = disp->freeRxMsgs.next;
752 SRTM_List_Remove(list);
753 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
754 }
755 EnableGlobalIRQ(primask);
756
757 assert(message); /* For debugging the shortage of RX buffer */
758
759 if (message != NULL)
760 {
761 message->direct = SRTM_MessageDirectRx;
762 (void)memcpy(message->data, buf, len);
763 message->dataLen = len;
764 message->channel = channel;
765 message->error = SRTM_Status_Success;
766
767 head = (srtm_packet_head_t *)buf;
768 message->type = (srtm_message_type_t)head->type;
769 message->priority = head->priority;
770
771 SRTM_Dispatcher_QueueMessage(disp, message);
772
773 return SRTM_Status_Success;
774 }
775
776 return SRTM_Status_OutOfMemory;
777 }
778
SRTM_Dispatcher_CallService(srtm_dispatcher_t disp,srtm_message_t msg)779 static srtm_status_t SRTM_Dispatcher_CallService(srtm_dispatcher_t disp, srtm_message_t msg)
780 {
781 srtm_list_t *list;
782 srtm_service_t service;
783 uint8_t category;
784 srtm_status_t status = SRTM_Status_ServiceNotFound;
785
786 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
787
788 category = SRTM_CommMessage_GetCategory(msg);
789
790 /* Don't need to lock mutex because service will not change when dispatcher is running */
791 for (list = disp->services.next; list != &disp->services; list = list->next)
792 {
793 service = SRTM_LIST_OBJ(srtm_service_t, node, list);
794 if (service->category == category)
795 {
796 if (msg->type == SRTM_MessageTypeRequest)
797 {
798 status = SRTM_Service_Request(service, msg);
799 }
800 else
801 {
802 status = SRTM_Service_Notify(service, msg);
803 }
804 break;
805 }
806 }
807
808 return status;
809 }
810
SRTM_Dispatcher_HandleResponse(srtm_dispatcher_t disp,srtm_message_t msg)811 static srtm_status_t SRTM_Dispatcher_HandleResponse(srtm_dispatcher_t disp, srtm_message_t msg)
812 {
813 srtm_list_t *list;
814 srtm_request_t req = NULL;
815 srtm_response_t resp;
816 uint8_t category;
817 uint8_t command;
818 srtm_status_t status = SRTM_Status_Success;
819
820 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
821
822 category = SRTM_CommMessage_GetCategory(msg);
823 command = SRTM_CommMessage_GetCommand(msg);
824
825 (void)SRTM_Mutex_Lock(disp->mutex);
826 for (list = disp->waitingReqs.next; list != &disp->waitingReqs; list = list->next)
827 {
828 req = SRTM_LIST_OBJ(srtm_request_t, node, list);
829 if (SRTM_CommMessage_GetCategory(req) == category && SRTM_CommMessage_GetCommand(req) == command &&
830 req->channel == msg->channel) /* FIXME: need a UID? */
831 {
832 SRTM_List_Remove(list);
833 break;
834 }
835 }
836 (void)SRTM_Mutex_Unlock(disp->mutex);
837
838 if ((req != NULL) && (list != &disp->waitingReqs)) /* Find corresponding request */
839 {
840 if (req->reqMsg.isSyncReq)
841 {
842 resp = SRTM_Message_Duplicate(msg);
843 if (resp != NULL)
844 {
845 if (msg->dataLen > 0U)
846 {
847 (void)memcpy(resp->data, msg->data, msg->dataLen);
848 }
849 /* Duplicated response will be freed by application */
850 req->reqMsg.sync.resp = resp;
851 }
852 else
853 {
854 req->error = SRTM_Status_OutOfMemory;
855 }
856 (void)SRTM_Sem_Post(req->reqMsg.sync.sig);
857 }
858 else
859 {
860 if (req->reqMsg.async.cb != NULL)
861 {
862 req->reqMsg.async.cb(disp, req, msg, req->reqMsg.async.param);
863 }
864 /* Request is destroyed in dispatcher for async call */
865 SRTM_Request_Destroy(req);
866 }
867 }
868 else
869 {
870 status = SRTM_Status_Error;
871 }
872
873 return status;
874 }
875
SRTM_Dispatcher_SendMessage(srtm_dispatcher_t disp,srtm_message_t msg)876 static srtm_status_t SRTM_Dispatcher_SendMessage(srtm_dispatcher_t disp, srtm_message_t msg)
877 {
878 srtm_status_t status;
879
880 status = SRTM_Channel_SendData(msg->channel, msg->data, msg->dataLen);
881 assert(status == SRTM_Status_Success); /* For debugging the message sending failure */
882
883 switch (msg->type)
884 {
885 case SRTM_MessageTypeRawData:
886 SRTM_RawData_Destroy(msg);
887 break;
888 case SRTM_MessageTypeRequest:
889 if (status == SRTM_Status_Success)
890 {
891 /* Add request to waiting queue to wait for response */
892 (void)SRTM_Mutex_Lock(disp->mutex);
893 SRTM_List_AddTail(&disp->waitingReqs, &msg->node);
894 (void)SRTM_Mutex_Unlock(disp->mutex);
895 }
896 else
897 {
898 SRTM_Dispatcher_FreeRequest(disp, msg, SRTM_Status_TransferFailed);
899 }
900 break;
901 case SRTM_MessageTypeResponse:
902 SRTM_Response_Destroy(msg);
903 break;
904 case SRTM_MessageTypeNotification:
905 SRTM_Notification_Destroy(msg);
906 break;
907 default:
908 /* Do nothing */
909 break;
910 }
911
912 return status;
913 }
914
SRTM_Dispatcher_HandleTxMessage(srtm_dispatcher_t disp,srtm_message_t msg)915 static srtm_status_t SRTM_Dispatcher_HandleTxMessage(srtm_dispatcher_t disp, srtm_message_t msg)
916 {
917 srtm_status_t status = SRTM_Status_Success;
918 srtm_peercore_t core;
919 srtm_peercore_state_t state;
920 srtm_list_t *list;
921 srtm_message_t message;
922
923 assert(msg->channel);
924 assert(msg->channel->core);
925
926 core = msg->channel->core;
927 state = SRTM_PeerCore_GetState(core);
928
929 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
930
931 switch (state)
932 {
933 case SRTM_PeerCore_State_Inactive:
934 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "SRTM_PeerCore_State_Inactive\r\n");
935 /* Peer core not active, discard the message */
936 SRTM_Dispatcher_FreeMessage(disp, msg);
937 break;
938 case SRTM_PeerCore_State_Activating:
939 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "SRTM_PeerCore_State_Activating\r\n");
940 /* Peer core in activating, put message to pendingQ */
941 SRTM_List_AddTail(&core->pendingQ, &msg->node);
942 break;
943 case SRTM_PeerCore_State_Activated:
944 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "SRTM_PeerCore_State_Activated\r\n");
945 /* Peer core working well, first send messages in pendingQ and then send the msg */
946 while (!SRTM_List_IsEmpty(&core->pendingQ))
947 {
948 list = core->pendingQ.next;
949 SRTM_List_Remove(list);
950 message = SRTM_LIST_OBJ(srtm_message_t, node, list);
951 (void)SRTM_Dispatcher_SendMessage(disp, message);
952 }
953 status = SRTM_Dispatcher_SendMessage(disp, msg);
954 break;
955 case SRTM_PeerCore_State_Deactivating:
956 case SRTM_PeerCore_State_Deactivated:
957 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "SRTM_PeerCore_State_Deactivate\r\n");
958 /* Peer core in deactivate state, put message to pendingQ and activate peer core */
959 SRTM_List_AddTail(&core->pendingQ, &msg->node);
960 status = SRTM_PeerCore_Activate(core);
961 break;
962 default:
963 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_WARN, "%s: wrong peer core state!\r\n", __func__);
964 break;
965 }
966
967 return status;
968 }
969
SRTM_Dispatcher_ProcessMessage(srtm_dispatcher_t disp,srtm_message_t msg)970 srtm_status_t SRTM_Dispatcher_ProcessMessage(srtm_dispatcher_t disp, srtm_message_t msg)
971 {
972 srtm_status_t status = SRTM_Status_Success;
973
974 assert(disp);
975 assert(msg);
976
977 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s\r\n", __func__);
978
979 SRTM_DumpMessage(msg);
980
981 if (msg->direct == SRTM_MessageDirectRx)
982 {
983 switch (msg->type)
984 {
985 case SRTM_MessageTypeRequest:
986 case SRTM_MessageTypeNotification:
987 status = SRTM_Dispatcher_CallService(disp, msg);
988 break;
989 case SRTM_MessageTypeResponse:
990 status = SRTM_Dispatcher_HandleResponse(disp, msg);
991 break;
992 default:
993 /* We cannot handle message other than Communication Message */
994 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_WARN, "%s: unsupported RX message type %d\r\n", __func__,
995 msg->type);
996 break;
997 }
998 SRTM_Dispatcher_FreeMessage(disp, msg);
999 }
1000 else /* TX message or procedure message */
1001 {
1002 switch (msg->type)
1003 {
1004 case SRTM_MessageTypeProcedure:
1005 SRTM_DEBUG_MESSAGE(SRTM_DEBUG_VERBOSE_DEBUG, "%s: Callback procedure 0x%08x\r\n", __func__,
1006 msg->procMsg.cb);
1007 msg->procMsg.cb(disp, msg->procMsg.param1, msg->procMsg.param2);
1008 if (msg->procMsg.sig != NULL) /* Called by SRTM_Dispatcher_CallProc() */
1009 {
1010 /* Synchronous procedure message will be freed by user. Here just wakeup user's task. */
1011 (void)SRTM_Sem_Post(msg->procMsg.sig);
1012 }
1013 else /* Called by SRTM_Dispatcher_PostProc() */
1014 {
1015 SRTM_Procedure_Destroy(msg);
1016 }
1017 break;
1018 default:
1019 status = SRTM_Dispatcher_HandleTxMessage(disp, msg);
1020 break;
1021 }
1022 }
1023
1024 return status;
1025 }
1026
