static void timerIRQHandler(uint32_t token) {
	//dump_memory(0x3e200000, 0x100);

	/* this function does not implement incrementing a counter at dword_18022B28 like Apple's */
	uint32_t stat = GET_REG(TIMER + TIMER_IRQSTAT);

	/* signal timer is being handled */
	volatile register uint32_t discard = GET_REG(TIMER + TIMER_IRQLATCH); discard --;

	if(stat & TIMER_SPECIALTIMER_BIT0) {
		SET_REG(TIMER + TIMER_UNKREG0, GET_REG(TIMER + TIMER_UNKREG0) | TIMER_SPECIALTIMER_BIT0);
	}

	if(stat & TIMER_SPECIALTIMER_BIT1) {
		SET_REG(TIMER + TIMER_UNKREG0, GET_REG(TIMER + TIMER_UNKREG0) | TIMER_SPECIALTIMER_BIT1);
	}

	int i;
	for(i = TIMER_Separator; i < NUM_TIMERS; i++) {
		callTimerHandler(i, stat >> (8 * (NUM_TIMERS - i - 1)));
	}

	/* signal timer has been handled */
	SET_REG(TIMER + TIMER_IRQLATCH, stat);
}

void setupDisconnectPin (void) {
  /* enable USB DISC Pin */
#ifdef STM32F2
#if !defined(BOARD_DiscoveryF4)
  pRCC->AHB1ENR |= RCC_AHB1ENR_USB;


	GPIO_TypeDef *p = (GPIO_TypeDef *)USB_DISC_BANK;
  /* set to output */
  p->MODER |= 1 << (2*USB_DISC);
  /* Configure pins speed to 100 MHz */
  //p->OSPEEDR = 0xffffc00f; // 2 bit/port medium speed = %01
  /* Configure pins Output type to open drain */
  p->OTYPER  |= 1 << (1*USB_DISC);
  /* No pull-up, pull-down for PEx pins */
  //p->PUPDR   = 0x00000000;
#endif
#else
  pRCC->APB2ENR |= RCC_APB2ENR_USB;
  /* Setup USB DISC pin as output open drain */
  #ifdef USB_DISC_OD
  	SET_REG(USB_DISC_CR, (GET_REG(USB_DISC_CR) & USB_DISC_CR_MASK) | USB_DISC_CR_OUTPUT_OD); // ala42 just pull down external pull up
  #else
  	SET_REG(USB_DISC_CR, (GET_REG(USB_DISC_CR) & USB_DISC_CR_MASK) | USB_DISC_CR_OUTPUT_PP); // ala42 actively pull up USBDP
  #endif
#endif
}

int dma_cancel(int channel) {

	//bufferPrintf("cdma: dma_cancel.\r\n");

	DMAInfo* dma = &dmaInfo[channel];
	uint64_t startTime = timer_get_system_microtime();

	if (!dma->signalled)
		return channel;

	dma_channel_activate(channel, 1);

	uint32_t channel_reg = DMA + (channel << 12);
	if (GET_BITS(GET_REG(channel_reg), 16, 2) == 1) {
		SET_REG(channel_reg, 4);

		while (GET_BITS(GET_REG(channel_reg), 16, 2) == 1) {
			if (has_elapsed(startTime, 10000))
				system_panic("CDMA: channel %d timeout during abort\r\n", channel);
		}

		SET_REG(channel_reg, 2);
	}

	dma->signalled = 1;
	dma_set_aes(channel, 0);

	return dma_channel_activate(channel, 0);
}

int spi_tx(int port, const uint8_t* buffer, int len, int block, int unknown) {
	if(port > (NUM_SPIPORTS - 1)) {
		return -1;
	}

	SET_REG(SPIRegs[port].control, GET_REG(SPIRegs[port].control) | (1 << 2));
	SET_REG(SPIRegs[port].control, GET_REG(SPIRegs[port].control) | (1 << 3));

	spi_info[port].txBuffer = buffer;

	if(len > MAX_TX_BUFFER)
		spi_info[port].txCurrentLen = MAX_TX_BUFFER;
	else
		spi_info[port].txCurrentLen = len;

	spi_info[port].txTotalLen = len;
	spi_info[port].txDone = FALSE;

	if(unknown == 0) {
		SET_REG(SPIRegs[port].cnt, 0);
	}

	spi_txdata(port, buffer, 0, spi_info[port].txCurrentLen);

	SET_REG(SPIRegs[port].control, 1);

	if(block) {
		while(!spi_info[port].txDone || GET_BITS(GET_REG(SPIRegs[port].status), 4, 4) != 0) {
			// yield
		}
		return len;
	} else {
		return 0;
	}
}

void setupLED(void) {
#if defined(ARCH_SI32)
    // Enable Crossbar 1 to use LED
    SET_REG(XBAR1_SET, 0x80000000);
    SET_REG(XBAR0H_SET, 0x80000000);

    // Set LED 3 as push pull output
    SET_REG(GPIO_OUTMD_SET(PORT_BANK3), 0x00000001);
    SET_REG(GPIO_MDSEL_SET(PORT_BANK3), 0x00000001);

#elif defined(ARC_STM32)
    // todo, swap out hardcoded pin/bank with macro
    u32 rwmVal; /* read-write-modify place holder var */

    /* Setup APB2 (GPIOA) */
    rwmVal =  GET_REG(RCC_APB2ENR);
    rwmVal |= 0x00000004;
    SET_REG(RCC_APB2ENR, rwmVal);

    /* Setup GPIOA Pin 5 as PP Out */
    SET_REG(GPIO_CRL(GPIOA), 0x00100000);

    rwmVal =  GET_REG(GPIO_CRL(GPIOA));
    rwmVal &= 0xFF0FFFFF;
    rwmVal |= 0x00100000;
    SET_REG(GPIO_CRL(GPIOA), rwmVal);

    setPin(GPIOA, 5);
#endif // defined(ARCH_SI32)
}

int gpio_get_value(rt_uint32_t gpio)
{
    rt_uint32_t tmp, base, offset;

    if (!gpio_available[gpio])
    {
        rt_kprintf("ERROR: %s, GPIO %d is not available\n", __func__, gpio);
        return -RT_EBUSY;
    }

    offset = gpio % 32;
    base   = gpio_to_base(gpio);

    tmp = GET_REG(base + REG_GPIO_SWPORTA_DDR);
    tmp &= BIT(offset);

    if (tmp)
    {
        tmp = GET_REG(base + REG_GPIO_SWPORTA_DR);
    }
    else
    {
        tmp = GET_REG(base + REG_GPIO_EXT_PORTA);
    }
    tmp &= BIT(offset);
    tmp = tmp >> offset;
    return tmp;
}

void updateScreen() {
	ili9340_setBackColor(0x0000);
	ili9340_setFrontColor(0xFFFF);
	//Draw info
	itoa(GET_REG(REG_ACC), buff, 10);
	ili9340_fillRect(     BOX_SPLIT + 8, BOX_TOP + INFO_HEIGHT*0 + 16, 8*4, 13, 0x0000);
	ili9340_drawString(   BOX_SPLIT + 8, BOX_TOP + INFO_HEIGHT*0 + 16, buff);
	itoa(GET_REG(REG_BAK), buff, 10);
	ili9340_fillRect(     BOX_SPLIT + 8, BOX_TOP + INFO_HEIGHT*1 + 16, 8*4, 13, 0x0000);
	ili9340_drawString(   BOX_SPLIT + 8, BOX_TOP + INFO_HEIGHT*1 + 16, buff);
	//TODO: LAST: Last side used when ANY
	//TODO: MODE: Current Running mode (IDLE, RUN, WRITING, READING)
	//IDLE PERCENTAGE
	itoa(0, buff, 10);
	ili9340_fillRect(     BOX_SPLIT + 8, BOX_TOP + INFO_HEIGHT*4 + 16, 8*4, 13, 0x0000);
	ili9340_drawString(   BOX_SPLIT + 8, BOX_TOP + INFO_HEIGHT*4 + 16, buff);

	//Draw instruction
	for(uint8_t i = 0; i < 15; i++) {
		if(i != lastPC && i != GET_REG(REG_PC))
			continue;
		drawInstr(i);
	}
	lastPC = GET_REG(REG_PC);
}

int spi_rx(int port, uint8_t* buffer, int len, int block, int unknown) {
	if(port >= (NUM_SPIPORTS - 1)) {
		return -1;
	}

	SET_REG(SPIRegs[port].control, GET_REG(SPIRegs[port].control) | (1 << 2));
	SET_REG(SPIRegs[port].control, GET_REG(SPIRegs[port].control) | (1 << 3));

	spi_info[port].rxBuffer = buffer;
	spi_info[port].rxDone = FALSE;
	spi_info[port].rxCurrentLen = 0;
	spi_info[port].rxTotalLen = len;
	spi_info[port].counter = 0;

	SET_REG(SPIRegs[port].unkReg2, len);
	SET_REG(SPIRegs[port].control, 1);

	if(unknown == 0) {
		SET_REG(SPIRegs[port].setup, GET_REG(SPIRegs[port].setup) | 1);
	}

	if(block) {
		while(!spi_info[port].rxDone) {
			// yield
		}
		if(unknown == 0) {
			SET_REG(SPIRegs[port].setup, GET_REG(SPIRegs[port].setup) & ~1);
		}
		return len;
	} else {
		return 0;
	}
}

int usb_start(USBEnumerateHandler hEnumerate, USBStartHandler hStart) {
	enumerateHandler = hEnumerate;
	startHandler = hStart;
	currentlySending = 0xFF;

	if(txQueue == NULL)
		txQueue = createRingBuffer(TX_QUEUE_LEN);

	initializeDescriptors();

	if(controlSendBuffer == NULL)
		controlSendBuffer = memalign(DMA_ALIGN, CONTROL_SEND_BUFFER_LEN);

	if(controlRecvBuffer == NULL)
		controlRecvBuffer = memalign(DMA_ALIGN, CONTROL_RECV_BUFFER_LEN);

	SET_REG(USB + GAHBCFG, GAHBCFG_DMAEN | GAHBCFG_BSTLEN_INCR8 | GAHBCFG_MASKINT);
	SET_REG(USB + GUSBCFG, GUSBCFG_PHYIF16BIT | GUSBCFG_SRPENABLE | GUSBCFG_HNPENABLE | ((5 & GUSBCFG_TURNAROUND_MASK) << GUSBCFG_TURNAROUND_SHIFT));
	SET_REG(USB + DCFG, DCFG_HISPEED); // some random setting. See specs
	SET_REG(USB + DCFG, GET_REG(USB + DCFG) & ~(DCFG_DEVICEADDRMSK));
	InEPRegs[0].control = USB_EPCON_ACTIVE;
	OutEPRegs[0].control = USB_EPCON_ACTIVE;

	SET_REG(USB + GRXFSIZ, RX_FIFO_DEPTH);
	SET_REG(USB + GNPTXFSIZ, (TX_FIFO_DEPTH << GNPTXFSIZ_DEPTH_SHIFT) | TX_FIFO_STARTADDR);

	int i;
	for(i = 0; i < USB_NUM_ENDPOINTS; i++) {
		InEPRegs[i].interrupt = USB_EPINT_INEPNakEff | USB_EPINT_INTknEPMis | USB_EPINT_INTknTXFEmp
			| USB_EPINT_TimeOUT | USB_EPINT_AHBErr | USB_EPINT_EPDisbld | USB_EPINT_XferCompl;
		OutEPRegs[i].interrupt = USB_EPINT_OUTTknEPDis
			| USB_EPINT_SetUp | USB_EPINT_AHBErr | USB_EPINT_EPDisbld | USB_EPINT_XferCompl;
	
		InEPRegs[i].control = (InEPRegs[i].control & ~(DCTL_NEXTEP_MASK << DCTL_NEXTEP_SHIFT));
	}

	SET_REG(USB + GINTMSK, GINTMSK_OTG | GINTMSK_SUSPEND | GINTMSK_RESET | GINTMSK_INEP | GINTMSK_OEP | GINTMSK_DISCONNECT);
	SET_REG(USB + DAINTMSK, DAINTMSK_ALL);

	SET_REG(USB + DOEPMSK, USB_EPINT_XferCompl | USB_EPINT_SetUp | USB_EPINT_Back2BackSetup);
	SET_REG(USB + DIEPMSK, USB_EPINT_XferCompl | USB_EPINT_AHBErr | USB_EPINT_TimeOUT);

	InEPRegs[0].interrupt = USB_EPINT_ALL;
	OutEPRegs[0].interrupt = USB_EPINT_ALL;

	SET_REG(USB + DCTL, DCTL_PROGRAMDONE + DCTL_CGOUTNAK + DCTL_CGNPINNAK);
	udelay(USB_PROGRAMDONE_DELAYUS);
	SET_REG(USB + GOTGCTL, GET_REG(USB + GOTGCTL) | GOTGCTL_SESSIONREQUEST);

	receiveControl(controlRecvBuffer, sizeof(USBSetupPacket));

	change_state(USBPowered);

	interrupt_enable(USB_INTERRUPT);

	return 0;
}

static void delay_cycles(unsigned long cycles)
{

    unsigned long now, start;
    SET_REG(P4A_TIMER2_CTRL_REG,GET_REG(P4A_TIMER2_CTRL_REG)|TIME_RESETCNT);
    now = start = GET_REG(P4A_TIMER2_CNT_REG);

    while ((now - start) < cycles)
        now = GET_REG(P4A_TIMER2_CNT_REG);
}

void usb_phy_shutdown()
{
	SET_REG(USB_PHY + OPHYPWR, OPHYPWR_FORCESUSPEND | OPHYPWR_PLLPOWERDOWN
		| OPHYPWR_XOPOWERDOWN | OPHYPWR_ANALOGPOWERDOWN | OPHYPWR_UNKNOWNPOWERDOWN); // power down phy
	
	SET_REG(USB_PHY + ORSTCON, GET_REG(USB_PHY + ORSTCON) | ORSTCON_PHYSWRESET);

	udelay(USB_RESET_DELAYUS);	// wait a millisecond for the changes to stick

	SET_REG(USB_PHY + OPHYUNK1, GET_REG(USB_PHY + OPHYUNK1) &~ OPHYUNK1_STOP_MASK);
}

static int si3054_switch_put(struct snd_kcontrol *kcontrol,
		               struct snd_ctl_elem_value *uvalue)
{
	struct hda_codec *codec = snd_kcontrol_chip(kcontrol);
	u16 reg  = PRIVATE_REG(kcontrol->private_value);
	u16 mask = PRIVATE_MASK(kcontrol->private_value);
	if (uvalue->value.integer.value[0])
		SET_REG_CACHE(codec, reg, (GET_REG(codec, reg)) | mask);
	else
		SET_REG_CACHE(codec, reg, (GET_REG(codec, reg)) & ~mask);
	return 0;
}

int power_ctrl(uint32_t device, OnOff on_off) {
	if(on_off == ON) {
		SET_REG(POWER + POWER_ONCTRL, device);
	} else {
		SET_REG(POWER + POWER_OFFCTRL, device);
	}

	/* wait for the new state to take effect */
	while((GET_REG(POWER + POWER_SETSTATE) & device) != (GET_REG(POWER + POWER_STATE) & device));

	return 0;
}

int gpio_set_irq_type(rt_uint32_t gpio, rt_uint32_t type)
{
    rt_uint32_t int_type, int_polarity;
    rt_uint32_t bit = gpio % 32;
    rt_uint32_t base;

    if (!gpio_available[gpio])
    {
        rt_kprintf("ERROR: %s, GPIO %d is not available\n", __func__, gpio);
        return -RT_EBUSY;
    }

    base = gpio_to_base(gpio);

    int_type     = GET_REG(base + REG_GPIO_INTTYPE_LEVEL);
    int_polarity = GET_REG(base + REG_GPIO_INT_POLARITY);

    switch (type & IRQ_TYPE_TRIGGER_MASK)
    {
    case IRQ_TYPE_EDGE_BOTH:
        int_type |= BIT(bit);
        // toggle trigger
        if (gpio_get_value(gpio))
            int_polarity &= ~BIT(bit);
        else
            int_polarity |= BIT(bit);
        break;
    case IRQ_TYPE_EDGE_RISING:
        int_type |= BIT(bit);
        int_polarity |= BIT(bit);
        break;
    case IRQ_TYPE_EDGE_FALLING:
        int_type |= BIT(bit);
        int_polarity &= ~BIT(bit);
        break;
    case IRQ_TYPE_LEVEL_HIGH:
        int_type &= ~BIT(bit);
        int_polarity |= BIT(bit);
        break;
    case IRQ_TYPE_LEVEL_LOW:
        int_type &= ~BIT(bit);
        int_polarity &= ~BIT(bit);
        break;
    case IRQ_TYPE_NONE:
        return 0;
    default:
        return -RT_ERROR;
    }
    SET_REG(base + REG_GPIO_INTTYPE_LEVEL, int_type);
    SET_REG(base + REG_GPIO_INT_POLARITY, int_polarity);
    return 0;
}

/*********************************************************************************************************
** 函数名称: sio16c550PollInput
** 功能描述: 串口接口查询方式输入
** 输　入  : pchan          SIO CHAN
**           pc             读取到的数据
** 输　出  : ERROR or OK
** 全局变量: 
** 调用模块: 
*********************************************************************************************************/
static INT sio16c550PollInput (SIO16C550_CHAN *psiochan, CHAR *pc)
{
    UINT8 poll_status = GET_REG(psiochan, LSR);

    if ((poll_status & LSR_DR) == 0x00) {
        _ErrorHandle(EAGAIN);
        return  (PX_ERROR);
    }

    *pc = GET_REG(psiochan, RBR);                                       /* got a character              */

    return  (ERROR_NONE);
}

int interrupt_enable(int irq_no) {
	if(irq_no >= VIC_MaxInterrupt) {
		return -1;
	}

	EnterCriticalSection();
	if(irq_no < VIC_InterruptSeparator) {
		SET_REG(VIC0 + VICINTENABLE, GET_REG(VIC0 + VICINTENABLE) | (1 << irq_no));
#ifndef CONFIG_IPHONE_4
	} else {
		SET_REG(VIC1 + VICINTENABLE, GET_REG(VIC1 + VICINTENABLE) | (1 << (irq_no - VIC_InterruptSeparator)));
#else
	} else if(irq_no < VIC_InterruptSeparator*2) {

static void usb_set_global_out_nak()
{
	usb_global_out_nak++;
	
	if(usb_global_out_nak > 1)
		return;

	//bufferPrintf("USB: SGOUTNAK\n");

	SET_REG(USB + GINTSTS, GINTMSK_GOUTNAKEFF);
	SET_REG(USB + DCTL, GET_REG(USB + DCTL) | DCTL_SGOUTNAK);
	while ((GET_REG(USB + GINTSTS) & GINTMSK_GOUTNAKEFF) != GINTMSK_GOUTNAKEFF);
	SET_REG(USB + GINTSTS, GINTMSK_GOUTNAKEFF);
}

static int wait_for_ready(int timeout) {
	if((GET_REG(NAND + NAND_STATUS) & NAND_STATUS_READY) != 0) {
		return 0;
	}

	uint64_t startTime = timer_get_system_microtime();
	while((GET_REG(NAND + NAND_STATUS) & NAND_STATUS_READY) == 0) {
		if(has_elapsed(startTime, timeout * 1000)) {
			return ERROR_TIMEOUT;
		}
	}

	return 0;
}

void usb_phy_init() {
	// power on PHY
	SET_REG(USB_PHY + OPHYPWR, OPHYPWR_POWERON);
	udelay(USB_PHYPWRPOWERON_DELAYUS);

	// select clock
	SET_REG(USB_PHY + OPHYCLK, (GET_REG(USB_PHY + OPHYCLK) & (~OPHYCLK_CLKSEL_MASK)) | OPHYCLK_CLKSEL_48MHZ);

	// reset phy
	SET_REG(USB_PHY + ORSTCON, GET_REG(USB_PHY + ORSTCON) | ORSTCON_PHYSWRESET);
	udelay(USB_RESET2_DELAYUS);
	SET_REG(USB_PHY + ORSTCON, GET_REG(USB_PHY + ORSTCON) & (~ORSTCON_PHYSWRESET));
	udelay(USB_RESET_DELAYUS);
}

int spi_tx(int port, const uint8_t* buffer, int len, int block, int unknown)
{
    if(port >= SPICount || port < 0)
        return -1;

    SPIStruct *spi = &SPIData[port];

    SET_REG(spi->registers->control, GET_REG(spi->registers->control) | (1 << 2));
    SET_REG(spi->registers->control, GET_REG(spi->registers->control) | (1 << 3));

    // Set len to number of words.
    len = (len + ((1<<spi->wordSize)-1)) >> spi->wordSize;

    spi->txBuffer = (void*)buffer;
    spi->rxBuffer = NULL;
    spi->txLength = len;

    if(len > MAX_TX_BUFFER)
        spi->txDone = MAX_TX_BUFFER;
    else
        spi->txDone = len;

    if(unknown == 0) {
        SET_REG(spi->registers->rxCount, 0);
    }
    else
    {
        spi->config |= 0x80;
    }

    //bufferPrintf("spi_tx(%d, %p, %d, %d, %d)\n", port, buffer, len, block, unknown);

    spi_txdata(spi, buffer, 0, spi->txDone);

    spi->config |= 0x200100;
    SET_REG(spi->registers->txCount, len);
    SET_REG(spi->registers->config, spi->config);
    SET_REG(spi->registers->control, 1);

    if(block)
    {
        while(spi->txBuffer != NULL || TX_BUFFER_LEFT(GET_REG(spi->registers->status)) > 0)
            task_yield();

        return len;
    }
    else
        return 0;
}