pico_icmp4.c

00001 /*********************************************************************
00002 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
00003 See LICENSE and COPYING for usage.
00004 
00005 .
00006 
00007 Authors: Daniele Lacamera
00008 *********************************************************************/
00009 
00010 
00011 #include "pico_icmp4.h"
00012 #include "pico_config.h"
00013 #include "pico_ipv4.h"
00014 #include "pico_eth.h"
00015 #include "pico_device.h"
00016 #include "pico_stack.h"
00017 #include "pico_tree.h"
00018 
00019 /* Queues */
00020 static struct pico_queue icmp_in = {};
00021 static struct pico_queue icmp_out = {};
00022 
00023 
00024 /* Functions */
00025 
00026 static int pico_icmp4_checksum(struct pico_frame *f)
00027 {
00028   struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
00029   if (!hdr) {
00030     pico_err = PICO_ERR_EINVAL;
00031     return -1;
00032   }
00033   hdr->crc = 0;
00034   hdr->crc = short_be(pico_checksum(hdr, f->transport_len));
00035   return 0;
00036 }
00037 
00038 #ifdef PICO_SUPPORT_PING
00039 static void ping_recv_reply(struct pico_frame *f);
00040 #endif
00041 
00042 static int pico_icmp4_process_in(struct pico_protocol *self, struct pico_frame *f)
00043 {
00044   struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
00045   if (hdr->type == PICO_ICMP_ECHO) {
00046     hdr->type = PICO_ICMP_ECHOREPLY;
00047     /* outgoing frames require a f->len without the ethernet header len */
00048     if (f->dev->eth)
00049       f->len -= PICO_SIZE_ETHHDR;
00050     pico_icmp4_checksum(f);
00051     pico_ipv4_rebound(f);
00052   } else if (hdr->type == PICO_ICMP_UNREACH) {
00053     f->net_hdr = f->transport_hdr + PICO_ICMPHDR_UN_SIZE;
00054     pico_ipv4_unreachable(f, hdr->code);
00055   } else if (hdr->type == PICO_ICMP_ECHOREPLY) {
00056 #ifdef PICO_SUPPORT_PING
00057     ping_recv_reply(f);
00058 #endif
00059     pico_frame_discard(f);
00060   } else {
00061     pico_frame_discard(f);
00062   }
00063   return 0;
00064 }
00065 
00066 static int pico_icmp4_process_out(struct pico_protocol *self, struct pico_frame *f)
00067 {
00068   dbg("Called %s\n", __FUNCTION__);
00069   return 0;
00070 }
00071 
00072 /* Interface: protocol definition */
00073 struct pico_protocol pico_proto_icmp4 = {
00074   .name = "icmp4",
00075   .proto_number = PICO_PROTO_ICMP4,
00076   .layer = PICO_LAYER_TRANSPORT,
00077   .process_in = pico_icmp4_process_in,
00078   .process_out = pico_icmp4_process_out,
00079   .q_in = &icmp_in,
00080   .q_out = &icmp_out,
00081 };
00082 
00083 static int pico_icmp4_notify(struct pico_frame *f, uint8_t type, uint8_t code)
00084 {
00085   struct pico_frame *reply;
00086   struct pico_icmp4_hdr *hdr;
00087   struct pico_ipv4_hdr *info;
00088   if (f == NULL) {
00089     pico_err = PICO_ERR_EINVAL;
00090     return -1;
00091   }
00092   reply = pico_proto_ipv4.alloc(&pico_proto_ipv4, 8 + sizeof(struct pico_ipv4_hdr) + PICO_ICMPHDR_UN_SIZE);
00093   info = (struct pico_ipv4_hdr*)(f->net_hdr);
00094   hdr = (struct pico_icmp4_hdr *) reply->transport_hdr;
00095   hdr->type = type;
00096   hdr->code = code;
00097   hdr->hun.ih_pmtu.ipm_nmtu = short_be(1500);
00098   hdr->hun.ih_pmtu.ipm_void = 0;
00099   reply->transport_len = 8 + sizeof(struct pico_ipv4_hdr) +  PICO_ICMPHDR_UN_SIZE;
00100   reply->payload = reply->transport_hdr + PICO_ICMPHDR_UN_SIZE;
00101   memcpy(reply->payload, f->net_hdr, 8 + sizeof(struct pico_ipv4_hdr));
00102   pico_icmp4_checksum(reply);
00103   pico_ipv4_frame_push(reply, &info->src, PICO_PROTO_ICMP4);
00104   return 0;
00105 }
00106 
00107 int pico_icmp4_port_unreachable(struct pico_frame *f)
00108 {
00109   /*Parameter check executed in pico_icmp4_notify*/
00110   return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PORT);
00111 }
00112 
00113 int pico_icmp4_proto_unreachable(struct pico_frame *f)
00114 {
00115   /*Parameter check executed in pico_icmp4_notify*/
00116   return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PROTOCOL);
00117 }
00118 
00119 int pico_icmp4_dest_unreachable(struct pico_frame *f)
00120 {
00121   /*Parameter check executed in pico_icmp4_notify*/
00122   return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_HOST);
00123 }
00124 
00125 int pico_icmp4_ttl_expired(struct pico_frame *f)
00126 {
00127   /*Parameter check executed in pico_icmp4_notify*/
00128   return pico_icmp4_notify(f, PICO_ICMP_TIME_EXCEEDED, PICO_ICMP_TIMXCEED_INTRANS);
00129 }
00130 
00131 int pico_icmp4_packet_filtered(struct pico_frame *f)
00132 {
00133   /*Parameter check executed in pico_icmp4_notify*/
00134   /*Packet Filtered: type 3, code 13 (Communication Administratively Prohibited)*/
00135   return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_FILTER_PROHIB);
00136 }
00137 
00138 /***********************/
00139 /* Ping implementation */
00140 /***********************/
00141 /***********************/
00142 /***********************/
00143 /***********************/
00144 
00145 
00146 #ifdef PICO_SUPPORT_PING
00147 
00148 
00149 struct pico_icmp4_ping_cookie
00150 {
00151   struct pico_ip4 dst;
00152   uint16_t err;
00153   uint16_t id;
00154   uint16_t seq;
00155   uint16_t size;
00156   int count;
00157   unsigned long timestamp;
00158   int interval;
00159   int timeout;
00160   void (*cb)(struct pico_icmp4_stats*);
00161 
00162 };
00163 
00164 static int cookie_compare(void *ka, void *kb)
00165 {
00166     struct pico_icmp4_ping_cookie *a = ka, *b = kb;
00167   if (a->id < b->id)
00168     return -1;
00169   if (a->id > b->id)
00170     return 1;
00171   return (a->seq - b->seq);
00172 }
00173 
00174 PICO_TREE_DECLARE(Pings,cookie_compare);
00175 
00176 static int pico_icmp4_send_echo(struct pico_icmp4_ping_cookie *cookie)
00177 {
00178   struct pico_frame *echo = pico_proto_ipv4.alloc(&pico_proto_ipv4, PICO_ICMPHDR_UN_SIZE + cookie->size);
00179   struct pico_icmp4_hdr *hdr;
00180 
00181   hdr = (struct pico_icmp4_hdr *) echo->transport_hdr;
00182 
00183   hdr->type = PICO_ICMP_ECHO;
00184   hdr->code = 0;
00185   hdr->hun.ih_idseq.idseq_id = short_be(cookie->id);
00186   hdr->hun.ih_idseq.idseq_seq = short_be(cookie->seq);
00187   echo->transport_len = PICO_ICMPHDR_UN_SIZE + cookie->size;
00188   echo->payload = echo->transport_hdr + PICO_ICMPHDR_UN_SIZE;
00189   echo->payload_len = cookie->size;
00190   /* XXX: Fill payload */
00191   pico_icmp4_checksum(echo);
00192   pico_ipv4_frame_push(echo, &cookie->dst, PICO_PROTO_ICMP4);
00193   return 0;
00194 }
00195 
00196 
00197 static void ping_timeout(unsigned long now, void *arg)
00198 {
00199   struct pico_icmp4_ping_cookie *cookie = (struct pico_icmp4_ping_cookie *)arg;
00200   if(pico_tree_findKey(&Pings,cookie)){
00201     if (cookie->err == PICO_PING_ERR_PENDING) {
00202       struct pico_icmp4_stats stats;
00203       stats.dst = cookie->dst;
00204       stats.seq = cookie->seq;
00205       stats.time = 0;
00206       stats.size = cookie->size;
00207       stats.err = PICO_PING_ERR_TIMEOUT;
00208       dbg(" ---- Ping timeout!!!\n");
00209       cookie->cb(&stats);
00210     }
00211 
00212     pico_tree_delete(&Pings,cookie);
00213     pico_free(cookie);
00214   }
00215 }
00216 
00217 static void next_ping(unsigned long now, void *arg);
00218 static inline void send_ping(struct pico_icmp4_ping_cookie *cookie)
00219 {
00220   pico_icmp4_send_echo(cookie);
00221   cookie->timestamp = pico_tick;
00222   pico_timer_add(cookie->timeout, ping_timeout, cookie);
00223   if (cookie->seq < cookie->count)
00224     pico_timer_add(cookie->interval, next_ping, cookie);
00225 }
00226 
00227 static void next_ping(unsigned long now, void *arg)
00228 {
00229   struct pico_icmp4_ping_cookie *newcookie, *cookie = (struct pico_icmp4_ping_cookie *)arg;
00230 
00231     if(pico_tree_findKey(&Pings,cookie)){
00232     if (cookie->seq < cookie->count) {
00233       newcookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
00234       if (!newcookie)
00235         return;
00236       memcpy(newcookie, cookie, sizeof(struct pico_icmp4_ping_cookie));
00237       newcookie->seq++;
00238 
00239         pico_tree_insert(&Pings,newcookie);
00240       send_ping(newcookie);
00241     }
00242   }
00243 }
00244 
00245 
00246 static void ping_recv_reply(struct pico_frame *f)
00247 {
00248   struct pico_icmp4_ping_cookie test, *cookie;
00249   struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
00250   test.id  = short_be(hdr->hun.ih_idseq.idseq_id );
00251   test.seq = short_be(hdr->hun.ih_idseq.idseq_seq);
00252 
00253   cookie = pico_tree_findKey(&Pings, &test);
00254   if (cookie) {
00255     struct pico_icmp4_stats stats;
00256     cookie->err = PICO_PING_ERR_REPLIED;
00257     stats.dst = cookie->dst;
00258     stats.seq = cookie->seq;
00259     stats.size = cookie->size;
00260     stats.time = pico_tick - cookie->timestamp;
00261     stats.err = cookie->err;
00262     stats.ttl = ((struct pico_ipv4_hdr *)f->net_hdr)->ttl;
00263         if(cookie->cb != NULL)
00264         cookie->cb(&stats);
00265   } else {
00266     dbg("Reply for seq=%d, not found.\n", test.seq);
00267   }
00268 }
00269 
00270 int pico_icmp4_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *))
00271 {
00272   static uint16_t next_id = 0x91c0;
00273   struct pico_icmp4_ping_cookie *cookie;
00274 
00275   if((dst == NULL) || (interval == 0) || (timeout == 0) || (count == 0)){
00276     pico_err = PICO_ERR_EINVAL;
00277     return -1;
00278   }
00279 
00280   cookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
00281   if (!cookie) {
00282     pico_err = PICO_ERR_ENOMEM;
00283     return -1;
00284   }
00285 
00286   if (pico_string_to_ipv4(dst, &cookie->dst.addr) < 0) {
00287     pico_err = PICO_ERR_EINVAL;
00288     pico_free(cookie);
00289     return -1;
00290   }
00291   cookie->seq = 1;
00292   cookie->id = next_id++;
00293   cookie->err = PICO_PING_ERR_PENDING;
00294   cookie->size = size;
00295   cookie->interval = interval;
00296   cookie->timeout = timeout;
00297   cookie->cb = cb;
00298   cookie->count = count;
00299 
00300   pico_tree_insert(&Pings,cookie);
00301   send_ping(cookie);
00302 
00303   return 0;
00304 
00305 }
00306 
00307 #endif