netudp.c

来自「一个tcp/ip协议栈,带有PPP、IP、TCP、UDP等协议」· C语言 代码 · 共 799 行 · 第 1/2 页

			ipHdr->ip_sum = inChkSum(pNBuf, sizeof(IPHdr), 0);
		} else {
			// Nothing copied ?!? This can only happen if nBuf size is eq. to sizeof(IPHdr) + sizeof(UDPHdr)
			// AND there are no more buffers free
			// This situation isn't actually handled! But we will free nBuf anyway
			nFreeChain(pNBuf);
		}

        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)                      \
            ));

        // Pass the datagram to IP and we're done. 
				if (pNBuf) 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);
    if (udps[ud].ourPort == 0)
        udps[ud].ourPort = htons(randPort++);
    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 UDP checksum.
     */
    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(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)
////////////////////////////////////////////////////////////////////////////////
#if 0

int udpWrite(u_int ud, const void* buf, long len)
{
    NBuf* pNBuf;
    NBuf* pNBufTail;
    IPHdr* ipHdr;
    UDPHdr* udpHdr;
    long rtn = len;
    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