/*
 * Manual context switch.  This is similar to the tick context switch,
 * but does not increment the tick count.  It must be identical to the
 * tick context switch in how it stores the stack of a task.
 */
void vPortYield( void )
{
    /* This can get called with interrupts either enabled or disabled.  We
    will save the INTCON register with the interrupt enable bits unmodified. */
    portSAVE_CONTEXT( portINTERRUPTS_UNCHANGED );

    /* Switch to the highest priority task that is ready to run. */
    vTaskSwitchContext();

    /* Start executing the task we have just switched to. */
    portRESTORE_CONTEXT();
}

void vEINT0_ISR_Wrapper( void )
{
	/* Save the context of the interrupted task. */
	portSAVE_CONTEXT();

	/* The handler must be a separate function from the wrapper to
	ensure the correct stack frame is set up. */
	vEINT0_ISR_Handler();

	/* Restore the context of whichever task is going to run next. */
	portRESTORE_CONTEXT();
}

/**
 @brief Wrapper for the serial interrupt vector.
 
 FreeRTOS can save the context (all registers, stack positions, etc.) 
 so everything doesn't get screwed up because the ARM doesn't do that for you.
*/
void vUART_ISR_Wrapper( void )
{
	/* Save the context of the interrupted task. */
	portSAVE_CONTEXT();

	/* Call the handler.  This must be a separate function from the wrapper
	to ensure the correct stack frame is set up. */
	__asm volatile ("bl vUART_ISR_Handler");

	/* Restore the context of whichever task is going to run next. */
	portRESTORE_CONTEXT();
}

void vEMACISR_Wrapper( void )
{
	/* Save the context of the interrupted task. */
	portSAVE_CONTEXT();
	
	/* Call the handler task to do the actual work.  This must be a separate
	function to ensure the stack frame is correctly set up. */
	__asm volatile ("bl vEMACISR_Handler");
	
	/* Restore the context of whichever task is the next to run. */
	portRESTORE_CONTEXT();
}

void vI2C_ISR_Wrapper( void )
{
	/* Save the context of the interrupted task. */
	portSAVE_CONTEXT();

	/* Call the handler to perform the actual work.  This must be a
	separate function to ensure the correct stack frame is set up. */
	vI2C_ISR_Handler();

	/* Restore the context of whichever task is going to run next. */
	portRESTORE_CONTEXT();
}

void  vEMACISR_Wrapper( void )
{
	/* Save the context of the interrupted task. */
	portSAVE_CONTEXT();

	/* Call the handler to do the work.  This must be a separate
	function to ensure the stack frame is set up correctly. */
	vEMACISR_Handler();

	/* Restore the context of whichever task will execute next. */
	portRESTORE_CONTEXT();
}

void vEMAC_ISR_Wrapper( void )
{
	/* Save the context of the interrupted task. */
    portSAVE_CONTEXT();
    
    /* Call the handler.  This must be a separate function unless you can
    guarantee that no stack will be used. */
    __asm volatile ( "bl vEMAC_ISR_Handler" );
    
    /* Restore the context of whichever task is going to run next. */
    portRESTORE_CONTEXT();
}

void vButtonISRWrapper( void )
{
	/* Save the context of the interrupted task. */
	portSAVE_CONTEXT();

	/* Call the handler to do the work.  This must be a separate function to
	the wrapper to ensure the correct stack frame is set up. */
	__asm volatile( "bl vButtonHandler" );

	/* Restore the context of whichever task is going to run once the interrupt
	completes. */
	portRESTORE_CONTEXT();
}

void vPortYieldFromTick( void )
{
	portSAVE_CONTEXT();

	if( xTaskIncrementTick() != pdFALSE )
	{
		vTaskSwitchContext();
	}

	portRESTORE_CONTEXT();

	__asm__ __volatile__ ( "ret" );
}

void i2c_isr()
{
	portSAVE_CONTEXT();
	//i2c_stateMachine();
	if (i2c_stateMachine())     //run the state machine code.
	{   //finished sending/receiving data.
		signed portBASE_TYPE prio_task; xSemaphoreGiveFromISR(i2c_done, &prio_task);
	} else { //not done yet.
		portYIELD_FROM_ISR(); //let other tasks happen, we're not done with the state machine yet.
	}
	VICVectAddr = 0;	//Acknowledge interrupt
	portRESTORE_CONTEXT();
}

ISR (TCC0_OVF_vect, ISR_NAKED) {
    /*
     * Context switch function used by the tick.  This must be identical to
     * vPortYield() from the call to vTaskSwitchContext() onwards.  The only
     * difference from vPortYield() is the tick count is incremented as the
     * call comes from the tick ISR.
     */
    portSAVE_CONTEXT();
    vTaskIncrementTick();
    vTaskSwitchContext();
    portRESTORE_CONTEXT();
    asm volatile ( "reti" );
}

static void
prvPortPreemptiveTick( void )
{
    asm volatile ( "move.w  #0x2700, %sr\n\t" );
#if _GCC_USES_FP == 1
    asm volatile ( "unlk %fp\n\t" );
#endif
    portSAVE_CONTEXT(  );
    MCF_PIT_PCSR0 |= MCF_PIT_PCSR_PIF;
    vTaskIncrementTick(  );
    vTaskSwitchContext(  );
    portRESTORE_CONTEXT(  );
}

void vUSB_ISR_Wrapper( void )
{
	/* Save the context of the interrupted task. */
	portSAVE_CONTEXT();

	/* Call the handler itself.  This must be a separate function as it uses
	the stack. */
	__asm volatile ("bl vUSB_ISR_Handler");

	/* Restore the context of the task that is going to 
	execute next. This might not be the same as the originally 
	interrupted task.*/
	portRESTORE_CONTEXT();
}

void spi_isr(void)
{
    portSAVE_CONTEXT();
    static signed portBASE_TYPE xHigherPriorityTaskWoken= pdFALSE;
    //FIO1PIN = FIO1PIN ^ (1<<19);   
    //Between Boilerplate..
    S0SPINT |= (1<<0);      //Clear SPI Interrupt Flag by writing a 1 to it.
    VICVectAddr = 0x0;      // Update VIC hardware
    xSemaphoreGiveFromISR( spi_IntSemaphore, &xHigherPriorityTaskWoken );
    if( xHigherPriorityTaskWoken ) {
	    portYIELD_FROM_ISR();
    }
    
    portRESTORE_CONTEXT();
}

	void prvTickISR( void )
	{
		/* Save the context of the interrupted task. */
		portSAVE_CONTEXT();

		/* Increment the tick count then switch to the highest priority task
		that is ready to run. */
		if( xTaskIncrementTick() != pdFALSE )
		{
			vTaskSwitchContext();
		}

		/* Restore the context of the new task. */
		portRESTORE_CONTEXT();
	}

void audioIsr_Wrapper( void )
{
    /* Save the context of the interrupted task. */
    portSAVE_CONTEXT();

#ifdef CFG_DEEP_SLEEP
    check_power_mode();
#endif

    /* Call the handler to do the work.  This must be a separate
       function to ensure the stack frame is set up correctly. */
    audio_isr();

    /* Restore the context of whichever task will execute next. */
    portRESTORE_CONTEXT();
}

/*
 * Called by portYIELD() or taskYIELD() to manually force a context switch.
 */
static void
prvPortYield( void )
{
    asm volatile ( "move.w  #0x2700, %sr\n\t" );
#if _GCC_USES_FP == 1
    asm volatile ( "unlk %fp\n\t" );
#endif
     /* Perform the context switch.  First save the context of the current task. */
    portSAVE_CONTEXT(  );

    /* Find the highest priority task that is ready to run. */
    vTaskSwitchContext(  );

    /* Restore the context of the new task. */
    portRESTORE_CONTEXT(  );
}

/*
 * Called by portYIELD() or taskYIELD() to manually force a context switch.
 *
 * When a context switch is performed from the task level the saved task 
 * context is made to look as if it occurred from within the tick ISR.  This
 * way the same restore context function can be used when restoring the context
 * saved from the ISR or that saved from a call to vPortYieldProcessor.
 */
void vPortYieldProcessor( void )
{
	/* Within an IRQ ISR the link register has an offset from the true return 
	address, but an SWI ISR does not.  Add the offset manually so the same 
	ISR return code can be used in both cases. */
	__asm volatile ( "ADD		LR, LR, #4" );

	/* Perform the context switch.  First save the context of the current task. */
	portSAVE_CONTEXT();

	/* Find the highest priority task that is ready to run. */
	__asm volatile( "bl			vTaskSwitchContext" );

	/* Restore the context of the new task. */
	portRESTORE_CONTEXT();	
}

/******************************************************************************
 External interrupt 0 handler
******************************************************************************/
void irq0Handler(void)
{
	//LOG(__FUNCTION__);
#if defined(FREE_RTOS)
  portSAVE_CONTEXT();
#endif

  if (NULL != IrqCallbackList[HAL_FIRST_VALID_IRQ])
    IrqCallbackList[HAL_FIRST_VALID_IRQ]();

#if defined(FREE_RTOS)
  /* End the interrupt in the AIC. */
  AT91C_BASE_AIC->AIC_EOICR = 0;

  portRESTORE_CONTEXT();
#endif
}

	void vPreemptiveTick( void )
	{
		/* Save the context of the interrupted task. */
		portSAVE_CONTEXT();	

		/* Increment the RTOS tick count, then look for the highest priority 
		task that is ready to run. */
		__asm volatile( "bl vTaskIncrementTick" );
		__asm volatile( "bl vTaskSwitchContext" );

		/* Ready for the next interrupt. */
		T0IR = 2;
		VICVectAddr = portCLEAR_VIC_INTERRUPT;
		
		/* Restore the context of the new task. */
		portRESTORE_CONTEXT();
	}