control.c

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