1 files changed, 106 insertions, 0 deletions
diff --git a/src/camera/veye_i2c.cpp b/src/camera/veye_i2c.cpp
new file mode 100644
index 0000000..9d04d9b
--- /dev/null
+++ b/src/camera/veye_i2c.cpp
@@ -0,0 +1,106 @@
+#include "veye_i2c.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <iostream>
+#include <linux/i2c-dev.h>
+#include <linux/i2c.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+#include "veyeimx287m_types.h"
+
+bool veye::imx287m::i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &value)
+{
+    int err;
+    int i = 0;
+    uint8_t buf[2] = {reg >> 8, reg & 0xff};
+    uint8_t bufout[4] = {0};
+    struct i2c_msg msgs[2] = {
+        {.addr = i2c_addr,
+         .flags = 0,
+         .len = 2,
+         // .buf = (uint8_t *) (&reg),
+         .buf = buf},
+        {
+            .addr = i2c_addr,
+            .flags = I2C_M_RD,
+            .len = 4,
+            .buf = bufout,
+        },
+    };
+
+    struct i2c_rdwr_ioctl_data msgset;
+    msgset.msgs = msgs;
+    msgset.nmsgs = 2;
+
+    err = ioctl(fd, I2C_RDWR, &msgset);
+    //printf("Read i2c addr %02X\n", i2c_addr);
+    if (err != msgset.nmsgs) {
+        std::cerr << "Read i2c err " << err << std::endl;
+        return false;
+    }
+
+    value = ntohl(*(uint32_t *) bufout);
+    // fprintf(stderr, "addr %04x : value %08x \n", reg + i, value);
+
+    return true;
+}
+
+bool veye::imx287m::test(uint32_t value)
+{
+    const std::string i2cDevName{"/dev/i2c-10"};
+    constexpr uint32_t i2cDevAddr{0x3b};
+    int fd = open(i2cDevName.c_str(), O_RDWR);
+
+    if (!fd) {
+        std::cerr << "cannot open i2c device " << i2cDevName << ", error: " << strerror(errno)
+                  << std::endl;
+        return false;
+    }
+
+    if (ioctl(fd, I2C_SLAVE_FORCE, i2cDevAddr) < 0) {
+        std::cerr << "cannot set i2c slave. dev: " << i2cDevName << ", addr: " << i2cDevAddr
+                  << ", error: " << strerror(errno) << std::endl;
+
+        return false;
+    }
+
+    uint32_t expTime{0};
+    // if (!i2cRead(fd, i2cDevAddr, (uint16_t) veye::imx287m::Register::ME_Time, expTime)) {
+    //     return false;
+    // }
+
+    // std::cout << "exp time is: " << expTime << std::endl;
+    std::cout << "set exp time to " << value;
+
+    if (!i2cWrite(fd, (uint16_t) veye::imx287m::Register::ME_Time, value)) {
+        return false;
+    }
+
+    if (!i2cRead(fd, i2cDevAddr, (uint16_t) veye::imx287m::Register::ME_Time, expTime)) {
+        return false;
+    }
+    std::cout << "\texp time is: " << expTime << std::endl;
+
+    return true;
+}
+
+bool veye::imx287m::i2cWrite(int fd, uint16_t reg, const uint32_t value)
+{
+    uint8_t msg[] = {reg >> 8, reg & 0xff, value >> 24, value >> 16, value >> 8, value >> 0};
+    int len = sizeof(msg) / sizeof(msg[0]);
+
+    if (write(fd, msg, len) != len) {
+        std::cerr << "cannot write value. reg: " << reg << ", error: " << strerror(errno)
+                  << std::endl;
+        return false;
+    }
+
+    // TODO: find a best way to get up-to-date values
+    usleep(100);
+
+    return true;
+}