mirror of https://github.com/ArduPilot/ardupilot
68 lines
2.0 KiB
C++
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;
|
||
|
|
}
|