netudp.c

基于东南大学开发的SEP3203的ARM7中的所有驱动

    long segLen;

    long packetLen;

    const unsigned char* d;

    u_int mtu;



    UDPDEBUG(("udpWrite()\n"));

    if (!(udps[ud].flags & FUDP_OPEN)) return -1;

    mtu = ipMTU(htonl(udps[ud].theirAddr.s_addr));

    if (!mtu) {

        UDPDEBUG(("no route to host\n"));

        return -1;

    }

    while (len) {

        pNBuf = NULL;

#if ONETASK_SUPPORT > 0

		// We can't do a loop forever in a onetask OS

        nGET(pNBuf);

		// No more nBuf buffers available, should return what we have copied so far

		if (!pNBuf) return rtn;

#else

		// BUSY WAITING! (not nice)

        do {

            nGET(pNBuf);

        } while (pNBuf == NULL);

#endif

        pNBufTail = pNBuf;

        packetLen = MIN(mtu, sizeof(IPHdr) + sizeof(UDPHdr) + len);



        /* build IP header */

        ipHdr = nBUFTOPTR(pNBufTail, IPHdr*);

        ipHdr->ip_v = 4;

        ipHdr->ip_hl = sizeof(IPHdr) / 4;

        ipHdr->ip_tos = udps[ud].tos;

        ipHdr->ip_len = packetLen;

        ipHdr->ip_id = IPNEWID();

        ipHdr->ip_off = 0;

        ipHdr->ip_ttl = 0;

        ipHdr->ip_p = IPPROTO_UDP;

        ipHdr->ip_sum = htons(ipHdr->ip_len - sizeof(IPHdr));



        ipHdr->ip_src.s_addr = htonl(localHost);

//        ipHdr->ip_dst.s_addr = htonl(udps[ud].theirAddr.s_addr);

        ipHdr->ip_dst.s_addr = udps[ud].theirAddr.s_addr;



        pNBuf->chainLen = packetLen;

        packetLen -= sizeof(IPHdr);



        /* Build the UDP header */

        udpHdr = (UDPHdr*)(ipHdr + 1);

        udpHdr->srcPort = udps[ud].ourPort;

        udpHdr->dstPort = udps[ud].theirPort;

        udpHdr->length = packetLen;

        udpHdr->checksum = 0;

        pNBufTail->data = (unsigned char*)(udpHdr+1);

        pNBufTail->len = sizeof(IPHdr) + sizeof(UDPHdr);

        packetLen -= sizeof(UDPHdr);

        len -= packetLen;





        UDPDEBUG(("-src=%s port %ld dest=%s port=%ld\n", \

            ip_ntoa(/*htonl*/(ipHdr->ip_src.s_addr)),   \

            htons(udpHdr->srcPort),                     \

            ip_ntoa2(/*htonl*/(ipHdr->ip_dst.s_addr)),  \

            htons(udpHdr->dstPort)                      \

            ));



        /* copy data to nBuf chain */

        d = (unsigned char*)buf;

        while (packetLen) {

            segLen = MIN(NBUFSZ - pNBufTail->len, packetLen);

            memcpy(pNBufTail->data, d, segLen);

            pNBufTail->len += segLen;

            packetLen -= segLen;

            if (packetLen) {

                do {

                    nGET(pNBufTail->nextBuf);

                } while (!pNBufTail->nextBuf);

                pNBufTail = pNBufTail->nextBuf;

                pNBufTail->nextBuf = NULL;

                pNBufTail->len = 0;

            }

        }

        pNBuf->data = pNBuf->body;

        udpHdr->checksum = inChkSum(pNBuf, pNBuf->chainLen - 8, 8);

        ipHdr->ip_ttl = UDPTTL;

      //  DUMPCHAIN(pNBuf);

        /* Pass the datagram to IP and we're done. */

        ipRawOut(pNBuf);

    }

    return rtn;

}



#endif



long udpRecvFrom(int ud, void* buf, long len, struct sockaddr_in* from)

{

    long rtn;



    UDPDEBUG(("udpRecvFrom()\n"));

    if (!(udps[ud].flags & FUDP_OPEN)) return -1;

    rtn = udpRead(ud, buf, len);

    if (rtn < 0) return rtn;

    from->sin_family = AF_INET;

    from->sin_addr.s_addr = htonl(udps[ud].theirAddr.s_addr);

    from->sin_port = htons(udps[ud].theirPort);

    return rtn;

}



long udpSendTo(int ud, const void* buf, long len, const struct sockaddr_in* to)

{

    long rtn;



    UDPDEBUG(("udpSendTo()\n"));

    if (!(udps[ud].flags & FUDP_OPEN)) return -1;

    udps[ud].theirAddr.s_addr = htonl(to->sin_addr.s_addr);

    udps[ud].theirPort = htons(to->sin_port);

    rtn = udpWrite(ud, buf, len);

    return rtn;

}



static UDPCB* udpResolveIncomingUDPCB(u_long srcAddr, u_int16_t port)

{

    int i;

    for (i = 0; (i < MAXUDP); i++) {

        if ((udps[i].flags & FUDP_OPEN) && 

            (udps[i].ourPort == port) &&

           ((udps[i].acceptFromAddr.s_addr == 0) ||

            (udps[i].acceptFromAddr.s_addr == srcAddr)))

            return &udps[i];

    }

    return NULL;

}





////////////// CHECKSUM DEBUG STUFF  (can be removed later) ////////////////////////////////////////////////////

#if DEBUG_SUPPORT > 0			

u_int16 CalcChkSum(u_int16* addr, u_int16 count, u_int16 initial)

{

    register unsigned long sum;



    sum = initial;

    while (count > 1) {

        sum += (*addr++);

        count -= 2;

    }

    // Add left over byte if any

    if (count > 0) {

        sum += *(FAR u_char*)addr;

    }

    while (sum >> 16) {

        sum = (sum & 0xffff) + (sum >> 16);

    }

    return (u_int16)(~sum);

}



typedef struct {

	struct	in_addr ih_src;

	struct	in_addr ih_dst;

	u_char Zero;

	u_char Protocol;

	u_int16 Length;

} PseudoHdr;





u_int16 CalcPseudoHdr(FAR IPHdr* IPPtr, u_int16 Length)

{

    PseudoHdr   PsHdr;

    memcpy(&PsHdr.ih_src, &IPPtr->ip_src, sizeof(struct	in_addr));

    memcpy(&PsHdr.ih_dst, &IPPtr->ip_dst, sizeof(struct	in_addr));

    PsHdr.Zero = 0;

    PsHdr.Protocol = IPPtr->ip_p;

    PsHdr.Length = Length;

//    return ~CalcChkSum((u_int16*)&PsHdr, 12, 0);  // Return the checksum of the pseudo header

    return ~CalcChkSum((void *)&PsHdr, 12, 0);  // Return the checksum of the pseudo header

}



u_int16 ToInt(u_int16 Indata)

{

    u_int16 swapped = (LOBYTE(Indata) << 8) | (HIBYTE(Indata));

    return swapped;

}



void RxUdp(NBuf* pNBuf)

{

    u_int16 UDPLength;

    u_int16 UDPChkSum;

    u_int16 ChkSum;

    UDPHdr* pUDPHdr = (UDPHdr*)(nBUFTOPTR(pNBuf, char*) + sizeof(IPHdr));

    IPHdr* pIPHdr = (IPHdr*)(nBUFTOPTR(pNBuf, char*));

    

    UDPLength = ntohs(pUDPHdr->length);

    UDPChkSum = pUDPHdr->checksum;

    pUDPHdr->checksum = 0;



    // First we evaluate the checksum to see if it is OK.

    ChkSum = CalcPseudoHdr(pIPHdr, pUDPHdr->length);

//    ChkSum = CalcChkSum((FAR u_int16*)pUDPHdr, UDPLength, ChkSum);

    ChkSum = CalcChkSum((void *)pUDPHdr, UDPLength, ChkSum);

    if (ChkSum != UDPChkSum) {

        TRACE("RxUdp(%p) FAIL", pNBuf);

    } else {

        TRACE("RxUdp(%p) PASS", pNBuf);

    }

    TRACE("  UDPChkSum: %04X ChkSum:%04X\n", UDPChkSum, ChkSum);

    pUDPHdr->checksum = UDPChkSum;

}

#endif

////////////////////////////////////////////////////////////////////////////////



void udpInput(NBuf* inBuf, u_int ipHeadLen)

{

    IPHdr* ipHdr;               /* Ptr to IP header in output buffer. */

    UDPHdr* udpHdr;             /* Ptr to UDP header in output buffer. */

    UDPCB* udpCB;

    u_int16_t recv_chkSum,calc_chkSum;



    ipHdr = nBUFTOPTR(inBuf, IPHdr*);

    UDPDEBUG(("udpInput()\n"));

  //  DUMPCHAIN(inBuf);

    if (inBuf == NULL) {

        UDPDEBUG(("udpInput: Null input dropped\n"));

        return;

    }



    /*

     * Strip off the IP options.  The UDP checksum includes fields from the

     * IP header but without the options.

     */

    if (ipHeadLen > sizeof(IPHdr)) {

        inBuf = ipOptStrip(inBuf, ipHeadLen);

        ipHeadLen = sizeof(IPHdr);

    }



    /*

     * Get IP and UDP header together in first nBuf.

     */

    if (inBuf->len < sizeof(UDPHdr) + sizeof(IPHdr)) {

        if ((inBuf = nPullup(inBuf, sizeof(UDPHdr) + ipHeadLen)) == 0) {

            UDPDEBUG(("udpInput: Runt packet dropped\n"));

            return;

        }

    }

    ipHdr = nBUFTOPTR(inBuf, IPHdr*);



    /*

     * Note: We use ipHeadLen below just in case we kept an option with

     *  the IP header.

     */

    udpHdr = (UDPHdr*)((char*)ipHdr + ipHeadLen);

    UDPDEBUG(("src=%s port %ld dest=%s port=%ld\n", \

        ip_ntoa(/*htonl*/(ipHdr->ip_src.s_addr)),   \

        htons(udpHdr->srcPort),                     \

        ip_ntoa2(/*htonl*/(ipHdr->ip_dst.s_addr)),  \

        htons(udpHdr->dstPort)                      \

        ));

    UDPDEBUG(("length=%d\n",htons(udpHdr->length)));

//    UDPDEBUG(("checksum=%04X\n", htons(udpHdr->checksum)));

//    UDPDEBUG(("nbuf_chkSum=%04X\n", udpHdr->checksum));



    /* Save the received checksum for validating the packet */

//    recv_chkSum = htons(udpHdr->checksum);

    recv_chkSum = udpHdr->checksum;

    udpHdr->checksum = 0;



    /*

     * Prepare the header for the TCP checksum.  The TCP checksum is

     * computed on a pseudo IP header as well as the TCP header and

     * the data segment.  The pseudo IP header includes the length

     * (not including the length of the IP header), protocol, source

     * address and destination address fields.  We prepare this by

     * clearing the TTL field and loading the length in the IP checksum

     * field.

     */

    ipHdr->ip_ttl = 0;

//    ipHdr->ip_sum = htons(ipHdr->ip_len - sizeof(IPHdr));

//    ipHdr->ip_sum = htons(ipHdr->ip_len) - sizeof(IPHdr);

    ipHdr->ip_sum = udpHdr->length;



    /* Calculate the checksum on the packet */

//    calc_chkSum = inChkSum(inBuf, inBuf->chainLen - 8, 8);

//    calc_chkSum = inChkSum(inBuf, inBuf->len - 8, 8);

//    calc_chkSum = inChkSum(inBuf, ntohs(udpHdr->length) + 12, 8);

    calc_chkSum = inChkSum(inBuf, ntohs((unsigned short)(udpHdr->length)) + 12, 8);



//    UDPDEBUG(("inChkSum  =%04X\n", inChkSum(inBuf, ToInt(udpHdr->length), sizeof(IPHdr))));

//    UDPDEBUG(("CalcChkSum=%04X\n", CalcChkSum((u_int16*)udpHdr, ToInt(udpHdr->length), 0)));

//    UDPDEBUG((" HeaderSum =%04X\n", inChkSum(inBuf, 12, 8)));

//    UDPDEBUG(("~CalcPseudo=%04X\n", ~CalcPseudoHdr(ipHdr, udpHdr->length) & 0xFFFF));

//    UDPDEBUG(("udpHdr->length=%04X\n",udpHdr->length));

//    UDPDEBUG(("calc_chkSum=%04X\n",calc_chkSum));

//    UDPDEBUG("recv_chkSum=%04X\n",recv_chkSum);

//    UDPDEBUG(("inBuf->len=%04X\n",inBuf->len));



    if (recv_chkSum != calc_chkSum) {

        // checksum failed, junk the packet - as UDP isn't a connection oriented protocol,

        //  we don't need to inform the sender, we can simply throw the packet away

        UDPDEBUG(("chkSum FAILED - recv_chkSum=%04X, calc_chkSum=%04X\n", recv_chkSum, calc_chkSum));



#if DEBUG_SUPPORT > 0			

        udpHdr->checksum = recv_chkSum;

        RxUdp(inBuf);

#endif



        nFreeChain(inBuf);

        return;

    }





	// If we have received an empty UDP packet (no payload) discard it

/*

	if (inBuf->chainLen <= (sizeof(IPHdr) + sizeof(UDPHdr))) {

		// Free "memory"

		nFreeChain(inBuf);

		return;

	}

*/

    /* Try to find an open udp port to receive the incoming packet */

    udpCB = udpResolveIncomingUDPCB(ipHdr->ip_src.s_addr, udpHdr->dstPort);

//    udpCB = udpResolveIncomingUDPCB(ntohl(ipHdr->ip_src.s_addr), ntohs(udpHdr->dstPort));

    if (udpCB == NULL) {



		// Only reflect the packet if it is a unicast!!!

		if (ntohl(ipHdr->ip_dst.s_addr) == localHost) {

            /* if port not open, or port is not listening for connections from that address reflect the packet */

            UDPDEBUG(("udpInput: port %ld not open, sending icmp_error\n", udpHdr->dstPort));

            icmp_error(inBuf, ICMP_UNREACH,ICMP_UNREACH_PORT, 0);

		}

        return;

    }



    /* Add NBuf chain to the incoming data queue for the port */

//    inBuf->data = (unsigned char*)(udpHdr + 1);

    if (udpCB->tail) {

        udpCB->tail->next = alloc_udp_q();

        if (udpCB->tail->next == NULL) {

            goto UDP_ALLOC_FAILED;

        }

        udpCB->tail = udpCB->tail->next;

    } else {

        udpCB->head = udpCB->tail = alloc_udp_q();

        if (udpCB->tail == NULL) {

            goto UDP_ALLOC_FAILED;

        }

    }

    udpCB->tail->next = NULL;

    udpCB->tail->srcAddr = ipHdr->ip_src;

    udpCB->tail->srcPort = udpHdr->srcPort;

    udpCB->tail->nBuf = inBuf;



    // **** Remove IP and UDP header stuff

	nTrim(NULL, &udpCB->tail->nBuf, sizeof(IPHdr) + sizeof(UDPHdr));



#if ONETASK_SUPPORT > 0

    if (udpCB->receiveEvent) 

          // Data received so notify user, 

          udpCB->receiveEvent((int)(udpCB - &udps[0]), udpCB->tail->nBuf->chainLen); 

#else

    /* finally, post a semaphore to wake up anyone waiting for data on this UDP port */

    OSSemPost(udpCB->sem);

#endif

 

    return;



UDP_ALLOC_FAILED:

    UDPDEBUG(("udpInput: port %ld, out of udp queue buffers\n", udpHdr->dstPort));

    return;

}





//#pragma warning (pop)

////////////////////////////////////////////////////////////////////////////////