add 1d mesh node and refactor 2d mesh node

+ Seems to decrease performance a little bit, presumably due to
  extra register copy, but simplifies code a lot and opens up more
  genericism so I'll consider it an upgrade for now. Copying packets
  around is rather slow though, might in the future move to some
  kind of pointer based packet handling

1 files changed, 281 insertions, 0 deletions

diff --git a/src/mesh/node2d.c b/src/mesh/node2d.c
new file mode 100644
index 0000000..81ef8dc
--- /dev/null
+++ b/src/mesh/node2d.c
@@ -0,0 +1,281 @@
+#include <gran/mesh/node2d.h>
+
+#define north_port(n) (n)->ports[(n)->elems + 0]
+#define east_port(n)  (n)->ports[(n)->elems + 1]
+#define south_port(n) (n)->ports[(n)->elems + 2]
+#define west_port(n)  (n)->ports[(n)->elems + 3]
+
+#define north_in(n) (n)->in[(n)->elems + 0]
+#define east_in(n)  (n)->in[(n)->elems + 1]
+#define south_in(n) (n)->in[(n)->elems + 2]
+#define west_in(n)  (n)->in[(n)->elems + 3]
+
+#define north_out(n) (n)->out[(n)->elems + 0]
+#define east_out(n)  (n)->out[(n)->elems + 1]
+#define south_out(n) (n)->out[(n)->elems + 2]
+#define west_out(n)  (n)->out[(n)->elems + 3]
+
+struct reg {
+	struct packet pkt;
+	bool busy;
+};
+
+struct node2d {
+	struct component component;
+	uint16_t elems;
+	uint16_t x, y;
+
+	uint64_t timestamp;
+
+	struct reg *in; /* countedby[elems + 4] */
+	struct reg *out; /* countedby[elems + 4] */
+	struct component **ports; /* countedby[elems + 4] */
+};
+
+static void clock_outputs(struct node2d *n)
+{
+	for (int i = 0; i < n->elems + 4; ++i) {
+		if (!n->out[i].busy)
+			continue;
+
+		stat ret = SEND(n, n->ports[i], n->out[i].pkt);
+		if (ret == EBUSY)
+			continue;
+
+		n->out[i].busy = false;
+	}
+}
+
+static void copy_reg(struct reg *r, struct reg *s)
+{
+	assert(s->busy);
+	if (r->busy)
+		return;
+
+	r->pkt = s->pkt;
+	r->busy = true;
+	s->busy = false;
+}
+
+static void propagate(struct reg *out,
+		size_t count, struct reg *in[static count],
+		bool (*sel)(struct reg *r, void *data), void *data)
+{
+	struct reg *r = NULL;
+	for (size_t i = 0; i < count; ++i) {
+		if (!in[i] || !in[i]->busy)
+			continue;
+
+		if (!sel(in[i], data))
+			continue;
+
+		if (!r || r->pkt.timestamp > in[i]->pkt.timestamp)
+			r = in[i];
+	}
+
+	if (!r)
+		return;
+
+	copy_reg(out, r);
+}
+
+struct sel_helper {
+	uint8_t x, y;
+	uint16_t elem;
+};
+
+static bool north_sel(struct reg *r, void *data)
+{
+	uint8_t y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh2d(r->pkt.to, NULL, &y, NULL, NULL);
+	return y > helper->y;
+}
+
+static bool south_sel(struct reg *r, void *data)
+{
+	uint8_t y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh2d(r->pkt.to, NULL, &y, NULL, NULL);
+	return y < helper->y;
+}
+
+static bool east_sel(struct reg *r, void *data)
+{
+	uint8_t x = 0, y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh2d(r->pkt.to, &x, &y, NULL, NULL);
+	return y == helper->y && x > helper->x;
+}
+
+static bool west_sel(struct reg *r, void *data)
+{
+	uint8_t x = 0, y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh2d(r->pkt.to, &x, &y, NULL, NULL);
+	return y == helper->y && x < helper->x;
+}
+
+static bool elem_sel(struct reg *r, void *data)
+{
+	uint16_t elem = 0;
+	uint8_t x = 0, y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh2d(r->pkt.to, &x, &y, &elem, NULL);
+	return x == helper->x && y == helper->y && elem == helper->elem;
+}
+
+static stat node2d_clock(struct node2d *n)
+{
+	n->timestamp++;
+	clock_outputs(n);
+
+	/* select oldest packet to process */
+	struct reg *r = NULL;
+	for (int i = 0; i < n->elems; ++i) {
+		if (!n->in[i].busy)
+			continue;
+
+		if (!r || r->pkt.timestamp > n->in[i].pkt.timestamp)
+			r = &n->in[i];
+	}
+
+	struct sel_helper helper = {
+		.elem = 0,
+		.x = n->x,
+		.y = n->y,
+	};
+
+	struct reg *north[] = {r, &east_in(n),  &south_in(n), &west_in(n)};
+	struct reg *east[]  = {r, &north_in(n), &south_in(n), &west_in(n)};
+	struct reg *south[] = {r, &north_in(n), &east_in(n),  &west_in(n)};
+	struct reg *west[]  = {r, &north_in(n), &east_in(n),  &south_in(n)};
+
+	propagate(&north_out(n), 4, north, north_sel, &helper);
+	propagate(&east_out(n),  4, east,  east_sel,  &helper);
+	propagate(&south_out(n), 4, south, south_sel, &helper);
+	propagate(&west_out(n),  4, west,  west_sel,  &helper);
+
+	struct reg *all[] = {r, &north_in(n), &east_in(n), &south_in(n), &west_in(n)};
+	for (int i = 0; i < n->elems; ++i) {
+		helper.elem = i;
+		propagate(&n->out[i], 5, all, elem_sel, &helper);
+	}
+
+	return OK;
+}
+
+static stat reg_busy(struct reg *r, struct packet pkt)
+{
+	bool busy = r->busy;
+	if (!busy) {
+		r->pkt = pkt;
+		r->busy = true;
+	}
+
+	return busy ? EBUSY : OK;
+}
+
+static stat node2d_receive(struct node2d *n, struct component *from, struct packet pkt)
+{
+	for (int i = 0; i < n->elems + 4; ++i) {
+		if (from != n->ports[i])
+			continue;
+
+		if (i < n->elems)
+			pkt.timestamp = n->timestamp;
+
+		return reg_busy(&n->in[i], pkt);
+	}
+
+	abort();
+	return OK;
+}
+
+struct component *create_mesh_node2d(uint8_t x, uint8_t y, uint16_t elems)
+{
+	struct node2d *n = calloc(1, sizeof(struct node2d));
+	if (!n)
+		return NULL;
+
+	n->in = calloc(elems + 4, sizeof(struct reg));
+	if (!n->in) {
+		free(n);
+		return NULL;
+	}
+
+	n->out = calloc(elems + 4, sizeof(struct reg));
+	if (!n->out) {
+		free(n->in);
+		free(n);
+		return NULL;
+	}
+
+	n->ports = calloc(elems + 4, sizeof(struct component *));
+	if (!n->ports) {
+		free(n->out);
+		free(n->in);
+		free(n);
+		return NULL;
+	}
+
+	n->component.receive = (receive_callback)node2d_receive;
+	n->component.clock = (clock_callback)node2d_clock;
+	n->elems = elems;
+	n->x = x;
+	n->y = y;
+	return (struct component *)n;
+}
+
+stat mesh_node2d_connect(struct component *c, struct component *e, uint16_t elem)
+{
+	struct node2d *n = (struct node2d *)c;
+	if (elem >= n->elems)
+		return ENOSUCH;
+
+	if (n->ports[elem])
+		return EEXISTS;
+
+	n->ports[elem] = e;
+	return OK;
+}
+
+stat mesh_node2d_connect_north(struct component *c, struct component *e)
+{
+	struct node2d *n = (struct node2d *)c;
+	if (north_port(n))
+		return EEXISTS;
+
+	north_port(n) = e;
+	return OK;
+}
+
+stat mesh_node2d_connect_east(struct component *c, struct component *e)
+{
+	struct node2d *n = (struct node2d *)c;
+	if (east_port(n))
+		return EEXISTS;
+
+	east_port(n) = e;
+	return OK;
+}
+
+stat mesh_node2d_connect_south(struct component *c, struct component *e)
+{
+	struct node2d *n = (struct node2d *)c;
+	if (south_port(n))
+		return EEXISTS;
+
+	south_port(n) = e;
+	return OK;
+}
+
+stat mesh_node2d_connect_west(struct component *c, struct component *e)
+{
+	struct node2d *n = (struct node2d *)c;
+	if (west_port(n))
+		return EEXISTS;
+
+	west_port(n) = e;
+	return OK;
+}