socketcan v0

4 files changed, 144 insertions, 2 deletions

diff --git a/cethcan/Makefile b/cethcan/Makefile
index ca4a433..a61d256 100644
--- a/cethcan/Makefile
+++ b/cethcan/Makefile
@@ -3,7 +3,7 @@ love: cethcan
 
 PKGS="libevent jansson"
 
-cethcan: main.o can.o ether.o light.o http.o
+cethcan: main.o can.o ether.o light.o http.o socketcan.o
 	gcc -g -o $@ `pkg-config --libs $(PKGS)` -lcrypto $^
 
 clean:
diff --git a/cethcan/cethcan.h b/cethcan/cethcan.h
index fd821fc..4326429 100644
--- a/cethcan/cethcan.h
+++ b/cethcan/cethcan.h
@@ -77,6 +77,7 @@ extern void can_init(void);
 
 extern void json_bump_longpoll(void);
 
+extern int socan_init(json_t *config);
 extern int ether_init(json_t *config);
 extern int light_init_conf(json_t *config);
 extern void http_init(void);
diff --git a/cethcan/main.c b/cethcan/main.c
index d6c5bbf..5bdca0a 100644
--- a/cethcan/main.c
+++ b/cethcan/main.c
@@ -7,7 +7,7 @@ int main(int argc, char **argv)
 	int optch = 0;
 	const char *cfgfile = "cethcan.json";
 	json_error_t je;
-	json_t *config, *ethercfg, *lightcfg;
+	json_t *config, *ethercfg, *lightcfg, *socancfg;
 
 	do {
 		optch = getopt(argc, argv, "c:");
@@ -54,6 +54,13 @@ int main(int argc, char **argv)
 			return 1;
 	}
 
+	socancfg = json_object_get(config, "socketcan");
+	for (size_t i = 0; i < json_array_size(socancfg); i++) {
+		json_t *c = json_array_get(socancfg, i);
+		if (socan_init(c))
+			return 1;
+	}
+
 	http_init();
 
 	event_base_loop(ev_base, 0);
diff --git a/cethcan/socketcan.c b/cethcan/socketcan.c
new file mode 100644
index 0000000..d73ed45
--- /dev/null
+++ b/cethcan/socketcan.c
@@ -0,0 +1,134 @@
+#include "cethcan.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>
+
+struct socan {
+	struct can_user *u;
+	struct event *ev;
+
+	char *ifname;
+	int ifindex;
+
+	int sock;
+};
+
+static void socan_handler(void *arg, struct can_message *msg)
+{
+	struct socan *sc = arg;
+	lprintf("%s: TX not implemented", sc->u->name);
+}
+
+static void socan_event(int sock, short event, void *arg)
+{
+	struct socan *sc = arg;
+	struct can_message msg;
+	union {
+		struct sockaddr_can can;
+		struct sockaddr_storage ss;
+		struct sockaddr su;
+	} canaddr;
+	socklen_t addrlen = sizeof(canaddr);
+	struct canfd_frame frame;
+
+	if (event & EV_READ) {
+		ssize_t rlen = recvfrom(sc->sock, &frame, sizeof(frame),
+			MSG_TRUNC, &canaddr.su, &addrlen);
+		if (rlen > 0) {
+#if 0
+			if (1) {
+				lprintf("got %zd bytes", rlen);
+				char pbuffer[16 * 3 + 1];
+				for (size_t i = 0; i < (size_t)rlen; i++) {
+					size_t j = i % 16;
+					sprintf(pbuffer + (3 * j), " %02x", buf.raw[i]);
+					if (j == 15)
+						lprintf(">>%s", pbuffer);
+				}
+				if ((rlen % 16) != 15)
+					lprintf(">>%s", pbuffer);
+			}
+#endif
+			if (frame.can_id & CAN_EFF_FLAG)
+				msg.daddr = 0x00080000
+					| (frame.can_id & 0x0003ffff)
+					| ((frame.can_id & 0x1ffc0000) << 3);
+			else
+				msg.daddr = (frame.can_id & 0x7ff) << 21;
+			msg.dlc = frame.len;
+			if (msg.dlc > 8)
+				msg.dlc = 8;
+			memcpy(msg.bytes, frame.data, msg.dlc);
+			can_broadcast(sc->u, &msg);
+		}
+	}
+}
+
+int socan_init(json_t *config)
+{
+	struct socan *sc;
+	const char *iface;
+	int ifindex;
+	int on = 1;
+	struct sockaddr_can addr;
+
+	if (!json_is_object(config)) {
+		lprintf("socketcan config must be an object/dictionary");
+		return 1;
+	}
+	if (!json_is_string(json_object_get(config, "interface"))) {
+		lprintf("socketcan config must have an 'interface' key");
+		return 1;
+	}
+	iface = json_string_value(json_object_get(config, "interface"));
+	ifindex = if_nametoindex(iface);
+
+	if (ifindex == 0) {
+		lprintf("socketcan interface '%s' error: %s",
+			iface, strerror(errno));
+		return 1;
+	}
+
+	sc = calloc(sizeof(*sc), 1);
+	sc->ifname = strdup(iface);
+	sc->ifindex = ifindex;
+
+	sc->sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+	if (sc->sock == -1) {
+		lprintf("socketcan interface '%s' socket() error: %s",
+			iface, strerror(errno));
+		free(sc);
+		return 1;
+	}
+	if (setsockopt(sc->sock, SOL_CAN_RAW, CAN_RAW_FD_FRAMES,
+		&on, sizeof(on))) {
+		lprintf("socketcan interface '%s' raw_fd_frames error: %s",
+			iface, strerror(errno));
+		return 1;
+	}
+/*	if (setsockopt(sc->sock, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on))) {
+		lprintf("socketcan interface '%s' so_timestamp error: %s",
+			iface, strerror(errno));
+		return 1;
+	} */
+
+	addr.can_family = AF_CAN;
+	addr.can_ifindex = ifindex;
+	if (bind(sc->sock, (struct sockaddr *)&addr, sizeof(addr))) {
+		lprintf("socketcan interface '%s' bind error: %s",
+			iface, strerror(errno));
+		return 1;
+	}
+
+	sc->u = can_register_alloc(sc, socan_handler, "socketcan[%s]", iface);
+	sc->ev = event_new(ev_base, sc->sock, EV_READ | EV_PERSIST,
+		socan_event, sc);
+	event_add(sc->ev, NULL);
+
+	return 0;
+}