canbus: canopen: add zephyr driver layer for CANopenNode

Add a Zephyr driver and abstraction layer for use by the 3rd party
CANopenNode module.

CANopenNode depends on the CO_driver.h file for platform-specific type
definitions, locking primitives, and CAN bus driver API.

This fixes #15278.

Signed-off-by: Henrik Brix Andersen <hebad@vestas.com>

CODEOWNERS

@@ -355,6 +355,7 @@
 /subsys/bluetooth/                        @joerchan @jhedberg @Vudentz
 /subsys/bluetooth/controller/             @carlescufi @cvinayak @thoh-ot
 /subsys/bluetooth/mesh/                   @jhedberg @trond-snekvik @joerchan @Vudentz
+/subsys/canbus/                           @alexanderwachter
 /subsys/cpp/                              @pabigot @vanwinkeljan
 /subsys/debug/                            @nashif
 /subsys/debug/asan_hacks.c                @vanwinkeljan @aescolar

subsys/CMakeLists.txt

@@ -20,3 +20,4 @@ add_subdirectory(power)
 add_subdirectory(stats)
 add_subdirectory(testsuite)
 add_subdirectory_if_kconfig(jwt)
+add_subdirectory(canbus)

subsys/Kconfig

@@ -38,3 +38,5 @@ source "subsys/testsuite/Kconfig"
 source "subsys/fb/Kconfig"
 
 source "subsys/jwt/Kconfig"
+
+source "subsys/canbus/Kconfig"

subsys/canbus/CMakeLists.txt

@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: Apache-2.0
+
+add_subdirectory_if_kconfig(canopen)

subsys/canbus/Kconfig

@@ -0,0 +1,10 @@
+# CAN bus subsystem configuration options
+
+# Copyright (c) 2019 Vestas Wind Systems A/S
+# SPDX-License-Identifier: Apache-2.0
+
+menu "Controller Area Network (CAN) bus subsystem"
+
+source "subsys/canbus/canopen/Kconfig"
+
+endmenu

subsys/canbus/canopen/CMakeLists.txt

@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: Apache-2.0
+
+zephyr_library()
+zephyr_library_sources_ifdef(CONFIG_CANOPEN CO_driver.c)
+zephyr_library_sources_ifdef(CONFIG_CANOPEN_SYNC_THREAD canopen_sync.c)
+zephyr_include_directories(.)

subsys/canbus/canopen/CO_driver.c

@@ -0,0 +1,470 @@
+/*
+ * Copyright (c) 2019 Vestas Wind Systems A/S
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr.h>
+#include <drivers/can.h>
+#include <init.h>
+#include <sys/util.h>
+
+#include <CO_driver.h>
+#include <CO_Emergency.h>
+#include <CO_SDO.h>
+
+#define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL
+#include <logging/log.h>
+LOG_MODULE_REGISTER(canopen_driver);
+
+K_THREAD_STACK_DEFINE(canopen_tx_workq_stack,
+		      CONFIG_CANOPEN_TX_WORKQUEUE_STACK_SIZE);
+
+struct k_work_q canopen_tx_workq;
+
+struct canopen_tx_work_container {
+	struct k_work work;
+	CO_CANmodule_t *CANmodule;
+};
+
+struct canopen_tx_work_container canopen_tx_queue;
+
+K_MUTEX_DEFINE(canopen_send_mutex);
+K_MUTEX_DEFINE(canopen_emcy_mutex);
+K_MUTEX_DEFINE(canopen_co_mutex);
+
+inline void canopen_send_lock(void)
+{
+	k_mutex_lock(&canopen_send_mutex, K_FOREVER);
+}
+
+inline void canopen_send_unlock(void)
+{
+	k_mutex_unlock(&canopen_send_mutex);
+}
+
+inline void canopen_emcy_lock(void)
+{
+	k_mutex_lock(&canopen_emcy_mutex, K_FOREVER);
+}
+
+inline void canopen_emcy_unlock(void)
+{
+	k_mutex_unlock(&canopen_emcy_mutex);
+}
+
+inline void canopen_od_lock(void)
+{
+	k_mutex_lock(&canopen_co_mutex, K_FOREVER);
+}
+
+inline void canopen_od_unlock(void)
+{
+	k_mutex_unlock(&canopen_co_mutex);
+}
+
+static void canopen_detach_all_rx_filters(CO_CANmodule_t *CANmodule)
+{
+	u16_t i;
+
+	if (!CANmodule || !CANmodule->rx_array || !CANmodule->configured) {
+		return;
+	}
+
+	for (i = 0U; i < CANmodule->rx_size; i++) {
+		if (CANmodule->rx_array[i].filter_id != CAN_NO_FREE_FILTER) {
+			can_detach(CANmodule->dev,
+				   CANmodule->rx_array[i].filter_id);
+			CANmodule->rx_array[i].filter_id = CAN_NO_FREE_FILTER;
+		}
+	}
+}
+
+static void canopen_rx_isr_callback(struct zcan_frame *msg, void *arg)
+{
+	CO_CANrx_t *buffer = (CO_CANrx_t *)arg;
+	CO_CANrxMsg_t rxMsg;
+
+	if (!buffer || !buffer->pFunct) {
+		LOG_ERR("failed to process CAN rx isr callback");
+		return;
+	}
+
+	rxMsg.ident = msg->std_id;
+	rxMsg.DLC = msg->dlc;
+	memcpy(rxMsg.data, msg->data, msg->dlc);
+	buffer->pFunct(buffer->object, &rxMsg);
+}
+
+static void canopen_tx_isr_callback(u32_t error_flags, void *arg)
+{
+	CO_CANmodule_t *CANmodule = arg;
+
+	if (!CANmodule) {
+		LOG_ERR("failed to process CAN tx isr callback");
+		return;
+	}
+
+	if (error_flags == CAN_TX_OK) {
+		CANmodule->first_tx_msg = false;
+	}
+
+	k_work_submit_to_queue(&canopen_tx_workq, &canopen_tx_queue.work);
+}
+
+static void canopen_tx_retry(struct k_work *item)
+{
+	struct canopen_tx_work_container *container =
+		CONTAINER_OF(item, struct canopen_tx_work_container, work);
+	CO_CANmodule_t *CANmodule = container->CANmodule;
+	struct zcan_frame msg;
+	CO_CANtx_t *buffer;
+	int err;
+	u16_t i;
+
+	CO_LOCK_CAN_SEND();
+
+	for (i = 0; i < CANmodule->tx_size; i++) {
+		buffer = &CANmodule->tx_array[i];
+		if (buffer->bufferFull) {
+			msg.id_type = CAN_STANDARD_IDENTIFIER;
+			msg.std_id = buffer->ident;
+			msg.dlc = buffer->DLC;
+			msg.rtr = (buffer->rtr ? 1 : 0);
+			memcpy(msg.data, buffer->data, buffer->DLC);
+
+			err = can_send(CANmodule->dev, &msg, K_NO_WAIT,
+				       canopen_tx_isr_callback, CANmodule);
+			if (err == CAN_TIMEOUT) {
+				break;
+			} else if (err != CAN_TX_OK) {
+				LOG_ERR("failed to send CAN frame (err %d)",
+					err);
+				CO_errorReport(CANmodule->em,
+					       CO_EM_GENERIC_SOFTWARE_ERROR,
+					       CO_EMC_COMMUNICATION, 0);
+
+			}
+
+			buffer->bufferFull = false;
+		}
+	}
+
+	CO_UNLOCK_CAN_SEND();
+}
+
+void CO_CANsetConfigurationMode(void *CANdriverState)
+{
+	/* No operation */
+}
+
+void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule)
+{
+	CANmodule->CANnormal = true;
+}
+
+CO_ReturnError_t CO_CANmodule_init(CO_CANmodule_t *CANmodule,
+				   void *CANdriverState,
+				   CO_CANrx_t rxArray[], u16_t rxSize,
+				   CO_CANtx_t txArray[], u16_t txSize,
+				   u16_t CANbitRate)
+{
+	u16_t i;
+	int err;
+
+	LOG_DBG("rxSize = %d, txSize = %d", rxSize, txSize);
+
+	if (!CANmodule || !rxArray || !txArray || !CANdriverState) {
+		LOG_ERR("failed to initialize CAN module");
+		return CO_ERROR_ILLEGAL_ARGUMENT;
+	}
+
+	if (rxSize > CONFIG_CAN_MAX_FILTER) {
+		LOG_ERR("insufficient number of concurrent CAN RX filters"
+			" (needs %d, %d available)", rxSize,
+			CONFIG_CAN_MAX_FILTER);
+		return CO_ERROR_OUT_OF_MEMORY;
+	} else if (rxSize < CONFIG_CAN_MAX_FILTER) {
+		LOG_DBG("excessive number of concurrent CAN RX filters enabled"
+			" (needs %d, %d available)", rxSize,
+			CONFIG_CAN_MAX_FILTER);
+	}
+
+	canopen_detach_all_rx_filters(CANmodule);
+	canopen_tx_queue.CANmodule = CANmodule;
+
+	CANmodule->dev = CANdriverState;
+	CANmodule->rx_array = rxArray;
+	CANmodule->rx_size = rxSize;
+	CANmodule->tx_array = txArray;
+	CANmodule->tx_size = txSize;
+	CANmodule->CANnormal = false;
+	CANmodule->first_tx_msg = true;
+	CANmodule->errors = 0;
+	CANmodule->em = NULL;
+
+	for (i = 0U; i < rxSize; i++) {
+		rxArray[i].ident = 0U;
+		rxArray[i].pFunct = NULL;
+		rxArray[i].filter_id = CAN_NO_FREE_FILTER;
+	}
+
+	for (i = 0U; i < txSize; i++) {
+		txArray[i].bufferFull = false;
+	}
+
+	err = can_configure(CANmodule->dev, CAN_NORMAL_MODE, KHZ(CANbitRate));
+	if (err) {
+		LOG_ERR("failed to configure CAN interface (err %d)", err);
+		return CO_ERROR_ILLEGAL_ARGUMENT;
+	}
+
+	CANmodule->configured = true;
+
+	return CO_ERROR_NO;
+}
+
+void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
+{
+	int err;
+
+	if (!CANmodule || !CANmodule->dev) {
+		return;
+	}
+
+	canopen_detach_all_rx_filters(CANmodule);
+
+	err = can_configure(CANmodule->dev, CAN_SILENT_MODE, 0);
+	if (err) {
+		LOG_ERR("failed to disable CAN interface (err %d)", err);
+	}
+}
+
+u16_t CO_CANrxMsg_readIdent(const CO_CANrxMsg_t *rxMsg)
+{
+	return rxMsg->ident;
+}
+
+CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, u16_t index,
+				u16_t ident, u16_t mask, bool_t rtr,
+				void *object,
+				CO_CANrxBufferCallback_t pFunct)
+{
+	struct zcan_filter filter;
+	CO_CANrx_t *buffer;
+
+	if (!CANmodule || !pFunct || (index >= CANmodule->rx_size)) {
+		LOG_ERR("failed to initialize CAN rx buffer, illegal argument");
+		CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
+			       CO_EMC_SOFTWARE_INTERNAL, 0);
+		return CO_ERROR_ILLEGAL_ARGUMENT;
+	}
+
+	buffer = &CANmodule->rx_array[index];
+	buffer->object = object;
+	buffer->pFunct = pFunct;
+
+	filter.id_type = CAN_STANDARD_IDENTIFIER;
+	filter.std_id = ident;
+	filter.std_id_mask = mask;
+	filter.rtr = (rtr ? 1 : 0);
+	filter.rtr_mask = 1;
+
+	if (buffer->filter_id != CAN_NO_FREE_FILTER) {
+		can_detach(CANmodule->dev, buffer->filter_id);
+	}
+
+	buffer->filter_id = can_attach_isr(CANmodule->dev,
+					   canopen_rx_isr_callback,
+					   buffer, &filter);
+	if (buffer->filter_id == CAN_NO_FREE_FILTER) {
+		LOG_ERR("failed to attach CAN rx isr, no free filter");
+		CO_errorReport(CANmodule->em, CO_EM_MEMORY_ALLOCATION_ERROR,
+			       CO_EMC_SOFTWARE_INTERNAL, 0);
+		return CO_ERROR_OUT_OF_MEMORY;
+	}
+
+	return CO_ERROR_NO;
+}
+
+CO_CANtx_t *CO_CANtxBufferInit(CO_CANmodule_t *CANmodule, u16_t index,
+			       u16_t ident, bool_t rtr, u8_t noOfBytes,
+			       bool_t syncFlag)
+{
+	CO_CANtx_t *buffer;
+
+	if (!CANmodule || (index >= CANmodule->tx_size)) {
+		LOG_ERR("failed to initialize CAN rx buffer, illegal argument");
+		CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
+			       CO_EMC_SOFTWARE_INTERNAL, 0);
+		return NULL;
+	}
+
+	buffer = &CANmodule->tx_array[index];
+	buffer->ident = ident;
+	buffer->rtr = rtr;
+	buffer->DLC = noOfBytes;
+	buffer->bufferFull = false;
+	buffer->syncFlag = syncFlag;
+
+	return buffer;
+}
+
+CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
+{
+	CO_ReturnError_t ret = CO_ERROR_NO;
+	struct zcan_frame msg;
+	int err;
+
+	if (!CANmodule || !CANmodule->dev || !buffer) {
+		return CO_ERROR_ILLEGAL_ARGUMENT;
+	}
+
+	CO_LOCK_CAN_SEND();
+
+	if (buffer->bufferFull) {
+		if (!CANmodule->first_tx_msg) {
+			CO_errorReport(CANmodule->em, CO_EM_CAN_TX_OVERFLOW,
+				       CO_EMC_CAN_OVERRUN, buffer->ident);
+		}
+		buffer->bufferFull = false;
+		ret = CO_ERROR_TX_OVERFLOW;
+	}
+
+	msg.id_type = CAN_STANDARD_IDENTIFIER;
+	msg.std_id = buffer->ident;
+	msg.dlc = buffer->DLC;
+	msg.rtr = (buffer->rtr ? 1 : 0);
+	memcpy(msg.data, buffer->data, buffer->DLC);
+
+	err = can_send(CANmodule->dev, &msg, K_NO_WAIT, canopen_tx_isr_callback,
+		       CANmodule);
+	if (err == CAN_TIMEOUT) {
+		buffer->bufferFull = true;
+	} else if (err != CAN_TX_OK) {
+		LOG_ERR("failed to send CAN frame (err %d)", err);
+		CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
+			       CO_EMC_COMMUNICATION, 0);
+		ret = CO_ERROR_TX_UNCONFIGURED;
+	}
+
+	CO_UNLOCK_CAN_SEND();
+
+	return ret;
+}
+
+void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule)
+{
+	bool_t tpdoDeleted = false;
+	CO_CANtx_t *buffer;
+	u16_t i;
+
+	if (!CANmodule) {
+		return;
+	}
+
+	CO_LOCK_CAN_SEND();
+
+	for (i = 0; i < CANmodule->tx_size; i++) {
+		buffer = &CANmodule->tx_array[i];
+		if (buffer->bufferFull && buffer->syncFlag) {
+			buffer->bufferFull = false;
+			tpdoDeleted = true;
+		}
+	}
+
+	CO_UNLOCK_CAN_SEND();
+
+	if (tpdoDeleted) {
+		CO_errorReport(CANmodule->em, CO_EM_TPDO_OUTSIDE_WINDOW,
+			       CO_EMC_COMMUNICATION, 0);
+	}
+}
+
+void CO_CANverifyErrors(CO_CANmodule_t *CANmodule)
+{
+	CO_EM_t *em = (CO_EM_t *)CANmodule->em;
+	struct can_bus_err_cnt err_cnt;
+	enum can_state state;
+	u8_t rx_overflows;
+	u32_t errors;
+
+	/*
+	 * TODO: Zephyr lacks an API for reading the rx mailbox
+	 * overflow counter.
+	 */
+	rx_overflows  = 0;
+
+	state = can_get_state(CANmodule->dev, &err_cnt);
+
+	errors = ((u32_t)err_cnt.tx_err_cnt << 16) |
+		 ((u32_t)err_cnt.rx_err_cnt << 8) |
+		 rx_overflows;
+
+	if (errors != CANmodule->errors) {
+		CANmodule->errors = errors;
+
+		if (state == CAN_BUS_OFF) {
+			/* Bus off */
+			CO_errorReport(em, CO_EM_CAN_TX_BUS_OFF,
+				       CO_EMC_BUS_OFF_RECOVERED, errors);
+		} else {
+			/* Bus not off */
+			CO_errorReset(em, CO_EM_CAN_TX_BUS_OFF, errors);
+
+			if ((err_cnt.rx_err_cnt >= 96U) ||
+			    (err_cnt.tx_err_cnt >= 96U)) {
+				/* Bus warning */
+				CO_errorReport(em, CO_EM_CAN_BUS_WARNING,
+					       CO_EMC_NO_ERROR, errors);
+			} else {
+				/* Bus not warning */
+				CO_errorReset(em, CO_EM_CAN_BUS_WARNING,
+					      errors);
+			}
+
+			if (err_cnt.rx_err_cnt >= 128U) {
+				/* Bus rx passive */
+				CO_errorReport(em, CO_EM_CAN_RX_BUS_PASSIVE,
+					       CO_EMC_CAN_PASSIVE, errors);
+			} else {
+				/* Bus not rx passive */
+				CO_errorReset(em, CO_EM_CAN_RX_BUS_PASSIVE,
+					      errors);
+			}
+
+			if (err_cnt.tx_err_cnt >= 128U &&
+			    !CANmodule->first_tx_msg) {
+				/* Bus tx passive */
+				CO_errorReport(em, CO_EM_CAN_TX_BUS_PASSIVE,
+					       CO_EMC_CAN_PASSIVE, errors);
+			} else if (CO_isError(em, CO_EM_CAN_TX_BUS_PASSIVE)) {
+				/* Bus not tx passive */
+				CO_errorReset(em, CO_EM_CAN_TX_BUS_PASSIVE,
+					      errors);
+				CO_errorReset(em, CO_EM_CAN_TX_OVERFLOW,
+					      errors);
+			}
+		}
+
+		if (rx_overflows != 0U) {
+			CO_errorReport(em, CO_EM_CAN_RXB_OVERFLOW,
+				       CO_EMC_CAN_OVERRUN, errors);
+		}
+	}
+}
+
+static int canopen_init(struct device *dev)
+{
+	ARG_UNUSED(dev);
+
+	k_work_q_start(&canopen_tx_workq, canopen_tx_workq_stack,
+		       K_THREAD_STACK_SIZEOF(canopen_tx_workq_stack),
+		       CONFIG_CANOPEN_TX_WORKQUEUE_PRIORITY);
+
+	k_work_init(&canopen_tx_queue.work, canopen_tx_retry);
+
+	return 0;
+}
+
+SYS_INIT(canopen_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);

subsys/canbus/canopen/CO_driver.h

@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2019 Vestas Wind Systems A/S
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_SUBSYS_CANBUS_CANOPEN_CO_DRIVER_H
+#define ZEPHYR_SUBSYS_CANBUS_CANOPEN_CO_DRIVER_H
+
+/*
+ * Zephyr RTOS CAN driver interface and configuration for CANopenNode
+ * CANopen protocol stack.
+ *
+ * See CANopenNode/stack/drvTemplate/CO_driver.h for API description.
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <zephyr.h>
+#include <zephyr/types.h>
+#include <device.h>
+#include <toolchain.h>
+
+/* Use static variables instead of calloc() */
+#define CO_USE_GLOBALS
+
+/* Use Zephyr provided crc16 implementation */
+#define CO_USE_OWN_CRC16
+
+/* Use SDO buffer size from Kconfig */
+#define CO_SDO_BUFFER_SIZE CONFIG_CANOPEN_SDO_BUFFER_SIZE
+
+/* Use trace buffer size from Kconfig */
+#define CO_TRACE_BUFFER_SIZE_FIXED CONFIG_CANOPEN_TRACE_BUFFER_SIZE
+
+#ifdef CONFIG_CANOPEN_LEDS
+#define CO_USE_LEDS 1
+#endif
+
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+#define CO_LITTLE_ENDIAN
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+#define CO_BIG_ENDIAN
+#else
+#error "Unsupported endianness"
+#endif
+
+typedef bool          bool_t;
+typedef float         float32_t;
+typedef long double   float64_t;
+typedef char          char_t;
+typedef unsigned char oChar_t;
+typedef unsigned char domain_t;
+
+typedef struct canopen_rx_msg {
+	u8_t data[8];
+	u16_t ident;
+	u8_t DLC;
+} CO_CANrxMsg_t;
+
+typedef void (*CO_CANrxBufferCallback_t)(void *object,
+					 const CO_CANrxMsg_t *message);
+
+typedef struct canopen_rx {
+	int filter_id;
+	void *object;
+	CO_CANrxBufferCallback_t pFunct;
+	u16_t ident;
+} CO_CANrx_t;
+
+typedef struct canopen_tx {
+	u8_t data[8];
+	u16_t ident;
+	u8_t DLC;
+	bool_t rtr : 1;
+	bool_t bufferFull : 1;
+	bool_t syncFlag : 1;
+} CO_CANtx_t;
+
+typedef struct canopen_module {
+	struct device *dev;
+	CO_CANrx_t *rx_array;
+	CO_CANtx_t *tx_array;
+	u16_t rx_size;
+	u16_t tx_size;
+	u32_t errors;
+	void *em;
+	bool_t configured : 1;
+	bool_t CANnormal : 1;
+	bool_t first_tx_msg : 1;
+} CO_CANmodule_t;
+
+void canopen_send_lock(void);
+void canopen_send_unlock(void);
+#define CO_LOCK_CAN_SEND()   canopen_send_lock()
+#define CO_UNLOCK_CAN_SEND() canopen_send_unlock()
+
+void canopen_emcy_lock(void);
+void canopen_emcy_unlock(void);
+#define CO_LOCK_EMCY()   canopen_emcy_lock()
+#define CO_UNLOCK_EMCY() canopen_emcy_unlock()
+
+void canopen_od_lock(void);
+void canopen_od_unlock(void);
+#define CO_LOCK_OD()   canopen_od_lock()
+#define CO_UNLOCK_OD() canopen_od_unlock()
+
+/*
+ * CANopenNode RX callbacks run in interrupt context, no memory
+ * barrier needed.
+ */
+#define CANrxMemoryBarrier()
+#define IS_CANrxNew(rxNew) ((uintptr_t)rxNew)
+#define SET_CANrxNew(rxNew) { CANrxMemoryBarrier(); rxNew = (void *)1L; }
+#define CLEAR_CANrxNew(rxNew) { CANrxMemoryBarrier(); rxNew = (void *)0L; }
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ZEPHYR_SUBSYS_CANBUS_CANOPEN_CO_DRIVER_H */

subsys/canbus/canopen/Kconfig

@@ -0,0 +1,77 @@
+# CANopen configuration options
+
+# Copyright (c) 2019 Vestas Wind Systems A/S
+# SPDX-License-Identifier: Apache-2.0
+
+menuconfig CANOPEN
+	bool "CANopen protocol support"
+	depends on CAN
+	select CANOPENNODE
+	help
+	  Enable CANopen (EN 50325-4) (CiA 301) protocol
+	  support. Support is provided by the 3rd party CANopenNode
+	  protocol stack.
+
+if CANOPEN
+
+module = CANOPEN
+module-str = CANOPEN
+source "subsys/logging/Kconfig.template.log_config"
+
+config CANOPEN_SDO_BUFFER_SIZE
+	int "CANopen SDO buffer size"
+	default 32
+	range 7 889
+	help
+	  Size of the internal CANopen SDO buffer in bytes. Size must
+	  be at least equal to the size of the largest variable in the
+	  object dictionary. If data type is DOMAIN, data length is
+	  not limited to the SDO buffer size. If block transfer is
+	  implemented, value should be set to 889.
+
+config CANOPEN_TRACE_BUFFER_SIZE
+	int "CANopen trace buffer size"
+	default 100
+	help
+	  Size of the CANopen trace buffer in bytes.
+
+config CANOPEN_TX_WORKQUEUE_STACK_SIZE
+	int "Stack size for the CANopen transmit workqueue"
+	default 320
+	help
+	  Size of the stack used for the internal CANopen transmit
+	  workqueue.
+
+config CANOPEN_TX_WORKQUEUE_PRIORITY
+	int "Priority for CANopen transmit workqueue"
+	default 0 if !COOP_ENABLED
+	default -1
+	help
+	  Priority level of the internal CANopen transmit workqueue.
+
+config CANOPEN_SYNC_THREAD
+	bool "CANopen SYNC thread"
+	default y
+	help
+	  Enable internal thread for processing CANopen SYNC RPDOs and
+	  TPDOs. Application layer must take care of SYNC RPDO and
+	  TPDO processing on its own if this is disabled.
+
+config CANOPEN_SYNC_THREAD_STACK_SIZE
+	int "Stack size for the CANopen SYNC thread"
+	depends on CANOPEN_SYNC_THREAD
+	default 256
+	help
+	  Size of the stack used for the internal thread which
+	  processes CANopen SYNC RPDOs and TPDOs.
+
+config CANOPEN_SYNC_THREAD_PRIORITY
+	int "Priority for CANopen SYNC thread"
+	depends on CANOPEN_SYNC_THREAD
+	default 0 if !COOP_ENABLED
+	default -5
+	help
+	  Priority level of the internal thread which processes
+	  CANopen SYNC RPDOs and TPDOs.
+
+endif # CANOPEN

subsys/canbus/canopen/canopen_sync.c

@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2019 Vestas Wind Systems A/S
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <CANopen.h>
+
+/**
+ * @brief CANopen sync thread.
+ *
+ * The CANopen real-time sync thread processes SYNC RPDOs and TPDOs
+ * through the CANopenNode stack with an interval of 1 millisecond.
+ *
+ * @param p1 Unused
+ * @param p2 Unused
+ * @param p3 Unused
+ */
+static void canopen_sync_thread(void *p1, void *p2, void *p3)
+{
+	u32_t start; /* cycles */
+	u32_t stop;  /* cycles */
+	u32_t delta; /* cycles */
+	u32_t elapsed = 0; /* microseconds */
+	bool sync;
+
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+
+	while (true) {
+		start = k_cycle_get_32();
+		if (CO && CO->CANmodule[0] && CO->CANmodule[0]->CANnormal) {
+			CO_LOCK_OD();
+			sync = CO_process_SYNC(CO, elapsed);
+			CO_process_TPDO(CO, sync, elapsed);
+			CO_UNLOCK_OD();
+		}
+
+		k_sleep(K_MSEC(1));
+		stop = k_cycle_get_32();
+		delta = stop - start;
+		elapsed = (u32_t)k_cyc_to_ns_floor64(delta) / NSEC_PER_USEC;
+	}
+}
+
+K_THREAD_DEFINE(canopen_sync, CONFIG_CANOPEN_SYNC_THREAD_STACK_SIZE,
+		canopen_sync_thread, NULL, NULL, NULL,
+		CONFIG_CANOPEN_SYNC_THREAD_PRIORITY, 0, K_MSEC(1));