1 files changed, 106 insertions, 53 deletions

diff --git a/iotbzh-CAN-binding.c b/iotbzh-CAN-binding.c
index e96796c0..50e88ef3 100644
--- a/iotbzh-CAN-binding.c
+++ b/iotbzh-CAN-binding.c
@@ -25,6 +25,7 @@
 #include <net/if.h>
 #include <sys/time.h>
 #include <linux/can.h>
+#include <linux/can/raw.h>
 #include <math.h>
 #include <fcntl.h>
 #include <systemd/sd-event.h>
@@ -57,6 +58,8 @@
  /* CAN FD ASCII hex short representation with DATA_SEPERATORs */
 #define CL_CFSZ (2*CL_ID + 64*CL_DATA)
 
+#define CANID_DELIM '#'
+
 /*
  * the type of position expected
  *
@@ -82,20 +85,21 @@ struct event {
 	int id;			/* id of the event for unsubscribe */
 };
 
+/* CAN variable initialization */
+
+static __u32 dropcnt[MAXSOCK];
+static __u32 last_dropcnt[MAXSOCK];
+struct timeval tv;
+struct canfd_frame canfd_frame;
+
 struct can_handler {
 	int socket;
 	char *device;
+	openxc_CanMessage *msg;
 	struct sockaddr_can txAddress;
 };
 
-static __u32 dropcnt[MAXSOCK];
-static __u32 last_dropcnt[MAXSOCK];
-char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))];
-struct timeval tv;
-struct iovec iov;
-struct msghdr msg;
-struct cmsghdr *cmsg;
-struct canfd_frame can_frame;
+
 
 /*****************************************************************************************/
 /*****************************************************************************************/
@@ -144,6 +148,25 @@ static int retry( int(*func)())
 /**											**/
 /*****************************************************************************************/
 /*****************************************************************************************/
+const char hex_asc_upper[] = "0123456789ABCDEF";
+
+#define hex_asc_upper_lo(x) hex_asc_upper[((x) & 0x0F)]
+#define hex_asc_upper_hi(x) hex_asc_upper[((x) & 0xF0) >> 4]
+
+static inline void _put_id(char *buf, int end_offset, canid_t id)
+{
+	/* build 3 (SFF) or 8 (EFF) digit CAN identifier */
+	while (end_offset >= 0) {
+		buf[end_offset--] = hex_asc_upper[id & 0xF];
+		id >>= 4;
+	}
+}
+
+#define put_sff_id(buf, id) _put_id(buf, 2, id)
+#define put_eff_id(buf, id) _put_id(buf, 7, id)
+
+static int canread_frame_parse(openxc_CanMessage *can_message, struct canfd_frame *canfd_frame, int maxdlen);
+
 /*
  * names of the types
  */
@@ -156,26 +179,19 @@ static const char * const type_NAMES[type_size] = {
 // Initialize default can_handler values
 static struct can_handler can_handler = {
 	.socket = -1,
-	.device = "vcan0"
+	.device = "vcan0",
 };
 
-/*
- * Parse the CAN frame data payload as a CAN packet
- * TODO: parse as an OpenXC Can Message
- */
-int can_frame_parse(openxc_CanMessage *can_message, struct can_frame *can_frame)
-{
-
-}
-
 
 /*
  * open the can socket
  */
 static int open_can_dev()
 {
+	const int canfd_on = 1;
 	struct ifreq ifr;
 	struct timeval timeout = {1,0};
+	openxc_CanMessage *can_msg;
 
 	DEBUG(interface, "open_can_dev: CAN Handler socket : %d", can_handler.socket);
 	close(can_handler.socket);
@@ -187,9 +203,19 @@ static int open_can_dev()
 	}
 	else
 	{
-		// Set timeout for read
+		/* Set timeout for read */
 		setsockopt(can_handler.socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
-		// Attempts to open a socket to CAN bus
+		/* try to switch the socket into CAN_FD mode */
+		can_msg->has_frame_format = true;
+		if (setsockopt(can_handler.socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0)
+		{
+			NOTICE(interface, "open_can_dev: Can not switch into CAN Extended frame format.");
+			can_msg->frame_format = openxc_CanMessage_FrameFormat_STANDARD;
+		} else {
+			can_msg->frame_format = openxc_CanMessage_FrameFormat_EXTENDED;
+		}
+
+		/* Attempts to open a socket to CAN bus */
 		strcpy(ifr.ifr_name, can_handler.device);
 		if(ioctl(can_handler.socket, SIOCGIFINDEX, &ifr) < 0)
 		{
@@ -200,7 +226,7 @@ static int open_can_dev()
 			can_handler.txAddress.can_family = AF_CAN;
 			can_handler.txAddress.can_ifindex = ifr.ifr_ifindex;
 
-			// And bind it to txAddress
+			/* And bind it to txAddress */
 			if (bind(can_handler.socket, (struct sockaddr *)&can_handler.txAddress, sizeof(can_handler.txAddress)) < 0)
 			{
 				ERROR(interface, "open_can_dev: bind failed");
@@ -208,6 +234,7 @@ static int open_can_dev()
 			else
 			{
 				fcntl(can_handler.socket, F_SETFL, O_NONBLOCK);
+				can_handler.msg = can_msg;
 				return 0;
 			}
 		}
@@ -227,7 +254,7 @@ static int write_can()
 /*
  * TODO change old hvac write can frame to generic on_event
  */
-		rc = sendto(can_handler.socket, &can_frame, sizeof(struct can_frame), 0,
+		rc = sendto(can_handler.socket, &canfd_frame, sizeof(struct canfd_frame), 0,
 			    (struct sockaddr*)&can_handler.txAddress, sizeof(can_handler.txAddress));
 		if (rc < 0)
 		{
@@ -245,18 +272,13 @@ static int write_can()
 /*
  * Read on CAN bus and return how much bytes has been read.
  */
-static int read_can(openxc_CanMessage *can_message)
+static int read_can()
 {
-	int byte_read, maxdlen;
-	iov.iov_base = &can_frame;
-	msg.msg_name = &can_handler.txAddress;
-	msg.msg_iov = &iov;
-	msg.msg_iovlen = 1;
-	msg.msg_control = &ctrlmsg;
+	int bytes_read, maxdlen;
 
-	byte_read = recvmsg(can_handler.socket, &msg, 0);
+	bytes_read = read(can_handler.socket, &canfd_frame, sizeof(struct canfd_frame));
 
-	if (byte_read < 0)
+	if (bytes_read < 0)
 	{
 		if (errno == ENETDOWN) {
 			ERROR(interface, "%s: interface down", can_handler.device);
@@ -265,10 +287,10 @@ static int read_can(openxc_CanMessage *can_message)
 		return -1;
 	}
 
-	// CAN frame integrity check
-	if ((size_t)byte_read == CAN_MTU)
+	/* CAN frame integrity check */
+	if ((size_t)bytes_read == CAN_MTU)
 		maxdlen = CAN_MAX_DLEN;
-	else if ((size_t)byte_read == CANFD_MTU)
+	else if ((size_t)bytes_read == CANFD_MTU)
 		maxdlen = CANFD_MAX_DLEN;
 	else
 	{
@@ -276,29 +298,60 @@ static int read_can(openxc_CanMessage *can_message)
 		return -2;
 	}
 
-	for (	cmsg = CMSG_FIRSTHDR(&msg);
-		cmsg && (cmsg->cmsg_level == SOL_SOCKET);
-		cmsg = CMSG_NXTHDR(&msg,cmsg))
-	{
-		if (cmsg->cmsg_type == SO_TIMESTAMP)
-			tv = *(struct timeval *)CMSG_DATA(cmsg);
-			else if (cmsg->cmsg_type == SO_RXQ_OVFL)
-				dropcnt[can_handler.socket] = *(__u32 *)CMSG_DATA(cmsg);
+	canread_frame_parse(can_message, &canfd_frame, maxdlen);
+}
+
+/*
+ * Parse the CAN frame data payload as a CAN packet
+ * TODO: parse as an OpenXC Can Message
+ */
+static int canread_frame_parse(openxc_CanMessage *can_message, struct canfd_frame *canfd_frame, int maxdlen)
+{
+	int i,offset;
+	int len = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
+
+	if (canfd_frame->can_id & CAN_ERR_FLAG) {
+		put_eff_id(buf, canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
+		buf[8] = '#';
+		offset = 9;
+	} else if (canfd_frame->can_id & CAN_EFF_FLAG) {
+		put_eff_id(buf, canfd_frame->can_id & CAN_EFF_MASK);
+		buf[8] = '#';
+		offset = 9;
+	} else {
+		put_sff_id(buf, canfd_frame->can_id & CAN_SFF_MASK);
+		buf[3] = '#';
+		offset = 4;
 	}
 
-	// Check if there is a new CAN frame dropped.
-	if (dropcnt[can_handler.socket] != last_dropcnt[can_handler.socket])
-	{
-		__u32 frames = dropcnt[can_handler.socket] - last_dropcnt[can_handler.socket];
-		WARNING(interface, "DROPCOUNT: dropped %d CAN frame%s on '%s' socket (total drops %d)",
-			frames, (frames > 1)?"s":"", can_handler.device, dropcnt[can_handler.socket]);
-			if (log)
-				WARNING(interface, "DROPCOUNT: dropped %d CAN frame%s on '%s' socket (total drops %d)\n",
-					frames, (frames > 1)?"s":"", can_handler.device, dropcnt[can_handler.socket]);
-
-			last_dropcnt[can_handler.socket] = dropcnt[can_handler.socket];
+	/* standard CAN frames may have RTR enabled. There are no ERR frames with RTR */
+	if (maxdlen == CAN_MAX_DLEN && canfd_frame->can_id & CAN_RTR_FLAG) {
+		buf[offset++] = 'R';
+		/* print a given CAN 2.0B DLC if it's not zero */
+		if (canfd_frame->len && canfd_frame->len <= CAN_MAX_DLC)
+			buf[offset++] = hex_asc_upper[canfd_frame->len & 0xF];
+
+		buf[offset] = 0;
+		return;
 	}
 
+	if (maxdlen == CANFD_MAX_DLEN) {
+		/* add CAN FD specific escape char and flags */
+		buf[offset++] = '#';
+		buf[offset++] = hex_asc_upper[canfd_frame->flags & 0xF];
+		if (sep && len)
+			buf[offset++] = '.';
+	}
+
+	for (i = 0; i < len; i++) {
+		put_hex_byte(buf + offset, canfd_frame->data[i]);
+		offset += 2;
+		if (sep && (i+1 < len))
+			buf[offset++] = '.';
+	}
+
+buf[offset] = 0;
+
 	can_message->has_id = true;
 	can_message->id = msg.id; // TODO make the parsing to extract id from data and only return left data into msg.msg_iov
 	can_message->has_data = true;