udp.c

杭州立宇泰豪华型44B0开发板

/* UDP functions*/

#include <stdio.h>
#include <string.h>
#include <ctype.h>

#include "ether.h"
#include "netutil.h"
#include "net.h"
#include "ip.h"
#include "udp.h"

int udpdebug;                   /* Flag to enable TCP packet display */
extern BYTE bcast[MACLEN];
extern UDPKT udpkt;

/* Return UDP data length (-1 if no data), 0 if not UDP */
int is_udp(IPKT *ip, int len)
{
    UDPKT *udp;
    WORD sum;
    int dlen=0;
                                        /* Check protocol & minimum length */
    if (ip->i.pcol==PUDP && len>=sizeof(UDPHDR))
    {
        udp = (UDPKT *)ip;              /* Do checksum */
        sum = check_udp(udp, swapl(ip->i.sip), swapl(ip->i.dip), len);
        if (!udp->u.check || sum==0xffff)
        {                               /* If zero or correct.. */
            swap_udp(udp);              /* Do byte-swaps */
            len -= sizeof(UDPHDR);      /* Subtract header len */
            if (udpdebug)               /* Display segment if in debug mode */
                disp_udp(udp, len, 0);
            dlen = len>0 ? len : -1;    /* Return -1 if data len=0 */
        }
        else if (udpdebug)              /* Display error */
            Uart_Printf("  ERROR: UDP checksum %04X\n", sum);
    }
    return(dlen);
}

/* Make a UDP datagram given the source & destination, data len */
int make_udp(GENFRAME *gfp, NODE *srcep, NODE *destp, WORD dlen)
{
    UDPKT *udp;
    int ulen, ilen,i;
	//WORD tempchk;
	//LWORD tempsip,tempdip;
	BYTE *tempp;
	
    //udp = (UDPKT *)getframe_datap(gfp);
    tempp = (BYTE *)getframe_datap(gfp);
	/*Uart_Printf("\n");
	for(i=0;i<64;i++)
		Uart_Printf("%x,",*(tempp+i));
	Uart_Printf("\n");*/
	udp = &udpkt;
	//udp=(UDPKT *)tempp;
    udp->u.sport = srcep->port;          /* Set ports */
    udp->u.dport = destp->port;
    udp->u.len = ulen = dlen + sizeof(UDPHDR);
    udp->u.check = 0;

	for(i=0;i<dlen;i++)
		udp->data[i]=*(tempp + 28+i);
    if (udpdebug)                       /* Display datagram if in debug mode */
        disp_udp(udp, dlen, 1);
    swap_udp(udp);                      /* Byte-swap */

    memcpy(&tempp[20],(BYTE *)(&(udp->u.sport)),sizeof(WORD));
	memcpy(&tempp[22],(BYTE *)(&(udp->u.dport)),sizeof(WORD));
	memcpy(&tempp[24],(BYTE *)(&(udp->u.len)),sizeof(WORD));
	memcpy(&tempp[26],(BYTE *)(&(udp->u.check)),sizeof(WORD));

    ilen = make_ip(gfp, srcep, destp, PUDP, (WORD)(ulen));
	memcpy((BYTE *)(&(udp->i)),tempp,20);

  /*  Uart_Printf("gft:\n");
	for(i=0;i<40;i++)
		Uart_Printf("%2x,",*(tempp+i));
	Uart_Printf("\n");

	tempudp=(BYTE *)(&udpkt);
	Uart_Printf("udp:\n");
	for(i=0;i<40;i++)
		Uart_Printf("%2x,",*(tempudp+i));
	Uart_Printf("\n");*/
	
    udp->u.check = ~check_udp(udp,udp->i.sip,udp->i.dip,ulen);
    if (udp->u.check == 0)              /* Change sum of 0 to FFFF */
        udp->u.check = 0xffff;
    
	memcpy(&tempp[26],(BYTE *)(&(udp->u.check)),sizeof(WORD));

    /*Uart_Printf("\n");
	for(i=0;i<64;i++)
		Uart_Printf("%x,",*(tempp+i));
	Uart_Printf("\n");*/
	
    return(ilen);                       /* Return IP length */
}

/* Return TCP checksum, given UDP (header + data) length.
** The values must be in network byte-order */
WORD check_udp(UDPKT *udp, LWORD sip, LWORD dip, int ulen)
{
    PHDR tph;
    LWORD sum;

    sum = csum((void *)(&udp->u), (WORD)ulen);            /* Checksum TCP segment */
    tph.len = swapw((WORD)ulen);                /* Make pseudo-header */
    tph.srce = sip;
    tph.dest = dip;
    tph.z = 0;
    tph.pcol = udp->i.pcol;
    sum += csum((void *)(&tph), sizeof(tph));             /* Checksum pseudo-header */
    return((WORD)(sum + (sum >> 16)));          /* Return total plus carry */
}

/* Swap byte order of ints in TCP header */
void swap_udp(UDPKT *udp)
{
    udp->u.sport = swapw(udp->u.sport);
    udp->u.dport = swapw(udp->u.dport);
    udp->u.len = swapw(udp->u.len);
}

/* Return the max TCP seg (data) size for a given frame without fragmentation */
int udp_maxdata(GENFRAME *gfp)
{
    return(maxi(ip_maxdata(gfp)-sizeof(UDPHDR), 0));
}

/* Get the frame driver type, source port, IP and Ethernet addrs */
void getudp_srce(GENFRAME *gfp, NODE *np)
{
    TCPKT *tcp;

    memset((BYTE *)np, 0, sizeof(NODE));        /* Clear unused fields */
    getip_srce(gfp, np);                /* Get dtype, srce IP and Ether addrs */
    tcp = getframe_datap(gfp);
    np->port = tcp->t.sport;            /* Get source port */
}

/* Get the frame driver type, destination port, IP and Ethernet addrs */
void getudp_dest(GENFRAME *gfp, NODE *np)
{
    TCPKT *tcp;

    memset((BYTE *)np, 0, sizeof(NODE));        /* Clear unused fields */
    getip_dest(gfp, np);                /* Get dtype, dest IP and Ether addrs */
    tcp = getframe_datap(gfp);
    np->port = tcp->t.dport;            /* Get dest port */
}

/* Get complete TCP local node data corresponding to frame dest IP address
** Return 0 if no matching node */
int getudp_locdest(GENFRAME *gfp, NODE *np)
{
    UDPKT *udp;
    int ok;

    ok = getip_locdest(gfp, np);        /* Get addresses, dtype & netmask */
    udp = getframe_datap(gfp);          /* Get dest port */
    np->port = udp->u.dport;
    return(ok);
}

/* Display TCP segment */
void disp_udp(UDPKT *udp, int dlen, int tx)
{
    if (tx)
        Uart_Printf("\n     /port %u->%u ", udp->u.sport, udp->u.dport);
    else
        Uart_Printf("\n     \\port %u<-%u ", udp->u.dport, udp->u.sport);
    Uart_Printf(" dlen %u", dlen);
}

/* Make a TCP segment given the socket, flags, data len */
int make_tftp_req(GENFRAME *gfp, NODE *sp, NODE *dp, WORD op, char *fname,
    char *mode)
{
    TFTP_REQ *tpr;
    WORD len;

    tpr = getframe_datap(gfp);
    tpr->op = swapw(op);
    len = strlen(strcpy(tpr->data, fname)) + 1;
    len += strlen(strcpy(&tpr->data[len], mode)) + 1;
    return(make_udp(gfp, sp, dp, (WORD)(len+2)));
}

/* EOF */