From 15e17ecc703153fa8ecddb3c8d13cf2d67223f06 Mon Sep 17 00:00:00 2001
From: Christian Giese <christian@rtbrick.com>
Date: Thu, 3 Mar 2022 22:46:59 +0100
Subject: [PATCH] BGP Initial (WIP)

---
 code/bngblaster/src/bbl.c           |   9 ++
 code/bngblaster/src/bbl.h           |   2 +
 code/bngblaster/src/bbl_config.c    | 104 ++++++++++++
 code/bngblaster/src/bbl_config.h    |   3 +-
 code/bngblaster/src/bbl_ctx.c       |   9 +-
 code/bngblaster/src/bbl_ctx.h       |  18 +++
 code/bngblaster/src/bbl_def.h       |   1 +
 code/bngblaster/src/bbl_interface.h |   1 +
 code/bngblaster/src/bbl_protocols.c |  36 +++++
 code/bngblaster/src/bbl_protocols.h |  10 ++
 code/bngblaster/src/bbl_rx.c        |  14 --
 code/bngblaster/src/isis/isis_def.h |   2 +-
 src/bbl_tcp.c                       | 241 ++++++++++++++++++++--------
 src/bbl_tcp.h                       |  49 +++---
 src/bgp/bgp.c                       | 132 +++++++++++++++
 src/bgp/bgp.h                       |  20 +++
 src/bgp/bgp_def.h                   |  80 +++++++++
 src/bgp/bgp_message.c               | 102 ++++++++++++
 src/bgp/bgp_message.h               |  15 ++
 src/bgp/bgp_mrt.h                   |  12 ++
 src/lwip/lwipopts.h                 |   2 +-
 21 files changed, 754 insertions(+), 108 deletions(-)
 create mode 100644 src/bgp/bgp.c
 create mode 100644 src/bgp/bgp.h
 create mode 100644 src/bgp/bgp_def.h
 create mode 100644 src/bgp/bgp_message.c
 create mode 100644 src/bgp/bgp_message.h
 create mode 100644 src/bgp/bgp_mrt.h

diff --git a/code/bngblaster/src/bbl.c b/code/bngblaster/src/bbl.c
index a31d88e4..88d4f19c 100644
--- a/code/bngblaster/src/bbl.c
+++ b/code/bngblaster/src/bbl.c
@@ -564,6 +564,15 @@ main(int argc, char *argv[])
         exit(1);
     }
 
+    /* Init TCP. */
+    bbl_tcp_init(ctx);
+
+    /* Init BGP sessions. */
+    if(!bgp_init(ctx)) {
+        fprintf(stderr, "Error: Failed to init BGP\n");
+        exit(1);
+    }
+
     /* Start curses. */
     if (interactive) {
         bbl_init_curses(ctx);
diff --git a/code/bngblaster/src/bbl.h b/code/bngblaster/src/bbl.h
index 58e54691..e47fdc47 100644
--- a/code/bngblaster/src/bbl.h
+++ b/code/bngblaster/src/bbl.h
@@ -30,6 +30,7 @@
 #include "bbl_def.h"
 
 #include "bbl_protocols.h"
+#include "bgp/bgp_def.h"
 #include "isis/isis_def.h"
 
 #include "bbl_stats.h"
@@ -43,6 +44,7 @@
 #include "bbl_a10nsp.h"
 #include "bbl_li.h"
 #include "bbl_tcp.h"
+#include "bgp/bgp.h"
 #include "isis/isis.h"
 
 WINDOW *log_win;
diff --git a/code/bngblaster/src/bbl_config.c b/code/bngblaster/src/bbl_config.c
index 812219ae..088e4916 100644
--- a/code/bngblaster/src/bbl_config.c
+++ b/code/bngblaster/src/bbl_config.c
@@ -282,6 +282,18 @@ json_parse_network_interface(bbl_ctx_s *ctx, json_t *network_interface, bbl_netw
         network_config->vlan &= 4095;
     }
 
+    value = json_object_get(network_interface, "mtu");
+    if (json_is_number(value)) {
+        network_config->mtu = json_number_value(value);
+    } else {
+        network_config->mtu = 1500;
+    }
+
+    if(network_config->mtu < 64 || network_config->mtu > 9000) {
+        fprintf(stderr, "JSON config error: Invalid value for network->mtu\n");
+        return false;
+    }
+
     value = json_object_get(network_interface, "gateway-resolve-wait");
     if (json_is_boolean(value)) {
         network_config->gateway_resolve_wait = json_boolean_value(value);
@@ -679,6 +691,69 @@ json_parse_a10nsp_interface(bbl_ctx_s *ctx, json_t *a10nsp_interface, bbl_a10nsp
     return true;
 }
 
+static bool
+json_parse_bgp_config(bbl_ctx_s *ctx, json_t *bgp, bgp_config_t *bgp_config) {
+    json_t *value = NULL;
+    const char *s = NULL;    
+    
+    ctx->tcp = true;
+
+    if (json_unpack(bgp, "{s:s}", "source-ipv4-address", &s) == 0) {
+        if (!inet_pton(AF_INET, s, &bgp_config->ipv4_src_address)) {
+            fprintf(stderr, "JSON config error: Invalid value for bgp->source-ipv4-address\n");
+            return false;
+        }
+        add_secondary_ipv4(ctx, bgp_config->ipv4_src_address);
+    }
+
+    if (json_unpack(bgp, "{s:s}", "destination-ipv4-address", &s) == 0) {
+        if (!inet_pton(AF_INET, s, &bgp_config->ipv4_dst_address)) {
+            fprintf(stderr, "JSON config error: Invalid value for bgp->destination-ipv4-address\n");
+            return false;
+        }
+    } else {
+        fprintf(stderr, "JSON config error: Missing value for bgp->destination-ipv4-address\n");
+        return false;   
+    }
+
+    value = json_object_get(bgp, "local-as");
+    if (value) {
+        bgp_config->local_as = json_number_value(value);
+    } else {
+        bgp_config->local_as = BGP_DEFAULT_AS;
+    }
+
+    value = json_object_get(bgp, "peer-as");
+    if (value) {
+        bgp_config->peer_as = json_number_value(value);
+    } else {
+        bgp_config->peer_as = bgp_config->local_as;
+    }
+
+    value = json_object_get(bgp, "holdtime");
+    if (value) {
+        bgp_config->holdtime = json_number_value(value);
+    } else {
+        bgp_config->holdtime = BGP_DEFAULT_HOLDTIME;
+    }
+
+    bgp_config->id = htobe32(0x01020304);
+    if (json_unpack(bgp, "{s:s}", "id", &s) == 0) {
+        if (!inet_pton(AF_INET, s, &bgp_config->id)) {
+            fprintf(stderr, "JSON config error: Invalid value for bgp->id\n");
+            return false;
+        }
+    } else {
+
+    }
+
+
+    if (json_unpack(bgp, "{s:s}", "mrt-file", &s) == 0) {
+        bgp_config->mrt_file = strdup(s);
+    }
+    return true;
+}
+
 static bool
 json_parse_isis_config(bbl_ctx_s *ctx, json_t *isis, isis_config_t *isis_config) {
     json_t *sub, *con, *c, *value = NULL;
@@ -1181,6 +1256,7 @@ json_parse_config(json_t *root, bbl_ctx_s *ctx) {
     bbl_access_config_s         *access_config          = NULL;
     bbl_a10nsp_config_s         *a10nsp_config          = NULL;
 
+    bgp_config_t                *bgp_config             = NULL;
     isis_config_t               *isis_config            = NULL;
 
     if (json_typeof(root) != JSON_OBJECT) {
@@ -1581,6 +1657,34 @@ json_parse_config(json_t *root, bbl_ctx_s *ctx) {
         }
     }
 
+    /* BGP Configuration */
+    sub = json_object_get(root, "bgp");
+    if (json_is_array(sub)) {
+        /* Config is provided as array (multiple bgp sessions) */
+        size = json_array_size(sub);
+        for (i = 0; i < size; i++) {
+            if (!bgp_config) {
+                ctx->config.bgp_config = calloc(1, sizeof(bgp_config_t));
+                bgp_config = ctx->config.bgp_config;
+            } else {
+                bgp_config->next = calloc(1, sizeof(bgp_config_t));
+                bgp_config = bgp_config->next;
+            }
+            if (!json_parse_bgp_config(ctx, json_array_get(sub, i), bgp_config)) {
+                return false;
+            }
+        }
+    } else if (json_is_object(sub)) {
+        /* Config is provided as object (single bgp session) */
+        bgp_config = calloc(1, sizeof(bgp_config_t));
+        if (!ctx->config.bgp_config) {
+            ctx->config.bgp_config = bgp_config;
+        }
+        if (!json_parse_bgp_config(ctx, sub, bgp_config)) {
+            return false;
+        }
+    }
+
     /* IS-IS Configuration */
     sub = json_object_get(root, "isis");
     if (json_is_array(sub)) {
diff --git a/code/bngblaster/src/bbl_config.h b/code/bngblaster/src/bbl_config.h
index d7193956..4bc63045 100644
--- a/code/bngblaster/src/bbl_config.h
+++ b/code/bngblaster/src/bbl_config.h
@@ -87,7 +87,8 @@ typedef struct bbl_network_config_
     uint8_t gateway_mac[ETH_ADDR_LEN];
     bool gateway_resolve_wait;
     uint16_t vlan;
-
+    uint16_t mtu;
+    
     ipv4_prefix ip;
     ipv4addr_t gateway;
 
diff --git a/code/bngblaster/src/bbl_ctx.c b/code/bngblaster/src/bbl_ctx.c
index 3159e498..3d6cb368 100644
--- a/code/bngblaster/src/bbl_ctx.c
+++ b/code/bngblaster/src/bbl_ctx.c
@@ -21,7 +21,7 @@ bbl_compare_key32 (void *key1, void *key2)
 }
 
 uint32_t
-bbl_key32_hash (const void* k)
+bbl_key32_hash(const void* k)
 {
     uint32_t hash = 2166136261U;
     hash ^= *(uint32_t *)k;
@@ -29,7 +29,7 @@ bbl_key32_hash (const void* k)
 }
 
 int
-bbl_compare_key64 (void *key1, void *key2)
+bbl_compare_key64(void *key1, void *key2)
 {
     const uint64_t a = *(const uint64_t*)key1;
     const uint64_t b = *(const uint64_t*)key2;
@@ -37,7 +37,7 @@ bbl_compare_key64 (void *key1, void *key2)
 }
 
 uint32_t
-bbl_key64_hash (const void* k)
+bbl_key64_hash(const void* k)
 {
     uint32_t hash = 2166136261U;
 
@@ -85,9 +85,6 @@ bbl_ctx_add (void)
     ctx->li_flow_dict = hashtable_dict_new((dict_compare_func)bbl_compare_key32, bbl_key32_hash, BBL_LI_HASHTABLE_SIZE);
     ctx->stream_flow_dict = hashtable_dict_new((dict_compare_func)bbl_compare_key64, bbl_key64_hash, BBL_STREAM_FLOW_HASHTABLE_SIZE);
 
-    /* Initialize TCP. */
-    bbl_tcp_init(ctx);
-
     return ctx;
 }
 
diff --git a/code/bngblaster/src/bbl_ctx.h b/code/bngblaster/src/bbl_ctx.h
index d1d7062d..39833322 100644
--- a/code/bngblaster/src/bbl_ctx.h
+++ b/code/bngblaster/src/bbl_ctx.h
@@ -84,6 +84,8 @@ typedef struct bbl_ctx_
 
     void *stream_thread; /* single linked list of threads */
 
+    bool tcp;
+
     /* Interfaces */
     struct {
         uint8_t count;
@@ -99,6 +101,7 @@ typedef struct bbl_ctx_
         struct bbl_interface_ *a10nsp_if[BBL_MAX_INTERFACES];
     } interfaces;
 
+    bgp_session_t *bgp_sessions;
     isis_instance_t *isis_instances;
 
     /* Scratchpad memory */
@@ -171,6 +174,9 @@ typedef struct bbl_ctx_
         /* Traffic Streams */
         void *stream_config;
 
+        /* BGP Instances */
+        bgp_config_t *bgp_config;
+
         /* ISIS Instances */
         isis_config_t *isis_config;
 
@@ -301,6 +307,18 @@ typedef struct bbl_ctx_
     } config;
 } bbl_ctx_s;
 
+int
+bbl_compare_key32 (void *key1, void *key2);
+
+uint
+bbl_key32_hash(const void* k);
+
+int
+bbl_compare_key64(void *key1, void *key2);
+
+uint
+bbl_key64_hash(const void* k);
+
 bbl_ctx_s *
 bbl_ctx_add(void);
 
diff --git a/code/bngblaster/src/bbl_def.h b/code/bngblaster/src/bbl_def.h
index c1a732f3..6fee0382 100644
--- a/code/bngblaster/src/bbl_def.h
+++ b/code/bngblaster/src/bbl_def.h
@@ -173,5 +173,6 @@ 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;
+typedef struct bbl_tcp_ctx_ bbl_tcp_ctx_t;
 
 #endif
\ No newline at end of file
diff --git a/code/bngblaster/src/bbl_interface.h b/code/bngblaster/src/bbl_interface.h
index 4c64ca83..a1608af6 100644
--- a/code/bngblaster/src/bbl_interface.h
+++ b/code/bngblaster/src/bbl_interface.h
@@ -86,6 +86,7 @@ typedef struct bbl_interface_
     uint16_t mc_packet_cursor;
 
     struct netif netif; /* LwIP network interface */
+    dict *tcp_connections; /* hashtable for tcp_connections */
 
     isis_adjacency_p2p_t *isis_adjacency_p2p;
     isis_adjacency_t     *isis_adjacency[ISIS_LEVELS];
diff --git a/code/bngblaster/src/bbl_protocols.c b/code/bngblaster/src/bbl_protocols.c
index a8715e84..bb43f97e 100644
--- a/code/bngblaster/src/bbl_protocols.c
+++ b/code/bngblaster/src/bbl_protocols.c
@@ -2555,6 +2555,35 @@ decode_udp(uint8_t *buf, uint16_t len,
     return ret_val;
 }
 
+/*
+ * decode_tcp
+ */
+protocol_error_t
+decode_tcp(uint8_t *buf, uint16_t len,
+           uint8_t *sp, uint16_t sp_len,
+           bbl_tcp_t **_tcp) {
+
+    protocol_error_t ret_val = PROTOCOL_SUCCESS;
+
+    bbl_tcp_t *tcp;
+
+    if(len < 20 || sp_len < sizeof(bbl_tcp_t)) {
+        return DECODE_ERROR;
+    }
+
+    /* Init UDP header */
+    tcp = (bbl_tcp_t*)sp; BUMP_BUFFER(sp, sp_len, sizeof(bbl_tcp_t));
+
+    tcp->src = be16toh(*(uint16_t*)buf);
+    BUMP_BUFFER(buf, len, sizeof(uint16_t));
+    tcp->dst = be16toh(*(uint16_t*)buf);
+    BUMP_BUFFER(buf, len, sizeof(uint16_t));
+
+    *_tcp = tcp;
+    return ret_val;
+}
+
+
 /*
  * decode_ipv6
  */
@@ -2607,6 +2636,9 @@ decode_ipv6(uint8_t *buf, uint16_t len,
         case IPV6_NEXT_HEADER_UDP:
             ret_val = decode_udp(buf, len, sp, sp_len, (bbl_udp_t**)&ipv6->next);
             break;
+        case IPV6_NEXT_HEADER_TCP:
+            ret_val = decode_tcp(buf, len, sp, sp_len, (bbl_tcp_t**)&ipv6->next);
+            break;
         default:
             ipv6->next = NULL;
             break;
@@ -2703,6 +2735,10 @@ decode_ipv4(uint8_t *buf, uint16_t len,
         case PROTOCOL_IPV4_UDP:
             ret_val = decode_udp(buf, len, sp, sp_len, (bbl_udp_t**)&ipv4->next);
             break;
+        case PROTOCOL_IPV4_TCP:
+            ret_val = decode_tcp(buf, len, sp, sp_len, (bbl_tcp_t**)&ipv4->next);
+            break;
+
         default:
             ipv4->next = NULL;
             break;
diff --git a/code/bngblaster/src/bbl_protocols.h b/code/bngblaster/src/bbl_protocols.h
index 1af06c5d..e6ec38e9 100644
--- a/code/bngblaster/src/bbl_protocols.h
+++ b/code/bngblaster/src/bbl_protocols.h
@@ -675,6 +675,16 @@ typedef struct bbl_udp_ {
     uint16_t    payload_len; /* UDP payload length */
 } bbl_udp_t;
 
+/*
+ * TCP Structure
+ */
+typedef struct bbl_tcp_ {
+    uint16_t    src;
+    uint16_t    dst;
+    uint16_t    len; /* TCP total length */
+    uint8_t    *hdr; /* TCP header start */
+} bbl_tcp_t;
+
 /*
  * IGMP Structure
  */
diff --git a/code/bngblaster/src/bbl_rx.c b/code/bngblaster/src/bbl_rx.c
index 7fa7870e..dee50815 100644
--- a/code/bngblaster/src/bbl_rx.c
+++ b/code/bngblaster/src/bbl_rx.c
@@ -1781,27 +1781,13 @@ bbl_rx_handler_access(bbl_ethernet_header_t *eth, bbl_interface_s *interface) {
     }
 }
 
-static void
-bbl_rx_test_tcp(bbl_interface_s *interface) {
-    static uint8_t test_packet[1024] = {0};
-    LOG(TCP, "START TCP SESSION on interface %s\n", interface->name);
-    bbl_tcp_t *tcp = bbl_tcp_ipv4_connect(interface, &interface->ip.address, &interface->gateway, 179);
-    if(tcp) {
-        bbl_tcp_send(tcp, test_packet, sizeof(test_packet));
-    }
-}
-
 static void
 bbl_rx_network_arp(bbl_ethernet_header_t *eth, bbl_interface_s *interface) {
     bbl_secondary_ip_s *secondary_ip;
 
     bbl_arp_t *arp = (bbl_arp_t*)eth->next;
     if(arp->sender_ip == interface->gateway) {
-        if(!interface->arp_resolved) {
-            bbl_rx_test_tcp(interface);
-        }
         interface->arp_resolved = true;
-
         if(*(uint32_t*)interface->gateway_mac == 0) {
             memcpy(interface->gateway_mac, arp->sender, ETH_ADDR_LEN);
         }
diff --git a/code/bngblaster/src/isis/isis_def.h b/code/bngblaster/src/isis/isis_def.h
index a9b73912..aa39a685 100644
--- a/code/bngblaster/src/isis/isis_def.h
+++ b/code/bngblaster/src/isis/isis_def.h
@@ -185,7 +185,7 @@ typedef struct isis_external_connection_ {
 } isis_external_connection_t;
 
 /*
- * IS-IS Instance
+ * IS-IS Instance Configuration
  */
 typedef struct isis_config_ {
 
diff --git a/src/bbl_tcp.c b/src/bbl_tcp.c
index 0c1925f1..105d732a 100644
--- a/src/bbl_tcp.c
+++ b/src/bbl_tcp.c
@@ -10,8 +10,8 @@
 #include "bbl.h"
 #include "lwip/priv/tcp_priv.h"
 
-void
-bbl_tcp_ctx_free(bbl_tcp_t *tcp) {
+static void
+bbl_tcp_ctx_free(bbl_tcp_ctx_t *tcp) {
     if(tcp) {
         if(tcp->pcb) {
             tcp_close(tcp->pcb);
@@ -20,13 +20,13 @@ bbl_tcp_ctx_free(bbl_tcp_t *tcp) {
     }
 }
 
-static bbl_tcp_t *
-bbl_tcp_new(bbl_interface_s *interface) {
+static bbl_tcp_ctx_t *
+bbl_tcp_ctx_new(bbl_interface_s *interface) {
     
-    bbl_tcp_t *tcp;
+    bbl_tcp_ctx_t *tcp;
 
     /* Init TCP context */
-    tcp = calloc(1, sizeof(bbl_tcp_t));
+    tcp = calloc(1, sizeof(bbl_tcp_ctx_t));
     if(!tcp) {
         return NULL;
     }
@@ -50,18 +50,24 @@ bbl_tcp_new(bbl_interface_s *interface) {
 
 err_t 
 bbl_tcp_sent_cb(void *arg, struct tcp_pcb *tpcb, u16_t len) {
-    bbl_tcp_t *tcp = arg;
+    bbl_tcp_ctx_t *tcpc = arg;
     size_t tx = tpcb->snd_buf;
 
     UNUSED(len);
 
-    if(tcp->tx.offset < tcp->tx.len) {
-        tcp->state = BBL_TCP_STATE_SEND;
-        if((tcp->tx.offset + tx) > tcp->tx.len) {
-            tx = tcp->tx.len - tcp->tx.offset;
+    if(tcpc->tx.offset < tcpc->tx.len) {
+        tcpc->state = BBL_TCP_STATE_SEND;
+        if((tcpc->tx.offset + tx) > tcpc->tx.len) {
+            tx = tcpc->tx.len - tcpc->tx.offset;
         }
-        if(tcp_write(tpcb, tcp->tx.buf + tcp->tx.offset, tx, 0) == ERR_OK) {
-            tcp->tx.offset += tx;
+        if(tcp_write(tpcb, tcpc->tx.buf + tcpc->tx.offset, tx, 0) == ERR_OK) {
+            tcpc->tx.offset += tx;
+            if(tcpc->af == AF_INET) {
+                LOG(TCP_DETAIL, "TCP %lu bytes send (%s %s:%u - %s:%u)\n",
+                    tx, tcpc->interface->name,
+                    format_ipv4_address(&tcpc->local_ipv4), tcpc->key.local_port,
+                    format_ipv4_address(&tcpc->remote_ipv4), tcpc->key.remote_port);
+            }
         }
     }
     return ERR_OK;
@@ -69,42 +75,100 @@ bbl_tcp_sent_cb(void *arg, struct tcp_pcb *tpcb, u16_t len) {
 
 err_t 
 bbl_tcp_recv_cb(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err) {
-    bbl_tcp_t *tcp = arg;
+    bbl_tcp_ctx_t *tcpc = arg;
     struct pbuf *_p;
 
     UNUSED(err); /* TODO!!! */
-    LOG(TCP, "TCP RX CB\n");
+
     if(p) {
         _p = p;
         while(_p) {
-            if(tcp->receive_cb) {
-                (tcp->receive_cb)(tcp->arg, p->payload, p->len);
+            if(tcpc->receive_cb) {
+                (tcpc->receive_cb)(tcpc->arg, p->payload, p->len);
             }
             _p = _p->next;
         }
-        tcp->bytes_rx += p->tot_len;
+        tcpc->bytes_rx += p->tot_len;
         tcp_recved(tpcb, p->tot_len);
         pbuf_free(p);
+
+        if(tcpc->af == AF_INET) {
+            LOG(TCP_DETAIL, "TCP %u bytes received (%s %s:%u - %s:%u)\n",
+                p->tot_len, tcpc->interface->name,
+                format_ipv4_address(&tcpc->local_ipv4), tcpc->key.local_port,
+                format_ipv4_address(&tcpc->remote_ipv4), tcpc->key.remote_port);
+        }
+    }
+    return ERR_OK;
+}
+
+/** 
+ * Called when the pcb receives a RST or is unexpectedly closed for any other reason.
+ *
+ * @note The corresponding pcb is already freed when this callback is called!
+ *
+ * @param arg Additional argument to pass to the callback function (@see tcp_arg())
+ * @param err Error code to indicate why the pcb has been closed
+ *            ERR_ABRT: aborted through tcp_abort or by a TCP timer
+ *            ERR_RST: the connection was reset by the remote host
+ */
+void 
+bbl_tcp_error_cb(void *arg, err_t err) {
+    bbl_tcp_ctx_t *tcpc = arg;
+
+    if(tcpc->af == AF_INET) {
+        LOG(TCP, "TCP error %u (%s %s:%u - %s:%u)\n",
+            err, tcpc->interface->name,
+            format_ipv4_address(&tcpc->local_ipv4), tcpc->key.local_port,
+            format_ipv4_address(&tcpc->remote_ipv4), tcpc->key.remote_port);
+    }
+
+    if(tcpc->error_cb) {
+        (tcpc->error_cb)(tcpc->arg, err);
+    }
+}
+
+/** 
+ * Called periodically.
+ *
+ * @param arg Additional argument to pass to the callback function (@see tcp_arg())
+ * @param tpcb tcp pcb
+ * @return ERR_OK: try to send some data by calling tcp_output
+ *         Only return ERR_ABRT if you have called tcp_abort from within the
+ *         callback function!
+ */
+err_t
+bbl_tcp_poll_cb(void *arg, struct tcp_pcb *tpcb) {
+    bbl_tcp_ctx_t *tcpc = arg;
+    if(tcpc->poll_cb) {
+        return (tcpc->poll_cb)(tcpc->arg, tpcb);
     }
     return ERR_OK;
 }
 
 err_t 
 bbl_tcp_connected(void *arg, struct tcp_pcb *tpcb, err_t err) {
-    bbl_tcp_t *tcp = arg;
+    bbl_tcp_ctx_t *tcpc = arg;
 
     UNUSED(err); /* TODO!!! */
 
     /* Add send/receive callback functions. */
     tcp_sent(tpcb, bbl_tcp_sent_cb);
     tcp_recv(tpcb, bbl_tcp_recv_cb);
-    //tcp_err(tpcb, bbl_tcp_error_cb);
-    //tcp_poll(tpcb, bbl_tcp_poll_cb, 0);
+    tcp_err(tpcb, bbl_tcp_error_cb);
+    if(tcpc->poll_cb && tcpc->poll_interval) {
+        tcp_poll(tpcb, bbl_tcp_poll_cb, tcpc->poll_interval);
+    }
 
-    LOG(TCP, "TCP session connected\n");
+    if(tcpc->af == AF_INET) {
+        LOG(TCP_DETAIL, "TCP session connected (%s %s:%u - %s:%u)\n",
+            tcpc->interface->name,
+            format_ipv4_address(&tcpc->local_ipv4), tcpc->key.local_port,
+            format_ipv4_address(&tcpc->remote_ipv4), tcpc->key.remote_port);
+    }
 
-    tcp->state = BBL_TCP_STATE_IDLE;
-    bbl_tcp_sent_cb(tcp, tpcb, 0);
+    tcpc->state = BBL_TCP_STATE_IDLE;
+    bbl_tcp_sent_cb(tcpc, tpcb, 0);
     return ERR_OK;
 }
 
@@ -115,44 +179,54 @@ bbl_tcp_connected(void *arg, struct tcp_pcb *tpcb, err_t err) {
  * @param src source address
  * @param dst destination address
  * @param port destination port
- * @return bbl_tcp_t* 
+ * @return bbl_tcp_ctx_t * 
  */
-bbl_tcp_t *
+bbl_tcp_ctx_t *
 bbl_tcp_ipv4_connect(bbl_interface_s *interface, ipv4addr_t *src, ipv4addr_t *dst, uint16_t port) {
 
-    bbl_tcp_t *tcp;
+    bbl_tcp_ctx_t *tcpc;
+    dict_insert_result result;
 
-    tcp = bbl_tcp_new(interface);
-    if(!tcp) {
+    if(!interface->ctx->tcp) return NULL;
+
+    tcpc = bbl_tcp_ctx_new(interface);
+    if(!tcpc) {
         return NULL;
     }
 
     /* Bind local IP address and port */
-    tcp_bind(tcp->pcb, (const ip_addr_t*)src, 0);
+    tcp_bind(tcpc->pcb, (const ip_addr_t*)src, 0);
 
     /* Connect session */
-    if(tcp_connect(tcp->pcb, (const ip_addr_t*)dst, port, bbl_tcp_connected) != ERR_OK) {
-        bbl_tcp_ctx_free(tcp);
+    if(tcp_connect(tcpc->pcb, (const ip_addr_t*)dst, port, bbl_tcp_connected) != ERR_OK) {
+        bbl_tcp_ctx_free(tcpc);
+        return NULL;
+    }
+
+    tcpc->af = AF_INET;
+    tcpc->key.local_port = tcpc->pcb->local_port;
+    tcpc->key.remote_port = port;
+    tcpc->local_ipv4 = *src;
+    tcpc->remote_ipv4 = *dst;
+
+    /* Add BBL TCP context to interface */
+    result = dict_insert(interface->tcp_connections, &tcpc->key);
+    if (!result.inserted) {
+        bbl_tcp_ctx_free(tcpc);
         return NULL;
     }
 
-    tcp->af = AF_INET;
-    tcp->local.ipv4 = *src;
-    tcp->local.port = tcp->pcb->local_port;
-    tcp->remote.ipv4 = *dst;
-    tcp->remote.port = port;
-    
     /* TODO: This looks wrong but is required. 
      * Further investigations needed! */
-    tcp->pcb->remote_ip.type = IPADDR_TYPE_V4;
+    tcpc->pcb->local_ip.type = IPADDR_TYPE_V4;
+    tcpc->pcb->remote_ip.type = IPADDR_TYPE_V4;
 
-    LOG(TCP, "TCP connect from %s (%u) %s:%u to %s:%u\n",
+    LOG(TCP, "TCP connect (%s %s:%u - %s:%u)\n",
         interface->name,
-        tcp->pcb->netif_idx,
-        format_ipv4_address(&tcp->local.ipv4), tcp->local.port,
-        format_ipv4_address(&tcp->remote.ipv4), tcp->remote.port);
+        format_ipv4_address(&tcpc->local_ipv4), tcpc->key.local_port,
+        format_ipv4_address(&tcpc->remote_ipv4), tcpc->key.remote_port);
 
-    return tcp;
+    return tcpc;
 }
 
 /**
@@ -165,20 +239,16 @@ bbl_tcp_ipv4_connect(bbl_interface_s *interface, ipv4addr_t *src, ipv4addr_t *ds
 void
 bbl_tcp_ipv4_rx(bbl_interface_s *interface, bbl_ethernet_header_t *eth, bbl_ipv4_t *ipv4) {
     struct pbuf *pbuf;
+    bbl_tcp_t *tcp = (bbl_tcp_t*)ipv4->next;
+
     UNUSED(eth);
 
-    LOG(TCP, "TCP RX from %s %s to %s\n",
-        interface->name,
-        format_ipv4_address(&ipv4->src),
-        format_ipv4_address(&ipv4->dst));
+    if(!interface->ctx->tcp) return;
 
-/* Alternative code...
-    struct ip_hdr *iphdr = (struct ip_hdr*)ipv4->hdr;
-    ip_addr_copy_from_ip4(*ip_current_dest_addr(), iphdr->dest);
-    ip_addr_copy_from_ip4(*ip_current_src_addr(), iphdr->src);
-    ip_current_netif() = &interface->netif;
-    ip_current_input_netif() = &interface->netif;
-*/
+    LOG(TCP_DETAIL, "TCP packet received (%s %s:%u - %s:%u)\n",
+        interface->name,
+        format_ipv4_address(&ipv4->dst), tcp->dst,
+        format_ipv4_address(&ipv4->src), tcp->src);
 
     ip_data.current_netif = &interface->netif;
     ip_data.current_input_netif = &interface->netif;
@@ -191,6 +261,26 @@ bbl_tcp_ipv4_rx(bbl_interface_s *interface, bbl_ethernet_header_t *eth, bbl_ipv4
     tcp_input(pbuf, &interface->netif);
 }
 
+/**
+ * bbl_tcp_ipv6_connect 
+ * 
+ * @param interface interface
+ * @param src source address
+ * @param dst destination address
+ * @param port destination port
+ * @return bbl_tcp_ctx_t * 
+ */
+bbl_tcp_ctx_t *
+bbl_tcp_ipv6_connect(bbl_interface_s *interface, ipv6addr_t *src, ipv6addr_t *dst, uint16_t port) {
+
+    if(!interface->ctx->tcp) return NULL;
+
+    UNUSED(src);
+    UNUSED(dst);
+    UNUSED(port);
+    return NULL;
+}
+
 /**
  * bbl_tcp_ipv6_rx 
  * 
@@ -201,7 +291,17 @@ bbl_tcp_ipv4_rx(bbl_interface_s *interface, bbl_ethernet_header_t *eth, bbl_ipv4
 void
 bbl_tcp_ipv6_rx(bbl_interface_s *interface, bbl_ethernet_header_t *eth, bbl_ipv6_t *ipv6) {
     struct pbuf *pbuf;
+    bbl_tcp_t *tcp = (bbl_tcp_t*)ipv6->next;
+
     UNUSED(eth);
+
+    if(!interface->ctx->tcp) return;
+
+    LOG(TCP_DETAIL, "TCP packet received (%s %s:%u - %s:%u)\n",
+        interface->name,
+        format_ipv6_address((ipv6addr_t*)ipv6->dst), tcp->dst,
+        format_ipv6_address((ipv6addr_t*)ipv6->src), tcp->src);
+
     pbuf = pbuf_alloc_reference(ipv6->hdr, ipv6->len, PBUF_ROM);
     interface->netif.input(pbuf, &interface->netif);
 }
@@ -215,19 +315,19 @@ bbl_tcp_ipv6_rx(bbl_interface_s *interface, bbl_ethernet_header_t *eth, bbl_ipv6
  * @return ERR_OK if successfull
  */
 err_t
-bbl_tcp_send(bbl_tcp_t *tcp, uint8_t *buf, uint16_t len) {
+bbl_tcp_send(bbl_tcp_ctx_t *tcpc, uint8_t *buf, uint16_t len) {
 
-    if(tcp->tx.offset < tcp->tx.len) {
+    if(tcpc->tx.offset < tcpc->tx.len) {
         /* There is still data to send */
         return ERR_INPROGRESS;
     }
 
-    tcp->tx.buf = buf;
-    tcp->tx.len = len;
-    tcp->tx.offset = 0;
+    tcpc->tx.buf = buf;
+    tcpc->tx.len = len;
+    tcpc->tx.offset = 0;
 
-    if(tcp->state == BBL_TCP_STATE_IDLE) {
-        bbl_tcp_sent_cb(tcp, tcp->pcb, 0);
+    if(tcpc->state == BBL_TCP_STATE_IDLE) {
+        bbl_tcp_sent_cb(tcpc, tcpc->pcb, 0);
     }
     return ERR_OK;
 }
@@ -263,8 +363,8 @@ bbl_tcp_netif_output_ipv4(struct netif *netif, struct pbuf *p, const ip4_addr_t
  */
 err_t 
 bbl_tcp_netif_output_ipv6(struct netif *netif, struct pbuf *p, const ip6_addr_t *ipaddr) {
-    bbl_tcp_t *tcp = netif->state;
-    bbl_interface_s *interface = tcp->interface;
+    bbl_tcp_ctx_t *tcpc = netif->state;
+    bbl_interface_s *interface = tcpc->interface;
     bbl_ethernet_header_t eth = {0};
 
     UNUSED(ipaddr);
@@ -306,12 +406,17 @@ bbl_tcp_netif_init(struct netif *netif) {
 bool
 bbl_tcp_interface_init(bbl_interface_s *interface, bbl_network_config_s *network_config) {
     UNUSED(network_config);
+
+    if(!interface->ctx->tcp) return true;
+
     if(!netif_add(&interface->netif, NULL, NULL, NULL, interface, bbl_tcp_netif_init, ip_input))  {
         return false;
     }
     interface->netif.state = interface;
-    interface->netif.mtu = 1492;
-    interface->netif.mtu6 = 1492;
+    interface->netif.mtu = network_config->mtu;
+    interface->netif.mtu6 = network_config->mtu;
+    interface->tcp_connections = hashtable_dict_new((dict_compare_func)bbl_compare_key32, bbl_key32_hash, BBL_TCP_HASHTABLE_SIZE);
+
     return true;
 }
 
@@ -333,9 +438,11 @@ bbl_tcp_timer(timer_s *timer) {
 void
 bbl_tcp_init(bbl_ctx_s *ctx) {
 
+    if(!ctx->tcp) return;
+
     lwip_init();
 
     /* Start TCP timer */
     timer_add_periodic(&ctx->timer_root, &ctx->tcp_timer, "TCP",
-                        0, BBL_TCP_INTERVAL, ctx, &bbl_tcp_timer);
+                       0, BBL_TCP_INTERVAL, ctx, &bbl_tcp_timer);
 }
\ No newline at end of file
diff --git a/src/bbl_tcp.h b/src/bbl_tcp.h
index 84d910ef..343fc3fb 100644
--- a/src/bbl_tcp.h
+++ b/src/bbl_tcp.h
@@ -12,6 +12,7 @@
 
 #define BBL_TCP_BUF_SIZE 65000
 #define BBL_TCP_INTERVAL 250*MSEC
+#define BBL_TCP_HASHTABLE_SIZE 32771
 
 typedef enum bbl_tcp_state_ {
     BBL_TCP_STATE_CONNECT,
@@ -22,29 +23,36 @@ typedef enum bbl_tcp_state_ {
     BBL_TCP_STATE_CLOSED,
 } bbl_tcp_state_t;
 
-typedef void (*bbl_tcp_receive_cb)(void *arg, uint8_t *buf, uint16_t len);
+typedef void (*bbl_tcp_receive_fn)(void *arg, uint8_t *buf, uint16_t len);
+typedef void (*bbl_tcp_error_fn)(void *arg, err_t err);
+typedef err_t (*bbl_tcp_poll_fn)(void *arg, struct tcp_pcb *tpcb);
 
-typedef struct bbl_tcp_
+typedef struct bbl_tcp_key_ {
+    uint16_t local_port;
+    uint16_t remote_port;
+} __attribute__ ((__packed__)) bbl_tcp_key_t;
+
+typedef struct bbl_tcp_ctx_
 {
     bbl_interface_s *interface;
-    
+
     uint8_t af; /* AF_INET or AF_INET6 */
+    
+    bbl_tcp_key_t key;
 
-    struct {
-        ipv4addr_t ipv4;
-        ipv6addr_t ipv6;
-        uint16_t port;
-    } local;
-
-    struct {
-        ipv4addr_t ipv4;
-        ipv6addr_t ipv6;
-        uint16_t port;
-    } remote;
+    ipv4addr_t local_ipv4;
+    ipv6addr_t local_ipv6;
+    ipv4addr_t remote_ipv4;
+    ipv6addr_t remote_ipv6;
     
     struct tcp_pcb *pcb;
     
-    bbl_tcp_receive_cb receive_cb; /* application receive callback */
+    bbl_tcp_receive_fn receive_cb; /* application receive callback */
+    bbl_tcp_error_fn error_cb; /* application receive callback */
+
+    bbl_tcp_poll_fn poll_cb; /* application poll callback */
+    uint8_t poll_interval;
+
     void *arg; /* application callback argument */
 
     bbl_tcp_state_t state;
@@ -55,22 +63,27 @@ typedef struct bbl_tcp_
         size_t   offset;
     } tx;
 
+    uint64_t packets_rx;
     uint64_t bytes_rx;
+    uint64_t packets_tx;
     uint64_t bytes_tx;
 
-} bbl_tcp_t;
+} bbl_tcp_ctx_t;
 
-bbl_tcp_t *
+bbl_tcp_ctx_t *
 bbl_tcp_ipv4_connect(bbl_interface_s *interface, ipv4addr_t *src, ipv4addr_t *dst, uint16_t port);
 
 void
 bbl_tcp_ipv4_rx(bbl_interface_s *interface, bbl_ethernet_header_t *eth, bbl_ipv4_t *ipv4);
 
+bbl_tcp_ctx_t *
+bbl_tcp_ipv6_connect(bbl_interface_s *interface, ipv6addr_t *src, ipv6addr_t *dst, uint16_t port);
+
 void
 bbl_tcp_ipv6_rx(bbl_interface_s *interface, bbl_ethernet_header_t *eth, bbl_ipv6_t *ipv6);
 
 err_t
-bbl_tcp_send(bbl_tcp_t *tcp, uint8_t *buf, uint16_t len);
+bbl_tcp_send(bbl_tcp_ctx_t *tcpc, uint8_t *buf, uint16_t len);
 
 bool
 bbl_tcp_interface_init(bbl_interface_s *interface, bbl_network_config_s *network_config);
diff --git a/src/bgp/bgp.c b/src/bgp/bgp.c
new file mode 100644
index 00000000..9ca64c8f
--- /dev/null
+++ b/src/bgp/bgp.c
@@ -0,0 +1,132 @@
+/*
+ * BNG Blaster (BBL) - BGP Functions
+ *
+ * Christian Giese, March 2022
+ *
+ * Copyright (C) 2020-2022, RtBrick, Inc.
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include "bgp.h"
+
+static const char *
+bgp_state_string(uint32_t state) {
+    switch(state) {
+        case BGP_IDLE: return "Idle";
+        case BGP_CONNECT: return "Connect";
+        case BGP_ACTIVE: return "Active";
+        case BGP_OPENSENT: return "OpenSent";
+        case BGP_OPENCONFIRM: return "OpenConfirm";
+        case BGP_ESTABLISHED: return "Established";
+        default: return "N/A";
+    }
+}
+
+static void
+bgp_state_change(bgp_session_t *session, bgp_state_t new_state)
+{
+    if (session->state == new_state) {
+	    return;
+    }
+
+    LOG(BGP, "BGP %s:%s state change from %s -> %s\n", 
+        format_ipv4_address(&session->ipv4_src_address),
+        format_ipv4_address(&session->ipv4_dst_address),
+        bgp_state_string(session->state),
+        bgp_state_string(new_state));
+
+    session->state = new_state;
+}
+
+void 
+bgp_receive_cb(void *arg, uint8_t *buf, uint16_t len) {
+    bgp_session_t *session = (bgp_session_t*)arg;
+
+    UNUSED(buf);
+    UNUSED(len);
+
+    if(session->state == BGP_CONNECT) {
+        bgp_message_open(session);
+        bbl_tcp_send(session->tcpc, session->write_buf, session->write_idx);
+        bgp_state_change(session, BGP_OPENSENT);
+    }
+}
+
+void
+bgp_connect_job(timer_s *timer) {
+    bgp_session_t *session = timer->data;
+    if(!session->interface->arp_resolved) {
+        return;
+    }
+
+    session->tcpc = bbl_tcp_ipv4_connect(
+        session->interface, 
+        &session->ipv4_src_address,
+        &session->ipv4_dst_address,
+        BGP_PORT);
+
+    if(!session->tcpc) {
+        /* Try again... */
+        return;
+    }
+
+    session->tcpc->arg = session;
+    session->tcpc->receive_cb = bgp_receive_cb;
+    
+    bgp_state_change(session, BGP_CONNECT);
+
+    /* Stop timer... */
+    timer->periodic = false;
+}
+
+/**
+ * bgp_init
+ * 
+ * This function inits all BGP sessions. 
+ * 
+ * @param ctx global context
+ */
+bool
+bgp_init(bbl_ctx_s *ctx) {
+
+    bgp_config_t *config = ctx->config.bgp_config;
+    bgp_session_t *session = NULL;
+    bbl_interface_s *network_if;
+
+    while(config) {
+        if(session) {
+            session->next = calloc(1, sizeof(bgp_session_t));
+            session = session->next;
+        } else {
+            session = calloc(1, sizeof(bgp_session_t));
+            ctx->bgp_sessions = session;
+        }
+        if(!session) {
+            return false;
+        }
+        
+        network_if = bbl_get_network_interface(ctx, config->network_interface);
+        if(!network_if) {
+            free(session);
+            return false;
+        }
+
+        session->ctx = ctx;
+        session->config = config;
+        session->interface = network_if;
+
+        if(config->ipv4_src_address) {
+            session->ipv4_src_address = config->ipv4_src_address;
+        } else {
+            session->ipv4_src_address = network_if->ip.address;
+        }
+        session->ipv4_dst_address = config->ipv4_dst_address;
+        session->write_buf = malloc(BGP_WRITEBUFSIZE);
+        session->write_idx = 0;
+
+        timer_add(&ctx->timer_root, &session->connect_timer, 
+                  "BGP CONNECT", 1, 0, session, &bgp_connect_job);
+        
+        config = config->next;
+    }
+    return true;
+}
\ No newline at end of file
diff --git a/src/bgp/bgp.h b/src/bgp/bgp.h
new file mode 100644
index 00000000..c4e25623
--- /dev/null
+++ b/src/bgp/bgp.h
@@ -0,0 +1,20 @@
+/*
+ * BNG Blaster (BBL) - BGP Main
+ *
+ * Christian Giese, Mach 2022
+ *
+ * Copyright (C) 2020-2022, RtBrick, Inc.
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef __BBL_BGP_H__
+#define __BBL_BGP_H__
+
+#include "../bbl.h"
+#include "bgp_def.h"
+#include "bgp_message.h"
+#include "bgp_mrt.h"
+
+bool
+bgp_init(bbl_ctx_s *ctx);
+
+#endif
\ No newline at end of file
diff --git a/src/bgp/bgp_def.h b/src/bgp/bgp_def.h
new file mode 100644
index 00000000..419555ef
--- /dev/null
+++ b/src/bgp/bgp_def.h
@@ -0,0 +1,80 @@
+/*
+ * BNG Blaster (BBL) - BGP Definitions
+ *
+ * Christian Giese, MARCH 2022
+ *
+ * Copyright (C) 2020-2022, RtBrick, Inc.
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef __BBL_BGP_DEF_H__
+#define __BBL_BGP_DEF_H__
+
+/* DEFINITIONS ... */
+
+#define BGP_PORT                179
+#define BGP_MAX_MESSAGE_SIZE    4096
+#define BGP_WRITEBUFSIZE        4096
+#define BGP_DEFAULT_AS          65000
+#define BGP_DEFAULT_HOLDTIME    90
+
+#define BGP_MSG_OPEN            1
+#define BGP_MSG_UPDATE          2
+#define BGP_MSG_NOTIFICATION    3
+#define BGP_MSG_KEEPALIVE       4
+
+typedef enum bgp_state_ {
+    BGP_IDLE,
+    BGP_CONNECT,
+    BGP_ACTIVE,
+    BGP_OPENSENT,
+    BGP_OPENCONFIRM,
+    BGP_ESTABLISHED
+} bgp_state_t;
+
+/*
+ * BGP Configuration
+ */
+typedef struct bgp_config_ {
+    uint32_t ipv4_src_address;
+    uint32_t ipv4_dst_address;
+    uint32_t id;
+    uint32_t local_as;
+    uint32_t peer_as;
+    uint16_t holdtime;
+
+    char *network_interface;
+    char *mrt_file;
+
+    /* Pointer to next instance */
+    struct bgp_config_ *next;
+} bgp_config_t;
+
+/*
+ * BGP Session
+ */
+typedef struct bgp_session_ {
+    struct bbl_ctx_ *ctx; /* parent */
+    
+    uint32_t ipv4_src_address;
+    uint32_t ipv4_dst_address;
+
+    bgp_config_t    *config;
+    bbl_interface_s *interface;
+    bbl_tcp_ctx_t   *tcpc;
+
+    struct timer_ *connect_timer;
+    struct timer_ *send_open_timer;
+    struct timer_ *open_sent_timer;
+    struct timer_ *keepalive_timer;
+    struct timer_ *hold_timer;
+    struct timer_ *close_timer;
+
+    uint8_t *write_buf;
+    uint16_t write_idx;
+
+    bgp_state_t state;
+
+    struct bgp_session_ *next; /* pointer to next instance */
+} bgp_session_t;
+
+#endif
\ No newline at end of file
diff --git a/src/bgp/bgp_message.c b/src/bgp/bgp_message.c
new file mode 100644
index 00000000..ce86ae7d
--- /dev/null
+++ b/src/bgp/bgp_message.c
@@ -0,0 +1,102 @@
+/*
+ * BNG Blaster (BBL) - BGP Functions
+ *
+ * Christian Giese, March 2022
+ *
+ * Copyright (C) 2020-2022, RtBrick, Inc.
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include "bgp.h"
+
+static uint16_t
+as4_capability(uint8_t *buf, uint32_t as) {
+    uint16_t len;
+
+    *buf = 2; /* cap code */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *buf = 6; /* cap length */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *buf = 65; /* AS4 capability */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *buf = 4; /* length to encode my AS4 */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *(uint32_t*)buf = htobe32(as); 
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint32_t));
+
+    return len;
+}
+
+static uint16_t
+mp_capability(uint8_t *buf, uint16_t afi, uint8_t safi) {
+    uint16_t len;
+
+    *buf = 2; /* cap code */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *buf = 6; /* cap length */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *buf = 1; /* MP extension capability */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *buf = 4; /* length */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *(uint16_t*)buf = htobe16(afi); 
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint16_t));
+    *buf = 0; /* Reserved */
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    *buf = safi;
+    BUMP_WRITE_BUFFER(buf, &len, sizeof(uint8_t));
+    return len;
+}
+
+void
+bgp_message_open(bgp_session_t *session) {
+    uint16_t opt_parms_idx, opt_parms_length;
+
+    uint8_t *buf = session->write_buf;
+    uint16_t cap_len;
+
+    memset(buf, 16, 0xff); /* marker */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, 16);
+    *(uint16_t*)buf = 0; /* length */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, sizeof(uint16_t));
+    *buf = BGP_MSG_OPEN; /* message type */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, sizeof(uint8_t));
+    *buf = BGP_MSG_OPEN; /* version 4 */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, sizeof(uint8_t));
+
+    /* local AS */
+    if(session->config->local_as > 65535) {
+        *(uint16_t*)buf = htobe16(23456); 
+    } else {
+        *(uint16_t*)buf = htobe16(session->config->local_as); 
+    }
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, sizeof(uint16_t));
+
+    *(uint16_t*)buf = htobe16(session->config->holdtime); /* holdtime */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, sizeof(uint16_t));
+    *(uint32_t*)buf = session->config->id; /* BGP ID */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, sizeof(uint32_t));
+
+    /* Optional parameters */
+    *buf = 0; /* optional parameter length */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, sizeof(uint8_t));
+    opt_parms_idx = session->write_idx;
+
+    cap_len = as4_capability(buf, session->config->local_as);
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, cap_len);
+
+    cap_len = mp_capability(buf, 1, 1); /* ipv4 unicast */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, cap_len);
+    cap_len = mp_capability(buf, 2, 1); /* ipv6 unicast */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, cap_len);
+    cap_len = mp_capability(buf, 1, 4); /* ipv4 labeled unicast */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, cap_len);
+    cap_len = mp_capability(buf, 2, 4); /* ipv6 labeled unicast */
+    BUMP_WRITE_BUFFER(buf, &session->write_idx, cap_len);
+
+    /* Update optional parameters length field */
+    opt_parms_length = session->write_idx - opt_parms_idx;
+    *(session->write_buf+opt_parms_idx-1) = opt_parms_length;
+
+    /* Update message length field */
+    *(uint16_t*)(session->write_buf+16) = htobe16(session->write_idx);
+}
\ No newline at end of file
diff --git a/src/bgp/bgp_message.h b/src/bgp/bgp_message.h
new file mode 100644
index 00000000..d82f9b98
--- /dev/null
+++ b/src/bgp/bgp_message.h
@@ -0,0 +1,15 @@
+/*
+ * BNG Blaster (BBL) - BGP MRT Files
+ *
+ * Christian Giese, March 2022
+ *
+ * Copyright (C) 2020-2022, RtBrick, Inc.
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef __BBL_BGP_MESSAGE_H__
+#define __BBL_BGP_MESSAGE_H__
+
+void
+bgp_message_open(bgp_session_t *session);
+
+#endif
\ No newline at end of file
diff --git a/src/bgp/bgp_mrt.h b/src/bgp/bgp_mrt.h
new file mode 100644
index 00000000..850f02be
--- /dev/null
+++ b/src/bgp/bgp_mrt.h
@@ -0,0 +1,12 @@
+/*
+ * BNG Blaster (BBL) - BGP MRT Files
+ *
+ * Christian Giese, March 2022
+ *
+ * Copyright (C) 2020-2022, RtBrick, Inc.
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef __BBL_BGP_MRT_H__
+#define __BBL_BGP_MRT_H__
+
+#endif
\ No newline at end of file
diff --git a/src/lwip/lwipopts.h b/src/lwip/lwipopts.h
index bf60c897..fce44175 100644
--- a/src/lwip/lwipopts.h
+++ b/src/lwip/lwipopts.h
@@ -153,7 +153,7 @@ a lot of data that needs to be copied, this should be set high. */
 #define TCP_MAXRTX              12
 
 /* Maximum number of retransmissions of SYN segments. */
-#define TCP_SYNMAXRTX           4
+#define TCP_SYNMAXRTX           12
 
 #define TCP_LISTEN_BACKLOG      1
 #define LWIP_CALLBACK_API       1