The Android Open Source Project | 8b23a6c | 2009-03-03 19:30:32 -0800 | [diff] [blame] | 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( 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( 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( 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( 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( 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( 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( 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 | |