2 files changed, 63 insertions, 4 deletions

diff --git a/sys/netinet/in.h b/sys/netinet/in.h
index 09fb134fa06f..09fd395f76ef 100644
--- a/sys/netinet/in.h
+++ b/sys/netinet/in.h
@@ -366,6 +366,7 @@ __END_DECLS
 #define	IP_RECVOPTS		5    /* bool; receive all IP opts w/dgram */
 #define	IP_RECVRETOPTS		6    /* bool; receive IP opts for response */
 #define	IP_RECVDSTADDR		7    /* bool; receive IP dst addr w/dgram */
+#define	IP_SENDSRCADDR		IP_RECVDSTADDR /* cmsg_type to set src addr */
 #define	IP_RETOPTS		8    /* ip_opts; set/get IP options */
 #define	IP_MULTICAST_IF		9    /* u_char; set/get IP multicast i/f  */
 #define	IP_MULTICAST_TTL	10   /* u_char; set/get IP multicast ttl */
diff --git a/sys/netinet/udp_usrreq.c b/sys/netinet/udp_usrreq.c
index 263a92e09703..437ee4e9012c 100644
--- a/sys/netinet/udp_usrreq.c
+++ b/sys/netinet/udp_usrreq.c
@@ -689,7 +689,8 @@ udp_output(inp, m, addr, control, td)
 	register struct udpiphdr *ui;
 	register int len = m->m_pkthdr.len;
 	struct in_addr faddr, laddr;
-	struct sockaddr_in *sin;
+	struct cmsghdr *cm;
+	struct sockaddr_in *sin, src;
 	int error = 0;
 	u_short fport, lport;
 
@@ -697,16 +698,73 @@ udp_output(inp, m, addr, control, td)
 	mac_create_mbuf_from_socket(inp->inp_socket, m);
 #endif
 
-	if (control)
-		m_freem(control);		/* XXX */
-
 	if (len + sizeof(struct udpiphdr) > IP_MAXPACKET) {
 		error = EMSGSIZE;
+		if (control)
+			m_freem(control);
 		goto release;
 	}
 
+	src.sin_addr.s_addr = INADDR_ANY;
+	if (control != NULL) {
+		/*
+		 * XXX: Currently, we assume all the optional information
+		 * is stored in a single mbuf.
+		 */
+		if (control->m_next) {
+			error = EINVAL;
+			m_freem(control);
+			goto release;
+		}
+		for (; control->m_len > 0;
+		    control->m_data += CMSG_ALIGN(cm->cmsg_len),
+		    control->m_len -= CMSG_ALIGN(cm->cmsg_len)) {
+			cm = mtod(control, struct cmsghdr *);
+			if (control->m_len < sizeof(*cm) || cm->cmsg_len == 0 ||
+			    cm->cmsg_len > control->m_len) {
+				error = EINVAL;
+				break;
+			}
+			if (cm->cmsg_level != IPPROTO_IP)
+				continue;
+
+			switch (cm->cmsg_type) {
+			case IP_SENDSRCADDR:
+				if (cm->cmsg_len !=
+				    CMSG_LEN(sizeof(struct in_addr))) {
+					error = EINVAL;
+					break;
+				}
+				bzero(&src, sizeof(src));
+				src.sin_family = AF_INET;
+				src.sin_len = sizeof(src);
+				src.sin_port = inp->inp_lport;
+				src.sin_addr = *(struct in_addr *)CMSG_DATA(cm);
+				break;
+			default:
+				error = ENOPROTOOPT;
+				break;
+			}
+			if (error)
+				break;
+		}
+		m_freem(control);
+	}
+	if (error)
+		goto release;
 	laddr = inp->inp_laddr;
 	lport = inp->inp_lport;
+	if (src.sin_addr.s_addr != INADDR_ANY) {
+		if (lport == 0) {
+			error = EINVAL;
+			goto release;
+		}
+		error = in_pcbbind_setup(inp, (struct sockaddr *)&src,
+		    &laddr.s_addr, &lport, td);
+		if (error)
+			goto release;
+	}
+
 	if (addr) {
 		sin = (struct sockaddr_in *)addr;
 		if (td && jailed(td->td_ucred))