1/********************************************************************* 2 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. 3 See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. 4 5 Authors: Laurens Miers, Daniele Lacamera 6 *********************************************************************/ 7 8 9#include "pico_config.h" 10#ifdef PICO_SUPPORT_IPV6 11#include "pico_ipv6.h" 12#include "pico_icmp6.h" 13#endif 14#ifdef PICO_SUPPORT_IPV4 15#include "pico_ipv4.h" 16#include "pico_icmp4.h" 17#endif 18#include "pico_stack.h" 19#include "pico_eth.h" 20#include "pico_udp.h" 21#include "pico_tcp.h" 22#include "pico_socket.h" 23#include "pico_device.h" 24#include "pico_tree.h" 25#include "pico_constants.h" 26#include "pico_fragments.h" 27 28#ifdef DEBUG_FRAG 29 #define frag_dbg dbg 30#else 31 #define frag_dbg(...) do {} while(0) 32#endif 33 34#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) 35#define IP6_FRAG_OFF(x) ((x & 0xFFF8u)) 36#define IP6_FRAG_MORE(x) ((x & 0x0001)) 37#define IP6_FRAG_ID(x) ((uint32_t)(((uint32_t)x->ext.frag.id[0] << 24) + ((uint32_t)x->ext.frag.id[1] << 16) + \ 38 ((uint32_t)x->ext.frag.id[2] << 8) + (uint32_t)x->ext.frag.id[3])) 39 40#else 41#define IP6_FRAG_OFF(x) (0) 42#define IP6_FRAG_MORE(x) (0) 43#define IP6_FRAG_ID(x) (0) 44#endif 45 46#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) 47#define IP4_FRAG_OFF(frag) (((uint32_t)frag & PICO_IPV4_FRAG_MASK) << 3ul) 48#define IP4_FRAG_MORE(frag) ((frag & PICO_IPV4_MOREFRAG) ? 1 : 0) 49#define IP4_FRAG_ID(hdr) (hdr->id) 50#else 51#define IP4_FRAG_OFF(frag) (0) 52#define IP4_FRAG_MORE(frag) (0) 53#define IP4_FRAG_ID(hdr) (0) 54#endif 55 56#define PICO_IPV6_FRAG_TIMEOUT 60000 57#define PICO_IPV4_FRAG_TIMEOUT 15000 58 59static void pico_frag_expire(pico_time now, void *arg); 60static void pico_fragments_complete(unsigned int bookmark, uint8_t proto, uint8_t net); 61static int pico_fragments_check_complete(struct pico_tree *tree, uint8_t proto, uint8_t net); 62static int pico_fragments_reassemble(struct pico_tree *tree, unsigned int len, uint8_t proto, uint8_t net); 63static int pico_fragments_get_more_flag(struct pico_frame *frame, uint8_t net); 64static uint32_t pico_fragments_get_offset(struct pico_frame *frame, uint8_t net); 65static void pico_fragments_send_notify(struct pico_frame *first); 66static uint16_t pico_fragments_get_header_length(uint8_t net); 67static void pico_fragments_empty_tree(struct pico_tree *tree); 68 69#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) 70static uint32_t ipv6_cur_frag_id = 0u; 71static uint32_t ipv6_fragments_timer = 0u; 72 73static int pico_ipv6_frag_compare(void *ka, void *kb) 74{ 75 struct pico_frame *a = ka, *b = kb; 76 if (IP6_FRAG_OFF(a->frag) > IP6_FRAG_OFF(b->frag)) 77 return 1; 78 79 if (IP6_FRAG_OFF(a->frag) < IP6_FRAG_OFF(b->frag)) 80 return -1; 81 82 return 0; 83} 84static PICO_TREE_DECLARE(ipv6_fragments, pico_ipv6_frag_compare); 85 86static void pico_ipv6_fragments_complete(unsigned int len, uint8_t proto) 87{ 88 if (pico_fragments_reassemble(&ipv6_fragments, len, proto, PICO_PROTO_IPV6) == 0) 89 { 90 pico_timer_cancel(ipv6_fragments_timer); 91 ipv6_fragments_timer = 0; 92 } 93} 94 95static void pico_ipv6_frag_timer_on(void) 96{ 97 ipv6_fragments_timer = pico_timer_add(PICO_IPV6_FRAG_TIMEOUT, pico_frag_expire, &ipv6_fragments); 98 if (!ipv6_fragments_timer) { 99 frag_dbg("FRAG: Failed to start IPv6 expiration timer\n"); 100 pico_fragments_empty_tree(&ipv6_fragments); 101 } 102} 103 104static int pico_ipv6_frag_match(struct pico_frame *a, struct pico_frame *b) 105{ 106 struct pico_ipv6_hdr *ha = NULL, *hb = NULL; 107 if (!a || !b) 108 return -1; 109 110 ha = (struct pico_ipv6_hdr *)a->net_hdr; 111 hb = (struct pico_ipv6_hdr *)b->net_hdr; 112 if (!ha || !hb) 113 return -2; 114 115 if (memcmp(ha->src.addr, hb->src.addr, PICO_SIZE_IP6) != 0) 116 return 1; 117 118 if (memcmp(ha->dst.addr, hb->dst.addr, PICO_SIZE_IP6) != 0) 119 return 2; 120 121 return 0; 122} 123#endif 124 125#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) 126static uint32_t ipv4_cur_frag_id = 0u; 127static uint32_t ipv4_fragments_timer = 0u; 128 129static int pico_ipv4_frag_compare(void *ka, void *kb) 130{ 131 struct pico_frame *a = ka, *b = kb; 132 if (IP4_FRAG_OFF(a->frag) > IP4_FRAG_OFF(b->frag)) 133 return 1; 134 135 if (IP4_FRAG_OFF(a->frag) < IP4_FRAG_OFF(b->frag)) 136 return -1; 137 138 return 0; 139} 140static PICO_TREE_DECLARE(ipv4_fragments, pico_ipv4_frag_compare); 141 142static void pico_ipv4_fragments_complete(unsigned int len, uint8_t proto) 143{ 144 if (pico_fragments_reassemble(&ipv4_fragments, len, proto, PICO_PROTO_IPV4) == 0) 145 { 146 pico_timer_cancel(ipv4_fragments_timer); 147 ipv4_fragments_timer = 0; 148 } 149} 150 151static void pico_ipv4_frag_timer_on(void) 152{ 153 ipv4_fragments_timer = pico_timer_add( PICO_IPV4_FRAG_TIMEOUT, pico_frag_expire, &ipv4_fragments); 154 if (!ipv4_fragments_timer) { 155 frag_dbg("FRAG: Failed to start IPv4 expiration timer\n"); 156 pico_fragments_empty_tree(&ipv4_fragments); 157 } 158} 159 160static int pico_ipv4_frag_match(struct pico_frame *a, struct pico_frame *b) 161{ 162 struct pico_ipv4_hdr *ha, *hb; 163 if (!a || !b) 164 return -1; 165 166 ha = (struct pico_ipv4_hdr *)a->net_hdr; 167 hb = (struct pico_ipv4_hdr *)b->net_hdr; 168 if (!ha || !hb) 169 return -2; 170 171 if (memcmp(&(ha->src.addr), &(hb->src.addr), PICO_SIZE_IP4) != 0) 172 return 1; 173 174 if (memcmp(&(ha->dst.addr), &(hb->dst.addr), PICO_SIZE_IP4) != 0) 175 return 2; 176 177 return 0; 178} 179#endif 180 181static void pico_fragments_complete(unsigned int bookmark, uint8_t proto, uint8_t net) 182{ 183 if (0) {} 184 185#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) 186 else if (net == PICO_PROTO_IPV4) 187 { 188 pico_ipv4_fragments_complete(bookmark, proto); 189 } 190#endif 191#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) 192 else if (net == PICO_PROTO_IPV6) 193 { 194 pico_ipv6_fragments_complete(bookmark, proto); 195 } 196#endif 197} 198 199static void pico_fragments_empty_tree(struct pico_tree *tree) 200{ 201 struct pico_tree_node *index, *tmp; 202 203 if (!tree) 204 { 205 return; 206 } 207 208 pico_tree_foreach_safe(index, tree, tmp) { 209 struct pico_frame * old = index->keyValue; 210 pico_tree_delete(tree, old); 211 pico_frame_discard(old); 212 } 213 214} 215 216static int pico_fragments_check_complete(struct pico_tree *tree, uint8_t proto, uint8_t net) 217{ 218 struct pico_tree_node *index, *temp; 219 struct pico_frame *cur; 220 unsigned int bookmark = 0; 221 222 if (!tree) 223 return 0; 224 225 pico_tree_foreach_safe(index, tree, temp) { 226 cur = index->keyValue; 227 if (cur) { 228 if (pico_fragments_get_offset(cur, net) != bookmark) 229 return -1; 230 231 bookmark += cur->transport_len; 232 if (!pico_fragments_get_more_flag(cur, net)) { 233 pico_fragments_complete(bookmark, proto, net); 234 return 0; 235 } 236 } 237 } 238 return 1; 239} 240 241static void pico_frag_expire(pico_time now, void *arg) 242{ 243 struct pico_tree *tree = (struct pico_tree *) arg; 244 struct pico_frame *first = NULL; 245 IGNORE_PARAMETER(now); 246 247 if (!tree) 248 { 249 frag_dbg("Expired packet but no tree supplied!\n"); 250 return; 251 } 252 253 first = pico_tree_first(tree); 254 255 if (!first) { 256 frag_dbg("Empty tree - not sending notify\n"); 257 return; 258 } 259 260 pico_fragments_send_notify(first); 261 262 pico_fragments_empty_tree(tree); 263} 264 265static void pico_fragments_send_notify(struct pico_frame *first) 266{ 267 uint8_t net = 0; 268 269 if (!first) 270 { 271 return; 272 } 273 274 if (0) {} 275 276#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) 277 else if (IS_IPV4(first)) 278 { 279 net = PICO_PROTO_IPV4; 280 frag_dbg("Packet expired! ID:%hu\n", ipv4_cur_frag_id); 281 } 282 283#endif 284#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) 285 else if (IS_IPV6(first)) 286 { 287 net = PICO_PROTO_IPV6; 288 frag_dbg("Packet expired! ID:%hu\n", ipv6_cur_frag_id); 289 } 290 291#endif 292 293 if (((pico_fragments_get_offset(first, net) == 0) && (pico_frame_dst_is_unicast(first)))) 294 { 295 frag_dbg("sending notify\n"); 296 pico_notify_frag_expired(first); 297 } 298 else 299 { 300 frag_dbg("Not first packet or not unicast address, not sending notify"); 301 } 302} 303 304static int pico_fragments_reassemble(struct pico_tree *tree, unsigned int len, uint8_t proto, uint8_t net) 305{ 306 struct pico_tree_node *index, *tmp; 307 struct pico_frame *f; 308 uint16_t header_length = 0; 309 unsigned int bookmark = 0; 310 struct pico_frame *full = NULL; 311 struct pico_frame *first = NULL; 312 313 if (!tree) 314 { 315 frag_dbg("Cannot reassemble packet, no tree supplied!\n"); 316 return -1; 317 } 318 319 first = pico_tree_first(tree); 320 321 if (!first) 322 { 323 frag_dbg("Cannot reassemble packet, empty tree supplied!\n"); 324 return -2; 325 } 326 327 header_length = pico_fragments_get_header_length(net); 328 329 if (!header_length) 330 { 331 return -3; 332 } 333 334 full = pico_frame_alloc((uint16_t)(header_length + len)); 335 if (full) { 336 full->net_hdr = full->buffer; 337 full->net_len = header_length; 338 memcpy(full->net_hdr, first->net_hdr, full->net_len); 339 full->transport_hdr = full->net_hdr + full->net_len; 340 full->transport_len = (uint16_t)len; 341 full->dev = first->dev; 342 pico_tree_foreach_safe(index, tree, tmp) { 343 f = index->keyValue; 344 memcpy(full->transport_hdr + bookmark, f->transport_hdr, f->transport_len); 345 bookmark += f->transport_len; 346 pico_tree_delete(tree, f); 347 pico_frame_discard(f); 348 } 349 if (pico_transport_receive(full, proto) == -1) 350 { 351 pico_frame_discard(full); 352 } 353 354 return 0; 355 } 356 357 return 1; 358} 359 360static uint16_t pico_fragments_get_header_length(uint8_t net) 361{ 362 if (0) {} 363 364#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) 365 else if (net == PICO_PROTO_IPV4) 366 { 367 return PICO_SIZE_IP4HDR; 368 } 369#endif 370#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) 371 else if (net == PICO_PROTO_IPV6) 372 { 373 return PICO_SIZE_IP6HDR; 374 } 375#endif 376 377 return 0; 378} 379 380static int pico_fragments_get_more_flag(struct pico_frame *frame, uint8_t net) 381{ 382 if (!frame) 383 { 384 frag_dbg("no frame given to determine more flag\n"); 385 return 0; 386 } 387 388 if (0) {} 389 390#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) 391 else if (net == PICO_PROTO_IPV4) 392 { 393 return IP4_FRAG_MORE(frame->frag); 394 } 395#endif 396#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) 397 else if (net == PICO_PROTO_IPV6) 398 { 399 return IP6_FRAG_MORE(frame->frag); 400 } 401#endif 402 403 return 0; 404} 405 406static uint32_t pico_fragments_get_offset(struct pico_frame *frame, uint8_t net) 407{ 408 if (!frame) 409 { 410 frag_dbg("no frame given to determine offset\n"); 411 return 0; 412 } 413 414 if (0) {} 415 416#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) 417 else if (net == PICO_PROTO_IPV4) 418 { 419 return IP4_FRAG_OFF(frame->frag); 420 } 421#endif 422#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) 423 else if (net == PICO_PROTO_IPV6) 424 { 425 return IP6_FRAG_OFF(frame->frag); 426 } 427#endif 428 429 return 0; 430} 431 432void pico_ipv6_process_frag(struct pico_ipv6_exthdr *frag, struct pico_frame *f, uint8_t proto) 433{ 434#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG) 435 struct pico_frame *first = NULL; 436 437 if (!f || !frag) 438 { 439 frag_dbg("Bad arguments provided to pico_ipv6_process_frag\n"); 440 return; 441 } 442 443 first = pico_tree_first(&ipv6_fragments); 444 445 if (first) 446 { 447 if ((pico_ipv6_frag_match(f, first) == 0 && (IP6_FRAG_ID(frag) == ipv6_cur_frag_id))) { 448 struct pico_frame *temp = NULL; 449 450 temp = pico_frame_copy(f); 451 452 if (!temp) { 453 frag_dbg("Could not allocate memory to continue reassembly of IPV6 fragmented packet (id: %hu)\n", ipv6_cur_frag_id); 454 return; 455 } 456 457 if (pico_tree_insert(&ipv6_fragments, temp)) { 458 frag_dbg("FRAG: Could not insert picoframe in tree\n"); 459 pico_frame_discard(temp); 460 return; 461 } 462 } 463 } 464 else 465 { 466 struct pico_frame *temp = NULL; 467 468 if (ipv6_cur_frag_id && (IP6_FRAG_ID(frag) == ipv6_cur_frag_id)) { 469 /* Discard late arrivals, without firing the timer. */ 470 frag_dbg("discarded late arrival, exp:%hu found:%hu\n", ipv6_cur_frag_id, IP6_FRAG_ID(frag)); 471 return; 472 } 473 474 temp = pico_frame_copy(f); 475 476 if (!temp) { 477 frag_dbg("Could not allocate memory to start reassembly of fragmented packet\n"); 478 return; 479 } 480 481 pico_ipv6_frag_timer_on(); 482 ipv6_cur_frag_id = IP6_FRAG_ID(frag); 483 frag_dbg("Started new reassembly, ID:%hu\n", ipv6_cur_frag_id); 484 485 if (pico_tree_insert(&ipv6_fragments, temp)) { 486 frag_dbg("FRAG: Could not insert picoframe in tree\n"); 487 pico_frame_discard(temp); 488 return; 489 } 490 } 491 492 pico_fragments_check_complete(&ipv6_fragments, proto, PICO_PROTO_IPV6); 493#else 494 IGNORE_PARAMETER(frag); 495 IGNORE_PARAMETER(f); 496 IGNORE_PARAMETER(proto); 497#endif 498} 499 500void pico_ipv4_process_frag(struct pico_ipv4_hdr *hdr, struct pico_frame *f, uint8_t proto) 501{ 502#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG) 503 struct pico_frame *first = NULL; 504 505 if (!f || !hdr) 506 { 507 frag_dbg("Bad arguments provided to pico_ipv4_process_frag\n"); 508 return; 509 } 510 511 first = pico_tree_first(&ipv4_fragments); 512 513 if (first) 514 { 515 /* fragments from old packets still in tree, and new first fragment ? */ 516 if ((IP4_FRAG_ID(hdr) != ipv4_cur_frag_id) && (IP4_FRAG_OFF(f->frag) == 0)) { 517 pico_fragments_empty_tree(&ipv4_fragments); 518 519 first = NULL; 520 ipv4_cur_frag_id = 0; 521 } 522 523 if ((pico_ipv4_frag_match(f, first) == 0 && (IP4_FRAG_ID(hdr) == ipv4_cur_frag_id))) { 524 struct pico_frame *temp = NULL; 525 526 temp = pico_frame_copy(f); 527 528 if (!temp) { 529 frag_dbg("Could not allocate memory to continue reassembly of IPV4 fragmented packet (id: %hu)\n", ipv4_cur_frag_id); 530 return; 531 } 532 533 if (pico_tree_insert(&ipv4_fragments, temp)) { 534 frag_dbg("FRAG: Could not insert picoframe in tree\n"); 535 pico_frame_discard(temp); 536 return; 537 } 538 } 539 } 540 else 541 { 542 struct pico_frame *temp = NULL; 543 544 if (ipv4_cur_frag_id && (IP4_FRAG_ID(hdr) == ipv4_cur_frag_id)) { 545 /* Discard late arrivals, without firing the timer */ 546 return; 547 } 548 549 temp = pico_frame_copy(f); 550 551 if (!temp) { 552 frag_dbg("Could not allocate memory to start reassembly fragmented packet\n"); 553 return; 554 } 555 556 pico_ipv4_frag_timer_on(); 557 ipv4_cur_frag_id = IP4_FRAG_ID(hdr); 558 frag_dbg("Started new reassembly, ID:%hu\n", ipv4_cur_frag_id); 559 560 if (pico_tree_insert(&ipv4_fragments, temp)) { 561 frag_dbg("FRAG: Could not insert picoframe in tree\n"); 562 pico_frame_discard(temp); 563 return; 564 } 565 } 566 567 pico_fragments_check_complete(&ipv4_fragments, proto, PICO_PROTO_IPV4); 568#else 569 IGNORE_PARAMETER(hdr); 570 IGNORE_PARAMETER(f); 571 IGNORE_PARAMETER(proto); 572#endif 573} 574