Implement support for layer 2 relaying and add further Relay Agent

Information (RFC 3046) options. While at there change the naming of the
remote destination parameters: instead of "server" call it "destination".

ok reyk@

6 files changed, 524 insertions, 102 deletions

diff --git a/usr.sbin/dhcrelay/bpf.c b/usr.sbin/dhcrelay/bpf.c
index 2b5e07fbbd2..493b425a463 100644
--- a/usr.sbin/dhcrelay/bpf.c
+++ b/usr.sbin/dhcrelay/bpf.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: bpf.c,v 1.13 2016/12/08 19:18:15 rzalamena Exp $ */
+/*	$OpenBSD: bpf.c,v 1.14 2016/12/12 15:41:05 rzalamena Exp $ */
 
 /* BPF socket interface code, originally contributed by Archie Cobbs. */
 
@@ -93,6 +93,38 @@ if_register_send(struct interface_info *info)
 }
 
 /*
+ * Packet filter program: 'ip and udp and dst port CLIENT_PORT'
+ */
+struct bpf_insn dhcp_bpf_sfilter[] = {
+	/* Make sure this is an IP packet... */
+	BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12),
+	BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_IP, 0, 8),
+
+	/* Make sure it's a UDP packet... */
+	BPF_STMT(BPF_LD + BPF_B + BPF_ABS, 23),
+	BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, IPPROTO_UDP, 0, 6),
+
+	/* Make sure this isn't a fragment... */
+	BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 20),
+	BPF_JUMP(BPF_JMP + BPF_JSET + BPF_K, 0x1fff, 4, 0),
+
+	/* Get the IP header length... */
+	BPF_STMT(BPF_LDX + BPF_B + BPF_MSH, 14),
+
+	/* Make sure it's to the right port... */
+	BPF_STMT(BPF_LD + BPF_H + BPF_IND, 16),
+	BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, CLIENT_PORT, 0, 1),
+
+	/* If we passed all the tests, ask for the whole packet. */
+	BPF_STMT(BPF_RET+BPF_K, (u_int)-1),
+
+	/* Otherwise, drop it. */
+	BPF_STMT(BPF_RET+BPF_K, 0),
+};
+
+int dhcp_bpf_sfilter_len = sizeof(dhcp_bpf_sfilter) / sizeof(struct bpf_insn);
+
+/*
  * Packet filter program: 'ip and udp and dst port SERVER_PORT'
  */
 struct bpf_insn dhcp_bpf_filter[] = {
@@ -161,6 +193,38 @@ struct bpf_insn dhcp_bpf_efilter[] = {
 int dhcp_bpf_efilter_len = sizeof(dhcp_bpf_efilter) / sizeof(struct bpf_insn);
 
 /*
+ * Packet write filter program: 'ip and udp and src port CLIENT_PORT'
+ */
+struct bpf_insn dhcp_bpf_swfilter[] = {
+	/* Make sure this is an IP packet... */
+	BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12),
+	BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_IP, 0, 8),
+
+	/* Make sure it's a UDP packet... */
+	BPF_STMT(BPF_LD + BPF_B + BPF_ABS, 23),
+	BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, IPPROTO_UDP, 0, 6),
+
+	/* Make sure this isn't a fragment... */
+	BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 20),
+	BPF_JUMP(BPF_JMP + BPF_JSET + BPF_K, 0x1fff, 4, 0),
+
+	/* Get the IP header length... */
+	BPF_STMT(BPF_LDX + BPF_B + BPF_MSH, 14),
+
+	/* Make sure it's from the right port... */
+	BPF_STMT(BPF_LD + BPF_H + BPF_IND, 14),
+	BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, CLIENT_PORT, 0, 1),
+
+	/* If we passed all the tests, ask for the whole packet. */
+	BPF_STMT(BPF_RET+BPF_K, (u_int)-1),
+
+	/* Otherwise, drop it. */
+	BPF_STMT(BPF_RET+BPF_K, 0),
+};
+
+int dhcp_bpf_swfilter_len = sizeof(dhcp_bpf_swfilter) / sizeof(struct bpf_insn);
+
+/*
  * Packet write filter program: 'ip and udp and src port SERVER_PORT'
  */
 struct bpf_insn dhcp_bpf_wfilter[] = {
@@ -193,7 +257,7 @@ struct bpf_insn dhcp_bpf_wfilter[] = {
 int dhcp_bpf_wfilter_len = sizeof(dhcp_bpf_wfilter) / sizeof(struct bpf_insn);
 
 void
-if_register_receive(struct interface_info *info)
+if_register_receive(struct interface_info *info, int isserver)
 {
 	struct bpf_version v;
 	struct bpf_program p;
@@ -234,7 +298,10 @@ if_register_receive(struct interface_info *info)
 	info->rbuf_len = 0;
 
 	/* Set up the bpf filter program structure. */
-	if (info->hw_address.htype == HTYPE_IPSEC_TUNNEL) {
+	if (isserver) {
+		p.bf_len = dhcp_bpf_sfilter_len;
+		p.bf_insns = dhcp_bpf_sfilter;
+	} else if (info->hw_address.htype == HTYPE_IPSEC_TUNNEL) {
 		p.bf_len = dhcp_bpf_efilter_len;
 		p.bf_insns = dhcp_bpf_efilter;
 	} else {
@@ -245,8 +312,13 @@ if_register_receive(struct interface_info *info)
 		error("Can't install packet filter program: %m");
 
 	/* Set up the bpf write filter program structure. */
-	p.bf_len = dhcp_bpf_wfilter_len;
-	p.bf_insns = dhcp_bpf_wfilter;
+	if (isserver) {
+		p.bf_len = dhcp_bpf_swfilter_len;
+		p.bf_insns = dhcp_bpf_swfilter;
+	} else {
+		p.bf_len = dhcp_bpf_wfilter_len;
+		p.bf_insns = dhcp_bpf_wfilter;
+	}
 
 	if (ioctl(info->rfdesc, BIOCSETWF, &p) == -1)
 		error("Can't install write filter program: %m");
diff --git a/usr.sbin/dhcrelay/dhcp.h b/usr.sbin/dhcrelay/dhcp.h
index 67feae846a2..9c3c9aa9872 100644
--- a/usr.sbin/dhcrelay/dhcp.h
+++ b/usr.sbin/dhcrelay/dhcp.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: dhcp.h,v 1.5 2009/09/03 11:56:49 reyk Exp $	*/
+/*	$OpenBSD: dhcp.h,v 1.6 2016/12/12 15:41:05 rzalamena Exp $	*/
 
 /* Protocol structures... */
 
@@ -49,6 +49,10 @@
 						/* Everything but options. */
 #define DHCP_MTU_MAX		1500
 #define DHCP_OPTION_LEN		(DHCP_MTU_MAX - DHCP_FIXED_LEN)
+/* The option/sub-option maximum length. */
+#define DHCP_OPTION_MAXLEN	255
+/* The option/sub-option header length. */
+#define DHCP_OPTION_HDR_LEN	2
 
 #define BOOTP_MIN_LEN		300
 
diff --git a/usr.sbin/dhcrelay/dhcpd.h b/usr.sbin/dhcrelay/dhcpd.h
index 48067e2c8ec..1f9d929882f 100644
--- a/usr.sbin/dhcrelay/dhcpd.h
+++ b/usr.sbin/dhcrelay/dhcpd.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: dhcpd.h,v 1.17 2016/12/08 19:18:15 rzalamena Exp $	*/
+/*	$OpenBSD: dhcpd.h,v 1.18 2016/12/12 15:41:05 rzalamena Exp $	*/
 
 /*
  * Copyright (c) 2004 Henning Brauer <henning@openbsd.org>
@@ -53,6 +53,11 @@ struct packet_ctx {
 
 	struct sockaddr_storage		 pc_src;
 	struct sockaddr_storage		 pc_dst;
+
+	u_int8_t			*pc_circuit;
+	int				 pc_circuitlen;
+	u_int8_t			*pc_remote;
+	int				 pc_remotelen;
 };
 
 struct iaddr {
@@ -77,6 +82,12 @@ enum dhcp_state {
 	S_REBINDING
 };
 
+/* DHCP relaying modes. */
+enum dhcp_relay_mode {
+	DRM_UNKNOWN,
+	DRM_LAYER2,
+	DRM_LAYER3,
+};
 
 struct interface_info {
 	struct interface_info	*next;
@@ -123,7 +134,7 @@ int debug(char *, ...) __attribute__ ((__format__ (__printf__, 1, 2)));
 /* bpf.c */
 int if_register_bpf(struct interface_info *);
 void if_register_send(struct interface_info *);
-void if_register_receive(struct interface_info *);
+void if_register_receive(struct interface_info *, int);
 ssize_t send_packet(struct interface_info *,
     struct dhcp_packet *, size_t, struct packet_ctx *);
 ssize_t receive_packet(struct interface_info *, unsigned char *, size_t,
@@ -133,7 +144,7 @@ ssize_t receive_packet(struct interface_info *, unsigned char *, size_t,
 extern void (*bootp_packet_handler)(struct interface_info *,
     struct dhcp_packet *, int, struct packet_ctx *);
 struct interface_info *get_interface(const char *,
-    void (*)(struct protocol *));
+    void (*)(struct protocol *), int isserver);
 void dispatch(void);
 void got_one(struct protocol *);
 void add_protocol(char *, int, void (*)(struct protocol *), void *);
diff --git a/usr.sbin/dhcrelay/dhcrelay.8 b/usr.sbin/dhcrelay/dhcrelay.8
index 620ac84813f..7831fd3f8bc 100644
--- a/usr.sbin/dhcrelay/dhcrelay.8
+++ b/usr.sbin/dhcrelay/dhcrelay.8
@@ -1,4 +1,4 @@
-.\"	$OpenBSD: dhcrelay.8,v 1.12 2013/07/16 11:13:33 schwarze Exp $
+.\"	$OpenBSD: dhcrelay.8,v 1.13 2016/12/12 15:41:05 rzalamena Exp $
 .\"
 .\" Copyright (c) 1997 The Internet Software Consortium.
 .\" All rights reserved.
@@ -36,7 +36,7 @@
 .\" see ``http://www.isc.org/isc''.  To learn more about Vixie
 .\" Enterprises, see ``http://www.vix.com''.
 .\"
-.Dd $Mdocdate: July 16 2013 $
+.Dd $Mdocdate: December 12 2016 $
 .Dt DHCRELAY 8
 .Os
 .Sh NAME
@@ -45,8 +45,10 @@
 .Sh SYNOPSIS
 .Nm
 .Op Fl do
+.Op Fl C Ar circuit-id
+.Op Fl R Ar remote-id
 .Fl i Ar interface
-.Ar server1 Op Ar ... serverN
+.Ar destination1 Op Ar ... destinationN
 .Sh DESCRIPTION
 The
 .Nm
@@ -58,10 +60,15 @@ other subnets.
 listens for DHCP requests on a given interface.
 When a query is received,
 .Nm
-forwards it to the list of DHCP servers specified on the command line.
+forwards it to the list of DHCP destinations specified on the command line.
 When a reply is received, it is broadcast or unicast on the network from
 whence the original request came.
 .Pp
+The server might be a name, address or interface.
+.Nm
+will operate in layer 2 mode when the specified servers are interfaces,
+otherwise it will operate in layer 3 mode.
+.Pp
 The name of at least one DHCP server to which DHCP and BOOTP requests
 should be relayed,
 as well as the name of the network interface that
@@ -73,12 +80,19 @@ must be specified on the command line.
 supports relaying of DHCP traffic to configure IPsec tunnel mode
 clients when listening on the
 .Xr enc 4
-interface.
+interface using layer 3 mode only.
 The DHCP server has to support RFC 3046 to echo back the relay agent
 information to allow stateless DHCP reply to IPsec tunnel mapping.
 .Pp
 The options are as follows:
 .Bl -tag -width Ds
+.It Fl C Ar circuit-id
+The
+.Ar circuit-id
+relay agent information sub-option value that
+.Nm
+should append on relayed packets.
+If this option is not specified it will use the interface number by default.
 .It Fl d
 .Nm
 normally runs in the foreground until it has configured
@@ -90,12 +104,20 @@ to always run as a foreground process.
 The name of the network interface that
 .Nm
 should attempt to configure.
-At least one IPv4 address has to be configured on this interface.
+For layer 3 mode at least one IPv4 address has to be configured on this
+interface.
 .It Fl o
 Add the relay agent information option.
 By default, this is only enabled for the
 .Xr enc 4
 interface.
+.It Fl R Ar remote-id
+The
+.Ar remote-id
+relay agent information sub-option value that
+.Nm
+should append on relayed packets.
+If this option is not specified it will use the destination address by default.
 .El
 .Sh SEE ALSO
 .Xr dhclient 8 ,
diff --git a/usr.sbin/dhcrelay/dhcrelay.c b/usr.sbin/dhcrelay/dhcrelay.c
index 75664b3c04f..b66d2259966 100644
--- a/usr.sbin/dhcrelay/dhcrelay.c
+++ b/usr.sbin/dhcrelay/dhcrelay.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: dhcrelay.c,v 1.49 2016/12/08 19:18:15 rzalamena Exp $ */
+/*	$OpenBSD: dhcrelay.c,v 1.50 2016/12/12 15:41:05 rzalamena Exp $ */
 
 /*
  * Copyright (c) 2004 Henning Brauer <henning@cvs.openbsd.org>
@@ -66,12 +66,19 @@ void	 usage(void);
 int	 rdaemon(int);
 void	 relay(struct interface_info *, struct dhcp_packet *, int,
 	    struct packet_ctx *);
+void	 l2relay(struct interface_info *, struct dhcp_packet *, int,
+	    struct packet_ctx *);
 char	*print_hw_addr(int, int, unsigned char *);
 void	 got_response(struct protocol *);
 int	 get_rdomain(char *);
 
-ssize_t	 relay_agentinfo(struct interface_info *, struct dhcp_packet *,
-	    size_t, struct in_addr *, struct in_addr *);
+void	 relay_agentinfo(struct packet_ctx *, struct interface_info *);
+
+int	 relay_agentinfo_cmp(struct packet_ctx *pc, uint8_t *, int);
+ssize_t	 relay_agentinfo_append(struct packet_ctx *, struct dhcp_packet *,
+	    size_t);
+ssize_t	 relay_agentinfo_remove(struct packet_ctx *, struct dhcp_packet *,
+	    size_t);
 
 time_t cur_time;
 
@@ -84,7 +91,12 @@ struct interface_info *interfaces = NULL;
 int server_fd;
 int oflag;
 
+enum dhcp_relay_mode	 drm = DRM_UNKNOWN;
+const char		*rai_circuit = NULL;
+const char		*rai_remote = NULL;
+
 struct server_list {
+	struct interface_info *intf;
 	struct server_list *next;
 	struct sockaddr_in to;
 	int fd;
@@ -98,6 +110,7 @@ main(int argc, char *argv[])
 	struct server_list	*sp = NULL;
 	struct passwd		*pw;
 	struct sockaddr_in	 laddr;
+	int			 optslen;
 
 	daemonize = 1;
 
@@ -105,8 +118,11 @@ main(int argc, char *argv[])
 	openlog(__progname, LOG_NDELAY, DHCPD_LOG_FACILITY);
 	setlogmask(LOG_UPTO(LOG_INFO));
 
-	while ((ch = getopt(argc, argv, "adi:o")) != -1) {
+	while ((ch = getopt(argc, argv, "aC:di:oR:")) != -1) {
 		switch (ch) {
+		case 'C':
+			rai_circuit = optarg;
+			break;
 		case 'd':
 			daemonize = 0;
 			break;
@@ -114,7 +130,7 @@ main(int argc, char *argv[])
 			if (interfaces != NULL)
 				usage();
 
-			interfaces = get_interface(optarg, got_one);
+			interfaces = get_interface(optarg, got_one, 0);
 			if (interfaces == NULL)
 				error("interface '%s' not found", optarg);
 			break;
@@ -122,6 +138,9 @@ main(int argc, char *argv[])
 			/* add the relay agent information option */
 			oflag++;
 			break;
+		case 'R':
+			rai_remote = optarg;
+			break;
 
 		default:
 			usage();
@@ -135,10 +154,42 @@ main(int argc, char *argv[])
 	if (argc < 1)
 		usage();
 
+	if (rai_remote != NULL && rai_circuit == NULL)
+		error("you must specify a circuit-id with a remote-id");
+
+	/* Validate that we have space for all suboptions. */
+	if (rai_circuit != NULL) {
+		optslen = 2 + strlen(rai_circuit);
+		if (rai_remote != NULL)
+			optslen += 2 + strlen(rai_remote);
+
+		if (optslen > DHCP_OPTION_MAXLEN)
+			error("relay agent information is too long");
+	}
+
 	while (argc > 0) {
 		struct hostent		*he;
 		struct in_addr		 ia, *iap = NULL;
 
+		if ((sp = calloc(1, sizeof(*sp))) == NULL)
+			error("calloc");
+
+		if ((sp->intf = get_interface(argv[0], got_one, 1)) != NULL) {
+			if (drm == DRM_LAYER3)
+				error("don't mix interfaces with hosts");
+
+			if (sp->intf->hw_address.htype == HTYPE_IPSEC_TUNNEL)
+				error("can't use IPSec with layer 2");
+
+			sp->next = servers;
+			servers = sp;
+
+			drm = DRM_LAYER2;
+			argc--;
+			argv++;
+			continue;
+		}
+
 		if (inet_aton(argv[0], &ia))
 			iap = &ia;
 		else {
@@ -149,12 +200,16 @@ main(int argc, char *argv[])
 				iap = ((struct in_addr *)he->h_addr_list[0]);
 		}
 		if (iap) {
-			if ((sp = calloc(1, sizeof *sp)) == NULL)
-				error("calloc");
+			if (drm == DRM_LAYER2)
+				error("don't mix interfaces with hosts");
+
+			drm = DRM_LAYER3;
 			sp->next = servers;
 			servers = sp;
 			memcpy(&sp->to.sin_addr, iap, sizeof *iap);
-		}
+		} else
+			free(sp);
+
 		argc--;
 		argv++;
 	}
@@ -167,7 +222,9 @@ main(int argc, char *argv[])
 
 	if (interfaces == NULL)
 		error("no interface given");
-	if (interfaces->primary_address.s_addr == 0)
+	/* We need an address for running layer 3 mode. */
+	if (drm == DRM_LAYER3 &&
+	    interfaces->primary_address.s_addr == 0)
 		error("interface '%s' does not have an address",
 		    interfaces->name);
 
@@ -192,6 +249,9 @@ main(int argc, char *argv[])
 	laddr.sin_addr.s_addr = interfaces->primary_address.s_addr;
 	/* Set up the server sockaddrs. */
 	for (sp = servers; sp; sp = sp->next) {
+		if (sp->intf != NULL)
+			break;
+
 		sp->to.sin_port = server_port;
 		sp->to.sin_family = AF_INET;
 		sp->to.sin_len = sizeof sp->to;
@@ -234,7 +294,10 @@ main(int argc, char *argv[])
 	tzset();
 
 	time(&cur_time);
-	bootp_packet_handler = relay;
+	if (drm == DRM_LAYER3)
+		bootp_packet_handler = relay;
+	else
+		bootp_packet_handler = l2relay;
 
 	if ((pw = getpwnam("_dhcp")) == NULL)
 		error("user \"_dhcp\" not found");
@@ -274,6 +337,8 @@ relay(struct interface_info *ip, struct dhcp_packet *packet, int length,
 		return;
 	}
 
+	relay_agentinfo(pc, ip);
+
 	/* If it's a bootreply, forward it to the client. */
 	if (packet->op == BOOTREPLY) {
 		bzero(&to, sizeof(to));
@@ -303,8 +368,8 @@ relay(struct interface_info *ip, struct dhcp_packet *packet, int length,
 			memset(pc->pc_dmac, 0xff, sizeof(pc->pc_dmac));
 		}
 
-		if ((length = relay_agentinfo(interfaces,
-		    packet, length, NULL, &to.sin_addr)) == -1) {
+		if ((length = relay_agentinfo_remove(pc, packet,
+		    length)) == -1) {
 			note("ignoring BOOTREPLY with invalid "
 			    "relay agent information");
 			return;
@@ -350,8 +415,7 @@ relay(struct interface_info *ip, struct dhcp_packet *packet, int length,
 	if (!packet->giaddr.s_addr)
 		packet->giaddr = ip->primary_address;
 
-	if ((length = relay_agentinfo(ip, packet, length,
-	    &ss2sin(&pc->pc_src)->sin_addr, NULL)) == -1) {
+	if ((length = relay_agentinfo_append(pc, packet, length)) == -1) {
 		note("ignoring BOOTREQUEST with invalid "
 		    "relay agent information");
 		return;
@@ -374,7 +438,8 @@ usage(void)
 {
 	extern char	*__progname;
 
-	fprintf(stderr, "usage: %s [-do] -i interface server1 [... serverN]\n",
+	fprintf(stderr, "usage: %s [-do] [-C circuit-id] [-R remote-id] "
+	    "-i interface\n\tdestination1 [... destinationN]\n",
 	    __progname);
 	exit(1);
 }
@@ -488,98 +553,283 @@ got_response(struct protocol *l)
 		(*bootp_packet_handler)(NULL, &u.packet, result, &pc);
 }
 
+void
+relay_agentinfo(struct packet_ctx *pc, struct interface_info *intf)
+{
+	static u_int8_t		buf[8];
+
+	if (oflag == 0)
+		return;
+
+	if (rai_remote != NULL) {
+		pc->pc_remote = (u_int8_t *)rai_remote;
+		pc->pc_remotelen = strlen(rai_remote);
+	} else
+		pc->pc_remotelen = 0;
+
+	if (rai_circuit == NULL) {
+		buf[0] = (uint8_t)(intf->index << 8);
+		buf[1] = intf->index & 0xff;
+		pc->pc_circuit = buf;
+		pc->pc_circuitlen = 2;
+
+		if (rai_remote == NULL) {
+			pc->pc_remote =
+			    (uint8_t *)&ss2sin(&pc->pc_dst)->sin_addr;
+			pc->pc_remotelen =
+			    sizeof(ss2sin(&pc->pc_dst)->sin_addr);
+		}
+	} else {
+		pc->pc_circuit = (u_int8_t *)rai_circuit;
+		pc->pc_circuitlen = strlen(rai_circuit);
+	}
+}
+
+int
+relay_agentinfo_cmp(struct packet_ctx *pc, uint8_t *p, int plen)
+{
+	int		 len;
+	char		 buf[256];
+
+	if (oflag == 0)
+		return (-1);
+
+	len = *(p + 1);
+	if (len > plen)
+		return (-1);
+
+	switch (*p) {
+	case RAI_CIRCUIT_ID:
+		if (pc->pc_circuit == NULL)
+			return (-1);
+		if (pc->pc_circuitlen != len)
+			return (-1);
+
+		memcpy(buf, p + DHCP_OPTION_HDR_LEN, len);
+		return (memcmp(pc->pc_circuit, buf, len));
+
+	case RAI_REMOTE_ID:
+		if (pc->pc_remote == NULL)
+			return (-1);
+		if (pc->pc_remotelen != len)
+			return (-1);
+
+		memcpy(buf, p + DHCP_OPTION_HDR_LEN, len);
+		return (memcmp(pc->pc_remote, buf, len));
+
+	default:
+		/* Unmatched type */
+		note("unmatched relay info %d", *p);
+		return (0);
+	}
+}
+
 ssize_t
-relay_agentinfo(struct interface_info *info, struct dhcp_packet *packet,
-    size_t length, struct in_addr *from, struct in_addr *to)
+relay_agentinfo_append(struct packet_ctx *pc, struct dhcp_packet *dp,
+    size_t dplen)
 {
-	u_int8_t	*p;
-	u_int		 i, j, railen;
-	ssize_t		 optlen, maxlen, grow;
+	uint8_t		*p, *startp;
+	ssize_t		 newtotal = dplen;
+	int		 opttotal, optlen, i, hasinfo = 0;
+	int		 maxlen, neededlen;
+
+	/* Only append when enabled. */
+	if (oflag == 0)
+		return (dplen);
+
+	startp = (uint8_t *)dp;
+	p = (uint8_t *)&dp->options;
+	if (memcmp(p, DHCP_OPTIONS_COOKIE, DHCP_OPTIONS_COOKIE_LEN)) {
+		note("invalid dhcp options cookie");
+		return (-1);
+	}
+
+	p += DHCP_OPTIONS_COOKIE_LEN;
+	opttotal = dplen - DHCP_FIXED_NON_UDP - DHCP_OPTIONS_COOKIE_LEN;
+	maxlen = DHCP_MTU_MAX - DHCP_FIXED_LEN - DHCP_OPTIONS_COOKIE_LEN - 1;
+	if (maxlen < 1 || opttotal < 1)
+		return (dplen);
+
+	for (i = 0; i < opttotal && *p != DHO_END;) {
+		if (*p == DHO_PAD)
+			optlen = 1;
+		else
+			optlen = p[1] + DHCP_OPTION_HDR_LEN;
+
+		if ((i + optlen) > opttotal) {
+			note("truncated dhcp options");
+			return (-1);
+		}
 
-	if (!oflag)
-		return (length);
+		if (*p == DHO_RELAY_AGENT_INFORMATION) {
+			hasinfo = 1;
+			continue;
+		}
+
+		p += optlen;
+		i += optlen;
+
+		/* We reached the end, append the relay agent info. */
+		if (i < opttotal && *p == DHO_END) {
+			/* We already have the Relay Agent Info, skip it. */
+			if (hasinfo)
+				continue;
+
+			/* Calculate needed length to append new data. */
+			neededlen = newtotal + DHCP_OPTION_HDR_LEN;
+			if (pc->pc_circuitlen > 0)
+				neededlen += DHCP_OPTION_HDR_LEN +
+				    pc->pc_circuitlen;
+			if (pc->pc_remotelen > 0)
+				neededlen += DHCP_OPTION_HDR_LEN +
+				    pc->pc_remotelen;
+
+			/* Save one byte for DHO_END. */
+			neededlen += 1;
+
+			/* Check if we have enough space for the new options. */
+			if (neededlen > maxlen) {
+				warning("no space for relay agent info");
+				return (newtotal);
+			}
+
+			/* New option header: 2 bytes. */
+			newtotal += DHCP_OPTION_HDR_LEN;
+
+			*p++ = DHO_RELAY_AGENT_INFORMATION;
+			*p = 0;
+			if (pc->pc_circuitlen > 0) {
+				newtotal += DHCP_OPTION_HDR_LEN +
+				    pc->pc_circuitlen;
+				*p = (*p) + DHCP_OPTION_HDR_LEN +
+				    pc->pc_circuitlen;
+			}
+
+			if (pc->pc_remotelen > 0) {
+				newtotal += DHCP_OPTION_HDR_LEN +
+				    pc->pc_remotelen;
+				*p = (*p) + DHCP_OPTION_HDR_LEN +
+				    pc->pc_remotelen;
+			}
+
+			p++;
+
+			/* Sub-option circuit-id header plus value. */
+			if (pc->pc_circuitlen > 0) {
+				*p++ = RAI_CIRCUIT_ID;
+				*p++ = pc->pc_circuitlen;
+				memcpy(p, pc->pc_circuit, pc->pc_circuitlen);
+
+				p += pc->pc_circuitlen;
+			}
+
+			/* Sub-option remote-id header plus value. */
+			if (pc->pc_remotelen > 0) {
+				*p++ = RAI_REMOTE_ID;
+				*p++ = pc->pc_remotelen;
+				memcpy(p, pc->pc_remote, pc->pc_remotelen);
+
+				p += pc->pc_remotelen;
+			}
+
+			*p = DHO_END;
+		}
+	}
+
+	/* Zero the padding so we don't leak anything. */
+	p++;
+	if (p < (startp + maxlen))
+		memset(p, 0, (startp + maxlen) - p);
+
+	return (newtotal);
+}
+
+ssize_t
+relay_agentinfo_remove(struct packet_ctx *pc, struct dhcp_packet *dp,
+    size_t dplen)
+{
+	uint8_t		*p, *np, *startp, *endp;
+	int		 opttotal, optleft;
+	int		 suboptlen, optlen, i;
+	int		 maxlen, remaining, matched = 0;
+
+	startp = (uint8_t *)dp;
+	p = (uint8_t *)&dp->options;
+	if (memcmp(p, DHCP_OPTIONS_COOKIE, DHCP_OPTIONS_COOKIE_LEN)) {
+		note("invalid dhcp options cookie");
+		return (-1);
+	}
 
-	/* Buffer length vs. received packet length */
 	maxlen = DHCP_MTU_MAX - DHCP_FIXED_LEN - DHCP_OPTIONS_COOKIE_LEN - 1;
-	optlen = length - DHCP_FIXED_NON_UDP - DHCP_OPTIONS_COOKIE_LEN;
-	if (maxlen < 1 || optlen < 1)
-		return (length);
+	opttotal = dplen - DHCP_FIXED_NON_UDP - DHCP_OPTIONS_COOKIE_LEN;
+	optleft = opttotal;
 
-	if (memcmp(packet->options, DHCP_OPTIONS_COOKIE,
-	    DHCP_OPTIONS_COOKIE_LEN) != 0)
-		return (length);
-	p = packet->options + DHCP_OPTIONS_COOKIE_LEN;
+	p += DHCP_OPTIONS_COOKIE_LEN;
+	endp = p + opttotal;
 
-	for (i = 0; i < (u_int)optlen && *p != DHO_END;) {
+	for (i = 0; i < opttotal && *p != DHO_END;) {
 		if (*p == DHO_PAD)
-			j = 1;
+			optlen = 1;
 		else
-			j = p[1] + 2;
+			optlen = p[1] + DHCP_OPTION_HDR_LEN;
 
-		if ((i + j) > (u_int)optlen) {
-			warning("truncated dhcp options");
-			break;
+		if ((i + optlen) > opttotal) {
+			note("truncated dhcp options");
+			return (-1);
 		}
 
-		/* Revert any other relay agent information */
 		if (*p == DHO_RELAY_AGENT_INFORMATION) {
-			if (to != NULL) {
-				/* Check the relay agent information */
-				railen = 8 + sizeof(struct in_addr);
-				if (j >= railen &&
-				    p[1] == (railen - 2) &&
-				    p[2] == RAI_CIRCUIT_ID &&
-				    p[3] == 2 &&
-				    p[4] == (u_int8_t)(info->index << 8) &&
-				    p[5] == (info->index & 0xff) &&
-				    p[6] == RAI_REMOTE_ID &&
-				    p[7] == sizeof(*to))
-					memcpy(to, p + 8, sizeof(*to));
-
-				/* It should be the last option */
-				memset(p, 0, j);
-				*p = DHO_END;
-			} else {
-				/* Discard invalid option from a client */
-				if (!packet->giaddr.s_addr)
-					return (-1);
+			/* Fast case: there is no next option. */
+			np = p + optlen;
+			if (*np == DHO_END) {
+				*p = *np;
+				endp = p + 1;
+				/* Zero the padding so we don't leak data. */
+				if (endp < (startp + maxlen))
+					memset(endp, 0,
+					    (startp + maxlen) - endp);
+
+				return (dplen);
 			}
-			return (length);
-		}
 
-		p += j;
-		i += j;
+			remaining = optlen;
+			while (remaining > 0) {
+				suboptlen = *(p + 1);
+				remaining -= DHCP_OPTION_HDR_LEN + suboptlen;
 
-		if (from != NULL && (*p == DHO_END || (i >= optlen))) {
-			j = 8 + sizeof(*from);
-			if ((i + j) > (u_int)maxlen) {
-				warning("skipping agent information");
+				matched = 1;
+				if (relay_agentinfo_cmp(pc, p, suboptlen) == 0)
+					continue;
+
+				matched = 0;
 				break;
 			}
+			/* It is not ours Relay Agent Info, don't remove it. */
+			if (matched == 0)
+				break;
 
-			/* Append the relay agent information if it fits */
-			p[0] = DHO_RELAY_AGENT_INFORMATION;
-			p[1] = j - 2;
-			p[2] = RAI_CIRCUIT_ID;
-			p[3] = 2;
-			p[4] = info->index << 8;
-			p[5] = info->index & 0xff;
-			p[6] = RAI_REMOTE_ID;
-			p[7] = sizeof(*from);
-			memcpy(p + 8, from, sizeof(*from));
-
-			/* Do we need to increase the packet length? */
-			grow = j + 1 - (optlen - i);
-			if (grow > 0)
-				length += grow;
-			p += j;
+			/* Move the other options on top of this one. */
+			optleft -= optlen;
+			endp -= optlen;
 
-			*p = DHO_END;
-			break;
+			/* Replace the old agent relay info. */
+			memmove(p, dp, optleft);
+
+			endp++;
+			/* Zero the padding so we don't leak data. */
+			if (endp < (startp + maxlen))
+				memset(endp, 0,
+				    (startp + maxlen) - endp);
+
+			return (endp - startp);
 		}
+
+		p += optlen;
+		i += optlen;
+		optleft -= optlen;
 	}
 
-	return (length);
+	return (endp - startp);
 }
 
 int
@@ -599,3 +849,65 @@ get_rdomain(char *name)
 	close(s);
 	return rv;
 }
+
+void
+l2relay(struct interface_info *ip, struct dhcp_packet *dp, int length,
+    struct packet_ctx *pc)
+{
+	struct server_list	*sp;
+	ssize_t			 dplen;
+
+	if (dp->hlen > sizeof(dp->chaddr)) {
+		note("Discarding packet with invalid hlen.");
+		return;
+	}
+
+	relay_agentinfo(pc, ip);
+
+	switch (dp->op) {
+	case BOOTREQUEST:
+		/* Add the relay agent info asked by the user. */
+		if ((dplen = relay_agentinfo_append(pc, dp, length)) == -1)
+			return;
+
+		/*
+		 * Re-send the packet to every interface except the one
+		 * it came in.
+		 */
+		for (sp = servers; sp != NULL; sp = sp->next) {
+			if (sp->intf == ip)
+				continue;
+
+			debug("forwarded BOOTREQUEST for %s to %s",
+			    print_hw_addr(pc->pc_htype, pc->pc_hlen,
+			    pc->pc_smac), sp->intf->name);
+
+			send_packet(sp->intf, dp, dplen, pc);
+		}
+		if (ip != interfaces) {
+			debug("forwarded BOOTREQUEST for %s to %s",
+			    print_hw_addr(pc->pc_htype, pc->pc_hlen,
+			    pc->pc_smac), interfaces->name);
+
+			send_packet(interfaces, dp, dplen, pc);
+		}
+		break;
+
+	case BOOTREPLY:
+		/* Remove relay agent info on offer. */
+		if ((dplen = relay_agentinfo_remove(pc, dp, length)) == -1)
+			return;
+
+		if (ip != interfaces) {
+			debug("forwarded BOOTREPLY for %s to %s",
+			    print_hw_addr(pc->pc_htype, pc->pc_hlen,
+			    pc->pc_dmac), interfaces->name);
+			send_packet(interfaces, dp, dplen, pc);
+		}
+		break;
+
+	default:
+		debug("invalid operation type '%d'", dp->op);
+		return;
+	}
+}
diff --git a/usr.sbin/dhcrelay/dispatch.c b/usr.sbin/dhcrelay/dispatch.c
index d212969158d..5274556e208 100644
--- a/usr.sbin/dhcrelay/dispatch.c
+++ b/usr.sbin/dhcrelay/dispatch.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: dispatch.c,v 1.14 2016/12/08 19:18:15 rzalamena Exp $	*/
+/*	$OpenBSD: dispatch.c,v 1.15 2016/12/12 15:41:05 rzalamena Exp $	*/
 
 /*
  * Copyright 2004 Henning Brauer <henning@openbsd.org>
@@ -74,7 +74,8 @@ void (*bootp_packet_handler)(struct interface_info *,
 static int interface_status(struct interface_info *ifinfo);
 
 struct interface_info *
-get_interface(const char *ifname, void (*handler)(struct protocol *))
+get_interface(const char *ifname, void (*handler)(struct protocol *),
+    int isserver)
 {
 	struct interface_info		*iface;
 	struct ifaddrs			*ifap, *ifa;
@@ -145,7 +146,7 @@ get_interface(const char *ifname, void (*handler)(struct protocol *))
 		error("interface name '%s' too long", ifname);
 
 	/* Register the interface... */
-	if_register_receive(iface);
+	if_register_receive(iface, isserver);
 	if_register_send(iface);
 	add_protocol(iface->name, iface->rfdesc, handler, iface);