3d mesh

2 files changed, 206 insertions, 1 deletions

diff --git a/src/grid/node3d.c b/src/grid/node3d.c
new file mode 100644
index 0000000..f404ea0
--- /dev/null
+++ b/src/grid/node3d.c
@@ -0,0 +1,205 @@
+#include <gran/grid/node3d.h>
+
+struct reg {
+	struct packet pkt;
+	bool busy;
+};
+
+struct node3d {
+	struct component component;
+	uint8_t x, y, z;
+
+	uint64_t timestamp;
+
+	struct component *n, *s, *w, *e, *u, *d, *l;
+
+	struct reg n_in, s_in, w_in, e_in, u_in, d_in, l_in;
+};
+
+static stat reg_receive(struct reg *r, struct packet pkt)
+{
+	if (r->busy)
+		return EBUSY;
+
+	r->pkt = pkt;
+	r->busy = true;
+	return OK;
+}
+
+/* LX = node.x > pkt.x, etc */
+enum match {
+	LX = (1 << 0),
+	LY = (1 << 1),
+	LZ = (1 << 2),
+	GX = (1 << 3),
+	GY = (1 << 4),
+	GZ = (1 << 5),
+	NX = (1 << 6),
+	NY = (1 << 7),
+	NZ = (1 << 8),
+};
+
+static void maybe_route_reg(struct node3d *node3d, struct reg *reg, enum match m, uint64_t *oldest, struct packet **pkt, bool **busy)
+{
+	if (!reg->busy)
+		return;
+
+	uint8_t x, y, z;
+	addr_grid3d(reg->pkt.to, &x, &y, &z, NULL);
+
+	if ((m & LX) && x < node3d->x)
+		return;
+
+	if ((m & LY) && y < node3d->y)
+		return;
+
+	if ((m & LZ) && z < node3d->z)
+		return;
+
+	if ((m & GX) && x > node3d->x)
+		return;
+
+	if ((m & GY) && y > node3d->y)
+		return;
+
+	if ((m & GZ) && z > node3d->z)
+		return;
+
+	if ((m & NX) && x == node3d->x)
+		return;
+
+	if ((m & NY) && y == node3d->y)
+		return;
+
+	if ((m & NZ) && z == node3d->z)
+		return;
+
+	if (reg->pkt.timestamp < *oldest) {
+		*oldest = reg->pkt.timestamp;
+		*busy = &reg->busy;
+		*pkt = &reg->pkt;
+	}
+}
+
+static stat route(struct node3d *node3d, struct component *next, enum match m)
+{
+	uint64_t oldest = -1; struct packet *pkt = NULL; bool *busy = NULL;
+	maybe_route_reg(node3d, &node3d->l_in, m, &oldest, &pkt, &busy);
+	maybe_route_reg(node3d, &node3d->n_in, m, &oldest, &pkt, &busy);
+	maybe_route_reg(node3d, &node3d->s_in, m, &oldest, &pkt, &busy);
+	maybe_route_reg(node3d, &node3d->e_in, m, &oldest, &pkt, &busy);
+	maybe_route_reg(node3d, &node3d->w_in, m, &oldest, &pkt, &busy);
+	maybe_route_reg(node3d, &node3d->u_in, m, &oldest, &pkt, &busy);
+	maybe_route_reg(node3d, &node3d->d_in, m, &oldest, &pkt, &busy);
+
+	/* no suitable match */
+	if (pkt == NULL)
+		return OK;
+
+	assert(busy);
+
+	if (!next) {
+		abort(); /* for now, eventually should probably return to sender */
+		return OK;
+	}
+
+	stat ret = SEND(node3d, next, *pkt);
+	if (ret == EBUSY)
+		return OK;
+
+	*busy = false;
+	return OK;
+}
+
+static stat node3d_clock(struct node3d *node3d)
+{
+	node3d->timestamp++;
+
+	stat ret = OK;
+	if ((ret = route(node3d, node3d->e, LX | NX)))
+		return ret;
+
+	if ((ret = route(node3d, node3d->w, GX | NX)))
+		return ret;
+
+	if ((ret = route(node3d, node3d->n, LX | GX | LY | NY)))
+		return ret;
+
+	if ((ret = route(node3d, node3d->s, LX | GX | GY | NY)))
+		return ret;
+
+	if ((ret = route(node3d, node3d->u, LX | GX | LY | GY | LZ | NZ)))
+		return ret;
+
+	if ((ret = route(node3d, node3d->d, LX | GX | LY | GY | GZ | NZ)))
+		return ret;
+
+	if ((ret = route(node3d, node3d->l, LX | LY | LZ | GX | GY | GZ)))
+		return ret;
+
+	return OK;
+}
+
+static stat node3d_receive(struct node3d *node3d, struct component *from, struct packet pkt)
+{
+	if (from == node3d->l) {
+		/* add time when packet entered network */
+		pkt.timestamp = node3d->timestamp;
+		return reg_receive(&node3d->l_in, pkt);
+	}
+
+	if (from == node3d->n)
+		return reg_receive(&node3d->n_in, pkt);
+
+	if (from == node3d->s)
+		return reg_receive(&node3d->s_in, pkt);
+
+	if (from == node3d->w)
+		return reg_receive(&node3d->w_in, pkt);
+
+	if (from == node3d->e)
+		return reg_receive(&node3d->e_in, pkt);
+
+	if (from == node3d->u)
+		return reg_receive(&node3d->u_in, pkt);
+
+	if (from == node3d->d)
+		return reg_receive(&node3d->d_in, pkt);
+
+	abort();
+	return OK;
+}
+
+struct component *create_grid_node3d(uint8_t x, uint8_t y, uint8_t z)
+{
+	struct node3d *node = calloc(1, sizeof(struct node3d));
+	if (!node)
+		return NULL;
+
+	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;
+}
+
+stat grid_node3d_connect(struct component *node,
+		struct component *n,
+		struct component *s,
+		struct component *w,
+		struct component *e,
+		struct component *u,
+		struct component *d,
+		struct component *l)
+{
+	struct node3d *node3d = (struct node3d *)node;
+	node3d->n = n;
+	node3d->s = s;
+	node3d->w = w;
+	node3d->e = e;
+	node3d->u = u;
+	node3d->d = d;
+	node3d->l = l;
+	return OK;
+}
diff --git a/src/grid/source.mk b/src/grid/source.mk
index e23d7db..79fe708 100644
--- a/src/grid/source.mk
+++ b/src/grid/source.mk
@@ -1 +1 @@
-SOURCES += src/grid/node.c
+SOURCES += src/grid/node.c src/grid/node3d.c