From 73071f996142d7cffe61a1922089f8e8b2928e64 Mon Sep 17 00:00:00 2001
From: Matthias Schiffer <mschiffer@universe-factory.net>
Date: Mon, 18 Mar 2013 01:36:34 +0100
Subject: [PATCH] Implement packet queueing

---
 include/gmrf/gmrf.h | 14 +++++++++++---
 mmss/iface.c        | 36 +++++++++++++++++++++++++++++-------
 mmss/mmss.c         |  5 ++++-
 mmss/mmss.h         |  5 +++--
 4 files changed, 47 insertions(+), 13 deletions(-)

diff --git a/include/gmrf/gmrf.h b/include/gmrf/gmrf.h
index 02442b1..05657fd 100644
--- a/include/gmrf/gmrf.h
+++ b/include/gmrf/gmrf.h
@@ -30,24 +30,32 @@
 #include <stdbool.h>
 #include <stddef.h>
 #include <stdint.h>
+#include <string.h>
 
 
 typedef struct gmrf_context gmrf_context_t;
 typedef struct gmrf_iface gmrf_iface_t;
 
 
+#define GMRF_ADDR_LEN 8
+
 typedef struct gmrf_addr {
-	uint8_t d[8];
+	uint8_t d[GMRF_ADDR_LEN];
 } gmrf_addr_t;
 
 
 static const gmrf_addr_t gmrf_addr_unspec = {};
 
 
+static inline bool gmrf_addr_equal(const gmrf_addr_t *addr1, const gmrf_addr_t *addr2) {
+	return (memcmp(addr1->d, addr2->d, GMRF_ADDR_LEN) == 0);
+}
+
+
 gmrf_addr_t gmrf_iface_get_addr(gmrf_context_t *ctx, gmrf_iface_t *iface);
 size_t gmrf_iface_get_mtu(gmrf_context_t *ctx, gmrf_iface_t *iface);
 
-bool gmrf_iface_send(gmrf_context_t *ctx, gmrf_iface_t *iface, void *data, size_t len, gmrf_addr_t dest);
-bool gmrf_iface_send_bc(gmrf_context_t *ctx, gmrf_iface_t *iface, void *data, size_t len);
+bool gmrf_iface_send(gmrf_context_t *ctx, gmrf_iface_t *iface, const void *data, size_t len, const gmrf_addr_t *dest);
+bool gmrf_iface_send_bc(gmrf_context_t *ctx, gmrf_iface_t *iface, const void *data, size_t len);
 
 #endif /* _GMRF_GMRF_H_ */
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_bc(gmrf_context_t *ctx, gmrf_iface_t *iface, void *data, size_t len) {
-	errno = ENOSYS;
-	return false;
+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, 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[];