6 #include <nsdp/packet.h>
9 void initNsdpHeader (struct nsdp_header *nh, const struct nsdp_cmd *nc)
11 memset(nh, 0, sizeof(struct nsdp_header));
12 nh->version = NSDP_VERSION;
14 nh->error = nc->error;
15 nh->attr = htons(nc->attr_error);
17 memcpy(nh->client_mac, &nc->client_mac, ETH_ALEN);
18 memcpy(nh->switch_mac, &nc->switch_mac, ETH_ALEN);
20 nh->seqnum = htonl(nc->seqnum);
21 memcpy(nh->proto_id, NSDP_PROTOID, 4);
25 bool extractNsdpHeader (const struct nsdp_header *nh, struct nsdp_cmd *nc)
30 if (nh->version != NSDP_VERSION)
33 if (nc->code > 0 && nh->code != nc->code)
37 nc->error = nh->error;
38 nc->attr_error = ntohs(nh->attr);
43 if (*(unsigned short*)nh->unk2 != 0)
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)
49 memcpy(&nc->client_mac, nh->client_mac, ETH_ALEN);
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)
54 memcpy(&nc->switch_mac, nh->switch_mac, ETH_ALEN);
56 if (nc->seqnum > 0 && ntohl(nh->seqnum) != nc->seqnum)
58 nc->seqnum = ntohl(nh->seqnum);
60 if (memcmp(nh->proto_id, NSDP_PROTOID, 4) != 0)
63 if (*(unsigned int*)nh->unk3 != 0)
70 int addPacketAttributes (struct nsdp_packet *np, const List* attr)
79 for (ln = attr->first; ln != NULL; ln = ln->next) {
82 /* encode attribute data */
83 if (encodeAttr(at) < 0)
86 /* attribute data bigger than the remaining size: error */
87 if ((int)(getPacketTotalSize(np) + sizeof(struct attr_header) + at->size) > np->maxlen)
90 /* set attribute code and size */
91 np->ah->attr = htons(at->attr);
92 np->ah->size = htons(at->size);
94 /* copy attribute data */
95 if (at->size > 0 && at->data != NULL)
96 memcpy(np->ah->data, at->data, at->size);
98 /* move to next attribute */
99 np->ah = (struct attr_header*)(np->ah->data + at->size);
107 int extractPacketAttributes (struct nsdp_packet *np, List *attr)
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)
118 /* create new attribute */
119 size = ntohs(np->ah->size);
120 at = newAttr(ntohs(np->ah->attr), size, NULL);
124 /* attribute data bigger than the remaining size: error */
125 if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) + size > np->maxlen) {
130 /* copy attribute raw data */
134 at->data = malloc(size * sizeof(unsigned char));
135 memcpy(at->data, np->ah->data, size);
138 /* decode attribute data */
139 if(decodeAttr(at) == 0) {
140 /* stop on an END attribute */
141 if (at->attr == ATTR_END) {
145 pushBackList(attr, at);
151 /* move to next attribute */
152 np->ah = (struct attr_header*)(np->ah->data + size);