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

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