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

   1
   2 #include <string.h>
   3 #include <errno.h>
   4
   5 #include <nsdp/attr.h>
   6 #include "encoding.h"
   7 #include "encoding_attr.h"
   8
   9
  10
  11 void initNsdpHeader (struct nsdp_header *nh, const struct nsdp_cmd *nc)
  12 {
  13         memset(nh, 0, sizeof(struct nsdp_header));
  14         nh->version = NSDP_VERSION;
  15         nh->code = nc->code;
  16         nh->error = nc->error;
  17         nh->attr = htons(nc->attr_error);
  18
  19         memcpy(nh->client_mac, &nc->client_mac, ETH_ALEN);
  20         memcpy(nh->switch_mac, &nc->switch_mac, ETH_ALEN);
  21
  22         nh->seqnum = htonl(nc->seqnum);
  23         memcpy(nh->proto_id, NSDP_PROTOID, 4);
  24 }
  25
  26
  27 bool validateNsdpHeader (const struct nsdp_header *nh, const struct nsdp_cmd *nc)
  28 {
  29         unsigned int i;
  30
  31
  32         if (nh->version != NSDP_VERSION)
  33                 return false;
  34
  35         if (nc->code > 0 && nh->code != nc->code)
  36                 return false;
  37
  38         if (nh->unk1 != 0)
  39                 return false;
  40
  41         if (*(unsigned short*)nh->unk2 != 0)
  42                 return false;
  43
  44         for (i = 0; i < ETH_ALEN && nc->client_mac.ether_addr_octet[i] == 0; i++);
  45         if (i < ETH_ALEN && memcmp(nh->client_mac, &nc->client_mac, ETH_ALEN) != 0)
  46                 return false;
  47
  48         for (i = 0; i < ETH_ALEN && nc->switch_mac.ether_addr_octet[i] == 0; i++);
  49         if (i < ETH_ALEN && memcmp(nh->switch_mac, &nc->switch_mac, ETH_ALEN) != 0)
  50                 return false;
  51
  52         if (nc->seqnum > 0 && ntohl(nh->seqnum) != nc->seqnum)
  53                 return false;
  54
  55         if (memcmp(nh->proto_id, NSDP_PROTOID, 4) != 0)
  56                 return false;
  57
  58         if (*(unsigned int*)nh->unk3 != 0)
  59                 return false;
  60
  61         return true;
  62 }
  63
  64
  65 static void addPacketAttr (struct nsdp_packet *np, struct attr *at)
  66 {
  67         struct attr_header *ah = np->ah;
  68
  69
  70         if ((int)(getPacketTotalSize(np) + sizeof(struct attr_header) + at->size) > np->maxlen)
  71                 return;
  72
  73         ah->attr = htons(at->attr);
  74         ah->size = htons(at->size);
  75
  76         if (at->size > 0 && at->data != NULL)
  77                 memcpy(ah->data, at->data, at->size);
  78
  79         np->ah = (struct attr_header*)(ah->data + at->size);
  80 }
  81
  82
  83 int addPacketAttributes (struct nsdp_packet *np, const List* attr, unsigned char ports)
  84 {
  85         ListNode *ln;
  86         struct attr *at;
  87         const struct attr_handler *ah;
  88
  89
  90         if (attr == NULL)
  91                 return 0;
  92
  93         for (ln = attr->first; ln != NULL; ln = ln->next) {
  94                 at = ln->data;
  95                 ah = getAttrHandler(at->attr);
  96                 if (ah != NULL) {
  97                         if (ah->size > 0 && at->size > 0 && at->size != ah->size)
  98                                 return -EINVAL;
  99                         if (at->size > 0 && ah->encode != NULL && !ah->encode(at, ports))
 100                                 return -EINVAL;
 101                 }
 102
 103                 addPacketAttr(np, at);
 104         }
 105
 106
 107         return 0;
 108 }
 109
 110
 111 int extractPacketAttributes (struct nsdp_packet *np, List *attr, unsigned char ports)
 112 {
 113         struct attr *at;
 114         const struct attr_handler *ah;
 115         int ret = 0;
 116         unsigned short size;
 117         bool valid;
 118
 119
 120         while (getPacketTotalSize(np) < np->maxlen) {
 121                 /* no room for an attribute header: error */
 122                 if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) > np->maxlen) {
 123                         ret = -1;
 124                         break;
 125                 }
 126
 127                 /* create new attribute */
 128                 size = ntohs(np->ah->size);
 129                 at = newAttr(ntohs(np->ah->attr), size, NULL);
 130                 if (at == NULL) {
 131                         ret = -1;
 132                         break;
 133                 }
 134
 135                 /* attribute data bigger than the remaining size: error */
 136                 if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) + size > np->maxlen) {
 137                         free(at);
 138                         ret = -1;
 139                         break;
 140                 }
 141
 142                 /* copy attribute raw data */
 143                 if (size == 0) {
 144                         at->data = NULL;
 145                 } else {
 146                         at->data = malloc(size * sizeof(unsigned char));
 147                         memcpy(at->data, np->ah->data, size);
 148                 }
 149
 150                 /* decode attribute data */
 151                 valid = true;
 152
 153                 ah = getAttrHandler(at->attr);
 154                 if (at->data == NULL || ah == NULL)
 155                         goto next;
 156
 157                 if (ah->size > 0 && size != ah->size) {
 158                         valid = false;
 159                         goto next;
 160                 }
 161
 162                 if (ah->decode != NULL)
 163                         valid = ah->decode(at, ports);
 164
 165 next:
 166                 /* stop on an END attribute */
 167                 if (at->attr == ATTR_END) {
 168                         free(at);
 169                         break;
 170                 }
 171
 172                 /* move to next attribute */
 173                 np->ah = (struct attr_header*)(np->ah->data + size);
 174
 175                 if (valid)
 176                         pushBackList(attr, at);
 177                 else
 178                         free(at);
 179         }
 180
 181
 182         return ret;
 183 }
 184
 185