pico_icmp4.c

00001 /*********************************************************************
00002    PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. 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     0
00022 };
00023 static struct pico_queue icmp_out = {
00024     0
00025 };
00026 
00027 
00028 /* Functions */
00029 
00030 static int pico_icmp4_checksum(struct pico_frame *f)
00031 {
00032     struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
00033     if (!hdr) {
00034         pico_err = PICO_ERR_EINVAL;
00035         return -1;
00036     }
00037 
00038     hdr->crc = 0;
00039     hdr->crc = short_be(pico_checksum(hdr, f->transport_len));
00040     return 0;
00041 }
00042 
00043 #ifdef PICO_SUPPORT_PING
00044 static void ping_recv_reply(struct pico_frame *f);
00045 #endif
00046 
00047 static int pico_icmp4_process_in(struct pico_protocol *self, struct pico_frame *f)
00048 {
00049     struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
00050     static int firstpkt = 1;
00051     static uint16_t last_id = 0;
00052     static uint16_t last_seq = 0;
00053     IGNORE_PARAMETER(self);
00054 
00055     if (hdr->type == PICO_ICMP_ECHO) {
00056         hdr->type = PICO_ICMP_ECHOREPLY;
00057         /* outgoing frames require a f->len without the ethernet header len */
00058         if (f->dev && f->dev->eth)
00059             f->len -= PICO_SIZE_ETHHDR;
00060 
00061         if (!firstpkt && (hdr->hun.ih_idseq.idseq_id ==  last_id) && (last_seq == hdr->hun.ih_idseq.idseq_seq)) {
00062             /* The network duplicated the echo. Do not reply. */
00063             pico_frame_discard(f);
00064             return 0;
00065         }
00066 
00067         firstpkt = 0;
00068         last_id = hdr->hun.ih_idseq.idseq_id;
00069         last_seq = hdr->hun.ih_idseq.idseq_seq;
00070         pico_icmp4_checksum(f);
00071         pico_ipv4_rebound(f);
00072     } else if (hdr->type == PICO_ICMP_UNREACH) {
00073         f->net_hdr = f->transport_hdr + PICO_ICMPHDR_UN_SIZE;
00074         pico_ipv4_unreachable(f, hdr->code);
00075     } else if (hdr->type == PICO_ICMP_ECHOREPLY) {
00076 #ifdef PICO_SUPPORT_PING
00077         ping_recv_reply(f);
00078 #endif
00079         pico_frame_discard(f);
00080     } else {
00081         pico_frame_discard(f);
00082     }
00083 
00084     return 0;
00085 }
00086 
00087 static int pico_icmp4_process_out(struct pico_protocol *self, struct pico_frame *f)
00088 {
00089     IGNORE_PARAMETER(self);
00090     IGNORE_PARAMETER(f);
00091     dbg("Called %s\n", __FUNCTION__);
00092     return 0;
00093 }
00094 
00095 /* Interface: protocol definition */
00096 struct pico_protocol pico_proto_icmp4 = {
00097     .name = "icmp4",
00098     .proto_number = PICO_PROTO_ICMP4,
00099     .layer = PICO_LAYER_TRANSPORT,
00100     .process_in = pico_icmp4_process_in,
00101     .process_out = pico_icmp4_process_out,
00102     .q_in = &icmp_in,
00103     .q_out = &icmp_out,
00104 };
00105 
00106 static int pico_icmp4_notify(struct pico_frame *f, uint8_t type, uint8_t code)
00107 {
00108     struct pico_frame *reply;
00109     struct pico_icmp4_hdr *hdr;
00110     struct pico_ipv4_hdr *info;
00111     uint16_t f_tot_len;
00112 
00113     f_tot_len = short_be(((struct pico_ipv4_hdr *)f->net_hdr)->len);
00114 
00115     if (f_tot_len < (sizeof(struct pico_ipv4_hdr)))
00116         return -1;
00117 
00118     /* Truncate tot len to be at most 8 bytes + iphdr */
00119     if (f_tot_len > (sizeof(struct pico_ipv4_hdr) + 8u)) {
00120         f_tot_len = (sizeof(struct pico_ipv4_hdr) + 8u);
00121     }
00122 
00123     if (f == NULL) {
00124         pico_err = PICO_ERR_EINVAL;
00125         return -1;
00126     }
00127 
00128     reply = pico_proto_ipv4.alloc(&pico_proto_ipv4, (uint16_t) (f_tot_len + PICO_ICMPHDR_UN_SIZE));
00129     info = (struct pico_ipv4_hdr*)(f->net_hdr);
00130     hdr = (struct pico_icmp4_hdr *) reply->transport_hdr;
00131     hdr->type = type;
00132     hdr->code = code;
00133     hdr->hun.ih_pmtu.ipm_nmtu = short_be(1500);
00134     hdr->hun.ih_pmtu.ipm_void = 0;
00135     reply->transport_len = (uint16_t)(f_tot_len +  PICO_ICMPHDR_UN_SIZE);
00136     reply->payload = reply->transport_hdr + PICO_ICMPHDR_UN_SIZE;
00137     memcpy(reply->payload, f->net_hdr, f_tot_len);
00138     pico_icmp4_checksum(reply);
00139     pico_ipv4_frame_push(reply, &info->src, PICO_PROTO_ICMP4);
00140     return 0;
00141 }
00142 
00143 int pico_icmp4_port_unreachable(struct pico_frame *f)
00144 {
00145     /*Parameter check executed in pico_icmp4_notify*/
00146     return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PORT);
00147 }
00148 
00149 int pico_icmp4_proto_unreachable(struct pico_frame *f)
00150 {
00151     /*Parameter check executed in pico_icmp4_notify*/
00152     return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PROTOCOL);
00153 }
00154 
00155 int pico_icmp4_dest_unreachable(struct pico_frame *f)
00156 {
00157     /*Parameter check executed in pico_icmp4_notify*/
00158     return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_HOST);
00159 }
00160 
00161 int pico_icmp4_ttl_expired(struct pico_frame *f)
00162 {
00163     /*Parameter check executed in pico_icmp4_notify*/
00164     return pico_icmp4_notify(f, PICO_ICMP_TIME_EXCEEDED, PICO_ICMP_TIMXCEED_INTRANS);
00165 }
00166 
00167 MOCKABLE int pico_icmp4_frag_expired(struct pico_frame *f)
00168 {
00169     /*Parameter check executed in pico_icmp4_notify*/
00170     return pico_icmp4_notify(f, PICO_ICMP_TIME_EXCEEDED, PICO_ICMP_TIMXCEED_REASS);
00171 }
00172 
00173 int pico_icmp4_mtu_exceeded(struct pico_frame *f)
00174 {
00175     /*Parameter check executed in pico_icmp4_notify*/
00176     return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_NEEDFRAG);
00177 }
00178 
00179 int pico_icmp4_packet_filtered(struct pico_frame *f)
00180 {
00181     /*Parameter check executed in pico_icmp4_notify*/
00182     /*Packet Filtered: type 3, code 13 (Communication Administratively Prohibited)*/
00183     return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_FILTER_PROHIB);
00184 }
00185 
00186 int pico_icmp4_param_problem(struct pico_frame *f, uint8_t code)
00187 {
00188     return pico_icmp4_notify(f, PICO_ICMP_PARAMPROB, code);
00189 }
00190 
00191 /***********************/
00192 /* Ping implementation */
00193 /***********************/
00194 /***********************/
00195 /***********************/
00196 /***********************/
00197 
00198 
00199 #ifdef PICO_SUPPORT_PING
00200 
00201 
00202 struct pico_icmp4_ping_cookie
00203 {
00204     struct pico_ip4 dst;
00205     uint16_t err;
00206     uint16_t id;
00207     uint16_t seq;
00208     uint16_t size;
00209     int count;
00210     pico_time timestamp;
00211     int interval;
00212     int timeout;
00213     void (*cb)(struct pico_icmp4_stats*);
00214 };
00215 
00216 static int cookie_compare(void *ka, void *kb)
00217 {
00218     struct pico_icmp4_ping_cookie *a = ka, *b = kb;
00219     if (a->id < b->id)
00220         return -1;
00221 
00222     if (a->id > b->id)
00223         return 1;
00224 
00225     return (a->seq - b->seq);
00226 }
00227 
00228 PICO_TREE_DECLARE(Pings, cookie_compare);
00229 
00230 static int8_t pico_icmp4_send_echo(struct pico_icmp4_ping_cookie *cookie)
00231 {
00232     struct pico_frame *echo = pico_proto_ipv4.alloc(&pico_proto_ipv4, (uint16_t)(PICO_ICMPHDR_UN_SIZE + cookie->size));
00233     struct pico_icmp4_hdr *hdr;
00234     if (!echo) {
00235         return -1;
00236     }
00237 
00238     hdr = (struct pico_icmp4_hdr *) echo->transport_hdr;
00239 
00240     hdr->type = PICO_ICMP_ECHO;
00241     hdr->code = 0;
00242     hdr->hun.ih_idseq.idseq_id = short_be(cookie->id);
00243     hdr->hun.ih_idseq.idseq_seq = short_be(cookie->seq);
00244     echo->transport_len = (uint16_t)(PICO_ICMPHDR_UN_SIZE + cookie->size);
00245     echo->payload = echo->transport_hdr + PICO_ICMPHDR_UN_SIZE;
00246     echo->payload_len = cookie->size;
00247     /* XXX: Fill payload */
00248     pico_icmp4_checksum(echo);
00249     pico_ipv4_frame_push(echo, &cookie->dst, PICO_PROTO_ICMP4);
00250     return 0;
00251 }
00252 
00253 
00254 static void ping_timeout(pico_time now, void *arg)
00255 {
00256     struct pico_icmp4_ping_cookie *cookie = (struct pico_icmp4_ping_cookie *)arg;
00257     IGNORE_PARAMETER(now);
00258 
00259     if(pico_tree_findKey(&Pings, cookie)) {
00260         if (cookie->err == PICO_PING_ERR_PENDING) {
00261             struct pico_icmp4_stats stats;
00262             stats.dst = cookie->dst;
00263             stats.seq = cookie->seq;
00264             stats.time = 0;
00265             stats.size = cookie->size;
00266             stats.err = PICO_PING_ERR_TIMEOUT;
00267             dbg(" ---- Ping timeout!!!\n");
00268             cookie->cb(&stats);
00269         }
00270 
00271         pico_tree_delete(&Pings, cookie);
00272         PICO_FREE(cookie);
00273     }
00274 }
00275 
00276 static void next_ping(pico_time now, void *arg);
00277 static inline void send_ping(struct pico_icmp4_ping_cookie *cookie)
00278 {
00279     pico_icmp4_send_echo(cookie);
00280     cookie->timestamp = pico_tick;
00281     pico_timer_add((uint32_t)cookie->timeout, ping_timeout, cookie);
00282     if (cookie->seq < (uint16_t)cookie->count)
00283         pico_timer_add((uint32_t)cookie->interval, next_ping, cookie);
00284 }
00285 
00286 static void next_ping(pico_time now, void *arg)
00287 {
00288     struct pico_icmp4_ping_cookie *newcookie, *cookie = (struct pico_icmp4_ping_cookie *)arg;
00289     IGNORE_PARAMETER(now);
00290 
00291     if(pico_tree_findKey(&Pings, cookie)) {
00292         if (cookie->err == PICO_PING_ERR_ABORTED)
00293             return;
00294 
00295         if (cookie->seq < (uint16_t)cookie->count) {
00296             newcookie = PICO_ZALLOC(sizeof(struct pico_icmp4_ping_cookie));
00297             if (!newcookie)
00298                 return;
00299 
00300             memcpy(newcookie, cookie, sizeof(struct pico_icmp4_ping_cookie));
00301             newcookie->seq++;
00302 
00303             pico_tree_insert(&Pings, newcookie);
00304             send_ping(newcookie);
00305         }
00306     }
00307 }
00308 
00309 
00310 static void ping_recv_reply(struct pico_frame *f)
00311 {
00312     struct pico_icmp4_ping_cookie test, *cookie;
00313     struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
00314     test.id  = short_be(hdr->hun.ih_idseq.idseq_id );
00315     test.seq = short_be(hdr->hun.ih_idseq.idseq_seq);
00316 
00317     cookie = pico_tree_findKey(&Pings, &test);
00318     if (cookie) {
00319         struct pico_icmp4_stats stats;
00320         if (cookie->err == PICO_PING_ERR_ABORTED)
00321             return;
00322 
00323         cookie->err = PICO_PING_ERR_REPLIED;
00324         stats.dst = ((struct pico_ipv4_hdr *)f->net_hdr)->src;
00325         stats.seq = cookie->seq;
00326         stats.size = cookie->size;
00327         stats.time = pico_tick - cookie->timestamp;
00328         stats.err = cookie->err;
00329         stats.ttl = ((struct pico_ipv4_hdr *)f->net_hdr)->ttl;
00330         if(cookie->cb != NULL)
00331             cookie->cb(&stats);
00332     } else {
00333         dbg("Reply for seq=%d, not found.\n", test.seq);
00334     }
00335 }
00336 
00337 int pico_icmp4_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *))
00338 {
00339     static uint16_t next_id = 0x91c0;
00340     struct pico_icmp4_ping_cookie *cookie;
00341 
00342     if((dst == NULL) || (interval == 0) || (timeout == 0) || (count == 0)) {
00343         pico_err = PICO_ERR_EINVAL;
00344         return -1;
00345     }
00346 
00347     cookie = PICO_ZALLOC(sizeof(struct pico_icmp4_ping_cookie));
00348     if (!cookie) {
00349         pico_err = PICO_ERR_ENOMEM;
00350         return -1;
00351     }
00352 
00353     if (pico_string_to_ipv4(dst, (uint32_t *)&cookie->dst.addr) < 0) {
00354         pico_err = PICO_ERR_EINVAL;
00355         PICO_FREE(cookie);
00356         return -1;
00357     }
00358 
00359     cookie->seq = 1;
00360     cookie->id = next_id++;
00361     cookie->err = PICO_PING_ERR_PENDING;
00362     cookie->size = (uint16_t)size;
00363     cookie->interval = interval;
00364     cookie->timeout = timeout;
00365     cookie->cb = cb;
00366     cookie->count = count;
00367 
00368     pico_tree_insert(&Pings, cookie);
00369     send_ping(cookie);
00370 
00371     return cookie->id;
00372 
00373 }
00374 
00375 int pico_icmp4_ping_abort(int id)
00376 {
00377     struct pico_tree_node *node;
00378     int found = 0;
00379     pico_tree_foreach(node, &Pings)
00380     {
00381         struct pico_icmp4_ping_cookie *ck =
00382             (struct pico_icmp4_ping_cookie *) node->keyValue;
00383         if (ck->id == (uint16_t)id) {
00384             ck->err = PICO_PING_ERR_ABORTED;
00385             found++;
00386         }
00387     }
00388     if (found > 0)
00389         return 0; /* OK if at least one pending ping has been canceled */
00390 
00391     pico_err = PICO_ERR_ENOENT;
00392     return -1;
00393 }
00394 
00395 #endif