X-Git-Url: http://git.sameswireless.fr/l2tpns.git/blobdiff_plain/c5e4c2cfc092317c1e3ceb0f654f880f6a8573f4..4b119fa4bf3455ce5eb17e9bccfd2eb1baac7f12:/control.c?ds=sidebyside

diff --git a/control.c b/control.c
index 9d83522..092ab8e 100644
--- a/control.c
+++ b/control.c
@@ -1,70 +1,164 @@
-#include <stdio.h>
-#include <arpa/inet.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <malloc.h>
-#include <netdb.h>
+// L2TPNS: control
+
 #include <string.h>
-#include <sys/time.h>
-#include <sys/types.h>
 #include <sys/socket.h>
-#include <netinet/in.h>
-#include <signal.h>
-#include <stdarg.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <time.h>
+#include <linux/rtnetlink.h>
+
+#include "l2tpns.h"
 #include "control.h"
 
-int new_packet(short type, char *packet)
+int pack_control(uint8_t *data, int len, uint8_t type, int argc, char *argv[])
 {
-	int id = (time(NULL) ^ (rand() * 1024*1024));
+    struct nsctl_packet pkt;
+    struct nsctl_args arg;
+    char *p = pkt.argv;
+    int sz = (p - (char *) &pkt);
+
+    if (len > sizeof(pkt))
+    	len = sizeof(pkt);
+
+    if (argc > 0xff)
+    	argc = 0xff; // paranoia
+
+    pkt.magic = ntohs(NSCTL_MAGIC);
+    pkt.type = type;
+    pkt.argc = argc;
+
+    while (argc-- > 0)
+    {
+	char *a = *argv++;
+	int s = strlen(a);
+
+	if (s > sizeof(arg.value))
+		s = sizeof(arg.value); // silently truncate
+
+	arg.len = s;
+	s += sizeof(arg.len);
+
+	if (sz + s > len)
+	    return -1; // overflow
 
-	*(short *)(packet + 0) = ntohs(0x9012);
-	*(short *)(packet + 2) = ntohs(type);
-	*(int *)(packet + 6) = ntohl(id);
+	if (arg.len)
+	    memcpy(arg.value, a, arg.len);
 
-	return 10;
+	memcpy(p, &arg, s);
+	sz += s;
+	p += s;
+    }
+
+    /*
+     * terminate:  this is both a sanity check and additionally
+     * ensures that there's a spare byte in the packet to null
+     * terminate the last argument when unpacking (see unpack_control)
+     */
+    if (sz + sizeof(arg.len) > len)
+    	return -1; // overflow
+
+    arg.len = 0xff;
+    memcpy(p, &arg.len, sizeof(arg.len));
+
+    sz += sizeof(arg.len);
+    memcpy(data, &pkt, sz);
+
+    return sz;
 }
 
-int send_packet(int sockfd, int dest_ip, int dest_port, char *packet, int len)
+int unpack_control(struct nsctl *control, uint8_t *data, int len)
 {
-	struct sockaddr_in addr;
+    struct nsctl_packet pkt;
+    char *p = pkt.argv;
+    int sz = (p - (char *) &pkt);
+    int i;
+
+    if (len < sz)
+    	return NSCTL_ERR_SHORT;
+
+    if (len > sizeof(pkt))
+    	return NSCTL_ERR_LONG;
+
+    memcpy(&pkt, data, len);
+    if (ntohs(pkt.magic) != NSCTL_MAGIC)
+    	return NSCTL_ERR_MAGIC;
+
+    switch (pkt.type)
+    {
+    case NSCTL_REQ_LOAD:
+    case NSCTL_REQ_UNLOAD:
+    case NSCTL_REQ_HELP:
+    case NSCTL_REQ_CONTROL:
+    case NSCTL_RES_OK:
+    case NSCTL_RES_ERR:
+	control->type = pkt.type;
+	break;
 
-	*(short *)(packet + 4) = ntohs(len);
+    default:
+	return NSCTL_ERR_TYPE;
+    }
 
-	memset(&addr, 0, sizeof(addr));
-	addr.sin_family = AF_INET;
-	*(int*)&addr.sin_addr = htonl(dest_ip);
-	addr.sin_port = htons(dest_port);
-	if (sendto(sockfd, packet, len, 0, (void *) &addr, sizeof(addr)) < 0)
+    control->argc = pkt.argc;
+    for (i = 0; i <= control->argc; i++)
+    {
+	unsigned s;
+
+	if (len < sz + 1)
+	    return NSCTL_ERR_SHORT;
+
+	s = (uint8_t) *p;
+	*p++ = 0; // null terminate previous arg
+	sz++;
+
+	if (i < control->argc)
+	{
+	    if (len < sz + s)
+		return NSCTL_ERR_SHORT;
+
+	    control->argv[i] = p;
+	    p += s;
+	    sz += s;
+	}
+	else
 	{
-		perror("sendto");
-		return 0;
+	    /* check for terminator */
+	    if (s != 0xff)
+	    	return NSCTL_ERR_SHORT;
 	}
-	return 1;
-}
+    }
 
-int read_packet(int sockfd, char *packet)
-{
-	struct sockaddr_in addr;
-	int alen = sizeof(addr);
-	memset(&addr, 0, sizeof(addr));
-	return recvfrom(sockfd, packet, 1400, 0, (void *) &addr, &alen);
+    if (sz != len)
+    	return NSCTL_ERR_LONG; // trailing cr*p
+
+    return control->type;
 }
 
-void dump_packet(char *packet, FILE *stream)
+void dump_control(struct nsctl *control, FILE *stream)
 {
-	if (htons(*(short *)(packet + 0)) != 0x9012)
-	{
-		fprintf(stream, "Invalid packet identifier %x\n", htons(*(short *)(packet + 0)));
-		return;
-	}
-	fprintf(stream, "Control packet:\n");
-	fprintf(stream, "	Type: %d\n", htons(*(short *)(packet + 2)));
-	fprintf(stream, "	Length: %d\n", htons(*(short *)(packet + 4)));
-	fprintf(stream, "	Identifier: %x\n", htonl(*(int *)(packet + 6)));
-	fprintf(stream, "\n");
-}
+    char *type = "*unknown*";
 
+    if (!stream)
+    	stream = stdout;
 
+    switch (control->type)
+    {
+    case NSCTL_REQ_LOAD:	type = "NSCTL_REQ_LOAD";	break;
+    case NSCTL_REQ_UNLOAD:	type = "NSCTL_REQ_UNLOAD";	break;
+    case NSCTL_REQ_HELP:	type = "NSCTL_REQ_HELP";	break;
+    case NSCTL_REQ_CONTROL:	type = "NSCTL_REQ_CONTROL";	break;
+    case NSCTL_RES_OK:		type = "NSCTL_RES_OK";		break;
+    case NSCTL_RES_ERR:		type = "NSCTL_RES_ERR";		break;
+    }
+
+    fprintf(stream, "Control packet:\n");
+    fprintf(stream, "	Type: %d (%s)\n", (int) control->type, type);
+    fprintf(stream, "	Args: %d", (int) control->argc);
+    if (control->argc)
+    {
+	int i;
+	fprintf(stream, " (\"");
+	for (i = 0; i < control->argc; i++)
+	    fprintf(stream, "%s%s", i ? "\", \"" : "", control->argv[i]);
+
+	fprintf(stream, "\")");
+    }
+
+    fprintf(stream, "\n\n");
+}