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
_packet_is_internal(const uint8_t * data,size_t size)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
queued_packet_create(const void * data,size_t size,void * opaque,int do_copy)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
queued_packet_free(QueuedPacket packet)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
netshaper_destroy(NetShaper shaper)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
netshaper_expires(NetShaper shaper)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
netshaper_create(int do_copy,NetShaperSendFunc send_func)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
netshaper_set_rate(NetShaper shaper,double rate)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
netshaper_send_aux(NetShaper shaper,void * data,size_t size,void * opaque)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
netshaper_send(NetShaper shaper,void * data,size_t size)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
netshaper_can_send(NetShaper shaper)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
session_free(Session session)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
_packet_SYN_flags(const void * _data,size_t size,Session info)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*
netdelay_lookup_session(NetDelay delay,Session info)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
netdelay_expires(NetDelay delay)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
netdelay_create(NetShaperSendFunc send_func)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
netdelay_set_latency(NetDelay delay,int min_ms,int max_ms)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
netdelay_send(NetDelay delay,const void * data,size_t size)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
netdelay_send_aux(NetDelay delay,const void * data,size_t size,void * opaque)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
netdelay_destroy(NetDelay delay)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