1 files changed, 63 insertions, 33 deletions

diff --git a/src/grid/node.c b/src/grid/node.c
index df237c7..512cae3 100644
--- a/src/grid/node.c
+++ b/src/grid/node.c
@@ -20,87 +20,117 @@ struct grid_node {
 	uint16_t x, y;
 
 	struct port left, right, up, down, lower;
+
+	unsigned priority;
 };
 
-static void port_clock(struct grid_node *grid, struct port *port)
+static stat port_clock(struct grid_node *grid, struct port *to, struct port *from)
 {
-	if (!port->busy)
-		return;
-
-	stat r = SEND(grid, port->send, port->pkt);
-	if (r == EBUSY)
-		return;
-
-	port->busy = false;
-}
+	assert(from->busy);
+	stat r = SEND(grid, to->send, from->pkt);
+	if (r == EBUSY) {
+		grid->priority++;
+		return OK;
+	}
 
-static stat grid_clock(struct grid_node *grid)
-{
-	port_clock(grid, &grid->left);
-	port_clock(grid, &grid->right);
-	port_clock(grid, &grid->up);
-	port_clock(grid, &grid->down);
-	port_clock(grid, &grid->lower);
+	from->busy = false;
 	return OK;
 }
 
-static stat port_receive(struct port *port, struct packet pkt)
+static struct port *select_input(struct grid_node *grid)
 {
-	if (port->busy)
-		return EBUSY;
+	struct port *ports[5] = {
+		&grid->left,
+		&grid->right,
+		&grid->up,
+		&grid->down,
+		&grid->lower,
+	};
+	for (size_t i = 0; i < 5; ++i) {
+		size_t idx = (i + grid->priority) % 5;
+		struct port *port = ports[idx];
+		if (!port)
+			continue;
 
-	port->pkt = pkt;
-	port->busy = true;
-	return OK;
+		if (!port->busy)
+			continue;
+
+		return port;
+	}
+
+	return NULL;
 }
 
-static stat grid_receive(struct grid_node *grid, struct component *from, struct packet pkt)
+static stat clock_once(struct grid_node *grid)
 {
-	if (rand() % 256 == 0)
-		return EBUSY;
+	struct port *input = select_input(grid);
+	if (!input)
+		return OK;
 
 	uint16_t x;
 	uint16_t y;
-	uint64_t addr = pkt.to;
+	uint64_t addr = input->pkt.to;
 	addr_grid(addr, NULL, &x, &y);
 
 	if (grid->x == x && grid->y == y) {
 		if (!grid->lower.send)
 			goto nosuch;
 
-		return port_receive(&grid->lower, pkt);
+		return port_clock(grid, &grid->lower, input);
 	}
 
 	if (y < grid->y) {
 		if (!grid->down.send)
 			goto nosuch;
 
-		return port_receive(&grid->down, pkt);
+		return port_clock(grid, &grid->down, input);
 	}
 
 	if (y > grid->y) {
 		if (!grid->up.send)
 			goto nosuch;
 
-		return port_receive(&grid->up, pkt);
+		return port_clock(grid, &grid->up, input);
 	}
 
 	if (x < grid->x) {
 		if (!grid->left.send)
 			goto nosuch;
 
-		return port_receive(&grid->left, pkt);
+		return port_clock(grid, &grid->left, input);
 	}
 
 	if (x > grid->x) {
 		if (!grid->right.send)
 			goto nosuch;
 
-		return port_receive(&grid->right, pkt);
+		return port_clock(grid, &grid->right, input);
 	}
 
 nosuch:
-	set_flags(&pkt, PACKET_ERROR);
+	abort();
+	return OK;
+}
+
+static stat grid_clock(struct grid_node *grid)
+{
+	clock_once(grid);
+	return OK;
+}
+
+
+static stat port_receive(struct port *port, struct packet pkt)
+{
+	if (port->busy)
+		return EBUSY;
+
+	port->pkt = pkt;
+	port->busy = true;
+	return OK;
+}
+
+static stat grid_receive(struct grid_node *grid, struct component *from, struct packet pkt)
+{
 	if (from == grid->lower.send)
 		return port_receive(&grid->lower, pkt);