control.c

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