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
20static 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 */
54typedef 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
63static QueuedPacket
64queued_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
90static void
91queued_packet_free( QueuedPacket  packet )
92{
93    if (packet) {
94        qemu_free( packet );
95    }
96}
97
98typedef 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
113void
114netshaper_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 */
134static void
135netshaper_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
161NetShaper
162netshaper_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
182void
183netshaper_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
206void
207netshaper_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
257void
258netshaper_send( NetShaper  shaper,
259                void*      data,
260                size_t     size )
261{
262    netshaper_send_aux(shaper, data, size, NULL);
263}
264
265
266int
267netshaper_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 */
289typedef 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
306static void
307session_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 */
320static const char*
321session_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 */
336int
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
383typedef 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
397static Session*
398netdelay_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 */
423static void
424netdelay_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
459NetDelay
460netdelay_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
479void
480netdelay_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
500void
501netdelay_send( NetDelay  delay, const void*  data, size_t  size )
502{
503    netdelay_send_aux(delay, data, size, NULL);
504}
505
506
507void
508netdelay_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
576void
577netdelay_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