int
nextelement(set *set1, int m, int pos)
{
    register setword setwd;
    register int w;

#if  MAXM==1
    if (pos < 0) setwd = set1[0];
    else         setwd = set1[0] & BITMASK(pos);

    if (setwd == 0) return -1;
    else            return FIRSTBIT(setwd);
#else
    if (pos < 0)
    {
        w = 0;
        setwd = set1[0];
    }
    else
    {
        w = SETWD(pos);
        setwd = set1[w] & BITMASK(SETBT(pos));
    }

    for (;;)
    {
        if (setwd != 0) return  TIMESWORDSIZE(w) + FIRSTBIT(setwd);
        if (++w == m) return -1;
        setwd = set1[w];
    }

#endif
}

static sys_res stm32_I2cUnlock(p_dev_i2c p)
{
	GPIO_InitTypeDef xGpio;
	uint_t i;

	xGpio.GPIO_Speed = GPIO_Speed_2MHz;
	//SCL ==> output
	xGpio.GPIO_Mode = GPIO_Mode_OUT;
	xGpio.GPIO_OType = GPIO_OType_OD;
	xGpio.GPIO_Pin = BITMASK(p->def->sclpin);
	stm32_GpioClockEnable(p->def->sclport);
	GPIO_Init(arch_GpioPortBase(p->def->sclport), &xGpio);
	//SDA ==> input
	xGpio.GPIO_Mode = GPIO_Mode_IN;
	xGpio.GPIO_PuPd =GPIO_PuPd_NOPULL;
	xGpio.GPIO_Pin = BITMASK(p->def->sdapin);
	stm32_GpioClockEnable(p->def->sdaport);
	GPIO_Init(arch_GpioPortBase(p->def->sdaport), &xGpio);

	for (i = 10; i; i--) {
		GPIO_ResetBits(arch_GpioPortBase(p->def->sclport), BITMASK(p->def->sclpin));
		sys_Delay(0x2000);
		GPIO_SetBits(arch_GpioPortBase(p->def->sclport), BITMASK(p->def->sclpin));
		sys_Delay(0x2000);
		if (GPIO_ReadInputDataBit(arch_GpioPortBase(p->def->sdaport), BITMASK(p->def->sdapin)))
			break;
	}
	if (i)
		return SYS_R_OK;
	return SYS_R_BUSY;
}

/*!
 * This function adds a completion report to the completion fifo.
 *
 * \param qid The queue id.
 * \param taskId Completion task ID as set by vTaskSetCurrentTaskTag().
 * \param completionType COMPLETION_INTERNAL or COMPLETION_EXTERNAL.
 * \param axiWriteCount AXI transactions counter from previous completion report.
 */
void CompletionFifoInsert(int qid, uint32_t taskId, CompletionType_t *completionType, uint32_t axiWriteCount)
{
	uint8_t	fifoItem = 0;
	uint32_t fifoIdx;
	
	if (((CompletionType_t)completionType != COMPLETION_INTERNAL) &&
	    ((CompletionType_t)completionType != COMPLETION_EXTERNAL)) {
		DX_PAL_Abort("Bad completionType");
	}
	if (taskId >= BITMASK(FIFO_TASK_ID_BIT_SIZE)) {
		DX_PAL_Abort("Bad completionTaskId");
	}
	if (axiWriteCount >= BITMASK(FIFO_AXI_COUNTER_BIT_SIZE)) {
		DX_PAL_Abort("Bad axiWriteCount");
	}
	if ((gCompletionFifoHead[qid] - gCompletionFifoTail[qid]) >= COMPLETION_FIFO_LEN) {
		DX_PAL_Abort("Completion FIFO overflow");
	}
	
	SET_FIFO_COMPLETION_TYPE(fifoItem, (CompletionType_t)completionType);
	SET_FIFO_AXI_COUNTER(fifoItem, axiWriteCount);
	SET_FIFO_COMPLETION_TASK_ID(fifoItem, taskId);
	fifoIdx = gCompletionFifoHead[qid] & (COMPLETION_FIFO_LEN - 1);
	gCompletionFifo[qid][fifoIdx] = fifoItem;
	
	gCompletionFifoHead[qid]++;
			
	DX_PAL_LOG_DEBUG("qid=%d taskId=%d fifoIdx=%d gCompletionFifoHead[qid]=%d completionType=%s axiWriteCount=%d\n", 
		(int)qid, (int)taskId, (int)fifoIdx, (int)gCompletionFifoHead[qid],
		((CompletionType_t)completionType==COMPLETION_INTERNAL)?"INTERNAL":"EXTERNAL", (int)axiWriteCount);
}

void sc16is7x_ItHandler()
{
	p_dev_uart p;
	p_uart_def pDef;
	uint_t i, nLen, nLsr, nRecd, nReg;
	uint8_t aTemp[64];

	for (pDef = tbl_bspUartDef, i = 0; pDef < ARR_ENDADR(tbl_bspUartDef); pDef++, i++) {
		if (pDef->type != UART_T_SC16IS7X)
			continue;
		p = uart_Dev(i);
		//中断识别,也可读IIR实现
		nLsr = sc16is7x_RegRead(SC16IS7X_LSR, pDef->id);
		if (nLsr & BITMASK(0)) {
			//Rx Fifo Not Empty
			nRecd = sc16is7x_RegRead(SC16IS7X_RXLVL, pDef->id);
			for (nReg = BITMASK(7) | (SC16IS7X_RHR << 3) | pDef->id; nRecd; nRecd -= nLen) {
				nLen = MIN(nRecd, sizeof(aTemp));
				sc16is7x_Nss(0);
				if (spi_Transce(sc16is7x_spi, nReg, aTemp, nLen) == SYS_R_OK) {
#if IO_BUF_TYPE == BUF_T_BUFFER
					buf_Push(p->bufrx, aTemp, nLen);
#else
					dque_Push(dqueue, p->parent->id | UART_DQUE_RX_CHL, aTemp, nLen);
#endif
				}
				sc16is7x_Nss(1);
			}
		}
		if (nLsr & 0xF8)
			sc16is7x_RegWrite(SC16IS7X_IER, pDef->id, 0x01);
	}
}

//-------------------------------------------------------------------------
//
//-------------------------------------------------------------------------
sys_res sc16is7x_UartConfig(uint_t nChl, t_uart_para *pPara)
{
	uint_t nTemp;

	nTemp = SC16IS7X_CRYSTAL / pPara->baud / 16;
	sc16is7x_RegWrite(SC16IS7X_LCR, nChl, BITMASK(7)); 	//除数锁存使能
	sc16is7x_RegWrite(SC16IS7X_DLL, nChl, nTemp);
	sc16is7x_RegWrite(SC16IS7X_DLH, nChl, nTemp >> 8);

	if (pPara->data == UART_DATA_7D)
		nTemp = BITMASK(1);
	else
		nTemp = BITMASK(1) | BITMASK(0);
	if (pPara->stop == UART_STOP_2D)
		nTemp |= BITMASK(6);
	switch (pPara->pari) {
		case UART_PARI_EVEN:
			nTemp |= BITMASK(4) | BITMASK(3);
			break;
		case UART_PARI_ODD:
			nTemp |= BITMASK(3);
			break;
		default:
			break;
	}
	sc16is7x_RegWrite(SC16IS7X_LCR, nChl, nTemp);		//关闭除数锁存器,访问通用寄存器
	sc16is7x_RegWrite(SC16IS7X_FCR, nChl, BITMASK(0));	//使能FIFO
	sc16is7x_RegWrite(SC16IS7X_IER, nChl, BITMASK(0)); 	//设置中断，允许睡眠模式，打开RX超时、RHR满、关闭RTHR中断	
	return SYS_R_OK;
}

void arch_GpioSet(uint_t nPort, uint_t nPin, uint_t nHL)
{

	if (nHL)
		MAP_GPIOPinWrite(arch_GpioPortBase(nPort), BITMASK(nPin), 0xFF);
	else
		MAP_GPIOPinWrite(arch_GpioPortBase(nPort), BITMASK(nPin), 0);
}

void arch_GpioSet(uint_t nPort, uint_t nPin, uint_t nHL)
{
	LPC_GPIO_TypeDef *p = lpc22xx_tblGpioBase[nPort];

	if (nHL)
		p->FIOSET = BITMASK(nPin);
	else
		p->FIOCLR = BITMASK(nPin);
}

sys_res vk321x_UartConfig(uint_t nChl, t_uart_para *pPara)
{
	uint_t nReg = 0;

	switch (pPara->pari) {
	case UART_PARI_EVEN:
		nReg = BITMASK(6) | BITMASK(4);
		break;
	case UART_PARI_ODD:
		nReg = BITMASK(6) | BITMASK(3);
		break;
	default:
		break;
	}
	if (pPara->stop == UART_STOP_2D)
		nReg |= BITMASK(7);
	vk321x_Lock();
	vk321x_RegWrite(nChl, VK321X_SCONR, nReg);
	//波特率设置
	vk321x_RegWrite(nChl, VK321X_SCTLR, (vk321x_BaudReg(pPara->baud) << 4) | BITMASK(3));
	//发送接收FIFO使能
	vk321x_RegWrite(nChl, VK321X_SFOCR, BITMASK(2) | BITMASK(3));
	//接收FIFO中断使能
	vk321x_RegWrite(nChl, VK321X_SIER, BITMASK(0));
	vk321x_Unlock();
	return SYS_R_OK;
}

uint32_t arch_GpioPortBase(uint_t nPort)
{

#if 0
	switch (nPort) {
	case GPIO_P0:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(0))
			return GPIO_PORTA_AHB_BASE;
		else
			return GPIO_PORTA_BASE;
	case GPIO_P1:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(1))
			return GPIO_PORTB_AHB_BASE;
		else
			return GPIO_PORTB_BASE;
	case GPIO_P2:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(2))
			return GPIO_PORTC_AHB_BASE;
		else
			return GPIO_PORTC_BASE;
	case GPIO_P3:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(3))
			return GPIO_PORTD_AHB_BASE;
		else
			return GPIO_PORTD_BASE;
	case GPIO_P4:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(4))
			return GPIO_PORTE_AHB_BASE;
		else
			return GPIO_PORTE_BASE;
	case GPIO_P5:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(5))
			return GPIO_PORTF_AHB_BASE;
		else
			return GPIO_PORTF_BASE;
	case GPIO_P6:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(6))
			return GPIO_PORTG_AHB_BASE;
		else
			return GPIO_PORTG_BASE;
	case GPIO_P7:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(7))
			return GPIO_PORTH_AHB_BASE;
		else
			return GPIO_PORTH_BASE;
	case GPIO_P9:
		if (__raw_readl(SYSCTL_GPIOHBCTL) & BITMASK(8))
			return GPIO_PORTJ_AHB_BASE;
		else
			return GPIO_PORTJ_BASE;
	default:
		return NULL;
	}
#else

	return lm3s_tblGpioBase[nPort];
#endif
}

static inline void cpsw_ale_set_field(u32 *ale_entry, u32 start, u32 bits,
				      u32 value)
{
	int idx;

	value &= BITMASK(bits);
	idx    = start / 32;
	start -= idx * 32;
	idx    = 2 - idx; /* flip */
	ale_entry[idx] &= ~(BITMASK(bits) << start);
	ale_entry[idx] |=  (value << start);
}

int arch_GpioRead(uint_t nPort, uint_t nPin)
{

	if (MAP_GPIOPinRead(arch_GpioPortBase(nPort), BITMASK(nPin)))
		return 1;
	return 0;
}

void
contract1(graph *g, graph *h, int v, int w, int n)  
/* Contract distinct vertices v and w (not necessarily adjacent)
   with result in h.  No loops are created. */
{
    int x,y;
    setword bitx,bity,mask1,mask2;
    int i;

    if (w < v)
    {
	x = w; 
	y = v;
    }
    else
    {
	x = v; 
	y = w;
    }

    bitx = bit[x];
    bity = bit[y];
    mask1 = ALLMASK(y);
    mask2 = BITMASK(y);

    for (i = 0; i < n; ++i)
        if (g[i] & bity)
	    h[i] = (g[i] & mask1) | bitx | ((g[i] & mask2) << 1);
	else
	    h[i] = (g[i] & mask1) | ((g[i] & mask2) << 1);

    h[x] |= h[y];
    for (i = y+1; i < n; ++i) h[i-1] = h[i];
    h[x] &= ~bitx;
}

//-------------------------------------------------------------------------
//
//-------------------------------------------------------------------------
sys_res arch_I2cRead(p_dev_i2c p, uint_t nDev, uint8_t *pBuf, uint_t nLen)
{
	I2C_TypeDef *pI2c = stm32_tblI2cId[p->parent->id];

	I2C_GenerateSTART(pI2c, ENABLE);
	I2C_AcknowledgeConfig(pI2c, ENABLE);
	while(!I2C_CheckEvent(pI2c, I2C_EVENT_MASTER_MODE_SELECT));
	pI2c->DR = nDev & BITANTI(0);
	while(!I2C_CheckEvent(pI2c, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
	pI2c->DR = nDev;
	while(!I2C_CheckEvent(pI2c, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
	I2C_GenerateSTART(pI2c, ENABLE);
	while(!I2C_CheckEvent(pI2c, I2C_EVENT_MASTER_MODE_SELECT));
	pI2c->DR = nDev | BITMASK(0);
	while(!I2C_CheckEvent(pI2c, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
	for (; nLen; nLen--) {
		while ((pI2c->SR1 & I2C_FLAG_RXNE) == 0);
		*pBuf++ = pI2c->DR;
		if (nLen == 1) {
			I2C_GenerateSTOP(I2C1, ENABLE);
			I2C_AcknowledgeConfig(I2C1, DISABLE);
		}
	}
	I2C_GenerateSTOP(I2C1, ENABLE);
	I2C_AcknowledgeConfig(I2C1, DISABLE);
	return SYS_R_OK;
}

sys_res vk321x_UartSend(uint_t nChl, const void *pData, uint_t nLen)
{
	int nSR;
	uint_t i, n;
	const uint8_t *p = (const uint8_t *)pData;

	vk321x_Lock();
	for (; nLen; nLen -= n, p += n) {
		//wait tx fifo empty
		for (i = 600 / OS_TICK_MS; i; i--) {
			if ((nSR = vk321x_RegRead(nChl, VK321X_SSR)) == -1) {
				vk321x_Unlock();
				return SYS_R_TMO;
			}
			if (nSR & BITMASK(2))
				break;
			os_thd_Slp1Tick();
		}
		if (i == 0)
			break;
		n = MIN(nLen, 16);
		nSR = 0xC0 | (nChl << 4) | (n - 1);
		uart_Send(vk321x_port, &nSR, 1);
		uart_Send(vk321x_port, p, n);
	}
	vk321x_Unlock();
	if (nLen)
		return SYS_R_ERR;
	return SYS_R_OK;
}

void arch_ExtIrqDisable(uint_t nPort, uint_t nPin)
{
	EXTI_InitTypeDef xEXTI;

	xEXTI.EXTI_Line = BITMASK(nPin);
	xEXTI.EXTI_LineCmd = DISABLE;
	EXTI_Init(&xEXTI);
}

static __inline__ int get_l3_size(void){

  int eax, ebx, ecx, edx;

  cpuid(0x80000006, &eax, &ebx, &ecx, &edx);

  return BITMASK(edx, 18, 0x3fff) * 512;
}

int main(void) {
  const uint8_t initial_bits = 0b00001111;
  const uint8_t inverted_bits = ~initial_bits; /* equals to 0b11110000 */

  const uint8_t first_six_bits = 0b11111100;
  const uint8_t last_six_bits  = 0b00111111;
  const uint8_t middle_four_bits = first_six_bits & last_six_bits; /* equals to 0b00111100 */

  const uint8_t some_bits = 0b10110010;
  const uint8_t more_bits = 0b01011110;
  const uint8_t combined_bits = some_bits | more_bits; /* equals to 0b11111110 */ 

  /*
   * Bits | 7th | 6th | 5th | 4th | 3rd | 2nd | 1st | 0th
   * 16   |  0  | 0   |  0  |  1  |  0  |  0  |  0  |  0
   */

  /* Powers of 2 in binary */
  const uint8_t zero                  = 0b00000000;
  const uint8_t one                   = 0b00000001;
  const uint8_t two                   = 0b00000010;
  const uint8_t four                  = 0b00000100;
  const uint8_t eight                 = 0b00001000;
  const uint8_t sixteen               = 0b00010000;
  const uint8_t thirtytwo             = 0b00100000;
  const uint8_t sixtyfour             = 0b01000000;
  const uint8_t onehundredtwentyeight = 0b10000000;

  printf("%d\n", zero);
  printf("%d\n", one);
  printf("%d\n", two);
  printf("%d\n", four);
  printf("%d\n", eight);
  printf("%d\n", sixteen);
  printf("%d\n", thirtytwo);
  printf("%d\n", sixtyfour);
  printf("%d\n", onehundredtwentyeight);

  /* Powers of 2 showing the use of left shift (<<) operator.
   *
   * What does it do? It telss the compiler to shift (deslocate) the bit on the most right
   * to the left by n positions. Since it starts with 0000 0001, it creates the power of 2
   * effect.
   */
  printf("%d\n", BITMASK(0)); /* 0000 0001 */
  printf("%d\n", BITMASK(1)); /* 0000 0010 */
  printf("%d\n", BITMASK(2)); /* 0000 0100 */
  printf("%d\n", BITMASK(3)); /* 0000 1000 */
  printf("%d\n", BITMASK(4)); /* 0001 0000 */
  printf("%d\n", BITMASK(5)); /* 0010 0000 */
  printf("%d\n", BITMASK(6)); /* 0100 0000 */
  printf("%d\n", BITMASK(7)); /* 1000 0000 */

  return 0;
}

void ht1621_DisSection(uint_t nSection)
{
	uint_t nBit;

	nBit = (nSection & 0xC0) >> 6;
	nSection = invert_bits(nSection & 0x3F, 6);
	ht1621_WrByte(nSection, ht1621_RrByte(nSection) | BITMASK(nBit));
	ht1621_Cs(1);
}

static inline int cpsw_ale_get_field(u32 *ale_entry, u32 start, u32 bits)
{
	int idx;

	idx    = start / 32;
	start -= idx * 32;
	idx    = 2 - idx; /* flip */
	return (ale_entry[idx] >> start) & BITMASK(bits);
}

//-------------------------------------------------------------------------
//
//-------------------------------------------------------------------------
int reg_GetBit(uint_t nDa, uint_t nID, uint_t nBit)
{
	uint64_t nData;

	reg_Get(nDa, nID, &nData);
	if (nData & BITMASK(nBit))
		return 1;
	return 0;
}