--- rtems-udp_usrreq.c	Thu Aug 20 17:47:37 1998
+++ udp_usrreq.c	Wed Jul 10 14:00:06 2002
@@ -31,7 +31,7 @@
  * SUCH DAMAGE.
  *
  *	@(#)udp_usrreq.c	8.6 (Berkeley) 5/23/95
- *	$Id: udp_usrreq.c,v 1.2 1998/08/20 21:47:37 joel Exp $
+ *	$Id: udp_usrreq.c,v 1.11 2002/07/09 09:10:28 cvs Exp $
  */
 
 #include <sys/param.h>
@@ -49,6 +49,7 @@
 #include <sys/syslog.h>
 
 #include <net/if.h>
+#include <net/pf.h>
 #include <net/route.h>
 
 #include <netinet/in.h>
@@ -60,6 +61,10 @@
 #include <netinet/ip_icmp.h>
 #include <netinet/udp.h>
 #include <netinet/udp_var.h>
+#include <time.h>
+
+#undef malloc
+#undef free
 
 /*
  * UDP protocol implementation.
@@ -71,11 +76,50 @@
 static int	udpcksum = 0;		/* XXX */
 #endif
 SYSCTL_INT(_net_inet_udp, UDPCTL_CHECKSUM, checksum, CTLFLAG_RW,
-		&udpcksum, 0, "");
+		&udpcksum, 0, "Calculate UDP checksum");
+		
+
+#ifdef FORWARD_PROTOCOL
+
+/*
+ * To implement udp broadcast forwarding we should check ``broadcast storm''
+ * condition that can be caused if there is alternate ways between two subnets
+ * and efficiently cut such loops. This can be done by caching sensetive packet
+ * information and performing tests on it:
+ * 1. If we got packet that is not in our cache we pass this packet and
+ *    add it into cache. If there is no room in cache - LRU packet is discarded
+ * 2. If we got packet that already in our cache we make it MRU and:          
+ *    We check ttl of ip packet - if it same or greater as in cache we just 
+ *    pass it. If it less - just drop this packet.
+ */
+
+#define MAXUDPCACHE  10  /* Seems to be reasonable size */
+#define UDPMAXEXPIRE 30  /* 30 sec expire cache         */
+
+struct packet_cache {
+    unsigned8      pc_ttl;   /* IP packet TTL */
+    unsigned16     pc_sum;   /* UDP packet checksum */
+    struct in_addr pc_src;   /* IP packet source address*/
+    struct udphdr  pc_uh;    /* UDP packet header */
+    time_t  pc_time;         /* Expiry value */
+    struct packet_cache *next , *prev;
+    
+};
+
+static struct packet_cache *udp_cache = NULL;
+
+#endif
+
+	
+static u_long	udp_sendspace = 9216;		/* really max datagram size */
+					/* 40 1K datagrams */
+SYSCTL_INT(_net_inet_udp, UDPCTL_MAXDGRAM, maxdgram, CTLFLAG_RW,
+	&udp_sendspace, 0, "Maximum UDP datagram size");
 
-static int log_in_vain = 0;
-SYSCTL_INT(_net_inet_udp, OID_AUTO, log_in_vain, CTLFLAG_RW, 
-	&log_in_vain, 0, "");
+static u_long	udp_recvspace = 40 * (1024 + sizeof(struct sockaddr_in));
+SYSCTL_INT(_net_inet_udp, UDPCTL_RECVSPACE, recvspace, CTLFLAG_RW,
+	&udp_recvspace, 0, "Maximum UDP receive buffer size");
+	
 
 static struct	inpcbhead udb;		/* from udp_var.h */
 static struct	inpcbinfo udbinfo;
@@ -84,9 +128,13 @@
 #define UDBHASHSIZE 64
 #endif
 
-       struct	udpstat udpstat;	/* from udp_var.h */
-SYSCTL_STRUCT(_net_inet_udp, UDPCTL_STATS, stats, CTLFLAG_RD,
-	&udpstat, udpstat, "");
+/*
+ * UDP statistics
+ */
+struct udpstat udpstat;
+
+SYSCTL_STRUCT(_net_inet_udp, UDPCTL_STATS, stats, CTLFLAG_RW,
+	&udpstat, udpstat, "UDP statistic");
 
 static struct	sockaddr_in udp_in = { sizeof(udp_in), AF_INET };
 
@@ -95,14 +143,218 @@
 			    struct mbuf *));
 static	void udp_notify __P((struct inpcb *, int));
 
+
+/* Added by Vasilkov. This system call is used by snmp agent code.
+ *
+ */
+void udp_get_struct_udb (struct inpcbhead * strudb)
+{
+	memcpy ((char*)strudb, (char*)&udb, sizeof(struct inpcbhead));
+}
+
+
+/*
+ * Register sysctl's
+ */
+void 
+sysctl_register_udp_usrreq() {
+
+	sysctl_register(_net_inet_udp,checksum);
+	sysctl_register(_net_inet_udp,maxdgram);
+	sysctl_register(_net_inet_udp,stats);
+	sysctl_register(_net_inet_udp,recvspace);
+}
+
 void
 udp_init()
 {
 	LIST_INIT(&udb);
 	udbinfo.listhead = &udb;
 	udbinfo.hashbase = hashinit(UDBHASHSIZE, M_PCB, &udbinfo.hashmask);
+	
 }
 
+#ifdef FORWARD_PROTOCOL
+static unsigned char udp_ports[65536/8];
+
+/*
+ * Enable/Disable udp port forwarding
+ */
+int udp_forward_port(int port,int forward) {
+
+    int byte = port/8;
+    int offset = port%8;
+    
+    if (forward)
+	udp_ports[byte] |= (0x80 >> offset);
+    else
+	udp_ports[byte] &= ~(0x80 >> offset);
+	
+    return 0;
+}
+
+/*
+ * Check if port should be forwarded
+ */
+static int udp_if_forward(int port) {
+
+    int byte = port/8;
+    int offset = port%8;
+    
+    return (udp_ports[byte] & (0x80 >> offset));
+
+}
+
+/*
+ * Get packet_cache from mbuf
+ */
+static int udp_pc_from_m(struct packet_cache *pc, struct mbuf *m) {
+
+    struct ip *iph = mtod(m,struct ip *);
+    struct udphdr *uh = (struct udphdr *)((char *)iph + sizeof(struct ip));
+    
+    pc->pc_ttl  = iph->ip_ttl;
+    pc->pc_sum  = uh->uh_sum;
+    pc->pc_src  = iph->ip_src;
+    pc->pc_uh   = *uh;
+    pc->pc_time = time(NULL) + UDPMAXEXPIRE;
+    
+    return 0;
+}
+
+/*
+ * Make cache entry MRU
+ */
+static void udp_make_mru(struct packet_cache *pc) {
+
+    if (pc == udp_cache)
+	return ;
+    
+    /* MRU it */
+    if (pc->prev) pc->prev->next = pc->next;
+    if (pc->next) pc->next->prev = pc->prev;
+    
+    pc->prev = NULL;
+    pc->next = udp_cache;
+    udp_cache->prev = pc;
+    udp_cache = pc;
+    
+    pc->pc_time = time(NULL) + UDPMAXEXPIRE;
+    
+    /*
+     * HUGE FIXME: pc_sum should be strong checksum of udp data. md5 seems 
+     * to be ok.
+     */
+    
+}
+
+
+/*
+ * 
+ */
+#define UDP_PASS 0
+#define UDP_DROP 1
+
+static int udp_analyze(struct mbuf *m) {
+
+    time_t now;
+    struct packet_cache *pc,my,*empty = NULL;
+    
+    
+    /*
+     * If still no cache allocated - allocate it
+     */
+    if (!udp_cache) {
+	int i;
+	
+	for (i=0;i<MAXUDPCACHE;i++) {
+	    struct packet_cache *pc = malloc(sizeof(struct packet_cache));
+	    if (pc) {
+		memset(pc,0,sizeof(struct packet_cache));
+		pc->next = udp_cache;
+		if (udp_cache) udp_cache->prev = pc;
+		udp_cache = pc;
+	    }
+	}
+    }
+    
+    /*
+     * If no memory - just drop packet
+     */
+    if (!udp_cache)
+	return UDP_DROP;
+    
+    pc = udp_cache;
+    now = time(NULL);
+    
+    udp_pc_from_m(&my,m);
+    
+    if (my.pc_ttl <= IPTTLDEC) 
+	return UDP_DROP;
+    
+    while( pc ) {
+    
+	if (pc->pc_ttl)	{ /*Non-empty entry*/
+	    if (pc->pc_time < now) {
+		pc->pc_ttl = 0;
+		empty = pc;
+		printf("Entry expired :%s, sum %lu\n",inet_ntoa(pc->pc_src),pc->pc_sum);
+	    }
+	    else {
+		if ((pc->pc_sum == my.pc_sum) &&
+		    (pc->pc_src.s_addr == my.pc_src.s_addr) &&
+		    (pc->pc_uh.uh_dport == my.pc_uh.uh_dport) &&
+		    (pc->pc_uh.uh_ulen == my.pc_uh.uh_ulen) &&
+		    (pc->pc_uh.uh_sport == my.pc_uh.uh_sport)) {
+		    
+			printf("Cache HIT\n");
+		    
+			udp_make_mru(pc);
+			if (pc->pc_ttl <= my.pc_ttl) 
+			    return UDP_PASS;
+			
+			printf("Loop detected!\n");
+			return UDP_DROP;
+		    }
+		    
+	    }
+	}
+	else
+	    empty = pc;
+    
+	pc = pc->next;
+    }
+    
+    /*
+     * If no free entry in cache - remove LRU entry
+     */
+    if (!empty) {
+    
+	printf("Cache full, removing LRU\n");
+	empty = udp_cache;
+	while(empty->next) 
+	    empty = empty->next;
+	
+    }
+    
+    /* Cache it and make MRU */
+    printf("Caching packet\n");
+    udp_make_mru(empty);
+    
+    empty->pc_ttl  = my.pc_ttl;
+    empty->pc_sum  = my.pc_sum;
+    empty->pc_src  = my.pc_src;
+    empty->pc_uh   = my.pc_uh;
+    empty->pc_time = my.pc_time;
+
+    return UDP_PASS;
+}
+
+
+
+#endif /* FORWARD_PROTOCOL */
+
+
 void
 udp_input(m, iphlen)
 	register struct mbuf *m;
@@ -111,9 +363,24 @@
 	register struct ip *ip;
 	register struct udphdr *uh;
 	register struct inpcb *inp;
-	struct mbuf *opts = 0;
+	struct mbuf *opts = 0, *fwd = 0;
 	int len;
 	struct ip save_ip;
+	struct ifnet *ifp = m->m_pkthdr.rcvif;
+	int log_in_vain = 0;
+	int blackhole = 0;
+
+	/*
+	 * Fetch logging flag from interface
+	 */	
+	if (ifp->if_ip.ifi_udp & IFNET_UDP_LOG_IN_VAIN)
+	    log_in_vain = 1;
+	    
+	/*
+	 * Check if we should silently discard refused connects
+	 */
+	if (ifp->if_ip.ifi_udp & IFNET_UDP_BLACKHOLE)
+	    blackhole = 1;
 
 	udpstat.udps_ipackets++;
 
@@ -139,7 +406,19 @@
 		}
 		ip = mtod(m, struct ip *);
 	}
-	uh = (struct udphdr *)((caddr_t)ip + iphlen);
+        uh = (struct udphdr *)((caddr_t)ip + iphlen);
+
+#ifdef FORWARD_PROTOCOL
+        /*
+         * Do local copy of mbuf
+         */
+        if (udp_if_forward(ntohs(uh->uh_dport))) {
+	
+            fwd = m_copypacket(m,M_DONTWAIT);
+	}
+#endif	
+
+        
 
 	/*
 	 * Make mbuf data length reflect UDP length.
@@ -172,6 +451,8 @@
 		if (uh->uh_sum) {
 			udpstat.udps_badsum++;
 			m_freem(m);
+			if (fwd) 
+			    m_freem(fwd);
 			return;
 		}
 	}
@@ -179,6 +460,116 @@
 	if (IN_MULTICAST(ntohl(ip->ip_dst.s_addr)) ||
 	    in_broadcast(ip->ip_dst, m->m_pkthdr.rcvif)) {
 		struct inpcb *last;
+		
+#ifdef FORWARD_PROTOCOL		
+		/*
+		 * If our router configured to route broadcasts (this may
+		 * be required to enable NetBIOS thru router) 
+                 */
+
+                if (fwd) {
+                    /*
+                     * For each interface that allow directed broadcast
+                     * we should reflect this packet with destanation address
+                     * equal to interface subnet broadcast address
+                     */
+                    struct ifnet *ifp = ifnet;
+                    struct mbuf *tmp;
+		    struct ip *ip = mtod(fwd,struct ip *);
+		    
+		    *ip = save_ip;
+		    uh->uh_sum = in_cksum(m, len + sizeof (struct ip));
+		    
+		    
+		    if (udp_analyze(fwd) == UDP_DROP) {
+#ifdef FORWARD_DEBUG
+			printf("UDP DROP <%s:%d>, ttl=%d, sum=%lu\n",inet_ntoa(ip->ip_src),uh->uh_sport,ip->ip_ttl,uh->uh_sum);
+			goto bad;
+#endif		    
+		    }
+		    
+		    printf("UDP PASS <%s:%d>, ttl=%d, sum=%lu\n",inet_ntoa(ip->ip_src),uh->uh_sport,ip->ip_ttl,uh->uh_sum);
+		    
+		    uh->uh_sum = 0; /* Ugly hack */
+		    
+		    ip->ip_len += iphlen;
+		    ip->ip_ttl -= IPTTLDEC;  
+		    
+		    
+                    while (ifp) {
+		    
+                        if ((ifp != fwd->m_pkthdr.rcvif) &&
+			    !(ifp->if_flags & IFF_LOOPBACK) /*&& 
+			    (ifp->if_ip.ifi_udp & IFNET_UDP_FORWARD_PROTOCOL)*/) {
+			    
+                            struct ifaddr *ifa = ifp->if_addrlist;
+			    
+#ifdef FORWARD_DEBUG			    
+			    printf("\tForwarding through %s%d\n",ifp->if_name,ifp->if_unit);
+#endif			    
+			    
+                            while(ifa) {
+				int error;
+				
+				struct ip *ip = mtod(fwd, struct ip *);
+				
+				if  (ifa->ifa_addr->sa_family == AF_INET) {
+		    
+                            	    struct route ro;
+				
+				    if (ifp->if_flags | IFF_BROADCAST) {
+				    	    if (ifa->ifa_dstaddr)
+					    ip->ip_dst.s_addr = ((struct sockaddr_in *)(ifa->ifa_dstaddr))->sin_addr.s_addr;
+					else {
+					    ip->ip_dst.s_addr = ((struct sockaddr_in *)(ifa->ifa_addr))->sin_addr.s_addr;
+					    ip->ip_dst.s_addr &= ((struct sockaddr_in *)(ifa->ifa_netmask))->sin_addr.s_addr;
+					    ip->ip_dst.s_addr |= ~(((struct sockaddr_in *)(ifa->ifa_netmask))->sin_addr.s_addr);
+					}
+				    }
+				    else
+					goto bad;
+
+#ifdef FORWARD_DEBUG
+				    printf("\t\tForwarding to %s\n",inet_ntoa(ip->ip_dst));
+#endif				    
+
+                            	    bzero(&ro, sizeof ro);
+
+                            	    tmp = m_copypacket(fwd,M_DONTWAIT);
+                            	    error = ip_output(fwd, NULL, &ro, IP_ALLOWBROADCAST, NULL,0);
+#ifdef FORWARD_DEBUG				    
+				    if (error)
+					printf("Output error %d\n",error);
+#endif					
+                            	    fwd = tmp;
+
+                            	    if (!fwd)
+                                	goto bad;
+
+                            	    if (ro.ro_rt)
+                                	RTFREE(ro.ro_rt);
+				}
+
+                            	ifa = ifa->ifa_next;
+                    	    }
+                    	}
+                        ifp = ifp->if_next;
+            	    }
+		    
+		    m_freem(m);
+		    if (fwd)
+			m_freem(fwd);
+		    if (opts)
+			m_freem(opts);
+			
+		    /*
+		     * FIXME: should I also pass udp packet to socket?
+		     */
+
+                    return ;
+                }
+#endif /* FORWARD_PROTOCOL */		
+		
 		/*
 		 * Deliver a multicast or broadcast datagram to *all* sockets
 		 * for which the local and remote addresses and ports match
@@ -260,6 +651,7 @@
 			 * (No need to send an ICMP Port Unreachable
 			 * for a broadcast or multicast datgram.)
 			 */
+			udpstat.udps_noport++;
 			udpstat.udps_noportbcast++;
 			goto bad;
 		}
@@ -273,6 +665,8 @@
 			goto bad;
 		}
 		sorwakeup(last->inp_socket);
+		if (fwd)
+		    m_freem(fwd);
 		return;
 	}
 	/*
@@ -282,13 +676,16 @@
 	    ip->ip_dst, uh->uh_dport, 1);
 	if (inp == NULL) {
 		if (log_in_vain) {
-			char buf[4*sizeof "123"];
+			char bufdst[20];
+			char bufsrc[20];
+			
+			inet_ntop(AF_INET,&(ip->ip_dst),bufdst,sizeof(bufdst));
+			inet_ntop(AF_INET,&(ip->ip_src),bufdst,sizeof(bufsrc));
 
-			strcpy(buf, inet_ntoa(ip->ip_dst));
 			log(LOG_INFO, "Connection attempt to UDP %s:%d"
 			    " from %s:%d\n",
-				buf, ntohs(uh->uh_dport),
-				inet_ntoa(ip->ip_src), ntohs(uh->uh_sport));
+				bufdst, ntohs(uh->uh_dport),
+				bufsrc, ntohs(uh->uh_sport));
 		}
 		udpstat.udps_noport++;
 		if (m->m_flags & (M_BCAST | M_MCAST)) {
@@ -296,7 +693,16 @@
 			goto bad;
 		}
 		*ip = save_ip;
+		
+		/*
+		 * If we forced to be silent as much as possible..
+		 */
+		if (blackhole)
+		    goto bad;
+		    
 		icmp_error(m, ICMP_UNREACH, ICMP_UNREACH_PORT, 0, 0);
+		if (fwd)
+		    m_freem(fwd);
 		return;
 	}
 
@@ -319,11 +725,15 @@
 		goto bad;
 	}
 	sorwakeup(inp->inp_socket);
+	if (fwd)
+	    m_freem(fwd);
 	return;
 bad:
 	m_freem(m);
+	if (fwd)
+	    m_freem(fwd);
 	if (opts)
-		m_freem(opts);
+	    m_freem(opts);
 }
 
 /*
@@ -431,7 +841,7 @@
 	 * Stuff checksum and output datagram.
 	 */
 	ui->ui_sum = 0;
-	if (udpcksum) {
+	if (udpcksum) { /*FIXME: should be taken from ouput interface */
 	    if ((ui->ui_sum = in_cksum(m, sizeof (struct udpiphdr) + len)) == 0)
 		ui->ui_sum = 0xffff;
 	}
@@ -441,7 +851,7 @@
 	udpstat.udps_opackets++;
 	error = ip_output(m, inp->inp_options, &inp->inp_route,
 	    inp->inp_socket->so_options & (SO_DONTROUTE | SO_BROADCAST),
-	    inp->inp_moptions);
+	    inp->inp_moptions,0);
 
 	if (addr) {
 		in_pcbdisconnect(inp);
@@ -455,14 +865,6 @@
 	return (error);
 }
 
-static u_long	udp_sendspace = 9216;		/* really max datagram size */
-					/* 40 1K datagrams */
-SYSCTL_INT(_net_inet_udp, UDPCTL_MAXDGRAM, maxdgram, CTLFLAG_RW,
-	&udp_sendspace, 0, "");
-
-static u_long	udp_recvspace = 40 * (1024 + sizeof(struct sockaddr_in));
-SYSCTL_INT(_net_inet_udp, UDPCTL_RECVSPACE, recvspace, CTLFLAG_RW,
-	&udp_recvspace, 0, "");
 
 /*ARGSUSED*/
 int
@@ -501,7 +903,7 @@
 		error = soreserve(so, udp_sendspace, udp_recvspace);
 		if (error)
 			break;
-		((struct inpcb *) so->so_pcb)->inp_ip_ttl = ip_defttl;
+		((struct inpcb *) so->so_pcb)->inp_ip_ttl = ip_defttl; /*FIXME: fetch ttl from interface first*/
 		break;
 
 	case PRU_DETACH:
