cethcan: more code

5 files changed, 110 insertions, 3 deletions

diff --git a/cethcan/Makefile b/cethcan/Makefile
index 33b2069..a81124c 100644
--- a/cethcan/Makefile
+++ b/cethcan/Makefile
@@ -3,7 +3,7 @@ love: cethcan
 
 PKGS="libevent jansson"
 
-cethcan: main.o can.o
+cethcan: main.o can.o ether.o
 	gcc -g -o $@ `pkg-config --libs $(PKGS)` $^
 
 clean:
diff --git a/cethcan/can.c b/cethcan/can.c
index be8c635..4a61756 100644
--- a/cethcan/can.c
+++ b/cethcan/can.c
@@ -32,8 +32,18 @@ struct can_user *can_register_alloc(void *arg, can_handler handler,
 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("%08x (%zu)%s", (unsigned)msg->daddr, msg->dlc, buf);
+
 	for (u = users; u; u = u->next)
 		if (u != origin)
 			u->handler(u->arg, msg);
diff --git a/cethcan/cethcan.h b/cethcan/cethcan.h
index 1ac898f..477221e 100644
--- a/cethcan/cethcan.h
+++ b/cethcan/cethcan.h
@@ -23,8 +23,16 @@
 #include <event2/event.h>
 #include <jansson.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;
-extern json_t *config;
 
 struct can_user;
 
@@ -57,4 +65,6 @@ extern struct can_user *can_register_alloc(void *arg, can_handler handler,
 extern void can_broadcast(struct can_user *origin, struct can_message *msg);
 extern void can_init(void);
 
+extern int ether_init(json_t *config);
+
 #endif /* _CETHCAN_H */
diff --git a/cethcan/ether.c b/cethcan/ether.c
new file mode 100644
index 0000000..5daac2e
--- /dev/null
+++ b/cethcan/ether.c
@@ -0,0 +1,80 @@
+#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;
+};
+
+static void ether_can_handler(void *arg, struct can_message *msg)
+{
+}
+
+static void ether_sock_handler(int sock, short event, void *arg)
+{
+}
+
+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/main.c b/cethcan/main.c
index 938721e..6d79a9f 100644
--- a/cethcan/main.c
+++ b/cethcan/main.c
@@ -1,13 +1,13 @@
 #include "cethcan.h"
 
 struct event_base *ev_base;
-json_t *config;
 
 int main(int argc, char **argv)
 {
 	int optch = 0;
 	const char *cfgfile = "cethcan.json";
 	json_error_t je;
+	json_t *config, *ethercfg;
 
 	do {
 		optch = getopt(argc, argv, "c:");
@@ -40,6 +40,13 @@ int main(int argc, char **argv)
 
 	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;
+	}
+
 	event_base_loop(ev_base, 0);
 	return 0;
 }