Home | History | Annotate | Line # | Download | only in ldpd
socketops.c revision 1.31
      1 /* $NetBSD: socketops.c,v 1.31 2013/07/27 14:35:41 kefren Exp $ */
      2 
      3 /*
      4  * Copyright (c) 2010 The NetBSD Foundation, Inc.
      5  * All rights reserved.
      6  *
      7  * This code is derived from software contributed to The NetBSD Foundation
      8  * by Mihai Chelaru <kefren (at) NetBSD.org>
      9  *
     10  * Redistribution and use in source and binary forms, with or without
     11  * modification, are permitted provided that the following conditions
     12  * are met:
     13  * 1. Redistributions of source code must retain the above copyright
     14  *    notice, this list of conditions and the following disclaimer.
     15  * 2. Redistributions in binary form must reproduce the above copyright
     16  *    notice, this list of conditions and the following disclaimer in the
     17  *    documentation and/or other materials provided with the distribution.
     18  *
     19  * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
     20  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
     21  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
     22  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
     23  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
     25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
     26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
     27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
     28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     29  * POSSIBILITY OF SUCH DAMAGE.
     30  */
     31 
     32 #include <sys/types.h>
     33 #include <sys/stat.h>
     34 #include <sys/socket.h>
     35 #include <sys/ioctl.h>
     36 #include <net/if.h>
     37 #include <netinet/in.h>
     38 #include <arpa/inet.h>
     39 
     40 #include <assert.h>
     41 #include <errno.h>
     42 #include <ifaddrs.h>
     43 #include <poll.h>
     44 #include <signal.h>
     45 #include <stdio.h>
     46 #include <stdlib.h>
     47 #include <strings.h>
     48 #include <unistd.h>
     49 
     50 #include "conffile.h"
     51 #include "fsm.h"
     52 #include "ldp.h"
     53 #include "ldp_command.h"
     54 #include "tlv.h"
     55 #include "ldp_peer.h"
     56 #include "notifications.h"
     57 #include "tlv_stack.h"
     58 #include "mpls_interface.h"
     59 #include "label.h"
     60 #include "mpls_routes.h"
     61 #include "ldp_errors.h"
     62 #include "socketops.h"
     63 
     64 int ls;				/* TCP listening socket on port 646 */
     65 int route_socket;		/* used to see when a route is added/deleted */
     66 int command_socket;		/* Listening socket for interface command */
     67 int current_msg_id = 0x233;
     68 int command_port = LDP_COMMAND_PORT;
     69 extern int      replay_index;
     70 extern struct rt_msg replay_rt[REPLAY_MAX];
     71 extern struct com_sock	csockets[MAX_COMMAND_SOCKETS];
     72 
     73 int	ldp_hello_time = LDP_HELLO_TIME;
     74 int	ldp_keepalive_time = LDP_KEEPALIVE_TIME;
     75 int	ldp_holddown_time = LDP_HOLDTIME;
     76 int	no_default_route = 1;
     77 int	loop_detection = 0;
     78 bool	may_connect;
     79 
     80 void	recv_pdu(int);
     81 void	send_hello_alarm(int);
     82 __dead static void bail_out(int);
     83 static void print_info(int);
     84 static int bind_socket(int s, int stype);
     85 static int set_tos(int);
     86 static int socket_reuse_port(int);
     87 static int get_local_addr(struct sockaddr_dl *, struct in_addr *);
     88 static int is_hello_socket(int);
     89 static int is_passive_if(const char *if_name);
     90 
     91 int
     92 create_hello_sockets()
     93 {
     94 	struct ip_mreq  mcast_addr;
     95 	int s, joined_groups;
     96 	struct ifaddrs *ifa, *ifb;
     97 	uint lastifindex;
     98 #ifdef INET6
     99 	struct ipv6_mreq mcast_addr6;
    100 	struct sockaddr_in6 *if_sa6;
    101 #endif
    102 	struct hello_socket *hs;
    103 
    104 	SLIST_INIT(&hello_socket_head);
    105 
    106 	s = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    107 	if (s < 0)
    108 		return s;
    109 	debugp("INET4 socket created (%d)\n", s);
    110 	/*
    111 	 * RFC5036 specifies we should listen to all subnet routers multicast
    112 	 * group
    113 	 */
    114 	mcast_addr.imr_multiaddr.s_addr = htonl(INADDR_ALLRTRS_GROUP);
    115 
    116 	if (socket_reuse_port(s) < 0)
    117 		goto chs_error;
    118 	/* Bind it to port 646 */
    119 	if (bind_socket(s, AF_INET) == -1) {
    120 		warnp("Cannot bind INET hello socket\n");
    121 		goto chs_error;
    122 	}
    123 
    124 	/* We don't need to receive back our messages */
    125 	if (setsockopt(s, IPPROTO_IP, IP_MULTICAST_LOOP, &(u_char){0},
    126 	    sizeof(u_char)) == -1) {
    127 		fatalp("INET setsockopt IP_MCAST_LOOP: %s\n", strerror(errno));
    128 		goto chs_error;
    129 	}
    130 	/* Finally join the group on all interfaces */
    131 	if (getifaddrs(&ifa) == -1) {
    132 		fatalp("Cannot iterate interfaces\n");
    133 		return -1;
    134 	}
    135 	lastifindex = UINT_MAX;
    136 	joined_groups = 0;
    137 	for (ifb = ifa; ifb; ifb = ifb->ifa_next) {
    138 		struct sockaddr_in *if_sa = (struct sockaddr_in *) ifb->ifa_addr;
    139 		if (if_sa->sin_family != AF_INET || (!(ifb->ifa_flags & IFF_UP)) ||
    140 		    (ifb->ifa_flags & IFF_LOOPBACK) ||
    141 		    (!(ifb->ifa_flags & IFF_MULTICAST)) ||
    142 		    (ntohl(if_sa->sin_addr.s_addr) >> 24 == IN_LOOPBACKNET) ||
    143 		    is_passive_if(ifb->ifa_name) ||
    144 		    lastifindex == if_nametoindex(ifb->ifa_name))
    145 			continue;
    146 		lastifindex = if_nametoindex(ifb->ifa_name);
    147 
    148 		mcast_addr.imr_interface.s_addr = if_sa->sin_addr.s_addr;
    149 		debugp("Join IPv4 mcast on %s\n", ifb->ifa_name);
    150         	if (setsockopt(s, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &mcast_addr,
    151 		    sizeof(mcast_addr)) == -1) {
    152         	        fatalp("setsockopt ADD_MEMBER: %s\n", strerror(errno));
    153 			goto chs_error;
    154         	}
    155 		joined_groups++;
    156 		if (joined_groups == IP_MAX_MEMBERSHIPS) {
    157 			warnp("Maximum group memberships reached for INET socket\n");
    158 			break;
    159 		}
    160 	}
    161 	/* TTL:1 for IPv4 */
    162 	if (setsockopt(s, IPPROTO_IP, IP_MULTICAST_TTL, &(int){1},
    163 	    sizeof(int)) == -1) {
    164 		fatalp("set mcast ttl: %s\n", strerror(errno));
    165 		goto chs_error;
    166 	}
    167 	/* TOS :0xc0 for IPv4 */
    168 	if (set_tos(s) == -1) {
    169 		fatalp("set_tos: %s", strerror(errno));
    170 		goto chs_error;
    171 	}
    172 	/* we need to get the input interface for message processing */
    173 	if (setsockopt(s, IPPROTO_IP, IP_RECVIF, &(uint32_t){1},
    174 	    sizeof(uint32_t)) == -1) {
    175 		fatalp("Cannot set IP_RECVIF\n");
    176 		goto chs_error;
    177 	}
    178 
    179 	hs = (struct hello_socket *)malloc(sizeof(*hs));
    180 	if (hs == NULL) {
    181 		fatalp("Cannot alloc hello_socket structure\n");
    182 		goto chs_error;
    183 	}
    184 	hs->type = AF_INET;
    185 	hs->socket = s;
    186 	SLIST_INSERT_HEAD(&hello_socket_head, hs, listentry);
    187 
    188 #ifdef INET6
    189 	/*
    190 	 * Now we do the same for IPv6
    191 	 */
    192 	s = socket(PF_INET6, SOCK_DGRAM, IPPROTO_UDP);
    193 	if (s < 0) {
    194 		fatalp("Cannot create INET6 socket\n");
    195 		return -1;
    196 	}
    197 	debugp("INET6 socket created (%d)\n", s);
    198 
    199 	if (socket_reuse_port(s) < 0)
    200 		goto chs_error;
    201 
    202 	if (bind_socket(s, AF_INET6) == -1) {
    203 		fatalp("Cannot bind INET6 hello socket\n");
    204 		goto chs_error;
    205 	}
    206 
    207 	if (setsockopt(s, IPPROTO_IPV6, IPV6_MULTICAST_LOOP,
    208 	    &(uint){0}, sizeof(uint)) == -1) {
    209 		fatalp("INET6 setsocketopt IP_MCAST_LOOP: %s\n",
    210 		    strerror(errno));
    211 		goto chs_error;
    212 	}
    213 
    214 	lastifindex = UINT_MAX;
    215 	mcast_addr6.ipv6mr_multiaddr = in6addr_linklocal_allrouters;
    216 	for (ifb = ifa; ifb; ifb = ifb->ifa_next) {
    217 		if_sa6 = (struct sockaddr_in6 *) ifb->ifa_addr;
    218 		if (if_sa6->sin6_family != AF_INET6 ||
    219 		    (!(ifb->ifa_flags & IFF_UP)) ||
    220 		    (!(ifb->ifa_flags & IFF_MULTICAST)) ||
    221 		    (ifb->ifa_flags & IFF_LOOPBACK) ||
    222 		    is_passive_if(ifb->ifa_name) ||
    223 		    IN6_IS_ADDR_LOOPBACK(&if_sa6->sin6_addr))
    224 			continue;
    225 		/*
    226 		 * draft-ietf-mpls-ldp-ipv6-07 Section 5.1:
    227 		 * Additionally, the link-local
    228 		 * IPv6 address MUST be used as the source IP address in IPv6
    229 		 * LDP Link Hellos.
    230 		 */
    231 		if (IN6_IS_ADDR_LINKLOCAL(&if_sa6->sin6_addr) == 0)
    232 			continue;
    233 		/* We should have only one LLADDR per interface, but... */
    234 		if (lastifindex == if_nametoindex(ifb->ifa_name))
    235 			continue;
    236 		mcast_addr6.ipv6mr_interface = lastifindex =
    237 		    if_nametoindex(ifb->ifa_name);
    238 
    239 		debugp("Join IPv6 mcast on %s\n", ifb->ifa_name);
    240 		if (setsockopt(s, IPPROTO_IPV6, IPV6_JOIN_GROUP,
    241 		    (char *)&mcast_addr6, sizeof(mcast_addr6)) == -1) {
    242 			fatalp("INET6 setsockopt JOIN: %s\n", strerror(errno));
    243 			goto chs_error;
    244 		}
    245 	}
    246 	freeifaddrs(ifa);
    247 
    248 	/* TTL: 255 for IPv6 - draft-ietf-mpls-ldp-ipv6-07 Section 9 */
    249 	if (setsockopt(s, IPPROTO_IPV6, IPV6_MULTICAST_HOPS,
    250 	    &(int){255}, sizeof(int)) == -1) {
    251 		fatalp("set mcast hops: %s\n", strerror(errno));
    252 		goto chs_error;
    253 	}
    254 	if (setsockopt(s, IPPROTO_IPV6, IPV6_RECVPKTINFO,
    255 	    &(uint32_t){1}, sizeof(uint32_t)) == -1)
    256 		goto chs_error;
    257 
    258 	hs = (struct hello_socket *)malloc(sizeof(*hs));
    259 	if (hs == NULL) {
    260 		fatalp("Memory alloc problem: hs\n");
    261 		goto chs_error;
    262 	}
    263 
    264 	hs->type = AF_INET6;
    265 	hs->socket = s;
    266 	SLIST_INSERT_HEAD(&hello_socket_head, hs, listentry);
    267 #endif
    268 	return 0;
    269 chs_error:
    270 	close(s);
    271 	return -1;
    272 }
    273 
    274 /* Check if parameter is a hello socket */
    275 int
    276 is_hello_socket(int s)
    277 {
    278 	struct hello_socket *hs;
    279 
    280 	SLIST_FOREACH(hs, &hello_socket_head, listentry)
    281 		if (hs->socket == s)
    282 			return 1;
    283 	return 0;
    284 }
    285 
    286 /* Check if interface is passive */
    287 static int
    288 is_passive_if(const char *if_name)
    289 {
    290 	struct passive_if *pif;
    291 
    292 	SLIST_FOREACH(pif, &passifs_head, listentry)
    293 		if (strncasecmp(if_name, pif->if_name, IF_NAMESIZE) == 0)
    294 			return 1;
    295 	return 0;
    296 }
    297 
    298 /* Sets the TTL to 1 as we don't want to transmit outside this subnet */
    299 int
    300 set_ttl(int s)
    301 {
    302 	int             ret;
    303 	if ((ret = setsockopt(s, IPPROTO_IP, IP_TTL, &(int){1}, sizeof(int)))
    304 	    == -1)
    305 		fatalp("set_ttl: %s", strerror(errno));
    306 	return ret;
    307 }
    308 
    309 /* Sets TOS to 0xc0 aka IP Precedence 6 */
    310 static int
    311 set_tos(int s)
    312 {
    313 	int             ret;
    314 	if ((ret = setsockopt(s, IPPROTO_IP, IP_TOS, &(int){0xc0},
    315 	    sizeof(int))) == -1)
    316 		fatalp("set_tos: %s", strerror(errno));
    317 	return ret;
    318 }
    319 
    320 static int
    321 socket_reuse_port(int s)
    322 {
    323 	int ret;
    324 	if ((ret = setsockopt(s, SOL_SOCKET, SO_REUSEPORT, &(int){1},
    325 	    sizeof(int))) == -1)
    326 		fatalp("socket_reuse_port: %s", strerror(errno));
    327 	return ret;
    328 }
    329 
    330 /* binds an UDP socket */
    331 static int
    332 bind_socket(int s, int stype)
    333 {
    334 	union sockunion su;
    335 
    336 	assert (stype == AF_INET || stype == AF_INET6);
    337 
    338 	if (stype == AF_INET) {
    339 		su.sin.sin_len = sizeof(su.sin);
    340 		su.sin.sin_family = AF_INET;
    341 		su.sin.sin_addr.s_addr = htonl(INADDR_ANY);
    342 		su.sin.sin_port = htons(LDP_PORT);
    343 	}
    344 #ifdef INET6
    345 	else if (stype == AF_INET6) {
    346 		su.sin6.sin6_len = sizeof(su.sin6);
    347 		su.sin6.sin6_family = AF_INET6;
    348 		su.sin6.sin6_addr = in6addr_any;
    349 		su.sin6.sin6_port = htons(LDP_PORT);
    350 	}
    351 #endif
    352 	if (bind(s, &su.sa, su.sa.sa_len)) {
    353 		fatalp("bind_socket: %s\n", strerror(errno));
    354 		return -1;
    355 	}
    356 	return 0;
    357 }
    358 
    359 /* Create / bind the TCP socket */
    360 int
    361 create_listening_socket(void)
    362 {
    363 	struct sockaddr_in sa;
    364 	int             s;
    365 
    366 	sa.sin_len = sizeof(sa);
    367 	sa.sin_family = AF_INET;
    368 	sa.sin_port = htons(LDP_PORT);
    369 	sa.sin_addr.s_addr = htonl(INADDR_ANY);
    370 
    371 	s = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
    372 	if (s < 0)
    373 		return s;
    374 	if (bind(s, (struct sockaddr *) & sa, sizeof(sa))) {
    375 		fatalp("bind: %s", strerror(errno));
    376 		close(s);
    377 		return -1;
    378 	}
    379 	if (listen(s, 10) == -1) {
    380 		fatalp("listen: %s", strerror(errno));
    381 		close(s);
    382 		return -1;
    383 	}
    384 /*	if (set_tos(s) == -1) {
    385 		fatalp("set_tos: %s", strerror(errno));
    386 		close(s);
    387 		return -1;
    388 	}
    389 */	return s;
    390 }
    391 
    392 /*
    393  * It's ugly. We need a function to pass all tlvs and create pdu but since I
    394  * use UDP socket only to send hellos, I didn't bother
    395  */
    396 void
    397 send_hello(void)
    398 {
    399 	struct hello_tlv *t;
    400 	struct common_hello_tlv *cht;
    401 	struct ldp_pdu  *spdu;
    402 	struct transport_address_tlv *trtlv;
    403 	void *v;
    404 	struct sockaddr_in sadest;	/* Destination ALL_ROUTERS */
    405 	ssize_t sb = 0;			/* sent bytes */
    406 	struct ifaddrs *ifa, *ifb;
    407 	struct sockaddr_in *if_sa;
    408 	int ip4socket = -1;
    409 	uint lastifindex;
    410 	struct hello_socket *hs;
    411 #ifdef INET6
    412 	struct sockaddr_in6 sadest6;
    413 	int ip6socket = -1;
    414 #endif
    415 
    416 #define BASIC_HELLO_MSG_SIZE (sizeof(struct ldp_pdu) + 	/* PDU */	\
    417 			TLV_TYPE_LENGTH + MSGID_SIZE +	/* Hello TLV */	\
    418 			/* Common Hello TLV */				\
    419 			sizeof(struct common_hello_tlv))
    420 #define GENERAL_HELLO_MSG_SIZE BASIC_HELLO_MSG_SIZE + 			\
    421 			/* Transport Address */				\
    422 			sizeof(struct transport_address_tlv)
    423 #define IPV4_HELLO_MSG_SIZE BASIC_HELLO_MSG_SIZE + 4 + sizeof(struct in_addr)
    424 #define IPV6_HELLO_MSG_SIZE BASIC_HELLO_MSG_SIZE + 4 + sizeof(struct in6_addr)
    425 
    426 	if ((v = calloc(1, GENERAL_HELLO_MSG_SIZE)) == NULL) {
    427 		fatalp("alloc problem in send_hello()\n");
    428 		return;
    429 	}
    430 
    431 	spdu = (struct ldp_pdu *)((char *)v);
    432 	t = (struct hello_tlv *)(spdu + 1);
    433 	cht = &t->ch;	/* Hello tlv struct includes CHT */
    434 	trtlv = (struct transport_address_tlv *)(t + 1);
    435 
    436 	/* Prepare PDU envelope */
    437 	spdu->version = htons(LDP_VERSION);
    438 	spdu->length = htons(IPV4_HELLO_MSG_SIZE - PDU_VER_LENGTH);
    439 	inet_aton(LDP_ID, &spdu->ldp_id);
    440 
    441 	/* Prepare Hello TLV */
    442 	t->type = htons(LDP_HELLO);
    443 	t->length = htons(MSGID_SIZE +
    444 			sizeof(struct common_hello_tlv) +
    445 			IPV4_HELLO_MSG_SIZE - BASIC_HELLO_MSG_SIZE);
    446 	/*
    447 	 * We used ID 0 instead of htonl(get_message_id()) because we've
    448 	 * seen hellos from Cisco routers doing the same thing
    449 	 */
    450 	t->messageid = 0;
    451 
    452 	/* Prepare Common Hello attributes */
    453 	cht->type = htons(TLV_COMMON_HELLO);
    454 	cht->length = htons(sizeof(cht->holdtime) + sizeof(cht->res));
    455 	cht->holdtime = htons(ldp_holddown_time);
    456 	cht->res = 0;
    457 
    458 	/*
    459 	 * Prepare Transport Address TLV RFC5036 says: "If this optional TLV
    460 	 * is not present the IPv4 source address for the UDP packet carrying
    461 	 * the Hello should be used." But we send it because everybody seems
    462 	 * to do so
    463 	 */
    464 	trtlv->type = htons(TLV_IPV4_TRANSPORT);
    465 	trtlv->length = htons(sizeof(struct in_addr));
    466 	/* trtlv->address will be set for each socket */
    467 
    468 	/* Destination sockaddr */
    469 	memset(&sadest, 0, sizeof(sadest));
    470 	sadest.sin_len = sizeof(sadest);
    471 	sadest.sin_family = AF_INET;
    472 	sadest.sin_port = htons(LDP_PORT);
    473 	sadest.sin_addr.s_addr = htonl(INADDR_ALLRTRS_GROUP);
    474 
    475 	/* Find our socket */
    476 	SLIST_FOREACH(hs, &hello_socket_head, listentry)
    477 		if (hs->type == AF_INET) {
    478 			ip4socket = hs->socket;
    479 			break;
    480 		}
    481 	assert(ip4socket >= 0);
    482 
    483 	if (getifaddrs(&ifa) == -1) {
    484 		free(v);
    485 		fatalp("Cannot enumerate interfaces\n");
    486 		return;
    487 	}
    488 
    489 	lastifindex = UINT_MAX;
    490 	/* Loop all interfaces in order to send IPv4 hellos */
    491 	for (ifb = ifa; ifb; ifb = ifb->ifa_next) {
    492 		if_sa = (struct sockaddr_in *) ifb->ifa_addr;
    493 		if (if_sa->sin_family != AF_INET ||
    494 		    (!(ifb->ifa_flags & IFF_UP)) ||
    495 		    (ifb->ifa_flags & IFF_LOOPBACK) ||
    496 		    (!(ifb->ifa_flags & IFF_MULTICAST)) ||
    497 		    is_passive_if(ifb->ifa_name) ||
    498 		    (ntohl(if_sa->sin_addr.s_addr) >> 24 == IN_LOOPBACKNET) ||
    499 		    lastifindex == if_nametoindex(ifb->ifa_name))
    500 			continue;
    501 
    502 		/* Send only once per interface, using primary address */
    503 		if (lastifindex == if_nametoindex(ifb->ifa_name))
    504 			continue;
    505 		lastifindex = if_nametoindex(ifb->ifa_name);
    506 
    507 		if (setsockopt(ip4socket, IPPROTO_IP, IP_MULTICAST_IF,
    508 		    &if_sa->sin_addr, sizeof(struct in_addr)) == -1) {
    509 			warnp("setsockopt failed: %s\n", strerror(errno));
    510 			continue;
    511 		}
    512 		trtlv->address.ip4addr.s_addr = if_sa->sin_addr.s_addr;
    513 
    514 		/* Put it on the wire */
    515 		sb = sendto(ip4socket, v, IPV4_HELLO_MSG_SIZE, 0,
    516 			    (struct sockaddr *) & sadest, sizeof(sadest));
    517 		if (sb < (ssize_t)(IPV4_HELLO_MSG_SIZE))
    518 		    fatalp("send: %s", strerror(errno));
    519 		else
    520 		    debugp("Sent (IPv4) %zd bytes on %s"
    521 			" (PDU: %d, Hello TLV: %d, CH: %d, TR: %d)\n",
    522 			sb, ifb->ifa_name,
    523 			ntohs(spdu->length), ntohs(t->length),
    524 			ntohs(cht->length), ntohs(trtlv->length));
    525 	}
    526 #ifdef INET6
    527 	/* Adjust lengths */
    528 	spdu->length = htons(IPV6_HELLO_MSG_SIZE - PDU_VER_LENGTH);
    529 	t->length = htons(MSGID_SIZE +
    530 			sizeof(struct common_hello_tlv) +
    531 			IPV6_HELLO_MSG_SIZE - BASIC_HELLO_MSG_SIZE);
    532 	trtlv->length = htons(sizeof(struct in6_addr));
    533 	trtlv->type = htons(TLV_IPV6_TRANSPORT);
    534 
    535 	/* Prepare destination sockaddr */
    536 	memset(&sadest6, 0, sizeof(sadest6));
    537 	sadest6.sin6_len = sizeof(sadest6);
    538 	sadest6.sin6_family = AF_INET6;
    539 	sadest6.sin6_port = htons(LDP_PORT);
    540 	sadest6.sin6_addr = in6addr_linklocal_allrouters;
    541 
    542 	SLIST_FOREACH(hs, &hello_socket_head, listentry)
    543 		if (hs->type == AF_INET6) {
    544 			ip6socket = hs->socket;
    545 			break;
    546 		}
    547 
    548 	lastifindex = UINT_MAX;
    549 	for (ifb = ifa; ifb; ifb = ifb->ifa_next) {
    550 		struct sockaddr_in6 * if_sa6 =
    551 		    (struct sockaddr_in6 *) ifb->ifa_addr;
    552 		if (if_sa6->sin6_family != AF_INET6 ||
    553 		    (!(ifb->ifa_flags & IFF_UP)) ||
    554 		    (!(ifb->ifa_flags & IFF_MULTICAST)) ||
    555 		    (ifb->ifa_flags & IFF_LOOPBACK) ||
    556 		    is_passive_if(ifb->ifa_name) ||
    557 		    IN6_IS_ADDR_LOOPBACK(&if_sa6->sin6_addr))
    558 			continue;
    559 		/*
    560 		 * draft-ietf-mpls-ldp-ipv6-07 Section 5.1:
    561 		 * Additionally, the link-local
    562 		 * IPv6 address MUST be used as the source IP address in IPv6
    563 		 * LDP Link Hellos.
    564 		 */
    565 		if (IN6_IS_ADDR_LINKLOCAL(&if_sa6->sin6_addr) == 0)
    566 			continue;
    567 		/* We should have only one LLADDR per interface, but... */
    568 		if (lastifindex == if_nametoindex(ifb->ifa_name))
    569 			continue;
    570 		lastifindex = if_nametoindex(ifb->ifa_name);
    571 
    572 		if (setsockopt(ip6socket, IPPROTO_IPV6, IPV6_MULTICAST_IF,
    573 		    &lastifindex, sizeof(int)) == -1) {
    574 			fatalp("ssopt6 IPV6_MULTICAST_IF failed: %s for %s\n",
    575 			    strerror(errno), ifb->ifa_name);
    576 			continue;
    577 		}
    578 
    579 		memcpy(&trtlv->address.ip6addr, &if_sa6->sin6_addr,
    580 		    sizeof(struct in6_addr));
    581 
    582 		/* Put it on the wire */
    583 		sb = sendto(ip6socket, v, IPV6_HELLO_MSG_SIZE,
    584 			    0, (struct sockaddr *)&sadest6, sizeof(sadest6));
    585 		if (sb < (ssize_t)(IPV6_HELLO_MSG_SIZE))
    586 		    fatalp("send6: %s", strerror(errno));
    587 		else
    588 		    debugp("Sent (IPv6) %zd bytes on %s "
    589 			"(PDU: %d, Hello TLV: %d, CH: %d TR: %d)\n",
    590 			sb, ifb->ifa_name, htons(spdu->length),
    591 			htons(t->length), htons(cht->length),
    592 			htons(trtlv->length));
    593 	}
    594 #endif
    595 	freeifaddrs(ifa);
    596 	free(v);
    597 }
    598 
    599 int
    600 get_message_id(void)
    601 {
    602 	current_msg_id++;
    603 	return current_msg_id;
    604 }
    605 
    606 static int
    607 get_local_addr(struct sockaddr_dl *sdl, struct in_addr *sin)
    608 {
    609 	struct ifaddrs *ifa, *ifb;
    610 	struct sockaddr_in *sinet;
    611 
    612 	if (sdl == NULL)
    613 		return -1;
    614 
    615 	if (getifaddrs(&ifa) == -1)
    616 		return -1;
    617 	for (ifb = ifa; ifb; ifb = ifb->ifa_next)
    618 		if (ifb->ifa_addr->sa_family == AF_INET) {
    619 			if (if_nametoindex(ifb->ifa_name) != sdl->sdl_index)
    620 				continue;
    621 			sinet = (struct sockaddr_in*) ifb->ifa_addr;
    622 			sin->s_addr = sinet->sin_addr.s_addr;
    623 			freeifaddrs(ifa);
    624 			return 0;
    625 		}
    626 	freeifaddrs(ifa);
    627 	return -1;
    628 }
    629 
    630 /* Receive PDUs on Multicast UDP socket */
    631 void
    632 recv_pdu(int sock)
    633 {
    634 	struct ldp_pdu  rpdu;
    635 	int             c, i;
    636 	struct msghdr msg;
    637 	struct iovec iov[1];
    638 	unsigned char recvspace[MAX_PDU_SIZE];
    639 	struct hello_tlv *t;
    640 	union sockunion sender;
    641 	struct sockaddr_dl *sdl = NULL;
    642 	struct in_addr my_ldp_addr, local_addr;
    643 	struct cmsghdr *cmptr;
    644 	union {
    645 		struct cmsghdr cm;
    646 		char control[1024];
    647 	} control_un;
    648 
    649 	memset(&msg, 0, sizeof(msg));
    650 	msg.msg_control = control_un.control;
    651 	msg.msg_controllen = sizeof(control_un.control);
    652 	msg.msg_flags = 0;
    653 	msg.msg_name = &sender;
    654 	msg.msg_namelen = sizeof(sender);
    655 	iov[0].iov_base = recvspace;
    656 	iov[0].iov_len = sizeof(recvspace);
    657 	msg.msg_iov = iov;
    658 	msg.msg_iovlen = 1;
    659 
    660 	c = recvmsg(sock, &msg, MSG_WAITALL);
    661 
    662 	/* Check to see if this is larger than MIN_PDU_SIZE */
    663 	if (c < MIN_PDU_SIZE)
    664 		return;
    665 
    666 	/* Read the PDU */
    667 	i = get_pdu(recvspace, &rpdu);
    668 
    669 	debugp("recv_pdu(%d): PDU(size: %d) from: %s\n", sock,
    670 	    c, satos(&sender.sa));
    671 
    672 	/* We currently understand Version 1 */
    673 	if (rpdu.version != LDP_VERSION) {
    674 		warnp("recv_pdu: Version mismatch\n");
    675 		return;
    676 	}
    677 
    678 	/* Check if it's our hello */
    679 	inet_aton(LDP_ID, &my_ldp_addr);
    680 	if (rpdu.ldp_id.s_addr == my_ldp_addr.s_addr) {
    681 		/* It should not be looped. We set MULTICAST_LOOP 0 */
    682 		fatalp("Received our PDU. Ignoring it\n");
    683 		return;
    684 	}
    685 
    686 	if (msg.msg_controllen < (socklen_t)sizeof(struct cmsghdr) ||
    687 	    (msg.msg_flags & MSG_CTRUNC))
    688 		local_addr.s_addr = my_ldp_addr.s_addr;
    689 	else {
    690 		for (cmptr = CMSG_FIRSTHDR(&msg); cmptr != NULL;
    691 		    cmptr = CMSG_NXTHDR(&msg, cmptr))
    692 			if (cmptr->cmsg_level == IPPROTO_IP &&
    693 			    cmptr->cmsg_type == IP_RECVIF) {
    694 				sdl = (struct sockaddr_dl *) CMSG_DATA(cmptr);
    695 				break;
    696 			}
    697 		if (get_local_addr(sdl, &local_addr) != 0)
    698 			local_addr.s_addr = my_ldp_addr.s_addr;
    699 	}
    700 
    701 
    702 	debugp("Read %d bytes from address %s Length: %.4d Version: %d\n",
    703 	       c, inet_ntoa(rpdu.ldp_id), rpdu.length, rpdu.version);
    704 
    705 	/* Fill the TLV messages */
    706 	t = get_hello_tlv(recvspace + i, c - i);
    707 	run_ldp_hello(&rpdu, t, &sender.sa, &local_addr, sock, may_connect);
    708 }
    709 
    710 void
    711 send_hello_alarm(int unused)
    712 {
    713 	struct ldp_peer *p, *ptmp;
    714 	struct hello_info *hi, *hinext;
    715 	time_t          t = time(NULL);
    716 	int             olderrno = errno;
    717 
    718 	if (may_connect == false)
    719 		may_connect = true;
    720 	/* Send hellos */
    721 	if (!(t % ldp_hello_time))
    722 		send_hello();
    723 
    724 	/* Timeout -- */
    725 	SLIST_FOREACH(p, &ldp_peer_head, peers)
    726 		p->timeout--;
    727 
    728 	/* Check for timeout */
    729 	SLIST_FOREACH_SAFE(p, &ldp_peer_head, peers, ptmp)
    730 		if (p->timeout < 1)
    731 			switch (p->state) {
    732 			case LDP_PEER_HOLDDOWN:
    733 				debugp("LDP holddown expired for peer %s\n",
    734 				       inet_ntoa(p->ldp_id));
    735 				ldp_peer_delete(p);
    736 				break;
    737 			case LDP_PEER_ESTABLISHED:
    738 			case LDP_PEER_CONNECTED:
    739 				send_notification(p, 0,
    740 				    NOTIF_FATAL|NOTIF_KEEP_ALIVE_TIMER_EXPIRED);
    741 				warnp("Keepalive expired for %s\n",
    742 				    inet_ntoa(p->ldp_id));
    743 				ldp_peer_holddown(p);
    744 				break;
    745 			}	/* switch */
    746 
    747 	/* send keepalives */
    748 	if (!(t % ldp_keepalive_time)) {
    749 		SLIST_FOREACH(p, &ldp_peer_head, peers)
    750 		    if (p->state == LDP_PEER_ESTABLISHED) {
    751 			debugp("Sending KeepAlive to %s\n",
    752 			    inet_ntoa(p->ldp_id));
    753 			keep_alive(p);
    754 		    }
    755 	}
    756 
    757 	/* Decrement and Check hello keepalives */
    758 	SLIST_FOREACH_SAFE(hi, &hello_info_head, infos, hinext) {
    759 		if (hi->keepalive != 0xFFFF)
    760 			hi->keepalive--;
    761 		if (hi->keepalive < 1)
    762 			SLIST_REMOVE(&hello_info_head, hi, hello_info, infos);
    763 	}
    764 
    765 	/* Set the alarm again and bail out */
    766 	alarm(1);
    767 	errno = olderrno;
    768 }
    769 
    770 static void
    771 bail_out(int x)
    772 {
    773 	ldp_peer_holddown_all();
    774 	flush_mpls_routes();
    775 	exit(0);
    776 }
    777 
    778 static void
    779 print_info(int x)
    780 {
    781 	printf("Info for %s\n-------\n", LDP_ID);
    782 	printf("Neighbours:\n");
    783 	show_neighbours(1, NULL);
    784 	printf("Bindings:\n");
    785 	show_bindings(1, NULL);
    786 	printf("Labels:\n");
    787 	show_labels(1, NULL);
    788 	printf("--------\n");
    789 }
    790 
    791 /*
    792  * The big poll that catches every single event
    793  * on every socket.
    794  */
    795 int
    796 the_big_loop(void)
    797 {
    798 	int		sock_error;
    799 	uint32_t	i;
    800 	socklen_t       sock_error_size = sizeof(int);
    801 	struct ldp_peer *p;
    802 	struct com_sock	*cs;
    803 	struct pollfd	pfd[MAX_POLL_FDS];
    804 	struct hello_socket *hs;
    805 	nfds_t pollsum;
    806 
    807 	assert(MAX_POLL_FDS > 5);
    808 
    809 	SLIST_INIT(&hello_info_head);
    810 
    811 	signal(SIGALRM, send_hello_alarm);
    812 	signal(SIGPIPE, SIG_IGN);
    813 	signal(SIGINT, bail_out);
    814 	signal(SIGTERM, bail_out);
    815 	signal(SIGINFO, print_info);
    816 
    817 	/* Send first hellos in 5 seconds. Avoid No hello notifications */
    818 	may_connect = false;
    819 	alarm(5);
    820 
    821 	route_socket = socket(PF_ROUTE, SOCK_RAW, AF_UNSPEC);
    822 	setsockopt(route_socket, SOL_SOCKET, SO_USELOOPBACK, &(int){0},
    823 		sizeof(int));
    824 
    825 	sock_error = bind_current_routes();
    826 	if (sock_error != LDP_E_OK) {
    827 		fatalp("Cannot get current routes\n");
    828 		return sock_error;
    829 	}
    830 
    831 	for (;;) {
    832 		pfd[0].fd = ls;
    833 		pfd[0].events = POLLRDNORM;
    834 		pfd[0].revents = 0;
    835 
    836 		pfd[1].fd = route_socket;
    837 		pfd[1].events = POLLRDNORM;
    838 		pfd[1].revents = 0;
    839 
    840 		pfd[2].fd = command_socket;
    841 		pfd[2].events = POLLRDNORM;
    842 		pfd[2].revents = 0;
    843 
    844 		/* Hello sockets */
    845 		pollsum = 3;
    846 		SLIST_FOREACH(hs, &hello_socket_head, listentry) {
    847 			pfd[pollsum].fd = hs->socket;
    848 			pfd[pollsum].events = POLLIN;
    849 			pfd[pollsum].revents = 0;
    850 			pollsum++;
    851 		}
    852 
    853 		/* Command sockets */
    854 		for (i=0; i < MAX_COMMAND_SOCKETS; i++)
    855 			if (csockets[i].socket != -1) {
    856 				if (pollsum >= MAX_POLL_FDS)
    857 					break;
    858 				pfd[pollsum].fd = csockets[i].socket;
    859 				pfd[pollsum].events = POLLIN;
    860 				pfd[pollsum].revents = 0;
    861 				pollsum++;
    862 			}
    863 
    864 		/* LDP Peer sockets */
    865 		SLIST_FOREACH(p, &ldp_peer_head, peers) {
    866 			if (p->socket < 1)
    867 				continue;
    868 			switch (p->state) {
    869 			    case LDP_PEER_CONNECTED:
    870 			    case LDP_PEER_ESTABLISHED:
    871 				if (pollsum >= MAX_POLL_FDS)
    872 					break;
    873 				pfd[pollsum].fd = p->socket;
    874 				pfd[pollsum].events = POLLRDNORM;
    875 				pfd[pollsum].revents = 0;
    876 				pollsum++;
    877 				break;
    878 			    case LDP_PEER_CONNECTING:
    879 				if (pollsum >= MAX_POLL_FDS)
    880 					break;
    881 				pfd[pollsum].fd = p->socket;
    882 				pfd[pollsum].events = POLLWRNORM;
    883 				pfd[pollsum].revents = 0;
    884 				pollsum++;
    885 				break;
    886 			}
    887 		}
    888 
    889 		if (pollsum >= MAX_POLL_FDS) {
    890 			fatalp("Too many sockets. Increase MAX_POLL_FDS\n");
    891 			return LDP_E_TOO_MANY_FDS;
    892 		}
    893 		if (poll(pfd, pollsum, INFTIM) < 0) {
    894 			if (errno != EINTR)
    895 				fatalp("poll: %s", strerror(errno));
    896 			continue;
    897 		}
    898 
    899 		for (i = 0; i < pollsum; i++) {
    900 			if ((pfd[i].revents & POLLRDNORM) ||
    901 			    (pfd[i].revents & POLLIN)) {
    902 				if(pfd[i].fd == ls)
    903 					new_peer_connection();
    904 				else if (pfd[i].fd == route_socket) {
    905 					struct rt_msg xbuf;
    906 					int l, rtmlen = sizeof(xbuf);
    907 					/* Read at least rtm_msglen */
    908 					l = recv(route_socket, &xbuf,
    909 					  sizeof(u_short), MSG_PEEK);
    910 					if (l == sizeof(u_short))
    911 						rtmlen = xbuf.m_rtm.rtm_msglen;
    912 					do {
    913 						l = recv(route_socket, &xbuf,
    914 						    rtmlen, MSG_WAITALL);
    915 					} while ((l == -1) && (errno == EINTR));
    916 
    917 					if (l == -1)
    918 						break;
    919 
    920 					check_route(&xbuf, l);
    921 
    922 				} else if (is_hello_socket(pfd[i].fd) == 1) {
    923 					/* Receiving hello socket */
    924 					recv_pdu(pfd[i].fd);
    925 				} else if (pfd[i].fd == command_socket) {
    926 					command_accept(command_socket);
    927 				} else if ((cs = is_command_socket(pfd[i].fd))
    928 						!= NULL) {
    929 					command_dispatch(cs);
    930 				} else {
    931 					/* ldp peer socket */
    932 					p = get_ldp_peer_by_socket(pfd[i].fd);
    933 					if (p)
    934 						recv_session_pdu(p);
    935 				}
    936 			} else if(pfd[i].revents & POLLWRNORM) {
    937 				p = get_ldp_peer_by_socket(pfd[i].fd);
    938 				if (!p)
    939 					continue;
    940 				assert(p->state == LDP_PEER_CONNECTING);
    941 				if (getsockopt(pfd[i].fd, SOL_SOCKET, SO_ERROR,
    942 				    &sock_error, &sock_error_size) != 0 ||
    943 				    sock_error != 0) {
    944 					ldp_peer_holddown(p);
    945 					sock_error = 0;
    946 				} else {
    947 					p->state = LDP_PEER_CONNECTED;
    948 					send_initialize(p);
    949 				}
    950 			}
    951 		}
    952 
    953 		for (int ri = 0; ri < replay_index; ri++) {
    954 			debugp("Replaying: PID %d, SEQ %d\n",
    955 				replay_rt[ri].m_rtm.rtm_pid,
    956 				replay_rt[ri].m_rtm.rtm_seq);
    957 			check_route(&replay_rt[ri], sizeof(struct rt_msg));
    958                 }
    959 		replay_index = 0;
    960 	}	/* for (;;) */
    961 }
    962 
    963 void
    964 new_peer_connection()
    965 {
    966 	union sockunion peer_address, my_address;
    967 	struct in_addr *peer_ldp_id = NULL;
    968 	struct hello_info *hi;
    969 	int s;
    970 
    971 	s = accept(ls, &peer_address.sa,
    972 		& (socklen_t) { sizeof(union sockunion) } );
    973 	if (s < 0) {
    974 		fatalp("accept: %s", strerror(errno));
    975 		return;
    976 	}
    977 
    978 	if (getsockname(s, &my_address.sa,
    979 	    & (socklen_t) { sizeof(union sockunion) } )) {
    980 		fatalp("new_peer_connection(): cannot getsockname\n");
    981 		close(s);
    982 		return;
    983 	}
    984 
    985 	if (peer_address.sa.sa_family == AF_INET)
    986 		peer_address.sin.sin_port = 0;
    987 	else if (peer_address.sa.sa_family == AF_INET6)
    988 		peer_address.sin6.sin6_port = 0;
    989 	else {
    990 		fatalp("Unknown peer address family\n");
    991 		close(s);
    992 		return;
    993 	}
    994 
    995 	/* Already peered or in holddown ? */
    996 	if (get_ldp_peer(&peer_address.sa) != NULL) {
    997 		close(s);
    998 		return;
    999 	}
   1000 
   1001 	warnp("Accepted a connection from %s\n", satos(&peer_address.sa));
   1002 
   1003 	/* Verify if it should connect - XXX: no check for INET6 */
   1004 	if (peer_address.sa.sa_family == AF_INET &&
   1005 	    ntohl(peer_address.sin.sin_addr.s_addr) <
   1006 	    ntohl(my_address.sin.sin_addr.s_addr)) {
   1007 		fatalp("Peer %s: connect from lower ID\n",
   1008 		    satos(&peer_address.sa));
   1009 		close(s);
   1010 		return;
   1011 	}
   1012 
   1013 	/* Match hello info in order to get ldp_id */
   1014 	SLIST_FOREACH(hi, &hello_info_head, infos) {
   1015 		if (sockaddr_cmp(&peer_address.sa,
   1016 		    &hi->transport_address.sa) == 0) {
   1017 			peer_ldp_id = &hi->ldp_id;
   1018 			break;
   1019 		}
   1020 	}
   1021 	if (peer_ldp_id == NULL) {
   1022 		warnp("Got connection from %s, but no hello info exists\n",
   1023 		    satos(&peer_address.sa));
   1024 		close(s);
   1025 		return;
   1026 	} else
   1027 		ldp_peer_new(peer_ldp_id, &peer_address.sa, NULL,
   1028 		    ldp_holddown_time, s);
   1029 
   1030 }
   1031 
   1032 void
   1033 send_initialize(const struct ldp_peer * p)
   1034 {
   1035 	struct init_tlv ti;
   1036 
   1037 	ti.type = htons(LDP_INITIALIZE);
   1038 	ti.length = htons(sizeof(struct init_tlv) - TLV_TYPE_LENGTH);
   1039 	ti.messageid = htonl(get_message_id());
   1040 	ti.cs_type = htons(TLV_COMMON_SESSION);
   1041 	ti.cs_len = htons(CS_LEN);
   1042 	ti.cs_version = htons(LDP_VERSION);
   1043 	ti.cs_keepalive = htons(2 * ldp_keepalive_time);
   1044 	ti.cs_adpvlim = 0;
   1045 	ti.cs_maxpdulen = htons(MAX_PDU_SIZE);
   1046 	ti.cs_peeraddress.s_addr = p->ldp_id.s_addr;
   1047 	ti.cs_peeraddrspace = 0;
   1048 
   1049 	send_tlv(p, (struct tlv *) (void *) &ti);
   1050 }
   1051 
   1052 void
   1053 keep_alive(const struct ldp_peer * p)
   1054 {
   1055 	struct ka_tlv   kt;
   1056 
   1057 	kt.type = htons(LDP_KEEPALIVE);
   1058 	kt.length = htons(sizeof(kt.messageid));
   1059 	kt.messageid = htonl(get_message_id());
   1060 
   1061 	send_tlv(p, (struct tlv *) (void *) &kt);
   1062 }
   1063 
   1064 /*
   1065  * Process a message received from a peer
   1066  */
   1067 void
   1068 recv_session_pdu(struct ldp_peer * p)
   1069 {
   1070 	struct ldp_pdu *rpdu;
   1071 	struct address_tlv *atlv;
   1072 	struct al_tlv  *altlv;
   1073 	struct init_tlv	*itlv;
   1074 	struct label_map_tlv *lmtlv;
   1075 	struct fec_tlv *fectlv;
   1076 	struct label_tlv *labeltlv;
   1077 	struct notification_tlv *nottlv;
   1078 	struct hello_info *hi;
   1079 
   1080 	int             c;
   1081 	int32_t         wo = 0;
   1082 	struct tlv     *ttmp;
   1083 	unsigned char   recvspace[MAX_PDU_SIZE];
   1084 
   1085 	memset(recvspace, 0, MAX_PDU_SIZE);
   1086 
   1087 	do {
   1088 		c = recv(p->socket, (void *) recvspace, MAX_PDU_SIZE, MSG_PEEK);
   1089 	} while (c == -1 && errno == EINTR);
   1090 
   1091 	debugp("Ready to read %d bytes\n", c);
   1092 
   1093 	if (c < 1) {		/* Session closed */
   1094 		warnp("Error in connection with %s\n", inet_ntoa(p->ldp_id));
   1095 		ldp_peer_holddown(p);
   1096 		return;
   1097 	}
   1098 	if (c > MAX_PDU_SIZE) {
   1099 		debugp("Incoming PDU size exceeds MAX_PDU_SIZE !\n");
   1100 		return;
   1101 	}
   1102 	if (c < MIN_PDU_SIZE) {
   1103 		debugp("PDU too small received from peer %s\n",
   1104 		    inet_ntoa(p->ldp_id));
   1105 		return;
   1106 	}
   1107 	rpdu = (struct ldp_pdu *) recvspace;
   1108 	do {
   1109 		c = recv(p->socket, (void *) recvspace,
   1110 		    ntohs(rpdu->length) + PDU_VER_LENGTH, MSG_WAITALL);
   1111 	} while (c == -1 && errno == EINTR);
   1112 
   1113 	/* sanity check */
   1114 	if (check_recv_pdu(p, rpdu, c) != 0)
   1115 		return;
   1116 
   1117 	debugp("Read %d bytes, PDU size: %d bytes\n", c, ntohs(rpdu->length));
   1118 	wo = sizeof(struct ldp_pdu);
   1119 
   1120 	while (wo + TLV_TYPE_LENGTH < (uint)c) {
   1121 
   1122 		ttmp = (struct tlv *) (&recvspace[wo]);
   1123 
   1124 		if ((ntohs(ttmp->type) != LDP_KEEPALIVE) &&
   1125 		    (ntohs(ttmp->type) != LDP_LABEL_MAPPING)) {
   1126 			debugp("Got Type: 0x%.4X (Length: %d) from %s\n",
   1127 			    ntohs(ttmp->type), ntohs(ttmp->length),
   1128 			    inet_ntoa(p->ldp_id));
   1129 		} else
   1130 			debugp("Got Type: 0x%.4X (Length: %d) from %s\n",
   1131 			    ntohs(ttmp->type), ntohs(ttmp->length),
   1132 			    inet_ntoa(p->ldp_id));
   1133 
   1134 		/* Should we get the message ? */
   1135 		if (p->state != LDP_PEER_ESTABLISHED &&
   1136 		    ntohs(ttmp->type) != LDP_INITIALIZE &&
   1137 		    ntohs(ttmp->type) != LDP_KEEPALIVE &&
   1138 		    ntohs(ttmp->type) != LDP_NOTIFICATION)
   1139 			break;
   1140 		/* The big switch */
   1141 		switch (ntohs(ttmp->type)) {
   1142 		case LDP_INITIALIZE:
   1143 			itlv = (struct init_tlv *)ttmp;
   1144 			/* Check size */
   1145 			if (ntohs(itlv->length) <
   1146 			    sizeof(struct init_tlv) - TLV_TYPE_LENGTH) {
   1147 				debugp("Bad size\n");
   1148 				send_notification(p, 0,
   1149 				    NOTIF_BAD_PDU_LEN | NOTIF_FATAL);
   1150 				ldp_peer_holddown(p);
   1151 				break;
   1152 			}
   1153 			/* Check version */
   1154 			if (ntohs(itlv->cs_version) != LDP_VERSION) {
   1155 				debugp("Bad version");
   1156 				send_notification(p, ntohl(itlv->messageid),
   1157 					NOTIF_BAD_LDP_VER | NOTIF_FATAL);
   1158 				ldp_peer_holddown(p);
   1159 				break;
   1160 			}
   1161 			/* Check if we got any hello from this one */
   1162 			SLIST_FOREACH(hi, &hello_info_head, infos)
   1163 				if (hi->ldp_id.s_addr == rpdu->ldp_id.s_addr)
   1164 					break;
   1165 			if (hi == NULL) {
   1166 			    debugp("No hello. Moving peer to holddown\n");
   1167 			    send_notification(p, ntohl(itlv->messageid),
   1168 				NOTIF_SESSION_REJECTED_NO_HELLO | NOTIF_FATAL);
   1169 			    ldp_peer_holddown(p);
   1170 			    break;
   1171 			}
   1172 
   1173 			if (!p->master) {
   1174 				send_initialize(p);
   1175 				keep_alive(p);
   1176 			} else {
   1177 				p->state = LDP_PEER_ESTABLISHED;
   1178 				p->established_t = time(NULL);
   1179 				keep_alive(p);
   1180 
   1181 				/*
   1182 				 * Recheck here ldp id because we accepted
   1183 				 * connection without knowing who is it for sure
   1184 				 */
   1185 				p->ldp_id.s_addr = rpdu->ldp_id.s_addr;
   1186 
   1187 				fatalp("LDP neighbour %s is UP\n",
   1188 				    inet_ntoa(p->ldp_id));
   1189 				mpls_add_ldp_peer(p);
   1190 				send_addresses(p);
   1191 				send_all_bindings(p);
   1192 			}
   1193 			break;
   1194 		case LDP_KEEPALIVE:
   1195 			if ((p->state == LDP_PEER_CONNECTED) && (!p->master)) {
   1196 				p->state = LDP_PEER_ESTABLISHED;
   1197 				p->established_t = time(NULL);
   1198 				fatalp("LDP neighbour %s is UP\n",
   1199 				    inet_ntoa(p->ldp_id));
   1200 				mpls_add_ldp_peer(p);
   1201 				send_addresses(p);
   1202 				send_all_bindings(p);
   1203 			}
   1204 			p->timeout = p->holdtime;
   1205 			break;
   1206 		case LDP_ADDRESS:
   1207 			/* Add peer addresses */
   1208 			atlv = (struct address_tlv *) ttmp;
   1209 			altlv = (struct al_tlv *) (&atlv[1]);
   1210 			add_ifaddresses(p, altlv);
   1211 			/*
   1212 			 * try to see if we have labels with null peer that
   1213 			 * would match the new peer
   1214 			 */
   1215 			label_check_assoc(p);
   1216 			print_bounded_addresses(p);
   1217 			break;
   1218 		case LDP_ADDRESS_WITHDRAW:
   1219 			atlv = (struct address_tlv *) ttmp;
   1220 			altlv = (struct al_tlv *) (&atlv[1]);
   1221 			del_ifaddresses(p, altlv);
   1222 			break;
   1223 		case LDP_LABEL_MAPPING:
   1224 			lmtlv = (struct label_map_tlv *) ttmp;
   1225 			fectlv = (struct fec_tlv *) (&lmtlv[1]);
   1226 			labeltlv = (struct label_tlv *)((unsigned char *)fectlv
   1227 				+ ntohs(fectlv->length) + TLV_TYPE_LENGTH);
   1228 			map_label(p, fectlv, labeltlv);
   1229 			break;
   1230 		case LDP_LABEL_REQUEST:
   1231 			lmtlv = (struct label_map_tlv *) ttmp;
   1232 			fectlv = (struct fec_tlv *) (&lmtlv[1]);
   1233 			switch (request_respond(p, lmtlv, fectlv)) {
   1234 			case LDP_E_BAD_FEC:
   1235 				send_notification(p, ntohl(lmtlv->messageid),
   1236 					NOTIF_UNKNOWN_TLV);
   1237 				break;
   1238 			case LDP_E_BAD_AF:
   1239 				send_notification(p, ntohl(lmtlv->messageid),
   1240 					NOTIF_UNSUPPORTED_AF);
   1241 				break;
   1242 			case LDP_E_NO_SUCH_ROUTE:
   1243 				send_notification(p, ntohl(lmtlv->messageid),
   1244 					NOTIF_NO_ROUTE);
   1245 				break;
   1246 			}
   1247 			break;
   1248 		case LDP_LABEL_WITHDRAW:
   1249 			lmtlv = (struct label_map_tlv *) ttmp;
   1250 			fectlv = (struct fec_tlv *) (&lmtlv[1]);
   1251 			if (withdraw_label(p, fectlv) == LDP_E_OK) {
   1252 				/* Send RELEASE */
   1253 				prepare_release(ttmp);
   1254 				send_tlv(p, ttmp);
   1255 				}
   1256 			break;
   1257 		case LDP_LABEL_RELEASE:
   1258 			/*
   1259 			 * XXX: we need to make a timed queue...
   1260 			 * For now I just assume peers are processing messages
   1261 			 * correctly so I just ignore confirmations
   1262 			 */
   1263 			wo = -1;	/* Ignore rest of message */
   1264 			break;
   1265 		case LDP_LABEL_ABORT:
   1266 		/* XXX: For now I pretend I can process everything
   1267 		 * RFC 5036, Section 3.5.9.1
   1268 		 * If an LSR receives a Label Abort Request Message after it
   1269 		 * has responded to the Label Request in question with a Label
   1270 		 * Mapping message or a Notification message, it ignores the
   1271 		 * abort request.
   1272 		 */
   1273 			wo = -1;
   1274 			break;
   1275 		case LDP_NOTIFICATION:
   1276 			nottlv = (struct notification_tlv *) ttmp;
   1277 			nottlv->st_code = ntohl(nottlv->st_code);
   1278 			fatalp("Got notification 0x%X from peer %s\n",
   1279 			    nottlv->st_code, inet_ntoa(p->ldp_id));
   1280 			if (nottlv->st_code >> 31) {
   1281 				fatalp("LDP peer %s signalized %s\n",
   1282 				    inet_ntoa(p->ldp_id),
   1283 				    NOTIF_STR[(nottlv->st_code << 1) >> 1]);
   1284 				ldp_peer_holddown(p);
   1285 				wo = -1;
   1286 			}
   1287 			break;
   1288 		case LDP_HELLO:
   1289 			/* No hellos should came on tcp session */
   1290 			wo = -1;
   1291 			break;
   1292 		default:
   1293 			warnp("Unknown TLV received from %s\n",
   1294 			    inet_ntoa(p->ldp_id));
   1295 			debug_tlv(ttmp);
   1296 			wo = -1;/* discard the rest of the message */
   1297 			break;
   1298 		}
   1299 		if (wo < 0) {
   1300 			debugp("Discarding the rest of the message\n");
   1301 			break;
   1302 		} else {
   1303 			wo += ntohs(ttmp->length) + TLV_TYPE_LENGTH;
   1304 			debugp("WORKED ON %u bytes (Left %d)\n", wo, c - wo);
   1305 		}
   1306 	}			/* while */
   1307 
   1308 }
   1309 
   1310 /* Sends a pdu, tlv pair to a connected peer */
   1311 int
   1312 send_message(const struct ldp_peer * p, const struct ldp_pdu * pdu,
   1313 	const struct tlv * t)
   1314 {
   1315 	unsigned char   sendspace[MAX_PDU_SIZE];
   1316 
   1317 	/* Check if peer is connected */
   1318 	switch (p->state) {
   1319 	case LDP_PEER_CONNECTED:
   1320 	case LDP_PEER_ESTABLISHED:
   1321 		break;
   1322 	default:
   1323 		return -1;
   1324 	}
   1325 
   1326 	/* Check length validity first */
   1327 	if (ntohs(pdu->length) !=
   1328 	    ntohs(t->length) + TLV_TYPE_LENGTH + PDU_PAYLOAD_LENGTH) {
   1329 		fatalp("LDP: TLV - PDU incompability. Message discarded\n");
   1330 		fatalp("LDP: TLV len %d - PDU len %d\n", ntohs(t->length),
   1331 		    ntohs(pdu->length));
   1332 		return -1;
   1333 	}
   1334 	if (ntohs(t->length) + PDU_VER_LENGTH > MAX_PDU_SIZE) {
   1335 		fatalp("Message to large discarded\n");
   1336 		return -1;
   1337 	}
   1338 	/* Arrange them in a buffer and send */
   1339 	memcpy(sendspace, pdu, sizeof(struct ldp_pdu));
   1340 	memcpy(sendspace + sizeof(struct ldp_pdu), t,
   1341 	    ntohs(t->length) + TLV_TYPE_LENGTH);
   1342 
   1343 	/* Report keepalives only for DEBUG */
   1344 	if ((ntohs(t->type) != 0x201) && (ntohs(t->type) != 0x400)) {
   1345 		debugp("Sending message type 0x%.4X to %s (size: %d)\n",
   1346 		    ntohs(t->type), inet_ntoa(p->ldp_id), ntohs(t->length));
   1347 	} else
   1348 	/* downgraded from warnp to debugp for now */
   1349 		debugp("Sending message type 0x%.4X to %s (size: %d)\n",
   1350 		    ntohs(t->type), inet_ntoa(p->ldp_id), ntohs(t->length));
   1351 
   1352 	/* Send it finally */
   1353 	return send(p->socket, sendspace,
   1354 		ntohs(pdu->length) + PDU_VER_LENGTH, 0);
   1355 }
   1356 
   1357 /*
   1358  * Encapsulates TLV into a PDU and sends it to a peer
   1359  */
   1360 int
   1361 send_tlv(const struct ldp_peer * p, const struct tlv * t)
   1362 {
   1363 	struct ldp_pdu  pdu;
   1364 
   1365 	pdu.version = htons(LDP_VERSION);
   1366 	inet_aton(LDP_ID, &pdu.ldp_id);
   1367 	pdu.label_space = 0;
   1368 	pdu.length = htons(ntohs(t->length) + TLV_TYPE_LENGTH +
   1369 		PDU_PAYLOAD_LENGTH);
   1370 
   1371 	return send_message(p, &pdu, t);
   1372 }
   1373 
   1374 
   1375 int
   1376 send_addresses(const struct ldp_peer * p)
   1377 {
   1378 	struct address_list_tlv *t;
   1379 	int             ret;
   1380 
   1381 	t = build_address_list_tlv();
   1382 
   1383 	ret = send_tlv(p, (struct tlv *) t);
   1384 	free(t);
   1385 	return ret;
   1386 
   1387 }
   1388