control.c

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