Implement packet queueing

3 files changed, 36 insertions, 10 deletions

diff --git a/mmss/iface.c b/mmss/iface.c
index 80aead9..0661c0e 100644
--- a/mmss/iface.c
+++ b/mmss/iface.c
@@ -26,7 +26,7 @@
 
 #include "mmss.h"
 
-#include <errno.h>
+#include <stdlib.h>
 
 
 gmrf_addr_t gmrf_iface_get_addr(gmrf_context_t *ctx, gmrf_iface_t *iface) {
@@ -37,12 +37,34 @@ size_t gmrf_iface_get_mtu(gmrf_context_t *ctx, gmrf_iface_t *iface) {
 	return iface->net->mtu;
 }
 
-bool gmrf_iface_send(gmrf_context_t *ctx, gmrf_iface_t *iface, void *data, size_t len, gmrf_addr_t dest) {
-	errno = ENOSYS;
-	return false;
+static void enqueue(mmss_context_t *ctx, gmrf_iface_t *source, gmrf_iface_t *dest, const void *data, size_t len) {
+	mmss_packet_t *packet = calloc(1, sizeof(mmss_packet_t) + len);
+
+	packet->sent = ctx->now;
+	packet->source = source;
+	packet->dest = dest;
+	packet->len = len;
+	memcpy(packet->data, data, len);
+
+	mmss_queue_put(ctx, &ctx->packet_queue, packet, ctx->now+1);
+}
+
+bool gmrf_iface_send(gmrf_context_t *ctx, gmrf_iface_t *iface, const void *data, size_t len, const gmrf_addr_t *dest) {
+	gmrf_iface_t *dest_iface;
+	for (dest_iface = iface->net->interfaces; dest_iface; dest_iface = dest_iface->network_next) {
+		if (gmrf_addr_equal(&dest_iface->address, dest)) {
+			enqueue(ctx->mmss, iface, dest_iface, data, len);
+			break;
+		}
+	}
+
+	return true;
 }
 
-bool gmrf_iface_send_bc(gmrf_context_t *ctx, gmrf_iface_t *iface, void *data, size_t len) {
-	errno = ENOSYS;
-	return false;
+bool gmrf_iface_send_bc(gmrf_context_t *ctx, gmrf_iface_t *iface, const void *data, size_t len) {
+	gmrf_iface_t *dest_iface;
+	for (dest_iface = iface->net->interfaces; dest_iface; dest_iface = dest_iface->network_next)
+		enqueue(ctx->mmss, iface, dest_iface, data, len);
+
+	return true;
 }
diff --git a/mmss/mmss.c b/mmss/mmss.c
index a0ae643..bcdf3a8 100644
--- a/mmss/mmss.c
+++ b/mmss/mmss.c
@@ -28,7 +28,7 @@
 
 
 int main() {
-	//mmss_context_t ctx = {0};
+	mmss_context_t ctx = { .now = 0 };
 
 	mmss_network_t net = { .mtu = 1500 };
 	mmss_node_t node1 = {}, node2 = {};
@@ -48,5 +48,8 @@ int main() {
 	iface2.network_next = &iface1;
 	net.interfaces = &iface2;
 
+	gmrf_context_t ctx_node1 = {&ctx, &node1};
+	gmrf_context_t ctx_node2 = {&ctx, &node2};
+
 	return 0;
 }
diff --git a/mmss/mmss.h b/mmss/mmss.h
index 0de6231..81a446f 100644
--- a/mmss/mmss.h
+++ b/mmss/mmss.h
@@ -34,13 +34,13 @@
 
 struct mmss_context {
 	uint64_t now;
+	mmss_queue_t packet_queue;
 };
 
 struct mmss_node {
 	mmss_node_t *next;
 
 	gmrf_iface_t *interfaces;
-	mmss_queue_t *packet_queue;
 };
 
 struct mmss_network {
@@ -54,7 +54,8 @@ struct mmss_network {
 struct mmss_packet {
 	uint64_t sent;
 
-	gmrf_addr_t source;
+	gmrf_iface_t *source;
+	gmrf_iface_t *dest;
 
 	size_t len;
 	uint8_t data[];