CMSIS OS None

/* ----------------------------------------------------------------------
 * Copyright (C) 2011 ARM Limited. All rights reserved.
 *
 * $Date:        10. Februar 2012
 * $Revision:    V0.03
 *
 * Project:      CMSIS-RTOS API
 * Title:        cmsis_os.h template header file
 *
 * Version 0.02
 *    Initial Proposal Phase
 * Version 0.03
 *    osKernelStart added, optional feature: main started as thread
 *    osSemaphores have standard behaviour
 *    osTimerCreate does not start the timer, added osTimerStart
 *    osThreadPass is renamed to osThreadYield
 * -------------------------------------------------------------------- */

#ifndef _CMSIS_OS_H
#define _CMSIS_OS_H

#include "lpclib_types.h"

/// \note MUST REMAIN UNCHANGED: \b osCMSIS identifies the CMSIS-RTOS API version
#define osCMSIS           0x00003      ///< API version (main [31:16] .sub [15:0])

/// \note CAN BE CHANGED: \b osCMSIS_KERNEL identifies the underlaying RTOS kernel and version number.
#define osCMSIS_KERNEL    0x10000       ///< RTOS identification and version (main [31:16] .sub [15:0])

/// \note MUST REMAIN UNCHANGED: \b osKernelSystemId shall be consistent in every CMSIS-RTOS.
#define osKernelSystemId "KERNEL V1.00"   ///< RTOS identification string

/// \note MUST REMAIN UNCHANGED: \b osFeature_xxx shall be consistent in every CMSIS-RTOS.
#define osFeature_MainThread   1       ///< main thread      1=main can be thread, 0=not available
#define osFeature_Pool         1       ///< Memory Pools:    1=available, 0=not available
#define osFeature_MailQ        1       ///< Mail Queues:     1=available, 0=not available
#define osFeature_MessageQ     1       ///< Message Queues:  1=available, 0=not available
#define osFeature_Signals      8       ///< maximum number of Signal Flags available per thread
#define osFeature_Semaphore    30      ///< maximum count for SemaphoreInit function
#define osFeature_Wait         1       ///< osWait function: 1=available, 0=not available

#include <stdint.h>
#include <stddef.h>

#ifdef  __cplusplus
extern "C"
{
#endif


// ==== Enumeration, structures, defines ====

/// Priority used for thread control.
/// \note MUST REMAIN UNCHANGED: \b osPriority shall be consistent in every CMSIS-RTOS.
typedef enum  {
  osPriorityIdle          = -3,          ///< priority: idle (lowest)
  osPriorityLow           = -2,          ///< priority: low
  osPriorityBelowNormal   = -1,          ///< priority: below normal
  osPriorityNormal        =  0,          ///< priority: normal (default)
  osPriorityAboveNormal   = +1,          ///< priority: above normal
  osPriorityHigh          = +2,          ///< priority: high
  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
  osPriorityError         =  0x84,       ///< system cannot determine priority or thread has illegal priority
} osPriority;

/// Timeout value
/// \note MUST REMAIN UNCHANGED: \b osWaitForever shall be consistent in every CMSIS-RTOS.
#define osWaitForever     0xFFFFFFFF     ///< wait forever timeout value

/// Status code values returned by CMSIS-RTOS functions
/// \note MUST REMAIN UNCHANGED: \b osStatus shall be consistent in every CMSIS-RTOS.
typedef enum {
  osOK                    =     0,       ///< function completed; no event occurred.
  osEventSignal           =  0x08,       ///< function completed; signal event occurred.
  osEventMessage          =  0x10,       ///< function completed; message event occurred.
  osEventMail             =  0x20,       ///< function completed; mail event occurred.
  osEventTimeout          =  0x40,       ///< function completed; timeout occurred.
  osErrorParameter        =  0x80,       ///< parameter error: a mandatory parameter was missing or specified an incorrect object.
  osErrorResource         =  0x81,       ///< resource not available: a specified resource was not available.
  osErrorTimeoutResource  =  0xC1,       ///< resource not available within given time: a specified resource was not available within the timeout period.
  osErrorISR              =  0x82,       ///< not allowed in ISR context: the function cannot be called from interrupt service routines.
  osErrorISRRecursive     =  0x83,       ///< function called multiple times from ISR with same object.
  osErrorPriority         =  0x84,       ///< system cannot determine priority or thread has illegal priority.
  osErrorNoMemory         =  0x85,       ///< system is out of memory: it was impossible to allocate or reserve memory for the operation.
  osErrorValue            =  0x86,       ///< value of a parameter is out of range.
  osErrorOS               =  0xFF,       ///< unspecified RTOS error: run-time error but no other error message fits.
  os_status_reserved      =  0x7FFFFFFF, ///< prevent from enum down-size compiler optimization.
} osStatus;


/// Timer type value for the timer definition
/// \note MUST REMAIN UNCHANGED: \b os_timer_type shall be consistent in every CMSIS-RTOS.
typedef enum   {
  osTimerOnce             =     0,       ///< one-shot timer
  osTimerPeriodic         =     1,       ///< repeating timer
} os_timer_type;

/// Entry point of a thread.
/// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS.
typedef void (*os_pthread) (void const *argument);

/// Entry point of a timer call back function.
/// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS.
typedef void (*os_ptimer) (void const *argument);

// >>> the following data type definitions may shall adapted towards a specific RTOS

/// Thread ID identifies the thread (pointer to a thread control block).
/// \note CAN BE CHANGED: \b os_thread_cb is implementation specific in every CMSIS-RTOS.
typedef void * osThreadId;

/// Timer ID identifies the timer (pointer to a timer control block).
/// \note CAN BE CHANGED: \b os_timer_cb is implementation specific in every CMSIS-RTOS.
typedef void * osTimerId;

/// Mutex ID identifies the mutex (pointer to a mutex control block).
/// \note CAN BE CHANGED: \b os_mutex_cb is implementation specific in every CMSIS-RTOS.
typedef volatile uint8_t * osMutexId;

/// Semaphore ID identifies the semaphore (pointer to a semaphore control block).
/// \note CAN BE CHANGED: \b os_semaphore_cb is implementation specific in every CMSIS-RTOS.
typedef volatile uint8_t * osSemaphoreId;

/// Pool ID identifies the memory pool (pointer to a memory pool control block).
/// \note CAN BE CHANGED: \b os_pool_cb is implementation specific in every CMSIS-RTOS.
typedef struct osPool_cb *osPoolId;

/// Message ID identifies the message queue (pointer to a message queue control block).
/// \note CAN BE CHANGED: \b os_messageQ_cb is implementation specific in every CMSIS-RTOS.
typedef void *osMessageQId;

/// Mail ID identifies the mail queue (pointer to a mail queue control block).
/// \note CAN BE CHANGED: \b os_mailQ_cb is implementation specific in every CMSIS-RTOS.
typedef struct osMailQ_cb *osMailQId;


/// Thread Definition structure contains startup information of a thread.
/// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS.
typedef const struct os_thread_def  {
  char *                 name;
  os_pthread             pthread;    ///< start address of thread function
  osPriority             tpriority;    ///< initial thread priority
  uint32_t               instances;    ///< maximum number of instances of that thread function
  uint32_t               stacksize;    ///< stack size requirements in bytes; 0 is default stack size
} osThreadDef_t;

/// Timer Definition structure contains timer parameters.
/// \note CAN BE CHANGED: \b os_timer_def is implementation specific in every CMSIS-RTOS.
struct os_timer_custom {
    struct os_timer_custom *previous;
    struct os_timer_custom *next;
    os_timer_type type;
    uint32_t remainingTicks;
    uint32_t period;
    void *argument;
    const struct os_timer_def *parent;
};

typedef const struct os_timer_def {
    os_ptimer                 ptimer;    ///< start address of a timer function
    struct os_timer_custom    *custom;
} osTimerDef_t;

/// Mutex Definition structure contains setup information for a mutex.
/// \note CAN BE CHANGED: \b os_mutex_def is implementation specific in every CMSIS-RTOS.
typedef struct os_mutex_def  {
    uint8_t               value;
} osMutexDef_t;

/// Semaphore Definition structure contains setup information for a semaphore.
/// \note CAN BE CHANGED: \b os_semaphore_def is implementation specific in every CMSIS-RTOS.
typedef struct os_semaphore_def  {
  uint8_t                   value;    ///< dummy value.
} osSemaphoreDef_t;

/// Definition structure for memory block allocation
/// \note CAN BE CHANGED: \b os_pool_def is implementation specific in every CMSIS-RTOS.
typedef const struct os_pool_def  {
    uint32_t                 pool_sz;    ///< number of items (elements) in the pool
    uint32_t                 item_sz;    ///< size of an item
    struct osPool_cb         *cb;      ///< pointer to memory for pool
} osPoolDef_t;

/// Definition structure for message queue
/// \note CAN BE CHANGED: \b os_messageQ_def is implementation specific in every CMSIS-RTOS.
typedef const struct os_messageQ_def  {
    uint32_t                queue_sz;    ///< number of elements in the queue
    uint32_t                 item_sz;    ///< size of an item
    void                       *pool;    ///< memory array for messages
} osMessageQDef_t;

/// Definition structure for mail queue
/// \note CAN BE CHANGED: \b os_mailQ_def is implementation specific in every CMSIS-RTOS.
typedef const struct os_mailQ_def  {
    uint32_t queue_sz;                      /**< number of elements in the queue */
    uint32_t item_sz;                       /**< size of an item */
    struct osMailQ_cb *cb;
} osMailQDef_t;

/// Event structure contains detailed information about an event.
/// \note MUST REMAIN UNCHANGED: \b os_event shall be consistent in every CMSIS-RTOS.
///       However the struct may be extended at the end.
typedef struct  {
  osStatus                 status;     ///< status code: event or error information
  union  {
    uint32_t                    v;     ///< message as 32-bit value
    void                       *p;     ///< message or mail as void pointer
    int32_t               signals;     ///< signal flags
  } value;                             ///< event value
  union  {
    osMailQId             mail_id;     ///< mail id obtained by \ref osMailCreate
    osMessageQId       message_id;     ///< message id obtained by \ref osMessageCreate
  } def;                               ///< event definition
} osEvent;



void _osSysTick_Handler (void);


//  ==== Kernel Control Functions ====

osStatus osKernelStart (osThreadDef_t *thread_def, void *argument);

static int32_t osKernelRunning (void);
__FORCEINLINE(int32_t osKernelRunning (void))
{
    return 1;
}


//  ==== Thread Management ====

/// Create a Thread Definition with function, priority, and stack requirements.
/// \param         name         name of the thread function.
/// \param         priority     initial priority of the thread function.
/// \param         instances    number of possible thread instances.
/// \param         stacksz      stack size (in bytes) requirements for the thread function.
/// \note CAN BE CHANGED: The parameters to \b osThreadDef shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#if defined (osObjectsExternal)  // object is external
#define osThreadDef(name, priority, instances, stacksz)  \
extern osThreadDef_t os_thread_def_##name;
#else                            // define the object
#define osThreadDef(name, priority, instances, stacksz)  \
osThreadDef_t os_thread_def_##name = \
{ #name, (name), (priority), (instances), (stacksz)  };
#endif

/// Access a Thread defintion.
/// \param         name          name of the thread definition object.
/// \note CAN BE CHANGED: The parameter to \b osThread shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#define osThread(name)  \
&os_thread_def_##name

/// Create a thread and add it to Active Threads and set it to state READY.
/// \param[in]     thread_def    thread definition referenced with \ref osThread.
/// \param[in]     argument      pointer that is passed to the thread function as start argument.
/// \return thread ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osThreadCreate shall be consistent in every CMSIS-RTOS.
__FORCEINLINE(osThreadId osThreadCreate (osThreadDef_t *thread_def, void *argument))
{
    (void) thread_def;
    (void) argument;

    return NULL;
}


/// Return the thread ID of the current running thread.
/// \return thread ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osThreadGetId shall be consistent in every CMSIS-RTOS.
__FORCEINLINE(osThreadId osThreadGetId (void))
{
    return NULL;
}


/// Terminate execution of a thread and remove it from Active Threads.
/// \param[in]     thread_id   thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osThreadTerminate shall be consistent in every CMSIS-RTOS.
__FORCEINLINE(osStatus osThreadTerminate (osThreadId thread_id))
{
    (void) thread_id;

    return osOK;
}


/// Pass control to next thread that is in state \b READY.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osThreadYield shall be consistent in every CMSIS-RTOS.
__FORCEINLINE(osStatus osThreadYield (void))
{
    return osOK;
}


/// Change priority of an active thread.
/// \param[in]     thread_id     thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
/// \param[in]     priority      new priority value for the thread function.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osThreadSetPriority shall be consistent in every CMSIS-RTOS.
__FORCEINLINE(osStatus osThreadSetPriority (osThreadId thread_id, osPriority priority))
{
    (void) thread_id;
    (void) priority;

    return osErrorResource;
}


/// Get current priority of an active thread.
/// \param[in]     thread_id     thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
/// \return current priority value of the thread function.
/// \note MUST REMAIN UNCHANGED: \b osThreadGetPriority shall be consistent in every CMSIS-RTOS.
__FORCEINLINE(osPriority osThreadGetPriority (osThreadId thread_id))
{
    (void) thread_id;

    return osPriorityError;
}


//  ==== Generic Wait Functions ====

extern volatile uint32_t os_time;


/// Wait for Timeout (Time Delay)
/// \param[in]     millisec      time delay value
/// \return status code that indicates the execution status of the function.
osStatus osDelay (uint32_t millisec);

#if (defined (osFeature_Wait)  &&  (osFeature_Wait != 0))     // Generic Wait available

/// Wait for Signal, Message, Mail, or Timeout
/// \param[in] millisec          timeout value or 0 in case of no time-out
/// \return event that contains signal, message, or mail information or error code.
/// \note MUST REMAIN UNCHANGED: \b osWait shall be consistent in every CMSIS-RTOS.
osEvent osWait (uint32_t millisec);

#endif  // Generic Wait available


//  ==== Timer Management Functions ====
/// Define a Timer object.
/// \param         name          name of the timer object.
/// \param         function      name of the timer call back function.
/// \note CAN BE CHANGED: The parameter to \b osTimerDef shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#if defined (osObjectsExternal)  // object is external
#define osTimerDef(name, function)  \
extern osTimerDef_t os_timer_def_##name; \
extern struct os_timer_custom os_timer_custome_##name;
#else                            // define the object
#define osTimerDef(name, function)  \
struct os_timer_custom os_timer_custom_##name; \
osTimerDef_t os_timer_def_##name = \
{ (function), (&os_timer_custom_##name) };
#endif

/// Access a Timer definition.
/// \param         name          name of the timer object.
/// \note CAN BE CHANGED: The parameter to \b osTimer shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#define osTimer(name) \
&os_timer_def_##name

/// Create a timer.
/// \param[in]     timer_def     timer object referenced with \ref osTimer.
/// \param[in]     type          osTimerOnce for one-shot or osTimerPeriodic for periodic behavior.
/// \param[in]     argument      argument to the timer call back function.
/// \return timer ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osTimerCreate shall be consistent in every CMSIS-RTOS.
osTimerId osTimerCreate (osTimerDef_t *timer_def, os_timer_type type, void *argument);

/// Start or restart a timer.
/// \param[in]     timer_id      timer ID obtained by \ref osTimerCreate.
/// \param[in]     millisec      time delay value of the timer.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osTimerStart shall be consistent in every CMSIS-RTOS.
osStatus osTimerStart (osTimerId timer_id, uint32_t millisec);

/// Stop the timer.
/// \param[in]     timer_id      timer ID obtained by \ref osTimerCreate.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osTimerStop shall be consistent in every CMSIS-RTOS.
osStatus osTimerStop (osTimerId timer_id);


//  ==== Signal Management ====

/// Set the specified Signal Flags of an active thread.
/// \param[in]     thread_id     thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
/// \param[in]     signals       specifies the signal flags of the thread that should be set.
/// \return previous signal flags of the specified thread or 0x80000000 in case of incorrect parameters.
/// \note MUST REMAIN UNCHANGED: \b osSignalSet shall be consistent in every CMSIS-RTOS.
int32_t osSignalSet (osThreadId thread_id, int32_t signal);

/// Clear the specified Signal Flags of an active thread.
/// \param[in]     thread_id     thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
/// \param[in]     signals       specifies the signal flags of the thread that shall be cleared.
/// \return previous signal flags of the specified thread or 0x80000000 in case of incorrect parameters.
/// \note MUST REMAIN UNCHANGED: \b osSignalClear shall be consistent in every CMSIS-RTOS.
int32_t osSignalClear (osThreadId thread_id, int32_t signal);

/// Get Signal Flags status of an active thread.
/// \param[in]     thread_id     thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
/// \return previous signal flags of the specified thread or 0x80000000 in case of incorrect parameters.
/// \note MUST REMAIN UNCHANGED: \b osSignalGet shall be consistent in every CMSIS-RTOS.
int32_t osSignalGet (osThreadId thread_id);

/// Wait for one or more Signal Flags to become signaled for the current \b RUNNING thread.
/// \param[in]     signals       wait until all specified signal flags set or 0 for any single signal flag.
/// \param[in]     millisec      timeout value or 0 in case of no time-out.
/// \return event flag information or error code.
/// \note MUST REMAIN UNCHANGED: \b osSignalWait shall be consistent in every CMSIS-RTOS.
osEvent osSignalWait (int32_t signals, uint32_t millisec);


//  ==== Mutex Management ====

/// Define a Mutex.
/// \param         name          name of the mutex object.
/// \note CAN BE CHANGED: The parameter to \b osMutexDef shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#if defined (osObjectsExternal)  // object is external
#define osMutexDef(name)  \
extern osMutexDef_t os_mutex_def_##name;
#else                            // define the object
#define osMutexDef(name)  \
osMutexDef_t os_mutex_def_##name = { 0 };
#endif

/// Access a Mutex defintion.
/// \param         name          name of the mutex object.
/// \note CAN BE CHANGED: The parameter to \b osMutex shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#define osMutex(name)  \
&os_mutex_def_##name

/// Create and Initialize a Mutex object
/// \param[in]     mutex_def     mutex definition referenced with \ref osMutex.
/// \return mutex ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osMutexCreate shall be consistent in every CMSIS-RTOS.
static osMutexId osMutexCreate (osMutexDef_t *mutex_def);
__FORCEINLINE(osMutexId osMutexCreate (osMutexDef_t *mutex_def))
{
    return &mutex_def->value;
}



/// Wait until a Mutex becomes available
/// \param[in]     mutex_id      mutex ID obtained by \ref osMutexCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out.
/// \return status code that indicates the execution status of the function.
static osStatus osMutexWait (osMutexId mutex_id, uint32_t millisec);
__FORCEINLINE(osStatus osMutexWait (osMutexId mutex_id, uint32_t millisec))
{
    if (millisec != 0) {
        while (*mutex_id != 0)
            ;               //TODO: add timeout
    }
    *mutex_id = 1;

    return osOK;
}



/// Release a Mutex that was obtained by \ref osMutexWait
/// \param[in]     mutex_id      mutex ID obtained by \ref osMutexCreate.
/// \return status code that indicates the execution status of the function.
static osStatus osMutexRelease (osMutexId mutex_id);
__FORCEINLINE(osStatus osMutexRelease (osMutexId mutex_id))
{
    *mutex_id = 0;

    return osOK;
}


//  ==== Semaphore Management Functions ====

#if (defined (osFeature_Semaphore)  &&  (osFeature_Semaphore != 0))     // Semaphore available

/// Define a Semaphore object.
/// \param         name          name of the semaphore object.
/// \note CAN BE CHANGED: The parameter to \b osSemaphoreDef shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#if defined (osObjectsExternal)  // object is external
#define osSemaphoreDef(name)  \
extern osSemaphoreDef_t os_semaphore_def_##name;
#else                            // define the object
#define osSemaphoreDef(name)  \
osSemaphoreDef_t os_semaphore_def_##name = { 0 };
#endif

/// Access a Semaphore definition.
/// \param         name          name of the semaphore object.
/// \note CAN BE CHANGED: The parameter to \b osSemaphore shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#define osSemaphore(name)  \
&os_semaphore_def_##name

/// Create and Initialize a Semaphore object used for managing resources
/// \param[in]     semaphore_def semaphore definition referenced with \ref osSemaphore.
/// \param[in]     count         number of available resources.
/// \return semaphore ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osSemaphoreCreate shall be consistent in every CMSIS-RTOS.
static osSemaphoreId osSemaphoreCreate (osSemaphoreDef_t *semaphore_def, int32_t count);
__FORCEINLINE(osSemaphoreId osSemaphoreCreate (osSemaphoreDef_t *semaphore_def, int32_t count))
{
    (void) count;

    semaphore_def->value = 1;               /* Only binary semaphore */

    return &semaphore_def->value;
}



/// Wait until a Semaphore token becomes available
/// \param[in]     semaphore_id  semaphore object referenced with \ref osSemaphore.
/// \param[in]     millisec      timeout value or 0 in case of no time-out.
/// \return number of available tokens, or -1 in case of incorrect parameters.
/// \note MUST REMAIN UNCHANGED: \b osSemaphoreWait shall be consistent in every CMSIS-RTOS.
static int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec);
__FORCEINLINE(int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec))
{
    if (*semaphore_id) {
        *semaphore_id = 0;
        
        return 1;
    }

    if (millisec != 0) {
        while (*semaphore_id == 0)
            ;               //TODO: add timeout

        *semaphore_id = 0;
        
        return 1;
    }

    return 0;
}



/// Release a Semaphore token
/// \param[in]     semaphore_id  semaphore object referenced with \ref osSemaphore.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osSemaphoreRelease shall be consistent in every CMSIS-RTOS.
static osStatus osSemaphoreRelease (osSemaphoreId semaphore_id);
__FORCEINLINE(osStatus osSemaphoreRelease (osSemaphoreId semaphore_id))
{
    *semaphore_id = 1;

    return osOK;
}



#endif     // Semaphore available




//  ==== Memory Pool Management Functions ====

#if (defined (osFeature_Pool)  &&  (osFeature_Pool != 0))  // Memory Pool Management available

struct osPool_cb {
    void *pool;
    uint8_t *markers;
    uint32_t pool_sz;
    uint32_t item_sz;
    uint32_t currentIndex;
};

/// \brief Define a Memory Pool.
/// \param         name          name of the memory pool.
/// \param         no            maximum number of objects (elements) in the memory pool.
/// \param         type          data type of a single object (element).
/// \note CAN BE CHANGED: The parameter to \b osPoolDef shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#if defined (osObjectsExternal)  // object is external
#define osPoolDef(name, no, type)   \
extern osPoolDef_t os_pool_def_##name;
#else                            // define the object
#define osPoolDef(name, no, type)   \
uint8_t os_pool_data_##name[no * sizeof(type)]; \
uint8_t os_pool_markers_##name[no]; \
struct osPool_cb os_pool_cb_##name = \
    {os_pool_data_##name, os_pool_markers_##name, (no), sizeof(type), 0}; \
osPoolDef_t os_pool_def_##name = \
    { (no), sizeof(type), &os_pool_cb_##name };
#endif

/// \brief Access a Memory Pool definition.
/// \param         name          name of the memory pool
/// \note CAN BE CHANGED: The parameter to \b osPool shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#define osPool(name) \
&os_pool_def_##name

/// Create and Initialize a memory pool
/// \param[in]     pool_def      memory pool definition referenced with \ref osPool.
/// \return memory pool ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osPoolCreate shall be consistent in every CMSIS-RTOS.
osPoolId osPoolCreate (osPoolDef_t *pool_def);

/// Allocate a memory block from a memory pool
/// \param[in]     pool_id       memory pool ID obtain referenced with \ref osPoolCreate.
/// \return address of the allocated memory block or NULL in case of no memory available.
/// \note MUST REMAIN UNCHANGED: \b osPoolAlloc shall be consistent in every CMSIS-RTOS.
void *osPoolAlloc (osPoolId pool_id);

/// Allocate a memory block from a memory pool and set memory block to zero
/// \param[in]     pool_id       memory pool ID obtain referenced with \ref osPoolCreate.
/// \return address of the allocated memory block or NULL in case of no memory available.
/// \note MUST REMAIN UNCHANGED: \b osPoolCAlloc shall be consistent in every CMSIS-RTOS.
void *osPoolCAlloc (osPoolId pool_id);

/// Return an allocated memory block back to a specific memory pool
/// \param[in]     pool_id       memory pool ID obtain referenced with \ref osPoolCreate.
/// \param[in]     block         address of the allocated memory block that is returned to the memory pool.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osPoolFree shall be consistent in every CMSIS-RTOS.
osStatus osPoolFree (osPoolId pool_id, void *block);

#endif   // Memory Pool Management available


//  ==== Message Queue Management Functions ====

#if (defined (osFeature_MessageQ)  &&  (osFeature_MessageQ != 0))     // Message Queues available

/// \brief Create a Message Queue Definition.
/// \param         name          name of the queue.
/// \param         queue_sz      maximum number of messages in the queue.
/// \param         type          data type of a single message element (for debugger).
/// \note CAN BE CHANGED: The parameter to \b osMessageQDef shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#if defined (osObjectsExternal)  // object is external
#define osMessageQDef(name, queue_sz, type)   \
extern osMessageQDef_t os_messageQ_def_##name;
#else                            // define the object
#define osMessageQDef(name, queue_sz, type)   \
osMessageQDef_t os_messageQ_def_##name = \
{ (queue_sz), sizeof (type)  };
#endif

/// \brief Access a Message Queue Definition.
/// \param         name          name of the queue
/// \note CAN BE CHANGED: The parameter to \b osMessageQ shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#define osMessageQ(name) \
&os_messageQ_def_##name

/// Create and Initialize a Message Queue.
/// \param[in]     queue_def     queue definition referenced with \ref osMessageQ.
/// \param[in]     thread_id     thread ID (obtained by \ref osThreadCreate or \ref osThreadGetId) or NULL.
/// \return message queue ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osMessageCreate shall be consistent in every CMSIS-RTOS.
osMessageQId osMessageCreate (osMessageQDef_t *queue_def, osThreadId thread_id);

/// Put a Message to a Queue.
/// \param[in]     queue_id      message queue ID obtained with \ref osMessageCreate.
/// \param[in]     info          message information.
/// \param[in]     millisec      timeout value or 0 in case of no time-out.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osMessagePut shall be consistent in every CMSIS-RTOS.
osStatus osMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec);

/// Get a Message or Wait for a Message from a Queue.
/// \param[in]     queue_id      message queue ID obtained with \ref osMessageCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out.
/// \return event information that includes status code.
/// \note MUST REMAIN UNCHANGED: \b osMessageGet shall be consistent in every CMSIS-RTOS.
osEvent osMessageGet (osMessageQId queue_id, uint32_t millisec);

#endif     // Message Queues available


//  ==== Mail Queue Management Functions ====

#if (defined (osFeature_MailQ)  &&  (osFeature_MailQ != 0))     // Mail Queues available

struct osMailQ_cb {
    volatile int wrptr;
    volatile int rdptr;
    int queueSize;
    int itemSize;
    void *data;
};


/// \brief Create a Mail Queue Definition
/// \param         name          name of the queue
/// \param         queue_sz      maximum number of messages in queue
/// \param         type          data type of a single message element
/// \note CAN BE CHANGED: The parameter to \b osMailQDef shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#if defined (osObjectsExternal)  // object is external
    #define osMailQDef(name, queue_sz, type) \
        extern struct os_mailQ_cb *os_mailQ_cb_##name; \
        extern osMailQDef_t os_mailQ_def_##name;
#else                            // define the object
    #define osMailQDef(name, queue_sz, type) \
        uint8_t os_mailQ_data_##name[(queue_sz) * sizeof(type)]; \
        struct osMailQ_cb os_mailQ_cb_##name = \
            {0, 0, (queue_sz), sizeof(type), os_mailQ_data_##name}; \
        osMailQDef_t os_mailQ_def_##name =  \
            {(queue_sz), sizeof(type), &os_mailQ_cb_##name};
#endif

/// \brief Access a Mail Queue Definition
/// \param         name          name of the queue
/// \note CAN BE CHANGED: The parameter to \b osMailQ shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.
#define osMailQ(name)  \
&os_mailQ_def_##name

/// Create and Initialize mail queue
/// \param[in]     queue_def     reference to the mail queue definition obtain with \ref osMailQ
/// \param[in]     thread_id     thread ID (obtained by \ref osThreadCreate or \ref osThreadGetId) or NULL.
/// \return mail queue ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osMailCreate shall be consistent in every CMSIS-RTOS.
osMailQId osMailCreate (osMailQDef_t *queue_def, osThreadId thread_id);

/// Allocate a memory block from a mail
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out
/// \return pointer to memory block that can be filled with mail or NULL in case error.
/// \note MUST REMAIN UNCHANGED: \b osMailAlloc shall be consistent in every CMSIS-RTOS.
void *osMailAlloc (osMailQId queue_id, uint32_t millisec);

/// Allocate a memory block from a mail and set memory block to zero
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out
/// \return pointer to memory block that can shall filled with mail or NULL in case error.
/// \note MUST REMAIN UNCHANGED: \b osMailCAlloc shall be consistent in every CMSIS-RTOS.
void *osMailCAlloc (osMailQId queue_id, uint32_t millisec);

/// Put a mail to a queue
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     mail          memory block previously allocated with \ref osMailAlloc or \ref osMailCAlloc.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osMailPut shall be consistent in every CMSIS-RTOS.
osStatus osMailPut (osMailQId queue_id, void *mail);

/// Get a mail from a queue
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out
/// \return event that contains mail information or error code.
/// \note MUST REMAIN UNCHANGED: \b osMailGet shall be consistent in every CMSIS-RTOS.
osEvent osMailGet (osMailQId queue_id, uint32_t millisec);

/// Free a memory block from a mail
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     mail          pointer to the memory block that was obtained with \ref osMailGet.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osMailFree shall be consistent in every CMSIS-RTOS.
osStatus osMailFree (osMailQId queue_id, void *mail);

#endif  // Mail Queues available


#ifdef  __cplusplus
}
#endif

#endif  // _CMSIS_OS_H

#include "lpclib.h"
#include "cmsis_os.h"


/* Determine whether we are in thread mode or handler mode. */
static int inHandlerMode (void)
{
    return 0;
}


/* Timer related globals */
volatile uint32_t os_time;                              /**< Number of SysTick ticks */
static struct os_timer_custom *appTimers;               /**< Timer list */
static volatile uint32_t appTicksMult = 1024;
static volatile uint32_t osDelayTimer;


void SysTick_Handler (void)
{
    struct os_timer_custom *p;

    ++os_time;

    /* Work on the list of timers */
    p = appTimers;
    while (p != NULL) {
        if (p->remainingTicks != 0) {
            --(p->remainingTicks);
            if (p->remainingTicks == 0) {
                p->parent->ptimer(p->argument);

                if (p->type == osTimerPeriodic) {
                    p->remainingTicks = p->period;
                }
            }
        }

        p = p->next;
    }

    if (osDelayTimer) {
        --osDelayTimer;
    }
}




osStatus osKernelStart (osThreadDef_t *thread_def, void *argument)
{
    uint32_t rvr;
    uint32_t tenms;
    (void) thread_def;
    (void) argument;


    /* Calculate factor for ticks<->milliseconds transformation */
    rvr = (SysTick->LOAD & SysTick_LOAD_RELOAD_Msk) >> SysTick_LOAD_RELOAD_Pos;
    tenms = (SysTick->CALIB & SysTick_CALIB_TENMS_Msk) >> SysTick_CALIB_TENMS_Pos;
    if (tenms == 0) {
        /* TODO: Wild guess: 100 MHz core clock */
        tenms = 1000000;
    }
    appTicksMult = (((tenms * 256) / rvr) * 4) / 10;

    return osOK;
}


//  ==== Generic Wait Functions ====

/// Wait for Timeout (Time Delay)
/// \param[in]     millisec      time delay value
/// \return status code that indicates the execution status of the function.
osStatus osDelay (uint32_t millisec)
{
    /* Assume 10-ms tick...  TODO */
    osDelayTimer = (millisec + 5) / 10 + 1;
    while (osDelayTimer)
        ;

    return osOK;
}


#if (defined (osFeature_Wait)  &&  (osFeature_Wait != 0))     // Generic Wait available

/// Wait for Signal, Message, Mail, or Timeout
/// \param[in] millisec          timeout value or 0 in case of no time-out
/// \return event that contains signal, message, or mail information or error code.
/// \note MUST REMAIN UNCHANGED: \b osWait shall be consistent in every CMSIS-RTOS.
osEvent osWait (uint32_t millisec);

#endif  // Generic Wait available


//  ==== Timer Management Functions ====


/// Create a timer.
/// \param[in]     timer_def     timer object referenced with \ref osTimer.
/// \param[in]     type          osTimerOnce for one-shot or osTimerPeriodic for periodic behavior.
/// \param[in]     argument      argument to the timer call back function.
/// \return timer ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osTimerCreate shall be consistent in every CMSIS-RTOS.
osTimerId osTimerCreate (osTimerDef_t *timer_def, os_timer_type type, void *argument)
{
    struct os_timer_custom *p;


    /* Store context parameters */
    timer_def->custom->argument = argument;
    timer_def->custom->type = type;
    timer_def->custom->remainingTicks = 0;
    timer_def->custom->period = 0;
    timer_def->custom->parent = timer_def;

    /* Add to timer list */
    timer_def->custom->next = NULL;

    if (appTimers == NULL) {
        timer_def->custom->previous = NULL;
        appTimers = timer_def->custom;
    }
    else {
        p = appTimers;
        while (p->next) {
            p = p->next;
        }

        timer_def->custom->previous = p;
        p->next = timer_def->custom;
    }

    return timer_def->custom;
}



/// Start or restart a timer.
/// \param[in]     timer_id      timer ID obtained by \ref osTimerCreate.
/// \param[in]     millisec      time delay value of the timer.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osTimerStart shall be consistent in every CMSIS-RTOS.
osStatus osTimerStart (osTimerId timer_id, uint32_t millisec)
{
    uint32_t ticks;


    if (timer_id == NULL) {
        return osErrorResource;
    }

    ticks = (appTicksMult * millisec) / 1024;

    ((struct os_timer_custom *)timer_id)->period = ticks;
    ((struct os_timer_custom *)timer_id)->remainingTicks = ticks;

    return osOK;
}



/// Stop the timer.
/// \param[in]     timer_id      timer ID obtained by \ref osTimerCreate.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osTimerStop shall be consistent in every CMSIS-RTOS.
osStatus osTimerStop (osTimerId timer_id)
{
    if (timer_id == NULL) {
        return osErrorResource;
    }

    ((struct os_timer_custom *)timer_id)->remainingTicks = 0;

    return osOK;
}



//  ==== Memory Pool Management Functions ====

#if (defined (osFeature_Pool)  &&  (osFeature_Pool != 0))  // Memory Pool Management available

/// \brief Define a Memory Pool.
/// \param         name          name of the memory pool.
/// \param         no            maximum number of objects (elements) in the memory pool.
/// \param         type          data type of a single object (element).
/// \note CAN BE CHANGED: The parameter to \b osPoolDef shall be consistent but the
///       macro body is implementation specific in every CMSIS-RTOS.

/* Create and Initialize a memory pool */
osPoolId osPoolCreate (osPoolDef_t *pool_def)
{
    return pool_def->cb;
}


/// Allocate a memory block from a memory pool
/// \param[in]     pool_id       memory pool ID obtain referenced with \ref osPoolCreate.
/// \return address of the allocated memory block or NULL in case of no memory available.
/// \note MUST REMAIN UNCHANGED: \b osPoolAlloc shall be consistent in every CMSIS-RTOS.
void *osPoolAlloc (osPoolId pool_id)
{
    void *p = NULL;
    uint32_t i;
    uint32_t index;

    if (inHandlerMode()) {
        __disable_irq();     //TODO: Save current status!
    }

    for (i = 0; i < pool_id->pool_sz; i++) {
        index = pool_id->currentIndex + i;
        if (index >= pool_id->pool_sz) {
            index = 0;
        }

        if (pool_id->markers[index] == 0) {
            pool_id->markers[index] = 1;
            p = (void *)((uint32_t)(pool_id->pool) + (index * pool_id->item_sz));
            pool_id->currentIndex = index;
            break;
        }
    }

    if (!inHandlerMode()) {
        __enable_irq();     //TODO: Restore previous status!
    }

    return p;
}


/// Allocate a memory block from a memory pool and set memory block to zero
/// \param[in]     pool_id       memory pool ID obtain referenced with \ref osPoolCreate.
/// \return address of the allocated memory block or NULL in case of no memory available.
/// \note MUST REMAIN UNCHANGED: \b osPoolCAlloc shall be consistent in every CMSIS-RTOS.
void *osPoolCAlloc (osPoolId pool_id)
{
    unsigned int i;
    void *p = osPoolAlloc(pool_id);

    for (i = 0; i < pool_id->item_sz; i++) {
        ((uint8_t *)p)[i] = 0;
    }

    return p;
}


/// Return an allocated memory block back to a specific memory pool
/// \param[in]     pool_id       memory pool ID obtain referenced with \ref osPoolCreate.
/// \param[in]     block         address of the allocated memory block that is returned to the memory pool.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osPoolFree shall be consistent in every CMSIS-RTOS.
osStatus osPoolFree (osPoolId pool_id, void *block)
{
    uint32_t index;

    if (pool_id == NULL) {
        return osErrorParameter;
    }

    if (block == NULL) {
        return osErrorParameter;
    }

    if (block < pool_id->pool) {
        return osErrorParameter;
    }

    index = (uint32_t)block - (uint32_t)(pool_id->pool);
    if (index % pool_id->item_sz) {
        return osErrorParameter;
    }
    index = index / pool_id->item_sz;
    if (index >= pool_id->pool_sz) {
        return osErrorParameter;
    }

    pool_id->markers[index] = 0;

    return osOK;
}


#endif   // Memory Pool Management available


//  ==== Message Queue Management Functions ====

#if (defined (osFeature_MessageQ)  &&  (osFeature_MessageQ != 0))     // Message Queues available


/// Create and Initialize a Message Queue.
/// \param[in]     queue_def     queue definition referenced with \ref osMessageQ.
/// \param[in]     thread_id     thread ID (obtained by \ref osThreadCreate or \ref osThreadGetId) or NULL.
/// \return message queue ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osMessageCreate shall be consistent in every CMSIS-RTOS.
osMessageQId osMessageCreate (osMessageQDef_t *queue_def, osThreadId thread_id)
{
    (void) queue_def;
    (void) thread_id;

    return NULL;
}


/// Put a Message to a Queue.
/// \param[in]     queue_id      message queue ID obtained with \ref osMessageCreate.
/// \param[in]     info          message information.
/// \param[in]     millisec      timeout value or 0 in case of no time-out.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osMessagePut shall be consistent in every CMSIS-RTOS.
osStatus osMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec)
{
    (void) queue_id;
    (void) info;
    (void) millisec;

    return osOK;
}



/// Get a Message or Wait for a Message from a Queue.
/// \param[in]     queue_id      message queue ID obtained with \ref osMessageCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out.
/// \return event information that includes status code.
/// \note MUST REMAIN UNCHANGED: \b osMessageGet shall be consistent in every CMSIS-RTOS.
osEvent osMessageGet (osMessageQId queue_id, uint32_t millisec);

#endif     // Message Queues available


//  ==== Mail Queue Management Functions ====

#if (defined (osFeature_MailQ)  &&  (osFeature_MailQ != 0))     // Mail Queues available


/// Create and Initialize mail queue
/// \param[in]     queue_def     reference to the mail queue definition obtain with \ref osMailQ
/// \param[in]     thread_id     thread ID (obtained by \ref osThreadCreate or \ref osThreadGetId) or NULL.
/// \return mail queue ID for reference by other functions or NULL in case of error.
/// \note MUST REMAIN UNCHANGED: \b osMailCreate shall be consistent in every CMSIS-RTOS.
osMailQId osMailCreate (osMailQDef_t *queue_def, osThreadId thread_id)
{
    (void) thread_id;

    return queue_def->cb;
}



/** Check if mail queue is full. */
static int _osMailQueueFull (osMailQId queue_id)
{
    if ((((queue_id->rdptr + queue_id->queueSize) - queue_id->wrptr) % queue_id->queueSize) == 1) {
        return 1;   /* Queue full */
    }

    return 0;       /* Queue not full */
}


/// Allocate a memory block from a mail
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out
/// \return pointer to memory block that can be filled with mail or NULL in case error.
/// \note MUST REMAIN UNCHANGED: \b osMailAlloc shall be consistent in every CMSIS-RTOS.
void *osMailAlloc (osMailQId queue_id, uint32_t millisec)
{
    void *p = NULL;
    (void) millisec;


    if (queue_id == NULL) {
        return NULL;
    }

    if (!inHandlerMode()) {
        __disable_irq();
    }


    if (!_osMailQueueFull(queue_id)) {
        p = (void *)((uint32_t)(queue_id->data) + queue_id->wrptr * queue_id->itemSize);
        queue_id->wrptr = (queue_id->wrptr + 1) % queue_id->queueSize;
    }

    if (!inHandlerMode()) {
        __enable_irq();
    }

    return p;
}



/// Allocate a memory block from a mail and set memory block to zero
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out
/// \return pointer to memory block that can shall filled with mail or NULL in case error.
/// \note MUST REMAIN UNCHANGED: \b osMailCAlloc shall be consistent in every CMSIS-RTOS.
void *osMailCAlloc (osMailQId queue_id, uint32_t millisec)
{
    uint32_t i;
    void *p = osMailAlloc(queue_id, millisec);

    if (p) {
        for (i = 0; i < sizeof(queue_id->itemSize); i++) {
            ((uint8_t *)p)[i] = 0;
        }
    }

    return p;
}



/// Put a mail to a queue
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     mail          memory block previously allocated with \ref osMailAlloc or \ref osMailCAlloc.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osMailPut shall be consistent in every CMSIS-RTOS.
osStatus osMailPut (osMailQId queue_id, void *mail)
{
    if (queue_id == NULL) {
        return osErrorParameter;
    }

    if (mail == NULL) {
        return osErrorParameter;
    }

    //TODO:
    //The pointer returned by alloc() is a pointer to the queue!
    //Therefore the data is already where we want it!
    //Better to use a pool and a separate mail queue?

    return osOK;
}



/// Get a mail from a queue
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     millisec      timeout value or 0 in case of no time-out
/// \return event that contains mail information or error code.
/// \note MUST REMAIN UNCHANGED: \b osMailGet shall be consistent in every CMSIS-RTOS.
osEvent osMailGet (osMailQId queue_id, uint32_t millisec)
{
//TODO This function ignores the "millisec" parameter, and returns immediately

    osEvent rtosEvent;
    rtosEvent.status = osOK;
    (void) millisec;

    if (queue_id == NULL) {
        rtosEvent.status = osErrorParameter;
        return rtosEvent;
    }

    if (!inHandlerMode()) {
        __disable_irq();
    }

    if (queue_id->rdptr != queue_id->wrptr) {
        rtosEvent.value.p = (void *)((uint32_t)queue_id->data + queue_id->rdptr * queue_id->itemSize);

        rtosEvent.status = osEventMail;
    }

    if (!inHandlerMode()) {
        __enable_irq();
    }

    return rtosEvent;
}



/// Free a memory block from a mail
/// \param[in]     queue_id      mail queue ID obtained with \ref osMailCreate.
/// \param[in]     mail          pointer to the memory block that was obtained with \ref osMailGet.
/// \return status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osMailFree shall be consistent in every CMSIS-RTOS.
osStatus osMailFree (osMailQId queue_id, void *mail)
{
    int mailIndex;


    if (queue_id == NULL) {
        return osErrorParameter;
    }

    if (mail == NULL) {
        return osErrorParameter;
    }


    if (!inHandlerMode()) {
        __disable_irq();
    }

    /* Calculate index from pinter */
    mailIndex = ((uint32_t)mail - (uint32_t)queue_id->data) / queue_id->itemSize;

    /* Invalidate all queue items up to this item */
    queue_id->rdptr = (mailIndex + 1) % queue_id->queueSize;

    if (!inHandlerMode()) {
        __enable_irq();
    }

    return osOK;
}



#endif  // Mail Queues available