packet.c

NDIS 实现pppoe例子

/*
    MikroTik PPPoE - MikroTik PPP over Ethernet client for Windows
    Copyright (C),  2001  MikroTikls

    The contents of this program are subject to the Mozilla Public License 
    Version 1.1; you may not use this program except in compliance with the 
    License. You may obtain a copy of the License at http://www.mozilla.org/MPL/ 

    
    http://www.mikrotik.com
    [e-mail]
*/


#include "packet.h"
#include "my.h"
#include "debug.h"
#include "main.h"

PPPOE_PACKET *PacketAllocate(UINT len) {
	NDIS_STATUS s;
	PPPOE_PACKET *p;

	FENTER("PacketAllocate");

	if (len == 0) len = 1514;
	s = NdisAllocateMemory(&p, len, 0, MaxAddress);
	if (s != NDIS_STATUS_SUCCESS) {
		DbgPrint("can not allocate packet, status: 0x%x\n", s);
		p = NULL;
	}
	else DbgPrint("packet: 0x%x, size: %d\n", (UINT)p, len);

	FLEAVE("PacketAllocate");
	return p;
}

void PacketFree(PPPOE_PACKET *p, UINT len) {
	FENTER("PacketFree");

	if (len == 0) len = 1514;

	DbgPrint("packet: 0x%x, size: %d\n", (UINT)p, len);

	NdisFreeMemory(p, len, 0);

	FLEAVE("PacketFree");
}

void PacketReset(PPPOE_PACKET *p) {
	FENTER("PacketReset");

	p->vertype = 0x11;
	p->len = 0;

	FLEAVE("PacketReset");
}

int PacketCheckHeader(PPPOE_PACKET *p, unsigned len) {
	int ret = 1;

	FENTER("PacketParse");

	if (p->vertype != 0x11) {
		DbgPrint("wrong wertype: 0x%x\n", p->vertype);
		goto err_out;
	}
	if ((unsigned)NTOHS(p->len) + 20 > len) {
		DbgPrint("length wrong: %d\n", len);
		goto err_out;
	}

	ret = 0;

err_out:
	FLEAVE("PacketParse");
	return ret;
}

int PacketParseTags(PPPOE_PACKET *p) {
	int ret = 1;
	unsigned taglen;
	PPPOE_TAGHDR *tag;

	FENTER("PacketParseTags");

	taglen = NTOHS(p->len);
	tag = (PPPOE_TAGHDR *)p->data;
    while (1) {
		unsigned tl;

        if (taglen == 0) break;
        if (taglen < sizeof(PPPOE_TAGHDR)) {
			DbgPrint("partial tag structure in frame\n");
			goto err_out;
        }

		taglen -= sizeof(PPPOE_TAGHDR);
		tl = NTOHS(tag->len);
		if (NTOHS(tag->type) == PPPOE_TAG_EOL) {
			if (tl != 0) {
				DbgPrint("EOL with non-zero len\n");
				goto err_out;
			}

			break;
		}

        if (taglen < tl) {
			DbgPrint("partial data for tag\n");
			goto err_out;
        }

		taglen -= tl;
		tag = (PPPOE_TAGHDR *)((PCHAR)tag + tl + sizeof(PPPOE_TAGHDR));
    }

	ret = 0;

err_out:
	FLEAVE("PacketParseTags");
	return ret;
}

int PacketGetFirstTag(PPPOE_PACKET *p, PPPOE_TAG *t) {
	int ret = 0;
	FENTER("PacketGetFirstTag");

	if (NTOHS(p->len) != 0) {
		PPPOE_TAGHDR *tag = (PPPOE_TAGHDR *)p->data;
		t->type = NTOHS(tag->type);
		if (t->type != PPPOE_TAG_EOL) {
			t->data = p->data + sizeof(PPPOE_TAGHDR);
			t->len = NTOHS(tag->len);
		}
		else ret = 1;
	}
	else ret = 1;

	FLEAVE("PacketGetFirstTag");
	return ret;
}

int PacketGetNextTag(PPPOE_PACKET *p, PPPOE_TAG *t) {
	PPPOE_TAGHDR *tag;
	int ret = 0;
	FENTER("PacketGetNextTag");

	tag = (PPPOE_TAGHDR *)(t->data + t->len);
	if (((PUCHAR)tag - p->data) <= NTOHS(p->len)) {
		t->type = NTOHS(tag->type);
		if (t->type != PPPOE_TAG_EOL) {
			t->data = (PUCHAR)tag + sizeof(PPPOE_TAGHDR);
			t->len = NTOHS(tag->len);
		}
		else {
			DbgPrint("exit 1\n");
			ret = 1;
		}
	}
	else {
		DbgPrint("exit 2, p->len: %d, delta: %d\n", NTOHS(p->len), (PUCHAR)tag - p->data);
		ret = 1;
	}

	FLEAVE("PacketGetNextTag");
	return ret;
}

int PacketAddTag(PPPOE_PACKET *p, unsigned short type, unsigned char *data,
				 unsigned len) {
	unsigned taglen;
	PPPOE_TAGHDR *tag;

	FENTER("PacketAddTag");

	taglen = NTOHS(p->len);
	if (taglen + sizeof(PPPOE_TAGHDR) + len > 1478) {
		DbgPrint("can not add tag\n");
		FLEAVE("PacketAddTag");
		return 1;
	}

	tag = (PPPOE_TAGHDR *)(p->data + taglen);
	tag->len = NTOHS(len);
	tag->type = NTOHS(type);
	if (len > 0) NdisMoveMemory((PUCHAR)tag + sizeof(PPPOE_TAGHDR), data, len);

	taglen += sizeof(PPPOE_TAGHDR) + len;
	p->len = NTOHS(taglen);

	FLEAVE("PacketAddTag");
	return 0;
}

int PacketLength(PPPOE_PACKET *p) {
	return NTOHS(p->len) + 14 + 6;
}