Line data    Source code

       1             : #include "SIM_SMBusDevice.h"
       2             : 
       3        6274 : int SITL::SMBusDevice::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
       4             : {
       5             :     // see if this is a block...
       6        6274 :     const uint8_t addr = data->msgs[0].buf[0];
       7        6274 :     if (blockname[addr] == nullptr) {
       8             :         // not a block
       9        6274 :         return I2CRegisters_16Bit::rdwr(data);
      10             :     }
      11             : 
      12             :     // it is a block.
      13           0 :     if (data->nmsgs == 2) {
      14             :         // data read request
      15           0 :         if (data->msgs[0].flags != 0) {
      16           0 :             AP_HAL::panic("Unexpected flags");
      17             :         }
      18           0 :         if (data->msgs[1].flags != I2C_M_RD) {
      19           0 :             AP_HAL::panic("Unexpected flags");
      20             :         }
      21             : 
      22           0 :         data->msgs[1].buf[0] = value_lengths[addr];
      23           0 :         const uint8_t to_copy = MIN(data->msgs[1].len-1, value_lengths[addr]);
      24           0 :         memcpy(&data->msgs[1].buf[1], values[addr], to_copy);
      25           0 :         data->msgs[1].len = to_copy + 1;
      26           0 :         return 0;
      27             :     }
      28             : 
      29           0 :     if (data->nmsgs == 1) {
      30             :         // data write request
      31           0 :         if (data->msgs[0].flags != 0) {
      32           0 :             AP_HAL::panic("Unexpected flags");
      33             :         }
      34           0 :         AP_HAL::panic("block writes not implemented");
      35             :     }
      36             : 
      37           0 :     return -1;
      38             : }
      39             : 
      40       19305 : void SITL::SMBusDevice::add_block(const char *name, uint8_t reg, I2CRegisters::RegMode mode)
      41             : {
      42             :     // ::fprintf(stderr, "Adding block %u (0x%02x) (%s)\n", reg, reg, name);
      43       19305 :     blockname[reg] = name;
      44       19305 :     if (mode == I2CRegisters::RegMode::RDONLY ||
      45             :         mode == I2CRegisters::RegMode::RDWR) {
      46       19305 :         readable_blocks.set((uint8_t)reg);
      47             :     }
      48       19305 :     if (mode == I2CRegisters::RegMode::WRONLY ||
      49             :         mode == I2CRegisters::RegMode::RDWR) {
      50           0 :         writable_blocks.set((uint8_t)reg);
      51             :     }
      52       19305 : }
      53             : 
      54       34320 : void SITL::SMBusDevice::set_block(uint8_t block, uint8_t *value, uint8_t valuelen)
      55             : {
      56       34320 :     if (blockname[block] == nullptr) {
      57           0 :         AP_HAL::panic("Setting un-named block %u", block);
      58             :     }
      59             :     // ::fprintf(stderr, "Setting %u (0x%02x) (%s) to 0x%02x (%c)\n", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
      60       34320 :     if (valuelen == 0) {
      61           0 :         AP_HAL::panic("Zero-length values not permitted by spec");
      62             :     }
      63       34320 :     if (values[block] != nullptr) {
      64       15015 :         free(values[block]);
      65             :     }
      66       34320 :     values[block] = (char*)malloc(valuelen);
      67       34320 :     memcpy(values[block], value, valuelen);
      68       34320 :     value_lengths[block] = valuelen;
      69       34320 : }