modules/canopennode/CO_driver.c

/*
 * Copyright (c) 2019 Vestas Wind Systems A/S
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/kernel.h>
#include <zephyr/drivers/can.h>
#include <zephyr/init.h>
#include <zephyr/sys/util.h>

#include <canopennode.h>

#define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(canopen_driver);

K_KERNEL_STACK_DEFINE(canopen_tx_workq_stack,
		      CONFIG_CANOPENNODE_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)
{
	uint16_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 != -ENOSPC) {
			can_remove_rx_filter(CANmodule->dev,
					     CANmodule->rx_array[i].filter_id);
			CANmodule->rx_array[i].filter_id = -ENOSPC;
		}
	}
}

static void canopen_rx_callback(const struct device *dev, struct can_frame *frame, void *arg)
{
	CO_CANrx_t *buffer = (CO_CANrx_t *)arg;
	CO_CANrxMsg_t rxMsg;

	ARG_UNUSED(dev);

	if (!buffer || !buffer->pFunct) {
		LOG_ERR("failed to process CAN rx callback");
		return;
	}

	rxMsg.ident = frame->id;
	rxMsg.DLC = frame->dlc;
	memcpy(rxMsg.data, frame->data, frame->dlc);
	buffer->pFunct(buffer->object, &rxMsg);
}

static void canopen_tx_callback(const struct device *dev, int error, void *arg)
{
	CO_CANmodule_t *CANmodule = arg;

	ARG_UNUSED(dev);

	if (!CANmodule) {
		LOG_ERR("failed to process CAN tx callback");
		return;
	}

	if (error == 0) {
		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 can_frame frame;
	CO_CANtx_t *buffer;
	int err;
	uint16_t i;

	CO_LOCK_CAN_SEND();

	for (i = 0; i < CANmodule->tx_size; i++) {
		buffer = &CANmodule->tx_array[i];
		if (buffer->bufferFull) {
			frame.id_type = CAN_STANDARD_IDENTIFIER;
			frame.id = buffer->ident;
			frame.dlc = buffer->DLC;
			frame.rtr = (buffer->rtr ? 1 : 0);
			memcpy(frame.data, buffer->data, buffer->DLC);

			err = can_send(CANmodule->dev, &frame, K_NO_WAIT,
				       canopen_tx_callback, CANmodule);
			if (err == -EAGAIN) {
				break;
			} else if (err != 0) {
				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)
{
	struct canopen_context *ctx = (struct canopen_context *)CANdriverState;
	int err;

	err = can_stop(ctx->dev);
	if (err != 0 && err != -EALREADY) {
		LOG_ERR("failed to stop CAN interface (err %d)", err);
	}
}

void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule)
{
	int err;

	err = can_start(CANmodule->dev);
	if (err != 0 && err != -EALREADY) {
		LOG_ERR("failed to start CAN interface (err %d)", err);
		return;
	}

	CANmodule->CANnormal = true;
}

CO_ReturnError_t CO_CANmodule_init(CO_CANmodule_t *CANmodule,
				   void *CANdriverState,
				   CO_CANrx_t rxArray[], uint16_t rxSize,
				   CO_CANtx_t txArray[], uint16_t txSize,
				   uint16_t CANbitRate)
{
	struct canopen_context *ctx = (struct canopen_context *)CANdriverState;
	uint16_t i;
	int err;
	int max_filters;

	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;
	}

	max_filters = can_get_max_filters(ctx->dev, CAN_STANDARD_IDENTIFIER);
	if (max_filters != -ENOSYS) {
		if (max_filters < 0) {
			LOG_ERR("unable to determine number of CAN RX filters");
			return CO_ERROR_SYSCALL;
		}

		if (rxSize > max_filters) {
			LOG_ERR("insufficient number of concurrent CAN RX filters"
				" (needs %d, %d available)", rxSize, max_filters);
			return CO_ERROR_OUT_OF_MEMORY;
		} else if (rxSize < max_filters) {
			LOG_DBG("excessive number of concurrent CAN RX filters enabled"
				" (needs %d, %d available)", rxSize, max_filters);
		}
	}

	canopen_detach_all_rx_filters(CANmodule);
	canopen_tx_queue.CANmodule = CANmodule;

	CANmodule->dev = ctx->dev;
	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 = -ENOSPC;
	}

	for (i = 0U; i < txSize; i++) {
		txArray[i].bufferFull = false;
	}

	err = can_set_bitrate(CANmodule->dev, KHZ(CANbitRate));
	if (err) {
		LOG_ERR("failed to configure CAN bitrate (err %d)", err);
		return CO_ERROR_ILLEGAL_ARGUMENT;
	}

	err = can_set_mode(CANmodule->dev, CAN_MODE_NORMAL);
	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_stop(CANmodule->dev);
	if (err != 0 && err != -EALREADY) {
		LOG_ERR("failed to disable CAN interface (err %d)", err);
	}
}

uint16_t CO_CANrxMsg_readIdent(const CO_CANrxMsg_t *rxMsg)
{
	return rxMsg->ident;
}

CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index,
				uint16_t ident, uint16_t mask, bool_t rtr,
				void *object,
				CO_CANrxBufferCallback_t pFunct)
{
	struct can_filter filter;
	CO_CANrx_t *buffer;

	if (CANmodule == NULL) {
		return CO_ERROR_ILLEGAL_ARGUMENT;
	}

	if (!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.id = ident;
	filter.id_mask = mask;
	filter.rtr = (rtr ? 1 : 0);
	filter.rtr_mask = 1;

	if (buffer->filter_id != -ENOSPC) {
		can_remove_rx_filter(CANmodule->dev, buffer->filter_id);
	}

	buffer->filter_id = can_add_rx_filter(CANmodule->dev,
					      canopen_rx_callback,
					      buffer, &filter);
	if (buffer->filter_id == -ENOSPC) {
		LOG_ERR("failed to add CAN rx callback, 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, uint16_t index,
			       uint16_t ident, bool_t rtr, uint8_t noOfBytes,
			       bool_t syncFlag)
{
	CO_CANtx_t *buffer;

	if (CANmodule == NULL) {
		return NULL;
	}

	if (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 can_frame frame;
	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;
	}

	frame.id_type = CAN_STANDARD_IDENTIFIER;
	frame.id = buffer->ident;
	frame.dlc = buffer->DLC;
	frame.rtr = (buffer->rtr ? 1 : 0);
	memcpy(frame.data, buffer->data, buffer->DLC);

	err = can_send(CANmodule->dev, &frame, K_NO_WAIT, canopen_tx_callback,
		       CANmodule);
	if (err == -EAGAIN) {
		buffer->bufferFull = true;
	} else if (err != 0) {
		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;
	uint16_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;
	uint8_t rx_overflows;
	uint32_t errors;
	int err;

	/*
	 * TODO: Zephyr lacks an API for reading the rx mailbox
	 * overflow counter.
	 */
	rx_overflows  = 0;

	err = can_get_state(CANmodule->dev, &state, &err_cnt);
	if (err != 0) {
		LOG_ERR("failed to get CAN controller state (err %d)", err);
		return;
	}

	errors = ((uint32_t)err_cnt.tx_err_cnt << 16) |
		 ((uint32_t)err_cnt.rx_err_cnt << 8) |
		 rx_overflows;

	if (errors != CANmodule->errors) {
		CANmodule->errors = errors;

		if (state == CAN_STATE_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);
			}
		}

		/* This code can be activated if we can read the overflows*/
		if (false && rx_overflows != 0U) {
			CO_errorReport(em, CO_EM_CAN_RXB_OVERFLOW,
				       CO_EMC_CAN_OVERRUN, errors);
		}
	}
}

static int canopen_init(const struct device *dev)
{
	ARG_UNUSED(dev);

	k_work_queue_start(&canopen_tx_workq, canopen_tx_workq_stack,
			   K_KERNEL_STACK_SIZEOF(canopen_tx_workq_stack),
			   CONFIG_CANOPENNODE_TX_WORKQUEUE_PRIORITY, NULL);

	k_thread_name_set(&canopen_tx_workq.thread, "canopen_tx_workq");

	k_work_init(&canopen_tx_queue.work, canopen_tx_retry);

	return 0;
}

SYS_INIT(canopen_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);