6 #include <nsdp/encoding.h>
7 #include "encoding_attr.h"
11 void initNsdpHeader (struct nsdp_header *nh, const struct nsdp_cmd *nc)
13 memset(nh, 0, sizeof(struct nsdp_header));
14 nh->version = NSDP_VERSION;
16 nh->error = nc->error;
17 nh->attr = htons(nc->attr_error);
19 memcpy(nh->client_mac, &nc->client_mac, ETH_ALEN);
20 memcpy(nh->switch_mac, &nc->switch_mac, ETH_ALEN);
22 nh->seqnum = htonl(nc->seqnum);
23 memcpy(nh->proto_id, NSDP_PROTOID, 4);
27 bool extractNsdpHeader (const struct nsdp_header *nh, struct nsdp_cmd *nc)
32 if (nh->version != NSDP_VERSION)
35 if (nc->code > 0 && nh->code != nc->code)
39 nc->error = nh->error;
40 nc->attr_error = ntohs(nh->attr);
45 if (*(unsigned short*)nh->unk2 != 0)
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)
51 memcpy(&nc->client_mac, nh->client_mac, ETH_ALEN);
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)
56 memcpy(&nc->switch_mac, nh->switch_mac, ETH_ALEN);
58 if (nc->seqnum > 0 && ntohl(nh->seqnum) != nc->seqnum)
60 nc->seqnum = ntohl(nh->seqnum);
62 if (memcmp(nh->proto_id, NSDP_PROTOID, 4) != 0)
65 if (*(unsigned int*)nh->unk3 != 0)
72 static void addPacketAttr (struct nsdp_packet *np, struct attr *at)
74 struct attr_header *ah = np->ah;
77 if ((int)(getPacketTotalSize(np) + sizeof(struct attr_header) + at->size) > np->maxlen)
80 ah->attr = htons(at->attr);
81 ah->size = htons(at->size);
83 if (at->size > 0 && at->data != NULL)
84 memcpy(ah->data, at->data, at->size);
86 np->ah = (struct attr_header*)(ah->data + at->size);
90 int addPacketAttributes (struct nsdp_packet *np, const List* attr, unsigned char ports)
94 const struct attr_handler *ah;
100 for (ln = attr->first; ln != NULL; ln = ln->next) {
102 ah = getAttrHandler(at->attr);
104 if (ah->size > 0 && at->size > 0 && at->size != ah->size)
106 if (at->size > 0 && ah->encode != NULL && !ah->encode(at, ports))
110 addPacketAttr(np, at);
118 int extractPacketAttributes (struct nsdp_packet *np, List *attr, unsigned char ports)
121 const struct attr_handler *ah;
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) {
134 /* create new attribute */
135 size = ntohs(np->ah->size);
136 at = newAttr(ntohs(np->ah->attr), size, NULL);
142 /* attribute data bigger than the remaining size: error */
143 if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) + size > np->maxlen) {
149 /* copy attribute raw data */
153 at->data = malloc(size * sizeof(unsigned char));
154 memcpy(at->data, np->ah->data, size);
157 /* decode attribute data */
160 ah = getAttrHandler(at->attr);
161 if (at->data == NULL || ah == NULL)
164 if (ah->size > 0 && size != ah->size) {
169 if (ah->decode != NULL)
170 valid = ah->decode(at, ports);
173 /* stop on an END attribute */
174 if (at->attr == ATTR_END) {
179 /* move to next attribute */
180 np->ah = (struct attr_header*)(np->ah->data + size);
183 pushBackList(attr, at);