1 /*
2 * Copyright 2017-2018,2021 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 "phOsal_Posix.h"
18
19 #include <asm-generic/errno-base.h>
20 #include <asm-generic/errno.h>
21 #include <errno.h>
22 #include <pthread.h>
23 #include <sched.h>
24 #include <semaphore.h>
25 #include <string.h>
26 #include <sys/time.h>
27 #include <unistd.h>
28 #include <cstdlib>
29 #include <ctime>
30
31 /*
32 ****************************** Macro Definitions ******************************
33 */
34 #define LPVOID void*
35
36 //#define LOG_FUNCTION_ENTRY phOsal_LogFunctionEntry((const
37 // uint8_t*)"Osal",(const uint8_t*)__FUNCTION__) #define LOG_FUNCTION_EXIT
38 // phOsal_LogFunctionExit((const uint8_t*)"Osal",(const uint8_t*)__FUNCTION__)
39
40 #define LOG_FUNCTION_ENTRY
41 #define LOG_FUNCTION_EXIT
42 /*
43 *************************** Function Definitions ******************************
44 */
45
phOsal_ThreadCreate(void ** hThread,pphOsal_ThreadFunction_t pThreadFunction,void * pParam)46 OSALSTATUS phOsal_ThreadCreate(void** hThread,
47 pphOsal_ThreadFunction_t pThreadFunction,
48 void* pParam) {
49 int32_t status = 0;
50 LOG_FUNCTION_ENTRY;
51 if ((NULL == hThread) || (NULL == pThreadFunction)) {
52 return OSALSTATUS_INVALID_PARAMS;
53 }
54
55 /* Check for successful creation of thread */
56 status = pthread_create((pthread_t*)hThread, NULL, pThreadFunction, pParam);
57 if (0 != status) {
58 phOsal_LogError((const uint8_t*)"Osal>Unable to create Thread");
59 return OSALSTATUS_FAILED;
60 }
61
62 LOG_FUNCTION_EXIT;
63 return OSALSTATUS_SUCCESS;
64 }
65
phOsal_ThreadGetTaskId(void)66 uint32_t phOsal_ThreadGetTaskId(void) {
67 uint32_t dwThreadId = 0;
68 LOG_FUNCTION_ENTRY;
69 LOG_FUNCTION_EXIT;
70 return dwThreadId;
71 }
72
phOsal_ThreadDelete(void * hThread)73 OSALSTATUS phOsal_ThreadDelete(void* hThread) {
74 void* pRetVal;
75 uint32_t status = 0;
76 LOG_FUNCTION_ENTRY;
77 if (NULL == hThread) {
78 return OSALSTATUS_INVALID_PARAMS;
79 }
80 status = pthread_join((pthread_t)hThread, &pRetVal);
81 if (0 != status) {
82 phOsal_LogError((const uint8_t*)"Osal>Unable to delete Thread");
83 return OSALSTATUS_FAILED;
84 }
85 LOG_FUNCTION_EXIT;
86 return OSALSTATUS_SUCCESS;
87 }
88
89 #ifdef ENABLE_ADVANCED_FUNCS
phOsal_ThreadSuspend(void * hThread)90 OSALSTATUS phOsal_ThreadSuspend(void* hThread) {
91 OSALSTATUS wSuspendStatus = OSALSTATUS_SUCCESS;
92 LOG_FUNCTION_ENTRY;
93 LOG_FUNCTION_EXIT;
94 return wSuspendStatus;
95 }
96
phOsal_ThreadWakeUp(void * hThread)97 OSALSTATUS phOsal_ThreadWakeUp(void* hThread) {
98 OSALSTATUS wResumeStatus = OSALSTATUS_SUCCESS;
99 LOG_FUNCTION_ENTRY;
100 LOG_FUNCTION_EXIT;
101 return wResumeStatus;
102 }
103 #endif
104
phOsal_ThreadSetPriority(void * hThread,int32_t sdwPriority)105 OSALSTATUS phOsal_ThreadSetPriority(void* hThread, int32_t sdwPriority) {
106 uint32_t dwStatus = 0;
107 struct sched_param param;
108 int32_t policy;
109 LOG_FUNCTION_ENTRY;
110 if (NULL == hThread) {
111 return OSALSTATUS_INVALID_PARAMS;
112 }
113 dwStatus = pthread_getschedparam((pthread_t)hThread, &policy, ¶m);
114 if (dwStatus != 0) {
115 phOsal_LogErrorU32h(
116 (const uint8_t*)"Osal>Unable to get thread params.Error=",
117 (uint32_t)dwStatus);
118 phOsal_LogErrorString((const uint8_t*)"Osal>",
119 (const uint8_t*)__FUNCTION__);
120 return OSALSTATUS_FAILED;
121 }
122 param.sched_priority = sdwPriority;
123 dwStatus = pthread_setschedparam((pthread_t)hThread, policy, ¶m);
124 if (dwStatus != 0) {
125 phOsal_LogErrorU32h(
126 (const uint8_t*)"Osal>Unable to Set thread Priority.Error=",
127 (uint32_t)dwStatus);
128 phOsal_LogErrorString((const uint8_t*)"Osal>",
129 (const uint8_t*)__FUNCTION__);
130 return OSALSTATUS_FAILED;
131 }
132 LOG_FUNCTION_EXIT;
133 return OSALSTATUS_SUCCESS;
134 }
135
136 /*static void * phOsal_ThreadProcedure(void *lpParameter)
137 {
138 return lpParameter;
139 }*/
140
phOsal_SemaphoreCreate(void ** hSemaphore,uint8_t bInitialValue,uint8_t bMaxValue)141 OSALSTATUS phOsal_SemaphoreCreate(void** hSemaphore, uint8_t bInitialValue,
142 __attribute__((unused)) uint8_t bMaxValue) {
143 int32_t status = 0;
144 LOG_FUNCTION_ENTRY;
145 // phOsal_LogInfoU32d((const uint8_t*)"Osal>Sem Max
146 // Value:",(uint32_t)bMaxValue);
147
148 if (hSemaphore == NULL) {
149 phOsal_LogError((const uint8_t*)"Osal>Invalid Semaphore Handle");
150 return OSALSTATUS_INVALID_PARAMS;
151 }
152
153 *hSemaphore = (sem_t*)malloc(sizeof(sem_t));
154 if (*hSemaphore == NULL) {
155 phOsal_LogError(
156 (const uint8_t*)"Osal>Unable to allocate memory for semaphore");
157 return OSALSTATUS_FAILED;
158 }
159
160 status = sem_init((sem_t*)*hSemaphore, 0, bInitialValue);
161 if (status == -1) {
162 phOsal_LogErrorU32d(
163 (const uint8_t*)"Osal>Unable to allocate memory for semaphore.Status=",
164 (uint32_t)status);
165 return OSALSTATUS_FAILED;
166 }
167 // phOsal_LogInfo((const uint8_t*)"Osal> Semaphore Created");
168 LOG_FUNCTION_EXIT;
169 return OSALSTATUS_SUCCESS;
170 }
171
phOsal_SemaphorePost(void * hSemaphore)172 OSALSTATUS phOsal_SemaphorePost(void* hSemaphore) {
173 int32_t checkval;
174 LOG_FUNCTION_ENTRY;
175 if (hSemaphore == NULL) {
176 phOsal_LogError((const uint8_t*)"Osal>Invalid Semaphore Handle");
177 return OSALSTATUS_INVALID_PARAMS;
178 }
179
180 if (sem_getvalue((sem_t*)hSemaphore, &checkval) == -1) {
181 phOsal_LogError((const uint8_t*)"Osal> Semaphore Not available");
182 return OSALSTATUS_INVALID_PARAMS;
183 }
184
185 if (sem_post((sem_t*)hSemaphore) == -1) {
186 phOsal_LogError((const uint8_t*)"Osal> error in sem Post");
187 return OSALSTATUS_INVALID_PARAMS;
188 }
189
190 LOG_FUNCTION_EXIT;
191 return OSALSTATUS_SUCCESS;
192 }
193
phOsal_SemaphoreWait(void * hSemaphore,uint32_t timeout_ms)194 OSALSTATUS phOsal_SemaphoreWait(void* hSemaphore, uint32_t timeout_ms) {
195 int32_t checkval;
196 LOG_FUNCTION_ENTRY;
197 if (hSemaphore == NULL) {
198 phOsal_LogError((const uint8_t*)"Osal>Invalid Semaphore Handle");
199 return OSALSTATUS_INVALID_PARAMS;
200 }
201
202 if (sem_getvalue((sem_t*)hSemaphore, &checkval) == -1) {
203 phOsal_LogError((const uint8_t*)"Osal> Semaphore Not available");
204 return OSALSTATUS_INVALID_PARAMS;
205 }
206
207 if (timeout_ms == 0) {
208 if (sem_wait((sem_t*)hSemaphore) == -1) {
209 phOsal_LogError(
210 (const uint8_t*)"Osal> Error in Semaphore infinite wait !!");
211 return OSALSTATUS_INVALID_PARAMS;
212 }
213 } else {
214 struct timespec xtms;
215 int32_t status = 0;
216 if (clock_gettime(CLOCK_REALTIME, &xtms) == -1) {
217 phOsal_LogError(
218 (const uint8_t*)"Osal> Error in Getting current CPU time!!");
219 return OSALSTATUS_INVALID_PARAMS;
220 }
221
222 /*Extract seconds and nanoseconds information from time in milliseconds*/
223 xtms.tv_sec += (time_t)timeout_ms / 1000;
224 xtms.tv_nsec += ((long)(timeout_ms % 1000)) * (1000000);
225
226 while ((status = sem_timedwait((sem_t*)hSemaphore, &xtms)) == -1 &&
227 errno == EINTR) {
228 phOsal_LogError(
229 (const uint8_t*)"Osal>Error in sem_timedwait restart it!!");
230 continue; /* Restart if interrupted by handler */
231 }
232 /* Check what happened */
233 if (status == -1) {
234 if (errno == ETIMEDOUT) {
235 phOsal_LogError((const uint8_t*)"Osal>sem_timedwait() timed out");
236 return OSALSTATUS_SEM_TIMEOUT;
237 } else {
238 phOsal_LogError((const uint8_t*)"Osal>sem_timedwait");
239 return OSALSTATUS_FAILED;
240 }
241 } else {
242 phOsal_LogInfo((const uint8_t*)"Osal>sem_timedwait() succeeded");
243 }
244 }
245 LOG_FUNCTION_EXIT;
246 return OSALSTATUS_SUCCESS;
247 }
248
phOsal_SemaphoreDelete(void * hSemaphore)249 OSALSTATUS phOsal_SemaphoreDelete(void* hSemaphore) {
250 int32_t checkval;
251 LOG_FUNCTION_ENTRY;
252 if (hSemaphore == NULL) {
253 phOsal_LogError((const uint8_t*)"Osal>Invalid Semaphore Handle");
254 return OSALSTATUS_INVALID_PARAMS;
255 }
256
257 if (sem_getvalue((sem_t*)hSemaphore, &checkval) == -1) {
258 phOsal_LogError((const uint8_t*)"Osal> Semaphore Not available");
259 return OSALSTATUS_INVALID_PARAMS;
260 }
261
262 if (sem_destroy((sem_t*)hSemaphore) == -1) {
263 phOsal_LogError((const uint8_t*)"Osal> Semaphore Destroy Failed");
264 return OSALSTATUS_FAILED;
265 }
266
267 free(hSemaphore);
268 LOG_FUNCTION_EXIT;
269 return OSALSTATUS_SUCCESS;
270 }
271
phOsal_MutexCreate(void ** hMutex)272 OSALSTATUS phOsal_MutexCreate(void** hMutex) {
273 int32_t status = 0;
274 LOG_FUNCTION_ENTRY;
275
276 if (hMutex == NULL) {
277 phOsal_LogError((const uint8_t*)"Osal>Invalid Mutex Handle");
278 return OSALSTATUS_INVALID_PARAMS;
279 }
280
281 *hMutex = (pthread_mutex_t*)malloc(sizeof(pthread_mutex_t));
282 if (*hMutex == NULL) {
283 phOsal_LogError((const uint8_t*)"Osal>Unable to allocate memory for mutex");
284 return OSALSTATUS_FAILED;
285 }
286
287 status = pthread_mutex_init((pthread_mutex_t*)*hMutex, 0);
288 if (status != 0) {
289 phOsal_LogErrorU32d((const uint8_t*)"Osal>Error in Mutex Lock",
290 (uint32_t)status);
291 return OSALSTATUS_FAILED;
292 }
293 // phOsal_LogInfo((const uint8_t*)"Osal> Mutex Created");
294 LOG_FUNCTION_EXIT;
295 return OSALSTATUS_SUCCESS;
296 }
297
phOsal_MutexLock(void * hMutex)298 OSALSTATUS phOsal_MutexLock(void* hMutex) {
299 LOG_FUNCTION_ENTRY;
300 if (hMutex == NULL) {
301 phOsal_LogError((const uint8_t*)"Osal>Invalid Mutex Handle");
302 return OSALSTATUS_INVALID_PARAMS;
303 }
304
305 if (pthread_mutex_lock((pthread_mutex_t*)hMutex) == -1) {
306 phOsal_LogError((const uint8_t*)"Osal>Error in Mutex Lock");
307 return OSALSTATUS_INVALID_PARAMS;
308 }
309
310 LOG_FUNCTION_EXIT;
311 return OSALSTATUS_SUCCESS;
312 }
313
phOsal_MutexUnlock(void * hMutex)314 OSALSTATUS phOsal_MutexUnlock(void* hMutex) {
315 LOG_FUNCTION_ENTRY;
316 if (hMutex == NULL) {
317 phOsal_LogError((const uint8_t*)"Osal>Invalid Mutex Handle");
318 return OSALSTATUS_INVALID_PARAMS;
319 }
320
321 if (pthread_mutex_unlock((pthread_mutex_t*)hMutex) == -1) {
322 phOsal_LogError((const uint8_t*)"Osal>Error in Mutex UnLock");
323 return OSALSTATUS_INVALID_PARAMS;
324 }
325
326 LOG_FUNCTION_EXIT;
327 return OSALSTATUS_SUCCESS;
328 }
329
phOsal_MutexDelete(void * hMutex)330 OSALSTATUS phOsal_MutexDelete(void* hMutex) {
331 LOG_FUNCTION_ENTRY;
332 if (hMutex == NULL) {
333 phOsal_LogError((const uint8_t*)"Osal>Invalid Mutex Handle");
334 return OSALSTATUS_INVALID_PARAMS;
335 }
336
337 if (pthread_mutex_destroy((pthread_mutex_t*)hMutex) == -1) {
338 phOsal_LogError((const uint8_t*)"Osal>Error in Mutex Destroy");
339 return OSALSTATUS_INVALID_PARAMS;
340 }
341
342 free(hMutex);
343
344 LOG_FUNCTION_EXIT;
345 return OSALSTATUS_SUCCESS;
346 }
347
phOsal_Init(pphOsal_Config_t pOsalConfig)348 OSALSTATUS phOsal_Init(pphOsal_Config_t pOsalConfig) {
349 // pphOsal_Config_t config = pOsalConfig;
350 memset((uint8_t*)&pOsalConfig, 0, sizeof(pphOsal_Config_t));
351 LOG_FUNCTION_ENTRY;
352 LOG_FUNCTION_EXIT;
353 return OSALSTATUS_SUCCESS;
354 }
355
phOsal_Delay(uint32_t dwDelayInMs)356 void phOsal_Delay(uint32_t dwDelayInMs) {
357 usleep(dwDelayInMs * 1000); /**< Converting milliseconds to Microseconds */
358 }
359