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, 307 insertions, 0 deletions

diff --git a/src/mesh/node1d.c b/src/mesh/node1d.c
new file mode 100644
index 0000000..23ba2ca
--- /dev/null
+++ b/src/mesh/node1d.c
@@ -0,0 +1,307 @@
+#include <gran/mesh/node1d.h>
+
+#define left_port(n) (n)->ports[(n)->elems + 0]
+#define right_port(n) (n)->ports[(n)->elems + 1]
+
+#define left_in(n) (n)->in[(n)->elems + 0]
+#define right_in(n) (n)->in[(n)->elems + 1]
+
+#define left_out(n) (n)->out[(n)->elems + 0]
+#define right_out(n) (n)->out[(n)->elems + 1]
+
+struct reg {
+	struct packet pkt;
+	bool busy;
+};
+
+struct node1d {
+	struct component component;
+	uint16_t cluster;
+	uint16_t elems;
+
+	uint64_t timestamp;
+
+	struct reg *in; /* countedby[elems + 2] */
+	struct reg *out; /* countedby[elems + 2] */
+	struct component **ports; /* countedby[elems + 2] */
+};
+
+static void node1d_destroy(struct node1d *n)
+{
+	free(n->in);
+	free(n->out);
+	free(n->ports);
+}
+
+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 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 stat node1d_receive(struct node1d *n, struct component *from, struct packet pkt)
+{
+	for (int i = 0; i < n->elems + 2; ++i) {
+		/* add timestamp to packets that originate with us */
+		if (i < n->elems)
+			pkt.timestamp = n->timestamp;
+
+		if (from == n->ports[i])
+			return reg_busy(&n->in[i], pkt);
+	}
+
+	/* shouldn't be possible */
+	abort();
+	return OK;
+}
+
+static void clock_outputs(struct node1d *n)
+{
+	for (int i = 0; i < n->elems + 2; ++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 propagate_left(struct node1d *n, struct reg *a, struct reg *b)
+{
+	struct reg *sel_a = NULL, *sel_b = NULL;
+	if (a && a->busy) {
+		uint16_t cluster = 0;
+		addr_mesh1d(a->pkt.to, &cluster, NULL, NULL);
+
+		if (cluster > n->cluster)
+			sel_a = a;
+	}
+
+	if (b && b->busy) {
+		uint16_t cluster = 0;
+		addr_mesh1d(b->pkt.to, &cluster, NULL, NULL);
+
+		if (cluster > n->cluster)
+			sel_b = b;
+	}
+
+	if (!sel_a && !sel_b)
+		return;
+
+	if (sel_a && !sel_b) {
+		copy_reg(&left_out(n), sel_a);
+		return;
+	}
+
+	if (!sel_a && sel_b) {
+		copy_reg(&left_out(n), sel_b);
+		return;
+	}
+
+	/* both available, select older */
+	if (sel_a->pkt.timestamp < sel_b->pkt.timestamp)
+		copy_reg(&left_out(n), sel_a);
+	else
+		copy_reg(&left_out(n), sel_b);
+}
+
+static void propagate_right(struct node1d *n, struct reg *a, struct reg *b)
+{
+	struct reg *sel_a = NULL, *sel_b = NULL;
+	if (a && a->busy) {
+		uint16_t cluster = 0;
+		addr_mesh1d(a->pkt.to, &cluster, NULL, NULL);
+
+		if (cluster < n->cluster)
+			sel_a = a;
+	}
+
+	if (b && b->busy) {
+		uint16_t cluster = 0;
+		addr_mesh1d(b->pkt.to, &cluster, NULL, NULL);
+
+		if (cluster < n->cluster)
+			sel_b = b;
+	}
+
+	if (!sel_a && !sel_b)
+		return;
+
+	if (sel_a && !sel_b) {
+		copy_reg(&right_out(n), sel_a);
+		return;
+	}
+
+	if (!sel_a && sel_b) {
+		copy_reg(&right_out(n), sel_b);
+		return;
+	}
+
+	/* both available, select older */
+	if (sel_a->pkt.timestamp < sel_b->pkt.timestamp)
+		copy_reg(&right_out(n), sel_a);
+	else
+		copy_reg(&right_out(n), sel_b);
+}
+
+static void propagate(struct node1d *n, int elem, struct reg *a, struct reg *b, struct reg *c)
+{
+
+	struct reg *sel_a = NULL, *sel_b = NULL, *sel_c = NULL;
+	if (a && a->busy) {
+		uint16_t cluster = 0, element = 0;
+		addr_mesh1d(a->pkt.to, &cluster, &element, NULL);
+
+		if (cluster == n->cluster && element == elem)
+			sel_a = a;
+	}
+
+	if (b && b->busy) {
+		uint16_t cluster = 0, element = 0;
+		addr_mesh1d(b->pkt.to, &cluster, &element, NULL);
+
+		if (cluster == n->cluster && element == elem)
+			sel_b = b;
+	}
+
+	if (c && c->busy) {
+		uint16_t cluster = 0, element = 0;
+		addr_mesh1d(c->pkt.to, &cluster, &element, NULL);
+
+		if (cluster == n->cluster && element == elem)
+			sel_c = c;
+	}
+
+	struct reg *sel_0 = NULL, *sel_1 = NULL;
+	if (sel_a && sel_b)
+		sel_0 = sel_a->pkt.timestamp < sel_b->pkt.timestamp ? sel_a : sel_b;
+	else
+		sel_0 = sel_a ? sel_a : sel_b;
+
+	if (sel_b && sel_c)
+		sel_1 = sel_b->pkt.timestamp < sel_c->pkt.timestamp ? sel_b : sel_c;
+	else
+		sel_1 = sel_b ? sel_b : sel_c;
+
+	struct reg *sel = NULL;
+	if (sel_0 && sel_1)
+		sel = sel_0->pkt.timestamp < sel_1->pkt.timestamp ? sel_0 : sel_1;
+	else
+		sel = sel_0 ? sel_0 : sel_1;
+
+	if (!sel)
+		return;
+
+	copy_reg(&n->out[elem], sel);
+}
+
+static stat node1d_clock(struct node1d *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];
+	}
+
+	propagate_left(n, r, &right_in(n));
+	propagate_right(n, r, &left_in(n));
+	for (int i = 0; i < n->elems; ++i)
+		propagate(n, i, r, &right_in(n), &left_in(n));
+
+	return OK;
+}
+
+stat mesh_node1d_connect(struct component *c, struct component *e, uint16_t elem)
+{
+	struct node1d *n = (struct node1d *)c;
+	if (elem >= n->elems)
+		return ENOSUCH;
+
+	if (n->ports[elem])
+		return EEXISTS;
+
+	n->ports[elem] = e;
+	return OK;
+}
+
+stat mesh_node1d_connect_left(struct component *c, struct component *e)
+{
+	struct node1d *n = (struct node1d *)c;
+	if (left_port(n))
+		return EEXISTS;
+
+	left_port(n) = e;
+	return OK;
+}
+
+stat mesh_node1d_connect_right(struct component *c, struct component *e)
+{
+	struct node1d *n = (struct node1d *)c;
+	if (right_port(n))
+		return EEXISTS;
+
+	right_port(n) = e;
+	return OK;
+}
+
+struct component *create_mesh_node1d(uint16_t cluster, uint16_t elems)
+{
+	struct node1d *n = (struct node1d *)calloc(1, sizeof(struct node1d));
+	if (!n)
+		return NULL;
+
+	n->in = (struct reg *)calloc(elems + 2, sizeof(struct reg));
+	if (!n->in) {
+		free(n);
+		return NULL;
+	}
+
+	n->out = (struct reg *)calloc(elems + 2, sizeof(struct reg));
+	if (!n->out) {
+		free(n->in);
+		free(n);
+		return NULL;
+	}
+
+	n->ports = (struct component **)calloc(elems + 2, sizeof(struct component *));
+	if (!n->ports) {
+		free(n->out);
+		free(n->in);
+		free(n);
+		return NULL;
+	}
+
+	n->component.destroy = (destroy_callback)node1d_destroy;
+	n->component.receive = (receive_callback)node1d_receive;
+	n->component.clock   = (clock_callback)node1d_clock;
+	n->cluster = cluster;
+	n->elems = elems;
+	return (struct component *)n;
+}