1 files changed, 237 insertions, 127 deletions

diff --git a/src/mesh/node3d.c b/src/mesh/node3d.c
index d00f762..8c6d776 100644
--- a/src/mesh/node3d.c
+++ b/src/mesh/node3d.c
@@ -1,189 +1,299 @@
 #include <gran/mesh/node3d.h>
 
-struct reg {
-	struct packet pkt;
-	bool busy;
-};
+#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 up_port(n) (n)->ports[(n)->elems + 4]
+#define down_port(n) (n)->ports[(n)->elems + 5]
+
+#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 up_in(n) (n)->in[(n)->elems + 4]
+#define down_in(n) (n)->in[(n)->elems + 5]
+
+#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]
+#define up_out(n) (n)->out[(n)->elems + 4]
+#define down_out(n) (n)->out[(n)->elems + 5]
 
 struct node3d {
 	struct component component;
 	uint8_t x, y, z;
+	uint8_t elems;
 
 	uint64_t timestamp;
 
-	struct component *n, *s, *e, *w, *u, *d, *l;
-
-	struct reg n_in, s_in, e_in, w_in, u_in, d_in, l_in;
+	struct reg *in; /* countedby[elems + 6] */
+	struct reg *out; /* countedby[elems + 6] */
+	struct component **ports; /* countedby[elems + 6] */
 };
 
-enum order {
-	N, S, E, W, U, D, L
-};
+static void node3d_destroy(struct node3d *n)
+{
+	free(n->in);
+	free(n->out);
+	free(n->ports);
+	free(n);
+}
 
-static inline void maybe_pick(struct reg *output[7], enum order d, struct reg *r)
+static void clock_outputs(struct node3d *n)
 {
-	if (output[d] && output[d]->pkt.timestamp < r->pkt.timestamp)
-		return;
+	for (int i = 0; i < n->elems + 6; ++i) {
+		if (!n->out[i].busy)
+			continue;
+
+		stat ret = SEND(n, n->ports[i], n->out[i].pkt);
+		if (ret == EBUSY)
+			continue;
 
-	output[d] = r;
+		n->out[i].busy = false;
+	}
 }
 
-static stat node3d_clock(struct node3d *node3d)
+struct sel_helper {
+	uint8_t x, y, z, elem;
+};
+
+static bool north_sel(struct reg *r, void *data)
 {
-	node3d->timestamp++;
+	uint8_t y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh3d(r->pkt.to, NULL, &y, NULL, NULL, NULL);
+	return y > helper->y;
+}
 
-	struct reg *output[7] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL};
-	struct reg *input[7] = {
-		&node3d->n_in,
-		&node3d->s_in,
-		&node3d->e_in,
-		&node3d->w_in,
-		&node3d->u_in,
-		&node3d->d_in,
-		&node3d->l_in
-	};
+static bool south_sel(struct reg *r, void *data)
+{
+	uint8_t y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh3d(r->pkt.to, NULL, &y, NULL, NULL, NULL);
+	return y < helper->y;
+}
 
-	uint8_t X = node3d->x, Y = node3d->y, Z = node3d->z;
-	for (size_t i = 0; i < 7; ++i) {
-		struct reg *r = input[i];
-		if (!r->busy)
-			continue;
+static bool east_sel(struct reg *r, void *data)
+{
+	uint8_t x = 0, y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh3d(r->pkt.to, &x, &y, NULL, NULL, NULL);
+	return y == helper->y && x > helper->x;
+}
 
-		uint8_t x, y, z;
-		addr_mesh3d(r->pkt.to, &x, &y, &z, NULL);
-		if (x < X) {
-			maybe_pick(output, W, r);
-			continue;
-		}
+static bool west_sel(struct reg *r, void *data)
+{
+	uint8_t x = 0, y = 0;
+	struct sel_helper *helper = data;
+	addr_mesh3d(r->pkt.to, &x, &y, NULL, NULL, NULL);
+	return y == helper->y && x < helper->x;
+}
 
-		if (x > X) {
-			maybe_pick(output, E, r);
-			continue;
-		}
+static bool up_sel(struct reg *r, void *data)
+{
+	uint8_t x = 0, y = 0, z = 0;
+	struct sel_helper *helper = data;
+	addr_mesh3d(r->pkt.to, &x, &y, &z, NULL, NULL);
+	return y == helper->y && x == helper->x && z > helper->z;
+}
 
-		if (y < Y) {
-			maybe_pick(output, S, r);
-			continue;
-		}
+static bool down_sel(struct reg *r, void *data)
+{
+	uint8_t x = 0, y = 0, z = 0;
+	struct sel_helper *helper = data;
+	addr_mesh3d(r->pkt.to, &x, &y, &z, NULL, NULL);
+	return y == helper->y && x == helper->x && z < helper->z;
+}
 
-		if (y > Y) {
-			maybe_pick(output, N, r);
-			continue;
-		}
+static bool elem_sel(struct reg *r, void *data)
+{
+	uint8_t x = 0, y = 0, z = 0, elem = 0;
+	struct sel_helper *helper = data;
+	addr_mesh3d(r->pkt.to, &x, &y, &z, &elem, NULL);
+	return x == helper->x && y == helper->y && z == helper->z && elem == helper->elem;
+}
 
-		if (z < Z) {
-			maybe_pick(output, D, r);
-			continue;
-		}
+static stat node3d_clock(struct node3d *n)
+{
+	n->timestamp++;
+	clock_outputs(n);
 
-		if (z > Z) {
-			maybe_pick(output, U, r);
+	/* select oldest packet to process */
+	struct reg *r = NULL;
+	for (int i = 0; i < n->elems; ++i) {
+		if (!n->in[i].busy)
 			continue;
-		}
 
-		maybe_pick(output, L, r);
-	}
+		if (!r || r->pkt.timestamp > n->in[i].pkt.timestamp)
+			r = &n->in[i];
+	};
 
-	struct component *target[7] = {
-		node3d->n,
-		node3d->s,
-		node3d->e,
-		node3d->w,
-		node3d->u,
-		node3d->d,
-		node3d->l
+	struct sel_helper helper = {
+		.elem = 0,
+		.x = n->x,
+		.y = n->y,
+		.z = n->z
 	};
 
-	for (size_t i = 0; i < 7; ++i) {
-		if (!output[i])
-			continue;
+	struct reg *north[] = {r,            &east_in(n),  &south_in(n),
+		               &west_in(n),  &up_in(n),    &down_in(n)};
 
-		if (!target[i]) {
-			/* for now, should send packet back with an error or something */
-			abort();
-		}
+	struct reg *east[]  = {r,            &north_in(n), &south_in(n),
+		               &west_in(n),  &up_in(n),    &down_in(n)};
 
-		stat ret = SEND(node3d, target[i], output[i]->pkt);
-		if (ret == EBUSY)
-			continue;
+	struct reg *south[] = {r,            &north_in(n), &east_in(n),
+		               &west_in(n),  &up_in(n),    &down_in(n)};
+
+	struct reg *west[]  = {r,            &north_in(n), &east_in(n),
+		               &south_in(n), &up_in(n),    &down_in(n)};
 
-		assert(ret == OK);
-		output[i]->busy = false;
+	struct reg *up[]    = {r,            &north_in(n), &east_in(n),
+		               &west_in(n),  &south_in(n), &down_in(n)};
+
+	struct reg *down[]  = {r,            &north_in(n), &east_in(n),
+		               &west_in(n),  &south_in(n), &up_in(n)};
+
+	propagate(&north_out(n), 6, north, north_sel, &helper);
+	propagate(&east_out(n),  6, east,  east_sel,  &helper);
+	propagate(&south_out(n), 6, south, south_sel, &helper);
+	propagate(&west_out(n),  6, west,  west_sel,  &helper);
+	propagate(&up_out(n),    6, up,    up_sel,    &helper);
+	propagate(&down_out(n),  6, down,  down_sel,  &helper);
+
+	struct reg *all[] = {r, &north_in(n), &east_in(n), &south_in(n),
+		                &west_in(n),  &up_in(n),   &down_in(n)};
+
+	for (int i = 0; i < n->elems; ++i) {
+		helper.elem = i;
+		propagate(&n->out[i], 7, all, elem_sel, &helper);
 	}
 
 	return OK;
 }
 
-static stat reg_receive(struct reg *r, struct packet pkt)
+static stat node3d_receive(struct node3d *n, struct component *from,
+                           struct packet pkt)
 {
-	if (r->busy)
-		return EBUSY;
+	for (int i = 0; i < n->elems + 6; ++i) {
+		if (from != n->ports[i])
+			continue;
+
+		if (i < n->elems)
+			pkt.timestamp = n->timestamp;
 
-	r->pkt = pkt;
-	r->busy = true;
+		return place_reg(&n->in[i], pkt);
+	}
+
+	abort();
 	return OK;
 }
 
-static stat node3d_receive(struct node3d *node3d, struct component *from, struct packet pkt)
+struct component *create_mesh_node3d(uint8_t x, uint8_t y, uint8_t z, uint8_t elems)
 {
-	if (from == node3d->l) {
-		/* add time when packet entered network */
-		pkt.timestamp = node3d->timestamp;
-		return reg_receive(&node3d->l_in, pkt);
+	struct node3d *n = calloc(1, sizeof(struct node3d));
+	if (!n)
+		return NULL;
+
+	n->in = calloc(elems + 6, sizeof(struct reg));
+	if (!n->in) {
+		node3d_destroy(n);
+		return NULL;
 	}
 
-	if (from == node3d->n)
-		return reg_receive(&node3d->n_in, pkt);
+	n->out = calloc(elems + 6, sizeof(struct reg));
+	if (!n->out) {
+		node3d_destroy(n);
+		return NULL;
+	}
 
-	if (from == node3d->s)
-		return reg_receive(&node3d->s_in, pkt);
+	n->ports = calloc(elems + 6, sizeof(struct component*));
+	if (!n->ports) {
+		node3d_destroy(n);
+		return NULL;
+	}
 
-	if (from == node3d->e)
-		return reg_receive(&node3d->e_in, pkt);
+	n->component.destroy = (destroy_callback)node3d_destroy;
+	n->component.receive = (receive_callback)node3d_receive;
+	n->component.clock = (clock_callback)node3d_clock;
+	n->elems = elems;
+	n->x = x;
+	n->y = y;
+	n->z = z;
+	return (struct component *)n;
+}
 
-	if (from == node3d->w)
-		return reg_receive(&node3d->w_in, pkt);
+stat mesh_node3d_connect(struct component *c, struct component *e,
+			 uint8_t elem)
+{
+	struct node3d *n = (struct node3d *)c;
+	if (elem >= n->elems)
+		return ENOSUCH;
 
-	if (from == node3d->u)
-		return reg_receive(&node3d->u_in, pkt);
+	if (n->ports[elem])
+		return EEXISTS;
 
-	if (from == node3d->d)
-		return reg_receive(&node3d->d_in, pkt);
+	n->ports[elem] = e;
+	return OK;
+}
 
-	abort();
+stat mesh_node3d_connect_north(struct component *c, struct component *e)
+{
+	struct node3d *n = (struct node3d *)c;
+	if (north_port(n))
+		return EEXISTS;
+
+	north_port(n) = e;
 	return OK;
 }
 
-struct component *create_mesh_node3d(uint8_t x, uint8_t y, uint8_t z)
+stat mesh_node3d_connect_east(struct component *c, struct component *e)
 {
-	struct node3d *node = calloc(1, sizeof(struct node3d));
-	if (!node)
-		return NULL;
+	struct node3d *n = (struct node3d *)c;
+	if (east_port(n))
+		return EEXISTS;
+
+	east_port(n) = e;
+	return OK;
+}
+
+stat mesh_node3d_connect_south(struct component *c, struct component *e)
+{
+	struct node3d *n = (struct node3d *)c;
+	if (south_port(n))
+		return EEXISTS;
+
+	south_port(n) = e;
+	return OK;
+}
+
+stat mesh_node3d_connect_west(struct component *c, struct component *e)
+{
+	struct node3d *n = (struct node3d *)c;
+	if (west_port(n))
+		return EEXISTS;
+
+	west_port(n) = e;
+	return OK;
+}
+
+stat mesh_node3d_connect_up(struct component *c, struct component *e)
+{
+	struct node3d *n = (struct node3d *)c;
+	if (up_port(n))
+		return EEXISTS;
 
-	node->component.receive = (receive_callback)node3d_receive;
-	node->component.clock = (clock_callback)node3d_clock;
-	node->x = x;
-	node->y = y;
-	node->z = z;
-	return (struct component *)node;
+	up_port(n) = e;
+	return OK;
 }
 
-stat mesh_node3d_connect(struct component *node,
-		struct component *n,
-		struct component *s,
-		struct component *e,
-		struct component *w,
-		struct component *u,
-		struct component *d,
-		struct component *l)
+stat mesh_node3d_connect_down(struct component *c, struct component *e)
 {
-	struct node3d *node3d = (struct node3d *)node;
-	node3d->n = n;
-	node3d->s = s;
-	node3d->e = e;
-	node3d->w = w;
-	node3d->u = u;
-	node3d->d = d;
-	node3d->l = l;
+	struct node3d *n = (struct node3d *)c;
+	if (down_port(n))
+		return EEXISTS;
+
+	down_port(n) = e;
 	return OK;
 }