// +FHDR------------------------------------------------------------
//                 Copyright (c) 2022 SmartLogic.
//                       ALL RIGHTS RESERVED
// -----------------------------------------------------------------
// Filename      : spu_sw_queue.s.c
// Author        : lishuang.xie
// Created On    : 2023-01-11
// Last Modified :
// -----------------------------------------------------------------
// Description:
//
//
// -FHDR------------------------------------------------------------
#include "spu_sw_queue.h"
#include "ucp_utility.h"
#include "ucp_handshake.h"
#include "ucp_printf.h"

#define SPU_APE_RFM_LOG_ADDR        (0xB2E00000)    /* 一共15M */

//#define SPU_APE_RFM_LOG_SIZE        (0x00080000)    /* Log软件队列一个队列的总内存(512K) */
//#define SPU_APE_RFM_LOG_QUE_NUM     (12)
//#define SPU_APE_RFM_LOG_QUEID_MAX   (11)            /* Log软件队列一共12个 */

//#define SPU_APE_RFM_SHELL_ADDR      (0xB3D00000)    /* 一共1M */
//#define SPU_APE_RFM_SHELL_SIZE      (0x00001000)    /* Shell软件队列一个队列的总内存(512*8) */
//#define SPU_APE_RFM_SHELL_QUE_NUM   (24)
//#define SPU_APE_REM_SHELL_QUEID_MAX (23)            /* Shell软件队列一共24个 */

#define SPU_APE_RFM_DEQUE_OFFSET    (0)
#define SPU_APE_RFM_ENQUE_OFFSET    (2)
#define SPU_APE_RFM_MSG_OFFSET      (8)
#define SPU_APE_RFM_MSG_SIZE        (2)

//#define QUEUE_TYPE_FOR_APE          (0)
//#define QUEUE_TYPE_FOR_RFM          (1)
//#define QUEUE_TYPE_FOR_ERROR        (2)

#define SPU_APE_RFM_QUE_NUM         (36)
#define SPU_SW_QUE_SHARE_BASE_ADDR  (0xB36F0000)    //(0xB3DE0000)

#define SPU_APE_RFM_QUE_FLAG_USED   (0x5a5a)
#define SPU_APE_FRM_QUE_FLAG_POS    (2)

spu_sw_queue_mem_t gst_sw_queue_mem[] = {
    // u32addr,  id,  type,         offset,pad,idx, dep,                     mask
    {0xB2E00000, 0,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//0
    {0xB2E80000, 1,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//1
    {0xB2F00000, 2,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//2
    {0xB2F80000, 3,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//3
    {0xB3000000, 4,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//4
    {0xB3080000, 5,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//5
    {0xB3100000, 6,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//6
    {0xB3180000, 7,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//7
    {0xB3200000, 8,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//8
    {0xB3280000, 9,  SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//9
    {0xB3300000, 10, SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//10
    {0xB3380000, 11, SPU_SW_QUEUE_MSG,   8, 0, 0, SPU_APE_RFM_MSG_QUE_DEP,   (SPU_APE_RFM_MSG_QUE_DEP - 1), 0, 0, 0},//11

    //{0xB3D00000, 12,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//12
    {0xB3600000, 12,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//12
    {0xB360A000, 13,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//13
    {0xB3614000, 14,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//14
    {0xB361E000, 15,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//15
    {0xB3628000, 16,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//16
    {0xB3632000, 17,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//17
    {0xB363C000, 18,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//18
    {0xB3646000, 19,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//19
    {0xB3650000, 20,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//20
    {0xB365A000, 21,  SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//21
    {0xB3664000, 22, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//22
    {0xB366E000, 23, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//23
    {0xB3678000, 24, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//24
    {0xB3682000, 25, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//25
    {0xB368C000, 26, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//26
    {0xB3696000, 27, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//27
    {0xB36A0000, 28, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//28
    {0xB36AA000, 29, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//29
    {0xB36B4000, 30, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//30
    {0xB36BE000, 31, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//31
    {0xB36C8000, 32, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//32
    {0xB36D2000, 33, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//33
    {0xB36DC000, 34, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//34
    {0xB36E6000, 35, SPU_SW_QUEUE_SHELL, 8, 0, 0, SPU_APE_RFM_SHELL_QUE_DEP, (SPU_APE_RFM_SHELL_QUE_DEP - 1), 0, 0, 0},//35
};

void* spu_get_sw_queue_share_info(void)
{
    volatile void* ptr = NULL;

    do
    {
        ptr = (void *)do_read_volatile(SPU_SW_QUE_SHARE_BASE_ADDR);
    }
    while (ptr == NULL);
    return (void *)ptr;
}


#if 0
/* APE核存在抢占等，需要互斥锁 */
uint8_t spu_sw_queue_spin_init(void)
{
    int32_t core_id = get_core_id();
    int32_t ret = 0;
    uint8_t u8Loop = 0;

    for(u8Loop = 0; u8Loop < PET_RFM_SPU0_CORE_ID; u8Loop++)
    {
        ret = spin_lock_init((LOCK_SW_APE0+u8Loop));
        if (0 != ret)
        {
            debug_write(DBG_DDR_ERR_IDX(core_id, (40+u8Loop)), ret);
            return ret;
        }
    }
    debug_write(DBG_DDR_COMMON_IDX(core_id, 120), 0x12121212);
    return 0;
}

uint8_t spu_sw_queue_get_type(spu_sw_queue_index_e emqueue_idx)
{
    uint8_t ret = 0;
    switch(emqueue_idx)
    {
        case SW_LOG_APE0:
        case SW_LOG_APE1:
        case SW_LOG_APE2:
        case SW_LOG_APE3:
        case SW_LOG_APE4:
        case SW_LOG_APE5:
        case SW_LOG_APE6:
        case SW_LOG_APE7:
        case SW_SHELL_APE0:
        case SW_SHELL_APE1:
        case SW_SHELL_APE2:
        case SW_SHELL_APE3:
        case SW_SHELL_APE4:
        case SW_SHELL_APE5:
        case SW_SHELL_APE6:
        case SW_SHELL_APE7:
        case SW_SHELL_ECHO_APE0:
        case SW_SHELL_ECHO_APE1:
        case SW_SHELL_ECHO_APE2:
        case SW_SHELL_ECHO_APE3:
        case SW_SHELL_ECHO_APE4:
        case SW_SHELL_ECHO_APE5:
        case SW_SHELL_ECHO_APE6:
        case SW_SHELL_ECHO_APE7:
        {
            ret = QUEUE_TYPE_FOR_APE;
            break;
        }
        case SW_LOG_PET_RFM0:
        case SW_LOG_PET_RFM1:
        case SW_LOG_ECS_RFM0:
        case SW_LOG_ECS_RFM1:
        case SW_SHELL_PET_RFM0:
        case SW_SHELL_PET_RFM1:
        case SW_SHELL_ECS_RFM0:
        case SW_SHELL_ECS_RFM1:
        case SW_SHELL_ECHO_PET_RFM0:
        case SW_SHELL_ECHO_PET_RFM1:
        case SW_SHELL_ECHO_ECS_RFM0:
        case SW_SHELL_ECHO_ECS_RFM1:
        {
            ret = QUEUE_TYPE_FOR_RFM;
            break;
        }
        default:
        {
            ret = QUEUE_TYPE_FOR_ERROR;
            break;
        }
    }
    return ret;
}

int8_t *spu_sw_mem_malloc_by_ape(spu_sw_queue_index_e emqueue_idx)
{
    int32_t  core_id = get_core_id();
    uint32_t u32addr = 0;
    uint8_t  u8spin_id = core_id + LOCK_SW_APE0;

    spin_lock(u8spin_id);
    u32addr = ((gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx << 9) + gst_sw_queue_mem[emqueue_idx].u32addr);
    gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx = (gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx + 1)&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    spin_unlock(u8spin_id);
    return ((int8_t*)(u32addr + (gst_sw_queue_mem[emqueue_idx].u8queue_offset)));
}

int8_t *spu_sw_mem_malloc_by_rfm(spu_sw_queue_index_e emqueue_idx)
{
    uint32_t u32addr = 0;

    u32addr = ((gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx << 9) + gst_sw_queue_mem[emqueue_idx].u32addr);
    gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx = (gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx + 1)&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    return ((int8_t*)(u32addr + (gst_sw_queue_mem[emqueue_idx].u8queue_offset)));
}

uint8_t spu_sw_enque_by_ape(spu_sw_queue_index_e emqueue_idx, int8_t *pi8addr)
{
    int32_t core_id = get_core_id();
    uint8_t u8spin_id = core_id + LOCK_SW_APE0;
    uint16_t u16size = SPU_SW_QUEUE_SHARE_INFO_SIZE;
    uint16_t u16enque_idx = ((uint32_t)(pi8addr) - gst_sw_queue_mem[emqueue_idx].u32addr - gst_sw_queue_mem[emqueue_idx].u8queue_offset)/SPU_APE_RFM_QUE_SIG_SIZE;
    uint16_t u16enqueue_cnt = 0;
    uint16_t u16enqueue_idx = 0;

    uint32_t u32queue_share_addr = (SPU_SW_QUE_SHARE_BASE_ADDR + emqueue_idx*u16size);
    uint32_t u32enque_addr = 0;
    uint32_t u32msg_addr   = 0;

    u32enque_addr = u32queue_share_addr + SPU_APE_RFM_ENQUE_OFFSET;

    spin_lock(u8spin_id);
    u16enqueue_cnt = do_read_short(u32enque_addr);
    u16enqueue_idx = u16enqueue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    u16enqueue_cnt++;
    u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16enqueue_idx;

    do_write_short(u32msg_addr, u16enque_idx);
    do_write_short(u32enque_addr, u16enqueue_cnt);
    spin_unlock(u8spin_id);
    return 0;
}

uint8_t spu_sw_enque_by_rfm(spu_sw_queue_index_e emqueue_idx, int8_t *pi8addr)
{
    //int32_t core_id = get_core_id();
    uint16_t u16size = SPU_SW_QUEUE_SHARE_INFO_SIZE;
    uint16_t u16enque_idx = ((uint32_t)(pi8addr) - gst_sw_queue_mem[emqueue_idx].u32addr - gst_sw_queue_mem[emqueue_idx].u8queue_offset)/SPU_APE_RFM_QUE_SIG_SIZE;
    uint16_t u16enqueue_cnt = 0;
    uint16_t u16enqueue_idx = 0;

    uint32_t u32queue_share_addr = (SPU_SW_QUE_SHARE_BASE_ADDR + emqueue_idx*u16size);
    uint32_t u32enque_addr = 0;
    uint32_t u32msg_addr   = 0;

    u32enque_addr = u32queue_share_addr + SPU_APE_RFM_ENQUE_OFFSET;

    u16enqueue_cnt = do_read_short(u32enque_addr);
    u16enqueue_idx = u16enqueue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    u16enqueue_cnt++;
    u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16enqueue_idx;

    do_write_short(u32msg_addr, u16enque_idx);
    do_write_short(u32enque_addr, u16enqueue_cnt);
    return 0;
}

uint8_t spu_sw_deque_by_ape(spu_sw_queue_index_e emqueue_idx, uint32_t *pu32value)
{
    int32_t core_id = get_core_id();
    uint8_t u8spin_id = core_id + LOCK_SW_APE0;
    uint16_t u16size = SPU_SW_QUEUE_SHARE_INFO_SIZE;
    uint16_t u16dequeue_cnt = 0;
    uint16_t u16dequeue_idx = 0;
    uint16_t u16queue_msg_idx = 0;

    uint32_t u32queue_share_addr = (SPU_SW_QUE_SHARE_BASE_ADDR + emqueue_idx*u16size);
    uint32_t u32deque_addr = 0;
    uint32_t u32msg_addr   = 0;

    u32deque_addr = u32queue_share_addr + SPU_APE_RFM_DEQUE_OFFSET;
    spin_lock(u8spin_id);
    u16dequeue_cnt = do_read_short(u32deque_addr);
    u16dequeue_idx = u16dequeue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    u16dequeue_cnt++;
    u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16dequeue_idx;

    u16queue_msg_idx = do_read_short(u32msg_addr);
    *pu32value = gst_sw_queue_mem[emqueue_idx].u32addr + (u16queue_msg_idx)*SPU_APE_RFM_QUE_SIG_SIZE;
    do_write_short(u32deque_addr, u16dequeue_cnt);
    spin_unlock(u8spin_id);
    return 0;
}

uint8_t spu_sw_deque_by_rfm(spu_sw_queue_index_e emqueue_idx, uint32_t *pu32value)
{
    //int32_t core_id = get_core_id();
    uint16_t u16size = SPU_SW_QUEUE_SHARE_INFO_SIZE;
    uint16_t u16dequeue_cnt = 0;
    uint16_t u16dequeue_idx = 0;
    uint16_t u16queue_msg_idx = 0;

    uint32_t u32queue_share_addr = (SPU_SW_QUE_SHARE_BASE_ADDR + emqueue_idx*u16size);
    uint32_t u32deque_addr = 0;
    uint32_t u32msg_addr   = 0;

    u32deque_addr = u32queue_share_addr + SPU_APE_RFM_DEQUE_OFFSET;
    u16dequeue_cnt = do_read_short(u32deque_addr);
    u16dequeue_idx = u16dequeue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    u16dequeue_cnt++;
    u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16dequeue_idx;

    u16queue_msg_idx = do_read_short(u32msg_addr);
    *pu32value = gst_sw_queue_mem[emqueue_idx].u32addr + (u16queue_msg_idx)*SPU_APE_RFM_QUE_SIG_SIZE;
    do_write_short(u32deque_addr, u16dequeue_cnt);
    return 0;
}

int8_t *spu_sw_msg_malloc(spu_sw_queue_index_e emqueue_idx)
{
    int32_t core_id = get_core_id();
    uint32_t u32addr = 0;
    uint8_t  ret = 0;
    uint8_t  u8spin_id = core_id + LOCK_SW_APE0;

    ret = spu_sw_queue_get_type(emqueue_idx);
    if (QUEUE_TYPE_FOR_APE == ret)
    {
        spin_lock(u8spin_id);
        u32addr = ((gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx << 9) + gst_sw_queue_mem[emqueue_idx].u32addr);
        gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx = (gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx + 1)&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
        debug_write(DBG_DDR_COMMON_IDX(core_id, 120), gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx);
        spin_unlock(u8spin_id);
    }
    else if (QUEUE_TYPE_FOR_RFM == ret)
    {
        u32addr = ((gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx << 9) + gst_sw_queue_mem[emqueue_idx].u32addr);
        gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx = (gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx + 1)&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    }
    else
    {
        return NULL;
    }
    return ((int8_t*)(u32addr + (gst_sw_queue_mem[emqueue_idx].u8queue_offset)));
}

void spu_sw_msg_enque(spu_sw_queue_index_e emqueue_idx, int8_t *pi8addr)
{
    uint32_t u32addr = (uint32_t)pi8addr - SPU_APE_FRM_QUE_FLAG_POS;
    do_write_short(u32addr, SPU_APE_RFM_QUE_FLAG_USED);
    return ;
}

uint8_t spu_sw_msg_deque(spu_sw_queue_index_e emqueue_idx, uint32_t *pu32value)
{
    return 0;
}

/* malloc */
int8_t *spu_sw_mem_malloc(spu_sw_queue_index_e emqueue_idx)
{
    int32_t core_id = get_core_id();
    uint32_t u32addr = 0;
    uint8_t  ret = 0;
    uint8_t  u8spin_id = core_id + LOCK_SW_APE0;

    ret = spu_sw_queue_get_type(emqueue_idx);
    if (QUEUE_TYPE_FOR_APE == ret)
    {
        spin_lock(u8spin_id);
        u32addr = ((gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx << 9) + gst_sw_queue_mem[emqueue_idx].u32addr);
        gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx = (gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx + 1)&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
        debug_write(DBG_DDR_COMMON_IDX(core_id, 120), gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx);
        spin_unlock(u8spin_id);
    }
    else if (QUEUE_TYPE_FOR_RFM == ret)
    {
        u32addr = ((gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx << 9) + gst_sw_queue_mem[emqueue_idx].u32addr);
        gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx = (gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx + 1)&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    }
    else
    {
        return NULL;
    }
    return ((int8_t*)(u32addr + (gst_sw_queue_mem[emqueue_idx].u8queue_offset)));
}

/****************************************************************/
uint8_t spu_sw_enque(spu_sw_queue_index_e emqueue_idx, int8_t *pi8addr)
{
    int32_t core_id = get_core_id();
    uint8_t ret = 0;
    uint8_t u8spin_id = core_id + LOCK_SW_APE0;
    uint16_t u16size = SPU_SW_QUEUE_SHARE_INFO_SIZE;
    uint16_t u16enque_idx = ((uint32_t)(pi8addr) - gst_sw_queue_mem[emqueue_idx].u32addr - gst_sw_queue_mem[emqueue_idx].u8queue_offset)/SPU_APE_RFM_QUE_SIG_SIZE;
    uint16_t u16enqueue_cnt = 0;
    uint16_t u16enqueue_idx = 0;

    uint32_t u32queue_share_addr = (SPU_SW_QUE_SHARE_BASE_ADDR + emqueue_idx*u16size);
    uint32_t u32enque_addr = 0;
    uint32_t u32msg_addr   = 0;
    debug_write(DBG_DDR_COMMON_IDX(core_id, 121), u16size);
    debug_write(DBG_DDR_COMMON_IDX(core_id, 122), emqueue_idx);
    debug_write(DBG_DDR_COMMON_IDX(core_id, 123), u32queue_share_addr);

    ret = spu_sw_queue_get_type(emqueue_idx);
    if (QUEUE_TYPE_FOR_APE == ret)
    {
        u32enque_addr = u32queue_share_addr + SPU_APE_RFM_ENQUE_OFFSET;

        spin_lock(u8spin_id);
        u16enqueue_cnt = do_read_short(u32enque_addr);
        u16enqueue_idx = u16enqueue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
        u16enqueue_cnt++;
        u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16enqueue_idx;

        do_write_short(u32msg_addr, u16enque_idx);
        debug_write(DBG_DDR_COMMON_IDX(core_id, 124), u16enque_idx);
        //debug_write(DBG_DDR_COMMON_IDX(core_id, 125), u16enqueue_cnt);
        debug_write(DBG_DDR_COMMON_IDX(core_id, 126), u32msg_addr);
        debug_write(DBG_DDR_COMMON_IDX(core_id, 127), u32enque_addr);
        do_write_short(u32enque_addr, u16enqueue_cnt);
        spin_unlock(u8spin_id);
    }
    else if (QUEUE_TYPE_FOR_RFM == ret)
    {
        u32enque_addr = u32queue_share_addr + SPU_APE_RFM_ENQUE_OFFSET;
        u16enqueue_cnt = do_read_short(u32enque_addr);
        u16enqueue_idx = u16enqueue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
        u16enqueue_cnt++;
        u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16enqueue_idx;

        do_write_short(u32msg_addr, u16enque_idx);
        do_write_short(u32enque_addr, u16enqueue_cnt);
    }
    else
    {
        return 1;
    }
    return 0;
}

uint8_t spu_sw_deque(spu_sw_queue_index_e emqueue_idx, uint32_t *pu32value)
{
    int32_t core_id = get_core_id();
    uint8_t ret = 0;
    uint8_t u8spin_id = core_id + LOCK_SW_APE0;
    uint16_t u16size = SPU_SW_QUEUE_SHARE_INFO_SIZE;
    uint16_t u16dequeue_cnt = 0;
    uint16_t u16dequeue_idx = 0;
    uint16_t u16queue_msg_idx = 0;

    uint32_t u32queue_share_addr = (SPU_SW_QUE_SHARE_BASE_ADDR + emqueue_idx*u16size);
    uint32_t u32deque_addr = 0;
    uint32_t u32msg_addr   = 0;

    ret = spu_sw_queue_get_type(emqueue_idx);
    if (QUEUE_TYPE_FOR_APE == ret)
    {
        u32deque_addr = u32queue_share_addr + SPU_APE_RFM_DEQUE_OFFSET;

        spin_lock(u8spin_id);
        u16dequeue_cnt = do_read_short(u32deque_addr);
        u16dequeue_idx = u16dequeue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
        u16dequeue_cnt++;
        u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16dequeue_idx;

        u16queue_msg_idx = do_read_short(u32msg_addr);
        *pu32value = gst_sw_queue_mem[emqueue_idx].u32addr + (u16queue_msg_idx)*SPU_APE_RFM_QUE_SIG_SIZE;
        do_write_short(u32deque_addr, u16dequeue_cnt);
        spin_unlock(u8spin_id);
    }
    else if (QUEUE_TYPE_FOR_RFM == ret)
    {
        u32deque_addr = u32queue_share_addr + SPU_APE_RFM_DEQUE_OFFSET;
        u16dequeue_cnt = do_read_short(u32deque_addr);
        u16dequeue_idx = u16dequeue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
        u16dequeue_cnt++;
        u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16dequeue_idx;

        u16queue_msg_idx = do_read_short(u32msg_addr);
        *pu32value = gst_sw_queue_mem[emqueue_idx].u32addr + (u16queue_msg_idx)*SPU_APE_RFM_QUE_SIG_SIZE;
        do_write_short(u32deque_addr, u16dequeue_cnt);
    }
    else
    {
        return 1;
    }
    return 0;
}

#endif


/* malloc */
int8_t *spu_sw_mem_malloc(spu_sw_queue_index_e emqueue_idx)
{
    int32_t core_id = get_core_id();
    uint32_t u32addr = 0;

    if (SW_QUEUE_MAX_INDEX <= emqueue_idx)
    {
        return NULL;
    }

    u32addr = ((gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx << 9) + gst_sw_queue_mem[emqueue_idx].u32addr);
    gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx = (gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx + 1)&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    debug_write(DBG_DDR_COMMON_IDX(core_id, 120), gst_sw_queue_mem[emqueue_idx].u16queue_mem_idx);
    return ((int8_t*)(u32addr + (gst_sw_queue_mem[emqueue_idx].u8queue_offset)));
}

/****************************************************************/
uint8_t spu_sw_enque(spu_sw_queue_index_e emqueue_idx, int8_t *pi8addr)
{
    if (SW_QUEUE_MAX_INDEX <= emqueue_idx)
    {
        return 1;
    }

    int32_t core_id = get_core_id();
    uint16_t u16size = SPU_SW_QUEUE_SHARE_INFO_SIZE;
    uint16_t u16enque_idx = ((uint32_t)(pi8addr) - gst_sw_queue_mem[emqueue_idx].u32addr - gst_sw_queue_mem[emqueue_idx].u8queue_offset)/SPU_APE_RFM_QUE_SIG_SIZE;
    uint16_t u16enqueue_cnt = 0;
    uint16_t u16enqueue_idx = 0;

    uint32_t u32queue_share_addr = (SPU_SW_QUE_SHARE_BASE_ADDR + emqueue_idx*u16size);
    uint32_t u32enque_addr = 0;
    uint32_t u32msg_addr   = 0;
    debug_write(DBG_DDR_COMMON_IDX(core_id, 121), u16size);
    debug_write(DBG_DDR_COMMON_IDX(core_id, 122), emqueue_idx);
    debug_write(DBG_DDR_COMMON_IDX(core_id, 123), u32queue_share_addr);

    u32enque_addr = u32queue_share_addr + SPU_APE_RFM_ENQUE_OFFSET;
    u16enqueue_cnt = do_read_short(u32enque_addr);
    u16enqueue_idx = u16enqueue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    u16enqueue_cnt++;
    u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16enqueue_idx;

    __ucps2_synch(0);
    do_write_short(u32msg_addr, u16enque_idx);
    debug_write(DBG_DDR_COMMON_IDX(core_id, 124), u16enque_idx);
    //debug_write(DBG_DDR_COMMON_IDX(core_id, 125), u16enqueue_cnt);
    debug_write(DBG_DDR_COMMON_IDX(core_id, 126), u32msg_addr);
    debug_write(DBG_DDR_COMMON_IDX(core_id, 127), u32enque_addr);

    __ucps2_synch(0);
    do_write_short(u32enque_addr, u16enqueue_cnt);
    return 0;
}

uint8_t spu_sw_deque(spu_sw_queue_index_e emqueue_idx, uint32_t **pu32value)
{
    if (SW_QUEUE_MAX_INDEX <= emqueue_idx)
    {
        return 1;
    }

    uint16_t u16size = SPU_SW_QUEUE_SHARE_INFO_SIZE;
    uint16_t u16dequeue_cnt = 0;
    uint16_t u16enqueue_cnt = 0;
    uint16_t u16dequeue_idx = 0;
    uint16_t u16queue_msg_idx = 0;

    uint32_t u32queue_share_addr = (SPU_SW_QUE_SHARE_BASE_ADDR + emqueue_idx*u16size);
    uint32_t u32deque_addr = 0;
    uint32_t u32enque_addr = 0;
    uint32_t u32msg_addr   = 0;

    u32deque_addr = u32queue_share_addr + SPU_APE_RFM_DEQUE_OFFSET;
    u32enque_addr = u32deque_addr + sizeof(uint16_t);

    u16dequeue_cnt = do_read_short(u32deque_addr);
    u16enqueue_cnt = do_read_short(u32enque_addr);

    if(u16dequeue_cnt == u16enqueue_cnt)
    {
        return 4;
    }
    u16dequeue_idx = u16dequeue_cnt&(gst_sw_queue_mem[emqueue_idx].u16queue_mask);
    u16dequeue_cnt++;
    u32msg_addr = u32queue_share_addr + SPU_APE_RFM_MSG_OFFSET + SPU_APE_RFM_MSG_SIZE*u16dequeue_idx;

    u16queue_msg_idx = do_read_short(u32msg_addr);
    *pu32value = (uint32_t *)(gst_sw_queue_mem[emqueue_idx].u32addr + (u16queue_msg_idx)*SPU_APE_RFM_QUE_SIG_SIZE);
    __ucps2_synch(0);
    do_write_short(u32deque_addr, u16dequeue_cnt);

    return 0;
}

