nikon/CANopenLinux
1
0
Fork 0
Code Activity Actions
CANopenLinux/CO_driver.c

877 lines
28 KiB
C
Raw Permalink Blame History

/*
* Linux socketCAN interface for CANopenNode.
*
* @file CO_driver.c
* @ingroup CO_driver
* @author Janez Paternoster, Martin Wagner
* @copyright 2004 - 2015 Janez Paternoster, 2017 - 2020 Neuberger Gebaeudeautomation GmbH
*
*
* This file is part of <https://github.com/CANopenNode/CANopenNode>, a CANopen Stack.
*
* Licensed under the Apache License, Version 2.0 (the "License"); you may not use this
* file except in compliance with the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software distributed under the License is
* distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and limitations under the License.
*/
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <syslog.h>
#include <linux/can/raw.h>
#include <linux/can/error.h>
#include <linux/net_tstamp.h>
#include <sys/socket.h>
#include <asm/socket.h>
#include <sys/eventfd.h>
#include <time.h>
#include "301/CO_driver.h"
#include "CO_error.h"
#ifndef CO_SINGLE_THREAD
pthread_mutex_t CO_EMCY_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t CO_OD_mutex = PTHREAD_MUTEX_INITIALIZER;
#endif
#if CO_DRIVER_MULTI_INTERFACE == 0
static CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t* CANmodule, int can_ifindex);
#endif
#if CO_DRIVER_MULTI_INTERFACE > 0
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 /* CO_DRIVER_MULTI_INTERFACE */
/* Disable socketCAN rx */
static CO_ReturnError_t
disableRx(CO_CANmodule_t* CANmodule) {
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++) {
int 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) {
size_t 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++) {
int 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(void* CANptr) {
(void)CANptr;
/* Can't do anything because no reference to CANmodule_t is provided */
}
void
CO_CANsetNormalMode(CO_CANmodule_t* CANmodule) {
CO_ReturnError_t ret;
if (CANmodule != NULL) {
CANmodule->CANnormal = false;
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, void* CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[],
uint16_t txSize, uint16_t CANbitRate) {
int32_t ret;
uint16_t i;
(void)CANbitRate;
/* verify arguments */
if (CANmodule == NULL || CANptr == NULL || rxArray == NULL || txArray == NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
CO_CANptrSocketCan_t* CANptrReal = (CO_CANptrSocketCan_t*)CANptr;
/* Configure object variables */
CANmodule->epoll_fd = CANptrReal->epoll_fd;
CANmodule->CANinterfaces = NULL;
CANmodule->CANinterfaceCount = 0;
CANmodule->rxArray = rxArray;
CANmodule->rxSize = rxSize;
CANmodule->txArray = txArray;
CANmodule->txSize = txSize;
CANmodule->CANerrorStatus = 0;
CANmodule->CANnormal = false;
CANmodule->CANtxCount = 0;
#if CO_DRIVER_MULTI_INTERFACE > 0
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].CANrx_callback = NULL;
rxArray[i].can_ifindex = 0;
rxArray[i].timestamp.tv_sec = 0;
rxArray[i].timestamp.tv_nsec = 0;
}
#if CO_DRIVER_MULTI_INTERFACE == 0
/* add one interface */
ret = CO_CANmodule_addInterface(CANmodule, CANptrReal->can_ifindex);
if (ret != CO_ERROR_NO) {
CO_CANmodule_disable(CANmodule);
return ret;
}
#endif
return CO_ERROR_NO;
}
/* enable socketCAN */
#if CO_DRIVER_MULTI_INTERFACE == 0
static
#endif
CO_ReturnError_t
CO_CANmodule_addInterface(CO_CANmodule_t* CANmodule, int can_ifindex) {
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 = {0};
#if CO_DRIVER_ERROR_REPORTING > 0
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->can_ifindex = can_ifindex;
ifName = if_indextoname(can_ifindex, 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;
}
/* 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;
}
// 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 = can_ifindex;
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;
}
#if CO_DRIVER_ERROR_REPORTING > 0
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 /* CO_DRIVER_ERROR_REPORTING */
/* Add socket to epoll */
ev.events = EPOLLIN;
ev.data.fd = interface->fd;
ret = epoll_ctl(CANmodule->epoll_fd, 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;
if (CANmodule == NULL) {
return;
}
CANmodule->CANnormal = false;
/* clear interfaces */
for (i = 0; i < CANmodule->CANinterfaceCount; i++) {
CO_CANinterface_t* interface = &CANmodule->CANinterfaces[i];
#if CO_DRIVER_ERROR_REPORTING > 0
CO_CANerror_disable(&interface->errorhandler);
#endif
epoll_ctl(CANmodule->epoll_fd, EPOLL_CTL_DEL, interface->fd, NULL);
close(interface->fd);
interface->fd = -1;
}
CANmodule->CANinterfaceCount = 0;
if (CANmodule->CANinterfaces != NULL) {
free(CANmodule->CANinterfaces);
}
CANmodule->CANinterfaces = NULL;
if (CANmodule->rxFilter != NULL) {
free(CANmodule->rxFilter);
}
CANmodule->rxFilter = NULL;
}
CO_ReturnError_t
CO_CANrxBufferInit(CO_CANmodule_t* CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void* object,
void (*CANrx_callback)(void* object, void* message)) {
CO_ReturnError_t ret = CO_ERROR_NO;
if ((CANmodule != NULL) && (index < CANmodule->rxSize)) {
CO_CANrx_t* buffer;
/* buffer, which will be configured */
buffer = &CANmodule->rxArray[index];
#if CO_DRIVER_MULTI_INTERFACE > 0
CO_CANsetIdentToIndex(CANmodule->rxIdentToIndex, index, ident, buffer->ident);
#endif
/* Configure object variables */
buffer->object = object;
buffer->CANrx_callback = CANrx_callback;
buffer->can_ifindex = 0;
buffer->timestamp.tv_nsec = 0;
buffer->timestamp.tv_sec = 0;
/* 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 {
log_printf(LOG_DEBUG, DBG_CAN_RX_PARAM_FAILED, "illegal argument");
ret = CO_ERROR_ILLEGAL_ARGUMENT;
}
return ret;
}
#if CO_DRIVER_MULTI_INTERFACE > 0
bool_t
CO_CANrxBuffer_getInterface(CO_CANmodule_t* CANmodule, uint16_t ident, int* can_ifindexRx, struct timespec* timestamp) {
CO_CANrx_t* buffer;
if (CANmodule == NULL) {
return false;
}
const uint32_t index = CO_CANgetIndexFromIdent(CANmodule->rxIdentToIndex, ident);
if ((index == CO_INVALID_COB_ID) || (index > CANmodule->rxSize)) {
return false;
}
buffer = &CANmodule->rxArray[index];
/* return values */
if (can_ifindexRx != NULL) {
*can_ifindexRx = buffer->can_ifindex;
}
if (timestamp != NULL) {
*timestamp = buffer->timestamp;
}
if (buffer->can_ifindex != 0) {
return true;
} else {
return false;
}
}
#endif /* CO_DRIVER_MULTI_INTERFACE */
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 = NULL;
if ((CANmodule != NULL) && (index < CANmodule->txSize)) {
/* get specific buffer */
buffer = &CANmodule->txArray[index];
#if CO_DRIVER_MULTI_INTERFACE > 0
CO_CANsetIdentToIndex(CANmodule->txIdentToIndex, index, ident, buffer->ident);
#endif
buffer->can_ifindex = 0;
/* 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;
}
#if CO_DRIVER_MULTI_INTERFACE > 0
CO_ReturnError_t
CO_CANtxBuffer_setInterface(CO_CANmodule_t* CANmodule, uint16_t ident, int can_ifindexTx) {
if (CANmodule != NULL) {
uint32_t index;
index = CO_CANgetIndexFromIdent(CANmodule->txIdentToIndex, ident);
if ((index == CO_INVALID_COB_ID) || (index > CANmodule->txSize)) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
CANmodule->txArray[index].can_ifindex = can_ifindexTx;
return CO_ERROR_NO;
}
return CO_ERROR_ILLEGAL_ARGUMENT;
}
#endif /* CO_DRIVER_MULTI_INTERFACE */
#if CO_DRIVER_MULTI_INTERFACE > 0
/* send CAN message */
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;
#if CO_DRIVER_ERROR_REPORTING > 0
CO_CANinterfaceState_t ifState;
#endif
ssize_t n;
if (CANmodule == NULL || interface == NULL || interface->fd < 0) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
#if CO_DRIVER_ERROR_REPORTING > 0
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 */
#if CO_DRIVER_ERROR_REPORTING > 0
interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW;
#endif
return CO_ERROR_TX_BUSY;
} else if (n != CAN_MTU) {
break;
}
} while (errno != 0);
if (n != CAN_MTU) {
#if CO_DRIVER_ERROR_REPORTING > 0
interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW;
#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;
}
/*
* The same as #CO_CANsend(), but ensures that there is enough space remaining
* in the driver for more important messages.
*
* The default threshold is 50%, or at least 1 message buffer. If sending
* would violate those limits, #CO_ERROR_TX_OVERFLOW is returned and the
* message will not be sent.
*
* (It is not in header, because usage of CO_CANCheckSend() is unclear.)
*
* @param CANmodule This object.
* @param buffer Pointer to transmit buffer, returned by CO_CANtxBufferInit().
* Data bytes must be written in buffer before function call.
*
* @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_TX_OVERFLOW, CO_ERROR_TX_BUSY or
* CO_ERROR_TX_PDO_WINDOW (Synchronous TPDO is outside window).
*/
CO_ReturnError_t CO_CANCheckSend(CO_CANmodule_t* CANmodule, CO_CANtx_t* buffer);
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" */
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->can_ifindex == 0) || buffer->can_ifindex == interface->can_ifindex) {
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;
}
#warning CO_CANsend() is outdated for CO_DRIVER_MULTI_INTERFACE > 0
#endif /* CO_DRIVER_MULTI_INTERFACE > 0 */
#if CO_DRIVER_MULTI_INTERFACE == 0
/* Change handling of tx buffer full in CO_CANsend(). Use CO_CANtx_t->bufferFull
* flag. Re-transmit undelivered message inside CO_CANmodule_process(). */
CO_ReturnError_t
CO_CANsend(CO_CANmodule_t* CANmodule, CO_CANtx_t* buffer) {
CO_ReturnError_t err = CO_ERROR_NO;
if (CANmodule == NULL || buffer == NULL || CANmodule->CANinterfaceCount == 0) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
CO_CANinterface_t* interface = &CANmodule->CANinterfaces[0];
if (interface == NULL || interface->fd < 0) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* Verify overflow */
if (buffer->bufferFull) {
#if CO_DRIVER_ERROR_REPORTING > 0
interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW;
#endif
log_printf(LOG_ERR, DBG_CAN_TX_FAILED, buffer->ident, interface->ifName);
err = CO_ERROR_TX_OVERFLOW;
}
errno = 0;
ssize_t n = send(interface->fd, buffer, CAN_MTU, MSG_DONTWAIT);
if (errno == 0 && n == CAN_MTU) {
/* success */
if (buffer->bufferFull) {
buffer->bufferFull = false;
CANmodule->CANtxCount--;
}
} else if (errno == EINTR || errno == EAGAIN || errno == ENOBUFS) {
/* Send failed, message will be re-sent by CO_CANmodule_process() */
if (!buffer->bufferFull) {
buffer->bufferFull = true;
CANmodule->CANtxCount++;
}
err = CO_ERROR_TX_BUSY;
} else {
/* Unknown error */
log_printf(LOG_DEBUG, DBG_ERRNO, "send()");
#if CO_DRIVER_ERROR_REPORTING > 0
interface->errorhandler.CANerrorStatus |= CO_CAN_ERRTX_OVERFLOW;
#endif
err = CO_ERROR_SYSCALL;
}
return err;
}
#endif /* CO_DRIVER_MULTI_INTERFACE == 0 */
void
CO_CANclearPendingSyncPDOs(CO_CANmodule_t* CANmodule) {
(void)CANmodule;
/* Messages are either written to the socket queue or dropped */
}
void
CO_CANmodule_process(CO_CANmodule_t* CANmodule) {
if (CANmodule == NULL || CANmodule->CANinterfaceCount == 0) {
return;
}
#if CO_DRIVER_ERROR_REPORTING > 0
/* 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.
* Here we just copy evaluated CANerrorStatus from the first CAN interface. */
CANmodule->CANerrorStatus = CANmodule->CANinterfaces[0].errorhandler.CANerrorStatus;
#endif
#if CO_DRIVER_MULTI_INTERFACE == 0
/* recall CO_CANsend(), if message was unsent before */
if (CANmodule->CANtxCount > 0) {
bool_t found = false;
for (uint16_t i = 0; i < CANmodule->txSize; i++) {
CO_CANtx_t* buffer = &CANmodule->txArray[i];
if (buffer->bufferFull) {
buffer->bufferFull = false;
CANmodule->CANtxCount--;
CO_CANsend(CANmodule, buffer);
found = true;
break;
}
}
if (!found) {
CANmodule->CANtxCount = 0;
}
}
#endif /* CO_DRIVER_MULTI_INTERFACE == 0 */
}
/* Read CAN message from socket and verify some errors */
static CO_ReturnError_t
CO_CANread(CO_CANmodule_t* CANmodule, CO_CANinterface_t* interface,
struct can_frame* msg, /* CAN message, return value */
struct timespec* timestamp) /* timestamp of CAN message, return value */
{
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) {
#if CO_DRIVER_ERROR_REPORTING > 0
interface->errorhandler.CANerrorStatus |= CO_CAN_ERRRX_OVERFLOW;
#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) {
#if CO_DRIVER_ERROR_REPORTING > 0
interface->errorhandler.CANerrorStatus |= CO_CAN_ERRRX_OVERFLOW;
#endif
log_printf(LOG_ERR, CAN_RX_SOCKET_QUEUE_OVERFLOW, interface->ifName, dropped);
}
CANmodule->rxDropCount = dropped;
// todo use this info!
}
}
return CO_ERROR_NO;
}
/* find msg inside rxArray and call corresponding CANrx_callback */
static int32_t
CO_CANrxMsg( /* return index of received message in rxArray or -1 */
CO_CANmodule_t* CANmodule, struct can_frame* msg, /* CAN message input */
CO_CANrxMsg_t* buffer) /* If not NULL, msg will be copied to 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, including the 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->CANrx_callback != NULL)) {
rcvMsgObj->CANrx_callback(rcvMsgObj->object, (void*)rcvMsg);
}
/* return message */
if (buffer != NULL) {
memcpy(buffer, rcvMsg, sizeof(*buffer));
}
retval = index;
} else {
retval = -1;
}
return retval;
}
bool_t
CO_CANrxFromEpoll(CO_CANmodule_t* CANmodule, struct epoll_event* ev, CO_CANrxMsg_t* buffer, int32_t* msgIndex) {
if (CANmodule == NULL || ev == NULL || CANmodule->CANinterfaceCount == 0) {
return false;
}
/* Verify for epoll events in CAN socket */
for (uint32_t i = 0; i < CANmodule->CANinterfaceCount; i++) {
CO_CANinterface_t* interface = &CANmodule->CANinterfaces[i];
if (ev->data.fd == interface->fd) {
if ((ev->events & (EPOLLERR | EPOLLHUP)) != 0) {
struct can_frame msg;
/* epoll detected close/error on socket. Try to pull event */
errno = 0;
recv(ev->data.fd, &msg, sizeof(msg), MSG_DONTWAIT);
log_printf(LOG_DEBUG, DBG_CAN_RX_EPOLL, ev->events, strerror(errno));
} else if ((ev->events & EPOLLIN) != 0) {
struct can_frame msg;
struct timespec timestamp;
/* get message */
CO_ReturnError_t err = CO_CANread(CANmodule, interface, &msg, &timestamp);
if (err == CO_ERROR_NO && CANmodule->CANnormal) {
if (msg.can_id & CAN_ERR_FLAG) {
/* error msg */
#if CO_DRIVER_ERROR_REPORTING > 0
CO_CANerror_rxMsgError(&interface->errorhandler, &msg);
#endif
} else {
/* data msg */
#if CO_DRIVER_ERROR_REPORTING > 0
/* clear listenOnly and noackCounter if necessary */
CO_CANerror_rxMsg(&interface->errorhandler);
#endif
int32_t idx = CO_CANrxMsg(CANmodule, &msg, buffer);
if (idx > -1) {
/* Store message info */
CANmodule->rxArray[idx].timestamp = timestamp;
CANmodule->rxArray[idx].can_ifindex = interface->can_ifindex;
}
if (msgIndex != NULL) {
*msgIndex = idx;
}
}
}
} else {
log_printf(LOG_DEBUG, DBG_EPOLL_UNKNOWN, ev->events, ev->data.fd);
}
return true;
} /* if (ev->data.fd == interface->fd) */
}
return false;
}
View git blame Copy permalink