git.sur5r.net Git - ngadmin/blob - raw/src/protocol.c

   1
   2 #include <stdio.h>
   3 #include <stdarg.h>
   4 #include <stdbool.h>
   5 #include <string.h>
   6 #include <errno.h>
   7
   8 #include "attr.h"
   9 #include "protocol.h"
  10
  11
  12
  13 const char passwordKey[] = "NtgrSmartSwitchRock";
  14
  15
  16 void passwordEndecode (char *buf, unsigned int len)
  17 {
  18         const char *k = passwordKey;
  19         unsigned int i;
  20
  21         if (buf == NULL || len <= 0)
  22                 return;
  23
  24         for (i = 0; i < len; i++) {
  25                 if (*k == '\0')
  26                         k = passwordKey;
  27                 buf[i] ^= *k++;
  28         }
  29 }
  30
  31
  32 int trim (char *txt, int start)
  33 {
  34         char *p;
  35
  36         if (txt == NULL)
  37                 return 0;
  38
  39         p = txt + start;
  40         while (p >= txt && (*p == ' ' || *p == '\n')) {
  41                 *p = '\0';
  42                 p--;
  43         }
  44
  45         return p - txt + 1;
  46 }
  47
  48
  49 void initNgHeader (struct ng_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum)
  50 {
  51         memset(nh, 0, sizeof(struct ng_header));
  52         nh->version = 1;
  53         nh->code = code;
  54
  55         memcpy(nh->client_mac, client_mac, ETH_ALEN);
  56
  57         if (switch_mac != NULL)
  58                 memcpy(nh->switch_mac, switch_mac, ETH_ALEN);
  59
  60         nh->seqnum = htonl(seqnum);
  61         memcpy(nh->proto_id, "NSDP", 4);
  62 }
  63
  64
  65 bool validateNgHeader (const struct ng_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum)
  66 {
  67         if (nh->version != 1)
  68                 return false;
  69
  70         if (code > 0 && nh->code != code)
  71                 return false;
  72
  73         if (nh->unk1 != 0)
  74                 return false;
  75
  76         if (*(unsigned short*)nh->unk2 != 0)
  77                 return false;
  78
  79         if (client_mac != NULL && memcmp(nh->client_mac, client_mac, ETH_ALEN) != 0)
  80                 return false;
  81
  82         if (switch_mac != NULL && memcmp(nh->switch_mac, switch_mac, ETH_ALEN) != 0)
  83                 return false;
  84
  85         if (seqnum > 0 && ntohl(nh->seqnum) != seqnum)
  86                 return false;
  87
  88         if (memcmp(nh->proto_id, "NSDP", 4) != 0)
  89                 return false;
  90
  91         if (*(unsigned int*)nh->unk3 != 0)
  92                 return false;
  93
  94         return true;
  95 }
  96
  97
  98 void addPacketAttr (struct ng_packet *np, struct attr *at)
  99 {
 100         struct attr_header *ah = np->ah;
 101
 102
 103         if ((int)(getPacketTotalSize(np) + sizeof(struct attr_header) + at->size) > np->maxlen)
 104                 return;
 105
 106         ah->attr = htons(at->attr);
 107         ah->size = htons(at->size);
 108
 109         if (at->size > 0 && at->data != NULL)
 110                 memcpy(ah->data, at->data, at->size);
 111
 112         np->ah = (struct attr_header*)(ah->data + at->size);
 113 }
 114
 115
 116 struct attr* newAttr (unsigned short attr, unsigned short size, void *data)
 117 {
 118         struct attr *at;
 119
 120
 121         at = malloc(sizeof(struct attr));
 122         if (at == NULL)
 123                 return NULL;
 124
 125         at->attr = attr;
 126         at->size = size;
 127         at->data = data;
 128
 129
 130         return at;
 131 }
 132
 133
 134 void freeAttr (struct attr *at)
 135 {
 136         if (at != NULL) {
 137                 free(at->data);
 138                 free(at);
 139         }
 140 }
 141
 142
 143 int addPacketAttributes (struct ng_packet *np, const List* attr, unsigned char ports)
 144 {
 145         ListNode *ln;
 146         struct attr *at;
 147         const struct attr_handler *ah;
 148
 149
 150         if (attr == NULL)
 151                 return 0;
 152
 153         for (ln = attr->first; ln != NULL; ln = ln->next) {
 154                 at = ln->data;
 155                 ah = getAttrHandler(at->attr);
 156                 if (ah != NULL) {
 157                         if (ah->size > 0 && at->size > 0 && at->size != ah->size)
 158                                 return -EINVAL;
 159                         if (at->size > 0 && ah->encode != NULL && !ah->encode(at, ports))
 160                                 return -EINVAL;
 161                 }
 162
 163                 addPacketAttr(np, at);
 164         }
 165
 166
 167         return 0;
 168 }
 169
 170
 171 int extractPacketAttributes (struct ng_packet *np, List *attr, unsigned char ports)
 172 {
 173         struct attr *at;
 174         const struct attr_handler *ah;
 175         int ret = 0;
 176         unsigned short size;
 177         bool valid;
 178
 179
 180         while (getPacketTotalSize(np) < np->maxlen) {
 181
 182                 /* no room for an attribute header: error */
 183                 if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) > np->maxlen) {
 184                         ret = -1;
 185                         break;
 186                 }
 187
 188                 /* create new attribute */
 189                 size = ntohs(np->ah->size);
 190                 at = newAttr(ntohs(np->ah->attr), size, NULL);
 191                 if (at == NULL) {
 192                         ret = -1;
 193                         break;
 194                 }
 195
 196                 /* attribute data bigger than the remaining size: error */
 197                 if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) + size > np->maxlen) {
 198                         free(at);
 199                         ret = -1;
 200                         break;
 201                 }
 202
 203                 /* copy attribute raw data */
 204                 if (size == 0) {
 205                         at->data = NULL;
 206                 } else {
 207                         at->data = malloc(size * sizeof(char));
 208                         memcpy(at->data, np->ah->data, size);
 209                 }
 210
 211                 /* decode attribute data */
 212                 valid = true;
 213
 214                 ah = getAttrHandler(at->attr);
 215                 if (at->data == NULL || ah == NULL)
 216                         goto next;
 217
 218                 if (ah->size > 0 && size != ah->size) {
 219                         valid = false;
 220                         goto next;
 221                 }
 222
 223                 if (ah->decode != NULL)
 224                         valid = ah->decode(at, ports);
 225
 226 next:
 227                 /* stop on an END attribute */
 228                 if (at->attr == ATTR_END) {
 229                         free(at);
 230                         break;
 231                 }
 232
 233                 /* move to next attribute */
 234                 np->ah = (struct attr_header*)(np->ah->data + size);
 235
 236                 if (valid)
 237                         pushBackList(attr, at);
 238                 else
 239                         free(at);
 240         }
 241
 242
 243         return ret;
 244 }
 245
 246
 247 void filterAttributes (List *attr, ...)
 248 {
 249         va_list ap;
 250         ListNode *ln, *pr;
 251         struct attr *at;
 252         unsigned short attrcode;
 253         bool keep;
 254
 255
 256         ln = attr->first;
 257         while (ln != NULL) {
 258                 at = ln->data;
 259
 260                 va_start(ap, attr);
 261                 keep = false;
 262                 attrcode = 0;
 263                 while (!keep && attrcode != ATTR_END) {
 264                         attrcode = (unsigned short)va_arg(ap, unsigned int);
 265                         keep = keep || (at->attr == attrcode);
 266                 }
 267                 va_end(ap);
 268
 269                 if (keep) {
 270                         ln = ln->next;
 271                 } else {
 272                         pr = ln;
 273                         ln = ln->next;
 274                         destroyElement(attr, pr, (void(*)(void*))freeAttr);
 275                 }
 276         }
 277
 278 }
 279
 280