discovery.c

PPPoE协议在Psos中的实现源代码

			   (unsigned) conn->peerEth[2],
			   (unsigned) conn->peerEth[3],
			   (unsigned) conn->peerEth[4],
			   (unsigned) conn->peerEth[5]);
		    printf("--------------------------------------------------\n");
	    }
	    syslog(LOG_DEBUG, "PADO: peer Mac: %02x:%02x:%02x:%02x:%02x:%02x", (unsigned) conn->peerEth[0],(unsigned) conn->peerEth[1],
	          (unsigned) conn->peerEth[2],(unsigned) conn->peerEth[3],(unsigned) conn->peerEth[4],(unsigned) conn->peerEth[5]);
	    conn->discoveryState = STATE_RECEIVED_PADO;
    }
    
    sendPADR(conn); 
    return;
error:
    pfree((char *)ptr);
    return;    	
}	
/***********************************************************************
*%FUNCTION: sendPADR
*%ARGUMENTS:
* conn -- PPPoE connection structur
*%RETURNS:
* Nothing
*%DESCRIPTION:
* Sends a PADR packet
***********************************************************************/
void
sendPADR(PPPoEConnection *conn)
{
    UINT16_t namelen = 0;
    UINT16_t plen;
    u_char *outp = outpacket_buf;
    char *ptr;
    int tm,i;
    
    if (conn->serviceName) {
	namelen = (UINT16_t) strlen(conn->serviceName);
    }
    plen = TAG_HDR_SIZE + namelen;
    
    if (conn->cookie.type)  plen += ntohs(conn->cookie.length) + TAG_HDR_SIZE;
    if (conn->relayId.type) plen += ntohs(conn->relayId.length) + TAG_HDR_SIZE;	
    
    tm = PADI_TIMEOUT;
    for(i=0;i<conn->padr_trans;i++) tm = tm*2;
  
    PUTSHORT(ETH_PPPOE_DISCOVERY, outp);
    PUTCHAR(0x11,outp); 
    PUTCHAR(CODE_PADR, outp);
    PUTSHORT(0, outp);
    PUTSHORT(plen, outp);
    PUTSHORT(TAG_SERVICE_NAME, outp);
    PUTSHORT(namelen, outp);
    if (conn->serviceName) {
	BCOPY(conn->serviceName, outp, namelen);
    }
    if (conn->cookie.type) {
    	ptr = (char *)(&(conn->cookie));
    	BCOPY(ptr, outp, (ntohs(conn->cookie.length) + TAG_HDR_SIZE));
    }
    if (conn->relayId.type) {
    	ptr = (char *)(&(conn->relayId));
    	BCOPY(ptr, outp, (ntohs(conn->relayId.length) + TAG_HDR_SIZE));
    }
    	
    output( conn->peerEth, 0, outpacket_buf, plen+8 );
   
    TIMEOUT(padr_timeout, (caddr_t)conn, tm);
    ++conn->padr_trans;
    conn->discoveryState = STATE_SENT_PADR;
    syslog(LOG_INFO, "send PADR: Sent code %d .", CODE_PADR);
   
}

/***********************************************************************
*%FUNCTION: recvPADS
*%ARGUMENTS:   *conn -- PPPoEConnection structure
*%RETURNS:      Nothing
*%DESCRIPTION:  Receive a PADO packet
***********************************************************************/
void
recvPADS(PPPoEConnection *conn, unsigned char *ptr, unsigned int num)
{
    PPPoEPacket *packet;
        
    packet = (PPPoEPacket *)ptr;
    
    if (ntohs(packet->length) + HDR_SIZE > num) {
	    syslog(LOG_ERR, "Bogus PPPoE length field (%u)", (unsigned int) ntohs(packet->length));
	    goto error;
    }
    
    if (memcmp(packet->h_source, conn->peerEth, ETH_ALEN)) {
    	    syslog(LOG_ERR, "This packet is not from AC !");
    	    goto error;
    } 
    
    if (memcmp(packet->h_dest, conn->myEth, ETH_ALEN)) { 
    	   syslog(LOG_ERR,"This packet is not for me !"); 
    	   goto error; 
    }
    
    parsePacket(packet, parsePADSTags, conn);
    conn->discoveryState = STATE_SESSION; 
    conn->session = ntohs(packet->session);
    syslog(LOG_INFO, "PPP session is %d", (int) ntohs(conn->session));

    /* RFC 2516 says session id MUST NOT be zero or 0xFFFF */
    if (conn->session == 0 || conn->session == 0xFFFF) {
	syslog(LOG_ERR, "Access concentrator used a session value of %x -- the AC is violating RFC 2516", (unsigned int) ntohs(conn->session));
    }
    lcp_lowerup(0);
    lcp_open(0);
    return;
error:
    pfree((char *)ptr);
    return;    	
    	
}	

static void
padi_timeout(arg)
    caddr_t arg;
{
    PPPoEConnection *u = (PPPoEConnection *) arg;

    if (u->discoveryState != STATE_SENT_PADI)
	return;

    if (u->padi_trans >= MAX_PADI_ATTEMPTS) {
	/* give up in disgust */
	syslog(LOG_ERR, "Dont receive PADO");
	return;
    }

    sendPADI(u);		/* Send PADI */
}	

static void
padr_timeout(arg)
    caddr_t arg;
{
    PPPoEConnection *u = (PPPoEConnection *) arg;

    if (u->discoveryState != STATE_SENT_PADR)
	return;

    if (u->padr_trans >= MAX_PADI_ATTEMPTS) {
	/* give up in disgust */
	syslog(LOG_ERR, "Dont receive PADS");
	return;
    }

    sendPADR(u);		/* Send PADI */
}	

int
parsePacket(PPPoEPacket *packet, ParseFunc *func, void *extra)
{
    UINT16_t len = ntohs(packet->length);
    unsigned char *curTag;
    UINT16_t tagType, tagLen;

    if (packet->ver != 1) {
	syslog(LOG_ERR, "Invalid PPPoE version (%d)", (int) packet->ver);
	return -1;
    }
    if (packet->type != 1) {
	syslog(LOG_ERR, "Invalid PPPoE type (%d)", (int) packet->type);
	return -1;
    }

    /* Do some sanity checks on packet */
    if (len > ETH_DATA_LEN - 6) { /* 6-byte overhead for PPPoE header */
	syslog(LOG_ERR, "Invalid PPPoE packet length (%u)", len);
	return -1;
    }

    /* Step through the tags */
    curTag = packet->payload;
    while(curTag - packet->payload < len) {
	/* Alignment is not guaranteed, so do this by hand... */
	tagType = (((UINT16_t) curTag[0]) << 8) +
	    (UINT16_t) curTag[1];
	tagLen = (((UINT16_t) curTag[2]) << 8) +
	    (UINT16_t) curTag[3];
	if (tagType == TAG_END_OF_LIST) {
	    return 0;
	}
	if ((curTag - packet->payload) + tagLen + TAG_HDR_SIZE > len) {
	    syslog(LOG_ERR, "Invalid PPPoE tag length (%u)", tagLen);
	    return -1;
	}
	func(tagType, tagLen, curTag+TAG_HDR_SIZE, extra);
	curTag = curTag + TAG_HDR_SIZE + tagLen;
    }
    return 0;
}

void
sendPADT(PPPoEConnection *conn, char const *msg)
{
    u_char *outp = outpacket_buf;         	
    UINT16_t plen = 0;
    char *ptr;
    UINT16_t elen;
    
    /* Do nothing if no session established yet */
    if (!conn->session) return;
    
    if (msg)  plen += strlen(msg) + TAG_HDR_SIZE;
    if (conn->cookie.type)  plen += ntohs(conn->cookie.length) + TAG_HDR_SIZE;
    if (conn->relayId.type) plen += ntohs(conn->relayId.length) + TAG_HDR_SIZE;	
    
    PUTSHORT(ETH_PPPOE_DISCOVERY, outp);
    PUTCHAR(0x11,outp); 
    PUTCHAR(CODE_PADT, outp);
    PUTSHORT(conn->session, outp);  conn->session = 0;
    PUTSHORT(plen, outp);	
    
    if (msg) {
    	elen = strlen(msg);
    	PUTSHORT(TAG_GENERIC_ERROR, outp);
    	PUTSHORT(elen, outp);
	BCOPY( msg, outp, elen );
	INCPTR(elen, outp);
	
    }
    if (conn->cookie.type) {
    	ptr = (char *)(&(conn->cookie));
    	BCOPY(ptr, outp, (ntohs(conn->cookie.length) + TAG_HDR_SIZE));
    	INCPTR((ntohs(conn->cookie.length) + TAG_HDR_SIZE), outp);
    }
    if (conn->relayId.type) {
    	ptr = (char *)(&(conn->relayId));
    	BCOPY(ptr, outp, (ntohs(conn->relayId.length) + TAG_HDR_SIZE));
    }
    	
    output( conn->peerEth, 0, outpacket_buf, plen+8 );

    syslog(LOG_INFO, "send PADT: Sent code %d .", CODE_PADT);
    
}	

void
recvPADT(PPPoEConnection *conn, unsigned char *ptr, unsigned int num)
{
    PPPoEPacket *packet;
        
    packet = (PPPoEPacket *)ptr;
    
    if (ntohs(packet->length) + HDR_SIZE > num) {
	    syslog(LOG_ERR, "PADT:Bogus PPPoE length field (%u)", (unsigned int) ntohs(packet->length));
	    goto error;
    }
    
    if (memcmp(packet->h_source, conn->peerEth, ETH_ALEN)) {
    	    syslog(LOG_ERR, "PADT:This packet is not from AC !");
    	    goto error;
    } 
    
    if (memcmp(packet->h_dest, conn->myEth, ETH_ALEN)) { 
    	   syslog(LOG_ERR,"PADT:This packet is not for me !"); 
    	   goto error; 
    }
    if (ntohs(packet->session) != conn->session) {
	   syslog(LOG_ERR,"PADT:This packet's id doesn't match !"); 
    	   goto error; 
    }	
    syslog(LOG_INFO,"Session %d terminated -- received PADT from peer",(int) ntohs(packet->session));
    
    parsePacket(packet, parseLogErrs, NULL);
    sendPADT(conn, "Received PADT from peer");
    
error:
    pfree((char *)ptr);
    return;    	    
    
}