pppoe.c

pppoe在windows实现的miniport驱动源代码

/*
    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 "discovery.h"
#include "pppoe.h"
#include "request.h"
#include "debug.h"

void MyFreeNdisPacket(PNDIS_PACKET p) {
	PNDIS_BUFFER b;
	while(1) {
		NdisUnchainBufferAtFront(p, &b);
		if(b != NULL) {
			NdisFreeBuffer(b);
		} else {
			break;
		}
	}
	NdisFreePacket(p);
}

void PppoeReceiveDiscovery(PADAPTER a, PPPOE_PACKET *p, UINT size) {
//	UINT count;

	FENTER("PppoeReceiveDiscovery");

	if (size < 20) {
		DbgPrint ("short pppoe discovery packet\n");
		FLEAVE("PppoeReceiveDiscovery");
		return;
	}
	if (PacketCheckHeader(p, size) != 0) {
		DbgPrint("wrong header\n");
		FLEAVE("PppoeReceiveDiscovery");
		return;
	}
	if (PacketParseTags(p) != 0) {
		DbgPrint("failed to parse tags\n");
		FLEAVE("PppoeReceiveDiscovery");
		return;
	}

    switch (p->code) {
    case PPPOE_CODE_PADO:
        if (p->sessionid != 0) {
			DbgPrint("PADO with nonzero session id\n");
			FLEAVE("PppoeReceiveDiscovery");
            return;
        }
		if (MyMemCmp(mainEthBcast, p->dest, 6) == 0) {
			DbgPrint("PADO to broadcast address\n");
			FLEAVE("PppoeReceiveDiscovery");
            return;
        }

		if (a->pppoeState != PPPOE_STATE_PADI) {
			DbgPrint("PADO in non-PADI state: %d\n", a->pppoeState);
			FLEAVE("PppoeReceiveDiscovery");
            return;
		}

		PppoeReceivePADO(a, p);
        break;

    case PPPOE_CODE_PADS:
		if (MyMemCmp(mainEthBcast, p->dest, 6) == 0) {
			DbgPrint("PADS to broadcast address\n");
			FLEAVE("PppoeReceiveDiscovery");
            return;
        }

/*		if (p->sessionid == 0) {
			DbgPrint("PADS with 0 session id\n");
			FLEAVE("PppoeReceiveDiscovery");
            return;
		}
*/
		if (a->pppoeState != PPPOE_STATE_PADR) {
			DbgPrint("PADS in non-PADR state: %d\n", a->pppoeState);
			FLEAVE("PppoeReceiveDiscovery");
            return;
		}

		PppoeReceivePADS(a, p);
        break;

    case PPPOE_CODE_PADT:
		if (MyMemCmp(mainEthBcast, p->dest, 6) == 0) {
			DbgPrint("PADT to broadcast address\n");
			FLEAVE("PppoeReceiveDiscovery");
            return;
        }
        if (p->sessionid == 0) {
			DbgPrint("PADT with 0 session id\n");
			FLEAVE("PppoeReceiveDiscovery");
            return;
		}

		if (a->pppoeState != PPPOE_STATE_SESSION) {
			DbgPrint("PADS in non-SESSION state: %d\n", a->pppoeState);
			FLEAVE("PppoeReceiveDiscovery");
            return;
		}

		PppoeReceivePADT(a, p);
        break;

    default:
		DbgPrint("packet with code: 0x%x\n", p->code);
        break;
    }

//	for(count = 0; count < 20; count++) DbgPrint("%2.2x ", ((PUCHAR)packet)[count]);
//	DbgPrint("\n");

	FLEAVE("PppoeReceiveDiscovery");
}

void PppoeReceiveSession(PADAPTER a, PPPOE_PACKET *packet, UINT size) {
	UINT count;
	NDIS_STATUS s;
	PUCHAR xx;

	FENTER("PppoeReceiveSession");

	if (size < 20) {
		DbgPrint ("short pppoe session packet\n");
		FLEAVE("PppoeReceiveSession");
		return;
	}
	if (PacketCheckHeader(packet, size) != 0) {
		DbgPrint("wrong header\n");
		FLEAVE("PppoeReceiveSession");
		return;
	}
	if (packet->code != 0) {
		DbgPrint("session stage packet with non-0 code\n");
		FLEAVE("PppoeReceiveSession");
		return;
	}
	if (packet->sessionid != a->pppoeSessionId) {
		DbgPrint("wrong session id\n");
		FLEAVE("PppoeReceiveSession");
		return;
	}
	if (MyMemCmp(packet->source, a->pppoeServerMacAddr, 6) != 0) {
		DbgPrint("session packet not from server\n");
		FLEAVE("PppoeReceiveSession");
		return;
	}
	if (MyMemCmp(packet->dest, a->protoMacAddr, 6) != 0) {
		DbgPrint("session packet not directly to us\n");
		FLEAVE("PppoeReceiveSession");
		return;
	}

	// Packet received. So we won't be needing send lcp-echo request this time.
	a->pppoePacketsReceived++;


	// dirty hack: we need to put ADDRESS CONTROL field in packet, so
	// PPP does not get mad at us
	xx = packet->data - 2;
	xx[0] = 0xff;
	xx[1] = 0x03;
	NdisMWanIndicateReceive(&s, a->miniAdapterHandle, a->miniNdisLinkContext,
							xx, size - 18);

	FLEAVE("PppoeReceiveSession");
}


NDIS_STATUS PppoeSendPacket(PADAPT a, PPPOE_PACKET *p, UINT plen) {
	PTRANSMIT_PROTOINFO pinfo;
	PNDIS_BUFFER buffer;
	PNDIS_PACKET packet;
	NDIS_STATUS s;
	UINT len;

	FENTER("PppoeSendPacket");

	NdisAllocatePacket(&s, &packet, a->pppoePacketPoolHandle);
	if (s != NDIS_STATUS_SUCCESS) {
		DbgPrint("failed to allocate packet\n");
		FLEAVE("sendPPPOEPacket");
		return NDIS_STATUS_FAILURE;
	}

	NdisAllocateBuffer(&s, &buffer, a->pppoeBufferPoolHandle, p, PacketLength(p));
	if (s != NDIS_STATUS_SUCCESS) {
		DbgPrint("failed to allocate buffer\n");
		FLEAVE("sendPPPOEPacket");
		return NDIS_STATUS_FAILURE;
	}
	NdisChainBufferAtFront(packet, buffer);

	pinfo = (PTRANSMIT_PROTOINFO)packet->ProtocolReserved;
	pinfo->pppoePacket = p;
	pinfo->pppoePacketLen = plen;
	pinfo->wanPacket = NULL;

	NdisSend(&s, a->protoBindingHandle, packet);
	if (s == NDIS_STATUS_PENDING) {
		DbgPrint("Packet send pending\n");
	}
	else {
		DbgPrint("NdisSend status: 0x%x\n", s);

		PacketFree(p, plen);
		MyFreeNdisPacket(packet);
	}

	FLEAVE("sendPPPOEPacket");
	return s;
}

static NDIS_STATUS setPacketFilter(PADAPT a, ULONG packetFilter) {
	NDIS_STATUS status;

	FENTER("setPacketFilter");

	status = ProtoSetRequestSync(a, OID_GEN_CURRENT_PACKET_FILTER, (PVOID) &packetFilter, sizeof(packetFilter), NULL);
	if (status != NDIS_STATUS_SUCCESS) {
		DbgPrint("Packet filter failed: 0x%x\n", status);
	}
	else DbgPrint("Packet filter : %x success\n", packetFilter);

	FLEAVE("setPacketFilter");
	return status;
}

void PppoeInitAdapter(PADAPTER a) {
	FENTER("PppoeInitAdapter");
	FLEAVE("PppoeInitAdapter");
}

static void PppoeInitService(PADAPTER a, PCHAR addr, ULONG alen) {
	NDIS_STATUS s;
	ULONG i = 0;
	PCHAR sn = NULL;
	PCHAR ac = NULL;
	UINT snLen = 0;
	UINT acLen = 0;
	int snRead = 1;

	sn = addr;
	while (1) {
		if (i >= alen) break;
		if (addr[i] == 0) break;

		if (addr[i] == '@' && snRead == 1) {
			++i;
			snRead = 0;
			ac = &addr[i];
			continue;
		}

		if (snRead == 1) ++snLen;
		else ++acLen;

		++i;
	}

	// hack: if no phoneno specified windows gives one space for us
	if (snLen == 1 && sn[0] == ' ') snLen = 0;

	if (snLen > 0) {
		s = NdisAllocateMemory(&a->pppoeServiceName, snLen, 0, MaxAddress);
		if (s == NDIS_STATUS_SUCCESS) {
			NdisMoveMemory(a->pppoeServiceName, sn, snLen);
			a->pppoeServiceName[snLen] = 0;
			a->pppoeServiceNameLen = snLen;
		}
		else DbgPrint("can not allocate service name string\n");
	}
	else DbgPrint("Service-Name empty\n");

	if (acLen > 0) {
		s = NdisAllocateMemory(&a->pppoeACName, acLen, 0, MaxAddress);
		if (s == NDIS_STATUS_SUCCESS) {
			NdisMoveMemory(a->pppoeACName, ac, acLen);
			a->pppoeACName[acLen] = 0;
			a->pppoeACNameLen = acLen;
		}
		else DbgPrint("can not allocate AC name string\n");
	}
	else DbgPrint("AC-Name empty\n");
}

static void PppoeTimerfunc(PVOID sys1, PVOID ctx, PVOID sys2, PVOID sys3) {
	PADAPTER a = (PADAPTER)ctx;
	PPPOE_PACKET *p = NULL;
	PPPOE_PACKET tmpPack;
	static unsigned char echoRequest[] = {0xC0, 0x21, 0x09, 0x05, 0x00, 0x08, 0x01,
	0x02, 0x03, 0x04};

	FENTER("PppoeTimerfunc");

	switch (a->pppoeState) {
	case PPPOE_STATE_PADI:
		DbgPrint("State padi\n");

		if (a->pppoeRetries >= 3) {
			DbgPrint("PADI retries exceeded\n");
			PppoeTerminate(a);
			MiniEnterCallState(a, LINECALLSTATE_DISCONNECTED, LINEDISCONNECTMODE_NOANSWER);

//			a->pppoeState = PPPOE_STATE_SESSION;
//			MiniEnterLinedevState(a, LINEDEVSTATE_CONNECTED);
		}
		else {
			DbgPrint("sending PADI\n");
			++a->pppoeRetries;

			NdisSetTimer(&a->pppoeTimer, 3000);
			PppoePADI(a);
		}
		break;

	case PPPOE_STATE_PADR:
		DbgPrint("State padr\n");
		if (a->pppoeRetries >= 3) {
			DbgPrint("PADR retries exceeded\n");
			PppoeTerminate(a);
			MiniEnterCallState(a, LINECALLSTATE_DISCONNECTED, LINEDISCONNECTMODE_NOANSWER);
		}
		else {
			DbgPrint("sending PADR\n");
			++a->pppoeRetries;

			NdisSetTimer(&a->pppoeTimer, 3000);
			PppoePADR(a);
		}
		break;

	case PPPOE_STATE_SESSION:
		DbgPrint("State session\n");
		if(a->pppoePacketsReceived == 0) {
			if(a->pppoeLcpRetries < 2) {
				DbgPrint("Should send lcp echo request\n");
				p = PacketAllocate(sizeof(PPPOE_PACKET) + 46);
				p->vertype = 0x11;
				p->code = 0; //PPPOE_CODE_PADS;
				p->proto = 0x6488;
				p->sessionid = a->pppoeSessionId;
				p->len = NTOHS(46);
				NdisMoveMemory(p->source, a->protoMacAddr, 6);
				NdisMoveMemory(p->dest, a->pppoeServerMacAddr, 6);
				NdisMoveMemory(p->data, echoRequest, sizeof(echoRequest));
				PppoeSendPacket(a, p, 46);
			} else {
				DbgPrint("Connections is dead... ;(\n");
				PppoePADT(a);
				tmpPack.sessionid = a->pppoeSessionId;
				PppoeReceivePADT(a, &tmpPack);
			}
			a->pppoeLcpRetries++;
		} else {
			a->pppoeLcpRetries = 0;
		}
		a->pppoePacketsReceived = 0;
		DbgPrint("Sceduling next timer\n");
		NdisSetTimer(&a->pppoeTimer, 10000);
		break;
	default:
		DbgPrint("strange, state %d\n", a->pppoeState);
		break;
	}

	FLEAVE("PppoeTimerfunc");
}

void PppoeInitialize(PADAPTER a) {
	NDIS_STATUS s;

	FENTER("PppoeInitialize");

	NdisAllocatePacketPool(&s, &(a->pppoePacketPoolHandle), PPPOE_POOL_SIZE, 16);
	if (s != NDIS_STATUS_SUCCESS) {
		DbgPrint("failed to allocate packet pool");
		a->pppoePacketPoolHandle = NULL;
	} else {
		DbgPrint("packet pool: 0x%x\n", a->pppoePacketPoolHandle);
	}
	NdisAllocateBufferPool(&s, &(a->pppoeBufferPoolHandle), PPPOE_POOL_SIZE);
	if (s != NDIS_STATUS_SUCCESS) {
		DbgPrint("failed to allocate buffer pool");
		a->pppoeBufferPoolHandle = NULL;
	} else {
		DbgPrint("buffer pool: 0x%x\n", a->pppoeBufferPoolHandle);
	}

	a->pppoeState = PPPOE_STATE_IDLE;

	a->pppoeACName = NULL;
	a->pppoeACNameLen = 0;
	a->pppoeServiceName = NULL;
	a->pppoeServiceNameLen = 0;
	a->pppoeRealServiceName = NULL;
	a->pppoeRealServiceNameLen = 0;

	a->pppoeHostUniq = 0;

	a->pppoeACCookie = NULL;
	a->pppoeACCookieLen = 0;

	NdisInitializeTimer(&a->pppoeTimer, &PppoeTimerfunc, a);

	FLEAVE("PppoeInitialize");
}

NDIS_STATUS PppoeConnect(PADAPTER a, PCHAR address, ULONG alen) {
	NDIS_STATUS s;
	ULONG count;
	ULONG current_packet;
	PPPOE_PACKET *p;

	FENTER("PppoeConnect");

	if (a->pppoeState != PPPOE_STATE_IDLE) {
		DbgPrint("wrong state: %d\n", a->pppoeState);
		FLEAVE("PppoeConnect");
		return NDIS_STATUS_TAPI_INUSE;
	}

	PppoeInitService(a, address, alen);

	a->pppoeState = PPPOE_STATE_PADI;

	setPacketFilter(a, NDIS_PACKET_TYPE_DIRECTED);

	if (a->pppoePacketPoolHandle == NULL) {
		DbgPrint("no packet pool\n");
		FLEAVE("PppoeConnect");
		return NDIS_STATUS_FAILURE;
	}

	// before packet sending, add timeout timer
	a->pppoeRetries = 0;
	NdisSetTimer(&a->pppoeTimer, 3000);

	PppoePADI(a);

	FLEAVE("PppoeConnect");
	return NDIS_STATUS_SUCCESS;
}

void PppoeTerminate(PADAPTER a) {
	FENTER("PppoeTerminate");

	if (a->pppoeState == PPPOE_STATE_IDLE) {
		DbgPrint("terminate when IDLE, ignoring\n");
		FLEAVE("PppoeTerminate");
		return;
	}

	if (a->pppoeState == PPPOE_STATE_SESSION) {
		DbgPrint("terminate in SESSION state, sending PADT\n");
		PppoePADT(a);
	}

	if (a->pppoeState != PPPOE_STATE_PADT) {
		DbgPrint("terminate in non PADT state, shuting eth card\n");
		setPacketFilter(a, 0);
	}

	a->pppoeState = PPPOE_STATE_IDLE;

	if (a->pppoeACNameLen != 0) {
		NdisFreeMemory(a->pppoeACName, a->pppoeACNameLen, 0);
		a->pppoeACName = NULL;
		a->pppoeACNameLen = 0;
	}

	if (a->pppoeServiceNameLen != 0) {
		NdisFreeMemory(a->pppoeServiceName, a->pppoeServiceNameLen, 0);
		a->pppoeServiceName = NULL;
		a->pppoeServiceNameLen = 0;
	}

	if (a->pppoeRealServiceNameLen != 0) {
		NdisFreeMemory(a->pppoeRealServiceName, a->pppoeRealServiceNameLen, 0);
		a->pppoeRealServiceName = NULL;
		a->pppoeRealServiceNameLen = 0;
	}

	if (a->pppoeACCookieLen != 0) {
		NdisFreeMemory(a->pppoeACCookie, a->pppoeACCookieLen, 0);
		a->pppoeACCookie = NULL;
		a->pppoeACCookieLen = 0;
	}

	FLEAVE("PppoeTerminate");
}

NDIS_STATUS PppoeTransmit(PADAPTER a, PNDIS_WAN_PACKET p) {
	PNDIS_PACKET packet;
	PNDIS_BUFFER buffer;
	PTRANSMIT_PROTOINFO pinfo;
	PPPOE_PACKET *pack;
	NDIS_STATUS s;
	UINT count;

	FENTER("PppoeTransmit");

	if (a->pppoeState != PPPOE_STATE_SESSION) {
		DbgPrint("tx in non-SESSION state\n");

		FLEAVE("PppoeTransmit");
		return NDIS_STATUS_FAILURE;
	}

	NdisAllocatePacket(&s, &packet, a->pppoePacketPoolHandle);
	if (s != NDIS_STATUS_SUCCESS) {
		DbgPrint("failed to allocate packet\n");

		FLEAVE("PppoeTransmit");
		return NDIS_STATUS_FAILURE;
	}

	// Dirty, but necessary hack here:
	// PPP gives us packets with ADDRESS CONTROL field at the beginning of PPP packet.
	// RFC tells us that we should not be sending this, so we wont :)
	// this means that we set up PPPOE_PACKET pointer so that pppoe header overwrites first 2 bytes of
	// PPP packet.

	NdisAllocateBuffer(&s, &buffer, a->pppoeBufferPoolHandle, p->CurrentBuffer - 18, p->CurrentLength + 18);
	if (s != NDIS_STATUS_SUCCESS) {
		DbgPrint("failed to allocate buffer\n");

		FLEAVE("PppoeTransmit");
		return NDIS_STATUS_FAILURE;
	}

	pack = (PPPOE_PACKET *)((ULONG)p->CurrentBuffer - 18);
	NdisMoveMemory(pack->dest, a->pppoeServerMacAddr, 6);
	NdisMoveMemory(pack->source, a->protoMacAddr, 6);
	pack->proto = 0x6488;
	pack->code = 0;
	pack->len = HTONS(p->CurrentLength - 2);
	pack->sessionid = a->pppoeSessionId;
	pack->vertype = 0x11;

	NdisChainBufferAtFront(packet, buffer);

	DbgPrint("Send packet length: %i\n", p->CurrentLength + 18);
	/*
	for(count = 0; count < (p->CurrentLength > 64 ? 64 : p->CurrentLength); count++) {
		DbgPrint("%2.2X ", p->CurrentBuffer[count]);
		if((count % 16) == 15) DbgPrint("\n");
	}
	DbgPrint("\n");
*/
	pinfo = (PTRANSMIT_PROTOINFO)packet->ProtocolReserved;
	pinfo->wanPacket = p;
	pinfo->pppoePacket = NULL;

	NdisSend(&s, a->protoBindingHandle, packet);
	if (s == NDIS_STATUS_PENDING) {
		DbgPrint("Packet send pending\n");
	}
	else {
		DbgPrint("NdisSend status: 0x%x\n", s);

		MyFreeNdisPacket(packet);
	}

	FLEAVE("PppoeTransmit");
	return s;
}