src/bsp/lk/lib/minip/minip.c

/*
 * Copyright (c) 2014 Chris Anderson
 * Copyright (c) 2014 Brian Swetland
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files
 * (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge,
 * publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include "minip-internal.h"

#include <err.h>
#include <stdio.h>
#include <debug.h>
#include <endian.h>
#include <errno.h>
#include <iovec.h>
#include <stdlib.h>
#include <string.h>
#include <trace.h>
#include <malloc.h>
#include <list.h>
#include <kernel/thread.h>

static struct list_node arp_list = LIST_INITIAL_VALUE(arp_list);

// TODO
// 1. Tear endian code out into something that flips words before/after tx/rx calls

#define LOCAL_TRACE 0
static uint32_t minip_ip      = IPV4_NONE;
static uint32_t minip_netmask = IPV4_NONE;
static uint32_t minip_broadcast = IPV4_BCAST;
static uint32_t minip_gateway = IPV4_NONE;

static const uint8_t broadcast_mac[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
static uint8_t minip_mac[6] = {0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC};

static char minip_hostname[32] = "";

static void dump_mac_address(const uint8_t *mac);
static void dump_ipv4_addr(uint32_t addr);

void minip_set_hostname(const char *name) {
    strlcpy(minip_hostname, name, sizeof(minip_hostname));
}

const char *minip_get_hostname(void) {
   return minip_hostname;
}

static void compute_broadcast_address(void)
{
    minip_broadcast = (minip_ip & minip_netmask) | (IPV4_BCAST & ~minip_netmask);
}

void minip_get_macaddr(uint8_t *addr) {
    mac_addr_copy(addr, minip_mac);
}

void minip_set_macaddr(const uint8_t *addr) {
    mac_addr_copy(minip_mac, addr);
}

uint32_t minip_get_ipaddr(void) {
    return minip_ip;
}

void minip_set_ipaddr(const uint32_t addr) {
    minip_ip = addr;
    compute_broadcast_address();
}

void gen_random_mac_address(uint8_t *mac_addr)
{
    for (size_t i = 0; i < 6; i++) {
        mac_addr[i] = rand() & 0xff;
    }
    /* unicast and locally administered */
    mac_addr[0] &= ~(1<<0);
    mac_addr[0] |= (1<<1);
}

/* This function is called by minip to send packets */
tx_func_t minip_tx_handler;
void *minip_tx_arg;

void minip_init(tx_func_t tx_handler, void *tx_arg,
    uint32_t ip, uint32_t mask, uint32_t gateway)
{
    minip_tx_handler = tx_handler;
    minip_tx_arg = tx_arg;

    minip_ip = ip;
    minip_netmask = mask;
    minip_gateway = gateway;
    compute_broadcast_address();

    arp_cache_init();
    net_timer_init();
}

uint16_t ipv4_payload_len(struct ipv4_hdr *pkt)
{
    return (pkt->len - ((pkt->ver_ihl >> 4) * 5));
}

void minip_build_mac_hdr(struct eth_hdr *pkt, const uint8_t *dst, uint16_t type)
{
    mac_addr_copy(pkt->dst_mac, dst);
    mac_addr_copy(pkt->src_mac, minip_mac);
    pkt->type = htons(type);
}

void minip_build_ipv4_hdr(struct ipv4_hdr *ipv4, uint32_t dst, uint8_t proto, uint16_t len)
{
    ipv4->ver_ihl       = 0x45;
    ipv4->dscp_ecn      = 0;
    ipv4->len           = htons(20 + len); // 5 * 4 from ihl, plus payload length
    ipv4->id            = 0;
    ipv4->flags_frags   = 0x40; // no offset, no fragments
    ipv4->ttl           = 64;
    ipv4->proto         = proto;
    ipv4->dst_addr      = dst;
    ipv4->src_addr      = minip_ip;

    /* This may be unnecessary if the controller supports checksum offloading */
    ipv4->chksum = 0;
    ipv4->chksum = rfc1701_chksum((uint8_t *) ipv4, sizeof(struct ipv4_hdr));
}

int send_arp_request(uint32_t addr)
{
    pktbuf_t *p;
    struct eth_hdr *eth;
    struct arp_pkt *arp;

    if ((p = pktbuf_alloc()) == NULL) {
        return -1;
    }

    eth = pktbuf_prepend(p, sizeof(struct eth_hdr));
    arp = pktbuf_append(p, sizeof(struct arp_pkt));
    minip_build_mac_hdr(eth, bcast_mac, ETH_TYPE_ARP);

    arp->htype = htons(0x0001);
    arp->ptype = htons(0x0800);
    arp->hlen = 6;
    arp->plen = 4;
    arp->oper = htons(ARP_OPER_REQUEST);
    arp->spa = minip_ip;
    arp->tpa = addr;
    mac_addr_copy(arp->sha, minip_mac);
    mac_addr_copy(arp->tha, bcast_mac);

    minip_tx_handler(p);
    return 0;
}

static void handle_arp_timeout_cb(void *arg) {
    *(bool *)arg = true;
}

const uint8_t *get_dest_mac(uint32_t host)
{
    uint8_t *dst_mac = NULL;
    bool arp_timeout = false;
    net_timer_t arp_timeout_timer;

    if (host == IPV4_BCAST) {
        return bcast_mac;
    }

    dst_mac = arp_cache_lookup(host);
    if (dst_mac == NULL) {
        send_arp_request(host);
        memset(&arp_timeout_timer, 0, sizeof(arp_timeout_timer));
        net_timer_set(&arp_timeout_timer, handle_arp_timeout_cb, &arp_timeout, 100);
        while (!arp_timeout) {
            dst_mac = arp_cache_lookup(host);
            if (dst_mac) {
                net_timer_cancel(&arp_timeout_timer);
                break;
            }
        }
    }

    return dst_mac;
}

status_t minip_ipv4_send(pktbuf_t *p, uint32_t dest_addr, uint8_t proto)
{
    status_t ret = 0;
    size_t data_len = p->dlen;
    const uint8_t *dst_mac;

    struct ipv4_hdr *ip = pktbuf_prepend(p, sizeof(struct ipv4_hdr));
    struct eth_hdr *eth = pktbuf_prepend(p, sizeof(struct eth_hdr));


    if (dest_addr == IPV4_BCAST || dest_addr == minip_broadcast) {
        dst_mac = bcast_mac;
        goto ready;
    }

    dst_mac = get_dest_mac(dest_addr);
    if (!dst_mac) {
        pktbuf_free(p, true);
        ret = -EHOSTUNREACH;
        goto err;
    }

ready:
    minip_build_mac_hdr(eth, dst_mac, ETH_TYPE_IPV4);
    minip_build_ipv4_hdr(ip, dest_addr, proto, data_len);

    minip_tx_handler(p);

err:
    return ret;
}

/* Swap the dst/src ip addresses and send an ICMP ECHO REPLY with the same payload.
 * According to spec the data portion doesn't matter, but ping itself validates that
 * the payload is identical
 */
void send_ping_reply(uint32_t ipaddr, struct icmp_pkt *req, size_t reqdatalen)
{
    pktbuf_t *p;
    size_t len;
    struct eth_hdr *eth;
    struct ipv4_hdr *ip;
    struct icmp_pkt *icmp;

    if ((p = pktbuf_alloc()) == NULL) {
        return;
    }

    icmp = pktbuf_prepend(p, sizeof(struct icmp_pkt));
    ip = pktbuf_prepend(p, sizeof(struct ipv4_hdr));
    eth = pktbuf_prepend(p, sizeof(struct eth_hdr));
    pktbuf_append_data(p, req->data, reqdatalen);

    len = sizeof(struct icmp_pkt) + reqdatalen;

    minip_build_mac_hdr(eth, arp_cache_lookup(ipaddr), ETH_TYPE_IPV4);
    minip_build_ipv4_hdr(ip, ipaddr, IP_PROTO_ICMP, len);

    icmp->type = ICMP_ECHO_REPLY;
    icmp->code = 0;
    memcpy(icmp->hdr_data, req->hdr_data, sizeof(icmp->hdr_data));
    icmp->chksum = 0;
    icmp->chksum = rfc1701_chksum((uint8_t *) icmp, len);

    minip_tx_handler(p);
}

static void dump_ipv4_addr(uint32_t addr)
{
    const uint8_t *a = (void *)&addr;

    printf("%hhu.%hhu.%hhu.%hhu", a[0], a[1], a[2], a[3]);
}

static void dump_ipv4_packet(const struct ipv4_hdr *ip)
{
    printf("IP ");
    dump_ipv4_addr(ip->src_addr);
    printf(" -> ");
    dump_ipv4_addr(ip->dst_addr);
    printf(" hlen 0x%x, prot 0x%x, cksum 0x%x, len 0x%x, ident 0x%x, frag offset 0x%x\n",
        (ip->ver_ihl & 0xf) * 4, ip->proto, ntohs(ip->chksum), ntohs(ip->len), ntohs(ip->id), ntohs(ip->flags_frags) & 0x1fff);
}

__NO_INLINE static void handle_ipv4_packet(pktbuf_t *p, const uint8_t *src_mac)
{
    struct ipv4_hdr *ip;

    ip = (struct ipv4_hdr *)p->data;
    if (p->dlen < sizeof(struct ipv4_hdr))
        return;

    /* print packets for us */
    if (LOCAL_TRACE) {
        dump_ipv4_packet(ip);
    }

    /* reject bad packets */
    if (((ip->ver_ihl >> 4) & 0xf) != 4) {
        /* not version 4 */
        LTRACEF("REJECT: not version 4\n");
        return;
    }

    /* do we have enough buffer to hold the full header + options? */
    size_t header_len = (ip->ver_ihl & 0xf) * 4;
    if (p->dlen < header_len) {
        LTRACEF("REJECT: not enough buffer to hold header\n");
        return;
    }

    /* compute checksum */
    if (rfc1701_chksum((void *)ip, header_len) != 0) {
        /* bad checksum */
        LTRACEF("REJECT: bad checksum\n");
        return;
    }

    /* is the pkt_buf large enough to hold the length the header says the packet is? */
    if (htons(ip->len) > p->dlen) {
        LTRACEF("REJECT: packet exceeds size of buffer (header %d, dlen %d)\n", htons(ip->len), p->dlen);
        return;
    }

    /* trim any excess bytes at the end of the packet */
    if (p->dlen > htons(ip->len)) {
        pktbuf_consume_tail(p, p->dlen - htons(ip->len));
    }

    /* remove the header from the front of the packet_buf  */
    if (pktbuf_consume(p, header_len) == NULL) {
        return;
    }

    /* the packet is good, we can use it to populate our arp cache */
    arp_cache_update(ip->src_addr, src_mac);

    /* see if it's for us */
    if (ip->dst_addr != IPV4_BCAST) {
        if (minip_ip != IPV4_NONE && ip->dst_addr != minip_ip && ip->dst_addr != minip_broadcast) {
            LTRACEF("REJECT: for another host\n");
            return;
        }
    }

    /* We only handle UDP and ECHO REQUEST */
    switch (ip->proto) {
        case IP_PROTO_ICMP: {
            struct icmp_pkt *icmp;
            if ((icmp = pktbuf_consume(p, sizeof(struct icmp_pkt))) == NULL) {
                break;
            }
            if (icmp->type == ICMP_ECHO_REQUEST) {
                send_ping_reply(ip->src_addr, icmp, p->dlen);
            }
        }
        break;

        case IP_PROTO_UDP:
            udp_input(p, ip->src_addr);
        break;

        case IP_PROTO_TCP:
            tcp_input(p, ip->src_addr, ip->dst_addr);
        break;
    }
}

__NO_INLINE static int handle_arp_pkt(pktbuf_t *p)
{
    struct eth_hdr *eth;
    struct arp_pkt *arp;

    eth = (void*) (p->data - sizeof(struct eth_hdr));

    if ((arp = pktbuf_consume(p, sizeof(struct arp_pkt))) == NULL) {
        return -1;
    }

    switch(ntohs(arp->oper)) {
        case ARP_OPER_REQUEST: {
            pktbuf_t *rp;
            struct eth_hdr *reth;
            struct arp_pkt *rarp;

            if (memcmp(&arp->tpa, &minip_ip, sizeof(minip_ip)) == 0) {
                if ((rp = pktbuf_alloc()) == NULL) {
                    break;
                }

                reth = pktbuf_prepend(rp, sizeof(struct eth_hdr));
                rarp = pktbuf_append(rp, sizeof(struct arp_pkt));

                // Eth header
                minip_build_mac_hdr(reth, eth->src_mac, ETH_TYPE_ARP);

                // ARP packet
                rarp->oper = htons(ARP_OPER_REPLY);
                rarp->htype = htons(0x0001);
                rarp->ptype = htons(0x0800);
                rarp->hlen = 6;
                rarp->plen = 4;
                mac_addr_copy(rarp->sha, minip_mac);
                rarp->spa = minip_ip;
                mac_addr_copy(rarp->tha, arp->sha);
                rarp->tpa = arp->spa;

                minip_tx_handler(rp);
            }
        }
        break;

        case ARP_OPER_REPLY: {
            uint32_t addr;
            memcpy(&addr, &arp->spa, sizeof(addr)); // unaligned word
            arp_cache_update(addr, arp->sha);
        }
        break;
    }

    return 0;
}

static void dump_mac_address(const uint8_t *mac)
{
    printf("%02x:%02x:%02x:%02x:%02x:%02x",
        mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}

static void dump_eth_packet(const struct eth_hdr *eth)
{
    printf("ETH src ");
    dump_mac_address(eth->src_mac);
    printf(" dst ");
    dump_mac_address(eth->dst_mac);
    printf(" type 0x%hx\n", htons(eth->type));
}

void minip_rx_driver_callback(pktbuf_t *p)
{
    struct eth_hdr *eth;

    if ((eth = (void*) pktbuf_consume(p, sizeof(struct eth_hdr))) == NULL) {
        return;
    }

    if (LOCAL_TRACE) {
        dump_eth_packet(eth);
    }

    if (memcmp(eth->dst_mac, minip_mac, 6) != 0 &&
        memcmp(eth->dst_mac, broadcast_mac, 6) != 0) {
        /* not for us */
        return;
    }

    switch(htons(eth->type)) {
        case ETH_TYPE_IPV4:
            LTRACEF("ipv4 pkt\n");
            handle_ipv4_packet(p, eth->src_mac);
            break;

        case ETH_TYPE_ARP:
            LTRACEF("arp pkt\n");
            handle_arp_pkt(p);
            break;
    }
}

uint32_t minip_parse_ipaddr(const char* ipaddr_str, size_t len)
{
    uint8_t ip[4] = { 0, 0, 0, 0 };
    uint8_t pos = 0, i = 0;

    while (pos < len) {
        char c = ipaddr_str[pos];
        if (c == '.') {
            i++;
        } else if (c == '\0') {
            break;
        } else {
            ip[i] *= 10;
            ip[i] += c - '0';
        }
        pos++;
    }

    return IPV4_PACK(ip);
}

// vim: set ts=4 sw=4 expandtab: