Move most SDO commands to separate modules, also allowing user space …

…to add new commands #25
jsphuebner · Feb 4, 2025 · 083e910 · 083e910
1 parent 91b26e2
commit 083e910
Show file tree

Hide file tree

Showing 4 changed files with 202 additions and 117 deletions.
diff --git a/include/cansdo.h b/include/cansdo.h
@@ -23,31 +23,49 @@
 #include "canhardware.h"
 #include "canmap.h"
 
+#define SDO_REQUEST_DOWNLOAD  (1 << 5)
+#define SDO_REQUEST_UPLOAD    (2 << 5)
+#define SDO_REQUEST_SEGMENT   (3 << 5)
+#define SDO_TOGGLE_BIT        (1 << 4)
+#define SDO_RESPONSE_UPLOAD   (2 << 5)
+#define SDO_RESPONSE_DOWNLOAD (3 << 5)
+#define SDO_EXPEDITED         (1 << 1)
+#define SDO_SIZE_SPECIFIED    (1)
+#define SDO_WRITE             (SDO_REQUEST_DOWNLOAD | SDO_EXPEDITED | SDO_SIZE_SPECIFIED)
+#define SDO_READ              SDO_REQUEST_UPLOAD
+#define SDO_ABORT             0x80
+#define SDO_WRITE_REPLY       SDO_RESPONSE_DOWNLOAD
+#define SDO_READ_REPLY        (SDO_RESPONSE_UPLOAD | SDO_EXPEDITED | SDO_SIZE_SPECIFIED)
+#define SDO_ERR_INVIDX        0x06020000
+#define SDO_ERR_RANGE         0x06090030
+#define SDO_ERR_GENERAL       0x08000000
+
 class CanSdo: CanCallback, public IPutChar
 {
    public:
+      struct SdoFrame
+      {
+         uint8_t cmd;
+         uint16_t index;
+         uint8_t subIndex;
+         uint32_t data;
+      } __attribute__((packed));
+
       /** Default constructor */
       CanSdo(CanHardware* hw, CanMap* cm = 0);
+      CanHardware* GetHardware() { return canHardware; }
       void HandleClear();
       void HandleRx(uint32_t canId, uint32_t data[2], uint8_t dlc) override;
       void SDOWrite(uint8_t nodeId, uint16_t index, uint8_t subIndex, uint32_t data);
       void SDORead(uint8_t nodeId, uint16_t index, uint8_t subIndex);
       bool SDOReadReply(uint32_t& data);
       void SetNodeId(uint8_t id);
       int GetPrintRequest() { return printRequest; }
+      SdoFrame* GetPendingUserspaceSdo() { return pendingUserSpaceSdo ? &pendingUserSpaceSdoFrame : 0; }
+      void SendSdoReply(SdoFrame* sdoFrame);
       void PutChar(char c);
-      void EnableSaving() { saveEnabled = true; }
-      void DisableSaving() { saveEnabled = false; }
 
    private:
-      struct CAN_SDO
-      {
-         uint8_t cmd;
-         uint16_t index;
-         uint8_t subIndex;
-         uint32_t data;
-      } __attribute__((packed));
-
       CanHardware* canHardware;
       CanMap* canMap;
       uint8_t nodeId;
@@ -64,12 +82,13 @@ class CanSdo: CanCallback, public IPutChar
       CanMap::CANPOS mapInfo;
       bool sdoReplyValid;
       uint32_t sdoReplyData;
-      bool saveEnabled;
+      SdoFrame pendingUserSpaceSdoFrame;
+      bool pendingUserSpaceSdo;
 
       void ProcessSDO(uint32_t data[2]);
-      void ProcessSpecialSDOObjects(CAN_SDO *sdo);
-      void ReadOrDeleteCanMap(CAN_SDO *sdo);
-      void AddCanMap(CAN_SDO *sdo, bool rx);
+      bool ProcessSpecialSDOObjects(SdoFrame *sdo);
+      void ReadOrDeleteCanMap(SdoFrame *sdo);
+      void AddCanMap(SdoFrame *sdo, bool rx);
       void InitiateSDOTransfer(uint8_t req, uint8_t nodeId, uint16_t index, uint8_t subIndex, uint32_t data);
 };
 

diff --git a/include/sdocommands.h b/include/sdocommands.h
@@ -0,0 +1,37 @@
+/*
+ * This file is part of the stm32-... project.
+ *
+ * Copyright (C) 2025 Johannes Huebner <[email protected]>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef SDOCOMMANDS_H
+#define SDOCOMMANDS_H
+#include "cansdo.h"
+#include "canmap.h"
+
+class SdoCommands
+{
+   public:
+      static void ProcessStandardCommands(CanSdo::SdoFrame* sdoFrame);
+      static void EnableSaving() { saveEnabled = true; }
+      static void DisableSaving() { saveEnabled = false; }
+      static void SetCanMap(CanMap* m) { canMap = m; }
+
+   private:
+      static CanMap* canMap;
+      static bool saveEnabled;
+};
+
+#endif // SDOCOMMANDS_H
diff --git a/src/cansdo.cpp b/src/cansdo.cpp
@@ -16,35 +16,9 @@
  * You should have received a copy of the GNU General Public License
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-#include <libopencm3/stm32/desig.h>
-#include <libopencm3/cm3/scb.h>
 #include "cansdo.h"
-#include "param_save.h"
 #include "my_math.h"
 
-//Some functions use the "register" keyword which C++ doesn't like
-//We can safely ignore that as we don't even use those functions
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wregister"
-#include <libopencm3/cm3/cortex.h>
-#pragma GCC diagnostic pop
-
-#define SDO_REQUEST_DOWNLOAD  (1 << 5)
-#define SDO_REQUEST_UPLOAD    (2 << 5)
-#define SDO_REQUEST_SEGMENT   (3 << 5)
-#define SDO_TOGGLE_BIT        (1 << 4)
-#define SDO_RESPONSE_UPLOAD   (2 << 5)
-#define SDO_RESPONSE_DOWNLOAD (3 << 5)
-#define SDO_EXPEDITED         (1 << 1)
-#define SDO_SIZE_SPECIFIED    (1)
-#define SDO_WRITE             (SDO_REQUEST_DOWNLOAD | SDO_EXPEDITED | SDO_SIZE_SPECIFIED)
-#define SDO_READ              SDO_REQUEST_UPLOAD
-#define SDO_ABORT             0x80
-#define SDO_WRITE_REPLY       SDO_RESPONSE_DOWNLOAD
-#define SDO_READ_REPLY        (SDO_RESPONSE_UPLOAD | SDO_EXPEDITED | SDO_SIZE_SPECIFIED)
-#define SDO_ERR_INVIDX        0x06020000
-#define SDO_ERR_RANGE         0x06090030
-#define SDO_ERR_GENERAL       0x08000000
 #define SDO_REQ_ID_BASE       0x600U
 #define SDO_REP_ID_BASE       0x580U
 
@@ -53,13 +27,7 @@
 #define SDO_INDEX_MAP_TX      0x3000
 #define SDO_INDEX_MAP_RX      0x3001
 #define SDO_INDEX_MAP_RD      0x3100
-#define SDO_INDEX_SERIAL      0x5000
 #define SDO_INDEX_STRINGS     0x5001
-#define SDO_INDEX_COMMANDS    0x5002
-#define SDO_CMD_SAVE          0
-#define SDO_CMD_LOAD          1
-#define SDO_CMD_RESET         2
-#define SDO_CMD_DEFAULTS      3
 
 #define PRINT_BUF_ENQUEUE(c)  printBuffer[(printByteIn++) & (sizeof(printBuffer) - 1)] = c
 #define PRINT_BUF_DEQUEUE()   printBuffer[(printByteOut++) & (sizeof(printBuffer) - 1)]
@@ -72,7 +40,7 @@
  *
  */
 CanSdo::CanSdo(CanHardware* hw, CanMap* cm)
- : canHardware(hw), canMap(cm), nodeId(1), remoteNodeId(255), printRequest(-1), saveEnabled(true)
+ : canHardware(hw), canMap(cm), nodeId(1), remoteNodeId(255), printRequest(-1)
 {
    canHardware->AddCallback(this);
    HandleClear();
@@ -125,7 +93,7 @@ void CanSdo::SetNodeId(uint8_t id)
 void CanSdo::InitiateSDOTransfer(uint8_t req, uint8_t nodeId, uint16_t index, uint8_t subIndex, uint32_t data)
 {
    uint32_t d[2];
-   CAN_SDO *sdo = (CAN_SDO*)d;
+   SdoFrame *sdo = (SdoFrame*)d;
 
    sdo->cmd = req;
    sdo->index = index;
@@ -146,7 +114,7 @@ void CanSdo::InitiateSDOTransfer(uint8_t req, uint8_t nodeId, uint16_t index, ui
 //http://www.byteme.org.uk/canopenparent/canopen/sdo-service-data-objects-canopen/
 void CanSdo::ProcessSDO(uint32_t data[2])
 {
-   CAN_SDO *sdo = (CAN_SDO*)data;
+   SdoFrame *sdo = (SdoFrame*)data;
 
    if ((sdo->cmd & SDO_REQUEST_SEGMENT) == SDO_REQUEST_SEGMENT)
    {
@@ -214,7 +182,8 @@ void CanSdo::ProcessSDO(uint32_t data[2])
    }
    else
    {
-      ProcessSpecialSDOObjects(sdo);
+      if (!ProcessSpecialSDOObjects(sdo))
+         return; //Don't send reply when handled by user space
    }
    canHardware->Send(0x580 + nodeId, data);
 }
@@ -228,31 +197,15 @@ void CanSdo::PutChar(char c)
    printRequest = -1; //We can clear the print start trigger as we've obviously started printing
 }
 
-void CanSdo::ProcessSpecialSDOObjects(CAN_SDO* sdo)
+void CanSdo::SendSdoReply(SdoFrame* sdoFrame)
 {
-   if (sdo->index == SDO_INDEX_SERIAL && sdo->cmd == SDO_READ)
-   {
-      sdo->cmd = SDO_READ_REPLY;
-      switch (sdo->subIndex)
-      {
-      case 0:
-         sdo->data = DESIG_UNIQUE_ID0;
-         break;
-      case 1:
-         sdo->data = DESIG_UNIQUE_ID1;
-         break;
-      case 2:
-         sdo->data = DESIG_UNIQUE_ID2;
-         break;
-      case 3:
-         sdo->data = Param::GetIdSum();
-         break;
-      default:
-         sdo->cmd = SDO_ABORT;
-         sdo->data = SDO_ERR_INVIDX;
-      }
-   }
-   else if (sdo->index == SDO_INDEX_STRINGS)
+   canHardware->Send(0x580 + nodeId, (uint32_t*)sdoFrame);
+   pendingUserSpaceSdo = false;
+}
+
+bool CanSdo::ProcessSpecialSDOObjects(SdoFrame* sdo)
+{
+   if (sdo->index == SDO_INDEX_STRINGS)
    {
       if (sdo->cmd == SDO_READ)
       {
@@ -261,55 +214,21 @@ void CanSdo::ProcessSpecialSDOObjects(CAN_SDO* sdo)
          printByteIn = 0;
          printByteOut = sizeof(printBuffer); //both point to the beginning of the physical buffer but virtually they are 64 bytes apart
          printRequest = sdo->subIndex;
-      }
-   }
-   else if (sdo->index == SDO_INDEX_COMMANDS && sdo->cmd == SDO_WRITE)
-   {
-      sdo->cmd = SDO_WRITE_REPLY;
-
-      switch (sdo->subIndex)
-      {
-      case SDO_CMD_SAVE:
-         if (saveEnabled)
-         {
-            cm_disable_interrupts();
-            if (0 != canMap) canMap->Save();
-            parm_save();
-            cm_enable_interrupts();
-         }
-         else
-         {
-            sdo->cmd = SDO_ABORT;
-            sdo->data = SDO_ERR_GENERAL;
-         }
-         break;
-      case SDO_CMD_LOAD:
-         //We disable interrupts to prevent concurrent access of the CRC unit
-         cm_disable_interrupts();
-         parm_load();
-         cm_enable_interrupts();
-         Param::Change(Param::PARAM_LAST);
-         break;
-      case SDO_CMD_RESET:
-         scb_reset_system();
-         break;
-      case SDO_CMD_DEFAULTS:
-         Param::LoadDefaults();
-         Param::Change(Param::PARAM_LAST);
-         break;
-      default:
-         sdo->cmd = SDO_ABORT;
-         sdo->data = SDO_ERR_INVIDX;
+         return true;
       }
    }
    else
    {
-      sdo->cmd = SDO_ABORT;
-      sdo->data = SDO_ERR_INVIDX;
+      pendingUserSpaceSdo = true;
+      pendingUserSpaceSdoFrame.cmd = sdo->cmd;
+      pendingUserSpaceSdoFrame.index = sdo->index;
+      pendingUserSpaceSdoFrame.subIndex = sdo->subIndex;
+      pendingUserSpaceSdoFrame.data = sdo->data;
    }
+   return false;
 }
 
-void CanSdo::ReadOrDeleteCanMap(CAN_SDO* sdo)
+void CanSdo::ReadOrDeleteCanMap(SdoFrame* sdo)
 {
    bool rx = (sdo->index & 0x80) != 0;
    uint32_t canId;
@@ -347,7 +266,7 @@ void CanSdo::ReadOrDeleteCanMap(CAN_SDO* sdo)
    }
 }
 
-void CanSdo::AddCanMap(CAN_SDO* sdo, bool rx)
+void CanSdo::AddCanMap(SdoFrame* sdo, bool rx)
 {
    if (sdo->cmd == SDO_WRITE)
    {