1 /* Copyright (C) 2007-2008 The Android Open Source Project 2 ** 3 ** This software is licensed under the terms of the GNU General Public 4 ** License version 2, as published by the Free Software Foundation, and 5 ** may be copied, distributed, and modified under those terms. 6 ** 7 ** This program is distributed in the hope that it will be useful, 8 ** but WITHOUT ANY WARRANTY; without even the implied warranty of 9 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 ** GNU General Public License for more details. 11 */ 12 #include "shaper.h" 13 #include "qemu-common.h" 14 #include "qemu-timer.h" 15 #include <stdlib.h> 16 17 #define SHAPER_CLOCK rt_clock 18 #define SHAPER_CLOCK_UNIT 1000. 19 20 static int 21 _packet_is_internal( const uint8_t* data, size_t size ) 22 { 23 const uint8_t* end = data + size; 24 25 /* must have room for Mac + IP header */ 26 if (data + 40 > end) 27 return 0; 28 29 if (data[12] != 0x08 || data[13] != 0x00 ) 30 return 0; 31 32 /* must have valid IP header */ 33 data += 14; 34 if ((data[0] >> 4) != 4 || (data[0] & 15) < 5) 35 return 0; 36 37 /* internal if both source and dest addresses are in 10.x.x.x */ 38 return ( data[12] == 10 && data[16] == 10); 39 } 40 41 /* here's how we implement network shaping. we want to limit the network 42 * rate to a given constant MAX_RATE expressed as bits/second. this means 43 * that it takes 1/MAX_RATE seconds to send a single bit, and count*8/MAX_RATE 44 * seconds to send 'count' bytes. 45 * 46 * we're going to implement a scheme where, when we send a packet of 47 * 'count' bytes, no other packet will go through in the same direction for 48 * at least 'count*8/MAX_RATE' seconds. any successive packet that is "sent" 49 * in this interval is placed in a queue, associated to a timer 50 * 51 * there are different (queue/timer/rate) values for the input and output 52 * direction of the user vlan. 53 */ 54 typedef struct QueuedPacketRec_ { 55 int64_t expiration; 56 struct QueuedPacketRec_* next; 57 size_t size; 58 void* opaque; 59 void* data; 60 } QueuedPacketRec, *QueuedPacket; 61 62 63 static QueuedPacket 64 queued_packet_create( const void* data, 65 size_t size, 66 void* opaque, 67 int do_copy ) 68 { 69 QueuedPacket packet; 70 size_t packet_size = sizeof(*packet); 71 72 if (do_copy) 73 packet_size += size; 74 75 packet = qemu_malloc(packet_size); 76 packet->next = NULL; 77 packet->expiration = 0; 78 packet->size = (size_t)size; 79 packet->opaque = opaque; 80 81 if (do_copy) { 82 packet->data = (void*)(packet+1); 83 memcpy( (char*)packet->data, (char*)data, packet->size ); 84 } else { 85 packet->data = (void*)data; 86 } 87 return packet; 88 } 89 90 static void 91 queued_packet_free( QueuedPacket packet ) 92 { 93 if (packet) { 94 qemu_free( packet ); 95 } 96 } 97 98 typedef struct NetShaperRec_ { 99 QueuedPacket packets; /* list of queued packets, ordered by expiration date */ 100 int num_packets; 101 int active; /* is this shaper active ? */ 102 int64_t block_until; 103 double max_rate; /* max rate expressed in bytes/second */ 104 double inv_rate; /* inverse of max rate */ 105 QEMUTimer* timer; /* QEMU timer */ 106 107 int do_copy; 108 NetShaperSendFunc send_func; 109 110 } NetShaperRec; 111 112 113 void 114 netshaper_destroy( NetShaper shaper ) 115 { 116 if (shaper) { 117 shaper->active = 0; 118 119 while (shaper->packets) { 120 QueuedPacket packet = shaper->packets; 121 shaper->packets = packet->next; 122 packet->next = NULL; 123 queued_packet_free(packet); 124 } 125 126 qemu_del_timer(shaper->timer); 127 qemu_free_timer(shaper->timer); 128 shaper->timer = NULL; 129 qemu_free(shaper); 130 } 131 } 132 133 /* this function is called when the shaper's timer expires */ 134 static void 135 netshaper_expires( NetShaper shaper ) 136 { 137 QueuedPacket packet; 138 139 while ((packet = shaper->packets) != NULL) { 140 int64_t now = qemu_get_clock_ms( SHAPER_CLOCK ); 141 142 if (packet->expiration > now) 143 break; 144 145 shaper->packets = packet->next; 146 shaper->send_func( packet->data, packet->size, packet->opaque ); 147 queued_packet_free(packet); 148 shaper->num_packets--; 149 } 150 151 /* reprogram timer if needed */ 152 if (shaper->packets) { 153 shaper->block_until = shaper->packets->expiration; 154 qemu_mod_timer( shaper->timer, shaper->block_until ); 155 } else { 156 shaper->block_until = -1; 157 } 158 } 159 160 161 NetShaper 162 netshaper_create( int do_copy, 163 NetShaperSendFunc send_func ) 164 { 165 NetShaper shaper = qemu_malloc(sizeof(*shaper)); 166 167 shaper->active = 0; 168 shaper->packets = NULL; 169 shaper->num_packets = 0; 170 shaper->timer = qemu_new_timer_ms( SHAPER_CLOCK, 171 (QEMUTimerCB*) netshaper_expires, 172 shaper ); 173 shaper->send_func = send_func; 174 shaper->max_rate = 1e6; 175 shaper->inv_rate = 0.; 176 177 shaper->block_until = -1; /* magic value, means to not block */ 178 179 return shaper; 180 } 181 182 void 183 netshaper_set_rate( NetShaper shaper, 184 double rate ) 185 { 186 /* send all current packets when changing the rate */ 187 while (shaper->packets) { 188 QueuedPacket packet = shaper->packets; 189 shaper->packets = packet->next; 190 shaper->send_func(packet->data, packet->size, packet->opaque); 191 qemu_free(packet); 192 shaper->num_packets = 0; 193 } 194 195 shaper->max_rate = rate; 196 if (rate > 1.) { 197 shaper->inv_rate = (8.*SHAPER_CLOCK_UNIT)/rate; /* qemu_get_clock returns time in ms */ 198 shaper->active = 1; /* for the real-time clock */ 199 } else { 200 shaper->active = 0; 201 } 202 203 shaper->block_until = -1; 204 } 205 206 void 207 netshaper_send_aux( NetShaper shaper, 208 void* data, 209 size_t size, 210 void* opaque ) 211 { 212 int64_t now; 213 214 if (!shaper->active || _packet_is_internal(data, size)) { 215 shaper->send_func( data, size, opaque ); 216 return; 217 } 218 219 now = qemu_get_clock_ms( SHAPER_CLOCK ); 220 if (now >= shaper->block_until) { 221 shaper->send_func( data, size, opaque ); 222 shaper->block_until = now + size*shaper->inv_rate; 223 //fprintf(stderr, "NETSHAPER: block for %.2fms\n", (shaper->block_until - now)*1.0 ); 224 return; 225 } 226 227 /* create new packet, add it to the queue */ 228 { 229 QueuedPacket packet; 230 231 packet = queued_packet_create( data, size, opaque, shaper->do_copy ); 232 233 packet->expiration = shaper->block_until; 234 235 { 236 QueuedPacket *pnode, node; 237 238 pnode = &shaper->packets; 239 for (;;) { 240 node = *pnode; 241 if (node == NULL || node->expiration > packet->expiration ) 242 break; 243 pnode = &node->next; 244 } 245 packet->next = *pnode; 246 *pnode = packet; 247 248 if (packet == shaper->packets) 249 qemu_mod_timer( shaper->timer, packet->expiration ); 250 } 251 shaper->num_packets += 1; 252 } 253 shaper->block_until += size*shaper->inv_rate; 254 //fprintf(stderr, "NETSHAPER: block2 for %.2fms\n", (shaper->block_until - now)*1.0 ); 255 } 256 257 void 258 netshaper_send( NetShaper shaper, 259 void* data, 260 size_t size ) 261 { 262 netshaper_send_aux(shaper, data, size, NULL); 263 } 264 265 266 int 267 netshaper_can_send( NetShaper shaper ) 268 { 269 int64_t now; 270 271 if (!shaper->active || shaper->block_until < 0) 272 return 1; 273 274 if (shaper->packets) 275 return 0; 276 277 now = qemu_get_clock_ms( SHAPER_CLOCK ); 278 return (now >= shaper->block_until); 279 } 280 281 282 283 284 285 286 /* this type is used to model a session connection/state 287 * if session->packet is != NULL, then the connection is delayed 288 */ 289 typedef struct SessionRec_ { 290 int64_t expiration; 291 struct SessionRec_* next; 292 unsigned src_ip; 293 unsigned dst_ip; 294 unsigned short src_port; 295 unsigned short dst_port; 296 uint8_t protocol; 297 QueuedPacket packet; 298 299 } SessionRec, *Session; 300 301 #define _PROTOCOL_TCP 6 302 #define _PROTOCOL_UDP 17 303 304 305 306 static void 307 session_free( Session session ) 308 { 309 if (session) { 310 if (session->packet) { 311 queued_packet_free(session->packet); 312 session->packet = NULL; 313 } 314 qemu_free( session ); 315 } 316 } 317 318 319 #if 0 /* useful for debugging */ 320 static const char* 321 session_to_string( Session session ) 322 { 323 static char temp[256]; 324 const char* format = (session->protocol == _PROTOCOL_TCP) ? "TCP" : "UDP"; 325 sprintf( temp, "%s[%d.%d.%d.%d:%d / %d.%d.%d.%d:%d]", format, 326 (session->src_ip >> 24) & 255, (session->src_ip >> 16) & 255, 327 (session->src_ip >> 8) & 255, (session->src_ip) & 255, session->src_port, 328 (session->dst_ip >> 24) & 255, (session->dst_ip >> 16) & 255, 329 (session->dst_ip >> 8) & 255, (session->dst_ip) & 255, session->dst_port); 330 331 return temp; 332 } 333 #endif 334 335 /* returns TRUE if this corresponds to a SYN packet */ 336 int 337 _packet_SYN_flags( const void* _data, size_t size, Session info ) 338 { 339 const uint8_t* data = (const uint8_t*)_data; 340 const uint8_t* end = data + size; 341 342 /* enough room for a Ethernet MAC packet ? */ 343 if (data + 14 > end - 4) 344 return 0; 345 346 /* is it an IP packet ? */ 347 if (data[12] != 0x8 || data[13] != 0) 348 return 0; 349 350 data += 14; 351 end -= 4; 352 353 if (data + 20 > end) 354 return 0; 355 356 /* IP version must be 4, and the header length in words at least 5 */ 357 if ((data[0] & 0xF) < 5 || (data[0] >> 4) != 4) 358 return 0; 359 360 /* time-to-live must be > 0 */ 361 if (data[8] == 0) 362 return 0; 363 364 /* must be TCP or UDP packet */ 365 if (data[9] != _PROTOCOL_TCP && data[9] != _PROTOCOL_UDP) 366 return 0; 367 368 info->protocol = data[9]; 369 info->src_ip = (data[12] << 24) | (data[13] << 16) | (data[14] << 8) | data[15]; 370 info->dst_ip = (data[16] << 24) | (data[17] << 16) | (data[18] << 8) | data[19]; 371 372 data += 4*(data[0] & 15); 373 if (data + 20 > end) 374 return 0; 375 376 info->src_port = (unsigned short)((data[0] << 8) | data[1]); 377 info->dst_port = (unsigned short)((data[2] << 8) | data[3]); 378 379 return (data[13] & 0x1f); 380 } 381 382 383 typedef struct NetDelayRec_ 384 { 385 Session sessions; 386 int num_sessions; 387 QEMUTimer* timer; 388 int active; 389 int min_ms; 390 int max_ms; 391 392 NetShaperSendFunc send_func; 393 394 } NetDelayRec; 395 396 397 static Session* 398 netdelay_lookup_session( NetDelay delay, Session info ) 399 { 400 Session* pnode = &delay->sessions; 401 Session node; 402 403 for (;;) { 404 node = *pnode; 405 if (node == NULL) 406 break; 407 408 if (node->src_ip == info->src_ip && 409 node->dst_ip == info->dst_ip && 410 node->src_port == info->src_port && 411 node->dst_port == info->dst_port && 412 node->protocol == info->protocol ) 413 break; 414 415 pnode = &node->next; 416 } 417 return pnode; 418 } 419 420 421 422 /* called by the delay's timer on expiration */ 423 static void 424 netdelay_expires( NetDelay delay ) 425 { 426 Session session; 427 int64_t now = qemu_get_clock_ms( SHAPER_CLOCK ); 428 int rearm = 0; 429 int64_t rearm_time = 0; 430 431 for (session = delay->sessions; session != NULL; session = session->next) 432 { 433 QueuedPacket packet = session->packet; 434 435 if (packet == NULL) 436 continue; 437 438 if (session->expiration <= now) { 439 /* send the SYN packet now */ 440 //fprintf(stderr, "NetDelay:RST: sending creation for %s\n", session_to_string(session) ); 441 delay->send_func( packet->data, packet->size, packet->opaque ); 442 session->packet = NULL; 443 queued_packet_free( packet ); 444 } else { 445 if (!rearm) { 446 rearm = 1; 447 rearm_time = session->expiration; 448 } 449 else if ( session->expiration < rearm_time ) 450 rearm_time = session->expiration; 451 } 452 } 453 454 if (rearm) 455 qemu_mod_timer( delay->timer, rearm_time ); 456 } 457 458 459 NetDelay 460 netdelay_create( NetShaperSendFunc send_func ) 461 { 462 NetDelay delay = qemu_malloc(sizeof(*delay)); 463 464 delay->sessions = NULL; 465 delay->num_sessions = 0; 466 delay->timer = qemu_new_timer_ms( SHAPER_CLOCK, 467 (QEMUTimerCB*) netdelay_expires, 468 delay ); 469 delay->active = 0; 470 delay->min_ms = 0; 471 delay->max_ms = 0; 472 473 delay->send_func = send_func; 474 475 return delay; 476 } 477 478 479 void 480 netdelay_set_latency( NetDelay delay, int min_ms, int max_ms ) 481 { 482 /* when changing the latency, accept all sessions */ 483 while (delay->sessions) { 484 Session session = delay->sessions; 485 delay->sessions = session->next; 486 session->next = NULL; 487 if (session->packet) { 488 QueuedPacket packet = session->packet; 489 delay->send_func( packet->data, packet->size, packet->opaque ); 490 } 491 session_free(session); 492 delay->num_sessions--; 493 } 494 495 delay->min_ms = min_ms; 496 delay->max_ms = max_ms; 497 delay->active = (min_ms <= max_ms) && min_ms > 0; 498 } 499 500 void 501 netdelay_send( NetDelay delay, const void* data, size_t size ) 502 { 503 netdelay_send_aux(delay, data, size, NULL); 504 } 505 506 507 void 508 netdelay_send_aux( NetDelay delay, const void* data, size_t size, void* opaque ) 509 { 510 if (delay->active && !_packet_is_internal(data, size)) { 511 SessionRec info[1]; 512 int flags; 513 514 flags = _packet_SYN_flags( data, size, info ); 515 if ((flags & 0x05) != 0) 516 { /* FIN or RST: drop connection */ 517 Session* lookup = netdelay_lookup_session( delay, info ); 518 Session session = *lookup; 519 if (session != NULL) { 520 //fprintf(stderr, "NetDelay:RST: dropping %s\n", session_to_string(info) ); 521 522 *lookup = session->next; 523 session_free( session ); 524 delay->num_sessions -= 1; 525 } 526 } 527 else if ((flags & 0x12) == 0x02) 528 { 529 /* SYN: create connection */ 530 Session* lookup = netdelay_lookup_session( delay, info ); 531 Session session = *lookup; 532 533 if (session != NULL) { 534 if (session->packet != NULL) { 535 /* this is a SYN re-transmission, since we didn't 536 * send the original SYN packet yet, just eat this one 537 */ 538 //fprintf(stderr, "NetDelay:RST: swallow SYN re-send for %s\n", session_to_string(info) ); 539 return; 540 } 541 } else { 542 /* establish a new session slightly in the future */ 543 int latency = delay->min_ms; 544 int range = delay->max_ms - delay->min_ms; 545 546 if (range > 0) 547 latency += rand() % range; 548 549 //fprintf(stderr, "NetDelay:RST: delay creation for %s\n", session_to_string(info) ); 550 session = qemu_malloc( sizeof(*session) ); 551 552 session->next = delay->sessions; 553 delay->sessions = session; 554 delay->num_sessions += 1; 555 556 session->expiration = qemu_get_clock_ms( SHAPER_CLOCK ) + latency; 557 558 session->src_ip = info->src_ip; 559 session->dst_ip = info->dst_ip; 560 session->src_port = info->src_port; 561 session->dst_port = info->dst_port; 562 session->protocol = info->protocol; 563 564 session->packet = queued_packet_create( data, size, opaque, 1 ); 565 566 netdelay_expires(delay); 567 return; 568 } 569 } 570 } 571 572 delay->send_func( (void*)data, size, opaque ); 573 } 574 575 576 void 577 netdelay_destroy( NetDelay delay ) 578 { 579 if (delay) { 580 while (delay->sessions) { 581 Session session = delay->sessions; 582 delay->sessions = session->next; 583 session_free(session); 584 delay->num_sessions -= 1; 585 } 586 delay->active = 0; 587 qemu_free( delay ); 588 } 589 } 590 591
