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