From 465f138f471cafac7a023fcf3747893ff54abb00 Mon Sep 17 00:00:00 2001
From: Andreas Fritiofson <andreas.fritiofson@unjo.com>
Date: Thu, 21 Dec 2023 16:44:41 +0100
Subject: [PATCH] Add RPDO deadline monitoring

As described in CiA 301, Object 1400h to 15FFh: RPDO communication
parameter, Subindex 05h.

The indication to the application is done via the EMCY callback. The
EMCY transmission is not described, but since there is an EMCY code
defined for RPDO timeout, it's assumed to be meant for this purpose.

Change-Id: Ie98f67fb20b615d56fdbdfc207b38bfeef6d4fb6
---
 src/co_emcy.c |  2 +-
 src/co_main.h |  3 +++
 src/co_pdo.c  | 41 +++++++++++++++++++++++++++++++++++++++++
 3 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/src/co_emcy.c b/src/co_emcy.c
index 238a762..ebce3ec 100644
--- a/src/co_emcy.c
+++ b/src/co_emcy.c
@@ -384,7 +384,7 @@ void co_emcy_handle_can_state (co_net_t * net)
    if (
       !net->emcy.state.overrun && !net->emcy.state.error_passive &&
       !net->emcy.state.bus_off && !net->emcy.node_guard_error &&
-      !net->emcy.heartbeat_error)
+      !net->emcy.heartbeat_error && !net->emcy.rpdo_timeout)
    {
       co_emcy_error_register_clear (net, CO_ERR_COMMUNICATION);
    }
diff --git a/src/co_main.h b/src/co_main.h
index c1d29d4..e5b0ed7 100644
--- a/src/co_main.h
+++ b/src/co_main.h
@@ -82,6 +82,8 @@ typedef struct co_pdo
    {
       bool queued : 1;
       bool sync_wait : 1;
+      bool rpdo_monitoring : 1;
+      bool rpdo_timeout : 1;
    };
    uint32_t mappings[MAX_PDO_ENTRIES];
    const co_obj_t * objs[MAX_PDO_ENTRIES];
@@ -220,6 +222,7 @@ typedef struct co_emcy
    os_channel_state_t state;         /**< CAN state */
    bool node_guard_error;            /**< Node guard error */
    bool heartbeat_error;             /**< Heartbeat error */
+   bool rpdo_timeout;                /**< RPDO timeout */
    uint32_t cobids[MAX_EMCY_COBIDS]; /**< EMCY consumer object */
 } co_emcy_t;
 
diff --git a/src/co_pdo.c b/src/co_pdo.c
index 7a74238..cae0ae8 100644
--- a/src/co_pdo.c
+++ b/src/co_pdo.c
@@ -142,6 +142,8 @@ static uint32_t co_pdo_mapping_validate (co_pdo_t * pdo, uint8_t number_of_mappi
    if (IS_CYCLIC (pdo->sync_start))
       pdo->sync_wait = true;
 
+   pdo->rpdo_monitoring = false;
+
    return 0;
 }
 
@@ -555,6 +557,7 @@ static void co_pdo_transmit (co_net_t * net, co_pdo_t * pdo)
 int co_pdo_timer (co_net_t * net, os_tick_t now)
 {
    unsigned int ix;
+   bool rpdo_timeout = false;
 
    if (net->state != STATE_OP)
       return -1;
@@ -577,6 +580,37 @@ int co_pdo_timer (co_net_t * net, os_tick_t now)
       }
    }
 
+   /* Check for RPDOs with event timer (deadline monitoring) */
+   for (ix = 0; ix < MAX_RX_PDO; ix++)
+   {
+      co_pdo_t * pdo = &net->pdo_rx[ix];
+
+      if (pdo->cobid & CO_COBID_INVALID)
+         continue;
+
+      if (pdo->rpdo_timeout)
+      {
+         /* Already signaled, just track the combined state. */
+         rpdo_timeout = true;
+         continue;
+      }
+
+      if (!pdo->rpdo_monitoring || pdo->event_timer == 0)
+         continue;
+
+      if (co_is_expired (now, pdo->timestamp, 1000 * pdo->event_timer))
+      {
+         /* Deadline timeout elapsed, transmit EMCY */
+         pdo->rpdo_monitoring = false;
+         pdo->rpdo_timeout = rpdo_timeout = true;
+         co_emcy_error_register_set (net, CO_ERR_COMMUNICATION);
+         co_emcy_tx (net, 0x8250, 0, NULL);
+      }
+   }
+
+   /* Update RPDO timeout state */
+   net->emcy.rpdo_timeout = rpdo_timeout;
+
    return 0;
 }
 
@@ -799,6 +833,13 @@ void co_pdo_rx (co_net_t * net, uint32_t id, void * msg, size_t dlc)
                memcpy (&pdo->frame, msg, dlc);
                pdo->timestamp = os_tick_current();
 
+               if (pdo->event_timer > 0)
+               {
+                  /* Arm RPDO deadline monitoring */
+                  pdo->rpdo_monitoring = true;
+                  pdo->rpdo_timeout = false;
+               }
+
                if (IS_EVENT (pdo->transmission_type))
                {
                   /* Deliver event-driven RPDOs asynchronously */