void timer2_enable()
{
	int i;

	map_mem(TIMER2_BASE, TIMER2_BASE, MMU_SECTION, USER_NO_ACCESS);

	for(i = 0; i < NUM_CALLBACKS; i++)
	{
		callback_vectors[i] = 0x00000000;
	}

        set_register_bit_value(TIMER2_BASE + TIOCP_CFG, 0, 1);

        while(get_register_bit(TIMER2_BASE + TIOCP_CFG, 0));

        set_register_bit_value(TIMER2_BASE + IRQENABLE_SET, 1, 1); // Enable overflow interrupt

        while(get_register_bit(TIMER2_BASE + TWPS, W_PEND_TCLR));

        set_register_bit_value(TIMER2_BASE + TCLR, 1, 1); // Auto reload

        while(get_register_bit(TIMER2_BASE + TWPS, W_PEND_TLDR));

        set_register(TIMER2_BASE + TLDR, 0xff000000); // Load value on reload

        while(get_register_bit(TIMER2_BASE + TWPS, W_PEND_TTGR));
        set_register(TIMER2_BASE + TTGR, 0x00000000);

        while(get_register_bit(TIMER2_BASE + TWPS, W_PEND_TCLR));
        set_register_bit_value(TIMER2_BASE + TCLR, 0, 1); // Start!
}

void exADD(enum addr_mode mode, char src,char dst)
{
    char val;
    switch( mode )
    {
        case VAL_REG :
            if(set_register(src, dst)) {
                printf("Bad register access\n");
                exit(-1);
            }
            break;
        case REG_REG:
            if(get_register(&val, src)) {
                printf("Bad register access\n");
                exit(-1);
            }
            if(set_register(val, dst)) {
                printf("Bad register access\n");
                exit(-1);
            }
            break;
        default :
            printf("Bad addressing mode\n");
    }
}

void first_stage(int r2_missing){
  inputs_vector_state[7] = 1; //r7 pending
  inputs_vector_values[7] = 11;
  inputs_vector_refs[7] = 0;

  // Beginning of payload building //

  add_node_to_payload(key_instructions.svc->addr, 0, key_instructions.svc->string, 0, 0, 0);

  // If writer r0 doesn't write into r1 ... //
  if(!gadget_writes_r(key_instructions.write_r0, 1)){
    set_register(1);
  }

  if(!state[0]){
    set_register(0);
  }

  if(!state[2] && !r2_missing){
    set_register(2);
  }

  if(!state[3] && !store_writes_r(7)){
    set_register(7);
  }
}

preSetAbortStates()
{
   SafeGateVal = 0;
   set_register(DDR,SoftwareGates,0);
   set_register(DDR,SoftwareGain,0);
   set_register(DDR,SoftwareAUX,0);
}

void Value::assign_register() {
  if (in_register()) {
    return;
  }

#if ENABLE_ARM_VFP
  if (stack_type() == T_FLOAT) {
    set_register(RegisterAllocator::allocate_float_register());
  } else if (stack_type() == T_DOUBLE) {
    set_vfp_double_register(RegisterAllocator::allocate_double_register());
  } else
#endif
  {
    if (!is_two_word()) {
      set_register(RegisterAllocator::allocate());
    } else {
      // NOTE: avoid doing this: set_registers(allocate(), allocate());
      // The order of parameter list evaluation is undefined in C, and
      // on linux/i386 and solaris/sparc the orders are opposite. The following
      // code forces the same order, so AOT generator will generate exact
      // same code on both Linux and Solaris hosts.
      Assembler::Register hi  = RegisterAllocator::allocate();
      Assembler::Register low = RegisterAllocator::allocate();
      set_registers(low, hi);
    }
  }
}

static inline void setup_pegasus_II(pegasus_t * pegasus)
{
	u16 data = 0xa5;
	
	set_register(pegasus, Reg1d, 0);
	set_register(pegasus, Reg7b, 1);
	mdelay(100);
	if ((pegasus->features & HAS_HOME_PNA) && mii_mode)
		set_register(pegasus, Reg7b, 0);
	else
		set_register(pegasus, Reg7b, 2);

	set_register(pegasus, 0x83, data);
	get_registers(pegasus, 0x83, 1, &data);

	if (data == 0xa5) {
		pegasus->chip = 0x8513;
	} else {
		pegasus->chip = 0;
	}

	set_register(pegasus, 0x80, 0xc0);
	set_register(pegasus, 0x83, 0xff);
	set_register(pegasus, 0x84, 0x01);
	
	if (pegasus->features & HAS_HOME_PNA && mii_mode)
		set_register(pegasus, Reg81, 6);
	else
		set_register(pegasus, Reg81, 2);
}

void execute(ARCH arch, uint32_t word)
{
    uint rs, rt, rd, sa;
	uint q, reste;
	uint val_rs, val_rt;
	uint bit_sign;

    parser_typeR(word, &rs, &rt, &rd, &sa);
	val_rs = (arch->registers)[rs];
	val_rt = (arch->registers)[rt];
	
	if (val_rt == 0) {
		set_register(arch, SR, 4096);
		print_error("can't divide by zero");
	}
	else {
		q = val_rs / val_rt;
		reste = val_rs % val_rt;
		set_register(arch, HI, q);
		set_register(arch, LO, reste);
		
		bit_sign = parser_instr(q, 31, 31);
		if (bit_sign == 1)
			set_register_bit(arch, SR, 7);	

		if (q == 0 && reste == 0) 
			set_register_bit(arch, SR, 6);	
	}
}

static inline void disable_eprom_write(pegasus_t * pegasus)
{
	__u8 tmp;

	get_registers(pegasus, EthCtrl2, 1, &tmp);
	set_register(pegasus, EpromCtrl, 0);
	set_register(pegasus, EthCtrl2, tmp & ~EPROM_WR_ENABLE);
}

static int setXY(int address,unsigned char row,unsigned char col)
{
	if(set_register(address,COMMAND_MODE,0xb0+row))
		return -1;
	if(set_register(address,COMMAND_MODE,0x00+(8*col&0x0f)))
		return -1;
	return set_register(address,COMMAND_MODE,0x10+((8*col>>4)&0x0f));
}

preSetAbortStates()
{
   SafeGateVal = 0;		
   set_register(MASTER,SoftwareGates,0);
   set_register(MASTER,SoftwareSpi0,0);
   set_register(MASTER,SoftwareSpi1,0);
   set_register(MASTER,SoftwareSpi2,0);
   set_register(MASTER,SoftwareAUX,0);
}

// Read a bunch of registers to see what happens, for initial debugging
void test_registers()
{
    u_long id;

    id = idcode(id);
    id = get_cpsr();
    id = 0x87654321;
    ice_set(watchpoint0_adr, id);
    id = idcode();
    id = ice_get(watchpoint0_adr);
    set_register(5, 0xaa5533cc);
    set_register(12, 0x1234fedc);
    id = get_register(5);
    id = get_register(12);
}

void exMOV(enum addr_mode mode, char src, char dst)
{
    char val;
    switch( mode )
    {
        case VAL_REG :
            if(set_register(src, dst)) {
                printf("Bad register access\n");
                exit(-1);
            }
            break;
        case REG_REG:
            if(get_register(&val, src)) {
                printf("Bad register access\n");
                exit(-1);
            }
            if(set_register(val, dst)) {
                printf("Bad register access\n");
                exit(-1);
            }
            break;
        case ADDR_REG :
            if( get_address( &val,src ))
            {
                printf("Bad Address Location -RO\n");
                exit(-1);
            }
            if( set_register( val, dst ))
            {
                printf("Bad Register access \n");
                exit(-1);
            }
            break;
        case REG_ADDR :
            if( get_register( &val,src ))
            {
                printf("Bad Register Access \n");
                exit(-1);
            }
            if( set_address( val,dst))
            {
                printf("Bad Address Location -RO \n");
            }
            break;
        default :
            printf("Bad addressing mode\n");
    }
}

uint8_t Radio_Transmit(radiopacket_t* payload, RADIO_TX_WAIT wait)
{
	//if (block && transmit_lock) while (transmit_lock);
	//if (!block && transmit_lock) return 0;
	uint8_t len = 32;

	// indicate that the driver is transmitting.
    transmit_lock = 1;

	// disable the radio while writing to the Tx FIFO.
    ioport_set_pin_low (CE);

	set_tx_mode();

    // for auto-ack to work, the pipe0 address must be set to the Tx address while the radio is transmitting.
    // The register will be set back to the original pipe 0 address when the TX_DS or MAX_RT interrupt is asserted.
    set_register(RX_ADDR_P0, (uint8_t*)tx_address, ADDRESS_LENGTH);

    // transfer the packet to the radio's Tx FIFO for transmission
    send_instruction(W_TX_PAYLOAD, payload, NULL, len);

    // start the transmission.
    ioport_set_pin_high (CE);

    if (wait == RADIO_WAIT_FOR_TX)
    {
    	while (transmit_lock);
    	return tx_last_status;
    }

    return RADIO_TX_SUCCESS;
}

/**
 * Reset the pipe 0 address if pipe 0 is enabled.  This is necessary when the radio is using Enhanced Shockburst, because
 * the pipe 0 address is set to the transmit address while the radio is transmitting (this is how the radio receives
 * auto-ack packets).
 */
static void reset_pipe0_address(void)
{
	if (rx_pipe_widths[RADIO_PIPE_0] != 0)
	{
		// reset the pipe 0 address if pipe 0 is enabled.
		set_register(RX_ADDR_P0, (uint8_t*)rx_pipe0_address, ADDRESS_LENGTH);
	}
}

void Value::force_to_byte_register() {
  GUARANTEE(in_register(), "must be in register");
  if (!Assembler::is_valid_byte_register(lo_register())) {
    Assembler::Register byte_register = RegisterAllocator::allocate_byte_register();
    Compiler::code_generator()->movl(byte_register, lo_register());
    set_register(byte_register);
  }
}

/*
 * reset fpga SW register to their safe values
 * used if other code uses the SW register to set hardware
 *
 *  Author Greg Brissey    1/12/05
 */
void resetSafeVals()
{
   extern void resetClearReg();
   extern void resetDelays();
   set_register(GRADIENT,SoftwareGates,SafeGateVal);
   set_register(GRADIENT,SoftwareUser,0);
   set_register(GRADIENT,SoftwareXAmp,SafeXAmpVal);
   set_register(GRADIENT,SoftwareYAmp,SafeYAmpVal);
   set_register(GRADIENT,SoftwareZAmp,SafeZAmpVal);
   set_register(GRADIENT,SoftwareB0Amp,SafeB0AmpVal);
   set_register(GRADIENT,SoftwareXEcc,SafeXEccVal);
   set_register(GRADIENT,SoftwareYEcc,SafeYEccVal);
   set_register(GRADIENT,SoftwareZEcc,SafeZEccVal);
   set_register(GRADIENT,SoftwareB0Ecc,SafeB0EccVal);
   resetClearReg();
   resetDelays();
}

static int Oled_set (void * _self, va_list *app)
{
	struct Oled *self = _self;
	char *text =  va_arg(*app, char *);
	BYTE type =  va_arg(*app, const BYTE);	
	BYTE bkground_colour =  va_arg(*app, const BYTE);	
	BYTE x =  va_arg(*app, const BYTE);
	BYTE y =  va_arg(*app, const BYTE);
	if(set_register(self->devaddress,COMMAND_MODE,bkground_colour))//Reset Mode register
		return -1;
	
	if(type != IMAGE)
	{
		setXY(self->devaddress,x,y);
		int i;
		while(*text)
		{
			for(i=0;i<8;i++)
			{
				SendChar(self->devaddress,myFont[(*text-0x20)][i]);
			}			
			text++;
		}
	}
	else
	{
		int dim =  va_arg(*app, const int);
		int i = 0;
		//set memory addressing
		if(set_register(self->devaddress,COMMAND_MODE,0x00))//Reset memory addressing
			return -1;
		if(set_register(self->devaddress,COMMAND_MODE,x))//Reset memory addressing
			return -1;
		if(set_register(self->devaddress,COMMAND_MODE,y))//Reset memory addressing
			return -1;
		for(i=0;i<dim;i++)
		{
			SendChar(self->devaddress,*text);
			text++;

		}
	}	
			
	
	return 0;
}

void send_reload_radio (){
  
  set_rf_mode(RF_STANDBY);
  set_rf_mode(RF_SYNTHESIZER);
  set_register(FTPRIREG, (FTPRIREG_SET & 0xFD) | 0x02);
  set_rf_mode(RF_STANDBY);
  set_rf_mode(RF_RECEIVER);
}

// default transmitter address is 0xe7e7e7e7e7.
void Radio_Set_Tx_Addr(uint8_t* address)
{
	tx_address[0] = address[0];
	tx_address[1] = address[1];
	tx_address[2] = address[2];
	tx_address[3] = address[3];
	tx_address[4] = address[4];
	set_register(TX_ADDR, address, ADDRESS_LENGTH);
}

/*
 * all board type specific initializations done here.
 *
 */
initBrdSpecific(int bringup_mode)
{
   XYZshims[0] = XYZshims[1] = XYZshims[2];
   initSPI();   /* initialize SPI to proper configuration */
   startGradParser(GRADPARSER_TASK_PRIORITY, STD_TASKOPTIONS, STD_STACKSIZE);
   set_register(GRADIENT,FIFOECCCalcSelect,3);
   gradCmdPubPatternSub();
   startShimRestorer(200,STD_TASKOPTIONS, STD_STACKSIZE);
}