pal_plat_rtos.c

00001 /*
00002  * Copyright (c) 2016 ARM Limited. All rights reserved.
00003  * SPDX-License-Identifier: Apache-2.0
00004  * Licensed under the Apache License, Version 2.0 (the License); you may
00005  * not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  * http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
00012  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 #define _GNU_SOURCE // This is for ppoll found in poll.h
00017 #include <stdlib.h>
00018 #include <stdio.h>
00019 #include <time.h>
00020 #include <pthread.h>
00021 #include <semaphore.h>
00022 #include <signal.h>
00023 #include <mqueue.h>
00024 #include <errno.h>
00025 #include <string.h>
00026 #include <unistd.h>
00027 #include <fcntl.h>
00028 #include <sys/reboot.h>
00029 #include <sys/types.h>
00030 #include <sys/wait.h>
00031 #include <sys/utsname.h>
00032 
00033 #include "pal.h"
00034 #include "pal_plat_rtos.h"
00035 
00036 
00037 
00038 typedef struct palThreadFuncWrapper
00039 {
00040     palTimerFuncPtr realThreadFunc;
00041     void* realThreadArgs;
00042     uint32_t threadIndex;
00043 } palThreadFuncWrapper_t;
00044 
00045 //! Thread structure
00046 typedef struct palThread
00047 {
00048     pthread_t  threadID;
00049     uint32_t   palThreadID;
00050     bool initialized;
00051     palThreadLocalStore_t* threadStore; //! please see pal_rtos.h for documentation
00052     palThreadFuncWrapper_t threadFuncWrapper;
00053     palThreadPriority_t priority;
00054     uint32_t stackSize;
00055 } palThread_t;
00056 
00057 /*! Count number of created threads. Initiate to zero.
00058 */
00059 PAL_PRIVATE uint32_t g_threadCounter = 0;
00060 
00061 palThread_t g_palThreads[PAL_MAX_NUMBER_OF_THREADS];
00062 
00063 /*
00064  * The realtime clock is in nano seconds resolution. This is too much for us, so we use "longer" ticks.
00065  * Below are relevant defines.
00066  * make sure they all coherent. Can use one at the other, but will add some unneeded calculations.
00067  */
00068 #define NANOS_PER_TICK 100
00069 #define TICKS_PER_MICRO  10L
00070 #define TICKS_PER_MILLI  TICKS_PER_MICRO * 1000
00071 #define TICKS_PER_SECOND TICKS_PER_MILLI * 1000
00072 
00073 // priorities must be positive, so shift all by this margin. we might want to do smarter convert.
00074 #define LINUX_THREAD_PRIORITY_BASE 10
00075 
00076 //  message Queues names related staff:
00077 #define MQ_FILENAME_LEN 10
00078 
00079 #ifndef CLOCK_MONOTONIC_RAW //a workaround for the operWRT port that missing this include
00080 #define CLOCK_MONOTONIC_RAW 4 //http://elixir.free-electrons.com/linux/latest/source/include/uapi/linux/time.h
00081 #endif
00082 
00083 PAL_PRIVATE char g_mqName[MQ_FILENAME_LEN];
00084 PAL_PRIVATE int g_mqNextNameNum = 0;
00085 
00086 
00087 extern palStatus_t pal_plat_getRandomBufferFromHW(uint8_t *randomBuf, size_t bufSizeBytes);
00088 
00089 inline PAL_PRIVATE void nextMessageQName()
00090 {
00091     g_mqNextNameNum++;
00092     for (int j = 4, divider = 10000; j < 9; j++, divider /= 10)
00093     {
00094         g_mqName[j] = '0' + (g_mqNextNameNum / divider) %10 ; //just to make sure we don't write more then 1 digit.
00095     }
00096     g_mqName[9] = '\0';
00097 }
00098 
00099 
00100 /*! Initiate a system reboot.
00101  */
00102 void pal_plat_osReboot (void)
00103 {
00104     struct utsname buf;
00105     buf.nodename[0] = 0;
00106     // Get the system names. Ignore error for this function call
00107     uname(&buf);
00108     // We assume that it is a desktop if "ubuntu" is returned.
00109     if(!strcmp(buf.nodename,"ubuntu"))
00110     {
00111         // We emulate resetting the device by running the application again.
00112         // Restart the application
00113         int status, timeout; // unused ifdef WAIT_FOR_COMPLETION ;
00114         const char *argv[] = {"0" , 0};
00115 
00116         //printf("program_invocation_name=%s, __progname=%s",program_invocation_name,__progname);
00117         argv[0] = program_invocation_name;
00118         pid_t   my_pid;
00119 
00120         if (0 == (my_pid = fork()))
00121         {
00122             char *const envp[] = { 0 };
00123             if (-1 == execve(argv[0], (char **)argv , envp))
00124             {
00125                 printf("child process execve failed [%s]",argv[0]);
00126             }
00127         }
00128         timeout = 1000;
00129 
00130         while (0 == waitpid(my_pid , &status , WNOHANG))
00131         {
00132             if ( --timeout < 0 ) {
00133                 perror("timeout");
00134                 //return -1;
00135             }
00136             sleep(1);
00137         }
00138     }
00139     else
00140     {
00141         // Reboot the device
00142         reboot(RB_AUTOBOOT);
00143     }
00144     return;
00145 }
00146 
00147 /*! Initialize all data structures (semaphores, mutexs, memory pools, message queues) at system initialization.
00148  *  In case of a failure in any of the initializations, the function returns with an error and stops the rest of the initializations.
00149  * @param[in] opaqueContext The context passed to the initialization (not required for generic CMSIS, pass NULL in this case).
00150  * \return PAL_SUCCESS(0) in case of success, PAL_ERR_CREATION_FAILED in case of failure.
00151  */
00152 palStatus_t pal_plat_RTOSInitialize (void* opaqueContext)
00153 {
00154     palStatus_t status = PAL_SUCCESS;
00155     (void) opaqueContext;
00156     strncpy(g_mqName, "/pal00001", MQ_FILENAME_LEN);
00157     g_mqNextNameNum = 1;   // used for the next name
00158 
00159     //Clean thread tables
00160     memset(g_palThreads,0,sizeof(palThread_t) * PAL_MAX_NUMBER_OF_THREADS);
00161 
00162     //Add implicit the running task as PAL main
00163     g_palThreads[0].initialized = true;
00164     g_palThreads[0].threadID =  pthread_self();
00165 
00166     pal_osAtomicIncrement((int32_t*)&g_threadCounter,1);
00167     //palThreadID = 24 bits for thread counter + 8 bits for thread index (= 0).
00168     g_palThreads[0].palThreadID = (g_threadCounter << 8 );
00169 
00170     return status;
00171 }
00172 
00173 /*! De-Initialize thread objects.
00174  */
00175 palStatus_t pal_plat_RTOSDestroy (void)
00176 {
00177     return PAL_SUCCESS;
00178 }
00179 
00180 
00181 /*return The RTOS kernel system timer counter, in microseconds
00182  */
00183 
00184 uint64_t pal_plat_osKernelSysTick (void) // optional API - not part of original CMSIS API.
00185 {
00186     /*Using clock_gettime is more accurate, but then we have to convert it to ticks. we are using a tick every 100 nanoseconds*/
00187     struct timespec ts;
00188     uint64_t ticks;
00189     //TODO: error handling
00190     clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
00191 
00192     ticks = (uint64_t) (ts.tv_sec * (uint64_t)TICKS_PER_SECOND
00193             + (ts.tv_nsec / NANOS_PER_TICK));
00194     return ticks;
00195 }
00196 
00197 /* Convert the value from microseconds to kernel sys ticks.
00198  * This is the same as CMSIS macro osKernelSysTickMicroSec.
00199  * since we return microsecods as ticks, just return the value
00200  */
00201 uint64_t pal_plat_osKernelSysTickMicroSec (uint64_t microseconds)
00202 {
00203 
00204     //convert to nanoseconds
00205     return microseconds * TICKS_PER_MICRO;
00206 }
00207 
00208 /*! Get the system tick frequency.
00209  * \return The system tick frequency.
00210  */
00211 inline uint64_t pal_plat_osKernelSysTickFrequency (void)
00212 {
00213     /* since we use clock_gettime, with resolution of 100 nanosecond per tick*/
00214     return TICKS_PER_SECOND;
00215 }
00216 
00217 inline PAL_PRIVATE void setDefaultThreadValues(palThread_t* thread)
00218 {
00219 
00220 #if PAL_UNIQUE_THREAD_PRIORITY
00221     g_palThreadPriorities[thread->priority + PRIORITY_INDEX_OFFSET] = 0;
00222 #endif //PAL_UNIQUE_THREAD_PRIORITY
00223     thread->threadStore = NULL;
00224     thread->threadFuncWrapper.realThreadArgs = NULL;
00225     thread->threadFuncWrapper.realThreadFunc = NULL;
00226     thread->threadFuncWrapper.threadIndex = 0;
00227     thread->priority = PAL_osPriorityError;
00228     thread->stackSize = 0;
00229     thread->threadID = (palThreadID_t)PAL_INVALID_THREAD;
00230     thread->palThreadID = 0;
00231     //! This line should be last thing to be done in this function.
00232     //! in order to prevent double accessing the same index between
00233     //! this function and the threadCreate function.
00234     thread->initialized = false;
00235 }
00236 
00237 /*! Clean thread data from the global thread data base (g_palThreads). Thread Safe API
00238  *
00239  * @param[in] index:    the index in the data base to be cleaned.
00240  */
00241 PAL_PRIVATE void threadCleanUp(uint32_t index)
00242 {
00243     uint32_t status = PAL_SUCCESS;
00244     uint32_t threadIndex = PAL_GET_THREAD_INDEX(index);
00245 
00246     status = pal_osMutexWait(g_palThreadInitMutex, PAL_RTOS_WAIT_FOREVER);
00247 
00248     if (PAL_SUCCESS != status)
00249     {
00250          PAL_LOG(ERR,"thread cleanup: mutex wait failed!\n");
00251     }
00252     else{
00253         if ((threadIndex < PAL_MAX_NUMBER_OF_THREADS) && (g_palThreads[threadIndex].palThreadID == index))
00254         {
00255             setDefaultThreadValues(&g_palThreads[threadIndex]);
00256         }
00257 
00258         status = pal_osMutexRelease(g_palThreadInitMutex);
00259         if (PAL_SUCCESS != status)
00260         {
00261              PAL_LOG(ERR,"thread cleanup: mutex release failed!\n");
00262         }
00263     }
00264     return;
00265 }
00266 
00267 /*! Thread wrapper function, this function will be set as the thread function (for every thread)
00268  *  and it will get as an argument the real data about the thread and call the REAL thread function
00269  *  with the REAL argument. Once the REAL thread function finished, \ref pal_threadClean() will be called.
00270  *
00271  *  @param[in] arg: data structure which contains the real data about the thread.
00272  */
00273 void* threadFunctionWrapper(void* arg)
00274 {
00275 
00276     palThreadFuncWrapper_t* threadWrapper = (palThreadFuncWrapper_t*) arg;
00277 
00278     if ((NULL != threadWrapper) && (NULL != threadWrapper->realThreadFunc))
00279     {
00280         if(g_palThreads[threadWrapper->threadIndex].threadID == NULLPTR)
00281         {
00282             g_palThreads[threadWrapper->threadIndex].threadID =  pthread_self();
00283         }
00284 
00285         threadWrapper->realThreadFunc(threadWrapper->realThreadArgs);
00286         threadCleanUp(g_palThreads[threadWrapper->threadIndex].palThreadID);
00287 
00288     }
00289     return NULL;
00290 }
00291 
00292 /*! Create and start a thread function.
00293  *
00294  * @param[in] function A function pointer to the thread callback function.
00295  * @param[in] funcArgument An argument for the thread function.
00296  * @param[in] priority The priority of the thread.
00297  * @param[in] stackSize The stack size of the thread.
00298  * @param[in] stackPtr A pointer to the thread's stack.
00299  * @param[in] store A pointer to thread's local store, can be NULL.
00300  * @param[out] threadID The created thread ID handle, zero indicates an error.
00301  *
00302  * \return The ID of the created thread, in case of error return zero.
00303  * \note Each thread MUST have a unique priority.
00304  * \note When the priority of the created thread function is higher than the current running thread, the
00305  *      created thread function starts instantly and becomes the new running thread.
00306  */
00307 palStatus_t pal_plat_osThreadCreate (palThreadFuncPtr function,
00308         void* funcArgument, palThreadPriority_t priority, uint32_t stackSize,
00309         uint32_t* stackPtr, palThreadLocalStore_t* store,
00310         palThreadID_t* threadID)
00311 {
00312     uint32_t firstAvailableThreadIndex = PAL_MAX_NUMBER_OF_THREADS;
00313     palStatus_t status = PAL_SUCCESS;
00314     uint32_t localPalThreadID = 0;
00315 
00316 
00317     {
00318         if ((NULL == threadID) || (NULL == function) || (0 == stackSize) || (priority > PAL_osPriorityRealtime))
00319         {
00320             return PAL_ERR_INVALID_ARGUMENT ;
00321         }
00322         status = pal_osMutexWait(g_palThreadInitMutex, PAL_RTOS_WAIT_FOREVER);
00323         if (PAL_SUCCESS == status)
00324         {
00325             for (uint32_t i = 0; i < PAL_MAX_NUMBER_OF_THREADS; ++i)
00326             {
00327                 if ((!g_palThreads[i].initialized))
00328                 {
00329                     g_palThreads[i].initialized = true;
00330                     firstAvailableThreadIndex = i;
00331                     break;
00332                 }
00333             }
00334 
00335             if (firstAvailableThreadIndex >= PAL_MAX_NUMBER_OF_THREADS)
00336             {
00337                 *threadID = PAL_INVALID_THREAD;
00338                 status = PAL_ERR_RTOS_RESOURCE ;
00339                 // release mutex if error.
00340                 pal_osMutexRelease(g_palThreadInitMutex);
00341                 goto finish;
00342             }
00343 
00344             g_palThreads[firstAvailableThreadIndex].threadStore = store;
00345             g_palThreads[firstAvailableThreadIndex].threadFuncWrapper.realThreadArgs =
00346                     funcArgument;
00347             g_palThreads[firstAvailableThreadIndex].threadFuncWrapper.realThreadFunc =
00348                     function;
00349             g_palThreads[firstAvailableThreadIndex].threadFuncWrapper.threadIndex =
00350                     firstAvailableThreadIndex;
00351             g_palThreads[firstAvailableThreadIndex].priority = priority;
00352             g_palThreads[firstAvailableThreadIndex].stackSize = stackSize;
00353             g_palThreads[firstAvailableThreadIndex].palThreadID = ((firstAvailableThreadIndex)+((pal_osAtomicIncrement((int32_t*)&g_threadCounter, 1)) << 8)); //palThreadID = 24 bits for thread counter + lower 8 bits for thread index.
00354             localPalThreadID = g_palThreads[firstAvailableThreadIndex].palThreadID;
00355 
00356             // release mutex before thread creation .
00357             status = pal_osMutexRelease(g_palThreadInitMutex);
00358 
00359             if (PAL_SUCCESS == status)
00360             {
00361                 // prepare thread attributes
00362                 pthread_attr_t attr;
00363                 pthread_attr_init(&attr);
00364 
00365                 int err = pthread_attr_setstacksize(&attr, stackSize);// Replace stack pointer with dynamically  allocated from the OS
00366                 if (0 != err)
00367                 {
00368                     status = PAL_ERR_INVALID_ARGUMENT ;
00369                     goto finish;
00370                 }
00371 
00372                 if (0 != pthread_attr_setschedpolicy(&attr, SCHED_RR))
00373                 {
00374                     status = PAL_ERR_INVALID_ARGUMENT ;
00375                     goto finish;
00376                 }
00377 
00378                 //PTHREAD_CREATE_JOINABLE in Linux save the stack TCB until join is call and detach clean every thing upon exit
00379                 //Because PAL is not forcing the user to call thread cancel the threads are detached
00380                 if (0 != pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED))
00381                 {
00382                     status = PAL_ERR_INVALID_ARGUMENT ;
00383                     goto finish;
00384                 }
00385 
00386                 struct sched_param schedParam;
00387                 schedParam.sched_priority = LINUX_THREAD_PRIORITY_BASE + (int) priority;
00388                 if (0 != pthread_attr_setschedparam(&attr, &schedParam))
00389                 {
00390                     status = PAL_ERR_INVALID_ARGUMENT ;
00391                     goto finish;
00392                 }
00393 
00394                 //create the thread
00395                 pthread_t thread = (pthread_t)NULL;
00396 
00397                 int retVal = pthread_create(&thread, &attr, threadFunctionWrapper, (void*) &(g_palThreads[firstAvailableThreadIndex].threadFuncWrapper));
00398                 pthread_attr_destroy(&attr); //Destroy the thread attributes object, since it is no longer needed
00399                 if (0 != retVal)
00400                 {
00401                     if (EPERM == retVal)
00402                     {
00403                         // cannot set the priority
00404                         status = PAL_ERR_RTOS_PRIORITY ;
00405                     }
00406                     else
00407                     {
00408                         status = PAL_ERR_RTOS_RESOURCE ;
00409                     }
00410                     goto finish;
00411                 }
00412 
00413                 if((thread != (palThreadID_t)PAL_INVALID_THREAD) && (thread != 0))
00414                 {
00415                     // if we managed to do it, set in the the array, set threadID, and return
00416                     g_palThreads[firstAvailableThreadIndex].threadID = (palThreadID_t) thread;
00417                     *threadID = localPalThreadID;
00418                 }
00419             }
00420 
00421         }
00422     }
00423     finish:
00424     if (PAL_SUCCESS != status)
00425     {
00426         if (firstAvailableThreadIndex < PAL_MAX_NUMBER_OF_THREADS)
00427         {
00428             threadCleanUp(localPalThreadID);
00429         }
00430         *threadID = PAL_INVALID_THREAD;
00431     }
00432     return status;
00433 
00434 }
00435 
00436 /*! Terminate and free allocated data for the thread.
00437  *
00438  * @param[in] threadID The ID of the thread to stop and terminate.
00439  *
00440  * \return palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
00441  */
00442 palStatus_t pal_plat_osThreadTerminate (palThreadID_t* threadID)
00443 {
00444     palStatus_t status = PAL_ERR_INVALID_ARGUMENT ;
00445     int statusOS = 0;
00446     uint32_t threadIndex = PAL_GET_THREAD_INDEX(*threadID);
00447 
00448     if ((PAL_INVALID_THREAD == *threadID) || (threadIndex >= PAL_MAX_NUMBER_OF_THREADS))
00449     {
00450         return PAL_ERR_INVALID_ARGUMENT ;
00451     }
00452 
00453     // if thread exited or was terminated already return success.
00454     if ((g_palThreads[threadIndex].palThreadID == 0) ||  // thread already exited
00455         (g_palThreads[threadIndex].palThreadID != *threadID) || // thread already exited and a new thread was created at the same index.
00456         (g_palThreads[threadIndex].threadID == (palThreadID_t)PAL_INVALID_THREAD))  // thread was terminsated.
00457     {
00458         return PAL_SUCCESS;
00459     }
00460 
00461 
00462     if((pthread_self() != g_palThreads[threadIndex].threadID))
00463     {//Kill only if not trying to kill from running task
00464         status = PAL_SUCCESS;
00465         if ((g_palThreads[threadIndex].initialized))
00466         {
00467             statusOS = pthread_cancel(g_palThreads[threadIndex].threadID);
00468             if((statusOS != 0) && (statusOS != ESRCH))
00469             {
00470                 status = PAL_ERR_RTOS_RESOURCE ;
00471             }
00472         }
00473     }
00474 
00475     if(status == PAL_SUCCESS)
00476     {
00477         threadCleanUp(*threadID);
00478         *threadID = PAL_INVALID_THREAD;
00479     }
00480 
00481     return status;
00482 }
00483 
00484 /*! Get the ID of the current thread.
00485  * \return The ID of the current thread, in case of error return PAL_maX_UINT32.
00486  * \note For a thread with real time priority, the function always returns PAL_maX_UINT32.
00487  */
00488 palThreadID_t pal_plat_osThreadGetId ()
00489 {
00490     int i = 0;
00491     pthread_t  osThreadID = pthread_self();
00492     palThreadID_t ret = PAL_INVALID_THREAD;
00493 
00494     for(i= 0; i < PAL_MAX_NUMBER_OF_THREADS; i++)
00495     {
00496         if(osThreadID == g_palThreads[i].threadID)
00497         {
00498             ret = i;
00499             break;
00500         }
00501     }
00502     return ret;
00503 }
00504 
00505 /*! Get the storage of the current thread.
00506  * \return The storage of the current thread.
00507  */
00508 palThreadLocalStore_t* pal_plat_osThreadGetLocalStore (void)
00509 {
00510     palThreadLocalStore_t* localStore = NULL;
00511     palThreadID_t id = pal_osThreadGetId();
00512 
00513     if( g_palThreads[id].initialized)
00514     {
00515         localStore = g_palThreads[id].threadStore;
00516     }
00517     return localStore;
00518 }
00519 
00520 /*! Wait for a specified period of time in milliseconds.
00521  *
00522  * @param[in] milliseconds The number of milliseconds to wait before proceeding.
00523  *
00524  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
00525  */
00526 palStatus_t pal_plat_osDelay (uint32_t milliseconds)
00527 {
00528     struct timespec sTime;
00529     struct timespec rTime; // this will return how much sleep time still left in case of interrupted sleep
00530     int stat;
00531     //init rTime, as we will copy it over to stime inside the do-while loop.
00532     rTime.tv_sec = milliseconds / 1000;
00533     rTime.tv_nsec = PAL_MILLI_TO_NANO(milliseconds);
00534 
00535     do
00536     {
00537         sTime.tv_sec = rTime.tv_sec;
00538         sTime.tv_nsec = rTime.tv_nsec;
00539         stat = nanosleep(&sTime, &rTime);
00540     } while ((-1 == stat) && (EINTR ==errno)) ;
00541     return (stat == 0) ? PAL_SUCCESS : PAL_ERR_GENERIC_FAILURE;
00542 }
00543 
00544 /*
00545  * Internal struct to handle timers.
00546  */
00547 
00548 struct palTimerInfo
00549 {
00550     timer_t handle;
00551     palTimerFuncPtr function;
00552     void *funcArgs;
00553     palTimerType_t timerType;
00554     bool isHighRes;
00555 };
00556 
00557 /*
00558  * internal function used to handle timers expiration events.
00559  */
00560 PAL_PRIVATE void palTimerEventHandler(void* args)
00561 {
00562     struct palTimerInfo* timer = (struct palTimerInfo *) args;
00563 
00564     if (NULL == timer)
00565     { // no timer anymore, so just return.
00566         return;
00567     }
00568 
00569     //call the callback function
00570     timer->function(timer->funcArgs);
00571 }
00572 
00573 
00574 /*
00575 * Internal struct to handle timers.
00576 */
00577 
00578 #define PAL_HIGH_RES_TIMER_THRESHOLD_MS 100
00579 
00580 typedef struct palHighResTimerThreadContext
00581 {
00582     palTimerFuncPtr function;
00583     void *funcArgs;
00584     uint32_t intervalMS;
00585 } palHighResTimerThreadContext_t;
00586 
00587 
00588 static pthread_t s_palHighResTimerThreadID = {0};
00589 static bool s_palHighResTimerThreadInUse =  0;
00590 static palHighResTimerThreadContext_t s_palHighResTimerThreadContext = {0};
00591 
00592 /*
00593 *  callback for handling high precision timer callbacks (currently only one is supported)
00594 */
00595 
00596 PAL_PRIVATE void* palHighResTimerThread(void* args)
00597 {
00598     palHighResTimerThreadContext_t* context = (palHighResTimerThreadContext_t*)args;
00599     uint32_t timer_period_ms = context->intervalMS;
00600     int err = 0;
00601     struct timespec next_timeout_ts;
00602     err = clock_gettime(CLOCK_MONOTONIC, &next_timeout_ts);
00603     assert(err == 0);
00604 
00605     while(1) {
00606         // Determine absolute time we want to sleep until
00607         next_timeout_ts.tv_nsec += PAL_NANO_PER_MILLI * timer_period_ms;
00608         if (next_timeout_ts.tv_nsec >= PAL_NANO_PER_SECOND) 
00609         {
00610             next_timeout_ts.tv_nsec = next_timeout_ts.tv_nsec - PAL_NANO_PER_SECOND;
00611             next_timeout_ts.tv_sec += 1;
00612         }
00613 
00614         // Call nanosleep until error or no interrupt, ie. return code is 0
00615         do {
00616             err = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_timeout_ts, NULL);
00617             assert(err == 0 || err == EINTR);
00618         } while(err == EINTR);
00619 
00620         // Done sleeping, call callback
00621         context->function(context->funcArgs);
00622     }
00623     return NULL;
00624 }
00625 
00626 PAL_PRIVATE palStatus_t startHighResTimerThread(palTimerFuncPtr function, void *funcArgs , uint32_t intervalMS)
00627 {
00628     int retVal = 0;
00629     palStatus_t status = PAL_SUCCESS;
00630     pthread_attr_t attr;
00631     pthread_attr_init(&attr);
00632 
00633     s_palHighResTimerThreadContext.function = function;
00634     s_palHighResTimerThreadContext.funcArgs = funcArgs;
00635     s_palHighResTimerThreadContext.intervalMS = intervalMS;
00636 
00637     retVal = pthread_attr_setstacksize(&attr, PAL_RTOS_HIGH_RES_TIMER_THREAD_STACK_SIZE);//sets the minimum stack size
00638     if (0 != retVal)
00639     {
00640         status = PAL_ERR_INVALID_ARGUMENT ;
00641         goto finish;
00642     }
00643 
00644     if (0 != pthread_attr_setschedpolicy(&attr, SCHED_RR))
00645     {
00646         status = PAL_ERR_INVALID_ARGUMENT ;
00647         goto finish;
00648     }
00649 
00650     //PTHREAD_CREATE_JOINABLE in Linux save the stack TCB until join is call and detach clean every thing upon exit
00651     //Because PAL is not forcing the user to call thread cancel the threads are detached
00652     if (0 != pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED))
00653     {
00654         status = PAL_ERR_INVALID_ARGUMENT ;
00655         goto finish;
00656     }
00657 
00658     struct sched_param schedParam;
00659     schedParam.sched_priority = LINUX_THREAD_PRIORITY_BASE + (int)PAL_osPriorityRealtime;
00660     if (0 != pthread_attr_setschedparam(&attr, &schedParam))
00661     {
00662         status = PAL_ERR_INVALID_ARGUMENT ;
00663         goto finish;
00664     }
00665 
00666    retVal = pthread_create(&s_palHighResTimerThreadID, &attr, &palHighResTimerThread, &s_palHighResTimerThreadContext);
00667     if (0 != retVal)
00668     {
00669         if (EPERM == retVal)
00670         {
00671             // cannot set the priority
00672             status = PAL_ERR_RTOS_PRIORITY ;
00673         }
00674         else
00675         {
00676             status = PAL_ERR_RTOS_RESOURCE ;
00677         }
00678     }
00679 
00680 finish:
00681     pthread_attr_destroy(&attr); //Destroy the thread attributes object, since it is no longer needed
00682 
00683     return status;
00684 
00685 }
00686 
00687 
00688 /*! Create a timer.
00689  *
00690  * @param[in] function A function pointer to the timer callback function.
00691  * @param[in] funcArgument An argument for the timer callback function.
00692  * @param[in] timerType The timer type to be created, periodic or oneShot.
00693  * @param[out] timerID The ID of the created timer, zero value indicates an error.
00694  *
00695  * \return PAL_SUCCESS when the timer was created successfully. A specific error in case of failure.
00696  */
00697 palStatus_t pal_plat_osTimerCreate (palTimerFuncPtr function, void* funcArgument,
00698         palTimerType_t timerType, palTimerID_t* timerID)
00699 {
00700 
00701     palStatus_t status = PAL_SUCCESS;
00702     struct palTimerInfo* timerInfo = NULL;
00703     {
00704         struct sigevent sig;
00705         timer_t localTimer;
00706 
00707         if ((NULL == timerID) || (NULL == (void*) function))
00708         {
00709             return PAL_ERR_INVALID_ARGUMENT ;
00710         }
00711 
00712         timerInfo = (struct palTimerInfo*) malloc(sizeof(struct palTimerInfo));
00713         if (NULL == timerInfo)
00714         {
00715             status = PAL_ERR_NO_MEMORY ;
00716             goto finish;
00717         }
00718 
00719         timerInfo->function = function;
00720         timerInfo->funcArgs = funcArgument;
00721         timerInfo->timerType = timerType;
00722         timerInfo->isHighRes = false;
00723 
00724         memset(&sig, 0, sizeof(sig));
00725 
00726         sig.sigev_notify = SIGEV_THREAD;
00727         sig.sigev_signo = 0;
00728         sig.sigev_value.sival_ptr = timerInfo;
00729         sig.sigev_notify_function = (void (*)(union sigval)) palTimerEventHandler;
00730 
00731         int ret = timer_create(CLOCK_MONOTONIC, &sig, &localTimer);
00732         if (-1 == ret)
00733         {
00734             if (EINVAL == errno)
00735             {
00736                 status = PAL_ERR_INVALID_ARGUMENT ;
00737                 goto finish;
00738             }
00739             if (ENOMEM == errno)
00740             {
00741                 status = PAL_ERR_NO_MEMORY ;
00742                 goto finish;
00743             }
00744             PAL_LOG(ERR, "Rtos timer create error %d", ret);
00745             status = PAL_ERR_GENERIC_FAILURE;
00746             goto finish;
00747         }
00748 
00749         // managed to create the timer - finish up
00750         timerInfo->handle = localTimer;
00751         *timerID = (palTimerID_t) timerInfo;
00752     }
00753     finish: if (PAL_SUCCESS != status)
00754     {
00755         if (NULL != timerInfo)
00756         {
00757             free(timerInfo);
00758             *timerID = (palTimerID_t) NULL;
00759         }
00760     }
00761     return status;
00762 }
00763 
00764 /* Convert milliseconds into seconds and nanoseconds inside a timespec struct
00765  */
00766 PAL_PRIVATE void convertMilli2Timespec(uint32_t millisec, struct timespec* ts)
00767 {
00768     ts->tv_sec = millisec / 1000;
00769     ts->tv_nsec = PAL_MILLI_TO_NANO(millisec);
00770 }
00771 
00772 /*! Start or restart a timer.
00773  *
00774  * @param[in] timerID The handle for the timer to start.
00775  * @param[in] millisec The time in milliseconds to set the timer to.
00776  *
00777  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
00778  */
00779 palStatus_t pal_plat_osTimerStart (palTimerID_t timerID, uint32_t millisec)
00780 {
00781     palStatus_t status = PAL_SUCCESS;
00782     if (NULL == (struct palTimerInfo *) timerID)
00783     {
00784         return PAL_ERR_INVALID_ARGUMENT ;
00785     }
00786 
00787     struct palTimerInfo* timerInfo = (struct palTimerInfo *) timerID;
00788     struct itimerspec its;
00789 
00790 
00791     if ((millisec <= PAL_HIGH_RES_TIMER_THRESHOLD_MS) && (palOsTimerPeriodic  == timerInfo->timerType )) // periodic high res timer  - we only support 1 (workaround for issue when lots of threads are created in linux)
00792     {
00793         if (true == s_palHighResTimerThreadInUse)
00794         {
00795             status = PAL_ERR_NO_HIGH_RES_TIMER_LEFT ;
00796         }
00797         else
00798         {
00799             status = startHighResTimerThread(timerInfo->function, timerInfo->funcArgs, millisec);
00800             if (PAL_SUCCESS == status)
00801             {
00802                 timerInfo->isHighRes = true;
00803         s_palHighResTimerThreadInUse = true;
00804             }
00805 
00806         }
00807 
00808     }
00809     else // otherwise handle normally
00810     {
00811         convertMilli2Timespec(millisec, &(its.it_value));
00812 
00813         if (palOsTimerPeriodic  == timerInfo->timerType)
00814         {
00815             convertMilli2Timespec(millisec, &(its.it_interval));
00816         }
00817         else
00818         {  // one time timer
00819             convertMilli2Timespec(0, &(its.it_interval));
00820         }
00821 
00822         if (-1 == timer_settime(timerInfo->handle, 0, &its, NULL))
00823         {
00824             status = PAL_ERR_INVALID_ARGUMENT ;
00825         }
00826     }
00827 
00828     return status;
00829 }
00830 
00831 /*! Stop a timer.
00832  *
00833  * @param[in] timerID The handle for the timer to stop.
00834  *
00835  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
00836  */
00837 palStatus_t pal_plat_osTimerStop (palTimerID_t timerID)
00838 {
00839     palStatus_t status = PAL_SUCCESS;
00840     if (NULL == (struct palTimerInfo *) timerID)
00841     {
00842         return PAL_ERR_INVALID_ARGUMENT ;
00843     }
00844 
00845     struct palTimerInfo* timerInfo = (struct palTimerInfo *) timerID;
00846     struct itimerspec its;
00847 
00848     if ((true == timerInfo->isHighRes) && (0 != s_palHighResTimerThreadInUse )) // if  high res timer clean up thread.
00849     {
00850         int statusOS = pthread_cancel(s_palHighResTimerThreadID);
00851         if ((statusOS != 0) && (statusOS != ESRCH))
00852         {
00853             return PAL_ERR_RTOS_RESOURCE ;
00854         }
00855         else
00856         {
00857             timerInfo->isHighRes = false;
00858             s_palHighResTimerThreadInUse = false;
00859             return PAL_SUCCESS;
00860         }
00861     }
00862     else // otherwise process normally
00863     {
00864         // set timer to 0 to disarm it.
00865         convertMilli2Timespec(0, &(its.it_value));
00866 
00867         convertMilli2Timespec(0, &(its.it_interval));
00868 
00869         if (-1 == timer_settime(timerInfo->handle, 0, &its, NULL))
00870         {
00871             status = PAL_ERR_INVALID_ARGUMENT ;
00872         }
00873     }
00874 
00875     return status;
00876 }
00877 
00878 /*! Delete the timer object
00879  *
00880  * @param[inout] timerID The handle for the timer to delete. In success, *timerID = NULL.
00881  *
00882  * \return PAL_SUCCESS when the timer was deleted successfully, PAL_ERR_RTOS_PARAMETER when the timerID is incorrect.
00883  */
00884 palStatus_t pal_plat_osTimerDelete (palTimerID_t* timerID)
00885 {
00886     palStatus_t status = PAL_SUCCESS;
00887     if (NULL == timerID)
00888     {
00889         return PAL_ERR_INVALID_ARGUMENT ;
00890     }
00891     struct palTimerInfo* timerInfo = (struct palTimerInfo *) *timerID;
00892     if (NULL == timerInfo)
00893     {
00894         status = PAL_ERR_RTOS_PARAMETER;
00895     }
00896 
00897     if ((true == timerInfo->isHighRes) && (0 != s_palHighResTimerThreadInUse)) //  if high res timer delted before stopping => clean up thread.
00898     {
00899         int statusOS = pthread_cancel(s_palHighResTimerThreadID);
00900         if ((statusOS != 0) && (statusOS != ESRCH))
00901         {
00902             status = PAL_ERR_RTOS_RESOURCE ;
00903         }
00904         else
00905         {
00906             timerInfo->isHighRes = false;
00907             s_palHighResTimerThreadInUse = false;
00908         }
00909     }
00910 
00911     if (PAL_SUCCESS == status)
00912     {
00913         timer_t lt = timerInfo->handle;
00914         if (-1 == timer_delete(lt))
00915         {
00916             status = PAL_ERR_RTOS_RESOURCE ;
00917         }
00918 
00919         free(timerInfo);
00920         *timerID = (palTimerID_t) NULL;
00921     }
00922     return status;
00923 }
00924 
00925 /*! Create and initialize a mutex object.
00926  *
00927  * @param[out] mutexID The created mutex ID handle, zero value indicates an error.
00928  *
00929  * \return PAL_SUCCESS when the mutex was created successfully, a specific error in case of failure.
00930  */
00931 palStatus_t pal_plat_osMutexCreate (palMutexID_t* mutexID)
00932 {
00933     palStatus_t status = PAL_SUCCESS;
00934     pthread_mutex_t* mutex = NULL;
00935     {
00936         int ret;
00937         if (NULL == mutexID)
00938         {
00939             return PAL_ERR_INVALID_ARGUMENT ;
00940         }
00941 
00942         mutex = malloc(sizeof(pthread_mutex_t));
00943         if (NULL == mutex)
00944         {
00945             status = PAL_ERR_NO_MEMORY ;
00946             goto finish;
00947         }
00948 
00949         pthread_mutexattr_t mutexAttr;
00950         pthread_mutexattr_init(&mutexAttr);
00951         pthread_mutexattr_settype(&mutexAttr, PTHREAD_MUTEX_RECURSIVE);
00952         ret = pthread_mutex_init(mutex, &mutexAttr);
00953 
00954         if (0 != ret)
00955         {
00956             if (ENOMEM == ret)
00957             {
00958                 status = PAL_ERR_NO_MEMORY ;
00959             }
00960             else
00961             {
00962                 PAL_LOG(ERR, "Rtos mutex create status %d", ret);
00963                 status = PAL_ERR_GENERIC_FAILURE;
00964             }
00965             goto finish;
00966         }
00967         *mutexID = (palMutexID_t) mutex;
00968     }
00969     finish: if (PAL_SUCCESS != status)
00970     {
00971         if (NULL != mutex)
00972         {
00973             free(mutex);
00974         }
00975     }
00976     return status;
00977 }
00978 
00979 /* Wait until a mutex becomes available.
00980  *
00981  * @param[in] mutexID The handle for the mutex.
00982  * @param[in] millisec The timeout for the waiting operation if the timeout expires before the semaphore is released and an error is returned from the function.
00983  *
00984  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, one of the following error codes in case of failure:
00985  *        PAL_ERR_RTOS_RESOURCE - Mutex not available but no timeout set.
00986  *        PAL_ERR_RTOS_TIMEOUT - Mutex was not available until timeout expired.
00987  *        PAL_ERR_RTOS_PARAMETER - Mutex ID is invalid.
00988  *        PAL_ERR_RTOS_ISR - Cannot be called from interrupt service routines.
00989  */
00990 palStatus_t pal_plat_osMutexWait (palMutexID_t mutexID, uint32_t millisec)
00991 {
00992     palStatus_t status = PAL_SUCCESS;
00993     int err;
00994     if (NULL == ((pthread_mutex_t*) mutexID))
00995     {
00996         return PAL_ERR_INVALID_ARGUMENT ;
00997     }
00998     pthread_mutex_t* mutex = (pthread_mutex_t*) mutexID;
00999 
01000     if (PAL_RTOS_WAIT_FOREVER != millisec)
01001     {
01002         /* calculate the wait absolute time */
01003         struct timespec ts;
01004         clock_gettime(CLOCK_REALTIME, &ts);
01005 
01006         ts.tv_sec += (millisec / PAL_MILLI_PER_SECOND);
01007         ts.tv_nsec += PAL_MILLI_TO_NANO(millisec);
01008         ts.tv_sec += ts.tv_nsec / PAL_NANO_PER_SECOND; // if there is some overflow in the addition of nanoseconds.
01009         ts.tv_nsec = ts.tv_nsec % PAL_NANO_PER_SECOND;
01010 
01011         while ((err = pthread_mutex_timedlock(mutex, &ts)) != 0 && err == EINTR)
01012         {
01013             continue; /* Restart if interrupted by handler */
01014         }
01015     }
01016     else
01017     { // wait for ever
01018         err = pthread_mutex_lock(mutex);
01019     }
01020 
01021     if (0 != err)
01022     {
01023         if (err == ETIMEDOUT)
01024         {
01025             status = PAL_ERR_RTOS_TIMEOUT ;
01026         }
01027         else
01028         {
01029             PAL_LOG(ERR, "Rtos mutex wait status %d", err);
01030             status = PAL_ERR_GENERIC_FAILURE;
01031         }
01032     }
01033 
01034     return status;
01035 }
01036 
01037 /* Release a mutex that was obtained by osMutexWait.
01038  *
01039  * @param[in] mutexID The handle for the mutex.
01040  *
01041  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
01042  */
01043 palStatus_t pal_plat_osMutexRelease (palMutexID_t mutexID)
01044 {
01045     palStatus_t status = PAL_SUCCESS;
01046     int result = 0;
01047 
01048     pthread_mutex_t* mutex = (pthread_mutex_t*) mutexID;
01049     if (NULL == mutex)
01050     {
01051         return PAL_ERR_INVALID_ARGUMENT ;
01052     }
01053 
01054     result = pthread_mutex_unlock(mutex);
01055     if (0 != result)
01056     {
01057         // only reason this might fail - process don't have permission for mutex.
01058         PAL_LOG(ERR, "Rtos mutex release failure - %d",result);
01059         status = PAL_ERR_GENERIC_FAILURE;
01060     }
01061     return status;
01062 }
01063 
01064 /*Delete a mutex object.
01065  *
01066  * @param[inout] mutexID The ID of the mutex to delete. In success, *mutexID = NULL.
01067  *
01068  * \return PAL_SUCCESS when the mutex was deleted successfully, one of the following error codes in case of failure:
01069  *        PAL_ERR_RTOS_RESOURCE - Mutex already released.
01070  *        PAL_ERR_RTOS_PARAMETER - Mutex ID is invalid.
01071  *        PAL_ERR_RTOS_ISR - Cannot be called from interrupt service routines.
01072  * \note After this call, mutex_id is no longer valid and cannot be used.
01073  */
01074 palStatus_t pal_plat_osMutexDelete (palMutexID_t* mutexID)
01075 {
01076     palStatus_t status = PAL_SUCCESS;
01077     uint32_t ret;
01078     if (NULL == mutexID)
01079     {
01080         return PAL_ERR_INVALID_ARGUMENT ;
01081     }
01082     pthread_mutex_t* mutex = (pthread_mutex_t*) *mutexID;
01083 
01084     if (NULL == mutex)
01085     {
01086         status = PAL_ERR_RTOS_RESOURCE ;
01087     }
01088     ret = pthread_mutex_destroy(mutex);
01089     if ((PAL_SUCCESS == status) && (0 != ret))
01090     {
01091         PAL_LOG(ERR,"pal_plat_osMutexDelete 0x%x",ret);
01092         status = PAL_ERR_RTOS_RESOURCE ;
01093     }
01094     if (NULL != mutex)
01095     {
01096         free(mutex);
01097     }
01098 
01099     *mutexID = (palMutexID_t) NULL;
01100     return status;
01101 }
01102 
01103 /* Create and initialize a semaphore object.
01104  *
01105  * Semaphore is shared between threads, but not process.
01106  *
01107  * @param[in] count The number of available resources.
01108  * @param[out] semaphoreID The ID of the created semaphore, zero value indicates an error.
01109  *
01110  * \return PAL_SUCCESS when the semaphore was created successfully, a specific error in case of failure.
01111  */
01112 palStatus_t pal_plat_osSemaphoreCreate (uint32_t count,
01113         palSemaphoreID_t* semaphoreID)
01114 {
01115     palStatus_t status = PAL_SUCCESS;
01116     sem_t* semaphore = NULL;
01117 
01118     {
01119         if (NULL == semaphoreID)
01120         {
01121             return PAL_ERR_INVALID_ARGUMENT ;
01122         }
01123         semaphore = malloc(sizeof(sem_t));
01124         if (NULL == semaphore)
01125         {
01126             status = PAL_ERR_NO_MEMORY ;
01127             goto finish;
01128         }
01129         /* create the semaphore as shared between threads */
01130         int ret = sem_init(semaphore, 0, count);
01131         if (-1 == ret)
01132         {
01133             if (EINVAL == errno)
01134             {
01135                 /* count is too big */
01136                 status = PAL_ERR_INVALID_ARGUMENT ;
01137             }
01138             else
01139             {
01140                 PAL_LOG(ERR, "Rtos semaphore init error %d", ret);
01141                 status = PAL_ERR_GENERIC_FAILURE;
01142             }
01143             goto finish;
01144         }
01145 
01146         *semaphoreID = (palSemaphoreID_t) semaphore;
01147     }
01148     finish: if (PAL_SUCCESS != status)
01149     {
01150         if (NULL != semaphore)
01151         {
01152             free(semaphore);
01153         }
01154         *semaphoreID = (palSemaphoreID_t) NULL;
01155     }
01156     return status;
01157 }
01158 
01159 /* Wait until a semaphore token becomes available.
01160  *
01161  * @param[in] semaphoreID The handle for the semaphore.
01162  * @param[in] millisec The timeout for the waiting operation if the timeout expires before the semaphore is released and an error is returned from the function.
01163  * @param[out] countersAvailable The number of semaphores available (before the wait), if semaphores are not available (timeout/error) zero is returned.
01164  *
01165  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, one of the following error codes in case of failure:
01166  *      PAL_ERR_RTOS_TIMEOUT - Semaphore was not available until timeout expired.
01167  *      PAL_ERR_RTOS_PARAMETER - Semaphore ID is invalid.
01168  *      PAL_ERR_INVALID_ARGUMENT - countersAvailable is NULL
01169  *
01170  *      NOTES: 1. counterAvailable returns 0 in case there are no semaphores available or there are other threads waiting on it.
01171  *                Value is not thread safe - it might be changed by the time it is read/returned.
01172  *             2. timed wait is using absolute time.
01173  */
01174 palStatus_t pal_plat_osSemaphoreWait (palSemaphoreID_t semaphoreID,
01175         uint32_t millisec, int32_t* countersAvailable)
01176 {
01177     palStatus_t status = PAL_SUCCESS;
01178     int tmpCounters = 0;
01179     {
01180         int err;
01181         sem_t* sem = (sem_t*) semaphoreID;
01182         if ((NULL == sem))
01183         {
01184             return PAL_ERR_INVALID_ARGUMENT ;
01185         }
01186 
01187         if (PAL_RTOS_WAIT_FOREVER != millisec)
01188         {
01189             /* calculate the wait absolute time */
01190             struct timespec ts;
01191             clock_gettime(CLOCK_REALTIME, &ts);
01192             ts.tv_sec += millisec / PAL_MILLI_PER_SECOND;
01193             ts.tv_nsec += PAL_MILLI_TO_NANO(millisec);
01194             ts.tv_sec += ts.tv_nsec / PAL_NANO_PER_SECOND; // in case there is overflow in the nanoseconds.
01195             ts.tv_nsec = ts.tv_nsec % PAL_NANO_PER_SECOND;
01196 
01197             while ((err = sem_timedwait(sem, &ts)) == -1 && errno == EINTR)
01198                 continue; /* Restart if interrupted by handler */
01199         }
01200         else
01201         { // wait for ever
01202             do
01203             {
01204                 err = sem_wait(sem);
01205 
01206                 /* loop again if the wait was interrupted by a signal */
01207             } while ((err == -1) && (errno == EINTR));
01208         }
01209 
01210         if (-1 == err)
01211         {
01212             tmpCounters = 0;
01213             if (errno == ETIMEDOUT)
01214             {
01215                 status = PAL_ERR_RTOS_TIMEOUT ;
01216             }
01217             else
01218             { /* seems this is not a valid semaphore */
01219                 status = PAL_ERR_RTOS_PARAMETER;
01220             }
01221             goto finish;
01222         }
01223         /* get the counter number, shouldn't fail, as we already know this is valid semaphore */
01224         sem_getvalue(sem, &tmpCounters);
01225     }
01226     finish:
01227     if (NULL != countersAvailable)
01228     {
01229         *countersAvailable = tmpCounters;
01230     }
01231     return status;
01232 }
01233 
01234 /*! Release a semaphore token.
01235  *
01236  * @param[in] semaphoreID The handle for the semaphore.
01237  *
01238  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
01239  */
01240 palStatus_t pal_plat_osSemaphoreRelease (palSemaphoreID_t semaphoreID)
01241 {
01242     palStatus_t status = PAL_SUCCESS;
01243     sem_t* sem = (sem_t*) semaphoreID;
01244 
01245     if (NULL == sem)
01246     {
01247         return PAL_ERR_INVALID_ARGUMENT ;
01248     }
01249 
01250     if (-1 == sem_post(sem))
01251     {
01252         if (EINVAL == errno)
01253         {
01254             status = PAL_ERR_RTOS_PARAMETER;
01255         }
01256         else
01257         { /* max value of semaphore exeeded */
01258             PAL_LOG(ERR, "Rtos semaphore release error %d", errno);
01259             status = PAL_ERR_GENERIC_FAILURE;
01260         }
01261     }
01262 
01263     return status;
01264 }
01265 
01266 /*! Delete a semaphore object.
01267  *
01268  * @param[inout] semaphoreID: The ID of the semaphore to delete. In success, *semaphoreID = NULL.
01269  *
01270  * \return PAL_SUCCESS when the semaphore was deleted successfully, one of the following error codes in case of failure:
01271  *        PAL_ERR_RTOS_RESOURCE - Semaphore already released.
01272  *        PAL_ERR_RTOS_PARAMETER - Semaphore ID is invalid.
01273  * \note After this call, the semaphore_id is no longer valid and cannot be used.
01274  */
01275 palStatus_t pal_plat_osSemaphoreDelete (palSemaphoreID_t* semaphoreID)
01276 {
01277     palStatus_t status = PAL_SUCCESS;
01278     {
01279         if (NULL == semaphoreID)
01280         {
01281             return PAL_ERR_INVALID_ARGUMENT ;
01282         }
01283 
01284         sem_t* sem = (sem_t*) (*semaphoreID);
01285         if (NULL == sem)
01286         {
01287             status = PAL_ERR_RTOS_RESOURCE ;
01288             goto finish;
01289         }
01290         if (-1 == sem_destroy(sem))
01291         {
01292             status = PAL_ERR_RTOS_PARAMETER;
01293             goto finish;
01294         }
01295 
01296         if (NULL != sem)
01297         {
01298             free(sem);
01299         }
01300         *semaphoreID = (palSemaphoreID_t) NULL;
01301     }
01302     finish: return status;
01303 }
01304 
01305 typedef struct palMemoryPool
01306 {
01307     void *start;
01308     uint32_t blockCount;
01309     uint32_t blockSize;
01310     bool* allocated;
01311 } palMemoryPool_t;
01312 
01313 /*! Create and initialize a memory pool.
01314  *
01315  * @param[in] blockSize The size of a single block in bytes.
01316  * @param[in] blockCount The maximum number of blocks in the memory pool.
01317  * @param[out] memoryPoolID The ID of the created memory pool, zero value indicates an error.
01318  *
01319  * \return PAL_SUCCESS when the memory pool was created successfully, a specific error in case of failure.
01320  */
01321 palStatus_t pal_plat_osPoolCreate (uint32_t blockSize, uint32_t blockCount, palMemoryPoolID_t* memoryPoolID)
01322 {
01323     palStatus_t status = PAL_SUCCESS;
01324     palMemoryPool_t* mp = NULL;
01325     {
01326 
01327         if ((NULL == memoryPoolID) || (0 == blockSize) || (0 == blockCount))
01328         {
01329             return PAL_ERR_INVALID_ARGUMENT ;
01330         }
01331 
01332         mp = (palMemoryPool_t*) malloc(sizeof(palMemoryPool_t));
01333         if (NULL == mp)
01334         {
01335             status = PAL_ERR_RTOS_NO_MEMORY ;
01336             goto finish;
01337         }
01338 
01339         mp->blockCount = blockCount;
01340         mp->blockSize = blockSize;
01341         mp->allocated = NULL;
01342         mp->start = malloc((size_t)blockCount * blockSize);
01343         if (NULL == mp->start)
01344         {
01345             status = PAL_ERR_RTOS_NO_MEMORY ;
01346             goto finish;
01347         }
01348 
01349         mp->allocated = (bool*) malloc(blockCount * sizeof(bool));
01350         if (NULL == mp->allocated)
01351         {
01352             status = PAL_ERR_RTOS_NO_MEMORY ;
01353             goto finish;
01354         }
01355         for (uint32_t i = 0; i < blockCount; i++)
01356         {
01357             mp->allocated[i] = false;
01358         }
01359 
01360         *memoryPoolID = (palMemoryPoolID_t) mp;
01361     }
01362 
01363     finish:
01364 
01365     if (PAL_SUCCESS != status)
01366     {
01367         if (NULL != mp)
01368         {
01369             if (NULL != mp->start)
01370             {
01371                 free(mp->start);
01372             }
01373             if (NULL != mp->allocated) // in current code - we are not supposed to be here.
01374             {
01375                 free(mp->allocated);
01376             }
01377             free(mp);
01378         }
01379     }
01380 
01381     return status;
01382 }
01383 
01384 PAL_PRIVATE inline void* poolAlloc(palMemoryPoolID_t memoryPoolID, bool zero)
01385 {
01386     if (NULL == (palMemoryPool_t*) memoryPoolID)
01387     {
01388         return NULL;
01389     }
01390 
01391     palMemoryPool_t* mp = (palMemoryPool_t*) memoryPoolID;
01392 
01393     for (uint32_t i = 0; i < mp->blockCount; i++)
01394     {
01395         if (mp->allocated[i] == false)
01396         {
01397             mp->allocated[i] = true;
01398             void* block = (mp->start + i * mp->blockSize);
01399             if (zero == true)
01400             {
01401                 memset(block, 0, mp->blockSize);
01402             }
01403             return block;
01404         }
01405     }
01406     //we didn't find any
01407     return NULL;
01408 }
01409 /*! Allocate a single memory block from a memory pool.
01410  *
01411  * @param[in] memoryPoolID The handle for the memory pool.
01412  *
01413  * \return A pointer to a single allocated memory from the pool, NULL in case of failure.
01414  */
01415 void* pal_plat_osPoolAlloc (palMemoryPoolID_t memoryPoolID)
01416 {
01417     return poolAlloc(memoryPoolID, false);
01418 }
01419 
01420 /*! Allocate a single memory block from a memory pool and set memory block to zero.
01421  *
01422  * @param[in] memoryPoolID The handle for the memory pool.
01423  *
01424  * \return A pointer to a single allocated memory from the pool, NULL in case of failure.
01425  */
01426 void* pal_plat_osPoolCAlloc (palMemoryPoolID_t memoryPoolID)
01427 {
01428     return poolAlloc(memoryPoolID, true);
01429 }
01430 
01431 /*! Return the memoryPoolID of the memory block back to a specific memory pool.
01432  *
01433  * @param[in] memoryPoolID The handle for the memory pool.
01434  * @param[in] block The block to be freed.
01435  *
01436  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
01437  */
01438 palStatus_t pal_plat_osPoolFree (palMemoryPoolID_t memoryPoolID, void* block)
01439 {
01440     palMemoryPool_t* mp = (palMemoryPool_t*) memoryPoolID;
01441     if ((NULL == (palMemoryPool_t*) memoryPoolID) || (NULL == block)
01442             || (mp->start > block)
01443             || ((mp->start + mp->blockCount * mp->blockSize) < block)
01444             || (0 != ((block - mp->start) % mp->blockSize)))
01445     {
01446         return PAL_ERR_INVALID_ARGUMENT ;
01447     }
01448 
01449     int index = (block - mp->start) / mp->blockSize;
01450     mp->allocated[index] = false;
01451     return PAL_SUCCESS;
01452 }
01453 
01454 /*! Delete a memory pool object.
01455  *
01456  * @param[inout] memoryPoolID The handle for the memory pool. In success, *memoryPoolID = NULL.
01457  *
01458  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
01459  */
01460 palStatus_t pal_plat_osPoolDestroy (palMemoryPoolID_t* memoryPoolID)
01461 {
01462     if (NULL == memoryPoolID)
01463     {
01464         return PAL_ERR_INVALID_ARGUMENT ;
01465     }
01466     palMemoryPool_t* mp = (palMemoryPool_t*) *memoryPoolID;
01467     *memoryPoolID = (palMemoryPoolID_t) NULL; // don't let anyone use it anymore.
01468     free(mp->start);
01469     free(mp->allocated);
01470     free(mp);
01471     return PAL_SUCCESS;
01472 }
01473 
01474 typedef struct palMessageQ
01475 {
01476     mqd_t handle;
01477     char name[MQ_FILENAME_LEN];
01478 } palMessageQ_t;
01479 
01480 /*! Create and initialize a message queue.
01481  *
01482  * @param[in] messageQSize The size of the message queue.
01483  * @param[out] messageQID The ID of the created message queue, zero value indicates an error.
01484  *
01485  * \return PAL_SUCCESS when the message queue was created successfully, a specific error in case of failure.
01486  */
01487 palStatus_t pal_plat_osMessageQueueCreate (uint32_t messageQSize, palMessageQID_t* messageQID)
01488 {
01489     palStatus_t status = PAL_SUCCESS;
01490     palMessageQ_t *mq_h = NULL;
01491     {
01492         if (NULL == messageQID)
01493         {
01494             return PAL_ERR_INVALID_ARGUMENT ;
01495         }
01496         mq_h = malloc(sizeof(palMessageQ_t));
01497         if (NULL == mq_h)
01498         {
01499             status = PAL_ERR_NO_MEMORY ;
01500             goto finish;
01501         }
01502         // copy the name to be used, and advance it.
01503         // Note - not thread safe!!
01504         strncpy(mq_h->name, g_mqName, MQ_FILENAME_LEN-1);
01505         mq_h->name[MQ_FILENAME_LEN-1] = '\0';
01506         nextMessageQName();
01507 
01508         // set the attributes for the queue:
01509         struct mq_attr mqAttr;
01510         mqAttr.mq_flags = O_RDWR | O_CREAT | O_EXCL; // if the file for the messageQueue exists - we will fail.
01511         mqAttr.mq_maxmsg = messageQSize;
01512         mqAttr.mq_msgsize = sizeof(uint32_t);
01513         mqAttr.mq_curmsgs = 0;
01514         // create the message Queue. make sure no such filename exists. open with read/write/execute
01515         // for user & group.
01516 
01517         mq_h->handle = mq_open(mq_h->name, O_RDWR | O_CREAT | O_EXCL,
01518         S_IRWXU | S_IRWXG, &mqAttr);
01519         if (-1 == mq_h->handle)
01520         {
01521             status = PAL_ERR_CREATION_FAILED ;
01522             goto finish;
01523         }
01524 
01525         *messageQID = (palMessageQID_t) mq_h;
01526     }
01527     finish:
01528     if (PAL_SUCCESS != status)
01529     {
01530         if (NULL != mq_h)
01531         {
01532             free(mq_h);
01533         }
01534         *messageQID = 0;
01535     }
01536     return status;
01537 }
01538 
01539 /*! Put a message to a queue.
01540  *
01541  * @param[in] messageQID The handle for the message queue.
01542  * @param[in] info The data to send.
01543  * @param[in] timeout The timeout in milliseconds.
01544  *
01545  * All messages has the same priority (set as 0).
01546  *
01547  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
01548  */
01549 palStatus_t pal_plat_osMessagePut (palMessageQID_t messageQID, uint32_t info,
01550         uint32_t timeout)
01551 {
01552     palStatus_t status = PAL_SUCCESS;
01553     int stat;
01554 
01555     palMessageQ_t* mq = (palMessageQ_t*) messageQID;
01556     if (NULL == mq)
01557     {
01558         return PAL_ERR_INVALID_ARGUMENT ;
01559     }
01560 
01561     if (PAL_RTOS_WAIT_FOREVER != timeout)
01562     {
01563         /* calculate the wait absolute time */
01564         struct timespec ts;
01565         clock_gettime(CLOCK_REALTIME, &ts);
01566 
01567         ts.tv_sec += timeout / PAL_MILLI_PER_SECOND;
01568         ts.tv_nsec += PAL_MILLI_TO_NANO(timeout);
01569         //in case there is overflow in the nanoseconds.
01570         ts.tv_sec += ts.tv_nsec / PAL_NANO_PER_SECOND;
01571         ts.tv_nsec = ts.tv_nsec % PAL_NANO_PER_SECOND;
01572 
01573         while ((-1
01574                 == (stat = mq_timedsend(mq->handle, (const char*) &info,
01575                         sizeof(uint32_t), 0, &ts))) && (EINTR == errno))
01576         {
01577             continue; /* Restart if interrupted by handler */
01578         }
01579     }
01580     else
01581     { // wait for ever
01582         stat = mq_send(mq->handle, (const char*) &info, sizeof(uint32_t), 0);
01583     }
01584 
01585     if (-1 == stat)
01586     {
01587         if (EBADF == errno)
01588         {
01589             status = PAL_ERR_INVALID_ARGUMENT ;
01590         }
01591         else if (ETIMEDOUT == errno)
01592         {
01593             status = PAL_ERR_RTOS_TIMEOUT ;
01594         }
01595         else
01596         {
01597             // all other cases - return generic error.
01598             PAL_LOG(ERR, "Rtos put message status %d", stat);
01599             status = PAL_ERR_GENERIC_FAILURE;
01600         }
01601     }
01602     return status;
01603 }
01604 
01605 /*! Get a message or wait for a message from a queue.
01606  *
01607  * @param[in] messageQID The handle for the message queue.
01608  * @param[in] timeout The timeout in milliseconds.
01609  * @param[out] messageValue The data to send.
01610  *
01611  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, one of the following error codes in case of failure:
01612  * PAL_ERR_RTOS_TIMEOUT -  No message arrived during the timeout period.
01613  * PAL_ERR_RTOS_RESOURCE -  No message received and there was no timeout.
01614  */
01615 palStatus_t pal_plat_osMessageGet (palMessageQID_t messageQID, uint32_t timeout,
01616         uint32_t* messageValue)
01617 {
01618     palStatus_t status = PAL_SUCCESS;
01619     int stat;
01620 
01621     palMessageQ_t* mq = (palMessageQ_t*) messageQID;
01622     if ((NULL == mq) || (NULL == messageValue))
01623     {
01624         return PAL_ERR_INVALID_ARGUMENT ;
01625     }
01626 
01627     if (PAL_RTOS_WAIT_FOREVER != timeout)
01628     {
01629         /* calculate the wait absolute time */
01630         struct timespec ts;
01631         clock_gettime(CLOCK_REALTIME, &ts);
01632 
01633         ts.tv_sec += timeout / PAL_MILLI_PER_SECOND;
01634         ts.tv_nsec += PAL_MILLI_TO_NANO(timeout);
01635         // in case there is an overflow in the nanoseconds
01636         ts.tv_sec += ts.tv_nsec / PAL_NANO_PER_SECOND;
01637         ts.tv_nsec = ts.tv_nsec % PAL_NANO_PER_SECOND;
01638 
01639         while ((-1
01640                 == (stat = mq_timedreceive(mq->handle, (char*) messageValue,
01641                         sizeof(uint32_t), 0, &ts))) && (EINTR == errno))
01642         {
01643             continue; /* Restart if interrupted by handler */
01644         }
01645     }
01646     else
01647     { // wait for ever
01648         stat = mq_receive(mq->handle, (char*) messageValue, sizeof(uint32_t), 0);
01649     }
01650 
01651     if (-1 == stat)
01652     {
01653         if (EBADF == errno)
01654         {
01655             status = PAL_ERR_INVALID_ARGUMENT ;
01656         }
01657         else if (ETIMEDOUT == errno)
01658         {
01659             status = PAL_ERR_RTOS_TIMEOUT ;
01660         }
01661         else
01662         {
01663             // all other cases - return resource error.
01664             status = PAL_ERR_RTOS_RESOURCE ;
01665         }
01666     }
01667 
01668     return status;
01669 }
01670 
01671 /*! Delete a message queue object.
01672  *
01673  * @param[inout] messageQID The handle for the message queue. In success, *messageQID = NULL.
01674  *
01675  * \return The status in the form of palStatus_t; PAL_SUCCESS(0) in case of success, a negative value indicating a specific error code in case of failure.
01676  */
01677 palStatus_t pal_plat_osMessageQueueDestroy (palMessageQID_t* messageQID)
01678 {
01679     palStatus_t status = PAL_SUCCESS;
01680     palMessageQ_t *mq = NULL;
01681     {
01682         if (NULL == messageQID)
01683         {
01684             return PAL_ERR_INVALID_ARGUMENT ;
01685         }
01686         //close the queue
01687         mq = (palMessageQ_t *) *messageQID;
01688         if (-1 == mq_close(mq->handle))
01689         {
01690             status = PAL_ERR_INVALID_ARGUMENT ;
01691             goto finish;
01692         }
01693 
01694         //unlink the file:
01695         if (-1 == mq_unlink(mq->name))
01696         {
01697             status = PAL_ERR_RTOS_RESOURCE ;
01698             goto finish;
01699         }
01700     }
01701     finish:
01702     free(mq);
01703     *messageQID = (palMessageQID_t) NULL;
01704     return status;
01705 }
01706 
01707 /*! Perform an atomic increment for a signed32 bit value.
01708  *
01709  * @param[in,out] valuePtr The address of the value to increment.
01710  * @param[in] increment The number by which to increment.
01711  *
01712  * \returns The value of the valuePtr after the increment operation.
01713  */
01714 int32_t pal_plat_osAtomicIncrement (int32_t* valuePtr, int32_t increment)
01715 {
01716     int32_t res = __sync_add_and_fetch(valuePtr, increment);
01717     return res;
01718 }
01719 
01720 
01721 void *pal_plat_malloc (size_t len)
01722 {
01723     return malloc(len);
01724 }
01725 
01726 
01727 void pal_plat_free (void * buffer)
01728 {
01729     return free(buffer);
01730 }
01731 
01732 palStatus_t pal_plat_osRandomBuffer (uint8_t *randomBuf, size_t bufSizeBytes)
01733 {
01734     palStatus_t status = PAL_SUCCESS;
01735 
01736     status = pal_plat_getRandomBufferFromHW(randomBuf, bufSizeBytes);
01737     return status;
01738 }
01739