update fmp

OpenRTX · Feb 28, 2024 · 7aa15e5 · 7aa15e5
1 parent 38f4dc6
commit 7aa15e5
Showing 1 changed file with 21 additions and 20 deletions.
diff --git a/openrtx/src/core/rtxlink_fmp.c b/openrtx/src/core/rtxlink_fmp.c
@@ -98,7 +98,8 @@ static size_t cmd_meminfo(const uint8_t *args, const uint8_t nArg, uint8_t *repl
     return replySize;
 }
 
-static size_t cmd_dumpRestore(const uint8_t *args, const uint8_t nArg, uint8_t *reply)
+static size_t cmd_dumpRestore(const uint8_t cmd, const uint8_t *args,
+                              const uint8_t nArg, uint8_t *reply)
 {
     (void) nArg;
 
@@ -111,15 +112,15 @@ static size_t cmd_dumpRestore(const uint8_t *args, const uint8_t nArg, uint8_t *
     reply[2]         = 0x00;    // Empty response, no parameters
 
     // Verify memory index
-    uint8_t area = args[0];
+    uint8_t area = args[1];
     if(area >= numAreas)
     {
         reply[1] = EINVAL;
         return replySize;
     }
 
     int ret;
-    if(reply[0] == FMP_FRAME_DUMP)
+    if(cmd == FMP_FRAME_DUMP)
         ret = dat_readNvmArea(&areas[area]);
     else
         ret = dat_writeNvmArea(&areas[area]);
@@ -130,10 +131,11 @@ static size_t cmd_dumpRestore(const uint8_t *args, const uint8_t nArg, uint8_t *
     return replySize;
 }
 
-static void fmpProtocolHandler(const uint8_t *data, size_t len)
+static size_t fmpProtocolHandler(const uint8_t *rxData, size_t rxLen,
+                                 uint8_t *txData, size_t txMaxLen)
 {
-    uint8_t reply[MAX_REPLY_SIZE];
-    size_t  rLen = 0;
+    (void) txMaxLen;
+    size_t rLen = 0;
 
     /*
      * FMP request structure:
@@ -142,30 +144,29 @@ static void fmpProtocolHandler(const uint8_t *data, size_t len)
      * - byte 1: number of arguments
      * - byte 2..N: command arguments
      */
-    const uint8_t cmd   =  data[0];
-    const uint8_t nArgs =  data[1];
-    const uint8_t *args = &data[3];
+    const uint8_t cmd   = rxData[0];
+    const uint8_t nArgs = rxData[1];
+    const uint8_t *args = rxData + 2;
 
     // A command frame must contain at least two bytes
-    if(len < 2)
+    if(rxLen < 2)
     {
-        reply[0] = cmd;
-        reply[1] = EPROTO;
+        txData[0] = cmd;
+        txData[1] = EPROTO;
 
-        rtxlink_send(RTXLINK_FRAME_FMP, reply, 2);
-        return;
+        return 2;
     }
 
     // Setup standard reply, content will be overridden by command handlers
-    reply[0] = cmd;
-    reply[1] = EPERM;
-    rLen     = 2;
+    txData[0] = cmd;
+    txData[1] = EPERM;
+    rLen      = 2;
 
     // Handle the incoming command
     switch(cmd)
     {
         case FMP_FRAME_MEMINFO:
-            rLen = cmd_meminfo(args, nArgs, reply);
+            rLen = cmd_meminfo(args, nArgs, txData);
             break;
 
         case FMP_FRAME_DUMP:
@@ -174,14 +175,14 @@ static void fmpProtocolHandler(const uint8_t *data, size_t len)
             if(state.devStatus != DATATRANSFER)
                 break;
 
-            rLen = cmd_dumpRestore(args, nArgs, reply);
+            rLen = cmd_dumpRestore(cmd, args, nArgs, txData);
             break;
 
         default:
             break;
     }
 
-    rtxlink_send(RTXLINK_FRAME_FMP, reply, rLen);
+    return rLen;
 }