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

   1 /* --------------------------------------------------------------------------
   2  * Copyright (c) 2013-2019 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
  34 #include "freertos_mpool.h"             // osMemoryPool definitions
  35 #include "freertos_os2.h"               // Configuration check and setup
  36
  37 /*---------------------------------------------------------------------------*/
  38 #ifndef __ARM_ARCH_6M__
  39   #define __ARM_ARCH_6M__         0
  40 #endif
  41 #ifndef __ARM_ARCH_7M__
  42   #define __ARM_ARCH_7M__         0
  43 #endif
  44 #ifndef __ARM_ARCH_7EM__
  45   #define __ARM_ARCH_7EM__        0
  46 #endif
  47 #ifndef __ARM_ARCH_8M_MAIN__
  48   #define __ARM_ARCH_8M_MAIN__    0
  49 #endif
  50 #ifndef __ARM_ARCH_7A__
  51   #define __ARM_ARCH_7A__         0
  52 #endif
  53
  54 #if   ((__ARM_ARCH_7M__      == 1U) || \
  55        (__ARM_ARCH_7EM__     == 1U) || \
  56        (__ARM_ARCH_8M_MAIN__ == 1U))
  57 #define IS_IRQ_MASKED()           ((__get_PRIMASK() != 0U) || (__get_BASEPRI() != 0U))
  58 #elif  (__ARM_ARCH_6M__      == 1U)
  59 #define IS_IRQ_MASKED()           (__get_PRIMASK() != 0U)
  60 #elif (__ARM_ARCH_7A__       == 1U)
  61 /* CPSR mask bits */
  62 #define CPSR_MASKBIT_I            0x80U
  63
  64 #define IS_IRQ_MASKED()           ((__get_CPSR() & CPSR_MASKBIT_I) != 0U)
  65 #else
  66 #define IS_IRQ_MASKED()           (__get_PRIMASK() != 0U)
  67 #endif
  68
  69 #if    (__ARM_ARCH_7A__      == 1U)
  70 /* CPSR mode bitmasks */
  71 #define CPSR_MODE_USER            0x10U
  72 #define CPSR_MODE_SYSTEM          0x1FU
  73
  74 #define IS_IRQ_MODE()             ((__get_mode() != CPSR_MODE_USER) && (__get_mode() != CPSR_MODE_SYSTEM))
  75 #else
  76 #define IS_IRQ_MODE()             (__get_IPSR() != 0U)
  77 #endif
  78
  79 #define IS_IRQ()                  (IS_IRQ_MODE() || (IS_IRQ_MASKED() && (KernelState == osKernelRunning)))
  80
  81 /* Limits */
  82 #define MAX_BITS_TASK_NOTIFY      31U
  83 #define MAX_BITS_EVENT_GROUPS     24U
  84
  85 #define THREAD_FLAGS_INVALID_BITS (~((1UL << MAX_BITS_TASK_NOTIFY)  - 1U))
  86 #define EVENT_FLAGS_INVALID_BITS  (~((1UL << MAX_BITS_EVENT_GROUPS) - 1U))
  87
  88 /* Kernel version and identification string definition (major.minor.rev: mmnnnrrrr dec) */
  89 #define KERNEL_VERSION            (((uint32_t)tskKERNEL_VERSION_MAJOR * 10000000UL) | \
  90                                    ((uint32_t)tskKERNEL_VERSION_MINOR *    10000UL) | \
  91                                    ((uint32_t)tskKERNEL_VERSION_BUILD *        1UL))
  92
  93 #define KERNEL_ID                 ("FreeRTOS " tskKERNEL_VERSION_NUMBER)
  94
  95 /* Timer callback information structure definition */
  96 typedef struct {
  97   osTimerFunc_t func;
  98   void         *arg;
  99 } TimerCallback_t;
 100
 101 /* Kernel initialization state */
 102 static osKernelState_t KernelState = osKernelInactive;
 103
 104 /*
 105   Heap region definition used by heap_5 variant
 106
 107   Define configAPPLICATION_ALLOCATED_HEAP as nonzero value in FreeRTOSConfig.h if
 108   heap regions are already defined and vPortDefineHeapRegions is called in application.
 109
 110   Otherwise vPortDefineHeapRegions will be called by osKernelInitialize using
 111   definition configHEAP_5_REGIONS as parameter. Overriding configHEAP_5_REGIONS
 112   is possible by defining it globally or in FreeRTOSConfig.h.
 113 */
 114 #if defined(USE_FreeRTOS_HEAP_5)
 115 #if (configAPPLICATION_ALLOCATED_HEAP == 0)
 116   /*
 117     FreeRTOS heap is not defined by the application.
 118     Single region of size configTOTAL_HEAP_SIZE (defined in FreeRTOSConfig.h)
 119     is provided by default. Define configHEAP_5_REGIONS to provide custom
 120     HeapRegion_t array.
 121   */
 122   #define HEAP_5_REGION_SETUP   1
 123
 124   #ifndef configHEAP_5_REGIONS
 125     #define configHEAP_5_REGIONS xHeapRegions
 126
 127     static uint8_t ucHeap[configTOTAL_HEAP_SIZE];
 128
 129     static HeapRegion_t xHeapRegions[] = {
 130       { ucHeap, configTOTAL_HEAP_SIZE },
 131       { NULL,   0                     }
 132     };
 133   #else
 134     /* Global definition is provided to override default heap array */
 135     extern HeapRegion_t configHEAP_5_REGIONS[];
 136   #endif
 137 #else
 138   /*
 139     The application already defined the array used for the FreeRTOS heap and
 140     called vPortDefineHeapRegions to initialize heap.
 141   */
 142   #define HEAP_5_REGION_SETUP   0
 143 #endif /* configAPPLICATION_ALLOCATED_HEAP */
 144 #endif /* USE_FreeRTOS_HEAP_5 */
 145
 146 #if defined(SysTick)
 147 #undef SysTick_Handler
 148
 149 /* CMSIS SysTick interrupt handler prototype */
 150 extern void SysTick_Handler     (void);
 151 /* FreeRTOS tick timer interrupt handler prototype */
 152 extern void xPortSysTickHandler (void);
 153
 154 /*
 155   SysTick handler implementation that also clears overflow flag.
 156 */
 157 void SysTick_Handler (void) {
 158   /* Clear overflow flag */
 159   SysTick->CTRL;
 160
 161   if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
 162     /* Call tick handler */
 163     xPortSysTickHandler();
 164   }
 165 }
 166 #endif /* SysTick */
 167
 168 /*
 169   Setup SVC to reset value.
 170 */
 171 __STATIC_INLINE void SVC_Setup (void) {
 172 #if (__ARM_ARCH_7A__ == 0U)
 173   /* Service Call interrupt might be configured before kernel start     */
 174   /* and when its priority is lower or equal to BASEPRI, svc intruction */
 175   /* causes a Hard Fault.                                               */
 176   NVIC_SetPriority (SVCall_IRQn, 0U);
 177 #endif
 178 }
 179
 180 /*
 181   Function macro used to retrieve semaphore count from ISR
 182 */
 183 #ifndef uxSemaphoreGetCountFromISR
 184 #define uxSemaphoreGetCountFromISR( xSemaphore ) uxQueueMessagesWaitingFromISR( ( QueueHandle_t ) ( xSemaphore ) )
 185 #endif
 186
 187 /*---------------------------------------------------------------------------*/
 188
 189 osStatus_t osKernelInitialize (void) {
 190   osStatus_t stat;
 191
 192   if (IS_IRQ()) {
 193     stat = osErrorISR;
 194   }
 195   else {
 196     if (KernelState == osKernelInactive) {
 197       #if defined(USE_TRACE_EVENT_RECORDER)
 198         EvrFreeRTOSSetup(0U);
 199       #endif
 200       #if defined(USE_FreeRTOS_HEAP_5) && (HEAP_5_REGION_SETUP == 1)
 201         vPortDefineHeapRegions (configHEAP_5_REGIONS);
 202       #endif
 203       KernelState = osKernelReady;
 204       stat = osOK;
 205     } else {
 206       stat = osError;
 207     }
 208   }
 209
 210   return (stat);
 211 }
 212
 213 osStatus_t osKernelGetInfo (osVersion_t *version, char *id_buf, uint32_t id_size) {
 214
 215   if (version != NULL) {
 216     /* Version encoding is major.minor.rev: mmnnnrrrr dec */
 217     version->api    = KERNEL_VERSION;
 218     version->kernel = KERNEL_VERSION;
 219   }
 220
 221   if ((id_buf != NULL) && (id_size != 0U)) {
 222     if (id_size > sizeof(KERNEL_ID)) {
 223       id_size = sizeof(KERNEL_ID);
 224     }
 225     memcpy(id_buf, KERNEL_ID, id_size);
 226   }
 227
 228   return (osOK);
 229 }
 230
 231 osKernelState_t osKernelGetState (void) {
 232   osKernelState_t state;
 233
 234   switch (xTaskGetSchedulerState()) {
 235     case taskSCHEDULER_RUNNING:
 236       state = osKernelRunning;
 237       break;
 238
 239     case taskSCHEDULER_SUSPENDED:
 240       state = osKernelLocked;
 241       break;
 242
 243     case taskSCHEDULER_NOT_STARTED:
 244     default:
 245       if (KernelState == osKernelReady) {
 246         state = osKernelReady;
 247       } else {
 248         state = osKernelInactive;
 249       }
 250       break;
 251   }
 252
 253   return (state);
 254 }
 255
 256 osStatus_t osKernelStart (void) {
 257   osStatus_t stat;
 258
 259   if (IS_IRQ()) {
 260     stat = osErrorISR;
 261   }
 262   else {
 263     if (KernelState == osKernelReady) {
 264       /* Ensure SVC priority is at the reset value */
 265       SVC_Setup();
 266       /* Change state to enable IRQ masking check */
 267       KernelState = osKernelRunning;
 268       /* Start the kernel scheduler */
 269       vTaskStartScheduler();
 270       stat = osOK;
 271     } else {
 272       stat = osError;
 273     }
 274   }
 275
 276   return (stat);
 277 }
 278
 279 int32_t osKernelLock (void) {
 280   int32_t lock;
 281
 282   if (IS_IRQ()) {
 283     lock = (int32_t)osErrorISR;
 284   }
 285   else {
 286     switch (xTaskGetSchedulerState()) {
 287       case taskSCHEDULER_SUSPENDED:
 288         lock = 1;
 289         break;
 290
 291       case taskSCHEDULER_RUNNING:
 292         vTaskSuspendAll();
 293         lock = 0;
 294         break;
 295
 296       case taskSCHEDULER_NOT_STARTED:
 297       default:
 298         lock = (int32_t)osError;
 299         break;
 300     }
 301   }
 302
 303   return (lock);
 304 }
 305
 306 int32_t osKernelUnlock (void) {
 307   int32_t lock;
 308
 309   if (IS_IRQ()) {
 310     lock = (int32_t)osErrorISR;
 311   }
 312   else {
 313     switch (xTaskGetSchedulerState()) {
 314       case taskSCHEDULER_SUSPENDED:
 315         lock = 1;
 316
 317         if (xTaskResumeAll() != pdTRUE) {
 318           if (xTaskGetSchedulerState() == taskSCHEDULER_SUSPENDED) {
 319             lock = (int32_t)osError;
 320           }
 321         }
 322         break;
 323
 324       case taskSCHEDULER_RUNNING:
 325         lock = 0;
 326         break;
 327
 328       case taskSCHEDULER_NOT_STARTED:
 329       default:
 330         lock = (int32_t)osError;
 331         break;
 332     }
 333   }
 334
 335   return (lock);
 336 }
 337
 338 int32_t osKernelRestoreLock (int32_t lock) {
 339
 340   if (IS_IRQ()) {
 341     lock = (int32_t)osErrorISR;
 342   }
 343   else {
 344     switch (xTaskGetSchedulerState()) {
 345       case taskSCHEDULER_SUSPENDED:
 346       case taskSCHEDULER_RUNNING:
 347         if (lock == 1) {
 348           vTaskSuspendAll();
 349         }
 350         else {
 351           if (lock != 0) {
 352             lock = (int32_t)osError;
 353           }
 354           else {
 355             if (xTaskResumeAll() != pdTRUE) {
 356               if (xTaskGetSchedulerState() != taskSCHEDULER_RUNNING) {
 357                 lock = (int32_t)osError;
 358               }
 359             }
 360           }
 361         }
 362         break;
 363
 364       case taskSCHEDULER_NOT_STARTED:
 365       default:
 366         lock = (int32_t)osError;
 367         break;
 368     }
 369   }
 370
 371   return (lock);
 372 }
 373
 374 uint32_t osKernelGetTickCount (void) {
 375   TickType_t ticks;
 376
 377   if (IS_IRQ()) {
 378     ticks = xTaskGetTickCountFromISR();
 379   } else {
 380     ticks = xTaskGetTickCount();
 381   }
 382
 383   return (ticks);
 384 }
 385
 386 uint32_t osKernelGetTickFreq (void) {
 387   return (configTICK_RATE_HZ);
 388 }
 389
 390 uint32_t osKernelGetSysTimerCount (void) {
 391   uint32_t irqmask = IS_IRQ_MASKED();
 392   TickType_t ticks;
 393   uint32_t val;
 394
 395   __disable_irq();
 396
 397   ticks = xTaskGetTickCount();
 398   val   = OS_Tick_GetCount();
 399
 400   if (OS_Tick_GetOverflow() != 0U) {
 401     val = OS_Tick_GetCount();
 402     ticks++;
 403   }
 404   val += ticks * OS_Tick_GetInterval();
 405
 406   if (irqmask == 0U) {
 407     __enable_irq();
 408   }
 409
 410   return (val);
 411 }
 412
 413 uint32_t osKernelGetSysTimerFreq (void) {
 414   return (configCPU_CLOCK_HZ);
 415 }
 416
 417 /*---------------------------------------------------------------------------*/
 418
 419 osThreadId_t osThreadNew (osThreadFunc_t func, void *argument, const osThreadAttr_t *attr) {
 420   const char *name;
 421   uint32_t stack;
 422   TaskHandle_t hTask;
 423   UBaseType_t prio;
 424   int32_t mem;
 425
 426   hTask = NULL;
 427
 428   if (!IS_IRQ() && (func != NULL)) {
 429     stack = configMINIMAL_STACK_SIZE;
 430     prio  = (UBaseType_t)osPriorityNormal;
 431
 432     name = NULL;
 433     mem  = -1;
 434
 435     if (attr != NULL) {
 436       if (attr->name != NULL) {
 437         name = attr->name;
 438       }
 439       if (attr->priority != osPriorityNone) {
 440         prio = (UBaseType_t)attr->priority;
 441       }
 442
 443       if ((prio < osPriorityIdle) || (prio > osPriorityISR) || ((attr->attr_bits & osThreadJoinable) == osThreadJoinable)) {
 444         return (NULL);
 445       }
 446
 447       if (attr->stack_size > 0U) {
 448         /* In FreeRTOS stack is not in bytes, but in sizeof(StackType_t) which is 4 on ARM ports.       */
 449         /* Stack size should be therefore 4 byte aligned in order to avoid division caused side effects */
 450         stack = attr->stack_size / sizeof(StackType_t);
 451       }
 452
 453       if ((attr->cb_mem    != NULL) && (attr->cb_size    >= sizeof(StaticTask_t)) &&
 454           (attr->stack_mem != NULL) && (attr->stack_size >  0U)) {
 455         mem = 1;
 456       }
 457       else {
 458         if ((attr->cb_mem == NULL) && (attr->cb_size == 0U) && (attr->stack_mem == NULL)) {
 459           mem = 0;
 460         }
 461       }
 462     }
 463     else {
 464       mem = 0;
 465     }
 466
 467     if (mem == 1) {
 468       hTask = xTaskCreateStatic ((TaskFunction_t)func, name, stack, argument, prio, (StackType_t  *)attr->stack_mem,
 469                                                                                     (StaticTask_t *)attr->cb_mem);
 470     }
 471     else {
 472       if (mem == 0) {
 473         if (xTaskCreate ((TaskFunction_t)func, name, (uint16_t)stack, argument, prio, &hTask) != pdPASS) {
 474           hTask = NULL;
 475         }
 476       }
 477     }
 478   }
 479
 480   return ((osThreadId_t)hTask);
 481 }
 482
 483 const char *osThreadGetName (osThreadId_t thread_id) {
 484   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 485   const char *name;
 486
 487   if (IS_IRQ() || (hTask == NULL)) {
 488     name = NULL;
 489   } else {
 490     name = pcTaskGetName (hTask);
 491   }
 492
 493   return (name);
 494 }
 495
 496 osThreadId_t osThreadGetId (void) {
 497   osThreadId_t id;
 498
 499   id = (osThreadId_t)xTaskGetCurrentTaskHandle();
 500
 501   return (id);
 502 }
 503
 504 osThreadState_t osThreadGetState (osThreadId_t thread_id) {
 505   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 506   osThreadState_t state;
 507
 508   if (IS_IRQ() || (hTask == NULL)) {
 509     state = osThreadError;
 510   }
 511   else {
 512     switch (eTaskGetState (hTask)) {
 513       case eRunning:   state = osThreadRunning;    break;
 514       case eReady:     state = osThreadReady;      break;
 515       case eBlocked:
 516       case eSuspended: state = osThreadBlocked;    break;
 517       case eDeleted:   state = osThreadTerminated; break;
 518       case eInvalid:
 519       default:         state = osThreadError;      break;
 520     }
 521   }
 522
 523   return (state);
 524 }
 525
 526 uint32_t osThreadGetStackSpace (osThreadId_t thread_id) {
 527   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 528   uint32_t sz;
 529
 530   if (IS_IRQ() || (hTask == NULL)) {
 531     sz = 0U;
 532   } else {
 533     sz = (uint32_t)(uxTaskGetStackHighWaterMark(hTask) * sizeof(StackType_t));
 534   }
 535
 536   return (sz);
 537 }
 538
 539 osStatus_t osThreadSetPriority (osThreadId_t thread_id, osPriority_t priority) {
 540   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 541   osStatus_t stat;
 542
 543   if (IS_IRQ()) {
 544     stat = osErrorISR;
 545   }
 546   else if ((hTask == NULL) || (priority < osPriorityIdle) || (priority > osPriorityISR)) {
 547     stat = osErrorParameter;
 548   }
 549   else {
 550     stat = osOK;
 551     vTaskPrioritySet (hTask, (UBaseType_t)priority);
 552   }
 553
 554   return (stat);
 555 }
 556
 557 osPriority_t osThreadGetPriority (osThreadId_t thread_id) {
 558   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 559   osPriority_t prio;
 560
 561   if (IS_IRQ() || (hTask == NULL)) {
 562     prio = osPriorityError;
 563   } else {
 564     prio = (osPriority_t)((int32_t)uxTaskPriorityGet (hTask));
 565   }
 566
 567   return (prio);
 568 }
 569
 570 osStatus_t osThreadYield (void) {
 571   osStatus_t stat;
 572
 573   if (IS_IRQ()) {
 574     stat = osErrorISR;
 575   } else {
 576     stat = osOK;
 577     taskYIELD();
 578   }
 579
 580   return (stat);
 581 }
 582
 583 osStatus_t osThreadSuspend (osThreadId_t thread_id) {
 584   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 585   osStatus_t stat;
 586
 587   if (IS_IRQ()) {
 588     stat = osErrorISR;
 589   }
 590   else if (hTask == NULL) {
 591     stat = osErrorParameter;
 592   }
 593   else {
 594     stat = osOK;
 595     vTaskSuspend (hTask);
 596   }
 597
 598   return (stat);
 599 }
 600
 601 osStatus_t osThreadResume (osThreadId_t thread_id) {
 602   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 603   osStatus_t stat;
 604
 605   if (IS_IRQ()) {
 606     stat = osErrorISR;
 607   }
 608   else if (hTask == NULL) {
 609     stat = osErrorParameter;
 610   }
 611   else {
 612     stat = osOK;
 613     vTaskResume (hTask);
 614   }
 615
 616   return (stat);
 617 }
 618
 619 __NO_RETURN void osThreadExit (void) {
 620 #ifndef USE_FreeRTOS_HEAP_1
 621   vTaskDelete (NULL);
 622 #endif
 623   for (;;);
 624 }
 625
 626 osStatus_t osThreadTerminate (osThreadId_t thread_id) {
 627   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 628   osStatus_t stat;
 629 #ifndef USE_FreeRTOS_HEAP_1
 630   eTaskState tstate;
 631
 632   if (IS_IRQ()) {
 633     stat = osErrorISR;
 634   }
 635   else if (hTask == NULL) {
 636     stat = osErrorParameter;
 637   }
 638   else {
 639     tstate = eTaskGetState (hTask);
 640
 641     if (tstate != eDeleted) {
 642       stat = osOK;
 643       vTaskDelete (hTask);
 644     } else {
 645       stat = osErrorResource;
 646     }
 647   }
 648 #else
 649   stat = osError;
 650 #endif
 651
 652   return (stat);
 653 }
 654
 655 uint32_t osThreadGetCount (void) {
 656   uint32_t count;
 657
 658   if (IS_IRQ()) {
 659     count = 0U;
 660   } else {
 661     count = uxTaskGetNumberOfTasks();
 662   }
 663
 664   return (count);
 665 }
 666
 667 uint32_t osThreadEnumerate (osThreadId_t *thread_array, uint32_t array_items) {
 668   uint32_t i, count;
 669   TaskStatus_t *task;
 670
 671   if (IS_IRQ() || (thread_array == NULL) || (array_items == 0U)) {
 672     count = 0U;
 673   } else {
 674     vTaskSuspendAll();
 675
 676     count = uxTaskGetNumberOfTasks();
 677     task  = pvPortMalloc (count * sizeof(TaskStatus_t));
 678
 679     if (task != NULL) {
 680       count = uxTaskGetSystemState (task, count, NULL);
 681
 682       for (i = 0U; (i < count) && (i < array_items); i++) {
 683         thread_array[i] = (osThreadId_t)task[i].xHandle;
 684       }
 685       count = i;
 686     }
 687     (void)xTaskResumeAll();
 688
 689     vPortFree (task);
 690   }
 691
 692   return (count);
 693 }
 694
 695 uint32_t osThreadFlagsSet (osThreadId_t thread_id, uint32_t flags) {
 696   TaskHandle_t hTask = (TaskHandle_t)thread_id;
 697   uint32_t rflags;
 698   BaseType_t yield;
 699
 700   if ((hTask == NULL) || ((flags & THREAD_FLAGS_INVALID_BITS) != 0U)) {
 701     rflags = (uint32_t)osErrorParameter;
 702   }
 703   else {
 704     rflags = (uint32_t)osError;
 705
 706     if (IS_IRQ()) {
 707       yield = pdFALSE;
 708
 709       (void)xTaskNotifyFromISR (hTask, flags, eSetBits, &yield);
 710       (void)xTaskNotifyAndQueryFromISR (hTask, 0, eNoAction, &rflags, NULL);
 711
 712       portYIELD_FROM_ISR (yield);
 713     }
 714     else {
 715       (void)xTaskNotify (hTask, flags, eSetBits);
 716       (void)xTaskNotifyAndQuery (hTask, 0, eNoAction, &rflags);
 717     }
 718   }
 719   /* Return flags after setting */
 720   return (rflags);
 721 }
 722
 723 uint32_t osThreadFlagsClear (uint32_t flags) {
 724   TaskHandle_t hTask;
 725   uint32_t rflags, cflags;
 726
 727   if (IS_IRQ()) {
 728     rflags = (uint32_t)osErrorISR;
 729   }
 730   else if ((flags & THREAD_FLAGS_INVALID_BITS) != 0U) {
 731     rflags = (uint32_t)osErrorParameter;
 732   }
 733   else {
 734     hTask = xTaskGetCurrentTaskHandle();
 735
 736     if (xTaskNotifyAndQuery (hTask, 0, eNoAction, &cflags) == pdPASS) {
 737       rflags = cflags;
 738       cflags &= ~flags;
 739
 740       if (xTaskNotify (hTask, cflags, eSetValueWithOverwrite) != pdPASS) {
 741         rflags = (uint32_t)osError;
 742       }
 743     }
 744     else {
 745       rflags = (uint32_t)osError;
 746     }
 747   }
 748
 749   /* Return flags before clearing */
 750   return (rflags);
 751 }
 752
 753 uint32_t osThreadFlagsGet (void) {
 754   TaskHandle_t hTask;
 755   uint32_t rflags;
 756
 757   if (IS_IRQ()) {
 758     rflags = (uint32_t)osErrorISR;
 759   }
 760   else {
 761     hTask = xTaskGetCurrentTaskHandle();
 762
 763     if (xTaskNotifyAndQuery (hTask, 0, eNoAction, &rflags) != pdPASS) {
 764       rflags = (uint32_t)osError;
 765     }
 766   }
 767
 768   return (rflags);
 769 }
 770
 771 uint32_t osThreadFlagsWait (uint32_t flags, uint32_t options, uint32_t timeout) {
 772   uint32_t rflags, nval;
 773   uint32_t clear;
 774   TickType_t t0, td, tout;
 775   BaseType_t rval;
 776
 777   if (IS_IRQ()) {
 778     rflags = (uint32_t)osErrorISR;
 779   }
 780   else if ((flags & THREAD_FLAGS_INVALID_BITS) != 0U) {
 781     rflags = (uint32_t)osErrorParameter;
 782   }
 783   else {
 784     if ((options & osFlagsNoClear) == osFlagsNoClear) {
 785       clear = 0U;
 786     } else {
 787       clear = flags;
 788     }
 789
 790     rflags = 0U;
 791     tout   = timeout;
 792
 793     t0 = xTaskGetTickCount();
 794     do {
 795       rval = xTaskNotifyWait (0, clear, &nval, tout);
 796
 797       if (rval == pdPASS) {
 798         rflags &= flags;
 799         rflags |= nval;
 800
 801         if ((options & osFlagsWaitAll) == osFlagsWaitAll) {
 802           if ((flags & rflags) == flags) {
 803             break;
 804           } else {
 805             if (timeout == 0U) {
 806               rflags = (uint32_t)osErrorResource;
 807               break;
 808             }
 809           }
 810         }
 811         else {
 812           if ((flags & rflags) != 0) {
 813             break;
 814           } else {
 815             if (timeout == 0U) {
 816               rflags = (uint32_t)osErrorResource;
 817               break;
 818             }
 819           }
 820         }
 821
 822         /* Update timeout */
 823         td = xTaskGetTickCount() - t0;
 824
 825         if (td > tout) {
 826           tout  = 0;
 827         } else {
 828           tout -= td;
 829         }
 830       }
 831       else {
 832         if (timeout == 0) {
 833           rflags = (uint32_t)osErrorResource;
 834         } else {
 835           rflags = (uint32_t)osErrorTimeout;
 836         }
 837       }
 838     }
 839     while (rval != pdFAIL);
 840   }
 841
 842   /* Return flags before clearing */
 843   return (rflags);
 844 }
 845
 846 osStatus_t osDelay (uint32_t ticks) {
 847   osStatus_t stat;
 848
 849   if (IS_IRQ()) {
 850     stat = osErrorISR;
 851   }
 852   else {
 853     stat = osOK;
 854
 855     if (ticks != 0U) {
 856       vTaskDelay(ticks);
 857     }
 858   }
 859
 860   return (stat);
 861 }
 862
 863 osStatus_t osDelayUntil (uint32_t ticks) {
 864   TickType_t tcnt, delay;
 865   osStatus_t stat;
 866
 867   if (IS_IRQ()) {
 868     stat = osErrorISR;
 869   }
 870   else {
 871     stat = osOK;
 872     tcnt = xTaskGetTickCount();
 873
 874     /* Determine remaining number of ticks to delay */
 875     delay = (TickType_t)ticks - tcnt;
 876
 877     /* Check if target tick has not expired */
 878     if((delay != 0U) && (0 == (delay >> (8 * sizeof(TickType_t) - 1)))) {
 879       vTaskDelayUntil (&tcnt, delay);
 880     }
 881     else
 882     {
 883       /* No delay or already expired */
 884       stat = osErrorParameter;
 885     }
 886   }
 887
 888   return (stat);
 889 }
 890
 891 /*---------------------------------------------------------------------------*/
 892
 893 static void TimerCallback (TimerHandle_t hTimer) {
 894   TimerCallback_t *callb;
 895
 896   callb = (TimerCallback_t *)pvTimerGetTimerID (hTimer);
 897
 898   if (callb != NULL) {
 899     callb->func (callb->arg);
 900   }
 901 }
 902
 903 osTimerId_t osTimerNew (osTimerFunc_t func, osTimerType_t type, void *argument, const osTimerAttr_t *attr) {
 904   const char *name;
 905   TimerHandle_t hTimer;
 906   TimerCallback_t *callb;
 907   UBaseType_t reload;
 908   int32_t mem;
 909
 910   hTimer = NULL;
 911
 912   if (!IS_IRQ() && (func != NULL)) {
 913     /* Allocate memory to store callback function and argument */
 914     callb = pvPortMalloc (sizeof(TimerCallback_t));
 915
 916     if (callb != NULL) {
 917       callb->func = func;
 918       callb->arg  = argument;
 919
 920       if (type == osTimerOnce) {
 921         reload = pdFALSE;
 922       } else {
 923         reload = pdTRUE;
 924       }
 925
 926       mem  = -1;
 927       name = NULL;
 928
 929       if (attr != NULL) {
 930         if (attr->name != NULL) {
 931           name = attr->name;
 932         }
 933
 934         if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticTimer_t))) {
 935           mem = 1;
 936         }
 937         else {
 938           if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
 939             mem = 0;
 940           }
 941         }
 942       }
 943       else {
 944         mem = 0;
 945       }
 946
 947       if (mem == 1) {
 948         hTimer = xTimerCreateStatic (name, 1, reload, callb, TimerCallback, (StaticTimer_t *)attr->cb_mem);
 949       }
 950       else {
 951         if (mem == 0) {
 952           hTimer = xTimerCreate (name, 1, reload, callb, TimerCallback);
 953         }
 954       }
 955     }
 956   }
 957
 958   return ((osTimerId_t)hTimer);
 959 }
 960
 961 const char *osTimerGetName (osTimerId_t timer_id) {
 962   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
 963   const char *p;
 964
 965   if (IS_IRQ() || (hTimer == NULL)) {
 966     p = NULL;
 967   } else {
 968     p = pcTimerGetName (hTimer);
 969   }
 970
 971   return (p);
 972 }
 973
 974 osStatus_t osTimerStart (osTimerId_t timer_id, uint32_t ticks) {
 975   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
 976   osStatus_t stat;
 977
 978   if (IS_IRQ()) {
 979     stat = osErrorISR;
 980   }
 981   else if (hTimer == NULL) {
 982     stat = osErrorParameter;
 983   }
 984   else {
 985     if (xTimerChangePeriod (hTimer, ticks, 0) == pdPASS) {
 986       stat = osOK;
 987     } else {
 988       stat = osErrorResource;
 989     }
 990   }
 991
 992   return (stat);
 993 }
 994
 995 osStatus_t osTimerStop (osTimerId_t timer_id) {
 996   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
 997   osStatus_t stat;
 998
 999   if (IS_IRQ()) {
1000     stat = osErrorISR;
1001   }
1002   else if (hTimer == NULL) {
1003     stat = osErrorParameter;
1004   }
1005   else {
1006     if (xTimerIsTimerActive (hTimer) == pdFALSE) {
1007       stat = osErrorResource;
1008     }
1009     else {
1010       if (xTimerStop (hTimer, 0) == pdPASS) {
1011         stat = osOK;
1012       } else {
1013         stat = osError;
1014       }
1015     }
1016   }
1017
1018   return (stat);
1019 }
1020
1021 uint32_t osTimerIsRunning (osTimerId_t timer_id) {
1022   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
1023   uint32_t running;
1024
1025   if (IS_IRQ() || (hTimer == NULL)) {
1026     running = 0U;
1027   } else {
1028     running = (uint32_t)xTimerIsTimerActive (hTimer);
1029   }
1030
1031   return (running);
1032 }
1033
1034 osStatus_t osTimerDelete (osTimerId_t timer_id) {
1035   TimerHandle_t hTimer = (TimerHandle_t)timer_id;
1036   osStatus_t stat;
1037 #ifndef USE_FreeRTOS_HEAP_1
1038   TimerCallback_t *callb;
1039
1040   if (IS_IRQ()) {
1041     stat = osErrorISR;
1042   }
1043   else if (hTimer == NULL) {
1044     stat = osErrorParameter;
1045   }
1046   else {
1047     callb = (TimerCallback_t *)pvTimerGetTimerID (hTimer);
1048
1049     if (xTimerDelete (hTimer, 0) == pdPASS) {
1050       vPortFree (callb);
1051       stat = osOK;
1052     } else {
1053       stat = osErrorResource;
1054     }
1055   }
1056 #else
1057   stat = osError;
1058 #endif
1059
1060   return (stat);
1061 }
1062
1063 /*---------------------------------------------------------------------------*/
1064
1065 osEventFlagsId_t osEventFlagsNew (const osEventFlagsAttr_t *attr) {
1066   EventGroupHandle_t hEventGroup;
1067   int32_t mem;
1068
1069   hEventGroup = NULL;
1070
1071   if (!IS_IRQ()) {
1072     mem = -1;
1073
1074     if (attr != NULL) {
1075       if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticEventGroup_t))) {
1076         mem = 1;
1077       }
1078       else {
1079         if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
1080           mem = 0;
1081         }
1082       }
1083     }
1084     else {
1085       mem = 0;
1086     }
1087
1088     if (mem == 1) {
1089       hEventGroup = xEventGroupCreateStatic (attr->cb_mem);
1090     }
1091     else {
1092       if (mem == 0) {
1093         hEventGroup = xEventGroupCreate();
1094       }
1095     }
1096   }
1097
1098   return ((osEventFlagsId_t)hEventGroup);
1099 }
1100
1101 uint32_t osEventFlagsSet (osEventFlagsId_t ef_id, uint32_t flags) {
1102   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1103   uint32_t rflags;
1104   BaseType_t yield;
1105
1106   if ((hEventGroup == NULL) || ((flags & EVENT_FLAGS_INVALID_BITS) != 0U)) {
1107     rflags = (uint32_t)osErrorParameter;
1108   }
1109   else if (IS_IRQ()) {
1110     yield = pdFALSE;
1111
1112     if (xEventGroupSetBitsFromISR (hEventGroup, (EventBits_t)flags, &yield) == pdFAIL) {
1113       rflags = (uint32_t)osErrorResource;
1114     } else {
1115       rflags = flags;
1116       portYIELD_FROM_ISR (yield);
1117     }
1118   }
1119   else {
1120     rflags = xEventGroupSetBits (hEventGroup, (EventBits_t)flags);
1121   }
1122
1123   return (rflags);
1124 }
1125
1126 uint32_t osEventFlagsClear (osEventFlagsId_t ef_id, uint32_t flags) {
1127   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1128   uint32_t rflags;
1129
1130   if ((hEventGroup == NULL) || ((flags & EVENT_FLAGS_INVALID_BITS) != 0U)) {
1131     rflags = (uint32_t)osErrorParameter;
1132   }
1133   else if (IS_IRQ()) {
1134     rflags = xEventGroupGetBitsFromISR (hEventGroup);
1135
1136     if (xEventGroupClearBitsFromISR (hEventGroup, (EventBits_t)flags) == pdFAIL) {
1137       rflags = (uint32_t)osErrorResource;
1138     }
1139   }
1140   else {
1141     rflags = xEventGroupClearBits (hEventGroup, (EventBits_t)flags);
1142   }
1143
1144   return (rflags);
1145 }
1146
1147 uint32_t osEventFlagsGet (osEventFlagsId_t ef_id) {
1148   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1149   uint32_t rflags;
1150
1151   if (ef_id == NULL) {
1152     rflags = 0U;
1153   }
1154   else if (IS_IRQ()) {
1155     rflags = xEventGroupGetBitsFromISR (hEventGroup);
1156   }
1157   else {
1158     rflags = xEventGroupGetBits (hEventGroup);
1159   }
1160
1161   return (rflags);
1162 }
1163
1164 uint32_t osEventFlagsWait (osEventFlagsId_t ef_id, uint32_t flags, uint32_t options, uint32_t timeout) {
1165   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1166   BaseType_t wait_all;
1167   BaseType_t exit_clr;
1168   uint32_t rflags;
1169
1170   if ((hEventGroup == NULL) || ((flags & EVENT_FLAGS_INVALID_BITS) != 0U)) {
1171     rflags = (uint32_t)osErrorParameter;
1172   }
1173   else if (IS_IRQ()) {
1174     rflags = (uint32_t)osErrorISR;
1175   }
1176   else {
1177     if (options & osFlagsWaitAll) {
1178       wait_all = pdTRUE;
1179     } else {
1180       wait_all = pdFAIL;
1181     }
1182
1183     if (options & osFlagsNoClear) {
1184       exit_clr = pdFAIL;
1185     } else {
1186       exit_clr = pdTRUE;
1187     }
1188
1189     rflags = xEventGroupWaitBits (hEventGroup, (EventBits_t)flags, exit_clr, wait_all, (TickType_t)timeout);
1190
1191     if (options & osFlagsWaitAll) {
1192       if ((flags & rflags) != flags) {
1193         if (timeout > 0U) {
1194           rflags = (uint32_t)osErrorTimeout;
1195         } else {
1196           rflags = (uint32_t)osErrorResource;
1197         }
1198       }
1199     }
1200     else {
1201       if ((flags & rflags) == 0U) {
1202         if (timeout > 0U) {
1203           rflags = (uint32_t)osErrorTimeout;
1204         } else {
1205           rflags = (uint32_t)osErrorResource;
1206         }
1207       }
1208     }
1209   }
1210
1211   return (rflags);
1212 }
1213
1214 osStatus_t osEventFlagsDelete (osEventFlagsId_t ef_id) {
1215   EventGroupHandle_t hEventGroup = (EventGroupHandle_t)ef_id;
1216   osStatus_t stat;
1217
1218 #ifndef USE_FreeRTOS_HEAP_1
1219   if (IS_IRQ()) {
1220     stat = osErrorISR;
1221   }
1222   else if (hEventGroup == NULL) {
1223     stat = osErrorParameter;
1224   }
1225   else {
1226     stat = osOK;
1227     vEventGroupDelete (hEventGroup);
1228   }
1229 #else
1230   stat = osError;
1231 #endif
1232
1233   return (stat);
1234 }
1235
1236 /*---------------------------------------------------------------------------*/
1237
1238 osMutexId_t osMutexNew (const osMutexAttr_t *attr) {
1239   SemaphoreHandle_t hMutex;
1240   uint32_t type;
1241   uint32_t rmtx;
1242   int32_t  mem;
1243   #if (configQUEUE_REGISTRY_SIZE > 0)
1244   const char *name;
1245   #endif
1246
1247   hMutex = NULL;
1248
1249   if (!IS_IRQ()) {
1250     if (attr != NULL) {
1251       type = attr->attr_bits;
1252     } else {
1253       type = 0U;
1254     }
1255
1256     if ((type & osMutexRecursive) == osMutexRecursive) {
1257       rmtx = 1U;
1258     } else {
1259       rmtx = 0U;
1260     }
1261
1262     if ((type & osMutexRobust) != osMutexRobust) {
1263       mem = -1;
1264
1265       if (attr != NULL) {
1266         if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticSemaphore_t))) {
1267           mem = 1;
1268         }
1269         else {
1270           if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
1271             mem = 0;
1272           }
1273         }
1274       }
1275       else {
1276         mem = 0;
1277       }
1278
1279       if (mem == 1) {
1280         if (rmtx != 0U) {
1281           hMutex = xSemaphoreCreateRecursiveMutexStatic (attr->cb_mem);
1282         }
1283         else {
1284           hMutex = xSemaphoreCreateMutexStatic (attr->cb_mem);
1285         }
1286       }
1287       else {
1288         if (mem == 0) {
1289           if (rmtx != 0U) {
1290             hMutex = xSemaphoreCreateRecursiveMutex ();
1291           } else {
1292             hMutex = xSemaphoreCreateMutex ();
1293           }
1294         }
1295       }
1296
1297       #if (configQUEUE_REGISTRY_SIZE > 0)
1298       if (hMutex != NULL) {
1299         if (attr != NULL) {
1300           name = attr->name;
1301         } else {
1302           name = NULL;
1303         }
1304         vQueueAddToRegistry (hMutex, name);
1305       }
1306       #endif
1307
1308       if ((hMutex != NULL) && (rmtx != 0U)) {
1309         hMutex = (SemaphoreHandle_t)((uint32_t)hMutex | 1U);
1310       }
1311     }
1312   }
1313
1314   return ((osMutexId_t)hMutex);
1315 }
1316
1317 osStatus_t osMutexAcquire (osMutexId_t mutex_id, uint32_t timeout) {
1318   SemaphoreHandle_t hMutex;
1319   osStatus_t stat;
1320   uint32_t rmtx;
1321
1322   hMutex = (SemaphoreHandle_t)((uint32_t)mutex_id & ~1U);
1323
1324   rmtx = (uint32_t)mutex_id & 1U;
1325
1326   stat = osOK;
1327
1328   if (IS_IRQ()) {
1329     stat = osErrorISR;
1330   }
1331   else if (hMutex == NULL) {
1332     stat = osErrorParameter;
1333   }
1334   else {
1335     if (rmtx != 0U) {
1336       if (xSemaphoreTakeRecursive (hMutex, timeout) != pdPASS) {
1337         if (timeout != 0U) {
1338           stat = osErrorTimeout;
1339         } else {
1340           stat = osErrorResource;
1341         }
1342       }
1343     }
1344     else {
1345       if (xSemaphoreTake (hMutex, timeout) != pdPASS) {
1346         if (timeout != 0U) {
1347           stat = osErrorTimeout;
1348         } else {
1349           stat = osErrorResource;
1350         }
1351       }
1352     }
1353   }
1354
1355   return (stat);
1356 }
1357
1358 osStatus_t osMutexRelease (osMutexId_t mutex_id) {
1359   SemaphoreHandle_t hMutex;
1360   osStatus_t stat;
1361   uint32_t rmtx;
1362
1363   hMutex = (SemaphoreHandle_t)((uint32_t)mutex_id & ~1U);
1364
1365   rmtx = (uint32_t)mutex_id & 1U;
1366
1367   stat = osOK;
1368
1369   if (IS_IRQ()) {
1370     stat = osErrorISR;
1371   }
1372   else if (hMutex == NULL) {
1373     stat = osErrorParameter;
1374   }
1375   else {
1376     if (rmtx != 0U) {
1377       if (xSemaphoreGiveRecursive (hMutex) != pdPASS) {
1378         stat = osErrorResource;
1379       }
1380     }
1381     else {
1382       if (xSemaphoreGive (hMutex) != pdPASS) {
1383         stat = osErrorResource;
1384       }
1385     }
1386   }
1387
1388   return (stat);
1389 }
1390
1391 osThreadId_t osMutexGetOwner (osMutexId_t mutex_id) {
1392   SemaphoreHandle_t hMutex;
1393   osThreadId_t owner;
1394
1395   hMutex = (SemaphoreHandle_t)((uint32_t)mutex_id & ~1U);
1396
1397   if (IS_IRQ() || (hMutex == NULL)) {
1398     owner = NULL;
1399   } else {
1400     owner = (osThreadId_t)xSemaphoreGetMutexHolder (hMutex);
1401   }
1402
1403   return (owner);
1404 }
1405
1406 osStatus_t osMutexDelete (osMutexId_t mutex_id) {
1407   osStatus_t stat;
1408 #ifndef USE_FreeRTOS_HEAP_1
1409   SemaphoreHandle_t hMutex;
1410
1411   hMutex = (SemaphoreHandle_t)((uint32_t)mutex_id & ~1U);
1412
1413   if (IS_IRQ()) {
1414     stat = osErrorISR;
1415   }
1416   else if (hMutex == NULL) {
1417     stat = osErrorParameter;
1418   }
1419   else {
1420     #if (configQUEUE_REGISTRY_SIZE > 0)
1421     vQueueUnregisterQueue (hMutex);
1422     #endif
1423     stat = osOK;
1424     vSemaphoreDelete (hMutex);
1425   }
1426 #else
1427   stat = osError;
1428 #endif
1429
1430   return (stat);
1431 }
1432
1433 /*---------------------------------------------------------------------------*/
1434
1435 osSemaphoreId_t osSemaphoreNew (uint32_t max_count, uint32_t initial_count, const osSemaphoreAttr_t *attr) {
1436   SemaphoreHandle_t hSemaphore;
1437   int32_t mem;
1438   #if (configQUEUE_REGISTRY_SIZE > 0)
1439   const char *name;
1440   #endif
1441
1442   hSemaphore = NULL;
1443
1444   if (!IS_IRQ() && (max_count > 0U) && (initial_count <= max_count)) {
1445     mem = -1;
1446
1447     if (attr != NULL) {
1448       if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticSemaphore_t))) {
1449         mem = 1;
1450       }
1451       else {
1452         if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
1453           mem = 0;
1454         }
1455       }
1456     }
1457     else {
1458       mem = 0;
1459     }
1460
1461     if (mem != -1) {
1462       if (max_count == 1U) {
1463         if (mem == 1) {
1464           hSemaphore = xSemaphoreCreateBinaryStatic ((StaticSemaphore_t *)attr->cb_mem);
1465         }
1466         else {
1467           hSemaphore = xSemaphoreCreateBinary();
1468         }
1469
1470         if ((hSemaphore != NULL) && (initial_count != 0U)) {
1471           if (xSemaphoreGive (hSemaphore) != pdPASS) {
1472             vSemaphoreDelete (hSemaphore);
1473             hSemaphore = NULL;
1474           }
1475         }
1476       }
1477       else {
1478         if (mem == 1) {
1479           hSemaphore = xSemaphoreCreateCountingStatic (max_count, initial_count, (StaticSemaphore_t *)attr->cb_mem);
1480         }
1481         else {
1482           hSemaphore = xSemaphoreCreateCounting (max_count, initial_count);
1483         }
1484       }
1485
1486       #if (configQUEUE_REGISTRY_SIZE > 0)
1487       if (hSemaphore != NULL) {
1488         if (attr != NULL) {
1489           name = attr->name;
1490         } else {
1491           name = NULL;
1492         }
1493         vQueueAddToRegistry (hSemaphore, name);
1494       }
1495       #endif
1496     }
1497   }
1498
1499   return ((osSemaphoreId_t)hSemaphore);
1500 }
1501
1502 osStatus_t osSemaphoreAcquire (osSemaphoreId_t semaphore_id, uint32_t timeout) {
1503   SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)semaphore_id;
1504   osStatus_t stat;
1505   BaseType_t yield;
1506
1507   stat = osOK;
1508
1509   if (hSemaphore == NULL) {
1510     stat = osErrorParameter;
1511   }
1512   else if (IS_IRQ()) {
1513     if (timeout != 0U) {
1514       stat = osErrorParameter;
1515     }
1516     else {
1517       yield = pdFALSE;
1518
1519       if (xSemaphoreTakeFromISR (hSemaphore, &yield) != pdPASS) {
1520         stat = osErrorResource;
1521       } else {
1522         portYIELD_FROM_ISR (yield);
1523       }
1524     }
1525   }
1526   else {
1527     if (xSemaphoreTake (hSemaphore, (TickType_t)timeout) != pdPASS) {
1528       if (timeout != 0U) {
1529         stat = osErrorTimeout;
1530       } else {
1531         stat = osErrorResource;
1532       }
1533     }
1534   }
1535
1536   return (stat);
1537 }
1538
1539 osStatus_t osSemaphoreRelease (osSemaphoreId_t semaphore_id) {
1540   SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)semaphore_id;
1541   osStatus_t stat;
1542   BaseType_t yield;
1543
1544   stat = osOK;
1545
1546   if (hSemaphore == NULL) {
1547     stat = osErrorParameter;
1548   }
1549   else if (IS_IRQ()) {
1550     yield = pdFALSE;
1551
1552     if (xSemaphoreGiveFromISR (hSemaphore, &yield) != pdTRUE) {
1553       stat = osErrorResource;
1554     } else {
1555       portYIELD_FROM_ISR (yield);
1556     }
1557   }
1558   else {
1559     if (xSemaphoreGive (hSemaphore) != pdPASS) {
1560       stat = osErrorResource;
1561     }
1562   }
1563
1564   return (stat);
1565 }
1566
1567 uint32_t osSemaphoreGetCount (osSemaphoreId_t semaphore_id) {
1568   SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)semaphore_id;
1569   uint32_t count;
1570
1571   if (hSemaphore == NULL) {
1572     count = 0U;
1573   }
1574   else if (IS_IRQ()) {
1575     count = uxQueueMessagesWaitingFromISR (hSemaphore);
1576   } else {
1577     count = (uint32_t)uxSemaphoreGetCount (hSemaphore);
1578   }
1579
1580   return (count);
1581 }
1582
1583 osStatus_t osSemaphoreDelete (osSemaphoreId_t semaphore_id) {
1584   SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)semaphore_id;
1585   osStatus_t stat;
1586
1587 #ifndef USE_FreeRTOS_HEAP_1
1588   if (IS_IRQ()) {
1589     stat = osErrorISR;
1590   }
1591   else if (hSemaphore == NULL) {
1592     stat = osErrorParameter;
1593   }
1594   else {
1595     #if (configQUEUE_REGISTRY_SIZE > 0)
1596     vQueueUnregisterQueue (hSemaphore);
1597     #endif
1598
1599     stat = osOK;
1600     vSemaphoreDelete (hSemaphore);
1601   }
1602 #else
1603   stat = osError;
1604 #endif
1605
1606   return (stat);
1607 }
1608
1609 /*---------------------------------------------------------------------------*/
1610
1611 osMessageQueueId_t osMessageQueueNew (uint32_t msg_count, uint32_t msg_size, const osMessageQueueAttr_t *attr) {
1612   QueueHandle_t hQueue;
1613   int32_t mem;
1614   #if (configQUEUE_REGISTRY_SIZE > 0)
1615   const char *name;
1616   #endif
1617
1618   hQueue = NULL;
1619
1620   if (!IS_IRQ() && (msg_count > 0U) && (msg_size > 0U)) {
1621     mem = -1;
1622
1623     if (attr != NULL) {
1624       if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(StaticQueue_t)) &&
1625           (attr->mq_mem != NULL) && (attr->mq_size >= (msg_count * msg_size))) {
1626         mem = 1;
1627       }
1628       else {
1629         if ((attr->cb_mem == NULL) && (attr->cb_size == 0U) &&
1630             (attr->mq_mem == NULL) && (attr->mq_size == 0U)) {
1631           mem = 0;
1632         }
1633       }
1634     }
1635     else {
1636       mem = 0;
1637     }
1638
1639     if (mem == 1) {
1640       hQueue = xQueueCreateStatic (msg_count, msg_size, attr->mq_mem, attr->cb_mem);
1641     }
1642     else {
1643       if (mem == 0) {
1644         hQueue = xQueueCreate (msg_count, msg_size);
1645       }
1646     }
1647
1648     #if (configQUEUE_REGISTRY_SIZE > 0)
1649     if (hQueue != NULL) {
1650       if (attr != NULL) {
1651         name = attr->name;
1652       } else {
1653         name = NULL;
1654       }
1655       vQueueAddToRegistry (hQueue, name);
1656     }
1657     #endif
1658
1659   }
1660
1661   return ((osMessageQueueId_t)hQueue);
1662 }
1663
1664 osStatus_t osMessageQueuePut (osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout) {
1665   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
1666   osStatus_t stat;
1667   BaseType_t yield;
1668
1669   (void)msg_prio; /* Message priority is ignored */
1670
1671   stat = osOK;
1672
1673   if (IS_IRQ()) {
1674     if ((hQueue == NULL) || (msg_ptr == NULL) || (timeout != 0U)) {
1675       stat = osErrorParameter;
1676     }
1677     else {
1678       yield = pdFALSE;
1679
1680       if (xQueueSendToBackFromISR (hQueue, msg_ptr, &yield) != pdTRUE) {
1681         stat = osErrorResource;
1682       } else {
1683         portYIELD_FROM_ISR (yield);
1684       }
1685     }
1686   }
1687   else {
1688     if ((hQueue == NULL) || (msg_ptr == NULL)) {
1689       stat = osErrorParameter;
1690     }
1691     else {
1692       if (xQueueSendToBack (hQueue, msg_ptr, (TickType_t)timeout) != pdPASS) {
1693         if (timeout != 0U) {
1694           stat = osErrorTimeout;
1695         } else {
1696           stat = osErrorResource;
1697         }
1698       }
1699     }
1700   }
1701
1702   return (stat);
1703 }
1704
1705 osStatus_t osMessageQueueGet (osMessageQueueId_t mq_id, void *msg_ptr, uint8_t *msg_prio, uint32_t timeout) {
1706   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
1707   osStatus_t stat;
1708   BaseType_t yield;
1709
1710   (void)msg_prio; /* Message priority is ignored */
1711
1712   stat = osOK;
1713
1714   if (IS_IRQ()) {
1715     if ((hQueue == NULL) || (msg_ptr == NULL) || (timeout != 0U)) {
1716       stat = osErrorParameter;
1717     }
1718     else {
1719       yield = pdFALSE;
1720
1721       if (xQueueReceiveFromISR (hQueue, msg_ptr, &yield) != pdPASS) {
1722         stat = osErrorResource;
1723       } else {
1724         portYIELD_FROM_ISR (yield);
1725       }
1726     }
1727   }
1728   else {
1729     if ((hQueue == NULL) || (msg_ptr == NULL)) {
1730       stat = osErrorParameter;
1731     }
1732     else {
1733       if (xQueueReceive (hQueue, msg_ptr, (TickType_t)timeout) != pdPASS) {
1734         if (timeout != 0U) {
1735           stat = osErrorTimeout;
1736         } else {
1737           stat = osErrorResource;
1738         }
1739       }
1740     }
1741   }
1742
1743   return (stat);
1744 }
1745
1746 uint32_t osMessageQueueGetCapacity (osMessageQueueId_t mq_id) {
1747   StaticQueue_t *mq = (StaticQueue_t *)mq_id;
1748   uint32_t capacity;
1749
1750   if (mq == NULL) {
1751     capacity = 0U;
1752   } else {
1753     /* capacity = pxQueue->uxLength */
1754     capacity = mq->uxDummy4[1];
1755   }
1756
1757   return (capacity);
1758 }
1759
1760 uint32_t osMessageQueueGetMsgSize (osMessageQueueId_t mq_id) {
1761   StaticQueue_t *mq = (StaticQueue_t *)mq_id;
1762   uint32_t size;
1763
1764   if (mq == NULL) {
1765     size = 0U;
1766   } else {
1767     /* size = pxQueue->uxItemSize */
1768     size = mq->uxDummy4[2];
1769   }
1770
1771   return (size);
1772 }
1773
1774 uint32_t osMessageQueueGetCount (osMessageQueueId_t mq_id) {
1775   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
1776   UBaseType_t count;
1777
1778   if (hQueue == NULL) {
1779     count = 0U;
1780   }
1781   else if (IS_IRQ()) {
1782     count = uxQueueMessagesWaitingFromISR (hQueue);
1783   }
1784   else {
1785     count = uxQueueMessagesWaiting (hQueue);
1786   }
1787
1788   return ((uint32_t)count);
1789 }
1790
1791 uint32_t osMessageQueueGetSpace (osMessageQueueId_t mq_id) {
1792   StaticQueue_t *mq = (StaticQueue_t *)mq_id;
1793   uint32_t space;
1794   uint32_t isrm;
1795
1796   if (mq == NULL) {
1797     space = 0U;
1798   }
1799   else if (IS_IRQ()) {
1800     isrm = taskENTER_CRITICAL_FROM_ISR();
1801
1802     /* space = pxQueue->uxLength - pxQueue->uxMessagesWaiting; */
1803     space = mq->uxDummy4[1] - mq->uxDummy4[0];
1804
1805     taskEXIT_CRITICAL_FROM_ISR(isrm);
1806   }
1807   else {
1808     space = (uint32_t)uxQueueSpacesAvailable ((QueueHandle_t)mq);
1809   }
1810
1811   return (space);
1812 }
1813
1814 osStatus_t osMessageQueueReset (osMessageQueueId_t mq_id) {
1815   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
1816   osStatus_t stat;
1817
1818   if (IS_IRQ()) {
1819     stat = osErrorISR;
1820   }
1821   else if (hQueue == NULL) {
1822     stat = osErrorParameter;
1823   }
1824   else {
1825     stat = osOK;
1826     (void)xQueueReset (hQueue);
1827   }
1828
1829   return (stat);
1830 }
1831
1832 osStatus_t osMessageQueueDelete (osMessageQueueId_t mq_id) {
1833   QueueHandle_t hQueue = (QueueHandle_t)mq_id;
1834   osStatus_t stat;
1835
1836 #ifndef USE_FreeRTOS_HEAP_1
1837   if (IS_IRQ()) {
1838     stat = osErrorISR;
1839   }
1840   else if (hQueue == NULL) {
1841     stat = osErrorParameter;
1842   }
1843   else {
1844     #if (configQUEUE_REGISTRY_SIZE > 0)
1845     vQueueUnregisterQueue (hQueue);
1846     #endif
1847
1848     stat = osOK;
1849     vQueueDelete (hQueue);
1850   }
1851 #else
1852   stat = osError;
1853 #endif
1854
1855   return (stat);
1856 }
1857
1858 /*---------------------------------------------------------------------------*/
1859 #ifdef FREERTOS_MPOOL_H_
1860
1861 /* Static memory pool functions */
1862 static void  FreeBlock   (MemPool_t *mp, void *block);
1863 static void *AllocBlock  (MemPool_t *mp);
1864 static void *CreateBlock (MemPool_t *mp);
1865
1866 osMemoryPoolId_t osMemoryPoolNew (uint32_t block_count, uint32_t block_size, const osMemoryPoolAttr_t *attr) {
1867   MemPool_t *mp;
1868   const char *name;
1869   int32_t mem_cb, mem_mp;
1870   uint32_t sz;
1871   SemaphoreHandle_t hSemaphore;
1872
1873   if (IS_IRQ()) {
1874     mp = NULL;
1875   }
1876   else if ((block_count == 0U) || (block_size == 0U)) {
1877     mp = NULL;
1878   }
1879   else {
1880     mp = NULL;
1881     sz = MEMPOOL_ARR_SIZE (block_count, block_size);
1882
1883     name = NULL;
1884     mem_cb = -1;
1885     mem_mp = -1;
1886
1887     if (attr != NULL) {
1888       if (attr->name != NULL) {
1889         name = attr->name;
1890       }
1891
1892       if ((attr->cb_mem != NULL) && (attr->cb_size >= sizeof(MemPool_t))) {
1893         /* Static control block is provided */
1894         mem_cb = 1;
1895       }
1896       else if ((attr->cb_mem == NULL) && (attr->cb_size == 0U)) {
1897         /* Allocate control block memory on heap */
1898         mem_cb = 0;
1899       }
1900
1901       if ((attr->mp_mem == NULL) && (attr->mp_size == 0U)) {
1902         /* Allocate memory array on heap */
1903           mem_mp = 0;
1904       }
1905       else {
1906         if (attr->mp_mem != NULL) {
1907           /* Check if array is 4-byte aligned */
1908           if (((uint32_t)attr->mp_mem & 3U) == 0U) {
1909             /* Check if array big enough */
1910             if (attr->mp_size >= sz) {
1911               /* Static memory pool array is provided */
1912               mem_mp = 1;
1913             }
1914           }
1915         }
1916       }
1917     }
1918     else {
1919       /* Attributes not provided, allocate memory on heap */
1920       mem_cb = 0;
1921       mem_mp = 0;
1922     }
1923
1924     if (mem_cb == 0) {
1925       mp = pvPortMalloc (sizeof(MemPool_t));
1926     } else {
1927       mp = attr->cb_mem;
1928     }
1929
1930     if (mp != NULL) {
1931       /* Create a semaphore (max count == initial count == block_count) */
1932       hSemaphore = xSemaphoreCreateCountingStatic (block_count, block_count, &mp->sem);
1933
1934       if (hSemaphore == (SemaphoreHandle_t)&mp->sem) {
1935         /* Setup memory array */
1936         if (mem_mp == 0) {
1937           mp->mem_arr = pvPortMalloc (sz);
1938         } else {
1939           mp->mem_arr = attr->mp_mem;
1940         }
1941       }
1942     }
1943
1944     if ((mp != NULL) && (mp->mem_arr != NULL)) {
1945       /* Memory pool can be created */
1946       mp->head    = NULL;
1947       mp->mem_sz  = sz;
1948       mp->name    = name;
1949       mp->bl_sz   = block_size;
1950       mp->bl_cnt  = block_count;
1951       mp->n       = 0U;
1952
1953       /* Set heap allocated memory flags */
1954       mp->status = MPOOL_STATUS;
1955
1956       if (mem_cb == 0) {
1957         /* Control block on heap */
1958         mp->status |= 1U;
1959       }
1960       if (mem_mp == 0) {
1961         /* Memory array on heap */
1962         mp->status |= 2U;
1963       }
1964     }
1965     else {
1966       /* Memory pool cannot be created, release allocated resources */
1967       if ((mem_cb == 0) && (mp != NULL)) {
1968         /* Free control block memory */
1969         vPortFree (mp);
1970       }
1971       mp = NULL;
1972     }
1973   }
1974
1975   return (mp);
1976 }
1977
1978 const char *osMemoryPoolGetName (osMemoryPoolId_t mp_id) {
1979   MemPool_t *mp = (osMemoryPoolId_t)mp_id;
1980   const char *p;
1981
1982   if (IS_IRQ()) {
1983     p = NULL;
1984   }
1985   else if (mp_id == NULL) {
1986     p = NULL;
1987   }
1988   else {
1989     p = mp->name;
1990   }
1991
1992   return (p);
1993 }
1994
1995 void *osMemoryPoolAlloc (osMemoryPoolId_t mp_id, uint32_t timeout) {
1996   MemPool_t *mp;
1997   void *block;
1998   uint32_t isrm;
1999
2000   if (mp_id == NULL) {
2001     /* Invalid input parameters */
2002     block = NULL;
2003   }
2004   else {
2005     block = NULL;
2006
2007     mp = (MemPool_t *)mp_id;
2008
2009     if ((mp->status & MPOOL_STATUS) == MPOOL_STATUS) {
2010       if (IS_IRQ()) {
2011         if (timeout == 0U) {
2012           if (xSemaphoreTakeFromISR ((SemaphoreHandle_t)&mp->sem, NULL) == pdTRUE) {
2013             if ((mp->status & MPOOL_STATUS) == MPOOL_STATUS) {
2014               isrm  = taskENTER_CRITICAL_FROM_ISR();
2015
2016               /* Get a block from the free-list */
2017               block = AllocBlock(mp);
2018
2019               if (block == NULL) {
2020                 /* List of free blocks is empty, 'create' new block */
2021                 block = CreateBlock(mp);
2022               }
2023
2024               taskEXIT_CRITICAL_FROM_ISR(isrm);
2025             }
2026           }
2027         }
2028       }
2029       else {
2030         if (xSemaphoreTake ((SemaphoreHandle_t)&mp->sem, timeout) == pdTRUE) {
2031           if ((mp->status & MPOOL_STATUS) == MPOOL_STATUS) {
2032             taskENTER_CRITICAL();
2033
2034             /* Get a block from the free-list */
2035             block = AllocBlock(mp);
2036
2037             if (block == NULL) {
2038               /* List of free blocks is empty, 'create' new block */
2039               block = CreateBlock(mp);
2040             }
2041
2042             taskEXIT_CRITICAL();
2043           }
2044         }
2045       }
2046     }
2047   }
2048
2049   return (block);
2050 }
2051
2052 osStatus_t osMemoryPoolFree (osMemoryPoolId_t mp_id, void *block) {
2053   MemPool_t *mp;
2054   osStatus_t stat;
2055   uint32_t isrm;
2056   BaseType_t yield;
2057
2058   if ((mp_id == NULL) || (block == NULL)) {
2059     /* Invalid input parameters */
2060     stat = osErrorParameter;
2061   }
2062   else {
2063     mp = (MemPool_t *)mp_id;
2064
2065     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2066       /* Invalid object status */
2067       stat = osErrorResource;
2068     }
2069     else if ((block < (void *)&mp->mem_arr[0]) || (block > (void*)&mp->mem_arr[mp->mem_sz-1])) {
2070       /* Block pointer outside of memory array area */
2071       stat = osErrorParameter;
2072     }
2073     else {
2074       stat = osOK;
2075
2076       if (IS_IRQ()) {
2077         if (uxSemaphoreGetCountFromISR ((SemaphoreHandle_t)&mp->sem) == mp->bl_cnt) {
2078           stat = osErrorResource;
2079         }
2080         else {
2081           isrm = taskENTER_CRITICAL_FROM_ISR();
2082
2083           /* Add block to the list of free blocks */
2084           FreeBlock(mp, block);
2085
2086           taskEXIT_CRITICAL_FROM_ISR(isrm);
2087
2088           yield = pdFALSE;
2089           xSemaphoreGiveFromISR ((SemaphoreHandle_t)&mp->sem, &yield);
2090           portYIELD_FROM_ISR (yield);
2091         }
2092       }
2093       else {
2094         if (uxSemaphoreGetCount ((SemaphoreHandle_t)&mp->sem) == mp->bl_cnt) {
2095           stat = osErrorResource;
2096         }
2097         else {
2098           taskENTER_CRITICAL();
2099
2100           /* Add block to the list of free blocks */
2101           FreeBlock(mp, block);
2102
2103           taskEXIT_CRITICAL();
2104
2105           xSemaphoreGive ((SemaphoreHandle_t)&mp->sem);
2106         }
2107       }
2108     }
2109   }
2110
2111   return (stat);
2112 }
2113
2114 uint32_t osMemoryPoolGetCapacity (osMemoryPoolId_t mp_id) {
2115   MemPool_t *mp;
2116   uint32_t  n;
2117
2118   if (mp_id == NULL) {
2119     /* Invalid input parameters */
2120     n = 0U;
2121   }
2122   else {
2123     mp = (MemPool_t *)mp_id;
2124
2125     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2126       /* Invalid object status */
2127       n = 0U;
2128     }
2129     else {
2130       n = mp->bl_cnt;
2131     }
2132   }
2133
2134   /* Return maximum number of memory blocks */
2135   return (n);
2136 }
2137
2138 uint32_t osMemoryPoolGetBlockSize (osMemoryPoolId_t mp_id) {
2139   MemPool_t *mp;
2140   uint32_t  sz;
2141
2142   if (mp_id == NULL) {
2143     /* Invalid input parameters */
2144     sz = 0U;
2145   }
2146   else {
2147     mp = (MemPool_t *)mp_id;
2148
2149     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2150       /* Invalid object status */
2151       sz = 0U;
2152     }
2153     else {
2154       sz = mp->bl_sz;
2155     }
2156   }
2157
2158   /* Return memory block size in bytes */
2159   return (sz);
2160 }
2161
2162 uint32_t osMemoryPoolGetCount (osMemoryPoolId_t mp_id) {
2163   MemPool_t *mp;
2164   uint32_t  n;
2165
2166   if (mp_id == NULL) {
2167     /* Invalid input parameters */
2168     n = 0U;
2169   }
2170   else {
2171     mp = (MemPool_t *)mp_id;
2172
2173     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2174       /* Invalid object status */
2175       n = 0U;
2176     }
2177     else {
2178       if (IS_IRQ()) {
2179         n = uxSemaphoreGetCountFromISR ((SemaphoreHandle_t)&mp->sem);
2180       } else {
2181         n = uxSemaphoreGetCount        ((SemaphoreHandle_t)&mp->sem);
2182       }
2183
2184       n = mp->bl_cnt - n;
2185     }
2186   }
2187
2188   /* Return number of memory blocks used */
2189   return (n);
2190 }
2191
2192 uint32_t osMemoryPoolGetSpace (osMemoryPoolId_t mp_id) {
2193   MemPool_t *mp;
2194   uint32_t  n;
2195
2196   if (mp_id == NULL) {
2197     /* Invalid input parameters */
2198     n = 0U;
2199   }
2200   else {
2201     mp = (MemPool_t *)mp_id;
2202
2203     if ((mp->status & MPOOL_STATUS) != MPOOL_STATUS) {
2204       /* Invalid object status */
2205       n = 0U;
2206     }
2207     else {
2208       if (IS_IRQ()) {
2209         n = uxSemaphoreGetCountFromISR ((SemaphoreHandle_t)&mp->sem);
2210       } else {
2211         n = uxSemaphoreGetCount        ((SemaphoreHandle_t)&mp->sem);
2212       }
2213     }
2214   }
2215
2216   /* Return number of memory blocks available */
2217   return (n);
2218 }
2219
2220 osStatus_t osMemoryPoolDelete (osMemoryPoolId_t mp_id) {
2221   MemPool_t *mp;
2222   osStatus_t stat;
2223
2224   if (mp_id == NULL) {
2225     /* Invalid input parameters */
2226     stat = osErrorParameter;
2227   }
2228   else if (IS_IRQ()) {
2229     stat = osErrorISR;
2230   }
2231   else {
2232     mp = (MemPool_t *)mp_id;
2233
2234     taskENTER_CRITICAL();
2235
2236     /* Invalidate control block status */
2237     mp->status  = mp->status & 3U;
2238
2239     /* Wake-up tasks waiting for pool semaphore */
2240     while (xSemaphoreGive ((SemaphoreHandle_t)&mp->sem) == pdTRUE);
2241
2242     mp->head    = NULL;
2243     mp->bl_sz   = 0U;
2244     mp->bl_cnt  = 0U;
2245
2246     if ((mp->status & 2U) != 0U) {
2247       /* Memory pool array allocated on heap */
2248       vPortFree (mp->mem_arr);
2249     }
2250     if ((mp->status & 1U) != 0U) {
2251       /* Memory pool control block allocated on heap */
2252       vPortFree (mp);
2253     }
2254
2255     taskEXIT_CRITICAL();
2256
2257     stat = osOK;
2258   }
2259
2260   return (stat);
2261 }
2262
2263 /*
2264   Create new block given according to the current block index.
2265 */
2266 static void *CreateBlock (MemPool_t *mp) {
2267   MPOOL_BLOCK *p = NULL;
2268
2269   if (mp->n < mp->bl_cnt) {
2270     /* Unallocated blocks exist, set pointer to new block */
2271     p = (void *)(mp->mem_arr + (mp->bl_sz * mp->n));
2272
2273     /* Increment block index */
2274     mp->n += 1U;
2275   }
2276
2277   return (p);
2278 }
2279
2280 /*
2281   Allocate a block by reading the list of free blocks.
2282 */
2283 static void *AllocBlock (MemPool_t *mp) {
2284   MPOOL_BLOCK *p = NULL;
2285
2286   if (mp->head != NULL) {
2287     /* List of free block exists, get head block */
2288     p = mp->head;
2289
2290     /* Head block is now next on the list */
2291     mp->head = p->next;
2292   }
2293
2294   return (p);
2295 }
2296
2297 /*
2298   Free block by putting it to the list of free blocks.
2299 */
2300 static void FreeBlock (MemPool_t *mp, void *block) {
2301   MPOOL_BLOCK *p = block;
2302
2303   /* Store current head into block memory space */
2304   p->next = mp->head;
2305
2306   /* Store current block as new head */
2307   mp->head = p;
2308 }
2309 #endif /* FREERTOS_MPOOL_H_ */
2310 /*---------------------------------------------------------------------------*/
2311
2312 /* Callback function prototypes */
2313 extern void vApplicationIdleHook (void);
2314 extern void vApplicationTickHook (void);
2315 extern void vApplicationMallocFailedHook (void);
2316 extern void vApplicationDaemonTaskStartupHook (void);
2317 extern void vApplicationStackOverflowHook (TaskHandle_t xTask, signed char *pcTaskName);
2318
2319 /**
2320   Dummy implementation of the callback function vApplicationIdleHook().
2321 */
2322 #if (configUSE_IDLE_HOOK == 1)
2323 __WEAK void vApplicationIdleHook (void){}
2324 #endif
2325
2326 /**
2327   Dummy implementation of the callback function vApplicationTickHook().
2328 */
2329 #if (configUSE_TICK_HOOK == 1)
2330  __WEAK void vApplicationTickHook (void){}
2331 #endif
2332
2333 /**
2334   Dummy implementation of the callback function vApplicationMallocFailedHook().
2335 */
2336 #if (configUSE_MALLOC_FAILED_HOOK == 1)
2337 __WEAK void vApplicationMallocFailedHook (void){}
2338 #endif
2339
2340 /**
2341   Dummy implementation of the callback function vApplicationDaemonTaskStartupHook().
2342 */
2343 #if (configUSE_DAEMON_TASK_STARTUP_HOOK == 1)
2344 __WEAK void vApplicationDaemonTaskStartupHook (void){}
2345 #endif
2346
2347 /**
2348   Dummy implementation of the callback function vApplicationStackOverflowHook().
2349 */
2350 #if (configCHECK_FOR_STACK_OVERFLOW > 0)
2351 __WEAK void vApplicationStackOverflowHook (TaskHandle_t xTask, signed char *pcTaskName) {
2352   (void)xTask;
2353   (void)pcTaskName;
2354   configASSERT(0);
2355 }
2356 #endif
2357
2358 /*---------------------------------------------------------------------------*/
2359
2360 /* External Idle and Timer task static memory allocation functions */
2361 extern void vApplicationGetIdleTaskMemory  (StaticTask_t **ppxIdleTaskTCBBuffer,  StackType_t **ppxIdleTaskStackBuffer,  uint32_t *pulIdleTaskStackSize);
2362 extern void vApplicationGetTimerTaskMemory (StaticTask_t **ppxTimerTaskTCBBuffer, StackType_t **ppxTimerTaskStackBuffer, uint32_t *pulTimerTaskStackSize);
2363
2364 /*
2365   vApplicationGetIdleTaskMemory gets called when configSUPPORT_STATIC_ALLOCATION
2366   equals to 1 and is required for static memory allocation support.
2367 */
2368 __WEAK void vApplicationGetIdleTaskMemory (StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize) {
2369   /* Idle task control block and stack */
2370   static StaticTask_t Idle_TCB;
2371   static StackType_t  Idle_Stack[configMINIMAL_STACK_SIZE];
2372
2373   *ppxIdleTaskTCBBuffer   = &Idle_TCB;
2374   *ppxIdleTaskStackBuffer = &Idle_Stack[0];
2375   *pulIdleTaskStackSize   = (uint32_t)configMINIMAL_STACK_SIZE;
2376 }
2377
2378 /*
2379   vApplicationGetTimerTaskMemory gets called when configSUPPORT_STATIC_ALLOCATION
2380   equals to 1 and is required for static memory allocation support.
2381 */
2382 __WEAK void vApplicationGetTimerTaskMemory (StaticTask_t **ppxTimerTaskTCBBuffer, StackType_t **ppxTimerTaskStackBuffer, uint32_t *pulTimerTaskStackSize) {
2383   /* Timer task control block and stack */
2384   static StaticTask_t Timer_TCB;
2385   static StackType_t  Timer_Stack[configTIMER_TASK_STACK_DEPTH];
2386
2387   *ppxTimerTaskTCBBuffer   = &Timer_TCB;
2388   *ppxTimerTaskStackBuffer = &Timer_Stack[0];
2389   *pulTimerTaskStackSize   = (uint32_t)configTIMER_TASK_STACK_DEPTH;
2390 }