1 /*
2 * Copyright 2012-2020 NXP
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <errno.h>
18 #include <linux/ipc.h>
19 #include <phDal4Uwb_messageQueueLib.h>
20 #include <phNxpLog.h>
21 #include <pthread.h>
22 #include <semaphore.h>
23
24 typedef struct phDal4Uwb_message_queue_item {
25 phLibUwb_Message_t nMsg;
26 struct phDal4Uwb_message_queue_item* pPrev;
27 struct phDal4Uwb_message_queue_item* pNext;
28 } phDal4Uwb_message_queue_item_t;
29
30 typedef struct phDal4Uwb_message_queue {
31 phDal4Uwb_message_queue_item_t* pItems;
32 pthread_mutex_t nCriticalSectionMutex;
33 sem_t nProcessSemaphore;
34
35 } phDal4Uwb_message_queue_t;
36
37 /*******************************************************************************
38 **
39 ** Function phDal4Uwb_msgget
40 **
41 ** Description Allocates message queue
42 **
43 ** Parameters Ignored, included only for Linux queue API compatibility
44 **
45 ** Returns (int) value of pQueue if successful
46 ** -1, if failed to allocate memory or to init mutex
47 **
48 *******************************************************************************/
phDal4Uwb_msgget(key_t key,int msgflg)49 intptr_t phDal4Uwb_msgget(key_t key, int msgflg) {
50 phDal4Uwb_message_queue_t* pQueue;
51 UNUSED(key);
52 UNUSED(msgflg);
53 pQueue =
54 (phDal4Uwb_message_queue_t*)malloc(sizeof(phDal4Uwb_message_queue_t));
55 if (pQueue == NULL) return -1;
56 memset(pQueue, 0, sizeof(phDal4Uwb_message_queue_t));
57 if (pthread_mutex_init(&pQueue->nCriticalSectionMutex, NULL) == -1) {
58 free(pQueue);
59 return -1;
60 }
61 if (sem_init(&pQueue->nProcessSemaphore, 0, 0) == -1) {
62 free(pQueue);
63 return -1;
64 }
65
66 return ((intptr_t)pQueue);
67 }
68
69 /*******************************************************************************
70 **
71 ** Function phDal4Uwb_msgrelease
72 **
73 ** Description Releases message queue
74 **
75 ** Parameters msqid - message queue handle
76 **
77 ** Returns None
78 **
79 *******************************************************************************/
phDal4Uwb_msgrelease(intptr_t msqid)80 void phDal4Uwb_msgrelease(intptr_t msqid) {
81 phDal4Uwb_message_queue_t* pQueue = (phDal4Uwb_message_queue_t*)msqid;
82
83 if (pQueue != NULL) {
84 sem_post(&pQueue->nProcessSemaphore);
85 usleep(3000);
86 if (sem_destroy(&pQueue->nProcessSemaphore)) {
87 NXPLOG_TML_E("Failed to destroy semaphore");
88 }
89 pthread_mutex_destroy(&pQueue->nCriticalSectionMutex);
90
91 free(pQueue);
92 }
93
94 return;
95 }
96
97 /*******************************************************************************
98 **
99 ** Function phDal4Uwb_msgctl
100 **
101 ** Description Destroys message queue
102 **
103 ** Parameters msqid - message queue handle
104 ** cmd, buf - ignored, included only for Linux queue API
105 ** compatibility
106 **
107 ** Returns 0, if successful
108 ** -1, if invalid handle is passed
109 **
110 *******************************************************************************/
phDal4Uwb_msgctl(intptr_t msqid,int cmd,void * buf)111 int phDal4Uwb_msgctl(intptr_t msqid, int cmd, void* buf) {
112 phDal4Uwb_message_queue_t* pQueue;
113 phDal4Uwb_message_queue_item_t* p;
114 UNUSED(cmd);
115 UNUSED(buf);
116 if (msqid == 0) return -1;
117
118 pQueue = (phDal4Uwb_message_queue_t*)msqid;
119 pthread_mutex_lock(&pQueue->nCriticalSectionMutex);
120 if (pQueue->pItems != NULL) {
121 p = pQueue->pItems;
122 while (p->pNext != NULL) {
123 p = p->pNext;
124 }
125 while (p->pPrev != NULL) {
126 p = p->pPrev;
127 free(p->pNext);
128 p->pNext = NULL;
129 }
130 free(p);
131 }
132 pQueue->pItems = NULL;
133 pthread_mutex_unlock(&pQueue->nCriticalSectionMutex);
134 pthread_mutex_destroy(&pQueue->nCriticalSectionMutex);
135 free(pQueue);
136
137 return 0;
138 }
139
140 /*******************************************************************************
141 **
142 ** Function phDal4Uwb_msgsnd
143 **
144 ** Description Sends a message to the queue. The message will be added at
145 ** the end of the queue as appropriate for FIFO policy
146 **
147 ** Parameters msqid - message queue handle
148 ** msgp - message to be sent
149 ** msgsz - message size
150 ** msgflg - ignored
151 **
152 ** Returns 0, if successful
153 ** -1, if invalid parameter passed or failed to allocate memory
154 **
155 *******************************************************************************/
phDal4Uwb_msgsnd(intptr_t msqid,phLibUwb_Message_t * msg,int msgflg)156 intptr_t phDal4Uwb_msgsnd(intptr_t msqid, phLibUwb_Message_t* msg, int msgflg) {
157 phDal4Uwb_message_queue_t* pQueue;
158 phDal4Uwb_message_queue_item_t* p;
159 phDal4Uwb_message_queue_item_t* pNew;
160 UNUSED(msgflg);
161 if ((msqid == 0) || (msg == NULL)) return -1;
162
163 pQueue = (phDal4Uwb_message_queue_t*)msqid;
164 pNew = (phDal4Uwb_message_queue_item_t*)malloc(
165 sizeof(phDal4Uwb_message_queue_item_t));
166 if (pNew == NULL) return -1;
167 memset(pNew, 0, sizeof(phDal4Uwb_message_queue_item_t));
168 memcpy(&pNew->nMsg, msg, sizeof(phLibUwb_Message_t));
169 pthread_mutex_lock(&pQueue->nCriticalSectionMutex);
170
171 if (pQueue->pItems != NULL) {
172 p = pQueue->pItems;
173 while (p->pNext != NULL) {
174 p = p->pNext;
175 }
176 p->pNext = pNew;
177 pNew->pPrev = p;
178 } else {
179 pQueue->pItems = pNew;
180 }
181 pthread_mutex_unlock(&pQueue->nCriticalSectionMutex);
182
183 sem_post(&pQueue->nProcessSemaphore);
184
185 return 0;
186 }
187
188 /*******************************************************************************
189 **
190 ** Function phDal4Uwb_msgrcv
191 **
192 ** Description Gets the oldest message from the queue.
193 ** If the queue is empty the function waits (blocks on a mutex)
194 ** until a message is posted to the queue with phDal4Uwb_msgsnd
195 **
196 ** Parameters msqid - message queue handle
197 ** msgp - message to be received
198 ** msgsz - message size
199 ** msgtyp - ignored
200 ** msgflg - ignored
201 **
202 ** Returns 0, if successful
203 ** -1, if invalid parameter passed
204 **
205 *******************************************************************************/
phDal4Uwb_msgrcv(intptr_t msqid,phLibUwb_Message_t * msg,long msgtyp,int msgflg)206 int phDal4Uwb_msgrcv(intptr_t msqid, phLibUwb_Message_t* msg, long msgtyp,
207 int msgflg) {
208 phDal4Uwb_message_queue_t* pQueue;
209 phDal4Uwb_message_queue_item_t* p;
210 UNUSED(msgflg);
211 UNUSED(msgtyp);
212 if ((msqid == 0) || (msg == NULL)) return -1;
213
214 pQueue = (phDal4Uwb_message_queue_t*)msqid;
215
216 if (sem_wait(&pQueue->nProcessSemaphore) != 0) {
217 NXPLOG_TML_E("Failed to wait semaphore (errno=0x%08x)", errno);
218 }
219
220 pthread_mutex_lock(&pQueue->nCriticalSectionMutex);
221
222 if (pQueue->pItems != NULL) {
223 memcpy(msg, &(pQueue->pItems)->nMsg, sizeof(phLibUwb_Message_t));
224 p = pQueue->pItems->pNext;
225 free(pQueue->pItems);
226 pQueue->pItems = p;
227 }
228 pthread_mutex_unlock(&pQueue->nCriticalSectionMutex);
229
230 return 0;
231 }
232