From 78a7085778868f1bc0e5e5542b7e208f943e3778 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Wed, 20 Jul 2011 00:29:02 +0200
Subject: [PATCH 01/16] Disable IPv6 routes adv when peer doesn't support it.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/bgp.c b/bgp.c
index 2deeeb3..ea15faf 100644
--- a/bgp.c
+++ b/bgp.c
@@ -1093,6 +1093,17 @@ static int bgp_handle_input(struct bgp_peer *peer)
 		return 0;
 	    }
 
+	    if (notification->error_code == BGP_ERR_OPEN
+		    && notification->subcode == BGP_ERR_OPN_UNSUP_CAP)
+	    {
+		/* the only capability we advertise is this one, so upon receiving
+		   an "unsupported capability" message, we disable IPv6 routes for
+		   this peer */
+		LOG(4, 0, 0, "BGP peer %s doesn't support IPv6 routes advertisement\n", peer->name);
+		peer->handle_ipv6_routes = 0;
+		break;
+	    }
+
 	    /* FIXME: should handle more notifications */
 	    LOG(4, 0, 0, "BGP peer %s sent unhandled NOTIFICATION %d\n",
 		peer->name, (int) notification->error_code);
-- 
2.20.1


From f2e59e98817d4959af549abccf4e798e6184f6d4 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Wed, 20 Jul 2011 00:30:16 +0200
Subject: [PATCH 02/16] Fix a typo from last commit.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/bgp.c b/bgp.c
index ea15faf..f12efb1 100644
--- a/bgp.c
+++ b/bgp.c
@@ -1094,7 +1094,7 @@ static int bgp_handle_input(struct bgp_peer *peer)
 	    }
 
 	    if (notification->error_code == BGP_ERR_OPEN
-		    && notification->subcode == BGP_ERR_OPN_UNSUP_CAP)
+		    && notification->error_subcode == BGP_ERR_OPN_UNSUP_CAP)
 	    {
 		/* the only capability we advertise is this one, so upon receiving
 		   an "unsupported capability" message, we disable IPv6 routes for
-- 
2.20.1


From 02611e54fc55643ec109b255707ce9832e005ddf Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Wed, 20 Jul 2011 00:49:28 +0200
Subject: [PATCH 03/16] Move NEXT_HOP attribute so that we can exclude it.

We will need to do that when we will send IPv6 routes (RFC4760 says we SHOULD
NOT carry this attribute when we will send UPDATE without NLRI). So, we save
the length of all the attributes except NEXT_HOP for later memcpy().

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 21 ++++++++++++---------
 bgp.h |  1 +
 2 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/bgp.c b/bgp.c
index f12efb1..10f06a3 100644
--- a/bgp.c
+++ b/bgp.c
@@ -193,15 +193,6 @@ int bgp_start(struct bgp_peer *peer, char *name, int as, int keepalive,
 
     ADD_ATTRIBUTE();
 
-    /* NEXT_HOP */
-    a.flags = BGP_PATH_ATTR_FLAG_TRANS;
-    a.code = BGP_PATH_ATTR_CODE_NEXT_HOP;
-    ip = my_address; /* we're it */
-    a.data.s.len = sizeof(ip);
-    memcpy(a.data.s.value, &ip, sizeof(ip));
-
-    ADD_ATTRIBUTE();
-
     /* MULTI_EXIT_DISC */
     a.flags = BGP_PATH_ATTR_FLAG_OPTIONAL;
     a.code = BGP_PATH_ATTR_CODE_MULTI_EXIT_DISC;
@@ -231,6 +222,18 @@ int bgp_start(struct bgp_peer *peer, char *name, int as, int keepalive,
 
     ADD_ATTRIBUTE();
 
+    /* remember the len before adding NEXT_HOP */
+    peer->path_attr_len_without_nexthop = peer->path_attr_len;
+
+    /* NEXT_HOP */
+    a.flags = BGP_PATH_ATTR_FLAG_TRANS;
+    a.code = BGP_PATH_ATTR_CODE_NEXT_HOP;
+    ip = my_address; /* we're it */
+    a.data.s.len = sizeof(ip);
+    memcpy(a.data.s.value, &ip, sizeof(ip));
+
+    ADD_ATTRIBUTE();
+
     if (!(peer->path_attrs = malloc(peer->path_attr_len)))
     {
 	LOG(0, 0, 0, "Can't allocate path_attrs for %s (%s)\n",
diff --git a/bgp.h b/bgp.h
index 91adb88..a791fc1 100644
--- a/bgp.h
+++ b/bgp.h
@@ -203,6 +203,7 @@ struct bgp_peer {
     int cli_flag;			/* updates requested from CLI */
     char *path_attrs;			/* path attrs to send in UPDATE message */
     int path_attr_len;			/* length of path attrs */
+    int path_attr_len_without_nexthop;	/* length of path attrs  without NEXT_HOP */
     uint32_t events;			/* events to poll */
     struct event_data edata;		/* poll data */
     int handle_ipv6_routes;		/* can handle IPv6 routes advertisements */
-- 
2.20.1


From f4fb6922fee52973e2f568f18411594c88b40aa2 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Wed, 20 Jul 2011 11:00:43 +0200
Subject: [PATCH 04/16] Add v6 routes handling.

Basically duplicate the v4 functions.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 162 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 bgp.h |  17 ++++++
 2 files changed, 179 insertions(+)

diff --git a/bgp.c b/bgp.c
index 10f06a3..1e1713d 100644
--- a/bgp.c
+++ b/bgp.c
@@ -32,8 +32,11 @@ static void bgp_set_retry(struct bgp_peer *peer);
 static void bgp_cidr(in_addr_t ip, in_addr_t mask, struct bgp_ip_prefix *pfx);
 static struct bgp_route_list *bgp_insert_route(struct bgp_route_list *head,
     struct bgp_route_list *new);
+static struct bgp_route6_list *bgp_insert_route6(struct bgp_route6_list *head,
+    struct bgp_route6_list *new);
 
 static void bgp_free_routes(struct bgp_route_list *routes);
+static void bgp_free_routes6(struct bgp_route6_list *routes);
 static char const *bgp_msg_type_str(uint8_t type);
 static int bgp_connect(struct bgp_peer *peer);
 static int bgp_handle_connect(struct bgp_peer *peer);
@@ -43,6 +46,7 @@ static int bgp_handle_input(struct bgp_peer *peer);
 static int bgp_send_open(struct bgp_peer *peer);
 static int bgp_send_keepalive(struct bgp_peer *peer);
 static int bgp_send_update(struct bgp_peer *peer);
+static int bgp_send_update6(struct bgp_peer *peer);
 static int bgp_send_notification(struct bgp_peer *peer, uint8_t code,
     uint8_t subcode);
 static int bgp_send_notification_full(struct bgp_peer *peer, uint8_t code,
@@ -50,6 +54,7 @@ static int bgp_send_notification_full(struct bgp_peer *peer, uint8_t code,
 
 static uint16_t our_as;
 static struct bgp_route_list *bgp_routes = 0;
+static struct bgp_route6_list *bgp_routes6 = 0;
 
 int bgp_configured = 0;
 struct bgp_peer *bgp_peers = 0;
@@ -90,6 +95,7 @@ int bgp_setup(int as)
     	return 0;
 
     bgp_routes = 0;
+    bgp_routes6 = 0;
     bgp_configured = 0; /* set by bgp_start */
 
     return 1;
@@ -272,6 +278,8 @@ static void bgp_clear(struct bgp_peer *peer)
 
     bgp_free_routes(peer->routes);
     peer->routes = 0;
+    bgp_free_routes6(peer->routes6);
+    peer->routes6 = 0;
 
     peer->outbuf->packet.header.len = 0;
     peer->outbuf->done = 0;
@@ -379,6 +387,33 @@ static struct bgp_route_list *bgp_insert_route(struct bgp_route_list *head,
     return head;
 }
 
+/* insert route6 into list; sorted */
+static struct bgp_route6_list *bgp_insert_route6(struct bgp_route6_list *head,
+    struct bgp_route6_list *new)
+{
+    struct bgp_route6_list *p = head;
+    struct bgp_route6_list *e = 0;
+
+    while (p && memcmp(&p->dest, &new->dest, sizeof(p->dest)) < 0)
+    {
+	e = p;
+	p = p->next;
+    }
+
+    if (e)
+    {
+	new->next = e->next;
+	e->next = new;
+    }
+    else
+    {
+	new->next = head;
+	head = new;
+    }
+
+    return head;
+}
+
 /* add route to list for peers */
 /*
  * Note:  this doesn't do route aggregation, nor drop routes if a less
@@ -431,6 +466,58 @@ int bgp_add_route(in_addr_t ip, in_addr_t mask)
     return 1;
 }
 
+/* add route to list for peers */
+/*
+ * Note: same provisions as above
+ */
+int bgp_add_route6(struct in6_addr ip, int prefixlen)
+{
+    struct bgp_route6_list *r = bgp_routes6;
+    struct bgp_route6_list add;
+    int i;
+    char ipv6addr[INET6_ADDRSTRLEN];
+
+    memcpy(&add.dest.prefix, &ip.s6_addr, 16);
+    add.dest.len = prefixlen;
+    add.next = 0;
+
+    /* check for duplicate */
+    while (r)
+    {
+	i = memcmp(&r->dest, &add.dest, sizeof(r->dest));
+	if (!i)
+	    return 1; /* already covered */
+
+	if (i > 0)
+	    break;
+
+	r = r->next;
+    }
+
+    /* insert into route list; sorted */
+    if (!(r = malloc(sizeof(*r))))
+    {
+	LOG(0, 0, 0, "Can't allocate route for %s/%d (%s)\n",
+	    inet_ntop(AF_INET6, &ip, ipv6addr, INET6_ADDRSTRLEN), add.dest.len,
+	    strerror(errno));
+
+	return 0;
+    }
+
+    memcpy(r, &add, sizeof(*r));
+    bgp_routes6 = bgp_insert_route6(bgp_routes6, r);
+
+    /* flag established peers for update */
+    for (i = 0; i < BGP_NUM_PEERS; i++)
+	if (bgp_peers[i].state == Established)
+	    bgp_peers[i].update_routes6 = 1;
+
+    LOG(4, 0, 0, "Registered BGP route %s/%d\n",
+	inet_ntop(AF_INET6, &ip, ipv6addr, INET6_ADDRSTRLEN), add.dest.len);
+
+    return 1;
+}
+
 /* remove route from list for peers */
 int bgp_del_route(in_addr_t ip, in_addr_t mask)
 {
@@ -480,6 +567,57 @@ int bgp_del_route(in_addr_t ip, in_addr_t mask)
     return 1;
 }
 
+/* remove route from list for peers */
+int bgp_del_route6(struct in6_addr ip, int prefixlen)
+{
+    struct bgp_route6_list *r = bgp_routes6;
+    struct bgp_route6_list *e = 0;
+    struct bgp_route6_list del;
+    int i;
+    char ipv6addr[INET6_ADDRSTRLEN];
+
+    memcpy(&del.dest.prefix, &ip.s6_addr, 16);
+    del.dest.len = prefixlen;
+    del.next = 0;
+
+    /* find entry in routes list and remove */
+    while (r)
+    {
+	i = memcmp(&r->dest, &del.dest, sizeof(r->dest));
+	if (!i)
+	{
+	    if (e)
+		e->next = r->next;
+	    else
+		bgp_routes6 = r->next;
+
+	    free(r);
+	    break;
+	}
+
+	e = r;
+
+	if (i > 0)
+	    r = 0; /* stop */
+	else
+	    r = r->next;
+    }
+
+    /* not found */
+    if (!r)
+	return 1;
+
+    /* flag established peers for update */
+    for (i = 0; i < BGP_NUM_PEERS; i++)
+	if (bgp_peers[i].state == Established)
+	    bgp_peers[i].update_routes6 = 1;
+
+    LOG(4, 0, 0, "Removed BGP route %s/%d\n",
+	inet_ntop(AF_INET6, &ip, ipv6addr, INET6_ADDRSTRLEN), del.dest.len);
+
+    return 1;
+}
+
 /* enable or disable routing */
 void bgp_enable_routing(int enable)
 {
@@ -624,6 +762,14 @@ int bgp_process(uint32_t events[])
 		continue;
 	}
 
+	/* process pending IPv6 updates */
+	if (peer->update_routes6
+	    && !peer->outbuf->packet.header.len) /* ditto */
+	{
+	    if (!bgp_send_update6(peer))
+		continue;
+	}
+
 	/* process timers */
 	if (peer->state == Established)
 	{
@@ -667,6 +813,17 @@ static void bgp_free_routes(struct bgp_route_list *routes)
     }
 }
 
+static void bgp_free_routes6(struct bgp_route6_list *routes)
+{
+    struct bgp_route6_list *tmp;
+
+    while ((tmp = routes))
+    {
+	routes = tmp->next;
+	free(tmp);
+    }
+}
+
 char const *bgp_state_str(enum bgp_state state)
 {
     switch (state)
@@ -1332,6 +1489,11 @@ static int bgp_send_update(struct bgp_peer *peer)
     return bgp_write(peer);
 }
 
+/* send/buffer UPDATE message for IPv6 routes */
+static int bgp_send_update6(struct bgp_peer *peer)
+{
+}
+
 /* send/buffer NOTIFICATION message */
 static int bgp_send_notification(struct bgp_peer *peer, uint8_t code,
     uint8_t subcode)
diff --git a/bgp.h b/bgp.h
index a791fc1..db8b1ae 100644
--- a/bgp.h
+++ b/bgp.h
@@ -71,11 +71,19 @@ struct bgp_mp_cap_param {
 #define BGP_MP_SAFI_UNICAST	1
 #define BGP_MP_SAFI_MULTICAST	2
 
+struct bgp_ip6_prefix {
+    uint8_t len;
+    uint8_t prefix[16]; /* variable */
+} __attribute__ ((packed));
+
+/* end of RFC4760 specific definitions */
+
 struct bgp_ip_prefix {
     uint8_t len;
     uint32_t prefix; /* variable */
 } __attribute__ ((packed));
 
+/* works for both IPv4 and IPv6 prefixes */
 #define BGP_IP_PREFIX_SIZE(p) (1 + ((p).len / 8) + ((p).len % 8 != 0))
 
 struct bgp_path_attr {
@@ -168,6 +176,11 @@ enum bgp_state {
     Established,			/* established */
 };
 
+struct bgp_route6_list {
+    struct bgp_ip6_prefix dest;
+    struct bgp_route6_list *next;
+};
+
 struct bgp_route_list {
     struct bgp_ip_prefix dest;
     struct bgp_route_list *next;
@@ -207,6 +220,8 @@ struct bgp_peer {
     uint32_t events;			/* events to poll */
     struct event_data edata;		/* poll data */
     int handle_ipv6_routes;		/* can handle IPv6 routes advertisements */
+    int update_routes6;			/* UPDATE required for IPv6 routes */
+    struct bgp_route6_list *routes6;	/* IPv6 routes known by this peer */
 };
 
 /* bgp_peer.cli_flag */
@@ -226,7 +241,9 @@ void bgp_stop(struct bgp_peer *peer);
 void bgp_halt(struct bgp_peer *peer);
 int bgp_restart(struct bgp_peer *peer);
 int bgp_add_route(in_addr_t ip, in_addr_t mask);
+int bgp_add_route6(struct in6_addr ip, int prefixlen);
 int bgp_del_route(in_addr_t ip, in_addr_t mask);
+int bgp_del_route6(struct in6_addr ip, int prefixlen);
 void bgp_enable_routing(int enable);
 int bgp_set_poll(void);
 int bgp_process(uint32_t events[]);
-- 
2.20.1


From 59411b879992b8b6b9a5f8d42ae102719f6b245c Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Wed, 20 Jul 2011 14:43:44 +0200
Subject: [PATCH 05/16] Add a nexthop6 config option.

Will be used to set the NEXT_HOP for IPv6 routes, otherwise our address in
ipv6_prefix is used as the next hop.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 l2tpns.c | 1 +
 l2tpns.h | 1 +
 2 files changed, 2 insertions(+)

diff --git a/l2tpns.c b/l2tpns.c
index 8fdcb53..d70cc45 100644
--- a/l2tpns.c
+++ b/l2tpns.c
@@ -157,6 +157,7 @@ config_descriptt config_values[] = {
 	CONFIG("cluster_hb_timeout", cluster_hb_timeout, INT),
  	CONFIG("cluster_master_min_adv", cluster_master_min_adv, INT),
 	CONFIG("ipv6_prefix", ipv6_prefix, IPv6),
+	CONFIG("nexthop6_address", nexthop6_address, IPv6),
 	{ NULL, 0, 0, 0 },
 };
 
diff --git a/l2tpns.h b/l2tpns.h
index 6849be9..55de73f 100644
--- a/l2tpns.h
+++ b/l2tpns.h
@@ -724,6 +724,7 @@ typedef struct
 		int keepalive;
 		int hold;
 	} neighbour[BGP_NUM_PEERS];
+	struct in6_addr	nexthop6_address;
 #endif
 } configt;
 
-- 
2.20.1


From db276bcc801cbd2662f90a6b22cfc551ecddbf0f Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Wed, 20 Jul 2011 14:45:09 +0200
Subject: [PATCH 06/16] Handle multiprotocol UPDATE.

Add IPv6 routes advertisement handling, with MP path attributes heading
prepared on initialization.
BTW, fix a bug in attribute size calculation (for extended attr).

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 201 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 bgp.h |  22 ++++++-
 2 files changed, 222 insertions(+), 1 deletion(-)

diff --git a/bgp.c b/bgp.c
index 1e1713d..d9c5818 100644
--- a/bgp.c
+++ b/bgp.c
@@ -250,6 +250,51 @@ int bgp_start(struct bgp_peer *peer, char *name, int as, int keepalive,
 
     memcpy(peer->path_attrs, path_attrs, peer->path_attr_len);
 
+    /* multiprotocol attributes initialization */
+    if (config->ipv6_prefix.s6_addr[0])
+    {
+	struct bgp_attr_mp_reach_nlri_partial mp_reach_nlri_partial;
+	struct bgp_attr_mp_unreach_nlri_partial mp_unreach_nlri_partial;
+
+	a.flags = BGP_PATH_ATTR_FLAG_OPTIONAL;
+	a.code = BGP_PATH_ATTR_CODE_MP_REACH_NLRI;
+	a.data.s.len = 0; /* will be set on UPDATE */
+
+	mp_reach_nlri_partial.afi = htons(AF_INET6);
+	mp_reach_nlri_partial.safi = BGP_MP_SAFI_UNICAST;
+	mp_reach_nlri_partial.reserved = 0;
+	mp_reach_nlri_partial.next_hop_len = 16;
+
+	/* use the defined nexthop6, or our address in ipv6_prefix */
+	if (config->nexthop6_address.s6_addr[0])
+	    memcpy(&mp_reach_nlri_partial.next_hop,
+		    &config->nexthop6_address.s6_addr, 16);
+	else
+	{
+	    /* our address is ipv6prefix::1 */
+	    memcpy(&mp_reach_nlri_partial.next_hop,
+		    &config->ipv6_prefix.s6_addr, 16);
+	    mp_reach_nlri_partial.next_hop[15] = 1;
+	}
+
+	memcpy(&a.data.s.value, &mp_reach_nlri_partial,
+		sizeof(struct bgp_attr_mp_reach_nlri_partial));
+	memcpy(&peer->mp_reach_nlri_partial, &a,
+		BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE);
+
+	a.flags = BGP_PATH_ATTR_FLAG_OPTIONAL | BGP_PATH_ATTR_FLAG_EXTLEN;
+	a.code = BGP_PATH_ATTR_CODE_MP_UNREACH_NLRI;
+	a.data.e.len = 0; /* will be set on UPDATE */
+
+	mp_unreach_nlri_partial.afi = htons(AF_INET6);
+	mp_unreach_nlri_partial.safi = BGP_MP_SAFI_UNICAST;
+
+	memcpy(&a.data.e.value, &mp_unreach_nlri_partial,
+		sizeof(struct bgp_attr_mp_unreach_nlri_partial));
+	memcpy(&peer->mp_unreach_nlri_partial, &a,
+		BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE);
+    }
+
     LOG(4, 0, 0, "Initiating BGP connection to %s (routing %s)\n",
 	name, enable ? "enabled" : "suspended");
 
@@ -1492,6 +1537,162 @@ static int bgp_send_update(struct bgp_peer *peer)
 /* send/buffer UPDATE message for IPv6 routes */
 static int bgp_send_update6(struct bgp_peer *peer)
 {
+    uint16_t unf_len = 0;
+    uint16_t attr_len;
+    char *unreach_len;
+    uint8_t reach_len;
+    uint16_t len = sizeof(peer->outbuf->packet.header);
+    struct bgp_route6_list *have = peer->routes6;
+    struct bgp_route6_list *want = peer->routing ? bgp_routes6 : 0;
+    struct bgp_route6_list *e = 0;
+    struct bgp_route6_list *add = 0;
+    int s;
+    char ipv6addr[INET6_ADDRSTRLEN];
+
+    char *data = (char *) &peer->outbuf->packet.data;
+
+    /* need leave room for attr_len, bgp_path_attrs and one prefix */
+    char *max = (char *) &peer->outbuf->packet.data
+	+ sizeof(peer->outbuf->packet.data)
+	- sizeof(attr_len) - peer->path_attr_len_without_nexthop
+	- BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE - sizeof(struct bgp_ip6_prefix);
+
+    memset(peer->outbuf->packet.header.marker, 0xff,
+	sizeof(peer->outbuf->packet.header.marker));
+
+    peer->outbuf->packet.header.type = BGP_MSG_UPDATE;
+
+    /* insert non-MP unf_len */
+    memcpy(data, &unf_len, sizeof(unf_len));
+    /* skip over attr_len too; will be filled when known */
+    data += sizeof(unf_len) + sizeof(attr_len);
+    len += sizeof(unf_len) + sizeof(attr_len);
+
+    /* copy usual attributes */
+    memcpy(data, peer->path_attrs, peer->path_attr_len_without_nexthop);
+    data += peer->path_attr_len_without_nexthop;
+    len += peer->path_attr_len_without_nexthop;
+
+    /* copy MP unreachable NLRI heading */
+    memcpy(data, peer->mp_unreach_nlri_partial,
+	    BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE);
+    /* remember where to update this attr len */
+    unreach_len = data + 2;
+    data += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+    len += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+
+    peer->update_routes6 = 0; /* tentatively clear */
+
+    /* find differences */
+    while ((have || want) && data < (max - sizeof(struct bgp_ip6_prefix)))
+    {
+	if (have)
+	    s = want
+		? memcmp(&have->dest, &want->dest, sizeof(have->dest))
+		: -1;
+	else
+	    s = 1;
+
+	if (s < 0) /* found one to delete */
+	{
+	    struct bgp_route6_list *tmp = have;
+	    have = have->next;
+
+	    s = BGP_IP_PREFIX_SIZE(tmp->dest);
+	    memcpy(data, &tmp->dest, s);
+	    data += s;
+	    unf_len += s;
+	    len += s;
+
+	    LOG(5, 0, 0, "Withdrawing route %s/%d from BGP peer %s\n",
+		inet_ntop(AF_INET6, &tmp->dest.prefix, ipv6addr, INET6_ADDRSTRLEN),
+		tmp->dest.len, peer->name);
+
+	    free(tmp);
+
+	    if (e)
+		e->next = have;
+	    else
+		peer->routes6 = have;
+	}
+	else
+	{
+	    if (!s) /* same */
+	    {
+		e = have; /* stash the last found to relink above */
+		have = have->next;
+		want = want->next;
+	    }
+	    else if (s > 0) /* addition reqd. */
+	    {
+		if (add)
+		{
+		    peer->update_routes6 = 1; /* only one add per packet */
+		    if (!have)
+			break;
+		}
+		else
+		    add = want;
+
+		if (want)
+		    want = want->next;
+	    }
+	}
+    }
+
+    if (have || want)
+	peer->update_routes6 = 1; /* more to do */
+
+    /* anything changed? */
+    if (!(unf_len || add))
+	return 1;
+
+    /* go back and insert MP unf_len */
+    unf_len += sizeof(struct bgp_attr_mp_unreach_nlri_partial);
+    unf_len = htons(unf_len);
+    memcpy(&unreach_len, &unf_len, sizeof(unf_len));
+
+    if (add)
+    {
+	if (!(e = malloc(sizeof(*e))))
+	{
+	    LOG(0, 0, 0, "Can't allocate route for %s/%d (%s)\n",
+		inet_ntop(AF_INET6, &add->dest.prefix, ipv6addr, INET6_ADDRSTRLEN),
+		add->dest.len, strerror(errno));
+
+	    return 0;
+	}
+
+	memcpy(e, add, sizeof(*e));
+	e->next = 0;
+	peer->routes6 = bgp_insert_route6(peer->routes6, e);
+
+	/* copy MP reachable NLRI heading */
+	memcpy(data, peer->mp_reach_nlri_partial,
+		BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE);
+	/* with proper len */
+	reach_len = BGP_IP_PREFIX_SIZE(add->dest);
+	data[2] = reach_len;
+	data += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+	len += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+
+	memcpy(data, &add->dest, reach_len);
+	data += reach_len;
+	len += reach_len;
+
+	LOG(5, 0, 0, "Advertising route %s/%d to BGP peer %s\n",
+	    inet_ntop(AF_INET6, &add->dest.prefix, ipv6addr, INET6_ADDRSTRLEN),
+	    add->dest.len, peer->name);
+    }
+
+    /* go back and insert attr_len */
+    attr_len = htons(len - 4);
+    memcpy(&peer->outbuf->packet.data + 2, &attr_len, sizeof(attr_len));
+
+    peer->outbuf->packet.header.len = htons(len);
+    peer->outbuf->done = 0;
+
+    return bgp_write(peer);
 }
 
 /* send/buffer NOTIFICATION message */
diff --git a/bgp.h b/bgp.h
index db8b1ae..6dbfbf8 100644
--- a/bgp.h
+++ b/bgp.h
@@ -101,6 +101,22 @@ struct bgp_path_attr {
     } data; /* variable */
 } __attribute__ ((packed));
 
+struct bgp_attr_mp_reach_nlri_partial {
+    uint16_t afi; /* sa_family_t */
+    uint8_t safi;
+    uint8_t next_hop_len;
+    uint8_t next_hop[16];
+    uint8_t reserved;
+} __attribute__ ((packed));
+#define BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE (3 + sizeof(struct bgp_attr_mp_reach_nlri_partial))
+
+struct bgp_attr_mp_unreach_nlri_partial {
+    uint16_t afi; /* sa_family_t */
+    uint8_t safi;
+} __attribute__ ((packed));
+/* we use it as an extended attribute */
+#define BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE (4 + sizeof(struct bgp_attr_mp_unreach_nlri_partial))
+
 /* bgp_path_attr.flags (bitfields) */
 #define BGP_PATH_ATTR_FLAG_OPTIONAL	(1 << 7)
 #define BGP_PATH_ATTR_FLAG_TRANS	(1 << 6)
@@ -121,9 +137,11 @@ struct bgp_path_attr {
 #define BGP_PATH_ATTR_CODE_ATOMIC_AGGREGATE	6	/* well-known, discretionary */
 #define BGP_PATH_ATTR_CODE_AGGREGATOR		7	/* optional, transitive */
 #define BGP_PATH_ATTR_CODE_COMMUNITIES		8	/* optional, transitive (RFC1997) */
+#define BGP_PATH_ATTR_CODE_MP_REACH_NLRI	14	/* optional, non-transitive (RFC4760) */
+#define BGP_PATH_ATTR_CODE_MP_UNREACH_NLRI	15	/* optional, non-transitive (RFC4760) */
 
 #define BGP_PATH_ATTR_SIZE(p) ((((p).flags & BGP_PATH_ATTR_FLAG_EXTLEN) \
-    ? ((p).data.e.len + 1) : (p).data.s.len) + 3)
+    ? ((p).data.e.len + 4) : (p).data.s.len) + 3)
 
 /* well known COMMUNITIES */
 #define BGP_COMMUNITY_NO_EXPORT			0xffffff01	/* don't advertise outside confederation */
@@ -222,6 +240,8 @@ struct bgp_peer {
     int handle_ipv6_routes;		/* can handle IPv6 routes advertisements */
     int update_routes6;			/* UPDATE required for IPv6 routes */
     struct bgp_route6_list *routes6;	/* IPv6 routes known by this peer */
+    char mp_reach_nlri_partial[BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE];
+    char mp_unreach_nlri_partial[BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE];
 };
 
 /* bgp_peer.cli_flag */
-- 
2.20.1


From 7407c1a1c49077a8d3abbaf276936b05aa9023c9 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Wed, 20 Jul 2011 14:53:19 +0200
Subject: [PATCH 07/16] Add config examples for nexthop/nexthop6.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 etc/startup-config.default | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/etc/startup-config.default b/etc/startup-config.default
index 77425c8..4a3ac84 100644
--- a/etc/startup-config.default
+++ b/etc/startup-config.default
@@ -102,6 +102,10 @@ set accounting_dir "/var/run/l2tpns/acct"
 # IPv6 address prefix
 #set ipv6_prefix ::
 
+# BGP NEXT_HOP path attribute
+#set nexthop 10.0.1.1
+#set nexthop6 2001:db8::1
+
 # Drop/kill sessions
 #load plugin "sessionctl"
 
-- 
2.20.1


From e86393ea90d7ca3d84ecdd2e749a2e1372c60827 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Wed, 20 Jul 2011 17:46:21 +0200
Subject: [PATCH 08/16] Better handle IPv6 routes advertisement (non-)support.

Don't try to enable multiprotocol BGP when the peer says it can't.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 49 ++++++++++++++++++++++++++++++++-----------------
 bgp.h |  8 +++++++-
 2 files changed, 39 insertions(+), 18 deletions(-)

diff --git a/bgp.c b/bgp.c
index d9c5818..7de3d3f 100644
--- a/bgp.c
+++ b/bgp.c
@@ -295,6 +295,8 @@ int bgp_start(struct bgp_peer *peer, char *name, int as, int keepalive,
 		BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE);
     }
 
+    peer->mp_handling = HandlingUnknown;
+
     LOG(4, 0, 0, "Initiating BGP connection to %s (routing %s)\n",
 	name, enable ? "enabled" : "suspended");
 
@@ -967,8 +969,6 @@ static int bgp_connect(struct bgp_peer *peer)
 
     LOG(4, 0, 0, "BGP peer %s: state Active\n", inet_ntoa(addr.sin_addr));
 
-    peer->handle_ipv6_routes = 0;
-
     return bgp_send_open(peer);
 }
 
@@ -992,8 +992,6 @@ static int bgp_handle_connect(struct bgp_peer *peer)
 
     LOG(4, 0, 0, "BGP peer %s: state Active\n", peer->name);
 
-    peer->handle_ipv6_routes = 0;
-
     return bgp_send_open(peer);
 }
 
@@ -1260,7 +1258,7 @@ static int bgp_handle_input(struct bgp_peer *peer)
 			continue;
 		    }
 
-		    peer->handle_ipv6_routes = 1;
+		    peer->mp_handling = HandleIPv6Routes;
 		}
 	    }
 
@@ -1298,6 +1296,15 @@ static int bgp_handle_input(struct bgp_peer *peer)
 		return 0;
 	    }
 
+	    if (notification->error_code == BGP_ERR_OPEN
+		    && notification->error_subcode == BGP_ERR_OPN_UNSUP_PARAM)
+	    {
+		LOG(4, 0, 0, "BGP peer %s doesn't support BGP Capabilities\n", peer->name);
+		peer->mp_handling = DoesntHandleIPv6Routes;
+		bgp_restart(peer);
+		return 0;
+	    }
+
 	    if (notification->error_code == BGP_ERR_OPEN
 		    && notification->error_subcode == BGP_ERR_OPN_UNSUP_CAP)
 	    {
@@ -1305,7 +1312,7 @@ static int bgp_handle_input(struct bgp_peer *peer)
 		   an "unsupported capability" message, we disable IPv6 routes for
 		   this peer */
 		LOG(4, 0, 0, "BGP peer %s doesn't support IPv6 routes advertisement\n", peer->name);
-		peer->handle_ipv6_routes = 0;
+		peer->mp_handling = DoesntHandleIPv6Routes;
 		break;
 	    }
 
@@ -1354,20 +1361,28 @@ static int bgp_send_open(struct bgp_peer *peer)
     data.hold_time = htons(peer->hold);
     data.identifier = my_address;
 
-    /* construct the param and capability */
-    cap_mp_ipv6.code = BGP_CAP_CODE_MP;
-    cap_mp_ipv6.len = sizeof(mp_ipv6);
-    memcpy(&cap_mp_ipv6.value, &mp_ipv6, cap_mp_ipv6.len);
+    /* if we know peer doesn't support MP (mp_handling == DoesntHandleIPv6Routes)
+       then don't add this parameter */
+    if (peer->mp_handling == HandlingUnknown
+	    || peer->mp_handling == HandleIPv6Routes)
+    {
+	/* construct the param and capability */
+	cap_mp_ipv6.code = BGP_CAP_CODE_MP;
+	cap_mp_ipv6.len = sizeof(mp_ipv6);
+	memcpy(&cap_mp_ipv6.value, &mp_ipv6, cap_mp_ipv6.len);
 
-    param_cap_mp_ipv6.type = BGP_PARAM_TYPE_CAPABILITY;
-    param_cap_mp_ipv6.len = 2 + sizeof(mp_ipv6);
-    memcpy(&param_cap_mp_ipv6.value, &cap_mp_ipv6, param_cap_mp_ipv6.len);
+	param_cap_mp_ipv6.type = BGP_PARAM_TYPE_CAPABILITY;
+	param_cap_mp_ipv6.len = 2 + sizeof(mp_ipv6);
+	memcpy(&param_cap_mp_ipv6.value, &cap_mp_ipv6, param_cap_mp_ipv6.len);
 
-    data.opt_len = 2 + param_cap_mp_ipv6.len;
-    memcpy(&data.opt_params, &param_cap_mp_ipv6, data.opt_len);
+	data.opt_len = 2 + param_cap_mp_ipv6.len;
+	memcpy(&data.opt_params, &param_cap_mp_ipv6, data.opt_len);
+    }
+    else
+	data.opt_len = 0;
 
-    memcpy(peer->outbuf->packet.data, &data, BGP_DATA_OPEN_SIZE);
-    len += BGP_DATA_OPEN_SIZE;
+    memcpy(peer->outbuf->packet.data, &data, BGP_DATA_OPEN_SIZE + data.opt_len);
+    len += BGP_DATA_OPEN_SIZE + data.opt_len;
 
     peer->outbuf->packet.header.len = htons(len);
     peer->outbuf->done = 0;
diff --git a/bgp.h b/bgp.h
index 6dbfbf8..8702838 100644
--- a/bgp.h
+++ b/bgp.h
@@ -209,6 +209,12 @@ struct bgp_buf {
     size_t done;			/* bytes sent/recvd */
 };
 
+enum bgp_mp_handling {
+    HandleIPv6Routes,
+    DoesntHandleIPv6Routes,
+    HandlingUnknown,
+};
+
 /* state */
 struct bgp_peer {
     char name[32];			/* peer name */
@@ -237,7 +243,7 @@ struct bgp_peer {
     int path_attr_len_without_nexthop;	/* length of path attrs  without NEXT_HOP */
     uint32_t events;			/* events to poll */
     struct event_data edata;		/* poll data */
-    int handle_ipv6_routes;		/* can handle IPv6 routes advertisements */
+    enum bgp_mp_handling mp_handling;	/* how it handles IPv6 routes advertisements */
     int update_routes6;			/* UPDATE required for IPv6 routes */
     struct bgp_route6_list *routes6;	/* IPv6 routes known by this peer */
     char mp_reach_nlri_partial[BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE];
-- 
2.20.1


From 3ea85f7c57f8d555396feb5121e7253f04ee6774 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Thu, 21 Jul 2011 02:49:38 +0200
Subject: [PATCH 09/16] Fix the AFI for IPv6.

The Address Family Identifier was not the one I thought it was.
Define it and fix its use for IPv6.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 8 ++++----
 bgp.h | 4 ++++
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/bgp.c b/bgp.c
index 7de3d3f..f0ff879 100644
--- a/bgp.c
+++ b/bgp.c
@@ -260,7 +260,7 @@ int bgp_start(struct bgp_peer *peer, char *name, int as, int keepalive,
 	a.code = BGP_PATH_ATTR_CODE_MP_REACH_NLRI;
 	a.data.s.len = 0; /* will be set on UPDATE */
 
-	mp_reach_nlri_partial.afi = htons(AF_INET6);
+	mp_reach_nlri_partial.afi = htons(BGP_MP_AFI_IPv6);
 	mp_reach_nlri_partial.safi = BGP_MP_SAFI_UNICAST;
 	mp_reach_nlri_partial.reserved = 0;
 	mp_reach_nlri_partial.next_hop_len = 16;
@@ -286,7 +286,7 @@ int bgp_start(struct bgp_peer *peer, char *name, int as, int keepalive,
 	a.code = BGP_PATH_ATTR_CODE_MP_UNREACH_NLRI;
 	a.data.e.len = 0; /* will be set on UPDATE */
 
-	mp_unreach_nlri_partial.afi = htons(AF_INET6);
+	mp_unreach_nlri_partial.afi = htons(BGP_MP_AFI_IPv6);
 	mp_unreach_nlri_partial.safi = BGP_MP_SAFI_UNICAST;
 
 	memcpy(&a.data.e.value, &mp_unreach_nlri_partial,
@@ -1247,7 +1247,7 @@ static int bgp_handle_input(struct bgp_peer *peer)
 
 		    mp_cap = (struct bgp_mp_cap_param *)&capability->value;
 		    /* the only <AFI, SAFI> tuple we support */
-		    if (ntohs(mp_cap->afi) != AF_INET6 && mp_cap->safi != BGP_MP_SAFI_UNICAST)
+		    if (ntohs(mp_cap->afi) != BGP_MP_AFI_IPv6 && mp_cap->safi != BGP_MP_SAFI_UNICAST)
 		    {
 			LOG(4, 0, 0, "Unsupported multiprotocol AFI %d and SAFI %d from BGP peer %s\n",
 			    mp_cap->afi, mp_cap->safi, peer->name);
@@ -1346,7 +1346,7 @@ static int bgp_handle_input(struct bgp_peer *peer)
 static int bgp_send_open(struct bgp_peer *peer)
 {
     struct bgp_data_open data;
-    struct bgp_mp_cap_param mp_ipv6 = { htons(AF_INET6), 0, BGP_MP_SAFI_UNICAST };
+    struct bgp_mp_cap_param mp_ipv6 = { htons(BGP_MP_AFI_IPv6), 0, BGP_MP_SAFI_UNICAST };
     struct bgp_capability cap_mp_ipv6;
     struct bgp_opt_param param_cap_mp_ipv6;
     uint16_t len = sizeof(peer->outbuf->packet.header);
diff --git a/bgp.h b/bgp.h
index 8702838..fc2e517 100644
--- a/bgp.h
+++ b/bgp.h
@@ -67,6 +67,10 @@ struct bgp_mp_cap_param {
     uint8_t safi;
 } __attribute__ ((packed));
 
+/* bgp_mp_cap_param.afi */
+#define BGP_MP_AFI_RESERVED	0
+#define BGP_MP_AFI_IPv4		1
+#define BGP_MP_AFI_IPv6		2
 /* bgp_mp_cap_param.safi */
 #define BGP_MP_SAFI_UNICAST	1
 #define BGP_MP_SAFI_MULTICAST	2
-- 
2.20.1


From 3bd675ad2c2b0c1cb3dfe52deb179f24e05c41b4 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Sat, 23 Jul 2011 18:53:12 +0200
Subject: [PATCH 10/16] Better check for IPv6 compatibility with the BGP peer.

We would advertise IPv6 routes to non-multiprotocol aware peers. Fix that.
Also, fix the way we parse options, to handle multiple optional parameters with
one capability in it (or many; it's just the way quagga send them).

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 27 ++++++++++++++++++---------
 1 file changed, 18 insertions(+), 9 deletions(-)

diff --git a/bgp.c b/bgp.c
index f0ff879..795ffc5 100644
--- a/bgp.c
+++ b/bgp.c
@@ -556,7 +556,8 @@ int bgp_add_route6(struct in6_addr ip, int prefixlen)
 
     /* flag established peers for update */
     for (i = 0; i < BGP_NUM_PEERS; i++)
-	if (bgp_peers[i].state == Established)
+	if (bgp_peers[i].state == Established
+		&& bgp_peers[i].mp_handling == HandleIPv6Routes)
 	    bgp_peers[i].update_routes6 = 1;
 
     LOG(4, 0, 0, "Registered BGP route %s/%d\n",
@@ -656,7 +657,8 @@ int bgp_del_route6(struct in6_addr ip, int prefixlen)
 
     /* flag established peers for update */
     for (i = 0; i < BGP_NUM_PEERS; i++)
-	if (bgp_peers[i].state == Established)
+	if (bgp_peers[i].state == Established
+		&& bgp_peers[i].mp_handling == HandleIPv6Routes)
 	    bgp_peers[i].update_routes6 = 1;
 
     LOG(4, 0, 0, "Removed BGP route %s/%d\n",
@@ -1185,7 +1187,7 @@ static int bgp_handle_input(struct bgp_peer *peer)
 		    param_offset < data.opt_len;
 		    param_offset += 2 + param->len)
 	    {
-		param = (struct bgp_opt_param *)(&data.opt_params + param_offset);
+		param = (struct bgp_opt_param *)((char *)&data.opt_params + param_offset);
 
 		/* sensible check */
 		if (data.opt_len - param_offset < 2
@@ -1210,11 +1212,8 @@ static int bgp_handle_input(struct bgp_peer *peer)
 
 		capabilities_len = param->len;
 		capabilities = (char *)&param->value;
-	    }
 
-	    /* look for BGP multiprotocol capability */
-	    if (capabilities)
-	    {
+		/* look for BGP multiprotocol capability */
 		for (capability_offset = 0;
 			capability_offset < capabilities_len;
 			capability_offset += 2 + capability->len)
@@ -1258,10 +1257,19 @@ static int bgp_handle_input(struct bgp_peer *peer)
 			continue;
 		    }
 
+		    /* yes it can! */
 		    peer->mp_handling = HandleIPv6Routes;
 		}
 	    }
 
+	    if (peer->mp_handling != HandleIPv6Routes)
+	    {
+		peer->mp_handling = DoesntHandleIPv6Routes;
+		if (config->ipv6_prefix.s6_addr[0])
+		    LOG(1, 0, 0, "Warning: BGP peer %s doesn't handle IPv6 prefixes updates\n",
+			    peer->name);
+	    }
+
 	    /* next transition requires an exchange of keepalives */
 	    bgp_send_keepalive(peer);
 	}
@@ -1363,8 +1371,9 @@ static int bgp_send_open(struct bgp_peer *peer)
 
     /* if we know peer doesn't support MP (mp_handling == DoesntHandleIPv6Routes)
        then don't add this parameter */
-    if (peer->mp_handling == HandlingUnknown
-	    || peer->mp_handling == HandleIPv6Routes)
+    if (config->ipv6_prefix.s6_addr[0]
+	    && (peer->mp_handling == HandlingUnknown
+		|| peer->mp_handling == HandleIPv6Routes))
     {
 	/* construct the param and capability */
 	cap_mp_ipv6.code = BGP_CAP_CODE_MP;
-- 
2.20.1


From c7e67e1de278393c129179469be6de9fed282187 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Sat, 23 Jul 2011 20:46:39 +0200
Subject: [PATCH 11/16] Don't send notification when receiving an unsupported
 capability.

Tested peer (quagga) doesn't interpret it nicely (i.e. it shuts the connection
down). Better not tell anything.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 15 ---------------
 1 file changed, 15 deletions(-)

diff --git a/bgp.c b/bgp.c
index 795ffc5..420a925 100644
--- a/bgp.c
+++ b/bgp.c
@@ -49,8 +49,6 @@ static int bgp_send_update(struct bgp_peer *peer);
 static int bgp_send_update6(struct bgp_peer *peer);
 static int bgp_send_notification(struct bgp_peer *peer, uint8_t code,
     uint8_t subcode);
-static int bgp_send_notification_full(struct bgp_peer *peer, uint8_t code,
-    uint8_t subcode, char *notification_data, uint16_t data_len);
 
 static uint16_t our_as;
 static struct bgp_route_list *bgp_routes = 0;
@@ -1238,8 +1236,6 @@ static int bgp_handle_input(struct bgp_peer *peer)
 			LOG(4, 0, 0, "Unsupported Capability code %d from BGP peer %s\n",
 			    capability->code, peer->name);
 
-			bgp_send_notification_full(peer, BGP_ERR_OPEN, BGP_ERR_OPN_UNSUP_CAP,
-				(char *)capability, 2 + capability->len);
 			/* we don't terminate, still; we just jump to the next one */
 			continue;
 		    }
@@ -1251,8 +1247,6 @@ static int bgp_handle_input(struct bgp_peer *peer)
 			LOG(4, 0, 0, "Unsupported multiprotocol AFI %d and SAFI %d from BGP peer %s\n",
 			    mp_cap->afi, mp_cap->safi, peer->name);
 
-			bgp_send_notification_full(peer, BGP_ERR_OPEN, BGP_ERR_OPN_UNSUP_CAP,
-				(char *)capability, 2 + capability->len);
 			/* we don't terminate, still; we just jump to the next one */
 			continue;
 		    }
@@ -1722,12 +1716,6 @@ static int bgp_send_update6(struct bgp_peer *peer)
 /* send/buffer NOTIFICATION message */
 static int bgp_send_notification(struct bgp_peer *peer, uint8_t code,
     uint8_t subcode)
-{
-    return bgp_send_notification_full(peer, code, subcode, NULL, 0);
-}
-
-static int bgp_send_notification_full(struct bgp_peer *peer, uint8_t code,
-    uint8_t subcode, char *notification_data, uint16_t data_len)
 {
     struct bgp_data_notification data;
     uint16_t len = 0;
@@ -1738,9 +1726,6 @@ static int bgp_send_notification_full(struct bgp_peer *peer, uint8_t code,
     data.error_subcode = subcode;
     len += sizeof(data.error_code);
 
-    memcpy(data.data, notification_data, data_len);
-    len += data_len;
-
     memset(peer->outbuf->packet.header.marker, 0xff,
 	sizeof(peer->outbuf->packet.header.marker));
 
-- 
2.20.1


From 7e1e91ef98ed8d48a0aa97c44a7a3da32ef088a8 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Sat, 23 Jul 2011 22:29:59 +0200
Subject: [PATCH 12/16] Call bgp_add/del_route6() in l2tpns.c.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 l2tpns.c | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/l2tpns.c b/l2tpns.c
index d70cc45..6a4656e 100644
--- a/l2tpns.c
+++ b/l2tpns.c
@@ -502,7 +502,12 @@ void route6set(sessionidt s, struct in6_addr ip, int prefixlen, int add)
 		LOG(0, 0, 0, "route6set() error in ioctl: %s\n",
 				strerror(errno));
 
-	// FIXME: need to add BGP routing (RFC2858)
+#ifdef BGP
+	if (add)
+		bgp_add_route6(ip, prefixlen);
+	else
+		bgp_del_route6(ip, prefixlen);
+#endif /* BGP */
 
 	if (s)
 	{
-- 
2.20.1


From 350b06e6f4e8c49fc68105c834e99db226de8720 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Mon, 25 Jul 2011 01:00:03 +0200
Subject: [PATCH 13/16] Fix bugs in IPv6 update routine.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 23 ++++++++++++++++-------
 1 file changed, 16 insertions(+), 7 deletions(-)

diff --git a/bgp.c b/bgp.c
index 420a925..31e6424 100644
--- a/bgp.c
+++ b/bgp.c
@@ -1665,10 +1665,19 @@ static int bgp_send_update6(struct bgp_peer *peer)
     if (!(unf_len || add))
 	return 1;
 
-    /* go back and insert MP unf_len */
-    unf_len += sizeof(struct bgp_attr_mp_unreach_nlri_partial);
-    unf_len = htons(unf_len);
-    memcpy(&unreach_len, &unf_len, sizeof(unf_len));
+    if (unf_len)
+    {
+	/* go back and insert MP unf_len */
+	unf_len += sizeof(struct bgp_attr_mp_unreach_nlri_partial);
+	unf_len = htons(unf_len);
+	memcpy(unreach_len, &unf_len, sizeof(unf_len));
+    }
+    else
+    {
+	/* we can remove this attribute, then */
+	data -= BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+	len -= BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+    }
 
     if (add)
     {
@@ -1690,9 +1699,9 @@ static int bgp_send_update6(struct bgp_peer *peer)
 		BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE);
 	/* with proper len */
 	reach_len = BGP_IP_PREFIX_SIZE(add->dest);
-	data[2] = reach_len;
-	data += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
-	len += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+	data[2] = sizeof(struct bgp_attr_mp_reach_nlri_partial) + reach_len;
+	data += BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE;
+	len += BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE;
 
 	memcpy(data, &add->dest, reach_len);
 	data += reach_len;
-- 
2.20.1


From 974d5f4408a77c2a8dae8aa112a6e85687d2421b Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Mon, 25 Jul 2011 11:40:29 +0200
Subject: [PATCH 14/16] A bit of renaming for clarity.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 30 +++++++++++++++---------------
 1 file changed, 15 insertions(+), 15 deletions(-)

diff --git a/bgp.c b/bgp.c
index 31e6424..e2939d2 100644
--- a/bgp.c
+++ b/bgp.c
@@ -1555,9 +1555,9 @@ static int bgp_send_update(struct bgp_peer *peer)
 /* send/buffer UPDATE message for IPv6 routes */
 static int bgp_send_update6(struct bgp_peer *peer)
 {
-    uint16_t unf_len = 0;
     uint16_t attr_len;
-    char *unreach_len;
+    uint16_t unreach_len = 0;
+    char *unreach_len_pos;
     uint8_t reach_len;
     uint16_t len = sizeof(peer->outbuf->packet.header);
     struct bgp_route6_list *have = peer->routes6;
@@ -1580,11 +1580,11 @@ static int bgp_send_update6(struct bgp_peer *peer)
 
     peer->outbuf->packet.header.type = BGP_MSG_UPDATE;
 
-    /* insert non-MP unf_len */
-    memcpy(data, &unf_len, sizeof(unf_len));
-    /* skip over attr_len too; will be filled when known */
-    data += sizeof(unf_len) + sizeof(attr_len);
-    len += sizeof(unf_len) + sizeof(attr_len);
+    /* insert non-MP unfeasible routes length */
+    memcpy(data, &unreach_len, sizeof(unreach_len));
+    /* skip over it and attr_len too; it will be filled when known */
+    data += sizeof(unreach_len) + sizeof(attr_len);
+    len += sizeof(unreach_len) + sizeof(attr_len);
 
     /* copy usual attributes */
     memcpy(data, peer->path_attrs, peer->path_attr_len_without_nexthop);
@@ -1595,7 +1595,7 @@ static int bgp_send_update6(struct bgp_peer *peer)
     memcpy(data, peer->mp_unreach_nlri_partial,
 	    BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE);
     /* remember where to update this attr len */
-    unreach_len = data + 2;
+    unreach_len_pos = data + 2;
     data += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
     len += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
 
@@ -1619,7 +1619,7 @@ static int bgp_send_update6(struct bgp_peer *peer)
 	    s = BGP_IP_PREFIX_SIZE(tmp->dest);
 	    memcpy(data, &tmp->dest, s);
 	    data += s;
-	    unf_len += s;
+	    unreach_len += s;
 	    len += s;
 
 	    LOG(5, 0, 0, "Withdrawing route %s/%d from BGP peer %s\n",
@@ -1662,15 +1662,15 @@ static int bgp_send_update6(struct bgp_peer *peer)
 	peer->update_routes6 = 1; /* more to do */
 
     /* anything changed? */
-    if (!(unf_len || add))
+    if (!(unreach_len || add))
 	return 1;
 
-    if (unf_len)
+    if (unreach_len)
     {
-	/* go back and insert MP unf_len */
-	unf_len += sizeof(struct bgp_attr_mp_unreach_nlri_partial);
-	unf_len = htons(unf_len);
-	memcpy(unreach_len, &unf_len, sizeof(unf_len));
+	/* go back and insert MP unreach_len */
+	unreach_len += sizeof(struct bgp_attr_mp_unreach_nlri_partial);
+	unreach_len = htons(unreach_len);
+	memcpy(unreach_len_pos, &unreach_len, sizeof(unreach_len));
     }
     else
     {
-- 
2.20.1


From e6fe57eec8fc00543e7ee5f4e137331a6c65217c Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Mon, 25 Jul 2011 12:12:53 +0200
Subject: [PATCH 15/16] Fix a bug in parameters length setting.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/bgp.c b/bgp.c
index e2939d2..7ac4c12 100644
--- a/bgp.c
+++ b/bgp.c
@@ -1714,7 +1714,7 @@ static int bgp_send_update6(struct bgp_peer *peer)
 
     /* go back and insert attr_len */
     attr_len = htons(len - 4);
-    memcpy(&peer->outbuf->packet.data + 2, &attr_len, sizeof(attr_len));
+    memcpy((char *)&peer->outbuf->packet.data + 2, &attr_len, sizeof(attr_len));
 
     peer->outbuf->packet.header.len = htons(len);
     peer->outbuf->done = 0;
-- 
2.20.1


From 60bd25ccdcd9f71e23d64d62dd6273dedc5abbb0 Mon Sep 17 00:00:00 2001
From: Benjamin Cama <benoar@dolka.fr>
Date: Mon, 25 Jul 2011 12:40:50 +0200
Subject: [PATCH 16/16] Really fix parameters length calculation this time.

Signed-off-by: Benjamin Cama <benoar@dolka.fr>
---
 bgp.c | 17 ++++++++++-------
 1 file changed, 10 insertions(+), 7 deletions(-)

diff --git a/bgp.c b/bgp.c
index 7ac4c12..0566393 100644
--- a/bgp.c
+++ b/bgp.c
@@ -1589,7 +1589,7 @@ static int bgp_send_update6(struct bgp_peer *peer)
     /* copy usual attributes */
     memcpy(data, peer->path_attrs, peer->path_attr_len_without_nexthop);
     data += peer->path_attr_len_without_nexthop;
-    len += peer->path_attr_len_without_nexthop;
+    attr_len = peer->path_attr_len_without_nexthop;
 
     /* copy MP unreachable NLRI heading */
     memcpy(data, peer->mp_unreach_nlri_partial,
@@ -1597,7 +1597,7 @@ static int bgp_send_update6(struct bgp_peer *peer)
     /* remember where to update this attr len */
     unreach_len_pos = data + 2;
     data += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
-    len += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+    attr_len += BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
 
     peer->update_routes6 = 0; /* tentatively clear */
 
@@ -1620,7 +1620,7 @@ static int bgp_send_update6(struct bgp_peer *peer)
 	    memcpy(data, &tmp->dest, s);
 	    data += s;
 	    unreach_len += s;
-	    len += s;
+	    attr_len += s;
 
 	    LOG(5, 0, 0, "Withdrawing route %s/%d from BGP peer %s\n",
 		inet_ntop(AF_INET6, &tmp->dest.prefix, ipv6addr, INET6_ADDRSTRLEN),
@@ -1676,7 +1676,7 @@ static int bgp_send_update6(struct bgp_peer *peer)
     {
 	/* we can remove this attribute, then */
 	data -= BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
-	len -= BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
+	attr_len -= BGP_PATH_ATTR_MP_UNREACH_NLRI_PARTIAL_SIZE;
     }
 
     if (add)
@@ -1701,19 +1701,22 @@ static int bgp_send_update6(struct bgp_peer *peer)
 	reach_len = BGP_IP_PREFIX_SIZE(add->dest);
 	data[2] = sizeof(struct bgp_attr_mp_reach_nlri_partial) + reach_len;
 	data += BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE;
-	len += BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE;
+	attr_len += BGP_PATH_ATTR_MP_REACH_NLRI_PARTIAL_SIZE;
 
 	memcpy(data, &add->dest, reach_len);
 	data += reach_len;
-	len += reach_len;
+	attr_len += reach_len;
 
 	LOG(5, 0, 0, "Advertising route %s/%d to BGP peer %s\n",
 	    inet_ntop(AF_INET6, &add->dest.prefix, ipv6addr, INET6_ADDRSTRLEN),
 	    add->dest.len, peer->name);
     }
 
+    /* update len with attributes we added */
+    len += attr_len;
+
     /* go back and insert attr_len */
-    attr_len = htons(len - 4);
+    attr_len = htons(attr_len);
     memcpy((char *)&peer->outbuf->packet.data + 2, &attr_len, sizeof(attr_len));
 
     peer->outbuf->packet.header.len = htons(len);
-- 
2.20.1