Daniele Lacamera / PicoTCP-Experimental_CDC_ECM_Branch

Free (GPLv2) TCP/IP stack developed by TASS Belgium

Fork of PicoTCP by Daniele Lacamera

modules/pico_icmp4.c@3:b4047e8a0123, 2013-05-24 (annotated)

Committer:
daniele
Date:
Fri May 24 15:25:25 2013 +0000
Revision:
3:b4047e8a0123
Updated from main repo + fixed Mutexes;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniele 3:b4047e8a0123 1 /*********************************************************************
daniele 3:b4047e8a0123 2 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
daniele 3:b4047e8a0123 3 See LICENSE and COPYING for usage.
daniele 3:b4047e8a0123 4
daniele 3:b4047e8a0123 5 .
daniele 3:b4047e8a0123 6
daniele 3:b4047e8a0123 7 Authors: Daniele Lacamera
daniele 3:b4047e8a0123 8 *********************************************************************/
daniele 3:b4047e8a0123 9
daniele 3:b4047e8a0123 10
daniele 3:b4047e8a0123 11 #include "pico_icmp4.h"
daniele 3:b4047e8a0123 12 #include "pico_config.h"
daniele 3:b4047e8a0123 13 #include "pico_ipv4.h"
daniele 3:b4047e8a0123 14 #include "pico_eth.h"
daniele 3:b4047e8a0123 15 #include "pico_device.h"
daniele 3:b4047e8a0123 16 #include "pico_stack.h"
daniele 3:b4047e8a0123 17 #include "pico_tree.h"
daniele 3:b4047e8a0123 18
daniele 3:b4047e8a0123 19 /* Queues */
daniele 3:b4047e8a0123 20 static struct pico_queue icmp_in = {};
daniele 3:b4047e8a0123 21 static struct pico_queue icmp_out = {};
daniele 3:b4047e8a0123 22
daniele 3:b4047e8a0123 23
daniele 3:b4047e8a0123 24 /* Functions */
daniele 3:b4047e8a0123 25
daniele 3:b4047e8a0123 26 static int pico_icmp4_checksum(struct pico_frame *f)
daniele 3:b4047e8a0123 27 {
daniele 3:b4047e8a0123 28 struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
daniele 3:b4047e8a0123 29 if (!hdr) {
daniele 3:b4047e8a0123 30 pico_err = PICO_ERR_EINVAL;
daniele 3:b4047e8a0123 31 return -1;
daniele 3:b4047e8a0123 32 }
daniele 3:b4047e8a0123 33 hdr->crc = 0;
daniele 3:b4047e8a0123 34 hdr->crc = short_be(pico_checksum(hdr, f->transport_len));
daniele 3:b4047e8a0123 35 return 0;
daniele 3:b4047e8a0123 36 }
daniele 3:b4047e8a0123 37
daniele 3:b4047e8a0123 38 #ifdef PICO_SUPPORT_PING
daniele 3:b4047e8a0123 39 static void ping_recv_reply(struct pico_frame *f);
daniele 3:b4047e8a0123 40 #endif
daniele 3:b4047e8a0123 41
daniele 3:b4047e8a0123 42 static int pico_icmp4_process_in(struct pico_protocol *self, struct pico_frame *f)
daniele 3:b4047e8a0123 43 {
daniele 3:b4047e8a0123 44 struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
daniele 3:b4047e8a0123 45 if (hdr->type == PICO_ICMP_ECHO) {
daniele 3:b4047e8a0123 46 hdr->type = PICO_ICMP_ECHOREPLY;
daniele 3:b4047e8a0123 47 /* Ugly, but the best way to get ICMP data size here. */
daniele 3:b4047e8a0123 48 f->transport_len = f->buffer_len - PICO_SIZE_IP4HDR;
daniele 3:b4047e8a0123 49 if (f->dev->eth)
daniele 3:b4047e8a0123 50 f->transport_len -= PICO_SIZE_ETHHDR;
daniele 3:b4047e8a0123 51 pico_icmp4_checksum(f);
daniele 3:b4047e8a0123 52 f->net_hdr = f->transport_hdr - PICO_SIZE_IP4HDR;
daniele 3:b4047e8a0123 53 f->start = f->net_hdr;
daniele 3:b4047e8a0123 54 f->len = f->buffer_len;
daniele 3:b4047e8a0123 55 if (f->dev->eth)
daniele 3:b4047e8a0123 56 f->len -= PICO_SIZE_ETHHDR;
daniele 3:b4047e8a0123 57 pico_ipv4_rebound(f);
daniele 3:b4047e8a0123 58 } else if (hdr->type == PICO_ICMP_UNREACH) {
daniele 3:b4047e8a0123 59 f->net_hdr = f->transport_hdr + PICO_ICMPHDR_UN_SIZE;
daniele 3:b4047e8a0123 60 pico_ipv4_unreachable(f, hdr->code);
daniele 3:b4047e8a0123 61 } else if (hdr->type == PICO_ICMP_ECHOREPLY) {
daniele 3:b4047e8a0123 62 #ifdef PICO_SUPPORT_PING
daniele 3:b4047e8a0123 63 ping_recv_reply(f);
daniele 3:b4047e8a0123 64 #endif
daniele 3:b4047e8a0123 65 pico_frame_discard(f);
daniele 3:b4047e8a0123 66 } else {
daniele 3:b4047e8a0123 67 pico_frame_discard(f);
daniele 3:b4047e8a0123 68 }
daniele 3:b4047e8a0123 69 return 0;
daniele 3:b4047e8a0123 70 }
daniele 3:b4047e8a0123 71
daniele 3:b4047e8a0123 72 static int pico_icmp4_process_out(struct pico_protocol *self, struct pico_frame *f)
daniele 3:b4047e8a0123 73 {
daniele 3:b4047e8a0123 74 dbg("Called %s\n", __FUNCTION__);
daniele 3:b4047e8a0123 75 return 0;
daniele 3:b4047e8a0123 76 }
daniele 3:b4047e8a0123 77
daniele 3:b4047e8a0123 78 /* Interface: protocol definition */
daniele 3:b4047e8a0123 79 struct pico_protocol pico_proto_icmp4 = {
daniele 3:b4047e8a0123 80 .name = "icmp4",
daniele 3:b4047e8a0123 81 .proto_number = PICO_PROTO_ICMP4,
daniele 3:b4047e8a0123 82 .layer = PICO_LAYER_TRANSPORT,
daniele 3:b4047e8a0123 83 .process_in = pico_icmp4_process_in,
daniele 3:b4047e8a0123 84 .process_out = pico_icmp4_process_out,
daniele 3:b4047e8a0123 85 .q_in = &icmp_in,
daniele 3:b4047e8a0123 86 .q_out = &icmp_out,
daniele 3:b4047e8a0123 87 };
daniele 3:b4047e8a0123 88
daniele 3:b4047e8a0123 89 static int pico_icmp4_notify(struct pico_frame *f, uint8_t type, uint8_t code)
daniele 3:b4047e8a0123 90 {
daniele 3:b4047e8a0123 91 struct pico_frame *reply;
daniele 3:b4047e8a0123 92 struct pico_icmp4_hdr *hdr;
daniele 3:b4047e8a0123 93 struct pico_ipv4_hdr *info;
daniele 3:b4047e8a0123 94 if (f == NULL) {
daniele 3:b4047e8a0123 95 pico_err = PICO_ERR_EINVAL;
daniele 3:b4047e8a0123 96 return -1;
daniele 3:b4047e8a0123 97 }
daniele 3:b4047e8a0123 98 reply = pico_proto_ipv4.alloc(&pico_proto_ipv4, 8 + sizeof(struct pico_ipv4_hdr) + PICO_ICMPHDR_UN_SIZE);
daniele 3:b4047e8a0123 99 info = (struct pico_ipv4_hdr*)(f->net_hdr);
daniele 3:b4047e8a0123 100 hdr = (struct pico_icmp4_hdr *) reply->transport_hdr;
daniele 3:b4047e8a0123 101 hdr->type = type;
daniele 3:b4047e8a0123 102 hdr->code = code;
daniele 3:b4047e8a0123 103 hdr->hun.ih_pmtu.ipm_nmtu = short_be(1500);
daniele 3:b4047e8a0123 104 hdr->hun.ih_pmtu.ipm_void = 0;
daniele 3:b4047e8a0123 105 reply->transport_len = 8 + sizeof(struct pico_ipv4_hdr) + PICO_ICMPHDR_UN_SIZE;
daniele 3:b4047e8a0123 106 reply->payload = reply->transport_hdr + PICO_ICMPHDR_UN_SIZE;
daniele 3:b4047e8a0123 107 memcpy(reply->payload, f->net_hdr, 8 + sizeof(struct pico_ipv4_hdr));
daniele 3:b4047e8a0123 108 pico_icmp4_checksum(reply);
daniele 3:b4047e8a0123 109 pico_ipv4_frame_push(reply, &info->src, PICO_PROTO_ICMP4);
daniele 3:b4047e8a0123 110 return 0;
daniele 3:b4047e8a0123 111 }
daniele 3:b4047e8a0123 112
daniele 3:b4047e8a0123 113 int pico_icmp4_port_unreachable(struct pico_frame *f)
daniele 3:b4047e8a0123 114 {
daniele 3:b4047e8a0123 115 /*Parameter check executed in pico_icmp4_notify*/
daniele 3:b4047e8a0123 116 return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PORT);
daniele 3:b4047e8a0123 117 }
daniele 3:b4047e8a0123 118
daniele 3:b4047e8a0123 119 int pico_icmp4_proto_unreachable(struct pico_frame *f)
daniele 3:b4047e8a0123 120 {
daniele 3:b4047e8a0123 121 /*Parameter check executed in pico_icmp4_notify*/
daniele 3:b4047e8a0123 122 return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PROTOCOL);
daniele 3:b4047e8a0123 123 }
daniele 3:b4047e8a0123 124
daniele 3:b4047e8a0123 125 int pico_icmp4_dest_unreachable(struct pico_frame *f)
daniele 3:b4047e8a0123 126 {
daniele 3:b4047e8a0123 127 /*Parameter check executed in pico_icmp4_notify*/
daniele 3:b4047e8a0123 128 return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_HOST);
daniele 3:b4047e8a0123 129 }
daniele 3:b4047e8a0123 130
daniele 3:b4047e8a0123 131 int pico_icmp4_ttl_expired(struct pico_frame *f)
daniele 3:b4047e8a0123 132 {
daniele 3:b4047e8a0123 133 /*Parameter check executed in pico_icmp4_notify*/
daniele 3:b4047e8a0123 134 return pico_icmp4_notify(f, PICO_ICMP_TIME_EXCEEDED, PICO_ICMP_TIMXCEED_INTRANS);
daniele 3:b4047e8a0123 135 }
daniele 3:b4047e8a0123 136
daniele 3:b4047e8a0123 137 #ifdef PICO_SUPPORT_IPFILTER
daniele 3:b4047e8a0123 138 int pico_icmp4_packet_filtered(struct pico_frame *f)
daniele 3:b4047e8a0123 139 {
daniele 3:b4047e8a0123 140 /*Parameter check executed in pico_icmp4_notify*/
daniele 3:b4047e8a0123 141 /*Packet Filtered: type 3, code 13 (Communication Administratively Prohibited)*/
daniele 3:b4047e8a0123 142 return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_FILTER_PROHIB);
daniele 3:b4047e8a0123 143 }
daniele 3:b4047e8a0123 144 #endif
daniele 3:b4047e8a0123 145
daniele 3:b4047e8a0123 146 /***********************/
daniele 3:b4047e8a0123 147 /* Ping implementation */
daniele 3:b4047e8a0123 148 /***********************/
daniele 3:b4047e8a0123 149 /***********************/
daniele 3:b4047e8a0123 150 /***********************/
daniele 3:b4047e8a0123 151 /***********************/
daniele 3:b4047e8a0123 152
daniele 3:b4047e8a0123 153
daniele 3:b4047e8a0123 154 #ifdef PICO_SUPPORT_PING
daniele 3:b4047e8a0123 155
daniele 3:b4047e8a0123 156
daniele 3:b4047e8a0123 157 struct pico_icmp4_ping_cookie
daniele 3:b4047e8a0123 158 {
daniele 3:b4047e8a0123 159 struct pico_ip4 dst;
daniele 3:b4047e8a0123 160 uint16_t err;
daniele 3:b4047e8a0123 161 uint16_t id;
daniele 3:b4047e8a0123 162 uint16_t seq;
daniele 3:b4047e8a0123 163 uint16_t size;
daniele 3:b4047e8a0123 164 int count;
daniele 3:b4047e8a0123 165 unsigned long timestamp;
daniele 3:b4047e8a0123 166 int interval;
daniele 3:b4047e8a0123 167 int timeout;
daniele 3:b4047e8a0123 168 void (*cb)(struct pico_icmp4_stats*);
daniele 3:b4047e8a0123 169
daniele 3:b4047e8a0123 170 };
daniele 3:b4047e8a0123 171
daniele 3:b4047e8a0123 172 static int cookie_compare(void *ka, void *kb)
daniele 3:b4047e8a0123 173 {
daniele 3:b4047e8a0123 174 struct pico_icmp4_ping_cookie *a = ka, *b = kb;
daniele 3:b4047e8a0123 175 if (a->id < b->id)
daniele 3:b4047e8a0123 176 return -1;
daniele 3:b4047e8a0123 177 if (a->id > b->id)
daniele 3:b4047e8a0123 178 return 1;
daniele 3:b4047e8a0123 179 return (a->seq - b->seq);
daniele 3:b4047e8a0123 180 }
daniele 3:b4047e8a0123 181
daniele 3:b4047e8a0123 182 PICO_TREE_DECLARE(Pings,cookie_compare);
daniele 3:b4047e8a0123 183
daniele 3:b4047e8a0123 184 static int pico_icmp4_send_echo(struct pico_icmp4_ping_cookie *cookie)
daniele 3:b4047e8a0123 185 {
daniele 3:b4047e8a0123 186 struct pico_frame *echo = pico_proto_ipv4.alloc(&pico_proto_ipv4, PICO_ICMPHDR_UN_SIZE + cookie->size);
daniele 3:b4047e8a0123 187 struct pico_icmp4_hdr *hdr;
daniele 3:b4047e8a0123 188
daniele 3:b4047e8a0123 189 hdr = (struct pico_icmp4_hdr *) echo->transport_hdr;
daniele 3:b4047e8a0123 190
daniele 3:b4047e8a0123 191 hdr->type = PICO_ICMP_ECHO;
daniele 3:b4047e8a0123 192 hdr->code = 0;
daniele 3:b4047e8a0123 193 hdr->hun.ih_idseq.idseq_id = short_be(cookie->id);
daniele 3:b4047e8a0123 194 hdr->hun.ih_idseq.idseq_seq = short_be(cookie->seq);
daniele 3:b4047e8a0123 195 echo->transport_len = PICO_ICMPHDR_UN_SIZE + cookie->size;
daniele 3:b4047e8a0123 196 echo->payload = echo->transport_hdr + PICO_ICMPHDR_UN_SIZE;
daniele 3:b4047e8a0123 197 echo->payload_len = cookie->size;
daniele 3:b4047e8a0123 198 /* XXX: Fill payload */
daniele 3:b4047e8a0123 199 pico_icmp4_checksum(echo);
daniele 3:b4047e8a0123 200 pico_ipv4_frame_push(echo, &cookie->dst, PICO_PROTO_ICMP4);
daniele 3:b4047e8a0123 201 return 0;
daniele 3:b4047e8a0123 202 }
daniele 3:b4047e8a0123 203
daniele 3:b4047e8a0123 204
daniele 3:b4047e8a0123 205 static void ping_timeout(unsigned long now, void *arg)
daniele 3:b4047e8a0123 206 {
daniele 3:b4047e8a0123 207 struct pico_icmp4_ping_cookie *cookie = (struct pico_icmp4_ping_cookie *)arg;
daniele 3:b4047e8a0123 208 if(pico_tree_findKey(&Pings,cookie)){
daniele 3:b4047e8a0123 209 if (cookie->err == PICO_PING_ERR_PENDING) {
daniele 3:b4047e8a0123 210 struct pico_icmp4_stats stats;
daniele 3:b4047e8a0123 211 stats.dst = cookie->dst;
daniele 3:b4047e8a0123 212 stats.seq = cookie->seq;
daniele 3:b4047e8a0123 213 stats.time = 0;
daniele 3:b4047e8a0123 214 stats.size = cookie->size;
daniele 3:b4047e8a0123 215 stats.err = PICO_PING_ERR_TIMEOUT;
daniele 3:b4047e8a0123 216 dbg(" ---- Ping timeout!!!\n");
daniele 3:b4047e8a0123 217 cookie->cb(&stats);
daniele 3:b4047e8a0123 218 }
daniele 3:b4047e8a0123 219
daniele 3:b4047e8a0123 220 pico_tree_delete(&Pings,cookie);
daniele 3:b4047e8a0123 221 pico_free(cookie);
daniele 3:b4047e8a0123 222 }
daniele 3:b4047e8a0123 223 }
daniele 3:b4047e8a0123 224
daniele 3:b4047e8a0123 225 static void next_ping(unsigned long now, void *arg);
daniele 3:b4047e8a0123 226 static inline void send_ping(struct pico_icmp4_ping_cookie *cookie)
daniele 3:b4047e8a0123 227 {
daniele 3:b4047e8a0123 228 pico_icmp4_send_echo(cookie);
daniele 3:b4047e8a0123 229 cookie->timestamp = pico_tick;
daniele 3:b4047e8a0123 230 pico_timer_add(cookie->timeout, ping_timeout, cookie);
daniele 3:b4047e8a0123 231 pico_timer_add(cookie->interval, next_ping, cookie);
daniele 3:b4047e8a0123 232 }
daniele 3:b4047e8a0123 233
daniele 3:b4047e8a0123 234 static void next_ping(unsigned long now, void *arg)
daniele 3:b4047e8a0123 235 {
daniele 3:b4047e8a0123 236 struct pico_icmp4_ping_cookie *newcookie, *cookie = (struct pico_icmp4_ping_cookie *)arg;
daniele 3:b4047e8a0123 237
daniele 3:b4047e8a0123 238 if(pico_tree_findKey(&Pings,cookie)){
daniele 3:b4047e8a0123 239 if (cookie->seq < cookie->count) {
daniele 3:b4047e8a0123 240 newcookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
daniele 3:b4047e8a0123 241 if (!newcookie)
daniele 3:b4047e8a0123 242 return;
daniele 3:b4047e8a0123 243 memcpy(newcookie, cookie, sizeof(struct pico_icmp4_ping_cookie));
daniele 3:b4047e8a0123 244 newcookie->seq++;
daniele 3:b4047e8a0123 245
daniele 3:b4047e8a0123 246 pico_tree_insert(&Pings,newcookie);
daniele 3:b4047e8a0123 247 send_ping(newcookie);
daniele 3:b4047e8a0123 248 }
daniele 3:b4047e8a0123 249 }
daniele 3:b4047e8a0123 250 }
daniele 3:b4047e8a0123 251
daniele 3:b4047e8a0123 252
daniele 3:b4047e8a0123 253 static void ping_recv_reply(struct pico_frame *f)
daniele 3:b4047e8a0123 254 {
daniele 3:b4047e8a0123 255 struct pico_icmp4_ping_cookie test, *cookie;
daniele 3:b4047e8a0123 256 struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
daniele 3:b4047e8a0123 257 test.id = short_be(hdr->hun.ih_idseq.idseq_id );
daniele 3:b4047e8a0123 258 test.seq = short_be(hdr->hun.ih_idseq.idseq_seq);
daniele 3:b4047e8a0123 259
daniele 3:b4047e8a0123 260 cookie = pico_tree_findKey(&Pings, &test);
daniele 3:b4047e8a0123 261 if (cookie) {
daniele 3:b4047e8a0123 262 struct pico_icmp4_stats stats;
daniele 3:b4047e8a0123 263 cookie->err = PICO_PING_ERR_REPLIED;
daniele 3:b4047e8a0123 264 stats.dst = cookie->dst;
daniele 3:b4047e8a0123 265 stats.seq = cookie->seq;
daniele 3:b4047e8a0123 266 stats.size = cookie->size;
daniele 3:b4047e8a0123 267 stats.time = pico_tick - cookie->timestamp;
daniele 3:b4047e8a0123 268 stats.err = cookie->err;
daniele 3:b4047e8a0123 269 stats.ttl = ((struct pico_ipv4_hdr *)f->net_hdr)->ttl;
daniele 3:b4047e8a0123 270 if(cookie->cb != NULL)
daniele 3:b4047e8a0123 271 cookie->cb(&stats);
daniele 3:b4047e8a0123 272 /* XXX cb */
daniele 3:b4047e8a0123 273 } else {
daniele 3:b4047e8a0123 274 dbg("Reply for seq=%d, not found.\n", test.seq);
daniele 3:b4047e8a0123 275 }
daniele 3:b4047e8a0123 276 }
daniele 3:b4047e8a0123 277
daniele 3:b4047e8a0123 278 int pico_icmp4_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *))
daniele 3:b4047e8a0123 279 {
daniele 3:b4047e8a0123 280 static uint16_t next_id = 0x91c0;
daniele 3:b4047e8a0123 281 struct pico_icmp4_ping_cookie *cookie;
daniele 3:b4047e8a0123 282
daniele 3:b4047e8a0123 283 if((dst == NULL) || (interval == 0) || (timeout == 0) || (count == 0)){
daniele 3:b4047e8a0123 284 pico_err = PICO_ERR_EINVAL;
daniele 3:b4047e8a0123 285 return -1;
daniele 3:b4047e8a0123 286 }
daniele 3:b4047e8a0123 287
daniele 3:b4047e8a0123 288 cookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
daniele 3:b4047e8a0123 289 if (!cookie) {
daniele 3:b4047e8a0123 290 pico_err = PICO_ERR_ENOMEM;
daniele 3:b4047e8a0123 291 return -1;
daniele 3:b4047e8a0123 292 }
daniele 3:b4047e8a0123 293
daniele 3:b4047e8a0123 294 if (pico_string_to_ipv4(dst, &cookie->dst.addr) < 0) {
daniele 3:b4047e8a0123 295 pico_err = PICO_ERR_EINVAL;
daniele 3:b4047e8a0123 296 pico_free(cookie);
daniele 3:b4047e8a0123 297 return -1;
daniele 3:b4047e8a0123 298 }
daniele 3:b4047e8a0123 299 cookie->seq = 1;
daniele 3:b4047e8a0123 300 cookie->id = next_id++;
daniele 3:b4047e8a0123 301 cookie->err = PICO_PING_ERR_PENDING;
daniele 3:b4047e8a0123 302 cookie->size = size;
daniele 3:b4047e8a0123 303 cookie->interval = interval;
daniele 3:b4047e8a0123 304 cookie->timeout = timeout;
daniele 3:b4047e8a0123 305 cookie->cb = cb;
daniele 3:b4047e8a0123 306 cookie->count = count;
daniele 3:b4047e8a0123 307
daniele 3:b4047e8a0123 308 pico_tree_insert(&Pings,cookie);
daniele 3:b4047e8a0123 309 send_ping(cookie);
daniele 3:b4047e8a0123 310
daniele 3:b4047e8a0123 311 return 0;
daniele 3:b4047e8a0123 312
daniele 3:b4047e8a0123 313 }
daniele 3:b4047e8a0123 314
daniele 3:b4047e8a0123 315 #endif