Merge branch 'cethcan'

10 files changed, 785 insertions, 0 deletions

diff --git a/cethcan/.gitignore b/cethcan/.gitignore
new file mode 100644
index 0000000..35ab3a7
--- /dev/null
+++ b/cethcan/.gitignore
@@ -0,0 +1,2 @@
+*.o
+cethcan
diff --git a/cethcan/Makefile b/cethcan/Makefile
new file mode 100644
index 0000000..a61d256
--- /dev/null
+++ b/cethcan/Makefile
@@ -0,0 +1,14 @@
+love: cethcan
+.PHONY: love
+
+PKGS="libevent jansson"
+
+cethcan: main.o can.o ether.o light.o http.o socketcan.o
+	gcc -g -o $@ `pkg-config --libs $(PKGS)` -lcrypto $^
+
+clean:
+	rm -f *.o *.y.c *.y.h *.l.c cethcan
+
+%.o: %.c *.h
+	gcc -c -g -O0 -Wall -Wextra -Wshadow -pedantic -Wno-unused-parameter -Wno-format -std=gnu99 `pkg-config --cflags $(PKGS)` -o $@ $<
+
diff --git a/cethcan/can.c b/cethcan/can.c
new file mode 100644
index 0000000..5145871
--- /dev/null
+++ b/cethcan/can.c
@@ -0,0 +1,65 @@
+#include "cethcan.h"
+
+static struct can_user *users = NULL, **userlast = &users;
+
+void can_register(struct can_user *user)
+{
+	user->next = NULL;
+	*userlast = user;
+	userlast = &user->next;
+}
+
+struct can_user *can_register_alloc(void *arg, can_handler handler,
+	const char *fmt, ...)
+{
+	struct can_user *user;
+	char *name = NULL;
+	va_list ap;
+
+	va_start(ap, fmt);
+	vasprintf(&name, fmt, ap);
+	va_end(ap);
+
+	user = calloc(sizeof(*user), 1);
+	user->arg = arg;
+	user->handler = handler;
+	user->name = name;
+
+	can_register(user);
+	return user;
+}
+
+void can_broadcast(struct can_user *origin, struct can_message *msg)
+{
+	struct can_user *u;
+	char buf[3*8+1];
+
+	msg->origin = origin;
+
+	if (msg->dlc > 8) {
+		lprintf("invalid CAN message (DLC = %zu)", msg->dlc);
+		return;
+	}
+	for (size_t i = 0; i < msg->dlc; i++)
+		sprintf(buf + 3 * i, " %02x", msg->bytes[i]);
+	lprintf("%s: %08x (%zu)%s", origin->name,
+		(unsigned)msg->daddr, msg->dlc, buf);
+
+	for (u = users; u; u = u->next)
+		if (u != origin)
+			u->handler(u->arg, msg);
+}
+
+void can_json(json_t *json, enum json_subtype type)
+{
+	struct can_user *u;
+	for (u = users; u; u = u->next)
+		if (u->json)
+			u->json(u->arg, json, type);
+}
+
+void can_init(void)
+{
+	/* nothing to do */
+}
+
diff --git a/cethcan/cethcan.h b/cethcan/cethcan.h
new file mode 100644
index 0000000..4326429
--- /dev/null
+++ b/cethcan/cethcan.h
@@ -0,0 +1,85 @@
+#ifndef _CETHCAN_H
+#define _CETHCAN_H
+
+#define _GNU_SOURCE
+
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <ctype.h>
+#include <sys/time.h>
+#include <sys/stat.h>
+#include <sys/mman.h>
+#include <sys/uio.h>
+#include <fcntl.h>
+#include <time.h>
+#include <assert.h>
+
+#include <event2/event.h>
+#include <jansson.h>
+
+#include "protocol.h"
+
+#define lprintf(...) do { \
+	struct timeval tv; struct tm tm; char tvbuf[64]; \
+	gettimeofday(&tv, NULL); localtime_r(&tv.tv_sec, &tm); \
+	strftime(tvbuf, sizeof(tvbuf), "%Y-%m-%d %H:%M:%S", &tm); \
+	fprintf(stderr, "%s.%03d ", tvbuf, tv.tv_usec / 1000); \
+	fprintf(stderr, __VA_ARGS__); \
+	fprintf(stderr, "\n"); \
+	} while (0)
+
+extern struct event_base *ev_base;
+
+struct can_user;
+
+struct can_message {
+	struct can_user *origin;
+
+	uint32_t daddr;
+
+	uint32_t flags;
+#define CAN_MSGF_RTR		(1 << 0)
+
+	size_t dlc;
+	uint8_t bytes[8];
+};
+
+enum json_subtype {
+	JSON_NORMAL = 0,
+	JSON_LONGPOLL = 1,
+};
+
+typedef void (*can_handler)(void *arg, struct can_message *msg);
+typedef void (*json_handler)(void *arg, json_t *json, enum json_subtype type);
+
+struct can_user {
+	struct can_user *next;
+
+	const char *name;
+
+	void *arg;
+	can_handler handler;
+	json_handler json;
+};
+
+extern void can_register(struct can_user *user);
+extern struct can_user *can_register_alloc(void *arg, can_handler handler,
+		const char *fmt, ...);
+extern void can_broadcast(struct can_user *origin, struct can_message *msg);
+extern void can_json(json_t *json, enum json_subtype type);
+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);
+
+#endif /* _CETHCAN_H */
diff --git a/cethcan/ether.c b/cethcan/ether.c
new file mode 100644
index 0000000..5b935ed
--- /dev/null
+++ b/cethcan/ether.c
@@ -0,0 +1,157 @@
+#include "cethcan.h"
+
+#include <net/if.h>
+#include <net/ethernet.h>
+#include <netpacket/packet.h>
+
+#define ETHER_PROTO 0x88b7
+#define ETHER_MCADDR 0xff, 0x3a, 0xf6, 'C', 'A', 'N'
+
+struct ether {
+	struct can_user *u;
+	struct event *ev;
+
+	char *ifname;
+	int ifindex;
+
+	int sock;
+};
+
+struct frame {
+	uint8_t daddr[ETH_ALEN];
+	uint8_t saddr[ETH_ALEN];
+	uint16_t proto;
+	uint8_t oui[3];
+	uint16_t protoid;
+	uint8_t protover;
+	uint32_t ts1, ts2;
+	uint32_t can_daddr;
+	uint8_t can_dlc;
+	uint8_t can_data[8];
+} __attribute__((packed));
+
+
+static void ether_can_handler(void *arg, struct can_message *msg)
+{
+	struct ether *e = arg;
+	lprintf("%s: TX not implemented", e->u->name);
+}
+
+static void ether_rx_dataframe(struct ether *e, struct frame *f)
+{
+	struct can_message msg;
+
+	msg.daddr = ntohl(f->can_daddr);
+	msg.dlc = f->can_dlc;
+	if (msg.dlc > 8)
+		msg.dlc = 8;
+	memcpy(msg.bytes, f->can_data, msg.dlc);
+
+	can_broadcast(e->u, &msg);
+}
+
+static void ether_sock_handler(int sock, short event, void *arg)
+{
+	struct ether *e = arg;
+	union {
+		struct sockaddr_ll ll;
+		struct sockaddr_storage ss;
+		struct sockaddr su;
+	} lladdr;
+	socklen_t addrlen = sizeof(lladdr);
+	union {
+		struct frame frame;
+		uint8_t raw[1536];
+	} buf;
+
+	if (event & EV_READ) {
+		ssize_t rlen = recvfrom(e->sock, &buf, sizeof(buf),
+			MSG_TRUNC, &lladdr.su, &addrlen);
+		if (rlen > 0) {
+			if (0) {
+				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);
+			}
+
+			if (ntohs(buf.frame.proto) == ETHER_PROTO
+				&& buf.frame.oui[0] == 0x00
+				&& buf.frame.oui[1] == 0x80
+				&& buf.frame.oui[2] == 0x41
+				&& ntohs(buf.frame.protoid) == 0xaaaa) {
+				switch (buf.frame.protover) {
+				case 3:
+					ether_rx_dataframe(e, &buf.frame);
+					break;
+				default:
+					lprintf("%s: unsupported CAN protocol version %d",
+						e->u->name, buf.frame.protover);
+				}
+			} else {
+				lprintf("%s: non-CAN frame (%zd bytes)",
+					e->u->name, rlen);
+			}
+		}
+	}
+}
+
+int ether_init(json_t *config)
+{
+	struct ether *e;
+	const char *iface;
+	int ifindex;
+	struct packet_mreq pmr = {
+		.mr_type = PACKET_MR_MULTICAST,
+		.mr_alen = ETH_ALEN,
+		.mr_address = { ETHER_MCADDR },
+	};
+
+	if (!json_is_object(config)) {
+		lprintf("ethernet config must be an object/dictionary");
+		return 1;
+	}
+	if (!json_is_string(json_object_get(config, "interface"))) {
+		lprintf("ethernet 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("ethernet interface '%s' error: %s",
+			iface, strerror(errno));
+		return 1;
+	}
+
+	e = calloc(sizeof(*e), 1);
+	e->ifname = strdup(iface);
+	e->ifindex = ifindex;
+
+	e->sock = socket(AF_PACKET, SOCK_RAW, htons(ETHER_PROTO));
+	if (e->sock == -1) {
+		lprintf("ethernet interface '%s' socket() error: %s",
+			iface, strerror(errno));
+		return 1;
+	}
+
+	pmr.mr_ifindex = ifindex;
+	if (setsockopt(e->sock, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
+		&pmr, sizeof(pmr))) {
+		lprintf("ethernet interface '%s' multicast join error: %s",
+			iface, strerror(errno));
+		return 1;
+	}
+
+	e->u = can_register_alloc(e, ether_can_handler, "ether[%s]", iface);
+	e->ev = event_new(ev_base, e->sock, EV_READ | EV_PERSIST, ether_sock_handler, e);
+	event_add(e->ev, NULL);
+
+	return 0;
+}
diff --git a/cethcan/http.c b/cethcan/http.c
new file mode 100644
index 0000000..8316abc
--- /dev/null
+++ b/cethcan/http.c
@@ -0,0 +1,143 @@
+#include "cethcan.h"
+
+#include <event2/http.h>
+#include <event2/buffer.h>
+#include <openssl/sha.h>
+
+static struct evhttp *evhttp;
+
+static int evb_json_add(const char *data, size_t size, void *arg)
+{
+	struct evbuffer *buf = arg;
+	return evbuffer_add(buf, data, size);
+}
+
+static void http_json_basic(struct evhttp_request *req, void *arg)
+{
+	struct evkeyvalq *outhdr = evhttp_request_get_output_headers(req);
+	struct evbuffer *out = evbuffer_new();
+
+	evhttp_add_header(outhdr, "Content-Type", "application/json; charset=utf-8");
+
+	json_t *jsout = json_object();
+	can_json(jsout, JSON_NORMAL);
+	json_dump_callback(jsout, evb_json_add, out, JSON_SORT_KEYS | JSON_INDENT(4));
+	json_decref(jsout);
+
+	evhttp_send_reply(req, 200, "OK", out);
+	evbuffer_free(out);
+}
+
+static void http_json_bump(struct evhttp_request *req, void *arg)
+{
+	struct evkeyvalq *outhdr = evhttp_request_get_output_headers(req);
+	struct evbuffer *out = evbuffer_new();
+
+	evhttp_add_header(outhdr, "Content-Type", "text/plain; charset=utf-8");
+	evbuffer_add_printf(out, "OK");
+	evhttp_send_reply(req, 200, "OK", out);
+	evbuffer_free(out);
+
+	json_bump_longpoll();
+}
+
+struct longpoll {
+	struct longpoll *next;
+	struct evhttp_request *req;
+};
+static struct longpoll *longpolls = NULL, **plongpoll = &longpolls;
+static size_t longpoll_count = 0;
+
+static char *longpollbuf = NULL;
+static size_t longpollbuflen = 0;
+static char longpollhash[SHA_DIGEST_LENGTH * 2 + 1] = "";
+
+static void http_json_longpoll(struct evhttp_request *req, void *arg)
+{
+	struct evkeyvalq *outhdr = evhttp_request_get_output_headers(req);
+	struct evbuffer *out;
+	struct longpoll *lp;
+	const char *query;
+	
+	evhttp_add_header(outhdr, "Content-Type", "application/json; charset=utf-8");
+
+	query = evhttp_uri_get_query(evhttp_request_get_evhttp_uri(req));
+	if (query && !strcmp(query, longpollhash)) {
+		lprintf("long poll");
+		evhttp_send_reply_start(req, 200, "OK Long Poll");
+		evhttp_request_own(req);
+
+		lp = calloc(sizeof(*lp), 1);
+		lp->req = req;
+		*plongpoll = lp;
+		plongpoll = &lp->next;
+		longpoll_count++;
+		return;
+	}
+
+	out = evbuffer_new();
+	evbuffer_add(out, longpollbuf, longpollbuflen);
+	evhttp_send_reply(req, 200, "OK Short Poll", out);
+	evbuffer_free(out);
+}
+
+static void longpoll_updatedata(void)
+{
+	json_t *jsout, *jswrap;
+	char *data;
+	unsigned char digest[SHA_DIGEST_LENGTH];
+
+	if (longpollbuf)
+		free(longpollbuf);
+	
+	jsout = json_object();
+	can_json(jsout, JSON_LONGPOLL);
+	data = json_dumps(jsout, JSON_SORT_KEYS | JSON_INDENT(4));
+	SHA1((unsigned char *)data, strlen(data), digest);
+	free(data);
+
+	for (size_t i = 0; i < SHA_DIGEST_LENGTH; i++)
+		sprintf(longpollhash + i * 2, "%02x", digest[i]);
+
+	jswrap = json_object();
+	json_object_set_new(jswrap, "data", jsout);
+	json_object_set_new(jswrap, "ref", json_string(longpollhash));
+	longpollbuf = json_dumps(jswrap, JSON_SORT_KEYS | JSON_INDENT(4));
+	longpollbuflen = strlen(longpollbuf);
+	json_decref(jswrap);
+}
+
+void json_bump_longpoll(void)
+{
+	struct longpoll *lp, *lpnext;
+	struct evbuffer *out;
+
+	longpoll_updatedata();
+
+	out = evbuffer_new();
+	for (lp = longpolls; lp; lp = lpnext) {
+		lpnext = lp->next;
+
+		evbuffer_add(out, longpollbuf, longpollbuflen);
+		evhttp_send_reply_chunk(lp->req, out);
+		/* send_reply_end calls request_free() */
+		evhttp_send_reply_end(lp->req);
+		free(lp);
+	}
+	evbuffer_free(out);
+
+	longpolls = NULL;
+	plongpoll = &longpolls;
+	longpoll_count = 0;
+}
+
+void http_init(void)
+{
+	evhttp = evhttp_new(ev_base);
+	evhttp_set_cb(evhttp, "/", http_json_basic, NULL);
+	evhttp_set_cb(evhttp, "/longpoll", http_json_longpoll, NULL);
+	evhttp_set_cb(evhttp, "/bump", http_json_bump, NULL);
+	evhttp_bind_socket(evhttp, "127.0.0.1", 34999);
+
+	longpoll_updatedata();
+}
diff --git a/cethcan/light.c b/cethcan/light.c
new file mode 100644
index 0000000..cd646d8
--- /dev/null
+++ b/cethcan/light.c
@@ -0,0 +1,93 @@
+#include "cethcan.h"
+
+struct value {
+	uint8_t val;
+	time_t change, valid;
+};
+
+struct light {
+	struct can_user *u;
+
+	char *name;
+	unsigned logical_addr;
+
+	struct value set, actual;
+};
+
+static void light_json_handler(void *arg, json_t *json, enum json_subtype type)
+{
+	struct light *l = arg;
+	json_t *lobj = json_object();
+
+	json_object_set_new(lobj, "klass", json_string("light"));
+	json_object_set_new(lobj, "addr", json_integer(l->logical_addr));
+
+	json_object_set_new(lobj, "actual", json_integer(l->actual.val));
+	if (type != JSON_LONGPOLL)
+		json_object_set_new(lobj, "actual_ts", json_integer(l->actual.valid));
+	json_object_set_new(lobj, "actual_tschg", json_integer(l->actual.change));
+
+	json_object_set_new(lobj, "set", json_integer(l->set.val));
+	if (type != JSON_LONGPOLL)
+		json_object_set_new(lobj, "set_ts", json_integer(l->set.valid));
+	json_object_set_new(lobj, "set_tschg", json_integer(l->set.change));
+
+	json_object_set_new(json, l->name, lobj);
+}
+
+static void light_can_handler(void *arg, struct can_message *msg)
+{
+	struct light *l = arg;
+	struct value *v = NULL;
+	unsigned laddr;
+	uint8_t dval;
+
+	if ((msg->daddr & CANA_PROTOCOL) == CANA_LIGHT)
+		v = &l->set;
+	if ((msg->daddr & CANA_PROTOCOL) == CANA_SENSOR)
+		v = &l->actual;
+	if (!v)
+		return;
+
+	laddr = msg->daddr & 0xfff;
+	if (l->logical_addr < laddr)
+		return;
+	if (l->logical_addr - laddr >= msg->dlc)
+		return;
+	dval = msg->bytes[l->logical_addr - laddr];
+
+	time(&v->valid);
+	if (dval != v->val || v->change == 0) {
+		v->val = dval;
+		time(&v->change);
+		lprintf("%s: set %02x", l->u->name, dval);
+
+		json_bump_longpoll();
+	}
+}
+
+int light_init_conf(json_t *config)
+{
+	struct light *l;
+
+	if (!json_is_object(config)) {
+		lprintf("light config must be an object/dictionary");
+		return 1;
+	}
+	if (!json_is_integer(json_object_get(config, "addr"))) {
+		lprintf("light config must have an 'addr' key");
+		return 1;
+	}
+	if (!json_is_string(json_object_get(config, "name"))) {
+		lprintf("light config must have a 'name' key");
+		return 1;
+	}
+	
+	l = calloc(sizeof(*l), 1);
+	l->name = strdup(json_string_value(json_object_get(config, "name")));
+	l->logical_addr = json_integer_value(json_object_get(config, "addr"));
+
+	l->u = can_register_alloc(l, light_can_handler, "light[%s]", l->name);
+	l->u->json = light_json_handler;
+	return 0;
+}
diff --git a/cethcan/main.c b/cethcan/main.c
new file mode 100644
index 0000000..5bdca0a
--- /dev/null
+++ b/cethcan/main.c
@@ -0,0 +1,68 @@
+#include "cethcan.h"
+
+struct event_base *ev_base;
+
+int main(int argc, char **argv)
+{
+	int optch = 0;
+	const char *cfgfile = "cethcan.json";
+	json_error_t je;
+	json_t *config, *ethercfg, *lightcfg, *socancfg;
+
+	do {
+		optch = getopt(argc, argv, "c:");
+		switch (optch) {
+		case 'c':
+			cfgfile = optarg;
+			break;
+		case -1:
+			break;
+		}
+	} while (optch != -1);
+
+	if (optind < argc) {
+		fprintf(stderr, "leftover arguments\n");
+		return 1;
+	}
+
+	config = json_load_file(cfgfile, JSON_REJECT_DUPLICATES, &je);
+	if (!config) {
+		fprintf(stderr, "failed to load config:\n%s:%d:%d %s\n",
+			je.source, je.line, je.column, je.text);
+		return 1;
+	}
+	if (!json_is_object(config)) {
+		fprintf(stderr, "config must be object/dictionary\n");
+		return 1;
+	}
+
+	ev_base = event_base_new();
+
+	can_init();
+
+	ethercfg = json_object_get(config, "ethernet");
+	for (size_t i = 0; i < json_array_size(ethercfg); i++) {
+		json_t *c = json_array_get(ethercfg, i);
+		if (ether_init(c))
+			return 1;
+	}
+
+	lightcfg = json_object_get(config, "lights");
+	for (size_t i = 0; i < json_array_size(lightcfg); i++) {
+		json_t *c = json_array_get(lightcfg, i);
+		if (light_init_conf(c))
+			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);
+	return 0;
+}
diff --git a/cethcan/protocol.h b/cethcan/protocol.h
new file mode 100644
index 0000000..34327d2
--- /dev/null
+++ b/cethcan/protocol.h
@@ -0,0 +1,24 @@
+			/* addrs: SSSEEEEE
+			 *           8 -> extended addr
+			 *          14 -> unused
+			 */
+#define CANA_TIME		0x10080000
+#define CANA_DISCOVERY		0x4c080000
+#define CANA_DISCOVERY_F(pg,dst) (CANA_DISCOVERY | (((uint32_t)(pg) & 0xf) << 12) | ((dst) & 0xfff))
+#define CANA_LIGHT		0xcc080000
+#define CANA_LIGHT_F(src,dst)	(CANA_LIGHT | (((src) & 0x3f) << 12) | ((dst) & 0xfff))
+#define CANA_SENSOR		0xe6080000
+#define CANA_SENSOR_F(src)	(CANA_SENSOR | ((src) & 0xfff))
+#define CANA_DEBUG		0xe7000000
+
+#define CANA_PROTOCOL		0xffe80000
+
+/*
+#define can_rx_isext()		(can_rx_addr.b[1] & 0x08)
+#define can_rx_ext_rr()		(can_rx_dlc & 0x40)
+#define can_rx_len()		(can_rx_dlc & 0x0f)
+
+#define can_rx_sublab_proto()	((can_rx_addr.b[0] << 8) | (can_rx_addr.b[1] & 0xe8))
+#define can_rx_sublab_addr()	(((can_rx_addr.b[2] & 0x0f) << 8) | can_rx_addr.b[3])
+#define can_rx_sublab_disco_page()	((can_rx_addr.b[2] & 0xf0) >> 4)
+*/
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;
+}