Index: sys/netinet/in.h
===================================================================
--- sys/netinet/in.h
+++ sys/netinet/in.h
@@ -432,6 +432,8 @@
 
 #define	IP_ONESBCAST		23   /* bool: send all-ones broadcast */
 #define	IP_BINDANY		24   /* bool: allow bind to any address */
+#define	IP_ORIGDSTADDR		25   /* bool: receive IP dst addr/port w/dgram */
+#define	IP_RECVORIGDSTADDR	IP_ORIGDSTADDR
 
 /*
  * Options for controlling the firewall and dummynet.
Index: sys/netinet/in_pcb.h
===================================================================
--- sys/netinet/in_pcb.h
+++ sys/netinet/in_pcb.h
@@ -546,6 +546,7 @@
 #define	INP_REUSEPORT		0x00000008 /* SO_REUSEPORT option is set */
 #define	INP_FREED		0x00000010 /* inp itself is not valid */
 #define	INP_REUSEADDR		0x00000020 /* SO_REUSEADDR option is set */
+#define	INP_ORIGDSTADDR		0x00000040 /* receive IP dst address/port */
 
 /*
  * Flags passed to in_pcblookup*() functions.
Index: sys/netinet/in_pcb.c
===================================================================
--- sys/netinet/in_pcb.c
+++ sys/netinet/in_pcb.c
@@ -2292,6 +2292,10 @@
 		db_printf("%sINP_RECVDSTADDR", comma ? ", " : "");
 		comma = 1;
 	}
+	if (inp_flags & INP_ORIGDSTADDR) {
+		db_printf("%sINP_ORIGDSTADDR", comma ? ", " : "");
+		comma = 1;
+	}
 	if (inp_flags & INP_HDRINCL) {
 		db_printf("%sINP_HDRINCL", comma ? ", " : "");
 		comma = 1;
Index: sys/netinet/ip_output.c
===================================================================
--- sys/netinet/ip_output.c
+++ sys/netinet/ip_output.c
@@ -986,6 +986,7 @@
 		case IP_MINTTL:
 		case IP_RECVOPTS:
 		case IP_RECVRETOPTS:
+		case IP_ORIGDSTADDR:
 		case IP_RECVDSTADDR:
 		case IP_RECVTTL:
 		case IP_RECVIF:
@@ -1035,6 +1036,15 @@
 				OPTSET(INP_RECVDSTADDR);
 				break;
 
+			case IP_ORIGDSTADDR:
+				INP_WLOCK(inp);
+				if (optval)
+					inp->inp_flags2 |= INP_ORIGDSTADDR;
+				else
+					inp->inp_flags2 &= ~INP_ORIGDSTADDR;
+				INP_WUNLOCK(inp);
+				break;
+
 			case IP_RECVTTL:
 				OPTSET(INP_RECVTTL);
 				break;
@@ -1160,6 +1170,7 @@
 		case IP_MINTTL:
 		case IP_RECVOPTS:
 		case IP_RECVRETOPTS:
+		case IP_ORIGDSTADDR:
 		case IP_RECVDSTADDR:
 		case IP_RECVTTL:
 		case IP_RECVIF:
@@ -1197,6 +1208,10 @@
 				optval = OPTBIT(INP_RECVDSTADDR);
 				break;
 
+			case IP_ORIGDSTADDR:
+				optval = inp->inp_flags2 & INP_ORIGDSTADDR;
+				break;
+
 			case IP_RECVTTL:
 				optval = OPTBIT(INP_RECVTTL);
 				break;
Index: sys/netinet/udp_usrreq.c
===================================================================
--- sys/netinet/udp_usrreq.c
+++ sys/netinet/udp_usrreq.c
@@ -290,7 +290,7 @@
 {
 	struct sockaddr *append_sa;
 	struct socket *so;
-	struct mbuf *opts = 0;
+	struct mbuf *tmpopts, *opts = 0;
 #ifdef INET6
 	struct sockaddr_in6 udp_in6;
 #endif
@@ -303,7 +303,7 @@
 	 */
 	up = intoudpcb(inp);
 	if (up->u_tun_func != NULL) {
-		(*up->u_tun_func)(n, off, inp, (struct sockaddr *)udp_in,
+		(*up->u_tun_func)(n, off, inp, (struct sockaddr *)&udp_in[0],
 		    up->u_tun_ctx);
 		return;
 	}
@@ -342,16 +342,27 @@
 #endif /* INET6 */
 			ip_savecontrol(inp, &opts, ip, n);
 	}
+	if (inp->inp_vflag & INP_IPV4 && inp->inp_flags2 & INP_ORIGDSTADDR) {
+		tmpopts = sbcreatecontrol((caddr_t)&udp_in[1],
+			sizeof(struct sockaddr_in), IP_ORIGDSTADDR, IPPROTO_IP);
+		if (tmpopts) {
+			if (opts) {
+				tmpopts->m_next = opts;
+				opts = tmpopts;
+			} else
+				opts = tmpopts;
+		}
+	}
 #ifdef INET6
 	if (inp->inp_vflag & INP_IPV6) {
 		bzero(&udp_in6, sizeof(udp_in6));
 		udp_in6.sin6_len = sizeof(udp_in6);
 		udp_in6.sin6_family = AF_INET6;
-		in6_sin_2_v4mapsin6(udp_in, &udp_in6);
+		in6_sin_2_v4mapsin6(&udp_in[0], &udp_in6);
 		append_sa = (struct sockaddr *)&udp_in6;
 	} else
 #endif /* INET6 */
-		append_sa = (struct sockaddr *)udp_in;
+		append_sa = (struct sockaddr *)&udp_in[0];
 	m_adj(n, off);
 
 	so = inp->inp_socket;
@@ -377,7 +388,7 @@
 	uint16_t len, ip_len;
 	struct inpcbinfo *pcbinfo;
 	struct ip save_ip;
-	struct sockaddr_in udp_in;
+	struct sockaddr_in udpin[2];
 	struct m_tag *fwd_tag;
 	int cscov_partial;
 	uint8_t pr;
@@ -420,11 +431,16 @@
 	 * Construct sockaddr format source address.  Stuff source address
 	 * and datagram in user buffer.
 	 */
-	bzero(&udp_in, sizeof(udp_in));
-	udp_in.sin_len = sizeof(udp_in);
-	udp_in.sin_family = AF_INET;
-	udp_in.sin_port = uh->uh_sport;
-	udp_in.sin_addr = ip->ip_src;
+	bzero(&udpin[0], sizeof(struct sockaddr_in));
+	udpin[0].sin_len = sizeof(struct sockaddr_in);
+	udpin[0].sin_family = AF_INET;
+	udpin[0].sin_port = uh->uh_sport;
+	udpin[0].sin_addr = ip->ip_src;
+	bzero(&udpin[1], sizeof(struct sockaddr_in));
+	udpin[1].sin_len = sizeof(struct sockaddr_in);
+	udpin[1].sin_family = AF_INET;
+	udpin[1].sin_port = uh->uh_dport;
+	udpin[1].sin_addr = ip->ip_dst;
 
 	/*
 	 * Make mbuf data length reflect UDP length.  If not enough data to
@@ -552,7 +568,7 @@
 
 				blocked = imo_multi_filter(imo, ifp,
 					(struct sockaddr *)&group,
-					(struct sockaddr *)&udp_in);
+					(struct sockaddr *)&udpin[0]);
 				if (blocked != MCAST_PASS) {
 					if (blocked == MCAST_NOTGMEMBER)
 						IPSTAT_INC(ips_notmember);
@@ -570,7 +586,7 @@
 					UDP_PROBE(receive, NULL, last, ip,
 					    last, uh);
 					udp_append(last, ip, n, iphlen,
-					    &udp_in);
+					    udpin);
 				}
 				INP_RUNLOCK(last);
 			}
@@ -601,7 +617,7 @@
 			goto badunlocked;
 		}
 		UDP_PROBE(receive, NULL, last, ip, last, uh);
-		udp_append(last, ip, m, iphlen, &udp_in);
+		udp_append(last, ip, m, iphlen, udpin);
 		INP_RUNLOCK(last);
 		INP_INFO_RUNLOCK(pcbinfo);
 		return;
@@ -690,7 +706,7 @@
 	}
 
 	UDP_PROBE(receive, NULL, inp, ip, inp, uh);
-	udp_append(inp, ip, m, iphlen, &udp_in);
+	udp_append(inp, ip, m, iphlen, udpin);
 	INP_RUNLOCK(inp);
 	return;
 
Index: sys/netinet6/in6.h
===================================================================
--- sys/netinet6/in6.h
+++ sys/netinet6/in6.h
@@ -481,6 +481,9 @@
 
 #define	IPV6_BINDANY		64 /* bool: allow bind to any address */
 
+#define	IPV6_ORIGDSTADDR	65 /* bool: allow getting dstaddr /port info */
+#define	IPV6_RECVORIGDSTADDR	IPV6_ORIGDSTADDR
+
 /*
  * The following option is private; do not use it from user applications.
  * It is deliberately defined to the same value as IP_MSFILTER.
Index: sys/netinet6/in6_pcb.h
===================================================================
--- sys/netinet6/in6_pcb.h
+++ sys/netinet6/in6_pcb.h
@@ -118,7 +118,7 @@
 int	in6_mapped_peeraddr(struct socket *so, struct sockaddr **nam);
 int	in6_selecthlim(struct in6pcb *, struct ifnet *);
 int	in6_pcbsetport(struct in6_addr *, struct inpcb *, struct ucred *);
-void	init_sin6(struct sockaddr_in6 *sin6, struct mbuf *m);
+void	init_sin6(struct sockaddr_in6 *sin6, struct mbuf *m, int);
 #endif /* _KERNEL */
 
 #endif /* !_NETINET6_IN6_PCB_H_ */
Index: sys/netinet6/in6_pcb.c
===================================================================
--- sys/netinet6/in6_pcb.c
+++ sys/netinet6/in6_pcb.c
@@ -1182,7 +1182,7 @@
 }
 
 void
-init_sin6(struct sockaddr_in6 *sin6, struct mbuf *m)
+init_sin6(struct sockaddr_in6 *sin6, struct mbuf *m, int srcordst)
 {
 	struct ip6_hdr *ip;
 
@@ -1190,7 +1190,7 @@
 	bzero(sin6, sizeof(*sin6));
 	sin6->sin6_len = sizeof(*sin6);
 	sin6->sin6_family = AF_INET6;
-	sin6->sin6_addr = ip->ip6_src;
+	sin6->sin6_addr = srcordst ? ip->ip6_dst : ip->ip6_src;
 
 	(void)sa6_recoverscope(sin6); /* XXX: should catch errors... */
 
Index: sys/netinet6/ip6_output.c
===================================================================
--- sys/netinet6/ip6_output.c
+++ sys/netinet6/ip6_output.c
@@ -1395,6 +1395,7 @@
 			case IPV6_RECVTCLASS:
 			case IPV6_V6ONLY:
 			case IPV6_AUTOFLOWLABEL:
+			case IPV6_ORIGDSTADDR:
 			case IPV6_BINDANY:
 				if (optname == IPV6_BINDANY && td != NULL) {
 					error = priv_check(td,
@@ -1560,6 +1561,14 @@
 					OPTSET(IN6P_AUTOFLOWLABEL);
 					break;
 
+				case IPV6_ORIGDSTADDR:
+					INP_WLOCK(in6p);
+					if (optval)
+						in6p->inp_flags2 |= INP_ORIGDSTADDR;
+					else
+						in6p->inp_flags2 &= ~INP_ORIGDSTADDR;
+					INP_WUNLOCK(in6p);
+					break;
 				case IPV6_BINDANY:
 					OPTSET(INP_BINDANY);
 					break;
@@ -1840,6 +1849,10 @@
 					optval = OPTBIT(IN6P_AUTOFLOWLABEL);
 					break;
 
+				case IPV6_ORIGDSTADDR:
+					optval = in6p->inp_flags2 & INP_ORIGDSTADDR;
+					break;
+
 				case IPV6_BINDANY:
 					optval = OPTBIT(INP_BINDANY);
 					break;
Index: sys/netinet6/raw_ip6.c
===================================================================
--- sys/netinet6/raw_ip6.c
+++ sys/netinet6/raw_ip6.c
@@ -174,7 +174,7 @@
 		return (IPPROTO_DONE);
 	}
 
-	init_sin6(&fromsa, m); /* general init */
+	init_sin6(&fromsa, m, 0); /* general init */
 
 	ifp = m->m_pkthdr.rcvif;
 
Index: sys/netinet6/udp6_usrreq.c
===================================================================
--- sys/netinet6/udp6_usrreq.c
+++ sys/netinet6/udp6_usrreq.c
@@ -138,7 +138,7 @@
     struct sockaddr_in6 *fromsa)
 {
 	struct socket *so;
-	struct mbuf *opts;
+	struct mbuf *opts = NULL, *tmp_opts;
 	struct udpcb *up;
 
 	INP_LOCK_ASSERT(inp);
@@ -148,7 +148,7 @@
 	 */
 	up = intoudpcb(inp);
 	if (up->u_tun_func != NULL) {
-		(*up->u_tun_func)(n, off, inp, (struct sockaddr *)fromsa,
+		(*up->u_tun_func)(n, off, inp, (struct sockaddr *)&fromsa[0],
 		    up->u_tun_ctx);
 		return;
 	}
@@ -170,11 +170,23 @@
 	if (inp->inp_flags & INP_CONTROLOPTS ||
 	    inp->inp_socket->so_options & SO_TIMESTAMP)
 		ip6_savecontrol(inp, n, &opts);
+	if (inp->inp_vflag & INP_IPV6 && inp->inp_flags2 & INP_ORIGDSTADDR) {
+		tmp_opts = sbcreatecontrol((caddr_t)&fromsa[1],
+                        sizeof(struct sockaddr_in6), IP_ORIGDSTADDR, IPPROTO_IPV6);
+                if (tmp_opts) {
+                        if (opts) {
+                                tmp_opts->m_next = opts;
+                                opts = tmp_opts;
+                        } else
+                                opts = tmp_opts;
+                }
+
+	}
 	m_adj(n, off + sizeof(struct udphdr));
 
 	so = inp->inp_socket;
 	SOCKBUF_LOCK(&so->so_rcv);
-	if (sbappendaddr_locked(&so->so_rcv, (struct sockaddr *)fromsa, n,
+	if (sbappendaddr_locked(&so->so_rcv, (struct sockaddr *)&fromsa[0], n,
 	    opts) == 0) {
 		SOCKBUF_UNLOCK(&so->so_rcv);
 		m_freem(n);
@@ -198,7 +210,7 @@
 	int off = *offp;
 	int cscov_partial;
 	int plen, ulen;
-	struct sockaddr_in6 fromsa;
+	struct sockaddr_in6 fromsa[2];
 	struct m_tag *fwd_tag;
 	uint16_t uh_sum;
 	uint8_t nxt;
@@ -279,8 +291,10 @@
 	/*
 	 * Construct sockaddr format source address.
 	 */
-	init_sin6(&fromsa, m);
-	fromsa.sin6_port = uh->uh_sport;
+	init_sin6(&fromsa[0], m, 0);
+	fromsa[0].sin6_port = uh->uh_sport;
+	init_sin6(&fromsa[1], m, 1);
+	fromsa[1].sin6_port = uh->uh_dport;
 
 	pcbinfo = get_inpcbinfo(nxt);
 	if (IN6_IS_ADDR_MULTICAST(&ip6->ip6_dst)) {
@@ -351,7 +365,7 @@
 
 				blocked = im6o_mc_filter(imo, ifp,
 					(struct sockaddr *)&mcaddr,
-					(struct sockaddr *)&fromsa);
+					(struct sockaddr *)&fromsa[0]);
 				if (blocked != MCAST_PASS) {
 					if (blocked == MCAST_NOTGMEMBER)
 						IP6STAT_INC(ip6s_notmember);
@@ -371,7 +385,7 @@
 					INP_RLOCK(last);
 					UDP_PROBE(receive, NULL, last, ip6,
 					    last, uh);
-					udp6_append(last, n, off, &fromsa);
+					udp6_append(last, n, off, fromsa);
 					INP_RUNLOCK(last);
 				}
 			}
@@ -402,7 +416,7 @@
 		INP_RLOCK(last);
 		INP_INFO_RUNLOCK(pcbinfo);
 		UDP_PROBE(receive, NULL, last, ip6, last, uh);
-		udp6_append(last, m, off, &fromsa);
+		udp6_append(last, m, off, fromsa);
 		INP_RUNLOCK(last);
 		return (IPPROTO_DONE);
 	}
@@ -481,7 +495,7 @@
 		}
 	}
 	UDP_PROBE(receive, NULL, inp, ip6, inp, uh);
-	udp6_append(inp, m, off, &fromsa);
+	udp6_append(inp, m, off, fromsa);
 	INP_RUNLOCK(inp);
 	return (IPPROTO_DONE);