initial torus stuff

+ Not quite deadlock free for 2D/3D for whatever reason

1 files changed, 221 insertions, 0 deletions

diff --git a/src/torus3d/node.c b/src/torus3d/node.c
new file mode 100644
index 0000000..15cc7ee
--- /dev/null
+++ b/src/torus3d/node.c
@@ -0,0 +1,221 @@
+#include <gran/torus3d/node.h>
+
+struct reg {
+	struct packet pkt;
+	bool busy;
+};
+
+struct port {
+	struct reg r[2];
+};
+
+struct torus3d_node {
+	struct component component;
+	uint8_t x, y, z /*, w for 4D but that might be a bit overkill*/;
+
+	struct component *x_next, *y_next, *z_next,
+			 *x_prev, *y_prev, *z_prev,
+			 *child;
+
+	struct port port_x, port_y, port_z;
+
+	struct reg x_in, y_in, z_in, child_in;
+};
+
+static stat port_receive(struct torus3d_node *torus3d, struct port *port, struct reg *reg)
+{
+	if (!reg->busy)
+		return OK;
+
+	/* source */
+	uint8_t sx, sy, sz;
+	addr_torus3d(reg->pkt.from, &sx, &sy, &sz, NULL);
+
+	/* dst */
+	uint8_t dx, dy, dz;
+	addr_torus3d(reg->pkt.to, &dx, &dy, &dz, NULL);
+
+	/* Dally/spiral routing though I'm a bit unsure if this works for 2D/3D
+	 * toruses (1D seems to work, 2D not so much atm) */
+	int chan = 0;
+	if (port == &torus3d->port_x)
+		chan = sx < dx;
+	else if (port == &torus3d->port_y)
+		chan = sy < dy;
+	else if (port == &torus3d->port_z)
+		chan = sz < dz;
+	else
+		abort();
+
+	if (port->r[chan].busy)
+		return OK;
+
+	printf("(%d, %d, %d) to (%d, %d, %d) via (%d, %d, %d)\n",
+			sx, sy, sz,
+			dx, dy, dz,
+			torus3d->x, torus3d->y, torus3d->z);
+
+	port->r[chan].pkt = reg->pkt;
+	port->r[chan].busy = true;
+	reg->busy = false;
+	return OK;
+}
+
+static stat reg_receive(struct reg *r, struct packet pkt)
+{
+	if (r->busy)
+		return EBUSY;
+
+	r->pkt = pkt;
+	r->busy = true;
+	return OK;
+}
+
+enum match {
+	MX = (1 << 0),
+	MY = (1 << 1),
+	MZ = (1 << 2),
+	NX = (1 << 3),
+	NY = (1 << 4),
+	NZ = (1 << 5),
+};
+
+static void maybe_route_reg(struct torus3d_node *torus3d, struct reg *reg, enum match m, uint64_t *oldest, struct packet **pkt, bool **busy)
+{
+	if (!reg->busy)
+		return;
+
+	uint8_t x, y, z;
+	addr_torus3d(reg->pkt.to, &x, &y, &z, NULL);
+
+	if ((m & MX) && x != torus3d->x)
+		return;
+
+	if ((m & MY) && y != torus3d->y)
+		return;
+
+	if ((m & MZ) && z != torus3d->z)
+		return;
+
+	if ((m & NX) && x == torus3d->x)
+		return;
+
+	if ((m & NY) && y == torus3d->y)
+		return;
+
+	if ((m & NZ) && z == torus3d->z)
+		return;
+
+	if (reg->pkt.timestamp < *oldest) {
+		*oldest = reg->pkt.timestamp;
+		*busy = &reg->busy;
+		*pkt = &reg->pkt;
+	}
+}
+
+static void maybe_route_port(struct torus3d_node *torus3d, struct port *port, enum match m, uint64_t *oldest, struct packet **pkt, bool **busy)
+{
+	maybe_route_reg(torus3d, &port->r[0], m, oldest, pkt, busy);
+	maybe_route_reg(torus3d, &port->r[1], m, oldest, pkt, busy);
+}
+
+static stat route(struct torus3d_node *torus3d, struct component *next, enum match m)
+{
+	uint64_t oldest = -1; struct packet *pkt = NULL; bool *busy = NULL;
+	maybe_route_port(torus3d, &torus3d->port_x, m, &oldest, &pkt, &busy);
+	maybe_route_port(torus3d, &torus3d->port_y, m, &oldest, &pkt, &busy);
+	maybe_route_port(torus3d, &torus3d->port_z, m, &oldest, &pkt, &busy);
+	maybe_route_reg(torus3d, &torus3d->child_in, m, &oldest, &pkt, &busy);
+
+	/* no suitable match */
+	if (pkt == NULL)
+		return OK;
+
+	assert(busy);
+
+	stat ret = SEND(torus3d, next, *pkt);
+	if (ret == EBUSY)
+		return OK;
+
+	*busy = false;
+	return OK;
+}
+
+static stat torus3d_clock(struct torus3d_node *torus3d)
+{
+	stat ret = OK;
+	if ((ret = port_receive(torus3d, &torus3d->port_x, &torus3d->x_in)))
+		return ret;
+
+	if ((ret = port_receive(torus3d, &torus3d->port_y, &torus3d->y_in)))
+		return ret;
+
+	if ((ret = port_receive(torus3d, &torus3d->port_z, &torus3d->z_in)))
+		return ret;
+
+	if ((ret = route(torus3d, torus3d->x_next, NX)))
+		return ret;
+
+	if ((ret = route(torus3d, torus3d->y_next, NY | MX)))
+		return ret;
+
+	if ((ret = route(torus3d, torus3d->z_next, NZ | MX | MY)))
+		return ret;
+
+	if ((ret = route(torus3d, torus3d->child, MX | MY | MZ)))
+		return ret;
+
+	return OK;
+}
+
+static stat torus3d_receive(struct torus3d_node *torus3d, struct component *from, struct packet pkt)
+{
+	if (from == torus3d->child)
+		return reg_receive(&torus3d->child_in, pkt);
+
+	if (from == torus3d->x_prev)
+		return reg_receive(&torus3d->x_in, pkt);
+
+	if (from == torus3d->y_prev)
+		return reg_receive(&torus3d->y_in, pkt);
+
+	if (from == torus3d->z_prev)
+		return reg_receive(&torus3d->z_in, pkt);
+
+	abort();
+	return OK;
+}
+
+struct component *create_torus3d_node(uint8_t x, uint8_t y, uint8_t z)
+{
+	struct torus3d_node *node = calloc(1, sizeof(struct torus3d_node));
+	if (!node)
+		return NULL;
+
+	node->component.receive = (receive_callback)torus3d_receive;
+	node->component.clock = (clock_callback)torus3d_clock;
+	node->x = x;
+	node->y = y;
+	node->z = z;
+	return (struct component *)node;
+}
+
+stat torus3d_node_connect(struct component *node,
+		struct component *x_in,
+		struct component *y_in,
+		struct component *z_in,
+		struct component *child,
+		struct component *x_out,
+		struct component *y_out,
+		struct component *z_out)
+{
+	struct torus3d_node *n = (struct torus3d_node *)node;
+	n->x_prev = x_in;
+	n->y_prev = y_in;
+	n->z_prev = z_in;
+	n->child = child;
+	n->x_next = x_out;
+	n->y_next = y_out;
+	n->z_next = z_out;
+	return OK;
+}