117 lines
2.6 KiB
C++
117 lines
2.6 KiB
C++
#include "socketcan.h"
|
|
#include "libdi-php.h"
|
|
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
|
|
#include <unistd.h>
|
|
#include <fcntl.h>
|
|
#include <sys/types.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/ioctl.h>
|
|
#include <net/if.h>
|
|
|
|
#include <linux/can.h>
|
|
#include <linux/can/raw.h>
|
|
|
|
#define SOCKETCAN_SEND_RETRIES 10U
|
|
#define SOCKETCAN_SEND_RETRY_TIME_MS 1U
|
|
#define SOCKETCAN_SELECT_TIMEOUT_US 1000
|
|
|
|
/**
|
|
* Opens the connection to the CAN-bus.
|
|
* @param iface Can interface name, e.g "can0"
|
|
*/
|
|
int socketcan_open(const char *iface) {
|
|
int fd;
|
|
struct ifreq ifr;
|
|
struct sockaddr_can addr;
|
|
|
|
fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
|
if (fd < 0)
|
|
return -1;
|
|
|
|
memset(&ifr, 0, sizeof(ifr));
|
|
memset(&addr, 0, sizeof(addr));
|
|
|
|
addr.can_family = AF_CAN;
|
|
snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", iface);
|
|
if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0)
|
|
return -1;
|
|
|
|
addr.can_ifindex = ifr.ifr_ifindex;
|
|
if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
|
|
return -1;
|
|
|
|
return fd;
|
|
}
|
|
|
|
/**
|
|
* CAN send null callback
|
|
*/
|
|
di_errno_t socketcan_send_cb(const struct di_can_frame_tx *frame) {
|
|
ssize_t ret;
|
|
struct can_frame f;
|
|
size_t tries = SOCKETCAN_SEND_RETRIES;
|
|
php_di_can_ctx *ctx = static_cast<php_di_can_ctx *>(di_can_frame_tx_get_private_data(frame));
|
|
|
|
memset(&f, 0, sizeof(f));
|
|
|
|
f.can_id = frame->canid & CAN_EFF_MASK;
|
|
f.can_id |= CAN_EFF_FLAG;
|
|
|
|
/* Set SocketCAN frame DLC to DI-Net CAN frame and copy data */
|
|
f.can_dlc = (uint8_t)frame->size;
|
|
memcpy(f.data, frame->buf, frame->size);
|
|
|
|
while (tries) {
|
|
ret = write(ctx->fd, &f, sizeof(struct can_frame));
|
|
if (ret == sizeof(struct can_frame)) {
|
|
break;
|
|
}
|
|
|
|
// Errors other than no buffer space should stop sending the can message
|
|
if (errno != ENOBUFS)
|
|
return DNE_CAN_IO;
|
|
|
|
// Sleep wait for socketcan to free up a few buffers
|
|
usleep(SOCKETCAN_SEND_RETRY_TIME_MS * 1000);
|
|
tries--;
|
|
if (tries == 0) {
|
|
return DNE_CAN_IO;
|
|
}
|
|
}
|
|
|
|
return DNOK;
|
|
}
|
|
|
|
/**
|
|
* CAN recv null callback
|
|
*/
|
|
di_errno_t socketcan_recv_cb(struct di_can_frame_rx *frame) {
|
|
ssize_t n;
|
|
struct can_frame f;
|
|
struct timeval timeout = {0, SOCKETCAN_SELECT_TIMEOUT_US};
|
|
php_di_can_ctx *c = static_cast<php_di_can_ctx *>(di_can_frame_rx_get_private_data(frame));
|
|
fd_set readSet;
|
|
FD_ZERO(&readSet);
|
|
FD_SET(c->fd, &readSet);
|
|
|
|
if (select((c->fd + 1), &readSet, NULL, NULL, &timeout) >= 0) {
|
|
if (FD_ISSET(c->fd, &readSet)) {
|
|
n = read(c->fd, &f, sizeof(struct can_frame));
|
|
if (n == sizeof(struct can_frame)) {
|
|
frame->canid = f.can_id & CAN_EFF_MASK;
|
|
frame->size = f.can_dlc;
|
|
memcpy(frame->buf, f.data, frame->size);
|
|
} else {
|
|
return DNE_CAN_IO;
|
|
}
|
|
} else {
|
|
return DNE_CAN_IO;
|
|
}
|
|
}
|
|
|
|
return DNOK;
|
|
}
|