tasks: usb_power: reimplement tusb320 API using embedded_hal_async traits, and implement a task for managing the power status

src/bin/main.rs

@@ -98,7 +98,7 @@ async fn main(spawner: Spawner) {
     wdt.enable();
 
     // Spawn the rendering task as soon as possible so it can start pushing pixels
-    spawner.must_spawn(renderbug_bike::tasks::render::render(peripherals.SPI2.degrade(), peripherals.DMA_CH2.degrade(), peripherals.GPIO5.degrade(), surfaces, safety_surfaces, display_controls, wdt));
+    spawner.must_spawn(renderbug_bike::tasks::render::render(peripherals.SPI2.degrade(), peripherals.DMA_CH2.degrade(), peripherals.GPIO5.degrade(), surfaces, safety_surfaces, display_controls.clone(), wdt));
 
     let imu_interrupt = PinInterrupt::new(Input::new(peripherals.GPIO36.degrade(), InputConfig::default()), Event::RisingEdge);
     let pd_interrupt = PinInterrupt::new(Input::new(peripherals.GPIO16.degrade(), InputConfig::default()), Event::RisingEdge);
@@ -131,7 +131,7 @@ async fn main(spawner: Spawner) {
         spawner.must_spawn(renderbug_bike::tasks::mpu::mpu_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus), imu_interrupt));
         
         spawner.must_spawn(renderbug_bike::tasks::gps::gps_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus)));
-        spawner.must_spawn(renderbug_bike::tasks::usb_power::usb_task(I2cDevice::new(i2c_bus), pd_interrupt));
+        spawner.must_spawn(renderbug_bike::tasks::usb_power::usb_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus), pd_interrupt, display_controls.clone()));
     }
 
     #[cfg(feature="oled")]

src/tasks/usb_power.rs

@@ -1,25 +1,52 @@
 use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
-use embassy_sync::blocking_mutex::raw::NoopRawMutex;
+use embassy_sync::{blocking_mutex::raw::NoopRawMutex, channel::DynamicSender};
 use esp_hal::Async;
+use figments_render::power::Milliwatts;
 use log::*;
 
-use crate::{gpio_interrupt::PinInterrupt, tusb320::TUSB320};
+use crate::{events::{Measurement, SensorSource, SensorState}, gpio_interrupt::PinInterrupt, graphics::display::DisplayControls, tusb320::{ActiveCableConnected, DEVICE_ID, InterruptStatus, TUSB320}};
 
 #[embassy_executor::task]
-pub async fn usb_task(bus: I2cDevice<'static, NoopRawMutex, esp_hal::i2c::master::I2c<'static, Async>>, interrupt_pin: PinInterrupt<'static>) {
+pub async fn usb_task(motion: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, NoopRawMutex, esp_hal::i2c::master::I2c<'static, Async>>, interrupt_pin: PinInterrupt<'static>, mut display_control: DisplayControls) {
     let mut tusb = TUSB320::new(bus);
 
+    motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::AcquiringFix)).await;
     match tusb.get_device_id().await {
-        Ok(val) => {
+        Ok(DEVICE_ID) => {
-            info!("TUSB320 Device ID: {val:?}");
+            info!("TUSB320 connected");
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Degraded)).await;
         },
-        Err(_) => {
+        Ok(val) => {
-            error!("Failed to read from TUSB320");
+            error!("Unexpected TUSB320 Device ID: {val:?}! Power management is disabled!!");
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Offline)).await;
+            return;
+        },
+        Err(e) => {
+            error!("Failed to read from TUSB320! Power management is disabled!! {e:?}");
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Offline)).await;
+            return;
         }
     }
 
     loop {
-        log::info!("Waiting for USB power interrupt...");
+        match tusb.active_cable_connected().read().await.unwrap() {
+            ActiveCableConnected::Connected => {
+                let current_mode = tusb.current_mode_detect().read().await.unwrap();
+                let cur_mw = Milliwatts(current_mode.milliwatts());
+                info!("USB reports current mode: {current_mode:?}, {cur_mw:?} mW");
+                display_control.set_max_power(cur_mw);
+                motion.send(Measurement::ExternalPower(cur_mw)).await;
+                motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Online)).await;
+            },
+            ActiveCableConnected::Disconnected => {
+                warn!("USB power is disconnected! Setting display power to zero.");
+                // FIXME: We should also send an event through the system to report that the power is down, and the renderer should stop
+                display_control.set_max_power(Milliwatts(0));
+                motion.send(Measurement::ExternalPower(Milliwatts(0))).await;
+                motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Degraded)).await;
+            }
+        }
         interrupt_pin.wait_for_interrupt().await;
+        tusb.interrupt_status().write(InterruptStatus::Clear).await.ok();
     }
 }

src/tusb320.rs

@@ -1,13 +1,6 @@
-use embassy_embedded_hal::shared_bus::{I2cDeviceError, asynch::i2c::I2cDevice};
+use core::marker::PhantomData;
-use embassy_sync::blocking_mutex::raw::NoopRawMutex;
-use esp_hal::Async;
-use embedded_hal_async::i2c::I2c;
 
-// TODO: rewrite this to only use embedded_hal_async traits, then publish as a crate
+use embedded_hal_async::i2c::I2c as AsyncI2c;
-const USB_ADDR: u8 = 0b1100000;
-pub struct TUSB320 {
-    bus: I2cDevice<'static, NoopRawMutex, esp_hal::i2c::master::I2c<'static, Async>>
-}
 
 #[derive(Clone, Copy, Debug)]
 pub enum RegisterAddress {
@@ -25,307 +18,253 @@ pub enum RegisterAddress {
     DisableRDRP = 0x45
 }
 
-#[derive(Clone, Copy, Debug)]
+pub trait RegisterField {
-pub enum Register {
+    fn mask() -> u8;
-    DeviceID,
+    fn shift() -> u8;
-    DeviceID1,
+    fn address() -> RegisterAddress;
-    DeviceID2,
-    DeviceID3,
-    DeviceID4,
-    DeviceID5,
-    DeviceID6,
-    DeviceID7,
-    CurrentAdvertisement,
-    CurrentDetect,
-    AccessoryConnected,
-    ActiveCableConnected,
-    AttachedState,
-    CableOrientation,
-    InterruptStatus,
-    DrpDutyCycle,
-    Debounce,
-    ModeSelect,
-    SoftReset,
-    DisableRDRP
 }
 
-impl From<Register> for RegisterAddress {
+macro_rules! define_register_field {
-    fn from(reg: Register) -> Self {
+    ($name:tt address=$address:tt offset=$shift:tt mask=$bitmask:tt $($value:tt = $valuemask:tt),+) => {
-        match reg {
+        #[derive(Debug, Clone, Copy, PartialEq, Eq)]
-            Register::DeviceID => RegisterAddress::DeviceID,
+        pub enum $name {
-            Register::DeviceID1 => RegisterAddress::DeviceID1,
+            $($value = $valuemask),+
-            Register::DeviceID2 => RegisterAddress::DeviceID2,
+        }
-            Register::DeviceID3 => RegisterAddress::DeviceID3,
-            Register::DeviceID4 => RegisterAddress::DeviceID4,
-            Register::DeviceID5 => RegisterAddress::DeviceID5,
-            Register::DeviceID6 => RegisterAddress::DeviceID6,
-            Register::DeviceID7 => RegisterAddress::DeviceID7,
 
-            Register::CurrentAdvertisement => RegisterAddress::Status1,
+        impl From<u8> for $name {
-            Register::CurrentDetect => RegisterAddress::Status1,
+            fn from(value: u8) -> Self {
-            Register::AccessoryConnected => RegisterAddress::Status1,
+                match value & 0b11 {
-            Register::ActiveCableConnected => RegisterAddress::Status1
+                    $($valuemask => $name::$value),+,
-            ,
+                    _ => unreachable!()
-            Register::AttachedState => RegisterAddress::Status2,
-            Register::CableOrientation => RegisterAddress::Status2,
-            Register::InterruptStatus => RegisterAddress::Status2,
-            Register::DrpDutyCycle => RegisterAddress::Status2,
-
-            Register::Debounce => RegisterAddress::Status3,
-            Register::ModeSelect => RegisterAddress::Status3,
-            Register::SoftReset => RegisterAddress::Status3,
-            Register::DisableRDRP => RegisterAddress::DisableRDRP
                 }
             }
         }
 
-pub enum CurrentAdvertisement {
+        impl From<$name> for u8 {
-    /// 500mA / 900mA
+            fn from(value: $name) -> Self {
+                value as u8
+            }
+        }
+
+        impl RegisterField for $name {
+            fn mask() -> u8 {
+                $bitmask
+            }
+
+            fn shift() -> u8 {
+                $shift
+            }
+
+            fn address() -> RegisterAddress {
+                RegisterAddress::$address
+            }
+        }
+    };
+}
+
+
+define_register_field!{
+    CurrentAdvertisement
+    address=Status1
+    offset=6
+    mask=0b11
     Default = 0b00,
-    /// 1.5A
+    // 1.5A
     Medium = 0b01,
-    /// 3A
+    // 3A
-    High = 0b10,
+    High = 0b10
-    /// Reserved, do not use
-    Reserved = 0b11
 }
 
-pub enum CurrentModeDetect {
+define_register_field!{
-    /// Default value at startup. TODO: Does this mean 500ma, or just 'no connection'?
+    CurrentModeDetect
+    address=Status1
+    offset=4
+    mask=0b11
+    // 500mA
     Default = 0b00,
-    /// 1.5A
+    // 1.5A
     Medium = 0b01,
-    /// 500ma
+    // 500mA
     ThroughAccessory = 0b10,
-    /// 3A
+    // 3A
-    High = 0b11,
+    High = 0b11
 }
 
-pub enum AccessoryConnectionState {
+impl CurrentModeDetect {
+    pub fn milliwatts(&self) -> u32 {
+        match self {
+            CurrentModeDetect::Default => 500,
+            CurrentModeDetect::Medium => 1500,
+            CurrentModeDetect::ThroughAccessory => 500,
+            CurrentModeDetect::High => 3000
+        }
+    }
+}
+
+define_register_field!{
+    AccessoryConnected
+    address=Status1
+    offset=1
+    mask=0b111
     None = 0b000,
     Audio = 0b100,
     ThroughAccessory = 0b101,
-    DebugAccessory = 0b110,
+    DebugAccessory = 0b110
-    // All other binary patterns are 'reserved'
+    // All other patterns are 'reserved' and should not be possible
-    Reserved
 }
 
-pub enum AttachedState {
+define_register_field!{
+    ActiveCableConnected
+    address=Status1
+    offset=0
+    mask=0b1
+    Disconnected = 0,
+    Connected = 1
+}
+
+define_register_field!{
+    AttachedState
+    address=Status2
+    offset=6
+    mask=0b11
     Unattached = 0b00,
     Source = 0b01,
     Sink = 0b10,
     Accessory = 0b11
 }
 
-pub enum CableOrientation {
+define_register_field! {
+    CableOrientation
+    address=Status2
+    offset=5
+    mask=0b1
     Normal = 0,
     Flipped = 1
 }
 
-pub enum DrpDutyCycle {
+define_register_field! {
-    /// 30%
+    InterruptStatus
+    address=Status2
+    offset=4
+    mask=0b1
+    Clear = 0,
+    Set = 1
+}
+
+define_register_field! {
+    DrpDutyCycle
+    address=Status2
+    offset=1
+    mask=0b11
+    // 30%
     Default = 0b00,
-    /// 40%
+    // 40%
     Fast = 0b01,
-    /// 50%
+    // 50%
     Faster = 0b10,
-    /// 60%
+    // 60%
     Fastest = 0b11
 }
 
-pub enum Debounce {
+define_register_field! {
-    /// 133ms (default)
+    Debounce
+    address=Status3
+    offset=6
+    mask=0b11
+    // 133ms (default)
     D133 = 0b00,
-    /// 116ms
+    // 116ms
     D116 = 0b01,
-    /// 151ms
+    // 151ms
     D151 = 0b10,
-    /// 168ms
+    // 168ms
     D168 = 0b11
 }
 
-pub enum Mode {
+define_register_field! {
+    ModeSelect
+    address=Status3
+    offset=4
+    mask=0b11
     UsePortPin = 0b00,
     UFP = 0b01,
     DFP = 0b10,
     DRP = 0b11
 }
 
-impl TUSB320 {
+define_register_field! {
-    pub const fn new(bus: I2cDevice<'static, NoopRawMutex, esp_hal::i2c::master::I2c<'static, Async>>) -> Self {
+    DisableRDRP
+    address=DisableRDRP
+    offset=2
+    mask=0b1
+    Enabled = 0,
+    Disabled = 1
+}
+
+pub const DEVICE_ID: [u8; 8] = *b"TUSB320A";
+
+const USB_ADDR_WRITE: u8 = 0b1100000;
+const USB_ADDR_READ: u8 = 0b1100001;
+pub struct TUSB320<T> {
+    bus: T
+}
+
+impl<T: AsyncI2c> TUSB320<T> {
+    pub const fn new(bus: T) -> Self {
         Self { bus }
     }
 
-    pub async fn get_current_advertisement(&mut self) -> Result<CurrentAdvertisement, I2cDeviceError<esp_hal::i2c::master::Error>> {
+    pub async fn soft_reset(&mut self) -> Result<(), T::Error> {
-        let reg: RegisterAddress = Register::CurrentAdvertisement.into();
+        let mut val = self.read_address(RegisterAddress::Status3).await?;
-        let val = self.read_address(reg).await? >> 6;
-        match val & 0b11 {
-            0b00 => Ok(CurrentAdvertisement::Default),
-            0b01 => Ok(CurrentAdvertisement::Medium),
-            0b10 => Ok(CurrentAdvertisement::High),
-            0b11 => Ok(CurrentAdvertisement::Reserved),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn set_current_advertisement(&mut self, adv: CurrentAdvertisement) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::CurrentAdvertisement.into();
-        let mut val = self.read_address(reg).await?;
-        let mask = !(0b11 << 6);
-        val &= mask | ((adv as u8) << 6);
-        self.write_address(reg, val).await?;
-        Ok(())
-    }
-
-    pub async fn get_current_mode_detect(&mut self) -> Result<CurrentModeDetect, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::CurrentDetect.into();
-        let val = self.read_address(reg).await? >> 4;
-        match val & 0b11 {
-            0b00 => Ok(CurrentModeDetect::Default),
-            0b01 => Ok(CurrentModeDetect::Medium),
-            0b10 => Ok(CurrentModeDetect::ThroughAccessory),
-            0b11 => Ok(CurrentModeDetect::High),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn get_accessory_connection_state(&mut self) -> Result<AccessoryConnectionState, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::AccessoryConnected.into();
-        let val = self.read_address(reg).await? >> 1;
-        match val & 0b111 {
-            0b000 => Ok(AccessoryConnectionState::None),
-            0b100 => Ok(AccessoryConnectionState::Audio),
-            0b101 => Ok(AccessoryConnectionState::ThroughAccessory),
-            0b110 => Ok(AccessoryConnectionState::DebugAccessory),
-            _ => Ok(AccessoryConnectionState::Reserved)
-        }
-    }
-
-    pub async fn get_cable_detected(&mut self) -> Result<bool, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::ActiveCableConnected.into();
-        let val = self.read_address(reg).await?;
-        Ok((val & 0b1) == 1)
-    }
-
-    pub async fn get_attached_state(&mut self) -> Result<AttachedState, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::AttachedState.into();
-        let val = self.read_address(reg).await? >> 6;
-        match val & 0b11 {
-            0b00 => Ok(AttachedState::Unattached),
-            0b01 => Ok(AttachedState::Source),
-            0b10 => Ok(AttachedState::Sink),
-            0b11 => Ok(AttachedState::Accessory),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn get_cable_orientation(&mut self) -> Result<CableOrientation, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::CableOrientation.into();
-        let val = self.read_address(reg).await? >> 5;
-        match val & 0b1 {
-            0 => Ok(CableOrientation::Normal),
-            1 => Ok(CableOrientation::Flipped),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn get_interrupt_status(&mut self) -> Result<bool, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::InterruptStatus.into();
-        Ok((self.read_address(reg).await? >> 4) == 1)
-    }
-
-    pub async fn clear_interrupt(&mut self) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::InterruptStatus.into();
-        let mut val = self.read_address(reg).await?;
-        val &= !(0b1 << 4);
-        self.write_address(reg, val).await
-    }
-
-    pub async fn set_drp_duty_cycle(&mut self, duty: DrpDutyCycle) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::DrpDutyCycle.into();
-        let mut val = self.read_address(reg).await?;
-        let mask = !(0b11 << 1);
-        val &= mask | ((duty as u8) << 1);
-        self.write_address(reg, val).await
-    }
-
-    pub async fn get_drp_duty_cycle(&mut self) -> Result<DrpDutyCycle, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::DrpDutyCycle.into();
-        let val = self.read_address(reg).await? >> 1;
-        match val & 0b11 {
-            0b00 => Ok(DrpDutyCycle::Default),
-            0b01 => Ok(DrpDutyCycle::Fast),
-            0b10 => Ok(DrpDutyCycle::Faster),
-            0b11 => Ok(DrpDutyCycle::Fastest),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn get_debounce(&mut self) -> Result<Debounce, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::Debounce.into();
-        let val = self.read_address(reg).await? >> 6;
-        match val & 0b11 {
-            0b00 => Ok(Debounce::D133),
-            0b01 => Ok(Debounce::D116),
-            0b10 => Ok(Debounce::D151),
-            0b11 => Ok(Debounce::D168),
-            _ => unreachable!()
-        }
-     }
-
-    pub async fn set_debounce(&mut self, debounce: Debounce) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::Debounce.into();
-        let mut val = self.read_address(reg).await?;
-        let mask = !(0b11 << 6);
-        val &= mask | ((debounce as u8) << 6);
-        self.write_address(reg, val).await
-    }
-
-    pub async fn get_mode(&mut self) -> Result<Mode, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::ModeSelect.into();
-        let val = self.read_address(reg).await? >> 4;
-        match val & 0b11 {
-            0b00 => Ok(Mode::UsePortPin),
-            0b01 => Ok(Mode::UFP),
-            0b10 => Ok(Mode::DFP),
-            0b11 => Ok(Mode::DRP),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn set_mode(&mut self, mode: Mode) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::ModeSelect.into();
-        let mut val = self.read_address(reg).await?;
-        let mask = !(0b11 << 4);
-        val &= mask | ((mode as u8) << 4);
-        self.write_address(reg, val).await
-    }
-
-    pub async fn soft_reset(&mut self) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::SoftReset.into();
-        let mut val = self.read_address(reg).await?;
         val |= 0b1 << 3;
-        self.write_address(reg, val).await
+        self.write_address(RegisterAddress::Status3, val).await
     }
 
-    pub async fn set_rdrp_disabled(&mut self, disable: bool) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
+    pub fn register<'a, R: RegisterField>(&'a mut self) -> UpdateRegister<'a, T, R> {
-        let reg: RegisterAddress = Register::DisableRDRP.into();
+        UpdateRegister::new(R::address(), self)
-        let mut val = self.read_address(reg).await?;
-        if disable {
-            val |= 0b1;
-        } else {
-            val &= !0b1;
-        }
-        self.write_address(reg, val).await
     }
 
-    pub async fn get_rdrp_disabled(&mut self) -> Result<bool, I2cDeviceError<esp_hal::i2c::master::Error>> {
+    pub fn current_advertisement<'a>(&'a mut self) -> UpdateRegister<'a, T, CurrentAdvertisement> {
-        let reg: RegisterAddress = Register::DisableRDRP.into();
+        self.register()
-        let val = self.read_address(reg).await? >> 2;
-        Ok((val & 0b1) == 1)
     }
 
-    pub async fn get_device_id(&mut self) -> Result<[u8;8], I2cDeviceError<esp_hal::i2c::master::Error>> {
+    pub fn current_mode_detect<'a>(&'a mut self) -> UpdateRegister<'a, T, CurrentModeDetect> {
+        self.register()
+    }
+
+    pub fn accessory_connected<'a>(&'a mut self) -> UpdateRegister<'a, T, AccessoryConnected> {
+        self.register()
+    }
+
+    pub fn active_cable_connected<'a>(&'a mut self) -> UpdateRegister<'a, T, ActiveCableConnected> {
+        self.register()
+    }
+
+    pub fn cable_orientation<'a>(&'a mut self) -> UpdateRegister<'a, T, CableOrientation> {
+        self.register()
+    }
+
+    pub fn interrupt_status<'a>(&'a mut self) -> UpdateRegister<'a, T, InterruptStatus> {
+        self.register()
+    }
+
+    pub fn drp_duty_cycle<'a>(&'a mut self) -> UpdateRegister<'a, T, DrpDutyCycle> {
+        self.register()
+    }
+
+    pub fn debounce<'a>(&'a mut self) -> UpdateRegister<'a, T, Debounce> {
+        self.register()
+    }
+
+    pub fn mode_select<'a>(&'a mut self) -> UpdateRegister<'a, T, ModeSelect> {
+        self.register()
+    }
+
+    pub fn disable_rdrp<'a>(&'a mut self) -> UpdateRegister<'a, T, DisableRDRP> {
+        self.register()
+    }
+
+    pub async fn get_device_id(&mut self) -> Result<[u8;8], T::Error> {
         Ok([
             self.read_address(RegisterAddress::DeviceID).await?,
             self.read_address(RegisterAddress::DeviceID1).await?,
@@ -338,19 +277,54 @@ impl TUSB320 {
         ])
     }
 
-    pub async fn read_address(&mut self, reg: RegisterAddress) -> Result<u8, I2cDeviceError<esp_hal::i2c::master::Error>> {
+    async fn read_address(&mut self, reg: RegisterAddress) -> Result<u8, T::Error> {
         let mut response = [0; 1];
-        // 1 means 'read' cycle
+        self.bus.write_read(USB_ADDR_READ, &[reg as u8], &mut response).await?;
-        let i2c_addr = USB_ADDR | 1;
-        self.bus.write_read(i2c_addr, &[reg as u8], &mut response).await?;
         Ok(response[0])
     }
 
-    pub async fn write_address(&mut self, reg: RegisterAddress, value: u8) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
+    async fn write_address(&mut self, reg: RegisterAddress, value: u8) -> Result<(), T::Error> {
-        // 0 means 'write' cycle
+        // FIXME: I think this needs to use self.bus.transaction()
-        let i2c_addr = USB_ADDR | 0;
+        self.bus.write(USB_ADDR_WRITE, &[reg as u8]).await?;
-        self.bus.write(i2c_addr, &[reg as u8]).await?;
+        self.bus.write(USB_ADDR_WRITE, &[value]).await?;
-        self.bus.write(i2c_addr, &[value]).await?;
         Ok(())
     }
 }
+
+pub struct UpdateRegister<'a, I2C: AsyncI2c, T> {
+    reg: RegisterAddress,
+    raw: u8,
+    dev: &'a mut TUSB320<I2C>,
+    _type: PhantomData<T>
+}
+
+impl<'a, I2C: AsyncI2c, T> UpdateRegister<'a, I2C, T> {
+    const fn new(reg: RegisterAddress, dev: &'a mut TUSB320<I2C>) -> Self {
+        Self { reg, raw: 0, dev, _type: PhantomData }
+    }
+
+    pub async fn refresh(&mut self) -> Result<(), I2C::Error> {
+        self.raw = self.dev.read_address(self.reg).await?;
+        Ok(())
+    }
+}
+
+impl<'a, I2C: AsyncI2c, T: RegisterField + Into<u8>> UpdateRegister<'a, I2C, T> where u8: Into<T> {
+    pub async fn write(&mut self, value: T) -> Result<(), I2C::Error> {
+        let mask = T::mask();
+        let v: u8 = value.into();
+        self.refresh().await?;
+        self.raw &= !(mask | (v << T::shift()));
+        self.dev.write_address(self.reg, self.raw).await?;
+        Ok(())
+    }
+
+    pub async fn read(&mut self) -> Result<T, I2C::Error> {
+        self.refresh().await?;
+        Ok(self.value())
+    }
+
+    pub fn value(&mut self) -> T {
+        (self.raw >> T::shift()).into()
+    }
+}