void aes_hard_init_CBC(void) 
{
    unsigned char key[ENCRYPTION_KEY_LENGTH];
    unsigned char IV[ENCRYPTION_BLOCK_LENGTH];
  
    TRACE_DEBUG("aes_hard_init_CBC\n\r");

    // Activate peripheral clock
#ifdef AT91C_ID_AES
    PMC_EnablePeripheral( AT91C_ID_AES );
#elif AT91C_ID_AESTDES
    PMC_EnablePeripheral( AT91C_ID_AESTDES );
#elif AT91C_ID_TDES
    PMC_EnablePeripheral( AT91C_ID_TDES );
#else
#error AES undefined
#endif

    // Load mode
    AT91C_BASE_AES->AES_MR = AT91C_AES_SMOD_PDC   // PDC Mode
#if (ENCRYPTION_KEY_LENGTH == 32)
                           | AT91C_AES_KEYSIZE_256_BIT
#endif
#if (ENCRYPTION_KEY_LENGTH == 24)
                           | AT91C_AES_KEYSIZE_192_BIT
#endif
                           | AT91C_AES_OPMOD_CBC; // Cipher Block Chaining mode
  
    // Convert and load key
    ASCII2Hex((unsigned char*)ENCRYPTION_KEY, key, ENCRYPTION_KEY_LENGTH);

    AES_SetKey((unsigned int*)key, ENCRYPTION_KEY_LENGTH);

    // Convert and load IV
    ASCII2Hex((unsigned char*)ENCRYPTION_IV, IV, ENCRYPTION_BLOCK_LENGTH);

    AES_SetVector((unsigned int*)IV);
}

void SensorsInit()
{
	char pinDirection = 0;
	int i;
	PIO_Configure(analogs, PIO_LISTSIZE(analogs));
	PIO_Configure(digitalIns, PIO_LISTSIZE(digitalIns));
	PIO_Configure(sensorsPower, PIO_LISTSIZE(sensorsPower));
	
	PMC_EnablePeripheral(AT91C_ID_PIOB);
	PMC_EnablePeripheral(AT91C_ID_PIOA);
	
	PMC_EnablePeripheral(AT91C_ID_ADC1);
	ADC_Initialize(AT91C_BASE_ADC1, AT91C_ID_ADC1, AT91C_ADC_TRGEN_DIS, \
				   0, AT91C_ADC_SLEEP_NORMAL_MODE, AT91C_ADC_LOWRES_10_BIT, \
				   MCK, BOARD_ADC_FREQ, 10, 3000);
	for(i = 0;i < 8;i++) ADC_EnableChannel(AT91C_BASE_ADC1, i);

	SetPitCallback(SensorsCallback,1);
	
	SelectAng0();		// select analog port 0 to be mesured instead of BattVoltage
	SensorPowerOn();	// turn on the Vcc line
	
	if(PMC_IsPeriphEnabled(AT91C_ID_TWI) == 0){
		PIO_Configure(twiPortPins, PIO_LISTSIZE(twiPortPins));
		PMC_EnablePeripheral(AT91C_ID_TWI);
		
		TWI_ConfigureMaster(AT91C_BASE_TWI,TWCK,MCK);
	}
	
	// set all pins on pullup chip to output
	WritePullupData(0x00,&pinDirection,1);
	WritePullupData(0x01,&pinDirection,1);
	
	for(i = 0;i < 8;i++){
		if(i>1) AnalogPullup(i,1);  // enable pullups if needed
		DigitalPullup(i,1);		// enable all digital pullups
	}
}

/**
 * Configure USART to work @ 115200
 */
static void _ConfigureUsart(void)
{
	PIO_Configure(pins, PIO_LISTSIZE(pins));
	PMC_EnablePeripheral(ID_USART);
	USART_DisableIt(BASE_USART, 0xFFFFFFFF);
	USART_Configure(BASE_USART,
					USART_MODE_ASYNCHRONOUS,
					115200,
					BOARD_MCK);

	USART_SetTransmitterEnabled(BASE_USART, 1);
	USART_SetReceiverEnabled(BASE_USART, 1);
	NVIC_EnableIRQ(USART2_IRQn);
}

/**
 *  \brief TC0 configuration
 *
 * Configures Timer Counter 0 (TC0) to generate an interrupt every second.
 *
 */
static void ConfigureTc0( void )
{

    /* Enable TC0 peripheral clock*/
    PMC_EnablePeripheral( ID_TC0 ) ;

    /* Configure TC for a 1s (= 1Hz) tick*/
    TC_Configure(TC0,0, 0x4 | TC_CMR0_ACPC_SET | TC_CMR0_WAVE
            | TC_CMR0_ACPA_CLEAR | (0x2 << 13));

    /* 50% duty ,1s frequency*/
    TC0->TC_CHANNEL[0].TC_RA = SAMPLING_PERIOD/2;
    TC0->TC_CHANNEL[0].TC_RC = SAMPLING_PERIOD;
}

/**
 * Configure USART to work @ 115200
 */
static void ConfigureUsart(void)
{
    PIO_Configure(pinsUsart, PIO_LISTSIZE(pinsUsart));
    PMC_EnablePeripheral(ID_USART);
    BASE_USART->US_IDR = 0xFFFFFFFF;
    USART_Configure(BASE_USART,
                    USART_MODE_ASYNCHRONOUS,
                    115200,
                    BOARD_MCK);

    USART_SetTransmitterEnabled(BASE_USART, 1);
    USART_SetReceiverEnabled(BASE_USART, 1);
    NVIC_EnableIRQ(IRQn_USART);
}

int platform_init()
{
  int i;
   
  // Enable the peripherals we use in the PMC
  PMC_EnablePeripheral( AT91C_ID_US0 );  
  PMC_EnablePeripheral( AT91C_ID_US1 );
  PMC_EnablePeripheral( AT91C_ID_PIOA );
  PMC_EnablePeripheral( AT91C_ID_PIOB );
  PMC_EnablePeripheral( AT91C_ID_TC0 );
  PMC_EnablePeripheral( AT91C_ID_TC1 );
  PMC_EnablePeripheral( AT91C_ID_TC2 );
  PMC_EnablePeripheral( AT91C_ID_PWMC );  
  
  // Configure the timers
  AT91C_BASE_TCB->TCB_BMR = 0x15;
  for( i = 0; i < 3; i ++ )
    TC_Configure( ( AT91S_TC* )timer_base[ i ], AT91C_TC_CLKS_TIMER_DIV5_CLOCK | AT91C_TC_WAVE );
        
  // PWM setup (only the clocks are set at this point)
  PWMC_ConfigureClocks( BOARD_MCK, BOARD_MCK, BOARD_MCK );
  PWMC_ConfigureChannel( 0, AT91C_PWMC_CPRE_MCKA, 0, 0 );
  PWMC_ConfigureChannel( 1, AT91C_PWMC_CPRE_MCKA, 0, 0 );  
  PWMC_ConfigureChannel( 2, AT91C_PWMC_CPRE_MCKB, 0, 0 );
  PWMC_ConfigureChannel( 3, AT91C_PWMC_CPRE_MCKB, 0, 0 );    
  for( i = 0; i < 4; i ++ )
  {
    PWMC_EnableChannel( i );
    PWMC_EnableChannelIt( i );
  }

  cmn_platform_init();
  
#if VTMR_NUM_TIMERS > 0
  // Virtual timer initialization
  TC_Configure( AT91C_BASE_TC2, AT91C_TC_CLKS_TIMER_DIV5_CLOCK | AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO );
  AT91C_BASE_TC2->TC_RC = ( BOARD_MCK / 1024 ) / VTMR_FREQ_HZ;
  AIC_DisableIT( AT91C_ID_TC2 );
  AIC_ConfigureIT( AT91C_ID_TC2, 0, ISR_Tc2 );
  AT91C_BASE_TC2->TC_IER = AT91C_TC_CPCS;
  AIC_EnableIT( AT91C_ID_TC2 );  
  TC_Start( AT91C_BASE_TC2 );
#endif
    
  return PLATFORM_OK;
} 

/**
 * \brief Configures the parameters for the device corresponding to the cs value.
 *
 * \param pSpid  Pointer to a Spid instance.
 * \param cs number corresponding to the SPI chip select.
 * \param csr SPI_CSR value to setup.
 */
void SPID_ConfigureCS( Spid *pSpid, 
                       uint32_t dwCS, 
                       uint32_t dwCsr)
{
    Spi *pSpiHw = pSpid->pSpiHw;
    
    /* Enable the SPI Peripheral */
    PMC_EnablePeripheral (pSpid->spiId );
    /* Configure SPI Chip Select Register */
    SPI_ConfigureNPCS( pSpiHw, dwCS, dwCsr );
    
    /* Disable the SPI Peripheral */
    PMC_DisablePeripheral (pSpid->spiId );
    
}

/**
 * \brief Initializes a TWI driver instance, using the given TWI peripheral.
 * \note The peripheral must have been initialized properly before calling this function.
 * \param pTwid  Pointer to the Twid instance to initialize.
 * \param pTwi  Pointer to the TWI peripheral to use.
 */
Twi* TWID_Initialize(uint8_t ucDeviceID,Twi* pTwi, uint32_t dwTwCk, uint32_t dwMCk)
{
    uint32_t dwCkDiv = 0 ;
    uint32_t dwClDiv ;
    uint32_t dwOk = 0 ;

    TRACE_DEBUG( "TWID_Initialize()\n\r" ) ;
    assert( pTwi != NULL ) ;

	PMC_EnablePeripheral(ucDeviceID);

    /* SVEN: TWI Slave Mode Enabled */
    pTwi->TWI_CR = TWI_CR_SVEN ;
    /* Reset the TWI */
    pTwi->TWI_CR = TWI_CR_SWRST ;
    pTwi->TWI_RHR ;

    /* TWI Slave Mode Disabled, TWI Master Mode Disabled. */
    pTwi->TWI_CR = TWI_CR_SVDIS ;
    pTwi->TWI_CR = TWI_CR_MSDIS ;

    /* Set master mode */
    pTwi->TWI_CR = TWI_CR_MSEN ;

    /* Configure clock */
    while ( !dwOk )
    {
        dwClDiv = ((dwMCk / (2 * dwTwCk)) - 4) / (1<<dwCkDiv) ;

        if ( dwClDiv <= 255 )
        {
            dwOk = 1 ;
        }
        else
        {
            dwCkDiv++ ;
        }
    }

    assert( dwCkDiv < 8 ) ;
    TRACE_DEBUG( "Using CKDIV = %u and CLDIV/CHDIV = %u\n\r", dwCkDiv, dwClDiv ) ;

    pTwi->TWI_CWGR = 0 ;
    pTwi->TWI_CWGR = (dwCkDiv << 16) | (dwClDiv << 8) | dwClDiv ;

	return pTwi;

}

/**
 * Configure the  MCI CLKDIV in the MCI_MR register. The max. for MCI clock is
 * MCK/2 and corresponds to CLKDIV = 0
 * \param pMci  Pointer to the low level MCI driver.
 * \param mciSpeed  MCI clock speed in Hz, 0 will not change current speed.
 * \param mck       MCK to generate MCI Clock, in Hz
 * \return The actual speed used, 0 for fail.
 */
uint32_t MCI_SetSpeed( Mcid* pMci, uint32_t mciSpeed, uint32_t mck )
{
    Hsmci *pMciHw = pMci->pMciHw;
    uint32_t mciMr;
    uint32_t clkdiv;
    uint8_t  mciDis;

    assert(pMci);
    assert(pMciHw);

    PMC_EnablePeripheral(pMci->mciId);
    mciDis = PMC_IsPeriphEnabled(pMci->mciId);

    mciMr = pMciHw->HSMCI_MR & (~(uint32_t)HSMCI_MR_CLKDIV_Msk);
    /* Multimedia Card Interface clock (MCCK or MCI_CK) is Master Clock (MCK)
     * divided by (2*(CLKDIV+1))
     * mciSpeed = MCK / (2*(CLKDIV+1)) */
    if (mciSpeed > 0)
    {
        clkdiv = (mck / 2 / mciSpeed);
        /* Speed should not bigger than expired one */
        if (mciSpeed < mck/2/clkdiv)
        {
            clkdiv ++;
        }
		
        if ( clkdiv > 0 )
        {
            clkdiv -= 1;
        }
        assert( (clkdiv & 0xFFFFFF00) == 0 ) ; /* "mciSpeed too small" */
    }
    else
    {
	    clkdiv = 0 ;
    }

    /* Actual MCI speed */
    mciSpeed = mck / 2 / (clkdiv + 1);
    /* Modify MR */
    pMciHw->HSMCI_MR = mciMr | clkdiv;
    if ( mciDis )
    {
        PMC_DisablePeripheral( pMci->mciId ) ;
    }

    return (mciSpeed);
}

/**
 * \brief Initialize the GMAC with the Gmac controller address
 *  \param pGmacd Pointer to GMAC Driver instance. 
 *  \param pHw    Pointer to HW address for registers.
 *  \param bID     HW ID for power management
 *  \param enableCAF    Enable/Disable CopyAllFrame.
 *  \param enableNBC    Enable/Disable NoBroadCast.
 */
 void GMACD_Init(sGmacd *pGmacd,
                Gmac *pHw,
                uint8_t bID, 
                uint8_t enableCAF, 
                uint8_t enableNBC )
{
    uint32_t dwNcfgr;
    
    /* Check parameters */
    assert(GRX_BUFFERS * GMAC_RX_UNITSIZE > GMAC_FRAME_LENTGH_MAX);

    TRACE_DEBUG("GMAC_Init\n\r");

    /* Initialize struct */
    pGmacd->pHw = pHw;
    pGmacd->bId = bID;

    /* Power ON */
    PMC_EnablePeripheral(bID);

    /* Disable TX & RX and more */
    GMAC_NetworkControl(pHw, 0);
    GMAC_DisableIt(pHw, ~0u);
    
    GMAC_ClearStatistics(pHw);
    /* Clear all status bits in the receive status register. */
    GMAC_ClearRxStatus(pHw, GMAC_RSR_RXOVR | GMAC_RSR_REC | GMAC_RSR_BNA |GMAC_RSR_HNO);

    /* Clear all status bits in the transmit status register */
    GMAC_ClearTxStatus(pHw, GMAC_TSR_UBR | GMAC_TSR_COL | GMAC_TSR_RLE
                            | GMAC_TSR_TXGO | GMAC_TSR_TFC | GMAC_TSR_TXCOMP
                            | GMAC_TSR_UND | GMAC_TSR_HRESP | GMAC_TSR_LCO);

    /* Clear interrupts */
    GMAC_GetItStatus(pHw);

    /* Enable the copy of data into the buffers
       ignore broadcasts, and don't copy FCS. */
    dwNcfgr = GMAC_NCFGR_FD | GMAC_NCFGR_GBE | GMAC_NCFGR_DBW_DBW64 | GMAC_NCFGR_CLK_MCK_64;
    if( enableCAF ) {
        dwNcfgr |= GMAC_NCFGR_CAF;
    }
    if( enableNBC ) {
        dwNcfgr |= GMAC_NCFGR_NBC;
    }
    
    GMAC_Configure(pHw, dwNcfgr);
}

/**
 * Configure TC0 to generate an interrupt every 4ms
 */
static void ConfigureTc0(void)
{
    uint32_t div, tcclks;

    /* Enable TC0 peripheral */
    PMC_EnablePeripheral(ID_TC0);
    /* Configure TC0 for 250Hz frequency and trigger on RC compare */
    TC_FindMckDivisor(250, BOARD_MCK, &div, &tcclks, BOARD_MCK);
    TC_Configure(TC0, 0, tcclks | TC_CMR_CPCTRG);
    TC0->TC_CHANNEL[0].TC_RC = (BOARD_MCK / div) / 250;
    /* Configure and enable interrupt on RC compare */
    NVIC_EnableIRQ(TC0_IRQn);
    TC0->TC_CHANNEL[0].TC_IER = TC_IER_CPCS;
    /* Start TC as event timer */
    TC_Start(TC0, 0);
}

static void _ConfigureTc0( void )
{
    /* Enable TC0 peripheral clock*/
    PMC_EnablePeripheral( ID_TC0 ) ;

    /* Configure TC for a 1s (= 1Hz) tick*/
    TC_Configure( TC0, 0, 0x4 | TC_CMR_CPCTRG ) ;

    TC0->TC_CHANNEL[0].TC_RC = 32768 ;

    /* Configure interrupt on RC compare*/
    TC0->TC_CHANNEL[0].TC_IER = TC_SR_CPCS ;

    NVIC_EnableIRQ( TC0_IRQn ) ;

}

/**
 * \brief Initialize the ADC controller
 *
 * \param pAdc Pointer to an Adc instance.
 * \param dwID ADC Index
 */
extern void ADC_Initialize( Adc* pAdc, uint32_t dwID )
{
    /* Enable peripheral clock*/
    PMC_EnablePeripheral(dwID);

    /*  Reset the controller */
    pAdc->ADC_CR = ADC_CR_SWRST;

    /* Reset Mode Register */
    pAdc->ADC_MR = 0;

    /* Reset PDC transfer */
    pAdc->ADC_PTCR = (ADC_PTCR_RXTDIS | ADC_PTCR_TXTDIS);
    pAdc->ADC_RCR = 0;
    pAdc->ADC_RNCR = 0;
}

/* _____GLOBAL FUNCTIONS_____________________________________________________ */
void usart0_init(void)
{
    // Clear flag
    usart0_tx_finished_flag = FALSE;

    // Initialise ring buffers
    ring_buffer_init(&usart0_rx_ring_buffer, usart0_rx_buffer, USART0_RX_BUFFER_SIZE);
    ring_buffer_init(&usart0_tx_ring_buffer, usart0_tx_buffer, USART0_TX_BUFFER_SIZE);

    // Configure PIO pins for USART0 peripheral
    PIO_Configure(USART0_Pins, PIO_LISTSIZE(USART0_Pins));    

    // Disable the interrupt on the interrupt controller
    AIC_DisableIT(AT91C_ID_US0);

    // Enable USART0 clock
    PMC_EnablePeripheral(AT91C_ID_US0);

    // Disable all USART0 interrupts
    AT91C_BASE_US0->US_IDR = 0xFFFFFFFF;

    // Configure USART
    USART_Configure(AT91C_BASE_US0,
                    AT91C_US_USMODE_NORMAL |
                    AT91C_US_CLKS_CLOCK    |
                    AT91C_US_CHRL_8_BITS   | 
                    AT91C_US_PAR_NONE      | 
                    AT91C_US_NBSTOP_1_BIT,
                    115200,
                    BOARD_MCK);

    USART_SetTransmitterEnabled(AT91C_BASE_US0, 1);
    USART_SetReceiverEnabled(AT91C_BASE_US0, 1);

    // Configure the AIC for USART0 interrupts
    AIC_ConfigureIT(AT91C_ID_US0, AT91C_AIC_PRIOR_LOWEST | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, usart0_interrupt);

    // Enable the interrupt on the interrupt controller
    AIC_EnableIT(AT91C_ID_US0);

    // Enable selected USART0 interrupts
    AT91C_BASE_US0->US_IER = AT91C_US_RXRDY     |
                             AT91C_US_RXBRK     |
                             AT91C_US_OVRE      |
                             AT91C_US_FRAME     |
                             AT91C_US_PARE;
}

/**
 * \brief Configures USART in spi slave mode
 */
static void _ConfigureUsartAsSpiSlave(void)
{
	uint32_t usartMode;

	/* Configure usart slave mode */
	usartMode = 0
		| US_MR_USART_MODE_SPI_SLAVE
		| US_MR_CHRL_8_BIT
		| US_SPI_BPMODE_1;
	PMC_EnablePeripheral(ID_USART);
	USART_SPI_Configure(USART, usartMode, spiClock, BOARD_MCK);

	NVIC_ClearPendingIRQ(USART_IRQn);
	NVIC_SetPriority(USART_IRQn ,1);
	NVIC_EnableIRQ(USART_IRQn);
	USART_DisableIt(USART, 0xffffffff);
}

/*
 * See the serial2.h header file.
 */
xComPortHandle xSerialPortInitMinimal( unsigned long ulWantedBaud, unsigned portBASE_TYPE uxQueueLength )
{
    xComPortHandle xReturn = serHANDLE;
    extern void ( vUART_ISR )( void );
    const Pin xUSART_Pins[] = { BOARD_PIN_USART_RXD, BOARD_PIN_USART_TXD };


    /* Create the queues used to hold Rx and Tx characters. */
    xRxedChars = xQueueCreate( uxQueueLength, ( unsigned portBASE_TYPE ) sizeof( signed char ) );
    xCharsForTx = xQueueCreate( uxQueueLength + 1, ( unsigned portBASE_TYPE ) sizeof( signed char ) );

    /* If the queues were created correctly then setup the serial port
    hardware. */
    if( ( xRxedChars != serINVALID_QUEUE ) && ( xCharsForTx != serINVALID_QUEUE ) )
    {
        portENTER_CRITICAL();
        {
            /* Enable the peripheral clock in the PMC. */
            PMC_EnablePeripheral( BOARD_ID_USART );

            /* Configure the USART. */
            USART_Configure( BOARD_USART_BASE, AT91C_US_CHRL_8_BITS | AT91C_US_PAR_NONE | AT91C_US_NBSTOP_1_BIT, ulWantedBaud, configCPU_CLOCK_HZ );

            /* Configure the interrupt.  Note the pre-emption priority is set
            in bits [8:15] of the priority value passed as the parameter. */
            IRQ_ConfigureIT( BOARD_ID_USART, ( configMAX_SYSCALL_INTERRUPT_PRIORITY << 8 ), vSerialISR );
            IRQ_EnableIT( BOARD_ID_USART );

            /* Enable receiver & transmitter. */
            USART_SetTransmitterEnabled( BOARD_USART_BASE, pdTRUE );
            USART_SetReceiverEnabled( BOARD_USART_BASE, pdTRUE );

            /* Configure IO for USART use. */
            PIO_Configure( xUSART_Pins, PIO_LISTSIZE( xUSART_Pins ) );
        }
        portEXIT_CRITICAL();
    }
    else
    {
        xReturn = ( xComPortHandle ) 0;
    }

    /* This demo file only supports a single port but we have to return
    something to comply with the standard demo header file. */
    return xReturn;
}

// =============================================================================
// 函数功能:__AtTimer_Alloc
//          分配定时器
// 输入参数:timerisr,定时器的中断处理函数
// 输出参数:
// 返回值  :分配的定时器句柄，NULL则分配不成功
// 说明    :
// =============================================================================
ptu32_t __AtTimer_Alloc(fntTimerIsr timerisr)
{
    u8 timerno;
    u8 irqline;
    struct AtTimerHandle  *timer;
    ptu32_t timerhandle;
    //原子操作，防止资源竞争
    atom_low_t  timeratom;
    timeratom = Int_LowAtomStart();

    //寻找空闲的timer
    timerno = __AtTimer_GetFirstZeroBit(gs_dwAtTimerBitmap);
    if(timerno < CN_ATTIMER_NUM)//还有空闲的，则设置标志位
    {
        gs_dwAtTimerBitmap = gs_dwAtTimerBitmap | (CN_ATTIMER_BITMAP_MSK<< timerno);
        Int_LowAtomEnd(timeratom);  //原子操作完毕
    }
    else//没有的话直接返回就可以了，用不着再啰嗦了
    {
        Int_LowAtomEnd(timeratom);   //原子操作完毕
        return NULL;
    }

    PMC_EnablePeripheral(CN_PERI_ID_TC0 + timerno);

    irqline = sgHaltimerIrq[timerno];
    timer = &stgTimerHandle[timerno];
    timer->cycle = 0;
    timer->timerno = timerno;
    timer->irqline = irqline;
    timer->timerstate = CN_TIMER_ENUSE;
    //好了，中断号和定时器号码都有了，该干嘛就干嘛了。
    //先设置好定时器周期
    __AtTimer_PauseCount(timer);
//    __AtTimer_SetCycle(timer,cycle);
    //设置定时器中断,先结束掉该中断所有的关联相关内容
    Int_Register(irqline);
    Int_CutLine(irqline);
    Int_IsrDisConnect(irqline);
    Int_EvttDisConnect(irqline);
    Int_SettoAsynSignal(irqline);
    Int_IsrConnect(irqline, timerisr);
    timerhandle = (ptu32_t)timer;

    return timerhandle;
}

/**
 * \brief An accurate one-to-one comparison is necessary between PSRAM and SMC waveforms for
 *   a complete SMC configuration.
 *  \note The system is running at 48 MHz for the EBI Bus.
 *        Please refer to the "AC Characteristics" section of the customer product datasheet.
 */
extern void BOARD_ConfigurePSRAM( Smc* pSmc )
{
    uint32_t dwTmp ;

    /* Enable peripheral clock */
    PMC_EnablePeripheral( ID_SMC ) ;

    /* Configure SMC, NCS1 is assigned to a external PSRAM */
    /**
     * PSRAM IS66WV51216BLL
     * 55 ns Access time
     * tdoe = 25 ns max
     * SMC1 (timing SAM3S read mode SMC) = 21 ns of setup
     * 21 + 55 = 76 ns => at least 5 cycles at 64 MHz
     * Write pulse width minimum = 45 ns (PSRAM)
     */
    pSmc->SMC_CS_NUMBER[1].SMC_SETUP = 
          SMC_SETUP_NWE_SETUP( 1 )
        | SMC_SETUP_NCS_WR_SETUP( 0 )
        | SMC_SETUP_NRD_SETUP( 2 )
        | SMC_SETUP_NCS_RD_SETUP( 0 ) ;

    pSmc->SMC_CS_NUMBER[1].SMC_PULSE = 
          SMC_PULSE_NWE_PULSE( 3 )
        | SMC_PULSE_NCS_WR_PULSE( 4 )
        | SMC_PULSE_NRD_PULSE( 3 )
        | SMC_PULSE_NCS_RD_PULSE( 5 ) ;

    /* NWE_CYCLE:     The total duration of the write cycle.
       NWE_CYCLE = NWE_SETUP + NWE_PULSE + NWE_HOLD
       = NCS_WR_SETUP + NCS_WR_PULSE + NCS_WR_HOLD
       (tWC) Write Cycle Time min. 70ns
NRD_CYCLE:     The total duration of the read cycle.
NRD_CYCLE = NRD_SETUP + NRD_PULSE + NRD_HOLD
= NCS_RD_SETUP + NCS_RD_PULSE + NCS_RD_HOLD
(tRC) Read Cycle Time min. 70ns. */
    pSmc->SMC_CS_NUMBER[1].SMC_CYCLE = 
          SMC_CYCLE_NWE_CYCLE( 4 )
        | SMC_CYCLE_NRD_CYCLE( 5 ) ;

    dwTmp = SMC->SMC_CS_NUMBER[0].SMC_MODE;
    pSmc->SMC_CS_NUMBER[1].SMC_MODE  = dwTmp
        | SMC_MODE_READ_MODE
        | SMC_MODE_WRITE_MODE;
}

//------------------------------------------------------------------------------
/// Initializes the LCD controller.
/// SMC are configured @ 96MHz for LCD.
/// \param pLcdBase   LCD base address.
//------------------------------------------------------------------------------
void LCDD_Initialize(void)
{
    const Pin pPins[] = {BOARD_LCD_PINS};
    AT91PS_HSMC4_CS pSMC = AT91C_BASE_HSMC4_CS2;
    unsigned int rMode;

    // Enable pins
    PIO_Configure(pPins, PIO_LISTSIZE(pPins));

    // Enable peripheral clock
    PMC_EnablePeripheral(AT91C_ID_HSMC4);

    // EBI SMC Configuration
    pSMC->HSMC4_SETUP = 0
                    | ((1 <<  0) & AT91C_HSMC4_NWE_SETUP)
                    | ((1 <<  8) & AT91C_HSMC4_NCS_WR_SETUP)
                    | ((9 << 16) & AT91C_HSMC4_NRD_SETUP)
                    | ((9 << 24) & AT91C_HSMC4_NCS_RD_SETUP)
                    ;

    pSMC->HSMC4_PULSE = 0
                    | (( 4 <<  0) & AT91C_HSMC4_NWE_PULSE)
                    | (( 4 <<  8) & AT91C_HSMC4_NCS_WR_PULSE)
                    | (( 36 << 16) & AT91C_HSMC4_NRD_PULSE)
                    | (( 36 << 24) & AT91C_HSMC4_NCS_RD_PULSE)
                    ;

    pSMC->HSMC4_CYCLE = 0
                    | ((10 <<  0) & AT91C_HSMC4_NWE_CYCLE)
                    | ((45 << 16) & AT91C_HSMC4_NRD_CYCLE)
                    ;
                 
    rMode = pSMC->HSMC4_MODE & ~(AT91C_HSMC4_DBW | AT91C_HSMC4_READ_MODE | AT91C_HSMC4_WRITE_MODE);
    pSMC->HSMC4_MODE = rMode
                 | (AT91C_HSMC4_READ_MODE)
                 | (AT91C_HSMC4_WRITE_MODE)
                 | (AT91C_HSMC4_DBW_WIDTH_SIXTEEN_BITS)
                 ;
                 
    // Initialize LCD controller (HX8347)
    LCD_Initialize((void *)BOARD_LCD_BASE);

    // Set LCD backlight
    LCDD_SetBacklight(25);
}

/**
 * \brief Starts a SPI master transfer. This is a non blocking function. It will
 *  return as soon as the transfer is started.
 *
 * \param pSpid  Pointer to a Spid instance.
 * \param pCommand Pointer to the SPI command to execute.
 * \returns 0 if the transfer has been started successfully; otherwise returns
 * SPID_ERROR_LOCK is the driver is in use, or SPID_ERROR if the command is not
 * valid.
 */
uint32_t SPID_SendCommand(Spid *pSpid, SpidCmd *pCommand)
{
	Spi *pSpiHw = pSpid->pSpiHw;

	/* Try to get the dataflash semaphore */
	if (pSpid->semaphore == 0)
		return SPID_ERROR_LOCK;

	pSpid->semaphore--;

	/* Enable the SPI Peripheral */
	PMC_EnablePeripheral (pSpid->spiId);

	/* SPI chip select */
	SPI_ChipSelect (pSpiHw, 1 << pCommand->spiCs);

	// Initialize the callback
	pSpid->pCurrentCommand = pCommand;

	/* Initialize DMA controller using channel 0 for RX, 1 for TX. */
	if (_spid_configureDmaChannels(pSpid))
		return SPID_ERROR_LOCK;

	/* Configure and enable interrupt on RC compare */
	NVIC_ClearPendingIRQ(XDMAC_IRQn);
	NVIC_SetPriority(XDMAC_IRQn , 1);
	NVIC_EnableIRQ(XDMAC_IRQn);


	if (_spid_configureLinkList(pSpiHw, pSpid->pXdmad, pCommand))
		return SPID_ERROR_LOCK;

	/* Enables the SPI to transfer and receive data. */
	SPI_Enable (pSpiHw);
	SCB_CleanDCache_by_Addr((uint32_t *)pCommand->pTxBuff, pCommand->TxSize);

	/* Start DMA 0(RX) && 1(TX) */
	if (XDMAD_StartTransfer(pSpid->pXdmad, spiDmaRxChannel))
		return SPID_ERROR_LOCK;

	if (XDMAD_StartTransfer(pSpid->pXdmad, spiDmaTxChannel))
		return SPID_ERROR_LOCK;

	return 0;
}