haiku.git

/*
 * Copyright 2003-2004, Waldemar Kornewald <wkornew@gmx.net>
 * Distributed under the terms of the MIT License.
 */

#include <ByteOrder.h>
#include <NetBufferUtilities.h>
#include <net_buffer.h>

#include "DiscoveryPacket.h"

DiscoveryPacket::DiscoveryPacket(uint8 code, uint16 sessionID)
	: fCode(code),
	fSessionID(sessionID),
	fInitStatus(B_OK)
{
}


DiscoveryPacket::DiscoveryPacket(net_buffer *packet, uint32 start)
{
	// decode packet
	NetBufferHeaderReader<pppoe_header> bufferheader(packet);
	if (bufferheader.Status() != B_OK) {
		dprintf("NetBufferHeaderReader create fail\n");
		fInitStatus = B_ERROR;
		return;
	}
	pppoe_header &header = bufferheader.Data();
	
	SetCode(header.code);
	
	uint16 length = ntohs(header.length);
	
	if(length > packet->size - PPPoE_HEADER_SIZE) {
		dprintf("packet size less than pppoe payload\n");
		dprintf("pppoe payload: %d\n", length);
		dprintf("PPPoE_HEADER_SIZE: %d\n", PPPoE_HEADER_SIZE);
		dprintf("packet->size: %" B_PRIu32 "\n", packet->size);
		fInitStatus = B_ERROR;
		return;
			// there are no tags (or one corrupted tag)
	}
	
	int32 position = 0;
	pppoe_tag *tag;
	
	while(position <= length - 4) {
		tag = (pppoe_tag*) (header.data + position);
		position += ntohs(tag->length) + 4;
		
		AddTag(ntohs(tag->type), tag->data, ntohs(tag->length));
	}
	
	fInitStatus = B_OK;
}


DiscoveryPacket::~DiscoveryPacket()
{
	for(int32 index = 0; index < CountTags(); index++)
		free(TagAt(index));
}


bool
DiscoveryPacket::AddTag(uint16 type, const void *data, uint16 length, int32 index)
{
	pppoe_tag *add = (pppoe_tag*) malloc(length + 4);
	add->type = type;
	add->length = length;
	memcpy(add->data, data, length);
	
	bool status;
	if(index < 0)
		status = fTags.AddItem(add);
	else
		status = fTags.AddItem(add, index);
	if(!status) {
		free(add);
		return false;
	}
	
	return true;
}


bool
DiscoveryPacket::RemoveTag(pppoe_tag *tag)
{
	if(!fTags.HasItem(tag))
		return false;
	
	fTags.RemoveItem(tag);
	free(tag);
	
	return true;
}


pppoe_tag*
DiscoveryPacket::TagAt(int32 index) const
{
	pppoe_tag *tag = fTags.ItemAt(index);
	
	if(tag == fTags.GetDefaultItem())
		return NULL;
	
	return tag;
}


pppoe_tag*
DiscoveryPacket::TagWithType(uint16 type) const
{
	pppoe_tag *tag;
	
	for(int32 index = 0; index < CountTags(); index++) {
		tag = TagAt(index);
		if(tag && tag->type == type)
			return tag;
	}
	
	return NULL;
}


net_buffer*
DiscoveryPacket::ToNetBuffer(uint32 MTU)
{
	net_buffer *packet = gBufferModule->create(256);
	if (packet == NULL) {
		dprintf("create buffer failure\n");
		return NULL;
	}

	pppoe_header *header ;
	status_t status = gBufferModule->append_size(packet, 1492, (void **)&header);
	if (status != B_OK) {
		dprintf("append size failure\n");
		return NULL;
	}
	
	header->version = PPPoE_VERSION;
	header->type = PPPoE_TYPE;
	header->code = Code();
	header->sessionID = SessionID();
	
	uint16 length = 0;
	pppoe_tag *tag;
	
	for(int32 index = 0; index < CountTags(); index++) {
		tag = TagAt(index);
		
		// make sure we have enough space left
		if(MTU - length < tag->length) {
			gBufferModule->free(packet);
			return NULL;
		}
		
		*((uint16*)(header->data + length)) = htons(tag->type);
		length += 2;
		*((uint16*)(header->data + length)) = htons(tag->length);
		length += 2;
		memcpy(header->data + length, tag->data, tag->length);
		length += tag->length;
	}
	
	header->length = htons(length);
	status = gBufferModule->trim(packet, length + PPPoE_HEADER_SIZE);
	if (status != B_OK) {
		dprintf("trim buffer failure\n");
		return NULL;
	}

	return packet;
}