/*!
    \file    lisoc_rom.c
    \brief   lisoc boot rom

*/

#include "lisoc_test.h"
#include "uart_test.h"
#include "timer_test.h"
#include "wdt_test.h"
#include "i2c_test.h"
#include "spi_test.h"
#include "gpio_test.h"
#include "flash_test.h"
#include "uart.h"
#include "interrupt_test.h"
#include "rcc.h"
#include "rcc_test.h"
#include "spi.h"
#include "timer.h"
#include "qspi.h"
//#include "gpio.h"
#include "ddr_test.h"
#include "sensor.h"
//#include "datapath.h"
#include "i2c.h"

#define CPU_USAGE
#ifdef CFG_RTOS

#define QUEUE_LEN                       16u             /* 队列的长度，最大可包含多少个消息 */
#define QUEUE_SIZE                      sizeof(uint16_t)/* 队列中每个消息大小（字节） */


#ifndef AX_MAIN
QueueHandle_t ADJ_Queue = NULL;
#endif
QueueHandle_t ISP_Queue = NULL;


/* ADJ task handle */
static TaskHandle_t ADJ_Task_Handle = NULL;
/* ISP task handle */
static TaskHandle_t ISP_Task_Handle = NULL;
static TaskHandle_t CPU_Task_Handle = NULL;

static const uint16_t crc_table[16u] =
{
  0x0000u, 0x1021u, 0x2042u, 0x3063u, 0x4084u, 0x50a5u, 0x60c6u, 0x70e7u,
  0x8108u, 0x9129u, 0xa14au, 0xb16bu, 0xc18cu, 0xd1adu, 0xe1ceu, 0xf1efu,
};

static spis_msg_t isp_msg;
#ifndef AX_MAIN
static spis_msg_t adj_msg;
#endif

static BaseType_t AppTaskCreate(void);                  /* 用于Create task */
static void ADJ_Task(void* pvParameters);               /* ADJ_Task task实现 */
static void ISP_Task(void* pvParameters);               /* ISP_Task task实现 */
static void CPU_Task(void* pvParameters);               /* CPU_Task task实现 */

/**************************************************************************
* Description:  calc CCITT CRC 16
* Input:        pMsg, pointer to the message being checked.
* Output:       none
* Return:       1= pdPASS,    0= pdFAIL
**************************************************************************/
static uint16_t calc_crc(uint8_t *data, uint16_t len)
{
  /* https://blog.csdn.net/j824117879/article/details/82019545 */
  uint16_t crc16;
  uint16_t crc_h4;
  uint16_t crc_l12;

  crc16 = 0x0000u;

  while (len) {
    crc_h4 = (crc16 >> 12u);
    crc_l12 = (crc16 << 4u);
    crc16 = crc_l12 ^ crc_table[crc_h4 ^ (*data >> 4u)];
    crc_h4 = (crc16 >> 12u);
    crc_l12 = (crc16 << 4u);
    crc16 = crc_l12 ^ crc_table[crc_h4 ^ (*data & 0x0fu)];

    data ++;
    len --;
  }

  return crc16;

}


/**************************************************************************
* Description:  check SPI slave message CRC
* Input:        pMsg, pointer to the message being checked.
* Output:       none
* Return:       1= pdPASS,    0= pdFAIL
**************************************************************************/
static BaseType_t check_crc(spis_msg_t *pMsg)
{
  /* https://blog.csdn.net/j824117879/article/details/82019545 */
  uint8_t *data;
  uint16_t len;
  uint16_t crc16;

  data = (uint8_t *)pMsg;   /* crc range = len field + cmd field + param field */
  len = 2u * (SPIS_MSG_LEN_SZ + pMsg->len);

  crc16 = calc_crc(data, len);

  if (crc16 == pMsg->data[pMsg->len - SPIS_MSG_CMD_SZ])
  {
    return pdPASS;
  }
  else
  {
    return pdFAIL;
  }

}

static BaseType_t AppTaskCreate(void)
{
  BaseType_t xReturn = pdFAIL;

  /********************************************************************************
  * Create timers
  ********************************************************************************/
  spis_txtmo_tmr = xTimerCreate("TxTmo",
                      1u,               /* tmo period. place-holder for now */
                      pdFALSE,          /* one-shot */
                      NULL,             /* timer ID not used */
                      spis_cb_txtmo );
  if (NULL == spis_txtmo_tmr)
  {
    printf("Create Tx tmo timer failed!\r\n");
    return xReturn;
  }

  /********************************************************************************
  * Create message queues
  ********************************************************************************/
#ifndef AX_MAIN
  /* 创建 ADJ_Queue */
  ADJ_Queue = xQueueCreate((UBaseType_t ) QUEUE_LEN,    /* 消息队列的长度 */
                          (UBaseType_t ) QUEUE_SIZE);   /* 消息的大小 */
  if (NULL == ADJ_Queue)
  {
    printf("Create ADJ_Queue Queue failed!\r\n");
    return xReturn;
  }
#endif

  /* 创建 ISP_Queue */
  ISP_Queue = xQueueCreate((UBaseType_t ) QUEUE_LEN,    /* 消息队列的长度 */
                          (UBaseType_t ) QUEUE_SIZE);   /* 消息的大小 */
  if (NULL == ISP_Queue)
  {
    printf("Create ISP_Queue Queue failed!\r\n");
    return xReturn;
  }

  /********************************************************************************
  * Create tasks
  ********************************************************************************/
  /* Create ADJ_Task task */
  xReturn = xTaskCreate((TaskFunction_t )ADJ_Task,      /*  task入口函数 */
                        (const char*    )"ADJ_Task",    /*  task名字 */
                        (uint16_t       )512,           /*  task栈大小 */
                        (void*          )NULL,          /*  task入口函数参数 */
                        (UBaseType_t    )5,             /*  task的优先级 */
                        (TaskHandle_t*  )&ADJ_Task_Handle); /*  task控制块指针 */
  if(pdPASS != xReturn)
  {
    printf("Create ADJ_Task task failed!\r\n");
    return xReturn;
  }

  /* Create ISP_Task task */
  xReturn = xTaskCreate((TaskFunction_t )ISP_Task,
                        (const char*    )"ISP_Task",
                        (uint16_t       )512,
                        (void*          )NULL,
                        (UBaseType_t    )3,
                        (TaskHandle_t*  )&ISP_Task_Handle);
  if(pdPASS != xReturn)
  {
    printf("Create ISP_Task task failed!\r\n");
    return xReturn;
  }
#ifdef CPU_USAGE
  /* Create cpu task */
  xReturn = xTaskCreate((TaskFunction_t )CPU_Task,
                        (const char*    )"CPU_Task",
                        (uint16_t       )512,
                        (void*          )NULL,
                        (UBaseType_t    )1,             /* set CPU task priority to VERY LOW */
                        (TaskHandle_t*  )&CPU_Task_Handle);
  if(pdPASS != xReturn)
  {
    printf("Create CPU_Task failed!\r\n");
  }
#endif
  return xReturn;
}

/**********************************************************************
*       ADJ_Task task
**********************************************************************/
static void ADJ_Task(void* parameter)
{
#ifdef AX_MAIN
  while (1){}
#else
  BaseType_t xReturn;
  BaseType_t msgOk;
  uint16_t recv_cmd;
  uint16_t resp_msg[4u];
  uint16_t len;
  uint16_t i;
	uint32_t asic_adr;
	uint32_t asic_data;
	uint16_t sens_adr;
	uint8_t sens_data;

  /**************************************************************************
  * Transit to Rx mode. Get ready for new Rx message
  **************************************************************************/
  spis_rx();

  while (1)
  {
    xReturn = xQueueReceive( ADJ_Queue, &recv_cmd, portMAX_DELAY);
    if (pdPASS == xReturn)
    {
      debug("[ADJ_Task]Queue recv cmd: 0x%04x\n", recv_cmd);

      switch (recv_cmd)
      {
      case SPIS_RX_MSG_RCVD:
        {
          len = spis_rxbuf_rd();
          //printf("len:%d\n",len);
          if ((len >= SPIS_MSG_CMD_SZ) && (len <= SPIS_MSG_PAYLOAD_SZ))
          {
            /* Move message from Ring buffer to local buffer */
            adj_msg.len = len;
            adj_msg.cmd = spis_rxbuf_rd();
            for (i = 0u; i < len - SPIS_MSG_CMD_SZ + SPIS_MSG_CRC_SZ; i ++)
            {
              adj_msg.data[i] = spis_rxbuf_rd();
              //printf("adj_msg.data[%d]:%x\n",i,adj_msg.data[i] );
            }
						
            /* Check CRC */
            msgOk = check_crc(&adj_msg);						
            if (msgOk == pdTRUE)
            {
              if ((adj_msg.cmd == (uint16_t)SPIS_REQ_RD_SYS_PAR)
								|| (adj_msg.cmd == (uint16_t)SPIS_REQ_RD_ISP_PAR)
								|| (adj_msg.cmd == (uint16_t)SPIS_REQ_RD_SENS_PAR))
              {
								if(adj_msg.cmd == (uint16_t)SPIS_REQ_RD_ISP_PAR){						// ISP read
									asic_adr = (adj_msg.data[0] << 16) | adj_msg.data[1];
									asic_data = reg32(asic_adr);
									/* Send reply message */
									resp_msg[0u] = adj_msg.cmd + 1u;
									resp_msg[1u] = (asic_data & 0xffff0000) >> 16;
									resp_msg[2u] = (asic_data & 0xffff);
									resp_msg[3u] = calc_crc((uint8_t *) & resp_msg[0u], 6);
									xReturn = spis_tx(&resp_msg[0u], 4u, SPIS_TX_TMO_MS);
									if (xReturn == pdFAIL)
									{
										/* TBD Failsafe processing */
										debug("[ADJ_Task] Error sending tx data (wrong sts or msg too long)\n");
									}
								}
								
								if(adj_msg.cmd == (uint16_t)SPIS_REQ_RD_SENS_PAR){				// SENSOR read
									sens_adr = adj_msg.data[0];
									i2c_reg16_read(LISOC_I2C0, sens_adr, &sens_data);
									resp_msg[0u] = adj_msg.cmd + 1u;
									resp_msg[1u] = sens_data;
									resp_msg[2u] = calc_crc((uint8_t *) & resp_msg[0u], 4);
									xReturn = spis_tx(&resp_msg[0u], 3u, SPIS_TX_TMO_MS);
									if (xReturn == pdFAIL)
									{
										/* TBD Failsafe processing */
										debug("[ADJ_Task] Error sending tx data (wrong sts or msg too long)\n");
									}
								}
							}

              if ((adj_msg.cmd == (uint16_t)SPIS_REQ_WR_SYS_PAR)
                || (adj_msg.cmd == (uint16_t)SPIS_REQ_WR_ISP_PAR)
                || (adj_msg.cmd == (uint16_t)SPIS_REQ_WR_RISr_PAR)
								|| (adj_msg.cmd == (uint16_t)SPIS_REQ_WR_SENS_PAR))
              {
								/* TBD */

                
								if(adj_msg.cmd == (uint16_t)SPIS_REQ_WR_ISP_PAR){					// ISP write
									asic_adr = (adj_msg.data[0] << 16) | adj_msg.data[1];
									asic_data = (adj_msg.data[2] << 16) | adj_msg.data[3];
									reg32_wr(asic_adr, asic_data);
								}
								if(adj_msg.cmd == (uint16_t)SPIS_REQ_WR_SENS_PAR){				// SENSOR write
									sens_adr = adj_msg.data[0];
									sens_data = adj_msg.data[1] & 0xff;
									i2c_reg16_write(LISOC_I2C0, sens_adr, sens_data);
								}
														
								if(adj_msg.cmd == (uint16_t)SPIS_REQ_WR_RISr_PAR){					// RISr write
									asic_adr = (adj_msg.data[0] << 16) | adj_msg.data[1];
									for(i=0 ; i<(adj_msg.len-3)/2 ; i++){									
										asic_data = (adj_msg.data[2+2*i] << 16) | adj_msg.data[3+2*i];
										reg32_wr(asic_adr, asic_data);
										asic_adr += 4;
									}
								}
															
								/* Send reply message */
								resp_msg[0u] = adj_msg.cmd + 1u;
								resp_msg[1u] = calc_crc((uint8_t *) & resp_msg[0u], 2);
								xReturn = spis_tx(&resp_msg[0u], 2u, SPIS_TX_TMO_MS);
								if (xReturn == pdFAIL)
								{
									/* TBD Failsafe processing */
									debug("[ADJ_Task] Error sending tx data (wrong sts or msg too long)\n");
								}
							}
							
              /* Send the message to ISP task */
              xReturn = xQueueSend( ISP_Queue,            /* 消息队列的句柄 */
                                    &adj_msg.cmd,         /* 发送的消息内容 */
                                    0 );                  /* 等待时间 0 */
            }
            else
            {
              /* TBD Failsafe processing */
              debug("[ADJ_Task] Error receiving rx data (CRC wrong)\n");
              /* Send reply message */
              resp_msg[0u] = SPIS_RSP_CRC_ERR;
              resp_msg[1u] = calc_crc((uint8_t *) & resp_msg[0u], sizeof(uint16_t));
              xReturn = spis_tx(&resp_msg[0u], 2u, SPIS_TX_TMO_MS);
            }
          }
          else
          {
            debug("[ADJ_Task] unknown Rx fifo error(len=%d)\n", len);
            /* Send reply message */
            resp_msg[0u] = SPIS_RSP_FIFO_ERR;
            resp_msg[1u] = calc_crc((uint8_t *) & resp_msg[0u], sizeof(uint16_t));
            xReturn = spis_tx(&resp_msg[0u], 2u, SPIS_TX_TMO_MS);
          }
        }
        break;

      case SPIS_RX_LEN_ERR:
        {
          /* TBD Failsafe processing */
          debug("[ADJ_Task] Error receiving rx data (len err)\n");
          /* Send reply message */
          resp_msg[0u] = SPIS_RSP_LEN_ERR;
          resp_msg[1u] = calc_crc((uint8_t *) & resp_msg[0u], sizeof(uint16_t));
          xReturn = spis_tx(&resp_msg[0u], 2u, SPIS_TX_TMO_MS);
        }
        break;

      case SPIS_RX_FIFO_OVRFLW:
        {
          /* TBD Failsafe processing */
          debug("[ADJ_Task] Error receiving rx data (fifo full)\n");
          /* Send reply message */
          resp_msg[0u] = SPIS_RSP_FIFO_ERR;
          resp_msg[1u] = calc_crc((uint8_t *) & resp_msg[0u], sizeof(uint16_t));
          xReturn = spis_tx(&resp_msg[0u], 2u, SPIS_TX_TMO_MS);
        }
        break;

      case SPIS_TX_MSG_SENT:
        {
          /* Stop Tx time-out timer */
          xTimerStop(spis_txtmo_tmr, 0);

          /**************************************************************************
          * Switch back to Rx mode. Get ready for new Rx message
          **************************************************************************/
          spis_rx();
        }
        break;

      case SPIS_TX_MSG_TMO:
        {
          if (spis_state == SPIS_ST_TX)
          {
            /* TBD Failsafe processing */
            debug("[ADJ_Task] Error sending tx data (time-out)\n");
            /**************************************************************************
            * Switch back to Rx mode. Get ready for new Rx message
            **************************************************************************/
            spis_rx();
          }
          else
          {
            /* Spurious message. Ignore it and do nothing */
          }
        }
        break;

      default:
        break;
      }
    }
    else
    {
      debug("[ADJ_Task]Queue recv ng: %d\n", xReturn);
    }
  }
#endif
}


/**********************************************************************
*       ISP_Task task
**********************************************************************/
static void ISP_Task(void* parameter)
{
  BaseType_t xReturn = pdTRUE;          /* 定义一个创建信息返回值，默认为 pdTRUE */

  while (1)
  {
    xReturn = xQueueReceive( ISP_Queue, /* 消息队列的句柄 */
                              &isp_msg.cmd, /* 发送的消息内容 */
                              portMAX_DELAY);   /* 等待时间 一直等 */
    if (pdTRUE == xReturn)
    {
      printf("[ISP_Task]Queue recv cmd %x\n",isp_msg.cmd);
    }
    else
    {
      printf("[ISP_Task]Queue recv ng: 0x%lx\n",xReturn);
    }
  }
}

static void CPU_Task(void* parameter)
{
  while (1)
  {
    //printf("cpu_task..\n");
		vTaskDelay(100);                   /* 延时1000个tick */
  }
}
#endif

void lisoc_init()
{
  (void)SysTick_Config(SYS_NUM_TICKS_PER_MSEC);
  uart0_init();
  printf("RTOS.\n");


  rcc_init();
  rcc_switch();


  NVIC_EnableIRQ(WDT_IRQn);   //wdt irq enable
  NVIC_EnableIRQ(ISP_IRQn);
  //printf("bootpin:%d\n",(uint8_t)(SYSC_BOOT_STATR & 0x3U));
  
  CPU_TIM_Init();
  spi_slave_init();
  spis_isr_init();
  ddr_init();
  //ddr_test();

  datapath_init();
#ifdef IMX412
	imx412_init();
#elif IMX415 
	imx415_init();
#else
	imx662_init();
#endif
  //ov_init();



  while (0) {
    printf("REG STATUS:\n");
    printf("LISOC_MIPI_RX->reg_0xd0c(ERR_DET_SC):\t\t%x.\r\n",LISOC_MIPI_RX->reg_0xD0C_W);
    printf("LISOC_MIPI_RX->reg_0xd4c(ERR_DET_SC_AL):\t%x.\r\n",LISOC_MIPI_RX->reg_0xD4C_W);
    printf("LISOC_MIPI_RX->reg_0x188(ECC8|WC16|DATA_ID8):\t%x.\r\n",LISOC_MIPI_RX->reg_0x188_W);
    printf("LISOC_MIPI_RX->reg_0x18c(PKT_COUNT16|LEN16):\t%x.\r\n",LISOC_MIPI_RX->reg_0x18C_W);
    printf("LISOC_MIPI_RX->reg_0x190(ERR_CS16|ERR_ECC16):\t%x.\r\n",LISOC_MIPI_RX->reg_0x190_W);
    printf("LISOC_MIPI_RX->reg_0x184(STATE3):\t\t%x.\r\n",LISOC_MIPI_RX->reg_0x184_W);
    printf("LISOC_MIPI_TX->reg_0x04c(PHYD5|LINK4|APP4):\t%x.\r\n",LISOC_MIPI_TX->reg_0x04C_W);
    //printf("LISOC_MIPI_TX->reg_0x030():\t%x.\r\n",LISOC_MIPI_TX->reg_0x030_W);
    printf("LISOC_LISR->reg_0xEC(FIFO_MON):\t\t\t%d.\r\n",LISOC_LISR->reg_0xEC_W);
    printf("LISOC_LISR->reg_0x100(VC):\t\t\t%d.\r\n",LISOC_LISR->reg_0x100_W);
    printf("LISOC_LISR->reg_0x104(HC):\t\t\t%d.\r\n",LISOC_LISR->reg_0x104_W);
    printf("LISOC_LISR->reg_0x108(VWID):\t\t\t%d.\r\n",LISOC_LISR->reg_0x108_W);
    printf("LISOC_LISR->reg_0x10c(HWID):\t\t\t%d.\r\n",LISOC_LISR->reg_0x10C_W);
    printf("LISOC_LISR->reg_0x110(VBP):\t\t\t%d.\r\n",LISOC_LISR->reg_0x110_W);
    printf("LISOC_LISR->reg_0x114(HBP):\t\t\t%d.\r\n",LISOC_LISR->reg_0x114_W);
    printf("LISOC_CCHG->reg_0x0c.W_OUT:\t\t\t%d.\r\n",LISOC_CCHG->reg_0x0c.W_OUT);

    //wc reg
    LISOC_CCHG->reg_0x0c.W_OUT = 1;
    LISOC_LISR->reg_0xEC.FLG_CLR = 1;
    printf("\n");

    delay_ms(1000);
  }

}

#ifdef AX_MAIN
int main__ (void)
#else
int main (void)
#endif
{
#ifdef CFG_RTOS
  BaseType_t xReturn;                   /* 定义一个Create 信息返回值，默认为pdPASS */
  lisoc_init();

  printf("lisoc init.\n");

  /* Create application tasks */
  xReturn = AppTaskCreate();
  printf("AppTaskCreate.\n");
  
  if(pdPASS == xReturn)
  {
    vTaskStartScheduler();              /* 启动 task，开启调度 */
  }
  else
  {
    printf("Create tasks failed!\r\n");
  }

#endif

  while(1)
  {

  }
}
