#define BASE                   0x20240100
#define reg_cm4_loader_run     *(volatile int *)(BASE + 0x00)

const char cm4_wfi[] = 
{
    0x00, 0x06, 0x24, 0x20, 0x99, 0x02, 0x24, 0x20, 0x63, 0x02, 0x24, 0x20, 0x63, 0x02, 0x24, 0x20, 
    0x63, 0x02, 0x24, 0x20, 0x63, 0x02, 0x24, 0x20, 0x63, 0x02, 0x24, 0x20, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x63, 0x02, 0x24, 0x20, 
    0x63, 0x02, 0x24, 0x20, 0x00, 0x00, 0x00, 0x00, 0x63, 0x02, 0x24, 0x20, 0x63, 0x02, 0x24, 0x20, 
    0x00, 0xF0, 0x0D, 0xF8, 0x00, 0x28, 0x01, 0xD0, 0xAF, 0xF3, 0x00, 0x80, 0xAF, 0xF3, 0x00, 0x80, 
    0x00, 0x20, 0xAF, 0xF3, 0x00, 0x80, 0x00, 0xF0, 0x05, 0xF8, 0x00, 0xF0, 0x0B, 0xF8, 0x01, 0x20, 
    0x70, 0x47, 0xFE, 0xE7, 0x01, 0x20, 0x02, 0x49, 0x08, 0x60, 0x30, 0xBF, 0xFD, 0xE7, 0x00, 0xBF, 
    0x00, 0x01, 0x24, 0x20, 0x00, 0xF0, 0x00, 0xB8, 0x07, 0x46, 0x38, 0x46, 0x00, 0xF0, 0x02, 0xF8, 
    0xFB, 0xE7, 0x00, 0x00, 0x80, 0xB5, 0xAF, 0xF3, 0x00, 0x80, 0x02, 0x4A, 0x11, 0x00, 0x18, 0x20, 
    0xAB, 0xBE, 0xFB, 0xE7, 0x26, 0x00, 0x02, 0x00, 0xAF, 0xF3, 0x00, 0x80, 0xAF, 0xF3, 0x00, 0x80, 
    0xFF, 0xF7, 0xCE, 0xFF, 
};

/*	This is the source code of cm4_wfi[], the main function is that 
	trigger GPC1 low power sequence to let itself entering SUSPEND mode and send the STBY request.*/

void main(void)
{
    reg_cm4_loader_run = 1;
  
    while(1)
    {
        asm (" wfi");
    }
}


static void _delay(int n)
{
    for(int i=0; i<n; i++)
       ;
}

static void launch_cm4(void)
{
    reg_cm4_loader_run = 0;
    
    // copy cm4 image
    memcpy((void *)0x20240200, cm4_wfi, sizeof(cm4_wfi));

    //CM4 Vector table offset value lower bits out of reset
    IOMUXC_LPSR_GPR->GPR0 = 0x0200;	
    //CM4 Vector table offset value higher bits out of reset
    IOMUXC_LPSR_GPR->GPR1 = 0x2024;	

     __DSB();
     __ISB();
    
    // Release CM4 by SRC
    SRC->SCR |= SRC_SCR_BT_RELEASE_M4_MASK;
    _delay(1000);

    // if CM4 launch successfully, reg_cm4_loader_run will be set to be 1.
    while(reg_cm4_loader_run == 0)
        ;
    
    _delay(1000);
}

void Powerdown_CM4(void)	//Power Down API
{
    //CM4 Controlled by CPU power mode of the domain
    PGMC_CPC1->CPC_CORE_MODE = PGMC_CPC_CPC_CORE_MODE_CTRL_MODE(1);

    // mask all of GPC CMC1 so that there is no interrupt can wake it up.
    GPC_CPU_MODE_CTRL_1->CM_IRQ_WAKEUP_MASK[0] = 0xffffffff;
    GPC_CPU_MODE_CTRL_1->CM_IRQ_WAKEUP_MASK[1] = 0xffffffff;
    GPC_CPU_MODE_CTRL_1->CM_IRQ_WAKEUP_MASK[2] = 0xffffffff;
    GPC_CPU_MODE_CTRL_1->CM_IRQ_WAKEUP_MASK[3] = 0xffffffff;
    GPC_CPU_MODE_CTRL_1->CM_IRQ_WAKEUP_MASK[4] = 0xffffffff;
    GPC_CPU_MODE_CTRL_1->CM_IRQ_WAKEUP_MASK[5] = 0xffffffff;
    GPC_CPU_MODE_CTRL_1->CM_IRQ_WAKEUP_MASK[6] = 0xffffffff;
    GPC_CPU_MODE_CTRL_1->CM_IRQ_WAKEUP_MASK[7] = 0xffffffff;
    
    //Mask non IRQ in GPC1
    GPC_CPU_MODE_CTRL_1->CM_NON_IRQ_WAKEUP_MASK = 0x3;

    //GPC1 Send STBY request on Suspend mode
    GPC_CPU_MODE_CTRL_1->CM_STBY_CTRL |= GPC_CPU_MODE_CTRL_CM_STBY_CTRL_STBY_SUSPEND_MASK;

    //GPC1 request suspend, 0x3 means suspend mode
    GPC_CPU_MODE_CTRL_1->CM_MODE_CTRL = GPC_CPU_MODE_CTRL_CM_MODE_CTRL_CPU_MODE_TARGET(0x3);    

   	//Release CM and trigger WFI instruction
	launch_cm4();

    // Lock CM4 power controller
    GPC_CPU_MODE_CTRL_1->CM_AUTHEN_CTRL = GPC_CPU_MODE_CTRL_CM_AUTHEN_CTRL_LOCK_SETTING_MASK |
                                          GPC_CPU_MODE_CTRL_CM_AUTHEN_CTRL_LOCK_LIST_MASK    |
                                          GPC_CPU_MODE_CTRL_CM_AUTHEN_CTRL_WHITE_LIST(0)     |
                                          GPC_CPU_MODE_CTRL_CM_AUTHEN_CTRL_LOCK_CFG_MASK;

    //Lock CM4 clock as gated status, no CPU can change this setting through authentication
    CCM->LPCG[1].DIRECT &= ~CCM_LPCG_DIRECT_ON_MASK;
    CCM->LPCG[1].AUTHEN  = CCM_LPCG_AUTHEN_LOCK_MODE_MASK |
                           CCM_LPCG_AUTHEN_LOCK_LIST_MASK |
                           CCM_LPCG_AUTHEN_WHITE_LIST(0)  |
                           CCM_LPCG_AUTHEN_LOCK_TZ_MASK;
    
    //Lock CM4 at power off status, no CPU can change this setting through authentication
    PGMC_CPC1->CPC_AUTHEN_CTRL = PGMC_CPC_CPC_AUTHEN_CTRL_LOCK_SETTING_MASK |
                                 PGMC_CPC_CPC_AUTHEN_CTRL_LOCK_LIST_MASK    |
                                 PGMC_CPC_CPC_AUTHEN_CTRL_WHITE_LIST(0)     |
                                 PGMC_CPC_CPC_AUTHEN_CTRL_LOCK_CFG_MASK;
}