ardupilot/libraries/SITL/SIM_SMBusDevice.cpp

68 lines
2.0 KiB
C++

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