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, unsigned char *error, unsigned short *attr_error, List *attr, unsigned short filter_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         if (error != NULL)
 162                 *error = np->nh->error;
 163         if (attr_error != NULL)
 164                 *attr_error = ntohs(np->nh->attr);
 165
 166         while (getPacketTotalSize(np) < np->maxlen) {
 167
 168                 /* no room for an attribute header: error */
 169                 if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) > np->maxlen) {
 170                         ret = -1;
 171                         break;
 172                 }
 173
 174                 /* create new attribute */
 175                 size = ntohs(np->ah->size);
 176                 at = newAttr(ntohs(np->ah->attr), size, NULL);
 177                 if (at == NULL) {
 178                         ret = -1;
 179                         break;
 180                 }
 181
 182                 /* attribute data bigger than the remaining size: error */
 183                 if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) + size > np->maxlen) {
 184                         free(at);
 185                         ret = -1;
 186                         break;
 187                 }
 188
 189                 /* copy attribute raw data */
 190                 if (size == 0) {
 191                         at->data = NULL;
 192                 } else {
 193                         at->data = malloc(size * sizeof(char));
 194                         memcpy(at->data, np->ah->data, size);
 195                 }
 196
 197                 /* decode attribute data */
 198                 valid = true;
 199
 200                 if (filter_attr != ATTR_END && at->attr != filter_attr) {
 201                         valid = false;
 202                         goto next;
 203                 }
 204
 205                 ah = getAttrHandler(at->attr);
 206                 if (at->data == NULL || ah == NULL)
 207                         goto next;
 208
 209                 if (ah->size > 0 && size != ah->size) {
 210                         valid = false;
 211                         goto next;
 212                 }
 213
 214                 if (ah->decode != NULL)
 215                         valid = ah->decode(at, ports);
 216
 217 next:
 218                 /* stop on an END attribute */
 219                 if (at->attr == ATTR_END) {
 220                         free(at);
 221                         break;
 222                 }
 223
 224                 /* move to next attribute */
 225                 np->ah = (struct attr_header*)(np->ah->data + size);
 226
 227                 if (valid)
 228                         pushBackList(attr, at);
 229                 else
 230                         free(at);
 231         }
 232
 233
 234         return ret;
 235 }
 236
 237