1 files changed, 98 insertions, 16 deletions

diff --git a/ffd/ffd.c b/ffd/ffd.c
index d182ee0..ce71d4b 100644
--- a/ffd/ffd.c
+++ b/ffd/ffd.c
@@ -26,6 +26,7 @@
 
 #include "ffd.h"
 #include "netif.h"
+#include "packet.h"
 
 #include <errno.h>
 #include <poll.h>
@@ -36,6 +37,9 @@
 
 #include <netpacket/packet.h>
 
+#include <sys/socket.h>
+#include <sys/uio.h>
+
 
 #define FFD_PROTO 0xffd
 #define FFD_VERSION 0
@@ -46,18 +50,27 @@ static const eth_addr_t ffd_addr = {{0x03, 0x00, 0x00, 0x00, 0x0f, 0xfd}};
 
 
 static char *mesh = "bat0";
-static unsigned mtu = 1500;
+static unsigned mtu = 1528;
 
 static int sockfd;
 static struct timespec now;
 
-//static ffd_neigh_t self = {NULL, ETH_ADDR_UNSPEC, 0};
-//static ffd_orig_t own_data = {NULL, ETH_ADDR_UNSPEC, 0, NULL};
+static ffd_neigh_t self;
+static ffd_orig_t own_data;
 
+/* neighs and origs that have been changed must be moved to front */
 //static ffd_neigh_t *neigh_data = NULL;
 static ffd_orig_t *orig_data = NULL;
 
 
+static inline bool use_netif(const char *ifname) {
+	char *if_mesh = netif_get_mesh(ifname);
+	bool ret = (if_mesh && !strcmp(if_mesh, mesh));
+	free(if_mesh);
+
+	return ret;
+}
+
 static void update_time() {
 	clock_gettime(CLOCK_MONOTONIC, &now);
 }
@@ -71,6 +84,22 @@ static bool check_config() {
 	return true;
 }
 
+static bool init_self() {
+	eth_addr_t primary_addr = netif_mesh_get_primary_addr(mesh);
+	if (is_eth_addr_unspec(&primary_addr))
+		return false;
+
+	memset(&self, 0, sizeof(self));
+	memset(&own_data, 0, sizeof(own_data));
+
+	self.addr = own_data.addr = primary_addr;
+
+	random_bytes(&self.rev, sizeof(self.rev));
+	random_bytes(&own_data.rev, sizeof(own_data.rev));
+
+	return true;
+}
+
 static bool init_socket() {
 	sockfd = socket(AF_PACKET, SOCK_DGRAM, htons(FFD_PROTO));
 	if (sockfd < 0) {
@@ -81,14 +110,6 @@ static bool init_socket() {
 	return true;
 }
 
-static inline bool use_netif(const char *ifname) {
-	char *if_mesh = netif_get_mesh(ifname);
-	bool ret = (if_mesh && !strcmp(if_mesh, mesh));
-	free(if_mesh);
-
-	return ret;
-}
-
 static void join_mcast(const char *ifname, unsigned ifindex, void *arg) {
 	if (!use_netif(ifname))
 		return;
@@ -103,28 +124,89 @@ static void join_mcast(const char *ifname, unsigned ifindex, void *arg) {
 		fprintf(stderr, "warning: setsockopt: %m\n");
 }
 
-static void send_announce() {
+static bool send_eth(const eth_addr_t *addr, unsigned ifindex, void *buf, size_t len) {
+	static const uint8_t zeros[46] = {0};
+
+	struct sockaddr_ll sa;
+	memset(&sa, 0, sizeof(sa));
+	sa.sll_family = AF_PACKET;
+	sa.sll_protocol = htons(FFD_PROTO);
+	sa.sll_ifindex = ifindex;
+	sa.sll_halen = ETH_ALEN;
+	memcpy(sa.sll_addr, addr->d, ETH_ALEN);
+
+	struct iovec vec[2] = {
+		{ .iov_base = buf, .iov_len = len },
+		{ .iov_base = (void*)zeros, .iov_len = sizeof(zeros) - len },
+	};
+
+	struct msghdr msg;
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_name = &sa;
+	msg.msg_namelen = sizeof(sa);
+	msg.msg_iov = vec;
+	msg.msg_iovlen = (len < 46) ? 2 : 1;
+
+
+	while (sendmsg(sockfd, &msg, 0) < 0) {
+		if (errno != EINTR)
+			return false;
+	}
+
+	return true;
+}
+
+static void send_announce(const char *ifname, unsigned ifindex, void *arg) {
+	if (!use_netif(ifname))
+		return;
+
+	ffd_packet_announce_t *announce = arg;
+
+	if (!send_eth(&ffd_addr, ifindex, announce, PACKET_ANNOUNCE_SIZE(announce->n_origs)))
+		fprintf(stderr, "send_eth: %m\n");
+}
+
+static void send_announces() {
 	if (!orig_data)
 		return;
+
+	ffd_packet_announce_t *announce = alloca(PACKET_ANNOUNCE_SIZE(PACKET_ANNOUNCE_MAX_ORIGS));
+
+	memset(announce, 0, PACKET_ANNOUNCE_SIZE(PACKET_ANNOUNCE_MAX_ORIGS));
+
+	announce->version = FFD_VERSION;
+	announce->type = PACKET_ANNOUNCE;
+	announce->self_rev = self.rev;
+
+	ffd_orig_t *orig;
+	for (orig = orig_data; orig && (announce->n_origs < PACKET_ANNOUNCE_MAX_ORIGS); orig = orig->next)
+		announce->origs[announce->n_origs++] = orig->rev;
+
+	netif_foreach(send_announce, announce);
 }
 
 static void receive_packet() {
 	uint8_t buf[mtu];
+	struct sockaddr_ll from;
+	socklen_t fromlen = sizeof(from);
 
-	int readlen = read(sockfd, buf, sizeof(buf));
+	ssize_t readlen = recvfrom(sockfd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &fromlen);
 
 	if (readlen < 0) {
-		fprintf(stderr, "error: read: %m\n");
+		fprintf(stderr, "error: recvfrom: %m\n");
 		return;
 	}
 
-	fprintf(stderr, "debug: read %i bytes.\n", readlen);
+	fprintf(stderr, "debug: read %zi bytes.\n", readlen);
 }
 
 int main() {
 	if (!check_config())
 		return 1;
 
+	if (!init_self())
+		return 1;
+
 	if (!init_socket())
 		return 1;
 
@@ -138,7 +220,7 @@ int main() {
 		int timeout = timespec_diff(&next_announce, &now);
 
 		if (timeout <= 0) {
-			send_announce();
+			send_announces();
 
 			next_announce.tv_sec += ANNOUNCE_INTERVAL;
 			continue;