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