void setup_fm()
{

    /* open /dev/mem */
    if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
        printf("can't open /dev/mem \n");
        exit (-1);
    }
    
    allof7e = (unsigned *)mmap(
                  NULL,
                  0x01000000,  //len
                  PROT_READ|PROT_WRITE,
                  MAP_SHARED,
                  mem_fd,
                  0x20000000  //base
              );

    if ((int)allof7e==-1) exit(-1);

    SETBIT(GPFSEL0 , 14);
    CLRBIT(GPFSEL0 , 13);
    CLRBIT(GPFSEL0 , 12);

 
    struct GPCTL setupword = {6/*SRC*/, 1, 0, 0, 0, 1,0x5a};

    ACCESS(CM_GP0CTL) = *((int*)&setupword);
}

void fibre_create(coStData *regs, fibreType rAdd, int stackSize, char *stackPointer)
{
	CLRBIT(regs->flags, JMPBIT); // = JMPFROMMAINw
	CLRBIT(regs->flags, CALLSTATUS); // = CALL
	CLRBIT(regs->flags, FINISHED);
	SETBIT(regs->flags, SHEDULED);

	regs->retAdd = rAdd;
	regs->mallocStack = stackPointer;
	regs->sp = regs->mallocStack + (stackSize - 1);
	//regs->sp = regs->mallocStack + (stackSize);
/*
#ifdef __AVR__
	*((coStData **)(regs->sp - sizeof(arg))) = arg;
	*((coStData **)(regs->sp - sizeof(arg) - sizeof(regs))) = regs;
	regs->sp -= 2;
#elif defined(X86)
	//Copy the arguments a user might want, onto the stack
	regs->sp -= sizeof(arg);
	*((coStData **)regs->sp) = arg;

	//copy a pointer to the specific routine's reg data structure
	//onto it's stack so it's passed in as an argument
	regs->sp -= sizeof(coStData *);
	//I think this will always be on aligned data, but must double check, otherwise
	//it'll cause a bus error on some architechtures if it's not aligned
	*((coStData **)regs->sp) = regs;
#endif
 */
}

void xio_1134_papertape (int32 iocc_addr, int32 iocc_func, int32 iocc_mod)
{
	char msg[80];

	switch (iocc_func) {
		case XIO_READ:											/* read: return last character read */
			M[iocc_addr & mem_mask] = (uint16) (ptr_char << 8);
			break;

		case XIO_WRITE:											/* write: initiate punch operation */
			if ((ptr_dsw & PTR1134_DSW_PUNCH_NOT_READY) == 0 && IS_ONLINE(ptp_unit)) {
				putc((M[iocc_addr & mem_mask] >> 8) & 0xFF, ptp_unit->fileref);
				ptp_unit->pos++;
			}
			sim_activate(ptp_unit, ptp_wait);					/* schedule interrupt */
			SETBIT(ptr_dsw, PTR1134_DSW_PUNCH_NOT_READY | PTR1134_DSW_PUNCH_BUSY);
			break;

		case XIO_SENSE_DEV:										/* sense device status */
			ACC = ptr_dsw;
			if (iocc_mod & 0x01) {								/* reset interrupts */
				CLRBIT(ptr_dsw, PTR1134_DSW_READER_RESPONSE | PTR1134_DSW_PUNCH_RESPONSE);
				CLRBIT(ILSW[4], ILSW_4_1134_TAPE);
			}
			break;

		case XIO_CONTROL:										/* control: initiate character read */
			sim_activate(ptr_unit, ptr_wait);					/* schedule interrupt */
			SETBIT(ptr_dsw, PTR1134_DSW_READER_BUSY | PTR1134_DSW_READER_NOT_READY);
			break;

		default:
			sprintf(msg, "Invalid 1134 reader/1055 punch XIO function %x", iocc_func);
			xio_error(msg);
	}

void dataflash_init()
{

	CLRBIT(DATAFLASH_DDR,DATAFLASH_MISO);
	SETBIT(DATAFLASH_DDR,DATAFLASH_RESET);
	SETBIT(DATAFLASH_DDR,DATAFLASH_CS);
	SETBIT(DATAFLASH_DDR,DATAFLASH_MOSI);
	SETBIT(DATAFLASH_DDR,DATAFLASH_SCK);
	SETBIT(DATAFLASH_DDR,PB0); //output or else master gets stopped


	CLRBIT(DATAFLASH_PORT,DATAFLASH_RESET); //reset
	asm("nop");
	asm("nop");
	SETBIT(DATAFLASH_PORT,DATAFLASH_RESET); //reset
	SETBIT(DATAFLASH_PORT,PB0); //output or else master gets stopped
	SETBIT(DATAFLASH_PORT,DATAFLASH_CS); 	//dataflash not selected

	/* Enable SPI, Master, set clock rate fck/16 */

//	SPCR = (1<<SPE) | (1<<MSTR) | (1<< SPR0);
	SPCR = 0x5C; //SPI mode 3
	SPSR = (1<<SPI2X);
//	SPCR |= (1<< SPR0);

}

//----关闭IIC模块-----------------------------------------------------------
//功能: 关闭IIC模块，禁止中断(本函数并不产生停止时序)
//参数: reg,iic外设寄存器基址
//返回: 无
//--------------------------------------------------------------------------
void _IIC_GenerateDisable(volatile tagI2CReg *reg)
{
    CLRBIT(reg->rTWIIMASK, TWITXINT);   //disable TX FIFO service interrupts
    CLRBIT(reg->rTWIIMASK, TWIRXINT);   //disable RX FIFO service interrupts
    CLRBIT(reg->rTWIIMASK, TWIMCOM);
    CLRBIT(reg->rTWIMITR, TWIEN);
}

int jackpifm_setup_fm() {
  /* open /dev/mem */
  if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
    fprintf(stderr, "Can't open /dev/mem: %s\n", strerror(errno));
    return 1;
  }

  allof7e = (unsigned *)mmap(
      NULL,
      0x01000000,  //len
      PROT_READ|PROT_WRITE,
      MAP_SHARED,
      mem_fd,
      0x20000000  //base
  );

  if ((int)allof7e==-1) return 1;

  SETBIT(GPFSEL0 , 14);
  CLRBIT(GPFSEL0 , 13);
  CLRBIT(GPFSEL0 , 12);

  struct GPCTL setupword = {6/*SRC*/, 1, 0, 0, 0, 1,0x5a};
  ACCESS(CM_GP0CTL) = *((int*)(&setupword));
  return 0;
}

//*******************************************************************************
unsigned int oled_status()
{
//unsigned char low, med, high;
unsigned char lB;

volatile unsigned int result;

	OLED_DATA_LOW_DDR = 0x00; //in
	OLED_DATA_MED_DDR = 0x00; //in
	OLED_DATA_HIGH_DDR = 0x00; //in

	CLRBIT(OLED_CTRL_PORT,OLED_DC);
	CLRBIT(OLED_CTRL_PORT,OLED_CS);
	CLRBIT(OLED_CTRL_PORT,OLED_RD);

	lB = OLED_DATA_LOW_PIN >> 3;
	lB |= (OLED_DATA_MED_PIN>>2) << 6;
	//hB = (OLED_DATA_MED_PIN<<1) >> 6;

	SETBIT(OLED_CTRL_PORT,OLED_RD);
	SETBIT(OLED_CTRL_PORT,OLED_CS);
	SETBIT(OLED_CTRL_PORT,OLED_DC);

	OLED_DATA_LOW_DDR = 0xFF; //output
	OLED_DATA_MED_DDR = 0xFF; //output
	OLED_DATA_HIGH_DDR = 0xFF; //output

//result = (hB << 8) + lB;
result = lB;
return result;
}

void
stealth_arduino_reset()
{
  CLRBIT(ARDUINO_RESET_PORT, ARDUINO_RESET_PIN);
  //arduino in reset
  CLRBIT(ARDUINO_TX_ENABLE_PORT, ARDUINO_TX_ENABLE_PIN);
  //Drive TX buffer
}

void kbCheckStatus() {
	if ( p_keyboard->BufPos <= KB_BUFFER_MIN_POS ) {
		SETBIT( p_keyboard->status , KB_BUFFER_EMPTY );
	} else {
		CLRBIT( p_keyboard->status , KB_BUFFER_EMPTY );
	}
	if ( p_keyboard->BufPos >= KB_BUFFER_SIZE ) {
		SETBIT( p_keyboard->status , KB_BUFFER_FULL );
	} else {
		CLRBIT( p_keyboard->status , KB_BUFFER_FULL );
	}
}

void beginSerial(unsigned long baud)
{
	//disable hardware UART
	CLRBIT(UCSR0B,RXEN0);
	CLRBIT(UCSR0B,TXEN0);


	SETBIT(RXTX_PORT, TX_PIN);  //idle state
	SETBIT(RXTX_DDR, TX_PIN);	//output
	CLRBIT(RXTX_DDR, RX_PIN);	//input
	sei();
}

/*
 * set BAUD
 * Databits	8
 */
extern void initUart(void) {
	SETBIT(P1SEL, (RX_UART1 + TX_UART1));			/* P1.1 - RXD and P1.2 - TXD */
	SETBIT(P1SEL2, (RX_UART1 + TX_UART1));
	SETBIT(UCA0CTL1, (UCSSEL_2 + UCSWRST));			/* SMCLK */
	CLRBIT(UCA0CTL1, UC7BIT);						/* 8-bit data */

	/* 8x0.66666667 = 5, 0x5b, http://www.glitovsky.com/Tutorialv0_3.pdf page 87 */
	UCA0BR0 = USCI_BR0_VAL;
	UCA0BR1 = USCI_BR1_VAL;
	UCA0MCTL = USCI_BRS_VAL;						/* http://embedded-funk.net/writing-c-code-smartly-1-using-your-c-preprocessor-as-your-calculator */
	CLRBIT(UCA0CTL1, UCSWRST);						/* Take UCA0 out of reset */
	SETBIT(IE2, UCA0RXIE);							/* Enable USCI_A0 RX interrupt */
}

/*
 * Redistributes elements in the neighborhood evenly.
 * If insert is 1, inserts elem into the packed memory
 * array during the redistribution.
 */
void redistribute(PMA* pma, int* nbrhd, int insert, int elem){
	double dist = (nbrhd[1] - nbrhd[0]) / (double)nbrhd[2];
	int* temp = (int*) malloc(sizeof(int) * nbrhd[2]);
	int count = 0;
	int cur = nbrhd[0] - 1;

	/* Copy everything to a separate array. This is simpler
	   than topological sorting.								*/
	while (count < nbrhd[2]){
		cur = next_greater(pma, cur);

		if (insert && (cur >= nbrhd[1] || cur == -1 || elem < pma->array[cur])){
			temp[count] = elem;
			insert = 0;
			count++;
		}

		if (cur < nbrhd[1] && cur != -1){
			temp[count] = pma->array[cur];
			CLRBIT(pma->bitmap, cur);
			count++;
		}
	}

	for (count = 0; count < nbrhd[2]; count++){
		pma->array[nbrhd[0] + (int)(count * dist)] = temp[count];
		SETBIT(pma->bitmap, nbrhd[0] + (int)(count * dist));
	}

	free(temp);
}

//Private Variables
void hc595_Write(uint_t nPin, uint_t nHL)
{
	static uint_t hc595_nStatus = 0x00;
	uint_t i, nTemp;

	if (nHL)
		CLRBIT(hc595_nStatus, nPin);
	else
		SETBIT(hc595_nStatus, nPin);

	hc595_RCK(0);
	hc595_OE(0);
	nTemp = hc595_nStatus;
	for(i = 0; i < 8; i++)
	{
		hc595_Clk(0);
		if((nTemp&0x80)==0)
			hc595_SER(1);
		else
			hc595_SER(0);
		hc595_Clk(1);
		nTemp <<= 1;
	}
	hc595_RCK(1);
	hc595_OE(0);
}

void dataflash_program_page(unsigned char * page_buff, unsigned int page_num)
{
unsigned char p=0;

dataflash_clear_buff();
dataflash_write_buff(&page_buff[0]);



//--Program the page
	CLRBIT(DATAFLASH_PORT,DATAFLASH_CS); //select dataflash


//	dataflash_out(0x88);
	dataflash_out(0x83);


	p= page_num>>6;
	dataflash_out(p);
	p= page_num<<2;
	dataflash_out(p);
	dataflash_out(0); //don't c


	SETBIT(DATAFLASH_PORT,DATAFLASH_CS); //Deselect flash chip

//	delay_ms(30); //wait until programmed
	while(dataflash_checkStatus() == DATAFLASH_BUSY) {;} //wait until erased
}

int mark_not_coded(unsigned char *p, unsigned n, unsigned nti)
{
  unsigned sc;

  resetbits(0);

  if ((sc = nextbits(p, 32)) == SC_GVOP) {
    while (currentpos() < n - 4) {
      if (nextbits(p, 32) == SC_VOP) goto VOP;
      resetbits(currentpos() - 3);
    }
    goto NO_VOP;
  }
  if (sc != SC_VOP) goto X;
VOP:
  nextbits(p, 2);               /* VOP type */
  skipbits(p, 1);               /* modulo_time_base */
  if (!nextbits(p, 1)) goto X;  /* marker bit */
  nextbits(p, nti);             /* vop_time_increment */
  if (!nextbits(p, 1)) goto X;  /* marker bit */
  CLRBIT(p[pos_], bit_ - 1);    /* set VOP not coded */
  if (nextbits(p, 1)) goto X;   /* VOP coded ? */
NO_VOP:
  return currentpos();

X: return 0;
}

void
efe_intr_disable(efe_t *efep)
{
	PUTCSR(efep, CSR_INTMASK, 0);

	CLRBIT(efep, CSR_GENCTL, GENCTL_INT);
}

/* Convert optional "[!123456789S]" string into a bitmask */
int conf_parse_runlevels(char *runlevels)
{
    int i, not = 0, bitmask = 0;

    if (!runlevels)
        runlevels = "[234]";
    i = 1;
    while (i) {
        int level;
        char lvl = runlevels[i++];

        if (']' == lvl || 0 == lvl)
            break;
        if ('!' == lvl) {
            not = 1;
            bitmask = 0x3FE;
            continue;
        }

        if ('s' == lvl || 'S' == lvl)
            lvl = '0';

        level = lvl - '0';
        if (level > 9 || level < 0)
            continue;

        if (not)
            CLRBIT(bitmask, level);
        else
            SETBIT(bitmask, level);
    }

    return bitmask;
}

static void KeyShiftRight( unsigned char *key, const int numbits )
{
    int i;
    unsigned char keep = key[6];

    for (i = 0; i < numbits; i++)
    {
        int j;

        for (j = 6; j >= 0; j--)
        {
            if (j!=6 && (key[j] & 0x01))
                key[j+1] |=  0x80;
            key[j] >>= 1;
        }

        if (GETBIT( key, 28 ))
        {
            CLRBIT( key, 28 );
            SETBIT( key, 0 );
        }

        if (keep & 0x01)
            SETBIT( key, 28 );

        keep >>= 1;
    }
}

static void KeyShiftLeft( unsigned char *key, const int numbits )
{
    int i;
    unsigned char keep = key[0];

    for (i = 0; i < numbits; i++)
    {
        int j;

        for (j = 0; j < 7; j++)
        {
            if (j && (key[j] & 0x80))
                key[j-1] |=  0x01;
            key[j] <<= 1;
        }

        if (GETBIT( key, 27 ))
        {
            CLRBIT( key, 27 );
            SETBIT( key, 55 );
        }

        if (keep & 0x80)
            SETBIT( key, 27 );

        keep <<= 1;
    }
}

int
small_free(void *ptr)
{

   __malloc_chunk_t *chunk = 0;
   __malloc_chunk_t *prev, *curr;
   int i;

   /* see if the ptr is on one of our lists */
   for (i = 0; i < NUM_CHUNK_LISTS; i++) {

      chunk = malloc_chunks[i];
      while (chunk) {
         if ((void *) ptr > (void *) chunk && ptr < (void *) (chunk + 1)) {
            /* yup, it was in our memory region */
            goto found_it;
         }
         chunk = chunk->next;
      }
   }

   /* nope -- didn't find it */
   return 0;

 found_it:
   /* which bit should we clear? */
   i = ((int) ptr - (int) chunk - 8) / chunk->size;

   CLRBIT(chunk->bitmap, i);

   if (chunk->bitmap == 0) {

      /* we can add this chunk to the free list */

      i = get_chunk_list(chunk->size);
      prev = 0;
      curr = malloc_chunks[i];

      /* find the chunk in it's list */
      while (curr && curr != chunk) {
         prev = curr;
         curr = curr->next;
      }

      /* unlink */
      if (prev == 0) {
         /* chunk was at head of list */
         malloc_chunks[i] = curr->next;
      } else {
         prev->next = curr->next;
      }

      /* add to head of free list */
      chunk->next = free_chunks;
      free_chunks = chunk;
   }

   return 1;
}