net_ipv4.c

em86xx 完整启动程序,支持网络下载与串通下载

    unsigned char *cp;

    for (i = 0; i < MAX_ARPTABLE; ++i) {
        if ((arp == NULL || arp == g_arptable + i) && arp_valid(g_arptable + i)) {
            cp = g_arptable[i].node;
            uart_printf("%-20s %02X:%02X:%02X:%02X:%02X:%02X\n", 
                ipaddr_to_str(g_arptable[i].ipaddr), 
                cp[0], cp[1], cp[2], cp[3], cp[4], cp[5]);
        }
    }
}

// 
// ARP protocol
//
// Refer to RFC 826 "An Ethernet Address Resolution Protocol"
// 

// send ARP packet : request or reply
// opcode : ARPOP_REQUEST or ARPOP_REPLY
// ipaddr : address to reply
// node : hardware address to reply
int arp_transmit(int opcode, in_addr_t ipaddr, unsigned char *node, int async)
{
    arprequest_t arpreq;

    // fill ARP header 
    arpreq.ar_hrd = htons(ARPHRD_ETHER);
    arpreq.ar_pro = htons(ETH_P_IP);
    arpreq.ar_hln = ETH_ALEN;
    arpreq.ar_pln = 4;
    arpreq.ar_op = htons(opcode);

    // fill ARP body
    memcpy(arpreq.ar_sha, g_arptable[ARP_CLIENT].node, ETH_ALEN);
    arpreq.ar_sip = htonl(g_arptable[ARP_CLIENT].ipaddr);
    
    if (node)
        memcpy(arpreq.ar_tha, node, ETH_ALEN);
    else
        memset(arpreq.ar_tha, 0, ETH_ALEN);
    arpreq.ar_tip = htonl(ipaddr);

    // send ARP request (broadcast) or reply packet
    if (opcode == ARPOP_REQUEST)
        return eth_transmit(g_eth_broadcast, ETH_P_ARP, sizeof(arpreq), &arpreq, async);
    else
        return eth_transmit(node, ETH_P_ARP, sizeof(arpreq), &arpreq, async);
}

arptable_t *arp_dorequest(unsigned long ipaddr, int async)
{
    int retry;
    arptable_t *arp = arp_lookup(ipaddr);
    struct sk_buff *skb;

    // look for available ARP slot
    if (arp == NULL) 
        if ((arp = arp_alloc()) == NULL)
            return NULL;

    // send ARP request packet and wait for reply
    for (retry = 1; retry <= MAX_ARP_RETRIES; retry++) {
        arp_transmit(ARPOP_REQUEST, ipaddr, NULL, async);
        skb = eth_receive(WAIT_ARP_REPLY, TIMEOUT);

        if (skb) {
            if (skb->arp->ar_op == ARPOP_REPLY && skb->arp->ar_sip == ipaddr) {
                arp_setaddr(arp, ipaddr, INADDR_ANY, skb->arp->ar_sha);
                skb_free(skb);
                break;
            }
        } else if (retry < MAX_ARP_RETRIES)
            uart_puts("ARP request timeout. Retry.\n");
        else {
            uart_puts("ARP request failed.\n");
            return NULL;
        }
    }
    
    return arp;
}

//
// send a IP packet
//
// Refer to RFC 791 "Internet Protocol" about IP header
//

int ip_transmit(in_addr_t destip, int protocol, int len, const void *buf, int async)
{
    struct iphdr *ip;

    len += sizeof(struct iphdr);

    // fill in the IP header
    ip = (struct iphdr *) buf;
    ip->version = 0x04;
    ip->ihl = 0x05;
    ip->tos = 0;
    ip->tot_len = htons(len);
    ip->id = 0;
    ip->frag_off = 0;
    ip->ttl = 60;
    ip->protocol = (unsigned char) protocol;
    ip->check = 0;
    ip->saddr = htonl(g_arptable[ARP_CLIENT].ipaddr);
    ip->daddr = htonl(destip);
    ip->check = ipv4_calc_checksum(ip, sizeof(struct iphdr));

    // transmit the packet
    if (destip == INADDR_BROADCAST) {
        eth_transmit(g_eth_broadcast, ETH_P_IP, len, buf, async);
    } else {
        arptable_t *arp;
        in_addr_t netmask = g_arptable[ARP_CLIENT].netmask;

        if (ipv4_ipaddr_valid(g_arptable[ARP_CLIENT].ipaddr)) {
            if ((destip & netmask) != (g_arptable[ARP_CLIENT].ipaddr & netmask)) {
                // not in the same class
                // send packet over gateway
                if (!ipv4_ipaddr_valid(g_arptable[ARP_GATEWAY].ipaddr)) {
                    // now the gateway is not specified
                    return 1;
                }
                destip = g_arptable[ARP_GATEWAY].ipaddr;
            }
        }

        if ((arp = arp_lookup(destip)) == NULL || !arp_valid(arp))
            if ((arp = arp_dorequest(destip, async)) == NULL) 
                return 1;

        eth_transmit(arp->node, ETH_P_IP, len, buf, async);
    }

    return 0;
}

unsigned int ipv4_calc_sum(const void *ip, int len)
{
    uint16_t *_ip = (uint16_t *) ip;
    unsigned int sum = 0;
    int odd = len & 1;
    
    for (len >>= 1; len > 0; --len) 
        sum += *(_ip++);

    // if the length is odd, assume that the next byte is padded by 0
    if (odd) 
        sum += (*(uint8_t *) _ip);

    return sum;
}

unsigned short ipv4_calc_checksum(const void *ip, int len)
{
    unsigned int sum = ipv4_calc_sum(ip, len);
    
    while (sum >> 16)
        sum = (sum & 0xffff) + (sum >> 16);

    return ((~sum) & 0x0000FFFF);
}

//
// send a ping ICMP packet
// 

int icmp_ping(in_addr_t destip, int count)
{
    static unsigned short s_id = 0, s_seq = 0;
    static unsigned char s_pingmsg[] = "FLYDUCK, SIGMADESIGNS";

    int i, nreply;
    icmpip_t packet;
    struct sk_buff *skb;

    for (i = 0, nreply = 0; i < count; ++i) {
        packet.icmp.type = ICMP_ECHO;
        packet.icmp.code = 0;
        packet.icmp.checksum = 0;
        packet.icmp.un.echo.id = htons(++s_id);
        packet.icmp.un.echo.sequence = htons(++s_seq);

        memset(packet.icmp.data, ' ', MAX_ICMP_DATA);
        memcpy(packet.icmp.data, s_pingmsg, sizeof s_pingmsg);
    
        packet.icmp.checksum = ipv4_calc_checksum(&packet.icmp, sizeof packet.icmp);

        if (ip_transmit(destip, IPPROTO_ICMP, sizeof packet.icmp, &packet, 0) != 0)
            break;

        if ((skb = eth_receive(WAIT_ICMP_REPLY, TIMEOUT)) != NULL) {
            uart_printf("Reply from %s : icmp_seq = %d\n", 
                ipaddr_to_str(skb->ip->saddr), skb->icmp->un.echo.sequence);
            ++nreply;
        } else {
            uart_printf("Unreachable to %s : icmp_seq = %d\n",
                ipaddr_to_str(destip), s_seq);
        }

        skb_free(skb);
    }

    return nreply;
}

//
// send a UDP datagram
//
// destip : destination IP address
// srcsock : source port number
// destsock : destination socket number
// len : size of UDP data (exept IP, UDP header)
// buf : packet buffer
// 
// buffer is consisted of 
//   struct iphdr ip;
//   struct udphdr udp;
//   protocol specific body;
//
// return 0 : success
// return non-zero : error
//
// Refer to RFC 768 "User Datagram Protocol" about header and checksum
//

int udp_transmit(in_addr_t destip, unsigned int srcsock, unsigned int destsock, int len, const void *buf, int async)
{
    struct udphdr *udp;

    len += sizeof(struct udphdr);

    // fill in the UDP header
    udp = (struct udphdr *)((char *)buf + sizeof(struct iphdr));
    udp->src = htons(srcsock);
    udp->dest = htons(destsock);
    udp->len = htons(len);
    udp->checksum = 0;

    udp->checksum = ipv4_calc_tcpudp_checksum(udp, len, g_arptable[ARP_CLIENT].ipaddr, destip);
    
    return ip_transmit(destip, IPPROTO_UDP, len, buf, async);
}

unsigned short ipv4_calc_tcpudp_checksum(const void *data, int len, in_addr_t src, in_addr_t dest)
{
    unsigned int sum;
    tcpudp_pseudohdr_t phdr;

    phdr.src = htonl(src);
    phdr.dest = htonl(dest);
    phdr.zero = 0;
    phdr.protocol = IPPROTO_UDP;
    phdr.length = htons(len);

    sum = ipv4_calc_sum(data, len);
    sum += ipv4_calc_sum(&phdr, sizeof phdr);

    while (sum >> 16)
        sum = (sum & 0xffff) + (sum >> 16);
        
    return ((~sum) & 0x0000FFFF);
}

struct sk_buff *udp_receive(in_addr_t fromip, unsigned int srcsock, unsigned int destsock, int timeout)
{
    struct sk_buff *skb;
    unsigned int startticks = timer_getticks();
   
    while ((skb = eth_receive(WAIT_UDP, timeout)) != NULL) {
        int match = 1;

        // uart_printf("from = %08x, %08x, src = %04x %04x, des = %04x %04x\n",
        //   fromip, skb->ip->saddr, srcsock, skb->udp->src, destsock, skb->udp->dest);
        
        if (fromip) {
            if (skb->ip->saddr != fromip)
                match = 0;
        }
        if (srcsock) {
            if (skb->udp->src != srcsock)
                match = 0;
        }
        if (destsock) {
            if (skb->udp->dest != destsock)
                match = 0;
        }

        if (match) {
            return skb;
        }

        skb_free(skb);

        if (timer_timeout(startticks, timeout))
            break;
    }

    return NULL;
}

//
// Miscellaneous
//

enum {
    DUMP_PACKET_DEFAULT,
    DUMP_PACKET_BOOTP,
    DUMP_PACKET_TFTP,
};

void dump_memory(void *memptr, unsigned int addr, int num, int unit);

static char *ipv4_ethproto_str(int proto)
{
    switch (proto) {
    case ETH_P_LOOP : 
        return "LOOP";
    case ETH_P_IP : 
        return "IP";
    case ETH_P_ARP : 
        return "ARP";
    case ETH_P_RARP :
        return "RARP";
    default : 
        return "Unknown";
    }
}

static char *ipv4_ipproto_str(int proto)
{
    switch (proto) {
    case IPPROTO_IP :
        return "IP";
    case IPPROTO_ICMP :
        return "ICMP";
    case IPPROTO_IGMP :
        return "IGMP";
    case IPPROTO_TCP :
        return "TCP";
    case IPPROTO_UDP :
        return "UDP";
    case IPPROTO_RAW :
        return "RAW";
    default :
        return "Other";
    }
}

static char *ipv4_ipport_str(int port)
{
    static struct {
        int port;
        char *name;
    } s_ipport_str_list[] = {
        { IPPORT_ECHO, "echo" },
        { IPPORT_FTP, "ftp" },
        { IPPORT_TELNET, "telnet" },
        { IPPORT_SMTP, "smtp" },
        { IPPORT_DOMAINSERVER, "nameserver" },
        { IPPORT_BOOTP_SERVER, "bootp server" },
        { IPPORT_BOOTP_CLIENT, "bootp client" },
        { IPPORT_TFTP, "tftp" },
        { IPPORT_SUNRPC, "RPC" },
        { 0, "Other" },
    };

    int i;

    for (i = 0; s_ipport_str_list[i].port != 0 && s_ipport_str_list[i].port != port; ++i)
        ;

    return s_ipport_str_list[i].name;
}

void ipv4_dump_packet(struct sk_buff *skb, int type, int need_parsing)
{
    if (need_parsing)
        ipv4_parsepacket(skb);

    uart_printf("Ethernet : 0x%x (%d)\n", skb->len, skb->len);
    uart_printf("  Target   : %02X:%02X:%02X:%02X:%02X:%02X\n",
        skb->eth->h_dest[0], skb->eth->h_dest[1], 
        skb->eth->h_dest[2], skb->eth->h_dest[3], 
        skb->eth->h_dest[4], skb->eth->h_dest[5]);
    uart_printf("  Source   : %02X:%02X:%02X:%02X:%02X:%02X\n",
        skb->eth->h_source[0], skb->eth->h_source[1], 
        skb->eth->h_source[2], skb->eth->h_source[3], 
        skb->eth->h_source[4], skb->eth->h_source[5]);
    uart_printf("  Protocol : %04x (%s)\n", 
        skb->eth->h_proto,
        ipv4_ethproto_str(skb->eth->h_proto));
    uart_printf("  data len : 0x%x (%d)\n", skb->ethdata_len, skb->ethdata_len);

    switch (skb->eth->h_proto) {
    case ETH_P_IP : 
        uart_printf("IP :\n");
        uart_printf("  Version + IHL : %02x\n", skb->ethdata[0]);
        uart_printf("  TOS           : %02x\n", skb->ip->tos);
        uart_printf("  Length        : %04x\n", skb->ip->tot_len);
        uart_printf("  ID            : %04x\n", skb->ip->id);
        uart_printf("  Frag          : %04x\n", skb->ip->frag_off);
        uart_printf("  TTL           : %02x\n", skb->ip->ttl);
        uart_printf("  Protocol      : %02x (%s)\n", skb->ip->protocol, ipv4_ipproto_str(skb->ip->protocol));
        uart_printf("  Checksum      : %04x\n", skb->ip->check);
        uart_printf("  Source        : %s (%08x)\n", ipaddr_to_str(skb->ip->saddr), skb->ip->saddr);
        uart_printf("  Target        : %s (%08x)\n", ipaddr_to_str(skb->ip->daddr), skb->ip->daddr);
        uart_printf("  data len      : 0x%x (%d)\n", skb->ipdata_len, skb->ipdata_len);

        switch (skb->ip->protocol) {
        case IPPROTO_IP : 
            uart_printf("Raw IP packet\n");
            break;
        case IPPROTO_ICMP :
            uart_printf("ICMP :\n");
            uart_printf("  Type : %02x\n", skb->icmp->type);
            uart_printf("  Code : %02x\n", skb->icmp->code);
            uart_printf("  Checksum : %04x\n", skb->icmp->checksum);
            uart_printf("ICMP header and data :\n");
            dump_memory(skb->ipdata, 0, skb->ipdata_len, 1);
            break;
        case IPPROTO_UDP :
            uart_printf("UDP :\n");
            uart_printf("  Source   : %04x (%d) (%s)\n", skb->udp->src, skb->udp->src, ipv4_ipport_str(skb->udp->src));
            uart_printf("  Destin   : %04x (%d) (%s)\n", skb->udp->dest, skb->udp->dest, ipv4_ipport_str(skb->udp->dest));
            uart_printf("  Length   : %04x\n", skb->udp->len);
            uart_printf("  Checksum : %04x\n", skb->udp->checksum);
            uart_printf("  data len : %x (%d)\n", skb->udpdata_len, skb->udpdata_len);

            switch (type) {
            case DUMP_PACKET_DEFAULT :
                uart_printf("UDP data :\n");
                dump_memory(skb->udpdata, 0, skb->udpdata_len, 1);
                break;
            case DUMP_PACKET_BOOTP :
                bootp_dump_packet(skb, need_parsing);
                break;
            case DUMP_PACKET_TFTP :
                tftp_dump_packet(skb, need_parsing);
                break;
            }
            break;
        }   
        break;

    case ETH_P_ARP :
        uart_printf("ARP :\n");
        uart_printf("  HWADDR type   : %04x\n", skb->arp->ar_hrd);
        uart_printf("  Protocol type : %04x\n", skb->arp->ar_pro);
        uart_printf("  HWADDR len    : %02x\n", skb->arp->ar_hln);
        uart_printf("  Address len   : %02x\n", skb->arp->ar_pln);
        uart_printf("  Opcode        : %04x\n", skb->arp->ar_op);
        uart_printf("  Sender HWADDR : %02X:%02X:%02X:%02X:%02X:%02X\n",
            skb->arp->ar_sha[0], skb->arp->ar_sha[1], 
            skb->arp->ar_sha[2], skb->arp->ar_sha[3], 
            skb->arp->ar_sha[4], skb->arp->ar_sha[5]);
        uart_printf("  Sender IPADDR : %s (%08x)\n", ipaddr_to_str(skb->arp->ar_sip), skb->arp->ar_sip);
        uart_printf("  Target HWADDR : %02X:%02X:%02X:%02X:%02X:%02X\n",
            skb->arp->ar_tha[0], skb->arp->ar_tha[1], 
            skb->arp->ar_tha[2], skb->arp->ar_tha[3], 
            skb->arp->ar_tha[4], skb->arp->ar_tha[5]);
        uart_printf("  Target IPADDR : %s (%08x)\n", ipaddr_to_str(skb->arp->ar_tip), skb->arp->ar_tip);
        break;

    case ETH_P_RARP :
        break;
    }
}