Allow EMCY callback to enable the error behavior

Change-Id: I4a78ba7b4a521f64fb271d528df6c56953b04bad

Author: nattgris

include/co_api.h

@@ -26,6 +26,7 @@ extern "C" {
 #endif
 
 #include <stdint.h>
+#include <stdbool.h>
 #include <stddef.h>
 
 #include "co_export.h"
@@ -303,8 +304,8 @@ typedef struct co_cfg
    /** SYNC callback */
    void (*cb_sync) (void * arg);
 
-   /** EMCY callback */
-   void (*cb_emcy) (
+   /** EMCY callback, return true to enable error behavior */
+   bool (*cb_emcy) (
       void * arg,
       uint8_t node,
       uint16_t code,
@@ -480,7 +481,7 @@ CO_EXPORT int co_sdo_write (
  * Calling this function adds an error to the error history object
  * (1003h). It also signals an error to the NMT state-machine which
  * may change state according to the setting of the error behavior
- * object (1029h).
+ * object (1029h) and the return value of the EMCY callback.
  *
  * The application will be notified via the EMCY callback. The node id
  * will be the active node id for this node.

src/co_emcy.c

@@ -60,6 +60,24 @@ static uint32_t co_emcy_error_set (co_net_t * net, uint8_t subindex, uint32_t *
    return 0;
 }
 
+static void co_trigger_error_behavior (co_net_t * net)
+{
+   /* Transition state according to error behavior setting */
+   switch (net->error_behavior)
+   {
+   case 0:
+      if (net->state == STATE_OP)
+         co_nmt_event (net, EVENT_PREOP);
+      break;
+   case 2:
+      co_nmt_event (net, EVENT_STOP);
+      break;
+   default:
+      /* Do nothing */
+      break;
+   }
+}
+
 uint32_t co_od1001_fn (
    co_net_t * net,
    od_event_t event,
@@ -214,6 +232,7 @@ int co_emcy_tx (co_net_t * net, uint16_t code, uint16_t info, uint8_t msef[5])
    uint8_t * p    = msg;
    uint8_t reg;
    uint32_t now;
+   bool error_behavior = false;
 
    if (net->number_of_errors < MAX_ERRORS)
       net->number_of_errors++;
@@ -254,26 +273,13 @@ int co_emcy_tx (co_net_t * net, uint16_t code, uint16_t info, uint8_t msef[5])
    /* Call user callback */
    if (net->cb_emcy)
    {
-      net->cb_emcy (net->cb_arg, net->node, code, reg, msef);
+      error_behavior = net->cb_emcy (net->cb_arg, net->node, code, reg, msef);
    }
 
-   if (code != 0x8140 && code != 0x8130) {
-      return 0;
-   }
-
-   /* Transition state according to error behavior setting */
-   switch (net->error_behavior)
-   {
-   case 0:
-      if (net->state == STATE_OP)
-         co_nmt_event (net, EVENT_PREOP);
-      break;
-   case 2:
-      co_nmt_event (net, EVENT_STOP);
-      break;
-   default:
-      /* Do nothing */
-      break;
+   /* Always trigger error behavior on the mandatory events,
+    * otherwise, follow the callback return value. */
+   if (code == 0x8130 || code == 0x8140 || error_behavior) {
+      co_trigger_error_behavior (net);
    }
 
    return 0;

src/co_main.h

@@ -267,7 +267,7 @@ struct co_net
    void (*cb_sync) (void * arg);
 
    /** EMCY callback */
-   void (*cb_emcy) (
+   bool (*cb_emcy) (
       void * arg,
       uint8_t node,
       uint16_t code,

test/mocks.cpp

@@ -149,14 +149,16 @@ uint8_t cb_emcy_node;
 uint16_t cb_emcy_code;
 uint8_t cb_emcy_reg;
 uint8_t cb_emcy_msef[5];
-void cb_emcy (void * arg, uint8_t node, uint16_t code, uint8_t reg, uint8_t msef[5])
+bool cb_emcy_result;
+bool cb_emcy (void * arg, uint8_t node, uint16_t code, uint8_t reg, uint8_t msef[5])
 {
    cb_emcy_calls++;
    cb_emcy_node = node;
    cb_emcy_code = code;
    cb_emcy_reg  = reg;
    if (msef != NULL)
       memcpy (cb_emcy_msef, msef, sizeof (cb_emcy_msef));
+   return cb_emcy_result;
 }
 
 unsigned int cb_sync_calls;

test/mocks.h

@@ -96,7 +96,8 @@ extern uint8_t cb_emcy_node;
 extern uint16_t cb_emcy_code;
 extern uint8_t cb_emcy_reg;
 extern uint8_t cb_emcy_msef[5];
-void cb_emcy (void * arg, uint8_t node, uint16_t code, uint8_t reg, uint8_t msef[5]);
+extern bool cb_emcy_result;
+bool cb_emcy (void * arg, uint8_t node, uint16_t code, uint8_t reg, uint8_t msef[5]);
 
 extern unsigned int cb_sync_calls;
 void cb_sync (void * arg);

test/test_emcy.cpp

@@ -26,6 +26,7 @@ class EmcyTest : public TestBase
    {
       TestBase::SetUp();
       net.emcy.cobid = 0x81;
+      cb_emcy_result = false;
    }
 
    uint8_t msef[5] = {5, 4, 3, 2, 1};
@@ -283,8 +284,15 @@ TEST_F (EmcyTest, NMTErrorBehavior)
    EXPECT_EQ (STATE_STOP, net.state);
 
    net.state = STATE_OP;
+   cb_emcy_result = false;
    co_emcy_tx (&net, 1, 0x1234, msef);
    EXPECT_EQ (STATE_OP, net.state);
+
+   net.state = STATE_OP;
+   cb_emcy_result = true;
+   co_emcy_tx (&net, 1, 0x1234, msef);
+   EXPECT_EQ (STATE_STOP, net.state);
+
 }
 
 TEST_F (EmcyTest, EmcyOverrun)