1/*
2 * Copyright 2003-2004, Waldemar Kornewald <wkornew@gmx.net>
3 * Distributed under the terms of the MIT License.
4 */
5
6#include "DiscoveryPacket.h"
7
8#include <core_funcs.h>
9
10
11DiscoveryPacket::DiscoveryPacket(uint8 code, uint16 sessionID)
12	: fCode(code),
13	fSessionID(sessionID),
14	fInitStatus(B_OK)
15{
16}
17
18
19DiscoveryPacket::DiscoveryPacket(struct mbuf *packet, uint32 start)
20{
21	// decode packet
22	uint8 *data = mtod(packet, uint8*);
23	data += start;
24	pppoe_header *header = (pppoe_header*) data;
25
26	SetCode(header->code);
27
28	uint16 length = ntohs(header->length);
29
30	if(length > packet->m_len - PPPoE_HEADER_SIZE - start) {
31		fInitStatus = B_ERROR;
32		return;
33			// there are no tags (or one corrupted tag)
34	}
35
36	int32 position = 0;
37	pppoe_tag *tag;
38
39	while(position <= length - 4) {
40		tag = (pppoe_tag*) (header->data + position);
41		position += ntohs(tag->length) + 4;
42
43		AddTag(ntohs(tag->type), tag->data, ntohs(tag->length));
44	}
45
46	fInitStatus = B_OK;
47}
48
49
50DiscoveryPacket::~DiscoveryPacket()
51{
52	for(int32 index = 0; index < CountTags(); index++)
53		free(TagAt(index));
54}
55
56
57bool
58DiscoveryPacket::AddTag(uint16 type, const void *data, uint16 length, int32 index)
59{
60	pppoe_tag *add = (pppoe_tag*) malloc(length + 4);
61	add->type = type;
62	add->length = length;
63	memcpy(add->data, data, length);
64
65	bool status;
66	if(index < 0)
67		status = fTags.AddItem(add);
68	else
69		status = fTags.AddItem(add, index);
70	if(!status) {
71		free(add);
72		return false;
73	}
74
75	return true;
76}
77
78
79bool
80DiscoveryPacket::RemoveTag(pppoe_tag *tag)
81{
82	if(!fTags.HasItem(tag))
83		return false;
84
85	fTags.RemoveItem(tag);
86	free(tag);
87
88	return true;
89}
90
91
92pppoe_tag*
93DiscoveryPacket::TagAt(int32 index) const
94{
95	pppoe_tag *tag = fTags.ItemAt(index);
96
97	if(tag == fTags.GetDefaultItem())
98		return NULL;
99
100	return tag;
101}
102
103
104pppoe_tag*
105DiscoveryPacket::TagWithType(uint16 type) const
106{
107	pppoe_tag *tag;
108
109	for(int32 index = 0; index < CountTags(); index++) {
110		tag = TagAt(index);
111		if(tag && tag->type == type)
112			return tag;
113	}
114
115	return NULL;
116}
117
118
119struct mbuf*
120DiscoveryPacket::ToMbuf(uint32 MTU, uint32 reserve)
121{
122	struct mbuf *packet = m_gethdr(MT_DATA);
123	packet->m_data += reserve;
124
125	pppoe_header *header = mtod(packet, pppoe_header*);
126
127	header->version = PPPoE_VERSION;
128	header->type = PPPoE_TYPE;
129	header->code = Code();
130	header->sessionID = SessionID();
131
132	uint16 length = 0;
133	pppoe_tag *tag;
134
135	for(int32 index = 0; index < CountTags(); index++) {
136		tag = TagAt(index);
137
138		// make sure we have enough space left
139		if(MTU - length < tag->length) {
140			m_freem(packet);
141			return NULL;
142		}
143
144		*((uint16*)(header->data + length)) = htons(tag->type);
145		length += 2;
146		*((uint16*)(header->data + length)) = htons(tag->length);
147		length += 2;
148		memcpy(header->data + length, tag->data, tag->length);
149		length += tag->length;
150	}
151
152	header->length = htons(length);
153	packet->m_pkthdr.len = packet->m_len = length + PPPoE_HEADER_SIZE;
154
155	return packet;
156}
157