Diff: modules/pico_icmp4.c

Revision:

2:540f6e142d59

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_icmp4.c	Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,307 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_icmp4.h"
+#include "pico_config.h"
+#include "pico_ipv4.h"
+#include "pico_eth.h"
+#include "pico_device.h"
+#include "pico_stack.h"
+#include "pico_tree.h"
+
+/* Queues */
+static struct pico_queue icmp_in = {};
+static struct pico_queue icmp_out = {};
+
+
+/* Functions */
+
+static int pico_icmp4_checksum(struct pico_frame *f)
+{
+  struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
+  if (!hdr) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  hdr->crc = 0;
+  hdr->crc = short_be(pico_checksum(hdr, f->transport_len));
+  return 0;
+}
+
+#ifdef PICO_SUPPORT_PING
+static void ping_recv_reply(struct pico_frame *f);
+#endif
+
+static int pico_icmp4_process_in(struct pico_protocol *self, struct pico_frame *f)
+{
+  struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
+  if (hdr->type == PICO_ICMP_ECHO) {
+    hdr->type = PICO_ICMP_ECHOREPLY;
+    /* outgoing frames require a f->len without the ethernet header len */
+    if (f->dev->eth)
+      f->len -= PICO_SIZE_ETHHDR;
+    pico_icmp4_checksum(f);
+    pico_ipv4_rebound(f);
+  } else if (hdr->type == PICO_ICMP_UNREACH) {
+    f->net_hdr = f->transport_hdr + PICO_ICMPHDR_UN_SIZE;
+    pico_ipv4_unreachable(f, hdr->code);
+  } else if (hdr->type == PICO_ICMP_ECHOREPLY) {
+#ifdef PICO_SUPPORT_PING
+    ping_recv_reply(f);
+#endif
+    pico_frame_discard(f);
+  } else {
+    pico_frame_discard(f);
+  }
+  return 0;
+}
+
+static int pico_icmp4_process_out(struct pico_protocol *self, struct pico_frame *f)
+{
+  dbg("Called %s\n", __FUNCTION__);
+  return 0;
+}
+
+/* Interface: protocol definition */
+struct pico_protocol pico_proto_icmp4 = {
+  .name = "icmp4",
+  .proto_number = PICO_PROTO_ICMP4,
+  .layer = PICO_LAYER_TRANSPORT,
+  .process_in = pico_icmp4_process_in,
+  .process_out = pico_icmp4_process_out,
+  .q_in = &icmp_in,
+  .q_out = &icmp_out,
+};
+
+static int pico_icmp4_notify(struct pico_frame *f, uint8_t type, uint8_t code)
+{
+  struct pico_frame *reply;
+  struct pico_icmp4_hdr *hdr;
+  struct pico_ipv4_hdr *info;
+  if (f == NULL) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  reply = pico_proto_ipv4.alloc(&pico_proto_ipv4, 8 + sizeof(struct pico_ipv4_hdr) + PICO_ICMPHDR_UN_SIZE);
+  info = (struct pico_ipv4_hdr*)(f->net_hdr);
+  hdr = (struct pico_icmp4_hdr *) reply->transport_hdr;
+  hdr->type = type;
+  hdr->code = code;
+  hdr->hun.ih_pmtu.ipm_nmtu = short_be(1500);
+  hdr->hun.ih_pmtu.ipm_void = 0;
+  reply->transport_len = 8 + sizeof(struct pico_ipv4_hdr) +  PICO_ICMPHDR_UN_SIZE;
+  reply->payload = reply->transport_hdr + PICO_ICMPHDR_UN_SIZE;
+  memcpy(reply->payload, f->net_hdr, 8 + sizeof(struct pico_ipv4_hdr));
+  pico_icmp4_checksum(reply);
+  pico_ipv4_frame_push(reply, &info->src, PICO_PROTO_ICMP4);
+  return 0;
+}
+
+int pico_icmp4_port_unreachable(struct pico_frame *f)
+{
+  /*Parameter check executed in pico_icmp4_notify*/
+  return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PORT);
+}
+
+int pico_icmp4_proto_unreachable(struct pico_frame *f)
+{
+  /*Parameter check executed in pico_icmp4_notify*/
+  return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PROTOCOL);
+}
+
+int pico_icmp4_dest_unreachable(struct pico_frame *f)
+{
+  /*Parameter check executed in pico_icmp4_notify*/
+  return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_HOST);
+}
+
+int pico_icmp4_ttl_expired(struct pico_frame *f)
+{
+  /*Parameter check executed in pico_icmp4_notify*/
+  return pico_icmp4_notify(f, PICO_ICMP_TIME_EXCEEDED, PICO_ICMP_TIMXCEED_INTRANS);
+}
+
+int pico_icmp4_packet_filtered(struct pico_frame *f)
+{
+  /*Parameter check executed in pico_icmp4_notify*/
+  /*Packet Filtered: type 3, code 13 (Communication Administratively Prohibited)*/
+  return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_FILTER_PROHIB);
+}
+
+/***********************/
+/* Ping implementation */
+/***********************/
+/***********************/
+/***********************/
+/***********************/
+
+
+#ifdef PICO_SUPPORT_PING
+
+
+struct pico_icmp4_ping_cookie
+{
+  struct pico_ip4 dst;
+  uint16_t err;
+  uint16_t id;
+  uint16_t seq;
+  uint16_t size;
+  int count;
+  unsigned long timestamp;
+  int interval;
+  int timeout;
+  void (*cb)(struct pico_icmp4_stats*);
+
+};
+
+static int cookie_compare(void *ka, void *kb)
+{
+    struct pico_icmp4_ping_cookie *a = ka, *b = kb;
+  if (a->id < b->id)
+    return -1;
+  if (a->id > b->id)
+    return 1;
+  return (a->seq - b->seq);
+}
+
+PICO_TREE_DECLARE(Pings,cookie_compare);
+
+static int pico_icmp4_send_echo(struct pico_icmp4_ping_cookie *cookie)
+{
+  struct pico_frame *echo = pico_proto_ipv4.alloc(&pico_proto_ipv4, PICO_ICMPHDR_UN_SIZE + cookie->size);
+  struct pico_icmp4_hdr *hdr;
+
+  hdr = (struct pico_icmp4_hdr *) echo->transport_hdr;
+
+  hdr->type = PICO_ICMP_ECHO;
+  hdr->code = 0;
+  hdr->hun.ih_idseq.idseq_id = short_be(cookie->id);
+  hdr->hun.ih_idseq.idseq_seq = short_be(cookie->seq);
+  echo->transport_len = PICO_ICMPHDR_UN_SIZE + cookie->size;
+  echo->payload = echo->transport_hdr + PICO_ICMPHDR_UN_SIZE;
+  echo->payload_len = cookie->size;
+  /* XXX: Fill payload */
+  pico_icmp4_checksum(echo);
+  pico_ipv4_frame_push(echo, &cookie->dst, PICO_PROTO_ICMP4);
+  return 0;
+}
+
+
+static void ping_timeout(unsigned long now, void *arg)
+{
+  struct pico_icmp4_ping_cookie *cookie = (struct pico_icmp4_ping_cookie *)arg;
+  if(pico_tree_findKey(&Pings,cookie)){
+    if (cookie->err == PICO_PING_ERR_PENDING) {
+      struct pico_icmp4_stats stats;
+      stats.dst = cookie->dst;
+      stats.seq = cookie->seq;
+      stats.time = 0;
+      stats.size = cookie->size;
+      stats.err = PICO_PING_ERR_TIMEOUT;
+      dbg(" ---- Ping timeout!!!\n");
+      cookie->cb(&stats);
+    }
+
+    pico_tree_delete(&Pings,cookie);
+    pico_free(cookie);
+  }
+}
+
+static void next_ping(unsigned long now, void *arg);
+static inline void send_ping(struct pico_icmp4_ping_cookie *cookie)
+{
+  pico_icmp4_send_echo(cookie);
+  cookie->timestamp = pico_tick;
+  pico_timer_add(cookie->timeout, ping_timeout, cookie);
+  if (cookie->seq < cookie->count)
+    pico_timer_add(cookie->interval, next_ping, cookie);
+}
+
+static void next_ping(unsigned long now, void *arg)
+{
+  struct pico_icmp4_ping_cookie *newcookie, *cookie = (struct pico_icmp4_ping_cookie *)arg;
+
+    if(pico_tree_findKey(&Pings,cookie)){
+    if (cookie->seq < cookie->count) {
+      newcookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
+      if (!newcookie)
+        return;
+      memcpy(newcookie, cookie, sizeof(struct pico_icmp4_ping_cookie));
+      newcookie->seq++;
+
+        pico_tree_insert(&Pings,newcookie);
+      send_ping(newcookie);
+    }
+  }
+}
+
+
+static void ping_recv_reply(struct pico_frame *f)
+{
+  struct pico_icmp4_ping_cookie test, *cookie;
+  struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
+  test.id  = short_be(hdr->hun.ih_idseq.idseq_id );
+  test.seq = short_be(hdr->hun.ih_idseq.idseq_seq);
+
+  cookie = pico_tree_findKey(&Pings, &test);
+  if (cookie) {
+    struct pico_icmp4_stats stats;
+    cookie->err = PICO_PING_ERR_REPLIED;
+    stats.dst = cookie->dst;
+    stats.seq = cookie->seq;
+    stats.size = cookie->size;
+    stats.time = pico_tick - cookie->timestamp;
+    stats.err = cookie->err;
+    stats.ttl = ((struct pico_ipv4_hdr *)f->net_hdr)->ttl;
+        if(cookie->cb != NULL)
+        cookie->cb(&stats);
+  } else {
+    dbg("Reply for seq=%d, not found.\n", test.seq);
+  }
+}
+
+int pico_icmp4_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *))
+{
+  static uint16_t next_id = 0x91c0;
+  struct pico_icmp4_ping_cookie *cookie;
+
+  if((dst == NULL) || (interval == 0) || (timeout == 0) || (count == 0)){
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+
+  cookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
+  if (!cookie) {
+    pico_err = PICO_ERR_ENOMEM;
+    return -1;
+  }
+
+  if (pico_string_to_ipv4(dst, &cookie->dst.addr) < 0) {
+    pico_err = PICO_ERR_EINVAL;
+    pico_free(cookie);
+    return -1;
+  }
+  cookie->seq = 1;
+  cookie->id = next_id++;
+  cookie->err = PICO_PING_ERR_PENDING;
+  cookie->size = size;
+  cookie->interval = interval;
+  cookie->timeout = timeout;
+  cookie->cb = cb;
+  cookie->count = count;
+
+  pico_tree_insert(&Pings,cookie);
+  send_ping(cookie);
+
+  return 0;
+
+}
+
+#endif