From 5c638395ac616927eeefe7716cbab0853212144c Mon Sep 17 00:00:00 2001
From: Christian Gabriel <ch_gabriel@web.de>
Date: Sun, 6 Oct 2024 09:52:27 +0200
Subject: [PATCH] modules: canopennode: add rxmsg callback

Implement callback for incoming CAN messages for any of the
configured filters for CANopenNode.

This can be used to wake the loop calling CO_process when a
new message was received which needs processing.

Signed-off-by: Christian Gabriel <ch_gabriel@web.de>
---
 modules/canopennode/CO_driver.c   | 11 +++++++++++
 modules/canopennode/canopennode.h | 25 +++++++++++++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/modules/canopennode/CO_driver.c b/modules/canopennode/CO_driver.c
index 4d161a49400..db441d94c83 100644
--- a/modules/canopennode/CO_driver.c
+++ b/modules/canopennode/CO_driver.c
@@ -31,6 +31,8 @@ K_MUTEX_DEFINE(canopen_send_mutex);
 K_MUTEX_DEFINE(canopen_emcy_mutex);
 K_MUTEX_DEFINE(canopen_co_mutex);
 
+static canopen_rxmsg_callback_t rxmsg_callback;
+
 inline void canopen_send_lock(void)
 {
 	k_mutex_lock(&canopen_send_mutex, K_FOREVER);
@@ -61,6 +63,11 @@ inline void canopen_od_unlock(void)
 	k_mutex_unlock(&canopen_co_mutex);
 }
 
+void canopen_set_rxmsg_callback(canopen_rxmsg_callback_t callback)
+{
+	rxmsg_callback = callback;
+}
+
 static void canopen_detach_all_rx_filters(CO_CANmodule_t *CANmodule)
 {
 	uint16_t i;
@@ -83,6 +90,7 @@ static void canopen_rx_callback(const struct device *dev, struct can_frame *fram
 	CO_CANmodule_t *CANmodule = (CO_CANmodule_t *)user_data;
 	CO_CANrxMsg_t rxMsg;
 	CO_CANrx_t *buffer;
+	canopen_rxmsg_callback_t callback = rxmsg_callback;
 	int i;
 
 	ARG_UNUSED(dev);
@@ -105,6 +113,9 @@ static void canopen_rx_callback(const struct device *dev, struct can_frame *fram
 			rxMsg.DLC = frame->dlc;
 			memcpy(rxMsg.data, frame->data, frame->dlc);
 			buffer->pFunct(buffer->object, &rxMsg);
+			if (callback != NULL) {
+				callback();
+			}
 			break;
 		}
 	}
diff --git a/modules/canopennode/canopennode.h b/modules/canopennode/canopennode.h
index dfa877abd25..da1cb1b9ef2 100644
--- a/modules/canopennode/canopennode.h
+++ b/modules/canopennode/canopennode.h
@@ -144,6 +144,31 @@ void canopen_leds_init(CO_NMT_t *nmt,
  */
 void canopen_leds_program_download(bool in_progress);
 
+/**
+ * @brief Callback for incoming CAN message
+ *
+ * This callback will be called from interrupt context and should therefore
+ * return quickly.
+ *
+ * It can be used to e.g. wake the loop polling calling CO_process.
+ */
+typedef void (*canopen_rxmsg_callback_t)(void);
+
+/**
+ * @brief Set callback for incoming CAN message
+ *
+ * Set up callback to be called on incoming CAN message on any of
+ * the configured filters for CANopenNode.
+ *
+ * This can be used to wake the loop calling CO_process when an incoming
+ * message needs to be processed.
+ *
+ * Setting a new callback will overwrite any existing callback.
+ *
+ * @param callback the callback to set
+ */
+void canopen_set_rxmsg_callback(canopen_rxmsg_callback_t callback);
+
 #ifdef __cplusplus
 }
 #endif