/**
 * \file    msq.c
 * \brief   SAL FIFO message queue
 *
 * \par     Copyright (c) 2018 UWE Technologies Corp., Ltd. All Rights Reserved.
 * \author  James
 * \date    2018-03-14
 * \version 
 * 
 * \history 
 *  Date        Note
 *  2018-03-14  Original Creation
 * 
 */

#include "types.h"
#include "msq.h"
#include "string.h"
#include "drivers.h"

#if SAL_MSQ_ENABLED

typedef struct msq_info_sub_s
{
    u32_t msq_head:8;  ///< position that msg will be pushed
    u32_t msq_tail:8;  ///< position that msg will be popped
    u32_t msq_num:8;   ///< message queue number
    u32_t reserved:8;  ///< reserved
} msq_info_sub_t;

struct msq_info_s 
{
    union {
        msq_info_sub_t info;
        u32_t u32;
    } u;
};

static volatile struct msq_info_s msg_info;

/** message queue */
static volatile msq_t msq_array[SAL_MSQ_SIZE];


void msq_init(void)
{
    msg_info.u.u32 = 0;
    memset((char *)msq_array, 0, sizeof(msq_array));
}

void msq_push(msq_t msg)    
{
    u32_t intr_local = DRV_INTR_GLOBAL_GET();
    DRV_INTR_GLOBAL_DISABLE();
    
    if (msg_info.u.info.msq_num >= SAL_MSQ_SIZE)
    {
        DRV_INTR_GLOBAL_SET(intr_local);
        return;
    }
    
    //first put in data, then increase msq_head
    msq_array[msg_info.u.info.msq_head] = msg;    
    msg_info.u.info.msq_head++;
    if (msg_info.u.info.msq_head >= SAL_MSQ_SIZE)
    {
        msg_info.u.info.msq_head = 0;
    }   
    msg_info.u.info.msq_num++;

    DRV_INTR_GLOBAL_SET(intr_local);
}

void msq_push_msg(u32_t msq_type, u32_t bitmap, u32_t val1, u32_t val2, u32_t val3)
{
    u32_t intr_local = DRV_INTR_GLOBAL_GET();
    DRV_INTR_GLOBAL_DISABLE();

    if (msg_info.u.info.msq_num >= SAL_MSQ_SIZE)
    {
    	DRV_INTR_GLOBAL_SET(intr_local);
        return;
    }

    //first put in data, then increase msq_head
    msq_array[msg_info.u.info.msq_head].type = msq_type;
    msq_array[msg_info.u.info.msq_head].bitmap = bitmap;
    msq_array[msg_info.u.info.msq_head].val1 = val1;
    msq_array[msg_info.u.info.msq_head].val2 = val2;
    msq_array[msg_info.u.info.msq_head].val3 = val3;
    
    msg_info.u.info.msq_head++;
    if (msg_info.u.info.msq_head >= SAL_MSQ_SIZE)
    {
        msg_info.u.info.msq_head = 0;
    }
    msg_info.u.info.msq_num++;
    DRV_INTR_GLOBAL_SET(intr_local);
}

/**
 * SAL pop message data from queue
 *
 *
 * \param p_msg pointer to hold data
 *
 * \see
 */
void msq_pop(msq_t *p_msg)
{
    u32_t intr_local = DRV_INTR_GLOBAL_GET();
    DRV_INTR_GLOBAL_DISABLE();

    if (msg_info.u.info.msq_num <= 0)
    {
        *(u32_t *)p_msg = 0x0;
        msg_info.u.info.msq_num = 0;
        DRV_INTR_GLOBAL_SET(intr_local);
        return;
    }

    //first get out data, then increase msq_tail
    *p_msg = msq_array[msg_info.u.info.msq_tail];
    msg_info.u.info.msq_tail++;
    if (msg_info.u.info.msq_tail >= SAL_MSQ_SIZE)
    {
        msg_info.u.info.msq_tail = 0;
    }
    msg_info.u.info.msq_num--;

    DRV_INTR_GLOBAL_SET(intr_local);
}
#endif /* SAL_MSQ_ENABLED */

