inverted counting semaphores

Hello, I have created a something what I am calling inverted counting semaphores. The main idea is that one task acquires inv_cnt_semaphore with defined value “v” and suspends itself until “v” times inv_cnt_semaphore is given. Inv_cnt_semaphore uses a struct with mutex and message queue. The mutex protects the inv_cnt_semaphore to be taken by higher priority task. The message queue is for re-usability(queue is deleted and created according to “v”) but can be also used standard counting semaphore. Inv_cnt_semaphore can timeout. Please if you see some horrible bug, please post. If you thing that this code is useful use it your will. Regards,
Martin
typedef struct INVCNT_SEMAPHORE {
    xSemaphoreHandle mutex;
    xQueueHandle msgQueue;
} INVCNT_SEMAPHORE;
portBASE_TYPE invCntCreate(INVCNT_SEMAPHORE * b) {
    b->mutex = xSemaphoreCreateMutex();
    return ((NULL == b->mutex) ? pdFALSE : pdTRUE);
}
portBASE_TYPE invCntTake(INVCNT_SEMAPHORE * b,
        unsigned portBASE_TYPE uxMaxCount, portTickType xTicksToWait) {
    unsigned portCHAR dummy;
    portBASE_TYPE err;
    portTickType timeout;
    // Lock resource to protect from taking by task with higher priority.
    err = xSemaphoreTake( b->mutex , (portTickType)0);
    if (pdTRUE != err) {
        // Taken failed. Maybe was taken before.
        return err;
    }
    // Delete message queue.
    if (NULL != b->msgQueue) {
        // Protect this deletion to avoid sending to null queue in condGive.
        portENTER_CRITICAL();
        // Delete queue.
        vQueueDelete(b->msgQueue);
        b->msgQueue = NULL;
        // Exit critical section.
        portEXIT_CRITICAL();
    }
    // Create message queue based on uxMaxCount value.
    // How many times can be condVar given.
    b->msgQueue = xQueueCreate(uxMaxCount, sizeof(unsigned portCHAR));
    // Process only if queue was created.
    if (NULL != b->msgQueue) {
        // Iterate thought uxMaxCount(until n*given or timeout).
        while (uxMaxCount != 0) {
            // Compute the timeout value.
            timeout = xTaskGetTickCount();
            // Receive item from queue.
            err = xQueueReceive(b->msgQueue, &(dummy), xTicksToWait);
            // Check for xQueueReceive return value.
            if (pdTRUE != err) {
                break;
            }
#if BARRIER_DUMMY_VAL_CHECK // Something wrong?
            // Check the value.
            if (BARRIER_DUMMY_VAL != dummy) {
                err = pdFALSE;
                break;
            }
#endif
            // If time is infinite, do not edit xTicksToWait.
            if (portMAX_DELAY != xTicksToWait) {
                // Get current tick time. To save function calling.
                portTickType timeElapsed = xTaskGetTickCount() - timeout;
                // Get current tick time and count how many ticks we should wait before the total timeout.
                // Probably xTicksToWait > timeElapsed should be always true.
                xTicksToWait = (xTicksToWait > timeElapsed) ? (xTicksToWait
                        - timeElapsed) : 0;
            }
            // Decrement number of given's.
            uxMaxCount--;
        }//while (uxMaxCount != 0)
    }
    // Release protecting mutex.
    xSemaphoreGive( b->mutex );
    // Return last error.
    return err;
}
portBASE_TYPE invCntGive(INVCNT_SEMAPHORE * b) {
    unsigned portCHAR dummy = BARRIER_DUMMY_VAL;
    portBASE_TYPE err;
    // Send to the cond var iff message queue is not NULL.
    if (NULL != b->msgQueue) {
        err = xQueueSend(b->msgQueue, &(dummy), (portTickType)0);
    } else {
        err = pdFALSE;
    }
    return err;
}
portBASE_TYPE invCntGiveFromISR(INVCNT_SEMAPHORE * b,
        portBASE_TYPE * pxHigherPriorityTaskWoken) {
    unsigned portCHAR dummy = BARRIER_DUMMY_VAL;
    if (NULL != b->msgQueue) {
        return xQueueSendFromISR(b->msgQueue, &(dummy), pxHigherPriorityTaskWoken);
    } else {
        return pdFALSE;
    }
}

inverted counting semaphores

There was a problem during invCntGive. After the initial check (NULL != b->msgQueue), the task could be preemted, queue deleted and invCntGive sent data to the NULL queue. Now, if invCntCreate is succesfull, INVCNT_SEMAPHOR.msgQueue point all the time on nonNULL value.
#include "task.h"
#include "queue.h"
#include "semphr.h"
#define BARRIER_DUMMY_VAL ((unsigned portCHAR)0xAA)
#define BARRIER_DUMMY_VAL_CHECK 1
typedef struct INVCNT_SEMAPHORE {
    xSemaphoreHandle mutex;
    xQueueHandle msgQueue;
    unsigned portBASE_TYPE uxMaxCount;
} INVCNT_SEMAPHORE;
portBASE_TYPE invCntCreate(INVCNT_SEMAPHORE * b) {
    // Create guarding mutex.
    b->mutex = xSemaphoreCreateMutex();
    // Create initial queue.
    b->msgQueue = xQueueCreate(1 , sizeof(unsigned portCHAR));
    // Return pdTRUE if all was created succesfully.
    return ((NULL == b->mutex) && (NULL == b->msgQueue) ? pdFALSE : pdTRUE);
}
portBASE_TYPE invCntTake(INVCNT_SEMAPHORE * b,
                         unsigned portBASE_TYPE uxMaxCount, portTickType xTicksToWait) 
{
    unsigned portCHAR dummy;
    portBASE_TYPE err;
    portTickType timeout;

    // Temporary created queue to avoid sending NULL during preemption in improper time.
    xQueueHandle tmpQueue;  
    // Previously created queue.
    xQueueHandle originalQueue;
    // Lock resource to protect from taking by task with higher priority.
    err = xSemaphoreTake( b->mutex , (portTickType)0);
    if (pdTRUE != err) {
        // Taken failed. Maybe was taken before.
        return err;
    }
    // Backup original pointer  
    originalQueue = b->msgQueue;
    // Create helper queue.
    tmpQueue = xQueueCreate(1 , sizeof(unsigned portCHAR));
    if(NULL == tmpQueue)
    {
        // If not created, release protecting mutex and return.
        xSemaphoreGive( b->mutex );
        return pdFALSE;
    }

    // If assigment is atomic, no need to enter critical section.
    portENTER_CRITICAL();
    // Assign the temporary queue into inv structure.
    b->msgQueue = tmpQueue;
    portEXIT_CRITICAL();
    // delete old msgqueue and create new.
    if (NULL != originalQueue) {                
        // Delete queue.
        vQueueDelete(originalQueue);
        // Create new queue with uxMaxCount items.
        originalQueue = xQueueCreate(uxMaxCount, sizeof(unsigned portCHAR));
    }   
    // Process only if queue was created.
    if (NULL != b->msgQueue) 
    {
        // If assigment is atomic, no need to enter critical section.
        portENTER_CRITICAL();
        // assign to new msgqueue to tmp in critical section
        b->msgQueue = originalQueue;
        portEXIT_CRITICAL();
        // Iterate thought uxMaxCount(until n*given or timeout).
        while (uxMaxCount != 0) {
            // Compute the timeout value.
            timeout = xTaskGetTickCount();
            // Receive item from queue.
            err = xQueueReceive(b->msgQueue, &(dummy), xTicksToWait);
            // Check for xQueueReceive return value.
            if (pdTRUE != err) {
                break;
            }
#if BARRIER_DUMMY_VAL_CHECK // Something wrong?
            // Check the value.
            if (BARRIER_DUMMY_VAL != dummy) {
                err = pdFALSE;
                break;
            }
#endif
            // If time is infinite, do not edit xTicksToWait.
            if (portMAX_DELAY != xTicksToWait) {
                // Get current tick time. To save function calling.
                portTickType timeElapsed = xTaskGetTickCount() - timeout;
                // Get current tick time and count how many ticks we should wait before the total timeout.
                // Probably xTicksToWait > timeElapsed should be always true.
                xTicksToWait = (xTicksToWait > timeElapsed) ? (xTicksToWait
                    - timeElapsed) : 0;
            }
            // Decrement number of given's.
            uxMaxCount--;
        }//while (uxMaxCount != 0)
    }
    // Delete tmp message queue.
    vQueueDelete(tmpQueue);
    // Release protecting mutex.
    xSemaphoreGive( b->mutex );
    // Return last error.
    return err;
}
portBASE_TYPE invCntGive(INVCNT_SEMAPHORE * b)
{
    unsigned portCHAR dummy = BARRIER_DUMMY_VAL;
    portBASE_TYPE err;
    // Send to the cond var iff message queue is not NULL.
    if (NULL != b->msgQueue) 
    {
        err = xQueueSend(b->msgQueue, &(dummy), (portTickType)0);
    } 
    else 
    {
        err = pdFALSE;
    }
    return err;
}
portBASE_TYPE invCntGiveFromISR(INVCNT_SEMAPHORE * b,
                                portBASE_TYPE * pxHigherPriorityTaskWoken) 
{
    unsigned portCHAR dummy = BARRIER_DUMMY_VAL;
    if (NULL != b->msgQueue)
    {
        return xQueueSendFromISR(b->msgQueue, &(dummy), pxHigherPriorityTaskWoken);
    }
    else 
    {
        return pdFALSE;
    }
}
INVCNT_SEMAPHORE cnt;
void start(void * par)
{
    printf("start %dn",invCntCreate(&cnt));        
    printf("start %dn",invCntTake(&cnt,3, 2000));
    vTaskSuspend(NULL);
}
void A(void * par)
{
    printf("A n"); 
    printf("A %dn",invCntGive(&cnt));
    vTaskSuspend(NULL);
}
void B(void * par)
{
    printf("B n"); 
    printf("B %dn",invCntGive(&cnt));
    vTaskSuspend(NULL);
}
void C(void * par)
{
    printf("C n"); 
    printf("C %dn",invCntGive(&cnt));  
    vTaskSuspend(NULL);
}
int main()
{   
    //setupHadware();
    xTaskCreate( start, "bat", 100,NULL, 5, NULL ); 
    xTaskCreate( A, "bat", 100,NULL, 3, NULL ); 
    xTaskCreate( B, "bat", 100,NULL, 2, NULL ); 
    xTaskCreate( C, "bat", 100,NULL, 1, NULL ); 
    /* Start the scheduler, this function should not return as it causes the execution
    context to change from main() to one of the created tasks. */
    vTaskStartScheduler();
    /* Should never get here! */
    return 0;
}