commit 777ba028b2714818ea3599a3a06a590b79e3d7f9
parent 9a91e5e7fdb368d3eded4723714aba5ec3a1ea83
Author: Yersa Nordman <yersa@finwo.nl>
Date: Wed, 11 Oct 2023 22:14:19 +0200
Start of new link detection
Diffstat:
4 files changed, 173 insertions(+), 40 deletions(-)
diff --git a/src/main.c b/src/main.c
@@ -1,6 +1,10 @@
+#include <linux/if_ether.h>
+#include <linux/if_packet.h>
+#include <netinet/in.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
+#include <string.h>
#include <sys/epoll.h>
#include <sys/time.h>
#include <unistd.h>
@@ -11,17 +15,101 @@
#include "util/routing-table.h"
#include "util/socket.h"
+#define RCVBUFSIZ 131072
+
+unsigned char *rcvbuf = NULL;
static const char *const usage[] = {
__NAME " [options]",
NULL
};
+
int64_t millis() {
struct timeval tv;
gettimeofday(&tv, NULL);
return (tv.tv_sec * ((int64_t)1000)) + (tv.tv_usec / 1000);
}
+void handle_packet_bond(struct pmlag_bond *bond) {
+ printf("Packet on bond %s\n", bond->name);
+}
+void handle_packet_iface(struct pmlag_iface *iface) {
+ printf("Packet on iface %s\n", iface->name);
+ int send_len, buflen;
+ uint16_t ethtype;
+ uint16_t command;
+
+ // Read the packet
+ buflen = recvfrom(iface->sockfd, rcvbuf, RCVBUFSIZ, 0, NULL, 0);
+ if (buflen < 0) {
+ perror("recvfrom");
+ return;
+ }
+
+ // Basic error checking
+ // ethernet says minimum of 60, we just need 18
+ if (buflen < 18) return;
+
+ // TODO: track all packets in routing table
+
+ // Get the ethtype
+ memcpy(ðtype, rcvbuf + (2*ETH_ALEN), sizeof(ethtype));
+ ethtype = ntohs(ethtype);
+
+ printf("Read %d bytes (%d)\n", buflen, ethtype);
+
+ // Don't touch packets that don't use our described protocol
+ if (ethtype != 0x0666) {
+ send_len = write(iface->bond->sockfd, rcvbuf, buflen);
+ if (send_len != buflen) {
+ perror("write");
+ return;
+ }
+ }
+
+ // Ignore our own packets (loop detected, may do something with it later)
+ if (!memcmp(rcvbuf + ETH_ALEN, iface->bond->hwaddr, ETH_ALEN)) {
+ printf("Loop detected\n");
+ return;
+ }
+
+ // Fetch the command that was called
+ memcpy(&command, rcvbuf + (2*ETH_ALEN) + sizeof(ethtype), sizeof(command));
+ command = ntohs(command);
+
+ printf("TODO: do something with this %.2x:%.2x:%.2x:%.2x:%.2x:%.2x - %.4x - %.4x - %.4x\n",
+ rcvbuf[ 6],
+ rcvbuf[ 7],
+ rcvbuf[ 8],
+ rcvbuf[ 9],
+ rcvbuf[10],
+ rcvbuf[11],
+ (rcvbuf[12] << 8) + rcvbuf[13],
+ (rcvbuf[14] << 8) + rcvbuf[15],
+ iface->bond->bc_id
+ );
+
+ // TODO:
+ // - received untriggered bc with timer > 0: we're a follower now (timer=3)
+ // - received untriggered bc with timer <= 0: trigger + timer=rand(0/2)
+ // - received triggered bc: ignore
+ // - if triggered but received new bc, untrigger
+ // - if untriggered but received bc, set bc_timer to 0 or 1 (random)
+ // TODO: commit routing table upon broadcast id change
+ // a.k.a. cleanup old entries not seen since bcid - 2?
+}
+
+void handle_packet(void *entity) {
+ struct pmlag_iface *iface = entity;
+ struct pmlag_bond *bond = entity;
+ if (iface->type == PMLAG_ENTITY_TYPE_IFACE) {
+ return handle_packet_iface(iface);
+ }
+ if (bond->type == PMLAG_ENTITY_TYPE_IFACE) {
+ return handle_packet_bond(bond);
+ }
+}
+
int main(int argc, const char **argv) {
char *config_file="/etc/pmlag/pmlag.ini";
struct epoll_event *epev;
@@ -30,9 +118,9 @@ int main(int argc, const char **argv) {
struct pmlag_iface *iface;
// Seed random
- unsigned int seed;
+ uint32_t seed;
FILE* urandom = fopen("/dev/urandom", "r");
- fread(&seed, sizeof(int), 1, urandom);
+ fread(&seed, sizeof(uint32_t), 1, urandom);
fclose(urandom);
srand(seed);
@@ -60,6 +148,9 @@ int main(int argc, const char **argv) {
return 1;
}
+ // Initialize packet buffer
+ rcvbuf = (unsigned char *)malloc(RCVBUFSIZ);
+
// Initialize epoll
epfd = epoll_create1(0);
@@ -73,6 +164,12 @@ int main(int argc, const char **argv) {
}
}
+ // Prepare announce buffer
+ uint16_t ethtype = htons(0x0666);
+ struct sockaddr_ll saddr_ll;
+ size_t anc_buflen = (ETH_ALEN*2) + sizeof(ethtype) + sizeof(uint16_t) + sizeof(uint16_t); // mac + ethertype + command + broadcast-index
+ char *anc_buffer = malloc(anc_buflen);
+
// Wait for things to happen & call tick methods periodically
int ev_count;
struct epoll_event events[8];
@@ -83,8 +180,8 @@ int main(int argc, const char **argv) {
// Calculate waiting time & periodic actions
tdiff = ttime - millis();
if (tdiff <= 0) {
- ttime += 1000;
- tdiff += 1000;
+ ttime += 100;
+ tdiff += 100;
// Handle sockets
for( b = 0 ; b < config->bond_count ; b++ ) {
@@ -92,13 +189,15 @@ int main(int argc, const char **argv) {
// (re)open bond tap
bond = config->bond[b];
if (!bond->sockfd) {
- bond->sockfd = tap_alloc(bond->name, bond->hwaddr);
+ bond->sockfd = tap_alloc(bond->name, bond->hwaddr);
+ bond->state = 0;
+ bond->bc_timer = 3;
if (bond->sockfd < 0) {
perror("tap_alloc");
bond->sockfd = 0;
continue;
}
- printf("Opened tap for %s\n", bond->name);
+ /* printf("Opened tap for %s\n", bond->name); */
}
// (re)initialize routing table
@@ -106,7 +205,16 @@ int main(int argc, const char **argv) {
bond->rt = rt_init(bond);
}
- for( i = 0 ; i < config->bond[b]->iface_count ; i++ ) {
+ // Fetch bond's mac address if missing (from random)
+ if (!bond->hwaddr) {
+ bond->hwaddr = iface_mac(bond->name);
+ }
+
+ // Mark the bond as untriggered, decrement timer and increment bc_id
+ bond->state &= ~PMLAG_STATE_TRIGGERED;
+ bond->bc_timer--;
+
+ for( i = 0 ; i < bond->iface_count ; i++ ) {
iface = bond->iface[i];
// (re)open interface socket
@@ -117,7 +225,8 @@ int main(int argc, const char **argv) {
iface->sockfd = 0;
continue;
}
- printf("Opened socket for %s\n", iface->name);
+ iface->ifidx = iface_idx(iface->sockfd, iface->name);
+ /* printf("Opened socket for %s\n", iface->name); */
}
// Register it with epoll
@@ -137,23 +246,48 @@ int main(int argc, const char **argv) {
iface->epev = NULL;
continue;
}
- printf("Registered %s with epoll\n", iface->name);
+ /* printf("Registered %s with epoll\n", iface->name); */
+ }
+ }
+
+ // Handle broadcasts
+ if (bond->bc_timer < 0) {
+ bond->bc_timer = 0;
+
+ // Increment broadcast id
+ bond->bc_id = ntohs(bond->bc_id);
+ bond->bc_id++;
+ bond->bc_id = htons(bond->bc_id);
+
+ // Prepare packet
+ memcpy(saddr_ll.sll_addr , bond->hwaddr , ETH_ALEN);
+ memset(anc_buffer , 0xFF , ETH_ALEN); // Send to broadcast
+ memcpy(anc_buffer + (1*ETH_ALEN) , bond->hwaddr , ETH_ALEN); // From bond
+ memcpy(anc_buffer + (2*ETH_ALEN) , ðtype , sizeof(ethtype)); // ethtype 0x0666
+ memset(anc_buffer + (2*ETH_ALEN) + sizeof(ethtype) , 0x00 , sizeof(uint16_t)); // command (interface detection broadcast)
+ memcpy(anc_buffer + (2*ETH_ALEN) + sizeof(ethtype) + sizeof(uint16_t), &(bond->bc_id), sizeof(bond->bc_id)); // broadcast id
+
+ // Send packet on all interfaces
+ for( i=0; i < bond->iface_count ; i++ ) {
+ iface = bond->iface[i];
+ saddr_ll.sll_ifindex = iface->ifidx;
+ if (sendto(iface->sockfd, anc_buffer, anc_buflen, 0, (const struct sockaddr*)&saddr_ll, sizeof(struct sockaddr_ll)) != anc_buflen) {
+ perror("sendto");
+ }
}
}
}
-
- // TODO: time tick method (for announce handling)
- printf("ttime: %ld\n", ttime);
}
// Handle incoming data
+ if (tdiff < 0) tdiff = 0;
ev_count = epoll_wait(epfd, events, 8, tdiff);
for( i=0 ; i<ev_count; i++) {
- // TODO: process(events[i].data.ptr);
+ handle_packet(events[i].data.ptr);
}
}
- return 69;
+ return 0;
}
diff --git a/src/util/config.c b/src/util/config.c
@@ -16,8 +16,8 @@ static int config_load_handler(
) {
struct pmlag_bond *bond = NULL;
struct pmlag_iface *iface = NULL;
- int bond_idx = 0;
- int iface_idx = 0;
+ int bond_index = 0;
+ int iface_index = 0;
struct pmlag_configuration* config = (struct pmlag_configuration *) user;
printf("[%s].%s: %s\n", section, name, value);
@@ -25,9 +25,9 @@ static int config_load_handler(
if (!config->bond) config->bond = calloc(1, sizeof(void*));
// Find the bond being configured
- for( bond_idx = 0 ; bond_idx < config->bond_count ; bond_idx ++ ) {
- if (!strcmp(config->bond[bond_idx]->name, section)) {
- bond = config->bond[bond_idx];
+ for( bond_index = 0 ; bond_index < config->bond_count ; bond_index ++ ) {
+ if (!strcmp(config->bond[bond_index]->name, section)) {
+ bond = config->bond[bond_index];
break;
}
}
@@ -50,9 +50,9 @@ static int config_load_handler(
if (!bond->iface) bond->iface = calloc(1, sizeof(void*));
// Find the interface you're referencing
- for( iface_idx = 0 ; iface_idx < bond->iface_count ; iface_idx ++ ) {
- if (!strcmp(config->bond[bond_idx]->iface[iface_idx]->name, value)) {
- iface = config->bond[bond_idx]->iface[iface_idx];
+ for( iface_index = 0 ; iface_index < bond->iface_count ; iface_index ++ ) {
+ if (!strcmp(config->bond[bond_index]->iface[iface_index]->name, value)) {
+ iface = config->bond[bond_index]->iface[iface_index];
break;
}
}
@@ -70,24 +70,13 @@ static int config_load_handler(
} else if (!strcmp(name, "hwaddr")) {
// Find the interface you're referencing
- for( iface_idx = 0 ; iface_idx < bond->iface_count ; iface_idx ++ ) {
- if (!strcmp(config->bond[bond_idx]->iface[iface_idx]->name, value)) {
- iface = config->bond[bond_idx]->iface[iface_idx];
+ for( iface_index = 0 ; iface_index < bond->iface_count ; iface_index ++ ) {
+ if (!strcmp(config->bond[bond_index]->iface[iface_index]->name, value)) {
+ iface = config->bond[bond_index]->iface[iface_index];
break;
}
}
- if (bond->hwaddr) {
- printf("Already had MAC: %.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
- bond->hwaddr[0],
- bond->hwaddr[1],
- bond->hwaddr[2],
- bond->hwaddr[3],
- bond->hwaddr[4],
- bond->hwaddr[5]
- );
- }
-
if (iface) {
// Got interface by that name = use it's hwaddr
if (bond->hwaddr) free(bond->hwaddr);
@@ -111,7 +100,7 @@ static int config_load_handler(
}
}
- return 1;
+ return 0;
}
struct pmlag_configuration * config_load(char * filepath, struct pmlag_configuration *config) {
diff --git a/src/util/config.h b/src/util/config.h
@@ -1,13 +1,19 @@
#ifndef __PMLAG_UTIL_CONFIG_H__
#define __PMLAG_UTIL_CONFIG_H__
-#define PMLAG_ENTITY_TYPE int
-#define PMLAG_ENTITY_TYPE_IFACE 1
-#define PMLAG_ENTITY_TYPE_BOND 2
+#include <stdint.h>
+
+#define PMLAG_ENTITY_TYPE int
+#define PMLAG_ENTITY_TYPE_IFACE 1
+#define PMLAG_ENTITY_TYPE_BOND 2
+
+#define PMLAG_STATE int
+#define PMLAG_STATE_TRIGGERED 1
struct pmlag_iface {
PMLAG_ENTITY_TYPE type;
char *name;
+ int ifidx;
struct pmlag_bond *bond;
int sockfd;
struct epoll_event *epev;
@@ -16,6 +22,9 @@ struct pmlag_iface {
struct pmlag_bond {
PMLAG_ENTITY_TYPE type;
char *name;
+ PMLAG_STATE state;
+ int bc_timer;
+ uint16_t bc_id;
struct pmlag_iface **iface;
int iface_count;
int sockfd;
diff --git a/src/util/socket.h b/src/util/socket.h
@@ -1,6 +1,7 @@
#ifndef __PMLAG_UTIL_SOCKET_H__
#define __PMLAG_UTIL_SOCKET_H__
+int iface_idx(int sockfd, char * ifname);
unsigned char * iface_mac(char * ifname);
int tap_alloc(char * ifname, unsigned char * mac);
int sockraw_open(char * ifname);
