/*
 * FreeRTOS Kernel V10.1.1
 * Copyright (C) 2018 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * http://www.FreeRTOS.org
 * http://aws.amazon.com/freertos
 *
 * 1 tab == 4 spaces!
 */


/* Standard includes. */
//#include <stdlib.h>

/* Scheduler includes. */
#include "freertos/include/FreeRTOS.h"
#include "freertos/include/task.h"

/* platform includes. */
#include "drivers/8258/bsp.h"
#include "drivers/8258/compiler.h"
#include "drivers/8258/register_8258.h"
#include "drivers/8258/irq.h"
#include "common/breakpoint.h"
#include "drivers/8258/gpio.h"
#include "drivers/8258/pm.h"
#include "stack/ble/ll/ll_pm.h"

/*_attribute_data_retention_*/
StackType_t	 StackSave;

/* The code generated by the GCC compiler uses the stack in different ways at
different optimisation levels.  The interrupt flags can therefore not always
be saved to the stack.  Instead the critical section nesting level is stored
in a variable, which is then saved as part of the stack context. */

void vPortEnterCritical( void ){
	portDISABLE_INTERRUPTS();
	//DEBUG_GPIO(GPIO_CHN8, 0);

}

void vPortExitCritical( void ){
	//DEBUG_GPIO(GPIO_CHN8, 1);
	portENABLE_INTERRUPTS();
}

/*
 * Initialise the stack of a task to look exactly as if a call to
 * portSAVE_CONTEXT had been called.
 *
 * See header file for description.
 */
StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters )
{
	pxTopOfStack--;
	*pxTopOfStack-- = ((StackType_t)  pxCode) & ~ 0x01; 		/* PC   */
	pxTopOfStack -= 7;											// r1 -- r7
	*pxTopOfStack-- = ( StackType_t ) pvParameters; 			//r0
	pxTopOfStack -= 5;											// r8 -- r12
	*pxTopOfStack-- = ( StackType_t ) 0x13; 					//CPSR
	*pxTopOfStack-- = 1; 										//IRQ status
	*pxTopOfStack = ((StackType_t)  pxCode) & ~ 0x01; 			/* LR   */
	return pxTopOfStack;
	
}


/* Setup the timer to generate the tick interrupts. */
static uint32_t irqTimer0Tick;
#define TICK_HZ					((1000 / configTICK_RATE_HZ) * (CLOCK_SYS_CLOCK_HZ / 1000))
static void prvRestoreTimerInterrupt(void){
	reg_tmr0_capt = TICK_HZ;
	reg_tmr0_tick = irqTimer0Tick; 													//clear counter
	reg_irq_mask |= FLD_IRQ_TMR0_EN;
	reg_tmr_ctrl |= FLD_TMR0_EN;  										//start timer
}

void prvSetupTimerInterrupt(void){
	reg_tmr_sta = FLD_TMR_STA_TMR0; 									//clear irq status
	prvRestoreTimerInterrupt();
}

_attribute_ram_code_ __attribute__((naked)) static void restoreActiveTask( void ){
	asm("tloadr 	r0, =pxCurrentTCB");
	asm("tloadr		r0, [r0]");
	asm("tloadr 	r1, [r0]"); 	// load new SP 

	asm("tloadr 	r0, [r1] ");	// load R14
	asm("tmov		r14, r0 "); 	// set R14
	asm("tadd		r1, #4 ");		// bottom of SP
	asm("tmov		r4, r1 ");		// irq status
	asm("tadd		r1, #64 "); 	// bottom of SP
	asm("tmov		r13, r1 "); 	// set task SP

	//	жģʽ
	asm("tloadr 	r0, _CPSR_IRQ_ ");			// don't worry CPSR, be will overide later
	asm("tmcsr		r0");			// switch to irq mode
	
	asm("tloadr 	r2, _REG_IRQ_EN "); // irq enable
	asm("tloadr 	r0, [r4]");
	asm("tstorerb	r0, [r2] ");

	asm("tmov		r2, r13 ");
	asm("tmov		r3, #15");
	asm("_TASK_TO_IRQ:");
	asm("tsub		r1, #4");
	asm("tsub		r2, #4");
	asm("tloadr 	r0, [r1]");
	asm("tstorer	r0, [r2]");
	asm("tsub		r3, #1");
	asm("tcmp		r3, #0");
	asm("tjne		_TASK_TO_IRQ");
	asm("tmov		r13, r2 ");

	asm("tpop		{r0-r5}");
	asm("tmov		r8, r1");
	asm("tmov		r9, r2");
	asm("tmov		r10,r3");
	asm("tmov		r11,r4");
	asm("tmov		r12,r5");
	asm("tmssr		r0");
	asm("tpop		{r0-r7}");
	asm("treti		{r15}");			// PC
	asm(".align 	4");
asm("_REG_IRQ_EN:");
	asm(".word		0x800643");
asm("_CPSR_IRQ_:");
	asm(".word		0x92");
	
}

BaseType_t xPortStartScheduler( void )
{
	/* Initialise the critical nesting count ready for the first task. */
//	uxCriticalNesting = 0;
	prvSetupTimerInterrupt();
	/* Start the first task. */
	restoreActiveTask();

	/* Should not get here! */
	return 0;
}

uint32_t taskLR;
_attribute_ram_code_ __attribute__((naked)) void vPortYield_i(){
	asm("tpush 		{r14}");
	asm("tpush		{r0-r7}");
	asm("tmrcs		r0");
	asm("tmov 		r1, r8");
	asm("tmov 		r2, r9");
	asm("tmov 		r3, r10");
	asm("tmov 		r4, r11");
	asm("tmov 		r5, r12");
	asm("tpush		{r0-r5}");

	asm("tloadr 	r2, _REG_IRQ_EN3 "); // irq enable
	asm("tloadrb 	r0, [r2]"); 
	asm("tpush 		{r0}"); 			// push irq status

	asm("tloadr 	r1, =taskLR"); 
	asm("tloadr 	r0, [r1]"); 
	asm("tpush 		{r0}"); 			// push r14
	
	asm("tloadr		r0, =pxCurrentTCB");
	asm("tloadr		r0, [r0]");
	asm("tmov		r1, r13");
	asm("tstorer 	r1, [r0]");			// set old SP to task context

	asm("tjl		vTaskSwitchContext");
	//DEBUG_GPIO(GPIO_CHN2, 0);
	asm("tjl		restoreActiveTask");
asm(".align 	4");
asm("_REG_IRQ_EN3:");
	asm(".word		0x800643");

}

_attribute_ram_code_ __attribute__((naked)) void vPortYield(){
	//DEBUG_GPIO(GPIO_CHN2, 1);
	asm("tpush		{lr}");
	asm("tloadr 	r1, _REG_IRQ_EN2 ");
	asm("tloadr 	r2, [r1] "); 		 // save irq status
	asm("tmov 		r0, #0"); 			 //  irq disable
	asm("tstorerb	r0, [r1]"); 

	asm("tloadr 	r1, =taskLR"); 
	asm("tmov 		r0, r14"); 
	asm("tstorer 	r0, [r1]"); 

	asm("tjl		vPortYield_i"); 

	asm("tloadr 	r1, _REG_IRQ_EN2 ");
	asm("tstorerb	r2, [r1]");  		// irq restore
	asm("tpop		{pc}");
asm(".align 	4");
asm("_REG_IRQ_EN2:");
	asm(".word		0x800643");
}

_attribute_ram_code_ __attribute__((naked)) void vPortYieldSvc (void *old, void *nw){
	asm("tpush 		{r14}");			// push r14, return address
	asm("tpush		{r0-r7}");			
	asm("tmov		r4, r0");			// save old task Control Block
	asm("tmov		r5, r1");			// save new task Control Block
	
	asm("tmrcs 		r0");				// save CPSR
	asm("tmov 		r6, r0");
	
	asm("tloadr 	r0, _CPSR_NORMAL_ ");			// don't worry CPSR, be will overide later
	asm("tmcsr		r0");				// switch from irq mode to normal mode

	// get  normal  SP
	asm("tmov		r3, #15");
	asm("tmov		r2, r13");			// r2 the the old task SP
	asm("tloadr		r1, =StackSave");
	asm("tloadr		r1, [r1]");
	asm("tadd		r1, #60");			// r1 is StackSave + 60,  that is the bottom of stack

	asm("_RAM_TO_TASK:");
	asm("tsub		r1, #4");
	asm("tsub		r2, #4");
	asm("tloadr		r0, [r1]");
	asm("tstorer 	r0, [r2]");
	asm("tsub		r3, #1");
	asm("tcmp		r3, #0");
	asm("tjne		_RAM_TO_TASK");

	asm("tmov 		r0, #1"); 
	asm("tsub		r2, #4");
	asm("tstorer 	r0, [r2]");		// irq must be enabled, because we are in IRQ

	asm("tmov		r0, r14");		// normal r14
	asm("tsub		r2, #4");
	asm("tstorer	r0, [r2]");
	asm("tstorer 	r2, [r4]");		// save task SP

	asm("tloadr 	r1, [r5]");		// new task SP
	asm("tloadr		r0, [r1]");
	asm("tmov		r14, r0");		// restore normal r14
	asm("tadd		r1, #4");

	asm("tloadr		r4, [r1]");		// get irq status
	asm("tadd		r1, #4");		// pop stack

	asm("tloadr		r0, =StackSave");
	asm("tloadr		r2, [r0]");		// saved irq SP to r2:  that is a pointer to the top of the calling frame.   see irq routine
	asm("tmov		r3, #15");
	asm("_TASK_TO_IRQ1:");
	asm("tloadr		r0, [r1]");
	asm("tstorer 	r0, [r2]");
	asm("tadd		r1, #4");
	asm("tadd		r2, #4");
	asm("tsub		r3, #1");
	asm("tcmp		r3, #0");
	asm("tjne		_TASK_TO_IRQ1");

	asm("tmov		r13, r1");		// Update SVC SP
	asm("tstorer 	r1, [r5]");		// pop up svc stack

	asm("tmov		r0, r6");		// Restore CPSR
	asm("tmcsr		r0");			// switch from svc to irq

	asm("tloadr 	r0, _REG_IRQ_EN1 "); // irq enable
	asm("tstorerb	r4, [r0] ");

	asm("tpop 		{r0-r7}");		// call entry
	asm("tpop		{r15}");

	asm(".align 	4");
asm("_REG_IRQ_EN1:");
	asm(".word		0x800643");
asm("_CPSR_NORMAL_:");
	asm(".word		0x13");

}

static uint32_t timeBeforeSleep;
static int tickRemain = 0;
void task_restore(int retention){
	uint32_t t = (uint32_t)(reg_system_tick - timeBeforeSleep);
	if(t < U32_MAX / 2){
		t += tickRemain;
		tickRemain = t / (CLOCK_SYS_CLOCK_HZ / configTICK_RATE_HZ);
		vTaskStepTick(tickRemain);
		tickRemain = t - tickRemain * (CLOCK_SYS_CLOCK_HZ / configTICK_RATE_HZ);
	}
	if(retention){
		prvRestoreTimerInterrupt();
		if(irqTimer0Tick + t >= TICK_HZ){
			reg_tmr0_tick = TICK_HZ - 16;    // to ensure irq triggered
		}
		restoreActiveTask();
	}
}

extern void blt_ll_sleep(uint32_t xExpectedIdleTime);
void suppress_tick_and_sleep(uint32_t xExpectedIdleTime){
	u8 r = irq_disable();	// just for test
//	DEBUG_GPIO(GPIO_CHN0, 0);
	timeBeforeSleep = reg_system_tick;
	irqTimer0Tick = reg_tmr0_tick;
	if(xExpectedIdleTime > U32_MAX / 2 / (CLOCK_SYS_CLOCK_HZ / configTICK_RATE_HZ)){
		blt_ll_sleep(-1);
	}else{
		blt_ll_sleep(xExpectedIdleTime * (CLOCK_SYS_CLOCK_HZ / configTICK_RATE_HZ));
	}
	task_restore(0);
	irq_restore(r);				// allow RF irq triggered
}

__attribute__((naked)) void vportSUPPRESS_TICKS_AND_SLEEP_i(uint32_t xExpectedIdleTime){
	asm("tpush 		{r14}");
	asm("tpush		{r0-r7}");
	asm("tmov 		r6, r0");			// save xExpectedIdleTime
	asm("tmrcs		r0");
	asm("tmov 		r1, r8");
	asm("tmov 		r2, r9");
	asm("tmov 		r3, r10");
	asm("tmov 		r4, r11");
	asm("tmov 		r5, r12");
	asm("tpush		{r0-r5}");

	asm("tloadr 	r2, _REG_IRQ_EN4 "); // irq enable
	asm("tloadrb 	r0, [r2]"); 
	asm("tpush 		{r0}"); 			// push irq status

	asm("tloadr 	r1, =taskLR"); 
	asm("tloadr 	r0, [r1]"); 
	asm("tpush 		{r0}"); 			// push r14

	asm("tloadr		r0, =pxCurrentTCB");
	asm("tloadr		r0, [r0]");
	asm("tmov		r1, r13");
	asm("tstorer 	r1, [r0]");			// set old SP to task context
	
	asm("tmov 		r0, r6");			// restore xExpectedIdleTime
	asm("tjl 		suppress_tick_and_sleep");			// set old SP to task context

	asm("tpop		{r0-r1}");			// by pass LR, irq status
	asm("tpop		{r0-r5}");
	asm("tmov		r8, r1");
	asm("tmov		r9, r2");
	asm("tmov		r10,r3");
	asm("tmov		r11,r4");
	asm("tmov		r12,r5");
										// by pass tmrssr  r0
	asm("tpop		{r0-r7}");
	asm("tpop		{r15}");
	
asm(".align 	4");
asm("_REG_IRQ_EN4:");
	asm(".word		0x800643");

}

extern void proto_task_notify();
void vportSUPPRESS_TICKS_AND_SLEEP(uint32_t xExpectedIdleTime){
#if(BLE_REMOTE_PM_ENABLE)	
	asm("tloadr 	r1, =taskLR"); 								// save LR
	asm("tmov 		r2, r14"); 
	asm("tstorer 	r2, [r1]"); 
	asm("tjl 		vportSUPPRESS_TICKS_AND_SLEEP_i");			// set old SP to task context
#endif	
	proto_task_notify();
}

