nikon/CANopenNode
1
0
Fork 0
Code Activity Actions
CANopenNode/stack/neuberger-socketCAN/CO_driver.c
Martin Wagner c46a2edd57 Add Linux socketCAN driver for CANopen manager application 
This driver is more sophisticated than the normal socketCAN driver. Compared
to the normal socketCAN driver, this one contains the linking exception license.

Changes compared to normal socketCAN driver:
- Re-implementation based on driver template
- Error detection works
- Setting up filters works properly
- Optional Support for socketCAN error frames. This currently handles bus-off
  and no-ack condition by setting driver into listen-only mode. If you decide
  to use this feature have a close look at your own requirements and fit
  error handling functions to that.
- Optional support for CAN interface combining (not redundancy!). With this
  feature enabled you can have multiple CAN interface represented as one within
  CANopenNode stack. By default, all TX messages are sent on all used CAN
  interfaces, but the user can change this behaviour inside own app (e.g. check
  on wich bus rx-sdo is received and set-up tx-sdo accordingly).
  Be aware that no bridging between the interfaces is done!
2019-05-10 09:52:11 +02:00

1007 lines
31 KiB
C
Raw Blame History

/*
* CAN module object for Linux socketCAN.
*
* This file is a template for other microcontrollers.
*
* @file CO_driver.c
* @ingroup CO_driver
* @author Janez Paternoster, Martin Wagner
* @copyright 2004 - 2015 Janez Paternoster, 2017 Neuberger Gebaeudeautomation GmbH
*
* This file is part of CANopenNode, an opensource CANopen Stack.
* Project home page is <https://github.com/CANopenNode/CANopenNode>.
* For more information on CANopen see <http://www.can-cia.org/>.
*
* CANopenNode is free and open source software: you can redistribute
* it and/or modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation, either version 2 of the
* License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Following clarification and special exception to the GNU General Public
* License is included to the distribution terms of CANopenNode:
*
* Linking this library statically or dynamically with other modules is
* making a combined work based on this library. Thus, the terms and
* conditions of the GNU General Public License cover the whole combination.
*
* As a special exception, the copyright holders of this library give
* you permission to link this library with independent modules to
* produce an executable, regardless of the license terms of these
* independent modules, and to copy and distribute the resulting
* executable under terms of your choice, provided that you also meet,
* for each linked independent module, the terms and conditions of the
* license of that module. An independent module is a module which is
* not derived from or based on this library. If you modify this
* library, you may extend this exception to your version of the
* library, but you are not obliged to do so. If you do not wish
* to do so, delete this exception statement from your version.
*/
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <linux/can/raw.h>
#include <linux/can/error.h>
#include <linux/net_tstamp.h>
#include <sys/socket.h>
#include <sys/epoll.h>
#include "CO_driver.h"
#if defined CO_DRIVER_ERROR_REPORTING && __has_include("syslog/log.h")
#include "syslog/log.h"
#include "msgs.h"
#else
#define log_printf(macropar_prio, macropar_message, ...)
#endif
#if __has_include("CO_Emergency.h")
#include "CO_Emergency.h"
#define USE_EMERGENCY_OBJECT
#endif
pthread_mutex_t CO_EMCY_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t CO_OD_mutex = PTHREAD_MUTEX_INITIALIZER;
#ifndef CO_DRIVER_MULTI_INTERFACE
static CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, int32_t CANbaseAddress);
#endif
#ifdef CO_DRIVER_MULTI_INTERFACE
static const uint32_t CO_INVALID_COB_ID = 0xffffffff;
/******************************************************************************/
void CO_CANsetIdentToIndex(
uint32_t *lookup,
uint32_t index,
uint32_t identNew,
uint32_t identCurrent)
{
/* entry changed, remove old one */
if (identCurrent<CO_CAN_MSG_SFF_MAX_COB_ID && identNew!=identCurrent) {
lookup[identCurrent] = CO_INVALID_COB_ID;
}
/* check if this COB ID is part of the table */
if (identNew > CO_CAN_MSG_SFF_MAX_COB_ID) {
return;
}
/* Special case COB ID "0" -> valid value in *xArray[0] (CO_*CAN_NMT),
* "entry unconfigured" for all others */
if (identNew == 0) {
if (index == 0) {
lookup[0] = 0;
}
}
else {
lookup[identNew] = index;
}
}
/******************************************************************************/
static uint32_t CO_CANgetIndexFromIdent(
uint32_t *lookup,
uint32_t ident)
{
/* check if this COB ID is part of the table */
if (ident > CO_CAN_MSG_SFF_MAX_COB_ID) {
return CO_INVALID_COB_ID;
}
return lookup[ident];
}
#endif
/** Disable socketCAN rx *****************************************************/
static CO_ReturnError_t disableRx(CO_CANmodule_t *CANmodule)
{
int ret;
uint32_t i;
CO_ReturnError_t retval;
/* insert a filter that doesn't match any messages */
retval = CO_ERROR_NO;
for (i = 0; i < CANmodule->CANinterfaceCount; i ++) {
ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER,
NULL, 0);
if(ret < 0){
log_printf(LOG_ERR, CAN_FILTER_FAILED,
CANmodule->CANinterfaces[i].ifName);
log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt()");
retval = CO_ERROR_SYSCALL;
}
}
return retval;
}
/** Set up or update socketCAN rx filters *************************************/
static CO_ReturnError_t setRxFilters(CO_CANmodule_t *CANmodule)
{
int ret;
int i;
int count;
CO_ReturnError_t retval;
struct can_filter rxFiltersCpy[CANmodule->rxSize];
count = 0;
/* remove unused entries ( id == 0 and mask == 0 ) as they would act as
* "pass all" filter */
for (i = 0; i < CANmodule->rxSize; i ++) {
if ((CANmodule->rxFilter[i].can_id != 0) ||
(CANmodule->rxFilter[i].can_mask != 0)) {
rxFiltersCpy[count] = CANmodule->rxFilter[i];
count ++;
}
}
if (count == 0) {
/* No filter is set, disable RX */
return disableRx(CANmodule);
}
retval = CO_ERROR_NO;
for (i = 0; i < CANmodule->CANinterfaceCount; i ++) {
ret = setsockopt(CANmodule->CANinterfaces[i].fd, SOL_CAN_RAW, CAN_RAW_FILTER,
rxFiltersCpy, sizeof(struct can_filter) * count);
if(ret < 0){
log_printf(LOG_ERR, CAN_FILTER_FAILED,
CANmodule->CANinterfaces[i].ifName);
log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt()");
retval = CO_ERROR_SYSCALL;
}
}
return retval;
}
/******************************************************************************/
void CO_CANsetConfigurationMode(int32_t CANbaseAddress)
{
/* Can't do anything because no object is provided */
}
/******************************************************************************/
void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule)
{
CO_ReturnError_t ret;
CANmodule->CANnormal = false;
if(CANmodule != NULL) {
ret = setRxFilters(CANmodule);
if (ret == CO_ERROR_NO) {
/* Put CAN module in normal mode */
CANmodule->CANnormal = true;
}
}
}
/******************************************************************************/
CO_ReturnError_t CO_CANmodule_init(
CO_CANmodule_t *CANmodule,
int32_t CANbaseAddress,
CO_CANrx_t rxArray[],
uint16_t rxSize,
CO_CANtx_t txArray[],
uint16_t txSize,
uint16_t CANbitRate)
{
int32_t ret;
uint16_t i;
struct epoll_event ev;
/* verify arguments */
if(CANmodule==NULL || rxArray==NULL || txArray==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* Create epoll FD */
CANmodule->fdEpoll = epoll_create(1);
if(CANmodule->fdEpoll < 0){
log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_create()");
CO_CANmodule_disable(CANmodule);
return CO_ERROR_SYSCALL;
}
/* Create notification pipe */
CANmodule->pipe = CO_NotifyPipeCreate();
if (CANmodule->pipe==NULL) {
log_printf(LOG_DEBUG, DBG_ERRNO, "pipe");
CO_CANmodule_disable(CANmodule);
return CO_ERROR_OUT_OF_MEMORY;
}
/* ...and add it to epoll */
ev.events = EPOLLIN;
ev.data.fd = CO_NotifyPipeGetFd(CANmodule->pipe);
ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev.data.fd, &ev);
if(ret < 0){
log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_ctl(pipe)");
CO_CANmodule_disable(CANmodule);
return CO_ERROR_SYSCALL;
}
/* Configure object variables */
CANmodule->CANinterfaces = NULL;
CANmodule->CANinterfaceCount = 0;
CANmodule->rxArray = rxArray;
CANmodule->rxSize = rxSize;
CANmodule->txArray = txArray;
CANmodule->txSize = txSize;
CANmodule->CANnormal = false;
CANmodule->em = NULL; //this is set inside CO_Emergency.c init function!
CANmodule->fdTimerRead = -1;
#ifdef CO_DRIVER_MULTI_INTERFACE
for (i = 0; i < CO_CAN_MSG_SFF_MAX_COB_ID; i++) {
CANmodule->rxIdentToIndex[i] = CO_INVALID_COB_ID;
CANmodule->txIdentToIndex[i] = CO_INVALID_COB_ID;
}
#endif
/* initialize socketCAN filters
* CAN module filters will be configured with CO_CANrxBufferInit()
* functions, called by separate CANopen init functions */
CANmodule->rxFilter = calloc(CANmodule->rxSize, sizeof(struct can_filter));
if(CANmodule->rxFilter == NULL){
log_printf(LOG_DEBUG, DBG_ERRNO, "malloc()");
return CO_ERROR_OUT_OF_MEMORY;
}
for(i=0U; i<rxSize; i++){
rxArray[i].ident = 0U;
rxArray[i].mask = 0xFFFFFFFFU;
rxArray[i].object = NULL;
rxArray[i].pFunct = NULL;
#ifdef CO_DRIVER_MULTI_INTERFACE
rxArray[i].CANbaseAddress = -1;
rxArray[i].timestamp.tv_sec = 0;
rxArray[i].timestamp.tv_nsec = 0;
#endif
}
#ifndef CO_DRIVER_MULTI_INTERFACE
/* add one interface */
ret = CO_CANmodule_addInterface(CANmodule, CANbaseAddress);
if (ret != CO_ERROR_NO) {
CO_CANmodule_disable(CANmodule);
}
#else
ret = CO_ERROR_NO;
#endif
return ret;
}
/** enable socketCAN *********************************************************/
#ifndef CO_DRIVER_MULTI_INTERFACE
static
#endif
CO_ReturnError_t CO_CANmodule_addInterface(
CO_CANmodule_t *CANmodule,
int32_t CANbaseAddress)
{
int32_t ret;
int32_t tmp;
int32_t bytes;
char *ifName;
socklen_t sLen;
CO_CANinterface_t *interface;
struct sockaddr_can sockAddr;
struct epoll_event ev;
#ifdef CO_DRIVER_ERROR_REPORTING
can_err_mask_t err_mask;
#endif
if (CANmodule->CANnormal != false) {
/* can't change config now! */
return CO_ERROR_INVALID_STATE;
}
/* Add interface to interface list */
CANmodule->CANinterfaceCount ++;
CANmodule->CANinterfaces = realloc(CANmodule->CANinterfaces,
((CANmodule->CANinterfaceCount) * sizeof(*CANmodule->CANinterfaces)));
if (CANmodule->CANinterfaces == NULL) {
log_printf(LOG_DEBUG, DBG_ERRNO, "malloc()");
return CO_ERROR_OUT_OF_MEMORY;
}
interface = &CANmodule->CANinterfaces[CANmodule->CANinterfaceCount - 1];
interface->CANbaseAddress = CANbaseAddress;
ifName = if_indextoname(CANbaseAddress, interface->ifName);
if (ifName == NULL) {
log_printf(LOG_DEBUG, DBG_ERRNO, "if_indextoname()");
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* Create socket */
interface->fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if(interface->fd < 0){
log_printf(LOG_DEBUG, DBG_ERRNO, "socket(can)");
return CO_ERROR_SYSCALL;
}
/* enable socket rx queue overflow detection */
tmp = 1;
ret = setsockopt(interface->fd, SOL_SOCKET, SO_RXQ_OVFL, &tmp, sizeof(tmp));
if(ret < 0){
log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(ovfl)");
return CO_ERROR_SYSCALL;
}
#ifdef CO_DRIVER_MULTI_INTERFACE
/* enable software time stamp mode (hardware timestamps do not work properly
* on all devices)*/
tmp = (SOF_TIMESTAMPING_SOFTWARE |
SOF_TIMESTAMPING_RX_SOFTWARE);
ret = setsockopt(interface->fd, SOL_SOCKET, SO_TIMESTAMPING, &tmp, sizeof(tmp));
if (ret < 0) {
log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(timestamping)");
return CO_ERROR_SYSCALL;
}
#endif
//todo - modify rx buffer size? first one needs root
//ret = setsockopt(fd, SOL_SOCKET, SO_RCVBUFFORCE, (void *)&bytes, sLen);
//ret = setsockopt(fd, SOL_SOCKET, SO_RCVBUF, (void *)&bytes, sLen);
/* print socket rx buffer size in bytes (In my experience, the kernel reserves
* around 450 bytes for each CAN message) */
sLen = sizeof(bytes);
getsockopt(interface->fd, SOL_SOCKET, SO_RCVBUF, (void *)&bytes, &sLen);
if (sLen == sizeof(bytes)) {
log_printf(LOG_INFO, CAN_SOCKET_BUF_SIZE, interface->ifName,
bytes / 446, bytes);
}
/* bind socket */
memset(&sockAddr, 0, sizeof(sockAddr));
sockAddr.can_family = AF_CAN;
sockAddr.can_ifindex = CANbaseAddress;
ret = bind(interface->fd, (struct sockaddr*)&sockAddr, sizeof(sockAddr));
if(ret < 0){
log_printf(LOG_ERR, CAN_BINDING_FAILED, interface->ifName);
log_printf(LOG_DEBUG, DBG_ERRNO, "bind()");
return CO_ERROR_SYSCALL;
}
#ifdef CO_DRIVER_ERROR_REPORTING
CO_CANerror_init(&interface->errorhandler, interface->fd, interface->ifName);
/* set up error frame generation. What actually is available depends on your
* CAN kernel driver */
#ifdef DEBUG
err_mask = CAN_ERR_MASK; //enable ALL error frames
#else
err_mask = CAN_ERR_ACK | CAN_ERR_CRTL | CAN_ERR_BUSOFF | CAN_ERR_BUSERROR;
#endif
ret = setsockopt(interface->fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask,
sizeof(err_mask));
if(ret < 0){
log_printf(LOG_ERR, CAN_ERROR_FILTER_FAILED, interface->ifName);
log_printf(LOG_DEBUG, DBG_ERRNO, "setsockopt(can err)");
return CO_ERROR_SYSCALL;
}
#endif
/* Add socket to epoll */
ev.events = EPOLLIN;
ev.data.fd = interface->fd;
ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev.data.fd, &ev);
if(ret < 0){
log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_ctl(can)");
return CO_ERROR_SYSCALL;
}
/* rx is started by calling #CO_CANsetNormalMode() */
ret = disableRx(CANmodule);
return ret;
}
/******************************************************************************/
void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
{
uint32_t i;
struct timespec wait;
if (CANmodule == NULL) {
return;
}
/* clear interfaces */
for (i = 0; i < CANmodule->CANinterfaceCount; i++) {
CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i];
#ifdef CO_DRIVER_ERROR_REPORTING
CO_CANerror_disable(&interface->errorhandler);
#endif
epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_DEL, interface->fd, NULL);
close(interface->fd);
interface->fd = -1;
}
if (CANmodule->CANinterfaces != NULL) {
free(CANmodule->CANinterfaces);
}
CANmodule->CANinterfaceCount = 0;
/* cancel rx */
if (CANmodule->pipe != NULL) {
CO_NotifyPipeSend(CANmodule->pipe);
/* give some time for delivery */
wait.tv_sec = 0;
wait.tv_nsec = 50 /* ms */ * 1000000;
nanosleep(&wait, NULL);
CO_NotifyPipeFree(CANmodule->pipe);
}
if (CANmodule->fdEpoll >= 0) {
close(CANmodule->fdEpoll);
}
CANmodule->fdEpoll = -1;
if (CANmodule->rxFilter != NULL) {
free(CANmodule->rxFilter);
}
CANmodule->rxFilter = NULL;
}
/******************************************************************************/
uint16_t CO_CANrxMsg_readIdent(const CO_CANrxMsg_t *rxMsg)
{
/* remove socketCAN flags */
return (uint16_t) (rxMsg->ident & CAN_SFF_MASK);
}
/******************************************************************************/
CO_ReturnError_t CO_CANrxBufferInit(
CO_CANmodule_t *CANmodule,
uint32_t index,
uint32_t ident,
uint32_t mask,
bool_t rtr,
void *object,
void (*pFunct)(void *object, const CO_CANrxMsg_t *message))
{
CO_ReturnError_t ret = CO_ERROR_NO;
if((CANmodule!=NULL) && (index < CANmodule->rxSize)){
uint32_t i;
CO_CANrx_t *buffer;
/* check if COB ID is already used */
for (i = 0; i < CANmodule->rxSize; i ++) {
buffer = &CANmodule->rxArray[i];
if (i!=index && ident>0 && ident==buffer->ident) {
log_printf(LOG_DEBUG, DBG_CAN_RX_PARAM_FAILED, "duplicate entry");
ret = CO_ERROR_ILLEGAL_ARGUMENT;
}
}
if (ret == CO_ERROR_NO) {
/* buffer, which will be configured */
buffer = &CANmodule->rxArray[index];
#ifdef CO_DRIVER_MULTI_INTERFACE
CO_CANsetIdentToIndex(CANmodule->rxIdentToIndex, index, ident,
buffer->ident);
#endif
/* Configure object variables */
buffer->object = object;
buffer->pFunct = pFunct;
#ifdef CO_DRIVER_MULTI_INTERFACE
buffer->CANbaseAddress = -1;
buffer->timestamp.tv_nsec = 0;
buffer->timestamp.tv_sec = 0;
#endif
/* CAN identifier and CAN mask, bit aligned with CAN module */
buffer->ident = ident & CAN_SFF_MASK;
if(rtr){
buffer->ident |= CAN_RTR_FLAG;
}
buffer->mask = (mask & CAN_SFF_MASK) | CAN_EFF_FLAG | CAN_RTR_FLAG;
/* Set CAN hardware module filter and mask. */
CANmodule->rxFilter[index].can_id = buffer->ident;
CANmodule->rxFilter[index].can_mask = buffer->mask;
if(CANmodule->CANnormal){
ret = setRxFilters(CANmodule);
}
}
}
else {
ret = CO_ERROR_ILLEGAL_ARGUMENT;
}
return ret;
}
#ifdef CO_DRIVER_MULTI_INTERFACE
/******************************************************************************/
bool_t CO_CANrxBuffer_getInterface(
CO_CANmodule_t *CANmodule,
uint32_t ident,
int32_t *CANbaseAddressRx,
struct timespec *timestamp)
{
if (CANmodule != NULL){
uint32_t index;
CO_CANrx_t *buffer;
index = CO_CANgetIndexFromIdent(CANmodule->rxIdentToIndex, ident);
if ((index == CO_INVALID_COB_ID) || (index > CANmodule->rxSize)) {
return false;
}
buffer = &CANmodule->rxArray[index];
/* return values */
if (CANbaseAddressRx != NULL) {
*CANbaseAddressRx = buffer->CANbaseAddress;
}
if (timestamp != NULL) {
*timestamp = buffer->timestamp;
}
if (buffer->CANbaseAddress >= 0) {
return true;
}
else {
return false;
}
}
return false;
}
#endif
/******************************************************************************/
CO_CANtx_t *CO_CANtxBufferInit(
CO_CANmodule_t *CANmodule,
uint32_t index,
uint32_t ident,
bool_t rtr,
uint8_t noOfBytes,
bool_t syncFlag)
{
CO_CANtx_t *buffer = NULL;
if((CANmodule != NULL) && (index < CANmodule->txSize)){
/* get specific buffer */
buffer = &CANmodule->txArray[index];
#ifdef CO_DRIVER_MULTI_INTERFACE
CO_CANsetIdentToIndex(CANmodule->txIdentToIndex, index, ident, buffer->ident);
#endif
buffer->CANbaseAddress = -1;
/* CAN identifier and rtr */
buffer->ident = ident & CAN_SFF_MASK;
if(rtr){
buffer->ident |= CAN_RTR_FLAG;
}
buffer->DLC = noOfBytes;
buffer->bufferFull = false;
buffer->syncFlag = syncFlag;
}
return buffer;
}
#ifdef CO_DRIVER_MULTI_INTERFACE
/******************************************************************************/
CO_ReturnError_t CO_CANtxBuffer_setInterface(
CO_CANmodule_t *CANmodule,
uint32_t ident,
int32_t CANbaseAddressTx)
{
if (CANmodule != NULL) {
uint32_t index;
index = CO_CANgetIndexFromIdent(CANmodule->txIdentToIndex, ident);
if ((index == CO_INVALID_COB_ID) || (index > CANmodule->txSize)) {
return CO_ERROR_PARAMETERS;
}
CANmodule->txArray[index].CANbaseAddress = CANbaseAddressTx;
return CO_ERROR_NO;
}
return CO_ERROR_PARAMETERS;
}
#endif
/******************************************************************************/
static CO_ReturnError_t CO_CANCheckSendInterface(
CO_CANmodule_t *CANmodule,
CO_CANtx_t *buffer,
CO_CANinterface_t *interface)
{
CO_ReturnError_t err = CO_ERROR_NO;
#ifdef CO_DRIVER_ERROR_REPORTING
CO_CANinterfaceState_t ifState;
#endif
ssize_t n;
if (CANmodule==NULL || interface==NULL || interface->fd < 0) {
return CO_ERROR_PARAMETERS;
}
#ifdef CO_DRIVER_ERROR_REPORTING
ifState = CO_CANerror_txMsg(&interface->errorhandler);
switch (ifState) {
case CO_INTERFACE_ACTIVE:
/* continue */
break;
case CO_INTERFACE_LISTEN_ONLY:
/* silently drop message */
return CO_ERROR_NO;
default:
return CO_ERROR_INVALID_STATE;
}
#endif
do {
errno = 0;
n = send(interface->fd, buffer, CAN_MTU, MSG_DONTWAIT);
if (errno == EINTR) {
/* try again */
continue;
}
else if (errno == EAGAIN) {
/* socket queue full */
break;
}
else if (errno == ENOBUFS) {
/* socketCAN doesn't support blocking write. You can wait here for
* a few hundred us and then try again */
return CO_ERROR_TX_BUSY;
}
else if (n != CAN_MTU) {
break;
}
} while (errno != 0);
if(n != CAN_MTU){
#ifdef USE_EMERGENCY_OBJECT
CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_TX_OVERFLOW, CO_EMC_CAN_OVERRUN, 0);
#endif
log_printf(LOG_ERR, DBG_CAN_TX_FAILED, buffer->ident, interface->ifName);
log_printf(LOG_DEBUG, DBG_ERRNO, "send()");
err = CO_ERROR_TX_OVERFLOW;
}
return err;
}
/******************************************************************************/
CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
{
CO_ReturnError_t err;
err = CO_CANCheckSend(CANmodule, buffer);
if (err == CO_ERROR_TX_BUSY) {
/* send doesn't have "busy" */
#ifdef USE_EMERGENCY_OBJECT
CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_TX_OVERFLOW, CO_EMC_CAN_OVERRUN, 0);
#endif
log_printf(LOG_ERR, DBG_CAN_TX_FAILED, buffer->ident, "CANx");
log_printf(LOG_DEBUG, DBG_ERRNO, "send()");
err = CO_ERROR_TX_OVERFLOW;
}
return err;
}
/******************************************************************************/
CO_ReturnError_t CO_CANCheckSend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
{
uint32_t i;
CO_ReturnError_t err = CO_ERROR_NO;
/* check on which interfaces to send this messages */
for (i = 0; i < CANmodule->CANinterfaceCount; i++) {
CO_CANinterface_t *interface = &CANmodule->CANinterfaces[i];
if (buffer->CANbaseAddress < 0 ||
buffer->CANbaseAddress == interface->CANbaseAddress) {
CO_ReturnError_t tmp;
/* match, use this one */
tmp = CO_CANCheckSendInterface(CANmodule, buffer, interface);
if (tmp) {
/* only last error is returned to callee */
err = tmp;
}
}
}
return err;
}
/******************************************************************************/
void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule)
{
/* Messages are either written to the socket queue or dropped */
}
/******************************************************************************/
void CO_CANverifyErrors(CO_CANmodule_t *CANmodule)
{
/* socketCAN doesn't support microcontroller-like error counters. If an
* error has occured, a special can message is created by the driver and
* received by the application like a regular message.
* Therefore, error counter evaluation is included in rx function.*/
}
/******************************************************************************/
static CO_ReturnError_t CO_CANread(
CO_CANmodule_t *CANmodule,
CO_CANinterface_t *interface,
struct can_frame *msg,
struct timespec *timestamp)
{
int32_t n;
uint32_t dropped;
/* recvmsg - like read, but generates statistics about the socket
* example in berlios candump.c */
struct iovec iov;
struct msghdr msghdr;
char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(dropped))];
struct cmsghdr *cmsg;
iov.iov_base = msg;
iov.iov_len = sizeof(*msg);
msghdr.msg_name = NULL;
msghdr.msg_namelen = 0;
msghdr.msg_iov = &iov;
msghdr.msg_iovlen = 1;
msghdr.msg_control = &ctrlmsg;
msghdr.msg_controllen = sizeof(ctrlmsg);
msghdr.msg_flags = 0;
n = recvmsg(interface->fd, &msghdr, 0);
if (n != CAN_MTU) {
#ifdef USE_EMERGENCY_OBJECT
CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_RXB_OVERFLOW,
CO_EMC_CAN_OVERRUN, n);
#endif
log_printf(LOG_DEBUG, DBG_CAN_RX_FAILED, interface->ifName);
log_printf(LOG_DEBUG, DBG_ERRNO, "recvmsg()");
return CO_ERROR_SYSCALL;
}
/* check for rx queue overflow, get rx time */
for (cmsg = CMSG_FIRSTHDR(&msghdr);
cmsg && (cmsg->cmsg_level == SOL_SOCKET);
cmsg = CMSG_NXTHDR(&msghdr, cmsg)) {
if (cmsg->cmsg_type == SO_TIMESTAMPING) {
/* this is system time, not monotonic time! */
*timestamp = ((struct timespec*)CMSG_DATA(cmsg))[0];
}
else if (cmsg->cmsg_type == SO_RXQ_OVFL) {
dropped = *(uint32_t*)CMSG_DATA(cmsg);
if (dropped > CANmodule->rxDropCount) {
#ifdef USE_EMERGENCY_OBJECT
CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_RXB_OVERFLOW,
CO_EMC_COMMUNICATION, 0);
#endif
log_printf(LOG_ERR, CAN_RX_SOCKET_QUEUE_OVERFLOW,
interface->ifName, dropped);
}
CANmodule->rxDropCount = dropped;
//todo use this info!
}
}
return CO_ERROR_NO;
}
static int32_t CO_CANrxMsg(
CO_CANmodule_t *CANmodule,
struct can_frame *msg,
CO_CANrxMsg_t *buffer)
{
int32_t retval;
const CO_CANrxMsg_t *rcvMsg; /* pointer to received message in CAN module */
uint16_t index; /* index of received message */
CO_CANrx_t *rcvMsgObj = NULL; /* receive message object from CO_CANmodule_t object. */
bool_t msgMatched = false;
/* CANopenNode can message is binary compatible to the socketCAN one, except
* for extension flags */
msg->can_id &= CAN_EFF_MASK;
rcvMsg = (CO_CANrxMsg_t *)msg;
/* Message has been received. Search rxArray from CANmodule for the
* same CAN-ID. */
rcvMsgObj = &CANmodule->rxArray[0];
for (index = 0; index < CANmodule->rxSize; index ++) {
if(((rcvMsg->ident ^ rcvMsgObj->ident) & rcvMsgObj->mask) == 0U){
msgMatched = true;
break;
}
rcvMsgObj++;
}
if(msgMatched) {
/* Call specific function, which will process the message */
if ((rcvMsgObj != NULL) && (rcvMsgObj->pFunct != NULL)){
rcvMsgObj->pFunct(rcvMsgObj->object, rcvMsg);
}
/* return message */
if (buffer != NULL) {
memcpy(buffer, rcvMsg, sizeof(*buffer));
}
retval = index;
}
else {
retval = -1;
}
return retval;
}
/******************************************************************************/
int32_t CO_CANrxWait(CO_CANmodule_t *CANmodule, int fdTimer, CO_CANrxMsg_t *buffer)
{
int32_t retval;
int32_t ret;
int32_t CANbaseAddress __attribute__((unused));
CO_ReturnError_t err;
CO_CANinterface_t *interface = NULL;
struct epoll_event ev[1];
struct can_frame msg;
struct timespec timestamp;
if (CANmodule==NULL || CANmodule->CANinterfaceCount==0) {
return -1;
}
if (fdTimer>=0 && fdTimer!=CANmodule->fdTimerRead) {
/* new timer, timer changed */
epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_DEL, CANmodule->fdTimerRead, NULL);
ev[0].events = EPOLLIN;
ev[0].data.fd = fdTimer;
ret = epoll_ctl(CANmodule->fdEpoll, EPOLL_CTL_ADD, ev[0].data.fd, &ev[0]);
if(ret < 0){
return -1;
}
CANmodule->fdTimerRead = fdTimer;
}
/*
* blocking read using epoll
*/
do {
errno = 0;
ret = epoll_wait(CANmodule->fdEpoll, ev, sizeof(ev) / sizeof(ev[0]), -1);
if (errno == EINTR) {
/* try again */
continue;
}
else if (ret < 0) {
/* epoll failed */
return -1;
}
else if ((ev[0].events & (EPOLLERR | EPOLLHUP)) != 0) {
/* epoll detected close/error on socket. Try to pull event */
errno = 0;
recv(ev[0].data.fd, &msg, sizeof(msg), MSG_DONTWAIT);
log_printf(LOG_DEBUG, DBG_CAN_RX_EPOLL, ev[0].events, strerror(errno));
continue;
}
else if ((ev[0].events & EPOLLIN) != 0) {
/* one of the sockets is ready */
if ((ev[0].data.fd == CO_NotifyPipeGetFd(CANmodule->pipe)) ||
(ev[0].data.fd == fdTimer)) {
/* timer/pipe socket */
return -1;
}
else {
/* CAN socket */
uint32_t i;
for (i = 0; i < CANmodule->CANinterfaceCount; i ++) {
interface = &CANmodule->CANinterfaces[i];
if (ev[0].data.fd == interface->fd) {
/* get interface handle */
CANbaseAddress = interface->CANbaseAddress;
/* get message */
err = CO_CANread(CANmodule, interface, &msg, &timestamp);
if (err != CO_ERROR_NO) {
return -1;
}
/* no need to continue search */
break;
}
}
}
}
} while (errno != 0);
/*
* evaluate Rx
*/
retval = -1;
if(CANmodule->CANnormal){
if (msg.can_id & CAN_ERR_FLAG) {
/* error msg */
#ifdef CO_DRIVER_ERROR_REPORTING
CO_CANerror_rxMsgError(&interface->errorhandler, &msg);
#endif
}
else {
/* data msg */
int32_t msgIndex;
#ifdef CO_DRIVER_ERROR_REPORTING
CO_CANerror_rxMsg(&interface->errorhandler);
#endif
msgIndex = CO_CANrxMsg(CANmodule, &msg, buffer);
if (msgIndex > -1) {
#ifdef CO_DRIVER_MULTI_INTERFACE
/* Store message info */
CANmodule->rxArray[msgIndex].timestamp = timestamp;
CANmodule->rxArray[msgIndex].CANbaseAddress = CANbaseAddress;
#endif
}
retval = msgIndex;
}
}
return retval;
}
View git blame Copy permalink