packetio.c

Vista 核心Rally技术之-LLTD 实现源代码

/*
 * LICENSE NOTICE.
 *
 * Use of the Microsoft Windows Rally Development Kit is covered under
 * the Microsoft Windows Rally Development Kit License Agreement,
 * which is provided within the Microsoft Windows Rally Development
 * Kit or at http://www.microsoft.com/whdc/rally/rallykit.mspx. If you
 * want a license from Microsoft to use the software in the Microsoft
 * Windows Rally Development Kit, you must (1) complete the designated
 * "licensee" information in the Windows Rally Development Kit License
 * Agreement, and (2) sign and return the Agreement AS IS to Microsoft
 * at the address provided in the Agreement.
 */

/*
 * Copyright (c) Microsoft Corporation 2005.  All rights reserved.
 * This software is provided with NO WARRANTY.
 */

#include <stdio.h>
#include <assert.h>

#include <string.h>
#include <errno.h>

#include "globals.h"

#include "statemachines.h"
#include "packetio.h"

/********************* sole linkage to QOS handlers **************************/

extern void qosrcvpkt( void );

/*****************************************************************************/

void
packetio_invalidate_retxbuf(void)
{
    g_re_tx_opcode = Opcode_INVALID;
    g_rtxseqnum = g_re_tx_len = 0;
}

/************************** P A C K E T   V A L I D A T O R S   **************************/

/* These are packet-validation handlers for each g_opcode that can be further validated.
 * Global header pointers have already been set up, and global length saved.
 *
 * These routines validate packet lengths, etc, and set the event type. They also
 * provide a point for tracing interesting packets.
 */

enum packet_verification_results {
    VALID_PACKET,
    INVALID_PACKET
};

static uint
validate_discover(void)
{
    if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) +  sizeof(topo_discover_header_t))
    {
        warn("verify_discover: frame with truncated Discover header (len=" FMT_SIZET " src="
              ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
              g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
        g_this_event.evtType =  evtInvalidPacket;
        return INVALID_PACKET;
    }
    g_this_event.evtType =  evtDiscoverRcvd;
    return VALID_PACKET;
}

static uint
validate_hello()
{
    if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + sizeof(topo_hello_header_t) + 1)
    {
	warn("rx_hello: frame with truncated Hello header (len=" FMT_SIZET " src="
	     ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
	     g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
        g_this_event.evtType =  evtInvalidPacket;
	return INVALID_PACKET;
    }

    IF_TRACED(TRC_PACKET)
	uint16_t gen = ntohs(g_hello_hdr->hh_gen);

	dbgprintf("hello-rcvd: gen=%d mapper=" ETHERADDR_FMT "\n",
		gen, ETHERADDR_PRINT(&g_hello_hdr->hh_curmapraddr));
    END_TRACE

    g_this_event.evtType =  evtPacketRcvd;
    return VALID_PACKET;
}


static uint
validate_queryltlv()
{
    if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + sizeof(topo_qltlv_header_t))
    {
	warn("query-ltlv-rcvd: frame with truncated qltlv header (len=" FMT_SIZET " src="
	     ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
	     g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
        g_this_event.evtType =  evtInvalidPacket;
	return INVALID_PACKET;
    }

    IF_TRACED(TRC_PACKET)
	dbgprintf("query-ltlv-rcvd: tlv# %d  reserved: %d  offset: %d\n",
	          g_qltlv_hdr->qh_type, g_qltlv_hdr->qh_rsvd1, g_qltlv_hdr->qh_offset);
    END_TRACE

    g_this_event.evtType =  evtPacketRcvd;
    return VALID_PACKET;
}


static char *
ed_type2name(uint8_t type)
{
    static char buf[12]; /* caution: not thread-safe! */
    switch (type)
    {
    case 0: return "Train";
    case 1: return "Probe";
    case 2: return "TimedProbe";
    default:
	snprintf(buf, sizeof(buf), "%d", type);
	return buf;
    }
}


static uint
validate_emit()
{
    topo_emit_header_t      *emit;	/* pointer to emit header in g_rxbuf */
    topo_emitee_desc_t *ed, *first_desc, *limit;
    uint16_t numdescs;
    int      i;

    /* Parse the Emit */
    g_totalPause = 0;
    g_neededPackets = 0;
    g_neededBytes = 0;

    if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + 
	sizeof(topo_emit_header_t))
    {
	warn("validate_emit: frame with truncated Emit header (len=" FMT_SIZET " src="
	     ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
	     g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
        g_this_event.evtType =  evtInvalidPacket;
	return INVALID_PACKET;
    }

    emit = (topo_emit_header_t*)(g_base_hdr + 1);	// base the emit header in g_rxbuf

    numdescs = ntohs(emit->eh_numdescs);
    limit = (topo_emitee_desc_t*)(((uint8_t*)g_ethernet_hdr) + g_rcvd_pkt_len);

    /* check the emitee_descs are asking for reasonable transmissions */
    first_desc = (topo_emitee_desc_t*)(emit + 1);

    IF_TRACED(TRC_PACKET)
	dbgprintf("numdescs=%d EmiteeDescs=[", numdescs);
    END_TRACE

    for (i=0, ed=first_desc; ed+1 <= limit && i < numdescs; ed++, i++)
    {
	IF_TRACED(TRC_PACKET)
	    dbgprintf("{%s: %dms " ETHERADDR_FMT " -> " ETHERADDR_FMT "} ",
		    ed_type2name(ed->ed_type), ed->ed_pause,
		    ETHERADDR_PRINT(&ed->ed_src), ETHERADDR_PRINT(&ed->ed_dst));
        END_TRACE

	g_totalPause += ed->ed_pause;

	if (ed->ed_type != 0x00 && ed->ed_type != 0x01)
	{
	    warn("validate_emit: emitee_desc from " ETHERADDR_FMT " with unknown type=%d; "
		 "ignoring whole Emit request\n",
		 ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ed->ed_type);
            g_this_event.evtType =  evtInvalidPacket;
	    return INVALID_PACKET;
	}
	
	if (!ETHERADDR_EQUALS(&ed->ed_src, &g_hwaddr) &&
	    !TOPO_ETHERADDR_OUI(&ed->ed_src))
	{
	    warn("validate_emit: emitee_desc with src=" ETHERADDR_FMT " is invalid; "
		 "ignoring whole Emit request\n",
		 ETHERADDR_PRINT(&ed->ed_src));
            g_this_event.evtType =  evtInvalidPacket;
	    return INVALID_PACKET;
	}
	
	if (ETHERADDR_IS_MCAST(&ed->ed_dst))
	{
	    warn("validate_emit: emitee_desc with dst=" ETHERADDR_FMT " is invalid; "
		 "ignoring whole Emit request\n",
		 ETHERADDR_PRINT(&ed->ed_dst));
            g_this_event.evtType =  evtInvalidPacket;
	    return INVALID_PACKET;
	}

	g_neededPackets++;
	g_neededBytes += sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
    }

    IF_TRACED(TRC_PACKET)
	dbgprintf("]");
	if (i != numdescs)
	    dbgprintf(" (numdescs too big!)");
	dbgprintf("\n");
    END_TRACE

    if (g_totalPause > TOPO_TOTALPAUSE_MAX)
    {
	warn("validate_emit: Emit contains emitee_descs with total pausetime " FMT_UINT32 "ms > %ums; "
	     "ignoring whole Emit request\n",
	     g_totalPause, TOPO_TOTALPAUSE_MAX);
        g_this_event.evtType =  evtInvalidPacket;
	return INVALID_PACKET;
    }

    if (i != numdescs)
    {
	warn("validate_emit: numdescs=%d but only %d descs in frame\n", numdescs, i);
	numdescs = i;
    }

    /* will we need to send an ACK or Flat too? */
    if (g_sequencenum != 0)
    {
	g_neededPackets++;
	g_neededBytes += sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
    }

    /* emitee_descs look ok; pass them up to state machine */

    g_this_event.numDescrs = numdescs;
    g_this_event.evtType =  evtEmitRcvd;
    return VALID_PACKET;
}


/************************** T X - F R A M E   F O R M A T T I N G **************************/

/* Format an Ethernet and Base header into "buf", returning a pointer to the next
 * header location.  Takes "srchw" and "dsthw" for the Base header's realsrc/dst, and 
 * uses them for the Ethernet header too unless "use_broadcast" is TRUE, in which case
 * the Ethernet dst is set to the broadcast address.
 * The "g_opcode" is which g_opcode to use
 * The "seqnum" is a host-endian sequence number (or 0).
 * Does no checking that "buf" contains sufficient space - use carefully! */
void *
fmt_base(uint8_t *buf, const etheraddr_t *srchw, const etheraddr_t *dsthw, lld2_tos_t tos,
	 topo_opcode_t g_opcode, uint16_t seqnum, bool_t use_broadcast)
{
    topo_ether_header_t *ehdr = (topo_ether_header_t*) buf;
    topo_base_header_t  *bhdr = (topo_base_header_t*)(ehdr + 1);

    IF_TRACED(TRC_PACKET)
        printf("fmt_base: src=" ETHERADDR_FMT "\n", ETHERADDR_PRINT(srchw));
        printf("fmt_base: dst=" ETHERADDR_FMT "\n", ETHERADDR_PRINT(dsthw));
        printf("fmt_base: tos=%d g_opcode=%d seq=%d bcast=%s\n", \
               tos, g_opcode, seqnum, use_broadcast?"true":"false");
    END_TRACE
   
    ehdr->eh_src = *srchw;
    ehdr->eh_dst = use_broadcast? Etheraddr_broadcast : *dsthw;
    ehdr->eh_ethertype = TOPO_ETHERTYPE;
    
    bhdr->tbh_version = TOPO_VERSION;
    bhdr->tbh_tos = (uint8_t)tos;
    bhdr->tbh_opcode = (uint8_t)g_opcode;
    bhdr->tbh_realsrc = *srchw;
    bhdr->tbh_realdst = *dsthw;
    bhdr->tbh_seqnum = htons(seqnum);

    return (void*)(bhdr+1);
}

/* low-level packet transmit function, used below */
void
tx_write(uint8_t *buf, size_t nbytes)
{
    ssize_t ret;

    ret = osl_write(g_osl, buf, nbytes);

    if (ret < 0)
	die("packetio_tx_write: write to socket failed: %s\n", strerror(errno));
    if (ret != (ssize_t)nbytes)
	die("packetio_tx_write: socket write for " FMT_SIZET " bytes, but %d went out\n",
	    nbytes, ret);
//#define BYTE_DUMP
#ifdef BYTE_DUMP
    {
	int i,j=(nbytes>48)?48:nbytes;
	printf("sending " FMT_SIZET " bytes:\n",nbytes);
	for (i=0; i<j; i++)
	{
	    printf("%02x%s%s", buf[i], (i%2)==1?" ": "", (i%16)==15? "\n": "");
	}
	printf("\n");
    }
#endif
}

/* usual way of sending a packet, potentially keeping a copy in the
 * rtx buffer and consuming a sequence number. */
static void
tx_sendpacket(size_t nbytes, uint16_t thisSeqNum)
{
    tx_write(g_txbuf, nbytes);
    g_tx_len = nbytes;

    /* if this is a reliable packet, then copy the g_txbuf aside to the
     * g_re_txbuf, and consume the sequence number. */
    if (thisSeqNum)
    {
	memcpy(g_re_txbuf, g_txbuf, nbytes);
	g_rtxseqnum = thisSeqNum;
	g_re_tx_len = nbytes;
        g_re_tx_opcode = g_opcode;	// save the opcode that generated this response, for retry-checking
    }
}

/* sent the retransmit buffer */
static void
tx_rtxpacket(void)
{
    assert(g_rtxseqnum != 0);
    IF_TRACED(TRC_PACKET)
	dbgprintf("packetio_recv_handler: retranmitting response with seqnum=%d\n", g_rtxseqnum);
    END_TRACE

    tx_write(g_re_txbuf, g_re_tx_len);    
}


/************************** M E S S A G E   S E N D E R S **************************/


void
packetio_tx_hello(void)
{
    topo_hello_header_t *tx_hh;
    size_t nbytes;

    tx_hh = fmt_base(g_txbuf, &g_hwaddr, &Etheraddr_broadcast, ToS_TopologyDiscovery, Opcode_Hello, 0, TRUE);
    tx_hh->hh_gen = htons(g_generation);
    if (g_topo_session != NULL && g_topo_session->ssn_is_valid)
    {
        tx_hh->hh_curmapraddr = g_topo_session->ssn_mapper_real;
        tx_hh->hh_aprmapraddr = g_topo_session->ssn_mapper_current;
    } else {
        memset(&tx_hh->hh_curmapraddr, 0, sizeof(etheraddr_t));
        memset(&tx_hh->hh_aprmapraddr, 0, sizeof(etheraddr_t));
    }
    nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + sizeof(topo_hello_header_t);

    /* write the TLVs (reserving room for the EOP byte) */
    nbytes += tlv_write_info(&g_txbuf[nbytes], TOPO_MAX_FRAMESZ - nbytes - 1);

    /* now add the EOP */
    g_txbuf[nbytes] = 0;
    nbytes++;

    tx_sendpacket(nbytes, 0);

    IF_TRACED(TRC_PACKET)
	dbgprintf("tx_hello (%s): topo-ssn=%p gen=%d\n",
	          (g_topo_session && g_topo_session->ssn_is_valid)?"topo":"quick",
	          g_topo_session, ntohs(tx_hh->hh_gen));
    END_TRACE
}


void
packetio_tx_queryresp(void)
{
    topo_queryresp_header_t *qr;
    topo_recvee_desc_t *p;
    size_t nbytes, bytes_left;

    qr = fmt_base(g_txbuf, &g_hwaddr, &g_topo_session->ssn_mapper_real, ToS_TopologyDiscovery, Opcode_QueryResp,
		  g_sequencenum, FALSE /*g_topo_session->ssn_use_broadcast*/);

    nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) +
	sizeof(topo_queryresp_header_t);