#include "ucps2-intrin.h"
#include "hwque.h"

void hwq_debug(void * addr , uint32_t v);
#define hwq_do_write(a,v)	__ucps2_store_ext_mem((void *)(a), (uint32_t)v, f_W)
#define hwq_do_read(ptr)    __ucps2_load_ext_mem_v((char*)(ptr), f_W)

#define CPSM_QUE_BASE            0x04d50000
#define JSSM_QUE_BASE            0x05f70000

#define CPSM_MEM_BASE  0x09300000ul  

#define JS_MEM_BASE    0x04d80000ul

#define dire_in   0
#define dire_out  1

int que_map_depth_array[16] = {

    0x04d51014,0x04d51018,0x04d5101c,0x04d51020,
    0x04d51024,0x04d51028,0x04d5102c,0x04d51030,

    0x05f71014,0x05f71018,0x05f7101c,0x05f71020,
    0x05f71024,0x05f71028,0x05f7102c,0x05f71030

};

int que_map_start_addr_array[16] ={
    
    0x09300000,0x09303F00,0x09307E00,0x0930BD00,0x0930FC00,0x09313B00,0x09317A00,0x931B900,

    0x04d80000,0x04d83F00,0x04d87E00,0x04D8BD00,0x04D8FC00,0x04D93B00,0x04D97A00,0x04D9B900
};

//#define HWQUE_HEADER_ADDR (0x931fc00 - 0x400)

extern int int_default_val;
hwq_debug_t *q_debug_info;
int q_debug_enable = 0;
uint32_t q_debug_addr = 0;


unsigned int out_failed_cnt[QUE_MAX_NR] = {0};
unsigned int in_failed_cnt[QUE_MAX_NR] = {0};
unsigned int out_success_cnt[QUE_MAX_NR] = {0};
unsigned int in_success_cnt[QUE_MAX_NR] = {0};
//unsigned int cur_que_depth[QUE_MAX_NR] = {0};
unsigned int bind_cpuid;
int g_coreid = 0;


const uint32_t com_header_addr = (0x931fc00 -0x400);

mpu_shm_obj *shm_head = NULL;

void hw_que_panic(int queid,int addr,int mode)
{

    if(que_map_depth_array[queid] != addr)
    {
         if(mode == dire_in)
         {
            hwq_debug(&(q_debug_info->per_hwq[queid].in_que_depth_addr), addr);
         }
         else
         {
            hwq_debug(&(q_debug_info->per_hwq[queid].out_que_depth_addr), addr);
         }
        __ucps2_IntEn(f_Disable);
        while(1);

    }

}

int smart_hq_init(int cpuid)
{
    shm_head = (mpu_shm_obj*)com_header_addr;

    if((cpuid > 12)||(cpuid < 0))
    {
        return -1;
    }
    g_coreid = cpuid;


    return 0;	
}

/************************************

*************************************/

int smart_get_hq_vector(int cpuid,int queid)
{
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);

    if((cpuid >= 0)&&(cpuid < 12))  
    {
        return que_ptr->ape_vector_id;
    }
    else if(cpuid == 12) 
    { 
        return que_ptr->arm_vector_id;
    }
    return -1;
}

/***************
    函数名称：smart_intr_in_que
    函数参数：queid 队列编号
    函数参数：val 入队值
    函数功能：硬件队列入队

    函数使用注意1 : 当前函数只能在中断处理函数中调用
    函数使用注意2 ：当前多生产使用时需要外部加锁

******************/
int smart_intr_in_que(int queid,uint32_t val)
{
    uint32_t tp_val;
    int ret;
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);
    hw_que_panic(queid,que_ptr->hw_depth_reg,dire_in);
    tp_val = hwq_do_read(que_ptr->hw_depth_reg);
    if(((tp_val >> 16)&0xffff) > B_128_16BLCOKS)
    {
        hwq_do_write(que_ptr->hw_start_addr,val);
        ++in_success_cnt[queid];
        ret = 0;
        hwq_debug(&(q_debug_info->per_hwq[queid].in_success_cnt), in_success_cnt[queid]);
    }
    else
    {
        ++in_failed_cnt[queid];
        ret = -1;
        hwq_debug(&(q_debug_info->per_hwq[queid].in_failed_cnt), in_failed_cnt[queid]);
    }

    return ret;
}

/***************
    函数名称：smart_in_que
    函数参数：queid 队列编号
    函数参数：val 入队值
    函数功能：硬件队列入队
    函数使用注意 :当前函数不能在中断处理函数中调用
    函数使用注意 :多核并发使用时需要外部加锁保护
******************/

int smart_in_que(int queid,uint32_t val)
{
    int ret = 0;
    int int_state = __ucps2_readIntEn();
    if(int_state == 1)
    {
        __ucps2_IntEn(f_Disable);
        ret = smart_intr_in_que(queid,val);
        __ucps2_IntEn(f_Enable);
    }
    else
    {
        ret = smart_intr_in_que(queid,val);
    }
    return ret;
}


/*********************************
函数名称：smart_intr_out_que
函数参数：queid 队列编号
函数参数：出队val 值
函数功能：硬件队列出队
函数使用注意          当前函数只能在中断处理函数中调用

********************************/
int smart_intr_out_que(int queid,uint32_t *val)
{
    uint32_t tp_val;
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);
    if(hwq_do_read(&que_ptr->cpuid) != g_coreid)
    {
        return -EMATCH;
    }
    hw_que_panic(queid,que_ptr->hw_depth_reg,dire_out);
    tp_val = hwq_do_read(que_ptr->hw_depth_reg);
    if((tp_val&0xffff) != 0)
    {
        ++out_success_cnt[queid];
        hwq_debug(&(q_debug_info->per_hwq[queid].out_success_cnt), out_success_cnt[queid]);
        *val = hwq_do_read(que_ptr->hw_start_addr);
        return 0;
    }
    ++out_failed_cnt[queid];
    hwq_debug(&(q_debug_info->per_hwq[queid].out_failed_cnt), out_failed_cnt[queid]);
    
    return -EQUEEMPTY;
}

/**********************
函数名称：smart_out_que
函数参数：queid 队列编号
输出参数：val
函数功能：队列出队获取当前出队值
函数使用注意  : 当前函数不能在中断处理函数中使用

*********************/

int smart_out_que(int queid,uint32_t *val)
{
    int ret = 0;
    int int_state = __ucps2_readIntEn();
    if(int_state == 1)
    {
        __ucps2_IntEn(f_Disable);
        ret = smart_intr_out_que(queid,val);
        __ucps2_IntEn(f_Enable);
    }
    else
    {
        ret = smart_intr_out_que(queid,val);
    }   
    return ret;
}


/*********************
函数名称：smart_que_is_full
函数参数：queid 队列编号
函数功能：判断队列是否满

*********************/

int smart_que_is_full(int queid)
{
    uint32_t tp_val;
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);
    tp_val = hwq_do_read(que_ptr->hw_depth_reg);
    if(((tp_val >> 16)&0xffff) == B_128_16BLCOKS)
    {
        return 1;   // que is full
    }
    
    return 0;  // que is not full	
}


int smart_que_is_empty(int queid)
{
    uint32_t tp_val;
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);
    tp_val = hwq_do_read(que_ptr->hw_depth_reg);
    if((tp_val&0xffff) == 0)
    {
        return 1;   // que is empty  true
    }
    else
    {
        return 0; // que is not empty FALSE
    }
}


int smart_get_que_info(int queid,int *dep,int *width,int *used,int *cpuid)
{
    uint32_t tmp;
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);
    if(queid >= 0 && queid < 8)
    {
        *dep = shm_head->shm_type_s[CPSM_SM].que_depths - B_128_16BLCOKS;
        *width = shm_head->shm_type_s[CPSM_SM].width_bits;
    }else if(queid >=8 && queid < 16)
    {
        *dep = shm_head->shm_type_s[JSSM_SM].que_depths - B_128_16BLCOKS;
        *width = shm_head->shm_type_s[JSSM_SM].width_bits;
    }
    *cpuid = que_ptr->cpuid;
    tmp = *(volatile uint32_t*)(que_ptr->hw_depth_reg);
    *used = (tmp&0xffff);

    return 0;
}

static int test_bit(void *s,unsigned int index)
{
    unsigned char *f_bit = s;
    f_bit += index >>3;
    return ((*f_bit&(1 <<((index%8)))) != 0);
}


int smart_enable_q_nempty(int queid) 
{
    volatile uint32_t *empty_reg;
    int tmp_val = 0;
    int ret = 0;
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);
    empty_reg = (volatile uint32_t*)(que_ptr->hw_base_reg+0x103c);
    if(queid < 8)
    {
        *empty_reg |= 1<< (queid+8);
        tmp_val = *empty_reg;
        __ucps2_synch(0);
        ret = !(test_bit(&tmp_val,(queid+8)) != 0);
    }
    else if((queid >= 8)&&(queid < 16))
    {
        *empty_reg |= 1<< ((queid -8)+8);
         tmp_val = *empty_reg;
         __ucps2_synch(0);
        ret = !(test_bit(&tmp_val,((queid -8)+8)) != 0);
    }
    else if((queid >= 16)&&(queid < 24))
    {
        *empty_reg |= 1<< ((queid -16)+8);
         tmp_val = *empty_reg;
         __ucps2_synch(0);
        ret = !(test_bit(&tmp_val,((queid -16)+8)) != 0);
    }
    else if((queid >= 24)&&(queid < 32))
    {
        *empty_reg |= 1<< ((queid - 24)+8);
        tmp_val = *empty_reg;
        __ucps2_synch(0);
        ret = !(test_bit(&tmp_val,((queid -24)+8)) != 0);
    }
    return ret;
}


void smart_disable_q_nempty(int queid)  
{
    volatile uint32_t *empty_reg;
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);
    empty_reg = (volatile uint32_t*)(que_ptr->hw_base_reg+0x103c);
    if(queid < 8)
    {
        *empty_reg &= ~(1<< (queid+8));
    }
    else if((queid >= 8)&&(queid < 16))
    {
        *empty_reg &= ~(1<< ((queid -8)+8));
    }else if((queid >= 16)&&(queid < 24))
    {
        *empty_reg &= ~(1<< ((queid -16)+8));
    }else if((queid >= 24)&&(queid < 32))
    {
        *empty_reg &= ~(1<< ((queid -24)+8));
    }
}

int smart_que_bind(int cpuid,int queid)
{
    if((queid >15)||(queid < 0))
    {
        return -EINVAL;
    }
    hw_reg_desc_t * que_ptr = ((hw_reg_desc_t*)shm_head->v_map_queid[queid]);
    if(que_ptr->cpuid == -1)
    {
        que_ptr->cpuid = cpuid;
        return 0;
    }
    else
    {
        return -1;
    }
}

/*
	debug interface
*/
int smart_hq_debug_init(uint32_t debug_base_addr, uint32_t len_bytes)
{
    if(debug_base_addr <= 0x10000000)
    {
        return -1;
    }
    if (len_bytes < 384)
    {
        return -2;
    }
  
    q_debug_enable = 1;
    q_debug_addr = debug_base_addr;
    q_debug_info = (hwq_debug_t *)q_debug_addr;

    return 0;
}


int smart_get_inque_ok_cnt(int queid)
{
    return in_success_cnt[queid];
}

int smart_get_inque_failed_cnt(int queid)
{
    return out_failed_cnt[queid];
}

int smart_get_outque_ok_cnt(int queid)
{
    return out_success_cnt[queid];
}

int smart_get_outque_failed_cnt(int queid)
{
    return out_failed_cnt[queid];
}

void hwq_debug(void * addr , uint32_t v)
{
    if(q_debug_enable == 1)
    {
        hwq_do_write(addr, v);
    }
}

