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