#include "core_ca.h"
#include "irq_ctrl.h"
#include "debug_log.h"
#include "v2p_uart.h"
#include "v2p_core.h"
#include "qemu_virio.h"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include <math.h>
#include <string.h>
#include "portAtom.h"
#include "portmacro.h"

#define LOG_SIZE  4096

typedef struct 
{
    uint32_t  mod_op;
    uint32_t  time;
    uint16_t  tickCnt;
    uint8_t  maskStat;
    uint8_t  runPrio;
    uint32_t  rsv;
}logInfo_t;

typedef struct 
{
    uint32_t  widx;
    logInfo_t  log[LOG_SIZE];
}offLineLog_t;


unsigned int    smp_start_flag = 0;
volatile uint32_t   spinTestVar =0;

offLineLog_t   g_offLogS;

void vGicPrint(char *tag);

static inline void delay(uint32_t loops)
{
	__asm__ volatile ("1:\n"
		"subs %0, %1, #1\n"
		"bne 1b" : "=r" (loops) : "0" (loops));
}

void task1(void *p_arg)
{ 
    int time = 0;
    for(;;)
    {
        vGicPrint("task1 in");
        debug_logdebug(LOG_SYS_INFO,"task1 0x%x,  core[%d]\n",time++, portGET_CORE_ID());
        
        vTaskDelay(pdMS_TO_TICKS(20));
        vGicPrint("task1 out");
    }
}

void task2(void *p_arg)
{ 
    int time = 0;
    for(;;)
    {
        vGicPrint("task2 in");
        debug_logdebug(LOG_SYS_INFO,"task2 0x%x,  core[%d]\n",time++, portGET_CORE_ID());
         
        vTaskDelay(pdMS_TO_TICKS(40));
        vGicPrint("task2 out");
    }
}

static void vTaskCreate (void *p_arg)
{ 
    xTaskCreate(task1,"task1",256,NULL,5,NULL);
    xTaskCreate(task2,"task2",256,NULL,4,NULL);

    vTaskDelete(NULL);
}

void  vSpinLockTest()
{
    portGET_TASK_LOCK();

    portGET_ISR_LOCK();

    ArchAtomicInc(&spinTestVar);
    ArchAtomicInc(&spinTestVar);    

    ArchAtomicDec(&spinTestVar);     
    ArchAtomicOr(&spinTestVar, 0x4);        

    ArchAtomicAnd(&spinTestVar, 0x3);        
    ArchAtomicSet(&spinTestVar,0x0);
    portRELEASE_ISR_LOCK();   


    portRELEASE_TASK_LOCK();   
}

int main()
{
    uint32_t mpid = __get_MPIDR()&0xFFF;
    pl01x_init(V2P_CA9_MP_UART0_BASE,115200);
    debug_logdebug(LOG_SYS_INFO,"this core MPIDR 0x%x\n",mpid);
    for(int i=0;i<1000;i++) delay(1000);
    smp_start_flag = mpid+1;
    L1C_CleanDCacheAll();
    asm volatile("sev");
    for(int i=0;i<500;i++) delay(10000);

    LogInit();

    vSpinLockTest();

    vGicPrint("main1");
    IRQ_SetPriorityGroupBits(7);
    vGicPrint("main2");

    xTaskCreate(vTaskCreate,"task creat",256,NULL,4,NULL);

    vTaskStartScheduler();
}

void SMPLowLiveInit(void)
{
    uint32_t mpid = __get_MPIDR()&0xFFF;
    if(mpid == 0x0)
    {
        return;
    }
    else
    {
        asm volatile("wfe");

        while (1)
        {
            if(smp_start_flag == mpid)
            {
                debug_logdebug(LOG_SYS_INFO,"this core MPIDR 0x%x\n",mpid);
                for(int i=0;i<1000;i++) delay(10000);
                if(mpid+1 != 4)
                    smp_start_flag = mpid+1;
                else
                    smp_start_flag = 0;
                asm volatile("sev");
                break;
            }
            else
            {
                asm volatile("wfe"); 
            }
        }
    }
}

void vConfigureTickInterrupt(void) 
{
    IRQ_SetHandler(PrivTimer_IRQn,FreeRTOS_Tick_Handler);
    uint32_t mode = (~IRQ_MODE_TRIG_Msk)&IRQ_GetMode(PrivTimer_IRQn);
    IRQ_SetMode(PrivTimer_IRQn, mode|IRQ_MODE_TRIG_EDGE_RISING);
    IRQ_Enable(PrivTimer_IRQn);
    IRQ_SetPriority(PrivTimer_IRQn, 20*8);
    PTIM_SetCurrentValue(0);
    //TODO:待确认时钟是否配置正确
    PTIM_SetLoadValue(configPERIPHERAL_CLOCK_HZ/configTICK_RATE_HZ);
    PTIM_SetControl((2<<8)|(1<<2)|(1<<1)|(1<<0));

    GBTIM_SetControl(1);
}

void vClearTickInterrupt(void) 
{ 
    PTIM_ClearEventFlag(); 
}

void vApplicationFPUSafeIRQHandler( uint32_t ulICCIAR )
{
    uint32_t ulInterruptID;
    IRQHandler_t InterruptHandler = NULL;

	/* Re-enable interrupts. */
	__asm ( "cpsie i" );

	/* The ID of the interrupt is obtained by bitwise anding the ICCIAR value
	with 0x3FF. */
	ulInterruptID = ulICCIAR & 0x3FFUL;
    InterruptHandler = IRQ_GetHandler((IRQn_ID_t)ulInterruptID);
    if(InterruptHandler != NULL)
    {
        InterruptHandler();
    }
}

uint32_t  g_idleCnt = 0;
void vApplicationLoadHook()
{
      
	volatile uint32_t g_priMsk  = portICCPMR_PRIORITY_MASK_REGISTER&0xff;
	volatile uint32_t g_runPrio=  portICCRPR_RUNNING_PRIORITY_REGISTER & 0xff;

    while(1)
    {
        g_idleCnt++;
        g_priMsk  = portICCPMR_PRIORITY_MASK_REGISTER&0xff;
        g_runPrio=  portICCRPR_RUNNING_PRIORITY_REGISTER & 0xff;
    }
}

uint32_t g_tickCnt = 0;
void vApplicationTickHook()
{
    uint32_t priMsk  = portICCPMR_PRIORITY_MASK_REGISTER&0xff;
    uint32_t runPrio=  portICCRPR_RUNNING_PRIORITY_REGISTER & 0xff;
    uint32_t binary = portICCBPR_BINARY_POINT_REGISTER &0xff;
    if((g_tickCnt  & 0x7f) == 0x0)
    {
        debug_logdebug(LOG_SYS_INFO,"[%08x]Tick cnt = 0x%x  , running prio=%d, priMsk=%d, binary=%d\n",
             GBTIM_GetCurrentValue(), g_tickCnt, runPrio, priMsk, binary);
    }
    g_tickCnt++;
    g_idleCnt =0;

}

void vGicPrint(char *tag)
{
    uint32_t priMsk  = portICCPMR_PRIORITY_MASK_REGISTER&0xff;
    uint32_t runPrio=  portICCRPR_RUNNING_PRIORITY_REGISTER & 0xff;
    uint32_t binary = portICCBPR_BINARY_POINT_REGISTER &0xff;
    debug_logdebug(LOG_SYS_INFO,"%s, GIC: running prio=%d, priMsk=%d, binary=%d\n",
             tag, runPrio, priMsk, binary);
}


void assert(char *str, int32_t cond)
{
    if(!cond)
    {   
        portDISABLE_INTERRUPTS();
        debug_logdebug(LOG_SYS_INFO,"Assert ID =0x%s\n",  str);
        while(1);
     
    }
}


void LogInit()
{
    g_offLogS.widx = 0;
}


uint32_t  xTaskGetTickCnt();
void logTrack(uint32_t  modu,  uint32_t op , uint32_t para)
{
    uint32_t widx;
   
    widx =ArchAtomicIncRet(&g_offLogS.widx);
    widx--;
    widx &= (LOG_SIZE-1); 
    {
        g_offLogS.log[widx].mod_op = (modu<<24)|((op&0xff)<<16)|(para&0xffff) ; 
        if(g_offLogS.log[widx].mod_op == 0x05370f90)
        {
            assert("this is wrong", 0);
        }

        g_offLogS.log[widx].time  =  GBTIM_GetCurrentValue();
        g_offLogS.log[widx].tickCnt = (uint16_t)xTaskGetTickCnt();
        g_offLogS.log[widx].runPrio = portICCRPR_RUNNING_PRIORITY_REGISTER & 0xff;
        g_offLogS.log[widx].maskStat = portICCPMR_PRIORITY_MASK_REGISTER&0xff;
    }
}