Merge pull request #48 from rtbrick/traffic-stream-enhancements

Traffic Stream Enhancements
2024-05-06 15:54:57 +00:00 · 2021-11-02 06:59:53 +01:00
parent ed72a05147 a4d2140adb
commit a5d73820d2
33 changed files with 1238 additions and 558 deletions
@@ -20,4 +20,8 @@ See the LICENSE file for more details.

 ## Copyright

-Copyright (C) 2020-2021, RtBrick, Inc.
+Copyright (C) 2020-2021, RtBrick, Inc.
+
+## Contact
+
+bngblaster@rtbrick.com
@@ -690,11 +690,12 @@ Attribute | Description | Default
 `destination-ipv4-address` | Overwrite the IPv4 destination address |
 `destination-ipv6-address` | Overwrite the IPv6 destination address |
 `threaded` | Run those streams in separate threads | false
+`thread-group` | Assign this stream to thread group (1-255) | 0 (thread per stream)

 For L2TP downstream traffic the IPv4 TOS is applied to the outer IPv4 and inner IPv4 header.

-The `pps` option has priority over `bps` where second is only a helper to calculate the `pps`
-based on given `bps` and `length`.
+The `pps` option supports also float numbers like 0.1, or 2.5 PPS and has priority over `bps` 
+where second is only a helper to calculate the `pps` based on given `bps` and `length`. 

 ## Access-Line

@@ -163,6 +163,8 @@ Attribute | Description
 `session-traffic-stop` (Alias: `session-traffic-disabled`) | Stop sending session traffic for all sessions
 `stream-traffic-start` (Alias: `stream-traffic-enabled`)  | Start sending stream traffic for all sessions
 `stream-traffic-stop` (Alias: `stream-traffic-disabled`) | Stop sending stream traffic for all sessions
+`stream-stats` | Display global stream traffic statistics
+`stream-info` | Display stream traffic statistics identified by flow identifier (`flow-id <id>`)
 `multicast-traffic-start` | Start sending multicast traffic from network interface
 `multicast-traffic-stop` | Stop sending multicast traffic from network interface
 `li-flows` | List all LI flows with detailed statistics
@@ -171,6 +173,8 @@ Attribute | Description
 `cfm-cc-stop` | Stop EOAM CFM CC
 `cfm-cc-rdi-on` | Set EOAM CFM CC RDI
 `cfm-cc-rdi-off` | Unset EOAM CFM CC RDI
+`traffic-start` | Start all traffic (session and streams)
+`traffic-stop` | Stop all traffic (session and streams)

 ### Session Commands

@@ -255,4 +259,4 @@ This command can be executed as shown below using the CLI tool.

 The L2TP session terminate command allows to test result (RFC2661) and disconnect (RFC3145) codes. 

-`$ sudo bngblaster-cli run.sock l2tp-session-terminate session-id 1 result-code 2 error-message "LCP request" disconnect-code 3 disconnect-message "LCP terminate request"`
+`$ sudo bngblaster-cli run.sock l2tp-session-terminate session-id 1 result-code 2 error-message "LCP request" disconnect-code 3 disconnect-message "LCP terminate request"`
@@ -4,21 +4,25 @@ Building a BNG from scratch requires a lot of testing but commercial BNG test so
 is often very expensive, resource intensive and provide a lot of administrative overhead
 to maintain such.

-Therefore we decided to build our own BNG test solution which is completely build from scratch
-targeted for max scaling with small resource footprint, simple to use and easy to integrate in
-our test automation infrastructure.
+Therefore we decided to build an open source network test software initially focused on BNG 
+and IPTV testing but constantly enhanced and planned for more common network equipment test 
+cases. The BNG Blaster was completely build from scratch, targeted for max scaling with small
+resource footprint, simple to use and easy to integrate in any test automation infrastructure.

 The BNG Blaster is able to simulate more than hundred thousand PPPoE and IPoE subscribers including
-IPTV, L2TPv2, QoS, forwarding verification and convergence testing capabilities.
+IPTV, L2TPv2 (LNS emulation), L2BSA, QoS, forwarding verification and convergence testing capabilities.

-* *High Scaling:* > 100K sessions
-* *Low CPU and Memory Footprint:* < 100MB memory for 16K sessions
+* *High Scaling:* > 100K sessions, > 1M PPS, and > 1M traffic flows
+* *Low CPU and Memory Footprint:* ~300MB memory for 16K sessions
 * *Portable:* runs on every modern linux, virtual machines and containers
 * *User Space:* all protocols implemented in user-space from scratch and optimized for performance
 * *IPTV:* IGMP version 1, 2 and 3 with automated channel zapping test
 * *QoS:* define and analyze traffic streams
 * *Automation:* the BNG Blaster Controller provides an automation friendly REST API and robot keywords

+Please send a mail to bngblaster@rtbrick.com if you are interested
+to get access to the BNG Blaster Controller!
+
 ```
 $ bngblaster --help

@@ -86,19 +90,21 @@ are currently considered as experimental. In the default mode (`packet_mmap_raw`
 buffer and send directly trough raw sockets.

 Sending and transmitting traffic is as easy as just by copying a packet into a buffer and setting a flag. This is super
-efficient and hence we have measured the I/O performance of roughly 1M pps per single CPU thread, which is more than enough for
-our purposes here.
+efficient and hence we have measured the I/O performance of up to 1M pps per single CPU thread, which is more than enough
+for our purposes here.

 BNG Blasters primary design goal is to simulate thousands of subscriber CPE's with a small hardware resource footprint. Simple
 to use and easy to integrate in our robot test automation infrastructure. This allows to simulate more than hundred thousand
 PPPoE subscribers including IPTV, traffic verification and convergence testing from a single medium scale virtual machine or to
 run the blaster directly from a laptop.

-The BNG Blaster provides two types of interfaces. The first interface is called the access interface which emulates the PPPoE
+The BNG Blaster provides three types of interfaces. The first interface is called the access interface which emulates the PPPoE
 sessions. The second interface-type is called network interface. This is used for emulating the core-facing side of the
-internet.
+internet. The last type is called a10nsp interface which emulates an layer two provider interface. The term A10 
+refers to the end-to-end ADSL network reference model from TR-025.  

 ![BNG Blaster Interfaces](images/bbl_interfaces.png)

 This allows to verify IP reachability by sending bidirectional traffic between all PPPoE sessions on access-interface and the
-network interface. The network interface is also used to inject downstream multicast test traffic for IPTV tests.
+network interface. The network interface is also used to inject downstream multicast test traffic for IPTV tests. It is also 
+possible to send RAW traffic streams between multiple network interfaces.
@@ -103,12 +103,12 @@ Session Traffic:
    Network IPv6:        500
    Network IPv6PD:      500
  First Sequence Number Received:
-    Access  IPv4    MIN:        1 MAX:        2
-    Access  IPv6    MIN:        2 MAX:        2
-    Access  IPv6PD  MIN:        2 MAX:        2
-    Network IPv4    MIN:        1 MAX:        2
-    Network IPv6    MIN:        2 MAX:        2
-    Network IPv6PD  MIN:        2 MAX:        2
+    Access  IPv4    MIN:        1 ( 1.000s) MAX:        2 ( 2.000s)
+    Access  IPv6    MIN:        2 ( 2.000s) MAX:        2 ( 2.000s)
+    Access  IPv6PD  MIN:        2 ( 2.000s) MAX:        2 ( 2.000s)
+    Network IPv4    MIN:        1 ( 1.000s) MAX:        2 ( 2.000s)
+    Network IPv6    MIN:        2 ( 2.000s) MAX:        2 ( 2.000s)
+    Network IPv6PD  MIN:        2 ( 2.000s) MAX:        2 ( 2.000s)
 ```

 ## JSON Reports
@@ -245,6 +245,10 @@ as shown in the example below.
 }
 ```

+The optional argument `-j sessions` allows to include per session statistics 
+in the report file. Similar with `-j streams` which allows to include per stream
+statistics. Both options could be also combined.
+
 ## Interface Statistics

 ## Session Traffic Statistics
@@ -287,16 +287,20 @@ multicast MAC address.

 ## Threaded Streams

-**WARNING**: The threading support is experimental and should be used with caution!
+With threading enabled, those streams will be started per default in a dedicated thread per flow. 
+This means one thread per session and stream direction. A threaded, bidirectional stream assigned
+to 10 sessions will therefore run in 20 threads. It is also possible to assign multiple
+steams to a single thread using thread groups. 

-With threading enabled, those streams will be started in a dedicated thread per flow. This
-means one thread per session and stream direction. A threaded , bidirectional stream assigned
-to 10 sessions will therefore run in 20 threads.
+In most environments we see between 200.000 and 300.000 PPS single threaded is working. Depending 
+on actual setup this can be also much more. With threaded streams we are also able to scale up to 
+10 million PPS depending on actual configuration and setup. This allows to start 1 million flows 
+with 1 PPS per flow over 4 threads in example to verify all prefixes of a BGP full table. 

-In most environments we see that 200.000 PPS single threaded is working. Depending on actual setup this
-can be also more. With threaded streams we are also able to scale up to three million PPS or more.
+The BNG Blaster is currently tested for up to 1 million flows which is not a hard limitation but 
+everything above should be considered as experimental. 

-## Start/Stop Session Stream Information
+## Start/Stop Session Stream Traffic

 Session stream traffic can be started/stopped dynamically
 using the commands `stream-traffic-enabled` and `stream-traffic-disabled`.
@@ -308,3 +312,20 @@ session identifier.

 `$ sudo bngblaster-cli run.sock stream-traffic-disabled`

+Alternatively all session and stream traffic (including RAW streams) 
+can be started or stopped globally using the `traffic-start` and 
+`traffic-stop` commands. 
+
+## Stream Configuration File
+
+The command line argument `-T <filename>` allows to include 
+streams defined from a separate file. The format is equal to
+streams defined in the actual configuration file. Such stream
+configuration files could be generated by scripts and
+easily merged with the base configuration.
+
+```json
+{
+    "streams": []
+}
+```
@@ -54,6 +54,24 @@ teardown_handler(int sig)
    g_teardown_request_count++;
 }

+void
+enable_disable_traffic(bbl_ctx_s *ctx, bool status)
+{
+    bbl_session_s *session;
+    uint32_t i;
+
+    g_traffic = status;
+
+    /* Iterate over all sessions */
+    for(i = 0; i < ctx->sessions; i++) {
+        session = ctx->session_list[i];
+        if(session) {
+            session->session_traffic = status;
+            session->stream_traffic = status;
+        }
+    }
+}
+
 static bool
 bbl_add_multicast_packets(bbl_ctx_s *ctx)
 {
@@ -93,10 +111,7 @@ bbl_add_multicast_packets(bbl_ctx_s *ctx)
            }
            group = htobe32(group);
            /* Generate multicast destination MAC */
-            *(uint32_t*)(&mac[2]) = group;
-            mac[0] = 0x01;
-            mac[2] = 0x5e;
-            mac[3] &= 0x7f;
+            ipv4_multicast_mac(group, mac);
            eth.src = interface->mac;
            eth.dst = mac;
            eth.vlan_outer = interface->vlan;
@@ -257,32 +272,10 @@ bbl_ctrl_job (timer_s *timer)

    if(ctx->sessions_outstanding) ctx->sessions_outstanding--;

-    if(ctx->sessions) {
-        if(ctx->sessions_terminated >= ctx->sessions) {
-            /* Now close all L2TP tunnels ... */
-            if(ctx->l2tp_tunnels == 0) {
-                /* Stop event loop to close application! */
-                CIRCLEQ_INIT(&ctx->timer_root.timer_bucket_qhead);
-            } else {
-                bbl_l2tp_stop_all_tunnel(ctx);
-            }
-            return;
-        }
-    } else {
-        /* Network interface only... */
-        if(g_teardown) {
-            if(ctx->l2tp_tunnels == 0) {
-                /* Stop event loop to close application! */
-                CIRCLEQ_INIT(&ctx->timer_root.timer_bucket_qhead);
-            } else {
-                bbl_l2tp_stop_all_tunnel(ctx);
-            }
-            return;
-        }
-        return;
-    }
-
    if(g_teardown) {
+        if(ctx->l2tp_tunnels && ctx->sessions_terminated >= ctx->sessions) {
+            bbl_l2tp_stop_all_tunnel(ctx);
+        }
        /* Teardown phase ... */
        if(g_teardown_request) {
            /* Put all sessions on the teardown list. */
@@ -507,7 +500,7 @@ main (int argc, char *argv[])
    }
    if(config_streams_file) {
        if(!bbl_config_streams_load_json(config_streams_file, ctx)) {
-            fprintf(stderr, "Error: Failed to load stream configuration file %s\n", config_file);
+            fprintf(stderr, "Error: Failed to load stream configuration file %s\n", config_streams_file);
            exit(1);
        }
    }
@@ -558,7 +551,7 @@ main (int argc, char *argv[])
            if (interactive) endwin();
            fprintf(stderr, "Error: Failed to init sessions\n");
            exit(1);
-        };
+        }
    }

    /* Setup control job. */
@@ -576,16 +569,39 @@ main (int argc, char *argv[])
     * such that we do not accidentally smear ourselves. */
    timer_add_periodic(&ctx->timer_root, &ctx->smear_timer, "Timer Smearing", 45, 12345678, ctx, &bbl_smear_job);

+    /* Smear all buckets. */
+    timer_smear_all_buckets(&ctx->timer_root);
+
+    /* Start threads. */
+    bbl_stream_start_threads(ctx);
+
    /* Start event loop. */
    log_open();
    clock_gettime(CLOCK_MONOTONIC, &ctx->timestamp_start);
    signal(SIGINT, teardown_handler);
-    timer_walk(&ctx->timer_root);
-    while(ctx->sessions_terminated < ctx->sessions && g_teardown_request_count < 10) {
+    while(g_teardown_request_count < 10) {
+        if(!ctx->l2tp_tunnels) {
+            if(ctx->sessions) {
+                /* With sessions, wait for all sessions
+                * to be terminated. */
+                if(ctx->sessions_terminated >= ctx->sessions && ctx->l2tp_tunnels == 0) {
+                    break;
+                }
+            } else {
+                /* Without sessions, we can stop immediately
+                * as soon as teardown was requested. */
+                if(g_teardown) {
+                    break;
+                }
+            }
+        }
        timer_walk(&ctx->timer_root);
    }
    clock_gettime(CLOCK_MONOTONIC, &ctx->timestamp_stop);

+    /* Stop threads. */
+    bbl_stream_stop_threads(ctx);
+
    /* Stop curses. Do this before the final reports. */
    if(g_interactive) {
        endwin();
@@ -35,6 +35,7 @@
 #include <sys/socket.h>
 #include <sys/types.h>
 #include <jansson.h>
+#include <pthread.h>

 #define NCURSES_NOMACROS 1
 #include <curses.h>
@@ -66,4 +67,7 @@
 WINDOW *log_win;
 WINDOW *stats_win;

+void
+enable_disable_traffic(bbl_ctx_s *ctx, bool status);
+
 #endif
@@ -652,6 +652,7 @@ json_parse_stream (bbl_ctx_s *ctx, json_t *stream, bbl_stream_config *stream_con
    json_t *value = NULL;
    const char *s = NULL;
    double bps;
+    double number;

    if (json_unpack(stream, "{s:s}", "type", &s) == 0) {
        if (strcmp(s, "ipv4") == 0) {
@@ -729,7 +730,7 @@ json_parse_stream (bbl_ctx_s *ctx, json_t *stream, bbl_stream_config *stream_con
    value = json_object_get(stream, "pps");
    if (value) {
        stream_config->pps = json_number_value(value);
-        if (stream_config->pps == 0) {
+        if (stream_config->pps <= 0) {
            fprintf(stderr, "JSON config error: Invalid value for stream->pps\n");
            return false;
        }
@@ -777,10 +778,21 @@ json_parse_stream (bbl_ctx_s *ctx, json_t *stream, bbl_stream_config *stream_con
        }
    }

+    /* Threading */
    value = json_object_get(stream, "threaded");
    if (json_is_boolean(value)) {
        stream_config->threaded = json_boolean_value(value);
    }
+    value = json_object_get(stream, "thread-group");
+    if (value) {
+        number = json_number_value(value);
+        if(number > 0 && number < 256) {
+            stream_config->thread_group = number;
+        } else {
+            fprintf(stderr, "JSON config error: Invalid thread-group (valid range is 1-255) for stream %s\n", stream_config->name);
+            return false;
+        }
+    }

    /* Validate configuration */
    if (stream_config->stream_group_id == 0) {
@@ -1605,8 +1617,8 @@ bbl_config_init_defaults (bbl_ctx_s *ctx) {
    ctx->config.qdisc_bypass = true;
    ctx->config.sessions = 1;
    ctx->config.sessions_max_outstanding = 800;
-    ctx->config.sessions_start_rate = 400,
-    ctx->config.sessions_stop_rate = 400,
+    ctx->config.sessions_start_rate = 400;
+    ctx->config.sessions_stop_rate = 400;
    ctx->config.pppoe_discovery_timeout = 5;
    ctx->config.pppoe_discovery_retry = 10;
    ctx->config.ppp_mru = 1492;
@@ -33,8 +33,6 @@
 extern volatile bool g_teardown;
 extern volatile bool g_teardown_request;

-extern bool g_traffic;
-
 typedef ssize_t callback_function(int fd, bbl_ctx_s *ctx, uint32_t session_id, json_t* arguments);

 static char *
@@ -1134,6 +1132,103 @@ bbl_ctrl_cfm_cc_rdi_off(int fd, bbl_ctx_s *ctx, uint32_t session_id, json_t* arg
    return bbl_ctrl_cfm_cc_rdi(fd, ctx, session_id, false);
 }

+ssize_t
+bbl_ctrl_stream_stats(int fd, bbl_ctx_s *ctx, uint32_t session_id __attribute__((unused)), json_t* arguments __attribute__((unused))) {
+    ssize_t result = 0;
+    json_t *root = json_pack("{ss si s{si si}}",
+                             "status", "ok",
+                             "code", 200,
+                             "stream-stats",
+                             "total-flows", ctx->stats.stream_traffic_flows,
+                             "verified-flows", ctx->stats.stream_traffic_flows_verified);
+    if(root) {
+        result = json_dumpfd(root, fd, 0);
+        json_decref(root);
+    }
+    return result;
+}
+
+ssize_t
+bbl_ctrl_stream_info(int fd, bbl_ctx_s *ctx, uint32_t session_id __attribute__((unused)), json_t* arguments) {
+    ssize_t result = 0;
+
+    json_t *root;
+    json_t *json_stream = NULL;
+
+    bbl_stream *stream;
+    void **search = NULL;
+
+    int number = 0;
+    uint64_t flow_id;
+
+    /* Unpack further arguments */
+    if (json_unpack(arguments, "{s:i}", "flow-id", &number) != 0) {
+        return bbl_ctrl_status(fd, "error", 400, "missing flow-id");
+    }
+
+    flow_id = number;
+    search = dict_search(ctx->stream_flow_dict, &flow_id);
+    if(search) {
+        stream = *search;
+        if(stream->thread.thread) {
+            pthread_mutex_lock(&stream->thread.mutex);
+        }
+        json_stream = bbl_stream_json(stream);
+        root = json_pack("{ss si so*}",
+                         "status", "ok",
+                         "code", 200,
+                         "stream-info", json_stream);
+        if(root) {
+            result = json_dumpfd(root, fd, 0);
+            json_decref(root);
+        } else {
+            result = bbl_ctrl_status(fd, "error", 500, "internal error");
+            json_decref(json_stream);
+        }
+        if(stream->thread.thread) {
+            pthread_mutex_unlock(&stream->thread.mutex);
+        }
+        return result;
+    } else {
+        return bbl_ctrl_status(fd, "warning", 404, "stream not found");
+    }
+}
+
+ssize_t
+bbl_ctrl_traffic(int fd, bbl_ctx_s *ctx, uint32_t session_id, bool status) {
+    bbl_session_s *session;
+    uint32_t i;
+    if(session_id) {
+        session = bbl_session_get(ctx, session_id);
+        if(session) {
+            session->stream_traffic = status;
+            return bbl_ctrl_status(fd, "ok", 200, NULL);
+        } else {
+            return bbl_ctrl_status(fd, "warning", 404, "session not found");
+        }
+    } else {
+        /* Iterate over all sessions */
+        for(i = 0; i < ctx->sessions; i++) {
+            session = ctx->session_list[i];
+            if(session) {
+                session->stream_traffic = status;
+            }
+        }
+        return bbl_ctrl_status(fd, "ok", 200, NULL);
+    }
+}
+
+ssize_t
+bbl_ctrl_traffic_start(int fd, bbl_ctx_s *ctx, uint32_t session_id __attribute__((unused)), json_t* arguments __attribute__((unused))) {
+    enable_disable_traffic(ctx, true);
+    return bbl_ctrl_status(fd, "ok", 200, NULL);
+}
+
+ssize_t
+bbl_ctrl_traffic_stop(int fd, bbl_ctx_s *ctx, uint32_t session_id __attribute__((unused)), json_t* arguments __attribute__((unused))) {
+    enable_disable_traffic(ctx, false);
+    return bbl_ctrl_status(fd, "ok", 200, NULL);
+}

 struct action {
    char *name;
@@ -1170,11 +1265,15 @@ struct action actions[] = {
    {"stream-traffic-start", bbl_ctrl_stream_traffic_start},
    {"stream-traffic-disabled", bbl_ctrl_stream_traffic_stop},
    {"stream-traffic-stop", bbl_ctrl_stream_traffic_stop},
+    {"stream-info", bbl_ctrl_stream_info},
+    {"stream-stats", bbl_ctrl_stream_stats},
    {"sessions-pending", bbl_ctrl_sessions_pending},
    {"cfm-cc-start", bbl_ctrl_cfm_cc_start},
    {"cfm-cc-stop", bbl_ctrl_cfm_cc_stop},
    {"cfm-cc-rdi-on", bbl_ctrl_cfm_cc_rdi_on},
    {"cfm-cc-rdi-off", bbl_ctrl_cfm_cc_rdi_off},
+    {"traffic-start", bbl_ctrl_traffic_start},
+    {"traffic-stop", bbl_ctrl_traffic_stop},
    {NULL, NULL},
 };

@@ -1206,7 +1305,7 @@ bbl_ctrl_socket_job (timer_s *timer) {
            }
        } else {
            /* New connection */
-            bzero(buf, sizeof(buf));
+            memset(buf, 0x0, sizeof(buf));
            len = read(fd, buf, INPUT_BUFFER);
            if(len) {
                root = json_loads((const char*)buf, 0, &error);
@@ -16,4 +16,4 @@ bbl_ctrl_socket_open (bbl_ctx_s *ctx);
 bool
 bbl_ctrl_socket_close (bbl_ctx_s *ctx);

-#endif
+#endif
@@ -77,6 +77,7 @@ bbl_ctx_add (void)
    CIRCLEQ_INIT(&ctx->interface_qhead);

    ctx->flow_id = 1;
+    ctx->multicast_traffic = true;

    /* Initialize hash table dictionaries. */
    ctx->vlan_session_dict = hashtable_dict_new((dict_compare_func)bbl_compare_key64, bbl_key64_hash, BBL_SESSION_HASHTABLE_SIZE);
@@ -126,4 +127,4 @@ bbl_ctx_del (bbl_ctx_s *ctx) {
    timer_flush_root(&ctx->timer_root);
    free(ctx);
    return;
-}
+}
@@ -78,6 +78,8 @@ typedef struct bbl_ctx_
    int ctrl_socket;
    char *ctrl_socket_path;

+    void *stream_thread; /* single linked list of threads */
+
    /* Interfaces */
    struct {
        uint8_t count;
@@ -290,4 +292,4 @@ bbl_ctx_add(void);
 void
 bbl_ctx_del(bbl_ctx_s *ctx);

-#endif
+#endif
@@ -1,8 +1,8 @@
 #ifndef __BBL_DEF_H__
 #define __BBL_DEF_H__

-#define IO_BUFFER_LEN               2048
-#define SCRATCHPAD_LEN              2048
+#define IO_BUFFER_LEN               9216
+#define SCRATCHPAD_LEN              4096
 #define CHALLENGE_LEN               16

 #define FILE_PATH_LEN               128
@@ -135,5 +135,8 @@ typedef struct bbl_ctx_ bbl_ctx_s;
 typedef struct bbl_interface_ bbl_interface_s;
 typedef struct bbl_session_ bbl_session_s;
 typedef struct bbl_a10nsp_session_ bbl_a10nsp_session_t;
+typedef struct bbl_stream_thread_ bbl_stream_thread;
+typedef struct bbl_stream_config_ bbl_stream_config;
+typedef struct bbl_stream_ bbl_stream;

 #endif
@@ -16,6 +16,17 @@

 #define PROGRESS_BAR_SIZE   60
 #define STATS_WIN_SIZE      90
+#define STRING_SP_SIZE      64
+
+#define VISIBLE(_expression) \
+    visible = false; \
+    if(_expression) { \
+        if(pos > stats_win_postion) { \
+            visible = true; \
+        } \
+        pos++; \
+    } \
+    if(visible)

 typedef enum {
    UI_VIEW_DEFAULT = 0,
@@ -34,8 +45,6 @@ extern volatile bool g_teardown;
 extern volatile bool g_teardown_request;
 extern volatile bool g_banner;

-extern bool g_traffic;
-
 /* This global variable is used to switch between access interfaces. */
 uint8_t g_access_if_selected  = 0;
 uint8_t g_network_if_selected = 0;
@@ -85,6 +94,7 @@ bbl_read_key_job (timer_s *timer)
    ch = getch();
    switch (ch) {
        case KEY_F(1):
+            stats_win_postion = 0;
            g_view_selected++;
            if(g_view_selected >= UI_VIEW_MAX) {
                g_view_selected = UI_VIEW_DEFAULT;
@@ -151,11 +161,11 @@ bbl_read_key_job (timer_s *timer)
            bbl_init_stats_win(ctx);
            break;
        case KEY_F(7):
-            g_traffic = true;
+            enable_disable_traffic(ctx, true);
            LOG(INFO, "Start traffic\n");
            break;
        case KEY_F(8):
-            g_traffic = false;
+            enable_disable_traffic(ctx, false);
            LOG(INFO, "Stop traffic\n");
            break;
        case KEY_F(9):
@@ -163,6 +173,18 @@ bbl_read_key_job (timer_s *timer)
            g_teardown_request = true;
            LOG(INFO, "Teardown request\n");
            break;
+        case KEY_DOWN:
+            if(g_view_selected == UI_VIEW_DEFAULT) {
+                if(stats_win_postion < 6) {
+                    stats_win_postion++;
+                }
+                bbl_init_stats_win(ctx);
+            }
+            break;
+        case KEY_UP:
+            if(stats_win_postion) stats_win_postion--;
+            bbl_init_stats_win(ctx);
+            break;
        default:
 	        break;
    }
@@ -171,8 +193,8 @@ bbl_read_key_job (timer_s *timer)
 /*
 * Format a progress bar.
 */
-char *
-bbl_format_progress (uint16_t complete, uint16_t current)
+static char *
+bbl_format_progress (uint32_t complete, uint32_t current)
 {
    static char buf[PROGRESS_BAR_SIZE+1];
    float percentage;
@@ -208,9 +230,14 @@ bbl_stats_job (timer_s *timer)
    struct bbl_interface_ *network_if;
    struct bbl_interface_ *a10nsp_if;

+    char strsp[STRING_SP_SIZE];
+
    bbl_session_s *session;
    int i;

+    int pos = 1; /* position */
+    bool visible = false; 
+
    access_if = ctx->interfaces.access_if[g_access_if_selected];
    network_if = ctx->interfaces.network_if[g_network_if_selected];
    a10nsp_if = ctx->interfaces.a10nsp_if[g_a10nsp_if_selected];
@@ -222,8 +249,7 @@ bbl_stats_job (timer_s *timer)
    }

    if(g_view_selected == UI_VIEW_DEFAULT) {
-
-        if(ctx->sessions) {
+        VISIBLE((ctx->sessions)) {
            wprintw(stats_win, "\nSessions      %10lu (%lu PPPoE / %lu IPoE)\n", ctx->sessions, ctx->sessions_pppoe, ctx->sessions_ipoe);

            /* Progress bar established sessions */
@@ -253,17 +279,11 @@ bbl_stats_job (timer_s *timer)
            wattroff(stats_win, COLOR_PAIR(COLOR_RED));
            wprintw(stats_win, "]\n");

-            /* Session stats */
-            wprintw(stats_win, "  Setup Time  %10lu ms\n", ctx->stats.setup_time);
-            wprintw(stats_win, "  Setup Rate  %10.02lf CPS (MIN: %0.02lf AVG: %0.02lf MAX: %0.02lf)\n",
-                    ctx->stats.cps, ctx->stats.cps_min, ctx->stats.cps_avg, ctx->stats.cps_max);
-            wprintw(stats_win, "  Flapped     %10lu\n", ctx->sessions_flapped);
-
-            /* DHCP */
+            /* DHCPv4 */
            if(ctx->dhcp_requested || ctx->dhcp_established_max) {
-                wprintw(stats_win, "\nDHCP\n");
-                wprintw(stats_win, "  Sessions    %10lu\n", ctx->dhcp_requested);
-                wprintw(stats_win, "  Established %10lu [", ctx->dhcp_established);
+
+                snprintf(strsp, STRING_SP_SIZE, "%u/%u", ctx->dhcp_established, ctx->dhcp_requested);
+                wprintw(stats_win, "  DHCPv4 %15s [", strsp);
                if(ctx->dhcp_requested == ctx->dhcp_established) {
                    wattron(stats_win, COLOR_PAIR(COLOR_GREEN));
                    wprintw(stats_win, "%s", bbl_format_progress(ctx->dhcp_requested, ctx->dhcp_established));
@@ -277,9 +297,8 @@ bbl_stats_job (timer_s *timer)
            }
            /* DHCPv6 */
            if(ctx->dhcpv6_requested || ctx->dhcpv6_established_max) {
-                wprintw(stats_win, "\nDHCPv6\n");
-                wprintw(stats_win, "  Sessions    %10lu\n", ctx->dhcpv6_requested);
-                wprintw(stats_win, "  Established %10lu [", ctx->dhcpv6_established);
+                snprintf(strsp, STRING_SP_SIZE, "%u/%u", ctx->dhcpv6_established, ctx->dhcpv6_requested);
+                wprintw(stats_win, "  DHCPv6 %15s [", strsp);
                if(ctx->dhcpv6_requested == ctx->dhcpv6_established) {
                    wattron(stats_win, COLOR_PAIR(COLOR_GREEN));
                    wprintw(stats_win, "%s", bbl_format_progress(ctx->dhcpv6_requested, ctx->dhcpv6_established));
@@ -291,16 +310,21 @@ bbl_stats_job (timer_s *timer)
                }
                wprintw(stats_win, "]\n");
            }
+            /* Session stats */
+            wprintw(stats_win, "  Setup Time  %10lu ms\n", ctx->stats.setup_time);
+            wprintw(stats_win, "  Setup Rate  %10.02lf CPS (MIN: %0.02lf AVG: %0.02lf MAX: %0.02lf)\n",
+                    ctx->stats.cps, ctx->stats.cps_min, ctx->stats.cps_avg, ctx->stats.cps_max);
+            wprintw(stats_win, "  Flapped     %10lu\n", ctx->sessions_flapped);
        }

        if(network_if) {
-            if(dict_count(ctx->li_flow_dict)) {
+            VISIBLE((dict_count(ctx->li_flow_dict))) {
                wprintw(stats_win, "\nLI Statistics\n");
                wprintw(stats_win, "  Flows                     %10lu\n", dict_count(ctx->li_flow_dict));
                wprintw(stats_win, "  Rx Packets                %10lu (%7lu PPS)\n",
                    network_if->stats.li_rx, network_if->stats.rate_li_rx.avg);
            }
-            if(ctx->config.l2tp_server) {
+            VISIBLE((ctx->config.l2tp_server)) {
                wprintw(stats_win, "\nL2TP LNS Statistics\n");
                wprintw(stats_win, "  Tunnels     %10lu\n", ctx->l2tp_tunnels);
                wprintw(stats_win, "  Established %10lu\n", ctx->l2tp_tunnels_established);
@@ -319,41 +343,39 @@ bbl_stats_job (timer_s *timer)
            }
        }

-        if(ctx->stats.session_traffic_flows) {
-            wprintw(stats_win, "\nSession Traffic\n");
-            wprintw(stats_win, "  Flows       %10lu\n", ctx->stats.session_traffic_flows);
-            /* Progress bar session traffic flows */
-            wprintw(stats_win, "  Verified    %10lu [", ctx->stats.session_traffic_flows_verified);
-            if(ctx->stats.session_traffic_flows == ctx->stats.session_traffic_flows_verified) {
-                wattron(stats_win, COLOR_PAIR(COLOR_GREEN));
-                wprintw(stats_win, "%s", bbl_format_progress(ctx->stats.session_traffic_flows, ctx->stats.session_traffic_flows_verified));
-                wattroff(stats_win, COLOR_PAIR(COLOR_GREEN));
-            } else {
-                wattron(stats_win, COLOR_PAIR(COLOR_BLACK));
-                wprintw(stats_win, "%s", bbl_format_progress(ctx->stats.session_traffic_flows, ctx->stats.session_traffic_flows_verified));
-                wattroff(stats_win, COLOR_PAIR(COLOR_BLACK));
+        VISIBLE((ctx->stats.session_traffic_flows || ctx->stats.stream_traffic_flows)) {
+            wprintw(stats_win, "\nTraffic Flows Verified\n");
+            if(ctx->stats.session_traffic_flows) {
+                snprintf(strsp, STRING_SP_SIZE, "%u/%u", ctx->stats.session_traffic_flows_verified, ctx->stats.session_traffic_flows);
+                wprintw(stats_win, "  Session %14s [", strsp);
+                if(ctx->stats.session_traffic_flows == ctx->stats.session_traffic_flows_verified) {
+                    wattron(stats_win, COLOR_PAIR(COLOR_GREEN));
+                    wprintw(stats_win, "%s", bbl_format_progress(ctx->stats.session_traffic_flows, ctx->stats.session_traffic_flows_verified));
+                    wattroff(stats_win, COLOR_PAIR(COLOR_GREEN));
+                } else {
+                    wattron(stats_win, COLOR_PAIR(COLOR_BLACK));
+                    wprintw(stats_win, "%s", bbl_format_progress(ctx->stats.session_traffic_flows, ctx->stats.session_traffic_flows_verified));
+                    wattroff(stats_win, COLOR_PAIR(COLOR_BLACK));
+                }
+                wprintw(stats_win, "]\n");
+            }
+            if(ctx->stats.stream_traffic_flows) {
+                snprintf(strsp, STRING_SP_SIZE, "%u/%u", ctx->stats.stream_traffic_flows_verified, ctx->stats.stream_traffic_flows);
+                wprintw(stats_win, "  Stream %15s [", strsp);
+                if(ctx->stats.stream_traffic_flows == ctx->stats.stream_traffic_flows_verified) {
+                    wattron(stats_win, COLOR_PAIR(COLOR_GREEN));
+                    wprintw(stats_win, "%s", bbl_format_progress(ctx->stats.stream_traffic_flows, ctx->stats.stream_traffic_flows_verified));
+                    wattroff(stats_win, COLOR_PAIR(COLOR_GREEN));
+                } else {
+                    wattron(stats_win, COLOR_PAIR(COLOR_BLACK));
+                    wprintw(stats_win, "%s", bbl_format_progress(ctx->stats.stream_traffic_flows, ctx->stats.stream_traffic_flows_verified));
+                    wattroff(stats_win, COLOR_PAIR(COLOR_BLACK));
+                }
+                wprintw(stats_win, "]\n");
            }
-            wprintw(stats_win, "]\n");
        }

-        if(ctx->stats.stream_traffic_flows) {
-            wprintw(stats_win, "\nTraffic Streams\n");
-            wprintw(stats_win, "  Flows       %10lu\n", ctx->stats.stream_traffic_flows);
-            /* Progress bar traffic stream flows */
-            wprintw(stats_win, "  Verified    %10lu [", ctx->stats.stream_traffic_flows_verified);
-            if(ctx->stats.stream_traffic_flows == ctx->stats.stream_traffic_flows_verified) {
-                wattron(stats_win, COLOR_PAIR(COLOR_GREEN));
-                wprintw(stats_win, "%s", bbl_format_progress(ctx->stats.stream_traffic_flows, ctx->stats.stream_traffic_flows_verified));
-                wattroff(stats_win, COLOR_PAIR(COLOR_GREEN));
-            } else {
-                wattron(stats_win, COLOR_PAIR(COLOR_BLACK));
-                wprintw(stats_win, "%s", bbl_format_progress(ctx->stats.stream_traffic_flows, ctx->stats.stream_traffic_flows_verified));
-                wattroff(stats_win, COLOR_PAIR(COLOR_BLACK));
-            }
-            wprintw(stats_win, "]\n");
-        }
-
-        if(network_if) {
+        VISIBLE(network_if) {
            wprintw(stats_win, "\nNetwork Interface (");
            for(i = 0; i < ctx->interfaces.network_if_count; i++) {
                if(i == g_network_if_selected) {
@@ -390,8 +412,7 @@ bbl_stats_job (timer_s *timer)
            wprintw(stats_win, "  Tx Multicast Packets      %10lu |%7lu PPS\n",
                network_if->stats.mc_tx, network_if->stats.rate_mc_tx.avg);
        }
-
-        if(a10nsp_if) {
+        VISIBLE(a10nsp_if) {
            wprintw(stats_win, "\nA10NSP Interface (");
            for(i = 0; i < ctx->interfaces.a10nsp_if_count; i++) {
                if(i == g_a10nsp_if_selected) {
@@ -416,8 +437,7 @@ bbl_stats_job (timer_s *timer)
                    a10nsp_if->stats.session_ipv4_loss);
            }
        }
-
-        if(access_if) {
+        VISIBLE(access_if) {
            wprintw(stats_win, "\nAccess Interface (");
            for(i = 0; i < ctx->interfaces.access_if_count; i++) {
                if(i == g_access_if_selected) {
@@ -599,6 +619,7 @@ bbl_init_curses (bbl_ctx_s *ctx)

    /* Stats window */
    stats_win = newwin(LINES, STATS_WIN_SIZE, 0, 0);
+    stats_win_postion = 0;

    /* Log window */
    log_win = newwin(LINES, COLS-STATS_WIN_SIZE, 0, STATS_WIN_SIZE);
@@ -623,4 +644,4 @@ bbl_init_curses (bbl_ctx_s *ctx)
 		               0, 100 * MSEC, ctx, &bbl_read_key_job);

    g_interactive = true;
-}
+}
@@ -266,6 +266,10 @@ bbl_add_network_interfaces(bbl_ctx_s *ctx)
        if(network_config->ip6.len && network_config->gateway6.len) {
            memcpy(&network_if->ip6, &network_config->ip6, sizeof(ipv6_prefix));
            memcpy(&network_if->gateway6, &network_config->gateway6, sizeof(ipv6_prefix));
+            memcpy(&network_if->gateway6_solicited_node_multicast, &ipv6_solicited_node_multicast, sizeof(ipv6addr_t));
+            memcpy(((uint8_t*)&network_if->gateway6_solicited_node_multicast)+13, 
+                   ((uint8_t*)&network_if->gateway6.address)+13, 3);
+
            /* Send initial ICMPv6 NS */
            network_if->send_requests |= BBL_IF_SEND_ICMPV6_NS;
        }
@@ -70,8 +70,9 @@ typedef struct bbl_interface_

    ipv6_prefix ip6;
    ipv6_prefix gateway6;
-    bool        icmpv6_nd_resolved;
+    ipv6addr_t gateway6_solicited_node_multicast;

+    bool icmpv6_nd_resolved;
    bool gateway_resolve_wait;
    
    uint8_t *mc_packets;
@@ -55,7 +55,7 @@ bbl_io_packet_mmap_rx_job (timer_s *timer) {
    /* Get RX timestamp */
    clock_gettime(CLOCK_MONOTONIC, &interface->rx_timestamp);

-    while ((tphdr->tp_status & TP_STATUS_USER)) {
+    while (tphdr->tp_status & TP_STATUS_USER) {
        eth_start = (uint8_t*)tphdr + tphdr->tp_mac;
        eth_len = tphdr->tp_len;
        interface->stats.packets_rx++;
@@ -547,8 +547,6 @@ encode_icmpv6(uint8_t *buf, uint16_t *len,

    if(icmp->data_len) {
        /* Copy data */
-        *(uint32_t*)buf = 0;
-        BUMP_WRITE_BUFFER(buf, len, sizeof(uint32_t));
        memcpy(buf, icmp->data, icmp->data_len);
        BUMP_WRITE_BUFFER(buf, len, icmp->data_len);
    } else {
@@ -809,7 +807,7 @@ encode_ipv4(uint8_t *buf, uint16_t *len,

    uint8_t *start = buf;
    uint16_t ipv4_len = *len;
-    uint16_t udp_len = *len;
+    uint16_t udp_len;
    uint8_t header_len = 5; /* header length 20 (4 * 5) */

    if(ipv4->router_alert_option) {
@@ -1483,7 +1481,7 @@ encode_pppoe_discovery(uint8_t *buf, uint16_t *len,
                *buf = ACCESS_LINE_DATA_LINK_ENCAPS;
                BUMP_WRITE_BUFFER(buf, len, sizeof(uint8_t));
                /* (1)byte   + (1)byte  + (1)byte
-                 * data link   encaps 1   encaps 2 */;
+                 * data link   encaps 1   encaps 2 */
                *(uint32_t*)buf = htobe32(access_line_profile->data_link_encaps);
                *buf = 3;
                BUMP_WRITE_BUFFER(buf, len, sizeof(uint32_t));
@@ -1883,7 +1881,6 @@ decode_icmp(uint8_t *buf, uint16_t len,

    /* Init ICMP header */
    icmp = (bbl_icmp_t*)sp; BUMP_BUFFER(sp, sp_len, sizeof(bbl_icmp_t));
-    //memset(icmp, 0x0, sizeof(bbl_icmp_t));

    icmp->type = *buf;
    BUMP_BUFFER(buf, len, sizeof(uint8_t));
@@ -2444,7 +2441,6 @@ decode_udp(uint8_t *buf, uint16_t len,

    /* Init UDP header */
    udp = (bbl_udp_t*)sp; BUMP_BUFFER(sp, sp_len, sizeof(bbl_udp_t));
-    //memset(udp, 0x0, sizeof(bbl_udp_t));

    udp->src = be16toh(*(uint16_t*)buf);
    BUMP_BUFFER(buf, len, sizeof(uint16_t));
@@ -2515,7 +2511,6 @@ decode_ipv6(uint8_t *buf, uint16_t len,

    /* Init IPv6 header */
    ipv6 = (bbl_ipv6_t*)sp; BUMP_BUFFER(sp, sp_len, sizeof(bbl_ipv6_t));
-    //memset(ipv6, 0x0, sizeof(bbl_ipv6_t));

    /* Check if version is 6 */
    if(((*buf >> 4) & 0xf) != 6) {
@@ -2596,7 +2591,7 @@ decode_ipv4(uint8_t *buf, uint16_t len,

    /* Init IPv4 header */
    ipv4 = (bbl_ipv4_t*)sp; BUMP_BUFFER(sp, sp_len, sizeof(bbl_ipv4_t));
-    //memset(ipv4, 0x0, sizeof(bbl_ipv4_t));
+    ipv4->router_alert_option = false;

    header = (struct ip*)buf;

@@ -3379,7 +3374,6 @@ decode_pppoe_session(uint8_t *buf, uint16_t len,

    /* Init PPPoE header */
    pppoe = (bbl_pppoe_session_t*)sp; BUMP_BUFFER(sp, sp_len, sizeof(bbl_pppoe_session_t));
-    //memset(pppoe, 0x0, sizeof(bbl_pppoe_session_t));

    header = (struct pppoe_ppp_session_header*)buf;
    BUMP_BUFFER(buf, len, sizeof(struct pppoe_ppp_session_header));
@@ -3444,7 +3438,6 @@ decode_arp(uint8_t *buf, uint16_t len,

    /* Init ARP header */
    arp = (bbl_arp_t*)sp; BUMP_BUFFER(sp, sp_len, sizeof(bbl_arp_t));
-    //memset(arp, 0x0, sizeof(bbl_arp_t));

    BUMP_BUFFER(buf, len, 6);
    arp->code = be16toh(*(uint16_t*)buf);
@@ -235,12 +235,11 @@ static const ipv6addr_t ipv6_link_local_prefix = {0xFE, 0x80, 0x00, 0x00, 0x00,
 static const ipv6addr_t ipv6_multicast_all_nodes = {0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01};
 static const ipv6addr_t ipv6_multicast_all_routers = {0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02};
 static const ipv6addr_t ipv6_multicast_all_dhcp = {0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02};
+static const ipv6addr_t ipv6_solicited_node_multicast = {0xFF, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x00, 0x00, 0x00};

 /* MAC Addresses */
 static const uint8_t broadcast_mac[ETH_ADDR_LEN] =  { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-static const uint8_t multicast_mac[ETH_ADDR_LEN] =  { 0x01, 0x00, 0x5e, 0x00, 0x00, 0x00};
-static const uint8_t ipv6_multicast_mac[ETH_ADDR_LEN] =  { 0x33, 0x33, 0xff, 0x00, 0x00, 0x10};
-static const uint8_t ipv6_multicast_mac_dhcp[ETH_ADDR_LEN] =  { 0x33, 0x33, 0x00, 0x01, 0x00, 0x02};
+

 typedef enum protocol_error_ {
    PROTOCOL_SUCCESS = 0,
@@ -495,6 +495,8 @@ bbl_rx_icmpv6(bbl_ethernet_header_t *eth, bbl_ipv6_t *ipv6, bbl_interface_s *int
        }
    } else if(icmpv6->type == IPV6_ICMPV6_NEIGHBOR_SOLICITATION) {
        session->send_requests |= BBL_SEND_ICMPV6_NA;
+    } else if(icmpv6->type == IPV6_ICMPV6_ECHO_REQUEST) {
+        bbl_send_icmpv6_echo_reply(interface, session, eth, ipv6, icmpv6);
    }
 }

@@ -1736,6 +1738,8 @@ bbl_rx_network_icmpv6(bbl_ethernet_header_t *eth, bbl_interface_s *interface) {
                secondary_ip6 = secondary_ip6->next;
            }
        }
+    } else if(icmpv6->type == IPV6_ICMPV6_ECHO_REQUEST) {
+        bbl_send_icmpv6_echo_reply(interface, NULL, eth, ipv6, icmpv6);
    }
 }

@@ -87,6 +87,7 @@ bbl_send_to_buffer(bbl_interface_s *interface, bbl_ethernet_header_t *eth)
        return BBL_SEND_FULL;
    }
    slot = interface->send.ring + interface->send.write;
+    slot->packet_len = 0;
    if(encode_ethernet(slot->packet, &slot->packet_len, eth) == PROTOCOL_SUCCESS) {
        interface->send.write = interface->send.next++;
        if(interface->send.next == interface->send.size) {
@@ -187,4 +188,23 @@ bbl_send_icmp_reply(bbl_interface_s *interface,
    ipv4->ttl = 64;
    icmp->type = ICMP_TYPE_ECHO_REPLY;
    return bbl_send_to_buffer(interface, eth);
+}
+
+bbl_send_result_t 
+bbl_send_icmpv6_echo_reply(bbl_interface_s *interface,
+                           bbl_session_s *session,
+                           bbl_ethernet_header_t *eth, 
+                           bbl_ipv6_t *ipv6, 
+                           bbl_icmpv6_t *icmpv6)
+{
+    update_eth(interface, session, eth);
+    ipv6->dst = ipv6->src;
+    if(session) {
+        ipv6->src = session->ipv6_address;
+    } else {
+        ipv6->src = interface->ip6.address;
+    }
+    ipv6->ttl = 255;
+    icmpv6->type = IPV6_ICMPV6_ECHO_REPLY;
+    return bbl_send_to_buffer(interface, eth);
 }
@@ -62,4 +62,11 @@ bbl_send_icmp_reply(bbl_interface_s *interface,
                    bbl_ipv4_t *ipv4, 
                    bbl_icmp_t *icmp);

+bbl_send_result_t 
+bbl_send_icmpv6_echo_reply(bbl_interface_s *interface,
+                           bbl_session_s *session,
+                           bbl_ethernet_header_t *eth, 
+                           bbl_ipv6_t *ipv6, 
+                           bbl_icmpv6_t *icmpv6);
+
 #endif
@@ -56,8 +56,10 @@ void
 bbl_stats_generate (bbl_ctx_s *ctx, bbl_stats_t * stats) {

    bbl_session_s *session;
+    bbl_stream *stream;
    struct bbl_interface_ *interface;

+    struct dict_itor *itor;
    uint32_t i;

    uint32_t join_delays = 0;
@@ -98,54 +100,65 @@ bbl_stats_generate (bbl_ctx_s *ctx, bbl_stats_t * stats) {
            }

            /* Session Traffic */
-            if(session->access_ipv4_rx_first_seq) stats->sessions_access_ipv4_rx++;
-            if(session->network_ipv4_rx_first_seq) stats->sessions_network_ipv4_rx++;
-            if(session->access_ipv6_rx_first_seq) stats->sessions_access_ipv6_rx++;
-            if(session->network_ipv6_rx_first_seq) stats->sessions_network_ipv6_rx++;
-            if(session->access_ipv6pd_rx_first_seq) stats->sessions_access_ipv6pd_rx++;
-            if(session->network_ipv6pd_rx_first_seq) stats->sessions_network_ipv6pd_rx++;
-
-            if(stats->min_access_ipv4_rx_first_seq) {
-                if(session->access_ipv4_rx_first_seq < stats->min_access_ipv4_rx_first_seq) stats->min_access_ipv4_rx_first_seq = session->access_ipv4_rx_first_seq;
-            } else {
-                stats->min_access_ipv4_rx_first_seq = session->access_ipv4_rx_first_seq;
+            if(session->access_ipv4_rx_first_seq) {
+                stats->sessions_access_ipv4_rx++;
+                if(stats->min_access_ipv4_rx_first_seq) {
+                    if(session->access_ipv4_rx_first_seq < stats->min_access_ipv4_rx_first_seq) stats->min_access_ipv4_rx_first_seq = session->access_ipv4_rx_first_seq;
+                } else {
+                    stats->min_access_ipv4_rx_first_seq = session->access_ipv4_rx_first_seq;
+                }
+                if(session->access_ipv4_rx_first_seq > stats->max_access_ipv4_rx_first_seq) stats->max_access_ipv4_rx_first_seq = session->access_ipv4_rx_first_seq;
            }
-            if(session->access_ipv4_rx_first_seq > stats->max_access_ipv4_rx_first_seq) stats->max_access_ipv4_rx_first_seq = session->access_ipv4_rx_first_seq;

-            if(stats->min_network_ipv4_rx_first_seq) {
-                if(session->network_ipv4_rx_first_seq < stats->min_network_ipv4_rx_first_seq) stats->min_network_ipv4_rx_first_seq = session->network_ipv4_rx_first_seq;
-            } else {
-                stats->min_network_ipv4_rx_first_seq = session->network_ipv4_rx_first_seq;
+            if(session->network_ipv4_rx_first_seq) {
+                stats->sessions_network_ipv4_rx++;
+                if(stats->min_network_ipv4_rx_first_seq) {
+                    if(session->network_ipv4_rx_first_seq < stats->min_network_ipv4_rx_first_seq) stats->min_network_ipv4_rx_first_seq = session->network_ipv4_rx_first_seq;
+                } else {
+                    stats->min_network_ipv4_rx_first_seq = session->network_ipv4_rx_first_seq;
+                }
+                if(session->network_ipv4_rx_first_seq > stats->max_network_ipv4_rx_first_seq) stats->max_network_ipv4_rx_first_seq = session->network_ipv4_rx_first_seq;
            }
-            if(session->network_ipv4_rx_first_seq > stats->max_network_ipv4_rx_first_seq) stats->max_network_ipv4_rx_first_seq = session->network_ipv4_rx_first_seq;

-            if(stats->min_access_ipv6_rx_first_seq) {
-                if(session->access_ipv6_rx_first_seq < stats->min_access_ipv6_rx_first_seq) stats->min_access_ipv6_rx_first_seq = session->access_ipv6_rx_first_seq;
-            } else {
-                stats->min_access_ipv6_rx_first_seq = session->access_ipv6_rx_first_seq;
+            if(session->access_ipv6_rx_first_seq) {
+                stats->sessions_access_ipv6_rx++;
+                if(stats->min_access_ipv6_rx_first_seq) {
+                    if(session->access_ipv6_rx_first_seq < stats->min_access_ipv6_rx_first_seq) stats->min_access_ipv6_rx_first_seq = session->access_ipv6_rx_first_seq;
+                } else {
+                    stats->min_access_ipv6_rx_first_seq = session->access_ipv6_rx_first_seq;
+                }
+                if(session->access_ipv6_rx_first_seq > stats->max_access_ipv6_rx_first_seq) stats->max_access_ipv6_rx_first_seq = session->access_ipv6_rx_first_seq;
            }
-            if(session->access_ipv6_rx_first_seq > stats->max_access_ipv6_rx_first_seq) stats->max_access_ipv6_rx_first_seq = session->access_ipv6_rx_first_seq;

-            if(stats->min_network_ipv6_rx_first_seq) {
-                if(session->network_ipv6_rx_first_seq < stats->min_network_ipv6_rx_first_seq) stats->min_network_ipv6_rx_first_seq = session->network_ipv6_rx_first_seq;
-            } else {
-                stats->min_network_ipv6_rx_first_seq = session->network_ipv6_rx_first_seq;
+            if(session->network_ipv6_rx_first_seq) {
+                stats->sessions_network_ipv6_rx++;
+                if(stats->min_network_ipv6_rx_first_seq) {
+                    if(session->network_ipv6_rx_first_seq < stats->min_network_ipv6_rx_first_seq) stats->min_network_ipv6_rx_first_seq = session->network_ipv6_rx_first_seq;
+                } else {
+                    stats->min_network_ipv6_rx_first_seq = session->network_ipv6_rx_first_seq;
+                }
+                if(session->network_ipv6_rx_first_seq > stats->max_network_ipv6_rx_first_seq) stats->max_network_ipv6_rx_first_seq = session->network_ipv6_rx_first_seq;
            }
-            if(session->network_ipv6_rx_first_seq > stats->max_network_ipv6_rx_first_seq) stats->max_network_ipv6_rx_first_seq = session->network_ipv6_rx_first_seq;

-            if(stats->min_access_ipv6pd_rx_first_seq) {
-                if(session->access_ipv6pd_rx_first_seq < stats->min_access_ipv6pd_rx_first_seq) stats->min_access_ipv6pd_rx_first_seq = session->access_ipv6pd_rx_first_seq;
-            } else {
-                stats->min_access_ipv6pd_rx_first_seq = session->access_ipv6pd_rx_first_seq;
+            if(session->access_ipv6pd_rx_first_seq) {
+                stats->sessions_access_ipv6pd_rx++;
+                if(stats->min_access_ipv6pd_rx_first_seq) {
+                    if(session->access_ipv6pd_rx_first_seq < stats->min_access_ipv6pd_rx_first_seq) stats->min_access_ipv6pd_rx_first_seq = session->access_ipv6pd_rx_first_seq;
+                } else {
+                    stats->min_access_ipv6pd_rx_first_seq = session->access_ipv6pd_rx_first_seq;
+                }
+                if(session->access_ipv6pd_rx_first_seq > stats->max_access_ipv6pd_rx_first_seq) stats->max_access_ipv6pd_rx_first_seq = session->access_ipv6pd_rx_first_seq;
            }
-            if(session->access_ipv6pd_rx_first_seq > stats->max_access_ipv6pd_rx_first_seq) stats->max_access_ipv6pd_rx_first_seq = session->access_ipv6pd_rx_first_seq;

-            if(stats->min_network_ipv6pd_rx_first_seq) {
-                if(session->network_ipv6pd_rx_first_seq < stats->min_network_ipv6pd_rx_first_seq) stats->min_network_ipv6pd_rx_first_seq = session->network_ipv6pd_rx_first_seq;
-            } else {
-                stats->min_network_ipv6pd_rx_first_seq = session->network_ipv6pd_rx_first_seq;
+            if(session->network_ipv6pd_rx_first_seq) {
+                stats->sessions_network_ipv6pd_rx++;
+                if(stats->min_network_ipv6pd_rx_first_seq) {
+                    if(session->network_ipv6pd_rx_first_seq < stats->min_network_ipv6pd_rx_first_seq) stats->min_network_ipv6pd_rx_first_seq = session->network_ipv6pd_rx_first_seq;
+                } else {
+                    stats->min_network_ipv6pd_rx_first_seq = session->network_ipv6pd_rx_first_seq;
+                }
+                if(session->network_ipv6pd_rx_first_seq > stats->max_network_ipv6pd_rx_first_seq) stats->max_network_ipv6pd_rx_first_seq = session->network_ipv6pd_rx_first_seq;
            }
-            if(session->network_ipv6pd_rx_first_seq > stats->max_network_ipv6pd_rx_first_seq) stats->max_network_ipv6pd_rx_first_seq = session->network_ipv6pd_rx_first_seq;
        }
    }

@@ -188,6 +201,37 @@ bbl_stats_generate (bbl_ctx_s *ctx, bbl_stats_t * stats) {
        stats->l2tp_data_rx += interface->stats.l2tp_data_rx;
        stats->li_rx += interface->stats.li_rx;
    }
+
+    /* Iterate over all traffic streams */
+    itor = dict_itor_new(ctx->stream_flow_dict);
+    dict_itor_first(itor);
+    for (; dict_itor_valid(itor); dict_itor_next(itor)) {
+        stream = (bbl_stream*)*dict_itor_datum(itor);
+        if(stream) {
+            if(stats->min_stream_loss) {
+                if(stream->loss < stats->min_stream_loss) stats->min_stream_loss = stream->loss;
+            } else {
+                stats->min_stream_loss = stream->loss;
+            }
+            if(stream->loss > stats->max_stream_loss) stats->max_stream_loss = stream->loss;
+
+            if(stream->rx_first_seq) {
+                if(stats->min_stream_rx_first_seq) {
+                    if(stream->rx_first_seq < stats->min_stream_rx_first_seq) stats->min_stream_rx_first_seq = stream->rx_first_seq;
+                } else {
+                    stats->min_stream_rx_first_seq = stream->rx_first_seq;
+                }
+                if(stream->rx_first_seq > stats->max_stream_rx_first_seq) stats->max_stream_rx_first_seq = stream->rx_first_seq;
+            
+                if(stats->min_stream_delay_ns) {
+                    if(stream->min_delay_ns < stats->min_stream_delay_ns) stats->min_stream_delay_ns = stream->min_delay_ns;
+                } else {
+                    stats->min_stream_delay_ns = stream->min_delay_ns;
+                }
+                if(stream->max_delay_ns > stats->max_stream_delay_ns) stats->max_stream_delay_ns = stream->max_delay_ns;
+            }
+        }
+    }
 }

 void
@@ -387,6 +431,15 @@ bbl_stats_stdout (bbl_ctx_s *ctx, bbl_stats_t * stats) {
        printf("\nTraffic Streams:\n");
        printf("  Verified Traffic Flows: %u/%u\n", 
            ctx->stats.stream_traffic_flows_verified, ctx->stats.stream_traffic_flows);
+        printf("  First Sequence Number Received  MIN: %8lu MAX: %8lu\n", 
+            stats->min_stream_rx_first_seq,
+            stats->max_stream_rx_first_seq);
+        printf("  Flow Receive Packet Loss        MIN: %8lu MAX: %8lu\n", 
+            stats->min_stream_loss,
+            stats->max_stream_loss);
+        printf("  Flow Receive Delay (nsec)       MIN: %8lu MAX: %8lu\n", 
+            stats->min_stream_delay_ns,
+            stats->max_stream_delay_ns);
    }

    if(ctx->config.igmp_group_count > 1) {
@@ -649,6 +702,12 @@ bbl_stats_json (bbl_ctx_s *ctx, bbl_stats_t * stats) {
        jobj_sub = json_object();
        json_object_set(jobj_sub, "total-flows", json_integer(ctx->stats.stream_traffic_flows));
        json_object_set(jobj_sub, "verified-flows", json_integer(ctx->stats.stream_traffic_flows_verified));
+        json_object_set(jobj_sub, "first-seq-rx-min", json_integer(stats->min_stream_rx_first_seq));
+        json_object_set(jobj_sub, "first-seq-rx-max", json_integer(stats->max_stream_rx_first_seq));
+        json_object_set(jobj_sub, "flow-rx-packet-loss-min", json_integer(stats->min_stream_loss));
+        json_object_set(jobj_sub, "flow-rx-packet-loss-max", json_integer(stats->max_stream_loss));
+        json_object_set(jobj_sub, "flow-rx-delay-min", json_integer(stats->min_stream_delay_ns));
+        json_object_set(jobj_sub, "flow-rx-delay-max", json_integer(stats->max_stream_delay_ns));
        json_object_set(jobj, "traffic-streams", jobj_sub);
    }

@@ -74,6 +74,13 @@ typedef struct bbl_stats_ {
    uint64_t l2tp_data_rx;

    uint64_t li_rx;
+
+    uint64_t min_stream_loss;
+    uint64_t max_stream_loss;
+    uint64_t min_stream_rx_first_seq;
+    uint64_t max_stream_rx_first_seq;
+    uint64_t min_stream_delay_ns;
+    uint64_t max_stream_delay_ns;
 } bbl_stats_t;

 void bbl_compute_avg_rate (bbl_rate_s *rate, uint64_t current_value);
@@ -12,7 +12,6 @@
 #include "bbl_stream.h"
 #include "bbl_stats.h"
 #include "bbl_io.h"
-#include <pthread.h>

 extern volatile bool g_teardown;
 extern bool g_init_phase;
@@ -91,6 +90,7 @@ FREE:
        stream->buf = NULL;
        stream->tx_len = 0;
    }
+    stream->send_window_packets = 0;
    return false;
 }

@@ -221,7 +221,6 @@ bbl_stream_build_access_pppoe_packet(bbl_stream *stream) {
    return true;
 }

-
 bool
 bbl_stream_build_a10nsp_pppoe_packet(bbl_stream *stream) {

@@ -523,10 +522,7 @@ bbl_stream_build_network_packet(bbl_stream *stream) {
                /* All IPv4 multicast addresses start with 1110 */
                if((ipv4.dst & htobe32(0xf0000000)) == htobe32(0xe0000000)) {
                    /* Generate multicast destination MAC */
-                    *(uint32_t*)(&mac[2]) = ipv4.dst;
-                    mac[0] = 0x01;
-                    mac[2] = 0x5e;
-                    mac[3] &= 0x7f;
+                    ipv4_multicast_mac(ipv4.dst, mac);
                    eth.dst = mac;
                    bbl.type = BBL_TYPE_MULTICAST;
                    bbl.mc_source = ipv4.src;
@@ -723,148 +719,322 @@ bbl_stream_build_packet(bbl_stream *stream) {
 void *
 bbl_stream_tx_thread (void *thread_data) {

-    bbl_stream *stream = thread_data;
-    bbl_interface_s *interface = stream->interface;
+    bbl_stream_thread *thread = thread_data;

-    struct timespec send_windwow;
-    struct timespec now;
-    struct timespec sleep;
-    struct timespec rem;
-
-    double d;
-    uint64_t packets;
-    uint64_t packets_expected;
-
-    struct sockaddr_ll addr = {0};
-    int fd_tx;
-    int qdisc_bypass = 1;
+    pthread_mutex_lock(&thread->mutex);
+    timer_smear_all_buckets(&thread->timer_root);
+    pthread_mutex_unlock(&thread->mutex);

    /* Open new TX socket for thread. */
-    fd_tx = socket(PF_PACKET, SOCK_RAW | SOCK_NONBLOCK, 0);
-    if (fd_tx == -1) {
-        if (errno == EPERM) {
-            LOG(ERROR, "socket() for interface %s Permission denied: Are you root?\n", interface->name);
-            return NULL;
-        }
-        LOG(ERROR, "socket() TX error %s (%d) for interface %s\n", strerror(errno), errno, interface->name);
-        return NULL;
-    }
-    addr.sll_family = PF_PACKET;
-    addr.sll_ifindex = interface->ifindex;
-    addr.sll_protocol = 0;
-    if (bind(fd_tx, (struct sockaddr*)&addr, sizeof(addr)) == -1) {
-        LOG(ERROR, "bind() TX error %s (%d) for interface %s\n",
-            strerror(errno), errno, interface->name);
-        return NULL;
-    }
-    if (setsockopt(fd_tx, SOL_PACKET, PACKET_QDISC_BYPASS, &qdisc_bypass, sizeof(qdisc_bypass)) == -1) {
-        LOG(ERROR, "Setting qdisc bypass error %s (%d) for interface %s\n", strerror(errno), errno, interface->name);
-        return NULL;
-    }
-
-    while(!g_teardown) {
-        packets = 0;
-
-        if(!bbl_stream_can_send(stream)) {
-            /* Close send window */
-            stream->send_window_packets = 0;
-            sleep.tv_nsec = (100 + (rand() % 900)) * MSEC;
-            nanosleep(&sleep, &rem);
-            continue;
-        }
-        if(!stream->buf) {
-            if(!bbl_stream_build_packet(stream)) {
-                LOG(ERROR, "Failed to build packet for stream %s\n", stream->config->name);
-                sleep.tv_nsec = 100 * MSEC;
-                nanosleep(&sleep, &rem);
-                continue;
-            }
-        }
-
-        if(!g_traffic || (stream->session && !stream->session->stream_traffic)) {
-            /* Close send window */
-            stream->send_window_packets = 0;
-            sleep.tv_nsec = 10 * MSEC;
-            nanosleep(&sleep, &rem);
-            continue;
-        }
-
-        clock_gettime(CLOCK_MONOTONIC, &now);
-        if(stream->send_window_packets == 0) {
-            /* Open new send window */
-            stream->send_window_start.tv_sec = now.tv_sec;
-            stream->send_window_start.tv_nsec = now.tv_nsec;
-            packets = 1;
+    while(true) {
+        pthread_mutex_lock(&thread->mutex);
+        if(thread->active) {
+            pthread_mutex_unlock(&thread->mutex);
+            timer_walk(&thread->timer_root);
        } else {
-            timespec_sub(&send_windwow, &now, &stream->send_window_start);
-            packets_expected = send_windwow.tv_sec * stream->config->pps;
-            d = (send_windwow.tv_nsec / 1000000000.0);
-            packets_expected += d * stream->config->pps;
-
-            if(packets_expected > stream->send_window_packets) {
-                packets = packets_expected - stream->send_window_packets;
-            }
+            pthread_mutex_unlock(&thread->mutex);
+            break;
        }
-
-        /* Update BBL header fields */
-        *(uint32_t*)(stream->buf + (stream->tx_len - 8)) = now.tv_sec;
-        *(uint32_t*)(stream->buf + (stream->tx_len - 4)) = now.tv_nsec;
-        while(packets) {
-            *(uint64_t*)(stream->buf + (stream->tx_len - 16)) = stream->flow_seq;
-            /* Send packet ... */
-            if (sendto(fd_tx, stream->buf, stream->tx_len, 0, (struct sockaddr*)&addr, sizeof(struct sockaddr_ll)) <0 ) {
-                LOG(IO, "Sendto failed with errno: %i\n", errno);
-                sleep.tv_nsec = 1 * MSEC;
-                nanosleep(&sleep, &rem);
-                clock_gettime(CLOCK_MONOTONIC, &now);
-                *(uint32_t*)(stream->buf + (stream->tx_len - 8)) = now.tv_sec;
-                *(uint32_t*)(stream->buf + (stream->tx_len - 4)) = now.tv_nsec;
-                continue;
-            }
-            stream->send_window_packets++;
-            stream->packets_tx++;
-            stream->flow_seq++;
-            packets--;
-        }
-        sleep.tv_nsec = 1000; /* 0.001ms */
-        nanosleep(&sleep, &rem);
    }
    return NULL;
 }

+/**
+ * This function synchronizes the data
+ * between TX stream threads and main 
+ * thread.
+ * 
+ * @param thread 
+ */
 void
-bbl_stream_tx_thread_counter_sync (timer_s *timer) {
-
-    bbl_stream *stream = timer->data;
-    bbl_session_s *session = stream->session;
-    bbl_interface_s *interface = stream->interface;
+bbl_stream_tx_thread_sync(bbl_stream_thread *thread) {
+    bbl_interface_s *interface = thread->interface;
+    bbl_stream *stream = thread->stream;
+    bbl_session_s *session = NULL;

    uint64_t packets_tx;
+    uint64_t bytes_tx;
    uint64_t delta_packets;
    uint64_t delta_bytes;
+    
+    pthread_mutex_lock(&stream->thread.mutex);

-    packets_tx = stream->packets_tx;
-    delta_packets = packets_tx - stream->packets_tx_last_sync;
-    delta_bytes = delta_packets * stream->tx_len;
+    packets_tx = thread->packets_tx;
+    delta_packets = packets_tx - thread->packets_tx_last_sync;
+    bytes_tx = thread->bytes_tx;
+    delta_bytes = bytes_tx - thread->bytes_tx_last_sync;

    interface->stats.packets_tx += delta_packets;
    interface->stats.bytes_tx += delta_bytes;
-    if(session) {
-        if(stream->direction == STREAM_DIRECTION_UP) {
-            session->stats.packets_tx += delta_packets;
-            session->stats.bytes_tx += delta_bytes;
-            session->stats.accounting_packets_tx += delta_packets;
-            session->stats.accounting_bytes_tx += delta_bytes;
-        } else {
-            if(session->l2tp_session) {
-                interface->stats.l2tp_data_tx++;
-                session->l2tp_session->tunnel->stats.data_tx++;
-                session->l2tp_session->stats.data_tx++;
-                session->l2tp_session->stats.data_ipv4_tx++;
+    
+    thread->packets_tx_last_sync = packets_tx;
+    thread->bytes_tx_last_sync = bytes_tx;
+
+    packets_tx = thread->sendto_failed;
+    delta_packets = packets_tx - thread->sendto_failed_last_sync;
+    interface->stats.sendto_failed += delta_packets;
+
+    pthread_mutex_unlock(&stream->thread.mutex);
+
+    while(stream) {
+        pthread_mutex_lock(&stream->thread.mutex);
+        if(stream->session) {
+            session = stream->session;
+            /* Sync counters ... */
+            packets_tx = stream->packets_tx;
+            delta_packets = packets_tx - stream->packets_tx_last_sync;
+            delta_bytes = delta_packets * stream->tx_len;
+            if(stream->direction == STREAM_DIRECTION_UP) {
+                session->stats.packets_tx += delta_packets;
+                session->stats.bytes_tx += delta_bytes;
+                session->stats.accounting_packets_tx += delta_packets;
+                session->stats.accounting_bytes_tx += delta_bytes;
+            } else {
+                if(session->l2tp_session) {
+                    interface->stats.l2tp_data_tx++;
+                    session->l2tp_session->tunnel->stats.data_tx++;
+                    session->l2tp_session->stats.data_tx++;
+                    session->l2tp_session->stats.data_ipv4_tx++;
+                }
            }
+            stream->packets_tx_last_sync = packets_tx;
+
+            /* Sync session states ... */
+            if(bbl_stream_can_send(stream)) {
+                if(!stream->buf) {
+                    if(!bbl_stream_build_packet(stream)) {
+                        LOG(ERROR, "Failed to build packet for stream %s\n", stream->config->name);
+                    }
+                }
+            }
+            if(stream->buf && g_traffic && session->stream_traffic) {
+                stream->thread.can_send = true;
+            } else {
+                stream->thread.can_send = false;
+                stream->send_window_packets = 0;
+            }
+        } else {
+            if(bbl_stream_can_send(stream)) {
+                if(!stream->buf) {
+                    if(!bbl_stream_build_packet(stream)) {
+                        LOG(ERROR, "Failed to build packet for stream %s\n", stream->config->name);
+                    }
+                }
+            }
+            if(stream->buf && g_traffic) {
+                stream->thread.can_send = true;
+            } else {
+                stream->thread.can_send = false;
+                stream->send_window_packets = 0;
+            }        
+        }
+        pthread_mutex_unlock(&stream->thread.mutex);
+        stream = stream->thread.next;
+    } 
+}
+
+void
+bbl_stream_tx_thread_sync_timer(timer_s *timer) {
+    bbl_stream_thread *thread = timer->data;
+    bbl_stream_tx_thread_sync(thread);
+}
+
+static bbl_stream_thread *
+bbl_stream_thread_create(uint8_t thread_group, bbl_stream *stream) {
+    bbl_stream_thread *thread;
+    bbl_interface_s *interface = stream->interface;
+
+    int qdisc_bypass = 1;
+
+    if(thread_group) {
+        LOG(INFO, "Create stream TX thread-group %u\n", thread_group);
+    } else {
+        LOG(INFO, "Create stream TX thread for stream %s\n", stream->config->name);
+    }
+    thread = calloc(1, sizeof(bbl_stream_thread));
+    thread->thread_group = thread_group;
+    thread->interface = interface;
+
+    /* Init thread timer root */
+    timer_init_root(&thread->timer_root);
+
+    /* Init thread mutex */
+    if (pthread_mutex_init(&thread->mutex, NULL) != 0) {
+        LOG(ERROR, "Failed to init stream TX thread mutex\n");
+        return NULL;
+    }
+
+    /* Setup RAW socket */
+    thread->socket.fd_tx = socket(PF_PACKET, SOCK_RAW | SOCK_NONBLOCK, 0);
+    if (thread->socket.fd_tx == -1) {
+        if (errno == EPERM) {
+            LOG(ERROR, "Thread: socket() for interface %s Permission denied: Are you root?\n", interface->name);
+            return NULL;
+        }
+        LOG(ERROR, "Thread: socket() TX error %s (%d) for interface %s\n", strerror(errno), errno, interface->name);
+        return NULL;
+    }
+    thread->socket.addr.sll_family = PF_PACKET;
+    thread->socket.addr.sll_ifindex = interface->ifindex;
+    thread->socket.addr.sll_protocol = 0;
+    if (bind(thread->socket.fd_tx, (struct sockaddr*)&thread->socket.addr, sizeof(thread->socket.addr)) == -1) {
+        LOG(ERROR, "Thread: bind() TX error %s (%d) for interface %s\n",
+            strerror(errno), errno, interface->name);
+        return NULL;
+    }
+    if(interface->ctx->config.qdisc_bypass) {
+        if (setsockopt(thread->socket.fd_tx, SOL_PACKET, PACKET_QDISC_BYPASS, &qdisc_bypass, sizeof(qdisc_bypass)) == -1) {
+            LOG(ERROR, "Thread: Setting qdisc bypass error %s (%d) for interface %s\n", strerror(errno), errno, interface->name);
+            return NULL;
        }
    }
-    stream->packets_tx_last_sync = packets_tx;
+    return thread;
+}
+
+/**
+ * This function will add a stream to an existing
+ * thread or create a new thread based on thread 
+ * group. 
+ * 
+ * @param ctx global context
+ * @param thread_group thread group
+ * @param stream traffic stream
+ */
+static bbl_stream_thread*
+bbl_stream_add_to_thread(bbl_ctx_s *ctx, uint8_t thread_group, bbl_stream *stream) {
+
+    bbl_stream_thread *thread;
+
+    /* Search for existing thread group or create a new one */
+    if(ctx->stream_thread) {
+        thread = ctx->stream_thread;
+        while(true) {
+            /* The thread group zero means that this stream 
+             * requests a dedicated thread. The scope of thread 
+             * groups is per interface. */
+            if(thread_group && thread->thread_group == thread_group &&
+               thread->interface == stream->interface) {
+                break;
+            }
+            if(thread->next) {
+                thread = thread->next;
+            } else {
+                /* Create new thread */
+                thread->next = bbl_stream_thread_create(thread_group, stream);
+                thread = thread->next;
+                if(!thread) {
+                    return NULL;
+                }
+                break;
+            }
+        }
+    } else {
+        /* Create first thread */
+        thread = bbl_stream_thread_create(thread_group, stream);
+        ctx->stream_thread = thread;
+    }
+
+    /* Append stream to thread */
+    if(thread->stream) {
+        thread->stream_tail->thread.next = stream;
+    } else {
+        /* First stream in thread */
+        thread->stream = stream;
+    }
+    thread->stream_tail = stream;
+    thread->stream_count++;
+
+    /* Init stream mutex */
+    if (pthread_mutex_init(&stream->thread.mutex, NULL) != 0) {
+        LOG(ERROR, "Failed to init stream mutex\n");
+        return NULL;
+    }
+
+    stream->thread.thread = thread;
+    return thread;
+}
+
+/**
+ * This function starts all stream threads.
+ * 
+ * @param ctx global context
+ * @return true if success and false if failed
+ */
+bool
+bbl_stream_start_threads(bbl_ctx_s *ctx) {
+    
+    bbl_stream_thread *thread = ctx->stream_thread;
+
+    while(thread) {
+        if(thread->thread_group) {
+            LOG(INFO, "Start stream TX thread-group %u\n", thread->thread_group);
+        } else {
+            LOG(INFO, "Start stream TX thread for stream %s\n", thread->stream->config->name);
+        }
+        thread->active = true;
+        timer_add_periodic(&ctx->timer_root, &thread->sync_timer, "Stream TX Thread Sync", 1, 0, thread, &bbl_stream_tx_thread_sync_timer);
+        pthread_create(&thread->thread_id, NULL, bbl_stream_tx_thread, (void *)thread);
+        thread = thread->next;
+    }
+    return true;
+}
+
+/**
+ * This function stops all stream threads.
+ * 
+ * @param ctx global context
+ */
+void
+bbl_stream_stop_threads(bbl_ctx_s *ctx) {
+    
+    bbl_stream_thread *thread = ctx->stream_thread;
+    while(thread) {
+        if(thread->active) {
+            if(thread->thread_group) {
+                LOG(INFO, "Stop stream TX thread-group %u\n", thread->thread_group);
+            } else {
+                LOG(INFO, "Stop stream TX thread for stream %s\n", thread->stream->config->name);
+            }
+            /* Mark thread as inactive and stop counter sync job */
+            pthread_mutex_lock(&thread->mutex);
+            thread->active = false;
+            pthread_mutex_unlock(&thread->mutex);
+            /* Wait for thread to be stopped */
+            pthread_join(thread->thread_id, NULL);
+            /* Do final sync */
+            bbl_stream_tx_thread_sync(thread);
+        }
+        thread = thread->next;
+    }
+}
+
+uint64_t
+bbl_stream_send_window(bbl_stream *stream, struct timespec *now) {
+
+    bbl_ctx_s *ctx = stream->interface->ctx;
+    uint64_t packets = 1;
+    uint64_t packets_expected;
+
+    struct timespec send_windwow;
+
+    if(stream->send_window_packets == 0) {
+        /* Open new send window */
+        stream->send_window_start.tv_sec = now->tv_sec;
+        stream->send_window_start.tv_nsec = now->tv_nsec;
+    } else {
+        timespec_sub(&send_windwow, now, &stream->send_window_start);
+        packets_expected = send_windwow.tv_sec * stream->config->pps;
+        packets_expected += stream->config->pps * ((double)send_windwow.tv_nsec / 1000000000.0);
+
+        if(packets_expected > stream->send_window_packets) {
+            packets = packets_expected - stream->send_window_packets;
+        }
+        if(packets > ctx->config.io_stream_max_ppi) {
+            packets = ctx->config.io_stream_max_ppi;
+        }
+    }
+
+    return packets;
 }

 void
@@ -874,15 +1044,11 @@ bbl_stream_tx_job (timer_s *timer) {
    bbl_session_s *session = stream->session;
    bbl_interface_s *interface = stream->interface;

-    struct timespec send_windwow;
    struct timespec now;

-    double d;
    uint64_t packets = 1;
-    uint64_t packets_expected;

    if(!bbl_stream_can_send(stream)) {
-        stream->send_window_packets = 0;
        return;
    }
    if(!stream->buf) {
@@ -899,24 +1065,7 @@ bbl_stream_tx_job (timer_s *timer) {
    }

    clock_gettime(CLOCK_MONOTONIC, &now);
-    if(stream->send_window_packets == 0) {
-        /* Open new send window */
-        stream->send_window_start.tv_sec = now.tv_sec;
-        stream->send_window_start.tv_nsec = now.tv_nsec;
-    } else {
-        timespec_sub(&send_windwow, &now, &stream->send_window_start);
-        packets_expected = send_windwow.tv_sec * stream->config->pps;
-        d = (send_windwow.tv_nsec / 1000000000.0);
-        packets_expected += d * stream->config->pps;
-
-        if(packets_expected > stream->send_window_packets) {
-            packets = packets_expected - stream->send_window_packets;
-        }
-        if(packets > interface->ctx->config.io_stream_max_ppi) {
-            packets = interface->ctx->config.io_stream_max_ppi;
-        }
-    }
-
+    packets = bbl_stream_send_window(stream, &now);
    /* Update BBL header fields */
    *(uint32_t*)(stream->buf + (stream->tx_len - 8)) = now.tv_sec;
    *(uint32_t*)(stream->buf + (stream->tx_len - 4)) = now.tv_nsec;
@@ -948,6 +1097,49 @@ bbl_stream_tx_job (timer_s *timer) {
    }
 }

+void
+bbl_stream_tx_job_threaded (timer_s *timer) {
+    bbl_stream *stream = timer->data;
+    bbl_stream_thread *thread = stream->thread.thread;
+
+    struct timespec now;
+
+    uint64_t packets;
+
+    pthread_mutex_lock(&stream->thread.mutex);
+    if(!stream->thread.can_send) {
+        pthread_mutex_unlock(&stream->thread.mutex);
+        return;
+    }
+
+    pthread_mutex_lock(&thread->mutex);
+
+    clock_gettime(CLOCK_MONOTONIC, &now);
+    packets = bbl_stream_send_window(stream, &now);
+
+    /* Update BBL header fields */
+    *(uint32_t*)(stream->buf + (stream->tx_len - 8)) = now.tv_sec;
+    *(uint32_t*)(stream->buf + (stream->tx_len - 4)) = now.tv_nsec;
+    while(packets) {
+        *(uint64_t*)(stream->buf + (stream->tx_len - 16)) = stream->flow_seq;
+        /* Send packet ... */
+        if (sendto(thread->socket.fd_tx, stream->buf, stream->tx_len, 0, (struct sockaddr*)&thread->socket.addr, sizeof(struct sockaddr_ll)) <0 ) {
+            LOG(IO, "Thread: Sendto failed with errno: %i\n", errno);
+            thread->sendto_failed++;
+            packets = 0;
+        } else {
+            stream->packets_tx++;
+            stream->send_window_packets++;
+            stream->flow_seq++;
+            packets--;
+            thread->packets_tx++;
+            thread->bytes_tx += stream->tx_len;
+        }
+    }
+    pthread_mutex_unlock(&thread->mutex);
+    pthread_mutex_unlock(&stream->thread.mutex);
+}
+
 void
 bbl_stream_rate_job (timer_s *timer) {
    bbl_stream *stream = timer->data;
@@ -955,20 +1147,28 @@ bbl_stream_rate_job (timer_s *timer) {
    bbl_compute_avg_rate(&stream->rate_packets_rx, stream->packets_rx);
 }

+void
+bbl_stream_rate_job_threaded (timer_s *timer) {
+    bbl_stream *stream = timer->data;
+    pthread_mutex_lock(&stream->thread.mutex);
+    bbl_compute_avg_rate(&stream->rate_packets_tx, stream->packets_tx);
+    bbl_compute_avg_rate(&stream->rate_packets_rx, stream->packets_rx);
+    pthread_mutex_unlock(&stream->thread.mutex);
+}
+
 bool
 bbl_stream_add(bbl_ctx_s *ctx, bbl_access_config_s *access_config, bbl_session_s *session) {

    bbl_stream_config *config;
    bbl_stream *stream;
-    bbl_stream *session_stream;
+    bbl_stream *session_stream; 
+    bbl_stream_thread *thread;

    dict_insert_result result;

    time_t timer_sec = 0;
    long timer_nsec  = 0;

-    pthread_t thread_id;
-
    config = ctx->config.stream_config;

    /* *
@@ -997,11 +1197,9 @@ bbl_stream_add(bbl_ctx_s *ctx, bbl_access_config_s *access_config, bbl_session_s
                return false;
            }

-            if(config->pps == 1) {
-                timer_sec = 1;
-            } else {
-                timer_nsec = 1000000000 / config->pps;
-            }
+            timer_nsec = SEC / config->pps;
+            timer_sec = timer_nsec / 1000000000;
+            timer_nsec = timer_nsec % 1000000000;

            if(config->direction & STREAM_DIRECTION_UP) {
                stream = calloc(1, sizeof(bbl_stream));
@@ -1029,14 +1227,20 @@ bbl_stream_add(bbl_ctx_s *ctx, bbl_access_config_s *access_config, bbl_session_s
                    session->stream = stream;
                }
                if(config->threaded) {
-                    pthread_create(&thread_id, NULL, bbl_stream_tx_thread, (void *)stream);
-                    timer_add_periodic(&ctx->timer_root, &stream->timer, config->name, 1, 0, stream, &bbl_stream_tx_thread_counter_sync);
+                    thread = bbl_stream_add_to_thread(ctx, config->thread_group, stream);
+                    if(!thread) {
+                        LOG(ERROR, "Failed to add stream %s to thread\n", config->name);
+                        free(stream);
+                        return false;
+                    } 
+                    timer_add_periodic(&thread->timer_root, &stream->timer, config->name, timer_sec, timer_nsec, stream, &bbl_stream_tx_job_threaded);
+                    timer_add_periodic(&thread->timer_root, &stream->timer_rate, "Threaded Rate Computation", 1, 0, stream, &bbl_stream_rate_job_threaded);
                } else {
                    timer_add_periodic(&ctx->timer_root, &stream->timer, config->name, timer_sec, timer_nsec, stream, &bbl_stream_tx_job);
+                    timer_add_periodic(&ctx->timer_root, &stream->timer_rate, "Rate Computation", 1, 0, stream, &bbl_stream_rate_job);
                }
-                timer_add_periodic(&ctx->timer_root, &stream->timer_rate, "Rate Computation", 1, 0, stream, &bbl_stream_rate_job);
                ctx->stats.stream_traffic_flows++;
-                LOG(DEBUG, "Traffic stream %s added in upstream with %u PPS (timer: %lu sec %lu nsec)\n", config->name, config->pps, timer_sec, timer_nsec);
+                LOG(DEBUG, "Traffic stream %s added in upstream with %lf PPS (timer: %lu sec %lu nsec)\n", config->name, config->pps, timer_sec, timer_nsec);
            }
            if(config->direction & STREAM_DIRECTION_DOWN) {
                stream = calloc(1, sizeof(bbl_stream));
@@ -1064,16 +1268,21 @@ bbl_stream_add(bbl_ctx_s *ctx, bbl_access_config_s *access_config, bbl_session_s
                    session->stream = stream;
                }
                if(config->threaded) {
-                    pthread_create(&thread_id, NULL, bbl_stream_tx_thread, (void *)stream);
-                    timer_add_periodic(&ctx->timer_root, &stream->timer, config->name, 1, 0, stream, &bbl_stream_tx_thread_counter_sync);
+                    thread = bbl_stream_add_to_thread(ctx, config->thread_group, stream);
+                    if(!thread) {
+                        LOG(ERROR, "Failed to add stream %s to thread\n", config->name);
+                        free(stream);
+                        return false;
+                    } 
+                    timer_add_periodic(&thread->timer_root, &stream->timer, config->name, timer_sec, timer_nsec, stream, &bbl_stream_tx_job_threaded);
+                    timer_add_periodic(&thread->timer_root, &stream->timer_rate, "Threaded Rate Computation", 1, 0, stream, &bbl_stream_rate_job_threaded);
                } else {
                    timer_add_periodic(&ctx->timer_root, &stream->timer, config->name, timer_sec, timer_nsec, stream, &bbl_stream_tx_job);
+                    timer_add_periodic(&ctx->timer_root, &stream->timer_rate, "Rate Computation", 1, 0, stream, &bbl_stream_rate_job);
                }
-                timer_add_periodic(&ctx->timer_root, &stream->timer_rate, "Rate Computation", 1, 0, stream, &bbl_stream_rate_job);
                ctx->stats.stream_traffic_flows++;
-                LOG(DEBUG, "Traffic stream %s added in downstream with %u PPS (timer %lu sec %lu nsec)\n", config->name, config->pps, timer_sec, timer_nsec);
+                LOG(DEBUG, "Traffic stream %s added in downstream with %lf PPS (timer %lu sec %lu nsec)\n", config->name, config->pps, timer_sec, timer_nsec);
            }
-            timer_smear_bucket(&ctx->timer_root, timer_sec, timer_nsec);
        }
        config = config->next;
    }
@@ -1086,6 +1295,7 @@ bbl_stream_raw_add(bbl_ctx_s *ctx) {

    bbl_stream_config *config;
    bbl_stream *stream;
+    bbl_stream_thread *thread;
    bbl_interface_s *network_if;

    dict_insert_result result;
@@ -1093,8 +1303,6 @@ bbl_stream_raw_add(bbl_ctx_s *ctx) {
    time_t timer_sec = 0;
    long timer_nsec  = 0;

-    pthread_t thread_id;
-
    config = ctx->config.stream_config;

    while(config) {
@@ -1103,11 +1311,11 @@ bbl_stream_raw_add(bbl_ctx_s *ctx) {
            if(!network_if) {
                return false;
            }
-            if(config->pps == 1) {
-                timer_sec = 1;
-            } else {
-                timer_nsec = 1000000000 / config->pps;
-            }
+
+            timer_nsec = SEC / config->pps;
+            timer_sec = timer_nsec / 1000000000;
+            timer_nsec = timer_nsec % 1000000000;
+
            if(config->direction & STREAM_DIRECTION_DOWN) {
                stream = calloc(1, sizeof(bbl_stream));
                stream->flow_id = ctx->flow_id++;
@@ -1124,16 +1332,21 @@ bbl_stream_raw_add(bbl_ctx_s *ctx) {
                }
                *result.datum_ptr = stream;
                if(config->threaded) {
-                    pthread_create(&thread_id, NULL, bbl_stream_tx_thread, (void *)stream);
-                    timer_add_periodic(&ctx->timer_root, &stream->timer, config->name, 1, 0, stream, &bbl_stream_tx_thread_counter_sync);
+                    thread = bbl_stream_add_to_thread(ctx, config->thread_group, stream);
+                    if(!thread) {
+                        LOG(ERROR, "Failed to add stream %s to thread\n", config->name);
+                        free(stream);
+                        return false;
+                    } 
+                    timer_add_periodic(&thread->timer_root, &stream->timer, config->name, timer_sec, timer_nsec, stream, &bbl_stream_tx_job_threaded);
+                    timer_add_periodic(&thread->timer_root, &stream->timer_rate, "Threaded Rate Computation", 1, 0, stream, &bbl_stream_rate_job_threaded);
                } else {
                    timer_add_periodic(&ctx->timer_root, &stream->timer, config->name, timer_sec, timer_nsec, stream, &bbl_stream_tx_job);
+                    timer_add_periodic(&ctx->timer_root, &stream->timer_rate, "Rate Computation", 1, 0, stream, &bbl_stream_rate_job);
                }
-                timer_add_periodic(&ctx->timer_root, &stream->timer_rate, "Rate Computation", 1, 0, stream, &bbl_stream_rate_job);
                ctx->stats.stream_traffic_flows++;
-                LOG(DEBUG, "RAW traffic stream %s added in downstream with %u PPS (timer %lu sec %lu nsec)\n", config->name, config->pps, timer_sec, timer_nsec);
+                LOG(DEBUG, "RAW traffic stream %s added in downstream with %lf PPS (timer %lu sec %lu nsec)\n", config->name, config->pps, timer_sec, timer_nsec);
            }
-            timer_smear_bucket(&ctx->timer_root, timer_sec, timer_nsec);
        }
        config = config->next;
    }
@@ -1,12 +1,11 @@
 /*
- * BNG Blaster (BBL) - Streams
+ * BNG Blaster (BBL) - Traffic Streams
 *
 * Christian Giese, Match 2021
 *
 * Copyright (C) 2020-2021, RtBrick, Inc.
 * SPDX-License-Identifier: BSD-3-Clause
 */
-
 #ifndef __BBL_STREAM_H__
 #define __BBL_STREAM_H__

@@ -30,7 +29,7 @@ typedef struct bbl_stream_config_
    bbl_stream_type_t type;
    bbl_stream_direction_t direction;

-    uint32_t pps;
+    double pps;
    uint16_t length;
    uint8_t  priority; /* IPv4 TOS or IPv6 TC */
    uint8_t  vlan_priority;
@@ -43,7 +42,9 @@ typedef struct bbl_stream_config_
    char *a10nsp_interface;

    bool threaded;
-    void *next; /* next bbl_stream_config */
+    uint8_t thread_group;
+
+    bbl_stream_config *next; /* Next stream config */
 } bbl_stream_config;

 typedef struct bbl_stream_
@@ -87,19 +88,81 @@ typedef struct bbl_stream_
    bbl_rate_s rate_packets_tx;
    bbl_rate_s rate_packets_rx;

-    void *next; /* next stream of same session */
+    bbl_stream *next; /* Next stream of same session */
+
+    /* Attributes used for threaded streams only! */
+    struct {
+        bbl_stream_thread *thread;
+        bbl_stream *next; /* Next stream in same thread */ 
+        pthread_mutex_t mutex;
+        bool can_send;
+    } thread;
 } bbl_stream;

+/* Structure for traffic stream threads
+ * with one or more streams. */
+typedef struct bbl_stream_thread_
+{
+    /* The thread-group allows to assign
+     * multiple streams to one thread. The 
+     * group zero has the special meaning of 
+     * one thread per stream. */
+    uint8_t thread_group;
+    pthread_t thread_id;
+    pthread_mutex_t mutex;
+
+    /* True if thread is active! */
+    bool active;
+
+    /* Root for thread local timers */
+    struct timer_root_ timer_root; 
+
+    /* Timer for synchronice job of thread 
+     * counters with main counters. */
+    struct timer_ *sync_timer;
+
+    /* TX interface */
+    bbl_interface_s *interface;
+
+    /* TX interface file RAW socket */
+    struct {
+        int fd_tx;
+        struct sockaddr_ll addr;
+    } socket;
+
+    uint32_t stream_count; /* Number of streams in group */
+    bbl_stream *stream; /* First stream in group */
+    bbl_stream *stream_tail; /* Last stream in group */
+
+    /* Thread counters ... */
+
+    uint64_t packets_tx;
+    uint64_t packets_tx_last_sync;
+    uint64_t bytes_tx;
+    uint64_t bytes_tx_last_sync;
+
+    uint64_t sendto_failed;
+    uint64_t sendto_failed_last_sync;
+
+    void *next; /* Next stream thread */
+} bbl_stream_thread;
+
 bool
 bbl_stream_add(bbl_ctx_s *ctx, bbl_access_config_s *access_config, bbl_session_s *session);

 bool
 bbl_stream_raw_add(bbl_ctx_s *ctx);

+bool
+bbl_stream_start_threads(bbl_ctx_s *ctx);
+
 void
-bbl_stream_tx_job (timer_s *timer);
+bbl_stream_stop_threads(bbl_ctx_s *ctx);
+
+void
+bbl_stream_tx_job(timer_s *timer);

 json_t *
 bbl_stream_json(bbl_stream *stream);

-#endif
+#endif
@@ -16,7 +16,7 @@
 #include "bbl.h"
 #include "bbl_timer.h"

-/*
+/**
 * Add two timestamps x and y, storing the result in result.
 */
 void
@@ -35,7 +35,7 @@ timespec_add (struct timespec *result, struct timespec *x, struct timespec *y)
  }
 }

-/*
+/**
 * Subtract the timestamps x from y, storing the result in result.
 */
 void
@@ -64,7 +64,7 @@ timespec_sub (struct timespec *result, struct timespec *x, struct timespec *y)
    }
 }

-/*
+/**
 * Format a timestamp in one of four buffers.
 * This way we can format upto 4 timespecs in one printf() call.
 */
@@ -83,7 +83,7 @@ timespec_format (struct timespec *x)
    return ret;
 }

-/*
+/**
 * Enqueue a timer for change processing.
 */
 void
@@ -103,7 +103,7 @@ timer_change (timer_s *timer)
    timer->on_change_list = true;
 }

-void
+static void
 timer_enqueue_bucket (timer_root_s *root, timer_s *timer, time_t sec, long nsec)
 {
    timer_bucket_s *timer_bucket;
@@ -146,6 +146,64 @@ timer_enqueue_bucket (timer_root_s *root, timer_s *timer, time_t sec, long nsec)
 }

 /*
+ * Dequeue a timer from its timer_bucket.
+ */
+static void
+timer_dequeue_bucket (timer_s *timer)
+{
+    timer_root_s *timer_root;
+    timer_bucket_s *timer_bucket;
+
+    timer_bucket = timer->timer_bucket;
+    timer_root = timer_bucket->timer_root;
+
+    CIRCLEQ_REMOVE(&timer_bucket->timer_qhead, timer, timer_qnode);
+    timer_bucket->timers--;
+    timer->timer_bucket = NULL;
+
+    /*
+     * If the last timer of a bucket is gone, remove the bucket as well.
+     */
+    if (!timer_bucket->timers) {
+        CIRCLEQ_REMOVE(&timer_root->timer_bucket_qhead, timer_bucket, timer_bucket_qnode);
+
+        LOG(TIMER_DETAIL, "  Delete timer bucket %lu.%06lus\n",
+            timer_bucket->sec, timer_bucket->nsec/1000);
+
+        free(timer_bucket);
+        timer_root->buckets--;
+    }
+}
+
+static void
+timer_requeue (timer_s *timer, time_t sec, long nsec)
+{
+    timer_root_s *timer_root;
+    timer_bucket_s *timer_bucket;
+
+    timer_bucket = timer->timer_bucket;
+    timer_root = timer_bucket->timer_root;
+
+    timer_set_expire(timer, sec, nsec);
+
+    /*
+     * If the expiration {sec,nsec} matches the bucket, then simply
+     * timer dequeue and enqueue to keep correct temporal ordering.
+     * If there is no match, do a slightly more expensive
+     * bucket dequeue and enqueue.
+     */
+    if (timer_bucket->sec == sec && timer_bucket->nsec == nsec) {
+        CIRCLEQ_REMOVE(&timer_bucket->timer_qhead, timer, timer_qnode);
+        CIRCLEQ_INSERT_TAIL(&timer_bucket->timer_qhead, timer, timer_qnode);
+    } else {
+        timer_dequeue_bucket(timer);
+        timer_enqueue_bucket(timer_root, timer, sec, nsec);
+    }
+
+    LOG(TIMER_DETAIL, "  Reset %s timer, expire in %lu.%06lus\n", timer->name, sec, nsec/1000);
+}
+
+/**
 * Smear all the timer of a given bucket to expire equi-distant.
 * Call this function periodically to avoid clustering of timers.
 */
@@ -179,8 +237,10 @@ timer_smear_bucket (timer_root_s *root, time_t sec, long nsec)
        step.tv_nsec = step_nsec - (step.tv_sec * 1e9);

        LOG(TIMER_DETAIL, "Smear %u timers in bucket %lu.%06lus\n", timer_bucket->timers, sec, nsec);
-        LOG(TIMER_DETAIL, "Now %s, last expire %s, step %s\n", timespec_format(&now),
-                          timespec_format(&last_timer->expire), timespec_format(&step));
+        LOG(TIMER_DETAIL, "Now %lu.%06lus, last expire %lu.%06lus, step %lu.%06lus\n", 
+            now.tv_sec, now.tv_nsec / 1000,
+            last_timer->expire.tv_sec, last_timer->expire.tv_nsec / 1000,
+            step.tv_sec, step.tv_nsec / 1000);

        /*
        * Now walk all timers and space them <step> apart.
@@ -188,75 +248,65 @@ timer_smear_bucket (timer_root_s *root, time_t sec, long nsec)
        CIRCLEQ_FOREACH(timer, &timer_bucket->timer_qhead, timer_qnode) {
            timespec_add(&timer->expire, &now, &step);
            now = timer->expire;
-            LOG(TIMER_DETAIL, "  Smear %s -> expire %s\n", timer->name, timespec_format(&timer->expire));
+            LOG(TIMER_DETAIL, "  Smear %s -> expire %lu.%06lus\n", timer->name, 
+                timer->expire.tv_sec, timer->expire.tv_nsec / 1000);
        }
 	    return;
    }
 }

-/*
- * Dequeue a timer from its timer_bucket.
+/**
+ * Smear all the timer of all buckets to expire equi-distant.
 */
 void
-timer_dequeue_bucket (timer_s *timer)
+timer_smear_all_buckets (timer_root_s *root)
 {
-    timer_root_s *timer_root;
    timer_bucket_s *timer_bucket;
-
-    timer_bucket = timer->timer_bucket;
-    timer_root = timer_bucket->timer_root;
-
-    CIRCLEQ_REMOVE(&timer_bucket->timer_qhead, timer, timer_qnode);
-    timer_bucket->timers--;
-    timer->timer_bucket = NULL;
+    timer_s *timer, *last_timer;
+    struct timespec now, diff, step;
+    long step_nsec;

    /*
-     * If the last timer of a bucket is gone, remove the bucket as well.
+     * Find the bucket for smearing.
     */
-    if (!timer_bucket->timers) {
-        CIRCLEQ_REMOVE(&timer_root->timer_bucket_qhead, timer_bucket, timer_bucket_qnode);
+    CIRCLEQ_FOREACH(timer_bucket, &root->timer_bucket_qhead, timer_bucket_qnode) {
+        /*
+         * Found the bucket. Next compute the timespan between now and last timer.
+         */
+        last_timer = CIRCLEQ_LAST(&timer_bucket->timer_qhead);
+        if (!last_timer) {
+            return;
+        }
+        clock_gettime(CLOCK_MONOTONIC, &now);
+        timespec_sub(&diff, &last_timer->expire, &now);
+        step_nsec = (diff.tv_sec * 1e9 + diff.tv_nsec) / (timer_bucket->timers); /* calculate smear step */
+        step.tv_sec = step_nsec / 1e9;
+        step.tv_nsec = step_nsec - (step.tv_sec * 1e9);

-        LOG(TIMER_DETAIL, "  Delete timer bucket %lu.%06lus\n",
-            timer_bucket->sec, timer_bucket->nsec/1000);
+        LOG(TIMER_DETAIL, "Smear %u timers in bucket %lu.%06lus\n", timer_bucket->timers, timer_bucket->sec, timer_bucket->nsec);
+        LOG(TIMER_DETAIL, "Now %lu.%06lus, last expire %lu.%06lus, step %lu.%06lus\n",
+            now.tv_sec, now.tv_nsec / 1000,
+            last_timer->expire.tv_sec, last_timer->expire.tv_nsec / 1000,
+            step.tv_sec, step.tv_nsec / 1000);

-        free(timer_bucket);
-        timer_root->buckets--;
+        /*
+        * Now walk all timers and space them <step> apart.
+        */
+        CIRCLEQ_FOREACH(timer, &timer_bucket->timer_qhead, timer_qnode) {
+            timespec_add(&timer->expire, &now, &step);
+            now = timer->expire;
+            LOG(TIMER_DETAIL, "  Smear %s -> expire %lu.%06lus\n", timer->name, 
+                last_timer->expire.tv_sec, last_timer->expire.tv_nsec / 1000);
+        }
+	    return;
    }
 }

-void
-timer_requeue (timer_s *timer, time_t sec, long nsec)
-{
-    timer_root_s *timer_root;
-    timer_bucket_s *timer_bucket;
-
-    timer_bucket = timer->timer_bucket;
-    timer_root = timer_bucket->timer_root;
-
-    timer_set_expire(timer, sec, nsec);
-
-    /*
-     * If the expiration {sec,nsec} matches the bucket, then simply
-     * timer dequeue and enqueue to keep correct temporal ordering.
-     * If there is no match, do a slightly more expensive
-     * bucket dequeue and enqueue.
-     */
-    if (timer_bucket->sec == sec && timer_bucket->nsec == nsec) {
-        CIRCLEQ_REMOVE(&timer_bucket->timer_qhead, timer, timer_qnode);
-        CIRCLEQ_INSERT_TAIL(&timer_bucket->timer_qhead, timer, timer_qnode);
-    } else {
-        timer_dequeue_bucket(timer);
-        timer_enqueue_bucket(timer_root, timer, sec, nsec);
-    }
-
-    LOG(TIMER_DETAIL, "  Reset %s timer, expire in %lu.%06lus\n", timer->name, sec, nsec/1000);
-}
-
-/*
+/**
 * We do not delete timers, but rather dequeue them and move them to
 * the garbage collection queue, where they may get recycled.
 */
-void
+static void
 timer_del_internal (timer_s *timer)
 {
    timer_bucket_s *timer_bucket;
@@ -276,7 +326,7 @@ timer_del_internal (timer_s *timer)
    }
 }

-/*
+/**
 * Mark a timer for deletion.
 */
 void
@@ -288,7 +338,7 @@ timer_del (timer_s *timer)
    }
 }

-/*
+/**
 * Set timer expiration.
 */
 void
@@ -309,10 +359,10 @@ timer_set_expire (timer_s *timer, time_t sec, long nsec)
    timer->expired = false;
 }

-/*
+/**
 * Deferred processing of all timers.
 */
-void
+static void
 timer_process_changes (timer_root_s *root)
 {
    timer_s *timer;
@@ -347,7 +397,7 @@ timer_process_changes (timer_root_s *root)
    }
 }

-/*
+/**
 * Enqueue a timer with a given callback function onto the hierarchical timer list.
 */
 void
@@ -369,9 +419,9 @@ timer_add (timer_root_s *root,
    if (timer) {
        timer_requeue(timer, sec, nsec);
        /*
-        * Update data and cb if there was a change.
-        * Do the reformatting of name only during a change due to snprintf() being expensive.
-        */
+         * Update data and cb if there was a change.
+         * Do the reformatting of name only during a change due to snprintf() being expensive.
+         */
        if (timer->data != data || timer->cb != cb) {
            snprintf(timer->name, sizeof(timer->name), "%s", name);
            timer->data = data;
@@ -433,7 +483,7 @@ timer_add_periodic (timer_root_s *root, timer_s **ptimer, char *name,
    }
 }

-/*
+/**
 * Compare two timespecs.
 *
 * return -1 if ts1 is older than ts2
@@ -462,8 +512,10 @@ timespec_compare (struct timespec *ts1, struct timespec *ts2)
    return 0;
 }

-/*
+/**
 * Process the timer queue.
+ * 
+ * @param root timer root
 */
 void
 timer_walk (timer_root_s *root)
@@ -473,138 +525,132 @@ timer_walk (timer_root_s *root)
    struct timespec now, min, sleep, rem;
    int res;

-    while (true) {
+    /*
+     * No buckets filled and we're done.
+     */
+    if (CIRCLEQ_EMPTY(&root->timer_bucket_qhead)) {
+        return;
+    }
+
+    clock_gettime(CLOCK_MONOTONIC, &now);
+    LOG(TIMER_DETAIL, "Walk timer queue, now %lu.%06lus\n",
+        now.tv_sec, now.tv_nsec / 1000);
+    min.tv_sec = 0;
+    min.tv_nsec = 0;
+
+    /*
+     * Walk all buckets.
+     */
+    CIRCLEQ_FOREACH(timer_bucket, &root->timer_bucket_qhead, timer_bucket_qnode) {
+
+        LOG(TIMER_DETAIL, "  Checking timer bucket %lu.%06lus\n",
+            timer_bucket->sec, timer_bucket->nsec/1000);

        /*
-         * No buckets filled and we're done.
+         * First pass. Call into expired nodes.
         */
-        if (CIRCLEQ_EMPTY(&root->timer_bucket_qhead)) {
-            return;
-        }
-
-        clock_gettime(CLOCK_MONOTONIC, &now);
-        LOG(TIMER_DETAIL, "Walk timer queue, now %s\n", timespec_format(&now));
-        min.tv_sec = 0;
-        min.tv_nsec = 0;
-
-        /*
-         * Walk all buckets.
-         */
-        CIRCLEQ_FOREACH(timer_bucket, &root->timer_bucket_qhead, timer_bucket_qnode) {
-
-            LOG(TIMER_DETAIL, "  Checking timer bucket %lu.%06lus\n",
-                timer_bucket->sec, timer_bucket->nsec/1000);
+        CIRCLEQ_FOREACH(timer, &timer_bucket->timer_qhead, timer_qnode) {

            /*
-             * First pass. Call into expired nodes.
+             * Hitting the first non-expired timer means
+             * we're done processing this buckets queue.
             */
-            CIRCLEQ_FOREACH(timer, &timer_bucket->timer_qhead, timer_qnode) {
+            if (timespec_compare(&timer->expire, &now) == 1) {
+                break;
+            }

-                /*
-                 * Hitting the first non-expired timer means
-                 * we're done processing this buckets queue.
-                 */
-                if ((timespec_compare(&timer->expire, &now) == 1)) {
-                    break;
-                }
+            /*
+             * Everything from here one is expired.
+             */
+            timer->expired = true;

-                /*
-                 * Everything from here one is expired.
-                 */
-                timer->expired = true;
-
-                /* Execute callback */
-                if (timer->cb) {
-                    LOG(TIMER_DETAIL, "  Firing %s timer\n", timer->name);
+            /* Execute callback */
+            if (timer->cb) {
+                LOG(TIMER_DETAIL, "  Firing %s timer\n", timer->name);
                    (*timer->cb)(timer);
-                }
+            }

-                if (timer->periodic) {
-                    /*
-                     * Periodic timers are simple de-queued and
-                     * re-inserted at the tail of this buckets queue.
-                     */
-		            timer_change(timer);
-                } else {
-                    /*
-                     * Everything else gets deleted.
-                     */
-                    timer_del(timer);
-                }
+            if (timer->periodic) {
+                /*
+                 * Periodic timers are simple de-queued and
+                 * re-inserted at the tail of this buckets queue.
+                 */
+                timer_change(timer);
+            } else {
+                /*
+                 * Everything else gets deleted.
+                 */
+                timer_del(timer);
            }
        }
+    }

-        /*
-        * Process all changes from the last timer run.
-        */
-        timer_process_changes(root);
+    /*
+     * Process all changes from the last timer run.
+     */
+    timer_process_changes(root);

-        /*
-        * Second pass. Figure out min sleep time.
-        */
-        CIRCLEQ_FOREACH(timer_bucket, &root->timer_bucket_qhead, timer_bucket_qnode) {
-            CIRCLEQ_FOREACH(timer, &timer_bucket->timer_qhead, timer_qnode) {
+    /*
+     * Second pass. Figure out min sleep time.
+     */
+    CIRCLEQ_FOREACH(timer_bucket, &root->timer_bucket_qhead, timer_bucket_qnode) {
+        CIRCLEQ_FOREACH(timer, &timer_bucket->timer_qhead, timer_qnode) {

-                /*
-                * Ignore deleted timers that wait for change processing.
-                */
-                if (timer->delete) {
-                    continue;
-                }
+            /*
+             * Ignore deleted timers that wait for change processing.
+             */
+            if (timer->delete) {
+                continue;
+            }

-                /*
-                * First timer in the queue becomes the actual minimum.
-                */
-                if (min.tv_sec == 0 && min.tv_nsec == 0) {
-                    min.tv_sec = timer->expire.tv_sec;
-                    min.tv_nsec = timer->expire.tv_nsec;
-                }
+            /*
+             * First timer in the queue becomes the actual minimum.
+             */
+            if (min.tv_sec == 0 && min.tv_nsec == 0) {
+                min.tv_sec = timer->expire.tv_sec;
+                min.tv_nsec = timer->expire.tv_nsec;
+            }

-                /*
-                 * Find the min timer.
-                 */
-                if (timespec_compare(&timer->expire, &min) == -1) {
-                    min.tv_sec = timer->expire.tv_sec;
-                    min.tv_nsec = timer->expire.tv_nsec;
-                    LOG(TIMER_DETAIL, "New minimum sleep (%s) timer, found %s\n",
-                        timer->name, timespec_format(&min));
-                }
+            /*
+             * Find the min timer.
+             */
+            if (timespec_compare(&timer->expire, &min) == -1) {
+                min.tv_sec = timer->expire.tv_sec;
+                min.tv_nsec = timer->expire.tv_nsec;
+                LOG(TIMER_DETAIL, "New minimum sleep (%s) timer, found %lu.%06lus\n",
+                    timer->name,
+                    min.tv_sec, min.tv_nsec / 1000);
+            }

-                /*
-                 * Hitting the first non-expired timer means
-                 * we're done processing this buckets queue.
-                 */
-                if ((timespec_compare(&timer->expire, &now) == 1)) {
-                    break;
-                }
+            /*
+             * Hitting the first non-expired timer means
+             * we're done processing this buckets queue.
+             */
+            if (timespec_compare(&timer->expire, &now) == 1) {
+                break;
            }
        }
+    }

-        /*
-        * Calculate the sleep timer.
-        */
-        LOG(TIMER_DETAIL, "  Now %s\n", timespec_format(&now));
-        LOG(TIMER_DETAIL, "  Min %s\n", timespec_format(&min));
+    /*
+     * Calculate the sleep timer.
+     */
+    LOG(TIMER_DETAIL, "  Now %lu.%06lus\n", now.tv_sec, now.tv_nsec / 1000);
+    LOG(TIMER_DETAIL, "  Min %lu.%06lus\n", min.tv_sec, min.tv_nsec / 1000);

-        clock_gettime(CLOCK_MONOTONIC, &now);
-        if (timespec_compare(&now, &min) == -1) {
-            timespec_sub(&sleep, &min, &now);
-        } else {
-            //sleep.tv_sec = 0;
-            //sleep.tv_nsec = 1 * MSEC; /* sleep time is negative, sleep at least some time */
-            continue;
-        }
+    clock_gettime(CLOCK_MONOTONIC, &now);
+    if (timespec_compare(&now, &min) == -1) {
+        timespec_sub(&sleep, &min, &now);

-        LOG(TIMER_DETAIL, "  Sleep %s\n", timespec_format(&sleep));
+        LOG(TIMER_DETAIL, "  Sleep %lu.%06lus\n", sleep.tv_sec, sleep.tv_nsec / 1000);
        res = nanosleep(&sleep, &rem);
        if (res == -1) {
            LOG(TIMER, "  nanosleep(): error %s (%d)\n", strerror(errno), errno);
-            return;
        }
    }
 }

-/*
+/**
 * Init a timer root.
 */
 void
@@ -628,9 +674,9 @@ timer_flush_root (timer_root_s *timer_root)
     * First step. Walk all timers and move them onto the GC thread.
     */
    CIRCLEQ_FOREACH(timer_bucket, &timer_root->timer_bucket_qhead, timer_bucket_qnode) {
-	CIRCLEQ_FOREACH(timer, &timer_bucket->timer_qhead, timer_qnode) {
-	    timer_del(timer);
-	}
+        CIRCLEQ_FOREACH(timer, &timer_bucket->timer_qhead, timer_qnode) {
+            timer_del(timer);
+        }
    }
    timer_process_changes(timer_root);

@@ -640,8 +686,8 @@ timer_flush_root (timer_root_s *timer_root)
    while (!CIRCLEQ_EMPTY(&timer_root->timer_gc_qhead)) {
        timer = CIRCLEQ_FIRST(&timer_root->timer_gc_qhead);
        CIRCLEQ_REMOVE(&timer_root->timer_gc_qhead, timer, timer_qnode);
-	timer_root->gc--;
-	free(timer);
+        timer_root->gc--;
+        free(timer);
    }
 }

@@ -690,4 +736,4 @@ timer_test (void *ctx)
    timer_smear_bucket(&root, 10, 0);

    timer_walk(&root);
-}
+}
@@ -10,7 +10,8 @@
 #ifndef __BBL_TIMER_H__
 #define __BBL_TIMER_H__

-#define MSEC 1000*1000 /* 1 million nanoseconds */
+#define MSEC 1000000 /* 1 million nanoseconds == 1 msec */
+#define SEC 1000000000 /* 1 billion nanoseconds == 1 sec */

 /*
 * Top level data structure for timers.
@@ -80,8 +81,8 @@ void timer_add(timer_root_s *, timer_s **, char *, time_t , long , void *, void
 void timer_add_periodic(timer_root_s *, timer_s **, char *, time_t , long , void *, void *);
 void timer_del(timer_s *);
 void timer_smear_bucket(timer_root_s *, time_t, long);
+void timer_smear_all_buckets (timer_root_s *root);
 void timer_walk(struct timer_root_ *);
-
 void timespec_add(struct timespec *, struct timespec *, struct timespec *);
 void timespec_sub(struct timespec *, struct timespec *, struct timespec *);

@@ -538,12 +538,12 @@ bbl_encode_packet_icmpv6_rs (bbl_session_s *session) {
    bbl_pppoe_session_t pppoe = {0};
    bbl_ipv6_t ipv6 = {0};
    bbl_icmpv6_t icmpv6 = {0};
+    uint8_t mac[ETH_ADDR_LEN];

    interface = session->interface;
    ctx = interface->ctx;
    interface->stats.icmpv6_tx++;

-    eth.dst = session->server_mac;
    eth.src = session->client_mac;
    eth.qinq = session->access_config->qinq;
    eth.vlan_outer = session->vlan_key.outer_vlan_id;
@@ -555,13 +555,17 @@ bbl_encode_packet_icmpv6_rs (bbl_session_s *session) {
        if(session->ip6cp_state != BBL_PPP_OPENED) {
            return WRONG_PROTOCOL_STATE;
        }
+        eth.dst = session->server_mac;
        eth.type = ETH_TYPE_PPPOE_SESSION;
        eth.next = &pppoe;
+
        pppoe.session_id = session->pppoe_session_id;
        pppoe.protocol = PROTOCOL_IPV6;
        pppoe.next = &ipv6;
    } else {
        /* IPoE */
+        ipv6_multicast_mac(ipv6_multicast_all_routers, mac);
+        eth.dst = mac;
        eth.type = ETH_TYPE_IPV6;
        eth.next = &ipv6;
    }
@@ -616,6 +620,7 @@ bbl_encode_packet_dhcpv6_request (bbl_session_s *session) {
    bbl_udp_t udp = {0};
    bbl_dhcpv6_t dhcpv6 = {0};
    access_line_t access_line = {0};
+    uint8_t mac[ETH_ADDR_LEN];

    if(session->dhcpv6_state == BBL_DHCP_INIT ||
       session->dhcpv6_state == BBL_DHCP_BOUND) {
@@ -649,7 +654,8 @@ bbl_encode_packet_dhcpv6_request (bbl_session_s *session) {
        pppoe.next = &ipv6;
    } else {
        /* IPoE */
-        eth.dst = (void*)ipv6_multicast_mac_dhcp;
+        ipv6_multicast_mac(ipv6_multicast_all_dhcp, mac);
+        eth.dst = mac;
        eth.vlan_outer_priority = ctx->config.dhcpv6_vlan_priority;
        eth.type = ETH_TYPE_IPV6;
        eth.next = &ipv6;
@@ -1628,6 +1634,7 @@ bbl_encode_interface_packet (bbl_interface_s *interface, uint8_t *buf, uint16_t
    bbl_arp_t arp = {0};
    bbl_ipv6_t ipv6 = {0};
    bbl_icmpv6_t icmpv6 = {0};
+    uint8_t mac[ETH_ADDR_LEN];

    *len = 0;

@@ -1649,15 +1656,12 @@ bbl_encode_interface_packet (bbl_interface_s *interface, uint8_t *buf, uint16_t
        result = encode_ethernet(buf, len, &eth);
    } else if(interface->send_requests & BBL_IF_SEND_ICMPV6_NS) {
        interface->send_requests &= ~BBL_IF_SEND_ICMPV6_NS;
-        if(*(uint32_t*)interface->gateway_mac == 0) {
-            eth.dst = (uint8_t*)ipv6_multicast_mac;
-        } else {
-            eth.dst = interface->gateway_mac;
-        }
+        ipv6_multicast_mac(interface->gateway6_solicited_node_multicast, mac);
+        eth.dst = mac;
        eth.type = ETH_TYPE_IPV6;
        eth.next = &ipv6;
        ipv6.src = interface->ip6.address;
-        ipv6.dst = interface->gateway6.address;
+        ipv6.dst = interface->gateway6_solicited_node_multicast;
        ipv6.protocol = IPV6_NEXT_HEADER_ICMPV6;
        ipv6.next = &icmpv6;
        ipv6.ttl = 255;
@@ -1778,7 +1782,7 @@ bbl_tx(bbl_ctx_s *ctx, bbl_interface_s *interface, uint8_t *buf, uint16_t *len)
                return PROTOCOL_SUCCESS;
            }
            /* Write Multicast frames. */
-            if(ctx->config.send_multicast_traffic && ctx->config.igmp_group_count && ctx->multicast_traffic ) {
+            if(ctx->config.send_multicast_traffic && ctx->config.igmp_group_count && ctx->multicast_traffic) {
                if(interface->mc_packet_cursor < ctx->config.igmp_group_count) {
                    memcpy(buf, interface->mc_packets + (interface->mc_packet_cursor*interface->mc_packet_len), interface->mc_packet_len);
                    *(uint64_t*)(buf + (interface->mc_packet_len - 16)) = interface->mc_packet_seq;
@@ -1786,10 +1790,11 @@ bbl_tx(bbl_ctx_s *ctx, bbl_interface_s *interface, uint8_t *buf, uint16_t *len)
                    *(uint32_t*)(buf + (interface->mc_packet_len - 4)) = interface->tx_timestamp.tv_nsec;
                    *len = interface->mc_packet_len;
                    interface->mc_packet_cursor++;
+                    interface->stats.mc_tx++;
                    return PROTOCOL_SUCCESS;
                } else {
                    /* This must be the last send operation in this function to fill up remaining slots
-                    * with multicast traffic but all other types of traffic have priority. */
+                     * with multicast traffic but all other types of traffic have priority. */
                    interface->mc_packet_cursor = 0;
                    interface->mc_packet_seq++;
                }
@@ -164,3 +164,31 @@ replace_substring (const char* source,
    *result_pos = '\0';
    return result;
 }
+
+/** 
+ * ipv4_multicast_mac
+ *
+ * @param ipv4 IPv4 multicast address
+ * @param mac target buffer to store multicast MAC
+ */
+void
+ipv4_multicast_mac(const uint32_t ipv4, uint8_t* mac) {
+    *(uint32_t*)(&mac[2]) = ipv4;
+    mac[0] = 0x01;
+    mac[1] = 0x00;
+    mac[2] = 0x5e;
+    mac[3] &= 0x7f;
+}
+
+/** 
+ * ipv6_multicast_mac
+ *
+ * @param ipv6 IPv6 multicast address
+ * @param mac target buffer to store multicast MAC
+ */
+void
+ipv6_multicast_mac(const uint8_t *ipv6, uint8_t* mac) {
+    *(uint32_t*)(&mac[2]) =*(uint32_t*)(&ipv6[12]);
+    mac[0] = 0x33;
+    mac[1] = 0x33;
+}
@@ -21,7 +21,9 @@ char *format_mac_address(uint8_t *mac);
 char *format_ipv4_address(uint32_t *addr4);
 char *format_ipv6_address(ipv6addr_t *addr6);
 char *format_ipv6_prefix(ipv6_prefix *addr6);
-
+void ipv4_multicast_mac(const uint32_t ipv4, uint8_t* mac);
+void ipv6_multicast_mac(const uint8_t *ipv6, uint8_t* mac);
 char *replace_substring (const char* s, const char* old, const char* new);
 const char *val2key (struct keyval_ *keyval, uint val);
-#endif
+
+#endif
@@ -77,6 +77,34 @@ test_replace_substring(void **unused) {
    assert_string_equal(replace_substring("1234{long-variable-name}567890", "{long-variable-name}", ""), "1234567890"); 
 }

+static void 
+test_ipv4_multicast_mac(void **unused) {
+    (void) unused;
+
+    uint32_t ipv4;
+    uint8_t mac[ETH_ADDR_LEN] = {0};
+    uint8_t mac_expected[ETH_ADDR_LEN] = {0x01, 0x00, 0x5e, 0x01, 0x02, 0x03};
+
+    inet_pton(AF_INET, "239.1.2.3", &ipv4);
+    ipv4_multicast_mac(ipv4, mac);
+
+    assert_memory_equal(mac_expected, mac, ETH_ADDR_LEN);
+}
+
+static void 
+test_ipv6_multicast_mac(void **unused) {
+    (void) unused;
+
+    ipv6addr_t ipv6;
+    uint8_t mac[ETH_ADDR_LEN] = {0};
+    uint8_t mac_expected[ETH_ADDR_LEN] = {0x33, 0x33, 0x01, 0x02, 0x03, 0x04};
+
+    inet_pton(AF_INET6, "ff02::0102:0304", ipv6);
+    ipv6_multicast_mac(ipv6, mac);
+
+    assert_memory_equal(mac_expected, mac, ETH_ADDR_LEN);
+}
+
 int main() {
    const struct CMUnitTest tests[] = {
        cmocka_unit_test(test_val2key),
@@ -85,6 +113,8 @@ int main() {
        cmocka_unit_test(test_format_ipv6_address),
        cmocka_unit_test(test_format_ipv6_prefix),
        cmocka_unit_test(test_replace_substring),
+        cmocka_unit_test(test_ipv4_multicast_mac),
+        cmocka_unit_test(test_ipv6_multicast_mac),
    };
    return cmocka_run_group_tests(tests, NULL, NULL);
-}
+}