diff --git a/capsules/extra/src/bus.rs b/capsules/extra/src/bus.rs index 77326f83d2..9576ee939d 100644 --- a/capsules/extra/src/bus.rs +++ b/capsules/extra/src/bus.rs @@ -301,7 +301,21 @@ impl<'a, I: I2CDevice> Bus<'a> for I2CMasterBus<'a, I> { } } }), - + BusWidth::Bits16LE => self + .addr_buffer + .take() + .map_or(Err(ErrorCode::NOMEM), |buffer| { + buffer[0] = 0x00; + buffer[1] = addr as u8; + self.status.set(BusStatus::SetAddress); + match self.i2c.write(buffer, 2) { + Ok(()) => Ok(()), + Err((error, buffer)) => { + self.addr_buffer.replace(buffer); + Err(error.into()) + } + } + }), _ => Err(ErrorCode::NOSUPPORT), } } @@ -315,6 +329,7 @@ impl<'a, I: I2CDevice> Bus<'a> for I2CMasterBus<'a, I> { // endianess does not matter as the buffer is sent as is let bytes = data_width.width_in_bytes(); self.len.set(len * bytes); + if len * bytes < 255 && buffer.len() >= len * bytes { debug!("write len {}", len); self.len.set(len);