path: root/src/mesh/node2d.c

#include <gran/mesh/node2d.h>

#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 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 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]

struct reg {
	struct packet pkt;
	bool busy;
};

struct node2d {
	struct component component;
	uint16_t elems;
	uint16_t x, y;

	uint64_t timestamp;

	struct reg *in; /* countedby[elems + 4] */
	struct reg *out; /* countedby[elems + 4] */
	struct component **ports; /* countedby[elems + 4] */
};

static void clock_outputs(struct node2d *n)
{
	for (int i = 0; i < n->elems + 4; ++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 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 void propagate(struct reg *out,
		size_t count, struct reg *in[static count],
		bool (*sel)(struct reg *r, void *data), void *data)
{
	struct reg *r = NULL;
	for (size_t i = 0; i < count; ++i) {
		if (!in[i] || !in[i]->busy)
			continue;

		if (!sel(in[i], data))
			continue;

		if (!r || r->pkt.timestamp > in[i]->pkt.timestamp)
			r = in[i];
	}

	if (!r)
		return;

	copy_reg(out, r);
}

struct sel_helper {
	uint8_t x, y;
	uint16_t elem;
};

static bool north_sel(struct reg *r, void *data)
{
	uint8_t y = 0;
	struct sel_helper *helper = data;
	addr_mesh2d(r->pkt.to, NULL, &y, NULL, NULL);
	return y > helper->y;
}

static bool south_sel(struct reg *r, void *data)
{
	uint8_t y = 0;
	struct sel_helper *helper = data;
	addr_mesh2d(r->pkt.to, NULL, &y, NULL, NULL);
	return y < helper->y;
}

static bool east_sel(struct reg *r, void *data)
{
	uint8_t x = 0, y = 0;
	struct sel_helper *helper = data;
	addr_mesh2d(r->pkt.to, &x, &y, NULL, NULL);
	return y == helper->y && x > helper->x;
}

static bool west_sel(struct reg *r, void *data)
{
	uint8_t x = 0, y = 0;
	struct sel_helper *helper = data;
	addr_mesh2d(r->pkt.to, &x, &y, NULL, NULL);
	return y == helper->y && x < helper->x;
}

static bool elem_sel(struct reg *r, void *data)
{
	uint16_t elem = 0;
	uint8_t x = 0, y = 0;
	struct sel_helper *helper = data;
	addr_mesh2d(r->pkt.to, &x, &y, &elem, NULL);
	return x == helper->x && y == helper->y && elem == helper->elem;
}

static stat node2d_clock(struct node2d *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];
	}

	struct sel_helper helper = {
		.elem = 0,
		.x = n->x,
		.y = n->y,
	};

	struct reg *north[] = {r, &east_in(n),  &south_in(n), &west_in(n)};
	struct reg *east[]  = {r, &north_in(n), &south_in(n), &west_in(n)};
	struct reg *south[] = {r, &north_in(n), &east_in(n),  &west_in(n)};
	struct reg *west[]  = {r, &north_in(n), &east_in(n),  &south_in(n)};

	propagate(&north_out(n), 4, north, north_sel, &helper);
	propagate(&east_out(n),  4, east,  east_sel,  &helper);
	propagate(&south_out(n), 4, south, south_sel, &helper);
	propagate(&west_out(n),  4, west,  west_sel,  &helper);

	struct reg *all[] = {r, &north_in(n), &east_in(n), &south_in(n), &west_in(n)};
	for (int i = 0; i < n->elems; ++i) {
		helper.elem = i;
		propagate(&n->out[i], 5, all, elem_sel, &helper);
	}

	return OK;
}

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 stat node2d_receive(struct node2d *n, struct component *from, struct packet pkt)
{
	for (int i = 0; i < n->elems + 4; ++i) {
		if (from != n->ports[i])
			continue;

		if (i < n->elems)
			pkt.timestamp = n->timestamp;

		return reg_busy(&n->in[i], pkt);
	}

	abort();
	return OK;
}

struct component *create_mesh_node2d(uint8_t x, uint8_t y, uint16_t elems)
{
	struct node2d *n = calloc(1, sizeof(struct node2d));
	if (!n)
		return NULL;

	n->in = calloc(elems + 4, sizeof(struct reg));
	if (!n->in) {
		free(n);
		return NULL;
	}

	n->out = calloc(elems + 4, sizeof(struct reg));
	if (!n->out) {
		free(n->in);
		free(n);
		return NULL;
	}

	n->ports = calloc(elems + 4, sizeof(struct component *));
	if (!n->ports) {
		free(n->out);
		free(n->in);
		free(n);
		return NULL;
	}

	n->component.receive = (receive_callback)node2d_receive;
	n->component.clock = (clock_callback)node2d_clock;
	n->elems = elems;
	n->x = x;
	n->y = y;
	return (struct component *)n;
}

stat mesh_node2d_connect(struct component *c, struct component *e, uint16_t elem)
{
	struct node2d *n = (struct node2d *)c;
	if (elem >= n->elems)
		return ENOSUCH;

	if (n->ports[elem])
		return EEXISTS;

	n->ports[elem] = e;
	return OK;
}

stat mesh_node2d_connect_north(struct component *c, struct component *e)
{
	struct node2d *n = (struct node2d *)c;
	if (north_port(n))
		return EEXISTS;

	north_port(n) = e;
	return OK;
}

stat mesh_node2d_connect_east(struct component *c, struct component *e)
{
	struct node2d *n = (struct node2d *)c;
	if (east_port(n))
		return EEXISTS;

	east_port(n) = e;
	return OK;
}

stat mesh_node2d_connect_south(struct component *c, struct component *e)
{
	struct node2d *n = (struct node2d *)c;
	if (south_port(n))
		return EEXISTS;

	south_port(n) = e;
	return OK;
}

stat mesh_node2d_connect_west(struct component *c, struct component *e)
{
	struct node2d *n = (struct node2d *)c;
	if (west_port(n))
		return EEXISTS;

	west_port(n) = e;
	return OK;
}