From: H. Peter Anvin <hpa@zytor.com>
Date: Thu, 5 Jun 2008 01:52:18 +0000 (-0700)
Subject: Update gPXE from gPXE git
X-Git-Tag: syslinux-3.70-pre13~6
X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=e63132bf03140d91ec74eca663f7bc2f16429fb0;p=platform%2Fupstream%2Fsyslinux.git

Update gPXE from gPXE git
---

diff --git a/gpxe/contrib/errcode/README b/gpxe/contrib/errcode/README
new file mode 100644
index 0000000..b2963c0
--- /dev/null
+++ b/gpxe/contrib/errcode/README
@@ -0,0 +1,35 @@
+Error Code Lookup for gPXE
+==========================
+This program looks up gPXE error codes so you can locate the line of source
+code which produced the error.
+
+Setup
+-----
+You must run:
+./build_errcodedb.py >errcodedb.py
+
+This extracts error code definitions from the gPXE source code and produces a
+"database" which is used by the main program.
+
+Once you have done this errcode.py and errcodedb.py are the only files you
+need.  They are now independent of the gPXE source code and can be moved
+anywhere.
+
+[OPTIONAL]
+A PHP script is provided as a web interface.  First edit errcode.php to point
+$ERRCODE_PATH to the errcode.py script.  Then move errcode.php to a location
+visible from your web server.
+
+[OPTIONAL]
+A simple IRC bot is provided.  Edit gpxebot.py to fill in the IRC details.
+
+Usage
+-----
+Looking up error codes on the command-line:
+./errcode.py 0x12345678
+
+Further information
+-------------------
+See http://etherboot.org/.
+
+Released under the GPL and written by Stefan Hajnoczi <stefanha@gmail.com>.
diff --git a/gpxe/contrib/errcode/build_errcodedb.py b/gpxe/contrib/errcode/build_errcodedb.py
new file mode 100644
index 0000000..1be9d85
--- /dev/null
+++ b/gpxe/contrib/errcode/build_errcodedb.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+# Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of the
+# License, or any later version.
+#
+# This program is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+import sys
+import re
+
+pxenv_status_files = ('../../src/include/errno.h', )
+errfile_files = ('../../src/include/gpxe/errfile.h',
+            '../../src/arch/i386/include/bits/errfile.h')
+posix_errno_files = ('../../src/include/errno.h', )
+
+PXENV_STATUS_RE = re.compile(r'^#define\s+(PXENV_STATUS_[^\s]+)\s+(.+)$', re.M)
+ERRFILE_RE = re.compile(r'^#define\s+(ERRFILE_[^\s]+)\s+(.+)$', re.M)
+POSIX_ERRNO_RE = re.compile(r'^#define\s+(E[A-Z0-9]+)\s+(?:\\\n)?.*(0x[0-9a-f]+).*$', re.M)
+
+def err(msg):
+    sys.stderr.write('%s: %s\n' % (sys.argv[0], msg))
+    sys.exit(1)
+
+def to_pxenv_status(errno):
+    return errno & 0xff
+
+def to_errfile(errno):
+    return (errno >> 13) & 0x7ff
+
+def to_posix_errno(errno):
+    return (errno >> 24) & 0x7f
+
+def load_header_file(filename, regexp):
+    defines = {}
+    data = open(filename, 'r').read()
+    for m in regexp.finditer(data):
+        key, val = m.groups()
+        defines[key] = val
+    return defines
+
+def evaluate(defines, expr):
+    pyexpr = ''
+    for token in expr.split():
+        if token in '()':
+            pass
+        elif token.startswith('/*') or token.startswith('//'):
+            break
+        elif token.startswith('0x') or token == '|':
+            pyexpr += token
+        else:
+            if token in defines:
+                pyexpr += '0x%x' % defines[token]
+            else:
+                return -1
+    if not re.match(r'^[0-9a-zA-Z_|]+$', pyexpr):
+        err('invalid expression')
+    return eval(pyexpr)
+
+def build(filenames, regexp, selector):
+    unevaluated = {}
+    for filename in filenames:
+        unevaluated.update(load_header_file(filename, regexp))
+
+    evaluated = {}
+    changed = True
+    while changed:
+        changed = False
+        for key in list(unevaluated.keys()):
+            val = evaluate(evaluated, unevaluated[key])
+            if val != -1:
+                del unevaluated[key]
+                evaluated[key] = val
+                changed = True
+    if unevaluated:
+        err('unable to evaluate all #defines')
+
+    lookup = {}
+    for key, val in evaluated.iteritems():
+        lookup[selector(val)] = key
+    return lookup
+
+print 'pxenv_status =', repr(build(pxenv_status_files, PXENV_STATUS_RE, to_pxenv_status))
+print 'errfile =', repr(build(errfile_files, ERRFILE_RE, to_errfile))
+print 'posix_errno =', repr(build(posix_errno_files, POSIX_ERRNO_RE, to_posix_errno))
diff --git a/gpxe/contrib/errcode/errcode.php b/gpxe/contrib/errcode/errcode.php
new file mode 100644
index 0000000..8711449
--- /dev/null
+++ b/gpxe/contrib/errcode/errcode.php
@@ -0,0 +1,83 @@
+<?
+/*
+ * Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+// The path to the errcode.py script.
+$ERRCODE_PATH = './errcode.py';
+?>
+
+<html>
+    <head>
+        <title>gPXE Error Code Lookup</title>
+        <style>
+            body, pre, div, form, p, h2, b, tt {
+                padding: 0;
+                border: 0;
+                margin: 0;
+            }
+            body {
+                padding: 0.5em;
+                width: 750px;
+                font-family: sans-serif;
+            }
+            pre {
+                margin: 0.2em;
+                padding: 0.1em;
+                background-color: #ddd;
+            }
+            form {
+                margin: 0.2em;
+            }
+            div {
+                margin: 0.2em;
+                padding: 0.4em;
+                border: 1px dashed black;
+            }
+        </style>
+    </head>
+    <body>
+<?
+if (!empty($_REQUEST['e']) && preg_match('/^(0x)?[0-9a-f]{8}$/', $_REQUEST['e'])) {
+?>
+        <pre>
+<?
+    system($ERRCODE_PATH . " " . $_REQUEST['e']);
+?>
+        </pre>
+<?
+}
+?>
+        <form action="" method="post">
+            <label for="e">Error code:</label>
+            <input type="text" name="e" id="e" value="0x12345678"></input>
+            <input type="submit" value="Lookup"></input>
+        </form>
+
+        <div>
+            <h2>Hint:</h2>
+            <p>
+            Firefox users can right-click on the <b>Error code</b>
+            text box and select <b>Add a Keyword for this Search...</b>.
+            Set <b>name</b> to <tt>gPXE Error Code Lookup</tt> and
+            <b>keyword</b> to <tt>gxpe</tt>  Then you can look up error
+            codes by typing something like the following in your address
+            bar: <tt>gpxe 0x3c018003</tt>
+            <p>
+        </div>
+    </body>
+</html>
diff --git a/gpxe/contrib/errcode/errcode.py b/gpxe/contrib/errcode/errcode.py
new file mode 100644
index 0000000..7bc8d9e
--- /dev/null
+++ b/gpxe/contrib/errcode/errcode.py
@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+# Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of the
+# License, or any later version.
+#
+# This program is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+import sys
+
+try:
+    import errcodedb
+except ImportError:
+    sys.stderr.write('Please run this first: ./build_errcodedb.py >errcodedb.py\n')
+    sys.exit(1)
+
+def to_pxenv_status(errno):
+    return errno & 0xff
+
+def to_uniq(errno):
+    return (errno >> 8) & 0x1f
+
+def to_errfile(errno):
+    return (errno >> 13) & 0x7ff
+
+def to_posix_errno(errno):
+    return (errno >> 24) & 0x7f
+
+def lookup_errno_component(defines, component):
+    if component in defines:
+        return defines[component]
+    else:
+        return '0x%x' % component
+
+class Errcode(object):
+    def __init__(self, errno):
+        self.pxenv_status = to_pxenv_status(errno)
+        self.uniq = to_uniq(errno)
+        self.errfile = to_errfile(errno)
+        self.posix_errno = to_posix_errno(errno)
+
+    def rawstr(self):
+        return 'pxenv_status=0x%x uniq=%d errfile=0x%x posix_errno=0x%x' % (self.pxenv_status, self.uniq, self.errfile, self.posix_errno)
+
+    def prettystr(self):
+        return 'pxenv_status=%s uniq=%d errfile=%s posix_errno=%s' % (
+                lookup_errno_component(errcodedb.pxenv_status, self.pxenv_status),
+                self.uniq,
+                lookup_errno_component(errcodedb.errfile, self.errfile),
+                lookup_errno_component(errcodedb.posix_errno, self.posix_errno)
+                )
+
+    def __str__(self):
+        return self.prettystr()
+
+def usage():
+    sys.stderr.write('usage: %s ERROR_NUMBER\n' % sys.argv[0])
+    sys.exit(1)
+
+if __name__ == '__main__':
+    if len(sys.argv) != 2:
+        usage()
+
+    try:
+        errno = int(sys.argv[1], 16)
+    except ValueError:
+        usage()
+
+    print Errcode(errno)
+    sys.exit(0)
diff --git a/gpxe/contrib/errcode/gpxebot.py b/gpxe/contrib/errcode/gpxebot.py
new file mode 100644
index 0000000..f975942
--- /dev/null
+++ b/gpxe/contrib/errcode/gpxebot.py
@@ -0,0 +1,124 @@
+#!/usr/bin/env python
+# Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License as
+# published by the Free Software Foundation; either version 2 of the
+# License, or any later version.
+#
+# This program is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+import re
+import socket
+import errcode
+
+HOST = 'irc.freenode.net'
+PORT = 6667
+NICK = 'gpxebot'
+CHAN = '#etherboot'
+NICKSERV_PASSWORD = None
+IDENT = 'gpxebot'
+REALNAME = 'gPXE bot'
+
+ERRCODE_RE = re.compile(r'(errcode|Error)\s+((0x)?[0-9a-fA-F]{8})')
+
+NO_ARGS = -1
+
+handlers = {}
+
+def nick_from_mask(mask):
+    return (mask.find('!') > -1 and mask.split('!', 1)[0]) or mask
+
+def autojoin():
+    del handlers['376']
+    if NICKSERV_PASSWORD:
+        pmsg('nickserv', 'identify %s' % NICKSERV_PASSWORD)
+    if CHAN:
+        cmd('JOIN %s' % CHAN)
+
+def ping(_, arg):
+    cmd('PONG %s' % arg)
+
+def privmsg(_, target, msg):
+    if target == CHAN:
+        replyto = target
+        if msg.find(NICK) == -1:
+            return
+    elif target == NICK:
+        replyto = nick_from_mask(who)
+    m = ERRCODE_RE.search(msg)
+    if m:
+        try:
+            pmsg(replyto, str(errcode.Errcode(int(m.groups()[1], 16))))
+        except ValueError:
+            pass
+    if msg.find('help') > -1:
+        pmsg(replyto, 'I look up gPXE error codes.  Message me like this:')
+        pmsg(replyto, 'errcode 0x12345678  OR  Error 0x12345678')
+
+def add_handler(command, handler, nargs):
+    handlers[command] = (handler, nargs)
+
+def cmd(msg):
+    sock.sendall('%s\r\n' % msg)
+
+def pmsg(target, msg):
+    cmd('PRIVMSG %s :%s' % (target, msg))
+
+def dispatch(args):
+    command = args[0]
+    if command in handlers:
+        h = handlers[command]
+        if h[1] == NO_ARGS:
+            h[0]()
+        elif len(args) == h[1]:
+            h[0](*args)
+
+def parse(line):
+    if line[0] == ':':
+        who, line = line.split(None, 1)
+        who = who[1:]
+    else:
+        who = None
+    args = []
+    while line and line[0] != ':' and line.find(' ') != -1:
+        fields = line.split(None, 1)
+        if len(fields) == 1:
+            fields.append(None)
+        arg, line = fields
+        args.append(arg)
+    if line:
+        if line[0] == ':':
+            args.append(line[1:])
+        else:
+            args.append(line)
+    return who, args
+
+add_handler('376', autojoin, NO_ARGS)
+add_handler('PING', ping, 2)
+add_handler('PRIVMSG', privmsg, 3)
+
+sock = socket.socket()
+sock.connect((HOST, PORT))
+cmd('NICK %s' % NICK)
+cmd('USER %s none none :%s' % (IDENT, REALNAME))
+
+rbuf = ''
+while True:
+    r = sock.recv(4096)
+    if not r:
+        break
+    rbuf += r
+
+    while rbuf.find('\r\n') != -1:
+        line, rbuf = rbuf.split('\r\n', 1)
+        if not line:
+            continue
+        who, args = parse(line)
+        dispatch(args)
diff --git a/gpxe/src/.gitignore b/gpxe/src/.gitignore
index cc8e33e..413f814 100644
--- a/gpxe/src/.gitignore
+++ b/gpxe/src/.gitignore
@@ -2,3 +2,4 @@
 .echocheck
 TAGS*
 bin*
+config-local.h
diff --git a/gpxe/src/Makefile b/gpxe/src/Makefile
index 0591bb0..c0b29ee 100644
--- a/gpxe/src/Makefile
+++ b/gpxe/src/Makefile
@@ -152,8 +152,6 @@ SRCDIRS		+= drivers/bus
 SRCDIRS		+= drivers/net
 SRCDIRS		+= drivers/net/e1000
 SRCDIRS		+= drivers/block
-SRCDIRS		+= drivers/scsi
-SRCDIRS		+= drivers/ata
 SRCDIRS		+= drivers/nvs
 SRCDIRS		+= drivers/bitbash
 SRCDIRS		+= drivers/infiniband
diff --git a/gpxe/src/Makefile.housekeeping b/gpxe/src/Makefile.housekeeping
index fe3addc..bbea2a5 100644
--- a/gpxe/src/Makefile.housekeeping
+++ b/gpxe/src/Makefile.housekeeping
@@ -121,8 +121,8 @@ CFLAGS	+= $(SP_FLAGS)
 CFLAGS	+= -include compiler.h
 
 # config/%.h files are generated from config.h using mkconfig.pl
-config/%.h : config.h
-	$(MKCONFIG) $<
+config/%.h : config*.h
+	$(MKCONFIG) config.h
 CLEANUP	+= config/*.h
 
 # SRCDIRS lists all directories containing source files.
@@ -278,8 +278,8 @@ TGT_PCI_DEVICE	= $(PCI_DEVICE_$(TGT_ROM_NAME))
 #
 TGT_LD_DRIVERS	= $(subst -,_,$(patsubst %,obj_%,$(TGT_DRIVERS)))
 TGT_LD_PREFIX	= obj_$(TGT_PREFIX)prefix
-TGT_LD_IDS	= $(if $(TGT_PCI_VENDOR),pci_vendor_id=$(TGT_PCI_VENDOR)) \
-		  $(if $(TGT_PCI_DEVICE),pci_device_id=$(TGT_PCI_DEVICE))
+TGT_LD_IDS	= pci_vendor_id=$(firstword $(TGT_PCI_VENDOR) 0) \
+		  pci_device_id=$(firstword $(TGT_PCI_DEVICE) 0)
 
 # Calculate linker flags based on link-time options for the current
 # target type (e.g. "bin/dfe538--prism2_pci.zrom.tmp") and derive the
diff --git a/gpxe/src/arch/i386/Makefile b/gpxe/src/arch/i386/Makefile
index da7976d..926daa1 100644
--- a/gpxe/src/arch/i386/Makefile
+++ b/gpxe/src/arch/i386/Makefile
@@ -8,9 +8,7 @@ SRCDIRS		+= arch/i386/core arch/i386/transitions arch/i386/prefix
 SRCDIRS		+= arch/i386/firmware/pcbios
 SRCDIRS		+= arch/i386/image
 SRCDIRS		+= arch/i386/drivers
-SRCDIRS		+= arch/i386/drivers/bus
 SRCDIRS		+= arch/i386/drivers/net
-SRCDIRS		+= arch/i386/drivers/disk
 SRCDIRS		+= arch/i386/interface/pcbios
 SRCDIRS		+= arch/i386/interface/pxe
 
diff --git a/gpxe/src/arch/i386/core/gdbidt.S b/gpxe/src/arch/i386/core/gdbidt.S
new file mode 100644
index 0000000..45d079f
--- /dev/null
+++ b/gpxe/src/arch/i386/core/gdbidt.S
@@ -0,0 +1,209 @@
+/*
+ * Interrupt Descriptor Table (IDT) setup and interrupt handlers for GDB stub.
+ */
+
+#include <virtaddr.h>
+
+#define SIZEOF_I386_REGS	32
+#define SIZEOF_I386_FLAGS	4
+
+/****************************************************************************
+ * Interrupt Descriptor Table
+ ****************************************************************************
+ */
+	.section ".data16"
+	.globl idtr
+idtr:
+idt_limit:
+	.word	idt_length - 1
+idt_base:
+	.long	0
+
+/* IDT entries have the following format:
+ * offset_lo, segment selector, flags, offset_hi
+ *
+ * Since it is not possible to specify relocations in arbitrary
+ * expressions like (int_overflow & 0xffff), we initialise the
+ * IDT with entries in an incorrect format.
+ *
+ * The entries are shuffled into the correct format in init_librm().
+ */
+#define IDT_ENTRY_EMPTY(name) .word 0, 0, 0, 0
+#define IDT_ENTRY_PRESENT(name) \
+	.long	int_##name; \
+	.word	0x8e00, VIRTUAL_CS
+
+.align 16
+idt:
+	IDT_ENTRY_PRESENT(divide_error)
+	IDT_ENTRY_PRESENT(debug_trap)
+	IDT_ENTRY_EMPTY(non_maskable_interrupt)
+	IDT_ENTRY_PRESENT(breakpoint)
+	IDT_ENTRY_PRESENT(overflow)
+	IDT_ENTRY_PRESENT(bound_range_exceeded)
+	IDT_ENTRY_PRESENT(invalid_opcode)
+	IDT_ENTRY_EMPTY(device_not_available)
+	IDT_ENTRY_PRESENT(double_fault)
+	IDT_ENTRY_EMPTY(coprocessor_segment_overrun)
+	IDT_ENTRY_PRESENT(invalid_tss)
+	IDT_ENTRY_PRESENT(segment_not_present)
+	IDT_ENTRY_PRESENT(stack_segment_fault)
+	IDT_ENTRY_PRESENT(general_protection)
+	IDT_ENTRY_PRESENT(page_fault)
+idt_end:
+	.equ	idt_length, idt_end - idt
+
+/* The IDT entries are fixed up (once) in init_librm() */
+idt_fixed:
+	.byte	0
+
+/****************************************************************************
+ * idt_init (real-mode near call, 16-bit real-mode near return address)
+ *
+ * Initialise the IDT, called from init_librm.
+ *
+ * Parameters:
+ *   %eax : IDT base address
+ *
+ * Destroys %ax, %bx, and %di.
+ ****************************************************************************
+ */
+	.section ".text16"
+	.code16
+	.globl idt_init
+idt_init:
+	movl	%eax, idt_base
+	addl	$idt, idt_base
+
+	/* IDT entries are only fixed up once */
+	movb	idt_fixed, %al
+	orb	%al, %al
+	jnz	2f
+	movb	$1, idt_fixed
+
+	/* Shuffle IDT entries into the correct format */
+	movb	$(idt_length / 8), %al
+	movw	$idt, %bx
+	or	%al, %al
+	jz	2f
+1:
+	movw	2(%bx), %di
+	xchg	%di, 6(%bx)
+	movw	%di, 2(%bx)
+	addw	$8, %bx
+	dec	%al
+	jnz	1b
+2:
+	ret
+
+/****************************************************************************
+ * Interrupt handlers
+ ****************************************************************************
+ */
+	.section ".text"
+	.code32
+
+/* POSIX signal numbers for reporting traps to GDB */
+#define SIGILL 4
+#define SIGTRAP 5
+#define SIGBUS 7
+#define SIGFPE 8
+#define SIGSEGV 11
+#define SIGSTKFLT 16
+
+int_divide_error:
+	pushl	$SIGFPE
+	jmp	do_interrupt
+
+int_debug_trap:
+int_breakpoint:
+	pushl	$SIGTRAP
+	jmp	do_interrupt
+
+int_overflow:
+int_bound_range_exceeded:
+	pushl	$SIGSTKFLT
+	jmp	do_interrupt
+
+int_invalid_opcode:
+	pushl	$SIGILL
+	jmp	do_interrupt
+
+int_double_fault:
+	movl	$SIGBUS, (%esp)
+	jmp	do_interrupt
+
+int_invalid_tss:
+int_segment_not_present:
+int_stack_segment_fault:
+int_general_protection:
+int_page_fault:
+	movl	$SIGSEGV, (%esp)
+	jmp	do_interrupt
+
+/* When invoked, the stack contains: eflags, cs, eip, signo. */
+#define IH_OFFSET_GDB_REGS ( 0 )
+#define IH_OFFSET_GDB_EIP ( IH_OFFSET_GDB_REGS + SIZEOF_I386_REGS )
+#define IH_OFFSET_GDB_EFLAGS ( IH_OFFSET_GDB_EIP + 4 )
+#define IH_OFFSET_GDB_SEG_REGS ( IH_OFFSET_GDB_EFLAGS + SIZEOF_I386_FLAGS )
+#define IH_OFFSET_GDB_END ( IH_OFFSET_GDB_SEG_REGS + 6 * 4 )
+#define IH_OFFSET_SIGNO ( IH_OFFSET_GDB_END )
+#define IH_OFFSET_OLD_EIP ( IH_OFFSET_SIGNO + 4 )
+#define IH_OFFSET_OLD_CS ( IH_OFFSET_OLD_EIP + 4 )
+#define IH_OFFSET_OLD_EFLAGS ( IH_OFFSET_OLD_CS + 4 )
+#define IH_OFFSET_END ( IH_OFFSET_OLD_EFLAGS + 4 )
+
+/* We also access the stack whilst still storing or restoring
+ * the register snapshot.  Since ESP is in flux, we need
+ * special offsets.
+ */
+#define IH_OFFSET_FLUX_OLD_CS ( IH_OFFSET_OLD_CS - 44 )
+#define IH_OFFSET_FLUX_OLD_EFLAGS ( IH_OFFSET_OLD_EFLAGS - 40 )
+#define IH_OFFSET_FLUX_OLD_EIP ( IH_OFFSET_OLD_EIP - 36 )
+#define IH_OFFSET_FLUX_END ( IH_OFFSET_END - 20 )
+do_interrupt:
+	/* Store CPU state in GDB register snapshot */
+	pushl	%gs
+	pushl	%fs
+	pushl	%es
+	pushl	%ds
+	pushl	%ss
+	pushl	IH_OFFSET_FLUX_OLD_CS(%esp)
+	pushl	IH_OFFSET_FLUX_OLD_EFLAGS(%esp)
+	pushl	IH_OFFSET_FLUX_OLD_EIP(%esp)
+	pushl	%edi
+	pushl	%esi
+	pushl	%ebp
+	leal	IH_OFFSET_FLUX_END(%esp), %edi
+	pushl	%edi /* old ESP */
+	pushl	%ebx
+	pushl	%edx
+	pushl	%ecx
+	pushl	%eax
+
+	/* Call GDB stub exception handler */
+	pushl	%esp
+	pushl	(IH_OFFSET_SIGNO + 4)(%esp)
+	call	gdbstub_handler
+	addl	$8, %esp
+
+	/* Restore CPU state from GDB register snapshot */
+	popl	%eax
+	popl	%ecx
+	popl	%edx
+	popl	%ebx
+	addl	$4, %esp /* Changing ESP currently not supported */
+	popl	%ebp
+	popl	%esi
+	popl	%edi
+	popl	IH_OFFSET_FLUX_OLD_EIP(%esp)
+	popl	IH_OFFSET_FLUX_OLD_EFLAGS(%esp)
+	popl	IH_OFFSET_FLUX_OLD_CS(%esp)
+	popl	%ss
+	popl	%ds
+	popl	%es
+	popl	%fs
+	popl	%gs
+
+	addl	$4, %esp /* drop signo */
+	iret
diff --git a/gpxe/src/arch/i386/core/gdbsym.c b/gpxe/src/arch/i386/core/gdbsym.c
deleted file mode 100644
index 2da1a1b..0000000
--- a/gpxe/src/arch/i386/core/gdbsym.c
+++ /dev/null
@@ -1,33 +0,0 @@
-#include <stdio.h>
-#include <gpxe/init.h>
-#include <console.h>
-#include <realmode.h>
-
-extern char __text[];
-extern char __rodata[];
-extern char __data[];
-extern char __bss[];
-extern char __text16[];
-extern char __data16[];
-
-
-static void gdb_symbol_line ( void ) {
-	printf ( "Commands to start up gdb:\n\n" );
-	printf ( "gdb\n" );
-	printf ( "target remote localhost:1234\n" );
-	printf ( "set confirm off\n" );
-	printf ( "add-symbol-file symbols %#lx", virt_to_phys ( __text ) );
-	printf ( " -s .rodata %#lx", virt_to_phys ( __rodata ) );
-	printf ( " -s .data %#lx", virt_to_phys ( __data ) );
-	printf ( " -s .bss %#lx", virt_to_phys ( __bss ) );
-	printf ( " -s .text16 %#x", ( ( rm_cs << 4 ) + (int)__text16 ) );
-	printf ( " -s .data16 %#x", ( ( rm_ds << 4 ) + (int)__data16 ) );
-	printf ( "\n" );
-	printf ( "add-symbol-file symbols 0\n" );
-	printf ( "set confirm on\n" );
-	getkey();
-}
-
-struct startup_fn gdb_startup_fn __startup_fn ( STARTUP_NORMAL ) = {
-	.startup = gdb_symbol_line,
-};
diff --git a/gpxe/src/arch/i386/drivers/net/undinet.c b/gpxe/src/arch/i386/drivers/net/undinet.c
index e3b9f85..2c66c2f 100644
--- a/gpxe/src/arch/i386/drivers/net/undinet.c
+++ b/gpxe/src/arch/i386/drivers/net/undinet.c
@@ -715,6 +715,9 @@ int undinet_probe ( struct undi_device *undi ) {
 		undinic->hacks |= UNDI_HACK_EB54;
 	}
 
+	/* Mark as link up; we don't handle link state */
+	netdev_link_up ( netdev );
+
 	/* Register network device */
 	if ( ( rc = register_netdev ( netdev ) ) != 0 )
 		goto err_register;
diff --git a/gpxe/src/arch/i386/image/pxe_image.c b/gpxe/src/arch/i386/image/pxe_image.c
index 9e634f1..77fa046 100644
--- a/gpxe/src/arch/i386/image/pxe_image.c
+++ b/gpxe/src/arch/i386/image/pxe_image.c
@@ -84,6 +84,14 @@ int pxe_load ( struct image *image ) {
 	size_t memsz = image->len;
 	int rc;
 
+	/* Images too large to fit in base memory cannot be PXE
+	 * images.  We include this check to help prevent unrecognised
+	 * images from being marked as PXE images, since PXE images
+	 * have no signature we can check against.
+	 */
+	if ( filesz > ( 0xa0000 - 0x7c00 ) )
+		return -ENOEXEC;
+
 	/* There are no signature checks for PXE; we will accept anything */
 	if ( ! image->type )
 		image->type = &pxe_image_type;
diff --git a/gpxe/src/arch/i386/include/gdbmach.h b/gpxe/src/arch/i386/include/gdbmach.h
new file mode 100644
index 0000000..9f6dc8f
--- /dev/null
+++ b/gpxe/src/arch/i386/include/gdbmach.h
@@ -0,0 +1,51 @@
+#ifndef GDBMACH_H
+#define GDBMACH_H
+
+/** @file
+ *
+ * GDB architecture specifics
+ *
+ * This file declares functions for manipulating the machine state and
+ * debugging context.
+ *
+ */
+
+typedef uint32_t gdbreg_t;
+
+/* The register snapshot, this must be in sync with interrupt handler and the
+ * GDB protocol. */
+enum {
+	GDBMACH_EAX,
+	GDBMACH_ECX,
+	GDBMACH_EDX,
+	GDBMACH_EBX,
+	GDBMACH_ESP,
+	GDBMACH_EBP,
+	GDBMACH_ESI,
+	GDBMACH_EDI,
+	GDBMACH_EIP,
+	GDBMACH_EFLAGS,
+	GDBMACH_CS,
+	GDBMACH_SS,
+	GDBMACH_DS,
+	GDBMACH_ES,
+	GDBMACH_FS,
+	GDBMACH_GS,
+	GDBMACH_NREGS,
+	GDBMACH_SIZEOF_REGS = GDBMACH_NREGS * sizeof ( gdbreg_t )
+};
+
+static inline void gdbmach_set_pc ( gdbreg_t *regs, gdbreg_t pc ) {
+	regs [ GDBMACH_EIP ] = pc;
+}
+
+static inline void gdbmach_set_single_step ( gdbreg_t *regs, int step ) {
+	regs [ GDBMACH_EFLAGS ] &= ~( 1 << 8 ); /* Trace Flag (TF) */
+	regs [ GDBMACH_EFLAGS ] |= ( step << 8 );
+}
+
+static inline void gdbmach_breakpoint ( void ) {
+	__asm__ __volatile__ ( "int $3\n" );
+}
+
+#endif /* GDBMACH_H */
diff --git a/gpxe/src/arch/i386/prefix/libprefix.S b/gpxe/src/arch/i386/prefix/libprefix.S
index deea5ab..cb09111 100644
--- a/gpxe/src/arch/i386/prefix/libprefix.S
+++ b/gpxe/src/arch/i386/prefix/libprefix.S
@@ -48,10 +48,9 @@
  *
  * Parameters:
  *   %al : character to print
+ *   %ds:di : output buffer (or %di=0 to print to console)
  * Returns:
- *   Nothing
- * Corrupts:
- *   %ax
+ *   %ds:di : next character in output buffer (if applicable)
  *****************************************************************************
  */
 	.section ".prefix.lib"
@@ -59,19 +58,27 @@
 	.globl	print_character
 print_character:
 	/* Preserve registers */
+	pushw	%ax
 	pushw	%bx
 	pushw	%bp
-	/* Print character */
+	/* If %di is non-zero, write character to buffer and exit */
+	testw	%di, %di
+	jz	1f
+	movb	%al, %ds:(%di)
+	incw	%di
+	jmp	3f
+1:	/* Print character */
 	movw	$0x0007, %bx		/* page 0, attribute 7 (normal) */
 	movb	$0x0e, %ah		/* write char, tty mode */
 	cmpb	$0x0a, %al		/* '\n'? */
-	jne	1f
+	jne	2f
 	int	$0x10
 	movb	$0x0d, %al
-1:	int	$0x10
+2:	int	$0x10
 	/* Restore registers and return */
-	popw	%bp
+3:	popw	%bp
 	popw	%bx
+	popw	%ax
 	ret
 	.size	print_character, . - print_character
 
@@ -80,8 +87,10 @@ print_character:
  *
  * Parameters:
  *   %ds:si : string to print
+ *   %ds:di : output buffer (or %di=0 to print to console)
  * Returns:
  *   %ds:si : character after terminating NUL
+ *   %ds:di : next character in output buffer (if applicable)
  *****************************************************************************
  */
 	.section ".prefix.lib"
@@ -109,8 +118,9 @@ print_message:
  *   %al : byte to print
  *   %ax : word to print
  *   %eax : dword to print
+ *   %ds:di : output buffer (or %di=0 to print to console)
  * Returns:
- *   Nothing
+ *   %ds:di : next character in output buffer (if applicable)
  *****************************************************************************
  */
 	.section ".prefix.lib"
@@ -151,6 +161,44 @@ print_hex_nibble:
 	ret
 	.size	print_hex_nibble, . - print_hex_nibble
 
+/*****************************************************************************
+ * Utility function: print PCI bus:dev.fn
+ *
+ * Parameters:
+ *   %ax : PCI bus:dev.fn to print
+ *   %ds:di : output buffer (or %di=0 to print to console)
+ * Returns:
+ *   %ds:di : next character in output buffer (if applicable)
+ *****************************************************************************
+ */
+	.section ".prefix.lib"
+	.code16
+	.globl	print_pci_busdevfn
+print_pci_busdevfn:
+	/* Preserve registers */
+	pushw	%ax
+	/* Print bus */
+	xchgb	%al, %ah
+	call	print_hex_byte
+	/* Print ":" */
+	movb	$':', %al
+	call	print_character
+	/* Print device */
+	movb	%ah, %al
+	shrb	$3, %al
+	call	print_hex_byte
+	/* Print "." */
+	movb	$'.', %al
+	call	print_character
+	/* Print function */
+	movb	%ah, %al
+	andb	$0x07, %al
+	call	print_hex_nibble
+	/* Restore registers and return */
+	popw	%ax
+	ret
+	.size	print_pci_busdevfn, . - print_pci_busdevfn
+
 /****************************************************************************
  * pm_call (real-mode near call)
  *
diff --git a/gpxe/src/arch/i386/prefix/pxeprefix.S b/gpxe/src/arch/i386/prefix/pxeprefix.S
index d7125b6..302f8e5 100644
--- a/gpxe/src/arch/i386/prefix/pxeprefix.S
+++ b/gpxe/src/arch/i386/prefix/pxeprefix.S
@@ -50,6 +50,7 @@
 	cld
 	/* Print welcome message */
 	movw	$10f, %si
+	xorw	%di, %di
 	call	print_message
 	.section ".prefix.data"
 10:	.asciz	"PXE->EB:"
@@ -61,24 +62,23 @@
  */
 detect_pxenv:
 	/* Signature check */
-	les	pxenv_segoff, %di
-	cmpl	$0x4e455850, %es:(%di)	/* 'PXEN' signature */
+	les	pxenv_segoff, %bx
+	cmpl	$0x4e455850, %es:(%bx)	/* 'PXEN' signature */
 	jne	no_pxenv
-	cmpw	$0x2b56, %es:4(%di)	/* 'V+' signature */
+	cmpw	$0x2b56, %es:4(%bx)	/* 'V+' signature */
 	jne	no_pxenv
 	/* Record entry point and UNDI segments */
-	pushl	%es:0x0a(%di)		/* Entry point */
+	pushl	%es:0x0a(%bx)		/* Entry point */
 	popl	entry_segoff
-	pushw	%es:0x24(%di)		/* UNDI code segment */
-	pushw	%es:0x26(%di)		/* UNDI code size */
+	pushw	%es:0x24(%bx)		/* UNDI code segment */
+	pushw	%es:0x26(%bx)		/* UNDI code size */
 	popl	undi_code_segoff
-	pushw	%es:0x20(%di)		/* UNDI data segment */
-	pushw	%es:0x22(%di)		/* UNDI data size */
+	pushw	%es:0x20(%bx)		/* UNDI data segment */
+	pushw	%es:0x22(%bx)		/* UNDI data size */
 	popl	undi_data_segoff
 	/* Print "PXENV+ at <address>" */
 	movw	$10f, %si
 	call	print_message
-	movw	%bx, %di
 	call	print_segoff
 	movb	$',', %al
 	call	print_character
@@ -99,20 +99,20 @@ no_pxenv:
  */
 detect_ppxe:
 	/* Signature check */
-	les	ppxe_segoff, %di
-	cmpl	$0x45585021, %es:(%di)	/* '!PXE' signature */
+	les	ppxe_segoff, %bx
+	cmpl	$0x45585021, %es:(%bx)	/* '!PXE' signature */
 	jne	no_ppxe
 	/* Record structure address, entry point, and UNDI segments */
 	pushw	%es
 	popw	ppxe_segment
-	movw	%di, ppxe_offset
-	pushl	%es:0x10(%di)		/* Entry point */
+	movw	%bx, ppxe_offset
+	pushl	%es:0x10(%bx)		/* Entry point */
 	popl	entry_segoff
-	pushw	%es:0x30(%di)		/* UNDI code segment */
-	pushw	%es:0x36(%di)		/* UNDI code size */
+	pushw	%es:0x30(%bx)		/* UNDI code segment */
+	pushw	%es:0x36(%bx)		/* UNDI code size */
 	popl	undi_code_segoff
-	pushw	%es:0x28(%di)		/* UNDI data segment */
-	pushw	%es:0x2e(%di)		/* UNDI data size */
+	pushw	%es:0x28(%bx)		/* UNDI data segment */
+	pushw	%es:0x2e(%bx)		/* UNDI data size */
 	popl	undi_data_segoff
 	/* Print "!PXE at <address>" */
 	movw	$10f, %si
@@ -180,7 +180,7 @@ print_structure_information:
 	/* Print entry point */
 	movw	$10f, %si
 	call	print_message
-	les	entry_segoff, %di
+	les	entry_segoff, %bx
 	call	print_segoff
 	.section ".prefix.data"
 10:	.asciz	" entry point at "
@@ -188,7 +188,7 @@ print_structure_information:
 	/* Print UNDI code segment */
 	movw	$10f, %si
 	call	print_message
-	les	undi_code_segoff, %di
+	les	undi_code_segoff, %bx
 	call	print_segoff
 	.section ".prefix.data"
 10:	.asciz	"\n         UNDI code segment "
@@ -196,7 +196,7 @@ print_structure_information:
 	/* Print UNDI data segment */
 	movw	$10f, %si
 	call	print_message
-	les	undi_data_segoff, %di
+	les	undi_data_segoff, %bx
 	call	print_segoff
 	.section ".prefix.data"
 10:	.asciz	", data segment "
@@ -285,8 +285,8 @@ unload_base_code:
 	call	print_pxe_error
 	jmp	99f
 1:	/* Free base memory used by PXE base code */
-	movw	%fs:(0x13), %si
-	movw	undi_fbms_start, %di
+	movw	undi_fbms_start, %ax
+	movw	%fs:(0x13), %bx
 	call	free_basemem
 99:
 
@@ -303,8 +303,8 @@ unload_undi:
 	call	print_pxe_error
 	jmp	99f
 1:	/* Free base memory used by UNDI */
-	movw	undi_fbms_start, %si
-	movw	undi_fbms_end, %di
+	movw	undi_fbms_end, %ax
+	movw	undi_fbms_start, %bx
 	call	free_basemem
 	/* Clear UNDI_FL_STARTED */
 	andw	$~UNDI_FL_STARTED, flags
@@ -338,9 +338,10 @@ finished:
  * Subroutine: print segment:offset address
  *
  * Parameters:
- *   %es:%di : segment:offset address to print
+ *   %es:%bx : segment:offset address to print
+ *   %ds:di : output buffer (or %di=0 to print to console)
  * Returns:
- *   Nothing
+ *   %ds:di : next character in output buffer (if applicable)
  *****************************************************************************
  */
 print_segoff:
@@ -351,7 +352,7 @@ print_segoff:
 	call	print_hex_word
 	movb	$':', %al
 	call	print_character
-	movw	%di, %ax
+	movw	%bx, %ax
 	call	print_hex_word
 	/* Restore registers and return */
 	popw	%ax
@@ -362,8 +363,9 @@ print_segoff:
  *
  * Parameters:
  *   %ax : word to print
+ *   %ds:di : output buffer (or %di=0 to print to console)
  * Returns:
- *   Nothing
+ *   %ds:di : next character in output buffer (if applicable)
  *****************************************************************************
  */
 print_word:
@@ -393,43 +395,10 @@ print_word:
 	ret
 	
 /*****************************************************************************
- * Subroutine: print PCI bus:dev.fn
- *
- * Parameters:
- *   %ax : PCI bus:dev.fn to print
- * Returns:
- *   Nothing
- *****************************************************************************
- */
-print_pci_busdevfn:
-	/* Preserve registers */
-	pushw	%ax
-	/* Print bus */
-	xchgb	%al, %ah
-	call	print_hex_byte
-	/* Print ":" */
-	movb	$':', %al
-	call	print_character
-	/* Print device */
-	movb	%ah, %al
-	shrb	$3, %al
-	call	print_hex_byte
-	/* Print "." */
-	movb	$'.', %al
-	call	print_character
-	/* Print function */
-	movb	%ah, %al
-	andb	$0x07, %al
-	call	print_hex_nibble
-	/* Restore registers and return */
-	popw	%ax
-	ret	
-
-/*****************************************************************************
  * Subroutine: zero 1kB block of base memory
  *
  * Parameters:
- *   %si : block to zero (in kB)
+ *   %bx : block to zero (in kB)
  * Returns:
  *   Nothing
  *****************************************************************************
@@ -441,7 +410,7 @@ zero_kb:
 	pushw	%di
 	pushw	%es
 	/* Zero block */
-	movw	%si, %ax
+	movw	%bx, %ax
 	shlw	$6, %ax
 	movw	%ax, %es
 	movw	$0x400, %cx
@@ -459,33 +428,31 @@ zero_kb:
  * Subroutine: free and zero base memory
  *
  * Parameters:
- *   %si : Expected current free base memory counter (in kB)
- *   %di : Desired new free base memory counter (in kB)
+ *   %ax : Desired new free base memory counter (in kB)
+ *   %bx : Expected current free base memory counter (in kB)
  *   %fs : BIOS data segment (0x40)
  * Returns:
- *   %ax : Actual new free base memory counter (in kB)
+ *   None
  *
- * The base memory from %si kB to %di kB is unconditionally zeroed.
+ * The base memory from %bx kB to %ax kB is unconditionally zeroed.
  * It will be freed if and only if the expected current free base
- * memory counter (%si) matches the actual current free base memory
+ * memory counter (%bx) matches the actual current free base memory
  * counter in 0x40:0x13; if this does not match then the memory will
  * be leaked.
  *****************************************************************************
  */
 free_basemem:
 	/* Zero base memory */
-	pushw	%si
-1:	cmpw	%si, %di
+	pushw	%bx
+1:	cmpw	%bx, %ax
 	je	2f
 	call	zero_kb
-	incw	%si
+	incw	%bx
 	jmp	1b
-2:	popw	%si
+2:	popw	%bx
 	/* Free base memory */
-	movw	%fs:(0x13), %ax		/* Current FBMS to %ax */
-	cmpw	%ax, %si		/* Update FBMS only if "old" value  */
+	cmpw	%fs:(0x13), %bx		/* Update FBMS only if "old" value  */
 	jne	1f			/* is correct			    */
-	movw	%di, %ax
 1:	movw	%ax, %fs:(0x13)
 	ret
 
diff --git a/gpxe/src/arch/i386/prefix/romprefix.S b/gpxe/src/arch/i386/prefix/romprefix.S
index d37cce9..19e6a9b 100644
--- a/gpxe/src/arch/i386/prefix/romprefix.S
+++ b/gpxe/src/arch/i386/prefix/romprefix.S
@@ -85,11 +85,24 @@ pnpheader:
 	.equ pnpheader_len, . - pnpheader
 	.size pnpheader, . - pnpheader
 
+/* Manufacturer string */
 mfgstr:
 	.asciz	"http://etherboot.org"
 	.size mfgstr, . - mfgstr
+
+/* Product string
+ *
+ * Defaults to "gPXE".  If the ROM image is writable at initialisation
+ * time, it will be filled in to include the PCI bus:dev.fn number of
+ * the card as well.
+ */
 prodstr:
-	.asciz	"gPXE"
+	.ascii	"gPXE"
+prodstr_separator:
+	.byte	0
+	.ascii	"(PCI "
+prodstr_pci_id:
+	.asciz	"xx:xx.x)"		/* Filled in by init code */
 	.size prodstr, . - prodstr
 	
 undiheader:
@@ -117,27 +130,37 @@ init:
 	pushaw
 	pushw	%ds
 	pushw	%es
+	pushw	%fs
 	cld
 	pushw	%cs
 	popw	%ds
+	pushw	$0x40
+	popw	%fs
+	movw	%di, %bx
+	xorw	%di, %di
 	/* Print message as early as possible */
 	movw	$init_message, %si
 	call	print_message
+	call	print_pci_busdevfn
+	/* Fill in product name string, if possible */
+	movw	$prodstr_pci_id, %di
+	call	print_pci_busdevfn
+	movb	$' ', prodstr_separator
+	xorw	%di, %di
 	/* Check for PnP BIOS */
-	testw	$0x0f, %di	/* PnP signature must be aligned - bochs    */
+	testw	$0x0f, %bx	/* PnP signature must be aligned - bochs    */
 	jnz	hook_int19	/* uses unalignment to indicate 'fake' PnP. */
-	cmpl	$PNP_SIGNATURE, %es:0(%di)
+	cmpl	$PNP_SIGNATURE, %es:0(%bx)
 	jne	hook_int19
 	/* Is PnP: print PnP message */
 	movw	$init_message_pnp, %si
 	call	print_message
-	xchgw	%bx, %bx
 	/* Check for BBS */
-	pushw	%es:0x1b(%di)	/* Real-mode data segment */
+	pushw	%es:0x1b(%bx)	/* Real-mode data segment */
 	pushw	%ds		/* &(bbs_version) */
 	pushw	$bbs_version
 	pushw	$PNP_GET_BBS_VERSION
-	lcall	*%es:0xd(%di)
+	lcall	*%es:0xd(%bx)
 	addw	$8, %sp
 	testw	%ax, %ax
 	jne	hook_int19
@@ -155,18 +178,18 @@ hook_int19:
 	popl	%es:( 0x19 * 4 )
 hook_bbs:
 	/* Check for PMM */
-	movw	$( 0xe000 - 1 ), %di
+	movw	$( 0xe00 - 1 ), %bx
 pmm_scan:
-	incw	%di
+	incw	%bx
 	jz	no_pmm
-	movw	%di, %es
+	movw	%bx, %es
 	cmpl	$PMM_SIGNATURE, %es:0
 	jne	pmm_scan
-	xorw	%bx, %bx
+	xorw	%dx, %dx
 	xorw	%si, %si
 	movzbw	%es:5, %cx
 1:	es lodsb
-	addb	%al, %bl
+	addb	%al, %dl
 	loop	1b
 	jnz	pmm_scan
 	/* PMM found: print PMM message */
@@ -207,11 +230,53 @@ gotpmm:	/* PMM allocation succeeded: copy ROM to PMM block */
 	loop	1b
 	subb	%bl, checksum
 	popal
-no_pmm:
-	/* Print CRLF to terminate messages */
-	movw	$'\n', %ax
-	call	print_character
+no_pmm:	/* Prompt for POST-time shell */
+	movw	$init_message_prompt, %si
+	call	print_message
+	/* Empty the keyboard buffer before waiting for input */
+empty_keyboard_buffer:
+	movb	$0x01, %ah
+	int	$0x16
+	jz	1f
+	xorw	%ax, %ax
+	int	$0x16
+	jmp	empty_keyboard_buffer
+1:	/* Wait for up to 3s for a key press */
+	movw	$(18 * 3), %cx	/* Approx 3s worth of timer ticks */
+wait_for_key:
+	decw	%cx
+	jz	no_key_pressed
+	/* Wait for timer tick to be updated */
+	movl	%fs:(0x6c), %eax
+1:	pushf
+	sti
+	hlt
+	popf
+	cmpl	%fs:(0x6c), %eax
+	je	1b
+	/* Check to see if a key was pressed */
+	movb	$0x01, %ah
+	int	$0x16
+	jz	wait_for_key
+	/* Check to see if key was Ctrl-B */
+	cmpb	$0x02, %al
+	je	1f
+	/* Key was not Ctrl-B: remove from buffer and stop waiting */
+	xorw	%ax, %ax
+	int	$0x16
+	jmp	no_key_pressed
+1:	/* Key was Ctrl-B: leave in keyboard buffer and invoke gPXE.
+	 * The keypress will be picked up by the initial shell
+	 * prompt, and we will drop into a shell.
+	 */
+	pushw	%cs
+	call	exec
+no_key_pressed:
+	/* Print blank lines to terminate messages */
+	movw	$init_message_end, %si
+	call	print_message
 	/* Restore registers */
+	popw	%fs
 	popw	%es
 	popw	%ds
 	popaw
@@ -221,23 +286,29 @@ no_pmm:
 	.size init, . - init
 
 init_message:
-	.asciz	"gPXE (http://etherboot.org) -"
+	.asciz	"gPXE (http://etherboot.org) - PCI "
 	.size	init_message, . - init_message
 init_message_pnp:
 	.asciz	" PnP"
-	.size init_message_pnp, . - init_message_pnp
+	.size	init_message_pnp, . - init_message_pnp
 init_message_bbs:
 	.asciz	" BBS"
-	.size init_message_bbs, . - init_message_bbs
+	.size	init_message_bbs, . - init_message_bbs
 init_message_pmm:
 	.asciz	" PMM"
-	.size init_message_pmm, . - init_message_pmm
+	.size	init_message_pmm, . - init_message_pmm
 init_message_pmm_failed:
 	.asciz	"(failed)"
-	.size init_message_pmm_failed, . - init_message_pmm_failed
+	.size	init_message_pmm_failed, . - init_message_pmm_failed
 init_message_int19:
 	.asciz	" INT19"
-	.size init_message_int19, . - init_message_int19
+	.size	init_message_int19, . - init_message_int19
+init_message_prompt:
+	.asciz	"\nPress Ctrl-B to configure gPXE..."
+	.size	init_message_prompt, . - init_message_prompt
+init_message_end:
+	.asciz	"\n\n\n"
+	.size	init_message_end, . - init_message_end
 
 /* ROM image location
  *
@@ -292,6 +363,7 @@ exec:	/* Set %ds = %cs */
 
 	/* Print message as soon as possible */
 	movw	$exec_message, %si
+	xorw	%di, %di
 	call	print_message
 
 	/* Store magic word on BIOS stack and remember BIOS %ss:sp */
@@ -340,7 +412,7 @@ exec:	/* Set %ds = %cs */
 	.previous
 
 exec_message:
-	.asciz	"gPXE starting boot\n"
+	.asciz	"Entering gPXE\n"
 	.size exec_message, . - exec_message
 
 /* UNDI loader
diff --git a/gpxe/src/arch/i386/transitions/librm.S b/gpxe/src/arch/i386/transitions/librm.S
index b1f9dd5..45e0d0f 100644
--- a/gpxe/src/arch/i386/transitions/librm.S
+++ b/gpxe/src/arch/i386/transitions/librm.S
@@ -50,6 +50,7 @@
 	.section ".data16"
 	.align 16
 gdt:
+gdtr:		/* The first GDT entry is unused, the GDTR can fit here. */
 gdt_limit:		.word gdt_length - 1
 gdt_base:		.long 0
 			.word 0 /* padding */
@@ -127,7 +128,7 @@ init_librm:
 	addr32 leal	(%eax, %edi), %ebx
 	movl	%ebx, _text16
 
-	/* Store rm_ds and _data16, set up real_ds segment and set GDT base */
+	/* Store rm_ds and _data16, set up real_ds segment */
 	xorl	%eax, %eax
 	movw	%ds, %ax
 	movw	%ax, %cs:rm_ds
@@ -136,9 +137,12 @@ init_librm:
 	call	set_seg_base
 	addr32 leal	(%eax, %edi), %ebx
 	movl	%ebx, _data16
-	addl	$gdt, %eax
+
+	/* Set GDT and IDT base */
 	movl	%eax, gdt_base
-		
+	addl	$gdt, gdt_base
+	call	idt_init
+
 	/* Restore registers */
 	negl	%edi
 	popl	%ebx
@@ -147,14 +151,16 @@ init_librm:
 
 	.section ".text16"
 	.code16
+	.weak idt_init
 set_seg_base:
 1:	movw	%ax, 2(%bx)
 	rorl	$16, %eax
 	movb	%al, 4(%bx)
 	movb	%ah, 7(%bx)
 	roll	$16, %eax
+idt_init: /* Reuse the return opcode here */
 	ret
-	
+
 /****************************************************************************
  * real_to_prot (real-mode near call, 32-bit virtual return address)
  *
@@ -197,7 +203,8 @@ real_to_prot:
 
 	/* Switch to protected mode */
 	cli
-	data32 lgdt	gdt
+	data32 lgdt	gdtr
+	data32 lidt	idtr
 	movl	%cr0, %eax
 	orb	$CR0_PE, %al
 	movl	%eax, %cr0
@@ -232,6 +239,14 @@ real_to_prot:
 	/* Return to virtual address */
 	ret
 
+	/* Default IDTR with no interrupts */
+	.section ".data16"
+	.weak idtr
+idtr:
+rm_idtr:
+	.word 0xffff /* limit */
+	.long 0 /* base */
+
 /****************************************************************************
  * prot_to_real (protected-mode near call, 32-bit real-mode return address)
  *
@@ -300,6 +315,9 @@ p2r_jump_target:
 	movw	%bp, %ss
 	movl	%edx, %esp
 
+	/* Reset IDTR to the real-mode defaults */
+	lidt	rm_idtr
+
 	/* Return to real-mode address */
 	data32 ret
 
@@ -318,7 +336,7 @@ rm_cs:	.word 0
 	.globl rm_ds
 	.section ".text16.data"
 rm_ds:	.word 0
-	
+
 /****************************************************************************
  * prot_call (real-mode far call, 16-bit real-mode far return address)
  *
@@ -354,7 +372,8 @@ rm_ds:	.word 0
  */
 
 #define PC_OFFSET_GDT ( 0 )
-#define PC_OFFSET_IX86 ( PC_OFFSET_GDT + 8 /* pad to 8 to keep alignment */ )
+#define PC_OFFSET_IDT ( PC_OFFSET_GDT + 8 /* pad to 8 to keep alignment */ )
+#define PC_OFFSET_IX86 ( PC_OFFSET_IDT + 8 /* pad to 8 to keep alignment */ )
 #define PC_OFFSET_RETADDR ( PC_OFFSET_IX86 + SIZEOF_I386_ALL_REGS )
 #define PC_OFFSET_FUNCTION ( PC_OFFSET_RETADDR + 4 )
 #define PC_OFFSET_END ( PC_OFFSET_FUNCTION + 4 )
@@ -372,8 +391,9 @@ prot_call:
 	pushw	%ds
 	pushw	%ss
 	pushw	%cs
-	subw	$8, %sp
+	subw	$16, %sp
 	movw	%sp, %bp
+	sidt	8(%bp)
 	sgdt	(%bp)
 
 	/* For sanity's sake, clear the direction flag as soon as possible */
@@ -402,10 +422,11 @@ prot_call:
 	.section ".text16"
 	.code16
 1:	
-	/* Reload GDT, restore registers and flags and return */
+	/* Reload GDT and IDT, restore registers and flags and return */
 	movw	%sp, %bp
 	lgdt	(%bp)
-	addw	$12, %sp /* also skip %cs and %ss */
+	lidt	8(%bp)
+	addw	$20, %sp /* also skip %cs and %ss */
 	popw	%ds
 	popw	%es
 	popw	%fs
@@ -495,7 +516,7 @@ real_call:
 	 */
 	.section ".data16"
 rc_function:	.word 0, 0
-	
+
 /****************************************************************************
  * Stored real-mode and protected-mode stack pointers
  *
diff --git a/gpxe/src/config.h b/gpxe/src/config.h
index 9a447ad..2d0980b 100644
--- a/gpxe/src/config.h
+++ b/gpxe/src/config.h
@@ -164,6 +164,8 @@
 #undef	BUILD_ID		/* Include a custom build ID string,
 				 * e.g "test-foo" */
 #undef	NULL_TRAP		/* Attempt to catch NULL function calls */
-#undef	DUMP_GDBSYM		/* Dump GDB symbol table information */
+#undef	GDBSTUB			/* Remote GDB debugging */
 
 /* @END general.h */
+
+/* @TRYSOURCE config-local.h */
diff --git a/gpxe/src/core/config.c b/gpxe/src/core/config.c
index ffd1125..01f709c 100644
--- a/gpxe/src/core/config.c
+++ b/gpxe/src/core/config.c
@@ -54,7 +54,7 @@
 REQUIRE_OBJECT ( bios_console );
 #endif
 #ifdef CONSOLE_SERIAL
-REQUIRE_OBJECT ( serial );
+REQUIRE_OBJECT ( serial_console );
 #endif
 #ifdef CONSOLE_DIRECT_VGA
 REQUIRE_OBJECT ( video_subr );
@@ -198,6 +198,6 @@ REQUIRE_OBJECT ( sanboot_cmd );
 #ifdef NULL_TRAP
 REQUIRE_OBJECT ( nulltrap );
 #endif
-#ifdef DUMP_GDBSYM
-REQUIRE_OBJECT ( gdbsym );
+#ifdef GDBSTUB
+REQUIRE_OBJECT ( gdbidt );
 #endif
diff --git a/gpxe/src/core/gdbstub.c b/gpxe/src/core/gdbstub.c
new file mode 100644
index 0000000..213887b
--- /dev/null
+++ b/gpxe/src/core/gdbstub.c
@@ -0,0 +1,330 @@
+/*
+ * Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/**
+ * @file
+ *
+ * GDB stub for remote debugging
+ *
+ */
+
+#include <stdlib.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <assert.h>
+#include <gpxe/process.h>
+#include <gpxe/serial.h>
+#include "gdbmach.h"
+
+enum {
+	POSIX_EINVAL = 0x1c /* used to report bad arguments to GDB */
+};
+
+struct gdbstub {
+	int signo;
+	gdbreg_t *regs;
+	int exit_handler; /* leave interrupt handler */
+
+	void ( * parse ) ( struct gdbstub *stub, char ch );
+	uint8_t cksum1;
+
+	/* Buffer for payload data when parsing a packet.  Once the
+	 * packet has been received, this buffer is used to hold
+	 * the reply payload. */
+	char payload [ 256 ];
+	int len;
+};
+
+/* Packet parser states */
+static void gdbstub_state_new ( struct gdbstub *stub, char ch );
+static void gdbstub_state_data ( struct gdbstub *stub, char ch );
+static void gdbstub_state_cksum1 ( struct gdbstub *stub, char ch );
+static void gdbstub_state_cksum2 ( struct gdbstub *stub, char ch );
+static void gdbstub_state_wait_ack ( struct gdbstub *stub, char ch );
+
+static uint8_t gdbstub_from_hex_digit ( char ch ) {
+	return ( isdigit ( ch ) ? ch - '0' : tolower ( ch ) - 'a' + 0xa ) & 0xf;
+}
+
+static uint8_t gdbstub_to_hex_digit ( uint8_t b ) {
+	b &= 0xf;
+	return ( b < 0xa ? '0' : 'a' - 0xa ) + b;
+}
+
+static void gdbstub_from_hex_buf ( char *dst, char *src, int len ) {
+	while ( len-- > 0 ) {
+		*dst = gdbstub_from_hex_digit ( *src++ );
+		if ( len-- > 0 ) {
+			*dst = (*dst << 4) | gdbstub_from_hex_digit ( *src++ );
+		}
+		dst++;
+	}
+}
+
+static void gdbstub_to_hex_buf ( char *dst, char *src, int len ) {
+	while ( len-- > 0 ) {
+		*dst++ = gdbstub_to_hex_digit ( *src >> 4 );
+		*dst++ = gdbstub_to_hex_digit ( *src++ );
+	}
+}
+
+static uint8_t gdbstub_cksum ( char *data, int len ) {
+	uint8_t cksum = 0;
+	while ( len-- > 0 ) {
+		cksum += ( uint8_t ) *data++;
+	}
+	return cksum;
+}
+
+static int gdbstub_getchar ( struct gdbstub *stub ) {
+	if ( stub->exit_handler ) {
+		return -1;
+	}
+	return serial_getc();
+}
+
+static void gdbstub_putchar ( struct gdbstub * stub __unused, char ch ) {
+	serial_putc ( ch );
+}
+
+static void gdbstub_tx_packet ( struct gdbstub *stub ) {
+	uint8_t cksum = gdbstub_cksum ( stub->payload, stub->len );
+	int i;
+
+	gdbstub_putchar ( stub, '$' );
+	for ( i = 0; i < stub->len; i++ ) {
+		gdbstub_putchar ( stub, stub->payload [ i ] );
+	}
+	gdbstub_putchar ( stub, '#' );
+	gdbstub_putchar ( stub, gdbstub_to_hex_digit ( cksum >> 4 ) );
+	gdbstub_putchar ( stub, gdbstub_to_hex_digit ( cksum ) );
+
+	stub->parse = gdbstub_state_wait_ack;
+}
+
+/* GDB commands */
+static void gdbstub_send_ok ( struct gdbstub *stub ) {
+	stub->payload [ 0 ] = 'O';
+	stub->payload [ 1 ] = 'K';
+	stub->len = 2;
+	gdbstub_tx_packet ( stub );
+}
+
+static void gdbstub_send_num_packet ( struct gdbstub *stub, char reply, int num ) {
+	stub->payload [ 0 ] = reply;
+	stub->payload [ 1 ] = gdbstub_to_hex_digit ( ( char ) num >> 4 );
+	stub->payload [ 2 ] = gdbstub_to_hex_digit ( ( char ) num );
+	stub->len = 3;
+	gdbstub_tx_packet ( stub );
+}
+
+/* Format is arg1,arg2,...,argn:data where argn are hex integers and data is not an argument */
+static int gdbstub_get_packet_args ( struct gdbstub *stub, unsigned long *args, int nargs, int *stop_idx ) {
+	int i;
+	char ch = 0;
+	int argc = 0;
+	unsigned long val = 0;
+	for ( i = 1; i < stub->len && argc < nargs; i++ ) {
+		ch = stub->payload [ i ];
+		if ( ch == ':' ) {
+			break;
+		} else if ( ch == ',' ) {
+			args [ argc++ ] = val;
+			val = 0;
+		} else {
+			val = ( val << 4 ) | gdbstub_from_hex_digit ( ch );
+		}
+	}
+	if ( stop_idx ) {
+		*stop_idx = i;
+	}
+	if ( argc < nargs ) {
+		args [ argc++ ] = val;
+	}
+	return ( ( i == stub->len || ch == ':' ) && argc == nargs );
+}
+
+static void gdbstub_send_errno ( struct gdbstub *stub, int errno ) {
+	gdbstub_send_num_packet ( stub, 'E', errno );
+}
+
+static void gdbstub_report_signal ( struct gdbstub *stub ) {
+	gdbstub_send_num_packet ( stub, 'S', stub->signo );
+}
+
+static void gdbstub_read_regs ( struct gdbstub *stub ) {
+	gdbstub_to_hex_buf ( stub->payload, ( char * ) stub->regs, GDBMACH_SIZEOF_REGS );
+	stub->len = GDBMACH_SIZEOF_REGS * 2;
+	gdbstub_tx_packet ( stub );
+}
+
+static void gdbstub_write_regs ( struct gdbstub *stub ) {
+	if ( stub->len != 1 + GDBMACH_SIZEOF_REGS * 2 ) {
+		gdbstub_send_errno ( stub, POSIX_EINVAL );
+		return;
+	}
+	gdbstub_from_hex_buf ( ( char * ) stub->regs, &stub->payload [ 1 ], stub->len );
+	gdbstub_send_ok ( stub );
+}
+
+static void gdbstub_read_mem ( struct gdbstub *stub ) {
+	unsigned long args [ 2 ];
+	if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], NULL ) ) {
+		gdbstub_send_errno ( stub, POSIX_EINVAL );
+		return;
+	}
+	args [ 1 ] = ( args [ 1 ] < sizeof stub->payload / 2 ) ? args [ 1 ] : sizeof stub->payload / 2;
+	gdbstub_to_hex_buf ( stub->payload, ( char * ) args [ 0 ], args [ 1 ] );
+	stub->len = args [ 1 ] * 2;
+	gdbstub_tx_packet ( stub );
+}
+
+static void gdbstub_write_mem ( struct gdbstub *stub ) {
+	unsigned long args [ 2 ];
+	int colon;
+	if ( !gdbstub_get_packet_args ( stub, args, sizeof args / sizeof args [ 0 ], &colon ) ||
+			colon >= stub->len || stub->payload [ colon ] != ':' ||
+			( stub->len - colon - 1 ) % 2 != 0 ) {
+		gdbstub_send_errno ( stub, POSIX_EINVAL );
+		return;
+	}
+	gdbstub_from_hex_buf ( ( char * ) args [ 0 ], &stub->payload [ colon + 1 ], stub->len - colon - 1 );
+	gdbstub_send_ok ( stub );
+}
+
+static void gdbstub_continue ( struct gdbstub *stub, int single_step ) {
+	gdbreg_t pc;
+	if ( stub->len > 1 && gdbstub_get_packet_args ( stub, &pc, 1, NULL ) ) {
+		gdbmach_set_pc ( stub->regs, pc );
+	}
+	gdbmach_set_single_step ( stub->regs, single_step );
+	stub->exit_handler = 1;
+	/* Reply will be sent when we hit the next breakpoint or interrupt */
+}
+
+static void gdbstub_rx_packet ( struct gdbstub *stub ) {
+	switch ( stub->payload [ 0 ] ) {
+		case '?':
+			gdbstub_report_signal ( stub );
+			break;
+		case 'g':
+			gdbstub_read_regs ( stub );
+			break;
+		case 'G':
+			gdbstub_write_regs ( stub );
+			break;
+		case 'm':
+			gdbstub_read_mem ( stub );
+			break;
+		case 'M':
+			gdbstub_write_mem ( stub );
+			break;
+		case 'c':
+			gdbstub_continue ( stub, 0 );
+			break;
+		case 's':
+			gdbstub_continue ( stub, 1 );
+			break;
+		default:
+			stub->len = 0;
+			gdbstub_tx_packet ( stub );
+			break;
+	}
+}
+
+/* GDB packet parser */
+static void gdbstub_state_new ( struct gdbstub *stub, char ch ) {
+	if ( ch == '$' ) {
+		stub->len = 0;
+		stub->parse = gdbstub_state_data;
+	}
+}
+
+static void gdbstub_state_data ( struct gdbstub *stub, char ch ) {
+	if ( ch == '#' ) {
+		stub->parse = gdbstub_state_cksum1;
+	} else if ( ch == '$' ) {
+		stub->len = 0; /* retry new packet */
+	} else {
+		/* If the length exceeds our buffer, let the checksum fail */
+		if ( stub->len < ( int ) sizeof stub->payload ) {
+			stub->payload [ stub->len++ ] = ch;
+		}
+	}
+}
+
+static void gdbstub_state_cksum1 ( struct gdbstub *stub, char ch ) {
+	stub->cksum1 = gdbstub_from_hex_digit ( ch ) << 4;
+	stub->parse = gdbstub_state_cksum2;
+}
+
+static void gdbstub_state_cksum2 ( struct gdbstub *stub, char ch ) {
+	uint8_t their_cksum;
+	uint8_t our_cksum;
+
+	stub->parse = gdbstub_state_new;
+	their_cksum = stub->cksum1 + gdbstub_from_hex_digit ( ch );
+	our_cksum = gdbstub_cksum ( stub->payload, stub->len );
+	if ( their_cksum == our_cksum ) {
+		gdbstub_putchar ( stub, '+' );
+		if ( stub->len > 0 ) {
+			gdbstub_rx_packet ( stub );
+		}
+	} else {
+		gdbstub_putchar ( stub, '-' );
+	}
+}
+
+static void gdbstub_state_wait_ack ( struct gdbstub *stub, char ch ) {
+	if ( ch == '+' ) {
+		stub->parse = gdbstub_state_new;
+	} else if ( ch == '-' ) {
+		gdbstub_tx_packet ( stub ); /* retransmit */
+	}
+}
+
+static void gdbstub_parse ( struct gdbstub *stub, char ch ) {
+	stub->parse ( stub, ch );
+}
+
+static struct gdbstub stub = {
+	.parse = gdbstub_state_new
+};
+
+__cdecl void gdbstub_handler ( int signo, gdbreg_t *regs ) {
+	int ch;
+	stub.signo = signo;
+	stub.regs = regs;
+	stub.exit_handler = 0;
+	gdbstub_report_signal ( &stub );
+	while ( ( ch = gdbstub_getchar( &stub ) ) != -1 ) {
+		gdbstub_parse ( &stub, ch );
+	}
+}
+
+/* Activity monitor to detect packets from GDB when we are not active */
+static void gdbstub_activity_step ( struct process *process __unused ) {
+	if ( serial_ischar() ) {
+		gdbmach_breakpoint();
+	}
+}
+
+struct process gdbstub_activity_process __permanent_process = {
+	.step = gdbstub_activity_step,
+};
diff --git a/gpxe/src/core/serial.c b/gpxe/src/core/serial.c
index a5b3f91..54c2295 100644
--- a/gpxe/src/core/serial.c
+++ b/gpxe/src/core/serial.c
@@ -12,10 +12,10 @@
  */
 
 #include "stddef.h"
-#include "console.h"
 #include <gpxe/init.h>
 #include "io.h"
 #include <unistd.h>
+#include <gpxe/serial.h>
 #include "config/serial.h"
 
 /* Set default values if none specified */
@@ -91,13 +91,11 @@
 #define uart_writeb(val,addr) outb((val),(addr))
 #endif
 
-struct console_driver serial_console __console_driver;
-
 /*
  * void serial_putc(int ch);
  *	Write character `ch' to port UART_BASE.
  */
-static void serial_putc ( int ch ) {
+void serial_putc ( int ch ) {
 	int i;
 	int status;
 	i = 1000; /* timeout */
@@ -116,7 +114,7 @@ static void serial_putc ( int ch ) {
  * int serial_getc(void);
  *	Read a character from port UART_BASE.
  */
-static int serial_getc ( void ) {
+int serial_getc ( void ) {
 	int status;
 	int ch;
 	do {
@@ -135,7 +133,7 @@ static int serial_getc ( void ) {
  *       If there is a character in the input buffer of port UART_BASE,
  *       return nonzero; otherwise return 0.
  */
-static int serial_ischar ( void ) {
+int serial_ischar ( void ) {
 	int status;
 	status = uart_readb(UART_BASE + UART_LSR);	/* line status reg; */
 	return status & 1;		/* rx char available */
@@ -217,7 +215,6 @@ static void serial_init ( void ) {
 		/* line status reg */
 		status = uart_readb(UART_BASE + UART_LSR);
 	} while(status & UART_LSR_DR);
-	serial_console.disabled = 0;
  out:
 	return;
 }
@@ -229,10 +226,6 @@ static void serial_init ( void ) {
  */
 static void serial_fini ( void ) {
 	int i, status;
-	if (serial_console.disabled) {
-		/* no serial interface */
-		return;
-	}
 	/* Flush the output buffer to avoid dropping characters,
 	 * if we are reinitializing the serial port.
 	 */
@@ -243,26 +236,17 @@ static void serial_fini ( void ) {
 	/* Don't mark it as disabled; it's still usable */
 }
 
-struct console_driver serial_console __console_driver = {
-	.putchar = serial_putc,
-	.getchar = serial_getc,
-	.iskey = serial_ischar,
-	.disabled = 1,
-};
-
-/** Serial console startup function */
-struct startup_fn serial_startup_fn __startup_fn ( STARTUP_NORMAL ) = {
-	.startup = serial_init,
-	.shutdown = serial_fini,
-};
-
 /**
- * Serial console initialisation function
+ * Serial driver initialisation function
  *
- * Initialise console early on so that it is available to capture
- * early debug messages.  It is safe to call serial_init() multiple
- * times.
+ * Initialise serial port early on so that it is available to capture
+ * early debug messages.
  */
-struct init_fn serial_init_fn __init_fn ( INIT_CONSOLE ) = {
+struct init_fn serial_init_fn __init_fn ( INIT_SERIAL ) = {
 	.initialise = serial_init,
 };
+
+/** Serial driver startup function */
+struct startup_fn serial_startup_fn __startup_fn ( STARTUP_NORMAL ) = {
+	.shutdown = serial_fini,
+};
diff --git a/gpxe/src/core/serial_console.c b/gpxe/src/core/serial_console.c
new file mode 100644
index 0000000..0300482
--- /dev/null
+++ b/gpxe/src/core/serial_console.c
@@ -0,0 +1,31 @@
+#include <gpxe/init.h>
+#include <gpxe/serial.h>
+#include "console.h"
+
+/** @file
+ *
+ * Serial console
+ *
+ */
+
+struct console_driver serial_console __console_driver;
+
+static void serial_console_init ( void ) {
+	/* Serial driver initialization should already be done,
+	 * time to enable the serial console. */
+	serial_console.disabled = 0;
+}
+
+struct console_driver serial_console __console_driver = {
+	.putchar = serial_putc,
+	.getchar = serial_getc,
+	.iskey = serial_ischar,
+	.disabled = 1,
+};
+
+/**
+ * Serial console initialisation function
+ */
+struct init_fn serial_console_init_fn __init_fn ( INIT_CONSOLE ) = {
+	.initialise = serial_console_init,
+};
diff --git a/gpxe/src/core/uuid.c b/gpxe/src/core/uuid.c
index dae26c1..c6e7f5d 100644
--- a/gpxe/src/core/uuid.c
+++ b/gpxe/src/core/uuid.c
@@ -37,9 +37,9 @@ char * uuid_ntoa ( union uuid *uuid ) {
 	static char buf[37]; /* "00000000-0000-0000-0000-000000000000" */
 
 	sprintf ( buf, "%08lx-%04x-%04x-%04x-%02x%02x%02x%02x%02x%02x",
-		  le32_to_cpu ( uuid->canonical.a ),
-		  le16_to_cpu ( uuid->canonical.b ),
-		  le16_to_cpu ( uuid->canonical.c ),
+		  be32_to_cpu ( uuid->canonical.a ),
+		  be16_to_cpu ( uuid->canonical.b ),
+		  be16_to_cpu ( uuid->canonical.c ),
 		  be16_to_cpu ( uuid->canonical.d ),
 		  uuid->canonical.e[0], uuid->canonical.e[1],
 		  uuid->canonical.e[2], uuid->canonical.e[3],
diff --git a/gpxe/src/drivers/infiniband/arbel.c b/gpxe/src/drivers/infiniband/arbel.c
index 462638e..2aced77 100644
--- a/gpxe/src/drivers/infiniband/arbel.c
+++ b/gpxe/src/drivers/infiniband/arbel.c
@@ -276,19 +276,30 @@ arbel_cmd_sw2hw_mpt ( struct arbel *arbel, unsigned int index,
 }
 
 static inline int
+arbel_cmd_map_eq ( struct arbel *arbel, unsigned long index_map,
+		   const struct arbelprm_event_mask *mask ) {
+	return arbel_cmd ( arbel,
+			   ARBEL_HCR_IN_CMD ( ARBEL_HCR_MAP_EQ,
+					      0, sizeof ( *mask ) ),
+			   0, mask, index_map, NULL );
+}
+
+static inline int
 arbel_cmd_sw2hw_eq ( struct arbel *arbel, unsigned int index,
-		     const struct arbelprm_eqc *eqc ) {
+		     const struct arbelprm_eqc *eqctx ) {
 	return arbel_cmd ( arbel,
 			   ARBEL_HCR_IN_CMD ( ARBEL_HCR_SW2HW_EQ,
-					      1, sizeof ( *eqc ) ),
-			   0, eqc, index, NULL );
+					      1, sizeof ( *eqctx ) ),
+			   0, eqctx, index, NULL );
 }
 
 static inline int
-arbel_cmd_hw2sw_eq ( struct arbel *arbel, unsigned int index ) {
+arbel_cmd_hw2sw_eq ( struct arbel *arbel, unsigned int index,
+		     struct arbelprm_eqc *eqctx ) {
 	return arbel_cmd ( arbel,
-			   ARBEL_HCR_VOID_CMD ( ARBEL_HCR_HW2SW_EQ ),
-			   1, NULL, index, NULL );
+			   ARBEL_HCR_OUT_CMD ( ARBEL_HCR_HW2SW_EQ,
+					       1, sizeof ( *eqctx ) ),
+			   1, NULL, index, eqctx );
 }
 
 static inline int
@@ -337,6 +348,15 @@ arbel_cmd_rtr2rts_qpee ( struct arbel *arbel, unsigned long qpn,
 }
 
 static inline int
+arbel_cmd_rts2rts_qp ( struct arbel *arbel, unsigned long qpn,
+		       const struct arbelprm_qp_ee_state_transitions *ctx ) {
+	return arbel_cmd ( arbel,
+			   ARBEL_HCR_IN_CMD ( ARBEL_HCR_RTS2RTS_QPEE,
+					      1, sizeof ( *ctx ) ),
+			   0, ctx, qpn, NULL );
+}
+
+static inline int
 arbel_cmd_2rst_qpee ( struct arbel *arbel, unsigned long qpn ) {
 	return arbel_cmd ( arbel,
 			   ARBEL_HCR_VOID_CMD ( ARBEL_HCR_2RST_QPEE ),
@@ -836,6 +856,39 @@ static int arbel_create_qp ( struct ib_device *ibdev,
 }
 
 /**
+ * Modify queue pair
+ *
+ * @v ibdev		Infiniband device
+ * @v qp		Queue pair
+ * @v mod_list		Modification list
+ * @ret rc		Return status code
+ */
+static int arbel_modify_qp ( struct ib_device *ibdev,
+			     struct ib_queue_pair *qp,
+			     unsigned long mod_list ) {
+	struct arbel *arbel = ib_get_drvdata ( ibdev );
+	struct arbelprm_qp_ee_state_transitions qpctx;
+	unsigned long optparammask = 0;
+	int rc;
+
+	/* Construct optparammask */
+	if ( mod_list & IB_MODIFY_QKEY )
+		optparammask |= ARBEL_QPEE_OPT_PARAM_QKEY;
+
+	/* Issue RTS2RTS_QP */
+	memset ( &qpctx, 0, sizeof ( qpctx ) );
+	MLX_FILL_1 ( &qpctx, 0, opt_param_mask, optparammask );
+	MLX_FILL_1 ( &qpctx, 44, qpc_eec_data.q_key, qp->qkey );
+	if ( ( rc = arbel_cmd_rts2rts_qp ( arbel, qp->qpn, &qpctx ) ) != 0 ){
+		DBGC ( arbel, "Arbel %p RTS2RTS_QP failed: %s\n",
+		       arbel, strerror ( rc ) );
+		return rc;
+	}
+
+	return 0;
+}
+
+/**
  * Destroy queue pair
  *
  * @v ibdev		Infiniband device
@@ -1204,6 +1257,205 @@ static void arbel_poll_cq ( struct ib_device *ibdev,
 
 /***************************************************************************
  *
+ * Event queues
+ *
+ ***************************************************************************
+ */
+
+/**
+ * Create event queue
+ *
+ * @v arbel		Arbel device
+ * @ret rc		Return status code
+ */
+static int arbel_create_eq ( struct arbel *arbel ) {
+	struct arbel_event_queue *arbel_eq = &arbel->eq;
+	struct arbelprm_eqc eqctx;
+	struct arbelprm_event_mask mask;
+	unsigned int i;
+	int rc;
+
+	/* Select event queue number */
+	arbel_eq->eqn = arbel->limits.reserved_eqs;
+
+	/* Calculate doorbell address */
+	arbel_eq->doorbell = ( arbel->eq_ci_doorbells +
+			       ARBEL_DB_EQ_OFFSET ( arbel_eq->eqn ) );
+
+	/* Allocate event queue itself */
+	arbel_eq->eqe_size =
+		( ARBEL_NUM_EQES * sizeof ( arbel_eq->eqe[0] ) );
+	arbel_eq->eqe = malloc_dma ( arbel_eq->eqe_size,
+				     sizeof ( arbel_eq->eqe[0] ) );
+	if ( ! arbel_eq->eqe ) {
+		rc = -ENOMEM;
+		goto err_eqe;
+	}
+	memset ( arbel_eq->eqe, 0, arbel_eq->eqe_size );
+	for ( i = 0 ; i < ARBEL_NUM_EQES ; i++ ) {
+		MLX_FILL_1 ( &arbel_eq->eqe[i].generic, 7, owner, 1 );
+	}
+	barrier();
+
+	/* Hand queue over to hardware */
+	memset ( &eqctx, 0, sizeof ( eqctx ) );
+	MLX_FILL_1 ( &eqctx, 0, st, 0xa /* "Fired" */ );
+	MLX_FILL_1 ( &eqctx, 2,
+		     start_address_l, virt_to_phys ( arbel_eq->eqe ) );
+	MLX_FILL_1 ( &eqctx, 3, log_eq_size, fls ( ARBEL_NUM_EQES - 1 ) );
+	MLX_FILL_1 ( &eqctx, 6, pd, ARBEL_GLOBAL_PD );
+	MLX_FILL_1 ( &eqctx, 7, lkey, arbel->reserved_lkey );
+	if ( ( rc = arbel_cmd_sw2hw_eq ( arbel, arbel_eq->eqn,
+					 &eqctx ) ) != 0 ) {
+		DBGC ( arbel, "Arbel %p SW2HW_EQ failed: %s\n",
+		       arbel, strerror ( rc ) );
+		goto err_sw2hw_eq;
+	}
+
+	/* Map events to this event queue */
+	memset ( &mask, 0, sizeof ( mask ) );
+	MLX_FILL_1 ( &mask, 1, port_state_change, 1 );
+	if ( ( rc = arbel_cmd_map_eq ( arbel,
+				       ( ARBEL_MAP_EQ | arbel_eq->eqn ),
+				       &mask ) ) != 0 ) {
+		DBGC ( arbel, "Arbel %p MAP_EQ failed: %s\n",
+		       arbel, strerror ( rc )  );
+		goto err_map_eq;
+	}
+
+	DBGC ( arbel, "Arbel %p EQN %#lx ring at [%p,%p])\n",
+	       arbel, arbel_eq->eqn, arbel_eq->eqe,
+	       ( ( ( void * ) arbel_eq->eqe ) + arbel_eq->eqe_size ) );
+	return 0;
+
+ err_map_eq:
+	arbel_cmd_hw2sw_eq ( arbel, arbel_eq->eqn, &eqctx );
+ err_sw2hw_eq:
+	free_dma ( arbel_eq->eqe, arbel_eq->eqe_size );
+ err_eqe:
+	memset ( arbel_eq, 0, sizeof ( *arbel_eq ) );
+	return rc;
+}
+
+/**
+ * Destroy event queue
+ *
+ * @v arbel		Arbel device
+ */
+static void arbel_destroy_eq ( struct arbel *arbel ) {
+	struct arbel_event_queue *arbel_eq = &arbel->eq;
+	struct arbelprm_eqc eqctx;
+	struct arbelprm_event_mask mask;
+	int rc;
+
+	/* Unmap events from event queue */
+	memset ( &mask, 0, sizeof ( mask ) );
+	MLX_FILL_1 ( &mask, 1, port_state_change, 1 );
+	if ( ( rc = arbel_cmd_map_eq ( arbel,
+				       ( ARBEL_UNMAP_EQ | arbel_eq->eqn ),
+				       &mask ) ) != 0 ) {
+		DBGC ( arbel, "Arbel %p FATAL MAP_EQ failed to unmap: %s\n",
+		       arbel, strerror ( rc ) );
+		/* Continue; HCA may die but system should survive */
+	}
+
+	/* Take ownership back from hardware */
+	if ( ( rc = arbel_cmd_hw2sw_eq ( arbel, arbel_eq->eqn,
+					 &eqctx ) ) != 0 ) {
+		DBGC ( arbel, "Arbel %p FATAL HW2SW_EQ failed: %s\n",
+		       arbel, strerror ( rc ) );
+		/* Leak memory and return; at least we avoid corruption */
+		return;
+	}
+
+	/* Free memory */
+	free_dma ( arbel_eq->eqe, arbel_eq->eqe_size );
+	memset ( arbel_eq, 0, sizeof ( *arbel_eq ) );
+}
+
+/**
+ * Handle port state event
+ *
+ * @v arbel		Arbel device
+ * @v eqe		Port state change event queue entry
+ */
+static void arbel_event_port_state_change ( struct arbel *arbel,
+					    union arbelprm_event_entry *eqe){
+	unsigned int port;
+	int link_up;
+
+	/* Get port and link status */
+	port = ( MLX_GET ( &eqe->port_state_change, data.p ) - 1 );
+	link_up = ( MLX_GET ( &eqe->generic, event_sub_type ) & 0x04 );
+	DBGC ( arbel, "Arbel %p port %d link %s\n", arbel, ( port + 1 ),
+	       ( link_up ? "up" : "down" ) );
+
+	/* Sanity check */
+	if ( port >= ARBEL_NUM_PORTS ) {
+		DBGC ( arbel, "Arbel %p port %d does not exist!\n",
+		       arbel, ( port + 1 ) );
+		return;
+	}
+
+	/* Notify Infiniband core of link state change */
+	ib_link_state_changed ( arbel->ibdev[port] );
+}
+
+/**
+ * Poll event queue
+ *
+ * @v ibdev		Infiniband device
+ */
+static void arbel_poll_eq ( struct ib_device *ibdev ) {
+	struct arbel *arbel = ib_get_drvdata ( ibdev );
+	struct arbel_event_queue *arbel_eq = &arbel->eq;
+	union arbelprm_event_entry *eqe;
+	union arbelprm_eq_doorbell_register db_reg;
+	unsigned int eqe_idx_mask;
+	unsigned int event_type;
+
+	while ( 1 ) {
+		/* Look for event entry */
+		eqe_idx_mask = ( ARBEL_NUM_EQES - 1 );
+		eqe = &arbel_eq->eqe[arbel_eq->next_idx & eqe_idx_mask];
+		if ( MLX_GET ( &eqe->generic, owner ) != 0 ) {
+			/* Entry still owned by hardware; end of poll */
+			break;
+		}
+		DBGCP ( arbel, "Arbel %p event:\n", arbel );
+		DBGCP_HD ( arbel, eqe, sizeof ( *eqe ) );
+
+		/* Handle event */
+		event_type = MLX_GET ( &eqe->generic, event_type );
+		switch ( event_type ) {
+		case ARBEL_EV_PORT_STATE_CHANGE:
+			arbel_event_port_state_change ( arbel, eqe );
+			break;
+		default:
+			DBGC ( arbel, "Arbel %p unrecognised event type "
+			       "%#x:\n", arbel, event_type );
+			DBGC_HD ( arbel, eqe, sizeof ( *eqe ) );
+			break;
+		}
+
+		/* Return ownership to hardware */
+		MLX_FILL_1 ( &eqe->generic, 7, owner, 1 );
+		barrier();
+
+		/* Update event queue's index */
+		arbel_eq->next_idx++;
+
+		/* Ring doorbell */
+		MLX_FILL_1 ( &db_reg.ci, 0, ci, arbel_eq->next_idx );
+		DBGCP ( arbel, "Ringing doorbell %08lx with %08lx\n",
+			virt_to_phys ( arbel_eq->doorbell ),
+			db_reg.dword[0] );
+		writel ( db_reg.dword[0], arbel_eq->doorbell );
+	}
+}
+
+/***************************************************************************
+ *
  * Infiniband link-layer operations
  *
  ***************************************************************************
@@ -1399,10 +1651,12 @@ static struct ib_device_operations arbel_ib_operations = {
 	.create_cq	= arbel_create_cq,
 	.destroy_cq	= arbel_destroy_cq,
 	.create_qp	= arbel_create_qp,
+	.modify_qp	= arbel_modify_qp,
 	.destroy_qp	= arbel_destroy_qp,
 	.post_send	= arbel_post_send,
 	.post_recv	= arbel_post_recv,
 	.poll_cq	= arbel_poll_cq,
+	.poll_eq	= arbel_poll_eq,
 	.open		= arbel_open,
 	.close		= arbel_close,
 	.mcast_attach	= arbel_mcast_attach,
@@ -1431,6 +1685,7 @@ static int arbel_start_firmware ( struct arbel *arbel ) {
 	unsigned int log2_fw_pages;
 	size_t fw_size;
 	physaddr_t fw_base;
+	uint64_t eq_set_ci_base_addr;
 	int rc;
 
 	/* Get firmware parameters */
@@ -1447,6 +1702,10 @@ static int arbel_start_firmware ( struct arbel *arbel ) {
 	fw_pages = ( 1 << log2_fw_pages );
 	DBGC ( arbel, "Arbel %p requires %d kB for firmware\n",
 	       arbel, ( fw_pages * 4 ) );
+	eq_set_ci_base_addr =
+		( ( (uint64_t) MLX_GET ( &fw, eq_set_ci_base_addr_h ) << 32 ) |
+		  ( (uint64_t) MLX_GET ( &fw, eq_set_ci_base_addr_l ) ) );
+	arbel->eq_ci_doorbells = ioremap ( eq_set_ci_base_addr, 0x200 );
 
 	/* Enable locally-attached memory.  Ignore failure; there may
 	 * be no attached memory.
@@ -1549,6 +1808,7 @@ static int arbel_get_limits ( struct arbel *arbel ) {
 	arbel->limits.reserved_cqs =
 		( 1 << MLX_GET ( &dev_lim, log2_rsvd_cqs ) );
 	arbel->limits.cqc_entry_size = MLX_GET ( &dev_lim, cqc_entry_sz );
+	arbel->limits.reserved_eqs = MLX_GET ( &dev_lim, num_rsvd_eqs );
 	arbel->limits.reserved_mtts =
 		( 1 << MLX_GET ( &dev_lim, log2_rsvd_mtts ) );
 	arbel->limits.mtt_entry_size = MLX_GET ( &dev_lim, mtt_entry_sz );
@@ -1679,7 +1939,7 @@ static int arbel_alloc_icm ( struct arbel *arbel,
 	icm_offset += icm_usage ( log_num_rdbs, 32 );
 
 	/* Event queue contexts */
-	log_num_eqs = 6;
+	log_num_eqs =  fls ( arbel->limits.reserved_eqs + ARBEL_MAX_EQS - 1 );
 	MLX_FILL_2 ( init_hca, 33,
 		     qpc_eec_cqc_eqc_rdb_parameters.eqc_base_addr_l,
 		     ( icm_offset >> 6 ),
@@ -1908,6 +2168,10 @@ static int arbel_probe ( struct pci_device *pci,
 	if ( ( rc = arbel_setup_mpt ( arbel ) ) != 0 )
 		goto err_setup_mpt;
 
+	/* Set up event queue */
+	if ( ( rc = arbel_create_eq ( arbel ) ) != 0 )
+		goto err_create_eq;
+
 	/* Register Infiniband devices */
 	for ( i = 0 ; i < ARBEL_NUM_PORTS ; i++ ) {
 		if ( ( rc = register_ibdev ( arbel->ibdev[i] ) ) != 0 ) {
@@ -1923,6 +2187,8 @@ static int arbel_probe ( struct pci_device *pci,
  err_register_ibdev:
 	for ( ; i >= 0 ; i-- )
 		unregister_ibdev ( arbel->ibdev[i] );
+	arbel_destroy_eq ( arbel );
+ err_create_eq:
  err_setup_mpt:
 	arbel_cmd_close_hca ( arbel );
  err_init_hca:
@@ -1938,7 +2204,7 @@ static int arbel_probe ( struct pci_device *pci,
 	i = ( ARBEL_NUM_PORTS - 1 );
  err_alloc_ibdev:
 	for ( ; i >= 0 ; i-- )
-		free_ibdev ( arbel->ibdev[i] );
+		ibdev_put ( arbel->ibdev[i] );
 	free ( arbel );
  err_alloc_arbel:
 	return rc;
@@ -1955,6 +2221,7 @@ static void arbel_remove ( struct pci_device *pci ) {
 
 	for ( i = ( ARBEL_NUM_PORTS - 1 ) ; i >= 0 ; i-- )
 		unregister_ibdev ( arbel->ibdev[i] );
+	arbel_destroy_eq ( arbel );
 	arbel_cmd_close_hca ( arbel );
 	arbel_free_icm ( arbel );
 	arbel_stop_firmware ( arbel );
@@ -1962,7 +2229,7 @@ static void arbel_remove ( struct pci_device *pci ) {
 	free_dma ( arbel->mailbox_out, ARBEL_MBOX_SIZE );
 	free_dma ( arbel->mailbox_in, ARBEL_MBOX_SIZE );
 	for ( i = ( ARBEL_NUM_PORTS - 1 ) ; i >= 0 ; i-- )
-		free_ibdev ( arbel->ibdev[i] );
+		ibdev_put ( arbel->ibdev[i] );
 	free ( arbel );
 }
 
diff --git a/gpxe/src/drivers/infiniband/arbel.h b/gpxe/src/drivers/infiniband/arbel.h
index 94fa67c..7d97b15 100644
--- a/gpxe/src/drivers/infiniband/arbel.h
+++ b/gpxe/src/drivers/infiniband/arbel.h
@@ -18,7 +18,7 @@
  */
 
 /* Ports in existence */
-#define ARBEL_NUM_PORTS			1
+#define ARBEL_NUM_PORTS			2
 #define ARBEL_PORT_BASE			1
 
 /* PCI BARs */
@@ -57,6 +57,7 @@
 #define ARBEL_HCR_RST2INIT_QPEE		0x0019
 #define ARBEL_HCR_INIT2RTR_QPEE		0x001a
 #define ARBEL_HCR_RTR2RTS_QPEE		0x001b
+#define ARBEL_HCR_RTS2RTS_QPEE		0x001c
 #define ARBEL_HCR_2RST_QPEE		0x0021
 #define ARBEL_HCR_MAD_IFC		0x0024
 #define ARBEL_HCR_READ_MGM		0x0025
@@ -86,6 +87,14 @@
 #define ARBEL_PAGE_SIZE			4096
 
 #define ARBEL_DB_POST_SND_OFFSET	0x10
+#define ARBEL_DB_EQ_OFFSET(_eqn)	( 0x08 * (_eqn) )
+
+#define ARBEL_QPEE_OPT_PARAM_QKEY	0x00000020UL
+
+#define ARBEL_MAP_EQ			( 0UL << 31 )
+#define ARBEL_UNMAP_EQ			( 1UL << 31 )
+
+#define ARBEL_EV_PORT_STATE_CHANGE	0x09
 
 /*
  * Datatypes that seem to be missing from the autogenerated documentation
@@ -104,6 +113,24 @@ struct arbelprm_scalar_parameter_st {
 	pseudo_bit_t value[0x00020];
 } __attribute__ (( packed ));
 
+struct arbelprm_event_mask_st {
+	pseudo_bit_t reserved0[0x00020];
+/* -------------- */
+	pseudo_bit_t completion[0x00001];
+	pseudo_bit_t reserved1[0x0008];
+	pseudo_bit_t port_state_change[0x00001];
+	pseudo_bit_t reserved2[0x00016];
+} __attribute__ (( packed ));
+
+struct arbelprm_eq_set_ci_st {
+	pseudo_bit_t ci[0x00020];
+} __attribute__ (( packed ));
+
+struct arbelprm_port_state_change_event_st {
+	pseudo_bit_t reserved[0x00020];
+	struct arbelprm_port_state_change_st data;
+} __attribute__ (( packed ));
+
 /*
  * Wrapper structures for hardware datatypes
  *
@@ -115,6 +142,9 @@ struct MLX_DECLARE_STRUCT ( arbelprm_completion_queue_entry );
 struct MLX_DECLARE_STRUCT ( arbelprm_completion_with_error );
 struct MLX_DECLARE_STRUCT ( arbelprm_cq_arm_db_record );
 struct MLX_DECLARE_STRUCT ( arbelprm_cq_ci_db_record );
+struct MLX_DECLARE_STRUCT ( arbelprm_event_mask );
+struct MLX_DECLARE_STRUCT ( arbelprm_event_queue_entry );
+struct MLX_DECLARE_STRUCT ( arbelprm_eq_set_ci );
 struct MLX_DECLARE_STRUCT ( arbelprm_eqc );
 struct MLX_DECLARE_STRUCT ( arbelprm_hca_command_register );
 struct MLX_DECLARE_STRUCT ( arbelprm_init_hca );
@@ -123,6 +153,7 @@ struct MLX_DECLARE_STRUCT ( arbelprm_mad_ifc );
 struct MLX_DECLARE_STRUCT ( arbelprm_mgm_entry );
 struct MLX_DECLARE_STRUCT ( arbelprm_mgm_hash );
 struct MLX_DECLARE_STRUCT ( arbelprm_mpt );
+struct MLX_DECLARE_STRUCT ( arbelprm_port_state_change_event );
 struct MLX_DECLARE_STRUCT ( arbelprm_qp_db_record );
 struct MLX_DECLARE_STRUCT ( arbelprm_qp_ee_state_transitions );
 struct MLX_DECLARE_STRUCT ( arbelprm_query_dev_lim );
@@ -172,6 +203,11 @@ union arbelprm_completion_entry {
 	struct arbelprm_completion_with_error error;
 } __attribute__ (( packed ));
 
+union arbelprm_event_entry {
+	struct arbelprm_event_queue_entry generic;
+	struct arbelprm_port_state_change_event port_state_change;
+} __attribute__ (( packed ));
+
 union arbelprm_doorbell_record {
 	struct arbelprm_cq_arm_db_record cq_arm;
 	struct arbelprm_cq_ci_db_record cq_ci;
@@ -183,6 +219,11 @@ union arbelprm_doorbell_register {
 	uint32_t dword[2];
 } __attribute__ (( packed ));
 
+union arbelprm_eq_doorbell_register {
+	struct arbelprm_eq_set_ci ci;
+	uint32_t dword[1];
+} __attribute__ (( packed ));
+
 union arbelprm_mad {
 	struct arbelprm_mad_ifc ifc;
 	union ib_mad mad;
@@ -215,6 +256,8 @@ struct arbel_dev_limits {
 	unsigned int reserved_cqs;
 	/** CQ context entry size */
 	size_t cqc_entry_size;
+	/** Number of reserved EQs */
+	unsigned int reserved_eqs;
 	/** Number of reserved MTTs */
 	unsigned int reserved_mtts;
 	/** MTT entry size */
@@ -304,6 +347,33 @@ struct arbel_completion_queue {
 	size_t cqe_size;
 };
 
+/** Maximum number of allocatable event queues
+ *
+ * This is a policy decision, not a device limit.
+ */
+#define ARBEL_MAX_EQS		64
+
+/** A Arbel event queue */
+struct arbel_event_queue {
+	/** Event queue entries */
+	union arbelprm_event_entry *eqe;
+	/** Size of event queue */
+	size_t eqe_size;
+	/** Event queue number */
+	unsigned long eqn;
+	/** Next event queue entry index */
+	unsigned long next_idx;
+	/** Doorbell register */
+	void *doorbell;
+};
+
+/** Number of event queue entries
+ *
+ * This is a policy decision.
+ */
+#define ARBEL_NUM_EQES		4
+
+
 /** An Arbel resource bitmask */
 typedef uint32_t arbel_bitmask_t;
 
@@ -318,6 +388,8 @@ struct arbel {
 	void *config;
 	/** PCI user Access Region */
 	void *uar;
+	/** Event queue consumer index doorbells */
+	void *eq_ci_doorbells;
 
 	/** Command input mailbox */
 	void *mailbox_in;
@@ -333,6 +405,8 @@ struct arbel {
 	/** ICM area */
 	userptr_t icm;
 
+	/** Event queue */
+	struct arbel_event_queue eq;
 	/** Doorbell records */
 	union arbelprm_doorbell_record *db_rec;
 	/** Reserved LKey
diff --git a/gpxe/src/drivers/infiniband/hermon.c b/gpxe/src/drivers/infiniband/hermon.c
index c10559f..c198556 100644
--- a/gpxe/src/drivers/infiniband/hermon.c
+++ b/gpxe/src/drivers/infiniband/hermon.c
@@ -317,19 +317,39 @@ hermon_cmd_write_mtt ( struct hermon *hermon,
 }
 
 static inline int
+hermon_cmd_map_eq ( struct hermon *hermon, unsigned long index_map,
+		    const struct hermonprm_event_mask *mask ) {
+	return hermon_cmd ( hermon,
+			    HERMON_HCR_IN_CMD ( HERMON_HCR_MAP_EQ,
+						0, sizeof ( *mask ) ),
+			    0, mask, index_map, NULL );
+}
+
+static inline int
 hermon_cmd_sw2hw_eq ( struct hermon *hermon, unsigned int index,
-		      const struct hermonprm_eqc *eqc ) {
+		      const struct hermonprm_eqc *eqctx ) {
 	return hermon_cmd ( hermon,
 			    HERMON_HCR_IN_CMD ( HERMON_HCR_SW2HW_EQ,
-						1, sizeof ( *eqc ) ),
-			    0, eqc, index, NULL );
+						1, sizeof ( *eqctx ) ),
+			    0, eqctx, index, NULL );
+}
+
+static inline int
+hermon_cmd_hw2sw_eq ( struct hermon *hermon, unsigned int index,
+		      struct hermonprm_eqc *eqctx ) {
+	return hermon_cmd ( hermon,
+			    HERMON_HCR_OUT_CMD ( HERMON_HCR_HW2SW_EQ,
+						 1, sizeof ( *eqctx ) ),
+			    1, NULL, index, eqctx );
 }
 
 static inline int
-hermon_cmd_hw2sw_eq ( struct hermon *hermon, unsigned int index ) {
+hermon_cmd_query_eq ( struct hermon *hermon, unsigned int index,
+		      struct hermonprm_eqc *eqctx ) {
 	return hermon_cmd ( hermon,
-			    HERMON_HCR_VOID_CMD ( HERMON_HCR_HW2SW_EQ ),
-			    1, NULL, index, NULL );
+			    HERMON_HCR_OUT_CMD ( HERMON_HCR_QUERY_EQ,
+						 1, sizeof ( *eqctx ) ),
+			    0, NULL, index, eqctx );
 }
 
 static inline int
@@ -378,6 +398,15 @@ hermon_cmd_rtr2rts_qp ( struct hermon *hermon, unsigned long qpn,
 }
 
 static inline int
+hermon_cmd_rts2rts_qp ( struct hermon *hermon, unsigned long qpn,
+			const struct hermonprm_qp_ee_state_transitions *ctx ) {
+	return hermon_cmd ( hermon,
+			    HERMON_HCR_IN_CMD ( HERMON_HCR_RTS2RTS_QP,
+						1, sizeof ( *ctx ) ),
+			    0, ctx, qpn, NULL );
+}
+
+static inline int
 hermon_cmd_2rst_qp ( struct hermon *hermon, unsigned long qpn ) {
 	return hermon_cmd ( hermon,
 			    HERMON_HCR_VOID_CMD ( HERMON_HCR_2RST_QP ),
@@ -646,7 +675,7 @@ static int hermon_create_cq ( struct ib_device *ibdev,
 	MLX_FILL_1 ( &cqctx, 2,
 		     page_offset, ( hermon_cq->mtt.page_offset >> 5 ) );
 	MLX_FILL_2 ( &cqctx, 3,
-		     usr_page, HERMON_UAR_PAGE,
+		     usr_page, HERMON_UAR_NON_EQ_PAGE,
 		     log_cq_size, fls ( cq->num_cqes - 1 ) );
 	MLX_FILL_1 ( &cqctx, 7, mtt_base_addr_l,
 		     ( hermon_cq->mtt.mtt_base_addr >> 3 ) );
@@ -752,6 +781,11 @@ static int hermon_create_qp ( struct ib_device *ibdev,
 		goto err_hermon_qp;
 	}
 
+	/* Calculate doorbell address */
+	hermon_qp->send.doorbell =
+		( hermon->uar + HERMON_UAR_NON_EQ_PAGE * HERMON_PAGE_SIZE +
+		  HERMON_DB_POST_SND_OFFSET );
+
 	/* Allocate work queue buffer */
 	hermon_qp->send.num_wqes = ( qp->send.num_wqes /* headroom */ + 1 +
 				( 2048 / sizeof ( hermon_qp->send.wqe[0] ) ) );
@@ -796,7 +830,7 @@ static int hermon_create_qp ( struct ib_device *ibdev,
 		     qpc_eec_data.log_sq_stride,
 		     ( fls ( sizeof ( hermon_qp->send.wqe[0] ) - 1 ) - 4 ) );
 	MLX_FILL_1 ( &qpctx, 5,
-		     qpc_eec_data.usr_page, HERMON_UAR_PAGE );
+		     qpc_eec_data.usr_page, HERMON_UAR_NON_EQ_PAGE );
 	MLX_FILL_1 ( &qpctx, 33, qpc_eec_data.cqn_snd, qp->send.cq->cqn );
 	MLX_FILL_1 ( &qpctx, 38, qpc_eec_data.page_offset,
 		     ( hermon_qp->mtt.page_offset >> 6 ) );
@@ -860,6 +894,39 @@ static int hermon_create_qp ( struct ib_device *ibdev,
 }
 
 /**
+ * Modify queue pair
+ *
+ * @v ibdev		Infiniband device
+ * @v qp		Queue pair
+ * @v mod_list		Modification list
+ * @ret rc		Return status code
+ */
+static int hermon_modify_qp ( struct ib_device *ibdev,
+			      struct ib_queue_pair *qp,
+			      unsigned long mod_list ) {
+	struct hermon *hermon = ib_get_drvdata ( ibdev );
+	struct hermonprm_qp_ee_state_transitions qpctx;
+	unsigned long optparammask = 0;
+	int rc;
+
+	/* Construct optparammask */
+	if ( mod_list & IB_MODIFY_QKEY )
+		optparammask |= HERMON_QP_OPT_PARAM_QKEY;
+
+	/* Issue RTS2RTS_QP */
+	memset ( &qpctx, 0, sizeof ( qpctx ) );
+	MLX_FILL_1 ( &qpctx, 0, opt_param_mask, optparammask );
+	MLX_FILL_1 ( &qpctx, 44, qpc_eec_data.q_key, qp->qkey );
+	if ( ( rc = hermon_cmd_rts2rts_qp ( hermon, qp->qpn, &qpctx ) ) != 0 ){
+		DBGC ( hermon, "Hermon %p RTS2RTS_QP failed: %s\n",
+		       hermon, strerror ( rc ) );
+		return rc;
+	}
+
+	return 0;
+}
+
+/**
  * Destroy queue pair
  *
  * @v ibdev		Infiniband device
@@ -975,9 +1042,8 @@ static int hermon_post_send ( struct ib_device *ibdev,
 	/* Ring doorbell register */
 	MLX_FILL_1 ( &db_reg.send, 0, qn, qp->qpn );
 	DBGCP ( hermon, "Ringing doorbell %08lx with %08lx\n",
-		virt_to_phys ( hermon->uar + HERMON_DB_POST_SND_OFFSET ),
-		db_reg.dword[0] );
-	writel ( db_reg.dword[0], ( hermon->uar + HERMON_DB_POST_SND_OFFSET ));
+		virt_to_phys ( hermon_send_wq->doorbell ), db_reg.dword[0] );
+	writel ( db_reg.dword[0], ( hermon_send_wq->doorbell ) );
 
 	/* Update work queue's index */
 	wq->next_idx++;
@@ -1155,7 +1221,217 @@ static void hermon_poll_cq ( struct ib_device *ibdev,
 
 		/* Update doorbell record */
 		MLX_FILL_1 ( &hermon_cq->doorbell, 0, update_ci,
-			     ( cq->next_idx & 0xffffffUL ) );
+			     ( cq->next_idx & 0x00ffffffUL ) );
+	}
+}
+
+/***************************************************************************
+ *
+ * Event queues
+ *
+ ***************************************************************************
+ */
+
+/**
+ * Create event queue
+ *
+ * @v hermon		Hermon device
+ * @ret rc		Return status code
+ */
+static int hermon_create_eq ( struct hermon *hermon ) {
+	struct hermon_event_queue *hermon_eq = &hermon->eq;
+	struct hermonprm_eqc eqctx;
+	struct hermonprm_event_mask mask;
+	unsigned int i;
+	int rc;
+
+	/* Select event queue number */
+	hermon_eq->eqn = ( 4 * hermon->cap.reserved_uars );
+	if ( hermon_eq->eqn < hermon->cap.reserved_eqs )
+		hermon_eq->eqn = hermon->cap.reserved_eqs;
+
+	/* Calculate doorbell address */
+	hermon_eq->doorbell =
+		( hermon->uar + HERMON_DB_EQ_OFFSET ( hermon_eq->eqn ) );
+
+	/* Allocate event queue itself */
+	hermon_eq->eqe_size =
+		( HERMON_NUM_EQES * sizeof ( hermon_eq->eqe[0] ) );
+	hermon_eq->eqe = malloc_dma ( hermon_eq->eqe_size,
+				      sizeof ( hermon_eq->eqe[0] ) );
+	if ( ! hermon_eq->eqe ) {
+		rc = -ENOMEM;
+		goto err_eqe;
+	}
+	memset ( hermon_eq->eqe, 0, hermon_eq->eqe_size );
+	for ( i = 0 ; i < HERMON_NUM_EQES ; i++ ) {
+		MLX_FILL_1 ( &hermon_eq->eqe[i].generic, 7, owner, 1 );
+	}
+	barrier();
+
+	/* Allocate MTT entries */
+	if ( ( rc = hermon_alloc_mtt ( hermon, hermon_eq->eqe,
+				       hermon_eq->eqe_size,
+				       &hermon_eq->mtt ) ) != 0 )
+		goto err_alloc_mtt;
+
+	/* Hand queue over to hardware */
+	memset ( &eqctx, 0, sizeof ( eqctx ) );
+	MLX_FILL_1 ( &eqctx, 0, st, 0xa /* "Fired" */ );
+	MLX_FILL_1 ( &eqctx, 2,
+		     page_offset, ( hermon_eq->mtt.page_offset >> 5 ) );
+	MLX_FILL_1 ( &eqctx, 3, log_eq_size, fls ( HERMON_NUM_EQES - 1 ) );
+	MLX_FILL_1 ( &eqctx, 7, mtt_base_addr_l,
+		     ( hermon_eq->mtt.mtt_base_addr >> 3 ) );
+	if ( ( rc = hermon_cmd_sw2hw_eq ( hermon, hermon_eq->eqn,
+					  &eqctx ) ) != 0 ) {
+		DBGC ( hermon, "Hermon %p SW2HW_EQ failed: %s\n",
+		       hermon, strerror ( rc ) );
+		goto err_sw2hw_eq;
+	}
+
+	/* Map events to this event queue */
+	memset ( &mask, 0, sizeof ( mask ) );
+	MLX_FILL_1 ( &mask, 1, port_state_change, 1 );
+	if ( ( rc = hermon_cmd_map_eq ( hermon,
+					( HERMON_MAP_EQ | hermon_eq->eqn ),
+					&mask ) ) != 0 ) {
+		DBGC ( hermon, "Hermon %p MAP_EQ failed: %s\n",
+		       hermon, strerror ( rc )  );
+		goto err_map_eq;
+	}
+
+	DBGC ( hermon, "Hermon %p EQN %#lx ring at [%p,%p])\n",
+	       hermon, hermon_eq->eqn, hermon_eq->eqe,
+	       ( ( ( void * ) hermon_eq->eqe ) + hermon_eq->eqe_size ) );
+	return 0;
+
+ err_map_eq:
+	hermon_cmd_hw2sw_eq ( hermon, hermon_eq->eqn, &eqctx );
+ err_sw2hw_eq:
+	hermon_free_mtt ( hermon, &hermon_eq->mtt );
+ err_alloc_mtt:
+	free_dma ( hermon_eq->eqe, hermon_eq->eqe_size );
+ err_eqe:
+	memset ( hermon_eq, 0, sizeof ( *hermon_eq ) );
+	return rc;
+}
+
+/**
+ * Destroy event queue
+ *
+ * @v hermon		Hermon device
+ */
+static void hermon_destroy_eq ( struct hermon *hermon ) {
+	struct hermon_event_queue *hermon_eq = &hermon->eq;
+	struct hermonprm_eqc eqctx;
+	struct hermonprm_event_mask mask;
+	int rc;
+
+	/* Unmap events from event queue */
+	memset ( &mask, 0, sizeof ( mask ) );
+	MLX_FILL_1 ( &mask, 1, port_state_change, 1 );
+	if ( ( rc = hermon_cmd_map_eq ( hermon,
+					( HERMON_UNMAP_EQ | hermon_eq->eqn ),
+					&mask ) ) != 0 ) {
+		DBGC ( hermon, "Hermon %p FATAL MAP_EQ failed to unmap: %s\n",
+		       hermon, strerror ( rc ) );
+		/* Continue; HCA may die but system should survive */
+	}
+
+	/* Take ownership back from hardware */
+	if ( ( rc = hermon_cmd_hw2sw_eq ( hermon, hermon_eq->eqn,
+					  &eqctx ) ) != 0 ) {
+		DBGC ( hermon, "Hermon %p FATAL HW2SW_EQ failed: %s\n",
+		       hermon, strerror ( rc ) );
+		/* Leak memory and return; at least we avoid corruption */
+		return;
+	}
+
+	/* Free MTT entries */
+	hermon_free_mtt ( hermon, &hermon_eq->mtt );
+
+	/* Free memory */
+	free_dma ( hermon_eq->eqe, hermon_eq->eqe_size );
+	memset ( hermon_eq, 0, sizeof ( *hermon_eq ) );
+}
+
+/**
+ * Handle port state event
+ *
+ * @v hermon		Hermon device
+ * @v eqe		Port state change event queue entry
+ */
+static void hermon_event_port_state_change ( struct hermon *hermon,
+					     union hermonprm_event_entry *eqe){
+	unsigned int port;
+	int link_up;
+
+	/* Get port and link status */
+	port = ( MLX_GET ( &eqe->port_state_change, data.p ) - 1 );
+	link_up = ( MLX_GET ( &eqe->generic, event_sub_type ) & 0x04 );
+	DBGC ( hermon, "Hermon %p port %d link %s\n", hermon, ( port + 1 ),
+	       ( link_up ? "up" : "down" ) );
+
+	/* Sanity check */
+	if ( port >= HERMON_NUM_PORTS ) {
+		DBGC ( hermon, "Hermon %p port %d does not exist!\n",
+		       hermon, ( port + 1 ) );
+		return;
+	}
+
+	/* Notify Infiniband core of link state change */
+	ib_link_state_changed ( hermon->ibdev[port] );
+}
+
+/**
+ * Poll event queue
+ *
+ * @v ibdev		Infiniband device
+ */
+static void hermon_poll_eq ( struct ib_device *ibdev ) {
+	struct hermon *hermon = ib_get_drvdata ( ibdev );
+	struct hermon_event_queue *hermon_eq = &hermon->eq;
+	union hermonprm_event_entry *eqe;
+	union hermonprm_doorbell_register db_reg;
+	unsigned int eqe_idx_mask;
+	unsigned int event_type;
+
+	while ( 1 ) {
+		/* Look for event entry */
+		eqe_idx_mask = ( HERMON_NUM_EQES - 1 );
+		eqe = &hermon_eq->eqe[hermon_eq->next_idx & eqe_idx_mask];
+		if ( MLX_GET ( &eqe->generic, owner ) ^
+		     ( ( hermon_eq->next_idx & HERMON_NUM_EQES ) ? 1 : 0 ) ) {
+			/* Entry still owned by hardware; end of poll */
+			break;
+		}
+		DBGCP ( hermon, "Hermon %p event:\n", hermon );
+		DBGCP_HD ( hermon, eqe, sizeof ( *eqe ) );
+
+		/* Handle event */
+		event_type = MLX_GET ( &eqe->generic, event_type );
+		switch ( event_type ) {
+		case HERMON_EV_PORT_STATE_CHANGE:
+			hermon_event_port_state_change ( hermon, eqe );
+			break;
+		default:
+			DBGC ( hermon, "Hermon %p unrecognised event type "
+			       "%#x:\n", hermon, event_type );
+			DBGC_HD ( hermon, eqe, sizeof ( *eqe ) );
+			break;
+		}
+
+		/* Update event queue's index */
+		hermon_eq->next_idx++;
+
+		/* Ring doorbell */
+		MLX_FILL_1 ( &db_reg.event, 0,
+			     ci, ( hermon_eq->next_idx & 0x00ffffffUL ) );
+		DBGCP ( hermon, "Ringing doorbell %08lx with %08lx\n",
+			virt_to_phys ( hermon_eq->doorbell ),
+			db_reg.dword[0] );
+		writel ( db_reg.dword[0], hermon_eq->doorbell );
 	}
 }
 
@@ -1356,10 +1632,12 @@ static struct ib_device_operations hermon_ib_operations = {
 	.create_cq	= hermon_create_cq,
 	.destroy_cq	= hermon_destroy_cq,
 	.create_qp	= hermon_create_qp,
+	.modify_qp	= hermon_modify_qp,
 	.destroy_qp	= hermon_destroy_qp,
 	.post_send	= hermon_post_send,
 	.post_recv	= hermon_post_recv,
 	.poll_cq	= hermon_poll_cq,
+	.poll_eq	= hermon_poll_eq,
 	.open		= hermon_open,
 	.close		= hermon_close,
 	.mcast_attach	= hermon_mcast_attach,
@@ -1900,9 +2178,8 @@ static int hermon_probe ( struct pci_device *pci,
 	/* Get PCI BARs */
 	hermon->config = ioremap ( pci_bar_start ( pci, HERMON_PCI_CONFIG_BAR),
 				   HERMON_PCI_CONFIG_BAR_SIZE );
-	hermon->uar = ioremap ( ( pci_bar_start ( pci, HERMON_PCI_UAR_BAR ) +
-				  HERMON_UAR_PAGE * HERMON_PAGE_SIZE ),
-				HERMON_PAGE_SIZE );
+	hermon->uar = ioremap ( pci_bar_start ( pci, HERMON_PCI_UAR_BAR ),
+				HERMON_UAR_NON_EQ_PAGE * HERMON_PAGE_SIZE );
 
 	/* Allocate space for mailboxes */
 	hermon->mailbox_in = malloc_dma ( HERMON_MBOX_SIZE,
@@ -1945,6 +2222,10 @@ static int hermon_probe ( struct pci_device *pci,
 	if ( ( rc = hermon_setup_mpt ( hermon ) ) != 0 )
 		goto err_setup_mpt;
 
+	/* Set up event queue */
+	if ( ( rc = hermon_create_eq ( hermon ) ) != 0 )
+		goto err_create_eq;
+
 	/* Register Infiniband devices */
 	for ( i = 0 ; i < HERMON_NUM_PORTS ; i++ ) {
 		if ( ( rc = register_ibdev ( hermon->ibdev[i] ) ) != 0 ) {
@@ -1960,6 +2241,8 @@ static int hermon_probe ( struct pci_device *pci,
  err_register_ibdev:
 	for ( ; i >= 0 ; i-- )
 		unregister_ibdev ( hermon->ibdev[i] );
+	hermon_destroy_eq ( hermon );
+ err_create_eq:
  err_setup_mpt:
 	hermon_cmd_close_hca ( hermon );
  err_init_hca:
@@ -1975,7 +2258,7 @@ static int hermon_probe ( struct pci_device *pci,
 	i = ( HERMON_NUM_PORTS - 1 );
  err_alloc_ibdev:
 	for ( ; i >= 0 ; i-- )
-		free_ibdev ( hermon->ibdev[i] );
+		ibdev_put ( hermon->ibdev[i] );
 	free ( hermon );
  err_alloc_hermon:
 	return rc;
@@ -1992,6 +2275,7 @@ static void hermon_remove ( struct pci_device *pci ) {
 
 	for ( i = ( HERMON_NUM_PORTS - 1 ) ; i >= 0 ; i-- )
 		unregister_ibdev ( hermon->ibdev[i] );
+	hermon_destroy_eq ( hermon );
 	hermon_cmd_close_hca ( hermon );
 	hermon_free_icm ( hermon );
 	hermon_stop_firmware ( hermon );
@@ -1999,7 +2283,7 @@ static void hermon_remove ( struct pci_device *pci ) {
 	free_dma ( hermon->mailbox_out, HERMON_MBOX_SIZE );
 	free_dma ( hermon->mailbox_in, HERMON_MBOX_SIZE );
 	for ( i = ( HERMON_NUM_PORTS - 1 ) ; i >= 0 ; i-- )
-		free_ibdev ( hermon->ibdev[i] );
+		ibdev_put ( hermon->ibdev[i] );
 	free ( hermon );
 }
 
diff --git a/gpxe/src/drivers/infiniband/hermon.h b/gpxe/src/drivers/infiniband/hermon.h
index 959e6a9..ed39da6 100644
--- a/gpxe/src/drivers/infiniband/hermon.h
+++ b/gpxe/src/drivers/infiniband/hermon.h
@@ -18,7 +18,7 @@
  */
 
 /* Ports in existence */
-#define HERMON_NUM_PORTS		1
+#define HERMON_NUM_PORTS		2
 #define HERMON_PORT_BASE		1
 
 /* PCI BARs */
@@ -43,11 +43,13 @@
 #define HERMON_HCR_MAP_EQ		0x0012
 #define HERMON_HCR_SW2HW_EQ		0x0013
 #define HERMON_HCR_HW2SW_EQ		0x0014
+#define HERMON_HCR_QUERY_EQ		0x0015
 #define HERMON_HCR_SW2HW_CQ		0x0016
 #define HERMON_HCR_HW2SW_CQ		0x0017
 #define HERMON_HCR_RST2INIT_QP		0x0019
 #define HERMON_HCR_INIT2RTR_QP		0x001a
 #define HERMON_HCR_RTR2RTS_QP		0x001b
+#define HERMON_HCR_RTS2RTS_QP		0x001c
 #define HERMON_HCR_2RST_QP		0x0021
 #define HERMON_HCR_MAD_IFC		0x0024
 #define HERMON_HCR_READ_MCG		0x0025
@@ -75,6 +77,15 @@
 #define HERMON_PAGE_SIZE		4096
 
 #define HERMON_DB_POST_SND_OFFSET	0x14
+#define HERMON_DB_EQ_OFFSET(_eqn)	\
+	( 0x800 + HERMON_PAGE_SIZE * ( (_eqn) / 4 ) + 0x08 * ( (_eqn) % 4 ) )
+
+#define HERMON_QP_OPT_PARAM_QKEY	0x00000020UL
+
+#define HERMON_MAP_EQ			( 0UL << 31 )
+#define HERMON_UNMAP_EQ			( 1UL << 31 )
+
+#define HERMON_EV_PORT_STATE_CHANGE	0x09
 
 /*
  * Datatypes that seem to be missing from the autogenerated documentation
@@ -108,12 +119,32 @@ struct hermonprm_send_db_register_st {
 	pseudo_bit_t qn[0x00018];
 } __attribute__ (( packed ));
 
+struct hermonprm_event_db_register_st {
+	pseudo_bit_t ci[0x00018];
+	pseudo_bit_t reserver[0x00007];
+	pseudo_bit_t a[0x00001];
+} __attribute__ (( packed ));
+
 struct hermonprm_scalar_parameter_st {
 	pseudo_bit_t value_hi[0x00020];
 /* -------------- */
 	pseudo_bit_t value[0x00020];
 } __attribute__ (( packed ));
 
+struct hermonprm_event_mask_st {
+	pseudo_bit_t reserved0[0x00020];
+/* -------------- */
+	pseudo_bit_t completion[0x00001];
+	pseudo_bit_t reserved1[0x0008];
+	pseudo_bit_t port_state_change[0x00001];
+	pseudo_bit_t reserved2[0x00016];
+} __attribute__ (( packed ));
+
+struct hermonprm_port_state_change_event_st {
+	pseudo_bit_t reserved[0x00020];
+	struct hermonprm_port_state_change_st data;
+} __attribute__ (( packed ));
+
 /*
  * Wrapper structures for hardware datatypes
  *
@@ -124,6 +155,9 @@ struct MLX_DECLARE_STRUCT ( hermonprm_completion_queue_entry );
 struct MLX_DECLARE_STRUCT ( hermonprm_completion_with_error );
 struct MLX_DECLARE_STRUCT ( hermonprm_cq_db_record );
 struct MLX_DECLARE_STRUCT ( hermonprm_eqc );
+struct MLX_DECLARE_STRUCT ( hermonprm_event_db_register );
+struct MLX_DECLARE_STRUCT ( hermonprm_event_mask );
+struct MLX_DECLARE_STRUCT ( hermonprm_event_queue_entry );
 struct MLX_DECLARE_STRUCT ( hermonprm_hca_command_register );
 struct MLX_DECLARE_STRUCT ( hermonprm_init_hca );
 struct MLX_DECLARE_STRUCT ( hermonprm_init_port );
@@ -132,6 +166,7 @@ struct MLX_DECLARE_STRUCT ( hermonprm_mcg_entry );
 struct MLX_DECLARE_STRUCT ( hermonprm_mgm_hash );
 struct MLX_DECLARE_STRUCT ( hermonprm_mpt );
 struct MLX_DECLARE_STRUCT ( hermonprm_mtt );
+struct MLX_DECLARE_STRUCT ( hermonprm_port_state_change_event );
 struct MLX_DECLARE_STRUCT ( hermonprm_qp_db_record );
 struct MLX_DECLARE_STRUCT ( hermonprm_qp_ee_state_transitions );
 struct MLX_DECLARE_STRUCT ( hermonprm_query_dev_cap );
@@ -175,8 +210,14 @@ union hermonprm_completion_entry {
 	struct hermonprm_completion_with_error error;
 } __attribute__ (( packed ));
 
+union hermonprm_event_entry {
+	struct hermonprm_event_queue_entry generic;
+	struct hermonprm_port_state_change_event port_state_change;
+} __attribute__ (( packed ));
+
 union hermonprm_doorbell_register {
 	struct hermonprm_send_db_register send;
+	struct hermonprm_event_db_register event;
 	uint32_t dword[1];
 } __attribute__ (( packed ));
 
@@ -252,7 +293,7 @@ enum hermon_icm_map_regions {
  * Pages 0-127 are reserved for event queue doorbells only, so we use
  * page 128.
  */
-#define HERMON_UAR_PAGE		128
+#define HERMON_UAR_NON_EQ_PAGE	128
 
 /** Maximum number of allocatable MTT entries
  *
@@ -294,6 +335,8 @@ struct hermon_send_work_queue {
 	union hermon_send_wqe *wqe;
 	/** Size of work queue */
 	size_t wqe_size;
+	/** Doorbell register */
+	void *doorbell;
 };
 
 /** Alignment of Hermon receive work queue entries */
@@ -360,7 +403,29 @@ struct hermon_completion_queue {
  *
  * This is a policy decision, not a device limit.
  */
-#define HERMON_MAX_EQS		4
+#define HERMON_MAX_EQS		8
+
+/** A Hermon event queue */
+struct hermon_event_queue {
+	/** Event queue entries */
+	union hermonprm_event_entry *eqe;
+	/** Size of event queue */
+	size_t eqe_size;
+	/** MTT descriptor */
+	struct hermon_mtt mtt;
+	/** Event queue number */
+	unsigned long eqn;
+	/** Next event queue entry index */
+	unsigned long next_idx;
+	/** Doorbell register */
+	void *doorbell;
+};
+
+/** Number of event queue entries
+ *
+ * This is a policy decision.
+ */
+#define HERMON_NUM_EQES		4
 
 /** A Hermon resource bitmask */
 typedef uint32_t hermon_bitmask_t;
@@ -391,6 +456,8 @@ struct hermon {
 	/** ICM area */
 	userptr_t icm;
 
+	/** Event queue */
+	struct hermon_event_queue eq;
 	/** Reserved LKey
 	 *
 	 * Used to get unrestricted memory access.
diff --git a/gpxe/src/drivers/net/e1000/e1000.c b/gpxe/src/drivers/net/e1000/e1000.c
index 739217c..a9aa508 100644
--- a/gpxe/src/drivers/net/e1000/e1000.c
+++ b/gpxe/src/drivers/net/e1000/e1000.c
@@ -876,6 +876,9 @@ e1000_probe ( struct pci_device *pdev,
 	
 	e1000_get_hw_control ( adapter );
 
+	/* Mark as link up; we don't yet handle link state */
+	netdev_link_up ( netdev );
+
 	if ( ( err = register_netdev ( netdev ) ) != 0)
 		goto err_register;
 		
diff --git a/gpxe/src/drivers/net/ipoib.c b/gpxe/src/drivers/net/ipoib.c
index d457b25..e3baa14 100644
--- a/gpxe/src/drivers/net/ipoib.c
+++ b/gpxe/src/drivers/net/ipoib.c
@@ -80,10 +80,14 @@ struct ipoib_device {
 	struct ib_gid broadcast_gid;
 	/** Broadcast LID */
 	unsigned int broadcast_lid;
-	/** Joined to broadcast group */
-	int broadcast_joined;
 	/** Data queue key */
 	unsigned long data_qkey;
+	/** Attached to multicast group
+	 *
+	 * This flag indicates whether or not we have attached our
+	 * data queue pair to the broadcast multicast GID.
+	 */
+	int broadcast_attached;
 };
 
 /**
@@ -272,6 +276,10 @@ static int ipoib_create_qset ( struct ipoib_device *ipoib,
 	struct ib_device *ibdev = ipoib->ibdev;
 	int rc;
 
+	/* Sanity check */
+	assert ( qset->cq == NULL );
+	assert ( qset->qp == NULL );
+
 	/* Store queue parameters */
 	qset->recv_max_fill = num_recv_wqes;
 
@@ -463,6 +471,12 @@ static int ipoib_transmit ( struct net_device *netdev,
 	}
 	iob_pull ( iobuf, ( sizeof ( *ipoib_pshdr ) ) );
 
+	/* Attempting transmission while link is down will put the
+	 * queue pair into an error state, so don't try it.
+	 */
+	if ( ! ibdev->link_up )
+		return -ENETUNREACH;
+
 	/* Construct address vector */
 	memset ( &av, 0, sizeof ( av ) );
 	av.qkey = IB_GLOBAL_QKEY;
@@ -617,14 +631,24 @@ static void ipoib_recv_path_record ( struct ipoib_device *ipoib __unused,
  */
 static void ipoib_recv_mc_member_record ( struct ipoib_device *ipoib,
 			  struct ib_mad_mc_member_record *mc_member_record ) {
+	int joined;
+	int rc;
+
 	/* Record parameters */
-	ipoib->broadcast_joined =
-		( mc_member_record->scope__join_state & 0x0f );
+	joined = ( mc_member_record->scope__join_state & 0x0f );
 	ipoib->data_qkey = ntohl ( mc_member_record->qkey );
 	ipoib->broadcast_lid = ntohs ( mc_member_record->mlid );
 	DBGC ( ipoib, "IPoIB %p %s broadcast group: qkey %lx mlid %x\n",
-	       ipoib, ( ipoib->broadcast_joined ? "joined" : "left" ),
-	       ipoib->data_qkey, ipoib->broadcast_lid );
+	       ipoib, ( joined ? "joined" : "left" ), ipoib->data_qkey,
+	       ipoib->broadcast_lid );
+
+	/* Update data queue pair qkey */
+	if ( ( rc = ib_modify_qp ( ipoib->ibdev, ipoib->data.qp,
+				   IB_MODIFY_QKEY, ipoib->data_qkey ) ) != 0 ){
+		DBGC ( ipoib, "IPoIB %p could not update data qkey: %s\n",
+		       ipoib, strerror ( rc ) );
+		return;
+	}
 }
 
 /**
@@ -742,6 +766,60 @@ static void ipoib_irq ( struct net_device *netdev __unused,
 }
 
 /**
+ * Join IPv4 broadcast multicast group
+ *
+ * @v ipoib		IPoIB device
+ * @ret rc		Return status code
+ */
+static int ipoib_join_broadcast_group ( struct ipoib_device *ipoib ) {
+	int rc;
+
+	/* Sanity check */
+	if ( ! ipoib->data.qp )
+		return 0;
+
+	/* Attach data queue to broadcast multicast GID */
+	assert ( ipoib->broadcast_attached == 0 );
+	if ( ( rc = ib_mcast_attach ( ipoib->ibdev, ipoib->data.qp,
+				      &ipoib->broadcast_gid ) ) != 0 ){
+		DBGC ( ipoib, "IPoIB %p could not attach to broadcast GID: "
+		       "%s\n", ipoib, strerror ( rc ) );
+		return rc;
+	}
+	ipoib->broadcast_attached = 1;
+
+	/* Initiate broadcast group join */
+	if ( ( rc = ipoib_mc_member_record ( ipoib, &ipoib->broadcast_gid,
+					     1 ) ) != 0 ) {
+		DBGC ( ipoib, "IPoIB %p could not send broadcast join: %s\n",
+		       ipoib, strerror ( rc ) );
+		return rc;
+	}
+
+	/* We will set link up on the network device when we receive
+	 * the broadcast join response.
+	 */
+
+	return 0;
+}
+
+/**
+ * Leave IPv4 broadcast multicast group
+ *
+ * @v ipoib		IPoIB device
+ */
+static void ipoib_leave_broadcast_group ( struct ipoib_device *ipoib ) {
+
+	/* Detach data queue from broadcast multicast GID */
+	if ( ipoib->broadcast_attached ) {
+		assert ( ipoib->data.qp != NULL );
+		ib_mcast_detach ( ipoib->ibdev, ipoib->data.qp,
+				  &ipoib->broadcast_gid );
+		ipoib->broadcast_attached = 0;
+	}
+}
+
+/**
  * Open IPoIB network device
  *
  * @v netdev		Network device
@@ -749,22 +827,53 @@ static void ipoib_irq ( struct net_device *netdev __unused,
  */
 static int ipoib_open ( struct net_device *netdev ) {
 	struct ipoib_device *ipoib = netdev->priv;
-	struct ib_device *ibdev = ipoib->ibdev;
+	struct ipoib_mac *mac = ( ( struct ipoib_mac * ) netdev->ll_addr );
 	int rc;
 
-	/* Attach to broadcast multicast GID */
-	if ( ( rc = ib_mcast_attach ( ibdev, ipoib->data.qp,
-				      &ipoib->broadcast_gid ) ) != 0 ) {
-		DBG ( "Could not attach to broadcast GID: %s\n",
-		      strerror ( rc ) );
-		return rc;
+	/* Allocate metadata queue set */
+	if ( ( rc = ipoib_create_qset ( ipoib, &ipoib->meta,
+					IPOIB_META_NUM_CQES,
+					IPOIB_META_NUM_SEND_WQES,
+					IPOIB_META_NUM_RECV_WQES,
+					IB_GLOBAL_QKEY ) ) != 0 ) {
+		DBGC ( ipoib, "IPoIB %p could not allocate metadata QP: %s\n",
+		       ipoib, strerror ( rc ) );
+		goto err_create_meta_qset;
 	}
 
+	/* Allocate data queue set */
+	if ( ( rc = ipoib_create_qset ( ipoib, &ipoib->data,
+					IPOIB_DATA_NUM_CQES,
+					IPOIB_DATA_NUM_SEND_WQES,
+					IPOIB_DATA_NUM_RECV_WQES,
+					IB_GLOBAL_QKEY ) ) != 0 ) {
+		DBGC ( ipoib, "IPoIB %p could not allocate data QP: %s\n",
+		       ipoib, strerror ( rc ) );
+		goto err_create_data_qset;
+	}
+
+	/* Update MAC address with data QPN */
+	mac->qpn = htonl ( ipoib->data.qp->qpn );
+
 	/* Fill receive rings */
 	ipoib_refill_recv ( ipoib, &ipoib->meta );
 	ipoib_refill_recv ( ipoib, &ipoib->data );
 
+	/* Join broadcast group */
+	if ( ( rc = ipoib_join_broadcast_group ( ipoib ) ) != 0 ) {
+		DBGC ( ipoib, "IPoIB %p could not join broadcast group: %s\n",
+		       ipoib, strerror ( rc ) );
+		goto err_join_broadcast;
+	}
+
 	return 0;
+
+ err_join_broadcast:
+	ipoib_destroy_qset ( ipoib, &ipoib->data );
+ err_create_data_qset:
+	ipoib_destroy_qset ( ipoib, &ipoib->meta );
+ err_create_meta_qset:
+	return rc;
 }
 
 /**
@@ -774,12 +883,17 @@ static int ipoib_open ( struct net_device *netdev ) {
  */
 static void ipoib_close ( struct net_device *netdev ) {
 	struct ipoib_device *ipoib = netdev->priv;
-	struct ib_device *ibdev = ipoib->ibdev;
+	struct ipoib_mac *mac = ( ( struct ipoib_mac * ) netdev->ll_addr );
 
-	/* Detach from broadcast multicast GID */
-	ib_mcast_detach ( ibdev, ipoib->data.qp, &ipoib->broadcast_gid );
+	/* Leave broadcast group */
+	ipoib_leave_broadcast_group ( ipoib );
 
-	/* FIXME: should probably flush the receive ring */
+	/* Remove data QPN from MAC address */
+	mac->qpn = 0;
+
+	/* Tear down the queues */
+	ipoib_destroy_qset ( ipoib, &ipoib->data );
+	ipoib_destroy_qset ( ipoib, &ipoib->meta );
 }
 
 /** IPoIB network device operations */
@@ -792,44 +906,61 @@ static struct net_device_operations ipoib_operations = {
 };
 
 /**
- * Join IPoIB broadcast group
+ * Update IPoIB dynamic Infiniband parameters
  *
  * @v ipoib		IPoIB device
- * @ret rc		Return status code
+ *
+ * The Infiniband port GID and partition key will change at runtime,
+ * when the link is established (or lost).  The MAC address is based
+ * on the port GID, and the broadcast GID is based on the partition
+ * key.  This function recalculates these IPoIB device parameters.
  */
-static int ipoib_join_broadcast_group ( struct ipoib_device *ipoib ) {
+static void ipoib_set_ib_params ( struct ipoib_device *ipoib ) {
 	struct ib_device *ibdev = ipoib->ibdev;
-	unsigned int delay_ms;
-	int rc;
+	struct net_device *netdev = ipoib->netdev;
+	struct ipoib_mac *mac;
 
-	/* Make sure we have some receive descriptors */
-	ipoib_refill_recv ( ipoib, &ipoib->meta );
+	/* Calculate GID portion of MAC address based on port GID */
+	mac = ( ( struct ipoib_mac * ) netdev->ll_addr );
+	memcpy ( &mac->gid, &ibdev->port_gid, sizeof ( mac->gid ) );
 
-	/* Send join request */
-	if ( ( rc = ipoib_mc_member_record ( ipoib, &ipoib->broadcast_gid,
-					     1 ) ) != 0 ) {
-		DBGC ( ipoib, "IPoIB %p could not send broadcast join: %s\n",
-		       ipoib, strerror ( rc ) );
-		return rc;
+	/* Calculate broadcast GID based on partition key */
+	memcpy ( &ipoib->broadcast_gid, &ipv4_broadcast_gid,
+		 sizeof ( ipoib->broadcast_gid ) );
+	ipoib->broadcast_gid.u.words[2] = htons ( ibdev->pkey );
+
+	/* Set net device link state to reflect Infiniband link state */
+	if ( ibdev->link_up ) {
+		netdev_link_up ( netdev );
+	} else {
+		netdev_link_down ( netdev );
 	}
+}
+
+/**
+ * Handle link status change
+ *
+ * @v ibdev		Infiniband device
+ */
+void ipoib_link_state_changed ( struct ib_device *ibdev ) {
+	struct net_device *netdev = ib_get_ownerdata ( ibdev );
+	struct ipoib_device *ipoib = netdev->priv;
+	int rc;
 
-	/* Wait for join to complete.  Ideally we wouldn't delay for
-	 * this long, but we need the queue key before we can set up
-	 * the data queue pair, which we need before we can know the
-	 * MAC address.
+	/* Leave existing broadcast group */
+	ipoib_leave_broadcast_group ( ipoib );
+
+	/* Update MAC address and broadcast GID based on new port GID
+	 * and partition key.
 	 */
-	for ( delay_ms = IPOIB_JOIN_MAX_DELAY_MS ; delay_ms ; delay_ms-- ) {
-		mdelay ( 1 );
-		ib_poll_cq ( ibdev, ipoib->meta.cq, ipoib_meta_complete_send,
-			     ipoib_meta_complete_recv );
-		ipoib_refill_recv ( ipoib, &ipoib->meta );
-		if ( ipoib->broadcast_joined )
-			return 0;
-	}
-	DBGC ( ipoib, "IPoIB %p timed out waiting for broadcast join\n",
-	       ipoib );
+	ipoib_set_ib_params ( ipoib );
 
-	return -ETIMEDOUT;
+	/* Join new broadcast group */
+	if ( ( rc = ipoib_join_broadcast_group ( ipoib ) ) != 0 ) {
+		DBGC ( ipoib, "IPoIB %p could not rejoin broadcast group: "
+		       "%s\n", ipoib, strerror ( rc ) );
+		return;
+	}
 }
 
 /**
@@ -841,7 +972,6 @@ static int ipoib_join_broadcast_group ( struct ipoib_device *ipoib ) {
 int ipoib_probe ( struct ib_device *ibdev ) {
 	struct net_device *netdev;
 	struct ipoib_device *ipoib;
-	struct ipoib_mac *mac;
 	int rc;
 
 	/* Allocate network device */
@@ -856,44 +986,11 @@ int ipoib_probe ( struct ib_device *ibdev ) {
 	ipoib->netdev = netdev;
 	ipoib->ibdev = ibdev;
 
-	/* Calculate broadcast GID */
-	memcpy ( &ipoib->broadcast_gid, &ipv4_broadcast_gid,
-		 sizeof ( ipoib->broadcast_gid ) );
-	ipoib->broadcast_gid.u.words[2] = htons ( ibdev->pkey );
-
-	/* Allocate metadata queue set */
-	if ( ( rc = ipoib_create_qset ( ipoib, &ipoib->meta,
-					IPOIB_META_NUM_CQES,
-					IPOIB_META_NUM_SEND_WQES,
-					IPOIB_META_NUM_RECV_WQES,
-					IB_GLOBAL_QKEY ) ) != 0 ) {
-		DBGC ( ipoib, "IPoIB %p could not allocate metadata QP: %s\n",
-		       ipoib, strerror ( rc ) );
-		goto err_create_meta_qset;
-	}
-
-	/* Join broadcast group */
-	if ( ( rc = ipoib_join_broadcast_group ( ipoib ) ) != 0 ) {
-		DBGC ( ipoib, "IPoIB %p could not join broadcast group: %s\n",
-		       ipoib, strerror ( rc ) );
-		goto err_join_broadcast_group;
-	}
-
-	/* Allocate data queue set */
-	if ( ( rc = ipoib_create_qset ( ipoib, &ipoib->data,
-					IPOIB_DATA_NUM_CQES,
-					IPOIB_DATA_NUM_SEND_WQES,
-					IPOIB_DATA_NUM_RECV_WQES,
-					ipoib->data_qkey ) ) != 0 ) {
-		DBGC ( ipoib, "IPoIB %p could not allocate data QP: %s\n",
-		       ipoib, strerror ( rc ) );
-		goto err_create_data_qset;
-	}
-
-	/* Construct MAC address */
-	mac = ( ( struct ipoib_mac * ) netdev->ll_addr );
-	mac->qpn = htonl ( ipoib->data.qp->qpn );
-	memcpy ( &mac->gid, &ibdev->port_gid, sizeof ( mac->gid ) );
+	/* Calculate as much of the broadcast GID and the MAC address
+	 * as we can.  We won't know either of these in full until we
+	 * have link-up.
+	 */
+	ipoib_set_ib_params ( ipoib );
 
 	/* Register network device */
 	if ( ( rc = register_netdev ( netdev ) ) != 0 )
@@ -902,11 +999,6 @@ int ipoib_probe ( struct ib_device *ibdev ) {
 	return 0;
 
  err_register_netdev:
-	ipoib_destroy_qset ( ipoib, &ipoib->data );
- err_join_broadcast_group:
- err_create_data_qset:
-	ipoib_destroy_qset ( ipoib, &ipoib->meta );
- err_create_meta_qset:
 	netdev_nullify ( netdev );
 	netdev_put ( netdev );
 	return rc;
@@ -919,11 +1011,8 @@ int ipoib_probe ( struct ib_device *ibdev ) {
  */
 void ipoib_remove ( struct ib_device *ibdev ) {
 	struct net_device *netdev = ib_get_ownerdata ( ibdev );
-	struct ipoib_device *ipoib = netdev->priv;
 
 	unregister_netdev ( netdev );
-	ipoib_destroy_qset ( ipoib, &ipoib->data );
-	ipoib_destroy_qset ( ipoib, &ipoib->meta );
 	netdev_nullify ( netdev );
 	netdev_put ( netdev );
 }
diff --git a/gpxe/src/drivers/net/legacy.c b/gpxe/src/drivers/net/legacy.c
index 32460ad..cbec3cf 100644
--- a/gpxe/src/drivers/net/legacy.c
+++ b/gpxe/src/drivers/net/legacy.c
@@ -112,6 +112,9 @@ int legacy_probe ( void *hwdev,
 	 */
 	dev->desc.irq = nic.irqno;
 
+	/* Mark as link up; legacy devices don't handle link state */
+	netdev_link_up ( netdev );
+
 	if ( ( rc = register_netdev ( netdev ) ) != 0 )
 		goto err_register;
 
diff --git a/gpxe/src/drivers/net/mlx_ipoib/mt23108.c b/gpxe/src/drivers/net/mlx_ipoib/mt23108.c
index 492bc90..e1f61db 100644
--- a/gpxe/src/drivers/net/mlx_ipoib/mt23108.c
+++ b/gpxe/src/drivers/net/mlx_ipoib/mt23108.c
@@ -10,6 +10,8 @@ Skeleton NIC driver for Etherboot
  * your option) any later version.
  */
 
+/* to get toupper() */
+#include <ctype.h>
 /* to get some global routines like printf */
 #include "etherboot.h"
 /* to get the interface to the body of the program */
@@ -31,12 +33,7 @@ int prompt_key(int secs, unsigned char *ch_p)
 
 	for (tmo = currticks() + secs * TICKS_PER_SEC; currticks() < tmo;) {
 		if (iskey()) {
-			ch = getchar();
-			/* toupper does not work ... */
-			if (ch == 'v')
-				ch = 'V';
-			if (ch == 'i')
-				ch = 'I';
+			ch = toupper(getchar());
 			if ((ch=='V') || (ch=='I')) {
 				*ch_p = ch;
 				return 1;
diff --git a/gpxe/src/drivers/net/mlx_ipoib/mt25218.c b/gpxe/src/drivers/net/mlx_ipoib/mt25218.c
index a603cde..8a252ea 100644
--- a/gpxe/src/drivers/net/mlx_ipoib/mt25218.c
+++ b/gpxe/src/drivers/net/mlx_ipoib/mt25218.c
@@ -10,6 +10,8 @@ Skeleton NIC driver for Etherboot
  * your option) any later version.
  */
 
+/* to get toupper() */
+#include <ctype.h>
 /* to get some global routines like printf */
 #include "etherboot.h"
 /* to get the interface to the body of the program */
@@ -31,12 +33,7 @@ int prompt_key(int secs, unsigned char *ch_p)
 
 	for (tmo = currticks() + secs * TICKS_PER_SEC; currticks() < tmo;) {
 		if (iskey()) {
-			ch = getchar();
-			/* toupper does not work ... */
-			if (ch == 'v')
-				ch = 'V';
-			if (ch == 'i')
-				ch = 'I';
+			ch = toupper(getchar());
 			if ((ch=='V') || (ch=='I')) {
 				*ch_p = ch;
 				return 1;
diff --git a/gpxe/src/drivers/net/mtnic.c b/gpxe/src/drivers/net/mtnic.c
index 536fcb8..dd577f4 100755
--- a/gpxe/src/drivers/net/mtnic.c
+++ b/gpxe/src/drivers/net/mtnic.c
@@ -35,8 +35,7 @@
 #include <errno.h>
 #include <gpxe/malloc.h>
 #include <gpxe/umalloc.h>
-#include <bits/byteswap.h>
-#include <little_bswap.h>
+#include <byteswap.h>
 #include <unistd.h>
 #include <gpxe/pci.h>
 #include <gpxe/ethernet.h>
@@ -54,6 +53,10 @@
 */
 
 
+/* (mcb30) - The Mellanox driver used "1" as a universal error code;
+ * this at least makes it a valid error number.
+ */
+#define MTNIC_ERROR -EIO
 
 
 /** Set port number to use
@@ -108,12 +111,12 @@ mtnic_alloc_cmdif(struct mtnic_priv *priv)
 
 	priv->hcr = ioremap(bar + MTNIC_HCR_BASE, MTNIC_HCR_SIZE);
 	if (!priv->hcr) {
-		eprintf("Couldn't map command register.");
+		DBG("Couldn't map command register.");
 		return MTNIC_ERROR;
 	}
 	mtnic_alloc_aligned(PAGE_SIZE, (void *)&priv->cmd.buf, &priv->cmd.mapping, PAGE_SIZE);
 	if (!priv->cmd.buf) {
-		eprintf("Error in allocating buffer for command interface\n");
+		DBG("Error in allocating buffer for command interface\n");
 		return MTNIC_ERROR;
 	}
         return 0;
@@ -153,8 +156,8 @@ mtnic_alloc_iobuf(struct mtnic_priv *priv, struct mtnic_ring *ring,
 		ring->iobuf[index] = alloc_iob(size);
 		if (!&ring->iobuf[index]) {
 			if (ring->prod <= (ring->cons + 1)) {
-				eprintf("Error allocating Rx io "
-					"buffer number %lx", index);
+				DBG("Error allocating Rx io "
+				    "buffer number %lx", index);
 				/* In case of error freeing io buffer */
 				mtnic_free_io_buffers(ring);
 				return MTNIC_ERROR;
@@ -208,8 +211,8 @@ mtnic_alloc_ring(struct mtnic_priv *priv, struct mtnic_ring *ring,
 	err = mtnic_alloc_aligned(ring->buf_size, (void *)&ring->buf,
 			    &ring->dma, PAGE_SIZE);
         if (err) {
-		eprintf("Failed allocating descriptor ring sizeof %lx\n",
-			ring->buf_size);
+		DBG("Failed allocating descriptor ring sizeof %lx\n",
+		    ring->buf_size);
 		return MTNIC_ERROR;
 	}
         memset(ring->buf, 0, ring->buf_size);
@@ -225,7 +228,7 @@ mtnic_alloc_ring(struct mtnic_priv *priv, struct mtnic_ring *ring,
 		err = mtnic_alloc_aligned(sizeof(struct mtnic_cq_db_record),
 				    (void *)&ring->db, &ring->db_dma, 32);
 		if (err) {
-			eprintf("Failed allocating Rx ring doorbell record\n");
+			DBG("Failed allocating Rx ring doorbell record\n");
 			free(ring->buf);
 			return MTNIC_ERROR;
 		}
@@ -243,7 +246,7 @@ mtnic_alloc_ring(struct mtnic_priv *priv, struct mtnic_ring *ring,
 		/* Alloc IO_BUFFERS */
 		err = mtnic_alloc_iobuf(priv, ring, DEF_IOBUF_SIZE);
 		if (err) {
-			eprintf("ERROR Allocating io buffer");
+			DBG("ERROR Allocating io buffer");
 			free(ring->buf);
 			return MTNIC_ERROR;
 		}
@@ -260,11 +263,11 @@ mtnic_alloc_ring(struct mtnic_priv *priv, struct mtnic_ring *ring,
 
 		/* Map Tx+CQ doorbells */
 		DBG("Mapping TxCQ doorbell at offset:0x%lx\n",
-			priv->fw.txcq_db_offset);
+		    priv->fw.txcq_db_offset);
 		ring->txcq_db = ioremap(mtnic_pci_dev.dev.bar[2] +
 				priv->fw.txcq_db_offset, PAGE_SIZE);
 		if (!ring->txcq_db) {
-			eprintf("Couldn't map txcq doorbell, aborting...\n");
+			DBG("Couldn't map txcq doorbell, aborting...\n");
 			free(ring->buf);
 			return MTNIC_ERROR;
 		}
@@ -299,7 +302,7 @@ mtnic_alloc_cq(struct net_device *dev, int num, struct mtnic_cq *cq,
 	err = mtnic_alloc_aligned(sizeof(struct mtnic_cq_db_record),
 			    (void *)&cq->db, &cq->db_dma, 32);
 	if (err) {
-		eprintf("Failed allocating CQ doorbell record\n");
+		DBG("Failed allocating CQ doorbell record\n");
 		return MTNIC_ERROR;
 	}
 	memset(cq->db, 0, sizeof(struct mtnic_cq_db_record));
@@ -309,16 +312,16 @@ mtnic_alloc_cq(struct net_device *dev, int num, struct mtnic_cq *cq,
 	err = mtnic_alloc_aligned(cq->buf_size,
 			    (void *)&cq->buf, &cq->dma, PAGE_SIZE);
 	if (err) {
-		eprintf("Failed allocating CQ buffer\n");
+		DBG("Failed allocating CQ buffer\n");
 		free(cq->db);
 		return MTNIC_ERROR;
 	}
         memset(cq->buf, 0, cq->buf_size);
         DBG("Allocated CQ (addr:%p) - size:%lx buf:%p buf_size:%lx "
-		"dma:%lx db:%p db_dma:%lx\n"
-		"cqn offset:%lx \n", cq, cq->size, cq->buf,
-		cq->buf_size, cq->dma, cq->db,
-		cq->db_dma, offset_ind);
+	    "dma:%lx db:%p db_dma:%lx\n"
+	    "cqn offset:%lx \n", cq, cq->size, cq->buf,
+	    cq->buf_size, cq->dma, cq->db,
+	    cq->db_dma, offset_ind);
 
 
 	/* Set ownership of all CQEs to HW */
@@ -349,7 +352,7 @@ mtnic_alloc_resources(struct net_device *dev)
         err = mtnic_alloc_cq(dev, cq_ind, &priv->cq[cq_ind], 1 /* RX */,
 			     UNITS_BUFFER_SIZE, cq_offset + cq_ind);
 	if (err) {
-		eprintf("Failed allocating Rx CQ\n");
+		DBG("Failed allocating Rx CQ\n");
 		return MTNIC_ERROR;
 	}
 
@@ -357,7 +360,7 @@ mtnic_alloc_resources(struct net_device *dev)
 	err = mtnic_alloc_ring(priv, &priv->rx_ring, UNITS_BUFFER_SIZE,
 			       sizeof(struct mtnic_rx_desc), cq_ind, /* RX */1);
 	if (err) {
-		eprintf("Failed allocating Rx Ring\n");
+		DBG("Failed allocating Rx Ring\n");
 		goto cq0_error;
 	}
 
@@ -367,7 +370,7 @@ mtnic_alloc_resources(struct net_device *dev)
 	err = mtnic_alloc_cq(dev, cq_ind, &priv->cq[cq_ind], 0 /* TX */,
 			     UNITS_BUFFER_SIZE, cq_offset + cq_ind);
 	if (err) {
-		eprintf("Failed allocating Tx CQ\n");
+		DBG("Failed allocating Tx CQ\n");
 		goto rx_error;
 	}
 
@@ -375,7 +378,7 @@ mtnic_alloc_resources(struct net_device *dev)
 	err = mtnic_alloc_ring(priv, &priv->tx_ring, UNITS_BUFFER_SIZE,
 			       sizeof(struct mtnic_tx_desc), cq_ind, /* TX */ 0);
 	if (err) {
-		eprintf("Failed allocating Tx ring\n");
+		DBG("Failed allocating Tx ring\n");
 		goto cq1_error;
 	}
 
@@ -412,7 +415,7 @@ mtnic_alloc_eq(struct mtnic_priv *priv)
 	priv->eq_db = ioremap(mtnic_pci_dev.dev.bar[2] +
 			      priv->fw.eq_db_offset, sizeof(u32));
 	if (!priv->eq_db) {
-		eprintf("Couldn't map EQ doorbell, aborting...\n");
+		DBG("Couldn't map EQ doorbell, aborting...\n");
 		return MTNIC_ERROR;
 	}
 
@@ -422,7 +425,7 @@ mtnic_alloc_eq(struct mtnic_priv *priv)
         err = mtnic_alloc_aligned(priv->eq.buf_size, (void *)&priv->eq.buf,
 			    &priv->eq.dma, PAGE_SIZE);
 	if (err) {
-		eprintf("Failed allocating EQ buffer\n");
+		DBG("Failed allocating EQ buffer\n");
 		iounmap(priv->eq_db);
 		return MTNIC_ERROR;
 	}
@@ -470,7 +473,7 @@ cmdif_go_bit(struct mtnic_priv *priv)
 		}
 	}
 
-	eprintf("Invalid tbit after %d retries!\n", TBIT_RETRIES);
+	DBG("Invalid tbit after %d retries!\n", TBIT_RETRIES);
 	return 1; /* Return busy... */
 }
 
@@ -495,7 +498,7 @@ mtnic_cmd(struct mtnic_priv *priv, void *in_imm,
 	token++;
 
 	if (cmdif_go_bit(priv)) {
-		eprintf("GO BIT BUSY:%p.\n", hcr + 6);
+		DBG("GO BIT BUSY:%p.\n", hcr + 6);
 		err = MTNIC_ERROR;
 		goto out;
 	}
@@ -529,7 +532,7 @@ mtnic_cmd(struct mtnic_priv *priv, void *in_imm,
 	}
 
 	if (cmdif_go_bit(priv)) {
-		eprintf("Command opcode:0x%x token:0x%x TIMEOUT.\n", op, token);
+		DBG("Command opcode:0x%x token:0x%x TIMEOUT.\n", op, token);
 		err = MTNIC_ERROR;
 		goto out;
 	}
@@ -570,8 +573,8 @@ mtnic_map_cmd(struct mtnic_priv *priv, u16 op, struct mtnic_pages pages)
 	DBG("Mapping pages: size: %lx address: %p\n", pages.num, pages.buf);
 
 	if (addr & (PAGE_MASK)) {
-		eprintf("Got FW area not aligned to %d (%llx/%x)\n",
-			   PAGE_SIZE, (u64) addr, len);
+		DBG("Got FW area not aligned to %d (%llx/%x)\n",
+		    PAGE_SIZE, (u64) addr, len);
 		return MTNIC_ERROR;
 	}
 
@@ -687,7 +690,7 @@ mtnic_HEART_BEAT(struct mtnic_priv *priv, u32 *link_state)
 	if (!err) {
 		flags = be32_to_cpu(heart_beat.flags);
 		if (flags & MTNIC_BC_MASK(MTNIC_MASK_HEAR_BEAT_INT_ERROR)) {
-			eprintf("Internal error detected\n");
+			DBG("Internal error detected\n");
 			return MTNIC_ERROR;
 		}
 		*link_state = flags &
@@ -746,9 +749,9 @@ mtnic_CONFIG_CQ(struct mtnic_priv *priv, int port,
 	config_cq->db_record_addr_l = cpu_to_be32(cq->db_dma);
         config_cq->page_address[1] = cpu_to_be32(cq->dma);
 	DBG("config cq address: %lx dma_address: %lx"
-		"offset: %d size %d index: %d "
-		, config_cq->page_address[1],cq->dma,
-		config_cq->offset, config_cq->size, config_cq->cq );
+	    "offset: %d size %d index: %d "
+	    , config_cq->page_address[1],cq->dma,
+	    config_cq->offset, config_cq->size, config_cq->cq );
 
 	return mtnic_cmd(priv, NULL, NULL, port + 1,
 			       MTNIC_IF_CMD_CONFIG_CQ);
@@ -798,8 +801,8 @@ mtnic_CONFIG_EQ(struct mtnic_priv *priv)
 	struct mtnic_if_config_eq_in_mbox *eq = priv->cmd.buf;
 
 	if (priv->eq.dma & (PAGE_MASK)) {
-		eprintf("misalligned eq buffer:%lx\n",
-			priv->eq.dma);
+		DBG("misalligned eq buffer:%lx\n",
+		    priv->eq.dma);
 		return MTNIC_ERROR;
         }
 
@@ -897,7 +900,7 @@ mtnic_QUERY_CAP(struct mtnic_priv *priv, u8 index, u8 mod, u64 *result)
 	*((u32*)result + 1) = be32_to_cpu(*out_imm);
 
 	DBG("Called Query cap with index:0x%x mod:%d result:0x%llx"
-		  " error:%d\n", index, mod, *result, err);
+	    " error:%d\n", index, mod, *result, err);
 	return err;
 }
 
@@ -1028,7 +1031,7 @@ mtnic_init_pci(struct pci_device *dev)
 					   &mtnic_pci_dev.dev.
 					   dev_config_space[i]);
 		if (err) {
-			eprintf("Can not save configuration space");
+			DBG("Can not save configuration space");
 			return err;
 		}
 	}
@@ -1056,7 +1059,7 @@ int mtnic_init_card(struct net_device *dev)
         /* Alloc command interface */
 	err = mtnic_alloc_cmdif(priv);
 	if (err) {
-		eprintf("Failed to init command interface, aborting.\n");
+		DBG("Failed to init command interface, aborting.\n");
 		return MTNIC_ERROR;
 	}
 
@@ -1067,7 +1070,7 @@ int mtnic_init_card(struct net_device *dev)
 	 */
 	err = mtnic_QUERY_FW(priv);
 	if (err) {
-		eprintf("QUERY_FW command failed, aborting.\n");
+		DBG("QUERY_FW command failed, aborting.\n");
 		goto cmd_error;
 	}
 
@@ -1076,7 +1079,7 @@ int mtnic_init_card(struct net_device *dev)
 	/* Allocate memory for FW and start it */
 	err = mtnic_map_cmd(priv, MTNIC_IF_CMD_MAP_FW, priv->fw.fw_pages);
 	if (err) {
-		eprintf("Eror In MAP_FW\n");
+		DBG("Eror In MAP_FW\n");
 		if (priv->fw.fw_pages.buf)
 			free(priv->fw.fw_pages.buf);
 		goto cmd_error;
@@ -1085,12 +1088,12 @@ int mtnic_init_card(struct net_device *dev)
 	/* Run firmware */
 	err = mtnic_cmd(priv, NULL, NULL, 0, MTNIC_IF_CMD_RUN_FW);
 	if (err) {
-		eprintf("Eror In RUN FW\n");
+		DBG("Eror In RUN FW\n");
 		goto map_fw_error;
 	}
 
         DBG("FW version:%d.%d.%d\n",
-		(u16) (priv->fw_ver >> 32),
+	    (u16) (priv->fw_ver >> 32),
 		(u16) ((priv->fw_ver >> 16) & 0xffff),
 		(u16) (priv->fw_ver & 0xffff));
 
@@ -1098,22 +1101,22 @@ int mtnic_init_card(struct net_device *dev)
 	/* Get device information */
 	err = mtnic_query_cap(priv);
 	if (err) {
-		eprintf("Insufficient resources, aborting.\n");
+		DBG("Insufficient resources, aborting.\n");
 		goto map_fw_error;
 	}
 
 	/* Open NIC */
 	err = mtnic_OPEN_NIC(priv);
 	if (err) {
-		eprintf("Failed opening NIC, aborting.\n");
+		DBG("Failed opening NIC, aborting.\n");
 		goto map_fw_error;
 	}
 
 	/* Allocate and map pages worksace */
 	err = mtnic_map_cmd(priv, MTNIC_IF_CMD_MAP_PAGES, priv->fw.extra_pages);
 	if (err) {
-		eprintf("Couldn't allocate %lx FW extra pages, aborting.\n",
-			priv->fw.extra_pages.num);
+		DBG("Couldn't allocate %lx FW extra pages, aborting.\n",
+		    priv->fw.extra_pages.num);
 		if (priv->fw.extra_pages.buf)
 			free(priv->fw.extra_pages.buf);
 		goto map_fw_error;
@@ -1122,7 +1125,7 @@ int mtnic_init_card(struct net_device *dev)
 	/* Get device offsets */
 	err = mtnic_query_offsets(priv);
 	if (err) {
-		eprintf("Failed retrieving resource offests, aborting.\n");
+		DBG("Failed retrieving resource offests, aborting.\n");
 		free(priv->fw.extra_pages.buf);
 		goto map_extra_error;
 	}
@@ -1131,24 +1134,24 @@ int mtnic_init_card(struct net_device *dev)
         /* Alloc EQ */
 	err = mtnic_alloc_eq(priv);
 	if (err) {
-		eprintf("Failed init shared resources. error: %d\n", err);
+		DBG("Failed init shared resources. error: %d\n", err);
 		goto map_extra_error;
         }
 
 	/* Configure HW */
 	err = mtnic_CONFIG_EQ(priv);
 	if (err) {
-		eprintf("Failed configuring EQ\n");
+		DBG("Failed configuring EQ\n");
 		goto eq_error;
 	}
 	err = mtnic_CONFIG_RX(priv);
 	if (err) {
-		eprintf("Failed Rx configuration\n");
+		DBG("Failed Rx configuration\n");
 		goto eq_error;
 	}
 	err = mtnic_CONFIG_TX(priv);
 	if (err) {
-		eprintf("Failed Tx configuration\n");
+		DBG("Failed Tx configuration\n");
 		goto eq_error;
 	}
 
@@ -1270,7 +1273,7 @@ next:
 	if (ring->prod - ring->cons < (MAX_GAP_PROD_CONS)) {
 		err = mtnic_alloc_iobuf(priv, &priv->rx_ring, DEF_IOBUF_SIZE);
 		if (err) {
-			eprintf("ERROR Allocating io buffer");
+			DBG("ERROR Allocating io buffer");
 			return MTNIC_ERROR;
 		}
 	}
@@ -1316,7 +1319,7 @@ mtnic_open(struct net_device *dev)
 	/* Alloc and configure CQs, TX, RX */
 	err = mtnic_alloc_resources(dev);
 	if (err) {
-		eprintf("Error allocating resources\n");
+		DBG("Error allocating resources\n");
 		return MTNIC_ERROR;
 	}
 
@@ -1325,8 +1328,8 @@ mtnic_open(struct net_device *dev)
 		cq = &priv->cq[cq_ind];
 		err = mtnic_CONFIG_CQ(priv, priv->port, cq_ind, cq);
 		if (err) {
-			eprintf("Failed configuring CQ:%d error %d\n",
-				cq_ind, err);
+			DBG("Failed configuring CQ:%d error %d\n",
+			    cq_ind, err);
 			if (cq_ind)
 				goto cq_error;
 			else
@@ -1341,7 +1344,7 @@ mtnic_open(struct net_device *dev)
 	ring = &priv->tx_ring;
         err = mtnic_CONFIG_TX_RING(priv, priv->port, 0, ring);
 	if (err) {
-		eprintf("Failed configuring Tx ring:0\n");
+		DBG("Failed configuring Tx ring:0\n");
                 goto cq_error;
 	}
 
@@ -1349,7 +1352,7 @@ mtnic_open(struct net_device *dev)
         ring = &priv->rx_ring;
         err = mtnic_CONFIG_RX_RING(priv, priv->port, 0, ring);
 	if (err) {
-		eprintf("Failed configuring Rx ring:0\n");
+		DBG("Failed configuring Rx ring:0\n");
 		goto tx_error;
 	}
 
@@ -1358,42 +1361,42 @@ mtnic_open(struct net_device *dev)
 	if (!err)
 		err = mtnic_SET_PORT_RSS_INDIRECTION(priv, priv->port);
 	if (err) {
-		eprintf("Failed configuring RSS steering\n");
+		DBG("Failed configuring RSS steering\n");
 		goto rx_error;
 	}
 
 	/* Set the port default ring to ring 0 */
 	err = mtnic_SET_PORT_DEFAULT_RING(priv, priv->port, 0);
 	if (err) {
-		eprintf("Failed setting default ring\n");
+		DBG("Failed setting default ring\n");
 		goto rx_error;
 	}
 
 	/* Set Mac address */
 	err = mtnic_SET_RX_RING_ADDR(priv, priv->port, &priv->fw.mac[priv->port]);
 	if (err) {
-		eprintf("Failed setting default MAC address\n");
+		DBG("Failed setting default MAC address\n");
 		goto rx_error;
 	}
 
 	/* Set MTU  */
 	err = mtnic_SET_PORT_MTU(priv, priv->port, DEF_MTU);
 	if (err) {
-		eprintf("Failed setting MTU\n");
+		DBG("Failed setting MTU\n");
 		goto rx_error;
 	}
 
 	/* Configure VLAN filter */
 	err = mtnic_CONFIG_PORT_VLAN_FILTER(priv, priv->port);
         if (err) {
-		eprintf("Failed configuring VLAN filter\n");
+		DBG("Failed configuring VLAN filter\n");
 		goto rx_error;
 	}
 
 	/* Bring up physical link */
 	err = mtnic_SET_PORT_STATE(priv, priv->port, 1);
 	if (err) {
-		eprintf("Failed bringing up port\n");
+		DBG("Failed bringing up port\n");
 		goto rx_error;
 	}
 	mdelay(300); /* Let link state stabilize if cable was connected */
@@ -1402,11 +1405,11 @@ mtnic_open(struct net_device *dev)
 
 	err = mtnic_HEART_BEAT(priv, &dev_link_state);
 	if (err) {
-		eprintf("Failed getting device link state\n");
+		DBG("Failed getting device link state\n");
 		return MTNIC_ERROR;
 	}
 	if (!(dev_link_state & 0x3)) {
-		eprintf("Link down, check cables and restart\n");
+		DBG("Link down, check cables and restart\n");
 		return MTNIC_ERROR;
 	}
 
@@ -1453,12 +1456,12 @@ mtnic_poll(struct net_device *dev)
 		/* Check device */
 		err = mtnic_HEART_BEAT(priv, &dev_link_state);
 		if (err) {
-			eprintf("Device has internal error\n");
+			DBG("Device has internal error\n");
 			priv->state = CARD_DOWN;
 			return;
 		}
 		if (!(dev_link_state & 0x3)) {
-			eprintf("Link down, check cables and restart\n");
+			DBG("Link down, check cables and restart\n");
 			priv->state = CARD_DOWN;
 			return;
 		}
@@ -1472,7 +1475,7 @@ mtnic_poll(struct net_device *dev)
                         err = mtnic_process_rx_cq(priv, cq->dev, cq);
 			if (err) {
 				priv->state = CARD_DOWN;
-				eprintf(" Error allocating RX buffers\n");
+				DBG(" Error allocating RX buffers\n");
 				return;
 			}
                 } else {
@@ -1664,17 +1667,11 @@ mtnic_probe(struct pci_device *pci,
 	void *dev_id;
 	int i;
 
-	if (pci->vendor != MELLANOX_VENDOR_ID) {
-		eprintf("");
-		return 0;
-	}
-	printf("\nMellanox Technologies LTD - Boot over MTNIC implementaion\n");
-
         adjust_pci_device(pci);
 
         err = mtnic_init_pci(pci);
 	if (err) {
-		eprintf("Error in pci_init\n");
+		DBG("Error in pci_init\n");
 		return MTNIC_ERROR;
 	}
 
@@ -1683,7 +1680,7 @@ mtnic_probe(struct pci_device *pci,
 
         err = restore_config();
 	if (err) {
-		eprintf("");
+		DBG("Error restoring config\n");
 		return err;
 	}
 
@@ -1693,14 +1690,14 @@ mtnic_probe(struct pci_device *pci,
 	result = ntohl(readl(dev_id));
 	iounmap(dev_id);
         if (result != MTNIC_DEVICE_ID) {
-		eprintf("Wrong Devie ID (0x%lx) !!!", result);
+		DBG("Wrong Devie ID (0x%lx) !!!", result);
 		return MTNIC_ERROR;
 	}
 
 	/* Initializing net device */
         dev = alloc_etherdev(sizeof(struct mtnic_priv));
 	if (dev == NULL) {
-		eprintf("Net device allocation failed\n");
+		DBG("Net device allocation failed\n");
 		return MTNIC_ERROR;
 	}
 	/*
@@ -1719,7 +1716,7 @@ mtnic_probe(struct pci_device *pci,
 	/* Initialize hardware */
 	err = mtnic_init_card(dev);
 	if (err) {
-		eprintf("Error in init_card\n");
+		DBG("Error in init_card\n");
 		return MTNIC_ERROR;
 	}
 
@@ -1731,8 +1728,11 @@ mtnic_probe(struct pci_device *pci,
                 mac = mac >> 8;
 	}
 
+	/* Mark as link up; we don't yet handle link state */
+	netdev_link_up ( dev );
+
 	if (register_netdev(dev)) {
-		eprintf("Netdev registration failed\n");
+		DBG("Netdev registration failed\n");
 		return MTNIC_ERROR;
 	}
 
diff --git a/gpxe/src/drivers/net/mtnic.h b/gpxe/src/drivers/net/mtnic.h
index f1d481c..70a238e 100755
--- a/gpxe/src/drivers/net/mtnic.h
+++ b/gpxe/src/drivers/net/mtnic.h
@@ -57,19 +57,11 @@
 #define ROUND_TO_CHECK		0x400
 
 
-/*
-* Helper macros
-*/
-/* Print in case of an error */
-#define eprintf(fmt, a...) \
-		 printf("%s:%d: " fmt "\n", __func__, __LINE__,  ##a)
-
 #define XNOR(x,y)		(!(x) == !(y))
 #define dma_addr_t 		unsigned long
 #define PAGE_SIZE		4096
 #define PAGE_MASK		(PAGE_SIZE - 1)
 #define MTNIC_MAILBOX_SIZE	PAGE_SIZE
-#define MTNIC_ERROR 1
 
 
 
diff --git a/gpxe/src/drivers/net/natsemi.c b/gpxe/src/drivers/net/natsemi.c
index 98a5ff8..028b905 100644
--- a/gpxe/src/drivers/net/natsemi.c
+++ b/gpxe/src/drivers/net/natsemi.c
@@ -205,6 +205,9 @@ static int natsemi_probe (struct pci_device *pci,
 		last = last1;
 	}
 
+	/* Mark as link up; we don't yet handle link state */
+	netdev_link_up ( netdev );
+
 	if ((rc = register_netdev (netdev)) != 0)
 		goto err_register_netdev;
 
diff --git a/gpxe/src/drivers/net/pnic.c b/gpxe/src/drivers/net/pnic.c
index b431ec5..c7f0867 100644
--- a/gpxe/src/drivers/net/pnic.c
+++ b/gpxe/src/drivers/net/pnic.c
@@ -250,6 +250,9 @@ static int pnic_probe ( struct pci_device *pci,
 	status = pnic_command ( pnic, PNIC_CMD_READ_MAC, NULL, 0,
 				netdev->ll_addr, ETH_ALEN, NULL );
 
+	/* Mark as link up; PNIC has no concept of link state */
+	netdev_link_up ( netdev );
+
 	/* Register network device */
 	if ( ( rc = register_netdev ( netdev ) ) != 0 )
 		goto err;
diff --git a/gpxe/src/drivers/net/r8169.c b/gpxe/src/drivers/net/r8169.c
index ab9a30f..885f054 100644
--- a/gpxe/src/drivers/net/r8169.c
+++ b/gpxe/src/drivers/net/r8169.c
@@ -40,6 +40,9 @@
 *    v1.5	01-17-2004	timlegge	Initial driver output cleanup
 *    v1.6	03-27-2004	timlegge	Additional Cleanup
 *    v1.7	11-22-2005	timlegge	Update to RealTek Driver Version 2.2
+*
+*		03-19-2008	Hilko Bengen	Cleanups and fixes for newer cards
+*				(successfully tested with 8110SC-d onboard NIC)
 *    
 *    Indent Options: indent -kr -i8
 ***************************************************************************/
@@ -50,8 +53,8 @@
 #include <gpxe/ethernet.h>
 #include <gpxe/malloc.h>
 
-#define drv_version "v1.6"
-#define drv_date "03-27-2004"
+#define drv_version "v1.7+"
+#define drv_date "03-19-2008"
 
 #define HZ 1000
 
@@ -72,16 +75,6 @@ static u32 ioaddr;
 #undef RTL8169_DYNAMIC_CONTROL
 #define RTL8169_USE_IO
 
-
-/* media options 
-        _10_Half = 0x01,
-        _10_Full = 0x02,
-        _100_Half = 0x04,
-        _100_Full = 0x08,
-        _1000_Full = 0x10,
-*/
-static int media = -1;
-
 #if 0
 /* Maximum events (Rx packets, etc.) to handle at each interrupt. */
 static int max_interrupt_work = 20;
@@ -148,29 +141,59 @@ static int multicast_filter_limit = 32;
 #define RTL_R32(reg)		((unsigned long) readl (ioaddr + (reg)))
 #endif
 
-#define MCFG_METHOD_1		0x01
-#define MCFG_METHOD_2		0x02
-#define MCFG_METHOD_3		0x03
-#define MCFG_METHOD_4		0x04
+enum mac_version {
+	RTL_GIGA_MAC_VER_01 = 0x01, // 8169
+	RTL_GIGA_MAC_VER_02 = 0x02, // 8169S
+	RTL_GIGA_MAC_VER_03 = 0x03, // 8110S
+	RTL_GIGA_MAC_VER_04 = 0x04, // 8169SB
+	RTL_GIGA_MAC_VER_05 = 0x05, // 8110SCd
+	RTL_GIGA_MAC_VER_06 = 0x06, // 8110SCe
+	RTL_GIGA_MAC_VER_11 = 0x0b, // 8168Bb
+	RTL_GIGA_MAC_VER_12 = 0x0c, // 8168Be
+	RTL_GIGA_MAC_VER_13 = 0x0d, // 8101Eb
+	RTL_GIGA_MAC_VER_14 = 0x0e, // 8101 ?
+	RTL_GIGA_MAC_VER_15 = 0x0f, // 8101 ?
+	RTL_GIGA_MAC_VER_16 = 0x11, // 8101Ec
+	RTL_GIGA_MAC_VER_17 = 0x10, // 8168Bf
+	RTL_GIGA_MAC_VER_18 = 0x12, // 8168CP
+	RTL_GIGA_MAC_VER_19 = 0x13, // 8168C
+	RTL_GIGA_MAC_VER_20 = 0x14  // 8168C
+};
 
-#define PCFG_METHOD_1		0x01	//PHY Reg 0x03 bit0-3 == 0x0000
-#define PCFG_METHOD_2		0x02	//PHY Reg 0x03 bit0-3 == 0x0001
-#define PCFG_METHOD_3		0x03	//PHY Reg 0x03 bit0-3 == 0x0002
+enum cfg_version {
+	RTL_CFG_0 = 0x00,
+	RTL_CFG_1,
+	RTL_CFG_2
+};
 
 static struct {
 	const char *name;
-	u8 mcfg;		/* depend on RTL8169 docs */
+	u8 mac_version;		/* depend on RTL8169 docs */
 	u32 RxConfigMask;	/* should clear the bits supported by this chip */
 } rtl_chip_info[] = {
-	{
-	"RTL-8169", MCFG_METHOD_1, 0xff7e1880,}, {
-	"RTL8169s/8110s", MCFG_METHOD_2, 0xff7e1880}, {
-"RTL8169s/8110s", MCFG_METHOD_3, 0xff7e1880},};
+        {"RTL8169",           RTL_GIGA_MAC_VER_01, 0xff7e1880}, // 8169
+        {"RTL8169s",          RTL_GIGA_MAC_VER_02, 0xff7e1880}, // 8169S
+        {"RTL8110s",          RTL_GIGA_MAC_VER_03, 0xff7e1880}, // 8110S
+        {"RTL8169sb/8110sb",  RTL_GIGA_MAC_VER_04, 0xff7e1880}, // 8169SB
+        {"RTL8169sc/8110sc-d",RTL_GIGA_MAC_VER_05, 0xff7e1880}, // 8110SCd
+        {"RTL8169sc/8110sc-e",RTL_GIGA_MAC_VER_06, 0xff7e1880}, // 8110SCe
+        {"RTL8168b/8111b",    RTL_GIGA_MAC_VER_11, 0xff7e1880}, // PCI-E
+        {"RTL8168b/8111b",    RTL_GIGA_MAC_VER_12, 0xff7e1880}, // PCI-E
+        {"RTL8101e",          RTL_GIGA_MAC_VER_13, 0xff7e1880}, // PCI-E 8139
+        {"RTL8100e",          RTL_GIGA_MAC_VER_14, 0xff7e1880}, // PCI-E 8139
+        {"RTL8100e",          RTL_GIGA_MAC_VER_15, 0xff7e1880}, // PCI-E 8139
+        {"RTL8168b/8111b",    RTL_GIGA_MAC_VER_17, 0xff7e1880}, // PCI-E
+        {"RTL8101e",          RTL_GIGA_MAC_VER_16, 0xff7e1880}, // PCI-E
+        {"RTL8168cp/8111cp",  RTL_GIGA_MAC_VER_18, 0xff7e1880}, // PCI-E
+        {"RTL8168c/8111c",    RTL_GIGA_MAC_VER_19, 0xff7e1880}, // PCI-E
+        {"RTL8168c/8111c",    RTL_GIGA_MAC_VER_20, 0xff7e1880}, // PCI-E
+};
 
 enum RTL8169_registers {
 	MAC0 = 0x0,		/* Ethernet hardware address. */
 	MAR0 = 0x8,		/* Multicast filter. */
-	TxDescStartAddr = 0x20,
+	TxDescAddrLow = 0x20,
+	TxDescAddrHigh	= 0x24,
 	TxHDescStartAddr = 0x28,
 	FLASH = 0x30,
 	ERSR = 0x36,
@@ -194,9 +217,11 @@ enum RTL8169_registers {
 	TBI_ANAR = 0x68,
 	TBI_LPAR = 0x6A,
 	PHYstatus = 0x6C,
-	RxMaxSize = 0xDA,
-	CPlusCmd = 0xE0,
-	RxDescStartAddr = 0xE4,
+	RxMaxSize = 0xda,
+	CPlusCmd = 0xe0,
+        IntrMitigate = 0xe2,
+	RxDescAddrLow = 0xe4,
+	RxDescAddrHigh	= 0xe8,
 	ETThReg = 0xEC,
 	FuncEvent = 0xF0,
 	FuncEventMask = 0xF4,
@@ -345,7 +370,7 @@ static struct rtl8169_private {
 	void *mmio_addr;	/* memory map physical address */
 	int chipset;
 	int pcfg;
-	int mcfg;
+	int mac_version;
 	unsigned long cur_rx;	/* Index into the Rx descriptor buffer of next Rx pkt. */
 	unsigned long cur_tx;	/* Index into the Tx descriptor buffer of next Rx pkt. */
 	struct TxDesc *TxDescArray;	/* Index of 256-alignment Tx Descriptor buffer */
@@ -354,16 +379,14 @@ static struct rtl8169_private {
 	unsigned char *Tx_skbuff[NUM_TX_DESC];
 } tpx;
 
-static struct rtl8169_private *tpc;
-
 static const u16 rtl8169_intr_mask =
     LinkChg | RxOverflow | RxFIFOOver | TxErr | TxOK | RxErr | RxOK;
 static const unsigned int rtl8169_rx_config =
     (RX_FIFO_THRESH << RxCfgFIFOShift) | (RX_DMA_BURST << RxCfgDMAShift) |
     0x0000000E;
 
-static void rtl8169_hw_PHY_config(struct nic *nic __unused);
-//static void rtl8169_hw_PHY_reset(struct net_device *dev);
+static void rtl8169_hw_phy_config(struct nic *nic __unused);
+//static void rtl8169_hw_phy_reset(struct net_device *dev);
 
 #define RTL8169_WRITE_GMII_REG_BIT( ioaddr, reg, bitnum, bitval )\
 { \
@@ -457,6 +480,65 @@ static int mdio_read(int RegAddr)
 }
 #endif
 
+static void rtl8169_get_mac_version( struct rtl8169_private *tp,
+                                     u32 ioaddr )
+{
+	/*
+	 * The driver currently handles the 8168Bf and the 8168Be identically
+	 * but they can be identified more specifically through the test below
+	 * if needed:
+	 *
+	 * (RTL_R32(TxConfig) & 0x700000) == 0x500000 ? 8168Bf : 8168Be
+	 *
+	 * Same thing for the 8101Eb and the 8101Ec:
+	 *
+	 * (RTL_R32(TxConfig) & 0x700000) == 0x200000 ? 8101Eb : 8101Ec
+	 */
+	const struct {
+		u32 mask;
+		u32 val;
+		int mac_version;
+	} mac_info[] = {
+		/* 8168B family. */
+		{ 0x7c800000, 0x3c800000,	RTL_GIGA_MAC_VER_18 },
+		{ 0x7cf00000, 0x3c000000,	RTL_GIGA_MAC_VER_19 },
+		{ 0x7cf00000, 0x3c200000,	RTL_GIGA_MAC_VER_20 },
+		{ 0x7c800000, 0x3c000000,	RTL_GIGA_MAC_VER_20 },
+		/* 8168B family. */
+		{ 0x7cf00000, 0x38000000,	RTL_GIGA_MAC_VER_12 },
+		{ 0x7cf00000, 0x38500000,	RTL_GIGA_MAC_VER_17 },
+		{ 0x7c800000, 0x38000000,	RTL_GIGA_MAC_VER_17 },
+		{ 0x7c800000, 0x30000000,	RTL_GIGA_MAC_VER_11 },
+		/* 8101 family. */
+		{ 0x7cf00000, 0x34000000,	RTL_GIGA_MAC_VER_13 },
+		{ 0x7cf00000, 0x34200000,	RTL_GIGA_MAC_VER_16 },
+		{ 0x7c800000, 0x34000000,	RTL_GIGA_MAC_VER_16 },
+		/* FIXME: where did these entries come from ? -- FR */
+		{ 0xfc800000, 0x38800000,	RTL_GIGA_MAC_VER_15 },
+		{ 0xfc800000, 0x30800000,	RTL_GIGA_MAC_VER_14 },
+		/* 8110 family. */
+		{ 0xfc800000, 0x98000000,	RTL_GIGA_MAC_VER_06 },
+		{ 0xfc800000, 0x18000000,	RTL_GIGA_MAC_VER_05 },
+		{ 0xfc800000, 0x10000000,	RTL_GIGA_MAC_VER_04 },
+		{ 0xfc800000, 0x04000000,	RTL_GIGA_MAC_VER_03 },
+		{ 0xfc800000, 0x00800000,	RTL_GIGA_MAC_VER_02 },
+		{ 0xfc800000, 0x00000000,	RTL_GIGA_MAC_VER_01 },
+		{ 0x00000000, 0x00000000,	RTL_GIGA_MAC_VER_01 }	/* Catch-all */
+	}, *p = mac_info;
+
+	unsigned long rv;
+
+        rv = (RTL_R32(TxConfig));
+
+	while ((rv & p->mask) != p->val)
+		p++;
+	tp->mac_version = p->mac_version;
+
+	if (p->mask == 0x00000000) {
+                DBG("unknown MAC (%08lx)\n", rv);
+	}
+}
+
 #define IORESOURCE_MEM 0x00000200
 
 static int rtl8169_init_board(struct pci_device *pdev)
@@ -464,6 +546,7 @@ static int rtl8169_init_board(struct pci_device *pdev)
 	int i;
 //	unsigned long mmio_end, mmio_flags
         unsigned long mmio_start, mmio_len;
+	struct rtl8169_private *tp = &tpx;
 
 	adjust_pci_device(pdev);
 
@@ -494,7 +577,7 @@ static int rtl8169_init_board(struct pci_device *pdev)
 	}
 #endif
 
-	tpc->mmio_addr = &ioaddr;
+	tp->mmio_addr = (void*)ioaddr;
 	/* Soft reset the chip. */
 	RTL_W8(ChipCmd, CmdReset);
 
@@ -504,48 +587,39 @@ static int rtl8169_init_board(struct pci_device *pdev)
 			break;
 		else
 			udelay(10);
-	// identify config method
-	{
-		unsigned long val32 = (RTL_R32(TxConfig) & 0x7c800000);
-		if (val32 == (0x1 << 28)) {
-			tpc->mcfg = MCFG_METHOD_4;
-		} else if (val32 == (0x1 << 26)) {
-			tpc->mcfg = MCFG_METHOD_3;
-		} else if (val32 == (0x1 << 23)) {
-			tpc->mcfg = MCFG_METHOD_2;
-		} else if (val32 == 0x00000000) {
-			tpc->mcfg = MCFG_METHOD_1;
-		} else {
-			tpc->mcfg = MCFG_METHOD_1;
-		}
-	}
+
+	/* Identify chip attached to board */
+        rtl8169_get_mac_version( tp, ioaddr );
+
+	// rtl8169_print_mac_version(tp);
+
 	{
 		unsigned char val8 =
 		    (unsigned char) (RTL8169_READ_GMII_REG(ioaddr, 3) &
 				     0x000f);
 		if (val8 == 0x00) {
-			tpc->pcfg = PCFG_METHOD_1;
+			tp->pcfg = RTL_CFG_0;
 		} else if (val8 == 0x01) {
-			tpc->pcfg = PCFG_METHOD_2;
+			tp->pcfg = RTL_CFG_1;
 		} else if (val8 == 0x02) {
-			tpc->pcfg = PCFG_METHOD_3;
+			tp->pcfg = RTL_CFG_2;
 		} else {
-			tpc->pcfg = PCFG_METHOD_3;
+			tp->pcfg = RTL_CFG_2;
 		}
 	}
 
 	/* identify chip attached to board */
 
 	for (i = ARRAY_SIZE(rtl_chip_info) - 1; i >= 0; i--)
-		if (tpc->mcfg == rtl_chip_info[i].mcfg) {
-			tpc->chipset = i;
+		if (tp->mac_version == rtl_chip_info[i].mac_version) {
+			tp->chipset = i;
 			goto match;
 		}
 	/* if unknown chip, assume array element #0, original RTL-8169 in this case */
 	DBG ( "PCI device: unknown chip version, assuming RTL-8169\n" );
 	DBG ( "PCI device: TxConfig = %#lX\n", ( unsigned long ) RTL_R32 ( TxConfig ) );
 
-	tpc->chipset = 0;
+	tp->chipset = 0;
 	return 1;
 
       match:
@@ -584,51 +658,57 @@ static void r8169_irq(struct nic *nic __unused, irq_action_t action)
 /**************************************************************************
 POLL - Wait for a frame
 ***************************************************************************/
-static int r8169_poll(struct nic *nic, int retreive)
+static int r8169_poll(struct nic *nic, int retrieve)
 {
 	/* return true if there's an ethernet packet ready to read */
 	/* nic->packet should contain data on return */
 	/* nic->packetlen should contain length of data */
 	int cur_rx;
 	unsigned int intr_status = 0;
-	cur_rx = tpc->cur_rx;
-	if ((tpc->RxDescArray[cur_rx].status & OWNbit) == 0) {
+	struct rtl8169_private *tp = &tpx;
+
+	cur_rx = tp->cur_rx;
+	if ((tp->RxDescArray[cur_rx].status & OWNbit) == 0) {
 		/* There is a packet ready */
-		if (!retreive)
+                DBG("r8169_poll(): packet ready\n");
+		if (!retrieve)
 			return 1;
 		intr_status = RTL_R16(IntrStatus);
 		/* h/w no longer present (hotplug?) or major error,
 		   bail */
-		if (intr_status == 0xFFFF)
+		if (intr_status == 0xFFFF) {
+                        DBG("r8169_poll(): unknown error\n");
 			return 0;
+                }
 		RTL_W16(IntrStatus, intr_status &
 			~(RxFIFOOver | RxOverflow | RxOK));
 
-		if (!(tpc->RxDescArray[cur_rx].status & RxRES)) {
-			nic->packetlen = (int) (tpc->RxDescArray[cur_rx].
+		if (!(tp->RxDescArray[cur_rx].status & RxRES)) {
+			nic->packetlen = (int) (tp->RxDescArray[cur_rx].
 						status & 0x00001FFF) - 4;
-			memcpy(nic->packet, tpc->RxBufferRing[cur_rx],
+			memcpy(nic->packet, tp->RxBufferRing[cur_rx],
 			       nic->packetlen);
 			if (cur_rx == NUM_RX_DESC - 1)
-				tpc->RxDescArray[cur_rx].status =
+				tp->RxDescArray[cur_rx].status =
 				    (OWNbit | EORbit) + RX_BUF_SIZE;
 			else
-				tpc->RxDescArray[cur_rx].status =
+				tp->RxDescArray[cur_rx].status =
 				    OWNbit + RX_BUF_SIZE;
-			tpc->RxDescArray[cur_rx].buf_addr =
-			    virt_to_bus(tpc->RxBufferRing[cur_rx]);
+			tp->RxDescArray[cur_rx].buf_addr =
+			    virt_to_bus(tp->RxBufferRing[cur_rx]);
+			tp->RxDescArray[cur_rx].buf_Haddr = 0;
 		} else
 			printf("Error Rx");
 		/* FIXME: shouldn't I reset the status on an error */
 		cur_rx = (cur_rx + 1) % NUM_RX_DESC;
-		tpc->cur_rx = cur_rx;
+		tp->cur_rx = cur_rx;
 		RTL_W16(IntrStatus, intr_status &
 			(RxFIFOOver | RxOverflow | RxOK));
 
 		return 1;
 
 	}
-	tpc->cur_rx = cur_rx;
+	tp->cur_rx = cur_rx;
 	/* FIXME: There is no reason to do this as cur_rx did not change */
 
 	return (0);		/* initially as this is called to flush the input */
@@ -648,10 +728,11 @@ static void r8169_transmit(struct nic *nic, const char *d,	/* Destination */
 	u16 nstype;
 	u32 to;
 	u8 *ptxb;
-	int entry = tpc->cur_tx % NUM_TX_DESC;
+	struct rtl8169_private *tp = &tpx;
+	int entry = tp->cur_tx % NUM_TX_DESC;
 
 	/* point to the current txb incase multiple tx_rings are used */
-	ptxb = tpc->Tx_skbuff[entry * MAX_ETH_FRAME_SIZE];
+	ptxb = tp->Tx_skbuff[entry * MAX_ETH_FRAME_SIZE];
 	memcpy(ptxb, d, ETH_ALEN);
 	memcpy(ptxb + ETH_ALEN, nic->node_addr, ETH_ALEN);
 	nstype = htons((u16) t);
@@ -662,20 +743,21 @@ static void r8169_transmit(struct nic *nic, const char *d,	/* Destination */
 	while (s < ETH_ZLEN)
 		ptxb[s++] = '\0';
 
-	tpc->TxDescArray[entry].buf_addr = virt_to_bus(ptxb);
+	tp->TxDescArray[entry].buf_addr = virt_to_bus(ptxb);
+	tp->TxDescArray[entry].buf_Haddr = 0;
 	if (entry != (NUM_TX_DESC - 1))
-		tpc->TxDescArray[entry].status =
+		tp->TxDescArray[entry].status =
 		    (OWNbit | FSbit | LSbit) | ((s > ETH_ZLEN) ? s :
 						ETH_ZLEN);
 	else
-		tpc->TxDescArray[entry].status =
+		tp->TxDescArray[entry].status =
 		    (OWNbit | EORbit | FSbit | LSbit) | ((s > ETH_ZLEN) ? s
 							 : ETH_ZLEN);
 	RTL_W8(TxPoll, 0x40);	/* set polling bit */
 
-	tpc->cur_tx++;
+	tp->cur_tx++;
 	to = currticks() + TX_TIMEOUT;
-	while ((tpc->TxDescArray[entry].status & OWNbit) && (currticks() < to));	/* wait */
+	while ((tp->TxDescArray[entry].status & OWNbit) && (currticks() < to));	/* wait */
 
 	if (currticks() >= to) {
 		printf("TX Time Out");
@@ -687,6 +769,7 @@ static void rtl8169_set_rx_mode(struct nic *nic __unused)
 	u32 mc_filter[2];	/* Multicast hash filter */
 	int rx_mode;
 	u32 tmp = 0;
+	struct rtl8169_private *tp = &tpx;
 
 	/* IFF_ALLMULTI */
 	/* Too many to filter perfectly -- accept all multicasts. */
@@ -695,7 +778,7 @@ static void rtl8169_set_rx_mode(struct nic *nic __unused)
 
 	tmp =
 	    rtl8169_rx_config | rx_mode | (RTL_R32(RxConfig) &
-					   rtl_chip_info[tpc->chipset].
+					   rtl_chip_info[tp->chipset].
 					   RxConfigMask);
 
 	RTL_W32(RxConfig, tmp);
@@ -705,6 +788,7 @@ static void rtl8169_set_rx_mode(struct nic *nic __unused)
 static void rtl8169_hw_start(struct nic *nic)
 {
 	u32 i;
+	struct rtl8169_private *tp = &tpx;
 
 	/* Soft reset the chip. */
 	RTL_W8(ChipCmd, CmdReset);
@@ -726,7 +810,7 @@ static void rtl8169_hw_start(struct nic *nic)
 
 	/* Set Rx Config register */
 	i = rtl8169_rx_config | (RTL_R32(RxConfig) &
-				 rtl_chip_info[tpc->chipset].RxConfigMask);
+				 rtl_chip_info[tp->chipset].RxConfigMask);
 	RTL_W32(RxConfig, i);
 
 	/* Set DMA burst size and Interframe Gap Time */
@@ -737,7 +821,7 @@ static void rtl8169_hw_start(struct nic *nic)
 
 	RTL_W16(CPlusCmd, RTL_R16(CPlusCmd));
 
-	if (tpc->mcfg == MCFG_METHOD_2 || tpc->mcfg == MCFG_METHOD_3) {
+	if (tp->mac_version == RTL_GIGA_MAC_VER_02 || tp->mac_version == RTL_GIGA_MAC_VER_03) {
 		RTL_W16(CPlusCmd,
 			(RTL_R16(CPlusCmd) | (1 << 14) | (1 << 3)));
 		DBG
@@ -748,18 +832,18 @@ static void rtl8169_hw_start(struct nic *nic)
 	}
 
 	{
-		//RTL_W16(0xE2, 0x1517);
-		//RTL_W16(0xE2, 0x152a);
-		//RTL_W16(0xE2, 0x282a);
-		RTL_W16(0xE2, 0x0000);
+		//RTL_W16(IntrMitigate, 0x1517);
+		//RTL_W16(IntrMitigate, 0x152a);
+		//RTL_W16(IntrMitigate, 0x282a);
+		RTL_W16(IntrMitigate, 0x0000);
 	}
 
+	tp->cur_rx = 0;
 
-
-	tpc->cur_rx = 0;
-
-	RTL_W32(TxDescStartAddr, virt_to_le32desc(tpc->TxDescArray));
-	RTL_W32(RxDescStartAddr, virt_to_le32desc(tpc->RxDescArray));
+	RTL_W32(TxDescAddrLow, virt_to_le32desc(tp->TxDescArray));
+	RTL_W32(TxDescAddrHigh, virt_to_le32desc(NULL));
+	RTL_W32(RxDescAddrLow, virt_to_le32desc(tp->RxDescArray));
+	RTL_W32(RxDescAddrHigh, virt_to_le32desc(NULL));
 	RTL_W8(Cfg9346, Cfg9346_Lock);
 	udelay(10);
 
@@ -777,26 +861,28 @@ static void rtl8169_hw_start(struct nic *nic)
 static void rtl8169_init_ring(struct nic *nic __unused)
 {
 	int i;
+	struct rtl8169_private *tp = &tpx;
 
-	tpc->cur_rx = 0;
-	tpc->cur_tx = 0;
-	memset(tpc->TxDescArray, 0x0, NUM_TX_DESC * sizeof(struct TxDesc));
-	memset(tpc->RxDescArray, 0x0, NUM_RX_DESC * sizeof(struct RxDesc));
+	tp->cur_rx = 0;
+	tp->cur_tx = 0;
+	memset(tp->TxDescArray, 0x0, NUM_TX_DESC * sizeof(struct TxDesc));
+	memset(tp->RxDescArray, 0x0, NUM_RX_DESC * sizeof(struct RxDesc));
 
 	for (i = 0; i < NUM_TX_DESC; i++) {
-		tpc->Tx_skbuff[i] = &txb[i];
+		tp->Tx_skbuff[i] = &txb[i];
 	}
 
 	for (i = 0; i < NUM_RX_DESC; i++) {
 		if (i == (NUM_RX_DESC - 1))
-			tpc->RxDescArray[i].status =
+			tp->RxDescArray[i].status =
 			    (OWNbit | EORbit) | RX_BUF_SIZE;
 		else
-			tpc->RxDescArray[i].status = OWNbit | RX_BUF_SIZE;
+			tp->RxDescArray[i].status = OWNbit | RX_BUF_SIZE;
 
-		tpc->RxBufferRing[i] = &rxb[i * RX_BUF_SIZE];
-		tpc->RxDescArray[i].buf_addr =
-		    virt_to_bus(tpc->RxBufferRing[i]);
+		tp->RxBufferRing[i] = &rxb[i * RX_BUF_SIZE];
+		tp->RxDescArray[i].buf_addr =
+		    virt_to_bus(tp->RxBufferRing[i]);
+		tp->RxDescArray[i].buf_Haddr = 0;
 	}
 }
 
@@ -806,9 +892,10 @@ RESET - Finish setting up the ethernet interface
 static void r8169_reset(struct nic *nic)
 {
 	int i;
+	struct rtl8169_private *tp = &tpx;
 
-	tpc->TxDescArray = tx_ring;
-	tpc->RxDescArray = rx_ring;
+	tp->TxDescArray = tx_ring;
+	tp->RxDescArray = rx_ring;
 
 	rtl8169_init_ring(nic);
 	rtl8169_hw_start(nic);
@@ -830,6 +917,8 @@ DISABLE - Turn off ethernet interface
 ***************************************************************************/
 static void r8169_disable ( struct nic *nic __unused ) {
 	int i;
+	struct rtl8169_private *tp = &tpx;
+
 	/* Stop the chip's Tx and Rx DMA processes. */
 	RTL_W8(ChipCmd, 0x00);
 
@@ -838,10 +927,10 @@ static void r8169_disable ( struct nic *nic __unused ) {
 
 	RTL_W32(RxMissed, 0);
 
-	tpc->TxDescArray = NULL;
-	tpc->RxDescArray = NULL;
+	tp->TxDescArray = NULL;
+	tp->RxDescArray = NULL;
 	for (i = 0; i < NUM_RX_DESC; i++) {
-		tpc->RxBufferRing[i] = NULL;
+		tp->RxBufferRing[i] = NULL;
 	}
 }
 
@@ -858,6 +947,10 @@ static struct pci_device_id r8169_nics[] = {
         PCI_ROM(0x16ec, 0x0116, "usr-r8169", "US Robotics RTL8169 Gigabit Ethernet"),
         PCI_ROM(0x1186, 0x4300, "dlink-r8169", "D-Link RTL8169 Gigabit Ethernet"),
 	PCI_ROM(0x1737, 0x1032, "linksys-r8169", "Linksys RTL8169 Gigabit Ethernet"),
+	PCI_ROM(0x10ec, 0x8129, "r8169-8129", "RealTek RT8129 Fast Ethernet Adapter"),
+	PCI_ROM(0x10ec, 0x8136, "r8169-8101e", "RealTek RTL8101E PCI Express Fast Ethernet controller"),
+	PCI_ROM(0x10ec, 0x8167, "r8169-8110sc/8169sc", "RealTek RTL-8110SC/8169SC Gigabit Ethernet"),
+	PCI_ROM(0x10ec, 0x8168, "r8169-8168b", "RealTek RTL8111/8168B PCI Express Gigabit Ethernet controller"),
 };
 
 PCI_DRIVER ( r8169_driver, r8169_nics, PCI_NO_CLASS );
@@ -872,6 +965,7 @@ static int r8169_probe ( struct nic *nic, struct pci_device *pci ) {
 
 	static int board_idx = -1;
 	static int printed_version = 0;
+	struct rtl8169_private *tp = &tpx;
 	int i, rc;
 	int option = -1, Cap10_100 = 0, Cap1000 = 0;
 
@@ -891,9 +985,6 @@ static int r8169_probe ( struct nic *nic, struct pci_device *pci ) {
 		return 0;
 	memset ( r8169_bufs, 0, sizeof ( *r8169_bufs ) );
 
-	/* point to private storage */
-	tpc = &tpx;
-
 	rc = rtl8169_init_board(pci);	/* Return code is meaningless */
 
 	/* Get MAC address.  FIXME: read EEPROM */
@@ -901,30 +992,32 @@ static int r8169_probe ( struct nic *nic, struct pci_device *pci ) {
 		nic->node_addr[i] = RTL_R8(MAC0 + i);
 
 	DBG ( "%s: Identified chip type is '%s'.\n", pci->driver_name,
-		 rtl_chip_info[tpc->chipset].name );
+		 rtl_chip_info[tp->chipset].name );
 
 	/* Print out some hardware info */
-	DBG ( "%s: %s at IOAddr %#hX, ", pci->driver_name, eth_ntoa ( nic->node_addr ),
+	DBG ( "%s: %s at ioaddr %#hx, ", pci->driver_name, eth_ntoa ( nic->node_addr ),
 	      (unsigned int) ioaddr );
 
 	/* Config PHY */
-	rtl8169_hw_PHY_config(nic);
-
+	rtl8169_hw_phy_config(nic);
+ 
 	DBG("Set MAC Reg C+CR Offset 0x82h = 0x01h\n");
 	RTL_W8(0x82, 0x01);
 
-	if (tpc->mcfg < MCFG_METHOD_3) {
-		DBG("Set PCI Latency=0x40\n");
-		pci_write_config_byte(pci, PCI_LATENCY_TIMER, 0x40);
-	}
+	pci_write_config_byte(pci, PCI_LATENCY_TIMER, 0x40);
+
+	if (tp->mac_version <= RTL_GIGA_MAC_VER_06)
+		pci_write_config_byte(pci, PCI_CACHE_LINE_SIZE, 0x08);
 
-	if (tpc->mcfg == MCFG_METHOD_2) {
+	if (tp->mac_version == RTL_GIGA_MAC_VER_02) {
 		DBG("Set MAC Reg C+CR Offset 0x82h = 0x01h\n");
 		RTL_W8(0x82, 0x01);
 		DBG("Set PHY Reg 0x0bh = 0x00h\n");
-		RTL8169_WRITE_GMII_REG(ioaddr, 0x0b, 0x0000);	//w 0x0b 15 0 0
+		RTL8169_WRITE_GMII_REG(ioaddr, 0x0b, 0x0000); //w 0x0b 15 0 0
 	}
 
+	r8169_reset(nic);
+
 	/* if TBI is not endbled */
 	if (!(RTL_R8(PHYstatus) & TBI_Enable)) {
 		int val = RTL8169_READ_GMII_REG(ioaddr, PHY_AUTO_NEGO_REG);
@@ -933,7 +1026,6 @@ static int r8169_probe ( struct nic *nic, struct pci_device *pci ) {
 		val |= PHY_Cap_PAUSE | PHY_Cap_ASYM_PAUSE;
 #endif				//end #define RTL8169_HW_FLOW_CONTROL_SUPPORT
 
-		option = media;
 		/* Force RTL8169 in 10/100/1000 Full/Half mode. */
 		if (option > 0) {
 			printf(" Force-mode Enabled.\n");
@@ -1062,113 +1154,160 @@ static void rtl8169_hw_PHY_reset(struct nic *nic __unused)
 
 */
 
-//======================================================================================================
-static void rtl8169_hw_PHY_config(struct nic *nic __unused)
+struct phy_reg {
+	u16 reg;
+	u16 val;
+};
+
+static void rtl_phy_write(void *ioaddr, struct phy_reg *regs, int len)
 {
+	while (len-- > 0) {
+		RTL8169_WRITE_GMII_REG((u32)ioaddr, regs->reg, regs->val);
+		regs++;
+	}
+}
 
-	DBG("priv->mcfg=%d, priv->pcfg=%d\n", tpc->mcfg, tpc->pcfg);
+static void rtl8169s_hw_phy_config(void *ioaddr)
+{
+	struct {
+		u16 regs[5]; /* Beware of bit-sign propagation */
+	} phy_magic[5] = { {
+		{ 0x0000,	//w 4 15 12 0
+		  0x00a1,	//w 3 15 0 00a1
+		  0x0008,	//w 2 15 0 0008
+		  0x1020,	//w 1 15 0 1020
+		  0x1000 } },{	//w 0 15 0 1000
+		{ 0x7000,	//w 4 15 12 7
+		  0xff41,	//w 3 15 0 ff41
+		  0xde60,	//w 2 15 0 de60
+		  0x0140,	//w 1 15 0 0140
+		  0x0077 } },{	//w 0 15 0 0077
+		{ 0xa000,	//w 4 15 12 a
+		  0xdf01,	//w 3 15 0 df01
+		  0xdf20,	//w 2 15 0 df20
+		  0xff95,	//w 1 15 0 ff95
+		  0xfa00 } },{	//w 0 15 0 fa00
+		{ 0xb000,	//w 4 15 12 b
+		  0xff41,	//w 3 15 0 ff41
+		  0xde20,	//w 2 15 0 de20
+		  0x0140,	//w 1 15 0 0140
+		  0x00bb } },{	//w 0 15 0 00bb
+		{ 0xf000,	//w 4 15 12 f
+		  0xdf01,	//w 3 15 0 df01
+		  0xdf20,	//w 2 15 0 df20
+		  0xff95,	//w 1 15 0 ff95
+		  0xbf00 }	//w 0 15 0 bf00
+		}
+	}, *p = phy_magic;
+	unsigned int i;
+
+	RTL8169_WRITE_GMII_REG((u32)ioaddr, 0x1f, 0x0001);		//w 31 2 0 1
+	RTL8169_WRITE_GMII_REG((u32)ioaddr, 0x15, 0x1000);		//w 21 15 0 1000
+	RTL8169_WRITE_GMII_REG((u32)ioaddr, 0x18, 0x65c7);		//w 24 15 0 65c7
+	RTL8169_WRITE_GMII_REG_BIT((u32)ioaddr, 4, 11, 0);	//w 4 11 11 0
+
+	for (i = 0; i < ARRAY_SIZE(phy_magic); i++, p++) {
+		int val, pos = 4;
+
+		val = (RTL8169_READ_GMII_REG((u32)ioaddr, pos) & 0x0fff) | (p->regs[0] & 0xffff);
+		RTL8169_WRITE_GMII_REG((u32)ioaddr, pos, val);
+		while (--pos >= 0)
+			RTL8169_WRITE_GMII_REG((u32)ioaddr, pos, p->regs[4 - pos] & 0xffff);
+		RTL8169_WRITE_GMII_REG_BIT((u32)ioaddr, 4, 11, 1); //w 4 11 11 1
+		RTL8169_WRITE_GMII_REG_BIT((u32)ioaddr, 4, 11, 0); //w 4 11 11 0
+	}
+	RTL8169_WRITE_GMII_REG((u32)ioaddr, 0x1f, 0x0000); //w 31 2 0 0
+}
 
-	if (tpc->mcfg == MCFG_METHOD_4) {
-/*
-                RTL8169_WRITE_GMII_REG( (unsigned long)ioaddr, 0x1F, 0x0001 );
-                RTL8169_WRITE_GMII_REG( (unsigned long)ioaddr, 0x1b, 0x841e );
-                RTL8169_WRITE_GMII_REG( (unsigned long)ioaddr, 0x0e, 0x7bfb );
-                RTL8169_WRITE_GMII_REG( (unsigned long)ioaddr, 0x09, 0x273a );
-*/
+static void rtl8169sb_hw_phy_config(void *ioaddr)
+{
+	struct phy_reg phy_reg_init[] = {
+		{ 0x1f, 0x0002 },
+		{ 0x01, 0x90d0 },
+		{ 0x1f, 0x0000 }
+	};
 
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x1F,
-				       0x0002);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x01,
-				       0x90D0);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x1F,
-				       0x0000);
-	} else if ((tpc->mcfg == MCFG_METHOD_2)
-		   || (tpc->mcfg == MCFG_METHOD_3)) {
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x1F,
-				       0x0001);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x15,
-				       0x1000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x18,
-				       0x65C7);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0x0000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x03,
-				       0x00A1);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x02,
-				       0x0008);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x01,
-				       0x1020);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x00,
-				       0x1000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0x0800);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0x0000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0x7000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x03,
-				       0xFF41);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x02,
-				       0xDE60);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x01,
-				       0x0140);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x00,
-				       0x0077);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0x7800);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0x7000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xA000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x03,
-				       0xDF01);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x02,
-				       0xDF20);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x01,
-				       0xFF95);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x00,
-				       0xFA00);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xA800);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xA000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xB000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x03,
-				       0xFF41);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x02,
-				       0xDE20);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x01,
-				       0x0140);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x00,
-				       0x00BB);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xB800);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xB000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xF000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x03,
-				       0xDF01);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x02,
-				       0xDF20);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x01,
-				       0xFF95);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x00,
-				       0xBF00);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xF800);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0xF000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x04,
-				       0x0000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x1F,
-				       0x0000);
-		RTL8169_WRITE_GMII_REG((unsigned long) ioaddr, 0x0B,
-				       0x0000);
-	} else {
-		DBG("tpc->mcfg=%d. Discard hw PHY config.\n",
-			  tpc->mcfg);
+	rtl_phy_write(ioaddr, phy_reg_init, ARRAY_SIZE(phy_reg_init));
+}
+
+static void rtl8168cp_hw_phy_config(void *ioaddr)
+{
+	struct phy_reg phy_reg_init[] = {
+		{ 0x1f, 0x0000 },
+		{ 0x1d, 0x0f00 },
+		{ 0x1f, 0x0002 },
+		{ 0x0c, 0x1ec8 },
+		{ 0x1f, 0x0000 }
+	};
+
+	rtl_phy_write(ioaddr, phy_reg_init, ARRAY_SIZE(phy_reg_init));
+}
+
+static void rtl8168c_hw_phy_config(void *ioaddr)
+{
+	struct phy_reg phy_reg_init[] = {
+		{ 0x1f, 0x0001 },
+		{ 0x12, 0x2300 },
+		{ 0x1f, 0x0002 },
+		{ 0x00, 0x88d4 },
+		{ 0x01, 0x82b1 },
+		{ 0x03, 0x7002 },
+		{ 0x08, 0x9e30 },
+		{ 0x09, 0x01f0 },
+		{ 0x0a, 0x5500 },
+		{ 0x0c, 0x00c8 },
+		{ 0x1f, 0x0003 },
+		{ 0x12, 0xc096 },
+		{ 0x16, 0x000a },
+		{ 0x1f, 0x0000 }
+	};
+
+	rtl_phy_write(ioaddr, phy_reg_init, ARRAY_SIZE(phy_reg_init));
+}
+
+static void rtl8168cx_hw_phy_config(void *ioaddr)
+{
+	struct phy_reg phy_reg_init[] = {
+		{ 0x1f, 0x0000 },
+		{ 0x12, 0x2300 },
+		{ 0x1f, 0x0003 },
+		{ 0x16, 0x0f0a },
+		{ 0x1f, 0x0000 },
+		{ 0x1f, 0x0002 },
+		{ 0x0c, 0x7eb8 },
+		{ 0x1f, 0x0000 }
+	};
+
+	rtl_phy_write(ioaddr, phy_reg_init, ARRAY_SIZE(phy_reg_init));
+}
+
+static void rtl8169_hw_phy_config(struct nic *nic __unused)
+{
+	struct rtl8169_private *tp = &tpx;
+	void *ioaddr = tp->mmio_addr;
+	DBG("rtl8169_hw_phy_config(): card at addr=0x%lx: priv->mac_version=%d, priv->pcfg=%d\n", (unsigned long) ioaddr, tp->mac_version, tp->pcfg);
+
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_01:
+		break;
+	case RTL_GIGA_MAC_VER_02:
+	case RTL_GIGA_MAC_VER_03:
+		rtl8169s_hw_phy_config(ioaddr);
+		break;
+	case RTL_GIGA_MAC_VER_04:
+		rtl8169sb_hw_phy_config(ioaddr);
+		break;
+	case RTL_GIGA_MAC_VER_18:
+		rtl8168cp_hw_phy_config(ioaddr);
+		break;
+	case RTL_GIGA_MAC_VER_19:
+		rtl8168c_hw_phy_config(ioaddr);
+		break;
+	case RTL_GIGA_MAC_VER_20:
+		rtl8168cx_hw_phy_config(ioaddr);
+		break;
+	default:
+		break;
 	}
 }
 
diff --git a/gpxe/src/drivers/net/rtl8139.c b/gpxe/src/drivers/net/rtl8139.c
index c432884..509047a 100644
--- a/gpxe/src/drivers/net/rtl8139.c
+++ b/gpxe/src/drivers/net/rtl8139.c
@@ -518,6 +518,9 @@ static int rtl_probe ( struct pci_device *pci,
 	rtl_reset ( netdev );
 	rtl_init_eeprom ( netdev );
 	nvs_read ( &rtl->eeprom.nvs, EE_MAC, netdev->ll_addr, ETH_ALEN );
+
+	/* Mark as link up; we don't yet handle link state */
+	netdev_link_up ( netdev );
 	
 	/* Register network device */
 	if ( ( rc = register_netdev ( netdev ) ) != 0 )
diff --git a/gpxe/src/drivers/net/tg3.c b/gpxe/src/drivers/net/tg3.c
index 2aa072b..ba228d7 100644
--- a/gpxe/src/drivers/net/tg3.c
+++ b/gpxe/src/drivers/net/tg3.c
@@ -1879,7 +1879,8 @@ static int tg3_setup_hw(struct tg3 *tp)
 	     (65 << GRC_MISC_CFG_PRESCALAR_SHIFT));
 
 	/* Initialize MBUF/DESC pool. */
-	if (GET_ASIC_REV(tp->pci_chip_rev_id) != ASIC_REV_5705) {
+	if ((GET_ASIC_REV(tp->pci_chip_rev_id) != ASIC_REV_5705) &&
+		(tp->pci_chip_rev_id != CHIPREV_ID_5721)) {
 		tw32(BUFMGR_MB_POOL_ADDR, NIC_SRAM_MBUF_POOL_BASE);
 		if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5704)
 			tw32(BUFMGR_MB_POOL_SIZE, NIC_SRAM_MBUF_POOL_SIZE64);
@@ -3365,6 +3366,7 @@ PCI_ROM(0x14e4, 0x1648, "tg3-5704",        "Broadcom Tigon 3 5704"),
 PCI_ROM(0x14e4, 0x164d, "tg3-5702FE",      "Broadcom Tigon 3 5702FE"),
 PCI_ROM(0x14e4, 0x1653, "tg3-5705",        "Broadcom Tigon 3 5705"),
 PCI_ROM(0x14e4, 0x1654, "tg3-5705_2",      "Broadcom Tigon 3 5705_2"),
+PCI_ROM(0x14e4, 0x1659, "tg3-5721",        "Broadcom Tigon 3 5721"),
 PCI_ROM(0x14e4, 0x165d, "tg3-5705M",       "Broadcom Tigon 3 5705M"),
 PCI_ROM(0x14e4, 0x165e, "tg3-5705M_2",     "Broadcom Tigon 3 5705M_2"),
 PCI_ROM(0x14e4, 0x1677, "tg3-5751",        "Broadcom Tigon 3 5751"),
diff --git a/gpxe/src/drivers/net/tg3.h b/gpxe/src/drivers/net/tg3.h
index fd038f5..9077f80 100644
--- a/gpxe/src/drivers/net/tg3.h
+++ b/gpxe/src/drivers/net/tg3.h
@@ -283,6 +283,7 @@ typedef unsigned long dma_addr_t;
 #define  CHIPREV_ID_5705_A1		 0x3001
 #define	 CHIPREV_ID_5705_A2              0x3002
 #define  CHIPREV_ID_5705_A3              0x3003
+#define  CHIPREV_ID_5721                 0x4101
 #define  CHIPREV_ID_5750_A0              0x4000
 #define  CHIPREV_ID_5750_A1              0x4001
 #define  CHIPREV_ID_5750_A3              0x4003
diff --git a/gpxe/src/hci/strerror.c b/gpxe/src/hci/strerror.c
index 4fc15d0..74995e8 100644
--- a/gpxe/src/hci/strerror.c
+++ b/gpxe/src/hci/strerror.c
@@ -108,14 +108,18 @@ const char * strerror ( int errno ) {
 /** The most common errors */
 struct errortab common_errors[] __errortab = {
 	{ 0, "No error" },
-	{ ENOMEM, "Out of memory" },
+	{ EACCES, "Permission denied" },
+	{ ECANCELED, "Operation cancelled" },
+	{ ECONNRESET, "Connection reset" },
 	{ EINVAL, "Invalid argument" },
-	{ ENOSPC, "No space left on device" },
 	{ EIO, "Input/output error" },
-	{ EACCES, "Permission denied" },
-	{ ENOENT, "File not found" },
 	{ ENETUNREACH, "Network unreachable" },
+	{ ENODEV, "No such device" },
+	{ ENOENT, "File not found" },
+	{ ENOEXEC, "Not an executable image" },
+	{ ENOMEM, "Out of memory" },
+	{ ENOSPC, "No space left on device" },
+	{ ENOTSUP, "Not supported" },
+	{ EPERM, "Operation not permitted" },
 	{ ETIMEDOUT, "Connection timed out" },
-	{ EPIPE, "Broken pipe" },
-	{ ECANCELED, "Operation cancelled" },
 };
diff --git a/gpxe/src/image/embedded.c b/gpxe/src/image/embedded.c
index e2782a4..9783313 100644
--- a/gpxe/src/image/embedded.c
+++ b/gpxe/src/image/embedded.c
@@ -39,6 +39,7 @@ struct image *embedded_image(void)
 		return image = NULL;
 	}
 	copy_to_user(image->data, 0, _embedded_image_start, eisize);
+	register_image(image);
 
 	/* Reclaim embedded image memory */
 	reclaimed = 1;
diff --git a/gpxe/src/include/ctype.h b/gpxe/src/include/ctype.h
index a79395d..7740443 100644
--- a/gpxe/src/include/ctype.h
+++ b/gpxe/src/include/ctype.h
@@ -6,10 +6,9 @@
  * Character types
  */
 
-#define isdigit(c)	((c & 0x04) != 0)
-#define islower(c)	((c & 0x02) != 0)
-//#define isspace(c)	((c & 0x20) != 0)
-#define isupper(c)	((c & 0x01) != 0)
+#define isdigit(c)	((c) >= '0' && (c) <= '9')
+#define islower(c)	((c) >= 'a' && (c) <= 'z')
+#define isupper(c)	((c) >= 'A' && (c) <= 'Z')
 
 static inline unsigned char tolower(unsigned char c)
 {
diff --git a/gpxe/src/include/gpxe/errfile.h b/gpxe/src/include/gpxe/errfile.h
index ae8b148..42952d7 100644
--- a/gpxe/src/include/gpxe/errfile.h
+++ b/gpxe/src/include/gpxe/errfile.h
@@ -104,6 +104,7 @@
 #define ERRFILE_ipoib		     ( ERRFILE_DRIVER | 0x00470000 )
 #define ERRFILE_e1000		     ( ERRFILE_DRIVER | 0x00480000 )
 #define ERRFILE_e1000_hw	     ( ERRFILE_DRIVER | 0x00490000 )
+#define ERRFILE_mtnic		     ( ERRFILE_DRIVER | 0x004a0000 )
 
 #define ERRFILE_scsi		     ( ERRFILE_DRIVER | 0x00700000 )
 #define ERRFILE_arbel		     ( ERRFILE_DRIVER | 0x00710000 )
@@ -151,6 +152,7 @@
 #define ERRFILE_uri_test	      ( ERRFILE_OTHER | 0x000b0000 )
 #define ERRFILE_ibft		      ( ERRFILE_OTHER | 0x000c0000 )
 #define ERRFILE_tls		      ( ERRFILE_OTHER | 0x000d0000 )
+#define ERRFILE_ifmgmt		      ( ERRFILE_OTHER | 0x000e0000 )
 
 /** @} */
 
diff --git a/gpxe/src/include/gpxe/infiniband.h b/gpxe/src/include/gpxe/infiniband.h
index 354dc57..f9e6534 100644
--- a/gpxe/src/include/gpxe/infiniband.h
+++ b/gpxe/src/include/gpxe/infiniband.h
@@ -8,6 +8,7 @@
  */
 
 #include <stdint.h>
+#include <gpxe/refcnt.h>
 #include <gpxe/device.h>
 
 /** Subnet administrator QPN */
@@ -95,6 +96,11 @@ struct ib_queue_pair {
 	void *owner_priv;
 };
 
+/** Infiniband queue pair modification flags */
+enum ib_queue_pair_mods {
+	IB_MODIFY_QKEY = 0x0001,
+};
+
 /** An Infiniband Completion Queue */
 struct ib_completion_queue {
 	/** Completion queue number */
@@ -187,6 +193,16 @@ struct ib_device_operations {
 	 */
 	int ( * create_qp ) ( struct ib_device *ibdev,
 			      struct ib_queue_pair *qp );
+	/** Modify queue pair
+	 *
+	 * @v ibdev		Infiniband device
+	 * @v qp		Queue pair
+	 * @v mod_list		Modification list
+	 * @ret rc		Return status code
+	 */
+	int ( * modify_qp ) ( struct ib_device *ibdev,
+			      struct ib_queue_pair *qp,
+			      unsigned long mod_list );
 	/** Destroy queue pair
 	 *
 	 * @v ibdev		Infiniband device
@@ -240,6 +256,12 @@ struct ib_device_operations {
 			     ib_completer_t complete_send,
 			     ib_completer_t complete_recv );
 	/**
+	 * Poll event queue
+	 *
+	 * @v ibdev		Infiniband device
+	 */
+	void ( * poll_eq ) ( struct ib_device *ibdev );
+	/**
 	 * Open port
 	 *
 	 * @v ibdev		Infiniband device
@@ -285,12 +307,18 @@ struct ib_device_operations {
 
 /** An Infiniband device */
 struct ib_device {
+	/** Reference counter */
+	struct refcnt refcnt;
+	/** List of Infiniband devices */
+	struct list_head list;
 	/** Underlying device */
 	struct device *dev;
 	/** Infiniband operations */
 	struct ib_device_operations *op;
 	/** Port number */
 	unsigned int port;
+	/** Link state */
+	int link_up;
 	/** Port GID */
 	struct ib_gid port_gid;
 	/** Subnet manager LID */
@@ -311,6 +339,8 @@ extern struct ib_queue_pair *
 ib_create_qp ( struct ib_device *ibdev, unsigned int num_send_wqes,
 	       struct ib_completion_queue *send_cq, unsigned int num_recv_wqes,
 	       struct ib_completion_queue *recv_cq, unsigned long qkey );
+extern int ib_modify_qp ( struct ib_device *ibdev, struct ib_queue_pair *qp,
+			  unsigned long mod_list, unsigned long qkey );
 extern void ib_destroy_qp ( struct ib_device *ibdev,
 			    struct ib_queue_pair *qp );
 extern struct ib_work_queue * ib_find_wq ( struct ib_completion_queue *cq,
@@ -318,7 +348,7 @@ extern struct ib_work_queue * ib_find_wq ( struct ib_completion_queue *cq,
 extern struct ib_device * alloc_ibdev ( size_t priv_size );
 extern int register_ibdev ( struct ib_device *ibdev );
 extern void unregister_ibdev ( struct ib_device *ibdev );
-extern void free_ibdev ( struct ib_device *ibdev );
+extern void ib_link_state_changed ( struct ib_device *ibdev );
 
 /**
  * Post send work queue entry
@@ -425,6 +455,28 @@ ib_mad ( struct ib_device *ibdev, struct ib_mad_hdr *mad, size_t len ) {
 }
 
 /**
+ * Get reference to Infiniband device
+ *
+ * @v ibdev		Infiniband device
+ * @ret ibdev		Infiniband device
+ */
+static inline __attribute__ (( always_inline )) struct ib_device *
+ibdev_get ( struct ib_device *ibdev ) {
+	ref_get ( &ibdev->refcnt );
+	return ibdev;
+}
+
+/**
+ * Drop reference to Infiniband device
+ *
+ * @v ibdev		Infiniband device
+ */
+static inline __attribute__ (( always_inline )) void
+ibdev_put ( struct ib_device *ibdev ) {
+	ref_put ( &ibdev->refcnt );
+}
+
+/**
  * Set Infiniband work queue driver-private data
  *
  * @v wq		Work queue
diff --git a/gpxe/src/include/gpxe/init.h b/gpxe/src/include/gpxe/init.h
index d83aa5e..c468213 100644
--- a/gpxe/src/include/gpxe/init.h
+++ b/gpxe/src/include/gpxe/init.h
@@ -22,8 +22,9 @@ struct init_fn {
  */
 
 #define INIT_EARLY	01	/**< Early initialisation */
-#define	INIT_CONSOLE	02	/**< Console initialisation */
-#define INIT_NORMAL	03	/**< Normal initialisation */
+#define INIT_SERIAL	02	/**< Serial driver initialisation */
+#define	INIT_CONSOLE	03	/**< Console initialisation */
+#define INIT_NORMAL	04	/**< Normal initialisation */
 
 /** @} */
 
diff --git a/gpxe/src/include/gpxe/ipoib.h b/gpxe/src/include/gpxe/ipoib.h
index 0551687..bcbdc4c 100644
--- a/gpxe/src/include/gpxe/ipoib.h
+++ b/gpxe/src/include/gpxe/ipoib.h
@@ -72,6 +72,7 @@ static inline struct net_device * alloc_ipoibdev ( size_t priv_size ) {
 	return netdev;
 }
 
+extern void ipoib_link_state_changed ( struct ib_device *ibdev );
 extern int ipoib_probe ( struct ib_device *ibdev );
 extern void ipoib_remove ( struct ib_device *ibdev );
 
diff --git a/gpxe/src/include/gpxe/iscsi.h b/gpxe/src/include/gpxe/iscsi.h
index e4df684..5c44675 100644
--- a/gpxe/src/include/gpxe/iscsi.h
+++ b/gpxe/src/include/gpxe/iscsi.h
@@ -224,10 +224,14 @@ struct iscsi_bhs_login_response {
 #define ISCSI_OPCODE_LOGIN_RESPONSE 0x23
 
 /* Login response status codes */
-#define ISCSI_STATUS_SUCCESS		0x00
-#define ISCSI_STATUS_REDIRECT		0x01
-#define ISCSI_STATUS_INITIATOR_ERROR	0x02
-#define ISCSI_STATUS_TARGET_ERROR	0x03
+#define ISCSI_STATUS_SUCCESS			0x00
+#define ISCSI_STATUS_REDIRECT			0x01
+#define ISCSI_STATUS_INITIATOR_ERROR		0x02
+#define ISCSI_STATUS_INITIATOR_ERROR_AUTHENTICATION	0x01
+#define ISCSI_STATUS_INITIATOR_ERROR_AUTHORISATION	0x02
+#define ISCSI_STATUS_INITIATOR_ERROR_NOT_FOUND		0x03
+#define ISCSI_STATUS_INITIATOR_ERROR_REMOVED		0x04
+#define ISCSI_STATUS_TARGET_ERROR		0x03
 
 /**
  * iSCSI SCSI command basic header segment
diff --git a/gpxe/src/include/gpxe/netdevice.h b/gpxe/src/include/gpxe/netdevice.h
index d8cb84d..1ef648e 100644
--- a/gpxe/src/include/gpxe/netdevice.h
+++ b/gpxe/src/include/gpxe/netdevice.h
@@ -254,6 +254,9 @@ struct net_device {
 /** Network device is open */
 #define NETDEV_OPEN 0x0001
 
+/** Network device has link */
+#define NETDEV_LINK_UP 0x0002
+
 /** Declare a link-layer protocol */
 #define __ll_protocol  __table ( struct ll_protocol, ll_protocols, 01 )
 
@@ -352,6 +355,37 @@ netdev_settings ( struct net_device *netdev ) {
 	return &netdev->settings.settings;
 }
 
+/**
+ * Mark network device as having link up
+ *
+ * @v netdev		Network device
+ */
+static inline __attribute__ (( always_inline )) void
+netdev_link_up ( struct net_device *netdev ) {
+	netdev->state |= NETDEV_LINK_UP;
+}
+
+/**
+ * Mark network device as having link down
+ *
+ * @v netdev		Network device
+ */
+static inline __attribute__ (( always_inline )) void
+netdev_link_down ( struct net_device *netdev ) {
+	netdev->state &= ~NETDEV_LINK_UP;
+}
+
+/**
+ * Check link state of network device
+ *
+ * @v netdev		Network device
+ * @ret link_up		Link is up
+ */
+static inline __attribute__ (( always_inline )) int
+netdev_link_ok ( struct net_device *netdev ) {
+	return ( netdev->state & NETDEV_LINK_UP );
+}
+
 extern int netdev_tx ( struct net_device *netdev, struct io_buffer *iobuf );
 extern void netdev_tx_complete_err ( struct net_device *netdev,
 				 struct io_buffer *iobuf, int rc );
diff --git a/gpxe/src/include/gpxe/pci.h b/gpxe/src/include/gpxe/pci.h
index b5c417c..fdcecb6 100644
--- a/gpxe/src/include/gpxe/pci.h
+++ b/gpxe/src/include/gpxe/pci.h
@@ -30,7 +30,10 @@
 #define PCI_COMMAND_IO			0x1	/* Enable response in I/O space */
 #define PCI_COMMAND_MEM			0x2	/* Enable response in mem space */
 #define PCI_COMMAND_MASTER		0x4	/* Enable bus mastering */
+
+#define PCI_CACHE_LINE_SIZE		0x0c	/* 8 bits */
 #define PCI_LATENCY_TIMER		0x0d	/* 8 bits */
+
 #define PCI_COMMAND_SPECIAL		0x8	/* Enable response to special cycles */
 #define PCI_COMMAND_INVALIDATE		0x10	/* Use memory write and invalidate */
 #define  PCI_COMMAND_VGA_PALETTE 0x20	/* Enable palette snooping */
diff --git a/gpxe/src/include/gpxe/serial.h b/gpxe/src/include/gpxe/serial.h
new file mode 100644
index 0000000..2825b93
--- /dev/null
+++ b/gpxe/src/include/gpxe/serial.h
@@ -0,0 +1,14 @@
+#ifndef _GPXE_SERIAL_H
+#define _GPXE_SERIAL_H
+
+/** @file
+ *
+ * Serial driver functions
+ *
+ */
+
+extern void serial_putc ( int ch );
+extern int serial_getc ( void );
+extern int serial_ischar ( void );
+
+#endif /* _GPXE_SERIAL_H */
diff --git a/gpxe/src/include/gpxe/uuid.h b/gpxe/src/include/gpxe/uuid.h
index 4f89be5..18d1f14 100644
--- a/gpxe/src/include/gpxe/uuid.h
+++ b/gpxe/src/include/gpxe/uuid.h
@@ -12,11 +12,11 @@
 union uuid {
 	/** Canonical form (00000000-0000-0000-0000-000000000000) */
 	struct {
-		/** 8 hex digits, little-endian */
+		/** 8 hex digits, big-endian */
 		uint32_t a;
-		/** 2 hex digits, little-endian */
+		/** 2 hex digits, big-endian */
 		uint16_t b;
-		/** 2 hex digits, little-endian */
+		/** 2 hex digits, big-endian */
 		uint16_t c;
 		/** 2 hex digits, big-endian */
 		uint16_t d;
diff --git a/gpxe/src/include/usr/ifmgmt.h b/gpxe/src/include/usr/ifmgmt.h
index c7d35da..7b49d34 100644
--- a/gpxe/src/include/usr/ifmgmt.h
+++ b/gpxe/src/include/usr/ifmgmt.h
@@ -12,5 +12,6 @@ struct net_device;
 extern int ifopen ( struct net_device *netdev );
 extern void ifclose ( struct net_device *netdev );
 extern void ifstat ( struct net_device *netdev );
+extern int iflinkwait ( struct net_device *netdev, unsigned int max_wait_ms );
 
 #endif /* _USR_IFMGMT_H */
diff --git a/gpxe/src/net/infiniband.c b/gpxe/src/net/infiniband.c
index 39d1128..ab76742 100644
--- a/gpxe/src/net/infiniband.c
+++ b/gpxe/src/net/infiniband.c
@@ -29,6 +29,7 @@
 #include <gpxe/netdevice.h>
 #include <gpxe/iobuf.h>
 #include <gpxe/ipoib.h>
+#include <gpxe/process.h>
 #include <gpxe/infiniband.h>
 
 /** @file
@@ -37,6 +38,9 @@
  *
  */
 
+/** List of Infiniband devices */
+struct list_head ib_devices = LIST_HEAD_INIT ( ib_devices );
+
 /**
  * Create completion queue
  *
@@ -153,14 +157,40 @@ struct ib_queue_pair * ib_create_qp ( struct ib_device *ibdev,
 }
 
 /**
+ * Modify queue pair
+ *
+ * @v ibdev		Infiniband device
+ * @v qp		Queue pair
+ * @v mod_list		Modification list
+ * @v qkey		New queue key, if applicable
+ * @ret rc		Return status code
+ */
+int ib_modify_qp ( struct ib_device *ibdev, struct ib_queue_pair *qp,
+		   unsigned long mod_list, unsigned long qkey ) {
+	int rc;
+
+	DBGC ( ibdev, "IBDEV %p modifying QPN %#lx\n", ibdev, qp->qpn );
+
+	if ( mod_list & IB_MODIFY_QKEY )
+		qp->qkey = qkey;
+
+	if ( ( rc = ibdev->op->modify_qp ( ibdev, qp, mod_list ) ) != 0 ) {
+		DBGC ( ibdev, "IBDEV %p could not modify QPN %#lx: %s\n",
+		       ibdev, qp->qpn, strerror ( rc ) );
+		return rc;
+	}
+
+	return 0;
+}
+
+/**
  * Destroy queue pair
  *
  * @v ibdev		Infiniband device
  * @v qp		Queue pair
  */
-void ib_destroy_qp ( struct ib_device *ibdev,
-		     struct ib_queue_pair *qp ) {
-	DBGC ( ibdev, "IBDEV %p destroying queue pair %#lx\n",
+void ib_destroy_qp ( struct ib_device *ibdev, struct ib_queue_pair *qp ) {
+	DBGC ( ibdev, "IBDEV %p destroying QPN %#lx\n",
 	       ibdev, qp->qpn );
 	ibdev->op->destroy_qp ( ibdev, qp );
 	list_del ( &qp->send.list );
@@ -280,38 +310,6 @@ static int ib_get_pkey_table ( struct ib_device *ibdev,
 }
 
 /**
- * Wait for link up
- *
- * @v ibdev		Infiniband device
- * @ret rc		Return status code
- *
- * This function shouldn't really exist.  Unfortunately, IB links take
- * a long time to come up, and we can't get various key parameters
- * e.g. our own IPoIB MAC address without information from the subnet
- * manager).  We should eventually make link-up an asynchronous event.
- */
-static int ib_wait_for_link ( struct ib_device *ibdev ) {
-	struct ib_mad_port_info port_info;
-	unsigned int retries;
-	int rc;
-
-	printf ( "Waiting for Infiniband link-up..." );
-	for ( retries = 20 ; retries ; retries-- ) {
-		if ( ( rc = ib_get_port_info ( ibdev, &port_info ) ) != 0 )
-			continue;
-		if ( ( ( port_info.port_state__link_speed_supported ) & 0xf )
-		     == 4 ) {
-			printf ( "ok\n" );
-			return 0;
-		}
-		printf ( "." );
-		sleep ( 1 );
-	}
-	printf ( "failed\n" );
-	return -ENODEV;
-};
-
-/**
  * Get MAD parameters
  *
  * @v ibdev		Infiniband device
@@ -326,9 +324,13 @@ static int ib_get_mad_params ( struct ib_device *ibdev ) {
 	} u;
 	int rc;
 
-	/* Port info gives us the first half of the port GID and the SM LID */
+	/* Port info gives us the link state, the first half of the
+	 * port GID and the SM LID.
+	 */
 	if ( ( rc = ib_get_port_info ( ibdev, &u.port_info ) ) != 0 )
 		return rc;
+	ibdev->link_up = ( ( u.port_info.port_state__link_speed_supported
+			     & 0xf ) == 4 );
 	memcpy ( &ibdev->port_gid.u.bytes[0], u.port_info.gid_prefix, 8 );
 	ibdev->sm_lid = ntohs ( u.port_info.mastersm_lid );
 
@@ -353,6 +355,50 @@ static int ib_get_mad_params ( struct ib_device *ibdev ) {
 
 /***************************************************************************
  *
+ * Event queues
+ *
+ ***************************************************************************
+ */
+
+/**
+ * Handle Infiniband link state change
+ *
+ * @v ibdev		Infiniband device
+ */
+void ib_link_state_changed ( struct ib_device *ibdev ) {
+	int rc;
+
+	/* Update MAD parameters */
+	if ( ( rc = ib_get_mad_params ( ibdev ) ) != 0 ) {
+		DBGC ( ibdev, "IBDEV %p could not update MAD parameters: %s\n",
+		       ibdev, strerror ( rc ) );
+		return;
+	}
+
+	/* Notify IPoIB of link state change */
+	ipoib_link_state_changed ( ibdev );
+}
+
+/**
+ * Single-step the Infiniband event queue
+ *
+ * @v process		Infiniband event queue process
+ */
+static void ib_step ( struct process *process __unused ) {
+	struct ib_device *ibdev;
+
+	list_for_each_entry ( ibdev, &ib_devices, list ) {
+		ibdev->op->poll_eq ( ibdev );
+	}
+}
+
+/** Infiniband event queue process */
+struct process ib_process __permanent_process = {
+	.step = ib_step,
+};
+
+/***************************************************************************
+ *
  * Infiniband device creation/destruction
  *
  ***************************************************************************
@@ -387,14 +433,14 @@ struct ib_device * alloc_ibdev ( size_t priv_size ) {
 int register_ibdev ( struct ib_device *ibdev ) {
 	int rc;
 
+	/* Add to device list */
+	ibdev_get ( ibdev );
+	list_add_tail ( &ibdev->list, &ib_devices );
+
 	/* Open link */
 	if ( ( rc = ib_open ( ibdev ) ) != 0 )
 		goto err_open;
 
-	/* Wait for link */
-	if ( ( rc = ib_wait_for_link ( ibdev ) ) != 0 )
-		goto err_wait_for_link;
-
 	/* Get MAD parameters */
 	if ( ( rc = ib_get_mad_params ( ibdev ) ) != 0 )
 		goto err_get_mad_params;
@@ -406,13 +452,16 @@ int register_ibdev ( struct ib_device *ibdev ) {
 		goto err_ipoib_probe;
 	}
 
+	DBGC ( ibdev, "IBDEV %p registered (phys %s)\n", ibdev,
+	       ibdev->dev->name );
 	return 0;
 
  err_ipoib_probe:
  err_get_mad_params:
- err_wait_for_link:
 	ib_close ( ibdev );
  err_open:
+	list_del ( &ibdev->list );
+	ibdev_put ( ibdev );
 	return rc;
 }
 
@@ -422,16 +471,13 @@ int register_ibdev ( struct ib_device *ibdev ) {
  * @v ibdev		Infiniband device
  */
 void unregister_ibdev ( struct ib_device *ibdev ) {
+
+	/* Close device */
 	ipoib_remove ( ibdev );
 	ib_close ( ibdev );
-}
 
-/**
- * Free Infiniband device
- *
- * @v ibdev		Infiniband device
- */
-void free_ibdev ( struct ib_device *ibdev ) {
-	free ( ibdev );
+	/* Remove from device list */
+	list_del ( &ibdev->list );
+	ibdev_put ( ibdev );
+	DBGC ( ibdev, "IBDEV %p unregistered\n", ibdev );
 }
-
diff --git a/gpxe/src/net/ipv4.c b/gpxe/src/net/ipv4.c
index 591293b..82a13c3 100644
--- a/gpxe/src/net/ipv4.c
+++ b/gpxe/src/net/ipv4.c
@@ -273,7 +273,7 @@ static int ipv4_ll_addr ( struct in_addr dest, struct in_addr src,
 		memcpy ( ll_dest, ll_protocol->ll_broadcast,
 			 ll_protocol->ll_addr_len );
 		return 0;
-	} else if ( IN_MULTICAST ( dest.s_addr ) ) {
+	} else if ( IN_MULTICAST ( ntohl ( dest.s_addr ) ) ) {
 		/* Special case: IPv4 multicast over Ethernet.	This
 		 * code may need to be generalised once we find out
 		 * what happens for other link layers.
diff --git a/gpxe/src/net/tcp.c b/gpxe/src/net/tcp.c
index da8e87b..df87fc1 100644
--- a/gpxe/src/net/tcp.c
+++ b/gpxe/src/net/tcp.c
@@ -65,6 +65,11 @@ struct tcp_connection {
 	 * Equivalent to RCV.NXT in RFC 793 terminology.
 	 */
 	uint32_t rcv_ack;
+	/** Receive window
+	 *
+	 * Equivalent to RCV.WND in RFC 793 terminology.
+	 */
+	uint32_t rcv_win;
 	/** Most recent received timestamp
 	 *
 	 * Equivalent to TS.Recent in RFC 1323 terminology.
@@ -394,7 +399,7 @@ static int tcp_xmit ( struct tcp_connection *tcp, int force_send ) {
 	size_t len = 0;
 	size_t seq_len;
 	size_t app_win;
-	size_t rcv_win;
+	size_t max_rcv_win;
 
 	/* If retransmission timer is already running, do nothing */
 	if ( timer_running ( &tcp->timer ) )
@@ -439,14 +444,16 @@ static int tcp_xmit ( struct tcp_connection *tcp, int force_send ) {
 	/* Fill data payload from transmit queue */
 	tcp_process_queue ( tcp, len, iobuf, 0 );
 
-	/* Estimate window size */
-	rcv_win = ( ( freemem * 3 ) / 4 );
-	if ( rcv_win > TCP_MAX_WINDOW_SIZE )
-		rcv_win = TCP_MAX_WINDOW_SIZE;
+	/* Expand receive window if possible */
+	max_rcv_win = ( ( freemem * 3 ) / 4 );
+	if ( max_rcv_win > TCP_MAX_WINDOW_SIZE )
+		max_rcv_win = TCP_MAX_WINDOW_SIZE;
 	app_win = xfer_window ( &tcp->xfer );
-	if ( rcv_win > app_win )
-		rcv_win = app_win;
-	rcv_win &= ~0x03; /* Keep everything dword-aligned */
+	if ( max_rcv_win > app_win )
+		max_rcv_win = app_win;
+	max_rcv_win &= ~0x03; /* Keep everything dword-aligned */
+	if ( tcp->rcv_win < max_rcv_win )
+		tcp->rcv_win = max_rcv_win;
 
 	/* Fill up the TCP header */
 	payload = iobuf->data;
@@ -472,7 +479,7 @@ static int tcp_xmit ( struct tcp_connection *tcp, int force_send ) {
 	tcphdr->ack = htonl ( tcp->rcv_ack );
 	tcphdr->hlen = ( ( payload - iobuf->data ) << 2 );
 	tcphdr->flags = flags;
-	tcphdr->win = htons ( rcv_win );
+	tcphdr->win = htons ( tcp->rcv_win );
 	tcphdr->csum = tcpip_chksum ( iobuf->data, iob_len ( iobuf ) );
 
 	/* Dump header */
@@ -633,6 +640,21 @@ static void tcp_rx_opts ( struct tcp_connection *tcp, const void *data,
 }
 
 /**
+ * Consume received sequence space
+ *
+ * @v tcp		TCP connection
+ * @v seq_len		Sequence space length to consume
+ */
+static void tcp_rx_seq ( struct tcp_connection *tcp, size_t seq_len ) {
+	tcp->rcv_ack += seq_len;
+	if ( tcp->rcv_win > seq_len ) {
+		tcp->rcv_win -= seq_len;
+	} else {
+		tcp->rcv_win = 0;
+	}
+}
+
+/**
  * Handle TCP received SYN
  *
  * @v tcp		TCP connection
@@ -659,7 +681,7 @@ static int tcp_rx_syn ( struct tcp_connection *tcp, uint32_t seq,
 			    TCP_STATE_RCVD ( TCP_SYN ) );
 
 	/* Acknowledge SYN */
-	tcp->rcv_ack++;
+	tcp_rx_seq ( tcp, 1 );
 
 	return 0;
 }
@@ -747,7 +769,8 @@ static int tcp_rx_data ( struct tcp_connection *tcp, uint32_t seq,
 		return rc;
 
 	/* Acknowledge new data */
-	tcp->rcv_ack += len;
+	tcp_rx_seq ( tcp, len );
+
 	return 0;
 }
 
@@ -766,7 +789,7 @@ static int tcp_rx_fin ( struct tcp_connection *tcp, uint32_t seq ) {
 
 	/* Mark FIN as received and acknowledge it */
 	tcp->tcp_state |= TCP_STATE_RCVD ( TCP_FIN );
-	tcp->rcv_ack++;
+	tcp_rx_seq ( tcp, 1 );
 
 	/* Close connection */
 	tcp_close ( tcp, 0 );
@@ -789,7 +812,7 @@ static int tcp_rx_rst ( struct tcp_connection *tcp, uint32_t seq ) {
 	 * ACKed.
 	 */
 	if ( tcp->tcp_state & TCP_STATE_RCVD ( TCP_SYN ) ) {
-		if ( ( tcp->rcv_ack - seq ) > 0 )
+		if ( ( seq - tcp->rcv_ack ) >= tcp->rcv_win )
 			return 0;
 	} else {
 		if ( ! ( tcp->tcp_state & TCP_STATE_ACKED ( TCP_SYN ) ) )
@@ -850,7 +873,8 @@ static int tcp_rx ( struct io_buffer *iobuf,
 		rc = -EINVAL;
 		goto discard;
 	}
-	csum = tcpip_continue_chksum ( pshdr_csum, iobuf->data, iob_len ( iobuf ));
+	csum = tcpip_continue_chksum ( pshdr_csum, iobuf->data,
+				       iob_len ( iobuf ) );
 	if ( csum != 0 ) {
 		DBG ( "TCP checksum incorrect (is %04x including checksum "
 		      "field, should be 0000)\n", csum );
@@ -922,10 +946,19 @@ static int tcp_rx ( struct io_buffer *iobuf,
 	/* Dump out any state change as a result of the received packet */
 	tcp_dump_state ( tcp );
 
-	/* Send out any pending data.  If peer is expecting an ACK for
-	 * this packet then force sending a reply.
+	/* Send out any pending data.  We force sending a reply if either
+	 *
+	 *  a) the peer is expecting an ACK (i.e. consumed sequence space), or
+	 *  b) either end of the packet was outside the receive window
+	 *
+	 * Case (b) enables us to support TCP keepalives using
+	 * zero-length packets, which we would otherwise ignore.  Note
+	 * that for case (b), we need *only* consider zero-length
+	 * packets, since non-zero-length packets will already be
+	 * caught by case (a).
 	 */
-	tcp_xmit ( tcp, ( start_seq != seq ) );
+	tcp_xmit ( tcp, ( ( start_seq != seq ) ||
+			  ( ( seq - tcp->rcv_ack ) > tcp->rcv_win ) ) );
 
 	/* If this packet was the last we expect to receive, set up
 	 * timer to expire and cause the connection to be freed.
diff --git a/gpxe/src/net/tcp/iscsi.c b/gpxe/src/net/tcp/iscsi.c
index c01ca44..a12fca8 100644
--- a/gpxe/src/net/tcp/iscsi.c
+++ b/gpxe/src/net/tcp/iscsi.c
@@ -456,17 +456,18 @@ static int iscsi_build_login_request_strings ( struct iscsi_session *iscsi,
 				    "InitiatorName=%s%c"
 				    "TargetName=%s%c"
 				    "SessionType=Normal%c"
-				    "AuthMethod=CHAP,None%c",
+				    "AuthMethod=%sNone%c",
 				    iscsi_initiator_iqn(), 0,
-				    iscsi->target_iqn, 0, 0, 0 );
+				    iscsi->target_iqn, 0, 0,
+				    ( ( iscsi->username && iscsi->password ) ?
+				      "CHAP," : "" ), 0 );
 	}
 
 	if ( iscsi->status & ISCSI_STATUS_STRINGS_CHAP_ALGORITHM ) {
 		used += ssnprintf ( data + used, len - used, "CHAP_A=5%c", 0 );
 	}
 	
-	if ( ( iscsi->status & ISCSI_STATUS_STRINGS_CHAP_RESPONSE ) &&
-	     iscsi->username ) {
+	if ( ( iscsi->status & ISCSI_STATUS_STRINGS_CHAP_RESPONSE ) ) {
 		used += ssnprintf ( data + used, len - used,
 				    "CHAP_N=%s%cCHAP_R=0x",
 				    iscsi->username, 0 );
@@ -830,6 +831,35 @@ static int iscsi_rx_buffered_data ( struct iscsi_session *iscsi,
 }
 
 /**
+ * Convert iSCSI response status to return status code
+ *
+ * @v status_class	iSCSI status class
+ * @v status_detail	iSCSI status detail
+ * @ret rc		Return status code
+ */
+static int iscsi_status_to_rc ( unsigned int status_class,
+				unsigned int status_detail ) {
+	switch ( status_class ) {
+	case ISCSI_STATUS_INITIATOR_ERROR :
+		switch ( status_detail ) {
+		case ISCSI_STATUS_INITIATOR_ERROR_AUTHENTICATION :
+			return -EACCES;
+		case ISCSI_STATUS_INITIATOR_ERROR_AUTHORISATION :
+			return -EPERM;
+		case ISCSI_STATUS_INITIATOR_ERROR_NOT_FOUND :
+		case ISCSI_STATUS_INITIATOR_ERROR_REMOVED :
+			return -ENODEV;
+		default :
+			return -ENOTSUP;
+		}
+	case ISCSI_STATUS_TARGET_ERROR :
+		return -EIO;
+	default :
+		return -EINVAL;
+	}
+}
+
+/**
  * Receive data segment of an iSCSI login response PDU
  *
  * @v iscsi		iSCSI session
@@ -876,8 +906,10 @@ static int iscsi_rx_login_response ( struct iscsi_session *iscsi,
 	if ( response->status_class != 0 ) {
 		DBGC ( iscsi, "iSCSI login failure: class %02x detail %02x\n",
 		       response->status_class, response->status_detail );
-		iscsi->instant_rc = -EPERM;
-		return -EPERM;
+		rc = iscsi_status_to_rc ( response->status_class,
+					  response->status_detail );
+		iscsi->instant_rc = rc;
+		return rc;
 	}
 
 	/* Handle login transitions */
@@ -1176,7 +1208,7 @@ static int iscsi_rx_data ( struct iscsi_session *iscsi, const void *data,
 			return 0;
 		DBGC ( iscsi, "iSCSI %p unknown opcode %02x\n", iscsi,
 		       response->opcode );
-		return -EOPNOTSUPP;
+		return -ENOTSUP;
 	}
 }
 
diff --git a/gpxe/src/tests/gdbstub_test.S b/gpxe/src/tests/gdbstub_test.S
new file mode 100644
index 0000000..ee594ea
--- /dev/null
+++ b/gpxe/src/tests/gdbstub_test.S
@@ -0,0 +1,29 @@
+	.arch i386
+	.section ".text"
+	.code32
+gdbstub_test:
+	/* 1. Read registers test */
+	movl    $0xea010203, %eax
+	movl    $0xeb040506, %ebx
+	movl    $0xec070809, %ecx
+	movl    $0xed0a0b0c, %edx
+	movl    $0x510d0e0f, %esi
+	movl    $0xd1102030, %edi
+	int	$3
+
+	/* 2. Write registers test */
+	int	$3
+
+	/* 3. Read memory test */
+	subl	$8, %esp
+	movl	$0x11223344, 4(%esp)
+	movw	$0x5566, 2(%esp)
+	movb	$0x77, (%esp)
+	int	$3
+
+	/* 4. Write memory test */
+	int	$3
+	addl	$8, %esp
+
+1:
+	jmp	1b
diff --git a/gpxe/src/tests/gdbstub_test.gdb b/gpxe/src/tests/gdbstub_test.gdb
new file mode 100644
index 0000000..10db863
--- /dev/null
+++ b/gpxe/src/tests/gdbstub_test.gdb
@@ -0,0 +1,80 @@
+#!/usr/bin/gdb -x
+# Test suite for GDB remote debugging
+# Run:
+#   make bin/gpxe.hd.tmp
+#   make
+#   tests/gdbstub_test.gdb
+
+define gpxe_load_symbols
+	file bin/gpxe.hd.tmp
+end
+
+define gpxe_connect
+	target remote localhost:4444
+end
+
+define gpxe_assert
+	if $arg0 != $arg1
+		echo FAIL $arg2\n
+	else
+		echo PASS $arg2\n
+	end
+end
+
+define gpxe_start_tests
+	jump gdbstub_test
+end
+
+define gpxe_test_regs_read
+	gpxe_assert $eax 0xea010203 "gpxe_test_regs_read eax"
+	gpxe_assert $ebx 0xeb040506 "gpxe_test_regs_read ebx"
+	gpxe_assert $ecx 0xec070809 "gpxe_test_regs_read ecx"
+	gpxe_assert $edx 0xed0a0b0c "gpxe_test_regs_read edx"
+	gpxe_assert $esi 0x510d0e0f "gpxe_test_regs_read esi"
+	gpxe_assert $edi 0xd1102030 "gpxe_test_regs_read edi"
+end
+
+define gpxe_test_regs_write
+	set $eax = 0xea112233
+	set $ebx = 0xeb445566
+	set $ecx = 0xec778899
+	set $edx = 0xedaabbcc
+	set $esi = 0x51ddeeff
+	set $edi = 0xd1010203
+	c
+	gpxe_assert $eax 0xea112233 "gpxe_test_regs_write eax"
+	gpxe_assert $ebx 0xeb445566 "gpxe_test_regs_write ebx"
+	gpxe_assert $ecx 0xec778899 "gpxe_test_regs_write ecx"
+	gpxe_assert $edx 0xedaabbcc "gpxe_test_regs_write edx"
+	gpxe_assert $esi 0x51ddeeff "gpxe_test_regs_write esi"
+	gpxe_assert $edi 0xd1010203 "gpxe_test_regs_write edi"
+
+	# This assumes segment selectors are always 0x10 or 0x8 (for code).
+	gpxe_assert $cs 0x08 "gpxe_test_regs_write cs"
+	gpxe_assert $ds 0x10 "gpxe_test_regs_write ds"
+end
+
+define gpxe_test_mem_read
+	c
+	gpxe_assert ({int}($esp+4)) 0x11223344 "gpxe_test_mem_read int"
+	gpxe_assert ({short}($esp+2)) 0x5566 "gpxe_test_mem_read short"
+	gpxe_assert ({char}($esp)) 0x77 "gpxe_test_mem_read char"
+end
+
+define gpxe_test_mem_write
+	set ({int}($esp+4)) = 0xaabbccdd
+	set ({short}($esp+2)) = 0xeeff
+	set ({char}($esp)) = 0x99
+	c
+	gpxe_assert ({int}($esp+4)) 0xaabbccdd "gpxe_test_mem_write int"
+	gpxe_assert ({short}($esp+2)) (short)0xeeff "gpxe_test_mem_write short"
+	gpxe_assert ({char}($esp)) (char)0x99 "gpxe_test_mem_write char"
+end
+
+gpxe_load_symbols
+gpxe_connect
+gpxe_start_tests
+gpxe_test_regs_read
+gpxe_test_regs_write
+gpxe_test_mem_read
+gpxe_test_mem_write
diff --git a/gpxe/src/usr/autoboot.c b/gpxe/src/usr/autoboot.c
index c1a61ec..cff6e95 100644
--- a/gpxe/src/usr/autoboot.c
+++ b/gpxe/src/usr/autoboot.c
@@ -38,6 +38,9 @@
  *
  */
 
+/** Time to wait for link-up */
+#define LINK_WAIT_MS 15000
+
 /**
  * Identify the boot network device
  *
@@ -136,6 +139,14 @@ static int netboot ( struct net_device *netdev ) {
 		return rc;
 	ifstat ( netdev );
 
+	/* Wait for link-up */
+	printf ( "Waiting for link-up on %s...", netdev->name );
+	if ( ( rc = iflinkwait ( netdev, LINK_WAIT_MS ) ) != 0 ) {
+		printf ( " no link detected\n" );
+		return rc;
+	}
+	printf ( " ok\n" );
+
 	/* Configure device via DHCP */
 	if ( ( rc = dhcp ( netdev ) ) != 0 )
 		return rc;
diff --git a/gpxe/src/usr/ifmgmt.c b/gpxe/src/usr/ifmgmt.c
index 5f4323d..9c88ab5 100644
--- a/gpxe/src/usr/ifmgmt.c
+++ b/gpxe/src/usr/ifmgmt.c
@@ -18,8 +18,11 @@
 
 #include <string.h>
 #include <stdio.h>
+#include <unistd.h>
+#include <errno.h>
 #include <gpxe/netdevice.h>
 #include <gpxe/device.h>
+#include <gpxe/process.h>
 #include <usr/ifmgmt.h>
 
 /** @file
@@ -61,9 +64,28 @@ void ifclose ( struct net_device *netdev ) {
  * @v netdev		Network device
  */
 void ifstat ( struct net_device *netdev ) {
-	printf ( "%s: %s on %s (%s) TX:%d TXE:%d RX:%d RXE:%d\n",
+	printf ( "%s: %s on %s (%s)\n"
+		 "  [Link:%s, TX:%d TXE:%d RX:%d RXE:%d]\n",
 		 netdev->name, netdev_hwaddr ( netdev ), netdev->dev->name,
 		 ( ( netdev->state & NETDEV_OPEN ) ? "open" : "closed" ),
+		 ( netdev_link_ok ( netdev ) ? "up" : "down" ),
 		 netdev->stats.tx_ok, netdev->stats.tx_err,
 		 netdev->stats.rx_ok, netdev->stats.rx_err );
 }
+
+/**
+ * Wait for link-up
+ *
+ * @v netdev		Network device
+ * @v max_wait_ms	Maximum time to wait, in ms
+ */
+int iflinkwait ( struct net_device *netdev, unsigned int max_wait_ms ) {
+	while ( 1 ) {
+		if ( netdev_link_ok ( netdev ) )
+			return 0;
+		if ( max_wait_ms-- == 0 )
+			return -ETIMEDOUT;
+		step();
+		mdelay ( 1 );
+	}
+}
diff --git a/gpxe/src/util/mkconfig.pl b/gpxe/src/util/mkconfig.pl
index 6a9c2f1..e55c2ca 100755
--- a/gpxe/src/util/mkconfig.pl
+++ b/gpxe/src/util/mkconfig.pl
@@ -7,6 +7,7 @@ use warnings;
 
 my $cfgdir = "config";
 my $config_h = shift || "config.h";
+my @input_files;
 
 # Read in a whole file
 #
@@ -110,15 +111,15 @@ sub postamble {
   return "\n#endif /* $guard */\n";
 } 
 
-# Get the new configuration by splitting config.h file using the
-# @BEGIN/@END tags
+# Parse one config.h file into an existing configuration
 #
-sub new_config {
+sub parse_config {
   my $file = shift;
-
-  my $cfg = {};
+  my $cfg = shift;
   my $cursor = "";
 
+  push ( @input_files, $file );
+
   open my $fh, "<$file" or die "Could not open $file: $!\n";
   while ( <$fh> ) {
     if ( ( my $newcursor, my $suffix ) = /\@BEGIN\s+(\w+\.h)(.*)$/ ) {
@@ -133,14 +134,28 @@ sub new_config {
 	  ." at $file line $.\n" unless $cursor eq $oldcursor;
       $cfg->{$cursor} .= $prefix."*/\n";
       $cursor = "";
+    } elsif ( ( my $newfile ) = /\@TRYSOURCE\s+([\w\-]+\.h)/ ) {
+      die "Missing \"\@END $cursor\" before \"\@TRYSOURCE $newfile\""
+	  ." at $file line $.\n" if $cursor;
+      parse_config ( $newfile, $cfg ) if -e $newfile;
     } else {
       $cfg->{$cursor} .= $_ if $cursor;
     }
   }
   close $fh;
   die "Missing \"\@END $cursor\" in $file\n" if $cursor;
+}
 
-  foreach $cursor ( keys %$cfg ) {
+# Get the new configuration by splitting config.h file using the
+# @BEGIN/@END tags
+#
+sub new_config {
+  my $file = shift;
+  my $cfg = {};
+
+  parse_config ( $file, $cfg );
+
+  foreach my $cursor ( keys %$cfg ) {
     $cfg->{$cursor} .= postamble ( $cursor );
   }
 
@@ -180,9 +195,11 @@ foreach my $file ( keys %$new ) {
 }
 
 # If we now have fragments that are older than config.h, set the
-# timestamp on config.h to match the oldest fragment, to prevent make
-# from always attempting to rebuild the fragments.
+# timestamp on each input file to match the oldest fragment, to
+# prevent make from always attempting to rebuild the fragments.
 #
-if ( $oldest < file_mtime ( $config_h ) ) {
-  utime time(), $oldest, $config_h or die "Could not touch $config_h: $!\n";
+foreach my $file ( @input_files ) {
+  if ( $oldest < file_mtime ( $file ) ) {
+    utime time(), $oldest, $file or die "Could not touch $file: $!\n";
+  }
 }