netudp.c

uC/IP Release Notes These release notes for uC/IP are divided into the following sections

        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;

        outHead->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;
        outTail->data = (unsigned char*)(udpHdr+1);
        outTail->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 - outTail->len, packetLen);
            memcpy(outTail->data, d, segLen);
            outTail->len += segLen;
            packetLen -= segLen;
            if (packetLen) {
                do {
                    nGET(outTail->nextBuf);
                } while (!outTail->nextBuf);
                outTail = outTail->nextBuf;
                outTail->nextBuf = NULL;
                outTail->len = 0;
            }
        }
        outHead->data = outHead->body;
        udpHdr->checksum = inChkSum(outHead, outHead->chainLen - 8, 8);
        ipHdr->ip_ttl = UDPTTL;
      //  DUMPCHAIN(outHead);
        /* Pass the datagram to IP and we're done. */
        ipRawOut(outHead);
    }
    return rtn;
}

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;
}


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

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 UCHAR*)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 ToInt(u_int16 Indata)
{
    u_int16 swapped = (LOBYTE(Indata) << 8) | (HIBYTE(Indata));
    return swapped;
}

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
}

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 = ToInt(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);
    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;
}

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

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 recieved 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 = ipHdr->ip_len;

    /* 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, ToInt(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(("inBuf->len=%04X\n",inBuf->len));

    if (recv_chkSum != calc_chkSum) {
        // checksum failed, junk the packet - as UDP isn't a connected 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));

        udpHdr->checksum = recv_chkSum;
        RxUdp(inBuf);

        nFreeChain(inBuf);
        return;
    }


    /* Try to find an open udp port to receive the incoming packet */
    udpCB = udpResolveIncomingUDPCB(ipHdr->ip_src.s_addr, udpHdr->dstPort);
    if (udpCB == NULL) {
        /* 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;

    /* finally, post a semaphore to wake up anyone waiting for data on this UDP port */
    OSSemPost(udpCB->sem);
    return;

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


#pragma warning (pop)
////////////////////////////////////////////////////////////////////////////////