control.c

   1 // L2TPNS: control
   2
   3 #include <string.h>
   4 #include <linux/rtnetlink.h>
   5 #include <netinet/ip6.h>
   6 #include "dhcp6.h"
   7
   8 #include "l2tpns.h"
   9 #include "control.h"
  10
  11 int pack_control(uint8_t *data, int len, uint8_t type, int argc, char *argv[])
  12 {
  13     struct nsctl_packet pkt;
  14     struct nsctl_args arg;
  15     char *p = pkt.argv;
  16     int sz = (p - (char *) &pkt);
  17
  18     if (len > sizeof(pkt))
  19         len = sizeof(pkt);
  20
  21     if (argc > 0xff)
  22         argc = 0xff; // paranoia
  23
  24     pkt.magic = ntohs(NSCTL_MAGIC);
  25     pkt.type = type;
  26     pkt.argc = argc;
  27
  28     while (argc-- > 0)
  29     {
  30         char *a = *argv++;
  31         int s = strlen(a);
  32
  33         if (s > sizeof(arg.value))
  34                 s = sizeof(arg.value); // silently truncate
  35
  36         arg.len = s;
  37         s += sizeof(arg.len);
  38
  39         if (sz + s > len)
  40             return -1; // overflow
  41
  42         if (arg.len)
  43             memcpy(arg.value, a, arg.len);
  44
  45         memcpy(p, &arg, s);
  46         sz += s;
  47         p += s;
  48     }
  49
  50     /*
  51      * terminate:  this is both a sanity check and additionally
  52      * ensures that there's a spare byte in the packet to null
  53      * terminate the last argument when unpacking (see unpack_control)
  54      */
  55     if (sz + sizeof(arg.len) > len)
  56         return -1; // overflow
  57
  58     arg.len = 0xff;
  59     memcpy(p, &arg.len, sizeof(arg.len));
  60
  61     sz += sizeof(arg.len);
  62     memcpy(data, &pkt, sz);
  63
  64     return sz;
  65 }
  66
  67 int unpack_control(struct nsctl *control, uint8_t *data, int len)
  68 {
  69     struct nsctl_packet pkt;
  70     char *p = pkt.argv;
  71     int sz = (p - (char *) &pkt);
  72     int i;
  73
  74     if (len < sz)
  75         return NSCTL_ERR_SHORT;
  76
  77     if (len > sizeof(pkt))
  78         return NSCTL_ERR_LONG;
  79
  80     memcpy(&pkt, data, len);
  81     if (ntohs(pkt.magic) != NSCTL_MAGIC)
  82         return NSCTL_ERR_MAGIC;
  83
  84     switch (pkt.type)
  85     {
  86     case NSCTL_REQ_LOAD:
  87     case NSCTL_REQ_UNLOAD:
  88     case NSCTL_REQ_HELP:
  89     case NSCTL_REQ_CONTROL:
  90     case NSCTL_RES_OK:
  91     case NSCTL_RES_ERR:
  92         control->type = pkt.type;
  93         break;
  94
  95     default:
  96         return NSCTL_ERR_TYPE;
  97     }
  98
  99     control->argc = pkt.argc;
 100     for (i = 0; i <= control->argc; i++)
 101     {
 102         unsigned s;
 103
 104         if (len < sz + 1)
 105             return NSCTL_ERR_SHORT;
 106
 107         s = (uint8_t) *p;
 108         *p++ = 0; // null terminate previous arg
 109         sz++;
 110
 111         if (i < control->argc)
 112         {
 113             if (len < sz + s)
 114                 return NSCTL_ERR_SHORT;
 115
 116             control->argv[i] = p;
 117             p += s;
 118             sz += s;
 119         }
 120         else
 121         {
 122             /* check for terminator */
 123             if (s != 0xff)
 124                 return NSCTL_ERR_SHORT;
 125         }
 126     }
 127
 128     if (sz != len)
 129         return NSCTL_ERR_LONG; // trailing cr*p
 130
 131     return control->type;
 132 }
 133
 134 void dump_control(struct nsctl *control, FILE *stream)
 135 {
 136     char *type = "*unknown*";
 137
 138     if (!stream)
 139         stream = stdout;
 140
 141     switch (control->type)
 142     {
 143     case NSCTL_REQ_LOAD:        type = "NSCTL_REQ_LOAD";        break;
 144     case NSCTL_REQ_UNLOAD:      type = "NSCTL_REQ_UNLOAD";      break;
 145     case NSCTL_REQ_HELP:        type = "NSCTL_REQ_HELP";        break;
 146     case NSCTL_REQ_CONTROL:     type = "NSCTL_REQ_CONTROL";     break;
 147     case NSCTL_RES_OK:          type = "NSCTL_RES_OK";          break;
 148     case NSCTL_RES_ERR:         type = "NSCTL_RES_ERR";         break;
 149     }
 150
 151     fprintf(stream, "Control packet:\n");
 152     fprintf(stream, "   Type: %d (%s)\n", (int) control->type, type);
 153     fprintf(stream, "   Args: %d", (int) control->argc);
 154     if (control->argc)
 155     {
 156         int i;
 157         fprintf(stream, " (\"");
 158         for (i = 0; i < control->argc; i++)
 159             fprintf(stream, "%s%s", i ? "\", \"" : "", control->argv[i]);
 160
 161         fprintf(stream, "\")");
 162     }
 163
 164     fprintf(stream, "\n\n");
 165 }