From 57177c074e225d210d6c7d456dda5e0185cf6df9 Mon Sep 17 00:00:00 2001
From: Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
Date: Wed, 3 Jan 2024 11:45:56 +0100
Subject: [PATCH] fix several issues with i2c implementation and add support
 for interacting with i2c switches

Signed-off-by: Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
---
 fpga/include/villas/fpga/ips/i2c.hpp | 27 +++++++++++++++
 fpga/lib/ips/i2c.cpp                 | 51 ++++++++++++++++++++++++----
 2 files changed, 71 insertions(+), 7 deletions(-)

diff --git a/fpga/include/villas/fpga/ips/i2c.hpp b/fpga/include/villas/fpga/ips/i2c.hpp
index 4208600ef..07b277f00 100644
--- a/fpga/include/villas/fpga/ips/i2c.hpp
+++ b/fpga/include/villas/fpga/ips/i2c.hpp
@@ -33,6 +33,32 @@ public:
   int receiveIntrs;
   int statusIntrs;
 
+  class Switch {
+  public:
+    Switch(I2c *i2c, uint8_t address)
+        : i2c(i2c), address(address), channel(0), readOnce(false){};
+    Switch(const Switch &other) = delete;
+    Switch &operator=(const Switch &other) = delete;
+    void setChannel(uint8_t channel);
+    uint8_t getChannel();
+    void setAddress(uint8_t address) { this->address = address; }
+    uint8_t getAddress() { return address; }
+
+  private:
+    I2c *i2c;
+    uint8_t address;
+    uint8_t channel;
+    bool readOnce;
+  };
+  Switch &getSwitch(uint8_t address = 0x70) {
+    if (switchInstance == nullptr) {
+      switchInstance = std::make_unique<Switch>(this, address);
+    } else {
+      switchInstance->setAddress(address);
+    }
+    return *switchInstance;
+  }
+
 private:
   static constexpr char registerMemory[] = "Reg";
   static constexpr char i2cInterrupt[] = "iic2intc_irpt";
@@ -45,6 +71,7 @@ private:
   bool configDone;
   bool initDone;
   bool polling;
+  std::unique_ptr<Switch> switchInstance;
 
   std::list<MemoryBlockName> getMemoryBlocks() const {
     return {registerMemory};
diff --git a/fpga/lib/ips/i2c.cpp b/fpga/lib/ips/i2c.cpp
index d6404bf7c..32fa7675b 100644
--- a/fpga/lib/ips/i2c.cpp
+++ b/fpga/lib/ips/i2c.cpp
@@ -17,9 +17,10 @@ using namespace villas::fpga::ip;
 
 I2c::I2c()
     : Node(), transmitIntrs(0), receiveIntrs(0), statusIntrs(0), xIic(),
-      xConfig(), hwLock(), configDone(false), initDone(false), polling(false) {}
+      xConfig(), hwLock(), configDone(false), initDone(false), polling(false),
+      switchInstance(nullptr) {}
 
-I2c::~I2c() {}
+I2c::~I2c() { reset(); }
 
 static void SendHandler(I2c *i2c, __attribute__((unused)) int bytesSend) {
   i2c->transmitIntrs++;
@@ -53,10 +54,16 @@ bool I2c::init() {
   irqs[i2cInterrupt].irqController->enableInterrupt(irqs[i2cInterrupt],
                                                     polling);
 
+  initDone = true;
   return true;
 }
 
-bool I2c::reset() { return true; }
+bool I2c::reset() {
+  XIic_Reset(&xIic);
+  irqs[i2cInterrupt].irqController->disableInterrupt(irqs[i2cInterrupt]);
+  initDone = false;
+  return true;
+}
 
 bool I2c::write(u8 address, std::vector<u8> &data) {
   int ret;
@@ -75,7 +82,7 @@ bool I2c::write(u8 address, std::vector<u8> &data) {
   xIic.Stats.TxErrors = 0;
 
   int retries = 10;
-  while (xIic.Stats.TxErrors != 0 && retries > 0) {
+  while (transmitIntrs == 0 && xIic.Stats.TxErrors == 0 && retries > 0) {
     ret = XIic_Start(&xIic);
     if (ret != XST_SUCCESS) {
       throw RuntimeError("Failed to start I2C");
@@ -99,8 +106,9 @@ bool I2c::write(u8 address, std::vector<u8> &data) {
   if (ret != XST_SUCCESS) {
     throw RuntimeError("Failed to stop I2C");
   }
-  if (retries == 0) {
-    throw RuntimeError("Failed to send I2C data");
+  if (retries == 0 || xIic.Stats.TxErrors != 0) {
+    throw RuntimeError("Failed to send I2C data: {} retries, {} errors",
+                       retries, xIic.Stats.TxErrors);
   }
   return true;
 }
@@ -129,7 +137,7 @@ bool I2c::read(u8 address, std::vector<u8> &data, size_t max_read) {
   }
 
   int retries = 10;
-  while (xIic.Stats.TxErrors != 0 && retries > 0) {
+  while (receiveIntrs == 0 && retries > 0) {
     ret = XIic_Start(&xIic);
     if (ret != XST_SUCCESS) {
       throw RuntimeError("Failed to start I2C");
@@ -160,6 +168,35 @@ bool I2c::read(u8 address, std::vector<u8> &data, size_t max_read) {
   return XST_SUCCESS;
 }
 
+static const uint8_t CHANNEL_MAP[] = {0x20, 0x80, 0x02, 0x08,
+                                      0x10, 0x40, 0x01, 0x04};
+void I2c::Switch::setChannel(uint8_t channel) {
+  if (channel > sizeof(CHANNEL_MAP) / sizeof(CHANNEL_MAP[0])) {
+    throw RuntimeError("Invalid channel number {}", channel);
+  }
+  this->channel = channel;
+  std::vector<u8> data = {CHANNEL_MAP[channel]};
+  i2c->write(address, data);
+}
+
+uint8_t I2c::Switch::getChannel() {
+  std::vector<u8> data(1);
+  i2c->read(address, data, 1);
+  if (readOnce && data[0] != CHANNEL_MAP[channel]) {
+    throw RuntimeError("Invalid channel readback: {:x} != {:x}", data[0],
+                       CHANNEL_MAP[channel]);
+  } else {
+    for (size_t i = 0; i < sizeof(CHANNEL_MAP) / sizeof(CHANNEL_MAP[0]); ++i) {
+      if (data[0] == CHANNEL_MAP[i]) {
+        channel = i;
+        break;
+      }
+    }
+    readOnce = true;
+  }
+  return channel;
+}
+
 void I2cFactory::parse(Core &ip, json_t *cfg) {
   NodeFactory::parse(ip, cfg);