begriffs open source - cmsis-freertos/blob - CMSIS/RTOS2/FreeRTOS/Source/cmsis_os2.c

   1 /* --------------------------------------------------------------------------
   2  * Copyright (c) 2013-2023 Arm Limited. All rights reserved.
   3  *
   4  * SPDX-License-Identifier: Apache-2.0
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the License); you may
   7  * not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
  14  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  *      Name:    cmsis_os2.c
  19  *      Purpose: CMSIS RTOS2 wrapper for FreeRTOS
  20  *
  21  *---------------------------------------------------------------------------*/
  22
  23 #include <string.h>
  24
  25 #include "cmsis_os2.h"                  // ::CMSIS:RTOS2
  26 #include "cmsis_compiler.h"             // Compiler agnostic definitions
  27 #include "os_tick.h"                    // OS Tick API
  28
  29 #include "FreeRTOS.h"                   // ARM.FreeRTOS::RTOS:Core
  30 #include "task.h"                       // ARM.FreeRTOS::RTOS:Core
  31 #include "event_groups.h"               // ARM.FreeRTOS::RTOS:Event Groups
  32 #include "semphr.h"                     // ARM.FreeRTOS::RTOS:Core
  33 #include "timers.h"                     // ARM.FreeRTOS::RTOS:Timers
  34
  35 #include "freertos_mpool.h"             // osMemoryPool definitions
  36 #include "freertos_os2.h"               // Configuration check and setup
  37
  38 /*---------------------------------------------------------------------------*/
  39 #ifndef __ARM_ARCH_6M__
  40   #define __ARM_ARCH_6M__         0
  41 #endif
  42 #ifndef __ARM_ARCH_7M__
  43   #define __ARM_ARCH_7M__         0
  44 #endif
  45 #ifndef __ARM_ARCH_7EM__
  46   #define __ARM_ARCH_7EM__        0
  47 #endif
  48 #ifndef __ARM_ARCH_8M_MAIN__
  49   #define __ARM_ARCH_8M_MAIN__    0
  50 #endif
  51 #ifndef __ARM_ARCH_7A__
  52   #define __ARM_ARCH_7A__         0
  53 #endif
  54
  55 #if   ((__ARM_ARCH_7M__      == 1U) || \
  56        (__ARM_ARCH_7EM__     == 1U) || \
  57        (__ARM_ARCH_8M_MAIN__ == 1U))
  58 #define IS_IRQ_MASKED()           ((__get_PRIMASK() != 0U) || (__get_BASEPRI() != 0U))
  59 #elif  (__ARM_ARCH_6M__      == 1U)
  60 #define IS_IRQ_MASKED()           (__get_PRIMASK() != 0U)
  61 #elif (__ARM_ARCH_7A__       == 1U)
  62 /* CPSR mask bits */
  63 #define CPSR_MASKBIT_I            0x80U
  64
  65 #define IS_IRQ_MASKED()           ((__get_CPSR() & CPSR_MASKBIT_I) != 0U)
  66 #else
  67 #define IS_IRQ_MASKED()           (__get_PRIMASK() != 0U)
  68 #endif
  69
  70 #if    (__ARM_ARCH_7A__      == 1U)
  71 /* CPSR mode bitmasks */
  72 #define CPSR_MODE_USER            0x10U
  73 #define CPSR_MODE_SYSTEM          0x1FU
  74
  75 #define IS_IRQ_MODE()             ((__get_mode() != CPSR_MODE_USER) && (__get_mode() != CPSR_MODE_SYSTEM))
  76 #else
  77 #define IS_IRQ_MODE()             (__get_IPSR() != 0U)
  78 #endif
  79
  80 /* Limits */
  81 #define MAX_BITS_TASK_NOTIFY      31U
  82 #define MAX_BITS_EVENT_GROUPS     24U
  83
  84 #define THREAD_FLAGS_INVALID_BITS (~((1UL << MAX_BITS_TASK_NOTIFY)  - 1U))
  85 #define EVENT_FLAGS_INVALID_BITS  (~((1UL << MAX_BITS_EVENT_GROUPS) - 1U))
  86
  87 /* Kernel version and identification string definition (major.minor.rev: mmnnnrrrr dec) */
  88 #define KERNEL_VERSION            (((uint32_t)tskKERNEL_VERSION_MAJOR * 10000000UL) | \
  89                                    ((uint32_t)tskKERNEL_VERSION_MINOR *    10000UL) | \
  90                                    ((uint32_t)tskKERNEL_VERSION_BUILD *        1UL))
  91
  92 #define KERNEL_ID                 ("FreeRTOS " tskKERNEL_VERSION_NUMBER)
  93
  94 /* Timer callback information structure definition */
  95 typedef struct {
  96   osTimerFunc_t func;
  97   void         *arg;
  98 } TimerCallback_t;
  99
 100 /* Kernel initialization state */
 101 static osKernelState_t KernelState = osKernelInactive;
 102
 103 /*
 104   Heap region definition used by heap_5 variant
 105
 106   Define configAPPLICATION_ALLOCATED_HEAP as nonzero value in FreeRTOSConfig.h if
 107   heap regions are already defined and vPortDefineHeapRegions is called in application.
 108
 109   Otherwise vPortDefineHeapRegions will be called by osKernelInitialize using
 110   definition configHEAP_5_REGIONS as parameter. Overriding configHEAP_5_REGIONS
 111   is possible by defining it globally or in FreeRTOSConfig.h.
 112 */
 113 #if defined(USE_FreeRTOS_HEAP_5)
 114 #if (configAPPLICATION_ALLOCATED_HEAP == 0)
 115   /*
 116     FreeRTOS heap is not defined by the application.
 117     Single region of size configTOTAL_HEAP_SIZE (defined in FreeRTOSConfig.h)
 118     is provided by default. Define configHEAP_5_REGIONS to provide custom
 119     HeapRegion_t array.
 120   */
 121   #define HEAP_5_REGION_SETUP   1
 122
 123   #ifndef configHEAP_5_REGIONS
 124     #define configHEAP_5_REGIONS xHeapRegions
 125
 126     static uint8_t ucHeap[configTOTAL_HEAP_SIZE];
 127
 128     static HeapRegion_t xHeapRegions[] = {
 129       { ucHeap, configTOTAL_HEAP_SIZE },
 130       { NULL,   0                     }
 131     };
 132   #else
 133     /* Global definition is provided to override default heap array */
 134     extern HeapRegion_t configHEAP_5_REGIONS[];
 135   #endif
 136 #else
 137   /*
 138     The application already defined the array used for the FreeRTOS heap and
 139     called vPortDefineHeapRegions to initialize heap.
 140   */
 141   #define HEAP_5_REGION_SETUP   0
 142 #endif /* configAPPLICATION_ALLOCATED_HEAP */
 143 #endif /* USE_FreeRTOS_HEAP_5 */
 144
 145 #if defined(SysTick)
 146 #undef SysTick_Handler
 147
 148 /* CMSIS SysTick interrupt handler prototype */
 149 extern void SysTick_Handler     (void);
 150 /* FreeRTOS tick timer interrupt handler prototype */
 151 extern void xPortSysTickHandler (void);
 152
 153 /*
 154   SysTick handler implementation that also clears overflow flag.
 155 */
 156 void SysTick_Handler (void) {
 157 #if (configUSE_TICKLESS_IDLE == 0)
 158   /* Clear overflow flag */
 159   SysTick->CTRL;
 160 #endif
 161
 162   if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
 163     /* Call tick handler */
 164     xPortSysTickHandler();
 165   }
 166 }
 167 #endif /* SysTick */
 168
 169 /*
 170   Setup SVC to reset value.
 171 */
 172 __STATIC_INLINE void SVC_Setup (void) {
 173 #if (__ARM_ARCH_7A__ == 0U)
 174   /* Service Call interrupt might be configured before kernel start      */
 175   /* and when its priority is lower or equal to BASEPRI, svc instruction */
 176   /* causes a Hard Fault.                                                */
 177   NVIC_SetPriority (SVCall_IRQn, 0U);
 178 #endif
 179 }
 180
 181 /*
 182   Function macro used to retrieve semaphore count from ISR
 183 */
 184 #ifndef uxSemaphoreGetCountFromISR
 185 #define uxSemaphoreGetCountFromISR( xSemaphore ) uxQueueMessagesWaitingFromISR( ( QueueHandle_t ) ( xSemaphore ) )
 186 #endif
 187
 188 /*
 189   Determine if CPU executes from interrupt context or if interrupts are masked.
 190 */
 191 __STATIC_INLINE uint32_t IRQ_Context (void) {
 192   uint32_t irq;
 193   BaseType_t state;
 194
 195   irq = 0U;
 196
 197   if (IS_IRQ_MODE()) {
 198     /* Called from interrupt context */
 199     irq = 1U;
 200   }
 201   else {
 202     /* Get FreeRTOS scheduler state */
 203     state = xTaskGetSchedulerState();
 204
 205     if (state != taskSCHEDULER_NOT_STARTED) {
 206       /* Scheduler was started */
 207       if (IS_IRQ_MASKED()) {
 208         /* Interrupts are masked */
 209         irq = 1U;
 210       }
 211     }
 212   }
 213
 214   /* Return context, 0: thread context, 1: IRQ context */
 215   return (irq);
 216 }
 217
 218
 219 /* ==== Kernel Management Functions ==== */
 220
 221 /*
 222   Initialize the RTOS Kernel.
 223 */
 224 osStatus_t osKernelInitialize (void) {
 225   osStatus_t stat;
 226   BaseType_t state;
 227
 228   if (IRQ_Context() != 0U) {
 229     stat = osErrorISR;
 230   }
 231   else {
 232     state = xTaskGetSchedulerState();
 233
 234     /* Initialize if scheduler not started and not initialized before */
 235     if ((state == taskSCHEDULER_NOT_STARTED) && (KernelState == osKernelInactive)) {
 236       #if defined(USE_TRACE_EVENT_RECORDER)
 237         /* Initialize the trace macro debugging output channel */
 238         EvrFreeRTOSSetup(0U);
 239       #endif
 240       #if defined(USE_FreeRTOS_HEAP_5) && (HEAP_5_REGION_SETUP == 1)
 241         /* Initialize the memory regions when using heap_5 variant */
 242         vPortDefineHeapRegions (configHEAP_5_REGIONS);
 243       #endif
 244       KernelState = osKernelReady;
 245       stat = osOK;
 246     } else {
 247       stat = osError;
 248     }
 249   }
 250
 251   /* Return execution status */
 252   return (stat);
 253 }
 254
 255 /*
 256   Get RTOS Kernel Information.
 257 */
 258 osStatus_t osKernelGetInfo (osVersion_t *version, char *id_buf, uint32_t id_size) {
 259
 260   if (version != NULL) {
 261     /* Version encoding is major.minor.rev: mmnnnrrrr dec */
 262     version->api    = KERNEL_VERSION;
 263     version->kernel = KERNEL_VERSION;
 264   }
 265
 266   if ((id_buf != NULL) && (id_size != 0U)) {
 267     /* Buffer for retrieving identification string is provided */
 268     if (id_size > sizeof(KERNEL_ID)) {
 269       id_size = sizeof(KERNEL_ID);
 270     }
 271     /* Copy kernel identification string into provided buffer */
 272     memcpy(id_buf, KERNEL_ID, id_size);
 273   }
 274
 275   /* Return execution status */
 276   return (osOK);
 277 }
 278
 279 /*
 280   Get the current RTOS Kernel state.
 281 */
 282 osKernelState_t osKernelGetState (void) {
 283   osKernelState_t state;
 284
 285   switch (xTaskGetSchedulerState()) {
 286     case taskSCHEDULER_RUNNING:
 287       state = osKernelRunning;
 288       break;
 289
 290     case taskSCHEDULER_SUSPENDED:
 291       state = osKernelLocked;
 292       break;
 293
 294     case taskSCHEDULER_NOT_STARTED:
 295     default:
 296       if (KernelState == osKernelReady) {
 297         /* Ready, osKernelInitialize was already called */
 298         state = osKernelReady;
 299       } else {
 300         /* Not initialized */
 301         state = osKernelInactive;
 302       }
 303       break;
 304   }
 305
 306   /* Return current state */
 307   return (state);
 308 }
 309
 310 /*
 311   Start the RTOS Kernel scheduler.
 312 */
 313 osStatus_t osKernelStart (void) {
 314   osStatus_t stat;
 315   BaseType_t state;
 316
 317   if (IRQ_Context() != 0U) {
 318     stat = osErrorISR;
 319   }
 320   else {
 321     state = xTaskGetSchedulerState();
 322
 323     /* Start scheduler if initialized and not started before */
 324     if ((state == taskSCHEDULER_NOT_STARTED) && (KernelState == osKernelReady)) {
 325       /* Ensure SVC priority is at the reset value */
 326       SVC_Setup();
 327       /* Change state to ensure correct API flow */
 328       KernelState = osKernelRunning;
 329       /* Start the kernel scheduler */
 330       vTaskStartScheduler();
 331       stat = osOK;
 332     } else {
 333       stat = osError;
 334     }
 335   }
 336
 337   /* Return execution status */
 338   return (stat);
 339 }
 340
 341 /*
 342   Lock the RTOS Kernel scheduler.
 343 */
 344 int32_t osKernelLock (void) {
 345   int32_t lock;
 346
 347   if (IRQ_Context() != 0U) {
 348     lock = (int32_t)osErrorISR;
 349   }
 350   else {
 351     switch (xTaskGetSchedulerState()) {
 352       case taskSCHEDULER_SUSPENDED:
 353         lock = 1;
 354         break;
 355
 356       case taskSCHEDULER_RUNNING:
 357         vTaskSuspendAll();
 358         lock = 0;
 359         break;
 360
 361       case taskSCHEDULER_NOT_STARTED:
 362       default:
 363         lock = (int32_t)osError;
 364         break;
 365     }
 366   }
 367
 368   /* Return previous lock state */
 369   return (lock);
 370 }
 371
 372 /*
 373   Unlock the RTOS Kernel scheduler.
 374 */
 375 int32_t osKernelUnlock (void) {
 376   int32_t lock;
 377
 378   if (IRQ_Context() != 0U) {
 379     lock = (int32_t)osErrorISR;
 380   }
 381   else {
 382     switch (xTaskGetSchedulerState()) {
 383       case taskSCHEDULER_SUSPENDED:
 384         lock = 1;
 385
 386         if (xTaskResumeAll() != pdTRUE) {
 387           if (xTaskGetSchedulerState() == taskSCHEDULER_SUSPENDED) {
 388             lock = (int32_t)osError;
 389           }
 390         }
 391         break;
 392
 393       case taskSCHEDULER_RUNNING:
 394         lock = 0;
 395         break;
 396
 397       case taskSCHEDULER_NOT_STARTED:
 398       default:
 399         lock = (int32_t)osError;
 400         break;
 401     }
 402   }
 403
 404   /* Return previous lock state */
 405   return (lock);
 406 }
 407
 408 /*
 409   Restore the RTOS Kernel scheduler lock state.
 410 */
 411 int32_t osKernelRestoreLock (int32_t lock) {
 412
 413   if (IRQ_Context() != 0U) {
 414     lock = (int32_t)osErrorISR;
 415   }
 416   else {
 417     switch (xTaskGetSchedulerState()) {
 418       case taskSCHEDULER_SUSPENDED:
 419       case taskSCHEDULER_RUNNING:
 420         if (lock == 1) {
 421           vTaskSuspendAll();
 422         }
 423         else {
 424           if (lock != 0) {
 425             lock = (int32_t)osError;
 426           }
 427           else {
 428             if (xTaskResumeAll() != pdTRUE) {
 429               if (xTaskGetSchedulerState() != taskSCHEDULER_RUNNING) {
 430                 lock = (int32_t)osError;
 431               }
 432             }
 433           }
 434         }
 435         break;
 436
 437       case taskSCHEDULER_NOT_STARTED:
 438       default:
 439         lock = (int32_t)osError;
 440         break;
 441     }
 442   }
 443
 444   /* Return new lock state */
 445   return (lock);
 446 }
 447
 448 /*
 449   Get the RTOS kernel tick count.
 450 */
 451 uint32_t osKernelGetTickCount (void) {
 452   TickType_t ticks;
 453
 454   if (IRQ_Context() != 0U) {
 455     ticks = xTaskGetTickCountFromISR();
 456   } else {
 457     ticks = xTaskGetTickCount();
 458   }
 459
 460   /* Return kernel tick count */
 461   return (ticks);
 462 }
 463
 464 /*
 465   Get the RTOS kernel tick frequency.
 466 */
 467 uint32_t osKernelGetTickFreq (void) {
 468   /* Return frequency in hertz */
 469   return (configTICK_RATE_HZ);
 470 }
 471
 472 /*
 473   Get the RTOS kernel system timer count.
 474 */
 475 uint32_t osKernelGetSysTimerCount (void) {
 476   uint32_t irqmask = IS_IRQ_MASKED();
 477   TickType_t ticks;
 478   uint32_t val;
 479 #if (configUSE_TICKLESS_IDLE != 0)
 480   uint32_t val0;
 481
 482   /* Low Power Tickless Idle controls timer overflow flag and therefore      */
 483   /* OS_Tick_GetOverflow may be non-functional. As a workaround a reference  */
 484   /* time is measured here before disabling interrupts. Timer value overflow */
 485   /* is then checked by comparing reference against latest time measurement. */
 486   /* Timer count value returned by this method is less accurate but if an    */
 487   /* overflow is missed, an invalid timer count would be returned.           */
 488   val0 = OS_Tick_GetCount();
 489 #endif
 490
 491   if (irqmask == 0U) {
 492     __disable_irq();
 493   }
 494
 495   ticks = xTaskGetTickCount();
 496   val   = OS_Tick_GetCount();
 497
 498   /* Update tick count and timer value when timer overflows */
 499 #if (configUSE_TICKLESS_IDLE != 0)
 500   if (val < val0) {
 501     ticks++;
 502   }
 503 #else
 504   if (OS_Tick_GetOverflow() != 0U) {
 505     val = OS_Tick_GetCount();
 506     ticks++;
 507   }
 508 #endif
 509
 510   val += ticks * OS_Tick_GetInterval();
 511
 512   if (irqmask == 0U) {
 513     __enable_irq();
 514   }
 515
 516   /* Return system timer count */
 517   return (val);
 518 }
 519
 520 /*
 521   Get the RTOS kernel system timer frequency.
 522 */
 523 uint32_t osKernelGetSysTimerFreq (void) {
 524   /* Return frequency in hertz */
 525   return (configCPU_CLOCK_HZ);
 526 }
 527
 528
 529 /* ==== Thread Management Functions ==== */
 530
 531 /*
 532   Create a thread and add it to Active Threads.
 533
 534   Limitations:
 535   - The memory for control block and stack must be provided in the osThreadAttr_t
 536     structure in order to allocate object statically.
 537   - Attribute osThreadJoinable is not supported, NULL is returned if used.
 538 */
 539 osThreadId_t osThreadNew (osThreadFunc_t func, void *argument, const osThreadAttr_t *attr) {
 540   const char *name;
 541   uint32_t stack;
 542   TaskHandle_t hTask;
 543   UBaseType_t prio;
 544   int32_t mem;
 545
 546   hTask = NULL;
 547
 548   if ((IRQ_Context() == 0U) && (func != NULL)) {
 549     stack = configMINIMAL_STACK_SIZE;
 550     prio  = (UBaseType_t)osPriorityNormal;
 551
 552     name = NULL;
 553     mem  = -1;
 554
 555     if (attr != NULL) {
 556       if (attr->name != NULL) {
 557         name = attr->name;
 558       }
 559       if (attr->priority != osPriorityNone) {
 560         prio = (UBaseType_t)attr->priority;
 561       }
 562
 563       if ((prio < osPriorityIdle) || (prio > osPriorityISR) || ((attr->attr_bits & osThreadJoinable) == osThreadJoinable)) {
 564         /* Invalid priority or unsupported osThreadJoinable attribute used */
 565         return (NULL);
 566       }
 567
 568       if (attr->stack_size > 0U) {
 569         /* In FreeRTOS stack is not in bytes, but in sizeof(StackType_t) which is 4 on ARM ports.       */
 570         /* Stack size should be therefore 4 byte aligned in order to avoid division caused side effects */
 571         stack = attr->stack_size / sizeof(StackType_t);
 572       }
 573
 574       if ((attr->cb_mem    != NULL) && (attr->cb_size    >= sizeof(StaticTask_t)) &&
 575           (attr->stack_mem != NULL) && (attr->stack_size >  0U)) {
 576         /* The memory for control block and stack is provided, use static object */
 577         mem = 1;
 578       }
 579       else {
 580         if ((attr->cb_mem == NULL) && (attr->cb_size == 0U) && (attr->stack_mem == NULL)) {
 581           /* Control block and stack memory will be allocated from the dynamic pool */
 582           mem = 0;
 583         }
 584       }
 585     }
 586     else {
 587       mem = 0;
 588     }
 589
 590     if (mem == 1) {
 591       #if (configSUPPORT_STATIC_ALLOCATION == 1)
 592         hTask = xTaskCreateStatic ((TaskFunction_t)func, name, stack, argument, prio, (StackType_t  *)attr->stack_mem,
 593                                                                                       (StaticTask_t *)attr->cb_mem);
 594       #endif
 595     }
 596     else {
 597       if (mem == 0) {
 598         #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
 599           if (xTaskCreate ((TaskFunction_t)func, name, (configSTACK_DEPTH_TYPE)stack, argument, prio, &hTask) != pdPASS) {
 600             hTask = NULL;
 601           }
 602         #endif
 603       }
 604     }
 605   }
 606
 607   /* Return thread ID */
 608   return ((osThreadId_t)hTask);
 609 }
 610
 611 /*
 612   Get name of a thread.
 613 */
 614 const char *osThreadGetName (osThreadId_t thread_id) {
 615   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 616   const char *name;
 617
 618   if ((IRQ_Context() != 0U) || (hTask == NULL)) {
 619     name = NULL;
 620   } else {
 621     name = pcTaskGetName (hTask);
 622   }
 623
 624   /* Return name as null-terminated string */
 625   return (name);
 626 }
 627
 628 /*
 629   Return the thread ID of the current running thread.
 630 */
 631 osThreadId_t osThreadGetId (void) {
 632   osThreadId_t id;
 633
 634   id = (osThreadId_t)xTaskGetCurrentTaskHandle();
 635
 636   /* Return thread ID */
 637   return (id);
 638 }
 639
 640 /*
 641   Get current thread state of a thread.
 642 */
 643 osThreadState_t osThreadGetState (osThreadId_t thread_id) {
 644   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 645   osThreadState_t state;
 646
 647   if ((IRQ_Context() != 0U) || (hTask == NULL)) {
 648     state = osThreadError;
 649   }
 650   else {
 651     switch (eTaskGetState (hTask)) {
 652       case eRunning:   state = osThreadRunning;    break;
 653       case eReady:     state = osThreadReady;      break;
 654       case eBlocked:
 655       case eSuspended: state = osThreadBlocked;    break;
 656       case eDeleted:
 657       case eInvalid:
 658       default:         state = osThreadError;      break;
 659     }
 660   }
 661
 662   /* Return current thread state */
 663   return (state);
 664 }
 665
 666 /*
 667   Get available stack space of a thread based on stack watermark recording during execution.
 668 */
 669 uint32_t osThreadGetStackSpace (osThreadId_t thread_id) {
 670   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 671   uint32_t sz;
 672
 673   if ((IRQ_Context() != 0U) || (hTask == NULL)) {
 674     sz = 0U;
 675   } else {
 676     sz = (uint32_t)(uxTaskGetStackHighWaterMark(hTask) * sizeof(StackType_t));
 677   }
 678
 679   /* Return remaining stack space in bytes */
 680   return (sz);
 681 }
 682
 683 /*
 684   Change priority of a thread.
 685 */
 686 osStatus_t osThreadSetPriority (osThreadId_t thread_id, osPriority_t priority) {
 687   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 688   osStatus_t stat;
 689
 690   if (IRQ_Context() != 0U) {
 691     stat = osErrorISR;
 692   }
 693   else if ((hTask == NULL) || (priority < osPriorityIdle) || (priority > osPriorityISR)) {
 694     stat = osErrorParameter;
 695   }
 696   else {
 697     stat = osOK;
 698     vTaskPrioritySet (hTask, (UBaseType_t)priority);
 699   }
 700
 701   /* Return execution status */
 702   return (stat);
 703 }
 704
 705 /*
 706   Get current priority of a thread.
 707 */
 708 osPriority_t osThreadGetPriority (osThreadId_t thread_id) {
 709   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 710   osPriority_t prio;
 711
 712   if ((IRQ_Context() != 0U) || (hTask == NULL)) {
 713     prio = osPriorityError;
 714   } else {
 715     prio = (osPriority_t)((int32_t)uxTaskPriorityGet (hTask));
 716   }
 717
 718   /* Return current thread priority */
 719   return (prio);
 720 }
 721
 722 /*
 723   Pass control to next thread that is in state READY.
 724 */
 725 osStatus_t osThreadYield (void) {
 726   osStatus_t stat;
 727
 728   if (IRQ_Context() != 0U) {
 729     stat = osErrorISR;
 730   } else {
 731     stat = osOK;
 732     taskYIELD();
 733   }
 734
 735   /* Return execution status */
 736   return (stat);
 737 }
 738
 739 #if (configUSE_OS2_THREAD_SUSPEND_RESUME == 1)
 740 /*
 741   Suspend execution of a thread.
 742 */
 743 osStatus_t osThreadSuspend (osThreadId_t thread_id) {
 744   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 745   osStatus_t stat;
 746
 747   if (IRQ_Context() != 0U) {
 748     stat = osErrorISR;
 749   }
 750   else if (hTask == NULL) {
 751     stat = osErrorParameter;
 752   }
 753   else {
 754     stat = osOK;
 755     vTaskSuspend (hTask);
 756   }
 757
 758   /* Return execution status */
 759   return (stat);
 760 }
 761
 762 /*
 763   Resume execution of a thread.
 764 */
 765 osStatus_t osThreadResume (osThreadId_t thread_id) {
 766   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 767   osStatus_t stat;
 768   eTaskState tstate;
 769
 770   if (IRQ_Context() != 0U) {
 771     stat = osErrorISR;
 772   }
 773   else if (hTask == NULL) {
 774     stat = osErrorParameter;
 775   }
 776   else {
 777     tstate = eTaskGetState (hTask);
 778
 779     if (tstate == eSuspended) {
 780       /* Thread is suspended */
 781       stat = osOK;
 782       vTaskResume (hTask);
 783     } else {
 784       /* Not suspended, might be blocked */
 785       if (xTaskAbortDelay(hTask) == pdPASS) {
 786         /* Thread was unblocked */
 787         stat = osOK;
 788       } else {
 789         /* Thread was not blocked */
 790         stat = osErrorResource;
 791       }
 792     }
 793   }
 794
 795   /* Return execution status */
 796   return (stat);
 797 }
 798 #endif /* (configUSE_OS2_THREAD_SUSPEND_RESUME == 1) */
 799
 800 /*
 801   Terminate execution of current running thread.
 802 */
 803 __NO_RETURN void osThreadExit (void) {
 804 #ifndef USE_FreeRTOS_HEAP_1
 805   vTaskDelete (NULL);
 806 #endif
 807   for (;;);
 808 }
 809
 810 /*
 811   Terminate execution of a thread.
 812 */
 813 osStatus_t osThreadTerminate (osThreadId_t thread_id) {
 814   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 815   osStatus_t stat;
 816 #ifndef USE_FreeRTOS_HEAP_1
 817   eTaskState tstate;
 818
 819   if (IRQ_Context() != 0U) {
 820     stat = osErrorISR;
 821   }
 822   else if (hTask == NULL) {
 823     stat = osErrorParameter;
 824   }
 825   else {
 826     tstate = eTaskGetState (hTask);
 827
 828     if (tstate != eDeleted) {
 829       stat = osOK;
 830       vTaskDelete (hTask);
 831     } else {
 832       stat = osErrorResource;
 833     }
 834   }
 835 #else
 836   stat = osError;
 837 #endif
 838
 839   /* Return execution status */
 840   return (stat);
 841 }
 842
 843 /*
 844   Get number of active threads.
 845 */
 846 uint32_t osThreadGetCount (void) {
 847   uint32_t count;
 848
 849   if (IRQ_Context() != 0U) {
 850     count = 0U;
 851   } else {
 852     count = uxTaskGetNumberOfTasks();
 853   }
 854
 855   /* Return number of active threads */
 856   return (count);
 857 }
 858
 859 #if (configUSE_OS2_THREAD_ENUMERATE == 1)
 860 /*
 861   Enumerate active threads.
 862 */
 863 uint32_t osThreadEnumerate (osThreadId_t *thread_array, uint32_t array_items) {
 864   uint32_t i, count;
 865   TaskStatus_t *task;
 866
 867   if ((IRQ_Context() != 0U) || (thread_array == NULL) || (array_items == 0U)) {
 868     count = 0U;
 869   } else {
 870     vTaskSuspendAll();
 871
 872     /* Allocate memory on heap to temporarily store TaskStatus_t information */
 873     count = uxTaskGetNumberOfTasks();
 874     task  = pvPortMalloc (count * sizeof(TaskStatus_t));
 875
 876     if (task != NULL) {
 877       /* Retrieve task status information */
 878       count = uxTaskGetSystemState (task, count, NULL);
 879
 880       /* Copy handles from task status array into provided thread array */
 881       for (i = 0U; (i < count) && (i < array_items); i++) {
 882         thread_array[i] = (osThreadId_t)task[i].xHandle;
 883       }
 884       count = i;
 885     }
 886     (void)xTaskResumeAll();
 887
 888     vPortFree (task);
 889   }
 890
 891   /* Return number of enumerated threads */
 892   return (count);
 893 }
 894 #endif /* (configUSE_OS2_THREAD_ENUMERATE == 1) */
 895
 896
 897 /* ==== Thread Flags Functions ==== */
 898
 899 #if (configUSE_OS2_THREAD_FLAGS == 1)
 900 /*
 901   Set the specified Thread Flags of a thread.
 902 */
 903 uint32_t osThreadFlagsSet (osThreadId_t thread_id, uint32_t flags) {
 904   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 905   uint32_t rflags;
 906   BaseType_t yield;
 907
 908   if ((hTask == NULL) || ((flags & THREAD_FLAGS_INVALID_BITS) != 0U)) {
 909     rflags = (uint32_t)osErrorParameter;
 910   }
 911   else {
 912     rflags = (uint32_t)osError;
 913
 914     if (IRQ_Context() != 0U) {
 915       yield = pdFALSE;
 916
 917       (void)xTaskNotifyFromISR (hTask, flags, eSetBits, &yield);
 918       (void)xTaskNotifyAndQueryFromISR (hTask, 0, eNoAction, &rflags, NULL);
 919
 920       portYIELD_FROM_ISR (yield);
 921     }
 922     else {
 923       (void)xTaskNotify (hTask, flags, eSetBits);
 924       (void)xTaskNotifyAndQuery (hTask, 0, eNoAction, &rflags);
 925     }
 926   }
 927   /* Return flags after setting */
 928   return (rflags);
 929 }
 930
 931 /*
 932   Clear the specified Thread Flags of current running thread.
 933 */
 934 uint32_t osThreadFlagsClear (uint32_t flags) {
 935   TaskHandle_t hTask;
 936   uint32_t rflags, cflags;
 937
 938   if (IRQ_Context() != 0U) {
 939     rflags = (uint32_t)osErrorISR;
 940   }
 941   else if ((flags & THREAD_FLAGS_INVALID_BITS) != 0U) {
 942     rflags = (uint32_t)osErrorParameter;
 943   }
 944   else {
 945     hTask = xTaskGetCurrentTaskHandle();
 946
 947     if (xTaskNotifyAndQuery (hTask, 0, eNoAction, &cflags) == pdPASS) {
 948       rflags = cflags;
 949       cflags &= ~flags;
 950
 951       if (xTaskNotify (hTask, cflags, eSetValueWithOverwrite) != pdPASS) {
 952         rflags = (uint32_t)osError;
 953       }
 954     }
 955     else {
 956       rflags = (uint32_t)osError;
 957     }
 958   }
 959
 960   /* Return flags before clearing */
 961   return (rflags);
 962 }
 963
 964 /*
 965   Get the current Thread Flags of current running thread.
 966 */
 967 uint32_t osThreadFlagsGet (void) {
 968   TaskHandle_t hTask;
 969   uint32_t rflags;
 970
 971   if (IRQ_Context() != 0U) {
 972     rflags = (uint32_t)osErrorISR;
 973   }
 974   else {
 975     hTask = xTaskGetCurrentTaskHandle();
 976
 977     if (xTaskNotifyAndQuery (hTask, 0, eNoAction, &rflags) != pdPASS) {
 978       rflags = (uint32_t)osError;
 979     }
 980   }
 981
 982   /* Return current flags */
 983   return (rflags);
 984 }
 985
 986 /*
 987   Wait for one or more Thread Flags of the current running thread to become signaled.
 988 */
 989 uint32_t osThreadFlagsWait (uint32_t flags, uint32_t options, uint32_t timeout) {
 990   uint32_t rflags, nval;
 991   uint32_t clear;
 992   TickType_t t0, td, tout;
 993   BaseType_t rval;
 994
 995   if (IRQ_Context() != 0U) {
 996     rflags = (uint32_t)osErrorISR;
 997   }
 998   else if ((flags & THREAD_FLAGS_INVALID_BITS) != 0U) {
 999     rflags = (uint32_t)osErrorParameter;
1000   }
1001   else {
1002     if ((options & osFlagsNoClear) == osFlagsNoClear) {
1003       clear = 0U;
1004     } else {
1005       clear = flags;
1006     }
1007
1008     rflags = 0U;
1009     tout   = timeout;
1010
1011     t0 = xTaskGetTickCount();
1012     do {
1013       rval = xTaskNotifyWait (0, clear, &nval, tout);
1014
1015       if (rval == pdPASS) {
1016         rflags &= flags;
1017         rflags |= nval;
1018
1019         if ((options & osFlagsWaitAll) == osFlagsWaitAll) {
1020           if ((flags & rflags) == flags) {
1021             break;
1022           } else {
1023             if (timeout == 0U) {
1024               rflags = (uint32_t)osErrorResource;
1025               break;
1026             }
1027           }
1028         }
1029         else {
1030           if ((flags & rflags) != 0) {
1031             break;
1032           } else {
1033             if (timeout == 0U) {
1034               rflags = (uint32_t)osErrorResource;
1035               break;
1036             }
1037           }
1038         }
1039
1040         /* Update timeout */
1041         td = xTaskGetTickCount() - t0;
1042
1043         if (td > timeout) {
1044           tout  = 0;
1045         } else {
1046           tout = timeout - td;
1047         }
1048       }
1049       else {
1050         if (timeout == 0) {
1051           rflags = (uint32_t)osErrorResource;
1052         } else {
1053           rflags = (uint32_t)osErrorTimeout;
1054         }
1055       }
1056     }
1057     while (rval != pdFAIL);
1058   }
1059
1060   /* Return flags before clearing */
1061   return (rflags);
1062 }
1063 #endif /* (configUSE_OS2_THREAD_FLAGS == 1) */
1064
1065
1066 /* ==== Generic Wait Functions ==== */
1067
1068 /*
1069   Wait for Timeout (Time Delay).
1070 */
1071 osStatus_t osDelay (uint32_t ticks) {
1072   osStatus_t stat;
1073
1074   if (IRQ_Context() != 0U) {
1075     stat = osErrorISR;
1076   }
1077   else {
1078     stat = osOK;
1079
1080     if (ticks != 0U) {
1081       vTaskDelay(ticks);
1082     }
1083   }
1084
1085   /* Return execution status */
1086   return (stat);
1087 }
1088
1089 /*
1090   Wait until specified time.
1091 */
1092 osStatus_t osDelayUntil (uint32_t ticks) {
1093   TickType_t tcnt, delay;
1094   osStatus_t stat;
1095
1096   if (IRQ_Context() != 0U) {
1097     stat = osErrorISR;
1098   }
1099   else {
1100     stat = osOK;
1101     tcnt = xTaskGetTickCount();
1102
1103     /* Determine remaining number of ticks to delay */
1104     delay = (TickType_t)ticks - tcnt;
1105
1106     /* Check if target tick has not expired */
1107     if((delay != 0U) && (0 == (delay >> (8 * sizeof(TickType_t) - 1)))) {
1108       if (xTaskDelayUntil (&tcnt, delay) == pdFALSE) {
1109         /* Did not delay */
1110         stat = osError;
1111       }
1112     }
1113     else
1114     {
1115       /* No delay or already expired */
1116       stat = osErrorParameter;
1117     }
1118   }
1119
1120   /* Return execution status */
1121   return (stat);
1122 }
1123
1124
1125 /* ==== Timer Management Functions ==== */
1126
1127 #if (configUSE_OS2_TIMER == 1)
1128
1129 static void TimerCallback (TimerHandle_t hTimer) {
1130   TimerCallback_t *callb;
1131
1132   /* Retrieve pointer to callback function and argument */
1133   callb = (TimerCallback_t *)pvTimerGetTimerID (hTimer);
1134
1135   /* Remove dynamic allocation flag */
1136   callb = (TimerCallback_t *)((uint32_t)callb & ~1U);
1137
1138   if (callb != NULL) {
1139     callb->func (callb->arg);
1140   }
1141 }
1142
1143 /*
1144   Create and Initialize a timer.
1145 */
1146 osTimerId_t osTimerNew (osTimerFunc_t func, osTimerType_t type, void *argument, const osTimerAttr_t *attr) {
1147   const char *name;
1148   TimerHandle_t hTimer;
1149   TimerCallback_t *callb;
1150   UBaseType_t reload;
1151   int32_t mem;
1152   uint32_t callb_dyn;
1153
1154   hTimer = NULL;
1155
1156   if ((IRQ_Context() == 0U) && (func != NULL)) {
1157     callb     = NULL;
1158     callb_dyn = 0U;
1159
1160     #if (configSUPPORT_STATIC_ALLOCATION == 1)
1161       /* Static memory allocation is available: check if memory for control block */
1162       /* is provided and if it also contains space for callback and its argument  */
1163       if ((attr != NULL) && (attr->cb_mem != NULL)) {
1164         if (attr->cb_size >= (sizeof(StaticTimer_t) + sizeof(TimerCallback_t))) {
1165           callb = (TimerCallback_t *)((uint32_t)attr->cb_mem + sizeof(StaticTimer_t));
1166         }
1167       }
1168     #endif
1169
1170     #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1171       /* Dynamic memory allocation is available: if memory for callback and */
1172       /* its argument is not provided, allocate it from dynamic memory pool */
1173       if (callb == NULL) {
1174         callb = (TimerCallback_t *)pvPortMalloc (sizeof(TimerCallback_t));
1175
1176         if (callb != NULL) {
1177           /* Callback memory was allocated from dynamic pool, set flag */
1178           callb_dyn = 1U;
1179         }
1180       }
1181     #endif
1182
1183     if (callb != NULL) {
1184       callb->func = func;
1185       callb->arg  = argument;
1186
1187       if (type == osTimerOnce) {
1188         reload = pdFALSE;
1189       } else {
1190         reload = pdTRUE;
1191       }
1192
1193       mem  = -1;
1194       name = NULL;
1195
1196       if (attr != NULL) {
1197         if (attr->name != NULL) {
1198           name = attr->name;
1199         }
1200
1201         if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticTimer_t))) {
1202           /* The memory for control block is provided, use static object */
1203           mem = 1;
1204         }
1205         else {
1206           if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
1207             /* Control block will be allocated from the dynamic pool */
1208             mem = 0;
1209           }
1210         }
1211       }
1212       else {
1213         mem = 0;
1214       }
1215       /* Store callback memory dynamic allocation flag */
1216       callb = (TimerCallback_t *)((uint32_t)callb | callb_dyn);
1217       /*
1218         TimerCallback function is always provided as a callback and is used to call application
1219         specified function with its argument both stored in structure callb.
1220       */
1221       if (mem == 1) {
1222         #if (configSUPPORT_STATIC_ALLOCATION == 1)
1223           hTimer = xTimerCreateStatic (name, 1, reload, callb, TimerCallback, (StaticTimer_t *)attr->cb_mem);
1224         #endif
1225       }
1226       else {
1227         if (mem == 0) {
1228           #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1229             hTimer = xTimerCreate (name, 1, reload, callb, TimerCallback);
1230           #endif
1231         }
1232       }
1233
1234       #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1235       if ((hTimer == NULL) && (callb != NULL) && (callb_dyn == 1U)) {
1236         /* Failed to create a timer, release allocated resources */
1237         callb = (TimerCallback_t *)((uint32_t)callb & ~1U);
1238
1239         vPortFree (callb);
1240       }
1241       #endif
1242     }
1243   }
1244
1245   /* Return timer ID */
1246   return ((osTimerId_t)hTimer);
1247 }
1248
1249 /*
1250   Get name of a timer.
1251 */
1252 const char *osTimerGetName (osTimerId_t timer_id) {
1253   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
1254   const char *p;
1255
1256   if ((IRQ_Context() != 0U) || (hTimer == NULL)) {
1257     p = NULL;
1258   } else {
1259     p = pcTimerGetName (hTimer);
1260   }
1261
1262   /* Return name as null-terminated string */
1263   return (p);
1264 }
1265
1266 /*
1267   Start or restart a timer.
1268 */
1269 osStatus_t osTimerStart (osTimerId_t timer_id, uint32_t ticks) {
1270   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
1271   osStatus_t stat;
1272
1273   if (IRQ_Context() != 0U) {
1274     stat = osErrorISR;
1275   }
1276   else if ((hTimer == NULL) || (ticks == 0U)) {
1277     stat = osErrorParameter;
1278   }
1279   else {
1280     if (xTimerChangePeriod (hTimer, ticks, 0) == pdPASS) {
1281       stat = osOK;
1282     } else {
1283       stat = osErrorResource;
1284     }
1285   }
1286
1287   /* Return execution status */
1288   return (stat);
1289 }
1290
1291 /*
1292   Stop a timer.
1293 */
1294 osStatus_t osTimerStop (osTimerId_t timer_id) {
1295   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
1296   osStatus_t stat;
1297
1298   if (IRQ_Context() != 0U) {
1299     stat = osErrorISR;
1300   }
1301   else if (hTimer == NULL) {
1302     stat = osErrorParameter;
1303   }
1304   else {
1305     if (xTimerIsTimerActive (hTimer) == pdFALSE) {
1306       stat = osErrorResource;
1307     }
1308     else {
1309       if (xTimerStop (hTimer, 0) == pdPASS) {
1310         stat = osOK;
1311       } else {
1312         stat = osError;
1313       }
1314     }
1315   }
1316
1317   /* Return execution status */
1318   return (stat);
1319 }
1320
1321 /*
1322   Check if a timer is running.
1323 */
1324 uint32_t osTimerIsRunning (osTimerId_t timer_id) {
1325   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
1326   uint32_t running;
1327
1328   if ((IRQ_Context() != 0U) || (hTimer == NULL)) {
1329     running = 0U;
1330   } else {
1331     running = (uint32_t)xTimerIsTimerActive (hTimer);
1332   }
1333
1334   /* Return 0: not running, 1: running */
1335   return (running);
1336 }
1337
1338 /*
1339   Delete a timer.
1340 */
1341 osStatus_t osTimerDelete (osTimerId_t timer_id) {
1342   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
1343   osStatus_t stat;
1344 #ifndef USE_FreeRTOS_HEAP_1
1345 #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1346   TimerCallback_t *callb;
1347 #endif
1348
1349   if (IRQ_Context() != 0U) {
1350     stat = osErrorISR;
1351   }
1352   else if (hTimer == NULL) {
1353     stat = osErrorParameter;
1354   }
1355   else {
1356     #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1357     callb = (TimerCallback_t *)pvTimerGetTimerID (hTimer);
1358     #endif
1359
1360     if (xTimerDelete (hTimer, 0) == pdPASS) {
1361       #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1362         if ((uint32_t)callb & 1U) {
1363           /* Callback memory was allocated from dynamic pool, clear flag */
1364           callb = (TimerCallback_t *)((uint32_t)callb & ~1U);
1365
1366           /* Return allocated memory to dynamic pool */
1367           vPortFree (callb);
1368         }
1369       #endif
1370       stat = osOK;
1371     } else {
1372       stat = osErrorResource;
1373     }
1374   }
1375 #else
1376   stat = osError;
1377 #endif
1378
1379   /* Return execution status */
1380   return (stat);
1381 }
1382 #endif /* (configUSE_OS2_TIMER == 1) */
1383
1384
1385 /* ==== Event Flags Management Functions ==== */
1386
1387 /*
1388   Create and Initialize an Event Flags object.
1389
1390   Limitations:
1391   - Event flags are limited to 24 bits.
1392 */
1393 osEventFlagsId_t osEventFlagsNew (const osEventFlagsAttr_t *attr) {
1394   EventGroupHandle_t hEventGroup;
1395   int32_t mem;
1396
1397   hEventGroup = NULL;
1398
1399   if (IRQ_Context() == 0U) {
1400     mem = -1;
1401
1402     if (attr != NULL) {
1403       if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticEventGroup_t))) {
1404         /* The memory for control block is provided, use static object */
1405         mem = 1;
1406       }
1407       else {
1408         if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
1409           /* Control block will be allocated from the dynamic pool */
1410           mem = 0;
1411         }
1412       }
1413     }
1414     else {
1415       mem = 0;
1416     }
1417
1418     if (mem == 1) {
1419       #if (configSUPPORT_STATIC_ALLOCATION == 1)
1420       hEventGroup = xEventGroupCreateStatic (attr->cb_mem);
1421       #endif
1422     }
1423     else {
1424       if (mem == 0) {
1425         #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1426           hEventGroup = xEventGroupCreate();
1427         #endif
1428       }
1429     }
1430   }
1431
1432   /* Return event flags ID */
1433   return ((osEventFlagsId_t)hEventGroup);
1434 }
1435
1436 /*
1437   Set the specified Event Flags.
1438
1439   Limitations:
1440   - Event flags are limited to 24 bits.
1441 */
1442 uint32_t osEventFlagsSet (osEventFlagsId_t ef_id, uint32_t flags) {
1443   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1444   uint32_t rflags;
1445   BaseType_t yield;
1446
1447   if ((hEventGroup == NULL) || ((flags & EVENT_FLAGS_INVALID_BITS) != 0U)) {
1448     rflags = (uint32_t)osErrorParameter;
1449   }
1450   else if (IRQ_Context() != 0U) {
1451   #if (configUSE_OS2_EVENTFLAGS_FROM_ISR == 0)
1452     (void)yield;
1453     /* Enable timers and xTimerPendFunctionCall function to support osEventFlagsSet from ISR */
1454     rflags = (uint32_t)osErrorResource;
1455   #else
1456     yield = pdFALSE;
1457
1458     if (xEventGroupSetBitsFromISR (hEventGroup, (EventBits_t)flags, &yield) == pdFAIL) {
1459       rflags = (uint32_t)osErrorResource;
1460     } else {
1461       /* Retrieve bits that are already set and add flags to be set in current call */
1462       rflags  = xEventGroupGetBitsFromISR (hEventGroup);
1463       rflags |= flags;
1464       portYIELD_FROM_ISR (yield);
1465     }
1466   #endif
1467   }
1468   else {
1469     rflags = xEventGroupSetBits (hEventGroup, (EventBits_t)flags);
1470   }
1471
1472   /* Return event flags after setting */
1473   return (rflags);
1474 }
1475
1476 /*
1477   Clear the specified Event Flags.
1478
1479   Limitations:
1480   - Event flags are limited to 24 bits.
1481 */
1482 uint32_t osEventFlagsClear (osEventFlagsId_t ef_id, uint32_t flags) {
1483   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1484   uint32_t rflags;
1485
1486   if ((hEventGroup == NULL) || ((flags & EVENT_FLAGS_INVALID_BITS) != 0U)) {
1487     rflags = (uint32_t)osErrorParameter;
1488   }
1489   else if (IRQ_Context() != 0U) {
1490   #if (configUSE_OS2_EVENTFLAGS_FROM_ISR == 0)
1491     /* Enable timers and xTimerPendFunctionCall function to support osEventFlagsSet from ISR */
1492     rflags = (uint32_t)osErrorResource;
1493   #else
1494     rflags = xEventGroupGetBitsFromISR (hEventGroup);
1495
1496     if (xEventGroupClearBitsFromISR (hEventGroup, (EventBits_t)flags) == pdFAIL) {
1497       rflags = (uint32_t)osErrorResource;
1498     }
1499     else {
1500       /* xEventGroupClearBitsFromISR only registers clear operation in the timer command queue. */
1501       /* Yield is required here otherwise clear operation might not execute in the right order. */
1502       /* See https://github.com/FreeRTOS/FreeRTOS-Kernel/issues/93 for more info.               */
1503       portYIELD_FROM_ISR (pdTRUE);
1504     }
1505   #endif
1506   }
1507   else {
1508     rflags = xEventGroupClearBits (hEventGroup, (EventBits_t)flags);
1509   }
1510
1511   /* Return event flags before clearing */
1512   return (rflags);
1513 }
1514
1515 /*
1516   Get the current Event Flags.
1517
1518   Limitations:
1519   - Event flags are limited to 24 bits.
1520 */
1521 uint32_t osEventFlagsGet (osEventFlagsId_t ef_id) {
1522   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1523   uint32_t rflags;
1524
1525   if (ef_id == NULL) {
1526     rflags = 0U;
1527   }
1528   else if (IRQ_Context() != 0U) {
1529     rflags = xEventGroupGetBitsFromISR (hEventGroup);
1530   }
1531   else {
1532     rflags = xEventGroupGetBits (hEventGroup);
1533   }
1534
1535   /* Return current event flags */
1536   return (rflags);
1537 }
1538
1539 /*
1540   Wait for one or more Event Flags to become signaled.
1541
1542   Limitations:
1543   - Event flags are limited to 24 bits.
1544   - osEventFlagsWait cannot be called from an ISR.
1545 */
1546 uint32_t osEventFlagsWait (osEventFlagsId_t ef_id, uint32_t flags, uint32_t options, uint32_t timeout) {
1547   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1548   BaseType_t wait_all;
1549   BaseType_t exit_clr;
1550   uint32_t rflags;
1551
1552   if ((hEventGroup == NULL) || ((flags & EVENT_FLAGS_INVALID_BITS) != 0U)) {
1553     rflags = (uint32_t)osErrorParameter;
1554   }
1555   else if (IRQ_Context() != 0U) {
1556     if (timeout == 0U) {
1557       /* Try semantic is not supported */
1558       rflags = (uint32_t)osErrorISR;
1559     } else {
1560       /* Calling osEventFlagsWait from ISR with non-zero timeout is invalid */
1561       rflags = (uint32_t)osFlagsErrorParameter;
1562     }
1563   }
1564   else {
1565     if (options & osFlagsWaitAll) {
1566       wait_all = pdTRUE;
1567     } else {
1568       wait_all = pdFAIL;
1569     }
1570
1571     if (options & osFlagsNoClear) {
1572       exit_clr = pdFAIL;
1573     } else {
1574       exit_clr = pdTRUE;
1575     }
1576
1577     rflags = xEventGroupWaitBits (hEventGroup, (EventBits_t)flags, exit_clr, wait_all, (TickType_t)timeout);
1578
1579     if (options & osFlagsWaitAll) {
1580       if ((flags & rflags) != flags) {
1581         if (timeout > 0U) {
1582           rflags = (uint32_t)osErrorTimeout;
1583         } else {
1584           rflags = (uint32_t)osErrorResource;
1585         }
1586       }
1587     }
1588     else {
1589       if ((flags & rflags) == 0U) {
1590         if (timeout > 0U) {
1591           rflags = (uint32_t)osErrorTimeout;
1592         } else {
1593           rflags = (uint32_t)osErrorResource;
1594         }
1595       }
1596     }
1597   }
1598
1599   /* Return event flags before clearing */
1600   return (rflags);
1601 }
1602
1603 /*
1604   Delete an Event Flags object.
1605 */
1606 osStatus_t osEventFlagsDelete (osEventFlagsId_t ef_id) {
1607   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1608   osStatus_t stat;
1609
1610 #ifndef USE_FreeRTOS_HEAP_1
1611   if (IRQ_Context() != 0U) {
1612     stat = osErrorISR;
1613   }
1614   else if (hEventGroup == NULL) {
1615     stat = osErrorParameter;
1616   }
1617   else {
1618     stat = osOK;
1619     vEventGroupDelete (hEventGroup);
1620   }
1621 #else
1622   stat = osError;
1623 #endif
1624
1625   /* Return execution status */
1626   return (stat);
1627 }
1628
1629
1630 /* ==== Mutex Management Functions ==== */
1631
1632 #if (configUSE_OS2_MUTEX == 1)
1633 /*
1634   Create and Initialize a Mutex object.
1635
1636   Limitations:
1637   - Priority inherit protocol is used by default, osMutexPrioInherit attribute is ignored.
1638   - Robust mutex is not supported, NULL is returned if used.
1639 */
1640 osMutexId_t osMutexNew (const osMutexAttr_t *attr) {
1641   SemaphoreHandle_t hMutex;
1642   uint32_t type;
1643   uint32_t rmtx;
1644   int32_t  mem;
1645
1646   hMutex = NULL;
1647
1648   if (IRQ_Context() == 0U) {
1649     if (attr != NULL) {
1650       type = attr->attr_bits;
1651     } else {
1652       type = 0U;
1653     }
1654
1655     if ((type & osMutexRecursive) == osMutexRecursive) {
1656       rmtx = 1U;
1657     } else {
1658       rmtx = 0U;
1659     }
1660
1661     if ((type & osMutexRobust) != osMutexRobust) {
1662       mem = -1;
1663
1664       if (attr != NULL) {
1665         if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticSemaphore_t))) {
1666           /* The memory for control block is provided, use static object */
1667           mem = 1;
1668         }
1669         else {
1670           if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
1671             /* Control block will be allocated from the dynamic pool */
1672             mem = 0;
1673           }
1674         }
1675       }
1676       else {
1677         mem = 0;
1678       }
1679
1680       if (mem == 1) {
1681         #if (configSUPPORT_STATIC_ALLOCATION == 1)
1682           if (rmtx != 0U) {
1683             #if (configUSE_RECURSIVE_MUTEXES == 1)
1684             hMutex = xSemaphoreCreateRecursiveMutexStatic (attr->cb_mem);
1685             #endif
1686           }
1687           else {
1688             hMutex = xSemaphoreCreateMutexStatic (attr->cb_mem);
1689           }
1690         #endif
1691       }
1692       else {
1693         if (mem == 0) {
1694           #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1695             if (rmtx != 0U) {
1696               #if (configUSE_RECURSIVE_MUTEXES == 1)
1697               hMutex = xSemaphoreCreateRecursiveMutex ();
1698               #endif
1699             } else {
1700               hMutex = xSemaphoreCreateMutex ();
1701             }
1702           #endif
1703         }
1704       }
1705
1706       #if (configQUEUE_REGISTRY_SIZE > 0)
1707       if (hMutex != NULL) {
1708         if ((attr != NULL) && (attr->name != NULL)) {
1709           /* Only non-NULL name objects are added to the Queue Registry */
1710           vQueueAddToRegistry (hMutex, attr->name);
1711         }
1712       }
1713       #endif
1714
1715       if ((hMutex != NULL) && (rmtx != 0U)) {
1716         /* Set LSB as 'recursive mutex flag' */
1717         hMutex = (SemaphoreHandle_t)((uint32_t)hMutex | 1U);
1718       }
1719     }
1720   }
1721
1722   /* Return mutex ID */
1723   return ((osMutexId_t)hMutex);
1724 }
1725
1726 /*
1727   Acquire a Mutex or timeout if it is locked.
1728 */
1729 osStatus_t osMutexAcquire (osMutexId_t mutex_id, uint32_t timeout) {
1730   SemaphoreHandle_t hMutex;
1731   osStatus_t stat;
1732   uint32_t rmtx;
1733
1734   hMutex = (SemaphoreHandle_t)((uint32_t)mutex_id & ~1U);
1735
1736   /* Extract recursive mutex flag */
1737   rmtx = (uint32_t)mutex_id & 1U;
1738
1739   stat = osOK;
1740
1741   if (IRQ_Context() != 0U) {
1742     stat = osErrorISR;
1743   }
1744   else if (hMutex == NULL) {
1745     stat = osErrorParameter;
1746   }
1747   else {
1748     if (rmtx != 0U) {
1749       #if (configUSE_RECURSIVE_MUTEXES == 1)
1750       if (xSemaphoreTakeRecursive (hMutex, timeout) != pdPASS) {
1751         if (timeout != 0U) {
1752           stat = osErrorTimeout;
1753         } else {
1754           stat = osErrorResource;
1755         }
1756       }
1757       #endif
1758     }
1759     else {
1760       if (xSemaphoreTake (hMutex, timeout) != pdPASS) {
1761         if (timeout != 0U) {
1762           stat = osErrorTimeout;
1763         } else {
1764           stat = osErrorResource;
1765         }
1766       }
1767     }
1768   }
1769
1770   /* Return execution status */
1771   return (stat);
1772 }
1773
1774 /*
1775   Release a Mutex that was acquired by osMutexAcquire.
1776 */
1777 osStatus_t osMutexRelease (osMutexId_t mutex_id) {
1778   SemaphoreHandle_t hMutex;
1779   osStatus_t stat;
1780   uint32_t rmtx;
1781
1782   hMutex = (SemaphoreHandle_t)((uint32_t)mutex_id & ~1U);
1783
1784   /* Extract recursive mutex flag */
1785   rmtx = (uint32_t)mutex_id & 1U;
1786
1787   stat = osOK;
1788
1789   if (IRQ_Context() != 0U) {
1790     stat = osErrorISR;
1791   }
1792   else if (hMutex == NULL) {
1793     stat = osErrorParameter;
1794   }
1795   else {
1796     if (rmtx != 0U) {
1797       #if (configUSE_RECURSIVE_MUTEXES == 1)
1798       if (xSemaphoreGiveRecursive (hMutex) != pdPASS) {
1799         stat = osErrorResource;
1800       }
1801       #endif
1802     }
1803     else {
1804       if (xSemaphoreGive (hMutex) != pdPASS) {
1805         stat = osErrorResource;
1806       }
1807     }
1808   }
1809
1810   /* Return execution status */
1811   return (stat);
1812 }
1813
1814 /*
1815   Get Thread which owns a Mutex object.
1816 */
1817 osThreadId_t osMutexGetOwner (osMutexId_t mutex_id) {
1818   SemaphoreHandle_t hMutex;
1819   osThreadId_t owner;
1820
1821   hMutex = (SemaphoreHandle_t)((uint32_t)mutex_id & ~1U);
1822
1823   if ((IRQ_Context() != 0U) || (hMutex == NULL)) {
1824     owner = NULL;
1825   } else {
1826     owner = (osThreadId_t)xSemaphoreGetMutexHolder (hMutex);
1827   }
1828
1829   /* Return owner thread ID */
1830   return (owner);
1831 }
1832
1833 /*
1834   Delete a Mutex object.
1835 */
1836 osStatus_t osMutexDelete (osMutexId_t mutex_id) {
1837   osStatus_t stat;
1838 #ifndef USE_FreeRTOS_HEAP_1
1839   SemaphoreHandle_t hMutex;
1840
1841   hMutex = (SemaphoreHandle_t)((uint32_t)mutex_id & ~1U);
1842
1843   if (IRQ_Context() != 0U) {
1844     stat = osErrorISR;
1845   }
1846   else if (hMutex == NULL) {
1847     stat = osErrorParameter;
1848   }
1849   else {
1850     #if (configQUEUE_REGISTRY_SIZE > 0)
1851     vQueueUnregisterQueue (hMutex);
1852     #endif
1853     stat = osOK;
1854     vSemaphoreDelete (hMutex);
1855   }
1856 #else
1857   stat = osError;
1858 #endif
1859
1860   /* Return execution status */
1861   return (stat);
1862 }
1863 #endif /* (configUSE_OS2_MUTEX == 1) */
1864
1865
1866 /* ==== Semaphore Management Functions ==== */
1867
1868 /*
1869   Create and Initialize a Semaphore object.
1870 */
1871 osSemaphoreId_t osSemaphoreNew (uint32_t max_count, uint32_t initial_count, const osSemaphoreAttr_t *attr) {
1872   SemaphoreHandle_t hSemaphore;
1873   int32_t mem;
1874
1875   hSemaphore = NULL;
1876
1877   if ((IRQ_Context() == 0U) && (max_count > 0U) && (initial_count <= max_count)) {
1878     mem = -1;
1879
1880     if (attr != NULL) {
1881       if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticSemaphore_t))) {
1882         /* The memory for control block is provided, use static object */
1883         mem = 1;
1884       }
1885       else {
1886         if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
1887           /* Control block will be allocated from the dynamic pool */
1888           mem = 0;
1889         }
1890       }
1891     }
1892     else {
1893       mem = 0;
1894     }
1895
1896     if (mem != -1) {
1897       if (max_count == 1U) {
1898         if (mem == 1) {
1899           #if (configSUPPORT_STATIC_ALLOCATION == 1)
1900             hSemaphore = xSemaphoreCreateBinaryStatic ((StaticSemaphore_t *)attr->cb_mem);
1901           #endif
1902         }
1903         else {
1904           #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1905             hSemaphore = xSemaphoreCreateBinary();
1906           #endif
1907         }
1908
1909         if ((hSemaphore != NULL) && (initial_count != 0U)) {
1910           if (xSemaphoreGive (hSemaphore) != pdPASS) {
1911             vSemaphoreDelete (hSemaphore);
1912             hSemaphore = NULL;
1913           }
1914         }
1915       }
1916       else {
1917         if (mem == 1) {
1918           #if (configSUPPORT_STATIC_ALLOCATION == 1)
1919             hSemaphore = xSemaphoreCreateCountingStatic (max_count, initial_count, (StaticSemaphore_t *)attr->cb_mem);
1920           #endif
1921         }
1922         else {
1923           #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
1924             hSemaphore = xSemaphoreCreateCounting (max_count, initial_count);
1925           #endif
1926         }
1927       }
1928
1929       #if (configQUEUE_REGISTRY_SIZE > 0)
1930       if (hSemaphore != NULL) {
1931         if ((attr != NULL) && (attr->name != NULL)) {
1932           /* Only non-NULL name objects are added to the Queue Registry */
1933           vQueueAddToRegistry (hSemaphore, attr->name);
1934         }
1935       }
1936       #endif
1937     }
1938   }
1939
1940   /* Return semaphore ID */
1941   return ((osSemaphoreId_t)hSemaphore);
1942 }
1943
1944 /*
1945   Acquire a Semaphore token or timeout if no tokens are available.
1946 */
1947 osStatus_t osSemaphoreAcquire (osSemaphoreId_t semaphore_id, uint32_t timeout) {
1948   SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)semaphore_id;
1949   osStatus_t stat;
1950   BaseType_t yield;
1951
1952   stat = osOK;
1953
1954   if (hSemaphore == NULL) {
1955     stat = osErrorParameter;
1956   }
1957   else if (IRQ_Context() != 0U) {
1958     if (timeout != 0U) {
1959       stat = osErrorParameter;
1960     }
1961     else {
1962       yield = pdFALSE;
1963
1964       if (xSemaphoreTakeFromISR (hSemaphore, &yield) != pdPASS) {
1965         stat = osErrorResource;
1966       } else {
1967         portYIELD_FROM_ISR (yield);
1968       }
1969     }
1970   }
1971   else {
1972     if (xSemaphoreTake (hSemaphore, (TickType_t)timeout) != pdPASS) {
1973       if (timeout != 0U) {
1974         stat = osErrorTimeout;
1975       } else {
1976         stat = osErrorResource;
1977       }
1978     }
1979   }
1980
1981   /* Return execution status */
1982   return (stat);
1983 }
1984
1985 /*
1986   Release a Semaphore token up to the initial maximum count.
1987 */
1988 osStatus_t osSemaphoreRelease (osSemaphoreId_t semaphore_id) {
1989   SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)semaphore_id;
1990   osStatus_t stat;
1991   BaseType_t yield;
1992
1993   stat = osOK;
1994
1995   if (hSemaphore == NULL) {
1996     stat = osErrorParameter;
1997   }
1998   else if (IRQ_Context() != 0U) {
1999     yield = pdFALSE;
2000
2001     if (xSemaphoreGiveFromISR (hSemaphore, &yield) != pdTRUE) {
2002       stat = osErrorResource;
2003     } else {
2004       portYIELD_FROM_ISR (yield);
2005     }
2006   }
2007   else {
2008     if (xSemaphoreGive (hSemaphore) != pdPASS) {
2009       stat = osErrorResource;
2010     }
2011   }
2012
2013   /* Return execution status */
2014   return (stat);
2015 }
2016
2017 /*
2018   Get current Semaphore token count.
2019 */
2020 uint32_t osSemaphoreGetCount (osSemaphoreId_t semaphore_id) {
2021   SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)semaphore_id;
2022   uint32_t count;
2023
2024   if (hSemaphore == NULL) {
2025     count = 0U;
2026   }
2027   else if (IRQ_Context() != 0U) {
2028     count = (uint32_t)uxSemaphoreGetCountFromISR (hSemaphore);
2029   } else {
2030     count = (uint32_t)uxSemaphoreGetCount (hSemaphore);
2031   }
2032
2033   /* Return number of tokens */
2034   return (count);
2035 }
2036
2037 /*
2038   Delete a Semaphore object.
2039 */
2040 osStatus_t osSemaphoreDelete (osSemaphoreId_t semaphore_id) {
2041   SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)semaphore_id;
2042   osStatus_t stat;
2043
2044 #ifndef USE_FreeRTOS_HEAP_1
2045   if (IRQ_Context() != 0U) {
2046     stat = osErrorISR;
2047   }
2048   else if (hSemaphore == NULL) {
2049     stat = osErrorParameter;
2050   }
2051   else {
2052     #if (configQUEUE_REGISTRY_SIZE > 0)
2053     vQueueUnregisterQueue (hSemaphore);
2054     #endif
2055
2056     stat = osOK;
2057     vSemaphoreDelete (hSemaphore);
2058   }
2059 #else
2060   stat = osError;
2061 #endif
2062
2063   /* Return execution status */
2064   return (stat);
2065 }
2066
2067
2068 /* ==== Message Queue Management Functions ==== */
2069
2070 /*
2071   Create and Initialize a Message Queue object.
2072
2073   Limitations:
2074   - The memory for control block and and message data must be provided in the
2075     osThreadAttr_t structure in order to allocate object statically.
2076 */
2077 osMessageQueueId_t osMessageQueueNew (uint32_t msg_count, uint32_t msg_size, const osMessageQueueAttr_t *attr) {
2078   QueueHandle_t hQueue;
2079   int32_t mem;
2080
2081   hQueue = NULL;
2082
2083   if ((IRQ_Context() == 0U) && (msg_count > 0U) && (msg_size > 0U)) {
2084     mem = -1;
2085
2086     if (attr != NULL) {
2087       if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticQueue_t)) &&
2088           (attr->mq_mem != NULL) && (attr->mq_size >= (msg_count * msg_size))) {
2089         /* The memory for control block and message data is provided, use static object */
2090         mem = 1;
2091       }
2092       else {
2093         if ((attr->cb_mem == NULL) && (attr->cb_size == 0U) &&
2094             (attr->mq_mem == NULL) && (attr->mq_size == 0U)) {
2095           /* Control block will be allocated from the dynamic pool */
2096           mem = 0;
2097         }
2098       }
2099     }
2100     else {
2101       mem = 0;
2102     }
2103
2104     if (mem == 1) {
2105       #if (configSUPPORT_STATIC_ALLOCATION == 1)
2106         hQueue = xQueueCreateStatic (msg_count, msg_size, attr->mq_mem, attr->cb_mem);
2107       #endif
2108     }
2109     else {
2110       if (mem == 0) {
2111         #if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
2112           hQueue = xQueueCreate (msg_count, msg_size);
2113         #endif
2114       }
2115     }
2116
2117     #if (configQUEUE_REGISTRY_SIZE > 0)
2118     if (hQueue != NULL) {
2119       if ((attr != NULL) && (attr->name != NULL)) {
2120         /* Only non-NULL name objects are added to the Queue Registry */
2121         vQueueAddToRegistry (hQueue, attr->name);
2122       }
2123     }
2124     #endif
2125
2126   }
2127
2128   /* Return message queue ID */
2129   return ((osMessageQueueId_t)hQueue);
2130 }
2131
2132 /*
2133   Put a Message into a Queue or timeout if Queue is full.
2134
2135   Limitations:
2136   - Message priority is ignored
2137 */
2138 osStatus_t osMessageQueuePut (osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout) {
2139   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
2140   osStatus_t stat;
2141   BaseType_t yield;
2142
2143   (void)msg_prio; /* Message priority is ignored */
2144
2145   stat = osOK;
2146
2147   if (IRQ_Context() != 0U) {
2148     if ((hQueue == NULL) || (msg_ptr == NULL) || (timeout != 0U)) {
2149       stat = osErrorParameter;
2150     }
2151     else {
2152       yield = pdFALSE;
2153
2154       if (xQueueSendToBackFromISR (hQueue, msg_ptr, &yield) != pdTRUE) {
2155         stat = osErrorResource;
2156       } else {
2157         portYIELD_FROM_ISR (yield);
2158       }
2159     }
2160   }
2161   else {
2162     if ((hQueue == NULL) || (msg_ptr == NULL)) {
2163       stat = osErrorParameter;
2164     }
2165     else {
2166       if (xQueueSendToBack (hQueue, msg_ptr, (TickType_t)timeout) != pdPASS) {
2167         if (timeout != 0U) {
2168           stat = osErrorTimeout;
2169         } else {
2170           stat = osErrorResource;
2171         }
2172       }
2173     }
2174   }
2175
2176   /* Return execution status */
2177   return (stat);
2178 }
2179
2180 /*
2181   Get a Message from a Queue or timeout if Queue is empty.
2182
2183   Limitations:
2184   - Message priority is ignored
2185 */
2186 osStatus_t osMessageQueueGet (osMessageQueueId_t mq_id, void *msg_ptr, uint8_t *msg_prio, uint32_t timeout) {
2187   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
2188   osStatus_t stat;
2189   BaseType_t yield;
2190
2191   (void)msg_prio; /* Message priority is ignored */
2192
2193   stat = osOK;
2194
2195   if (IRQ_Context() != 0U) {
2196     if ((hQueue == NULL) || (msg_ptr == NULL) || (timeout != 0U)) {
2197       stat = osErrorParameter;
2198     }
2199     else {
2200       yield = pdFALSE;
2201
2202       if (xQueueReceiveFromISR (hQueue, msg_ptr, &yield) != pdPASS) {
2203         stat = osErrorResource;
2204       } else {
2205         portYIELD_FROM_ISR (yield);
2206       }
2207     }
2208   }
2209   else {
2210     if ((hQueue == NULL) || (msg_ptr == NULL)) {
2211       stat = osErrorParameter;
2212     }
2213     else {
2214       if (xQueueReceive (hQueue, msg_ptr, (TickType_t)timeout) != pdPASS) {
2215         if (timeout != 0U) {
2216           stat = osErrorTimeout;
2217         } else {
2218           stat = osErrorResource;
2219         }
2220       }
2221     }
2222   }
2223
2224   /* Return execution status */
2225   return (stat);
2226 }
2227
2228 /*
2229   Get maximum number of messages in a Message Queue.
2230 */
2231 uint32_t osMessageQueueGetCapacity (osMessageQueueId_t mq_id) {
2232   StaticQueue_t *mq = (StaticQueue_t *)mq_id;
2233   uint32_t capacity;
2234
2235   if (mq == NULL) {
2236     capacity = 0U;
2237   } else {
2238     /* capacity = pxQueue->uxLength */
2239     capacity = mq->uxDummy4[1];
2240   }
2241
2242   /* Return maximum number of messages */
2243   return (capacity);
2244 }
2245
2246 /*
2247   Get maximum message size in a Message Queue.
2248 */
2249 uint32_t osMessageQueueGetMsgSize (osMessageQueueId_t mq_id) {
2250   StaticQueue_t *mq = (StaticQueue_t *)mq_id;
2251   uint32_t size;
2252
2253   if (mq == NULL) {
2254     size = 0U;
2255   } else {
2256     /* size = pxQueue->uxItemSize */
2257     size = mq->uxDummy4[2];
2258   }
2259
2260   /* Return maximum message size */
2261   return (size);
2262 }
2263
2264 /*
2265   Get number of queued messages in a Message Queue.
2266 */
2267 uint32_t osMessageQueueGetCount (osMessageQueueId_t mq_id) {
2268   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
2269   UBaseType_t count;
2270
2271   if (hQueue == NULL) {
2272     count = 0U;
2273   }
2274   else if (IRQ_Context() != 0U) {
2275     count = uxQueueMessagesWaitingFromISR (hQueue);
2276   }
2277   else {
2278     count = uxQueueMessagesWaiting (hQueue);
2279   }
2280
2281   /* Return number of queued messages */
2282   return ((uint32_t)count);
2283 }
2284
2285 /*
2286   Get number of available slots for messages in a Message Queue.
2287 */
2288 uint32_t osMessageQueueGetSpace (osMessageQueueId_t mq_id) {
2289   StaticQueue_t *mq = (StaticQueue_t *)mq_id;
2290   uint32_t space;
2291   uint32_t isrm;
2292
2293   if (mq == NULL) {
2294     space = 0U;
2295   }
2296   else if (IRQ_Context() != 0U) {
2297     isrm = taskENTER_CRITICAL_FROM_ISR();
2298
2299     /* space = pxQueue->uxLength - pxQueue->uxMessagesWaiting; */
2300     space = mq->uxDummy4[1] - mq->uxDummy4[0];
2301
2302     taskEXIT_CRITICAL_FROM_ISR(isrm);
2303   }
2304   else {
2305     space = (uint32_t)uxQueueSpacesAvailable ((QueueHandle_t)mq);
2306   }
2307
2308   /* Return number of available slots */
2309   return (space);
2310 }
2311
2312 /*
2313   Reset a Message Queue to initial empty state.
2314 */
2315 osStatus_t osMessageQueueReset (osMessageQueueId_t mq_id) {
2316   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
2317   osStatus_t stat;
2318
2319   if (IRQ_Context() != 0U) {
2320     stat = osErrorISR;
2321   }
2322   else if (hQueue == NULL) {
2323     stat = osErrorParameter;
2324   }
2325   else {
2326     stat = osOK;
2327     (void)xQueueReset (hQueue);
2328   }
2329
2330   /* Return execution status */
2331   return (stat);
2332 }
2333
2334 /*
2335   Delete a Message Queue object.
2336 */
2337 osStatus_t osMessageQueueDelete (osMessageQueueId_t mq_id) {
2338   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
2339   osStatus_t stat;
2340
2341 #ifndef USE_FreeRTOS_HEAP_1
2342   if (IRQ_Context() != 0U) {
2343     stat = osErrorISR;
2344   }
2345   else if (hQueue == NULL) {
2346     stat = osErrorParameter;
2347   }
2348   else {
2349     #if (configQUEUE_REGISTRY_SIZE > 0)
2350     vQueueUnregisterQueue (hQueue);
2351     #endif
2352
2353     stat = osOK;
2354     vQueueDelete (hQueue);
2355   }
2356 #else
2357   stat = osError;
2358 #endif
2359
2360   /* Return execution status */
2361   return (stat);
2362 }
2363
2364
2365 /* ==== Memory Pool Management Functions ==== */
2366
2367 #ifdef FREERTOS_MPOOL_H_
2368 /* Static memory pool functions */
2369 static void  FreeBlock   (MemPool_t *mp, void *block);
2370 static void *AllocBlock  (MemPool_t *mp);
2371 static void *CreateBlock (MemPool_t *mp);
2372
2373 /*
2374   Create and Initialize a Memory Pool object.
2375 */
2376 osMemoryPoolId_t osMemoryPoolNew (uint32_t block_count, uint32_t block_size, const osMemoryPoolAttr_t *attr) {
2377   MemPool_t *mp;
2378   const char *name;
2379   int32_t mem_cb, mem_mp;
2380   uint32_t sz;
2381
2382   if (IRQ_Context() != 0U) {
2383     mp = NULL;
2384   }
2385   else if ((block_count == 0U) || (block_size == 0U)) {
2386     mp = NULL;
2387   }
2388   else {
2389     mp = NULL;
2390     sz = MEMPOOL_ARR_SIZE (block_count, block_size);
2391
2392     name = NULL;
2393     mem_cb = -1;
2394     mem_mp = -1;
2395
2396     if (attr != NULL) {
2397       if (attr->name != NULL) {
2398         name = attr->name;
2399       }
2400
2401       if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(MemPool_t))) {
2402         /* Static control block is provided */
2403         mem_cb = 1;
2404       }
2405       else if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
2406         /* Allocate control block memory on heap */
2407         mem_cb = 0;
2408       }
2409
2410       if ((attr->mp_mem == NULL) && (attr->mp_size == 0U)) {
2411         /* Allocate memory array on heap */
2412           mem_mp = 0;
2413       }
2414       else {
2415         if (attr->mp_mem != NULL) {
2416           /* Check if array is 4-byte aligned */
2417           if (((uint32_t)attr->mp_mem & 3U) == 0U) {
2418             /* Check if array big enough */
2419             if (attr->mp_size >= sz) {
2420               /* Static memory pool array is provided */
2421               mem_mp = 1;
2422             }
2423           }
2424         }
2425       }
2426     }
2427     else {
2428       /* Attributes not provided, allocate memory on heap */
2429       mem_cb = 0;
2430       mem_mp = 0;
2431     }
2432
2433     if (mem_cb == 0) {
2434       mp = pvPortMalloc (sizeof(MemPool_t));
2435     } else {
2436       mp = attr->cb_mem;
2437     }
2438
2439     if (mp != NULL) {
2440       /* Create a semaphore (max count == initial count == block_count) */
2441       #if (configSUPPORT_STATIC_ALLOCATION == 1)
2442         mp->sem = xSemaphoreCreateCountingStatic (block_count, block_count, &mp->mem_sem);
2443       #elif (configSUPPORT_DYNAMIC_ALLOCATION == 1)
2444         mp->sem = xSemaphoreCreateCounting (block_count, block_count);
2445       #else
2446         mp->sem = NULL;
2447       #endif
2448
2449       if (mp->sem != NULL) {
2450         /* Setup memory array */
2451         if (mem_mp == 0) {
2452           mp->mem_arr = pvPortMalloc (sz);
2453         } else {
2454           mp->mem_arr = attr->mp_mem;
2455         }
2456       }
2457     }
2458
2459     if ((mp != NULL) && (mp->mem_arr != NULL)) {
2460       /* Memory pool can be created */
2461       mp->head    = NULL;
2462       mp->mem_sz  = sz;
2463       mp->name    = name;
2464       mp->bl_sz   = block_size;
2465       mp->bl_cnt  = block_count;
2466       mp->n       = 0U;
2467
2468       /* Set heap allocated memory flags */
2469       mp->status = MPOOL_STATUS;
2470
2471       if (mem_cb == 0) {
2472         /* Control block on heap */
2473         mp->status |= 1U;
2474       }
2475       if (mem_mp == 0) {
2476         /* Memory array on heap */
2477         mp->status |= 2U;
2478       }
2479     }
2480     else {
2481       /* Memory pool cannot be created, release allocated resources */
2482       if ((mem_cb == 0) && (mp != NULL)) {
2483         /* Free control block memory */
2484         vPortFree (mp);
2485       }
2486       mp = NULL;
2487     }
2488   }
2489
2490   /* Return memory pool ID */
2491   return (mp);
2492 }
2493
2494 /*
2495   Get name of a Memory Pool object.
2496 */
2497 const char *osMemoryPoolGetName (osMemoryPoolId_t mp_id) {
2498   MemPool_t *mp = (osMemoryPoolId_t)mp_id;
2499   const char *p;
2500
2501   if (IRQ_Context() != 0U) {
2502     p = NULL;
2503   }
2504   else if (mp_id == NULL) {
2505     p = NULL;
2506   }
2507   else {
2508     p = mp->name;
2509   }
2510
2511   /* Return name as null-terminated string */
2512   return (p);
2513 }
2514
2515 /*
2516   Allocate a memory block from a Memory Pool.
2517 */
2518 void *osMemoryPoolAlloc (osMemoryPoolId_t mp_id, uint32_t timeout) {
2519   MemPool_t *mp;
2520   void *block;
2521   uint32_t isrm;
2522
2523   if (mp_id == NULL) {
2524     /* Invalid input parameters */
2525     block = NULL;
2526   }
2527   else {
2528     block = NULL;
2529
2530     mp = (MemPool_t *)mp_id;
2531
2532     if ((mp->status & MPOOL_STATUS) == MPOOL_STATUS) {
2533       if (IRQ_Context() != 0U) {
2534         if (timeout == 0U) {
2535           if (xSemaphoreTakeFromISR (mp->sem, NULL) == pdTRUE) {
2536             if ((mp->status & MPOOL_STATUS) == MPOOL_STATUS) {
2537               isrm  = taskENTER_CRITICAL_FROM_ISR();
2538
2539               /* Get a block from the free-list */
2540               block = AllocBlock(mp);
2541
2542               if (block == NULL) {
2543                 /* List of free blocks is empty, 'create' new block */
2544                 block = CreateBlock(mp);
2545               }
2546
2547               taskEXIT_CRITICAL_FROM_ISR(isrm);
2548             }
2549           }
2550         }
2551       }
2552       else {
2553         if (xSemaphoreTake (mp->sem, (TickType_t)timeout) == pdTRUE) {
2554           if ((mp->status & MPOOL_STATUS) == MPOOL_STATUS) {
2555             taskENTER_CRITICAL();
2556
2557             /* Get a block from the free-list */
2558             block = AllocBlock(mp);
2559
2560             if (block == NULL) {
2561               /* List of free blocks is empty, 'create' new block */
2562               block = CreateBlock(mp);
2563             }
2564
2565             taskEXIT_CRITICAL();
2566           }
2567         }
2568       }
2569     }
2570   }
2571
2572   /* Return memory block address */
2573   return (block);
2574 }
2575
2576 /*
2577   Return an allocated memory block back to a Memory Pool.
2578 */
2579 osStatus_t osMemoryPoolFree (osMemoryPoolId_t mp_id, void *block) {
2580   MemPool_t *mp;
2581   osStatus_t stat;
2582   uint32_t isrm;
2583   BaseType_t yield;
2584
2585   if ((mp_id == NULL) || (block == NULL)) {
2586     /* Invalid input parameters */
2587     stat = osErrorParameter;
2588   }
2589   else {
2590     mp = (MemPool_t *)mp_id;
2591
2592     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2593       /* Invalid object status */
2594       stat = osErrorResource;
2595     }
2596     else if ((block < (void *)&mp->mem_arr[0]) || (block > (void*)&mp->mem_arr[mp->mem_sz-1])) {
2597       /* Block pointer outside of memory array area */
2598       stat = osErrorParameter;
2599     }
2600     else {
2601       stat = osOK;
2602
2603       if (IRQ_Context() != 0U) {
2604         if (uxSemaphoreGetCountFromISR (mp->sem) == mp->bl_cnt) {
2605           stat = osErrorResource;
2606         }
2607         else {
2608           isrm = taskENTER_CRITICAL_FROM_ISR();
2609
2610           /* Add block to the list of free blocks */
2611           FreeBlock(mp, block);
2612
2613           taskEXIT_CRITICAL_FROM_ISR(isrm);
2614
2615           yield = pdFALSE;
2616           xSemaphoreGiveFromISR (mp->sem, &yield);
2617           portYIELD_FROM_ISR (yield);
2618         }
2619       }
2620       else {
2621         if (uxSemaphoreGetCount (mp->sem) == mp->bl_cnt) {
2622           stat = osErrorResource;
2623         }
2624         else {
2625           taskENTER_CRITICAL();
2626
2627           /* Add block to the list of free blocks */
2628           FreeBlock(mp, block);
2629
2630           taskEXIT_CRITICAL();
2631
2632           xSemaphoreGive (mp->sem);
2633         }
2634       }
2635     }
2636   }
2637
2638   /* Return execution status */
2639   return (stat);
2640 }
2641
2642 /*
2643   Get maximum number of memory blocks in a Memory Pool.
2644 */
2645 uint32_t osMemoryPoolGetCapacity (osMemoryPoolId_t mp_id) {
2646   MemPool_t *mp;
2647   uint32_t  n;
2648
2649   if (mp_id == NULL) {
2650     /* Invalid input parameters */
2651     n = 0U;
2652   }
2653   else {
2654     mp = (MemPool_t *)mp_id;
2655
2656     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2657       /* Invalid object status */
2658       n = 0U;
2659     }
2660     else {
2661       n = mp->bl_cnt;
2662     }
2663   }
2664
2665   /* Return maximum number of memory blocks */
2666   return (n);
2667 }
2668
2669 /*
2670   Get memory block size in a Memory Pool.
2671 */
2672 uint32_t osMemoryPoolGetBlockSize (osMemoryPoolId_t mp_id) {
2673   MemPool_t *mp;
2674   uint32_t  sz;
2675
2676   if (mp_id == NULL) {
2677     /* Invalid input parameters */
2678     sz = 0U;
2679   }
2680   else {
2681     mp = (MemPool_t *)mp_id;
2682
2683     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2684       /* Invalid object status */
2685       sz = 0U;
2686     }
2687     else {
2688       sz = mp->bl_sz;
2689     }
2690   }
2691
2692   /* Return memory block size in bytes */
2693   return (sz);
2694 }
2695
2696 /*
2697   Get number of memory blocks used in a Memory Pool.
2698 */
2699 uint32_t osMemoryPoolGetCount (osMemoryPoolId_t mp_id) {
2700   MemPool_t *mp;
2701   uint32_t  n;
2702
2703   if (mp_id == NULL) {
2704     /* Invalid input parameters */
2705     n = 0U;
2706   }
2707   else {
2708     mp = (MemPool_t *)mp_id;
2709
2710     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2711       /* Invalid object status */
2712       n = 0U;
2713     }
2714     else {
2715       if (IRQ_Context() != 0U) {
2716         n = uxSemaphoreGetCountFromISR (mp->sem);
2717       } else {
2718         n = uxSemaphoreGetCount        (mp->sem);
2719       }
2720
2721       n = mp->bl_cnt - n;
2722     }
2723   }
2724
2725   /* Return number of memory blocks used */
2726   return (n);
2727 }
2728
2729 /*
2730   Get number of memory blocks available in a Memory Pool.
2731 */
2732 uint32_t osMemoryPoolGetSpace (osMemoryPoolId_t mp_id) {
2733   MemPool_t *mp;
2734   uint32_t  n;
2735
2736   if (mp_id == NULL) {
2737     /* Invalid input parameters */
2738     n = 0U;
2739   }
2740   else {
2741     mp = (MemPool_t *)mp_id;
2742
2743     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2744       /* Invalid object status */
2745       n = 0U;
2746     }
2747     else {
2748       if (IRQ_Context() != 0U) {
2749         n = uxSemaphoreGetCountFromISR (mp->sem);
2750       } else {
2751         n = uxSemaphoreGetCount        (mp->sem);
2752       }
2753     }
2754   }
2755
2756   /* Return number of memory blocks available */
2757   return (n);
2758 }
2759
2760 /*
2761   Delete a Memory Pool object.
2762 */
2763 osStatus_t osMemoryPoolDelete (osMemoryPoolId_t mp_id) {
2764   MemPool_t *mp;
2765   osStatus_t stat;
2766
2767   if (mp_id == NULL) {
2768     /* Invalid input parameters */
2769     stat = osErrorParameter;
2770   }
2771   else if (IRQ_Context() != 0U) {
2772     stat = osErrorISR;
2773   }
2774   else {
2775     mp = (MemPool_t *)mp_id;
2776
2777     taskENTER_CRITICAL();
2778
2779     /* Invalidate control block status */
2780     mp->status  = mp->status & 3U;
2781
2782     /* Wake-up tasks waiting for pool semaphore */
2783     while (xSemaphoreGive (mp->sem) == pdTRUE);
2784
2785     mp->head    = NULL;
2786     mp->bl_sz   = 0U;
2787     mp->bl_cnt  = 0U;
2788
2789     if ((mp->status & 2U) != 0U) {
2790       /* Memory pool array allocated on heap */
2791       vPortFree (mp->mem_arr);
2792     }
2793     if ((mp->status & 1U) != 0U) {
2794       /* Memory pool control block allocated on heap */
2795       vPortFree (mp);
2796     }
2797
2798     taskEXIT_CRITICAL();
2799
2800     stat = osOK;
2801   }
2802
2803   /* Return execution status */
2804   return (stat);
2805 }
2806
2807 /*
2808   Create new block given according to the current block index.
2809 */
2810 static void *CreateBlock (MemPool_t *mp) {
2811   MemPoolBlock_t *p = NULL;
2812
2813   if (mp->n < mp->bl_cnt) {
2814     /* Unallocated blocks exist, set pointer to new block */
2815     p = (void *)(mp->mem_arr + (mp->bl_sz * mp->n));
2816
2817     /* Increment block index */
2818     mp->n += 1U;
2819   }
2820
2821   return (p);
2822 }
2823
2824 /*
2825   Allocate a block by reading the list of free blocks.
2826 */
2827 static void *AllocBlock (MemPool_t *mp) {
2828   MemPoolBlock_t *p = NULL;
2829
2830   if (mp->head != NULL) {
2831     /* List of free block exists, get head block */
2832     p = mp->head;
2833
2834     /* Head block is now next on the list */
2835     mp->head = p->next;
2836   }
2837
2838   return (p);
2839 }
2840
2841 /*
2842   Free block by putting it to the list of free blocks.
2843 */
2844 static void FreeBlock (MemPool_t *mp, void *block) {
2845   MemPoolBlock_t *p = block;
2846
2847   /* Store current head into block memory space */
2848   p->next = mp->head;
2849
2850   /* Store current block as new head */
2851   mp->head = p;
2852 }
2853 #endif /* FREERTOS_MPOOL_H_ */
2854 /*---------------------------------------------------------------------------*/
2855
2856 /* Callback function prototypes */
2857 extern void vApplicationIdleHook (void);
2858 extern void vApplicationMallocFailedHook (void);
2859 extern void vApplicationDaemonTaskStartupHook (void);
2860
2861 /**
2862   Dummy implementation of the callback function vApplicationIdleHook().
2863 */
2864 #if (configUSE_IDLE_HOOK == 1)
2865 __WEAK void vApplicationIdleHook (void){}
2866 #endif
2867
2868 /**
2869   Dummy implementation of the callback function vApplicationTickHook().
2870 */
2871 #if (configUSE_TICK_HOOK == 1)
2872  __WEAK void vApplicationTickHook (void){}
2873 #endif
2874
2875 /**
2876   Dummy implementation of the callback function vApplicationMallocFailedHook().
2877 */
2878 #if (configUSE_MALLOC_FAILED_HOOK == 1)
2879 __WEAK void vApplicationMallocFailedHook (void) {
2880   /* Assert when malloc failed hook is enabled but no application defined function exists */
2881   configASSERT(0);
2882 }
2883 #endif
2884
2885 /**
2886   Dummy implementation of the callback function vApplicationDaemonTaskStartupHook().
2887 */
2888 #if (configUSE_DAEMON_TASK_STARTUP_HOOK == 1)
2889 __WEAK void vApplicationDaemonTaskStartupHook (void){}
2890 #endif
2891
2892 /**
2893   Dummy implementation of the callback function vApplicationStackOverflowHook().
2894 */
2895 #if (configCHECK_FOR_STACK_OVERFLOW > 0)
2896 __WEAK void vApplicationStackOverflowHook (TaskHandle_t xTask, char *pcTaskName) {
2897   (void)xTask;
2898   (void)pcTaskName;
2899
2900   /* Assert when stack overflow is enabled but no application defined function exists */
2901   configASSERT(0);
2902 }
2903 #endif
2904
2905 /*---------------------------------------------------------------------------*/
2906 #if (configSUPPORT_STATIC_ALLOCATION == 1)
2907 /*
2908   vApplicationGetIdleTaskMemory gets called when configSUPPORT_STATIC_ALLOCATION
2909   equals to 1 and is required for static memory allocation support.
2910 */
2911 __WEAK void vApplicationGetIdleTaskMemory (StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize) {
2912   /* Idle task control block and stack */
2913   static StaticTask_t Idle_TCB;
2914   static StackType_t  Idle_Stack[configMINIMAL_STACK_SIZE];
2915
2916   *ppxIdleTaskTCBBuffer   = &Idle_TCB;
2917   *ppxIdleTaskStackBuffer = &Idle_Stack[0];
2918   *pulIdleTaskStackSize   = (uint32_t)configMINIMAL_STACK_SIZE;
2919 }
2920
2921 /*
2922   vApplicationGetTimerTaskMemory gets called when configSUPPORT_STATIC_ALLOCATION
2923   equals to 1 and is required for static memory allocation support.
2924 */
2925 __WEAK void vApplicationGetTimerTaskMemory (StaticTask_t **ppxTimerTaskTCBBuffer, StackType_t **ppxTimerTaskStackBuffer, uint32_t *pulTimerTaskStackSize) {
2926   /* Timer task control block and stack */
2927   static StaticTask_t Timer_TCB;
2928   static StackType_t  Timer_Stack[configTIMER_TASK_STACK_DEPTH];
2929
2930   *ppxTimerTaskTCBBuffer   = &Timer_TCB;
2931   *ppxTimerTaskStackBuffer = &Timer_Stack[0];
2932   *pulTimerTaskStackSize   = (uint32_t)configTIMER_TASK_STACK_DEPTH;
2933 }
2934 #endif