cethcan: ESPnet

4 files changed, 283 insertions, 6 deletions

diff --git a/cethcan/Makefile b/cethcan/Makefile
index 6acbbd7..bc8fab4 100644
--- a/cethcan/Makefile
+++ b/cethcan/Makefile
@@ -5,7 +5,8 @@ PKGS="libevent jansson"
 L_CFLAGS=-g -O0 -Wall -Wextra -Wshadow -pedantic -Wno-unused-parameter -Wno-format -std=gnu99 `pkg-config --cflags $(PKGS)` $(CFLAGS)
 L_LDFLAGS=-g `pkg-config --libs $(PKGS)` -lcrypto $(LDFLAGS)
 
-cethcan: main.o can.o ether.o light.o beanctr.o http.o socketcan.o jsonrpc.o rpc.o
+cethcan: main.o can.o ether.o light.o beanctr.o \
+		http.o socketcan.o jsonrpc.o rpc.o espnet.o
 	gcc $(L_LDFLAGS) -o $@ $^
 
 clean:
diff --git a/cethcan/cethcan.h b/cethcan/cethcan.h
index 6adf6bb..6763006 100644
--- a/cethcan/cethcan.h
+++ b/cethcan/cethcan.h
@@ -86,12 +86,20 @@ extern int light_set(struct light *l, unsigned value);
 extern unsigned light_getset(struct light *l);
 extern unsigned light_getact(struct light *l);
 
+struct espnet_device;
+struct espnet_device *espnet_find(const char *name);
+extern int espnet_set(struct espnet_device *dev,
+		unsigned r, unsigned g, unsigned b);
+extern void espnet_get(struct espnet_device *dev,
+		unsigned *r, unsigned *g, unsigned *b);
+
 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 int bean_init_conf(json_t *config);
+extern int espnet_init_conf(json_t *config);
 extern void http_init(void);
 
 #endif /* _CETHCAN_H */
diff --git a/cethcan/espnet.c b/cethcan/espnet.c
new file mode 100644
index 0000000..0661351
--- /dev/null
+++ b/cethcan/espnet.c
@@ -0,0 +1,261 @@
+#include "cethcan.h"
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#define ESPNET_PORT		3333
+#define ESPNET_TIMER		500*1000 /*us*/
+#define ESPNET_TIMER_SHORT	5*1000 /*us*/
+
+struct espnet_device;
+
+struct espnet_sink {
+	struct espnet_sink *next;
+
+	struct can_user *u;
+	int fd;
+	uint8_t universe;
+	char *ifname;
+	int ifindex;
+
+	struct event *writer;
+	bool writer_resched;
+
+	struct espnet_device *devs;
+};
+
+struct espnet_device {
+	struct espnet_device *next;
+	struct espnet_sink *sink;
+
+	char *name;
+	unsigned baseaddr;
+	uint8_t r, g, b;
+};
+
+struct espnet_packet {
+	char signature[4];
+	uint8_t universe;
+	uint8_t startcode;
+	uint8_t datatype;
+	uint16_t length;
+	uint8_t dmx[512];
+} __attribute__((packed));
+
+static struct espnet_sink *sinks = NULL, **psinks = &sinks;
+
+struct espnet_device *espnet_find(const char *name)
+{
+	for (struct espnet_sink *sink = sinks; sink; sink = sink->next)
+		for (struct espnet_device *d = sink->devs; d; d = d->next)
+			if (!strcmp(d->name, name))
+				return d;
+	return NULL;
+}
+
+int espnet_set(struct espnet_device *dev, unsigned r, unsigned g, unsigned b)
+{
+	dev->r = r;
+	dev->g = g;
+	dev->b = b;
+
+	if (!dev->sink->writer_resched) {
+		struct timeval tvs = { .tv_sec = 0,
+				.tv_usec = ESPNET_TIMER_SHORT };
+		event_del(dev->sink->writer);
+		event_add(dev->sink->writer, &tvs);
+		dev->sink->writer_resched = true;
+	}
+	return 0;
+}
+
+void espnet_get(struct espnet_device *dev,
+		unsigned *r, unsigned *g, unsigned *b)
+{
+	*r = dev->r;
+	*g = dev->g;
+	*b = dev->b;
+}
+
+void espnet_sink_writer(evutil_socket_t fd, short event, void *arg)
+{
+	struct espnet_sink *sink = arg;
+	struct timeval tvs = { .tv_sec = 0, .tv_usec = ESPNET_TIMER };
+	struct espnet_packet packet = {
+		.signature = { 'E', 'S', 'D', 'D' },
+		.universe = sink->universe,
+		.startcode = 0,
+		.datatype = 1,
+		.length = htons(512),
+	};
+	struct sockaddr_in addr = {
+		.sin_family = AF_INET,
+		.sin_port = htons(ESPNET_PORT),
+		.sin_addr = { .s_addr = INADDR_BROADCAST },
+	};
+
+	for (struct espnet_device *d = sink->devs; d; d = d->next) {
+		if (d->baseaddr >= 510)
+			continue;
+		packet.dmx[d->baseaddr] = d->r;
+		packet.dmx[d->baseaddr + 1] = d->g;
+		packet.dmx[d->baseaddr + 2] = d->b;
+	}
+
+	if (sendto(sink->fd, &packet, sizeof(packet), 0,
+		(struct sockaddr *)&addr, sizeof(addr) != sizeof(packet)))
+		lprintf("ESPnet[%s#%d] send failed: %s",
+			sink->ifname, sink->universe, strerror(errno));
+
+	sink->writer_resched = false;
+	event_add(sink->writer, &tvs);
+}
+
+static void espnet_json_one(struct espnet_sink *sink,
+		struct espnet_device *dev,
+		json_t *json, enum json_subtype type)
+{
+	json_t *lobj = json_object();
+
+	json_object_set_new(lobj, "klass", json_string("dmxrgb"));
+	json_object_set_new(lobj, "dmxaddr", json_integer(dev->baseaddr));
+
+	json_object_set_new(lobj, "r", json_integer(dev->r));
+	json_object_set_new(lobj, "g", json_integer(dev->g));
+	json_object_set_new(lobj, "b", json_integer(dev->b));
+
+	json_object_set_new(json, dev->name, lobj);
+}
+
+static void espnet_json_handler(void *arg, json_t *json,
+		enum json_subtype type)
+{
+	struct espnet_sink *sink = arg;
+	for (struct espnet_device *d = sink->devs; d; d = d->next)
+		espnet_json_one(sink, d, json, type);
+}
+
+static void espnet_can_handler(void *arg, struct can_message *msg)
+{
+}
+
+static struct espnet_device *espnet_add_dev(struct espnet_sink *sink,
+		json_t *config)
+{
+	struct espnet_device *d;
+
+	if (!json_is_object(config)) {
+		lprintf("ESPnet device config must be an object/dictionary");
+		return NULL;
+	}
+	if (!json_is_string(json_object_get(config, "name"))) {
+		lprintf("ESPnet device config must specify str 'name'");
+		return NULL;
+	}
+	if (!json_is_integer(json_object_get(config, "baseaddr"))) {
+		lprintf("ESPnet device config must specify int 'baseaddr'");
+		return NULL;
+	}
+
+	d = calloc(sizeof(*d), 1);
+	d->sink = sink;
+	d->baseaddr = json_integer_value(json_object_get(config, "baseaddr"));
+	d->name = strdup(json_string_value(json_object_get(config, "name")));
+	return d;
+}
+
+int espnet_init_conf(json_t *config)
+{
+	struct espnet_sink *sink;
+	int universe = 0;
+	const char *iface;
+	int ifindex;
+	struct espnet_device **devp;
+
+	if (!json_is_object(config)) {
+		lprintf("ESPnet config must be an object/dictionary");
+		return 1;
+	}
+	if (!json_is_string(json_object_get(config, "interface"))) {
+		lprintf("ESPnet config must have a string 'interface' key");
+		return 1;
+	}
+	if (!json_is_object(json_object_get(config, "devices"))) {
+		lprintf("ESPnet config must have an object 'device' key");
+		return 1;
+	}
+
+	json_t *univ = json_object_get(config, "universe");
+	if (univ && !json_is_integer(univ)) {
+		lprintf("ESPnet universe number must be integer if present");
+		return 1;
+	}
+	if (univ)
+		universe = json_integer_value(univ);
+	if (universe > 255 || universe < 0) {
+		lprintf("ESPnet supports universes 0 to 255");
+		return 1;
+	}
+
+	iface = json_string_value(json_object_get(config, "interface"));
+	ifindex = if_nametoindex(iface);
+
+	if (ifindex == 0) {
+		lprintf("ESPnet interface '%s' error: %s",
+			iface, strerror(errno));
+		return 1;
+	}
+
+	sink = calloc(sizeof(*sink), 1);
+	sink->ifname = strdup(iface);
+	sink->ifindex = ifindex;
+	sink->universe = universe;
+
+	struct ip_mreqn mrn = { .imr_ifindex = ifindex };
+	struct sockaddr_in addr = { .sin_family = AF_INET,
+		.sin_port = htons(ESPNET_PORT) };
+
+	sink->fd = socket(AF_INET, SOCK_DGRAM, 0);
+	if (sink->fd < 0
+		|| setsockopt(sink->fd, SOL_IP, IP_MULTICAST_IF,
+				&mrn, sizeof(mrn))
+		|| setsockopt(sink->fd, SOL_SOCKET, SO_BINDTODEVICE,
+				iface, strlen(iface) + 1)
+		|| bind(sink->fd, (struct sockaddr *)&addr, sizeof(addr))) {
+		lprintf("ESPnet interface '%s' initalisation error: %s",
+			iface, strerror(errno));
+		free(sink->ifname);
+		free(sink);
+		return 1;
+	}
+
+	json_t *devcfg = json_object_get(config, "devices");
+	devp = &sink->devs;
+	for (size_t i = 0; i < json_array_size(devcfg); i++) {
+		struct espnet_device *dev;
+		json_t *c = json_array_get(devcfg, i);
+		dev = espnet_add_dev(sink, c);
+		if (!dev) {
+			close(sink->fd);
+			free(sink->ifname);
+			free(sink);
+			return 1;
+		}
+		*devp = dev;
+		devp = &dev->next;
+	}
+
+	sink->u = can_register_alloc(sink, espnet_can_handler,
+		"ESPnet[%s#%d]", sink->ifname, universe);
+	sink->u->json = espnet_json_handler;
+
+	sink->writer = event_new(ev_base, -1, 0, espnet_sink_writer, sink);
+
+	*psinks = sink;
+	psinks = &sink->next;
+	return 0;
+}
diff --git a/cethcan/main.c b/cethcan/main.c
index cc3522b..99a32c5 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, *beancfg, *socancfg;
+	json_t *config;
 
 	do {
 		optch = getopt(argc, argv, "c:");
@@ -40,34 +40,41 @@ int main(int argc, char **argv)
 
 	can_init();
 
-	ethercfg = json_object_get(config, "ethernet");
+	json_t *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");
+	json_t *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;
 	}
 
-	beancfg = json_object_get(config, "beans");
+	json_t *beancfg = json_object_get(config, "beans");
 	for (size_t i = 0; i < json_array_size(beancfg); i++) {
 		json_t *c = json_array_get(beancfg, i);
 		if (bean_init_conf(c))
 			return 1;
 	}
 
-	socancfg = json_object_get(config, "socketcan");
+	json_t *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;
 	}
 
+	json_t *espcfg = json_object_get(config, "espnet");
+	for (size_t i = 0; i < json_array_size(espcfg); i++) {
+		json_t *c = json_array_get(espcfg, i);
+		if (espnet_init_conf(c))
+			return 1;
+	}
+
 	http_init();
 
 	event_base_loop(ev_base, 0);