Add ICMP functions

Makefile

@@ -38,6 +38,7 @@ NET_OBJS = \
 	net/common.o\
 	net/e1000.o\
 	net/ethernet.o\
+	net/icmp.o\
 	net/ip.o\
 	net/net.o\
 

net/defs.h

@@ -26,6 +26,10 @@ ssize_t         ethernet_rx_helper(struct netdev *dev, uint8_t *frame, size_t fl
 ssize_t         ethernet_tx_helper(struct netdev *dev, uint16_t type, const uint8_t *payload, size_t plen, const void *dst, ssize_t (*cb)(struct netdev*, uint8_t*, size_t));
 void            ethernet_netdev_setup(struct netdev *dev);
 
+// icmp.c
+int             icmp_tx(struct netif *netif, uint8_t type, uint8_t code, uint32_t values, uint8_t *data, size_t len, ip_addr_t *dst);
+int             icmp_init(void);
+
 // ip.c
 int             ip_addr_pton(const char *p, ip_addr_t *n);
 char *          ip_addr_ntop(const ip_addr_t *n, char *p, size_t size);

net/icmp.c

@@ -0,0 +1,143 @@
+#include "../types.h"
+#include "../defs.h"
+#include "../ext/types.h"
+#include "../ext/defs.h"
+#include "types.h"
+#include "defs.h"
+#include "net.h"
+#include "ip.h"
+#include "icmp.h"
+
+struct icmp_hdr {
+    uint8_t type;
+    uint8_t code;
+    uint16_t sum;
+    union {
+        struct {
+            uint16_t id;
+            uint16_t seq;
+        } echo;
+        ip_addr_t gateway;
+        uint8_t ptr;
+        uint32_t unused;
+    } un;
+#define ih_ptr     un.ptr
+#define ih_id      un.echo.id
+#define ih_seq     un.echo.seq
+#define ih_unused  un.unused
+#define ih_values  un.unused
+#define ih_gateway un.gateway
+    uint8_t data[0];
+};
+
+#define ICMP_BUFSIZ IP_PAYLOAD_SIZE_MAX
+
+static char *
+icmp_type_ntoa (uint8_t type) {
+    switch (type) {
+    case ICMP_TYPE_ECHOREPLY:
+        return "Echo Reply";
+    case ICMP_TYPE_DEST_UNREACH:
+        return "Destination Unreachable";
+    case ICMP_TYPE_SOURCE_QUENCH:
+        return "Source Quench";
+    case ICMP_TYPE_REDIRECT:
+        return "Redirect";
+    case ICMP_TYPE_ECHO:
+        return "Echo";
+    case ICMP_TYPE_TIME_EXCEEDED:
+        return "Time Exceeded";
+    case ICMP_TYPE_PARAM_PROBLEM:
+        return "Parameter Problem";
+    case ICMP_TYPE_TIMESTAMP:
+        return "Timestamp";
+    case ICMP_TYPE_TIMESTAMPREPLY:
+        return "Timestamp Reply";
+    case ICMP_TYPE_INFO_REQUEST:
+        return "Information Request";
+    case ICMP_TYPE_INFO_REPLY:
+        return "Information Reply";
+    }
+    return "UNKNOWN";
+}
+
+void
+icmp_dump (struct netif *netif, ip_addr_t *src, ip_addr_t *dst, uint8_t *packet, size_t plen) {
+    struct netif_ip *iface;
+    char addr[IP_ADDR_STR_LEN];
+    struct icmp_hdr *hdr;
+    uint32_t *timestamp;
+
+    iface = (struct netif_ip *)netif;
+    cprintf("   dev: %s (%s)\n", netif->dev->name, ip_addr_ntop(&iface->unicast, addr, sizeof(addr)));
+    cprintf("   src: %s\n", src ? ip_addr_ntop(src, addr, sizeof(addr)) : "(self)");
+    cprintf("   dst: %s\n", ip_addr_ntop(dst, addr, sizeof(addr)));
+    hdr = (struct icmp_hdr *)packet;
+    cprintf("  type: %u (%s)\n", hdr->type, icmp_type_ntoa(hdr->type));
+    cprintf("  code: %u\n", hdr->code);
+    cprintf("   sum: %u\n", ntoh16(hdr->sum));
+    switch (hdr->type) {
+    case ICMP_TYPE_ECHOREPLY:
+    case ICMP_TYPE_ECHO:
+    case ICMP_TYPE_TIMESTAMP:
+    case ICMP_TYPE_TIMESTAMPREPLY:
+    case ICMP_TYPE_INFO_REQUEST:
+    case ICMP_TYPE_INFO_REPLY:
+        cprintf("    id: %u\n", ntoh16(hdr->ih_id));
+        cprintf("   seq: %u\n", ntoh16(hdr->ih_seq));
+        break;
+    case ICMP_TYPE_REDIRECT:
+        cprintf("    gw: %s\n", ip_addr_ntop(&hdr->ih_gateway, addr, sizeof(addr)));
+        break;
+    }
+    if (hdr->type == ICMP_TYPE_TIMESTAMP || hdr->type == ICMP_TYPE_TIMESTAMPREPLY) {
+        timestamp = (uint32_t *)hdr->data;
+        cprintf(" otime: %u\n", ntoh32(*timestamp++));
+        cprintf(" rtime: %u\n", ntoh32(*timestamp++));
+        cprintf(" ttime: %u\n", ntoh32(*timestamp++));
+    }
+    hexdump(packet, plen);
+}
+
+static void
+icmp_rx (uint8_t *packet, size_t plen, ip_addr_t *src, ip_addr_t *dst, struct netif *netif) {
+    struct icmp_hdr *hdr;
+
+    (void)dst;
+    if (plen < sizeof(struct icmp_hdr)) {
+        return;
+    }
+    cprintf(">>> icmp_rx <<<\n");
+    icmp_dump(netif, src, dst, packet, plen);
+    hdr = (struct icmp_hdr *)packet;
+    switch (hdr->type) {
+    case ICMP_TYPE_ECHO:
+        icmp_tx(netif, ICMP_TYPE_ECHOREPLY, hdr->code, hdr->ih_values, hdr->data, plen - sizeof(struct icmp_hdr), src);
+        break;
+    }
+}
+
+int
+icmp_tx (struct netif *netif, uint8_t type, uint8_t code, uint32_t values, uint8_t *data, size_t len, ip_addr_t *dst) {
+    uint8_t buf[ICMP_BUFSIZ];
+    struct icmp_hdr *hdr;
+    size_t msg_len;
+
+    hdr = (struct icmp_hdr *)buf;
+    hdr->type = type;
+    hdr->code = code;
+    hdr->sum = 0;
+    hdr->ih_values = values;
+    memcpy(hdr->data, data, len);
+    msg_len = sizeof(struct icmp_hdr) + len;
+    hdr->sum = cksum16((uint16_t *)hdr, msg_len, 0);
+    cprintf(">>> icmp_tx <<<\n");
+    icmp_dump(netif, NULL, dst, (uint8_t *)hdr, msg_len);
+    return ip_tx(netif, IP_PROTOCOL_ICMP, (uint8_t *)hdr, msg_len, dst);
+}
+
+int
+icmp_init (void) {
+    ip_add_protocol(IP_PROTOCOL_ICMP, icmp_rx);
+    return 0;
+}

net/icmp.h

@@ -0,0 +1,31 @@
+#define ICMP_TYPE_ECHOREPLY 0
+#define ICMP_TYPE_DEST_UNREACH 3
+#define ICMP_TYPE_SOURCE_QUENCH 4
+#define ICMP_TYPE_REDIRECT 5
+#define ICMP_TYPE_ECHO 8
+#define ICMP_TYPE_TIME_EXCEEDED 11
+#define ICMP_TYPE_PARAM_PROBLEM 12
+#define ICMP_TYPE_TIMESTAMP 13
+#define ICMP_TYPE_TIMESTAMPREPLY 14
+#define ICMP_TYPE_INFO_REQUEST 15
+#define ICMP_TYPE_INFO_REPLY 16
+
+/* for UNREACH */
+#define ICMP_CODE_NET_UNREACH 0
+#define ICMP_CODE_HOST_UNREACH 1
+#define ICMP_CODE_PROTO_UNREACH 2
+#define ICMP_CODE_PORT_UNREACH 3
+#define ICMP_CODE_FRAGMENT_NEEDED 4
+#define ICMP_CODE_SOURCE_ROUTE_FAILED 5
+
+/* for REDIRECT */
+#define ICMP_CODE_REDIRECT_NET 0
+#define ICMP_CODE_REDIRECT_HOST 1
+#define ICMP_CODE_REDIRECT_TOS_NET 2
+#define ICMP_CODE_REDIRECT_TOS_HOST 3
+
+/* for TIME_EXEEDED */
+#define ICMP_CODE_EXCEEDED_TTL 0
+#define ICMP_CODE_EXCEEDED_FRAGMENT 1
+
+#define ICMP_COPY_LEN(x) ((((x)->vhl & 0x0f) << 2) + 8)

net/net.c

@@ -118,6 +118,7 @@ netinit(void)
 {
     arp_init();
     ip_init();
+    icmp_init();
 
     // dummy setting
     for (struct netdev *dev = devices; dev; dev = dev->next) {