From b6833eee9b43d4f51564586024e6a9d7a0afad3c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Jan 2021 13:59:19 +1100 Subject: [PATCH] SITL: correct SMBus block reads --- libraries/SITL/SIM_BattMonitor_SMBus.cpp | 25 +++---- libraries/SITL/SIM_BattMonitor_SMBus.h | 11 ++- .../SITL/SIM_BattMonitor_SMBus_Rotoye.cpp | 3 +- libraries/SITL/SIM_I2CDevice.h | 4 ++ libraries/SITL/SIM_SMBusDevice.cpp | 67 +++++++++++++++++++ libraries/SITL/SIM_SMBusDevice.h | 57 ++++++++++++++++ 6 files changed, 142 insertions(+), 25 deletions(-) create mode 100644 libraries/SITL/SIM_SMBusDevice.cpp create mode 100644 libraries/SITL/SIM_SMBusDevice.h diff --git a/libraries/SITL/SIM_BattMonitor_SMBus.cpp b/libraries/SITL/SIM_BattMonitor_SMBus.cpp index 3e31f57167..87ac6e4f26 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus.cpp +++ b/libraries/SITL/SIM_BattMonitor_SMBus.cpp @@ -3,7 +3,7 @@ #include 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))); - uint8_t i = 1; // already sent the first byte out.... - while (i < strlen(manufacturer_name)) { - const uint8_t a = manufacturer_name[i++]; - 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); - } + set_block(SMBusBattDevReg::MANUFACTURE_NAME, manufacturer_name); + + const char *device_name = "SITLBatMon_V0.99"; + set_block(SMBusBattDevReg::DEVICE_NAME, device_name); + // TODO: manufacturer data } - void SITL::SIM_BattMonitor_SMBus::update(const class Aircraft &aircraft) { const uint32_t now = AP_HAL::millis(); diff --git a/libraries/SITL/SIM_BattMonitor_SMBus.h b/libraries/SITL/SIM_BattMonitor_SMBus.h index 22a5ae54c6..b622b4f91d 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus.h +++ b/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; diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp b/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp index 94ef2082c8..f5d8f63a62 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp +++ b/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) } } diff --git a/libraries/SITL/SIM_I2CDevice.h b/libraries/SITL/SIM_I2CDevice.h index d7396b06a8..6fc6b48573 100644 --- a/libraries/SITL/SIM_I2CDevice.h +++ b/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]; diff --git a/libraries/SITL/SIM_SMBusDevice.cpp b/libraries/SITL/SIM_SMBusDevice.cpp new file mode 100644 index 0000000000..b7a4b27c01 --- /dev/null +++ b/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; +} diff --git a/libraries/SITL/SIM_SMBusDevice.h b/libraries/SITL/SIM_SMBusDevice.h new file mode 100644 index 0000000000..10a921815d --- /dev/null +++ b/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