tw.h

#ifndef TW_H_
#define TW_H_

#include <Wire.h>

namespace tw {

enum class Signage {
  UNSIGNED,
  SIGNED
};

namespace detail {

// Template to pick integer type to fit a register of a particular bit
// width and signedness
template <uint8_t BitWidth, Signage Signed>
struct RegValue_ {
  // Recursively try next largest bit width if there isn't a
  // specialization
  typedef typename RegValue_ < BitWidth + 1, Signed >::Type Type;
};

template<>
struct RegValue_<8, Signage::UNSIGNED> {
  typedef uint8_t Type;
};

template<>
struct RegValue_<8, Signage::SIGNED> {
  typedef int8_t Type;
};

template<>
struct RegValue_<16, Signage::UNSIGNED> {
  typedef uint16_t Type;
};

template<>
struct RegValue_<16, Signage::SIGNED> {
  typedef int16_t Type;
};

template<>
struct RegValue_<32, Signage::UNSIGNED> {
  typedef uint32_t Type;
};

template<>
struct RegValue_<32, Signage::SIGNED> {
  typedef int32_t Type;
};

template<uint8_t BitWidth, Signage Signed>
using RegValue = typename RegValue_<BitWidth, Signed>::Type;

void rawRead(uint8_t addr, uint8_t reg, uint8_t* output, uint8_t size)
{
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom(addr, size, static_cast<uint8_t>(true));
  uint8_t rcvBytes = 0;
  while (Wire.available())
  {
    output[rcvBytes++] = Wire.read();
  }

#if 0
  Serial.print("READ "); Serial.print(addr, HEX); Serial.print("@"); Serial.print(reg, HEX); Serial.print(": ");
  for (uint8_t i = 0; i < size; ++i) {
    Serial.print(output[i] >> 4, HEX); Serial.print(output[i] & 0xF, HEX);
  }
  Serial.println();
#endif
}

void rawWrite(uint8_t addr, uint8_t reg, uint8_t const* input, uint8_t size)
{
#if 0
  Serial.print("WRITE "); Serial.print(addr, HEX); Serial.print("@"); Serial.print(reg, HEX); Serial.print(": ");
  for (uint8_t i = 0; i < size; ++i) {
    Serial.print(input[i] >> 4, HEX); Serial.print(input[i] & 0xF, HEX);
  }
  Serial.println();
#endif
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.write(input, size);
  Wire.endTransmission();
}

// Number of bytes covered by a register with a particular offset and
// width
static constexpr uint8_t byteSize(uint8_t BitOffset, uint8_t BitWidth) {
  return (BitOffset + BitWidth + 7) / 8;
}

// Decode a register from the raw bytes it covers
template<uint8_t BitOffset, uint8_t BitWidth>
RegValue<BitWidth, Signage::UNSIGNED> regDecode(uint8_t* data) {
  RegValue<BitWidth, Signage::UNSIGNED> value;
  // For the least significant byte, just shift off the leading bits
  // we don't want
  value = data[0] >> BitOffset;
  // Append the rest of the bytes to the value.  Note the correction
  // by -BitOffset to line the positions up with the least significant
  // byte above.
  for (uint8_t i = 1; i < byteSize(BitOffset, BitWidth); ++i) {
    value |= static_cast<RegValue<BitWidth, Signage::UNSIGNED>>(data[i]) << (8 * i - BitOffset);
  }
  // Mask off extraneous bits from the most significant byte
  value &= (1 << BitWidth) - 1;
  return value;
}

template<uint8_t BitOffset, uint8_t BitWidth>
RegValue<BitWidth, Signage::UNSIGNED> regRead(uint8_t addr, uint8_t offset) {
  static const uint8_t bs = byteSize(BitOffset, BitWidth);
  uint8_t data[bs];
  rawRead(addr, offset, data, bs);
  return regDecode<BitOffset, BitWidth>(data);
}

// This silences a compiler warning because GCC can't tell that a
// negative result only occurs when the if branch isn't taken
static inline uint8_t rightShift(uint8_t size, uint8_t offset) {
  return 8 * (size - 1) - offset;
}

// Encode a register into the raw bytes it covers
template<uint8_t BitOffset, uint8_t BitWidth>
void regEncode(RegValue<BitWidth, Signage::UNSIGNED> value, uint8_t* data) {
  static const uint8_t bs = byteSize(BitOffset, BitWidth);
  // Special case when only one byte is covered
  if (bs == 1) {
    // Create a mask of the specified width and position
    static const uint8_t mask = static_cast<uint8_t>(((1u << BitWidth) - 1) << BitOffset);
    // Move value into position, mask it, and combine with existing
    // data
    data[0] = (data[0] & ~mask) | ((value << BitOffset) & mask);
  } else {
    // For the least significant byte, create a mask from the bit
    // offset up
    static const uint8_t mask = static_cast<uint8_t>(0xFFu << BitOffset);
    // Move value into position, mask it, and combine with existing
    // data
    data[0] = (data[0] & ~mask) | ((value << BitOffset) & mask);
    // For remaining bytes other than the most significant, the entire
    // byte is covered, so simply shift into position and truncate
    for (uint8_t i = 1; i < bs - 1; ++i) {
      data[i] = value >> (8 * i - BitOffset);
    }
    // For the most significant byte, create a mask for the covered
    // lower bits
    static const uint8_t mask2 = (1 << (BitWidth + BitOffset - (bs - 1) * 8)) - 1;
    // Apply it.  rightShift is really (8 * (bs - 1) - BitOffset), but
    // gcc doesn't realize that bs - 1 can't be negative in this
    // branch and warns about a negative shift value.  The helper
    // function silences the warning.
    data[bs - 1] = (data[bs - 1] & ~mask2) | (value >> rightShift(bs, BitOffset));
  }
}

template<uint8_t BitOffset, uint8_t BitWidth>
void regWrite(uint8_t addr, uint8_t offset, RegValue<BitWidth, Signage::UNSIGNED> value) {
  static const uint8_t bs = byteSize(BitOffset, BitWidth);
  uint8_t data[bs];
  if (BitOffset == 0 && BitWidth % 8 == 0)
    memset(data, 0, sizeof(data));
  else
    rawRead(addr, offset, data, bs);
  regEncode<BitOffset, BitWidth>(value, data);
  rawWrite(addr, offset, data, bs);
}

// Template to compute the range of byte offsets covered by a list of
// registers
template<class... Regs>
struct RegRange {};

template<>
struct RegRange<> {
  static const uint8_t start = 0xFF;
  static const uint8_t end = 0x00;
};

template<class Reg, class... Tail>
struct RegRange<Reg, Tail...> {
  private:
    static const uint8_t tailStart = RegRange<Tail...>::start;
    static const uint8_t tailEnd = RegRange<Tail...>::end;
    static const uint8_t regEnd = Reg::byteOffset + byteSize(Reg::bitOffset, Reg::bitWidth);
  public:
    static const uint8_t start = Reg::byteOffset < tailStart ? Reg::byteOffset : tailStart;
    static const uint8_t end = regEnd > tailEnd ? regEnd : tailEnd;
};

// Template to check whether a list of segmented registers are all in
// the same segment.
template<class SegValue, class... Regs>
struct SegCompatible {};

template<class SegValue, class Reg>
struct SegCompatible<SegValue, Reg> {
  static const bool compatible = true;
  static constexpr SegValue segment = Reg::segment;
};

template<class SegValue, class Reg1, class Reg2, class... Tail>
struct SegCompatible<SegValue, Reg1, Reg2, Tail...> {
  static const bool compatible = Reg1::segment == Reg2::segment && SegCompatible<SegValue, Reg2, Tail...>::compatible;
  static constexpr SegValue segment = Reg1::segment;
};

}

template <uint8_t Addr>
class Slave {
  protected:
    template <uint8_t ByteOffset, uint8_t BitOffset, uint8_t BitWidth, Signage Signed>
    class Register {
        static_assert(BitWidth <= 32, "Register too wide");
        static_assert(BitOffset < 8, "Register bit offset too large");

      public:
        static const uint8_t byteOffset = ByteOffset;
        static const uint8_t bitOffset = BitOffset;
        static const uint8_t bitWidth = BitWidth;
        static const Signage signage = Signed;

        typedef detail::RegValue<BitWidth, Signed> Value;

        static inline Value read(void) {
          return detail::regRead<BitOffset, BitWidth>(Addr, ByteOffset);
        }

        static inline void write(Value value) {
          detail::regWrite<BitOffset, BitWidth>(Addr, ByteOffset, value);
        }
    };

    // Ties several registers together so they can be
    // read/written/modified in a single transaction.
    template <class... Regs>
    class Tie {
      public:
        static void read(typename Regs::Value &... values) {
          static const uint8_t start = detail::RegRange<Regs...>::start;
          static const uint8_t end = detail::RegRange<Regs...>::end;
          static const uint8_t size = end - start;
          uint8_t data[size];
          detail::rawRead(Addr, start, data, size);
          // We really want a regDecode statement for each register, but
          // template parameter packs can only be expanded in expression
          // list contexts such as function parameters or initializer
          // lists.  We turn each call into an int expression with the
          // comma operator and capture the results into a dummy array.
          // The attribute prevents gcc from warning about an unused
          // variable.
          int dummy[] __attribute__((unused)) = {
            (values = detail::regDecode<Regs::bitOffset, Regs::bitWidth>(data + Regs::byteOffset - start), 0)...
          };
        }

        static void write(typename Regs::Value... values) {
          static const uint8_t start = detail::RegRange<Regs...>::start;
          static const uint8_t end = detail::RegRange<Regs...>::end;
          static const uint8_t size = end - start;
          uint8_t data[size];
          // FIXME: we can avoid this if registers are contiguous and
          // aligned to byte boundaries at both start and end.  The
          // template logic for determining this would be a bit complex
          // since we would need to sort the register list first.
          detail::rawRead(Addr, start, data, size);
          int dummy[] __attribute__((unused)) = {
            (detail::regEncode<Regs::bitOffset, Regs::bitWidth>(values, data + Regs::byteOffset - start), 0)...
          };
          detail::rawWrite(Addr, start, data, size);
        }
    };
};

template<uint8_t Addr, class Attrs>
class SegmentedSlave : public Slave<Addr> {
  private:
    typedef tw::Slave<Addr> Base;
    template<uint8_t ByteOffset, uint8_t BitOffset, uint8_t BitWidth, Signage Signed>
    using BaseReg = typename Base::template Register<ByteOffset, BitOffset, BitWidth, Signed>;
    typedef BaseReg<Attrs::SegByteOffset, Attrs::SegBitOffset, Attrs::SegBitWidth, Signage::UNSIGNED> Segment;

  protected:
    typedef typename Segment::Value SegValue;

  private:
    static void setSeg(SegValue seg) {
      static SegValue curSeg = Attrs::SegInitial;
      if (curSeg != seg) {
        Segment::write(seg);
        curSeg = seg;
      }
    }

  public:
    template<SegValue Seg, uint8_t ByteOffset, uint8_t BitOffset, uint8_t BitWidth, Signage Signed>
    class Register : public BaseReg<ByteOffset, BitOffset, BitWidth, Signed> {
      private:
        typedef BaseReg<ByteOffset, BitOffset, BitWidth, Signed> Base;
      public:
        typedef typename Base::Value Value;
        static const SegValue segment = Seg;

        static Value read(void) {
          setSeg(Seg);
          return Base::read();
        }

        static void write(Value value) {
          setSeg(Seg);
          Base::write(value);
        }
    };

    template<class... Regs>
    class Tie : public Base::template Tie<Regs...> {
      private:
      static_assert(detail::SegCompatible<SegValue, Regs...>::compatible, "Tied registers must all be in the same segment");
      static const SegValue segment = detail::SegCompatible<SegValue, Regs...>::segment;
      typedef typename Base::template Tie<Regs...> BaseTie;

      public:
    static void read(typename Regs::Value &... values) {
      setSeg(segment);
      BaseTie::read(values...);
    }

    static void write(typename Regs::Value... values) {
      setSeg(segment);
      BaseTie::write(values...);
    }

    template<class... Funcs>
    static void modify(Funcs... funcs) {
      setSeg(segment);
      BaseTie::modify(funcs...);
    }
            };

    static void read(SegValue seg, uint8_t offset, uint8_t* output, uint8_t size) {
      setSeg(seg);
      detail::rawRead(Addr, offset, output, size);
    }

    static uint8_t read(SegValue seg, uint8_t offset) {
      uint8_t value;
      read(seg, offset, &value, sizeof(value));
      return value;
    }

    static void write(SegValue seg, uint8_t offset, uint8_t const* input, uint8_t size) {
      setSeg(seg);
      detail::rawWrite(Addr, offset, input, size);
    }

    static void write(SegValue seg, uint8_t offset, uint8_t value) {
      write(seg, offset, &value, sizeof(value));
    }
};

}

#endif