SITL: add I2CRegister::8Bit for 8-bit-quantity devices

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;
+};

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: