fix nak increment

[l2tpns.git] / control.c

diff --git a/control.c b/control.c
index 9d83522..805a90b 100644 (file)
--- a/control.c
+++ b/control.c
@@ -1,70 +1,163 @@
-#include <stdio.h>
-#include <arpa/inet.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <malloc.h>
-#include <netdb.h>
+// L2TPNS: control
+
+char const *cvs_id_control = "$Id: control.c,v 1.5 2005-07-31 10:04:10 bodea Exp $";
+
 #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 "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");
+}