// Copyright (c) 2026 Rigless Labs.
// SPDX-License-Identifier: MIT OR Apache-2.0
//! The MPU-9250, an inertial unit: a gyroscope and an accelerometer on a serial interface, a FIFO a
//! driver drains in bursts, and an I2C master inside the package with the [`Ak8963`] magnetometer die on
//! the far side of it.
//!
//! Written against the MPU-9250 Register Map and Descriptions, `RM-MPU-9250A-00` rev 1.4,
//! 9/9/2013, cited as **RM**, and the MPU-9250 Product Specification, `PS-MPU-9250A-01` rev 1.1,
//! 06/20/2016, cited as **PS**. Every section below is one or the other's.
//!
//! **The FIFO is derived from virtual time.** The part samples eight thousand times a second and a
//! driver reads a thousand times a second. Nothing is scheduled per sample. The count of samples
//! since the last reset is computed at each select from the rate the registers give. The samples
//! not yet materialized are generated then from the signals as held, into the FIFO and the data
//! registers. What a driver can observe is the same. What it can't is a signal changing between two
//! reads, and the samples either side of the change differing.
//!
//! | Register or section | Behavior | Source | Test |
//! |---|---|---|---|
//! | The serial protocol | The first byte after the select falls is an address whose top bit is the direction, "Read (1) or Write (0)". Every byte after it is the next register, "Single or Burst Read/Writes" | PS 7.5 | `every_register_reads_and_writes_at_its_own_address_and_bursts_increment` |
//! | Reset values | `0x00` for every register "other than … Register 107 (0x01) Power Management 1 and Register 117 (0x71) `WHO_AM_I`" | RM 3 | `who_am_i_is_0x71`, `a_reset_restores_the_defaults_and_clears_itself` |
//! | `WHO_AM_I` | `0x71`. RM 4.38's header says "Reset value: 0x68" and its text "The default value of the register is 0x71". The register table and the driver that reads this part say `0x71` | RM 3, 4.38 | `who_am_i_is_0x71` |
//! | `PWR_MGMT_1` | `H_RESET` "restores the default settings … the bit will auto clear". `SLEEP` and `CLKSEL` 7 stop the sampling clock, and the other codes run it | RM 4.34 | `a_reset_restores_the_defaults_and_clears_itself`, `a_sleeping_unit_takes_no_samples` |
//! | `PWR_MGMT_2` | An axis disabled reads zero: "Reset value: 0x00 (if sensor disabled)" | RM 4.35, 4.22 | `a_disabled_axis_reads_zero` |
//! | `USER_CTRL` | `FIFO_EN` enables the FIFO and `I2C_MST_EN` the master. `I2C_IF_DIS`, `FIFO_RST`, `I2C_MST_RST` and `SIG_COND_RST` "auto clear", the second emptying the FIFO, the third recomputing the external data allocation, the fourth clearing "all the sensor registers" | RM 4.33 | `a_fifo_reset_empties_it`, `the_allocation_is_recomputed_only_when_all_are_disabled_or_the_master_is_reset` |
//! | The sample rate | 8 kHz with `FCHOICE_B` non-zero or `DLPF_CFG` 0 or 7, else 1 kHz over `1 + SMPLRT_DIV`, the divider "only used for 1kHz internal sampling" | RM 4.4, 4.5, 4.6 | `the_sample_rate_follows_the_filter_and_the_divider` |
//! | The words | Gyro at `2^16 LSB / 500dps` halved per step of `GYRO_FS_SEL`, which is PS table 1's 131, 65.5, 32.8 and 16.4 LSB/(°/s), less `X_OFFS_USR × 4 / 2^FS_SEL`. Accelerometer at 16384, 8192, 4096 or 2048 LSB/g by `ACCEL_FS_SEL`, less the fifteen-bit offset in 0.98 mg steps. Temperature `(TEMP_OUT − 0) / 333.87 + 21 °C`. All big-endian, sixteen bits | RM 4.3, 4.22–4.24, 4.39, PS 3.1, 3.2, 3.4.2 | `the_words_are_the_signals_at_the_selected_range`, `the_temperature_word_is_the_specifications`, `the_offset_registers_survive_a_round_trip_and_are_subtracted` |
//! | `FIFO_EN` | Temperature, each gyro axis and the accelerometer written "at the sample rate", "in order of register number" | RM 4.11, 4.37 | `the_fifo_holds_what_fifo_en_says_in_register_order` |
//! | `FIFO_COUNT`, `FIFO_R_W` | 512 bytes. The count is thirteen bits, and "Reading this byte latches the data for both". A read takes the oldest byte, and empty "will return the last byte that was previously read". Full, `FIFO_OFLOW_INT` is set and the oldest is dropped, unless `FIFO_MODE` keeps the oldest and drops the new | RM 4.36, 4.37, 4.5, PS 4.17 | `the_fifo_count_is_the_samples_since_the_reset`, `an_empty_fifo_answers_the_last_byte`, `the_fifo_stops_at_512_under_fifo_mode_and_drops_the_oldest_otherwise` |
//! | `INT_STATUS`, `INT_PIN_CFG` | `RAW_DATA_RDY_INT` at each sample, and `FIFO_OVERFLOW_INT`. "R/C", cleared by its own read, or by any read under `INT_ANYRD_2CLEAR` | RM 4.19–4.21 | `raw_data_ready_is_set_by_a_sample_and_cleared_as_int_pin_cfg_says` |
//! | The I2C master | Each enabled slave 0 to 3 with a length is run "at the Sample Rate … in order of Slave number", a read landing "in order of slave number, starting from `EXT_SENS_DATA_00`", a write sending its `DO` byte. `REG_DIS` skips the register byte. `I2C_MST_P_NSR` puts "a stop between reads" or a restart. A slave with `DLY_EN` runs "every (1+`I2C_MST_DLY`) samples". The allocation is "recomputed only when (1) all slaves are disabled, or (2) the `I2C_MST_RST` bit is set". Slave 4 transfers once and its `EN` "is cleared when a single transfer is complete", a read landing in `I2C_SLV4_DI`. A slave that doesn't acknowledge sets its `NACK` bit in `I2C_MST_STATUS`, which a read clears, and a chip the board silenced on the auxiliary bus never acknowledges | RM 4.12–4.18, 4.25–4.30 | `a_slave_read_lands_in_ext_sens_data_in_slave_order`, `a_slave_write_sends_its_do_byte`, `slave_4_transfers_once_and_clears_its_enable`, `a_slave_that_does_not_acknowledge_sets_its_nack_bit`, `a_silent_auxiliary_chip_does_not_acknowledge`, `the_allocation_is_recomputed_only_when_all_are_disabled_or_the_master_is_reset`, `a_delayed_slave_runs_every_n_plus_one_samples`, `the_master_puts_a_stop_or_a_restart_between_reads_as_p_nsr_says` |
//! | Unmapped addresses | RM 3 lists what exists. An address it doesn't list reads zero and takes no write | RM 3 | `every_register_reads_and_writes_at_its_own_address_and_bursts_increment` |
//! | What it measures | Angular rate in thousandths of a degree per second, specific force in millionths of a gravity, temperature in hundredths of a degree, on the unit's own axes, PS figure 4. Set them with [`Mpu9250::set_rate`], [`Mpu9250::set_acceleration`] and [`Mpu9250::set_temperature`]. The field is [`Mpu9250::set_field`] on the package's axes, turned onto the die's, figure 5 against figure 4, and reaches [`Ak8963::set_field`] through [`Mpu9250::magnetometer`] | PS 9.1 | `the_words_are_the_signals_at_the_selected_range`, `sensing_puts_the_field_on_the_compass_die_s_own_axes` |
//!
//! # What is not, and why
//!
//! | Departure | Reason |
//! |---|---|
//! | A signal changing between two reads | Every sample not yet materialized is generated at the read from the value held then, so the samples either side of the change don't differ. A driver averaging eight into a millisecond can't tell. The accelerometer's 4 kHz against the gyro's 8 kHz is the same rule, and comes out the same. As the driver puts it, "every 2nd accel sample is a duplicate" |
//! | The master running once per slave per read | The part runs each enabled slave at every sample. The model runs it once for the samples that elapsed, since only the last result is in the registers. The delay rule is still counted per sample |
//! | `BYTE_SW`, `GRP`, `WAIT_FOR_ES`, `MULT_MST_EN`, `DELAY_ES_SHADOW`, the master clock divider | Stored, and the driver sets none of them. Swapping bytes would be a line, and isn't written without a reader |
//! | External sensor data in the FIFO, `SLV_x` bits of `FIFO_EN` and `SLV_3_FIFO_EN` | Stored. The driver keeps the slaves out of its FIFO |
//! | The digital low-pass filters, self-test, wake-on-motion, `LP_ACCEL_ODR`, `CYCLE`, `GYRO_STANDBY`, `FSYNC`, `EXT_SYNC_SET` | Stored. A filter's bandwidth is nothing to a signal a plant holds |
//! | The `INT` pin | Not driven. A chip's seam carries no line back to a pin, and a firmware that waits on this one needs that line first |
//! | Start-up time from sleep, 35 ms for the gyroscope and 30 ms for the accelerometer | PS 3.1 and 3.2. Data is valid from the first sample. The driver waits 100 ms after its reset |
//! | The clock ceilings of 1 MHz and 20 MHz on `SCLK` | The seam carries words, not a clock |
//! | Writes to `FIFO_R_W` | "Read/Write command", so a write would push a byte. The driver never writes there, and a write is dropped |
use crate::chip::Timeline;
use crate::chip::ak8963::Ak8963;
use crate::chip::invensense::{self, Access, Fifo, SampleClock, Serial, Signals};
use crate::chip::spi::Peer;
use crate::chip::{Place, i2c};
use crate::snapshot::{self, Reader, Store, Writer};
use crate::time::VirtualNanos;
/// RM 3's register map, by address.
const XG_OFFSET_H: usize = 0x13;
const SMPLRT_DIV: usize = 0x19;
const CONFIG: usize = 0x1A;
const GYRO_CONFIG: usize = 0x1B;
const ACCEL_CONFIG: usize = 0x1C;
const ACCEL_CONFIG_2: usize = 0x1D;
const FIFO_EN: usize = 0x23;
const I2C_MST_CTRL: usize = 0x24;
const I2C_SLV0_ADDR: usize = 0x25;
const I2C_SLV4_ADDR: usize = 0x31;
const I2C_SLV4_REG: usize = 0x32;
const I2C_SLV4_DO: usize = 0x33;
const I2C_SLV4_CTRL: usize = 0x34;
const I2C_SLV4_DI: usize = 0x35;
const I2C_MST_STATUS: usize = 0x36;
const INT_PIN_CFG: usize = 0x37;
const INT_STATUS: usize = 0x3A;
const ACCEL_XOUT_H: usize = 0x3B;
const GYRO_ZOUT_L: usize = 0x48;
const EXT_SENS_DATA_00: usize = 0x49;
const I2C_SLV0_DO: usize = 0x63;
const I2C_MST_DELAY_CTRL: usize = 0x67;
const USER_CTRL: usize = 0x6A;
const PWR_MGMT_1: usize = 0x6B;
const PWR_MGMT_2: usize = 0x6C;
const FIFO_COUNTH: usize = 0x72;
const FIFO_COUNTL: usize = 0x73;
const WHO_AM_I: usize = 0x75;
const XA_OFFSET_H: usize = 0x77;
const REGISTERS: usize = 0x80;
/// The bits the model acts on, by register, RM 4.
const DEVICE_ID: u8 = 0x71;
const H_RESET: u8 = 0x80;
const SLEEP: u8 = 0x40;
const CLKSEL: u8 = 0x07;
const CLOCK_STOPPED: u8 = 0x07;
const FIFO_MODE: u8 = 0x40;
const DLPF_CFG: u8 = 0x07;
const FS_SEL: u8 = 0x18;
const FCHOICE_B: u8 = 0x03;
const I2C_MST_P_NSR: u8 = 0x10;
const I2C_SLV_RNW: u8 = 0x80;
const I2C_SLV_EN: u8 = 0x80;
const I2C_SLV_REG_DIS: u8 = 0x20;
const I2C_SLV_LENG: u8 = 0x0F;
const I2C_MST_DLY: u8 = 0x1F;
const I2C_SLV4_DONE: u8 = 0x40;
const I2C_SLV4_NACK: u8 = 0x10;
const INT_ANYRD_2CLEAR: u8 = 0x10;
const FIFO_OVERFLOW_INT: u8 = 0x10;
const RAW_DATA_RDY_INT: u8 = 0x01;
const USER_FIFO_EN: u8 = 0x40;
const I2C_MST_EN: u8 = 0x20;
const I2C_IF_DIS: u8 = 0x10;
const FIFO_RST: u8 = 0x04;
const I2C_MST_RST: u8 = 0x02;
const SIG_COND_RST: u8 = 0x01;
const AUTO_CLEARING: u8 = I2C_IF_DIS | FIFO_RST | I2C_MST_RST | SIG_COND_RST;
const DISABLE_ACCEL: [u8; 3] = [0x20, 0x10, 0x08];
const DISABLE_GYRO: [u8; 3] = [0x04, 0x02, 0x01];
/// PS 4.17: "a 512-byte FIFO register".
pub const FIFO_BYTES: usize = invensense::FIFO_BYTES;
/// RM 4.5's table: the two internal sample rates.
const FAST_HZ: u64 = 8_000;
const SLOW_HZ: u64 = 1_000;
/// PS 3.4.2: 333.87 LSB per degree and no offset at 21 °C, as hundredths and a ten-thousandth.
const TEMP_HUNDREDTHS_PER_DEGREE: i64 = 33_387;
const TEMP_ZERO_CENTI: i32 = 2_100;
/// RM 4.25: twenty-four external sensor data registers.
const EXTERNAL_BYTES: usize = 24;
/// One slave's transaction as its registers describe it, RM 4.13 and 4.17.
#[derive(Debug, Clone, Copy, serde::Serialize, serde::Deserialize)]
struct Transfer {
/// `I2C_SLVx_ADDR`: the direction in bit 7, the address below it.
address: u8,
register: u8,
skip_register: bool,
length: usize,
out: u8,
stop_between: bool,
}
/// Slaves 0 to 3 of the master, RM 4.13 to 4.16: a slave's place in the external data registers.
#[derive(Debug, Clone, Copy, Default, serde::Serialize, serde::Deserialize)]
struct Allocation {
/// Offset and length in `EXT_SENS_DATA`, or `None` for a slave not allocated.
slots: [Option<(usize, usize)>; 4],
next: usize,
held: bool,
}
/// One MPU-9250, with its magnetometer die and whatever else a board puts on its auxiliary bus.
#[derive(Debug)]
pub struct Mpu9250 {
registers: [u8; REGISTERS],
signals: Signals,
clock: SampleClock,
fifo: Fifo,
last_byte: u8,
allocation: Allocation,
magnetometer: Ak8963,
others: Vec<Place<dyn i2c::Peer>>,
serial: Serial,
}
/// The unit keeps its registers, its sampling and its FIFO, and each chip on its auxiliary bus keeps
/// itself. The registers go as a slice, since serde stops at arrays of 32.
impl Store for Mpu9250 {
fn store(&self, to: &mut Writer) -> Result<(), snapshot::Error> {
let Self {
registers,
signals,
clock,
fifo,
last_byte,
allocation,
magnetometer,
others,
serial,
} = self;
to.put(®isters[..])?;
to.put(signals)?;
to.put(clock)?;
to.put(fifo)?;
to.put(last_byte)?;
to.put(allocation)?;
to.put(magnetometer)?;
to.put(serial)?;
others.store(to)
}
fn load(&mut self, from: &mut Reader) -> Result<(), snapshot::Error> {
let Self {
registers,
signals,
clock,
fifo,
last_byte,
allocation,
magnetometer,
others,
serial,
} = self;
let saved: Vec<u8> = from.take()?;
*registers = saved
.try_into()
.map_err(|saved: Vec<u8>| snapshot::Error::Shape {
what: "MPU-9250 registers",
saved: saved.len(),
built: REGISTERS,
})?;
*signals = from.take()?;
*clock = from.take()?;
*fifo = from.take()?;
*last_byte = from.take()?;
*allocation = from.take()?;
*magnetometer = from.take()?;
*serial = from.take()?;
others.load(from)
}
}
impl Default for Mpu9250 {
fn default() -> Self {
Self::new()
}
}
impl Mpu9250 {
/// A unit at power-on: the registers at RM 3's reset values, the FIFO empty, and the
/// magnetometer die on its bus. The signals are a still, level unit's at 21 °C, with no rate
/// and one gravity on Z.
#[must_use]
pub fn new() -> Self {
let mut unit = Self {
registers: [0; REGISTERS],
signals: Signals::still(TEMP_ZERO_CENTI),
clock: SampleClock::new(),
fifo: Fifo::new(),
last_byte: 0,
allocation: Allocation::default(),
magnetometer: Ak8963::new(),
others: Vec::new(),
serial: Serial::new(),
};
unit.reset(VirtualNanos(0));
unit
}
/// The angular rate the unit measures from now on, in thousandths of a degree per second on its axes.
pub fn set_rate(&mut self, x: i32, y: i32, z: i32) {
self.signals.rate = [x, y, z];
}
/// The specific force the unit measures from now on, in millionths of a gravity on its axes.
pub fn set_acceleration(&mut self, x: i32, y: i32, z: i32) {
self.signals.acceleration = [x, y, z];
}
/// The die temperature from now on, in hundredths of a degree.
pub fn set_temperature(&mut self, centi_celsius: i32) {
self.signals.centi_celsius = centi_celsius;
}
/// The magnetic field from now on, in nanotesla on the package's axes: the die inside sits turned
/// against them, PS figure 5 against figure 4, so the package's `y` is the die's `x`, the package's
/// `x` the die's `y`, and the package's `z` against the die's.
pub fn set_field(&mut self, x: i32, y: i32, z: i32) {
self.magnetometer.set_field(y, x, -z);
}
/// The magnetometer die on the package's own bus, to drive the field it measures.
pub fn magnetometer(&mut self) -> &mut Ak8963 {
&mut self.magnetometer
}
/// Hangs another chip on the auxiliary bus beside the die, which is what a board with a sensor on
/// `AUX_DA` and `AUX_CL` does.
pub fn attach_auxiliary(&mut self, peer: impl i2c::Peer + 'static) {
let peer: Box<dyn i2c::Peer> = Box::new(peer);
self.others.push(Place::new(peer));
}
/// Stops the nth chip the board put on the auxiliary bus answering, or lets it answer again: a
/// board condition, [`Place::silence`]. A silent chip never acknowledges, so its slave's `NACK`
/// bit sets. Each transaction runs whole, so it takes effect at the next one. The die inside the
/// package isn't the board's to silence. Answers whether a chip hangs there.
#[must_use]
pub fn silence_auxiliary(&mut self, nth: usize, silent: bool) -> bool {
self.others
.get_mut(nth)
.map(|place| place.silence(silent))
.is_some()
}
/// RM 3's reset values, the FIFO emptied, sampling restarted.
fn reset(&mut self, now: VirtualNanos) {
self.registers = [0; REGISTERS];
self.registers[PWR_MGMT_1] = 0x01;
self.registers[WHO_AM_I] = DEVICE_ID;
self.fifo.clear();
self.last_byte = 0;
self.allocation = Allocation::default();
self.clock.restart(now);
}
/// RM 4.5's table and 4.4: the sample rate the registers now give, in hertz, or nothing while the
/// clock is stopped or the unit asleep.
fn sample_hz(&self) -> u64 {
let power = self.registers[PWR_MGMT_1];
if power & SLEEP != 0 || power & CLKSEL == CLOCK_STOPPED {
return 0;
}
let dlpf = self.registers[CONFIG] & DLPF_CFG;
if self.registers[GYRO_CONFIG] & FCHOICE_B != 0 || dlpf == 0 || dlpf == 7 {
FAST_HZ
} else {
SLOW_HZ / (1 + u64::from(self.registers[SMPLRT_DIV]))
}
}
/// A change of rate or of running: the samples so far stand, the next come at the new rate.
fn rebase(&mut self, now: VirtualNanos) {
self.clock.rebase(now, self.sample_hz());
}
/// Bits 4:3 of a configuration register, the range step.
fn range_step(register: u8) -> u32 {
u32::from((register & FS_SEL) >> 3)
}
/// The fourteen bytes of one sample, registers 59 to 72 in order, from the signals as held.
fn sample(&self) -> [u8; invensense::SAMPLE_BYTES] {
let accel_step = Self::range_step(self.registers[ACCEL_CONFIG]);
let gyro_step = Self::range_step(self.registers[GYRO_CONFIG]);
let mut accel = [0; 3];
let mut gyro = [0; 3];
for axis in 0..3 {
if self.registers[PWR_MGMT_2] & DISABLE_ACCEL[axis] == 0 {
accel[axis] = self
.signals
.accel_counts(axis, accel_step, self.accel_offset(axis));
}
if self.registers[PWR_MGMT_2] & DISABLE_GYRO[axis] == 0 {
let offset = i64::from(i16::from_be_bytes([
self.registers[XG_OFFSET_H + 2 * axis],
self.registers[XG_OFFSET_H + 2 * axis + 1],
]));
gyro[axis] =
self.signals.gyro_counts(axis, gyro_step) - ((offset * 4) >> gyro_step);
}
}
let temperature = invensense::round(
i64::from(self.signals.centi_celsius - TEMP_ZERO_CENTI) * TEMP_HUNDREDTHS_PER_DEGREE,
10_000,
);
invensense::pack(accel, temperature, gyro)
}
/// RM 4.39: fifteen bits, `XA_OFFS[14:7]` in the high register and `[6:0]` in bits 7:1 of the low.
fn accel_offset(&self, axis: usize) -> i64 {
invensense::offset15(
self.registers[XA_OFFSET_H + 3 * axis],
self.registers[XA_OFFSET_H + 3 * axis + 1],
)
}
/// Everything that happened between the last select and `now`: the samples that fell due land in the
/// data registers and the FIFO, and the master runs the slaves that were due.
fn catch_up(&mut self, now: VirtualNanos, context: &mut Timeline<'_, '_>) {
let taken = self.clock.catch_up(now, self.sample_hz());
if taken.is_empty() {
return;
}
let sample = self.sample();
self.registers[ACCEL_XOUT_H..=GYRO_ZOUT_L].copy_from_slice(&sample);
self.registers[INT_STATUS] |= RAW_DATA_RDY_INT;
if self.registers[USER_CTRL] & USER_FIFO_EN != 0 {
let bytes = invensense::fifo_bytes(&sample, self.registers[FIFO_EN]);
let keep_oldest = self.registers[CONFIG] & FIFO_MODE != 0;
if self.fifo.push(&bytes, taken.end - taken.start, keep_oldest) {
self.registers[INT_STATUS] |= FIFO_OVERFLOW_INT;
}
}
if self.registers[USER_CTRL] & I2C_MST_EN != 0 {
self.run_master(taken.start + 1..=taken.end, context);
}
}
/// RM 4.25 and 4.30: which of slaves 0 to 3 fell due over the samples `cycles`, then each once.
fn run_master(
&mut self,
cycles: std::ops::RangeInclusive<u64>,
context: &mut Timeline<'_, '_>,
) {
let delay = u64::from(self.registers[I2C_SLV4_CTRL] & I2C_MST_DLY) + 1;
let delayed = self.registers[I2C_MST_DELAY_CTRL];
let p_nsr = self.registers[I2C_MST_CTRL] & I2C_MST_P_NSR != 0;
for slave in 0..4 {
let control = self.registers[I2C_SLV0_ADDR + 3 * slave + 2];
let length = usize::from(control & I2C_SLV_LENG);
if control & I2C_SLV_EN == 0 || length == 0 {
continue;
}
let runs = if delayed & (1 << slave) != 0 {
cycles.clone().any(|cycle| cycle % delay == 0)
} else {
true
};
if !runs {
continue;
}
let slot = self.allocate(slave, length);
let transfer = Transfer {
address: self.registers[I2C_SLV0_ADDR + 3 * slave],
register: self.registers[I2C_SLV0_ADDR + 3 * slave + 1],
skip_register: control & I2C_SLV_REG_DIS != 0,
length,
out: self.registers[I2C_SLV0_DO + slave],
stop_between: p_nsr,
};
match self.transaction(transfer, context) {
Some(data) => {
if let Some((offset, _)) = slot {
for (index, byte) in data.iter().enumerate() {
if offset + index < EXTERNAL_BYTES {
self.registers[EXT_SENS_DATA_00 + offset + index] = *byte;
}
}
}
}
None => self.registers[I2C_MST_STATUS] |= 1 << slave,
}
}
let control = self.registers[I2C_SLV4_CTRL];
if control & I2C_SLV_EN != 0 {
let transfer = Transfer {
address: self.registers[I2C_SLV4_ADDR],
register: self.registers[I2C_SLV4_REG],
skip_register: control & I2C_SLV_REG_DIS != 0,
length: 1,
out: self.registers[I2C_SLV4_DO],
stop_between: p_nsr,
};
match self.transaction(transfer, context) {
Some(data) => {
if let Some(byte) = data.first() {
self.registers[I2C_SLV4_DI] = *byte;
}
}
None => self.registers[I2C_MST_STATUS] |= I2C_SLV4_NACK,
}
self.registers[I2C_SLV4_CTRL] &= !I2C_SLV_EN;
self.registers[I2C_MST_STATUS] |= I2C_SLV4_DONE;
}
}
/// RM 4.25: a slave's registers, "in order of slave number, starting from `EXT_SENS_DATA_00`", held
/// until every slave is disabled or the master is reset.
fn allocate(&mut self, slave: usize, length: usize) -> Option<(usize, usize)> {
if !self.allocation.held {
self.allocation = Allocation {
held: true,
..Allocation::default()
};
}
if self.allocation.slots[slave].is_none() {
let offset = self.allocation.next;
self.allocation.slots[slave] = Some((offset, length));
self.allocation.next += length;
}
self.allocation.slots[slave]
}
/// One transaction on the auxiliary bus, RM 4.13: the address written with the register unless
/// `REG_DIS`, then `length` bytes read after a restart or a stop and a start, or one byte written.
/// `None` where no peer acknowledged the address.
fn transaction(
&mut self,
transfer: Transfer,
context: &mut Timeline<'_, '_>,
) -> Option<Vec<u8>> {
let Transfer {
address,
register,
skip_register,
length,
out,
stop_between,
} = transfer;
let read = address & I2C_SLV_RNW != 0;
let address = address & 0x7F;
let Self {
magnetometer,
others,
..
} = self;
let mut peers: Vec<&mut dyn i2c::Peer> = vec![magnetometer];
peers.extend(others.iter_mut().filter_map(Place::answering));
let mut answering = None;
for peer in peers {
if peer.start(address, read && skip_register, context) {
answering = Some(peer);
break;
}
}
let peer = answering?;
if !skip_register {
peer.write(register, context);
}
if !read {
peer.write(out, context);
peer.stop(context);
return Some(Vec::new());
}
if !skip_register {
if stop_between {
peer.stop(context);
}
peer.start(address, true, context);
}
let data: Vec<u8> = (0..length)
.map(|index| peer.read(index + 1 == length, context))
.collect();
peer.stop(context);
Some(data)
}
/// RM 3's table: whether an address is a register at all.
fn mapped(register: usize) -> bool {
matches!(
register,
0x00..=0x02
| 0x0D..=0x0F
| 0x13..=0x1F
| 0x23..=0x38
| 0x3A..=0x60
| 0x63..=0x6C
| 0x72..=0x75
| 0x77
| 0x78
| 0x7A
| 0x7B
| 0x7D
| 0x7E
)
}
/// RM 3's "Serial I/F" column: the registers a write lands in.
fn writable(register: usize) -> bool {
Self::mapped(register)
&& !matches!(
register,
I2C_SLV4_DI | I2C_MST_STATUS | INT_STATUS | ACCEL_XOUT_H
..=0x60 | FIFO_COUNTH | FIFO_COUNTL | WHO_AM_I
)
}
fn read_register(&mut self, register: usize) -> u8 {
let value = match register {
FIFO_COUNTH => self.fifo.latch(),
FIFO_COUNTL => self.fifo.latched_low(),
invensense::FIFO_R_W => {
if let Some(byte) = self.fifo.pop() {
self.last_byte = byte;
}
self.last_byte
}
INT_STATUS => {
let value = self.registers[INT_STATUS];
self.registers[INT_STATUS] = 0;
value
}
I2C_MST_STATUS => {
let value = self.registers[I2C_MST_STATUS];
self.registers[I2C_MST_STATUS] = 0;
value
}
// Every unmapped address reads zero because no write lands in one, RM 3.
_ => self.registers[register],
};
if self.registers[INT_PIN_CFG] & INT_ANYRD_2CLEAR != 0 {
self.registers[INT_STATUS] = 0;
}
value
}
fn write_register(&mut self, register: usize, byte: u8, now: VirtualNanos) {
if !Self::writable(register) {
return;
}
match register {
PWR_MGMT_1 => {
if byte & H_RESET != 0 {
self.reset(now);
return;
}
self.rebase(now);
self.registers[PWR_MGMT_1] = byte;
}
USER_CTRL => {
self.registers[USER_CTRL] = byte & !AUTO_CLEARING;
if byte & FIFO_RST != 0 {
self.fifo.clear();
}
if byte & I2C_MST_RST != 0 {
self.allocation.held = false;
}
if byte & SIG_COND_RST != 0 {
for data in ACCEL_XOUT_H..=GYRO_ZOUT_L {
self.registers[data] = 0;
}
}
}
SMPLRT_DIV | CONFIG | GYRO_CONFIG | ACCEL_CONFIG_2 => {
self.rebase(now);
self.registers[register] = byte;
}
_ => {
self.registers[register] = byte;
if (I2C_SLV0_ADDR..I2C_SLV4_ADDR).contains(®ister)
&& (register - I2C_SLV0_ADDR) % 3 == 2
&& (0..4).all(|slave| {
self.registers[I2C_SLV0_ADDR + 3 * slave + 2] & I2C_SLV_EN == 0
})
{
self.allocation.held = false;
}
}
}
}
}
impl Peer for Mpu9250 {
/// The select falling: everything due by now happens, then the first byte is an address.
fn select(&mut self, context: &mut Timeline<'_, '_>) {
self.serial.select();
self.catch_up(context.now(), context);
}
fn deselect(&mut self, _context: &mut Timeline<'_, '_>) {
self.serial.deselect();
}
/// PS 7.5: the address byte with its direction bit, then a byte per register, incrementing.
/// PS 4.17: "The FIFO register supports burst reads", so a burst stays on it.
fn transfer(&mut self, mosi: u32, _bits: u8, context: &mut Timeline<'_, '_>) -> u32 {
match self.serial.step((mosi & 0xFF) as u8) {
Some(Access::Read(register)) => u32::from(self.read_register(register)),
Some(Access::Write(register, byte)) => {
self.write_register(register, byte, context.now());
0
}
None => 0,
}
}
}