]> git.kernelconcepts.de Git - karo-tx-uboot.git/blobdiff - net/net.c
Merge branch 'u-boot-imx/master' into 'u-boot-arm/master'
[karo-tx-uboot.git] / net / net.c
index 4cdd9cd0f278e12ae32c8c64d9fd88cc4cad8c68..f7cc29f03956a6b1b56ccc27e479f351d86cf570 100644 (file)
--- a/net/net.c
+++ b/net/net.c
  *                     - name of bootfile
  *     Next step:      ARP
  *
+ * LINK_LOCAL:
+ *
+ *     Prerequisites:  - own ethernet address
+ *     We want:        - own IP address
+ *     Next step:      ARP
+ *
  * RARP:
  *
  *     Prerequisites:  - own ethernet address
 
 
 #include <common.h>
-#include <watchdog.h>
 #include <command.h>
-#include <linux/compiler.h>
+#include <environment.h>
 #include <net.h>
-#include "arp.h"
-#include "bootp.h"
-#include "tftp.h"
-#include "rarp.h"
-#include "nfs.h"
-#ifdef CONFIG_STATUS_LED
-#include <status_led.h>
+#if defined(CONFIG_STATUS_LED)
 #include <miiphy.h>
+#include <status_led.h>
 #endif
-#if defined(CONFIG_CMD_SNTP)
-#include "sntp.h"
-#endif
+#include <watchdog.h>
+#include <linux/compiler.h>
+#include "arp.h"
+#include "bootp.h"
 #include "cdp.h"
 #if defined(CONFIG_CMD_DNS)
 #include "dns.h"
 #endif
+#include "link_local.h"
+#include "nfs.h"
 #include "ping.h"
+#include "rarp.h"
+#if defined(CONFIG_CMD_SNTP)
+#include "sntp.h"
+#endif
+#include "tftp.h"
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -149,7 +157,7 @@ uchar               NetEtherNullAddr[6];
 void           (*push_packet)(void *, int len) = 0;
 #endif
 /* Network loop state */
-int            NetState;
+enum net_loop_state net_state;
 /* Tried all network devices */
 int            NetRestartWrap;
 /* Network loop restarted */
@@ -173,15 +181,18 @@ IPaddr_t  NetNtpServerIP;
 int            NetTimeOffset;
 #endif
 
-uchar PktBuf[(PKTBUFSRX+1) * PKTSIZE_ALIGN + PKTALIGN];
+static uchar PktBuf[(PKTBUFSRX+1) * PKTSIZE_ALIGN + PKTALIGN];
 
 /* Receive packet */
 uchar *NetRxPackets[PKTBUFSRX];
 
-/* Current RX packet handler */
-static rxhand_f *packetHandler;
+/* Current UDP RX packet handler */
+static rxhand_f *udp_packet_handler;
+/* Current ARP RX packet handler */
+static rxhand_f *arp_packet_handler;
 #ifdef CONFIG_CMD_TFTPPUT
-static rxhand_icmp_f *packet_icmp_handler;     /* Current ICMP rx handler */
+/* Current ICMP rx handler */
+static rxhand_icmp_f *packet_icmp_handler;
 #endif
 /* Current timeout handler */
 static thand_f *timeHandler;
@@ -196,39 +207,55 @@ static int net_check_prereq(enum proto_t protocol);
 
 static int NetTryCount;
 
+int __maybe_unused net_busy_flag;
+
 /**********************************************************************/
 
+static int on_bootfile(const char *name, const char *value, enum env_op op,
+       int flags)
+{
+       switch (op) {
+       case env_op_create:
+       case env_op_overwrite:
+               copy_filename(BootFile, value, sizeof(BootFile));
+               break;
+       default:
+               break;
+       }
+
+       return 0;
+}
+U_BOOT_ENV_CALLBACK(bootfile, on_bootfile);
+
 /*
  * Check if autoload is enabled. If so, use either NFS or TFTP to download
  * the boot file.
  */
 void net_auto_load(void)
 {
+#if defined(CONFIG_CMD_NFS)
        const char *s = getenv("autoload");
 
-       if (s != NULL) {
-               if (*s == 'n') {
-                       /*
-                        * Just use BOOTP/RARP to configure system;
-                        * Do not use TFTP to load the bootfile.
-                        */
-                       NetState = NETLOOP_SUCCESS;
-                       return;
-               }
-#if defined(CONFIG_CMD_NFS)
-               if (strcmp(s, "NFS") == 0) {
-                       /*
-                        * Use NFS to load the bootfile.
-                        */
-                       NfsStart();
-                       return;
-               }
+       if (s != NULL && strcmp(s, "NFS") == 0) {
+               /*
+                * Use NFS to load the bootfile.
+                */
+               NfsStart();
+               return;
+       }
 #endif
+       if (getenv_yesno("autoload") == 0) {
+               /*
+                * Just use BOOTP/RARP to configure system;
+                * Do not use TFTP to load the bootfile.
+                */
+               net_set_state(NETLOOP_SUCCESS);
+               return;
        }
        TftpStart(TFTPGET);
 }
 
-static void NetInitLoop(enum proto_t protocol)
+static void NetInitLoop(void)
 {
        static int env_changed_id;
        int env_id = get_env_id();
@@ -246,10 +273,49 @@ static void NetInitLoop(enum proto_t protocol)
 #endif
                env_changed_id = env_id;
        }
+       if (eth_get_dev())
+               memcpy(NetOurEther, eth_get_dev()->enetaddr, 6);
 
        return;
 }
 
+static void net_clear_handlers(void)
+{
+       net_set_udp_handler(NULL);
+       net_set_arp_handler(NULL);
+       NetSetTimeout(0, NULL);
+}
+
+static void net_cleanup_loop(void)
+{
+       net_clear_handlers();
+}
+
+void net_init(void)
+{
+       static int first_call = 1;
+
+       if (first_call) {
+               /*
+                *      Setup packet buffers, aligned correctly.
+                */
+               int i;
+
+               NetTxPacket = &PktBuf[0] + (PKTALIGN - 1);
+               NetTxPacket -= (ulong)NetTxPacket % PKTALIGN;
+               for (i = 0; i < PKTBUFSRX; i++)
+                       NetRxPackets[i] = NetTxPacket + (i + 1) * PKTSIZE_ALIGN;
+
+               ArpInit();
+               net_clear_handlers();
+
+               /* Only need to setup buffer pointers once. */
+               first_call = 0;
+       }
+
+       NetInitLoop();
+}
+
 /**********************************************************************/
 /*
  *     Main network processing loop.
@@ -262,42 +328,34 @@ int NetLoop(enum proto_t protocol)
 
        NetRestarted = 0;
        NetDevExists = 0;
-
-       NetTxPacket = NULL;
        NetTryCount = 1;
-
-       ArpInit();
-
-       if (!NetTxPacket) {
-               int     i;
-               /*
-                *      Setup packet buffers, aligned correctly.
-                */
-               NetTxPacket = &PktBuf[0] + (PKTALIGN - 1);
-               NetTxPacket -= (ulong)NetTxPacket % PKTALIGN;
-               for (i = 0; i < PKTBUFSRX; i++)
-                       NetRxPackets[i] = NetTxPacket + (i+1)*PKTSIZE_ALIGN;
-       }
+       debug_cond(DEBUG_INT_STATE, "--- NetLoop Entry\n");
 
        bootstage_mark_name(BOOTSTAGE_ID_ETH_START, "eth_start");
-       eth_halt();
-       eth_set_current();
-       if (eth_init(bd) < 0) {
+       net_init();
+       if (eth_is_on_demand_init() || protocol != NETCONS) {
                eth_halt();
-               return -1;
-       }
+               eth_set_current();
+               if (eth_init(bd) < 0) {
+                       eth_halt();
+                       return -1;
+               }
+       } else
+               eth_init_state_only(bd);
 
 restart:
-       memcpy(NetOurEther, eth_get_dev()->enetaddr, 6);
-
-       NetState = NETLOOP_CONTINUE;
+#ifdef CONFIG_USB_KEYBOARD
+       net_busy_flag = 0;
+#endif
+       net_set_state(NETLOOP_CONTINUE);
 
        /*
         *      Start the ball rolling with the given start function.  From
         *      here on, this code is a state machine driven by received
         *      packets and timer events.
         */
-       NetInitLoop(protocol);
+       debug_cond(DEBUG_INT_STATE, "--- NetLoop Init\n");
+       NetInitLoop();
 
        switch (net_check_prereq(protocol)) {
        case 1:
@@ -375,6 +433,11 @@ restart:
                case DNS:
                        DnsStart();
                        break;
+#endif
+#if defined(CONFIG_CMD_LINK_LOCAL)
+               case LINKLOCAL:
+                       link_local_start();
+                       break;
 #endif
                default:
                        break;
@@ -396,10 +459,13 @@ restart:
                status_led_set(STATUS_LED_RED, STATUS_LED_ON);
 #endif /* CONFIG_SYS_FAULT_ECHO_LINK_DOWN, ... */
 #endif /* CONFIG_MII, ... */
+#ifdef CONFIG_USB_KEYBOARD
+       net_busy_flag = 1;
+#endif
 
        /*
         *      Main packet reception loop.  Loop receiving packets until
-        *      someone sets `NetState' to a state that terminates.
+        *      someone sets `net_state' to a state that terminates.
         */
        for (;;) {
                WATCHDOG_RESET();
@@ -416,8 +482,18 @@ restart:
                 *      Abort if ctrl-c was pressed.
                 */
                if (ctrlc()) {
+                       /* cancel any ARP that may not have completed */
+                       NetArpWaitPacketIP = 0;
+
+                       net_cleanup_loop();
                        eth_halt();
+                       /* Invalidate the last protocol */
+                       eth_set_last_protocol(BOOTP);
+
                        puts("\nAbort\n");
+                       /* include a debug print as well incase the debug
+                          messages are directed to stderr */
+                       debug_cond(DEBUG_INT_STATE, "--- NetLoop Abort!\n");
                        goto done;
                }
 
@@ -445,43 +521,58 @@ restart:
                        }
 #endif /* CONFIG_SYS_FAULT_ECHO_LINK_DOWN, ... */
 #endif /* CONFIG_MII, ... */
+                       debug_cond(DEBUG_INT_STATE, "--- NetLoop timeout\n");
                        x = timeHandler;
                        timeHandler = (thand_f *)0;
                        (*x)();
                }
 
 
-               switch (NetState) {
+               switch (net_state) {
 
                case NETLOOP_RESTART:
                        NetRestarted = 1;
                        goto restart;
 
                case NETLOOP_SUCCESS:
+                       net_cleanup_loop();
                        if (NetBootFileXferSize > 0) {
-                               char buf[20];
                                printf("Bytes transferred = %ld (%lx hex)\n",
                                        NetBootFileXferSize,
                                        NetBootFileXferSize);
-                               sprintf(buf, "%lX", NetBootFileXferSize);
-                               setenv("filesize", buf);
-
-                               sprintf(buf, "%lX", (unsigned long)load_addr);
-                               setenv("fileaddr", buf);
+                               setenv_hex("filesize", NetBootFileXferSize);
+                               setenv_hex("fileaddr", load_addr);
                        }
-                       eth_halt();
+                       if (protocol != NETCONS)
+                               eth_halt();
+                       else
+                               eth_halt_state_only();
+
+                       eth_set_last_protocol(protocol);
+
                        ret = NetBootFileXferSize;
+                       debug_cond(DEBUG_INT_STATE, "--- NetLoop Success!\n");
                        goto done;
 
                case NETLOOP_FAIL:
+                       net_cleanup_loop();
+                       /* Invalidate the last protocol */
+                       eth_set_last_protocol(BOOTP);
+                       debug_cond(DEBUG_INT_STATE, "--- NetLoop Fail!\n");
                        goto done;
+
+               case NETLOOP_CONTINUE:
+                       continue;
                }
        }
 
 done:
+#ifdef CONFIG_USB_KEYBOARD
+       net_busy_flag = 0;
+#endif
 #ifdef CONFIG_CMD_TFTPPUT
        /* Clear out the handlers */
-       NetSetHandler(NULL);
+       net_set_udp_handler(NULL);
        net_set_icmp_handler(NULL);
 #endif
        return ret;
@@ -492,14 +583,7 @@ done:
 static void
 startAgainTimeout(void)
 {
-       NetState = NETLOOP_RESTART;
-}
-
-static void
-startAgainHandler(uchar *pkt, unsigned dest, IPaddr_t sip,
-                 unsigned src, unsigned len)
-{
-       /* Totally ignore the packet */
+       net_set_state(NETLOOP_RESTART);
 }
 
 void NetStartAgain(void)
@@ -523,7 +607,7 @@ void NetStartAgain(void)
 
        if ((!retry_forever) && (NetTryCount >= retrycnt)) {
                eth_halt();
-               NetState = NETLOOP_FAIL;
+               net_set_state(NETLOOP_FAIL);
                return;
        }
 
@@ -538,12 +622,12 @@ void NetStartAgain(void)
                NetRestartWrap = 0;
                if (NetDevExists) {
                        NetSetTimeout(10000UL, startAgainTimeout);
-                       NetSetHandler(startAgainHandler);
+                       net_set_udp_handler(NULL);
                } else {
-                       NetState = NETLOOP_FAIL;
+                       net_set_state(NETLOOP_FAIL);
                }
        } else {
-               NetState = NETLOOP_RESTART;
+               net_set_state(NETLOOP_RESTART);
        }
 }
 
@@ -552,17 +636,38 @@ void NetStartAgain(void)
  *     Miscelaneous bits.
  */
 
-rxhand_f *
-NetGetHandler(void)
+static void dummy_handler(uchar *pkt, unsigned dport,
+                       IPaddr_t sip, unsigned sport,
+                       unsigned len)
 {
-       return packetHandler;
 }
 
+rxhand_f *net_get_udp_handler(void)
+{
+       return udp_packet_handler;
+}
 
-void
-NetSetHandler(rxhand_f *f)
+void net_set_udp_handler(rxhand_f *f)
+{
+       debug_cond(DEBUG_INT_STATE, "--- NetLoop UDP handler set (%p)\n", f);
+       if (f == NULL)
+               udp_packet_handler = dummy_handler;
+       else
+               udp_packet_handler = f;
+}
+
+rxhand_f *net_get_arp_handler(void)
+{
+       return arp_packet_handler;
+}
+
+void net_set_arp_handler(rxhand_f *f)
 {
-       packetHandler = f;
+       debug_cond(DEBUG_INT_STATE, "--- NetLoop ARP handler set (%p)\n", f);
+       if (f == NULL)
+               arp_packet_handler = dummy_handler;
+       else
+               arp_packet_handler = f;
 }
 
 #ifdef CONFIG_CMD_TFTPPUT
@@ -576,25 +681,29 @@ void
 NetSetTimeout(ulong iv, thand_f *f)
 {
        if (iv == 0) {
+               debug_cond(DEBUG_INT_STATE,
+                       "--- NetLoop timeout handler cancelled\n");
                timeHandler = (thand_f *)0;
        } else {
+               debug_cond(DEBUG_INT_STATE,
+                       "--- NetLoop timeout handler set (%p)\n", f);
                timeHandler = f;
                timeStart = get_timer(0);
-               timeDelta = iv;
+               timeDelta = iv * CONFIG_SYS_HZ / 1000;
        }
 }
 
-
-void
-NetSendPacket(uchar *pkt, int len)
-{
-       (void) eth_send(pkt, len);
-}
-
-int
-NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport, int len)
+int NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport,
+               int payload_len)
 {
        uchar *pkt;
+       int eth_hdr_size;
+       int pkt_hdr_size;
+
+       /* make sure the NetTxPacket is initialized (NetInit() was called) */
+       assert(NetTxPacket != NULL);
+       if (NetTxPacket == NULL)
+               return -1;
 
        /* convert to new style broadcast */
        if (dest == 0)
@@ -604,44 +713,35 @@ NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport, int len)
        if (dest == 0xFFFFFFFF)
                ether = NetBcastAddr;
 
-       /*
-        * if MAC address was not discovered yet, save the packet and do
-        * an ARP request
-        */
-       if (memcmp(ether, NetEtherNullAddr, 6) == 0) {
+       pkt = (uchar *)NetTxPacket;
 
-               debug("sending ARP for %08x\n", dest);
+       eth_hdr_size = NetSetEther(pkt, ether, PROT_IP);
+       pkt += eth_hdr_size;
+       net_set_udp_header(pkt, dest, dport, sport, payload_len);
+       pkt_hdr_size = eth_hdr_size + IP_UDP_HDR_SIZE;
 
+       /* if MAC address was not discovered yet, do an ARP request */
+       if (memcmp(ether, NetEtherNullAddr, 6) == 0) {
+               debug_cond(DEBUG_DEV_PKT, "sending ARP for %pI4\n", &dest);
+
+               /* save the ip and eth addr for the packet to send after arp */
                NetArpWaitPacketIP = dest;
                NetArpWaitPacketMAC = ether;
 
-               pkt = NetArpWaitTxPacket;
-               pkt += NetSetEther(pkt, NetArpWaitPacketMAC, PROT_IP);
-
-               NetSetIP(pkt, dest, dport, sport, len);
-               memcpy(pkt + IP_UDP_HDR_SIZE, (uchar *)NetTxPacket +
-                      (pkt - (uchar *)NetArpWaitTxPacket) +
-                      IP_UDP_HDR_SIZE, len);
-
                /* size of the waiting packet */
-               NetArpWaitTxPacketSize = (pkt - NetArpWaitTxPacket) +
-                       IP_UDP_HDR_SIZE + len;
+               NetArpWaitTxPacketSize = pkt_hdr_size + payload_len;
 
                /* and do the ARP request */
                NetArpWaitTry = 1;
                NetArpWaitTimerStart = get_timer(0);
                ArpRequest();
                return 1;       /* waiting */
+       } else {
+               debug_cond(DEBUG_DEV_PKT, "sending UDP to %pI4/%pM\n",
+                       &dest, ether);
+               NetSendPacket(NetTxPacket, pkt_hdr_size + payload_len);
+               return 0;       /* transmitted */
        }
-
-       debug("sending UDP to %08x/%pM\n", dest, ether);
-
-       pkt = (uchar *)NetTxPacket;
-       pkt += NetSetEther(pkt, ether, PROT_IP);
-       NetSetIP(pkt, dest, dport, sport, len);
-       eth_send(NetTxPacket, (pkt - NetTxPacket) + IP_UDP_HDR_SIZE + len);
-
-       return 0;       /* transmitted */
 }
 
 #ifdef CONFIG_IP_DEFRAG
@@ -853,15 +953,15 @@ NetReceive(uchar *inpkt, int len)
 {
        struct ethernet_hdr *et;
        struct ip_udp_hdr *ip;
-       IPaddr_t tmp;
+       IPaddr_t dst_ip;
        IPaddr_t src_ip;
-       int     x;
+       int eth_proto;
 #if defined(CONFIG_CMD_CDP)
        int iscdp;
 #endif
        ushort cti = 0, vlanid = VLAN_NONE, myvlanid, mynvlanid;
 
-       debug("packet received\n");
+       debug_cond(DEBUG_NET_PKT, "packet received\n");
 
        NetRxPacket = inpkt;
        NetRxPacketLen = len;
@@ -890,21 +990,20 @@ NetReceive(uchar *inpkt, int len)
        if (mynvlanid == (ushort)-1)
                mynvlanid = VLAN_NONE;
 
-       x = ntohs(et->et_protlen);
+       eth_proto = ntohs(et->et_protlen);
 
-       debug("packet received\n");
-
-       if (x < 1514) {
+       if (eth_proto < 1514) {
                struct e802_hdr *et802 = (struct e802_hdr *)et;
                /*
-                *      Got a 802 packet.  Check the other protocol field.
+                *      Got a 802.2 packet.  Check the other protocol field.
+                *      XXX VLAN over 802.2+SNAP not implemented!
                 */
-               x = ntohs(et802->et_prot);
+               eth_proto = ntohs(et802->et_prot);
 
                ip = (struct ip_udp_hdr *)(inpkt + E802_HDR_SIZE);
                len -= E802_HDR_SIZE;
 
-       } else if (x != PROT_VLAN) {    /* normal packet */
+       } else if (eth_proto != PROT_VLAN) {    /* normal packet */
                ip = (struct ip_udp_hdr *)(inpkt + ETHER_HDR_SIZE);
                len -= ETHER_HDR_SIZE;
 
@@ -912,7 +1011,7 @@ NetReceive(uchar *inpkt, int len)
                struct vlan_ethernet_hdr *vet =
                        (struct vlan_ethernet_hdr *)et;
 
-               debug("VLAN packet received\n");
+               debug_cond(DEBUG_NET_PKT, "VLAN packet received\n");
 
                /* too small packet? */
                if (len < VLAN_ETHER_HDR_SIZE)
@@ -928,17 +1027,17 @@ NetReceive(uchar *inpkt, int len)
 
                cti = ntohs(vet->vet_tag);
                vlanid = cti & VLAN_IDMASK;
-               x = ntohs(vet->vet_type);
+               eth_proto = ntohs(vet->vet_type);
 
                ip = (struct ip_udp_hdr *)(inpkt + VLAN_ETHER_HDR_SIZE);
                len -= VLAN_ETHER_HDR_SIZE;
        }
 
-       debug("Receive from protocol 0x%x\n", x);
+       debug_cond(DEBUG_NET_PKT, "Receive from protocol 0x%x\n", eth_proto);
 
 #if defined(CONFIG_CMD_CDP)
        if (iscdp) {
-               CDPHandler((uchar *)ip, len);
+               cdp_receive((uchar *)ip, len);
                return;
        }
 #endif
@@ -951,7 +1050,7 @@ NetReceive(uchar *inpkt, int len)
                        return;
        }
 
-       switch (x) {
+       switch (eth_proto) {
 
        case PROT_ARP:
                ArpReceive(et, ip, len);
@@ -963,7 +1062,7 @@ NetReceive(uchar *inpkt, int len)
                break;
 #endif
        case PROT_IP:
-               debug("Got IP\n");
+               debug_cond(DEBUG_NET_PKT, "Got IP\n");
                /* Before we start poking the header, make sure it is there */
                if (len < IP_UDP_HDR_SIZE) {
                        debug("len bad %d < %lu\n", len,
@@ -972,11 +1071,12 @@ NetReceive(uchar *inpkt, int len)
                }
                /* Check the packet length */
                if (len < ntohs(ip->ip_len)) {
-                       printf("len bad %d < %d\n", len, ntohs(ip->ip_len));
+                       debug("len bad %d < %d\n", len, ntohs(ip->ip_len));
                        return;
                }
                len = ntohs(ip->ip_len);
-               debug("len=%d, v=%02x\n", len, ip->ip_hl_v & 0xff);
+               debug_cond(DEBUG_NET_PKT, "len=%d, v=%02x\n",
+                       len, ip->ip_hl_v & 0xff);
 
                /* Can't deal with anything except IPv4 */
                if ((ip->ip_hl_v & 0xf0) != 0x40)
@@ -986,14 +1086,14 @@ NetReceive(uchar *inpkt, int len)
                        return;
                /* Check the Checksum of the header */
                if (!NetCksumOk((uchar *)ip, IP_HDR_SIZE / 2)) {
-                       puts("checksum bad\n");
+                       debug("checksum bad\n");
                        return;
                }
                /* If it is not for us, ignore it */
-               tmp = NetReadIP(&ip->ip_dst);
-               if (NetOurIP && tmp != NetOurIP && tmp != 0xFFFFFFFF) {
+               dst_ip = NetReadIP(&ip->ip_dst);
+               if (NetOurIP && dst_ip != NetOurIP && dst_ip != 0xFFFFFFFF) {
 #ifdef CONFIG_MCAST_TFTP
-                       if (Mcast_addr != tmp)
+                       if (Mcast_addr != dst_ip)
 #endif
                                return;
                }
@@ -1035,6 +1135,10 @@ NetReceive(uchar *inpkt, int len)
                        return;
                }
 
+               debug_cond(DEBUG_DEV_PKT,
+                       "received UDP (to=%pI4, from=%pI4, len=%d)\n",
+                       &dst_ip, &src_ip, len);
+
 #ifdef CONFIG_UDP_CHECKSUM
                if (ip->udp_xsum != 0) {
                        ulong   xsum;
@@ -1080,6 +1184,7 @@ NetReceive(uchar *inpkt, int len)
 
 #ifdef CONFIG_NETCONSOLE
                nc_input_packet((uchar *)ip + IP_UDP_HDR_SIZE,
+                                       src_ip,
                                        ntohs(ip->udp_dst),
                                        ntohs(ip->udp_src),
                                        ntohs(ip->udp_len) - UDP_HDR_SIZE);
@@ -1087,11 +1192,11 @@ NetReceive(uchar *inpkt, int len)
                /*
                 *      IP header OK.  Pass the packet to the current handler.
                 */
-               (*packetHandler)((uchar *)ip + IP_UDP_HDR_SIZE,
-                                       ntohs(ip->udp_dst),
-                                       src_ip,
-                                       ntohs(ip->udp_src),
-                                       ntohs(ip->udp_len) - UDP_HDR_SIZE);
+               (*udp_packet_handler)((uchar *)ip + IP_UDP_HDR_SIZE,
+                               ntohs(ip->udp_dst),
+                               src_ip,
+                               ntohs(ip->udp_src),
+                               ntohs(ip->udp_len) - UDP_HDR_SIZE);
                break;
        }
 }
@@ -1156,6 +1261,7 @@ common:
        case BOOTP:
        case CDP:
        case DHCP:
+       case LINKLOCAL:
                if (memcmp(NetOurEther, "\0\0\0\0\0\0", 6) == 0) {
                        int num = eth_get_dev_index();
 
@@ -1243,40 +1349,72 @@ NetSetEther(uchar *xet, uchar * addr, uint prot)
        }
 }
 
-void NetSetIP(uchar *xip, IPaddr_t dest, int dport, int sport, int len)
+int net_update_ether(struct ethernet_hdr *et, uchar *addr, uint prot)
 {
-       struct ip_udp_hdr *ip = (struct ip_udp_hdr *)xip;
+       ushort protlen;
 
-       /*
-        *      If the data is an odd number of bytes, zero the
-        *      byte after the last byte so that the checksum
-        *      will work.
-        */
-       if (len & 1)
-               xip[IP_UDP_HDR_SIZE + len] = 0;
+       memcpy(et->et_dest, addr, 6);
+       memcpy(et->et_src, NetOurEther, 6);
+       protlen = ntohs(et->et_protlen);
+       if (protlen == PROT_VLAN) {
+               struct vlan_ethernet_hdr *vet =
+                       (struct vlan_ethernet_hdr *)et;
+               vet->vet_type = htons(prot);
+               return VLAN_ETHER_HDR_SIZE;
+       } else if (protlen > 1514) {
+               et->et_protlen = htons(prot);
+               return ETHER_HDR_SIZE;
+       } else {
+               /* 802.2 + SNAP */
+               struct e802_hdr *et802 = (struct e802_hdr *)et;
+               et802->et_prot = htons(prot);
+               return E802_HDR_SIZE;
+       }
+}
+
+void net_set_ip_header(uchar *pkt, IPaddr_t dest, IPaddr_t source)
+{
+       struct ip_udp_hdr *ip = (struct ip_udp_hdr *)pkt;
 
        /*
-        *      Construct an IP and UDP header.
-        *      (need to set no fragment bit - XXX)
+        *      Construct an IP header.
         */
        /* IP_HDR_SIZE / 4 (not including UDP) */
        ip->ip_hl_v  = 0x45;
        ip->ip_tos   = 0;
-       ip->ip_len   = htons(IP_UDP_HDR_SIZE + len);
+       ip->ip_len   = htons(IP_HDR_SIZE);
        ip->ip_id    = htons(NetIPID++);
        ip->ip_off   = htons(IP_FLAGS_DFRAG);   /* Don't fragment */
        ip->ip_ttl   = 255;
-       ip->ip_p     = 17;              /* UDP */
        ip->ip_sum   = 0;
        /* already in network byte order */
-       NetCopyIP((void *)&ip->ip_src, &NetOurIP);
-       /* - "" - */
+       NetCopyIP((void *)&ip->ip_src, &source);
+       /* already in network byte order */
        NetCopyIP((void *)&ip->ip_dst, &dest);
+}
+
+void net_set_udp_header(uchar *pkt, IPaddr_t dest, int dport, int sport,
+                       int len)
+{
+       struct ip_udp_hdr *ip = (struct ip_udp_hdr *)pkt;
+
+       /*
+        *      If the data is an odd number of bytes, zero the
+        *      byte after the last byte so that the checksum
+        *      will work.
+        */
+       if (len & 1)
+               pkt[IP_UDP_HDR_SIZE + len] = 0;
+
+       net_set_ip_header(pkt, dest, NetOurIP);
+       ip->ip_len   = htons(IP_UDP_HDR_SIZE + len);
+       ip->ip_p     = IPPROTO_UDP;
+       ip->ip_sum   = ~NetCksum((uchar *)ip, IP_HDR_SIZE >> 1);
+
        ip->udp_src  = htons(sport);
        ip->udp_dst  = htons(dport);
        ip->udp_len  = htons(UDP_HDR_SIZE + len);
        ip->udp_xsum = 0;
-       ip->ip_sum   = ~NetCksum((uchar *)ip, IP_HDR_SIZE / 2);
 }
 
 void copy_filename(char *dst, const char *src, int size)