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
12DiscoveryPacket::DiscoveryPacket(uint8 code, uint16 sessionID)
13	: fCode(code),
14	fSessionID(sessionID),
15	fInitStatus(B_OK)
16{
17}
18
19
20DiscoveryPacket::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
59DiscoveryPacket::~DiscoveryPacket()
60{
61	for(int32 index = 0; index < CountTags(); index++)
62		free(TagAt(index));
63}
64
65
66bool
67DiscoveryPacket::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
88bool
89DiscoveryPacket::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
101pppoe_tag*
102DiscoveryPacket::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
113pppoe_tag*
114DiscoveryPacket::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
128net_buffer*
129DiscoveryPacket::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