diff --git a/drivers/graphics/ihdgd/src/device/gmbus.rs b/drivers/graphics/ihdgd/src/device/gmbus.rs index a8e16bfe95..24310fc04b 100644 --- a/drivers/graphics/ihdgd/src/device/gmbus.rs +++ b/drivers/graphics/ihdgd/src/device/gmbus.rs @@ -109,8 +109,29 @@ impl<'a> Transactional for GmbusPinPair<'a> { } } Operation::Write(buf) => { - log::warn!("TODO: GMBUS WRITE"); - return Err(()); + for chunk in buf.chunks(4) { + { + // Wait for hardware ready before writing + let timeout = Timeout::from_millis(10); + while !self.regs[2].readf(GMBUS2_HW_RDY) { + timeout.run().map_err(|()| { + log::debug!( + "timeout on GMBUS write 0x{:08x}", + self.regs[2].read() + ); + () + })?; + } + } + + // Write data to GMBUS data register (index 3). + // Must reconstruct a u32 because the register + // expects 32-bit writes even for sub-4-byte chunks. + let mut word = 0u32; + let len = chunk.len().min(4); + word.to_le_bytes()[..len].copy_from_slice(chunk); + self.regs[3].write(word); + } } } }