From 9113e8db57ce69a4f067707b30de4818da142008 Mon Sep 17 00:00:00 2001
From: surenyi <surenyi82@163.com>
Date: Fri, 25 Oct 2024 15:33:22 +0800
Subject: [PATCH] fix sm2130 driver's readReg/writeReg

Signed-off-by: surenyi <surenyi82@163.com>
---
 vxbSm2130SpiDev.c | 65 ++++++++++++++++++++++++++---------------------
 1 file changed, 36 insertions(+), 29 deletions(-)

diff --git a/vxbSm2130SpiDev.c b/vxbSm2130SpiDev.c
index 5a69edc..5d995e2 100644
--- a/vxbSm2130SpiDev.c
+++ b/vxbSm2130SpiDev.c
@@ -179,35 +179,6 @@ LOCAL int drv1553bClose(SM2310_DEV_HANDLE *dev)
     return (OK);
 }
 
-LOCAL int readReg(SM2130_SPI_DEV *pDrvCtrl, SM2130_XFER *xfer)
-{
-    UINT8 cmd;
-
-    if (xfer->reg <= 15) {
-        cmd = (xfer->reg << 2);
-    } else {
-        cmd = xfer->reg & 0xff;
-    }
-
-    xfer->val = pDrvCtrl->read(pDrvCtrl->pDev, cmd);
-    return 0;
-}
-
-LOCAL int writeReg(SM2130_SPI_DEV *pDrvCtrl, SM2130_XFER *xfer)
-{
-    UINT8 cmd;
-
-    if (xfer->reg <= 63) {
-        cmd = 0x80 | (xfer->reg & 0x3f);
-    } else {
-        cmd = xfer->reg & 0xff;
-    }
-
-    pDrvCtrl->write(pDrvCtrl->pDev, cmd, xfer->val);
-
-    return 0;
-}
-
 LOCAL int setMemAddress(SM2130_SPI_DEV *pDrvCtrl, SM2130_XFER *xfer)
 {
     SPI_TRANSFER transInfo = { NULL, NULL, 0, 0, 0 };
@@ -291,6 +262,42 @@ LOCAL int writeMemory(SM2130_SPI_DEV *pDrvCtrl, SM2130_MEM_XFER *mxfr)
     return OK;
 }
 
+LOCAL int readReg(SM2130_SPI_DEV *pDrvCtrl, SM2130_XFER *xfer)
+{
+    UINT8 cmd;
+    SM2130_MEM_XFER smfr = { 0, 0, 0, NULL };
+
+    if (xfer->reg <= 15) {
+        cmd = (xfer->reg << 2);
+        xfer->val = pDrvCtrl->read(pDrvCtrl->pDev, cmd);
+    } else {
+        smfr.count = 1;
+        smfr.reg = 0xb;
+        smfr.address = xfer->reg;
+        smfr.membuf = &xfer->val;
+        readMemory(pDrvCtrl, &smfr);
+    }
+    return 0;
+}
+
+LOCAL int writeReg(SM2130_SPI_DEV *pDrvCtrl, SM2130_XFER *xfer)
+{
+    UINT8 cmd;
+    SM2130_MEM_XFER smfr = { 0, 0, 0, NULL };
+
+    if (xfer->reg <= 63) {
+        cmd = 0x80 | (xfer->reg & 0x3f);
+        pDrvCtrl->write(pDrvCtrl->pDev, cmd, xfer->val);
+    } else {
+        smfr.count = 1;
+        smfr.reg = 0xb;
+        smfr.address = xfer->reg;
+        smfr.membuf = &xfer->val;
+        writeMemory(pDrvCtrl, &smfr);
+    }
+    return 0;
+}
+
 LOCAL ssize_t drv1553bRead(SM2310_DEV_HANDLE *dev, char *buffer, size_t maxbytes)
 {
     SM2130_SPI_DEV *pDrvCtrl = dev->pDevCtrl;