Daniele Lacamera / USB_Ethernet

CDC/ECM driver for mbed, based on USBDevice by mbed-official. Uses PicoTCP to access Ethernet USB device. License: GPLv2

Dependents:   USBEthernet_TEST

Fork of USB_Ethernet by Daniele Lacamera

modules/pico_icmp4.c@2:540f6e142d59, 2013-08-03 (annotated)

Committer:
daniele
Date:
Sat Aug 03 13:16:14 2013 +0000
Revision:
2:540f6e142d59
Moved to single package

Who changed what in which revision?

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