1 /*
2 * Copyright 2003-2004, Waldemar Kornewald <wkornew@gmx.net>
3 * Distributed under the terms of the MIT License.
4 */
5
6 #include <ByteOrder.h>
7 #include <NetBufferUtilities.h>
8 #include <net_buffer.h>
9
10 #include "DiscoveryPacket.h"
11
DiscoveryPacket(uint8 code,uint16 sessionID)12 DiscoveryPacket::DiscoveryPacket(uint8 code, uint16 sessionID)
13 : fCode(code),
14 fSessionID(sessionID),
15 fInitStatus(B_OK)
16 {
17 }
18
19
DiscoveryPacket(net_buffer * packet,uint32 start)20 DiscoveryPacket::DiscoveryPacket(net_buffer *packet, uint32 start)
21 {
22 // decode packet
23 NetBufferHeaderReader<pppoe_header> bufferheader(packet);
24 if (bufferheader.Status() != B_OK) {
25 dprintf("NetBufferHeaderReader create fail\n");
26 fInitStatus = B_ERROR;
27 return;
28 }
29 pppoe_header &header = bufferheader.Data();
30
31 SetCode(header.code);
32
33 uint16 length = ntohs(header.length);
34
35 if(length > packet->size - PPPoE_HEADER_SIZE) {
36 dprintf("packet size less than pppoe payload\n");
37 dprintf("pppoe payload: %d\n", length);
38 dprintf("PPPoE_HEADER_SIZE: %d\n", PPPoE_HEADER_SIZE);
39 dprintf("packet->size: %" B_PRIu32 "\n", packet->size);
40 fInitStatus = B_ERROR;
41 return;
42 // there are no tags (or one corrupted tag)
43 }
44
45 int32 position = 0;
46 pppoe_tag *tag;
47
48 while(position <= length - 4) {
49 tag = (pppoe_tag*) (header.data + position);
50 position += ntohs(tag->length) + 4;
51
52 AddTag(ntohs(tag->type), tag->data, ntohs(tag->length));
53 }
54
55 fInitStatus = B_OK;
56 }
57
58
~DiscoveryPacket()59 DiscoveryPacket::~DiscoveryPacket()
60 {
61 for(int32 index = 0; index < CountTags(); index++)
62 free(TagAt(index));
63 }
64
65
66 bool
AddTag(uint16 type,const void * data,uint16 length,int32 index)67 DiscoveryPacket::AddTag(uint16 type, const void *data, uint16 length, int32 index)
68 {
69 pppoe_tag *add = (pppoe_tag*) malloc(length + 4);
70 add->type = type;
71 add->length = length;
72 memcpy(add->data, data, length);
73
74 bool status;
75 if(index < 0)
76 status = fTags.AddItem(add);
77 else
78 status = fTags.AddItem(add, index);
79 if(!status) {
80 free(add);
81 return false;
82 }
83
84 return true;
85 }
86
87
88 bool
RemoveTag(pppoe_tag * tag)89 DiscoveryPacket::RemoveTag(pppoe_tag *tag)
90 {
91 if(!fTags.HasItem(tag))
92 return false;
93
94 fTags.RemoveItem(tag);
95 free(tag);
96
97 return true;
98 }
99
100
101 pppoe_tag*
TagAt(int32 index) const102 DiscoveryPacket::TagAt(int32 index) const
103 {
104 pppoe_tag *tag = fTags.ItemAt(index);
105
106 if(tag == fTags.GetDefaultItem())
107 return NULL;
108
109 return tag;
110 }
111
112
113 pppoe_tag*
TagWithType(uint16 type) const114 DiscoveryPacket::TagWithType(uint16 type) const
115 {
116 pppoe_tag *tag;
117
118 for(int32 index = 0; index < CountTags(); index++) {
119 tag = TagAt(index);
120 if(tag && tag->type == type)
121 return tag;
122 }
123
124 return NULL;
125 }
126
127
128 net_buffer*
ToNetBuffer(uint32 MTU)129 DiscoveryPacket::ToNetBuffer(uint32 MTU)
130 {
131 net_buffer *packet = gBufferModule->create(256);
132 if (packet == NULL) {
133 dprintf("create buffer failure\n");
134 return NULL;
135 }
136
137 pppoe_header *header ;
138 status_t status = gBufferModule->append_size(packet, 1492, (void **)&header);
139 if (status != B_OK) {
140 dprintf("append size failure\n");
141 return NULL;
142 }
143
144 header->version = PPPoE_VERSION;
145 header->type = PPPoE_TYPE;
146 header->code = Code();
147 header->sessionID = SessionID();
148
149 uint16 length = 0;
150 pppoe_tag *tag;
151
152 for(int32 index = 0; index < CountTags(); index++) {
153 tag = TagAt(index);
154
155 // make sure we have enough space left
156 if(MTU - length < tag->length) {
157 gBufferModule->free(packet);
158 return NULL;
159 }
160
161 *((uint16*)(header->data + length)) = htons(tag->type);
162 length += 2;
163 *((uint16*)(header->data + length)) = htons(tag->length);
164 length += 2;
165 memcpy(header->data + length, tag->data, tag->length);
166 length += tag->length;
167 }
168
169 header->length = htons(length);
170 status = gBufferModule->trim(packet, length + PPPoE_HEADER_SIZE);
171 if (status != B_OK) {
172 dprintf("trim buffer failure\n");
173 return NULL;
174 }
175
176 return packet;
177 }
178