SITL: correct SMBus block reads

libraries/SITL/SIM_BattMonitor_SMBus.cpp

@@ -3,7 +3,7 @@
 #include <AP_Stats/AP_Stats.h>
 
 SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() :
-    I2CRegisters_16Bit()
+    SMBusDevice()
 {
     add_register("Temperature", SMBusBattDevReg::TEMP, O_RDONLY);
     add_register("Voltage", SMBusBattDevReg::VOLTAGE, O_RDONLY);
@@ -13,7 +13,8 @@ SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() :
     add_register("Cycle_Count", SMBusBattDevReg::CYCLE_COUNT, O_RDONLY);
     add_register("Specification Info", SMBusBattDevReg::SPECIFICATION_INFO, O_RDONLY);
     add_register("Serial", SMBusBattDevReg::SERIAL, O_RDONLY);
-    add_register("Manufacture Name", SMBusBattDevReg::MANUFACTURE_NAME, O_RDONLY);
+    add_block("Manufacture Name", SMBusBattDevReg::MANUFACTURE_NAME, O_RDONLY);
+    add_block("Device Name", SMBusBattDevReg::DEVICE_NAME, O_RDONLY);
     add_register("Manufacture Data", SMBusBattDevReg::MANUFACTURE_DATA, O_RDONLY);
 
     set_register(SMBusBattDevReg::TEMP, (int16_t)((15 + 273.15)*10));
@@ -43,25 +44,15 @@ SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() :
 
      set_register(SMBusBattDevReg::SERIAL, (uint16_t)12345);
 
-     // Set MANUFACTURE_NAME
      const char *manufacturer_name = "ArduPilot";
-     set_register(SMBusBattDevReg::MANUFACTURE_NAME, uint16_t(manufacturer_name[0]<<8|strlen(manufacturer_name)));
+     set_block(SMBusBattDevReg::MANUFACTURE_NAME, manufacturer_name);
-     uint8_t i = 1;  // already sent the first byte out....
+
-     while (i < strlen(manufacturer_name)) {
+     const char *device_name = "SITLBatMon_V0.99";
-         const uint8_t a = manufacturer_name[i++];
+     set_block(SMBusBattDevReg::DEVICE_NAME, device_name);
-         uint8_t b = 0;
+
-         if (i < strlen(manufacturer_name)) {
-             b = manufacturer_name[i];
-         }
-         i++;
-         const uint16_t value = b<<8 | a;
-         add_register("Name", SMBusBattDevReg::MANUFACTURE_NAME + i/2, O_RDONLY);
-         set_register(SMBusBattDevReg::MANUFACTURE_NAME + i/2, value);
-     }
      // TODO: manufacturer data
 }
 
-
 void SITL::SIM_BattMonitor_SMBus::update(const class Aircraft &aircraft)
 {
     const uint32_t now = AP_HAL::millis();

libraries/SITL/SIM_BattMonitor_SMBus.h

@@ -1,10 +1,10 @@
-#include "SIM_I2CDevice.h"
+#include "SIM_SMBusDevice.h"
 
 #pragma once
 
 namespace SITL {
 
-class SMBusBattDevReg : public I2CRegEnum {
+class SMBusBattDevReg : public SMBusRegEnum {
 public:
     static const uint8_t TEMP = 0x08;                 // Temperature
     static const uint8_t VOLTAGE = 0x09;              // Voltage
@@ -15,10 +15,11 @@ public:
     static const uint8_t SPECIFICATION_INFO = 0x1A;   // Specification Info
     static const uint8_t SERIAL = 0x1C;               // Serial Number
     static const uint8_t MANUFACTURE_NAME = 0x20;     // Manufacture Name
+    static const uint8_t DEVICE_NAME = 0x21;          // Device Name
     static const uint8_t MANUFACTURE_DATA = 0x23;     // Manufacture Data
 };
 
-class SIM_BattMonitor_SMBus : public I2CDevice, protected I2CRegisters_16Bit
+class SIM_BattMonitor_SMBus : public SMBusDevice
 {
 public:
 
@@ -26,10 +27,6 @@ public:
 
     virtual void update(const class Aircraft &aircraft) override;
 
-    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
-        return I2CRegisters_16Bit::rdwr(data);
-    }
-
 private:
 
     uint32_t last_update_ms;

libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp

@@ -16,6 +16,7 @@ void SITL::Rotoye::update(const class Aircraft &aircraft)
     const uint32_t now = AP_HAL::millis();
     if (now - last_temperature_update_ms > 1000) {
         last_temperature_update_ms = now;
-        set_register(SMBusBattRotoyeDevReg::TEMP_EXT, int16_t(be16toh(word[SMBusBattRotoyeDevReg::TEMP]) + 100));  // it's a little warmer inside.... (10 degrees here)
+        int16_t outside_temp = get_reg_value(SMBusBattRotoyeDevReg::TEMP);
+        set_register(SMBusBattRotoyeDevReg::TEMP_EXT, int16_t(outside_temp + 100));  // it's a little warmer inside.... (10 degrees here)
     }
 }

libraries/SITL/SIM_I2CDevice.h

@@ -30,6 +30,10 @@ public:
     void set_register(uint8_t reg, uint16_t value);
     void set_register(uint8_t reg, int16_t value);
 
+    uint16_t get_reg_value(uint8_t reg) {
+        return be16toh(word[reg]);
+    }
+
 protected:
 
     uint16_t word[256];

libraries/SITL/SIM_SMBusDevice.cpp

@@ -0,0 +1,67 @@
+#include "SIM_SMBusDevice.h"
+
+int SITL::SMBusDevice::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    // see if this is a block...
+    const uint8_t addr = data->msgs[0].buf[0];
+    if (blockname[addr] == nullptr) {
+        // not a block
+        return I2CRegisters_16Bit::rdwr(data);
+    }
+
+    // it is a block.
+    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");
+        }
+
+        data->msgs[1].buf[0] = value_lengths[addr];
+        const uint8_t to_copy = MIN(data->msgs[1].len-1, value_lengths[addr]);
+        memcpy(&data->msgs[1].buf[1], values[addr], to_copy);
+        data->msgs[1].len = to_copy + 1;
+        return 0;
+    }
+
+    if (data->nmsgs == 1) {
+        // data write request
+        if (data->msgs[0].flags != 0) {
+            AP_HAL::panic("Unexpected flags");
+        }
+        AP_HAL::panic("block writes not implemented");
+    }
+
+    return -1;
+}
+
+void SITL::SMBusDevice::add_block(const char *name, uint8_t reg, int8_t mode)
+{
+    // ::fprintf(stderr, "Adding block %u (0x%02x) (%s)\n", reg, reg, name);
+    blockname[reg] = name;
+    if (mode == O_RDONLY || mode == O_RDWR) {
+        readable_blocks.set((uint8_t)reg);
+    }
+    if (mode == O_WRONLY || mode == O_RDWR) {
+        writable_blocks.set((uint8_t)reg);
+    }
+}
+
+void SITL::SMBusDevice::set_block(uint8_t block, uint8_t *value, uint8_t valuelen)
+{
+    if (blockname[block] == nullptr) {
+        AP_HAL::panic("Setting un-named block %u", block);
+    }
+    // ::fprintf(stderr, "Setting %u (0x%02x) (%s) to 0x%02x (%c)\n", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
+    if (valuelen == 0) {
+        AP_HAL::panic("Zero-length values not permitted by spec");
+    }
+    if (values[block] != nullptr) {
+        free(values[block]);
+    }
+    values[block] = (char*)malloc(valuelen);
+    memcpy(values[block], value, valuelen);
+    value_lengths[block] = valuelen;
+}

libraries/SITL/SIM_SMBusDevice.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "SIM_I2CDevice.h"
+
+namespace SITL {
+
+class SMBusRegEnum : public I2CRegEnum {
+};
+
+class SMBusDevice : public I2CDevice, private I2CRegisters_16Bit {
+public:
+
+    SMBusDevice() :
+        I2CDevice(),
+        I2CRegisters_16Bit()
+        { }
+
+    void set_register(uint8_t reg, uint16_t value) {
+        I2CRegisters_16Bit::set_register(reg, value);
+    }
+    void set_register(uint8_t reg, int16_t value) {
+        I2CRegisters_16Bit::set_register(reg, value);
+    }
+
+    uint16_t get_reg_value(uint8_t reg) {
+        return I2CRegisters_16Bit::get_reg_value(reg);
+    }
+
+
+protected:
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
+
+    void set_block(uint8_t block, uint8_t *value, uint8_t valuelen);
+    void add_block(const char *name, uint8_t reg, int8_t mode);
+
+    void set_block(uint8_t block, const char *value) {
+        set_block(block, (uint8_t*)value, strlen(value));
+    }
+
+
+    void add_register(const char *name, uint8_t reg, int8_t mode) {
+        I2CRegisters_16Bit::add_register(name, reg, mode);
+    }
+
+private:
+
+    const char *blockname[256];
+    Bitmask<256> writable_blocks;
+    Bitmask<256> readable_blocks;
+
+    // 256 pointers-to-malloced-values:
+    char *values[256];
+    uint8_t value_lengths[256];
+};
+
+} // namespace SITL