static uint16_t read_val12(spi_device dev, adxl362_axis_t axis)
{
        adxl362_reg_t reg_h, reg_l;
        uint8_t val_h, val_l;

        switch (axis) {
        case ADXL362_AXIS_X:
                reg_h = ADXL362_REG_XDATA_H;
                reg_l = ADXL362_REG_XDATA_L;
                break;
        case ADXL362_AXIS_Y:
                reg_h = ADXL362_REG_YDATA_H;
                reg_l = ADXL362_REG_YDATA_L;
                break;
        case ADXL362_AXIS_Z:
                reg_h = ADXL362_REG_ZDATA_H;
                reg_l = ADXL362_REG_ZDATA_L;
                break;
        default:
                return 0xFFFF;
        }

        val_h = read_register(dev, reg_h);
        val_l = read_register(dev, reg_l);

        return create_value12(val_h, val_l);
}

// read latest values from sensor and fill in x,y and totals
void
AP_OpticalFlow_ADNS3080::update(uint32_t now)
{
    uint8_t motion_reg;
    surface_quality = read_register(ADNS3080_SQUAL);
    hal.scheduler->delay_microseconds(50);

    // check for movement, update x,y values
    motion_reg = read_register(ADNS3080_MOTION);
    // check if we've had an overflow
    _overflow = ((motion_reg & 0x10) != 0);
    if( (motion_reg & 0x80) != 0 ) {
        raw_dx = ((int8_t)read_register(ADNS3080_DELTA_X));
        hal.scheduler->delay_microseconds(50);
        raw_dy = ((int8_t)read_register(ADNS3080_DELTA_Y));
        _motion = true;
    }else{
        raw_dx = 0;
        raw_dy = 0;
    }

    last_update = hal.scheduler->millis();

    apply_orientation_matrix();
}

// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
void
AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed)
{
    NumericIntType aNum;
    aNum.uintValue = shutter_speed;

    // set shutter speed to manual
    set_shutter_speed_auto(false);
    delayMicroseconds(50);      // small delay

    // set specific shutter speed
    write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
    delayMicroseconds(50);      // small delay
    write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
    delayMicroseconds(50);      // small delay

    // larger delay
    delay(50);

    // need to update frame period to cause shutter value to take effect
    aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
    delayMicroseconds(50);      // small delay
    aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
    delayMicroseconds(50);      // small delay
    write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
    delayMicroseconds(50);      // small delay
    write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
    delayMicroseconds(50);      // small delay
}

// read latest values from sensor and fill in x,y and totals
bool
AP_OpticalFlow_ADNS3080::update()
{
    byte motion_reg;
    surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
    delayMicroseconds(50);      // small delay

    // check for movement, update x,y values
    motion_reg = read_register(ADNS3080_MOTION);
    _overflow = ((motion_reg & 0x10) != 0);      // check if we've had an overflow
    if( (motion_reg & 0x80) != 0 ) {
        raw_dx = ((char)read_register(ADNS3080_DELTA_X));
        delayMicroseconds(50);          // small delay
        raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
        _motion = true;
    }else{
        raw_dx = 0;
        raw_dy = 0;
    }

    last_update = millis();

    apply_orientation_matrix();

    return true;
}

int em8300_setregblock(struct em8300_s *em, int offset, int val, int len)
{
	int i;

	for (i = 1000; i; i--) {
		if (!read_register(0x1c1a)) {
			break;
		}
		if (!i) {
			return -ETIME;
		}
	}
#if 0 /* FIXME: was in the zeev01 branch, verify if it is necessary */
	val = val | (val << 8) | (val << 16) | (val << 24);
#endif

	writel(offset & 0xffff, &em->mem[0x1c11]);
	writel((offset >> 16) & 0xffff, &em->mem[0x1c12]);
	writel(len, &em->mem[0x1c13]);
	writel(len, &em->mem[0x1c14]);
	writel(0, &em->mem[0x1c15]);
	writel(1, &em->mem[0x1c16]);
	writel(1, &em->mem[0x1c17]);
	writel(offset & 0xffff, &em->mem[0x1c18]);
	writel(0, &em->mem[0x1c19]);

	writel(1, &em->mem[0x1c1a]);

	for (i = 0; i < len / 4; i++) {
		writel(val, &em->mem[0x11800]);
	}

	switch (len % 4) {
	case 1:
		writel(val, &em->mem[0x10000]);
		break;
	case 2:
		writel(val, &em->mem[0x10800]);
		break;
	case 3:
		writel(val, &em->mem[0x11000]);
		break;
	}

	for (i = 1000; i; i--) {
		if (!read_register(0x1c1a)) {
			break;
		}
		if (!i) {
			return -ETIME;
		}
	}

#if 0 /* FIXME: was in zeev01 branch, verify if it is necessary */
	if (em8300_waitfor(em, 0x1c1a, 0, 1))
		return -ETIME;
#endif

	return 0;
}

void Radio::startListening(void) {
  write_register(CONFIG, read_register(CONFIG) | _BV(PRIM_RX));
  write_register(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT));

  if (read_register(FEATURE) & _BV(EN_ACK_PAY)) {
    flush_tx();
  }
}

irom io_error_t io_mcp_get_pin_info(string_t *dst, const struct io_info_entry_T *info, io_data_pin_entry_t *pin_data, const io_config_pin_entry_t *pin_config, int pin)
{
	int bank, bankpin, tv;
	int io, olat, cached;
	mcp_data_pin_t *mcp_pin_data;

	bank = (pin & 0x08) >> 3;
	bankpin = pin & 0x07;

	mcp_pin_data = &mcp_data_pin_table[info->instance][pin];

	switch(pin_config->llmode)
	{
		case(io_pin_ll_input_analog):
		{
			if(read_register(dst, info->address, GPIO(bank), &tv) != io_ok)
				return(io_error);

			string_format(dst, "current io: %s", onoff(tv & (1 << bankpin)));

			break;
		}

		case(io_pin_ll_counter):
		{
			if(read_register(dst, info->address, GPIO(bank), &tv) != io_ok)
				return(io_error);

			string_format(dst, "current io: %s, debounce: %d", onoff(tv & (1 << bankpin)), mcp_pin_data->debounce);

			break;
		}

		case(io_pin_ll_output_digital):
		{
			if(read_register(dst, info->address, GPIO(bank), &tv) != io_ok)
				return(io_error);

			io = tv & (1 << bankpin);

			if(read_register(dst, info->address, OLAT(bank), &tv) != io_ok)
				return(io_error);

			olat = tv & (1 << bankpin);
			cached = pin_output_cache[bank] & (1 << bankpin);

			string_format(dst, "current latch: %s, io: %s, cache: %s", onoff(io), onoff(olat), onoff(cached));

			break;
		}

		default:
		{
		}
	}

	return(io_ok);
}

// get frame period
unsigned int
AP_OpticalFlow_ADNS3080::get_frame_period()
{
    NumericIntType aNum;
    aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
    delayMicroseconds(50);      // small delay
    aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
    return aNum.uintValue;
}

// Public Methods //////////////////////////////////////////////////////////////
// init - initialise sensor
// assumes SPI bus has been initialised but will attempt to initialise 
// nonstandard SPI3 bus if required
bool
AP_OpticalFlow_ADNS3080::init()
{
    int8_t retry = 0;
    bool retvalue = false;

    // suspend timer while we set-up SPI communication
    hal.scheduler->suspend_timer_procs();

    if( _reset_pin != 0)
        hal.gpio->pinMode(_reset_pin, GPIO_OUTPUT);
    // reset the device
    reset();

    // check 3 times for the sensor on standard SPI bus
    _spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI0);
    if (_spi == NULL) {
        retvalue = false; goto finish;
    }

    while( retvalue == false && retry < 3 ) {
        if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
            retvalue = true;
            goto finish;
        }
        retry++;
    }

    // if not found, check 3 times on SPI3
    _spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI3);
    if (_spi == NULL) {
        retvalue = false; goto finish;
    }
    retry = 0;
    while( retvalue == false && retry < 3 ) {
        if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
            retvalue = true;
        }
        retry++;
    }
    // If we fail to find on SPI3, no connection available.
    retvalue = false;
    _spi = NULL;

finish:
    // resume timer
    hal.scheduler->resume_timer_procs();

    // if device is working register the global static read function to
    // be called at 1khz
    if( retvalue ) {
        hal.scheduler->register_timer_process( AP_OpticalFlow_ADNS3080::read );
    }

    return retvalue;
}

void
single_step ()
{
  branch_type br, isannulled();
  CORE_ADDR pc;
  long pc_instruction;

  if (!one_stepped)
    {
      /* Always set breakpoint for NPC.  */
      next_pc = read_register (NPC_REGNUM);
      npc4 = next_pc + 4; /* branch not taken */

      target_insert_breakpoint (next_pc, break_mem[0]);
      /* printf ("set break at %x\n",next_pc); */

      pc = read_register (PC_REGNUM);
      pc_instruction = read_memory_integer (pc, sizeof(pc_instruction));
      br = isannulled (pc_instruction, pc, &target);
      brknpc4 = brktrg = 0;

      if (br == bicca)
	{
	  /* Conditional annulled branch will either end up at
	     npc (if taken) or at npc+4 (if not taken).
	     Trap npc+4.  */
	  brknpc4 = 1;
	  target_insert_breakpoint (npc4, break_mem[1]);
	}
      else if (br == baa && target != next_pc)
	{
	  /* Unconditional annulled branch will always end up at
	     the target.  */
	  brktrg = 1;
	  target_insert_breakpoint (target, break_mem[2]);
	}

      /* We are ready to let it go */
      one_stepped = 1;
      return;
    }
  else
    {
      /* Remove breakpoints */
      target_remove_breakpoint (next_pc, break_mem[0]);

      if (brknpc4)
	target_remove_breakpoint (npc4, break_mem[1]);

      if (brktrg)
	target_remove_breakpoint (target, break_mem[2]);

      one_stepped = 0;
    }
}

params read_params()
{
  params read_params;

  read_params.feedback = read_register(FEEDBACK_ADDR);
  read_params.commanded_freq = read_register(COMMANDED_FREQ_ADDR);
  read_params.output_current = read_register(OUTPUT_CURRENT_ADDR);
  read_params.output_voltage = read_register(OUTPUT_VOLTAGE_ADDR);

  return read_params;
}

int Sonar::read() {
    if(!this->has_init) {
        init();
    }
    set_mode(SINGLE_SHOT);
    if(sonar_mode == METRIC) {
        return read_register(RESULT_1);
    }
    else {
        return ( (read_register(RESULT_1))  * 3937)/1000;
    }
}

int ni_tio_rinsn(struct ni_gpct *counter, struct comedi_insn *insn,
		 unsigned int *data)
{
	struct ni_gpct_device *counter_dev = counter->counter_dev;
	const unsigned channel = CR_CHAN(insn->chanspec);
	unsigned first_read;
	unsigned second_read;
	unsigned correct_read;

	if (insn->n < 1)
		return 0;
	switch (channel) {
	case 0:
		ni_tio_set_bits(counter,
				NITIO_Gi_Command_Reg(counter->counter_index),
				Gi_Save_Trace_Bit, 0);
		ni_tio_set_bits(counter,
				NITIO_Gi_Command_Reg(counter->counter_index),
				Gi_Save_Trace_Bit, Gi_Save_Trace_Bit);
		/* The count doesn't get latched until the next clock edge, so it is possible the count
		   may change (once) while we are reading.  Since the read of the SW_Save_Reg isn't
		   atomic (apparently even when it's a 32 bit register according to 660x docs),
		   we need to read twice and make sure the reading hasn't changed.  If it has,
		   a third read will be correct since the count value will definitely have latched by then. */
		first_read =
		    read_register(counter,
				  NITIO_Gi_SW_Save_Reg(counter->counter_index));
		second_read =
		    read_register(counter,
				  NITIO_Gi_SW_Save_Reg(counter->counter_index));
		if (first_read != second_read)
			correct_read =
			    read_register(counter,
					  NITIO_Gi_SW_Save_Reg(counter->
							       counter_index));
		else
			correct_read = first_read;
		data[0] = correct_read;
		return 0;
		break;
	case 1:
		data[0] =
		    counter_dev->
		    regs[NITIO_Gi_LoadA_Reg(counter->counter_index)];
		break;
	case 2:
		data[0] =
		    counter_dev->
		    regs[NITIO_Gi_LoadB_Reg(counter->counter_index)];
		break;
	}
	return 0;
}

void Radio::stopListening(void) {
  if (read_register(FEATURE) & _BV(EN_ACK_PAY)) {
    _delay_us(155);
    flush_tx();
  }

  write_register(CONFIG, (read_register(CONFIG)) & ~_BV(PRIM_RX));

  // for 3 pins solution TX mode is only left with additional powerDown/powerUp cycle.
  powerDown();
  powerUp();
}

int lem_interrupt(void)
{
	uint16_t cycles = 0;
	uint16_t b = read_register(REG_B);

	switch (read_register(REG_A)) {
	case 0:
		if (b) {
			if (connected == 0) {
				clock_gettime(CLOCK_REALTIME, &tp_prev);
				// TODO don't start up for ~1s
			}
			connected = 1;
			vram = b;
		} else {
			connected = 0;
		}
		break;
	case 1:
		if (b) {
			custom_font = 1;
			font_ram = b;
		} else {
			custom_font = 1;
		}
		break;
	case 2:
		if (b) {
			custom_pal = 1;
			pal_ram = b;
		} else {
			custom_pal = 1;
		}
		break;
	case 3:
		border_col = b;
		break;
	case 4:
		write_memory(b, FONT_LEN, font);
		cycles = 256;
		break;
	case 5:
		write_memory(b, PAL_LEN, pal);
		cycles = 16;
		break;
	default:
		break;
	}

	return cycles;
}

bool Radio::setup(void) {
  HalfDuplexSPI::setup();

  csnHigh();

  // Must allow the radio time to settle else configuration bits will not necessarily stick.
  // This is actually only required following power up but some settling time also appears to
  // be required after resets too. For full coverage, we'll always assume the worst.
  // Enabling 16b CRC is by far the most obvious case if the wrong timing is used - or skipped.
  // Technically we require 4.5ms + 14us as a worst case. We'll just call it 5ms for good measure.
  // WARNING: Delay is based on P-variant whereby non-P *may* require different timing.
  _delay_ms(5);

  // Reset CONFIG and enable 16-bit CRC.
  write_register(CONFIG, 0 | _BV(EN_CRC) | _BV(CRCO));

  // Set 1500uS (minimum for 32B payload in [email protected]) timeouts, to make testing a little easier
  // WARNING: If this is ever lowered, either 250KBS mode with AA is broken or maximum packet
  // sizes must never be used. See documentation for a more complete explanation.
  setRetries(5, 15);

  uint8_t setup = read_register(RF_SETUP);

  // Then set the data rate to the slowest (and most reliable) speed supported by all
  // hardware.
  setDataRate(DataRate::RATE_1MBPS);

  write_register(FEATURE, 0);
  write_register(DYNPD, 0);

  // Reset current status
  // Notice reset and flush is the last thing we do
  write_register(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT));

  setChannel(76);

  // Flush buffers
  flush_rx();
  flush_tx();

  //Power up by default when setup() is called.
  powerUp();

  // Enable PTX, do not write CE high so radio will remain in standby I mode ( 130us max to transition to RX or TX
  // instead of 1500us from powerUp ) PTX should use only 22uA of power.
  write_register(CONFIG, (read_register(CONFIG)) & ~_BV(PRIM_RX));

  // If setup is 0 or ff then there was no response from module.
  return setup != 0 && setup != 0xff;
}

float MAX44009::get_lux(void)
{
	int luxHigh = read_register(0x03);
	
	int luxLow = read_register(0x04);
	
	int exponent = (luxHigh & 0xf0) >> 4;
	
	int mant = (luxHigh & 0x0f) << 4 | luxLow;
	
	
	return (float)(((0x00000001 << exponent) * (float)mant) * 0.045);
	//return (float)((pow(2,exponent) * mant) * 0.045);
 
}

void set_time(struct tm *t)
{
  int regA, regB;

  if (Wflag) {
	/* Set A and B registers to their proper values according to the AT
	 * reference manual.  (For if it gets messed up, but the BIOS doesn't
	 * repair it.)
	 */
	write_register(RTC_REG_A, RTC_A_DV_OK | RTC_A_RS_DEF);
	write_register(RTC_REG_B, RTC_B_24);
  }

  /* Inhibit updates. */
  regB= read_register(RTC_REG_B);
  write_register(RTC_REG_B, regB | RTC_B_SET);

  t->tm_mon++;	/* Counts from 1. */

  if (y2kflag) {
	/* Set the clock back 20 years to avoid Y2K bug, good until 2020. */
	if (t->tm_year >= 100) t->tm_year -= 20;
  }

  if ((regB & 0x04) == 0) {
	/* Convert binary to BCD (default RTC mode) */
	t->tm_year = dec_to_bcd(t->tm_year % 100);
	t->tm_mon = dec_to_bcd(t->tm_mon);
	t->tm_mday = dec_to_bcd(t->tm_mday);
	t->tm_hour = dec_to_bcd(t->tm_hour);
	t->tm_min = dec_to_bcd(t->tm_min);
	t->tm_sec = dec_to_bcd(t->tm_sec);
  }
  write_register(RTC_YEAR, t->tm_year);
  write_register(RTC_MONTH, t->tm_mon);
  write_register(RTC_MDAY, t->tm_mday);
  write_register(RTC_HOUR, t->tm_hour);
  write_register(RTC_MIN, t->tm_min);
  write_register(RTC_SEC, t->tm_sec);

  /* Stop the clock. */
  regA= read_register(RTC_REG_A);
  write_register(RTC_REG_A, regA | RTC_A_DV_STOP);

  /* Allow updates and restart the clock. */
  write_register(RTC_REG_B, regB);
  write_register(RTC_REG_A, regA);
}

/*
 * Reads current pressure value
 */
long SCP1000::pressure()
{
	unsigned long pressure;
	
	// Pressure value is in 19-bit unsigned format
	// Value = Pa * 4
	pressure = read_register(PRESSURE, 1);  // Read MSB
	pressure &= 0b00000111;					// mask unused bits
	pressure <<= 16;						// shift into upper word
	
	pressure  |= read_register(PRESSURE_LSB, 2); // read low word
	
	// Convert to real pressure in hPa
	return pressure>>2;
	
}

void
ecompass_init(void)
{
    DECO("Starting ecompass init...");
    write_register(CTRL_REG1, 0x37);
    transfer(ECOMPASS_ID);
    // Acceleromter data ready on int1
    //write_register(CTRL_REG3, 1 << 2); 
    // Magnometer data ready on int2
    //write_register(CTRL_REG4, 1 << 2);

    write_register(CTRL_REG5, 0x8);
    transfer(ECOMPASS_ID);
    write_register(CTRL_REG7, 0x0);
    transfer(ECOMPASS_ID);
	udelay(400000);

    int ret;
    int ret_i = read_register(WHO_AM_I_REG | READ_MASK);
    transfer(ECOMPASS_ID); 
    if ((ret = get_data(ret_i)) != WHO_AM_I) {
        DECO("E-Comapss initilisation failed (who_am_i is %d)\n", ret);
    } else {
        DECO("Init success!\n");
    }

    //udelay(500000);
    //calibrate();
}