From 73e02bfe98c1c2cf7c9d39fbffb9e7200c19fa59 Mon Sep 17 00:00:00 2001
From: Adam Honse <calcprogrammer1@gmail.com>
Date: Fri, 24 Dec 2021 13:26:27 -0600
Subject: [PATCH] Implement register functionality for Linux and NvAPI i2c
 drivers

---
 i2c_smbus/i2c_smbus_linux.cpp | 39 ++++++++++++++++++++++++++++-------
 i2c_smbus/i2c_smbus_nvapi.cpp |  4 ++--
 2 files changed, 33 insertions(+), 10 deletions(-)

diff --git a/i2c_smbus/i2c_smbus_linux.cpp b/i2c_smbus/i2c_smbus_linux.cpp
index d981b1ff1..7b2bfa956 100644
--- a/i2c_smbus/i2c_smbus_linux.cpp
+++ b/i2c_smbus/i2c_smbus_linux.cpp
@@ -33,16 +33,39 @@ s32 i2c_smbus_linux::i2c_smbus_xfer(u8 addr, char read_write, u8 command, int si
 s32 i2c_smbus_linux::i2c_xfer(u8 addr, char read_write, int reg_size, u8* reg_data, int* size, u8* data)
 {
     i2c_rdwr_ioctl_data rdwr;
-    i2c_msg msg;
+    i2c_msg msgs[2];
 
-    msg.addr  = addr;
-    msg.flags = read_write;
-    msg.len   = *size;
-    msg.buf   = (u8*)malloc(*size);
-    memcpy(&msg.buf, &data, *size);
+    /*-------------------------------------------------*\
+    | Create register message                           |
+    \*-------------------------------------------------*/
+    if(reg_size > 0)
+    {
+        msgs[0].addr    = addr;
+        msgs[0].flags   = I2C_SMBUS_WRITE;
+        msgs[0].len     = reg_size;
+        msgs[0].buf     = (u8*)malloc(reg_size);
+        memcpy(&msgs[0].buf, &reg_data, reg_size);
+    }
 
-    rdwr.msgs  = &msg;
-    rdwr.nmsgs = 1;
+    /*-------------------------------------------------*\
+    | Create data message                               |
+    \*-------------------------------------------------*/
+    msgs[1].addr  = addr;
+    msgs[1].flags = read_write;
+    msgs[1].len   = *size;
+    msgs[1].buf   = (u8*)malloc(*size);
+    memcpy(&msgs[1].buf, &data, *size);
+
+    if(reg_size > 0)
+    {
+        rdwr.msgs   = &msgs[0];
+        rdwr.nmsgs  = 2;
+    }
+    else
+    {
+        rdwr.msgs   = &msgs[1];
+        rdwr.nmsgs  = 1;
+    }
 
     ioctl(handle, I2C_SLAVE, addr);
     return ioctl(handle, I2C_RDWR, &rdwr);
diff --git a/i2c_smbus/i2c_smbus_nvapi.cpp b/i2c_smbus/i2c_smbus_nvapi.cpp
index d5804bd46..de74636ff 100644
--- a/i2c_smbus/i2c_smbus_nvapi.cpp
+++ b/i2c_smbus/i2c_smbus_nvapi.cpp
@@ -124,8 +124,8 @@ s32 i2c_smbus_nvapi::i2c_xfer(u8 addr, char read_write, int reg_size, u8* reg_da
     unsigned int unknown = 0;
     NV_I2C_INFO_V3 i2c_data;
 
-    i2c_data.i2c_reg_address = NULL;
-    i2c_data.reg_addr_size = 0;
+    i2c_data.i2c_reg_address = reg_data;
+    i2c_data.reg_addr_size = reg_size;
 
     // Set up data buffer, zero bytes in length
 	i2c_data.data = data;