diff --git a/libraries/SITL/SIM_I2CDevice.cpp b/libraries/SITL/SIM_I2CDevice.cpp index 0f352819d1..54a2bc6a4a 100644 --- a/libraries/SITL/SIM_I2CDevice.cpp +++ b/libraries/SITL/SIM_I2CDevice.cpp @@ -64,6 +64,7 @@ int SITL::I2CRegisters_16Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data) if (data->msgs[0].flags != 0) { AP_HAL::panic("Unexpected flags"); } + // FIXME: handle multi-register writes const uint8_t reg_addr = data->msgs[0].buf[0]; if (!writable_registers.get(reg_addr)) { AP_HAL::panic("Register 0x%02x is not writable!", reg_addr); @@ -75,3 +76,52 @@ int SITL::I2CRegisters_16Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data) return -1; }; + + + +int SITL::I2CRegisters_8Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +{ + if (data->nmsgs == 2) { + // data read request + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + if (data->msgs[1].flags != I2C_M_RD) { + AP_HAL::panic("Unexpected flags"); + } + const uint8_t reg_base_addr = data->msgs[0].buf[0]; + uint8_t bytes_copied = 0; + while (bytes_copied < data->msgs[1].len) { + const uint8_t reg_addr = reg_base_addr + bytes_copied; + if (!readable_registers.get(reg_addr)) { + // ::printf("Register 0x%02x is not readable!\n", reg_addr); + return -1; + } + const uint8_t register_value = byte[reg_addr]; + data->msgs[1].buf[bytes_copied++] = register_value; + } + data->msgs[1].len = bytes_copied; + return 0; + } + + if (data->nmsgs == 1) { + // data write request + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + const uint8_t reg_base_addr = data->msgs[0].buf[0]; + uint8_t bytes_copied = 0; + while (bytes_copied < data->msgs[0].len-1) { + const uint8_t reg_addr = reg_base_addr + bytes_copied; + if (!writable_registers.get(reg_addr)) { + AP_HAL::panic("Register 0x%02x is not writable!", reg_addr); + } + const uint8_t register_value = data->msgs[0].buf[1+bytes_copied]; + byte[reg_addr] = register_value; + bytes_copied++; + } + return 0; + } + + return -1; +}; diff --git a/libraries/SITL/SIM_I2CDevice.h b/libraries/SITL/SIM_I2CDevice.h index 703212d58d..7112bda90c 100644 --- a/libraries/SITL/SIM_I2CDevice.h +++ b/libraries/SITL/SIM_I2CDevice.h @@ -35,6 +35,20 @@ protected: uint16_t word[256]; }; +class I2CRegisters_8Bit : public I2CRegisters { +public: + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; + void set_register(uint8_t reg, uint8_t value); + void set_register(uint8_t reg, int8_t value); + + uint8_t get_register(uint8_t num) { + return byte[(uint8_t)num]; + } + +protected: + + uint8_t byte[256]; +}; class I2CDevice { public: