/**
*
* QSPI bus fill TX FIFO function. 
*
* @param    qspi_bus     - QSPI bus handle
*
* @return   none
*
*****************************************************************************/
static void
qspi_xc7z_fill_tx_fifo(cyg_qspi_xc7z_bus_t    *qspi_bus, int max)
{
    entry_debug();
    cyg_uint32 data = 0;
    cyg_uint32 val  = 0;
    int count = 0;
    HAL_READ_UINT32(qspi_bus->base + XQSPIPS_SR_OFFSET, val);
    while ((!(val & XQSPIPS_IXR_TXFULL_MASK)) && (qspi_bus->us_tx_bytes > 0) && (count < max)) {
    	count++;
	if (qspi_bus->us_tx_bytes < 4) {
	    int tp = qspi_bus->us_tx_bytes;
            qspi_xc7z_copy_write_data(qspi_bus, &data, qspi_bus->us_tx_bytes);
	    if (tp == 1) {
		    HAL_WRITE_UINT32(qspi_bus->base + XQSPIPS_TXD_01_OFFSET, data);
	    } else if (tp == 2) {
		    HAL_WRITE_UINT32(qspi_bus->base + XQSPIPS_TXD_10_OFFSET, data);
	    } else {
		    HAL_WRITE_UINT32(qspi_bus->base + XQSPIPS_TXD_11_OFFSET, data);
	    }
        } else {
            qspi_xc7z_copy_write_data(qspi_bus, &data, 4);
	    HAL_WRITE_UINT32(qspi_bus->base + XQSPIPS_TXD_00_OFFSET, data);

        }
        HAL_READ_UINT32(qspi_bus->base + XQSPIPS_SR_OFFSET,      val);
   }
}

cyg_uint32
mpc5xxx_i2c_isr(cyg_vector_t vector, cyg_addrword_t data)
{

    cyg_uint32     dr;
    HAL_WRITE_UINT32(I2C_0_STATUS_REG, 0x00000000);				// Clear the status register of all interrupts

    if (CYG_MPC5XXX_I2C_XFER_MODE_STARTRX == global_i2c_mode) 	// Sent the address and now read the dummy byte
    {

        HAL_WRITE_UINT32(I2C_0_CONTROL_REG, I2C_BEGIN_RX);
        HAL_READ_UINT32(I2C_0_DATA_REG,dr); 					// Read the dummy byte to initiate next transfer

        global_i2c_mode = CYG_MPC5XXX_I2C_XFER_MODE_RX1;		// Change transfer mode to receive first byte

        resetHigh();			// Set reset pin to High that drains the capacitor to 0V, IS THIS where it goes?
    }

    else if (CYG_MPC5XXX_I2C_XFER_MODE_RX1 == global_i2c_mode) 	// Transmit mode to read first data byte
    {

        HAL_WRITE_UINT32(I2C_0_CONTROL_REG, I2C_SEND_TXAK);

        HAL_READ_UINT32(I2C_0_DATA_REG, byte0);
        byte0 >>= 16;

        global_i2c_mode = CYG_MPC5XXX_I2C_XFER_MODE_RX2;		// Change transfer mode to receive second byte

        resetLow();
    }

void
hasp_mpc5xxx_i2c_init()
{
    initReset();										// initialise the reset pin
    resetHigh();

    cyg_drv_mutex_init(&i2c_lock);
    cyg_drv_cond_init(&i2c_wait, &i2c_lock);
    cyg_drv_interrupt_create(i2c_intr_vector,
                             0,
                             (cyg_addrword_t) 0,
                             &mpc5xxx_i2c_isr,
                             &mpc5xxx_i2c_dsr,
                             &(i2c_interrupt_handle),
                             &(i2c_interrupt_data));
    cyg_drv_interrupt_attach(i2c_interrupt_handle);

    HAL_WRITE_UINT32(I2C_0_FDR_REG, 0x89000000);		// Set clock to 100MHz / 352

    HAL_WRITE_UINT32(I2C_0_ADDRESS_REG, 0x00000000);	// Set MPC5xxx slave address, not useful to us

    HAL_WRITE_UINT32(I2C_0_CONTROL_REG, I2C_ENABLE);	// Enable the I2C device but do not start any transfers and leave interrupts disabled.

    HAL_WRITE_UINT32(I2C_0_STATUS_REG, 0x00000000);		// Clear any pending conditions including interrupts.

    HAL_INTERRUPT_UNMASK(i2c_intr_vector);				// Interrupts can now be safely unmasked

    i2c_flag = 0;

    resetLow();

}

// Internal function to actually configure the hardware to desired baud rate, etc.
static bool
smdk2410_serial_config_port(serial_channel *chan, cyg_serial_info_t *new_config, bool init)
{
    smdk2410_serial_info *smdk2410_chan = (smdk2410_serial_info *)chan->dev_priv;
    CYG_ADDRWORD base = smdk2410_chan->base;
    unsigned short baud_divisor = select_baud[new_config->baud];
    cyg_uint32 _lcr;

    if (baud_divisor == 0) return false;

    if (init) {
        //UART FIFO control register
        HAL_WRITE_UINT32(base+OFS_UFCON, (3<<6) | (3<<4) | (1<<2) | (1<<1) | (1<<0));

        //UART modem control register
        HAL_WRITE_UINT32(base+OFS_UMCON, 0);
    }

    _lcr = select_word_length[new_config->word_length - CYGNUM_SERIAL_WORD_LENGTH_5] | 
        select_stop_bits[new_config->stop] |
        select_parity[new_config->parity];
    HAL_WRITE_UINT32(base+OFS_ULCON, _lcr);

    //UART control register, Enable Rx Timeout Int
    HAL_WRITE_UINT32(base+OFS_UCON, 0x085);

    if (new_config != &chan->config) {
        chan->config = *new_config;
    }
    return true;
}

void hal_delay_us(cyg_int32 usecs)
{
    CYG_ADDRESS pit_base = MAC7100_PIT_BASE;
    cyg_uint32 pit_en;


    // Clear flag
    HAL_WRITE_UINT32(MAC7100_PIT_FLG(pit_base),
                     MAC7100_PIT_FLAG_TIF(CYGNUM_PIT_CHAN_US));

    // Set timer
    HAL_WRITE_UINT32(MAC7100_PIT_TLVAL(pit_base, CYGNUM_PIT_CHAN_US),
                     usecs*CYGNUM_1_US-1);
    HAL_READ_UINT32(MAC7100_PIT_EN(pit_base), pit_en);
    pit_en |= MAC7100_PIT_EN_PEN(CYGNUM_PIT_CHAN_US);
    HAL_WRITE_UINT32(MAC7100_PIT_EN(pit_base), pit_en);

    do {
        HAL_READ_UINT32(MAC7100_PIT_FLG(pit_base), pit_en);
    } while (!(pit_en & MAC7100_PIT_FLAG_TIF(CYGNUM_PIT_CHAN_US)));

    // Disable counter
    HAL_READ_UINT32(MAC7100_PIT_EN(pit_base), pit_en);
    pit_en &= ~MAC7100_PIT_EN_PEN(CYGNUM_PIT_CHAN_US);
    HAL_WRITE_UINT32(MAC7100_PIT_EN(pit_base), pit_en);
}

void hal_clock_initialize(cyg_uint32 period)
{
    CYG_ADDRESS pit_base = MAC7100_PIT_BASE;
    cyg_uint32 pit_en;

    CYG_ASSERT(period < 0x10000, "Invalid clock period");

    // Disable counter
    HAL_READ_UINT32(MAC7100_PIT_EN(pit_base), pit_en);
    pit_en &= ~MAC7100_PIT_EN_PEN(CYGNUM_PIT_CHAN_CLOCK);
    HAL_WRITE_UINT32(MAC7100_PIT_EN(pit_base), pit_en);

    // Set registers
    _period=period;
    HAL_WRITE_UINT32(MAC7100_PIT_TLVAL(pit_base, CYGNUM_PIT_CHAN_CLOCK),
                     period);

    // Start timer
    pit_en |= MAC7100_PIT_EN_PEN(CYGNUM_PIT_CHAN_CLOCK);
    HAL_WRITE_UINT32(MAC7100_PIT_EN(pit_base), pit_en);

    // Enable timer interrupt
    HAL_READ_UINT32(MAC7100_PIT_INTEN(pit_base), pit_en);
    pit_en |= MAC7100_PIT_EN_PEN(CYGNUM_PIT_CHAN_CLOCK);
    HAL_WRITE_UINT32(MAC7100_PIT_INTEN(pit_base), pit_en);

    HAL_READ_UINT32(MAC7100_PIT_INTSEL(pit_base), pit_en);
    pit_en |= MAC7100_PIT_EN_PEN(CYGNUM_PIT_CHAN_CLOCK);
    HAL_WRITE_UINT32(MAC7100_PIT_INTSEL(pit_base), pit_en);
}

void init_timer( cyg_uint32 base, cyg_uint32 interval )
{
    cyg_uint32 period = hal_stm32_pclk1;

    if( base == CYGHWR_HAL_STM32_TIM1 || base == CYGHWR_HAL_STM32_TIM8 )
    {
        period = hal_stm32_pclk2;
        if( CYGHWR_HAL_CORTEXM_STM32_CLOCK_PCLK2_DIV != 1 )
            period *= 2;
    }
    else
    {
        if( CYGHWR_HAL_CORTEXM_STM32_CLOCK_PCLK1_DIV != 1 )
            period *= 2;
    }

    period = period / 1000000;
    
    HAL_WRITE_UINT32(base+CYGHWR_HAL_STM32_TIM_PSC, period-1 );

    HAL_WRITE_UINT32(base+CYGHWR_HAL_STM32_TIM_CR2, 0 );

    HAL_WRITE_UINT32(base+CYGHWR_HAL_STM32_TIM_DIER, CYGHWR_HAL_STM32_TIM_DIER_UIE );

    HAL_WRITE_UINT32(base+CYGHWR_HAL_STM32_TIM_ARR, interval );
    
    HAL_WRITE_UINT32(base+CYGHWR_HAL_STM32_TIM_CR1, CYGHWR_HAL_STM32_TIM_CR1_CEN);
}

void hal_clock_initialize(cyg_uint32 period)
{
    cyg_uint32 tmod;

    // Disable timer 0
    HAL_READ_UINT32(E7T_TMOD, tmod);
    tmod &= ~(E7T_TMOD_TE0);
    HAL_WRITE_UINT32(E7T_TMOD, 0);

    tmod &= ~(E7T_TMOD_TMD0 | E7T_TMOD_TCLR0);
    tmod |= E7T_TMOD_TE0;

    // Set counter
    HAL_WRITE_UINT32(E7T_TDATA0, period);

    // And enable timer
    HAL_WRITE_UINT32(E7T_TMOD, tmod);

    _period = period;

#ifdef CYGDBG_HAL_DEBUG_GDB_BREAK_SUPPORT
    cyg_drv_interrupt_create(CYGNUM_HAL_INTERRUPT_EXT0,
                             99,           // Priority
                             0,            // Data item passed to interrupt handler
                             e7t_abort_isr,
                             0,
                             &abort_interrupt_handle,
                             &abort_interrupt);
    cyg_drv_interrupt_attach(abort_interrupt_handle);
    cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_EXT0);
#endif
}

static cyg_uint32 phy_read_register(cyg_uint32 address, cyg_uint8 reg)
{
	cyg_uint32 retVal;
	cyg_uint32 miimcom, miimind;
	int i;

	HAL_WRITE_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMADD , ((((cyg_uint32)address) << 8) | reg));

	HAL_READ_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMCOM , miimcom);
	miimcom &= ~MIIMCOM_READ;
	HAL_WRITE_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMCOM , miimcom);
	miimcom |= MIIMCOM_READ;
	HAL_WRITE_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMCOM , miimcom);

	HAL_READ_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMIND , miimind);
	i = 0;
	while ((miimind & MIIMIND_BUSY) == MIIMIND_BUSY && i++ < 500)
	{
//		os_printf(".");
		HAL_DELAY_US(10000);
		HAL_READ_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMIND , miimind);
	}
	//status register
	HAL_READ_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMSTAT , retVal);
	//			phy_state = qi->regs->miimstat;
	HAL_READ_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMCOM , miimcom);
	miimcom &= ~MIIMCOM_READ;
	HAL_WRITE_UINT32( CYGARC_IMM_BASE + CYGARC_REG_IMM_TSEC1_MIIMCOM , miimcom);
	return retVal;
}

externC void
_mb93091_pci_cfg_write_uint32(int bus, int devfn, int offset, cyg_uint32 cfg_val)
{
    cyg_uint32 cfg_addr, addr, status;

    if (!_mb93091_has_vdk)
	    return;
#ifdef CYGPKG_IO_PCI_DEBUG
    diag_printf("%s(bus=%x, devfn=%x, offset=%x, val=%x)\n", __FUNCTION__, bus, devfn, offset, cfg_val);
#endif // CYGPKG_IO_PCI_DEBUG
    if ((bus == 0) && (CYG_PCI_DEV_GET_DEV(devfn) == 0)) {
        // PCI bridge
        addr = _MB93091_PCI_CONFIG + (offset << 1);
    } else {
        cfg_addr = _cfg_addr(bus, devfn, offset);
        HAL_WRITE_UINT32(_MB93091_PCI_CONFIG_ADDR, cfg_addr);
        addr = _MB93091_PCI_CONFIG_DATA;
    }
    HAL_WRITE_UINT32(addr, cfg_val);
    HAL_READ_UINT16(_MB93091_PCI_STAT_CMD, status);
    if (status & _MB93091_PCI_STAT_ERROR_MASK) {
        // Cycle failed - clean up and get out
        HAL_WRITE_UINT16(_MB93091_PCI_STAT_CMD, status & _MB93091_PCI_STAT_ERROR_MASK);
    }
    HAL_WRITE_UINT32(_MB93091_PCI_CONFIG_ADDR, 0);
}

externC cyg_uint16 
_mb93091_pci_cfg_read_uint16(int bus, int devfn, int offset)
{
    cyg_uint32 cfg_addr, addr, status;
    cyg_uint16 cfg_val = (cyg_uint16)0xFFFF;

    if (!_mb93091_has_vdk)
	    return cfg_val;
#ifdef CYGPKG_IO_PCI_DEBUG
    diag_printf("%s(bus=%x, devfn=%x, offset=%x) = ", __FUNCTION__, bus, devfn, offset);
#endif // CYGPKG_IO_PCI_DEBUG
    if ((bus == 0) && (CYG_PCI_DEV_GET_DEV(devfn) == 0)) {
        // PCI bridge
        addr = _MB93091_PCI_CONFIG + ((offset << 1) ^ 0x02);
    } else {
        cfg_addr = _cfg_addr(bus, devfn, offset ^ 0x02);
        HAL_WRITE_UINT32(_MB93091_PCI_CONFIG_ADDR, cfg_addr);
        addr = _MB93091_PCI_CONFIG_DATA + ((offset & 0x03) ^ 0x02);
    }
    HAL_READ_UINT16(addr, cfg_val);
    HAL_READ_UINT16(_MB93091_PCI_STAT_CMD, status);
    if (status & _MB93091_PCI_STAT_ERROR_MASK) {
        // Cycle failed - clean up and get out
        cfg_val = (cyg_uint16)0xFFFF;
        HAL_WRITE_UINT16(_MB93091_PCI_STAT_CMD, status & _MB93091_PCI_STAT_ERROR_MASK);
    }
#ifdef CYGPKG_IO_PCI_DEBUG
    diag_printf("%x\n", cfg_val);
#endif // CYGPKG_IO_PCI_DEBUG
    HAL_WRITE_UINT32(_MB93091_PCI_CONFIG_ADDR, 0);
    return cfg_val;
}

void hal_diag_led(int x)
{
  HAL_WRITE_UINT32(HAL_DISPLAY_LEDBAR, x);
#if !defined(CYG_KERNEL_DIAG_LCD)
  HAL_WRITE_UINT32(HAL_DISPLAY_ASCIIWORD, x);
#endif
}

static void
hal_stm32_serial_init_channel(void* __ch_data)
{
    channel_data_t *chan = (channel_data_t*)__ch_data;
    CYG_ADDRESS base = chan->base;
    cyg_uint32 cr1, cr2;

    // Enable the PIO lines for the serial channel

    CYGHWR_HAL_STM32_GPIO_SET( chan->rxpin );
    CYGHWR_HAL_STM32_GPIO_SET( chan->txpin );

    cr2 = CYGHWR_HAL_STM32_UART_CR2_STOP_1;
    HAL_WRITE_UINT32( base+CYGHWR_HAL_STM32_UART_CR2, cr2 );

    cr1 = CYGHWR_HAL_STM32_UART_CR1_M_8;
    cr1 |= CYGHWR_HAL_STM32_UART_CR1_TE | CYGHWR_HAL_STM32_UART_CR1_RE;
    HAL_WRITE_UINT32( base+CYGHWR_HAL_STM32_UART_CR1, cr1 );
//while(1)HAL_WRITE_UINT32 (CYGHWR_HAL_STM32_GPIOC+CYGHWR_HAL_STM32_GPIO_BSRR,0x000040);
    // Set up Baud rate
    chan->baud_rate = CYGNUM_HAL_VIRTUAL_VECTOR_CONSOLE_CHANNEL_BAUD;
  //  while(1)HAL_WRITE_UINT32 (CYGHWR_HAL_STM32_GPIOC+CYGHWR_HAL_STM32_GPIO_BSRR,0x000040);
    hal_stm32_uart_setbaud( base, chan->baud_rate );

    // Enable the uart
    cr1 |= CYGHWR_HAL_STM32_UART_CR1_UE;
    HAL_WRITE_UINT32( base+CYGHWR_HAL_STM32_UART_CR1, cr1 );    

}

//==========================================================================
// This function is called from the generic ADC package to enable the
// channel in response to a CYG_IO_SET_CONFIG_ADC_ENABLE config operation.
// It should take any steps needed to start the channel generating samples
//==========================================================================
static void at91_adc_enable(cyg_adc_channel *chan)
{
    at91_adc_info *info      = chan->device->dev_priv;

    // Enable the channel
    HAL_WRITE_UINT32((info->adc_base + AT91_ADC_CHER), \
                      AT91_ADC_CHER_CHx(chan->channel));

    //
    // Unmask interrupt as soon as 1 channel is enable
    //
    if (!info->chan_mask)
    {
       cyg_drv_interrupt_unmask(info->timer_vector);

       // Enable timer interrupt
       HAL_WRITE_UINT32(info->tc_base+AT91_TC_IER, AT91_TC_IER_CPC);

       // Enable the clock
       HAL_WRITE_UINT32(info->tc_base+AT91_TC_CCR, AT91_TC_CCR_TRIG | AT91_TC_CCR_CLKEN);

       // Start timer
       HAL_WRITE_UINT32(info->tc_base+AT91_TC_CCR,  AT91_TC_CCR_TRIG);

       // Start ADC sampling
       HAL_WRITE_UINT32((info->adc_base + AT91_ADC_CR), AT91_ADC_CR_START);

    }

    info->chan_mask |= AT91_ADC_CHER_CHx(chan->channel);

}

// There has been a change in state. Update the end point.
static void
usbs_state_notify (usbs_control_endpoint * pcep)
{
  static int old_state = USBS_STATE_CHANGE_POWERED;
  int state = pcep->state & USBS_STATE_MASK;
  
  if (pcep->state != old_state) {
    usbs_end_all_transfers (-EPIPE);
    switch (state) {
      case USBS_STATE_DETACHED:
      case USBS_STATE_ATTACHED:
      case USBS_STATE_POWERED:
        // Nothing to do
        break;
      case USBS_STATE_DEFAULT:
        HAL_WRITE_UINT32 (AT91_UDP + AT91_UDP_GLB_STATE, 0);
        break;
      case USBS_STATE_ADDRESSED:
        HAL_WRITE_UINT32 (AT91_UDP + AT91_UDP_GLB_STATE, AT91_UDP_GLB_FADDEN);
        break;
      case USBS_STATE_CONFIGURED:
        HAL_WRITE_UINT32 (AT91_UDP + AT91_UDP_GLB_STATE, AT91_UDP_GLB_CONFG);
        break;
      default:
        CYG_FAIL("Unknown endpoint state");
    }
    
    if (pcep->state_change_fn) {
      (*pcep->state_change_fn) (pcep, 0, pcep->state, old_state);
    }
    
    old_state = pcep->state;
  }
}

void
usbs_at91_endpoint_init (usbs_rx_endpoint * pep, cyg_uint8 endpoint_type,
                         cyg_bool enable)
{  
  int epn = usbs_at91_pep_to_number(pep);
  cyg_addrword_t pCSR = pCSRn(epn);
  
  CYG_ASSERT (AT91_USB_ENDPOINTS > epn, "Invalid end point");
  
  usbs_at91_endpoint_interrupt_enable (epn, false);
  /* Reset endpoint */
  HAL_WRITE_UINT32 (AT91_UDP + AT91_UDP_RST_EP, 1 << epn);      
  HAL_WRITE_UINT32 (AT91_UDP + AT91_UDP_RST_EP, 0);
  
  pep->halted = false;

  /* Type | In */
  HAL_WRITE_UINT32 (pCSR, (((((cyg_uint32) endpoint_type) & 0x03) << 8) | 
                           ((((cyg_uint32) endpoint_type) & 0x80) << 3)));

  usbs_at91_endpoint_bytes_in_fifo[epn] = 0;
  usbs_at91_endpoint_bytes_received[epn] = THERE_IS_A_NEW_PACKET_IN_THE_UDP;
  usbs_at91_endpoint_bank1[epn] = false;

  if (enable) {
    SET_BITS (pCSR, AT91_UDP_CSR_EPEDS);
  }
}

void 
cyg_hal_plf_pci_cfg_write_byte (cyg_uint32 bus, cyg_uint32 devfn,
                                cyg_uint32 offset, cyg_uint8  data)
{
    cyg_uint32 config_dword, shift;

    HAL_WRITE_UINT32(CYGARC_REG_PCI_CFG_ADDR,
                     CYGARC_REG_PCI_CFG_ADDR_ENABLE |
                     (bus << CYGARC_REG_PCI_CFG_ADDR_BUSNO_shift) |
                     (devfn << CYGARC_REG_PCI_CFG_ADDR_FUNC_shift) |
                     (offset & ~3));
    HAL_WRITE_UINT32(CYGARC_REG_PCI_CFG_CMD, CYGARC_REG_PCI_CFG_CMD_RCFG);
    _DELAY();
    HAL_READ_UINT32(CYGARC_REG_PCI_CFG_DATA, config_dword);

    shift = (offset & 3) * 8;
    config_dword &= ~(0xff << shift);
    config_dword |= (data << shift);

    HAL_WRITE_UINT32(CYGARC_REG_PCI_CFG_ADDR,
                     CYGARC_REG_PCI_CFG_ADDR_ENABLE |
                     (bus << CYGARC_REG_PCI_CFG_ADDR_BUSNO_shift) |
                     (devfn << CYGARC_REG_PCI_CFG_ADDR_FUNC_shift) |
                     (offset & ~3));
    HAL_WRITE_UINT32(CYGARC_REG_PCI_CFG_DATA, config_dword);
    _DELAY();
    HAL_WRITE_UINT32(CYGARC_REG_PCI_CFG_CMD, CYGARC_REG_PCI_CFG_CMD_WCFG);
    _DELAY();
}

void
cyg_hal_sh_pcic_pci_io_write_dword (cyg_uint32 addr, cyg_uint32 data)
{
    HAL_WRITE_UINT32(CYGARC_REG_PCIC_IOBR, addr & CYGARC_REG_PCIC_IOBR_MASK);
    HAL_WRITE_UINT32(CYGARC_REG_PCIC_IO_BASE + (addr & CYGARC_REG_PCIC_IO_BASE_MASK),
                     data);
}

// Profiling timer setup
int hal_enable_profile_timer(int resolution)
{
    cyg_uint32 period;

    // Calculate how many timer ticks the requested resolution in
    // microseconds equates to. We do this calculation in 64 bit
    // arithmetic to avoid overflow.
    period = (cyg_uint32)((((cyg_uint64)resolution) *
        ((cyg_uint64)CYGNUM_HAL_ARM_AT91_CLOCK_SPEED))/32000000LL);

    CYG_ASSERT(period < 0x10000, "Invalid profile timer resolution"); // 16 bits only

    // Attach ISR
    HAL_INTERRUPT_ATTACH(HAL_INTERRUPT_PROFILE, &profile_isr, 0x1111, 0);
    HAL_INTERRUPT_UNMASK(HAL_INTERRUPT_PROFILE);

    // Disable counter
    HAL_WRITE_UINT32(AT91_TC+AT91_TC_PROFILE+AT91_TC_CCR, AT91_TC_CCR_CLKDIS);

    // Set registers
    HAL_WRITE_UINT32(AT91_TC+AT91_TC_PROFILE+AT91_TC_CMR, AT91_TC_CMR_CPCTRG | // Reset counter on CPC
                                                          AT91_TC_CMR_CLKS_MCK32); // Use MCLK/32
    HAL_WRITE_UINT32(AT91_TC+AT91_TC_PROFILE+AT91_TC_RC, period);

    // Start timer
    HAL_WRITE_UINT32(AT91_TC+AT91_TC_PROFILE+AT91_TC_CCR, AT91_TC_CCR_TRIG | AT91_TC_CCR_CLKEN);

    // Enable timer interrupt
    HAL_WRITE_UINT32(AT91_TC+AT91_TC_PROFILE+AT91_TC_IER, AT91_TC_IER_CPC);

    return resolution;
}

static void
spi_at91_start_transfer(cyg_spi_at91_device_t *dev)
{
    cyg_spi_at91_bus_t *spi_bus = (cyg_spi_at91_bus_t *)dev->spi_device.spi_bus;
  
    if (spi_bus->cs_up)
        return;
 
    // Force minimal delay between two transfers - in case two transfers
    // follow each other w/o delay, then we have to wait here in order for
    // the peripheral device to detect cs transition from inactive to active. 
    CYGACC_CALL_IF_DELAY_US(dev->tr_bt_udly);
    
    // Raise CS

#ifdef CYGHWR_DEVS_SPI_ARM_AT91_PCSDEC
    HAL_WRITE_UINT32(AT91_SPI_PIO+AT91_PIO_CODR, 
                     AT91_SPI_PIO_NPCS(~dev->dev_num));
#else
    HAL_WRITE_UINT32(AT91_SPI_PIO+AT91_PIO_CODR,
                     AT91_SPI_PIO_NPCS((~(1<<dev->dev_num)) & 0x0F));
#endif
    CYGACC_CALL_IF_DELAY_US(dev->cs_up_udly);
   
    spi_bus->cs_up = true;
}