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, &param);
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, &param);
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