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_MONOTONIC, &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 if (xtms.tv_nsec >= 1000000000L) {
226 xtms.tv_sec += 1;
227 xtms.tv_nsec -= 1000000000L;
228 }
229
230 while ((status = sem_timedwait_monotonic_np((sem_t*)hSemaphore, &xtms)) == -1 &&
231 errno == EINTR) {
232 phOsal_LogError(
233 (const uint8_t*)"Osal>Error in sem_timedwait restart it!!");
234 continue; /* Restart if interrupted by handler */
235 }
236 /* Check what happened */
237 if (status == -1) {
238 if (errno == ETIMEDOUT) {
239 phOsal_LogError((const uint8_t*)"Osal>sem_timedwait() timed out");
240 return OSALSTATUS_SEM_TIMEOUT;
241 } else {
242 phOsal_LogError((const uint8_t*)"Osal>sem_timedwait");
243 return OSALSTATUS_FAILED;
244 }
245 } else {
246 phOsal_LogInfo((const uint8_t*)"Osal>sem_timedwait() succeeded");
247 }
248 }
249 LOG_FUNCTION_EXIT;
250 return OSALSTATUS_SUCCESS;
251 }
252
phOsal_SemaphoreDelete(void * hSemaphore)253 OSALSTATUS phOsal_SemaphoreDelete(void* hSemaphore) {
254 int32_t checkval;
255 LOG_FUNCTION_ENTRY;
256 if (hSemaphore == NULL) {
257 phOsal_LogError((const uint8_t*)"Osal>Invalid Semaphore Handle");
258 return OSALSTATUS_INVALID_PARAMS;
259 }
260
261 if (sem_getvalue((sem_t*)hSemaphore, &checkval) == -1) {
262 phOsal_LogError((const uint8_t*)"Osal> Semaphore Not available");
263 return OSALSTATUS_INVALID_PARAMS;
264 }
265
266 if (sem_destroy((sem_t*)hSemaphore) == -1) {
267 phOsal_LogError((const uint8_t*)"Osal> Semaphore Destroy Failed");
268 return OSALSTATUS_FAILED;
269 }
270
271 free(hSemaphore);
272 LOG_FUNCTION_EXIT;
273 return OSALSTATUS_SUCCESS;
274 }
275
phOsal_MutexCreate(void ** hMutex)276 OSALSTATUS phOsal_MutexCreate(void** hMutex) {
277 int32_t status = 0;
278 LOG_FUNCTION_ENTRY;
279
280 if (hMutex == NULL) {
281 phOsal_LogError((const uint8_t*)"Osal>Invalid Mutex Handle");
282 return OSALSTATUS_INVALID_PARAMS;
283 }
284
285 *hMutex = (pthread_mutex_t*)malloc(sizeof(pthread_mutex_t));
286 if (*hMutex == NULL) {
287 phOsal_LogError((const uint8_t*)"Osal>Unable to allocate memory for mutex");
288 return OSALSTATUS_FAILED;
289 }
290
291 status = pthread_mutex_init((pthread_mutex_t*)*hMutex, 0);
292 if (status != 0) {
293 phOsal_LogErrorU32d((const uint8_t*)"Osal>Error in Mutex Lock",
294 (uint32_t)status);
295 return OSALSTATUS_FAILED;
296 }
297 // phOsal_LogInfo((const uint8_t*)"Osal> Mutex Created");
298 LOG_FUNCTION_EXIT;
299 return OSALSTATUS_SUCCESS;
300 }
301
phOsal_MutexLock(void * hMutex)302 OSALSTATUS phOsal_MutexLock(void* hMutex) {
303 LOG_FUNCTION_ENTRY;
304 if (hMutex == NULL) {
305 phOsal_LogError((const uint8_t*)"Osal>Invalid Mutex Handle");
306 return OSALSTATUS_INVALID_PARAMS;
307 }
308
309 if (pthread_mutex_lock((pthread_mutex_t*)hMutex) == -1) {
310 phOsal_LogError((const uint8_t*)"Osal>Error in Mutex Lock");
311 return OSALSTATUS_INVALID_PARAMS;
312 }
313
314 LOG_FUNCTION_EXIT;
315 return OSALSTATUS_SUCCESS;
316 }
317
phOsal_MutexUnlock(void * hMutex)318 OSALSTATUS phOsal_MutexUnlock(void* hMutex) {
319 LOG_FUNCTION_ENTRY;
320 if (hMutex == NULL) {
321 phOsal_LogError((const uint8_t*)"Osal>Invalid Mutex Handle");
322 return OSALSTATUS_INVALID_PARAMS;
323 }
324
325 if (pthread_mutex_unlock((pthread_mutex_t*)hMutex) == -1) {
326 phOsal_LogError((const uint8_t*)"Osal>Error in Mutex UnLock");
327 return OSALSTATUS_INVALID_PARAMS;
328 }
329
330 LOG_FUNCTION_EXIT;
331 return OSALSTATUS_SUCCESS;
332 }
333
phOsal_MutexDelete(void * hMutex)334 OSALSTATUS phOsal_MutexDelete(void* hMutex) {
335 LOG_FUNCTION_ENTRY;
336 if (hMutex == NULL) {
337 phOsal_LogError((const uint8_t*)"Osal>Invalid Mutex Handle");
338 return OSALSTATUS_INVALID_PARAMS;
339 }
340
341 if (pthread_mutex_destroy((pthread_mutex_t*)hMutex) == -1) {
342 phOsal_LogError((const uint8_t*)"Osal>Error in Mutex Destroy");
343 return OSALSTATUS_INVALID_PARAMS;
344 }
345
346 free(hMutex);
347
348 LOG_FUNCTION_EXIT;
349 return OSALSTATUS_SUCCESS;
350 }
351
phOsal_Init(pphOsal_Config_t pOsalConfig)352 OSALSTATUS phOsal_Init(pphOsal_Config_t pOsalConfig) {
353 // pphOsal_Config_t config = pOsalConfig;
354 memset((uint8_t*)&pOsalConfig, 0, sizeof(pphOsal_Config_t));
355 LOG_FUNCTION_ENTRY;
356 LOG_FUNCTION_EXIT;
357 return OSALSTATUS_SUCCESS;
358 }
359
phOsal_Delay(uint32_t dwDelayInMs)360 void phOsal_Delay(uint32_t dwDelayInMs) {
361 usleep(dwDelayInMs * 1000); /**< Converting milliseconds to Microseconds */
362 }
363