From cf5d94b81ff0fcc2b413e8aea7f88e7af2588dc8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 17 Feb 2022 21:11:09 +1100
Subject: [PATCH] AP_CANManager: fixed slcan receive of CANFD frames

---
 libraries/AP_CANManager/AP_SLCANIface.cpp | 17 ++++++++---------
 libraries/AP_CANManager/AP_SLCANIface.h   |  2 +-
 2 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_CANManager/AP_SLCANIface.cpp b/libraries/AP_CANManager/AP_SLCANIface.cpp
index 24fb97e999..da89d24937 100644
--- a/libraries/AP_CANManager/AP_SLCANIface.cpp
+++ b/libraries/AP_CANManager/AP_SLCANIface.cpp
@@ -120,10 +120,11 @@ bool SLCAN::CANIface::push_Frame(AP_HAL::CANFrame &frame)
  *  <type> <id> <dlc> <data>
  * The emitting functions below are highly optimized for speed.
  */
-bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd)
+bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd, bool canfd)
 {
     AP_HAL::CANFrame f {};
     hex2nibble_error = false;
+    f.canfd = canfd;
     f.id = f.FlagEFF |
            (hex2nibble(cmd[1]) << 28) |
            (hex2nibble(cmd[2]) << 24) |
@@ -133,16 +134,14 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd)
            (hex2nibble(cmd[6]) <<  8) |
            (hex2nibble(cmd[7]) <<  4) |
            (hex2nibble(cmd[8]) <<  0);
-    if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) {
-        return false;
-    }
-    f.dlc = cmd[9] - '0';
-    if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) {
+    f.dlc = hex2nibble(cmd[9]);
+    if (hex2nibble_error || f.dlc > (canfd?15:8)) {
         return false;
     }
     {
         const char* p = &cmd[10];
-        for (unsigned i = 0; i < f.dlc; i++) {
+        const uint8_t dlen = AP_HAL::CANFrame::dlcToDataLength(f.dlc);
+        for (unsigned i = 0; i < dlen; i++) {
             f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1));
             p += 2;
         }
@@ -401,8 +400,8 @@ const char* SLCAN::CANIface::processCommand(char* cmd)
     /*
     * High-traffic SLCAN commands go first
     */
-    if (cmd[0] == 'T') {
-        return handle_FrameDataExt(cmd) ? "Z\r" : "\a";
+    if (cmd[0] == 'T' || cmd[0] == 'D') {
+        return handle_FrameDataExt(cmd, cmd[0]=='D') ? "Z\r" : "\a";
     } else if (cmd[0] == 't') {
         return handle_FrameDataStd(cmd) ? "z\r" : "\a";
     } else if (cmd[0] == 'R') {
diff --git a/libraries/AP_CANManager/AP_SLCANIface.h b/libraries/AP_CANManager/AP_SLCANIface.h
index 87771170c9..00e96eb44b 100644
--- a/libraries/AP_CANManager/AP_SLCANIface.h
+++ b/libraries/AP_CANManager/AP_SLCANIface.h
@@ -50,7 +50,7 @@ class CANIface: public AP_HAL::CANIface
     bool handle_FrameRTRStd(const char* cmd);
     bool handle_FrameRTRExt(const char* cmd);
     bool handle_FrameDataStd(const char* cmd);
-    bool handle_FrameDataExt(const char* cmd);
+    bool handle_FrameDataExt(const char* cmd, bool canfd);
 
     bool handle_FDFrameDataExt(const char* cmd);