Register operations with I2C

gistfile1.txt

#include <stdio.h>
#include <stdint.h>

void printb(const char* name, const uint16_t value) {
  printf("%s: 0b", name);
  for (int i = 15; i >= 0; --i) {
    printf("%d", (value >> i) & 1);
  }
  printf("\n");
}

// --------------------------------------------------------


#define DRIVER_REG_Pos (0x00)
#define DRIVER_REG_Msk (0x03C0)
#define DRIVER_REG_Ctz (6U)


static uint16_t regMap[15U] = {0x0000};



void I2C_read16(const uint8_t regAddr, uint16_t * const data)
{
  *data = regMap[regAddr];
}

void I2C_write16(const uint8_t regAddr, const uint16_t data)
{
  regMap[regAddr] = data;
}

// --------------------------------------------------------

void readData(const uint8_t regAddr, const uint16_t bitmask, const uint8_t ctz, uint16_t * const data )
{
  uint16_t readData;
  I2C_read16(regAddr, &readData);
  *data = (readData & bitmask) >> ctz;
}

void writeData(const uint8_t regAddr, const uint16_t bitmask, const uint8_t ctz, const uint16_t data )
{
  // Read reg
  uint16_t readData;
  I2C_read16(regAddr, &readData);

  // Clear
  readData &= ~bitmask;

  // Apply
  readData |= ((uint16_t)data << ctz) & bitmask;

  // Write reg
  I2C_write16(regAddr, readData);
}

// --------------------------------------------------------

int main(void)
{
  const uint8_t regAddr = DRIVER_REG_Pos;
  const uint16_t bitmask = DRIVER_REG_Msk;
  const uint8_t ctz = DRIVER_REG_Ctz;
  printf("%s\n", "No:   0b5432109876543210");

  // Read
  uint16_t data = 0xFFFF;
  readData(regAddr, bitmask, ctz, &data );
  printb("Data", data);

  // Write
  writeData(regAddr, bitmask, ctz, 0xFFFF );

  // Read again
  readData(regAddr, bitmask, ctz, &data );
  printb("Data", data);

  return 0;
}