1/* $NetBSD: socketops.c,v 1.10 2011/06/16 14:48:30 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@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 "fsm.h"
51#include "ldp.h"
52#include "ldp_command.h"
53#include "tlv.h"
54#include "ldp_peer.h"
55#include "notifications.h"
56#include "tlv_stack.h"
57#include "mpls_interface.h"
58#include "label.h"
59#include "mpls_routes.h"
60#include "ldp_errors.h"
61#include "socketops.h"
62
63int             ls;			/* TCP listening socket on port 646 */
64int             route_socket;		/* used to see when a route is added/deleted */
65int		hello_socket;		/* hello multicast listener - transmitter */
66int		command_socket;		/* Listening socket for interface command */
67int             current_msg_id = 0x233;
68int		command_port = LDP_COMMAND_PORT;
69extern int      replay_index;
70extern struct rt_msg replay_rt[REPLAY_MAX];
71extern struct com_sock	csockets[MAX_COMMAND_SOCKETS];
72
73int	ldp_hello_time = LDP_HELLO_TIME;
74int	ldp_keepalive_time = LDP_KEEPALIVE_TIME;
75int	ldp_holddown_time = LDP_HOLDTIME;
76int	no_default_route = 1;
77
78void	recv_pdu(int);
79void	send_hello_alarm(int);
80__dead static void bail_out(int);
81static int get_local_addr(struct sockaddr_dl *, struct in_addr *);
82
83int
84create_hello_socket()
85{
86	struct ip_mreq  mcast_addr;
87	int             s = socket(PF_INET, SOCK_DGRAM, 17);
88
89	if (s < 0)
90		return s;
91
92	/*
93	 * RFC3036 specifies we should listen to all subnet routers multicast
94	 * group
95	 */
96	mcast_addr.imr_multiaddr.s_addr = inet_addr(ALL_ROUTERS);
97	mcast_addr.imr_interface.s_addr = htonl(INADDR_ANY);
98
99	socket_reuse_port(s);
100	/* Bind it to port 646 on specific address */
101	if (bind_socket(s, htonl(INADDR_ANY)) == -1) {
102		warnp("Cannot bind hello socket\n");
103		close(s);
104		return -1;
105	}
106	/* We don't need to receive back our messages */
107	if (setsockopt(s, IPPROTO_IP, IP_MULTICAST_LOOP, &(uint8_t){0},
108	    sizeof(uint8_t)) == -1) {
109		fatalp("setsockopt: %s", strerror(errno));
110		close(s);
111		return -1;
112	}
113	/* Finally join the group */
114        if (setsockopt(s, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &mcast_addr,
115	    sizeof(mcast_addr)) == -1) {
116                fatalp("setsockopt: %s", strerror(errno));
117                close(s);
118                return -1;
119        }
120	/* TTL:1, TOS: 0xc0 */
121	if (set_mcast_ttl(s) == -1) {
122		close(s);
123		return -1;
124	}
125	if (set_tos(s) == -1) {
126		fatalp("set_tos: %s", strerror(errno));
127		close(s);
128		return -1;
129	}
130	if (setsockopt(s, IPPROTO_IP, IP_RECVIF, &(uint32_t){1}, sizeof(uint32_t)) == -1) {
131		fatalp("Cannot set IP_RECVIF\n");
132		close(s);
133		return -1;
134	}
135	hello_socket = s;
136	return hello_socket;
137}
138
139/* Sets the TTL to 1 as we don't want to transmit outside this subnet */
140int
141set_ttl(int s)
142{
143	int             ret;
144	if ((ret = setsockopt(s, IPPROTO_IP, IP_TTL, &(int){1}, sizeof(int)))
145	    == -1)
146		fatalp("set_ttl: %s", strerror(errno));
147	return ret;
148}
149
150/* Sets multicast TTL to 1 */
151int
152set_mcast_ttl(int s)
153{
154	int	ret;
155	if ((ret = setsockopt(s, IPPROTO_IP, IP_MULTICAST_TTL, &(int){1},
156	    sizeof(int))) == -1)
157		fatalp("set_mcast_ttl: %s", strerror(errno));
158	return ret;
159}
160
161/* Sets TOS to 0xc0 aka IP Precedence 6 */
162int
163set_tos(int s)
164{
165	int             ret;
166	if ((ret = setsockopt(s, IPPROTO_IP, IP_TOS, &(int){0xc0},
167	    sizeof(int))) == -1)
168		fatalp("set_tos: %s", strerror(errno));
169	return ret;
170}
171
172int
173socket_reuse_port(int s)
174{
175	int             ret;
176	if ((ret = setsockopt(s, SOL_SOCKET, SO_REUSEPORT, &(int){1},
177	    sizeof(int))) == -1)
178		fatalp("socket_reuse_port: %s", strerror(errno));
179	return ret;
180}
181
182/* binds an UDP socket */
183int
184bind_socket(int s, uint32_t addr)
185{
186	struct sockaddr_in sa;
187
188	sa.sin_len = sizeof(sa);
189	sa.sin_family = AF_INET;
190	sa.sin_port = htons(LDP_PORT);
191	sa.sin_addr.s_addr = addr;
192	if (bind(s, (struct sockaddr *) (&sa), sizeof(sa))) {
193		fatalp("bind_socket: %s", strerror(errno));
194		return -1;
195	}
196	return 0;
197}
198
199/* Create / bind the TCP socket */
200int
201create_listening_socket(void)
202{
203	struct sockaddr_in sa;
204	int             s;
205
206	sa.sin_len = sizeof(sa);
207	sa.sin_family = AF_INET;
208	sa.sin_port = htons(LDP_PORT);
209	sa.sin_addr.s_addr = htonl(INADDR_ANY);
210
211	s = socket(PF_INET, SOCK_STREAM, 6);
212	if (s < 0)
213		return s;
214	if (bind(s, (struct sockaddr *) & sa, sizeof(sa))) {
215		fatalp("bind: %s", strerror(errno));
216		close(s);
217		return -1;
218	}
219	if (listen(s, 10) == -1) {
220		fatalp("listen: %s", strerror(errno));
221		close(s);
222		return -1;
223	}
224/*	if (set_tos(s) == -1) {
225		fatalp("set_tos: %s", strerror(errno));
226		close(s);
227		return -1;
228	}
229*/	return s;
230}
231
232/*
233 * It's ugly. We need a function to pass all tlvs and create pdu but since I
234 * use UDP socket only to send hellos, I didn't bother
235 */
236void
237send_hello(void)
238{
239	struct hello_tlv *t;
240	struct common_hello_tlv *cht;
241	struct ldp_pdu  *spdu;
242	struct transport_address_tlv *trtlv;
243	void *v;
244	struct sockaddr_in sadest;	/* Destination ALL_ROUTERS */
245	int sb = 0;			/* sent bytes */
246	struct ifaddrs *ifa, *ifb;
247	struct sockaddr_in *if_sa;
248	char lastifname[20];
249
250#define HELLO_MSG_SIZE (sizeof(struct ldp_pdu) + 	/* PDU */	\
251			TLV_TYPE_LENGTH + MSGID_SIZE +	/* Hello TLV */	\
252			/* Common Hello TLV */				\
253			sizeof(struct common_hello_tlv) +		\
254			/* IPv4 Transport Address */			\
255			sizeof(struct transport_address_tlv))
256
257	if ((v = calloc(1, HELLO_MSG_SIZE)) == NULL) {
258		fatalp("malloc problem in send_hello()\n");
259		return;
260	}
261
262	spdu = (struct ldp_pdu *)((char *)v);
263	t = (struct hello_tlv *)(spdu + 1);
264	cht = &t->ch;	/* Hello tlv struct includes CHT */
265	trtlv = (struct transport_address_tlv *)(t + 1);
266
267	/* Prepare PDU envelope */
268	spdu->version = htons(LDP_VERSION);
269	spdu->length = htons(HELLO_MSG_SIZE - PDU_VER_LENGTH);
270	inet_aton(LDP_ID, &spdu->ldp_id);
271
272	/* Prepare Hello TLV */
273	t->type = htons(LDP_HELLO);
274	t->length = htons(MSGID_SIZE +
275			sizeof(struct common_hello_tlv) +
276			sizeof(struct transport_address_tlv));
277	/*
278	 * I used ID 0 instead of htonl(get_message_id()) because I've
279	 * seen hellos from a cisco router doing the same thing
280	 */
281	t->messageid = 0;
282
283	/* Prepare Common Hello attributes */
284	cht->type = htons(TLV_COMMON_HELLO);
285	cht->length = htons(sizeof(cht->holdtime) + sizeof(cht->res));
286	cht->holdtime = htons(ldp_holddown_time);
287	cht->res = 0;
288
289	/*
290	 * Prepare Transport Address TLV RFC3036 says: "If this optional TLV
291	 * is not present the IPv4 source address for the UDP packet carrying
292	 * the Hello should be used." But we send it because everybody seems
293	 * to do so
294	 */
295	trtlv->type = htons(TLV_IPV4_TRANSPORT);
296	trtlv->length = htons(sizeof(struct in_addr));
297	/* trtlv->address will be set for each socket */
298
299	/* Destination sockaddr */
300	memset(&sadest, 0, sizeof(sadest));
301	sadest.sin_len = sizeof(sadest);
302	sadest.sin_family = AF_INET;
303	sadest.sin_port = htons(LDP_PORT);
304	inet_aton(ALL_ROUTERS, &sadest.sin_addr);
305
306	if (getifaddrs(&ifa) == -1) {
307		free(v);
308		return;
309	}
310
311	lastifname[0] = '\0';
312	for (ifb = ifa; ifb; ifb = ifb->ifa_next) {
313		if_sa = (struct sockaddr_in *) ifb->ifa_addr;
314		if (if_sa->sin_family != AF_INET)
315			continue;
316		if (ntohl(if_sa->sin_addr.s_addr) >> 24 == IN_LOOPBACKNET ||
317		    ntohl(if_sa->sin_addr.s_addr) >> 24 == 0)
318			continue;
319		/* Send only once per interface, using master address */
320		if (strcmp(ifb->ifa_name, lastifname) == 0)
321			continue;
322		debugp("Sending hello on %s\n", ifb->ifa_name);
323		if (setsockopt(hello_socket, IPPROTO_IP, IP_MULTICAST_IF,
324		    &if_sa->sin_addr, sizeof(struct in_addr)) == -1) {
325			warnp("setsockopt failed: %s\n", strerror(errno));
326			continue;
327		}
328		trtlv->address.s_addr = if_sa->sin_addr.s_addr;
329
330		strlcpy(lastifname, ifb->ifa_name, sizeof(lastifname));
331
332		/* Send to the wire */
333		sb = sendto(hello_socket, v, HELLO_MSG_SIZE,
334			    0, (struct sockaddr *) & sadest, sizeof(sadest));
335		if (sb < (int)HELLO_MSG_SIZE)
336		    fatalp("send: %s", strerror(errno));
337		else
338		    debugp("Send %d bytes (PDU: %d, Hello TLV: %d, CH: %d)\n",
339			sb, (int) (sizeof(struct ldp_pdu) - PDU_VER_LENGTH),
340		       (int) (TLV_TYPE_LENGTH + MSGID_SIZE),
341		       (int) (sizeof(struct common_hello_tlv)));
342
343	}
344	freeifaddrs(ifa);
345	free(v);
346}
347
348int
349get_message_id(void)
350{
351	current_msg_id++;
352	return current_msg_id;
353}
354
355static int
356get_local_addr(struct sockaddr_dl *sdl, struct in_addr *sin)
357{
358	struct ifaddrs *ifa, *ifb;
359	struct sockaddr_in *sinet;
360
361	if (sdl == NULL)
362		return -1;
363
364	if (getifaddrs(&ifa) == -1)
365		return -1;
366	for (ifb = ifa; ifb; ifb = ifb->ifa_next)
367		if (ifb->ifa_addr->sa_family == AF_INET) {
368			if (if_nametoindex(ifb->ifa_name) != sdl->sdl_index)
369				continue;
370			sinet = (struct sockaddr_in*) ifb->ifa_addr;
371			sin->s_addr = sinet->sin_addr.s_addr;
372			freeifaddrs(ifa);
373			return 0;
374		}
375	freeifaddrs(ifa);
376	return -1;
377}
378
379/* Receive PDUs on Multicast UDP socket */
380void
381recv_pdu(int sock)
382{
383	struct ldp_pdu  rpdu;
384	int             c, i;
385	struct msghdr msg;
386	struct iovec iov[1];
387	unsigned char recvspace[MAX_PDU_SIZE];
388	struct hello_tlv *t;
389	struct sockaddr_in fromsa;
390	struct sockaddr_dl *sdl = NULL;
391	struct in_addr my_ldp_addr, local_addr;
392	struct cmsghdr *cmptr;
393	union {
394		struct cmsghdr cm;
395		char control[1024];
396	} control_un;
397
398	debugp("Entering RECV_PDU\n");
399
400	memset(&msg, 0, sizeof(msg));
401	msg.msg_control = control_un.control;
402	msg.msg_controllen = sizeof(control_un.control);
403	msg.msg_flags = 0;
404	msg.msg_name = &fromsa;
405	msg.msg_namelen = sizeof(fromsa);
406	iov[0].iov_base = recvspace;
407	iov[0].iov_len = sizeof(recvspace);
408	msg.msg_iov = iov;
409	msg.msg_iovlen = 1;
410
411	c = recvmsg(sock, &msg, MSG_WAITALL);
412	debugp("Incoming PDU size: %d\n", c);
413
414	debugp("PDU from: %s\n", inet_ntoa(fromsa.sin_addr));
415
416	/* Check to see if this is larger than MIN_PDU_SIZE */
417	if (c < MIN_PDU_SIZE)
418		return;
419
420	/* Read the PDU */
421	i = get_pdu(recvspace, &rpdu);
422
423	/* We currently understand Version 1 */
424	if (rpdu.version != LDP_VERSION) {
425		fatalp("recv_pdu: Version mismatch\n");
426		return;
427	}
428
429	/* Maybe it's our hello */
430	inet_aton(LDP_ID, &my_ldp_addr);
431	if (rpdu.ldp_id.s_addr == my_ldp_addr.s_addr) {
432		fatalp("Received our PDU..\n");	/* it should be not looped */
433		return;
434	}
435
436	if (msg.msg_controllen < (socklen_t)sizeof(struct cmsghdr) ||
437	    (msg.msg_flags & MSG_CTRUNC))
438		local_addr.s_addr = my_ldp_addr.s_addr;
439	else {
440		for (cmptr = CMSG_FIRSTHDR(&msg); cmptr != NULL;
441		    cmptr = CMSG_NXTHDR(&msg, cmptr))
442			if (cmptr->cmsg_level == IPPROTO_IP &&
443			    cmptr->cmsg_type == IP_RECVIF) {
444				sdl = (struct sockaddr_dl *) CMSG_DATA(cmptr);
445				break;
446			}
447		if (get_local_addr(sdl, &local_addr) != 0)
448			local_addr.s_addr = my_ldp_addr.s_addr;
449	}
450
451
452	debugp("Read %d bytes from address %s Length: %.4d Version: %d\n",
453	       c, inet_ntoa(rpdu.ldp_id), rpdu.length, rpdu.version);
454
455	/* Fill the TLV messages */
456	t = get_hello_tlv(recvspace + i, c - i);
457	run_ldp_hello(&rpdu, t, &fromsa.sin_addr, &local_addr, sock);
458}
459
460void
461send_hello_alarm(int unused)
462{
463	struct ldp_peer *p, *ptmp;
464	struct hello_info *hi, *hinext;
465	time_t          t = time(NULL);
466	int             olderrno = errno;
467
468	/* Send hellos */
469	if (!(t % ldp_hello_time))
470		send_hello();
471
472	/* Timeout -- */
473	SLIST_FOREACH(p, &ldp_peer_head, peers)
474		p->timeout--;
475
476	/* Check for timeout */
477	SLIST_FOREACH_SAFE(p, &ldp_peer_head, peers, ptmp)
478		if (p->timeout < 1)
479			switch (p->state) {
480			case LDP_PEER_HOLDDOWN:
481				debugp("LDP holddown expired for peer %s\n",
482				       inet_ntoa(p->ldp_id));
483				ldp_peer_delete(p);
484				break;
485			case LDP_PEER_ESTABLISHED:
486			case LDP_PEER_CONNECTED:
487				send_notification(p, 0,
488				    NOTIF_KEEP_ALIVE_TIMER_EXPIRED);
489				warnp("Keepalive expired for %s\n",
490				    inet_ntoa(p->ldp_id));
491				ldp_peer_holddown(p);
492				break;
493			}	/* switch */
494
495	/* send keepalives */
496	if (!(t % ldp_keepalive_time)) {
497		SLIST_FOREACH(p, &ldp_peer_head, peers)
498		    if (p->state == LDP_PEER_ESTABLISHED) {
499			debugp("Sending KeepAlive to %s\n",
500			    inet_ntoa(p->ldp_id));
501			keep_alive(p);
502		    }
503	}
504
505	/* Decrement hello info keepalives */
506	SLIST_FOREACH(hi, &hello_info_head, infos)
507		if (hi->keepalive != 0xFFFF)
508			hi->keepalive--;
509
510	/* Check hello keepalives */
511	SLIST_FOREACH_SAFE(hi, &hello_info_head, infos, hinext)
512		if (hi->keepalive < 1)
513			SLIST_REMOVE(&hello_info_head, hi, hello_info, infos);
514
515	/* Set the alarm again and bail out */
516	alarm(1);
517	errno = olderrno;
518}
519
520static void
521bail_out(int x)
522{
523	ldp_peer_holddown_all();
524	flush_mpls_routes();
525	exit(0);
526}
527
528/*
529 * The big poll that catches every single event
530 * on every socket.
531 */
532int
533the_big_loop(void)
534{
535	int		sock_error;
536	uint32_t	i;
537	socklen_t       sock_error_size = sizeof(int);
538	struct ldp_peer *p;
539	struct com_sock	*cs;
540	struct pollfd	pfd[MAX_POLL_FDS];
541
542	assert(MAX_POLL_FDS > 3);
543
544	SLIST_INIT(&hello_info_head);
545
546	signal(SIGALRM, send_hello_alarm);
547	signal(SIGPIPE, SIG_IGN);
548	signal(SIGINT, bail_out);
549	signal(SIGTERM, bail_out);
550	send_hello_alarm(1);
551
552	route_socket = socket(PF_ROUTE, SOCK_RAW, AF_UNSPEC);
553
554	sock_error = bind_current_routes();
555	if (sock_error != LDP_E_OK) {
556		fatalp("Cannot get current routes\n");
557		return sock_error;
558	}
559
560	for (;;) {
561		nfds_t pollsum = 4;
562
563		pfd[0].fd = ls;
564		pfd[0].events = POLLRDNORM;
565		pfd[0].revents = 0;
566
567		pfd[1].fd = route_socket;
568		pfd[1].events = POLLRDNORM;
569		pfd[1].revents = 0;
570
571		pfd[2].fd = command_socket;
572		pfd[2].events = POLLRDNORM;
573		pfd[2].revents = 0;
574
575		/* Hello socket */
576		pfd[3].fd = hello_socket;
577		pfd[3].events = POLLIN;
578		pfd[3].revents = 0;
579
580		/* Command sockets */
581		for (i=0; i < MAX_COMMAND_SOCKETS; i++)
582			if (csockets[i].socket != -1) {
583				if (pollsum >= MAX_POLL_FDS)
584					break;
585				pfd[pollsum].fd = csockets[i].socket;
586				pfd[pollsum].events = POLLIN;
587				pfd[pollsum].revents = 0;
588				pollsum++;
589			}
590
591		/* LDP Peer sockets */
592		SLIST_FOREACH(p, &ldp_peer_head, peers) {
593			if (p->socket < 1)
594				continue;
595			switch (p->state) {
596			    case LDP_PEER_CONNECTED:
597			    case LDP_PEER_ESTABLISHED:
598				if (pollsum >= MAX_POLL_FDS)
599					break;
600				pfd[pollsum].fd = p->socket;
601				pfd[pollsum].events = POLLRDNORM;
602				pfd[pollsum].revents = 0;
603				pollsum++;
604				break;
605			    case LDP_PEER_CONNECTING:
606				if (pollsum >= MAX_POLL_FDS)
607					break;
608				pfd[pollsum].fd = p->socket;
609				pfd[pollsum].events = POLLWRNORM;
610				pfd[pollsum].revents = 0;
611				pollsum++;
612				break;
613			}
614		}
615
616		if (pollsum >= MAX_POLL_FDS) {
617			fatalp("Too many sockets. Increase MAX_POLL_FDS\n");
618			return LDP_E_TOO_MANY_FDS;
619		}
620		if (poll(pfd, pollsum, INFTIM) < 0) {
621			if (errno != EINTR)
622				fatalp("poll: %s", strerror(errno));
623			continue;
624		}
625
626		for (i = 0; i < pollsum; i++) {
627			if ((pfd[i].revents & POLLRDNORM) ||
628			    (pfd[i].revents & POLLIN)) {
629				if(pfd[i].fd == ls)
630					new_peer_connection();
631				else if (pfd[i].fd == route_socket) {
632					struct rt_msg xbuf;
633					int l;
634					do {
635						l = read(route_socket, &xbuf,
636						    sizeof(xbuf));
637					} while ((l == -1) && (errno == EINTR));
638
639					if (l == -1)
640						break;
641
642					check_route(&xbuf, l);
643
644				} else if (pfd[i].fd == hello_socket) {
645					/* Receiving hello socket */
646					recv_pdu(pfd[i].fd);
647				} else if (pfd[i].fd == command_socket) {
648					command_accept(command_socket);
649				} else if ((cs = is_command_socket(pfd[i].fd))
650						!= NULL) {
651					command_dispatch(cs);
652				} else {
653					/* ldp peer socket */
654					p = get_ldp_peer_by_socket(pfd[i].fd);
655					if (p)
656						recv_session_pdu(p);
657				}
658			} else if(pfd[i].revents & POLLWRNORM) {
659				p = get_ldp_peer_by_socket(pfd[i].fd);
660				if (!p)
661					continue;
662				if (getsockopt(pfd[i].fd, SOL_SOCKET, SO_ERROR,
663				    &sock_error, &sock_error_size) != 0 ||
664				    sock_error != 0) {
665					ldp_peer_holddown(p);
666					sock_error = 0;
667				} else {
668					p->state = LDP_PEER_CONNECTED;
669					send_initialize(p);
670				}
671			}
672		}
673
674		for (int ri = 0; ri < replay_index; ri++) {
675			debugp("Replaying: PID %d, SEQ %d\n",
676				replay_rt[ri].m_rtm.rtm_pid,
677				replay_rt[ri].m_rtm.rtm_seq);
678			check_route(&replay_rt[ri], sizeof(struct rt_msg));
679                }
680		replay_index = 0;
681	}	/* for (;;) */
682}
683
684void
685new_peer_connection()
686{
687	struct sockaddr_in sa, sin_me;
688	int             s;
689
690	s = accept(ls, (struct sockaddr *) & sa,
691		& (socklen_t) { sizeof(struct sockaddr_in) } );
692	if (s < 0) {
693		fatalp("accept: %s", strerror(errno));
694		return;
695	}
696
697	if (get_ldp_peer(&sa.sin_addr) != NULL) {
698		close(s);
699		return;
700	}
701
702	warnp("Accepted a connection from %s\n", inet_ntoa(sa.sin_addr));
703
704	if (getsockname(s, (struct sockaddr *)&sin_me,
705	    & (socklen_t) { sizeof(struct sockaddr_in) } )) {
706		fatalp("new_peer_connection(): cannot getsockname\n");
707		close(s);
708		return;
709	}
710
711	if (ntohl(sa.sin_addr.s_addr) < ntohl(sin_me.sin_addr.s_addr)) {
712		fatalp("Peer %s: connect from lower ID\n",
713		    inet_ntoa(sa.sin_addr));
714		close(s);
715		return;
716	}
717	/* XXX: sa.sin_addr ain't peer LDP ID ... */
718	ldp_peer_new(&sa.sin_addr, &sa.sin_addr, NULL, ldp_holddown_time, s);
719
720}
721
722void
723send_initialize(struct ldp_peer * p)
724{
725	struct init_tlv ti;
726
727	ti.type = htons(LDP_INITIALIZE);
728	ti.length = htons(sizeof(struct init_tlv) - TLV_TYPE_LENGTH);
729	ti.messageid = htonl(get_message_id());
730	ti.cs_type = htons(TLV_COMMON_SESSION);
731	ti.cs_len = htons(CS_LEN);
732	ti.cs_version = htons(LDP_VERSION);
733	ti.cs_keepalive = htons(2 * ldp_keepalive_time);
734	ti.cs_adpvlim = 0;
735	ti.cs_maxpdulen = htons(MAX_PDU_SIZE);
736	ti.cs_peeraddress.s_addr = p->ldp_id.s_addr;
737	ti.cs_peeraddrspace = 0;
738
739	send_tlv(p, (struct tlv *) (void *) &ti);
740}
741
742void
743keep_alive(struct ldp_peer * p)
744{
745	struct ka_tlv   kt;
746
747	kt.type = htons(LDP_KEEPALIVE);
748	kt.length = htons(sizeof(kt.messageid));
749	kt.messageid = htonl(get_message_id());
750
751	send_tlv(p, (struct tlv *) (void *) &kt);
752
753}
754
755void
756recv_session_pdu(struct ldp_peer * p)
757{
758	struct ldp_pdu *rpdu;
759	struct address_tlv *atlv;
760	struct al_tlv  *altlv;
761	struct init_tlv	*itlv;
762	struct label_map_tlv *lmtlv;
763	struct fec_tlv *fectlv;
764	struct label_tlv *labeltlv;
765	struct notification_tlv *nottlv;
766	struct hello_info *hi;
767
768	int             c;
769	int32_t         wo = 0;
770	struct tlv     *ttmp;
771	unsigned char   recvspace[MAX_PDU_SIZE];
772
773	memset(recvspace, 0, MAX_PDU_SIZE);
774
775	c = recv(p->socket, (void *) recvspace, MAX_PDU_SIZE, MSG_PEEK);
776
777	debugp("Ready to read %d bytes\n", c);
778
779	if (c < 1) {		/* Session closed */
780		warnp("Error in connection with %s\n", inet_ntoa(p->ldp_id));
781		ldp_peer_holddown(p);
782		return;
783	}
784	if (c > MAX_PDU_SIZE) {
785		debugp("Incoming PDU size exceeds MAX_PDU_SIZE !\n");
786		return;
787	}
788	if (c < MIN_PDU_SIZE) {
789		debugp("PDU too small received from peer %s\n", inet_ntoa(p->ldp_id));
790		return;
791	}
792	rpdu = (struct ldp_pdu *) recvspace;
793	/* XXX: buggy messages may crash the whole thing */
794	c = recv(p->socket, (void *) recvspace,
795		ntohs(rpdu->length) + PDU_VER_LENGTH, MSG_WAITALL);
796	rpdu = (struct ldp_pdu *) recvspace;
797
798	/* Check if it's somehow OK... */
799	if (check_recv_pdu(p, rpdu, c) != 0)
800		return;
801
802	debugp("Read %d bytes, PDU size: %d bytes\n", c, ntohs(rpdu->length));
803	wo = sizeof(struct ldp_pdu);
804
805	while (wo + TLV_TYPE_LENGTH < (uint)c) {
806
807		ttmp = (struct tlv *) (&recvspace[wo]);
808
809		if ((ntohs(ttmp->type) != LDP_KEEPALIVE) &&
810		    (ntohs(ttmp->type) != LDP_LABEL_MAPPING)) {
811			debugp("Got Type: 0x%.4X (Length: %d) from %s\n",
812			    ntohs(ttmp->type), ntohs(ttmp->length),
813			    inet_ntoa(p->ldp_id));
814		} else
815			debugp("Got Type: 0x%.4X (Length: %d) from %s\n",
816			    ntohs(ttmp->type), ntohs(ttmp->length),
817			    inet_ntoa(p->ldp_id));
818
819		/* Should we get the message ? */
820		if (p->state != LDP_PEER_ESTABLISHED &&
821		    ntohs(ttmp->type) != LDP_INITIALIZE &&
822		    ntohs(ttmp->type) != LDP_KEEPALIVE)
823			break;
824		/* The big switch */
825		switch (ntohs(ttmp->type)) {
826		case LDP_INITIALIZE:
827			itlv = (struct init_tlv *)ttmp;
828			/* Check size */
829			if (ntohs(itlv->length) <
830			    sizeof(struct init_tlv) - TLV_TYPE_LENGTH) {
831				send_notification(p, 0,
832				    NOTIF_BAD_PDU_LEN | NOTIF_FATAL);
833				ldp_peer_holddown(p);
834				break;
835			}
836			/* Check version */
837			if (ntohs(itlv->cs_version) != LDP_VERSION) {
838				send_notification(p, ntohl(itlv->messageid),
839					NOTIF_BAD_LDP_VER | NOTIF_FATAL);
840				ldp_peer_holddown(p);
841				break;
842			}
843			/* Check if we got any hello from this one */
844			SLIST_FOREACH(hi, &hello_info_head, infos)
845				if (hi->ldp_id.s_addr == rpdu->ldp_id.s_addr)
846					break;
847			if (hi == NULL) {
848			    send_notification(p, ntohl(itlv->messageid),
849				NOTIF_SESSION_REJECTED_NO_HELLO | NOTIF_FATAL);
850			    ldp_peer_holddown(p);
851			    break;
852			}
853
854			if (!p->master) {
855				keep_alive(p);
856				send_initialize(p);
857			} else {
858				p->state = LDP_PEER_ESTABLISHED;
859				p->established_t = time(NULL);
860				keep_alive(p);
861
862				/*
863				 * Recheck here ldp id because we accepted
864				 * connection without knowing who is it for sure
865				 */
866				p->ldp_id.s_addr = rpdu->ldp_id.s_addr;
867
868				fatalp("LDP neighbour %s is UP\n",
869				    inet_ntoa(p->ldp_id));
870				mpls_add_ldp_peer(p);
871				send_addresses(p);
872				send_all_bindings(p);
873			}
874			break;
875		case LDP_KEEPALIVE:
876			if ((p->state == LDP_PEER_CONNECTED) && (!p->master)) {
877				p->state = LDP_PEER_ESTABLISHED;
878				p->established_t = time(NULL);
879				fatalp("LDP neighbour %s is UP\n",
880				    inet_ntoa(p->ldp_id));
881				mpls_add_ldp_peer(p);
882				send_addresses(p);
883				send_all_bindings(p);
884			}
885			p->timeout = p->holdtime;
886			break;
887		case LDP_ADDRESS:
888			/* Add peer addresses */
889			atlv = (struct address_tlv *) ttmp;
890			altlv = (struct al_tlv *) (&atlv[1]);
891			add_ifaddresses(p, altlv);
892			print_bounded_addresses(p);
893			break;
894		case LDP_ADDRESS_WITHDRAW:
895			atlv = (struct address_tlv *) ttmp;
896			altlv = (struct al_tlv *) (&atlv[1]);
897			del_ifaddresses(p, altlv);
898			break;
899		case LDP_LABEL_MAPPING:
900			lmtlv = (struct label_map_tlv *) ttmp;
901			fectlv = (struct fec_tlv *) (&lmtlv[1]);
902			labeltlv = (struct label_tlv *)((unsigned char *)fectlv
903				+ ntohs(fectlv->length) + TLV_TYPE_LENGTH);
904			map_label(p, fectlv, labeltlv);
905			break;
906		case LDP_LABEL_REQUEST:
907			lmtlv = (struct label_map_tlv *) ttmp;
908			fectlv = (struct fec_tlv *) (&lmtlv[1]);
909			switch (request_respond(p, lmtlv, fectlv)) {
910			case LDP_E_BAD_FEC:
911				send_notification(p, ntohl(lmtlv->messageid),
912					NOTIF_UNKNOWN_TLV);
913				break;
914			case LDP_E_BAD_AF:
915				send_notification(p, ntohl(lmtlv->messageid),
916					NOTIF_UNSUPPORTED_AF);
917				break;
918			case LDP_E_NO_SUCH_ROUTE:
919				send_notification(p, ntohl(lmtlv->messageid),
920					NOTIF_NO_ROUTE);
921				break;
922			}
923			break;
924		case LDP_LABEL_WITHDRAW:
925			lmtlv = (struct label_map_tlv *) ttmp;
926			fectlv = (struct fec_tlv *) (&lmtlv[1]);
927			if (withdraw_label(p, fectlv) == LDP_E_OK) {
928				/* Send RELEASE */
929				prepare_release(ttmp);
930				send_tlv(p, ttmp);
931				}
932			break;
933		case LDP_LABEL_RELEASE:
934			/*
935			 * XXX: we need to make a timed queue...
936			 * For now I just assume peers are processing messages
937			 * correctly so I just ignore confirmations
938			 */
939			wo = -1;	/* Ignore rest of message */
940			break;
941		case LDP_LABEL_ABORT:
942		/* XXX: For now I pretend I can process everything
943		 * RFC 3036, Section 3.5.9.1
944		 * If an LSR receives a Label Abort Request Message after it
945		 * has responded to the Label Request in question with a Label
946		 * Mapping message or a Notification message, it ignores the
947		 * abort request.
948		 */
949			wo = -1;
950			break;
951		case LDP_NOTIFICATION:
952			nottlv = (struct notification_tlv *) ttmp;
953			nottlv->st_code = ntohl(nottlv->st_code);
954			fatalp("Got notification 0x%X from peer %s\n",
955			    nottlv->st_code, inet_ntoa(p->ldp_id));
956			if (nottlv->st_code >> 31) {
957				fatalp("LDP peer %s signalized %s\n",
958				    inet_ntoa(p->ldp_id),
959				    NOTIF_STR[(nottlv->st_code << 1) >> 1]);
960				ldp_peer_holddown(p);
961				wo = -1;
962			}
963			break;
964		case LDP_HELLO:
965			/* No hellos should came on tcp session */
966			wo = -1;
967			break;
968		default:
969			warnp("Unknown TLV received from %s\n",
970			    inet_ntoa(p->ldp_id));
971			debug_tlv(ttmp);
972			wo = -1;/* discard the rest of the message */
973			break;
974		}
975		if (wo < 0) {
976			debugp("Discarding the rest of the message\n");
977			break;
978		} else {
979			wo += ntohs(ttmp->length) + TLV_TYPE_LENGTH;
980			debugp("WORKED ON %u bytes (Left %d)\n", wo, c - wo);
981		}
982	}			/* while */
983
984}
985
986/* Sends a pdu, tlv pair to a connected peer */
987int
988send_message(struct ldp_peer * p, struct ldp_pdu * pdu, struct tlv * t)
989{
990	unsigned char   sendspace[MAX_PDU_SIZE];
991
992	/* Check if peer is connected */
993	switch (p->state) {
994	case LDP_PEER_CONNECTED:
995	case LDP_PEER_ESTABLISHED:
996		break;
997	default:
998		return -1;
999	}
1000
1001	/* Check length validity first */
1002	if (ntohs(pdu->length) !=
1003	    ntohs(t->length) + TLV_TYPE_LENGTH + PDU_PAYLOAD_LENGTH) {
1004		fatalp("LDP: TLV - PDU incompability. Message discarded\n");
1005		fatalp("LDP: TLV len %d - PDU len %d\n", ntohs(t->length),
1006		    ntohs(pdu->length));
1007		return -1;
1008	}
1009	if (ntohs(t->length) + PDU_VER_LENGTH > MAX_PDU_SIZE) {
1010		fatalp("Message to large discarded\n");
1011		return -1;
1012	}
1013	/* Arrange them in a buffer and send */
1014	memcpy(sendspace, pdu, sizeof(struct ldp_pdu));
1015	memcpy(sendspace + sizeof(struct ldp_pdu), t,
1016	    ntohs(t->length) + TLV_TYPE_LENGTH);
1017
1018	/* Report keepalives only for DEBUG */
1019	if ((ntohs(t->type) != 0x201) && (ntohs(t->type) != 0x400)) {
1020		debugp("Sending message type 0x%.4X to %s (size: %d)\n",
1021		    ntohs(t->type), inet_ntoa(p->ldp_id), ntohs(t->length));
1022	} else
1023	/* downgraded from warnp to debugp for now */
1024		debugp("Sending message type 0x%.4X to %s (size: %d)\n",
1025		    ntohs(t->type), inet_ntoa(p->ldp_id), ntohs(t->length));
1026
1027	/* Send it finally */
1028	return send(p->socket, sendspace,
1029		ntohs(pdu->length) + PDU_VER_LENGTH, 0);
1030}
1031
1032/*
1033 * Encapsulates TLV into a PDU and sends it to a peer
1034 */
1035int
1036send_tlv(struct ldp_peer * p, struct tlv * t)
1037{
1038	struct ldp_pdu  pdu;
1039
1040	pdu.version = htons(LDP_VERSION);
1041	inet_aton(LDP_ID, &pdu.ldp_id);
1042	pdu.label_space = 0;
1043	pdu.length = htons(ntohs(t->length) + TLV_TYPE_LENGTH +
1044		PDU_PAYLOAD_LENGTH);
1045
1046	return send_message(p, &pdu, t);
1047}
1048
1049
1050int
1051send_addresses(struct ldp_peer * p)
1052{
1053	struct address_list_tlv *t;
1054	int             ret;
1055
1056	t = build_address_list_tlv();
1057
1058	ret = send_tlv(p, (struct tlv *) t);
1059	free(t);
1060	return ret;
1061
1062}
1063