icmp.c

完整的Bell实验室的嵌入式文件系统TFS

    
            if (icmptime->seq < 20) {
                ICMPTimeStampTbl[icmptime->seq] = xmit;
                SendICMPRequest(ICMP_TIMEREQUEST,(uchar *)&ipp->ip_src,
                    (uchar *)&ehdr->ether_shost,0,icmptime->seq+1);
            } else {
                
                printf("TS00: %d\n",ICMPTimeStampTbl[0]);
                for(i=1;i<20;i++) {
                    printf("TS%02d: %d (dlta=%d)\n",i,
                        ICMPTimeStampTbl[i],
                        ICMPTimeStampTbl[i]-ICMPTimeStampTbl[i-1]);
                }
                ICMPTimeStamp = xmit;
            }
        }
        else {
            ICMPTimeStamp = xmit;
        }
    }
    else {
        if (EtherVerbose & SHOW_INCOMING) {
            printf("    ICMPTYPE=%d\n",icmpp->type);
        }
        return(-1);
    }
    return(0);
}

/* SendEchoResp():
 *  Called in response to an ICMP ECHO REQUEST.  Typically used as
 *  a response to a PING.
 */
int
SendEchoResp(struct ether_header *re)
{
    int i, icmp_len, datalen, oddlen;
    ulong   t;
    ushort  *sp, ip_len;
    struct ether_header *te;
    struct ip *ti, *ri;
    struct icmp_echo_hdr *ticmp, *ricmp;

    ri = (struct ip *) (re + 1);
    datalen = ecs(ri->ip_len) - ((ri->ip_vhl & 0x0f) * 4);

    te = EtherCopy(re);
    ti = (struct ip *) (te + 1);
    ti->ip_vhl = ri->ip_vhl;
    ti->ip_tos = ri->ip_tos;
    ti->ip_len = ri->ip_len;
    ti->ip_id = ipId();
    ti->ip_off = 0;
    ti->ip_ttl = UDP_TTL;
    ti->ip_p = IP_ICMP;
    memcpy((char *)&(ti->ip_src.s_addr),(char *)&(ri->ip_dst.s_addr),
        sizeof(struct in_addr));
    memcpy((char *)&(ti->ip_dst.s_addr),(char *)&(ri->ip_src.s_addr),
        sizeof(struct in_addr));

    ticmp = (struct icmp_echo_hdr *) (ti + 1);
    ricmp = (struct icmp_echo_hdr *) (ri + 1);
    ticmp->type = ICMP_ECHOREPLY;
    ticmp->code = 0;
    ticmp->cksum = 0;
    ticmp->id = ricmp->id;
    ticmp->seq = ricmp->seq;
    memcpy((char *)(ticmp+1),(char *)(ricmp+1),datalen-8);

    ip_len = ecs(ti->ip_len);
    if (ip_len & 1) {       /* If size is odd, temporarily bump up ip_len by */
        oddlen = 1;         /* one and add append a null byte for csum. */ 
        ((char *)ti)[ip_len] = 0;
        ip_len++;
    }
    else
        oddlen = 0;
    icmp_len = (ip_len - sizeof(struct ip))/2;

    ipChksum(ti);   /* compute checksum of ip hdr: (3rd Edition Comer pg 100) */

    /* compute checksum of icmp message: (3rd Edition Comer pg 126) */
    ticmp->cksum = 0;
    sp = (ushort *) ticmp;
    for (i=0,t=0;i<icmp_len;i++,sp++)
        t += ecs(*sp);
    t = (t & 0xffff) + (t >> 16);
    ticmp->cksum = ~t;

    self_ecs(ticmp->cksum);

    if (oddlen)
        ip_len--;

    sendBuffer(sizeof(struct ether_header) + ip_len);
    if (EtherVerbose & SHOW_OUTGOING)
        printf("  Sent Echo Response\n");
    return(0);
}

/* SendICMPRequest():
 *  Currently supports ICMP_TIMEREQUEST and ICMP_ECHOREQUEST.
 */
int
SendICMPRequest(uchar type,uchar *binip,uchar *binenet,
                ulong origtime,ushort seq)
{
    ulong   csum;
    ushort  *sp;
    uchar   *data;
    int     i, icmp_len;
    struct  ip *ti;
    struct  ether_header *te;
    struct  icmp_time_hdr *ticmp;

    /* Retrieve an ethernet buffer from the driver and populate the */
    /* ethernet level of packet: */
    te = (struct ether_header *) getXmitBuffer();
    memcpy((char *)&te->ether_shost,BinEnetAddr,6);
    memcpy((char *)&te->ether_dhost,binenet,6);
    te->ether_type = ecs(ETHERTYPE_IP);

    /* Move to the IP portion of the packet and populate it appropriately: */
    ti = (struct ip *) (te + 1);
    ti->ip_vhl = IP_HDR_VER_LEN;
    ti->ip_tos = 0;
    if (type == ICMP_TIMEREQUEST)
        ti->ip_len = sizeof(struct ip) + sizeof(struct icmp_time_hdr);
    else 
        ti->ip_len = sizeof(struct ip) + sizeof(struct icmp_echo_hdr) +
            ICMP_ECHO_DATASIZE;

    ti->ip_id = ipId();
    ti->ip_off = 0;
    ti->ip_ttl = UDP_TTL;
    ti->ip_p = IP_ICMP;
    memcpy((char *)&ti->ip_src.s_addr,BinIpAddr,4);
    memcpy((char *)&ti->ip_dst.s_addr,binip,4);

    icmp_len = (ti->ip_len - sizeof(struct ip))/2;
    self_ecs(ti->ip_len);
    self_ecs(ti->ip_off);
    self_ecs(ti->ip_id);

    ipChksum(ti);       /* Compute csum of ip hdr */

    /* Move to the ICMP portion of the packet and populate it appropriately: */
    ticmp = (struct icmp_time_hdr *)(ti+1);
    ticmp->type = type;
    ticmp->code = 0;
    ticmp->seq = ecs(seq);
    if (type == ICMP_TIMEREQUEST) {
        ticmp->id = ICMP_TIME_ID;
        memcpy((char *)&ticmp->orig,(char *)&origtime,4);
        memset((char *)&ticmp->recv,0,4);
        memset((char *)&ticmp->xmit,0,4);
    }
    else {
        ticmp->id = ICMP_ECHO_ID;

        /* Add ICMP_ECHO_DATASIZE bytes of data... */
        data = (uchar *)((struct icmp_echo_hdr *)ticmp+1);
        for(i=0;i<ICMP_ECHO_DATASIZE;i++)
            data[i] = 'a'+i;

    }

    /* compute checksum of icmp message: (3rd Edition Comer pg 126) */
    csum = 0;
    ticmp->cksum = 0;
    sp = (ushort *) ticmp;
    for (i=0;i<icmp_len;i++,sp++)
        csum += ecs(*sp);
    csum = (csum & 0xffff) + (csum >> 16);
    ticmp->cksum = ~csum;

    self_ecs(ticmp->cksum);
    self_ecs(ticmp->id);
    self_ecs(ticmp->seq);

    if (type == ICMP_TIMEREQUEST) {
        self_ecl(ticmp->orig);
        self_ecl(ticmp->recv);
        self_ecl(ticmp->xmit);
        sendBuffer(ICMP_TIMERQSTSIZE);
    }
    else
        sendBuffer(ICMP_ECHORQSTSIZE+ICMP_ECHO_DATASIZE);
    return(0);
}

/* Send an ICMP Unreachable message. All ICMP Unreachable messages
 * include the IP header and 64 bits of the datagram that could not be
 * delivered.
 *
 * 're' is the pointer to the packet that was received in response to which
 * the unreachable message is being sent. 'icmp_code' is the code of the
 * ICMP message.
 */
int
SendICMPUnreachable(struct ether_header *re,uchar icmp_code)
{
    int i, len;
    ushort *sp, r_iphdr_len, ip_len;
    ulong t;
    struct ether_header *te;
    struct ip *ti, *ri;
    struct icmp_unreachable_hdr *ticmp;

    ri = (struct ip *) (re + 1);

    /* Length of the received IP hdr */
    r_iphdr_len = ((ri->ip_vhl & 0x0f)<<2);

    te = EtherCopy(re);

    ti = (struct ip *) (te + 1);
    ti->ip_vhl = IP_HDR_VER_LEN;
    ti->ip_tos = 0;
    /* Length of the outgoing IP packet = IP header + ICMP header + 
     * IP header and 8 bytes of unreachable datagram 
     */
    ip_len = (IP_HDR_LEN<<2)+sizeof(struct icmp_unreachable_hdr)+r_iphdr_len+8;
    ti->ip_len = ecs(ip_len);
    ti->ip_id = ipId();
    ti->ip_off = 0;
    ti->ip_ttl = UDP_TTL;
    ti->ip_p = IP_ICMP;

    /* Note that we do memcpy instead of struct copy because the ip
     * header is not not word-aligned. As a result, the source and dest
     * addresses are not word aligned either. The compiler generates word
     * copy instructions for struct copy and this causes address alignement
     * exceptions.
     */
    memcpy((char *)&(ti->ip_src.s_addr),(char *)&(ri->ip_dst.s_addr),
        sizeof(struct in_addr));
    memcpy((char *)&(ti->ip_dst.s_addr),(char *)&(ri->ip_src.s_addr),
        sizeof(struct in_addr));

    ticmp = (struct icmp_unreachable_hdr *) (ti + 1);
    ticmp->type = ICMP_DESTUNREACHABLE;
    ticmp->code = icmp_code;
    ticmp->cksum = 0;
    ticmp->unused1=0;
    ticmp->unused2=0;

    /* Copy the IP header and 8 bytes of the received datagram */
    memcpy((char *)(ticmp+1),(char *)ri,r_iphdr_len+8);

    ipChksum(ti);   /* compute checksum of ip hdr */

    /* compute checksum of icmp message: (see Comer pg 91) */
    len = (ip_len - sizeof(struct ip))/2;
    ticmp->cksum = 0;
    sp = (ushort *) ticmp;
    for (i=0,t=0;i<len;i++,sp++)
        t += ecs(*sp);
    t = (t & 0xffff) + (t >> 16);
    ticmp->cksum = ~t;
    self_ecs(ticmp->cksum);

    sendBuffer(sizeof(struct ether_header) + ip_len);

    if (EtherVerbose & SHOW_OUTGOING)
        printf("  Sent ICMP Dest Unreachable message. Code='%s'.\n",
            IcmpDestUnreachableCode[icmp_code]);
    return(0);
}

#endif