Merge "These changes are the raw update to qemu-2.6."

110 files changed, 2414 insertions, 1548 deletions

diff --git a/qemu/roms/SLOF/README b/qemu/roms/SLOF/README
index 58e929427..294458880 100644
--- a/qemu/roms/SLOF/README
+++ b/qemu/roms/SLOF/README
@@ -11,6 +11,8 @@ Index
 2.2 Overview of the source code
 2.4 Extending the Forth engine
 3.0 Limitations
+4.0 Submitting patches
+5.0 Coding style
 
 
 1.0 Introduction to Slimline Open Firmware
@@ -236,6 +238,34 @@ To add primitives:
  On a JS21 all memory configurations should work.
 
 
+4.0 Submitting patches
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+Patches for SLOF should be made against https://github.com/aik/SLOF,
+the master branch and posted to slof@lists.ozlabs.org.
+The patches must be signed using "Signed-off-by" tag with a real name to
+confirm that you certify the Developer Certificate of Origin  Version 1.1,
+see [3] for details.
+
+
+5.0 Coding style
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+New C code submitted to SLOF should follow the coding style guidelines
+for the Linux kernel [4] with the following exceptions:
+
+- in the event that you require a specific width, use a standard type
+  like int32_t, uint32_t, uint64_t, etc. Don't use Linux kernel internal
+  types like u32, __u32 or __le32.
+
+New Forth code should use 4 space indentations and no tabs. Patches for
+the old code should keep the existing style which usually is
+3 space indentation.
+
+New assembly code submitted to SLOF should follow the coding style
+guidelines for the Linux kernel [4], i.e. indent with tabs, not with spaces.
+
+
 Documentation
 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
@@ -244,3 +274,9 @@ Documentation
 
 [2] PAPR Standard, Power.org(TM) Standard for Power Architecture(R) Platform
     Requirements (Workstation, Server), Version 2.4, December 7, 2009
+
+[3] Developer Certificate of Origin Version 1.1
+    http://developercertificate.org/
+
+[4] Linux kernel coding style
+    https://github.com/torvalds/linux/blob/master/Documentation/CodingStyle
diff --git a/qemu/roms/SLOF/VERSION b/qemu/roms/SLOF/VERSION
index 20bdb2eb7..aded5537b 100644
--- a/qemu/roms/SLOF/VERSION
+++ b/qemu/roms/SLOF/VERSION
@@ -1 +1 @@
-20150429
+20160223
diff --git a/qemu/roms/SLOF/board-js2x/llfw/stage2.lds b/qemu/roms/SLOF/board-js2x/llfw/stage2.lds
index f91f0658a..e6315c3c8 100644
--- a/qemu/roms/SLOF/board-js2x/llfw/stage2.lds
+++ b/qemu/roms/SLOF/board-js2x/llfw/stage2.lds
@@ -45,7 +45,8 @@ SECTIONS {
 	__bss_end = .;
 	__bss_size = (__bss_end - __bss_start);
 
-	__toc_start = .;
+	. = ALIGN(256);
+	__toc_start = DEFINED (.TOC.) ? .TOC. : ADDR (.got) + 0x8000;
 	.got       :
 	{
 		 *(.toc .got)
diff --git a/qemu/roms/SLOF/board-js2x/llfw/stage2_head.S b/qemu/roms/SLOF/board-js2x/llfw/stage2_head.S
index 5460bfebb..f3f5e0c8c 100644
--- a/qemu/roms/SLOF/board-js2x/llfw/stage2_head.S
+++ b/qemu/roms/SLOF/board-js2x/llfw/stage2_head.S
@@ -79,8 +79,6 @@ bsscdone:
 	/* ------------------------------------ */
 ASM_ENTRY(toc_init)	
 	LOAD64(r2, __toc_start)
-	addi	r2,r2,0x4000
-	addi	r2,r2,0x4000
 	blr
 
 	/* ------------------------------------ */
diff --git a/qemu/roms/SLOF/board-js2x/slof/Makefile b/qemu/roms/SLOF/board-js2x/slof/Makefile
index ab3e683a4..4cdd5fa36 100644
--- a/qemu/roms/SLOF/board-js2x/slof/Makefile
+++ b/qemu/roms/SLOF/board-js2x/slof/Makefile
@@ -57,6 +57,7 @@ OF_FFS_FILES = \
 	$(SLOFBRDDIR)/attu.fs \
 	$(SLOFBRDDIR)/cpu.fs \
 	$(SLOFBRDDIR)/ioapic.fs \
+	$(SLOFBRDDIR)/dma-function.fs \
 	$(SLOFBRDDIR)/pci-bridge_1022_7460.fs \
 	$(SLOFBRDDIR)/pci-device_1014_028c.fs \
 	$(SLOFBRDDIR)/pci-device_1014_02bd.fs \
diff --git a/qemu/roms/SLOF/board-js2x/slof/dma-function.fs b/qemu/roms/SLOF/board-js2x/slof/dma-function.fs
new file mode 100644
index 000000000..2e314cdaa
--- /dev/null
+++ b/qemu/roms/SLOF/board-js2x/slof/dma-function.fs
@@ -0,0 +1,31 @@
+\ *****************************************************************************
+\ * Copyright (c) 2004, 2011 IBM Corporation
+\ * All rights reserved.
+\ * This program and the accompanying materials
+\ * are made available under the terms of the BSD License
+\ * which accompanies this distribution, and is available at
+\ * http://www.opensource.org/licenses/bsd-license.php
+\ *
+\ * Contributors:
+\ *     IBM Corporation - initial implementation
+\ ****************************************************************************/
+
+: dma-alloc ( ... size -- virt )
+   \ ." dma-alloc called: " .s cr
+   alloc-mem
+;
+
+: dma-free ( virt size -- )
+   \ ." dma-free called: " .s cr
+   free-mem
+;
+
+: dma-map-in ( ... virt size cacheable? -- devaddr )
+   \ ." dma-map-in called: " .s cr
+   2drop
+;
+
+: dma-map-out ( virt devaddr size -- )
+   \ ." dma-map-out called: " .s cr
+   2drop drop
+;
diff --git a/qemu/roms/SLOF/board-js2x/slof/helper.fs b/qemu/roms/SLOF/board-js2x/slof/helper.fs
index 34d60da1f..1e2b03063 100644
--- a/qemu/roms/SLOF/board-js2x/slof/helper.fs
+++ b/qemu/roms/SLOF/board-js2x/slof/helper.fs
@@ -26,3 +26,14 @@
    s" , " $cat
    bdate2human $cat encode-string THEN
 ;
+
+: invert-region ( addr len -- )
+   2dup or 7 and CASE
+      0 OF 3 rshift 0 ?DO dup dup rx@ -1 xor swap rx! xa1+ LOOP ENDOF
+      4 OF 2 rshift 0 ?DO dup dup rl@ -1 xor swap rl! la1+ LOOP ENDOF
+      3 and
+      2 OF 1 rshift 0 ?DO dup dup rw@ -1 xor swap rw! wa1+ LOOP ENDOF
+      dup OF 0 ?DO dup dup rb@ -1 xor swap rb! 1+ LOOP ENDOF
+   ENDCASE
+   drop
+;
diff --git a/qemu/roms/SLOF/board-qemu/llfw/stage2.lds b/qemu/roms/SLOF/board-qemu/llfw/stage2.lds
index e060dd189..28c9dca93 100644
--- a/qemu/roms/SLOF/board-qemu/llfw/stage2.lds
+++ b/qemu/roms/SLOF/board-qemu/llfw/stage2.lds
@@ -49,7 +49,8 @@ SECTIONS {
 	__bss_end = .;
 	__bss_size = (__bss_end - __bss_start);
 
-	__toc_start = .;
+	. = ALIGN(256);
+	__toc_start = DEFINED (.TOC.) ? .TOC. : ADDR (.got) + 0x8000;
 	.got :
 	{
 		 *(.toc .got)
diff --git a/qemu/roms/SLOF/board-qemu/llfw/stage2_head.S b/qemu/roms/SLOF/board-qemu/llfw/stage2_head.S
index c56b117ce..adf75547b 100644
--- a/qemu/roms/SLOF/board-qemu/llfw/stage2_head.S
+++ b/qemu/roms/SLOF/board-qemu/llfw/stage2_head.S
@@ -77,8 +77,6 @@ bsscdone:
 	/* ------------------------------------ */
 ASM_ENTRY(toc_init)
 	LOAD64(r2, __toc_start)
-	addi	r2,r2,0x4000
-	addi	r2,r2,0x4000
 	blr
 
 	/* ------------------------------------ */
diff --git a/qemu/roms/SLOF/board-qemu/slof/Makefile b/qemu/roms/SLOF/board-qemu/slof/Makefile
index 283f77d32..a000a2519 100644
--- a/qemu/roms/SLOF/board-qemu/slof/Makefile
+++ b/qemu/roms/SLOF/board-qemu/slof/Makefile
@@ -66,9 +66,13 @@ USB_FFS_FILES = \
 
 VIO_FFS_FILES = \
 	$(SLOFBRDDIR)/pci-device_1af4_1000.fs \
+	$(SLOFBRDDIR)/pci-device_1af4_1041.fs \
 	$(SLOFBRDDIR)/pci-device_1af4_1001.fs \
+	$(SLOFBRDDIR)/pci-device_1af4_1042.fs \
 	$(SLOFBRDDIR)/pci-device_1af4_1004.fs \
+	$(SLOFBRDDIR)/pci-device_1af4_1048.fs \
 	$(SLOFBRDDIR)/pci-device_1af4_1009.fs \
+	$(SLOFBRDDIR)/pci-device_1af4_1050.fs \
 	$(SLOFBRDDIR)/vio-hvterm.fs \
 	$(SLOFBRDDIR)/vio-vscsi.fs \
 	$(SLOFBRDDIR)/vio-veth.fs \
@@ -103,6 +107,7 @@ OF_FFS_FILES = \
 	$(SLOFBRDDIR)/pci-device_1013_00b8.fs \
 	$(SLOFBRDDIR)/pci-device_8086_100e.fs \
 	$(SLOFBRDDIR)/e1k.fs \
+	$(SLOFBRDDIR)/qemu-vga.fs \
 	$(FCODE_FFS_FILES)
 
 # Uncomment the following line to enable the USB code:
diff --git a/qemu/roms/SLOF/board-qemu/slof/OF.fs b/qemu/roms/SLOF/board-qemu/slof/OF.fs
index 561d89225..69ee5c122 100644
--- a/qemu/roms/SLOF/board-qemu/slof/OF.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/OF.fs
@@ -134,10 +134,6 @@ check-boot-menu
 \ Grab rtas from qemu
 #include "rtas.fs"
 
-390 cp
-
-#include "virtio.fs"
-
 3f0 cp
 
 #include "tree.fs"
diff --git a/qemu/roms/SLOF/slof/fs/archsupport.fs b/qemu/roms/SLOF/board-qemu/slof/archsupport.fs
index cc4668769..a8ace3cc5 100644
--- a/qemu/roms/SLOF/slof/fs/archsupport.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/archsupport.fs
@@ -10,15 +10,16 @@
 \ *     IBM Corporation - initial implementation
 \ ****************************************************************************/
 
-\ Qemu supports max 256cpus, 32K will be able to accomodate the fdt changes if
-\ needed.
-8000 VALUE size
+\ 2 MiB FDT buffer size is enough to accommodate 255 CPU cores
+\ and 16 TiB of maxmem specification.
+200000 CONSTANT cas-buffer-size
 : ibm,client-architecture-support         ( vec -- err? )
     \ Store require parameters in nvram
     \ to come back to right boot device
     \ Allocate memory for H_CALL
-    size alloc-mem                        ( vec memaddr )
-    swap over size                        ( memaddr vec memaddr size )
+    cas-buffer-size alloc-mem             ( vec memaddr )
+    dup 0= IF ." out of memory during ibm,client-architecture-support" cr THEN
+    swap over cas-buffer-size             ( memaddr vec memaddr size )
     \ make h_call to hypervisor
     hv-cas 0= IF                          ( memaddr )
 	dup l@ 1 >= IF                    \ Version number >= 1
@@ -34,5 +35,5 @@
     ELSE
 	TRUE
     THEN
-    >r size free-mem r>
+    >r cas-buffer-size free-mem r>
 ;
diff --git a/qemu/roms/SLOF/board-qemu/slof/helper.fs b/qemu/roms/SLOF/board-qemu/slof/helper.fs
index 96da49894..40d4abc3a 100644
--- a/qemu/roms/SLOF/board-qemu/slof/helper.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/helper.fs
@@ -33,3 +33,16 @@
   swap -
 ;
 
+: invert-region-cs ( addr len cellsize -- )
+   >r over swap r@ rshift r> swap 1 hv-logical-memop drop
+;
+
+: invert-region ( addr len -- )
+   2dup or 7 and CASE
+      0 OF 3 invert-region-cs ENDOF
+      4 OF 2 invert-region-cs ENDOF
+      3 and
+      2 OF 1 invert-region-cs ENDOF
+      dup OF 0 invert-region-cs ENDOF
+   ENDCASE
+;
diff --git a/qemu/roms/SLOF/board-qemu/slof/pci-device_1234_1111.fs b/qemu/roms/SLOF/board-qemu/slof/pci-device_1234_1111.fs
index a5c3584f9..22ea45d5c 100644
--- a/qemu/roms/SLOF/board-qemu/slof/pci-device_1234_1111.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/pci-device_1234_1111.fs
@@ -10,233 +10,6 @@
 \ *     IBM Corporation - initial implementation
 \ ****************************************************************************/
 
-my-space pci-device-generic-setup
-
-\ Defaults, overriden from qemu
-d# 800 VALUE disp-width
-d# 600 VALUE disp-height
-d#   8 VALUE disp-depth
-
-\ Determine base address
-10 config-l@ translate-my-address f not AND VALUE fb-base
-
-\ Fixed up later
--1 VALUE io-base
-
-\ We support only one instance
-false VALUE is-installed?
-
-: vga-io-xlate ( port -- addr )
-  io-base -1 = IF
-    dup translate-my-address fff not and to io-base
-  THEN
-  io-base +
-;
-
-: vga-w! ( value port -- )
-  vga-io-xlate rw!-le
-;
-
-: vga-w@ ( port -- value )
-  vga-io-xlate rw@-le
-;
-
-: vga-b! ( value port -- )
-  vga-io-xlate rb!
-;
-
-: vga-b@ ( port -- value )
-  vga-io-xlate rb@
-;
-
-: vbe!	( value index -- )
-  1ce vga-w! 1d0 vga-w!
-;
-
-: vbe@	( index -- value )
-  1ce vga-w! 1d0 vga-w@
-;
-
-: color! ( r g b number -- ) 
-   3c8 vga-b!
-   rot 3c9 vga-b!
-   swap 3c9 vga-b!
-   3c9 vga-b!
-;
-
-: color@ ( number -- r g b ) 
-   3c8 vga-b!
-   3c9 vga-b@
-   3c9 vga-b@
-   3c9 vga-b@
-;
-
-: set-colors ( adr number #numbers -- )
-   over 3c8 vga-b!
-   swap DO
-     rb@ 3c9 vga-b!
-     rb@ 3c9 vga-b!
-     rb@ 3c9 vga-b!
-   LOOP
-   3drop
-;
-
-: get-colors ( adr number #numbers -- )
-   3drop
-;
-
-include graphics.fs
-
-\ qemu fake VBE IO registers
-0 CONSTANT VBE_DISPI_INDEX_ID
-1 CONSTANT VBE_DISPI_INDEX_XRES
-2 CONSTANT VBE_DISPI_INDEX_YRES
-3 CONSTANT VBE_DISPI_INDEX_BPP
-4 CONSTANT VBE_DISPI_INDEX_ENABLE
-5 CONSTANT VBE_DISPI_INDEX_BANK
-6 CONSTANT VBE_DISPI_INDEX_VIRT_WIDTH
-7 CONSTANT VBE_DISPI_INDEX_VIRT_HEIGHT
-8 CONSTANT VBE_DISPI_INDEX_X_OFFSET
-9 CONSTANT VBE_DISPI_INDEX_Y_OFFSET
-a CONSTANT VBE_DISPI_INDEX_NB
-
-\ ENABLE register
-00 CONSTANT VBE_DISPI_DISABLED
-01 CONSTANT VBE_DISPI_ENABLED
-02 CONSTANT VBE_DISPI_GETCAPS
-20 CONSTANT VBE_DISPI_8BIT_DAC
-40 CONSTANT VBE_DISPI_LFB_ENABLED
-80 CONSTANT VBE_DISPI_NOCLEARMEM
-
-: init-mode
-  0 3c0 vga-b!
-  VBE_DISPI_DISABLED VBE_DISPI_INDEX_ENABLE vbe!
-  0 VBE_DISPI_INDEX_X_OFFSET vbe!
-  0 VBE_DISPI_INDEX_Y_OFFSET vbe!
-  disp-width VBE_DISPI_INDEX_XRES vbe!
-  disp-height VBE_DISPI_INDEX_YRES vbe!
-  disp-depth VBE_DISPI_INDEX_BPP vbe!
-  VBE_DISPI_ENABLED VBE_DISPI_8BIT_DAC or VBE_DISPI_INDEX_ENABLE vbe!
-  0 3c0 vga-b!
-  20 3c0 vga-b!
-;
-
-: clear-screen
-  fb-base disp-width disp-height disp-depth 7 + 8 / * * 0 rfill
-;
-
-: read-settings
-  s" qemu,graphic-width" get-chosen IF
-     decode-int to disp-width 2drop
-  THEN
-  s" qemu,graphic-height" get-chosen IF
-     decode-int to disp-height 2drop
-  THEN
-  s" qemu,graphic-depth" get-chosen IF
-     decode-int nip nip     
-       dup 8 =
-       over f = or
-       over 10 = or
-       over 20 = or IF
-         to disp-depth
-       ELSE
-         ." Unsupported bit depth, using 8bpp " drop cr
-       THEN
-  THEN
-;
-
-: add-legacy-reg
-  \ add legacy I/O Ports / Memory regions to assigned-addresses
-  \ see PCI Bus Binding Revision 2.1 Section 7.
-  s" reg" get-node get-property IF
-    \ "reg" does not exist, create new
-    encode-start
-  ELSE
-    \ "reg" does exist, copy it 
-    encode-bytes
-  THEN
-  \ I/O Range 0x1ce-0x1d2
-  my-space a1000000 or encode-int+ \ non-relocatable, aliased I/O space
-  1ce encode-64+ 4 encode-64+ \ addr size
-  \ I/O Range 0x3B0-0x3BB
-  my-space a1000000 or encode-int+ \ non-relocatable, aliased I/O space
-  3b0 encode-64+ c encode-64+ \ addr size
-  \ I/O Range 0x3C0-0x3DF
-  my-space a1000000 or encode-int+ \ non-relocatable, aliased I/O space
-  3c0 encode-64+ 20 encode-64+ \ addr size
-  \ Memory Range 0xA0000-0xBFFFF
-  my-space a2000000 or encode-int+ \ non-relocatable, <1MB Memory space
-  a0000 encode-64+ 20000 encode-64+ \ addr size
-  s" reg" property \ store "reg" property
-;
-
-: setup-properties
-   \ Shouldn't this be done from open ?
-   disp-width encode-int s" width" property
-   disp-height encode-int s" height" property
-   disp-width disp-depth 7 + 8 / * encode-int s" linebytes" property
-   disp-depth encode-int s" depth" property
-   s" ISO8859-1" encode-string s" character-set" property \ i hope this is ok...
-   \ add "device_type" property
-   s" display" device-type
-   s" qemu,std-vga" encode-string s" compatible" property
-   \ XXX We don't create an "address" property because Linux doesn't know what
-   \ to do with it for >32-bit
-;
-
-\ words for installation/removal, needed by is-install/is-remove, see display.fs
-: display-remove ( -- ) 
-;
-
-: hcall-invert-screen ( -- )
-    frame-buffer-adr frame-buffer-adr 3
-    screen-height screen-width * screen-depth * /x /
-    1 hv-logical-memop
-    drop
-;
-
-: hcall-blink-screen ( -- )
-    \ 32 msec delay for visually noticing the blink
-    hcall-invert-screen 20 ms hcall-invert-screen
-;
-
-: display-install ( -- )
-    is-installed? NOT IF
-        ." Installing QEMU fb" cr
-        fb-base to frame-buffer-adr
-        clear-screen
-        default-font 
-        set-font
-        disp-width disp-height
-        disp-width char-width / disp-height char-height /
-        disp-depth 7 + 8 /                      ( width height #lines #cols depth )
-        fb-install
-	['] hcall-invert-screen to invert-screen
-	['] hcall-blink-screen to blink-screen
-         true to is-installed?
-    THEN
-;
-
-: set-alias
-    s" screen" find-alias 0= IF
-      \ no previous screen alias defined, define it...
-      s" screen" get-node node>path set-alias
-    ELSE
-       drop
-    THEN 
-;
-
-
 ." qemu vga" cr
 
-pci-master-enable
-pci-mem-enable
-pci-io-enable
-add-legacy-reg
-read-settings
-init-mode
-init-default-palette
-setup-properties
-' display-install is-install
-' display-remove is-remove
-set-alias
+s" qemu-vga.fs" included
diff --git a/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1001.fs b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1001.fs
index fb2463467..db0bb3fb8 100644
--- a/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1001.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1001.fs
@@ -22,13 +22,4 @@ pci-io-enable
 
 s" virtio-block.fs" included
 
-\ Allocate memory for virtio queue:
-virtiodev 0 virtio-get-qsize virtio-vring-size
-1000 CLAIM VALUE queue-addr
-
-\ Write queue address into device:
-queue-addr c rshift
-virtiodev vd>base @ 8 +
-rl!-le
-
 pci-device-disable
diff --git a/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1009.fs b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1009.fs
index 03964a6db..9c8be249d 100644
--- a/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1009.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1009.fs
@@ -22,13 +22,4 @@ pci-io-enable
 
 s" virtio-fs.fs" included
 
-\ Allocate memory for virtio queue:
-virtiodev 0 virtio-get-qsize virtio-vring-size
-1000 CLAIM VALUE queue-addr
-
-\ Write queue address into device:
-queue-addr c rshift
-virtiodev vd>base @ 8 +
-rl!-le
-
 pci-device-disable
diff --git a/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1041.fs b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1041.fs
new file mode 100644
index 000000000..552b0ef8a
--- /dev/null
+++ b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1041.fs
@@ -0,0 +1,15 @@
+\ *****************************************************************************
+\ * Copyright (c) 2016 IBM Corporation
+\ * All rights reserved.
+\ * This program and the accompanying materials
+\ * are made available under the terms of the BSD License
+\ * which accompanies this distribution, and is available at
+\ * http://www.opensource.org/licenses/bsd-license.php
+\ *
+\ * Contributors:
+\ *     IBM Corporation - initial implementation
+\ ****************************************************************************/
+
+\ Device ID 1041 is for virtio-net non-transitional device.
+\ Include the driver for virtio-net
+s" pci-device_1af4_1000.fs" included
diff --git a/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1042.fs b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1042.fs
new file mode 100644
index 000000000..2b0a848d4
--- /dev/null
+++ b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1042.fs
@@ -0,0 +1,15 @@
+\ *****************************************************************************
+\ * Copyright (c) 2016 IBM Corporation
+\ * All rights reserved.
+\ * This program and the accompanying materials
+\ * are made available under the terms of the BSD License
+\ * which accompanies this distribution, and is available at
+\ * http://www.opensource.org/licenses/bsd-license.php
+\ *
+\ * Contributors:
+\ *     IBM Corporation - initial implementation
+\ ****************************************************************************/
+
+\ Device ID 1042 is for virtio-blk non-transitional device.
+\ Include the driver for virtio-blk
+s" pci-device_1af4_1001.fs" included
diff --git a/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1048.fs b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1048.fs
new file mode 100644
index 000000000..055ad8960
--- /dev/null
+++ b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1048.fs
@@ -0,0 +1,15 @@
+\ *****************************************************************************
+\ * Copyright (c) 2016 IBM Corporation
+\ * All rights reserved.
+\ * This program and the accompanying materials
+\ * are made available under the terms of the BSD License
+\ * which accompanies this distribution, and is available at
+\ * http://www.opensource.org/licenses/bsd-license.php
+\ *
+\ * Contributors:
+\ *     IBM Corporation - initial implementation
+\ ****************************************************************************/
+
+\ Device ID 1048 is for virtio-scsi non-transitional device.
+\ Include the driver for virtio-scsi
+s" pci-device_1af4_1004.fs" included
diff --git a/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1050.fs b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1050.fs
new file mode 100644
index 000000000..516056aad
--- /dev/null
+++ b/qemu/roms/SLOF/board-qemu/slof/pci-device_1af4_1050.fs
@@ -0,0 +1,15 @@
+\ *****************************************************************************
+\ * Copyright (c) 2015 IBM Corporation
+\ * All rights reserved.
+\ * This program and the accompanying materials
+\ * are made available under the terms of the BSD License
+\ * which accompanies this distribution, and is available at
+\ * http://www.opensource.org/licenses/bsd-license.php
+\ *
+\ * Contributors:
+\ *     IBM Corporation - initial implementation
+\ ****************************************************************************/
+
+s" virtio [ vga ]" type cr
+
+s" qemu-vga.fs" included
diff --git a/qemu/roms/SLOF/board-qemu/slof/qemu-vga.fs b/qemu/roms/SLOF/board-qemu/slof/qemu-vga.fs
new file mode 100644
index 000000000..3f4c237fc
--- /dev/null
+++ b/qemu/roms/SLOF/board-qemu/slof/qemu-vga.fs
@@ -0,0 +1,198 @@
+\ *****************************************************************************
+\ * Copyright (c) 2015 IBM Corporation
+\ * All rights reserved.
+\ * This program and the accompanying materials
+\ * are made available under the terms of the BSD License
+\ * which accompanies this distribution, and is available at
+\ * http://www.opensource.org/licenses/bsd-license.php
+\ *
+\ * Contributors:
+\ *     IBM Corporation - initial implementation
+\ ****************************************************************************/
+
+my-space pci-device-generic-setup
+
+\ Defaults, overriden from qemu
+d# 800 VALUE disp-width
+d# 600 VALUE disp-height
+d#   8 VALUE disp-depth
+
+: map-in  " map-in" my-phandle parent $call-static ;
+: map-out  " map-out" my-phandle parent $call-static ;
+
+\ Determine base address
+0 0  my-space h# 02000010 + 1  map-in VALUE fb-base
+0 0  my-space h# 02000018 + 1 map-in VALUE reg-base
+
+\ We support only one instance
+false VALUE is-installed?
+
+: vga-w! ( value port -- )
+  3c0 - reg-base 400 + + rw!-le
+;
+
+: vga-w@ ( port -- value )
+  3c0 - reg-base 400 + + rw@-le
+;
+
+: vga-b! ( value port -- )
+  3c0 - reg-base 400 + + rb!
+;
+
+: vga-b@ ( port -- value )
+  3c0 - reg-base 400 + + rb@
+;
+
+: vbe!	( value index -- )
+  1 << reg-base 500 + + rw!-le
+;
+
+: vbe@	( index -- value )
+  1 << reg-base 500 + + rw@-le
+;
+
+: color! ( r g b number -- )
+   3c8 vga-b!
+   rot 3c9 vga-b!
+   swap 3c9 vga-b!
+   3c9 vga-b!
+;
+
+: color@ ( number -- r g b )
+   3c8 vga-b!
+   3c9 vga-b@
+   3c9 vga-b@
+   3c9 vga-b@
+;
+
+: set-colors ( adr number #numbers -- )
+   over 3c8 vga-b!
+   swap DO
+     rb@ 3c9 vga-b!
+     rb@ 3c9 vga-b!
+     rb@ 3c9 vga-b!
+   LOOP
+   3drop
+;
+
+: get-colors ( adr number #numbers -- )
+   3drop
+;
+
+include graphics.fs
+
+\ qemu fake VBE IO registers
+0 CONSTANT VBE_DISPI_INDEX_ID
+1 CONSTANT VBE_DISPI_INDEX_XRES
+2 CONSTANT VBE_DISPI_INDEX_YRES
+3 CONSTANT VBE_DISPI_INDEX_BPP
+4 CONSTANT VBE_DISPI_INDEX_ENABLE
+5 CONSTANT VBE_DISPI_INDEX_BANK
+6 CONSTANT VBE_DISPI_INDEX_VIRT_WIDTH
+7 CONSTANT VBE_DISPI_INDEX_VIRT_HEIGHT
+8 CONSTANT VBE_DISPI_INDEX_X_OFFSET
+9 CONSTANT VBE_DISPI_INDEX_Y_OFFSET
+a CONSTANT VBE_DISPI_INDEX_NB
+
+\ ENABLE register
+00 CONSTANT VBE_DISPI_DISABLED
+01 CONSTANT VBE_DISPI_ENABLED
+02 CONSTANT VBE_DISPI_GETCAPS
+20 CONSTANT VBE_DISPI_8BIT_DAC
+40 CONSTANT VBE_DISPI_LFB_ENABLED
+80 CONSTANT VBE_DISPI_NOCLEARMEM
+
+: init-mode
+  0 3c0 vga-b!
+  VBE_DISPI_DISABLED VBE_DISPI_INDEX_ENABLE vbe!
+  0 VBE_DISPI_INDEX_X_OFFSET vbe!
+  0 VBE_DISPI_INDEX_Y_OFFSET vbe!
+  disp-width VBE_DISPI_INDEX_XRES vbe!
+  disp-height VBE_DISPI_INDEX_YRES vbe!
+  disp-depth VBE_DISPI_INDEX_BPP vbe!
+  VBE_DISPI_ENABLED VBE_DISPI_8BIT_DAC or VBE_DISPI_INDEX_ENABLE vbe!
+  0 3c0 vga-b!
+  20 3c0 vga-b!
+;
+
+: clear-screen
+  fb-base disp-width disp-height disp-depth 7 + 8 / * * 0 rfill
+;
+
+: read-settings
+  s" qemu,graphic-width" get-chosen IF
+     decode-int to disp-width 2drop
+  THEN
+  s" qemu,graphic-height" get-chosen IF
+     decode-int to disp-height 2drop
+  THEN
+  s" qemu,graphic-depth" get-chosen IF
+     decode-int nip nip
+       dup 8 =
+       over f = or
+       over 10 = or
+       over 20 = or IF
+         to disp-depth
+       ELSE
+         ." Unsupported bit depth, using 8bpp " drop cr
+       THEN
+  THEN
+;
+
+: setup-properties
+   \ Shouldn't this be done from open ?
+   disp-width encode-int s" width" property
+   disp-height encode-int s" height" property
+   disp-width disp-depth 7 + 8 / * encode-int s" linebytes" property
+   disp-depth encode-int s" depth" property
+   s" ISO8859-1" encode-string s" character-set" property \ i hope this is ok...
+   \ add "device_type" property
+   s" display" device-type
+   s" qemu,std-vga" encode-string s" compatible" property
+   \ XXX We don't create an "address" property because Linux doesn't know what
+   \ to do with it for >32-bit
+;
+
+\ words for installation/removal, needed by is-install/is-remove, see display.fs
+: display-remove ( -- )
+;
+
+: slow-blink-screen ( -- )
+    \ 32 msec delay for visually noticing the blink
+    invert-screen 20 ms invert-screen
+;
+
+: display-install ( -- )
+    is-installed? NOT IF
+        ." Installing QEMU fb" cr
+        fb-base to frame-buffer-adr
+        clear-screen
+        default-font
+        set-font
+        disp-width disp-height
+        disp-width char-width / disp-height char-height /
+        disp-depth 7 + 8 /                      ( width height #lines #cols depth )
+        fb-install
+	['] slow-blink-screen to blink-screen
+	true to is-installed?
+    THEN
+;
+
+: set-alias
+    s" screen" find-alias 0= IF
+      \ no previous screen alias defined, define it...
+      s" screen" get-node node>path set-alias
+    ELSE
+       drop
+    THEN
+;
+
+pci-master-enable
+pci-mem-enable
+read-settings
+init-mode
+init-default-palette
+setup-properties
+' display-install is-install
+' display-remove is-remove
+set-alias
diff --git a/qemu/roms/SLOF/board-qemu/slof/tree.fs b/qemu/roms/SLOF/board-qemu/slof/tree.fs
index 4aba4c53f..78dafab71 100644
--- a/qemu/roms/SLOF/board-qemu/slof/tree.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/tree.fs
@@ -26,6 +26,8 @@
 \ 2 encode-int s" #size-cells" property
 \ s" chrp" device-type
 
+#include "archsupport.fs"
+
 480 cp
 
 \ See 3.6.5, and the PowerPC OF binding document.
diff --git a/qemu/roms/SLOF/board-qemu/slof/virtio-block.fs b/qemu/roms/SLOF/board-qemu/slof/virtio-block.fs
index ea388fb00..bc9013eea 100644
--- a/qemu/roms/SLOF/board-qemu/slof/virtio-block.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/virtio-block.fs
@@ -23,8 +23,7 @@ FALSE VALUE initialized?
 
 INSTANCE VARIABLE deblocker
 
-/vd-len BUFFER: virtiodev
-virtiodev virtio-setup-vd
+virtio-setup-vd VALUE virtiodev
 
 \ Quiesce the virtqueue of this device so that no more background
 \ transactions can be pending.
diff --git a/qemu/roms/SLOF/board-qemu/slof/virtio-fs.fs b/qemu/roms/SLOF/board-qemu/slof/virtio-fs.fs
index 8632b465f..3898d0b7d 100644
--- a/qemu/roms/SLOF/board-qemu/slof/virtio-fs.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/virtio-fs.fs
@@ -20,8 +20,7 @@ FALSE VALUE initialized?
 
 2000 CONSTANT VIRTFS-BUF-SIZE \ 8k
 
-/vd-len BUFFER: virtiodev
-virtiodev virtio-setup-vd
+virtio-setup-vd VALUE virtiodev
 
 \
 \ Support methods.
diff --git a/qemu/roms/SLOF/board-qemu/slof/virtio-net.fs b/qemu/roms/SLOF/board-qemu/slof/virtio-net.fs
index 412b34fa6..b16fffe39 100644
--- a/qemu/roms/SLOF/board-qemu/slof/virtio-net.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/virtio-net.fs
@@ -16,20 +16,28 @@ s" network" device-type
 
 INSTANCE VARIABLE obp-tftp-package
 
-/vd-len BUFFER: virtiodev
-virtiodev virtio-setup-vd
+virtio-setup-vd VALUE virtiodev
 0 VALUE virtio-net-priv
 0 VALUE open-count
 
+\ Set up MAC address from config virtqueue
+6 BUFFER: local-mac
+: setup-mac  ( -- )
+   s" local-mac-address" get-node get-property not IF 2drop EXIT THEN
+   6 0 DO
+      virtiodev i 1 virtio-get-config
+      local-mac i + c!
+   LOOP
+   local-mac 6 encode-bytes  s" local-mac-address"  property
+;
+
 : open  ( -- okay? )
    open-count 0= IF
       open IF
          \ my-unit 1 rtas-set-tce-bypass
-         s" local-mac-address" get-node get-property not IF
-            virtiodev virtio-net-open dup not IF ." virtio-net-open failed" EXIT THEN
-            drop TO virtio-net-priv
-         THEN
-         true
+         virtiodev virtio-net-open not IF ." virtio-net-open failed" false EXIT THEN
+         TO virtio-net-priv
+         setup-mac true
       ELSE
          false
       THEN
@@ -77,17 +85,6 @@ virtiodev virtio-setup-vd
    s" ping" obp-tftp-package @ $call-method
 ;
 
-\ Set up MAC address from config virtqueue
-6 BUFFER: local-mac
-: setup-mac  ( -- )
-   6 0 DO
-      virtiodev i 1 virtio-get-config
-      local-mac i + c!
-   LOOP
-   local-mac 6 encode-bytes  s" local-mac-address"  property
-;
-setup-mac
-
 : setup-alias  ( -- )
    " net" get-next-alias ?dup IF
       get-node node>path set-alias
diff --git a/qemu/roms/SLOF/board-qemu/slof/virtio-scsi.fs b/qemu/roms/SLOF/board-qemu/slof/virtio-scsi.fs
index ca5fb13aa..4fedeeeb1 100644
--- a/qemu/roms/SLOF/board-qemu/slof/virtio-scsi.fs
+++ b/qemu/roms/SLOF/board-qemu/slof/virtio-scsi.fs
@@ -22,8 +22,7 @@ FALSE CONSTANT virtio-scsi-debug
 
 FALSE VALUE initialized?
 
-/vd-len BUFFER: virtiodev
-virtiodev virtio-setup-vd
+virtio-setup-vd VALUE virtiodev
 
 STRUCT \ virtio-scsi-config
     /l FIELD vs-cfg>num-queues
diff --git a/qemu/roms/SLOF/board-qemu/slof/virtio.fs b/qemu/roms/SLOF/board-qemu/slof/virtio.fs
deleted file mode 100644
index 818c1320e..000000000
--- a/qemu/roms/SLOF/board-qemu/slof/virtio.fs
+++ /dev/null
@@ -1,35 +0,0 @@
-\ *****************************************************************************
-\ * Copyright (c) 2011 IBM Corporation
-\ * All rights reserved.
-\ * This program and the accompanying materials
-\ * are made available under the terms of the BSD License
-\ * which accompanies this distribution, and is available at
-\ * http://www.opensource.org/licenses/bsd-license.php
-\ *
-\ * Contributors:
-\ *     IBM Corporation - initial implementation
-\ ****************************************************************************/
-
-\ This struct must match "struct virtio_device" in virtio.h!
-STRUCT
-   /n FIELD vd>base
-   /l FIELD vd>type
-CONSTANT /vd-len
-
-
-\ Initialize virtiodev structure for the current node
-: virtio-setup-vd  ( vdstruct -- )
-   >r
-   \ Does it have a "class-code" property? If yes, assume we're a PCI device
-   s" class-code" get-node get-property 0= IF
-      \ Set up for PCI device interface
-      2drop
-      s" 10 config-l@ translate-my-address 3 not AND" evaluate
-      ( io-base ) r@ vd>base !
-      0 r@ vd>type l!
-   ELSE
-      ." unsupported virtio interface!" cr
-      1 r@ vd>type l!
-   THEN
-   r> drop
-;
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netapps/netboot.c b/qemu/roms/SLOF/clients/net-snk/app/netapps/netboot.c
index cf20b5915..bb1db03e3 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netapps/netboot.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netapps/netboot.c
@@ -332,7 +332,13 @@ int dhcp(char *ret_buffer, filename_ip_t * fn_ip, unsigned int retries, int flag
 	int i = (int) retries+1;
 	int rc = -1;
 
-	printf("    ");
+	printf("  Requesting information via DHCP%s:     ",
+	       flags == F_IPV4 ? "v4" : flags == F_IPV6 ? "v6" : "");
+
+	if (flags != F_IPV6)
+		dhcpv4_generate_transaction_id();
+	if (flags != F_IPV4)
+		dhcpv6_generate_transaction_id();
 
 	do {
 		printf("\b\b\b%03d", i-1);
@@ -353,7 +359,6 @@ int dhcp(char *ret_buffer, filename_ip_t * fn_ip, unsigned int retries, int flag
 			set_ipv6_address(fn_ip->fd, 0);
 			rc = dhcpv6(ret_buffer, fn_ip);
 			if (rc == 0) {
-				printf("\n");
 				memcpy(&fn_ip->own_ip6, get_ipv6_address(), 16);
 				break;
 			}
@@ -362,11 +367,23 @@ int dhcp(char *ret_buffer, filename_ip_t * fn_ip, unsigned int retries, int flag
 		if (rc != -1) /* either success or non-dhcp failure */
 			break;
 	} while (1);
-	printf("\b\b\b\b");
+	printf("\b\b\b\bdone\n");
 
 	return rc;
 }
 
+/**
+ * Seed the random number generator with our mac and current timestamp
+ */
+static void seed_rng(uint8_t mac[])
+{
+	unsigned int seed;
+
+	asm volatile("mftbl %0" : "=r"(seed));
+	seed ^= (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) | mac[5];
+	srand(seed);
+}
+
 int
 netboot(int argc, char *argv[])
 {
@@ -388,8 +405,7 @@ netboot(int argc, char *argv[])
 	int32_t block_size = strtol(argv[5], 0, 10);
 	uint8_t own_mac[6];
 
-	printf("\n");
-	printf(" Bootloader 1.6 \n");
+	puts("\n Initializing NIC");
 	memset(&fn_ip, 0, sizeof(filename_ip_t));
 
 	/***********************************************************
@@ -438,6 +454,8 @@ netboot(int argc, char *argv[])
 	// init ethernet layer
 	set_mac_address(own_mac);
 
+	seed_rng(own_mac);
+
 	if (argc > 6) {
 		parse_args(argv[6], &obp_tftp_args);
 		if(obp_tftp_args.bootp_retries - rc < DEFAULT_BOOT_RETRIES)
@@ -468,10 +486,8 @@ netboot(int argc, char *argv[])
 		}
 	}
 	else if (ip_version == 6) {
-		if (memcmp(&obp_tftp_args.ci6addr, null_ip6, 16) != 0
-		    && memcmp(&obp_tftp_args.si6addr, null_ip6, 16) != 0
+		if (memcmp(&obp_tftp_args.si6addr, null_ip6, 16) != 0
 		    && obp_tftp_args.filename[0] != 0) {
-
 			memcpy(&fn_ip.server_ip6.addr[0],
 			       &obp_tftp_args.si6addr.addr, 16);
 			obp_tftp_args.ip_init = IP_INIT_IPV6_MANUAL;
@@ -484,7 +500,6 @@ netboot(int argc, char *argv[])
 	// construction of fn_ip from parameter
 	switch(obp_tftp_args.ip_init) {
 	case IP_INIT_BOOTP:
-		printf("  Requesting IP address via BOOTP: ");
 		// if giaddr in not specified, then we have to identify
 		// the BOOTP server via broadcasts
 		if(memcmp(obp_tftp_args.giaddr, null_ip, 4) == 0) {
@@ -499,19 +514,25 @@ netboot(int argc, char *argv[])
 		rc = bootp(ret_buffer, &fn_ip, obp_tftp_args.bootp_retries);
 		break;
 	case IP_INIT_DHCP:
-		printf("  Requesting IP address via DHCPv4: ");
 		rc = dhcp(ret_buffer, &fn_ip, obp_tftp_args.bootp_retries, F_IPV4);
 		break;
 	case IP_INIT_DHCPV6_STATELESS:
-		printf("  Requesting information via DHCPv6: ");
 		rc = dhcp(ret_buffer, &fn_ip,
 			  obp_tftp_args.bootp_retries, F_IPV6);
 		break;
 	case IP_INIT_IPV6_MANUAL:
-		set_ipv6_address(fn_ip.fd, &obp_tftp_args.ci6addr);
+		if (memcmp(&obp_tftp_args.ci6addr, null_ip6, 16)) {
+			set_ipv6_address(fn_ip.fd, &obp_tftp_args.ci6addr);
+		} else {
+			/*
+			 * If no client address has been specified, then
+			 * use a link-local or stateless autoconfig address
+			 */
+			set_ipv6_address(fn_ip.fd, NULL);
+			memcpy(&fn_ip.own_ip6, get_ipv6_address(), 16);
+		}
 		break;
 	case IP_INIT_DEFAULT:
-		printf("  Requesting IP address via DHCP: ");
 		rc = dhcp(ret_buffer, &fn_ip, obp_tftp_args.bootp_retries, 0);
 		break;
 	case IP_INIT_NONE:
@@ -548,10 +569,15 @@ netboot(int argc, char *argv[])
 		return -101;
 	}
 
-	if(ip_version == 4)
-		printf("%d.%d.%d.%d\n",
+	if (ip_version == 4) {
+		printf("  Using IPv4 address: %d.%d.%d.%d\n",
 			((fn_ip.own_ip >> 24) & 0xFF), ((fn_ip.own_ip >> 16) & 0xFF),
 			((fn_ip.own_ip >>  8) & 0xFF), ( fn_ip.own_ip        & 0xFF));
+	} else if (ip_version == 6) {
+		char ip6_str[40];
+		ipv6_to_str(fn_ip.own_ip6.addr, ip6_str);
+		printf("  Using IPv6 address: %s\n", ip6_str);
+	}
 
 	if (rc == -2) {
 		sprintf(buf,
@@ -818,7 +844,7 @@ int parse_tftp_args(char buffer[], char *server_ip, char filename[], int fd,
 		tmp = raw + 7;
 		tmp[j] = '\0';
 		strcpy(domainname, tmp);
-		if (dns_get_ip(fd, (int8_t *)domainname, server_ip6, 6) == 0) {
+		if (dns_get_ip(fd, domainname, server_ip6, 6) == 0) {
 			printf("\n DNS failed for IPV6\n");
                         return -1;
                 }
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/bootp.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/bootp.c
index 1bc6efe5b..6d58cef7d 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/bootp.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/bootp.c
@@ -232,7 +232,7 @@ bootp(char *ret_buffer, filename_ip_t * fn_ip, unsigned int retries)
 	int i = (int) retries+1;
 	fn_ip->own_ip = 0;
 
-	printf("   ");
+	printf("  Requesting IP address via BOOTP:    ");
 
 	response_buffer = ret_buffer;
 
@@ -249,6 +249,7 @@ bootp(char *ret_buffer, filename_ip_t * fn_ip, unsigned int retries)
 		 * in case the previous one was lost. And because we don't
 		 * trust the network cable we keep on doing this 30 times */
 	} while (receive_bootp(fn_ip) != 0);
-	printf("\b\b\b");
+
+	printf("\b\b\bdone\n");
 	return 0;
 }
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcp.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcp.c
index 5f26f3afb..7e2e88ccf 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcp.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcp.c
@@ -11,7 +11,7 @@
  *****************************************************************************/
 
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ALGORITHMS <<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/
+/******************************* ALGORITHMS ******************************/
 
 /** \file dhcp.c <pre>
  * **************** State-transition diagram for DHCP client  *************
@@ -41,13 +41,14 @@
  * </pre> */
 
 
-/*>>>>>>>>>>>>>>>>>>>>> DEFINITIONS & DECLARATIONS <<<<<<<<<<<<<<<<<<<<<<*/
+/********************** DEFINITIONS & DECLARATIONS ***********************/
 
 #include <dhcp.h>
 #include <ethernet.h>
 #include <ipv4.h>
 #include <udp.h>
 #include <dns.h>
+#include <netapps/args.h>
 
 #include <stdio.h>
 #include <string.h>
@@ -110,11 +111,11 @@ static uint8_t dhcp_magic[] = {0x63, 0x82, 0x53, 0x63};
  *  If flag[i] == TRUE then field for i-th option retains valid value and
  *  information from this field may retrived (in case of receiving) or will
  *  be transmitted (in case of transmitting).
- *  
+ *
  */
 typedef struct {
 	uint8_t    flag[256];         /**< Show if corresponding opt. is valid */
-	uint8_t    request_list[256]; /**< o.55 If i-th member is TRUE, then i-th  
+	uint8_t    request_list[256]; /**< o.55 If i-th member is TRUE, then i-th
 	                                  option will be requested from server */
 	uint32_t   server_ID;         /**< o.54 Identifies DHCP-server         */
 	uint32_t   requested_IP;      /**< o.50 Must be filled in DHCP-Request */
@@ -132,65 +133,57 @@ typedef struct {
 static uint8_t dhcp_state;
 
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>> PROTOTYPES <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/
-
-static int32_t
-dhcp_attempt(int fd);
+/***************************** PROTOTYPES ********************************/
 
-static int32_t
-dhcp_encode_options(uint8_t * opt_field, dhcp_options_t * opt_struct);
+static int32_t dhcp_attempt(int fd);
 
-static int32_t
-dhcp_decode_options(uint8_t opt_field[], uint32_t opt_len,
-                    dhcp_options_t * opt_struct);
+static int32_t dhcp_encode_options(uint8_t * opt_field, dhcp_options_t * opt_struct);
 
-static int8_t
-dhcp_merge_options(uint8_t dst_options[], uint32_t * dst_len,
-                   uint8_t src_options[], uint32_t src_len);
+static int32_t dhcp_decode_options(uint8_t opt_field[], uint32_t opt_len,
+				   dhcp_options_t * opt_struct);
 
-static int8_t
-dhcp_find_option(uint8_t options[], uint32_t len,
-                 uint8_t op_code, uint32_t * op_offset);
-
-static void
-dhcp_append_option(uint8_t dst_options[], uint32_t * dst_len,
-                   uint8_t * new_option);
+static int8_t dhcp_merge_options(uint8_t dst_options[], uint32_t * dst_len,
+				 uint8_t src_options[], uint32_t src_len);
 
-static void
-dhcp_combine_option(uint8_t dst_options[], uint32_t * dst_len,
-                    uint32_t dst_offset, uint8_t * new_option);
+static int8_t dhcp_find_option(uint8_t options[], uint32_t len,
+			       uint8_t op_code, uint32_t * op_offset);
 
-static void
-dhcp_send_discover(int fd);
+static void dhcp_append_option(uint8_t dst_options[], uint32_t * dst_len,
+			       uint8_t * new_option);
 
-static void
-dhcp_send_request(int fd);
+static void dhcp_combine_option(uint8_t dst_options[], uint32_t * dst_len,
+				uint32_t dst_offset, uint8_t * new_option);
 
-static uint8_t
-strtoip(int8_t * str, uint32_t * ip);
+static void dhcp_send_discover(int fd);
 
+static void dhcp_send_request(int fd);
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>> LOCAL VARIABLES <<<<<<<<<<<<<<<<<<<<<<<<<<*/
+/***************************** LOCAL VARIABLES ***************************/
 
 static uint8_t  ether_packet[ETH_MTU_SIZE];
 static uint32_t dhcp_own_ip        = 0;
 static uint32_t dhcp_server_ip     = 0;
 static uint32_t dhcp_siaddr_ip     = 0;
-static int8_t   dhcp_filename[256];
-static int8_t   dhcp_tftp_name[256];
+static char   dhcp_filename[256];
+static char   dhcp_tftp_name[256];
+static uint32_t dhcp_xid;
 
 static char   * response_buffer;
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>> IMPLEMENTATION <<<<<<<<<<<<<<<<<<<<<<<<<<<*/
+/***************************** IMPLEMENTATION ****************************/
 
-int32_t
-dhcpv4(char *ret_buffer, filename_ip_t * fn_ip) {
+void dhcpv4_generate_transaction_id(void)
+{
+	dhcp_xid = (rand() << 16) ^ rand();
+}
 
+int32_t dhcpv4(char *ret_buffer, filename_ip_t *fn_ip)
+{
 	uint32_t dhcp_tftp_ip     = 0;
 	int fd = fn_ip->fd;
 
-	strcpy((char *) dhcp_filename, "");
-	strcpy((char *) dhcp_tftp_name, "");
+	strcpy(dhcp_filename, "");
+	strcpy(dhcp_tftp_name, "");
 
 	response_buffer = ret_buffer;
 
@@ -204,11 +197,11 @@ dhcpv4(char *ret_buffer, filename_ip_t * fn_ip) {
 		dhcp_siaddr_ip = fn_ip->server_ip;
 	}
 	if(fn_ip->filename[0] != 0) {
-		strcpy((char *) dhcp_filename, (char *) fn_ip->filename);
+		strcpy(dhcp_filename, (char *) fn_ip->filename);
 	}
 
 	// TFTP SERVER
-	if (!strlen((char *) dhcp_tftp_name)) {
+	if (!strlen(dhcp_tftp_name)) {
 		if (!dhcp_siaddr_ip) {
 			// ERROR: TFTP name is not presented
 			return -3;
@@ -219,9 +212,9 @@ dhcpv4(char *ret_buffer, filename_ip_t * fn_ip) {
 	}
 	else {
 		// TFTP server defined by its name
-		if (!strtoip(dhcp_tftp_name, &(dhcp_tftp_ip))) {
-			if (!dns_get_ip(fd, dhcp_tftp_name, (uint8_t *)&(dhcp_tftp_ip), 4)) {
-				// DNS error - can't obtain TFTP-server name  
+		if (!strtoip(dhcp_tftp_name, (char *)&dhcp_tftp_ip)) {
+			if (!dns_get_ip(fd, dhcp_tftp_name, (uint8_t *)&dhcp_tftp_ip, 4)) {
+				// DNS error - can't obtain TFTP-server name
 				// Use TFTP-ip from siaddr field, if presented
 				if (dhcp_siaddr_ip) {
 					dhcp_tftp_ip = dhcp_siaddr_ip;
@@ -237,7 +230,7 @@ dhcpv4(char *ret_buffer, filename_ip_t * fn_ip) {
 	// Store configuration info into filename_ip strucutre
 	fn_ip -> own_ip = dhcp_own_ip;
 	fn_ip -> server_ip = dhcp_tftp_ip;
-	strcpy((char *) fn_ip -> filename, (char *) dhcp_filename);
+	strcpy((char *) fn_ip -> filename, dhcp_filename);
 
 	return 0;
 }
@@ -245,8 +238,8 @@ dhcpv4(char *ret_buffer, filename_ip_t * fn_ip) {
 /**
  * DHCP: Tries o obtain DHCP parameters, refer to state-transition diagram
  */
-static int32_t
-dhcp_attempt(int fd) {
+static int32_t dhcp_attempt(int fd)
+{
 	int sec;
 
 	// Send DISCOVER message and switch DHCP-client to SELECT state
@@ -270,7 +263,7 @@ dhcp_attempt(int fd) {
 		} while (get_timer() > 0);
 	}
 
-	// timeout 
+	// timeout
 	return 0;
 }
 
@@ -278,7 +271,7 @@ dhcp_attempt(int fd) {
  * DHCP: Supplements DHCP-message with options stored in structure.
  *       For more information about option coding see dhcp_options_t.
  *
- * @param  opt_field     Points to the "vend" field of DHCP-message  
+ * @param  opt_field     Points to the "vend" field of DHCP-message
  *                       (destination)
  * @param  opt_struct    this structure stores info about the options which
  *                       will be added to DHCP-message (source)
@@ -286,8 +279,8 @@ dhcp_attempt(int fd) {
  *                       FALSE - error condition occurs.
  * @see                  dhcp_options_t
  */
-static int32_t
-dhcp_encode_options(uint8_t * opt_field, dhcp_options_t * opt_struct) {
+static int32_t dhcp_encode_options(uint8_t * opt_field, dhcp_options_t * opt_struct)
+{
 	uint8_t * options = opt_field;
 	uint16_t i, sum; // used to define is any options set
 
@@ -380,7 +373,7 @@ dhcp_encode_options(uint8_t * opt_field, dhcp_options_t * opt_struct) {
  * DHCP: Extracts encoded options from DHCP-message into the structure.
  *       For more information about option coding see dhcp_options_t.
  *
- * @param  opt_field     Points to the "options" field of DHCP-message  
+ * @param  opt_field     Points to the "options" field of DHCP-message
  *                       (source).
  * @param  opt_len       Length of "options" field.
  * @param  opt_struct    this structure stores info about the options which
@@ -389,10 +382,10 @@ dhcp_encode_options(uint8_t * opt_field, dhcp_options_t * opt_struct) {
  *                       FALSE - error condition occurs.
  * @see                  dhcp_options_t
  */
-static int32_t
-dhcp_decode_options(uint8_t opt_field[], uint32_t opt_len,
-                    dhcp_options_t * opt_struct) {
-	int32_t offset = 0;
+static int32_t dhcp_decode_options(uint8_t opt_field[], uint32_t opt_len,
+				   dhcp_options_t * opt_struct)
+{
+	uint32_t offset = 0;
 
 	memset(opt_struct, 0, sizeof(dhcp_options_t));
 
@@ -407,30 +400,30 @@ dhcp_decode_options(uint8_t opt_field[], uint32_t opt_len,
 		switch(opt_field[offset]) {
 		case DHCP_OVERLOAD :
 			opt_struct -> overload = opt_field[offset + 2];
-			offset += 2 + opt_field[offset + 1]; 
+			offset += 2 + opt_field[offset + 1];
 			break;
 
 		case DHCP_REQUESTED_IP :
 			opt_struct -> requested_IP = htonl(* (uint32_t *) (opt_field + offset + 2));
-			offset += 2 + opt_field[offset + 1]; 
+			offset += 2 + opt_field[offset + 1];
 			break;
 
 		case DHCP_MASK :
 			opt_struct -> flag[DHCP_MASK] = 1;
 			opt_struct -> subnet_mask = htonl(* (uint32_t *) (opt_field + offset + 2));
-			offset += 2 + opt_field[offset + 1]; 
+			offset += 2 + opt_field[offset + 1];
 			break;
 
 		case DHCP_DNS :
 			opt_struct -> flag[DHCP_DNS] = 1;
 			opt_struct -> dns_IP = htonl(* (uint32_t *) (opt_field + offset + 2));
-			offset += 2 + opt_field[offset + 1]; 
+			offset += 2 + opt_field[offset + 1];
 			break;
 
 		case DHCP_ROUTER :
 			opt_struct -> flag[DHCP_ROUTER] = 1;
 			opt_struct -> router_IP = htonl(* (uint32_t *) (opt_field + offset + 2));
-			offset += 2 + opt_field[offset + 1]; 
+			offset += 2 + opt_field[offset + 1];
 			break;
 
 		case DHCP_MSG_TYPE :
@@ -492,11 +485,12 @@ dhcp_decode_options(uint8_t opt_field[], uint32_t opt_len,
  *                       FALSE - error condition occurs.
  */
 static int8_t dhcp_merge_options(uint8_t dst_options[], uint32_t * dst_len,
-                                 uint8_t src_options[], uint32_t src_len) {
-	int32_t dst_offset, src_offset = 0;
+				 uint8_t src_options[], uint32_t src_len)
+{
+	uint32_t dst_offset, src_offset = 0;
 
 	// remove ENDOPT if presented
-	if (dhcp_find_option(dst_options, * dst_len, DHCP_ENDOPT, (uint32_t *) &dst_offset))
+	if (dhcp_find_option(dst_options, * dst_len, DHCP_ENDOPT, &dst_offset))
 		* dst_len = dst_offset;
 
 	while (src_offset < src_len) {
@@ -509,7 +503,7 @@ static int8_t dhcp_merge_options(uint8_t dst_options[], uint32_t * dst_len,
 		default:
 			if (dhcp_find_option(dst_options, * dst_len,
 			                     src_options[src_offset],
-			                     (uint32_t *) &dst_offset)) {
+			                     &dst_offset)) {
 				dhcp_combine_option(dst_options, dst_len,
 				                    dst_offset,
 				                    (uint8_t *) src_options +
@@ -522,7 +516,7 @@ static int8_t dhcp_merge_options(uint8_t dst_options[], uint32_t * dst_len,
 		}
 	}
 
-	if (src_offset == src_len) 
+	if (src_offset == src_len)
 		return 1;
 	return 0;
 }
@@ -540,7 +534,8 @@ static int8_t dhcp_merge_options(uint8_t dst_options[], uint32_t * dst_len,
  *                       FALSE - option wasn't find.
  */
 static int8_t dhcp_find_option(uint8_t options[], uint32_t len,
-                               uint8_t op_code, uint32_t * op_offset) {
+			       uint8_t op_code, uint32_t * op_offset)
+{
 	uint32_t srch_offset = 0;
 	* op_offset = 0;
 
@@ -568,9 +563,9 @@ static int8_t dhcp_find_option(uint8_t options[], uint32_t len,
  * @param  dst_len       length of the "options" field (modified)
  * @param  new_option    points to an option in another list (src)
  */
-static void
-dhcp_append_option(uint8_t dst_options[], uint32_t * dst_len,
-                   uint8_t * new_option) {
+static void dhcp_append_option(uint8_t dst_options[], uint32_t * dst_len,
+			       uint8_t * new_option)
+{
 	memcpy(dst_options + ( * dst_len), new_option, 2 + (* (new_option + 1)));
 	* dst_len += 2 + *(new_option + 1);
 }
@@ -586,10 +581,9 @@ dhcp_append_option(uint8_t dst_options[], uint32_t * dst_len,
  * @param  dst_offset    offset of the option from beginning of the list
  * @param  new_option    points to an option in another list (src)
  */
-static void
-dhcp_combine_option(uint8_t dst_options[], uint32_t * dst_len,
-                    uint32_t dst_offset, uint8_t * new_option) {
-
+static void dhcp_combine_option(uint8_t dst_options[], uint32_t * dst_len,
+				uint32_t dst_offset, uint8_t * new_option)
+{
 	uint8_t tmp_buffer[1024]; // use to provide safe memcpy
 	uint32_t tail_len;
 
@@ -612,8 +606,8 @@ dhcp_combine_option(uint8_t dst_options[], uint32_t * dst_len,
 /**
  * DHCP: Sends DHCP-Discover message. Looks for DHCP servers.
  */
-static void
-dhcp_send_discover(int fd) {
+static void dhcp_send_discover(int fd)
+{
 	uint32_t packetsize = sizeof(struct iphdr) +
 	                      sizeof(struct udphdr) + sizeof(struct btphdr);
 	struct btphdr *btph;
@@ -627,6 +621,7 @@ dhcp_send_discover(int fd) {
 	btph -> op = 1;
 	btph -> htype = 1;
 	btph -> hlen = 6;
+	btph -> xid = dhcp_xid;
 	memcpy(btph -> chaddr, get_mac_address(), 6);
 
 	memset(&opt, 0, sizeof(dhcp_options_t));
@@ -655,8 +650,8 @@ dhcp_send_discover(int fd) {
 /**
  * DHCP: Sends DHCP-Request message. Asks for acknowledgment to occupy IP.
  */
-static void
-dhcp_send_request(int fd) {
+static void dhcp_send_request(int fd)
+{
 	uint32_t packetsize = sizeof(struct iphdr) +
 	                      sizeof(struct udphdr) + sizeof(struct btphdr);
 	struct btphdr *btph;
@@ -670,6 +665,7 @@ dhcp_send_request(int fd) {
 	btph -> op = 1;
 	btph -> htype = 1;
 	btph -> hlen = 6;
+	btph -> xid = dhcp_xid;
 	memcpy(btph -> chaddr, get_mac_address(), 6);
 
 	memset(&opt, 0, sizeof(dhcp_options_t));
@@ -704,7 +700,8 @@ dhcp_send_request(int fd) {
 /**
  * DHCP: Sends DHCP-Release message. Releases occupied IP.
  */
-void dhcp_send_release(int fd) {
+void dhcp_send_release(int fd)
+{
 	uint32_t packetsize = sizeof(struct iphdr) +
 	                      sizeof(struct udphdr) + sizeof(struct btphdr);
 	struct btphdr *btph;
@@ -718,6 +715,7 @@ void dhcp_send_release(int fd) {
 	btph -> op = 1;
 	btph -> htype = 1;
 	btph -> hlen = 6;
+	btph -> xid = dhcp_xid;
 	strcpy((char *) btph -> file, "");
 	memcpy(btph -> chaddr, get_mac_address(), 6);
 	btph -> ciaddr = htonl(dhcp_own_ip);
@@ -730,7 +728,7 @@ void dhcp_send_release(int fd) {
 
 	dhcp_encode_options(btph -> vend, &opt);
 
-	fill_udphdr(&ether_packet[sizeof(struct iphdr)], 
+	fill_udphdr(&ether_packet[sizeof(struct iphdr)],
 	            sizeof(struct btphdr) + sizeof(struct udphdr),
 	            UDPPORT_BOOTPC, UDPPORT_BOOTPS);
 	fill_iphdr(ether_packet, sizeof(struct btphdr) +
@@ -753,18 +751,21 @@ void dhcp_send_release(int fd) {
  * @see               btphdr
  */
 
-int8_t
-handle_dhcp(int fd, uint8_t * packet, int32_t packetsize) {
+int8_t handle_dhcp(int fd, uint8_t * packet, int32_t packetsize)
+{
 	struct btphdr * btph;
 	struct iphdr * iph;
 	dhcp_options_t opt;
 
-	memset(&opt, 0, sizeof(dhcp_options_t));  
+	memset(&opt, 0, sizeof(dhcp_options_t));
 	btph = (struct btphdr *) packet;
 	iph = (struct iphdr *) packet - sizeof(struct udphdr) -
 	      sizeof(struct iphdr);
-	if (btph -> op != 2)
-		return -1; // it is not Boot Reply
+
+	if (btph->op != 2)
+		return -1;		/* It is not a Bootp/DHCP reply */
+	if (btph->xid != dhcp_xid)
+		return -1;		/* The transaction ID does not match */
 
 	if (memcmp(btph -> vend, dhcp_magic, 4)) {
 		// It is BootP - RFC 951
@@ -788,7 +789,7 @@ handle_dhcp(int fd, uint8_t * packet, int32_t packetsize) {
 	}
 
 
-	// decode options  
+	// decode options
 	if (!dhcp_decode_options(btph -> vend, packetsize -
 	                         sizeof(struct btphdr) + sizeof(btph -> vend),
 	                         &opt)) {
@@ -902,7 +903,7 @@ handle_dhcp(int fd, uint8_t * packet, int32_t packetsize) {
 				else {
 					strcpy((char *) dhcp_filename, "");
 					if (opt.overload != DHCP_OVERLOAD_FILE &&
-						opt.overload != DHCP_OVERLOAD_BOTH && 
+						opt.overload != DHCP_OVERLOAD_BOTH &&
 						strlen((char *) btph -> file)) {
 						strncpy((char *) dhcp_filename,
 						        (char *) btph->file,
@@ -952,47 +953,3 @@ handle_dhcp(int fd, uint8_t * packet, int32_t packetsize) {
 
 	return 0;
 }
-
-/**
- * DHCP: Converts "255.255.255.255" -> 32-bit long IP
- *
- * @param  str        string to be converted
- * @param  ip         in case of SUCCESS - 32-bit long IP
-                      in case of FAULT - zero
- * @return            TRUE - IP converted successfully;
- *                    FALSE - error condition occurs (e.g. bad format)
- */
-static uint8_t
-strtoip(int8_t * str, uint32_t * ip) {
-	int8_t ** ptr = &str;
-	int16_t i = 0, res, len;
-	char octet[256];
-
-	* ip = 0;
-
-	while (**ptr != 0) {
-		if (i > 3 || !isdigit(**ptr))
-			return 0;
-		if (strstr((char *) * ptr, ".") != NULL) {
-			len = (int16_t) ((int8_t *) strstr((char *) * ptr, ".") - 
-			      (int8_t *) (* ptr));
-			strncpy(octet, (char *) * ptr, len); octet[len] = 0;
-			* ptr += len;
-		}
-		else {
-			strcpy(octet, (char *) * ptr);
-			* ptr += strlen(octet);
-		}
-		res = strtol(octet, NULL, 10);
-		if ((res > 255) || (res < 0))
-			return 0;
-		* ip = ((* ip) << 8) + res;
-		i++;
-		if (** ptr == '.')
-			(*ptr)++;
-	}
-
-	if (i != 4)
-		return 0;
-	return 1;
-}
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcp.h b/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcp.h
index 69dd49d4a..54fb1eed3 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcp.h
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcp.h
@@ -43,6 +43,7 @@ struct btphdr {
 	uint8_t vend[64];    /**< Optional parameters field (DHCP-options)     */
 };
 
+void dhcpv4_generate_transaction_id(void);
 int bootp(char *ret_buffer, filename_ip_t *, unsigned int);
 int dhcpv4(char *ret_buffer, filename_ip_t *);
 void dhcp_send_release(int fd);
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcpv6.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcpv6.c
index 4deef30f2..d0a22d555 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcpv6.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcpv6.c
@@ -27,13 +27,15 @@ static uint8_t tid[3];
 static uint32_t dhcpv6_state = -1;
 static filename_ip_t *my_fn_ip;
 
-static void
-generate_transaction_id(void)
+static struct ip6addr_list_entry all_dhcpv6_ll; /* All DHCPv6 servers address */
+
+void
+dhcpv6_generate_transaction_id(void)
 {
-	/* TODO: as per RFC 3315 transaction IDs should be generated randomly */
-	tid[0] = 1;
-	tid[1] = 2;
-	tid[2] = 4;
+	/* As per RFC 3315 transaction IDs should be generated randomly */
+	tid[0] = rand();
+	tid[1] = rand();
+	tid[2] = rand();
 }
 
 static void
@@ -45,8 +47,6 @@ send_info_request(int fd)
 
 	memset(ether_packet, 0, ETH_MTU_SIZE);
 
-	generate_transaction_id();
-
 	/* Get an IPv6 packet */
 	payload_length = sizeof(struct udphdr) + sizeof(struct dhcp_message_header);
 	fill_ip6hdr (ether_packet + sizeof(struct ethhdr),
@@ -72,16 +72,14 @@ send_info_request(int fd)
 	dhcph->option.el_time.length = 2;
 	dhcph->option.el_time.time = 0x190; /* 4000 ms */
 	dhcph->option.option_request_option.code = DHCPV6_OPTION_ORO;
-	dhcph->option.option_request_option.length= 6;
+	dhcph->option.option_request_option.length = DHCPV6_OPTREQUEST_NUMOPTS * 2;
 	dhcph->option.option_request_option.option_code[0] = DHCPV6_OPTION_DNS_SERVERS;
 	dhcph->option.option_request_option.option_code[1] = DHCPV6_OPTION_DOMAIN_LIST;
 	dhcph->option.option_request_option.option_code[2] = DHCPV6_OPTION_BOOT_URL;
 
-
 	send_ipv6(fd, ether_packet + sizeof(struct ethhdr),
-	         sizeof(struct ethhdr)+ sizeof(struct ip6hdr)
-		 + sizeof(struct udphdr)
-	         + sizeof( struct dhcp_message_header) );
+		  sizeof(struct ip6hdr) + sizeof(struct udphdr)
+		  + sizeof(struct dhcp_message_header));
 }
 
 static int32_t
@@ -119,6 +117,9 @@ dhcpv6 ( char *ret_buffer, void *fn_ip)
 {
 	int fd;
 
+	all_dhcpv6_ll.addr.part.prefix = 0xff02000000000000ULL;
+	all_dhcpv6_ll.addr.part.interface_id = 0x10002ULL;
+
 	my_fn_ip = (filename_ip_t *) fn_ip;
 	fd = my_fn_ip->fd;
 
@@ -129,8 +130,7 @@ dhcpv6 ( char *ret_buffer, void *fn_ip)
 	return 0;
 }
 
-static struct dhcp6_received_options *
-dhcp6_process_options (uint8_t *option, int32_t option_length)
+static void dhcp6_process_options (uint8_t *option, int32_t option_length)
 {
 	struct dhcp_boot_url *option_boot_url;
 	struct client_identifier *option_clientid;
@@ -138,24 +138,19 @@ dhcp6_process_options (uint8_t *option, int32_t option_length)
 	struct dhcp_dns *option_dns;
 	struct dhcp_dns_list *option_dns_list;
 	struct dhcp6_gen_option *option_gen;
-	struct dhcp6_received_options *received_options;
 	char buffer[256];
 
-
-	received_options = malloc (sizeof(struct dhcp6_received_options));
 	while (option_length > 0) {
 		switch ((uint16_t) *(option+1)) {
 		case DHCPV6_OPTION_CLIENTID:
 			option_clientid = (struct client_identifier *) option;
 			option = option +  option_clientid->length + 4;
 			option_length = option_length - option_clientid->length - 4;
-			received_options->client_id = 1;
 			break;
 		case DHCPV6_OPTION_SERVERID:
 			option_serverid = (struct server_identifier *) option;
 			option = option +  option_serverid->length + 4;
 			option_length = option_length - option_serverid->length - 4;
-			received_options->server_id = 1;
 			break;
 		case DHCPV6_OPTION_DNS_SERVERS:
 			option_dns = (struct dhcp_dns *) option;
@@ -184,7 +179,7 @@ dhcp6_process_options (uint8_t *option, int32_t option_length)
 					    (char *)my_fn_ip->filename,
 					    (int)my_fn_ip->fd,
 					    option_boot_url->length) == -1)
-				return NULL;
+				return;
 			break;
 		default:
 			option_gen = (struct dhcp6_gen_option *) option;
@@ -192,8 +187,6 @@ dhcp6_process_options (uint8_t *option, int32_t option_length)
 			option_length = option_length - option_gen->length - 4;
 		}
 	}
-
-	return received_options;
 }
 
 uint32_t
@@ -205,6 +198,9 @@ handle_dhcpv6(uint8_t * packet, int32_t packetsize)
 	struct dhcp_message_reply *reply;
 	reply = (struct dhcp_message_reply *) packet;
 
+	if (memcmp(reply->transaction_id, tid, 3))
+		return -1;			/* Wrong transaction ID */
+
 	if (reply->type == 7)
 		dhcpv6_state = DHCP_STATUSCODE_SUCCESS;
 
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcpv6.h b/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcpv6.h
index 078a9f11f..fb77da648 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcpv6.h
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/dhcpv6.h
@@ -77,6 +77,7 @@
 #define DUID_LL		3 /* DUID based on Link-layer Address */
 
 /* Prototypes */
+void dhcpv6_generate_transaction_id(void);
 int32_t dhcpv6 ( char *ret_buffer, void *fn_ip);
 uint32_t handle_dhcpv6(uint8_t * , int32_t);
 
@@ -102,6 +103,8 @@ struct server_identifier {
 	uint8_t mac[6];
 };
 
+#define DHCPV6_OPTREQUEST_NUMOPTS 3
+
 struct dhcp_info_request {
 	struct client_identifier client_id;
 	struct elapsed_time {
@@ -112,7 +115,7 @@ struct dhcp_info_request {
 	struct option_request {
 		uint16_t code;
 		uint16_t length;
-		uint16_t option_code[5];
+		uint16_t option_code[DHCPV6_OPTREQUEST_NUMOPTS];
 	} option_request_option;
 };
 
@@ -141,12 +144,6 @@ struct dhcp_boot_url {
 	uint8_t url[256];
 };
 
-struct dhcp6_received_options {
-	uint8_t filename;
-	uint8_t ip;
-	uint8_t client_id;
-	uint8_t server_id;
-};
 struct dhcp_message_reply {
 	uint8_t type;			    /* Message type   */
 	uint8_t transaction_id[3];          /* Transaction id */
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/dns.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/dns.c
index 0ab1346c9..a5a36a18e 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/dns.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/dns.c
@@ -133,7 +133,7 @@ dns_init(uint32_t _dns_server_ip, uint8_t _dns_server_ipv6[16], uint8_t ip_versi
  *                   FALSE - error condition occurs.
  */
 int8_t
-dns_get_ip(int fd, int8_t * url, uint8_t * domain_ip, uint8_t ip_version)
+dns_get_ip(int fd, char* url, uint8_t * domain_ip, uint8_t ip_version)
 {
 	/* this counter is used so that we abort after 30 DNS request */
 	int32_t i;
@@ -143,7 +143,7 @@ dns_get_ip(int fd, int8_t * url, uint8_t * domain_ip, uint8_t ip_version)
 	(* domain_ip) = 0;
 
 	// Retrieve host name from URL
-	if (!urltohost((char *) url, (char *) host_name)) {
+	if (!urltohost(url, (char *) host_name)) {
 		printf("\nERROR:\t\t\tBad URL!\n");
 		return 0;
 	}
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/dns.h b/qemu/roms/SLOF/clients/net-snk/app/netlib/dns.h
index 82eea4e4d..b8756afca 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/dns.h
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/dns.h
@@ -20,7 +20,7 @@
 extern int8_t dns_init(uint32_t _dns_server_ip, uint8_t _dns_server_ipv6[16], uint8_t ip_version);
 
 /* For given URL retrieves IPv4 from DNS-server. */
-extern int8_t dns_get_ip(int fd, int8_t * url, uint8_t * domain_ip, uint8_t ip_version);
+extern int8_t dns_get_ip(int fd, char * url, uint8_t * domain_ip, uint8_t ip_version);
 
 /* Handles DNS-packets, which are detected by receive_ether. */
 extern int32_t handle_dns(uint8_t * packet, int32_t packetsize);
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/ethernet.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/ethernet.c
index bbfd6d1c3..1e03a0bf3 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/ethernet.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/ethernet.c
@@ -11,7 +11,7 @@
  *****************************************************************************/
 
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ALGORITHMS <<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/
+/******************************* ALGORITHMS ******************************/
 
 /** \file netbase.c <pre>
  * *********************** Receive-handle diagram *************************
@@ -36,12 +36,12 @@
  *  | APPLICATION        +----------------+-----------+
  *  V                    |                            |
  * upper               DNS (handle_dns)      BootP / DHCP (handle_bootp_client)
- * 
+ *
  * ************************************************************************
  * </pre> */
 
 
-/*>>>>>>>>>>>>>>>>>>>>>>> DEFINITIONS & DECLARATIONS <<<<<<<<<<<<<<<<<<<<*/
+/************************ DEFINITIONS & DECLARATIONS *********************/
 
 #include <ethernet.h>
 #include <string.h>
@@ -50,22 +50,22 @@
 #include <ipv6.h>
 
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> LOCAL VARIABLES <<<<<<<<<<<<<<<<<<<<<<<<<*/
+/****************************** LOCAL VARIABLES **************************/
 
 static uint8_t ether_packet[ETH_MTU_SIZE];
 static uint8_t own_mac[6] = {0, 0, 0, 0, 0, 0};
 static uint8_t multicast_mac[] = {0x01, 0x00, 0x5E};
 static const uint8_t broadcast_mac[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> IMPLEMENTATION <<<<<<<<<<<<<<<<<<<<<<<<<<*/
+/****************************** IMPLEMENTATION ***************************/
 
 /**
  * Ethernet: Set the own MAC address to initializes ethernet layer.
  *
  * @param  own_mac  own hardware-address (MAC)
  */
-void
-set_mac_address(const uint8_t * _own_mac) {
+void set_mac_address(const uint8_t * _own_mac)
+{
 	if (_own_mac)
 		memcpy(own_mac, _own_mac, 6);
 	else
@@ -77,19 +77,19 @@ set_mac_address(const uint8_t * _own_mac) {
  *
  * @return  own hardware-address (MAC)
  */
-const uint8_t *
-get_mac_address(void) {
+const uint8_t *get_mac_address(void)
+{
 	return own_mac;
 }
 
 /**
  * Ethernet: Check if given multicast address is a multicast MAC address
- *           starting with 0x3333 
+ *           starting with 0x3333
  *
- * @return  true or false 
+ * @return  true or false
  */
-static uint8_t
-is_multicast_mac(uint8_t * mac) {
+static uint8_t is_multicast_mac(uint8_t * mac)
+{
 
     	uint16_t mc = 0x3333;
     	if (memcmp(mac, &mc, 2) == 0)
@@ -98,7 +98,6 @@ is_multicast_mac(uint8_t * mac) {
 	return 0;
 }
 
-
 /**
  * Ethernet: Receives an ethernet-packet and handles it according to
  *      Receive-handle diagram.
@@ -107,8 +106,8 @@ is_multicast_mac(uint8_t * mac) {
  * @return  ZERO - packet was handled or no packets received;
  *          NON ZERO - error condition occurs.
  */
-int32_t
-receive_ether(int fd) {
+int32_t receive_ether(int fd)
+{
 	int32_t bytes_received;
 	struct ethhdr * ethh;
 
@@ -118,7 +117,10 @@ receive_ether(int fd) {
 	if (!bytes_received) // No messages
 		return 0;
 
-	if (bytes_received < sizeof(struct ethhdr))
+	if (bytes_received < 0)
+		return -1; /* recv() failed */
+
+	if ((size_t) bytes_received < sizeof(struct ethhdr))
 		return -1; // packet is too small
 
 	ethh = (struct ethhdr *) ether_packet;
@@ -176,9 +178,9 @@ send_ether(int fd, void* buffer, int len)
  * @see                fill_dnshdr
  * @see                fill_btphdr
  */
-void
-fill_ethhdr(uint8_t * packet, uint16_t eth_type,
-            const uint8_t * src_mac, const uint8_t * dest_mac) {
+void fill_ethhdr(uint8_t * packet, uint16_t eth_type,
+		 const uint8_t * src_mac, const uint8_t * dest_mac)
+{
 	struct ethhdr * ethh = (struct ethhdr *) packet;
 
 	ethh -> type = htons(eth_type);
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/icmpv6.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/icmpv6.c
index be6cc110f..c104f7015 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/icmpv6.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/icmpv6.c
@@ -40,9 +40,8 @@ send_router_solicitation (int fd)
 			  sizeof(struct ip6hdr));
 
 	/* Destination is "All routers multicast address" (link-local) */
-	dest_addr.part.prefix       = all_routers_ll.addr.part.prefix;
-	dest_addr.part.interface_id = all_routers_ll.addr.part.interface_id;
-
+	dest_addr.part.prefix       = 0xff02000000000000ULL;
+	dest_addr.part.interface_id = 2;
 
 	/* Fill IPv6 header */
 	fill_ip6hdr (ether_packet + sizeof(struct ethhdr),
@@ -78,8 +77,8 @@ handle_prefixoption (uint8_t *option)
 	prefix_option = (struct option_prefix *) option;
 	memcpy( &(prefix.addr), &(prefix_option->prefix.addr), IPV6_ADDR_LENGTH);
 
-	/* Link-local adresses in RAs are nonsense                  */
-	if ( (IPV6_LL_PREFIX & (prefix_option->prefix.part.prefix)) == IPV6_LL_PREFIX )
+	/* Link-local adresses in RAs are nonsense */
+	if (ip6_is_linklocal(&prefix))
 		return;
 
 	if (prefix_option->preferred_lifetime > prefix_option->valid_lifetime)
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv4.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv4.c
index 8185de5e1..2b92c77c4 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv4.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv4.c
@@ -11,7 +11,7 @@
  *****************************************************************************/
 
 
-/*>>>>>>>>>>>>>>>>>>>>> DEFINITIONS & DECLARATIONS <<<<<<<<<<<<<<<<<<<<<<*/
+/********************** DEFINITIONS & DECLARATIONS ***********************/
 
 #include <ipv4.h>
 #include <udp.h>
@@ -81,32 +81,26 @@ struct icmphdr {
 	} payload;
 };
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> PROTOTYPES <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/
+/****************************** PROTOTYPES *******************************/
 
-static unsigned short
-checksum(unsigned short *packet, int words);
+static unsigned short checksum(unsigned short *packet, int words);
 
-static void
-arp_send_request(int fd, uint32_t dest_ip);
+static void arp_send_request(int fd, uint32_t dest_ip);
 
-static void
-arp_send_reply(int fd, uint32_t src_ip, uint8_t * src_mac);
+static void arp_send_reply(int fd, uint32_t src_ip, uint8_t * src_mac);
 
-static void
-fill_arphdr(uint8_t * packet, uint8_t opcode,
-            const uint8_t * src_mac, uint32_t src_ip,
-            const uint8_t * dest_mac, uint32_t dest_ip);
+static void fill_arphdr(uint8_t * packet, uint8_t opcode,
+			const uint8_t * src_mac, uint32_t src_ip,
+			const uint8_t * dest_mac, uint32_t dest_ip);
 
-static arp_entry_t*
-lookup_mac_addr(uint32_t ipv4_addr);
+static arp_entry_t *lookup_mac_addr(uint32_t ipv4_addr);
 
-static void
-fill_udp_checksum(struct iphdr *ipv4_hdr);
+static void fill_udp_checksum(struct iphdr *ipv4_hdr);
 
-static int8_t
-handle_icmp(int fd, struct iphdr * iph, uint8_t * packet, int32_t packetsize);
+static int8_t handle_icmp(int fd, struct iphdr * iph, uint8_t * packet,
+			  int32_t packetsize);
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> LOCAL VARIABLES <<<<<<<<<<<<<<<<<<<<<<<<<*/
+/****************************** LOCAL VARIABLES **************************/
 
 /* Routing parameters */
 static uint32_t own_ip       = 0;
@@ -126,18 +120,19 @@ static       uint8_t multicast_mac[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 static unsigned int arp_consumer = 0;
 static unsigned int arp_producer = 0;
 static arp_entry_t  arp_table[ARP_ENTRIES];
-static arp_entry_t  pending_pkt;
+
+static uint8_t pending_pkt_frame[ETH_MTU_SIZE];
+static int pending_pkt_len;
 
 /* Function pointer send_ip. Points either to send_ipv4() or send_ipv6() */
 int   (*send_ip) (int fd, void *, int);
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>> IMPLEMENTATION <<<<<<<<<<<<<<<<<<<<<<<<<<<*/
+/***************************** IMPLEMENTATION ****************************/
 
 /**
  * IPv4: Initialize the environment for the IPv4 layer.
  */
-static void
-ipv4_init(void)
+static void ipv4_init(void)
 {
 	int i;
 
@@ -153,7 +148,7 @@ ipv4_init(void)
 		arp_table[i].pkt_pending = 0;
 	}
 
-	/* Set IP send function to send_ipv4() */ 
+	/* Set IP send function to send_ipv4() */
 	send_ip = &send_ipv4;
 }
 
@@ -162,8 +157,7 @@ ipv4_init(void)
  *
  * @param  _own_ip  client IPv4 address (e.g. 127.0.0.1)
  */
-void
-set_ipv4_address(uint32_t _own_ip)
+void set_ipv4_address(uint32_t _own_ip)
 {
 	own_ip = _own_ip;
 	ipv4_init();
@@ -174,8 +168,7 @@ set_ipv4_address(uint32_t _own_ip)
  *
  * @return client IPv4 address (e.g. 127.0.0.1)
  */
-uint32_t
-get_ipv4_address(void)
+uint32_t get_ipv4_address(void)
 {
 	return own_ip;
 }
@@ -185,8 +178,7 @@ get_ipv4_address(void)
  *
  * @param  _own_ip  multicast IPv4 address (224.0.0.0 - 239.255.255.255)
  */
-void
-set_ipv4_multicast(uint32_t _multicast_ip)
+void set_ipv4_multicast(uint32_t _multicast_ip)
 {
 	// is this IP Multicast out of range (224.0.0.0 - 239.255.255.255)
 	if((htonl(_multicast_ip) < 0xE0000000)
@@ -210,8 +202,7 @@ set_ipv4_multicast(uint32_t _multicast_ip)
  *
  * @return multicast IPv4 address (224.0.0.0 - 239.255.255.255 or 0 if not set)
  */
-uint32_t
-get_ipv4_multicast(void)
+uint32_t get_ipv4_multicast(void)
 {
 	return multicast_ip;
 }
@@ -221,8 +212,7 @@ get_ipv4_multicast(void)
  *
  * @param  _router_ip   router IPv4 address
  */
-void
-set_ipv4_router(uint32_t _router_ip)
+void set_ipv4_router(uint32_t _router_ip)
 {
 	router_ip = _router_ip;
 	ipv4_init();
@@ -233,8 +223,7 @@ set_ipv4_router(uint32_t _router_ip)
  *
  * @return router IPv4 address
  */
-uint32_t
-get_ipv4_router(void)
+uint32_t get_ipv4_router(void)
 {
 	return router_ip;
 }
@@ -244,8 +233,7 @@ get_ipv4_router(void)
  *
  * @param  _subnet_mask   netmask of the own IPv4 address
  */
-void
-set_ipv4_netmask(uint32_t _subnet_mask)
+void set_ipv4_netmask(uint32_t _subnet_mask)
 {
 	subnet_mask = _subnet_mask;
 	ipv4_init();
@@ -256,8 +244,7 @@ set_ipv4_netmask(uint32_t _subnet_mask)
  *
  * @return netmask of the own IPv4 address
  */
-uint32_t
-get_ipv4_netmask(void)
+uint32_t get_ipv4_netmask(void)
 {
 	return subnet_mask;
 }
@@ -280,9 +267,9 @@ get_ipv4_netmask(void)
  * @see                fill_dnshdr
  * @see                fill_btphdr
  */
-void
-fill_iphdr(uint8_t * packet, uint16_t packetsize,
-           uint8_t ip_proto, uint32_t ip_src, uint32_t ip_dst) {
+void fill_iphdr(uint8_t * packet, uint16_t packetsize,
+           uint8_t ip_proto, uint32_t ip_src, uint32_t ip_dst)
+{
 	struct iphdr * iph = (struct iphdr *) packet;
 
 	iph -> ip_hlv = 0x45;
@@ -308,8 +295,7 @@ fill_iphdr(uint8_t * packet, uint16_t packetsize,
  * @see               receive_ether
  * @see               iphdr
  */
-int8_t
-handle_ipv4(int fd, uint8_t * ip_packet, int32_t packetsize)
+int8_t handle_ipv4(int fd, uint8_t * ip_packet, uint32_t packetsize)
 {
 	struct iphdr * iph;
 	int32_t old_sum;
@@ -422,8 +408,7 @@ handle_ipv4(int fd, uint8_t * ip_packet, int32_t packetsize)
  * @see               receive_ether
  * @see               iphdr
  */
-int
-send_ipv4(int fd, void* buffer, int len)
+int send_ipv4(int fd, void* buffer, int len)
 {
 	arp_entry_t *arp_entry = 0;
 	struct iphdr *ip;
@@ -506,13 +491,11 @@ send_ipv4(int fd, void* buffer, int len)
 		arp_entry->pkt_pending = 1;
 		arp_entry->ipv4_addr = ip_dst;
 		memset(arp_entry->mac_addr, 0, 6);
-		pending_pkt.ipv4_addr = ip_dst;
-		memset(pending_pkt.mac_addr, 0, 6);
-		fill_ethhdr (pending_pkt.eth_frame, htons(ETHERTYPE_IP),
+		fill_ethhdr (pending_pkt_frame, htons(ETHERTYPE_IP),
 		             get_mac_address(), null_mac_addr);
-		memcpy(&pending_pkt.eth_frame[sizeof(struct ethhdr)],
+		memcpy(&pending_pkt_frame[sizeof(struct ethhdr)],
 		       buffer, len);
-		pending_pkt.eth_len = len + sizeof(struct ethhdr);
+		pending_pkt_len = len + sizeof(struct ethhdr);
 
 		set_timer(TICKS_SEC);
 		do {
@@ -538,11 +521,9 @@ send_ipv4(int fd, void* buffer, int len)
  *
  * @param  ipv4_hdr    Points to the place where IPv4-header starts.
  */
-
-static void
-fill_udp_checksum(struct iphdr *ipv4_hdr)
+static void fill_udp_checksum(struct iphdr *ipv4_hdr)
 {
-	int i;
+	unsigned i;
 	unsigned long checksum = 0;
 	struct iphdr ip_hdr;
 	char *ptr;
@@ -585,8 +566,7 @@ fill_udp_checksum(struct iphdr *ipv4_hdr)
  * @return            Checksum
  * @see               iphdr
  */
-static unsigned short
-checksum(unsigned short * packet, int words)
+static unsigned short checksum(unsigned short * packet, int words)
 {
 	unsigned long checksum;
 
@@ -598,8 +578,7 @@ checksum(unsigned short * packet, int words)
 	return ~checksum;
 }
 
-static arp_entry_t*
-lookup_mac_addr(uint32_t ipv4_addr)
+static arp_entry_t* lookup_mac_addr(uint32_t ipv4_addr)
 {
 	unsigned int i;
 
@@ -618,8 +597,7 @@ lookup_mac_addr(uint32_t ipv4_addr)
  * @param  fd        socket fd
  * @param  dest_ip   IP of the host which MAC should be obtained
  */
-static void
-arp_send_request(int fd, uint32_t dest_ip)
+static void arp_send_request(int fd, uint32_t dest_ip)
 {
 	arp_entry_t *arp_entry = &arp_table[arp_producer];
 
@@ -642,8 +620,7 @@ arp_send_request(int fd, uint32_t dest_ip)
  * @param  src_ip    requester IP address (foreign IP)
  * @param  src_mac   requester MAC address (foreign MAC)
  */
-static void
-arp_send_reply(int fd, uint32_t src_ip, uint8_t * src_mac)
+static void arp_send_reply(int fd, uint32_t src_ip, uint8_t * src_mac)
 {
 	arp_entry_t *arp_entry = &arp_table[arp_producer];
 
@@ -674,10 +651,9 @@ arp_send_reply(int fd, uint32_t src_ip, uint8_t * src_mac)
  * @see                arphdr
  * @see                fill_ethhdr
  */
-static void
-fill_arphdr(uint8_t * packet, uint8_t opcode,
-	    const uint8_t * src_mac, uint32_t src_ip,
-	    const uint8_t * dest_mac, uint32_t dest_ip)
+static void fill_arphdr(uint8_t * packet, uint8_t opcode,
+                        const uint8_t * src_mac, uint32_t src_ip,
+                        const uint8_t * dest_mac, uint32_t dest_ip)
 {
 	struct arphdr * arph = (struct arphdr *) packet;
 
@@ -706,8 +682,7 @@ fill_arphdr(uint8_t * packet, uint8_t opcode,
  * @see               receive_ether
  * @see               arphdr
  */
-int8_t
-handle_arp(int fd, uint8_t * packet, int32_t packetsize)
+int8_t handle_arp(int fd, uint8_t * packet, uint32_t packetsize)
 {
 	struct arphdr * arph = (struct arphdr *) packet;
 
@@ -754,11 +729,11 @@ handle_arp(int fd, uint8_t * packet, int32_t packetsize)
 
 		// do we have something to send
 		if (arp_table[i].pkt_pending) {
-			struct ethhdr * ethh = (struct ethhdr *) pending_pkt.eth_frame;
+			struct ethhdr * ethh = (struct ethhdr *) pending_pkt_frame;
 			memcpy(ethh -> dest_mac, arp_table[i].mac_addr, 6);
 
-			send_ether(fd, pending_pkt.eth_frame, pending_pkt.eth_len);
-			pending_pkt.pkt_pending = 0;
+			send_ether(fd, pending_pkt_frame, pending_pkt_len);
+			arp_table[i].pkt_pending = 0;
 			arp_table[i].eth_len = 0;
 		}
 		return 0; // no error
@@ -780,8 +755,7 @@ handle_arp(int fd, uint8_t * packet, int32_t packetsize)
  * @param  fd            socket descriptor
  * @param  _ping_dst_ip  destination IPv4 address
  */
-void
-ping_ipv4(int fd, uint32_t _ping_dst_ip)
+void ping_ipv4(int fd, uint32_t _ping_dst_ip)
 {
 	unsigned char packet[sizeof(struct iphdr) + sizeof(struct icmphdr)];
 	struct icmphdr *icmp;
@@ -814,8 +788,7 @@ ping_ipv4(int fd, uint32_t _ping_dst_ip)
  *
  * @return  ping_dst_ip  host IPv4 address
  */
-uint32_t
-pong_ipv4(void)
+uint32_t pong_ipv4(void)
 {
 	return ping_dst_ip;
 }
@@ -830,8 +803,8 @@ pong_ipv4(void)
  *                      NON ZERO - packet was not handled (e.g. bad format)
  * @see                 handle_ipv4
  */
-static int8_t
-handle_icmp(int fd, struct iphdr * iph, uint8_t * packet, int32_t packetsize)
+static int8_t handle_icmp(int fd, struct iphdr * iph, uint8_t * packet,
+			  int32_t packetsize)
 {
 	struct icmphdr *icmp = (struct icmphdr *) packet;
 
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv4.h b/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv4.h
index eb719f8b2..18821ea74 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv4.h
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv4.h
@@ -60,7 +60,7 @@ struct arphdr {
 	uint32_t dest_ip;    /**< Proto address of target of this packet       */
 } __attribute((packed));
 
-/*>>>>>>>>>>>>> Initialization of the IPv4 network layer. <<<<<<<<<<<<<*/
+/************** Initialization of the IPv4 network layer. **************/
 extern void     set_ipv4_address(uint32_t own_ip);
 extern uint32_t get_ipv4_address(void);
 extern void     set_ipv4_multicast(uint32_t multicast_ip);
@@ -88,9 +88,9 @@ extern void ping_ipv4(int fd, uint32_t _ping_dst_ip);
 extern uint32_t pong_ipv4(void);
 
 /* Handles IPv4-packets that are detected by receive_ether. */
-extern int8_t handle_ipv4(int fd, uint8_t * packet, int32_t packetsize);
+extern int8_t handle_ipv4(int fd, uint8_t * packet, uint32_t packetsize);
 
 /* Handles ARP-packets that are detected by receive_ether. */
-extern int8_t handle_arp(int fd, uint8_t * packet, int32_t packetsize);
+extern int8_t handle_arp(int fd, uint8_t * packet, uint32_t packetsize);
 
 #endif
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv6.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv6.c
index 0cb0a2e7b..62d29ea86 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv6.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv6.c
@@ -37,15 +37,23 @@ static int ip6_is_multicast (ip6_addr_t * ip);
 
 /****************************** LOCAL VARIABLES **************************/
 
+/* List of Ipv6 Addresses */
+static struct ip6addr_list_entry *first_ip6;
+static struct ip6addr_list_entry *last_ip6;
+
 /* Own IPv6 address */
 static struct ip6addr_list_entry *own_ip6;
 
+/* All nodes link-local address */
+struct ip6addr_list_entry all_nodes_ll;
+
 /* Null IPv6 address */
 static ip6_addr_t null_ip6;
 
 /* helper variables */
 static uint8_t null_mac[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 
+struct ip6_config ip6_state;
 
 /****************************** IMPLEMENTATION ***************************/
 
@@ -55,9 +63,10 @@ static uint8_t null_mac[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  * @param  fd            Socket descriptor
  * @param  _own_ip       client IPv6 address (e.g. ::1)
  */
-void
-set_ipv6_address (int fd, ip6_addr_t *_own_ip6)
+void set_ipv6_address(int fd, ip6_addr_t *_own_ip6)
 {
+	struct ip6addr_list_entry *ile;
+
 	own_ip6 = malloc (sizeof(struct ip6addr_list_entry));
 
 	/* If no address was passed as a parameter generate a link-local
@@ -73,6 +82,20 @@ set_ipv6_address (int fd, ip6_addr_t *_own_ip6)
 	ip6addr_add (own_ip6);
 
 	ipv6_init(fd);
+
+	/*
+	 * Check whether we've got a non-link-local address during
+	 * ipv6_init() and use that as preferred address if possible
+	 */
+	if (_own_ip6 == NULL) {
+		for (ile = first_ip6; ile != NULL ; ile = ile->next) {
+			if (!ip6_is_multicast(&ile->addr) &&
+			    !ip6_is_linklocal(&ile->addr)) {
+				own_ip6 = ile;
+				break;
+			}
+		}
+	}
 }
 
 /**
@@ -80,8 +103,7 @@ set_ipv6_address (int fd, ip6_addr_t *_own_ip6)
  *
  * @return pointer to client IPv6 address (e.g. ::1)
  */
-ip6_addr_t *
-get_ipv6_address (void)
+ip6_addr_t *get_ipv6_address(void)
 {
 	return (ip6_addr_t *) &(own_ip6->addr);
 }
@@ -92,8 +114,7 @@ get_ipv6_address (void)
  * @return 0 - IPv6 address is not in list
  *         1 - IPv6 address is in list
  */
-static int8_t
-find_ip6addr (ip6_addr_t *ip)
+static int8_t find_ip6addr(ip6_addr_t *ip)
 {
 	struct ip6addr_list_entry *n = NULL;
 
@@ -119,8 +140,7 @@ find_ip6addr (ip6_addr_t *ip)
  * @see handle_udp
  * @see ip6hdr
  */
-int8_t
-handle_ipv6 (int fd, uint8_t * ip6_packet, int32_t packetsize)
+int8_t handle_ipv6(int fd, uint8_t * ip6_packet, uint32_t packetsize)
 {
 
 	struct ip6hdr *ip6 = NULL;
@@ -164,11 +184,9 @@ handle_ipv6 (int fd, uint8_t * ip6_packet, int32_t packetsize)
  * @see                fill_dnshdr
  * @see                fill_btphdr
  */
-void
-fill_ip6hdr (uint8_t * packet, uint16_t packetsize,
-             uint8_t ip_proto, ip6_addr_t *ip6_src, ip6_addr_t *ip6_dst)
+void fill_ip6hdr(uint8_t * packet, uint16_t packetsize,
+		 uint8_t ip_proto, ip6_addr_t *ip6_src, ip6_addr_t *ip6_dst)
 {
-
 	struct ip6hdr * ip6h = (struct ip6hdr *) packet;
 
 	ip6h->ver_tc_fl = 6 << 28;	// set version to 6
@@ -184,8 +202,7 @@ fill_ip6hdr (uint8_t * packet, uint16_t packetsize,
  *      See RFC 4291 "IP Version 6 Addressing Architecture"
  *
  */
-uint64_t
-mac2eui64 (const uint8_t *mac)
+uint64_t mac2eui64(const uint8_t *mac)
 {
 	uint8_t eui64id[8];
 	uint64_t retid;
@@ -205,8 +222,7 @@ mac2eui64 (const uint8_t *mac)
  * @param  own_mac    MAC of NIC
  * @return ll_addr    pointer to newly created link-local address
  */
-ip6_addr_t *
-ip6_create_ll_address (const uint8_t *own_mac)
+ip6_addr_t *ip6_create_ll_address(const uint8_t *own_mac)
 {
 	ip6_addr_t *ll_addr;
 
@@ -223,8 +239,7 @@ ip6_create_ll_address (const uint8_t *own_mac)
  * @param  struct ip6_addr_list_entry *ip6
  * @return true or false
  */
-int8_t
-unknown_prefix (ip6_addr_t *ip)
+int8_t unknown_prefix(ip6_addr_t *ip)
 {
 	struct ip6addr_list_entry *node;
 
@@ -240,8 +255,7 @@ unknown_prefix (ip6_addr_t *ip)
  * @return NULL - malloc failed
  *	   ! NULL - pointer to new prefix_info
  */
-struct prefix_info *
-ip6_create_prefix_info ()
+struct prefix_info *ip6_create_prefix_info()
 {
 	struct prefix_info *prfx_info;
 
@@ -259,8 +273,7 @@ ip6_create_prefix_info ()
  * @param  ip6_addr prefix (as received in RA)
  * @return NULL - pointer to new ip6addr_list entry
  */
-void *
-ip6_prefix2addr (ip6_addr_t prefix)
+void *ip6_prefix2addr(ip6_addr_t prefix)
 {
 	struct ip6addr_list_entry *new_address;
 	uint64_t interface_id;
@@ -287,8 +300,7 @@ ip6_prefix2addr (ip6_addr_t prefix)
  * @return  0 - passed pointer = NULL;
  *	    1 - ok
  */
-int8_t
-ip6addr_add (struct ip6addr_list_entry *new_address)
+int8_t ip6addr_add(struct ip6addr_list_entry *new_address)
 {
 	struct ip6addr_list_entry *solicited_node;
 
@@ -332,8 +344,7 @@ ip6addr_add (struct ip6addr_list_entry *new_address)
  *
  * @param  fd            socket fd
  */
-static void
-ipv6_init (int fd)
+static void ipv6_init(int fd)
 {
 	int i = 0;
 
@@ -349,21 +360,9 @@ ipv6_init (int fd)
 	/* Multicast addresses */
 	all_nodes_ll.addr.part.prefix         = 0xff02000000000000;
 	all_nodes_ll.addr.part.interface_id   = 1;
-	all_dhcpv6_ll.addr.part.prefix        = 0xff02000000000000ULL;
-	all_dhcpv6_ll.addr.part.interface_id  = 0x10002ULL;
-	all_routers_ll.addr.part.prefix       = 0xff02000000000000;
-	all_routers_ll.addr.part.interface_id      = 2;
-
 	ip6addr_add(&all_nodes_ll);
-	/* ... */
 
-	/* Router list */
-	first_router = NULL;
-	last_router = first_router;
-
-	/* Init Neighbour cache */
-	first_neighbor = NULL;
-	last_neighbor  = first_neighbor;
+	ndp_init();
 
 	send_router_solicitation (fd);
 	for(i=0; i < 4 && !is_ra_received(); i++) {
@@ -382,8 +381,7 @@ ipv6_init (int fd)
  * @param  ip6_addr ip_1
  * @param  ip6_addr ip_2
  */
-int8_t
-ip6_cmp (ip6_addr_t *ip_1, ip6_addr_t *ip_2)
+int8_t ip6_cmp(ip6_addr_t *ip_1, ip6_addr_t *ip_2)
 {
 	return ((int8_t) !memcmp( &(ip_1->addr[0]), &(ip_2->addr[0]),
 		IPV6_ADDR_LENGTH ));
@@ -396,11 +394,9 @@ ip6_cmp (ip6_addr_t *ip_1, ip6_addr_t *ip_2)
  * @param  *ip    - pointer to IPv6 address
  * @return true or false
  */
-int
-ip6_is_multicast (ip6_addr_t * ip)
+int ip6_is_multicast(ip6_addr_t * ip)
 {
-	uint8_t mc = 0xFF;
-	return ! memcmp(&ip->addr[0], &mc, 1);
+	return ip->addr[0] == 0xFF;
 }
 
 /**
@@ -408,17 +404,11 @@ ip6_is_multicast (ip6_addr_t * ip)
  *      (e.g. UDP or ICMPv6)
  *
  * @param  *ip    - pointer to IPv6 address
+ * @param  *mc_mac  pointer to an array with 6 bytes (for the MAC address)
  * @return pointer to Multicast MAC address
  */
-static uint8_t *
-ip6_to_multicast_mac (ip6_addr_t * ip)
+static uint8_t *ip6_to_multicast_mac(ip6_addr_t * ip, uint8_t *mc_mac)
 {
-	uint8_t *mc_mac;
-
-	mc_mac = malloc(ETH_ALEN);
-	if (!mc_mac)
-		return NULL;
-
 	mc_mac[0] = 0x33;
 	mc_mac[1] = 0x33;
 	memcpy (mc_mac+2, (uint8_t *) &(ip->addr)+12, 4);
@@ -437,8 +427,8 @@ ip6_to_multicast_mac (ip6_addr_t * ip)
  *				    starting from *packet
  * @return checksum
  */
-static unsigned short
-ip6_checksum (struct ip6hdr *ip6h, unsigned short *packet, int words)
+static unsigned short ip6_checksum(struct ip6hdr *ip6h, unsigned short *packet,
+				   int words)
 {
 	int i=0;
 	unsigned long checksum;
@@ -478,8 +468,7 @@ ip6_checksum (struct ip6hdr *ip6h, unsigned short *packet, int words)
  * @see receive_ether
  * @see ip6hdr
  */
-int
-send_ipv6 (int fd, void* buffer, int len)
+int send_ipv6(int fd, void* buffer, int len)
 {
 	struct neighbor *n;
 	struct ip6hdr *ip6h;
@@ -519,17 +508,9 @@ send_ipv6 (int fd, void* buffer, int len)
 
 	n = find_neighbor (&ip_dst);
 
-	// If packet is a neighbor solicitation
-	if (icmp6h->type == ICMPV6_NEIGHBOUR_SOLICITATION) {
-		mac_addr = ip6_to_multicast_mac (&ip_dst);
-		fill_ethhdr( buffer-sizeof(struct ethhdr), htons(ETHERTYPE_IPv6),
-			     get_mac_address(),
-			     mac_addr);
-	}
-
 	// If address is a multicast address, create a proper mac address
-	else if (ip6_is_multicast (&ip_dst)) {
-		mac_addr = ip6_to_multicast_mac (&ip_dst);
+	if (ip6_is_multicast (&ip_dst)) {
+		mac_addr = ip6_to_multicast_mac (&ip_dst, mac);
 	}
 	else {
 		// Check if the MAC address is already cached
@@ -572,8 +553,7 @@ send_ipv6 (int fd, void* buffer, int len)
 	return send_ether (fd, n->eth_frame, len + sizeof(struct ethhdr));
 }
 
-static int
-check_colons(const char *str)
+static int check_colons(const char *str)
 {
 	char *pch, *prv;
 	int col = 0;
@@ -595,7 +575,7 @@ check_colons(const char *str)
 	dprintf("The number of  col : %d \n",col);
 	dprintf("The number of dcol : %d \n",dcol);
 
-	if((dcol > 1) ||                      /* Cannot have 2 "::" */ 
+	if((dcol > 1) ||                      /* Cannot have 2 "::" */
 	   ((dcol == 1) && (col > 5)) ||      /* Too many ':'s */
 	   ((dcol == 0) && (col != 7)) ) {    /* Too few ':'s */
 		dprintf(" exiting for check_colons \n");
@@ -605,8 +585,7 @@ check_colons(const char *str)
 	return (col+dcol);
 }
 
-static int
-ipv6str_to_bytes(const char *str, char *ip)
+static int ipv6str_to_bytes(const char *str, char *ip)
 {
 	char block[5];
 	int res;
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv6.h b/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv6.h
index b496364f3..72c6ee22f 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv6.h
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/ipv6.h
@@ -26,6 +26,7 @@
 
 #define IPV6_ADDR_LENGTH	 16 /* Size of IPv6 adress in bytes */
 #define IPV6_LL_PREFIX		 0xFE80000000000000ULL
+#define IPV6_LL_PREFIX_MASK	 0xFFC0000000000000ULL
 #define IPV6_SOLIC_NODE_PREFIX   0xFF02000000000000ULL
 #define IPV6_SOLIC_NODE_IFACE_ID 0x00000001FF000000ULL
 
@@ -126,32 +127,17 @@ struct ip6_config {
 	uint8_t managed_mode:1,
 		other_config:1,
 		reserved:6;
-} ip6_state;
+};
 
 /******************** VARIABLES **********************************************/
 /* Function pointer send_ip. Points either to send_ipv4() or send_ipv6() */
 extern int   (*send_ip) (int fd, void *, int);
 
-/* IPv6 link-local multicast addresses */
-struct ip6addr_list_entry all_routers_ll; // Routers
-struct ip6addr_list_entry all_dhcpv6_ll;  // DHCPv6 servers
-struct ip6addr_list_entry all_nodes_ll;   // All IPv6 nodes
-
-/* List of Ipv6 Addresses */
-struct ip6addr_list_entry *first_ip6;
-struct ip6addr_list_entry *last_ip6;
-
-/* Neighbor cache */
-struct neighbor *first_neighbor;
-struct neighbor *last_neighbor;
-
-/* Router list */
-struct router *first_router;
-struct router *last_router;
+extern struct ip6_config ip6_state;
 
 /******************** FUNCTIONS *********************************************/
 /* Handles IPv6-packets that are detected by receive_ether. */
-int8_t handle_ipv6(int fd, uint8_t * ip6_packet, int32_t packetsize);
+int8_t handle_ipv6(int fd, uint8_t * ip6_packet, uint32_t packetsize);
 
 /* Fill IPv6 header */
 void fill_ip6hdr(uint8_t * packet, uint16_t packetsize,
@@ -179,6 +165,12 @@ void * ip6_prefix2addr (ip6_addr_t prefix);
 /* Compare IPv6 adresses */
 int8_t ip6_cmp( ip6_addr_t *ip_1, ip6_addr_t *ip_2 );
 
+/* Check if it is a link-local address */
+static inline int ip6_is_linklocal(ip6_addr_t *ip)
+{
+	return (ip->part.prefix & IPV6_LL_PREFIX_MASK) == IPV6_LL_PREFIX;
+}
+
 /* Check if prefix is already in our list */
 int8_t unknown_prefix (ip6_addr_t *ip);
 
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/ndp.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/ndp.c
index ed9d61f4a..96faa8716 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/ndp.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/ndp.c
@@ -17,6 +17,14 @@
 #include <netlib/icmpv6.h>
 #include <netlib/ndp.h>
 
+/* Neighbor cache */
+static struct neighbor *first_neighbor;
+static struct neighbor *last_neighbor;
+
+/* Router list */
+static struct router *first_router;
+static struct router *last_router;
+
 /*
  * NET: add new router to list
  * @param  struct router nghb  - new router
@@ -145,3 +153,14 @@ find_neighbor (ip6_addr_t *ip)
 
 	return NULL; /* neighbor is unknown */
 }
+
+void ndp_init(void)
+{
+	/* Router list */
+	first_router = NULL;
+	last_router = first_router;
+
+	/* Init Neighbour cache */
+	first_neighbor = NULL;
+	last_neighbor  = first_neighbor;
+}
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/ndp.h b/qemu/roms/SLOF/clients/net-snk/app/netlib/ndp.h
index ee5235fe1..c785c4897 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/ndp.h
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/ndp.h
@@ -59,6 +59,7 @@ struct neighbor {
 };
 
 /******************** FUNCTIONS *********************************************/
+void ndp_init(void);
 int8_t neighbor_add (struct neighbor *);
 void * neighbor_create (uint8_t *packet, struct packeth *headers);
 struct neighbor * find_neighbor (ip6_addr_t *);
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/tcp.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/tcp.c
index 5511aa00a..faa0b83ac 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/tcp.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/tcp.c
@@ -10,16 +10,14 @@
  *     IBM Corporation - initial implementation
  *****************************************************************************/
 
-/*>>>>>>>>>>>>>>>>>>>>>>> DEFINITIONS & DECLARATIONS <<<<<<<<<<<<<<<<<<<<*/
+/************************ DEFINITIONS & DECLARATIONS *********************/
 
 #include <tcp.h>
 #include <sys/socket.h>
 
+/****************************** LOCAL VARIABLES **************************/
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> LOCAL VARIABLES <<<<<<<<<<<<<<<<<<<<<<<<<*/
-
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> IMPLEMENTATION <<<<<<<<<<<<<<<<<<<<<<<<<<*/
-
+/****************************** IMPLEMENTATION ***************************/
 
 /**
  * TCP: Handles TCP-packets according to Receive-handle diagram.
@@ -29,13 +27,11 @@
  * @return            ZERO - packet handled successfully;
  *                    NON ZERO - packet was not handled (e.g. bad format)
  */
-int8_t
-handle_tcp(uint8_t * tcp_packet, int32_t packetsize)
+int8_t handle_tcp(uint8_t * tcp_packet, int32_t packetsize)
 {
 	return -1;
 }
 
-
 /**
  * NET: This function handles situation when "Destination unreachable"
  *      ICMP-error occurs during sending TCP-packet.
@@ -45,6 +41,6 @@ handle_tcp(uint8_t * tcp_packet, int32_t packetsize)
  * @param  packetsize length of the packet
  * @see               handle_icmp
  */
-void
-handle_tcp_dun(uint8_t * tcp_packet, uint32_t packetsize, uint8_t err_code) {
+void handle_tcp_dun(uint8_t * tcp_packet, uint32_t packetsize, uint8_t err_code)
+{
 }
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/tftp.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/tftp.c
index 0a7c0ec63..c1197cf17 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/tftp.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/tftp.c
@@ -50,13 +50,13 @@ static unsigned short block = 0;
 static unsigned short blocksize;
 static char blocksize_str[6];    /* Blocksize string for read request */
 static int received_len = 0;
-static int retries = 0;
+static unsigned int retries = 0;
 static int huge_load;
 static int len;
 static int tftp_finished = 0;
 static int lost_packets = 0;
-static int tftp_errno = 0; 
-static int ip_version = 0; 
+static int tftp_errno = 0;
+static int ip_version = 0;
 static short port_number = -1;
 static tftp_err_t *tftp_err;
 static filename_ip_t  *fn_ip;
@@ -69,8 +69,7 @@ static filename_ip_t  *fn_ip;
  */
 #ifdef __DEBUG__
 
-static void
-dump_package(unsigned char *buffer, unsigned int len)
+static void dump_package(unsigned char *buffer, unsigned int len)
 {
 	int i;
 
@@ -89,8 +88,7 @@ dump_package(unsigned char *buffer, unsigned int len)
  *
  * @fd:          Socket Descriptor
  */
-static void
-send_rrq(int fd)
+static void send_rrq(int fd)
 {
 	int ip_len = 0;
 	int ip6_payload_len    = 0;
@@ -121,7 +119,7 @@ send_rrq(int fd)
 			+ strlen("blksize") + strlen(blocksize_str) + 2;
 		ip_len = sizeof(struct ip6hdr) + ip6_payload_len;
 		fill_ip6hdr ((uint8_t *) ip6, ip6_payload_len, IPTYPE_UDP, get_ipv6_address(),
-			     &(fn_ip->server_ip6)); 
+			     &(fn_ip->server_ip6));
 
 	}
 	udp_len = htons(sizeof(struct udphdr)
@@ -158,8 +156,7 @@ send_rrq(int fd)
  * @blckno: block number
  * @dport:  UDP destination port
  */
-static void
-send_ack(int fd, int blckno, unsigned short dport)
+static void send_ack(int fd, int blckno, unsigned short dport)
 {
 	int ip_len 	       = 0;
 	int ip6_payload_len    = 0;
@@ -182,8 +179,7 @@ send_ack(int fd, int blckno, unsigned short dport)
 		ip6 = (struct ip6hdr *) packet;
 		udph = (struct udphdr *) (ip6 + 1);
 		ip6_payload_len = sizeof(struct udphdr) + 4;
-		ip_len = sizeof(struct ethhdr) + sizeof(struct ip6hdr) +
-		    	 ip6_payload_len;
+		ip_len = sizeof(struct ip6hdr) + ip6_payload_len;
 		fill_ip6hdr ((uint8_t *) ip6, ip6_payload_len, IPTYPE_UDP, get_ipv6_address(),
 			     &(fn_ip->server_ip6));
 	}
@@ -210,8 +206,7 @@ send_ack(int fd, int blckno, unsigned short dport)
  * @error_code:  Used sub code for error packet
  * @dport:       UDP destination port
  */
-static void
-send_error(int fd, int error_code, unsigned short dport)
+static void send_error(int fd, int error_code, unsigned short dport)
 {
 	int ip_len 	       = 0;
 	int ip6_payload_len    = 0;
@@ -234,8 +229,7 @@ send_error(int fd, int error_code, unsigned short dport)
 		ip6 = (struct ip6hdr *) packet;
 		udph = (struct udphdr *) (ip6 + 1);
 		ip6_payload_len = sizeof(struct udphdr) + 5;
-		ip_len = sizeof(struct ethhdr) + sizeof(struct ip6hdr) +
-		         ip6_payload_len; 
+		ip_len = sizeof(struct ip6hdr) + ip6_payload_len;
 		fill_ip6hdr ((uint8_t *) ip6, ip6_payload_len, IPTYPE_UDP, get_ipv6_address(),
 			    &(fn_ip->server_ip6));
 	}
@@ -256,8 +250,7 @@ send_error(int fd, int error_code, unsigned short dport)
 	return;
 }
 
-static void
-print_progress(int urgent, int received_bytes)
+static void print_progress(int urgent, int received_bytes)
 {
 	static unsigned int i = 1;
 	static int first = -1;
@@ -265,7 +258,7 @@ print_progress(int urgent, int received_bytes)
 	char buffer[100];
 	char *ptr;
 
-	// 1MB steps or 0x400 times or urgent 
+	// 1MB steps or 0x400 times or urgent
 	if(((received_bytes - last_bytes) >> 20) > 0
 	|| (i & 0x3FF) == 0 || urgent) {
 		if(!first) {
@@ -295,8 +288,7 @@ print_progress(int urgent, int received_bytes)
  * @param len  the length of the network packet
  * @return  the blocksize the server supports or 0 for error
  */
-static int
-get_blksize(unsigned char *buffer, unsigned int len)
+static int get_blksize(unsigned char *buffer, unsigned int len)
 {
 	unsigned char *orig = buffer;
 	/* skip all headers until tftp has been reached */
@@ -325,7 +317,7 @@ get_blksize(unsigned char *buffer, unsigned int len)
 }
 
 /**
- * Handle incoming tftp packets after read request was sent 
+ * Handle incoming tftp packets after read request was sent
  *
  * this function also prints out some status characters
  * \|-/ for each packet received
@@ -334,13 +326,12 @@ get_blksize(unsigned char *buffer, unsigned int len)
  * #+* for different unexpected TFTP packets (not very good)
  *
  * @param fd     socket descriptor
- * @param packet points to the UDP header of the packet 
+ * @param packet points to the UDP header of the packet
  * @param len    the length of the network packet
  * @return       ZERO if packet was handled successfully
- *               ERRORCODE if error occurred 
+ *               ERRORCODE if error occurred
  */
-int32_t
-handle_tftp(int fd, uint8_t *pkt, int32_t packetsize) 
+int32_t handle_tftp(int fd, uint8_t *pkt, int32_t packetsize)
 {
 	struct udphdr *udph;
 	struct tftphdr *tftp;
@@ -397,7 +388,7 @@ handle_tftp(int fd, uint8_t *pkt, int32_t packetsize)
 		case ENOUSER:
 			tftp_errno = -7;	// ERROR: no such user
 			break;
-		default:	
+		default:
 			tftp_errno = -1;	// ERROR: unknown error
 		}
 		goto error;
@@ -489,8 +480,7 @@ error:
  *
  * @param  err_code   Error Code (e.g. "Host unreachable")
  */
-void
-handle_tftp_dun(uint8_t err_code)
+void handle_tftp_dun(uint8_t err_code)
 {
 	tftp_errno = - err_code - 10;
 	tftp_finished = 1;
@@ -510,10 +500,9 @@ handle_tftp_dun(uint8_t err_code)
  * @return               ZERO - error condition occurs
  *                       NON ZERO - size of received file
  */
-int
-tftp(filename_ip_t * _fn_ip, unsigned char *_buffer, int _len,
-     unsigned int _retries, tftp_err_t * _tftp_err,
-     int32_t _mode, int32_t _blocksize, int _ip_version)
+int tftp(filename_ip_t * _fn_ip, unsigned char *_buffer, int _len,
+	 unsigned int _retries, tftp_err_t * _tftp_err,
+	 int32_t _mode, int32_t _blocksize, int _ip_version)
 {
 	retries     = _retries;
 	fn_ip       = _fn_ip;
@@ -592,6 +581,6 @@ tftp(filename_ip_t * _fn_ip, unsigned char *_buffer, int _len,
 	printf("\n");
 	if (lost_packets)
 		printf("Lost ACK packets: %d\n", lost_packets);
-		
+
 	return received_len;
 }
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/udp.c b/qemu/roms/SLOF/clients/net-snk/app/netlib/udp.c
index db29bc90f..5d16e52c6 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/udp.c
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/udp.c
@@ -10,7 +10,7 @@
  *     IBM Corporation - initial implementation
  *****************************************************************************/
 
-/*>>>>>>>>>>>>>>>>>>>>>>> DEFINITIONS & DECLARATIONS <<<<<<<<<<<<<<<<<<<<*/
+/************************ DEFINITIONS & DECLARATIONS *********************/
 
 #include <udp.h>
 #include <sys/socket.h>
@@ -25,7 +25,7 @@
 
 
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> LOCAL VARIABLES <<<<<<<<<<<<<<<<<<<<<<<<<*/
+/****************************** LOCAL VARIABLES **************************/
 
 
 #ifdef USE_MTFTP
@@ -33,17 +33,19 @@
 uint16_t net_tftp_uport;
 uint16_t net_mtftp_uport;
 
-void net_set_tftp_port(uint16_t tftp_port) {
+void net_set_tftp_port(uint16_t tftp_port)
+{
 	net_tftp_uport = tftp_port;
 }
 
-void net_set_mtftp_port(uint16_t tftp_port) {
+void net_set_mtftp_port(uint16_t tftp_port)
+{
 	net_mtftp_uport = tftp_port;
 }
 
 #endif
 
-/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> IMPLEMENTATION <<<<<<<<<<<<<<<<<<<<<<<<<<*/
+/****************************** IMPLEMENTATION ***************************/
 
 
 /**
@@ -56,8 +58,8 @@ void net_set_mtftp_port(uint16_t tftp_port) {
  * @see               receive_ether
  * @see               udphdr
  */
-int8_t
-handle_udp(int fd, uint8_t * udp_packet, int32_t packetsize) {
+int8_t handle_udp(int fd, uint8_t * udp_packet, uint32_t packetsize)
+{
 	struct udphdr * udph = (struct udphdr *) udp_packet;
 
 	if (packetsize < sizeof(struct udphdr))
@@ -108,8 +110,8 @@ handle_udp(int fd, uint8_t * udp_packet, int32_t packetsize) {
  * @param  packetsize length of the packet
  * @see               handle_icmp
  */
-void
-handle_udp_dun(uint8_t * udp_packet, uint32_t packetsize, uint8_t err_code) {
+void handle_udp_dun(uint8_t * udp_packet, uint32_t packetsize, uint8_t err_code)
+{
 	struct udphdr * udph = (struct udphdr *) udp_packet;
 
 	if (packetsize < sizeof(struct udphdr))
@@ -139,9 +141,9 @@ handle_udp_dun(uint8_t * udp_packet, uint32_t packetsize, uint8_t err_code) {
  * @see                fill_dnshdr
  * @see                fill_btphdr
  */
-void
-fill_udphdr(uint8_t * packet, uint16_t packetsize,
-            uint16_t src_port, uint16_t dest_port) {
+void fill_udphdr(uint8_t * packet, uint16_t packetsize,
+		 uint16_t src_port, uint16_t dest_port)
+{
 	struct udphdr * udph = (struct udphdr *) packet;
 
 	udph -> uh_sport = htons(src_port);
diff --git a/qemu/roms/SLOF/clients/net-snk/app/netlib/udp.h b/qemu/roms/SLOF/clients/net-snk/app/netlib/udp.h
index 1ba9332ce..f154542dd 100644
--- a/qemu/roms/SLOF/clients/net-snk/app/netlib/udp.h
+++ b/qemu/roms/SLOF/clients/net-snk/app/netlib/udp.h
@@ -40,7 +40,7 @@ typedef int32_t *(*handle_upper_udp_t)(uint8_t *, int32_t);
 typedef void    *(*handle_upper_udp_dun_t)(uint8_t);
 
 /* Handles UDP-packets that are detected by any network layer. */
-extern int8_t handle_udp(int fd, uint8_t * udp_packet, int32_t packetsize);
+extern int8_t handle_udp(int fd, uint8_t * udp_packet, uint32_t packetsize);
 
 /* Handles UDP related ICMP-Dest.Unreachable packets that are detected by
  * the network layers. */
diff --git a/qemu/roms/SLOF/clients/net-snk/client.lds b/qemu/roms/SLOF/clients/net-snk/client.lds
index 39d04594e..c2086445b 100644
--- a/qemu/roms/SLOF/clients/net-snk/client.lds
+++ b/qemu/roms/SLOF/clients/net-snk/client.lds
@@ -44,10 +44,10 @@ SECTIONS {
 	  *(.opd)
 	}
 
-	. = ALIGN(0x10);
+	. = ALIGN(256);
 	.got :
 	{
-	  _got = .;
+	  _got = DEFINED (.TOC.) ? .TOC. : ADDR (.got) + 0x8000;
           *(.got)
 	  *(.toc)
 	  _got_end = .;
diff --git a/qemu/roms/SLOF/clients/net-snk/kernel/entry.S b/qemu/roms/SLOF/clients/net-snk/kernel/entry.S
index 8849fb9d1..bf10542bd 100644
--- a/qemu/roms/SLOF/clients/net-snk/kernel/entry.S
+++ b/qemu/roms/SLOF/clients/net-snk/kernel/entry.S
@@ -44,7 +44,7 @@ C_ENTRY(_entry)
 	bcl	20,31,over	# branch after pointer table
 base:	
 	.align  3
-.LCgot:		.quad   _got-base+0x8000
+.LCgot:		.quad   _got-base
 .LCstack:	.quad   _stack+STACKSIZE-0x80-base
 over:	
 	mflr	r8		# gpr 8 is the base
diff --git a/qemu/roms/SLOF/clients/takeover/client.lds b/qemu/roms/SLOF/clients/takeover/client.lds
index 2701d8e1e..0ab428a01 100644
--- a/qemu/roms/SLOF/clients/takeover/client.lds
+++ b/qemu/roms/SLOF/clients/takeover/client.lds
@@ -43,8 +43,8 @@ SECTIONS {
 
 	.got :
 	{
-	  . = ALIGN(8);
-	  _got = .;
+	  . = ALIGN(256);
+	  _got = DEFINED (.TOC.) ? .TOC. : ADDR (.got) + 0x8000;
           *(.got .toc)
 	  _got_end = .;
 	}  	
diff --git a/qemu/roms/SLOF/clients/takeover/entry.S b/qemu/roms/SLOF/clients/takeover/entry.S
index a1030eb40..ff482732d 100644
--- a/qemu/roms/SLOF/clients/takeover/entry.S
+++ b/qemu/roms/SLOF/clients/takeover/entry.S
@@ -21,7 +21,7 @@ _wrapclient:
 	bcl	20,31,over	# branch after pointer table
 base:
 	.align  3
-.LCgot:		.quad   _got-base+0x8000
+.LCgot:		.quad   _got-base
 over:
 	mflr	r8		# gpr 8 is the base
 	ld	r2, .LCgot-base(r8)	# load got pointer
diff --git a/qemu/roms/SLOF/clients/takeover/main.c b/qemu/roms/SLOF/clients/takeover/main.c
index 360d8eaed..1e1b02614 100644
--- a/qemu/roms/SLOF/clients/takeover/main.c
+++ b/qemu/roms/SLOF/clients/takeover/main.c
@@ -16,7 +16,7 @@
 #include <of.h>
 #include <pci.h>
 #include <cpu.h>
-#include <ioctl.h>
+#include <unistd.h>
 #include <takeover.h>
 
 extern void call_client_interface(of_arg_t *);
diff --git a/qemu/roms/SLOF/include/byteorder.h b/qemu/roms/SLOF/include/byteorder.h
index 60ca67267..d4a2c8ca7 100644
--- a/qemu/roms/SLOF/include/byteorder.h
+++ b/qemu/roms/SLOF/include/byteorder.h
@@ -19,38 +19,36 @@
 
 #include <stdint.h>
 
-static inline uint16_t
-bswap_16 (uint16_t x)
+typedef uint16_t le16;
+typedef uint32_t le32;
+typedef uint64_t le64;
+
+static inline uint16_t bswap_16 (uint16_t x)
 {
 	return __builtin_bswap16(x);
 }
 
-static inline uint32_t
-bswap_32 (uint32_t x)
+static inline uint32_t bswap_32 (uint32_t x)
 {
 	return __builtin_bswap32(x);
 }
 
-static inline uint64_t
-bswap_64 (uint64_t x)
+static inline uint64_t bswap_64 (uint64_t x)
 {
 	return __builtin_bswap64(x);
 }
 
-static inline void
-bswap_16p (uint16_t *x)
+static inline void bswap_16p (uint16_t *x)
 {
 	*x = __builtin_bswap16(*x);
 }
 
-static inline void
-bswap_32p (uint32_t *x)
+static inline void bswap_32p (uint32_t *x)
 {
 	*x = __builtin_bswap32(*x);
 }
 
-static inline void
-bswap_64p (uint64_t *x)
+static inline void bswap_64p (uint64_t *x)
 {
 	*x = __builtin_bswap64(*x);
 }
diff --git a/qemu/roms/SLOF/include/helpers.h b/qemu/roms/SLOF/include/helpers.h
index fb105345e..5b3d711ac 100644
--- a/qemu/roms/SLOF/include/helpers.h
+++ b/qemu/roms/SLOF/include/helpers.h
@@ -30,8 +30,10 @@ extern long SLOF_dma_map_in(void *virt, long size, int cacheable);
 extern void SLOF_dma_map_out(long phys, void *virt, long size);
 extern long SLOF_pci_config_read32(long offset);
 extern long SLOF_pci_config_read16(long offset);
+extern long SLOF_pci_config_read8(long offset);
 extern void SLOF_pci_config_write32(long offset, long value);
 extern void SLOF_pci_config_write16(long offset, long value);
+extern void SLOF_pci_config_write8(long offset, long value);
 extern void *SLOF_translate_my_address(void *addr);
 
 #define offset_of(type, member) ((long) &((type *)0)->member)
diff --git a/qemu/roms/SLOF/include/ppc970/cache.h b/qemu/roms/SLOF/include/ppc970/cache.h
index b74868986..500182ea6 100644
--- a/qemu/roms/SLOF/include/ppc970/cache.h
+++ b/qemu/roms/SLOF/include/ppc970/cache.h
@@ -55,8 +55,8 @@ cache_inhibited_access(uint64_t, 64)
 #define _FASTMOVE(s, d, size) \
 	switch (((type_u)s | (type_u)d | size) & (sizeof(type_u)-1)) { \
 		case 0:			_MOVE(s, d, size, type_u); break; \
-		case sizeof(type_l):	_MOVE(s, d, size, type_l); break; \
-		case sizeof(type_w):	_MOVE(s, d, size, type_w); break; \
+		case 4:			_MOVE(s, d, size, type_l); break; \
+		case 2: case 6:		_MOVE(s, d, size, type_w); break; \
 		default:		_MOVE(s, d, size, type_c); break; \
 	}
 
@@ -78,9 +78,51 @@ cache_inhibited_access(uint64_t, 64)
 #define _FASTRMOVE(s, d, size) \
 	switch (((type_u)s | (type_u)d | size) & (sizeof(type_u)-1)) { \
 		case 0:			_RMOVE(s, d, size, type_u); break; \
-		case sizeof(type_l):	_RMOVE(s, d, size, type_l); break; \
-		case sizeof(type_w):	_RMOVE(s, d, size, type_w); break; \
+		case 4:			_RMOVE(s, d, size, type_l); break; \
+		case 2: case 6:		_RMOVE(s, d, size, type_w); break; \
 		default:		_RMOVE(s, d, size, type_c); break; \
 	}
 
+/* main RAM to IO memory move */
+#define FAST_MRMOVE_TYPED(s, d, size, t)	\
+{ \
+	t *s1 = (s), *d1 = (d); \
+	register t tmp; \
+	while (size > 0) { \
+		tmp = *s1++; SET_CI; *d1++ = tmp; CLR_CI; size -= sizeof(t); \
+	} \
+}
+
+#define FAST_MRMOVE(s, d, size) \
+	switch (((type_u)(s) | (type_u)(d) | (size)) & (sizeof(type_u)-1)) { \
+	case 0:		FAST_MRMOVE_TYPED(s, d, size, type_u); break; \
+	case 4:		FAST_MRMOVE_TYPED(s, d, size, type_l); break; \
+	case 2: case 6:	FAST_MRMOVE_TYPED(s, d, size, type_w); break; \
+	default:	FAST_MRMOVE_TYPED(s, d, size, type_c); break; \
+	}
+
+/* fill IO memory with pattern */
+#define FAST_RFILL_TYPED(dst, size, pat, t) \
+{ \
+	t *d1 = (dst); \
+	register t tmp = 0; \
+	int i = sizeof(t); \
+	while (i-- > 0) { \
+		tmp <<= 8; tmp |= pat & 0xff; \
+	} \
+	SET_CI; \
+	while (size > 0) { \
+		*d1++ = tmp; size -= sizeof(t); \
+	} \
+	CLR_CI; \
+}
+
+#define FAST_RFILL(dst, size, pat) \
+	switch (((type_u)dst | size) & (sizeof(type_u)-1)) { \
+	case 0:		FAST_RFILL_TYPED(dst, size, pat, type_u); break; \
+	case 4:		FAST_RFILL_TYPED(dst, size, pat, type_l); break; \
+	case 2: case 6:	FAST_RFILL_TYPED(dst, size, pat, type_w); break; \
+	default:	FAST_RFILL_TYPED(dst, size, pat, type_c); break; \
+	}
+
 #endif
diff --git a/qemu/roms/SLOF/include/ppcp7/cache.h b/qemu/roms/SLOF/include/ppcp7/cache.h
index dc6837196..3c02bb10d 100644
--- a/qemu/roms/SLOF/include/ppcp7/cache.h
+++ b/qemu/roms/SLOF/include/ppcp7/cache.h
@@ -81,8 +81,8 @@ cache_inhibited_access(uint64_t, 64)
 #define _FASTMOVE(s, d, size) \
 	switch (((type_u)s | (type_u)d | size) & (sizeof(type_u)-1)) { \
 	case 0:			_MOVE(s, d, size, type_u); break;	\
-	case sizeof(type_l):	_MOVE(s, d, size, type_l); break;	\
-	case sizeof(type_w):	_MOVE(s, d, size, type_w); break;	\
+	case 4:			_MOVE(s, d, size, type_l); break;	\
+	case 2: case 6:		_MOVE(s, d, size, type_w); break;	\
 	default:		_MOVE(s, d, size, type_c); break;	\
 	}
 
@@ -116,12 +116,17 @@ static inline void ci_rmove(void *dst, void *src, unsigned long esize,
 #define _FASTRMOVE(s, d, size) do {					      \
 		switch (((type_u)s | (type_u)d | size) & (sizeof(type_u)-1)) {\
 		case 0:			ci_rmove(d,s,3,size>>3); break;	      \
-		case sizeof(type_l):	ci_rmove(d,s,2,size>>2); break;	      \
-		case sizeof(type_w):	ci_rmove(d,s,1,size>>1); break;	      \
+		case 4:			ci_rmove(d,s,2,size>>2); break;	      \
+		case 2: case 6:		ci_rmove(d,s,1,size>>1); break;	      \
 		default:		ci_rmove(d,s,0,size); break;	      \
 		}							      \
 	} while(0)
 
+#define FAST_MRMOVE(s, d, size) _FASTRMOVE(s, d, size)
+
+extern void fast_rfill(char *dst, long size, char pat);
+#define FAST_RFILL(dst, size, pat) fast_rfill(dst, size, pat)
+
 static inline uint16_t bswap16_load(uint64_t addr)
 {
 	unsigned int val;
diff --git a/qemu/roms/SLOF/lib/libc/include/stdlib.h b/qemu/roms/SLOF/lib/libc/include/stdlib.h
index dff57f577..5e0eda9ff 100644
--- a/qemu/roms/SLOF/lib/libc/include/stdlib.h
+++ b/qemu/roms/SLOF/lib/libc/include/stdlib.h
@@ -29,5 +29,6 @@ unsigned long int strtoul(const char *nptr, char **endptr, int base);
 long int strtol(const char *nptr, char **endptr, int base);
 
 int rand(void);
+void srand(unsigned int seed);
 
 #endif
diff --git a/qemu/roms/SLOF/lib/libc/stdio/vsnprintf.c b/qemu/roms/SLOF/lib/libc/stdio/vsnprintf.c
index e78fb3d8e..21dd04dfe 100644
--- a/qemu/roms/SLOF/lib/libc/stdio/vsnprintf.c
+++ b/qemu/roms/SLOF/lib/libc/stdio/vsnprintf.c
@@ -10,72 +10,110 @@
  *     IBM Corporation - initial implementation
  *****************************************************************************/
 
+#include <stdbool.h>
 #include "stdio.h"
 #include "stdlib.h"
 #include "string.h"
+#include "ctype.h"
 
-const static unsigned long long convert[] = {
+static const unsigned long long convert[] = {
 	0x0, 0xFF, 0xFFFF, 0xFFFFFF, 0xFFFFFFFF,
 	0xFFFFFFFFFFULL, 0xFFFFFFFFFFFFULL, 0xFFFFFFFFFFFFFFULL, 0xFFFFFFFFFFFFFFFFULL
 };
 
-
-
 static int
-print_itoa(char **buffer,unsigned long value, unsigned short int base)
+print_str_fill(char **buffer, size_t bufsize, char *sizec,
+					const char *str, char c)
 {
-	const char zeichen[] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
-	static char sign = 0;
-
-	if(base <= 2 || base > 16)
-		return 0;
-
-	if(value < 0) {
-		sign = 1;
-		value *= -1;
-	}
+	int i, sizei, len;
+	char *bstart = *buffer;
 
-	if(value < base) {
-		if(sign) {
-			**buffer = '-';
+	sizei = strtoul(sizec, NULL, 10);
+	len = strlen(str);
+	if (sizei > len) {
+		for (i = 0;
+			(i < (sizei - len)) && ((*buffer - bstart) < bufsize);
+									i++) {
+			**buffer = c;
 			*buffer += 1;
-			sign = 0;
 		}
-		**buffer = zeichen[value];
-		*buffer += 1;
-	} else {
-		print_itoa(buffer, value / base, base);
-		**buffer = zeichen[(value % base)];
-		*buffer += 1;
 	}
-
 	return 1;
 }
 
+static int
+print_str(char **buffer, size_t bufsize, const char *str)
+{
+	char *bstart = *buffer;
+	size_t i;
+
+	for (i = 0; (i < strlen(str)) && ((*buffer - bstart) < bufsize); i++) {
+		**buffer = str[i];
+		*buffer += 1;
+	}
+	return 1;
+}
 
 static unsigned int
 print_intlen(unsigned long value, unsigned short int base)
 {
 	int i = 0;
 
-	while(value > 0) {
+	while (value > 0) {
 		value /= base;
 		i++;
 	}
-	if(i == 0) i = 1;
+	if (i == 0)
+		i = 1;
 	return i;
 }
 
+static int
+print_itoa(char **buffer, size_t bufsize, unsigned long value,
+					unsigned short base, bool upper)
+{
+	const char zeichen[] = {'0','1','2','3','4','5','6','7','8','9','a','b','c','d','e','f'};
+	char c;
+	size_t i, len;
+
+	if(base <= 2 || base > 16)
+		return 0;
+
+	len = i = print_intlen(value, base);
+
+	/* Don't print to buffer if bufsize is not enough. */
+	if (len > bufsize)
+		return 0;
+
+	do {
+		c = zeichen[value % base];
+		if (upper)
+			c = toupper(c);
+
+		(*buffer)[--i] = c;
+		value /= base;
+	} while(value);
+
+	*buffer += len;
+
+	return 1;
+}
+
+
 
 static int
-print_fill(char **buffer, char *sizec, unsigned long size, unsigned short int base, char c, int optlen)
+print_fill(char **buffer, size_t bufsize, char *sizec, unsigned long size,
+				unsigned short int base, char c, int optlen)
 {
 	int i, sizei, len;
+	char *bstart = *buffer;
 
 	sizei = strtoul(sizec, NULL, 10);
  	len = print_intlen(size, base) + optlen;
-	if(sizei > len) {
-		for(i = 0; i < (sizei - len); i++) {
+	if (sizei > len) {
+		for (i = 0;
+			(i < (sizei - len)) && ((*buffer - bstart) < bufsize);
+									i++) {
 			**buffer = c;
 			*buffer += 1;
 		}
@@ -86,17 +124,18 @@ print_fill(char **buffer, char *sizec, unsigned long size, unsigned short int ba
 
 
 static int
-print_format(char **buffer, const char *format, void *var)
+print_format(char **buffer, size_t bufsize, const char *format, void *var)
 {
-	unsigned long start;
-	unsigned int i = 0, sizei = 0, len = 0, length_mod = sizeof(int);
+	char *start;
+	unsigned int i = 0, length_mod = sizeof(int);
 	unsigned long value = 0;
 	unsigned long signBit;
 	char *form, sizec[32];
 	char sign = ' ';
+	bool upper = false;
 
 	form  = (char *) format;
-	start = (unsigned long) *buffer;
+	start = *buffer;
 
 	form++;
 	if(*form == '0' || *form == '.') {
@@ -104,7 +143,7 @@ print_format(char **buffer, const char *format, void *var)
 		form++;
 	}
 
-	while(*form != '\0') {
+	while ((*form != '\0') && ((*buffer - start) < bufsize)) {
 		switch(*form) {
 			case 'u':
 			case 'd':
@@ -112,57 +151,59 @@ print_format(char **buffer, const char *format, void *var)
 				sizec[i] = '\0';
 				value = (unsigned long) var;
 				signBit = 0x1ULL << (length_mod * 8 - 1);
-				if (signBit & value) {
+				if ((*form != 'u') && (signBit & value)) {
 					**buffer = '-';
 					*buffer += 1;
 					value = (-(unsigned long)value) & convert[length_mod];
 				}
-				print_fill(buffer, sizec, value, 10, sign, 0);
-				print_itoa(buffer, value, 10);
+				print_fill(buffer, bufsize - (*buffer - start),
+						sizec, value, 10, sign, 0);
+				print_itoa(buffer, bufsize - (*buffer - start),
+							value, 10, upper);
 				break;
 			case 'X':
+				upper = true;
 			case 'x':
 				sizec[i] = '\0';
 				value = (unsigned long) var & convert[length_mod];
-				print_fill(buffer, sizec, value, 16, sign, 0);
-				print_itoa(buffer, value, 16);
+				print_fill(buffer, bufsize - (*buffer - start),
+						sizec, value, 16, sign, 0);
+				print_itoa(buffer, bufsize - (*buffer - start),
+							value, 16, upper);
 				break;
 			case 'O':
 			case 'o':
 				sizec[i] = '\0';
 				value = (long int) var & convert[length_mod];
-				print_fill(buffer, sizec, value, 8, sign, 0);
-				print_itoa(buffer, value, 8);
+				print_fill(buffer, bufsize - (*buffer - start),
+						sizec, value, 8, sign, 0);
+				print_itoa(buffer, bufsize - (*buffer - start),
+							value, 8, upper);
 				break;
 			case 'p':
 				sizec[i] = '\0';
-				print_fill(buffer, sizec, (unsigned long) var, 16, ' ', 2);
-				**buffer = '0';
-				*buffer += 1;	
-				**buffer = 'x';
-				*buffer += 1;
-				print_itoa(buffer,(unsigned long) var, 16);
+				print_fill(buffer, bufsize - (*buffer - start),
+					sizec, (unsigned long) var, 16, ' ', 2);
+				print_str(buffer, bufsize - (*buffer - start),
+									"0x");
+				print_itoa(buffer, bufsize - (*buffer - start),
+						(unsigned long) var, 16, upper);
 				break;
 			case 'c':
 				sizec[i] = '\0';
-				print_fill(buffer, sizec, 1, 10, ' ', 0);
+				print_fill(buffer, bufsize - (*buffer - start),
+							sizec, 1, 10, ' ', 0);
 				**buffer = (unsigned long) var;
 				*buffer += 1;
 				break;
 			case 's':
 				sizec[i] = '\0';
-				sizei = strtoul(sizec, NULL, 10);
-				len = strlen((char *) var);
-				if(sizei > len) {
-					for(i = 0; i < (sizei - len); i++) {
-						**buffer = ' ';
-						*buffer += 1;
-					}
-				}
-				for(i = 0; i < strlen((char *) var); i++) {
-					**buffer = ((char *) var)[i];
-					*buffer += 1;
-				}
+				print_str_fill(buffer,
+					bufsize - (*buffer - start), sizec,
+							(char *) var, ' ');
+
+				print_str(buffer, bufsize - (*buffer - start),
+								(char *) var);
 				break;
 			case 'l':
 				form++;
@@ -182,6 +223,9 @@ print_format(char **buffer, const char *format, void *var)
 					length_mod = sizeof(short int);
 				}
 				break;
+			case 'z':
+				length_mod = sizeof(size_t);
+				break;
 			default:
 				if(*form >= '0' && *form <= '9')
 					sizec[i++] = *form;
@@ -206,6 +250,16 @@ vsnprintf(char *buffer, size_t bufsize, const char *format, va_list arg)
 	bstart = buffer;
 	ptr = (char *) format;
 
+	/*
+	 * Return from here if size passed is zero, otherwise we would
+	 * overrun buffer while setting NULL character at the end.
+	 */
+	if (!buffer || !bufsize)
+		return 0;
+
+	/* Leave one space for NULL character */
+	bufsize--;
+
 	while(*ptr != '\0' && (buffer - bstart) < bufsize)
 	{
 		if(*ptr == '%') {
@@ -224,7 +278,9 @@ vsnprintf(char *buffer, size_t bufsize, const char *format, va_list arg)
 			if(*ptr == '%') {
 				*buffer++ = '%';
 			} else {
-				print_format(&buffer, formstr, va_arg(arg, void *));
+				print_format(&buffer,
+					bufsize - (buffer - bstart),
+					formstr, va_arg(arg, void *));
 			}
 			ptr++;
 		} else {
diff --git a/qemu/roms/SLOF/lib/libc/stdlib/rand.c b/qemu/roms/SLOF/lib/libc/stdlib/rand.c
index 87e3efd29..39f5a9a2c 100644
--- a/qemu/roms/SLOF/lib/libc/stdlib/rand.c
+++ b/qemu/roms/SLOF/lib/libc/stdlib/rand.c
@@ -18,7 +18,12 @@ static unsigned long _rand = 1;
 int
 rand(void)
 {
-	_rand = _rand * 25364735 + 34563;
+	_rand = _rand * 1237732973 + 34563;
 
-	return ((unsigned int) (_rand << 16) & RAND_MAX);
+	return ((unsigned int) (_rand >> 16) & RAND_MAX);
+}
+
+void srand(unsigned int seed)
+{
+	_rand = seed;
 }
diff --git a/qemu/roms/SLOF/lib/libhvcall/Makefile b/qemu/roms/SLOF/lib/libhvcall/Makefile
index 2a9b2d7d1..def532509 100644
--- a/qemu/roms/SLOF/lib/libhvcall/Makefile
+++ b/qemu/roms/SLOF/lib/libhvcall/Makefile
@@ -24,7 +24,7 @@ TARGET = ../libhvcall.a
 
 all: $(TARGET)
 
-SRCS = brokensc1.c
+SRCS = brokensc1.c rfill.c
 SRCSS = hvcall.S
 
 
diff --git a/qemu/roms/SLOF/lib/libhvcall/rfill.c b/qemu/roms/SLOF/lib/libhvcall/rfill.c
new file mode 100644
index 000000000..5407cd2a6
--- /dev/null
+++ b/qemu/roms/SLOF/lib/libhvcall/rfill.c
@@ -0,0 +1,38 @@
+/*****************************************************************************
+ * Fast function for filling cache-inhibited memory regions via h-call.
+ *
+ * Copyright 2015 Red Hat, Inc.
+ *
+ * This program and the accompanying materials
+ * are made available under the terms of the BSD License
+ * which accompanies this distribution, and is available at
+ * http://www.opensource.org/licenses/bsd-license.php
+ *
+ * Contributors:
+ *     Thomas Huth, Red Hat Inc. - initial implementation
+ *****************************************************************************/
+
+#include <cache.h>
+#include <string.h>
+
+typedef unsigned long type_u;
+
+/**
+ * fast_rfill is the implementation of the FAST_RFILL macro with h-calls.
+ * This is defined here instead of cache.h since we need a temporary
+ * local buffer - and that caused stack size problems in engine() when
+ * we used it directly in the FAST_RFILL macro.
+ */
+void fast_rfill(char *dst, long size, char pat)
+{
+	type_u buf[64];
+
+	memset(buf, pat, size < sizeof(buf) ? size : sizeof(buf));
+
+	while (size > sizeof(buf)) {
+		FAST_MRMOVE(buf, dst, sizeof(buf));
+		dst += sizeof(buf);
+		size -= sizeof(buf);
+	}
+	FAST_MRMOVE(buf, dst, size);
+}
diff --git a/qemu/roms/SLOF/lib/libnvram/envvar.c b/qemu/roms/SLOF/lib/libnvram/envvar.c
index 87aaf27a0..ee943fce5 100644
--- a/qemu/roms/SLOF/lib/libnvram/envvar.c
+++ b/qemu/roms/SLOF/lib/libnvram/envvar.c
@@ -17,7 +17,7 @@
 #include "nvram.h"
 
 /* returns the offset of the first byte after the searched envvar */
-static int get_past_env_pos(partition_t part, char *envvar)
+static int get_past_env_pos(partition_t part, char *envvar, int evlen)
 {
 	int offset, len;
 	static char temp[256];
@@ -32,7 +32,7 @@ static int get_past_env_pos(partition_t part, char *envvar)
 		while((data=nvram_read_byte(offset++)) && len < 256) {
 			temp[len++]=data;
 		}
-		if (!strncmp(envvar, temp, strlen(envvar))) {
+		if (!strncmp(envvar, temp, evlen)) {
 			return offset;
 		}
 	} while (len);
@@ -43,16 +43,16 @@ static int get_past_env_pos(partition_t part, char *envvar)
 /**
  * @param partition name of the envvar partition
  * @param envvar name of the environment variable
+ * @param evlen string length of the envvar parameter
  * @return pointer to temporary string containing the value of envvar
  */
-
-char *get_env(partition_t part, char *envvar)
+char *nvram_get_env(partition_t part, char *envvar, int evlen)
 {
 	static char temp[256+1];
 	int len, offset;
 	uint8_t data;
 
-	DEBUG("get_env %s... ", envvar);
+	DEBUG("nvram_get_env %p... ", envvar);
 	if(!part.addr) {
 		/* ERROR: No environment variable partition */
 		DEBUG("invalid partition.\n");
@@ -68,7 +68,7 @@ char *get_env(partition_t part, char *envvar)
 		}
 		temp[len]=0;
 
-		if (!strncmp(envvar, temp, strlen(envvar))) {
+		if (!strncmp(envvar, temp, evlen)) {
 			int pos=0;
 			while (temp[pos]!='=' && pos < len) pos++;
 			// DEBUG("value='%s'\n", temp+pos+1); 
@@ -100,7 +100,7 @@ static int find_last_envvar(partition_t part)
 	return -1;
 }
 
-int add_env(partition_t part, char *envvar, char *value)
+int nvram_add_env(partition_t part, char *envvar, int evlen, char *value, int vallen)
 {
 	int freespace, last, len, offset;
 	unsigned int i;
@@ -112,7 +112,7 @@ int add_env(partition_t part, char *envvar, char *value)
 	freespace = part.addr+part.len-last;
 
 	/* how long is the entry we want to write? */
-	len = strlen(envvar) + strlen(value) + 2;
+	len = evlen + vallen + 2;
 
 	if(freespace<len) {
 		// TODO try to increase partition size
@@ -121,18 +121,18 @@ int add_env(partition_t part, char *envvar, char *value)
 
 	offset=last;
 
-	for(i=0; i<strlen(envvar); i++)
+	for (i = 0; i < evlen; i++)
 		nvram_write_byte(offset++, envvar[i]);
 
 	nvram_write_byte(offset++, '=');
 
-	for(i=0; i<strlen(value); i++)
+	for (i = 0; i < vallen; i++)
 		nvram_write_byte(offset++, value[i]);
 
 	return 0;
 }
 
-int del_env(partition_t part, char *envvar)
+int nvram_del_env(partition_t part, char *envvar, int evlen)
 {
 	int last, current, pos, i;
 	char *buffer;
@@ -141,7 +141,7 @@ int del_env(partition_t part, char *envvar)
 		return -1;
 
 	last=find_last_envvar(part);
-	current = pos = get_past_env_pos(part, envvar);
+	current = pos = get_past_env_pos(part, envvar, evlen);
 	
 	// TODO is this really required?
 	/* go back to non-0 value */
@@ -168,25 +168,25 @@ int del_env(partition_t part, char *envvar)
 	return 0;
 }
 
-int set_env(partition_t part, char *envvar, char *value)
+int nvram_set_env(partition_t part, char *envvar, int evlen, char *value, int vallen)
 {
 	char *oldvalue, *buffer;
 	int last, current, buffersize, i;
 
-	DEBUG("set_env %lx[%lx]: %s=%s\n", part.addr, part.len, envvar, value);
+	DEBUG("nvram_set_env %lx[%lx]: %p=>%p\n", part.addr, part.len, envvar, value);
 
 	if(!part.addr)
 		return -1;
 
 	/* Check whether the environment variable exists already */
-	oldvalue = get_env(part, envvar);
+	oldvalue = nvram_get_env(part, envvar, evlen);
 
-	if(oldvalue==NULL)
-		return add_env(part, envvar, value);
+	if (oldvalue == NULL)
+		return nvram_add_env(part, envvar, evlen, value, vallen);
 
 
 	/* The value did not change. So we succeeded! */
-	if(!strncmp(oldvalue, value, strlen(value)+1))
+	if (strlen(oldvalue) == vallen && !strncmp(oldvalue, value, vallen))
 		return 0;
 
 	/* we need to overwrite environment variables, back them up first */
@@ -195,7 +195,7 @@ int set_env(partition_t part, char *envvar, char *value)
 
 	/* allocate a buffer */
 	last=find_last_envvar(part);
-	current=get_past_env_pos(part, envvar);
+	current = get_past_env_pos(part, envvar, evlen);
 	buffersize = last - current;
 	buffer=get_nvram_buffer(buffersize);
 	if(!buffer)
@@ -214,7 +214,7 @@ int set_env(partition_t part, char *envvar, char *value)
 	current++;
 
 	/* Write the new value */
-	for(i=0; i<(int)strlen(value); i++) {
+	for(i = 0; i < vallen; i++) {
 		nvram_write_byte(current++, value[i]);
 	}
 	
diff --git a/qemu/roms/SLOF/lib/libnvram/libnvram.code b/qemu/roms/SLOF/lib/libnvram/libnvram.code
index 723941d3e..8481f57f5 100644
--- a/qemu/roms/SLOF/lib/libnvram/libnvram.code
+++ b/qemu/roms/SLOF/lib/libnvram/libnvram.code
@@ -11,17 +11,6 @@
  *****************************************************************************/
 #include <nvram.h>
 
-#define STRING_INIT(str)	\
-	char str[255];		\
-	char * str##_address;	\
-	int  str##_length;
-
-#define STRING_FROM_STACK(str)				\
-	str##_length = TOS.u; POP;				\
-	str##_address = TOS.a; POP;			\
-	memcpy(str, str##_address, str##_length);	\
-	memset(str + str##_length, 0, 255 - str##_length);
-
 PRIM(nvram_X2d_c_X40)
 	unsigned int offset = TOS.u;
 	TOS.u=nvram_read_byte(offset);
@@ -80,21 +69,18 @@ MIRP
 
 /* get-named-nvram-partition ( name.addr name.len -- addr len FAILED? ) */
 PRIM(get_X2d_named_X2d_nvram_X2d_partition)
-	STRING_INIT(name)
 	partition_t partition;
+	int namelen = TOS.n; POP;
 
-	STRING_FROM_STACK(name)
-	partition = get_partition(-1, name);
+	partition = get_partition_fs(TOS.a, namelen);
 
 	if(partition.len && partition.len != -1) {
-		PUSH;
 		TOS.u = partition.addr;
 		PUSH;
 		TOS.u = partition.len;
 		PUSH;
 		TOS.u = 0; // FALSE
 	} else {
-		PUSH;
 		TOS.u = -1; // TRUE
 	}
 MIRP
@@ -103,23 +89,16 @@ MIRP
 
 /* new-nvram-partition ( type name.addr name.len len -- part.offs part.len FALSE | TRUE) */
 PRIM(new_X2d_nvram_X2d_partition)
-	int type, len, i, slen;
-	char name[12], *addr;
+	int type, len, namelen;
 	partition_t partition;
+	char *name;
 
 	len = TOS.u; POP;
-	slen = TOS.u; POP;
-	addr = (char *)TOS.u; POP;
+	namelen = TOS.u; POP;
+	name = (char *)TOS.u; POP;
 	type = TOS.u; POP;
 
-	for (i=0; i<12; i++) {
-		if(slen>i)
-			name[i]=addr[i];
-		else
-			name[i]=0;
-	}
-
-	partition=new_nvram_partition(type, name, len);
+	partition = new_nvram_partition_fs(type, name, namelen, len);
 
 	if(!partition.len) {
 		PUSH; TOS.u = -1; // TRUE
@@ -165,15 +144,17 @@ MIRP
 
 // ( part.start part.len name.addr name.len -- var.addr var.len TRUE | false )
 PRIM(internal_X2d_get_X2d_env)
-	STRING_INIT(name)
+	char *name;
+	int namelen;
 	partition_t part;
 	char *val;
 
-	STRING_FROM_STACK(name)
+	namelen = TOS.u; POP;
+	name = TOS.a; POP;
 	part.len = TOS.u; POP;
 	part.addr = TOS.u; POP;
 
-	val=get_env(part, name);
+	val = nvram_get_env(part, name, namelen);
 	if(val) {
 		PUSH; TOS.a = val;
 		PUSH; TOS.u = strlen(val);
@@ -185,17 +166,19 @@ MIRP
 
 // ( part.start part.len name.addr name.len val.addr val.len -- FALSE|TRUE)
 PRIM(internal_X2d_add_X2d_env)
-	STRING_INIT(name)
-	STRING_INIT(value)
+	char *name, *val;
+	int namelen, vallen;
 	partition_t part;
 	int ret;
 
-	STRING_FROM_STACK(value)
-	STRING_FROM_STACK(name)
+	vallen = TOS.u; POP;
+	val = TOS.a; POP;
+	namelen = TOS.u; POP;
+	name = TOS.a; POP;
 	part.len = TOS.u; POP;
 	part.addr = TOS.u; POP;
 
-	ret=add_env(part, name, value);
+	ret = nvram_add_env(part, name, namelen, val, vallen);
 	if(ret) {
 		PUSH; TOS.u = -1; // TRUE
 	} else {
@@ -205,15 +188,17 @@ MIRP
 
 // ( part.addr part.len name.addr name.len -- FALSE|TRUE)
 PRIM(internal_X2d_del_X2d_env)
-	STRING_INIT(name)
+	char *name;
+	int namelen;
 	partition_t part;
 	int ret;
 
-	STRING_FROM_STACK(name);
+	namelen = TOS.u; POP;
+	name = TOS.a; POP;
 	part.len = TOS.u; POP;
 	part.addr = TOS.u; POP;
 
-	ret=del_env(part, name);
+	ret = nvram_del_env(part, name, namelen);
 	if(ret) {
 		PUSH; TOS.u = -1; // TRUE
 	} else {
@@ -224,17 +209,19 @@ MIRP
 
 // internal-set-env ( part.addr part.len name.addr name.len val.addr val.len -- FALSE|TRUE)
 PRIM(internal_X2d_set_X2d_env)
-	STRING_INIT(name)
-	STRING_INIT(value)
+	char *name, *value;
+	int namelen, valuelen;
 	partition_t part;
 	int ret;
 
-	STRING_FROM_STACK(value)
-	STRING_FROM_STACK(name)
+	valuelen = TOS.u; POP;
+	value = TOS.a; POP;
+	namelen = TOS.u; POP;
+	name = TOS.a; POP;
 	part.len = TOS.u; POP;
 	part.addr = TOS.u; POP;
 
-	ret=set_env(part, name, value);
+	ret = nvram_set_env(part, name, namelen, value, valuelen);
 	if(ret) {
 		PUSH; TOS.u = -1; // TRUE
 	} else {
diff --git a/qemu/roms/SLOF/lib/libnvram/nvram.c b/qemu/roms/SLOF/lib/libnvram/nvram.c
index 5c1137669..473814e5c 100644
--- a/qemu/roms/SLOF/lib/libnvram/nvram.c
+++ b/qemu/roms/SLOF/lib/libnvram/nvram.c
@@ -358,6 +358,17 @@ partition_t get_partition(unsigned int type, char *name)
 	return ret;
 }
 
+/* Get partition specified by a Forth string */
+partition_t get_partition_fs(char *name, int namelen)
+{
+	char buf[namelen + 1];
+
+	memcpy(buf, name, namelen);
+	buf[namelen] = 0;
+
+	return get_partition(-1, buf);
+}
+
 void erase_nvram(int offset, int len)
 {
 	int i;
@@ -466,6 +477,22 @@ partition_t new_nvram_partition(int type, char *name, int len)
 	return new_part;
 }
 
+partition_t new_nvram_partition_fs(int type, char *name, int namelen, int len)
+{
+	char buf[13];
+	int i;
+
+	for (i = 0; i < 12; i++) {
+		if (i < namelen)
+			buf[i] = name[i];
+		else
+			buf[i] = 0;
+	}
+	buf[12] = 0;
+
+	return new_nvram_partition(type, buf, len);
+}
+
 /**
  * @param partition   partition structure pointing to the partition to wipe.
  */
diff --git a/qemu/roms/SLOF/lib/libnvram/nvram.h b/qemu/roms/SLOF/lib/libnvram/nvram.h
index fa6bdd425..73fe44424 100644
--- a/qemu/roms/SLOF/lib/libnvram/nvram.h
+++ b/qemu/roms/SLOF/lib/libnvram/nvram.h
@@ -51,9 +51,11 @@ char *get_nvram_buffer(int len);
 void free_nvram_buffer(char *buffer);
 int nvramlog_printf(const char* fmt, ...);
 partition_t get_partition(unsigned int type, char *name);
+partition_t get_partition_fs(char *name, int namelen);
 void erase_nvram(int offset, int len);
 int wipe_partition(partition_t partition, int header_only);
 partition_t new_nvram_partition(int type, char *name, int len);
+partition_t new_nvram_partition_fs(int type, char *name, int namelen, int len);
 int increase_nvram_partition_size(partition_t partition, int newsize);
 int clear_nvram_partition(partition_t part);
 int delete_nvram_partition(partition_t part);
@@ -65,9 +67,9 @@ void nvram_init(uint32_t store_token, uint32_t fetch_token,
 unsigned int get_nvram_size(void);
 
 /* envvar.c */
-char *get_env(partition_t part, char *envvar);
-int add_env(partition_t part, char *envvar, char *value);
-int del_env(partition_t part, char *envvar);
-int set_env(partition_t part, char *envvar, char *value);
+char *nvram_get_env(partition_t part, char *envvar, int evlen);
+int nvram_add_env(partition_t part, char *envvar, int evlen, char *value, int vallen);
+int nvram_del_env(partition_t part, char *envvar, int evlen);
+int nvram_set_env(partition_t part, char *envvar, int evlen, char *val, int vlen);
 
 #endif
diff --git a/qemu/roms/SLOF/lib/libusb/usb-core.c b/qemu/roms/SLOF/lib/libusb/usb-core.c
index 6719c5726..4c720ce2f 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-core.c
+++ b/qemu/roms/SLOF/lib/libusb/usb-core.c
@@ -383,8 +383,6 @@ int usb_hid_exit(void *vdev)
 	return true;
 }
 
-#define usb_get_intf_class(x) ((x & 0x00FF0000) >> 16)
-
 int usb_msc_init(void *vdev)
 {
 	struct usb_dev *dev;
@@ -420,7 +418,7 @@ int usb_msc_exit(void *vdev)
 	return true;
 }
 
-static int usb_msc_reset(struct usb_dev *dev)
+int usb_msc_reset(struct usb_dev *dev)
 {
 	struct usb_dev_req req;
 
@@ -477,7 +475,7 @@ static int usb_handle_device(struct usb_dev *dev, struct usb_dev_config_descr *c
 		case DESCR_TYPE_HUB:
 			break;
 		default:
-			printf("ptr %p desc_type %d\n", ptr, desc_type);
+			dprintf("ptr %p desc_type %d\n", ptr, desc_type);
 		}
 		ptr += desc_len;
 		len -= desc_len;
@@ -485,7 +483,7 @@ static int usb_handle_device(struct usb_dev *dev, struct usb_dev_config_descr *c
 	return true;
 }
 
-int setup_new_device(struct usb_dev *dev, unsigned int port)
+int usb_setup_new_device(struct usb_dev *dev, unsigned int port)
 {
 	struct usb_dev_descr descr;
 	struct usb_dev_config_descr cfg;
@@ -552,35 +550,6 @@ int setup_new_device(struct usb_dev *dev, unsigned int port)
 	if (!usb_handle_device(dev, &cfg, data, len))
 		goto fail_mem_free;
 
-	switch (usb_get_intf_class(dev->class)) {
-	case 3:
-		dprintf("HID found %06X\n", dev->class);
-		slof_usb_handle(dev);
-		break;
-	case 8:
-		dprintf("MASS STORAGE found %d %06X\n", dev->intf_num,
-			dev->class);
-		if ((dev->class & 0x50) != 0x50) { /* Bulk-only supported */
-			printf("Device not supported %06X\n", dev->class);
-			goto fail_mem_free;
-		}
-
-		if (!usb_msc_reset(dev)) {
-			printf("%s: bulk reset failed\n", __func__);
-			goto fail_mem_free;
-		}
-		SLOF_msleep(100);
-		slof_usb_handle(dev);
-		break;
-	case 9:
-		dprintf("HUB found\n");
-		slof_usb_handle(dev);
-		break;
-	default:
-		printf("USB Interface class -%x- Not supported\n", dev->class);
-		break;
-	}
-
 	SLOF_dma_free(data, len);
 	return true;
 fail_mem_free:
diff --git a/qemu/roms/SLOF/lib/libusb/usb-core.h b/qemu/roms/SLOF/lib/libusb/usb-core.h
index 7441979e9..a35df3485 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-core.h
+++ b/qemu/roms/SLOF/lib/libusb/usb-core.h
@@ -261,6 +261,8 @@ struct usb_hcd_ops {
 	unsigned int usb_type;
 };
 
+#define usb_get_intf_class(x) ((x & 0x00FF0000) >> 16)
+
 extern void usb_hcd_register(struct usb_hcd_ops *ops);
 extern struct usb_pipe *usb_get_pipe(struct usb_dev *dev, struct usb_ep_descr *ep,
 				char *buf, size_t len);
@@ -269,11 +271,12 @@ extern int usb_poll_intr(struct usb_pipe *pipe, uint8_t *buf);
 extern int usb_send_ctrl(struct usb_pipe *pipe, struct usb_dev_req *req, void *data);
 extern struct usb_dev *usb_devpool_get(void);
 extern void usb_devpool_put(struct usb_dev *);
-extern int setup_new_device(struct usb_dev *dev, unsigned int port);
-extern int slof_usb_handle(struct usb_dev *dev);
+extern int usb_setup_new_device(struct usb_dev *dev, unsigned int port);
+extern void usb_slof_populate_new_device(struct usb_dev *dev);
 extern int usb_dev_populate_pipe(struct usb_dev *dev, struct usb_ep_descr *ep,
 				void *buf, size_t len);
 extern int usb_hid_kbd_init(struct usb_dev *dev);
 extern int usb_hid_kbd_exit(struct usb_dev *dev);
+extern int usb_msc_reset(struct usb_dev *dev);
 extern void usb_msc_resetrecovery(struct usb_dev *dev);
 #endif
diff --git a/qemu/roms/SLOF/lib/libusb/usb-ehci.c b/qemu/roms/SLOF/lib/libusb/usb-ehci.c
index 4cca0da15..60af9e101 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-ehci.c
+++ b/qemu/roms/SLOF/lib/libusb/usb-ehci.c
@@ -79,7 +79,9 @@ static int ehci_hub_check_ports(struct ehci_hcd *ehcd)
 			dprintf("usb-ehci: allocated device %p\n", dev);
 			dev->hcidev = ehcd->hcidev;
 			dev->speed = USB_HIGH_SPEED; /* TODO: Check for Low/Full speed device */
-			if (!setup_new_device(dev, i))
+			if (usb_setup_new_device(dev, i))
+				usb_slof_populate_new_device(dev);
+			else
 				printf("usb-ehci: unable to setup device on port %d\n", i);
 		}
 	}
diff --git a/qemu/roms/SLOF/lib/libusb/usb-hid.c b/qemu/roms/SLOF/lib/libusb/usb-hid.c
index f0cab8a69..ac6616aba 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-hid.c
+++ b/qemu/roms/SLOF/lib/libusb/usb-hid.c
@@ -28,6 +28,10 @@
 #define HID_REQ_SET_IDLE                0x0A
 #define HID_REQ_SET_PROTOCOL            0x0B
 
+//key position for latin letters
+#define KEYP_LATIN_A 4
+#define KEYP_LATIN_Z 29
+
 //#define KEY_DEBUG
 
 /* HID SPEC - 7.2.6 Set_Protocol Request */
@@ -83,6 +87,8 @@ uint8_t set_leds;
 const uint8_t *key_std       = NULL;
 const uint8_t *key_std_shift = NULL;
 
+uint8_t ctrl; /* modifiers */
+
 /**
  * read character from Keyboard-Buffer
  *
@@ -111,6 +117,16 @@ static void write_key(uint8_t key)
 }
 
 /**
+ * Checks if keypos is a latin key
+ * @param  keypos
+ * @return -
+ */
+static bool is_latin(uint8_t keypos)
+{
+	return keypos >= KEYP_LATIN_A && keypos <= KEYP_LATIN_Z;
+}
+
+/**
  * Convert keyboard usage-ID to ANSI-Code
  *
  * @param   Ctrl=Modifier Byte
@@ -120,22 +136,24 @@ static void write_key(uint8_t key)
 static void get_char(uint8_t ctrl, uint8_t keypos)
 {
 	uint8_t ch;
+	bool caps = false;
 
 #ifdef KEY_DEBUG
 	printf("pos %02X\n", keypos);
 #endif
 
 	if (set_leds & LED_CAPS_LOCK)	                /* is CAPS Lock set ? */
-		ctrl |= MODIFIER_SHIFT;	                    /* simulate shift */
+		caps = true;
 
-	if (ctrl == 0) {
+	/* caps is a shift only for latin chars */
+	if ((!caps && ctrl == 0) || (caps && !is_latin(keypos))) {
 		ch = key_std[keypos];
 		if (ch != 0)
 			write_key(ch);
 		return;
 	}
 
-	if (ctrl & MODIFIER_SHIFT) {
+	if ((ctrl & MODIFIER_SHIFT) || caps) {
 		ch = key_std_shift[keypos];
 		if (ch != 0)
 			write_key(ch);
@@ -187,36 +205,38 @@ static void check_key_code(uint8_t *buf)
 					set_leds ^= LED_CAPS_LOCK;
 					break;
 
+				case 0x36:		                /*Shift pressed*/
+					ctrl |= MODIFIER_SHIFT;
+					break;
+				case 0xb6:		                /*Shift unpressed*/
+					ctrl &= ~MODIFIER_SHIFT;
+					break;
 				case 0x3a:	                        /* F1 */
 					write_key(0x1b);
 					write_key(0x5b);
-					write_key(0x31);
-					write_key(0x31);
-					write_key(0x7e);
+					write_key(0x4f);
+					write_key(0x50);
 					break;
 
 				case 0x3b:		                /* F2 */
 					write_key(0x1b);
 					write_key(0x5b);
-					write_key(0x31);
-					write_key(0x32);
-					write_key(0x7e);
+					write_key(0x4f);
+					write_key(0x51);
 					break;
 
 				case 0x3c:
 					write_key(0x1b);               /* F3 */
 					write_key(0x5b);
-					write_key(0x31);
-					write_key(0x33);
-					write_key(0x7e);
+					write_key(0x4f);
+					write_key(0x52);
 					break;
 
 				case 0x3d:
 					write_key(0x1b);		/* F4 */
 					write_key(0x5b);
-					write_key(0x31);
-					write_key(0x34);
-					write_key(0x7e);
+					write_key(0x4f);
+					write_key(0x53);
 					break;
 
 				case 0x3e:
@@ -254,7 +274,7 @@ static void check_key_code(uint8_t *buf)
 				case 0x42:
 					write_key(0x1b);		/* F9 */
 					write_key(0x5b);
-					write_key(0x31);
+					write_key(0x32);
 					write_key(0x30);
 					write_key(0x7e);
 					break;
@@ -262,7 +282,7 @@ static void check_key_code(uint8_t *buf)
 				case 0x43:
 					write_key(0x1b);	       /* F10 */
 					write_key(0x5b);
-					write_key(0x31);
+					write_key(0x32);
 					write_key(0x31);
 					write_key(0x7e);
 					break;
@@ -270,7 +290,7 @@ static void check_key_code(uint8_t *buf)
 				case 0x44:
 					write_key(0x1b);	       /* F11 */
 					write_key(0x5b);
-					write_key(0x31);
+					write_key(0x32);
 					write_key(0x33);
 					write_key(0x7e);
 					break;
@@ -278,7 +298,7 @@ static void check_key_code(uint8_t *buf)
 				case 0x45:
 					write_key(0x1b);	       /* F12 */
 					write_key(0x5b);
-					write_key(0x31);
+					write_key(0x32);
 					write_key(0x34);
 					write_key(0x7e);
 					break;
@@ -290,36 +310,34 @@ static void check_key_code(uint8_t *buf)
 				case 0x49:
 					write_key(0x1b);	       /* INS */
 					write_key(0x5b);
-					write_key(0x31);
+					write_key(0x32);
 					write_key(0x7e);
 					break;
 
 				case 0x4a:
 					write_key(0x1b);	      /* HOME */
-					write_key(0x5b);
-					write_key(0x32);
-					write_key(0x7e);
+					write_key(0x4f);
+					write_key(0x48);
 					break;
 
 				case 0x4b:
 					write_key(0x1b);	      /* PgUp */
 					write_key(0x5b);
-					write_key(0x33);
+					write_key(0x35);
 					write_key(0x7e);
 					break;
 
 				case 0x4c:
 					write_key(0x1b);	       /* DEL */
 					write_key(0x5b);
-					write_key(0x34);
+					write_key(0x33);
 					write_key(0x7e);
 					break;
 
 				case 0x4d:
 					write_key(0x1b);	       /* END */
-					write_key(0x5b);
-					write_key(0x35);
-					write_key(0x7e);
+					write_key(0x4f);
+					write_key(0x46);
 					break;
 
 				case 0x4e:
@@ -443,11 +461,8 @@ unsigned char usb_key_available(void *dev)
 
 unsigned char usb_read_keyb(void *vdev)
 {
-	if (!vdev)
-		return false;
-
-	while (usb_poll_key(vdev)) {
-		/* loop for all pending keys */
-	}
-	return read_key();
+	if (usb_key_available(vdev))
+		return read_key();
+	else
+		return 0;
 }
diff --git a/qemu/roms/SLOF/lib/libusb/usb-hub.c b/qemu/roms/SLOF/lib/libusb/usb-hub.c
index 7059cd019..bb8a30915 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-hub.c
+++ b/qemu/roms/SLOF/lib/libusb/usb-hub.c
@@ -175,7 +175,9 @@ unsigned int usb_hub_init(void *hubdev)
 			newdev = usb_devpool_get();
 			dprintf("usb-hub: allocated device %p\n", newdev);
 			newdev->hcidev = dev->hcidev;
-			if (!setup_new_device(newdev, i))
+			if (usb_setup_new_device(newdev, i))
+				usb_slof_populate_new_device(newdev);
+			else
 				printf("usb-hub: unable to setup device on port %d\n", i);
 		}
 	}
diff --git a/qemu/roms/SLOF/lib/libusb/usb-ohci.c b/qemu/roms/SLOF/lib/libusb/usb-ohci.c
index 0e8400481..d06c754d1 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-ohci.c
+++ b/qemu/roms/SLOF/lib/libusb/usb-ohci.c
@@ -192,7 +192,9 @@ static void ohci_hub_check_ports(struct ohci_hcd *ohcd)
 			dev = usb_devpool_get();
 			dprintf("usb-ohci: Device reset, setting up %p\n", dev);
 			dev->hcidev = ohcd->hcidev;
-			if (!setup_new_device(dev, i))
+			if (usb_setup_new_device(dev, i))
+				usb_slof_populate_new_device(dev);
+			else
 				printf("usb-ohci: unable to setup device on port %d\n", i);
 		}
 		if (port_status & RH_PS_PESC) {
@@ -252,7 +254,7 @@ static int ohci_alloc_pipe_pool(struct ohci_hcd *ohcd)
 		return false;
 
 	ohcd->pool_phys = opipe_phys = SLOF_dma_map_in(opipe, OHCI_PIPE_POOL_SIZE, true);
-	dprintf("usb-ohci: %s opipe %x, opipe_phys %x size %d count %d\n",
+	dprintf("usb-ohci: %s opipe %p, opipe_phys %lx size %ld count %d\n",
 		__func__, opipe, opipe_phys, sizeof(*opipe), count);
 	/* Although an array, link them*/
 	for (i = 0, curr = opipe, prev = NULL; i < count; i++, curr++) {
@@ -446,7 +448,7 @@ again:
 	/* Interrupt is there, read from done_head pointer */
 	td_phys = (struct ohci_td *)(uint64_t) le32_to_cpu(hcca->done_head);
 	if (!td_phys) {
-		dprintf("Again td_phys null %ld\n");
+		dprintf("Again td_phys null\n");
 		goto again;
 	}
 	hcca->done_head = 0;
@@ -553,7 +555,7 @@ static int ohci_send_ctrl(struct usb_pipe *pipe, struct usb_dev_req *req, void *
 	attr = EDA_FADDR(pipe->dev->addr) | EDA_MPS(pipe->mps) | EDA_SKIP;
 	ohci_fill_ed(ed, PTR_U32(td_phys), td_next, attr, 0);
 	ed->tailp = 0; /* HACK */
-	dprintf("usb-ohci: %s - td_start %x td_end %x req %x\n", __func__,
+	dprintf("usb-ohci: %s - td_start %p td_end %lx req %lx\n", __func__,
 		td_phys, td_next, req_phys);
 	mb();
 	ed->attr &= cpu_to_le32(~EDA_SKIP);
@@ -642,7 +644,7 @@ static int ohci_transfer_bulk(struct usb_pipe *pipe, void *td_ptr,
 
 	td = tds = (struct ohci_td *) td_ptr;
 	td_phys = (long)td_phys_ptr;
-	dprintf("usb-ohci: %s pipe %p data_phys %p len %d DIR_IN %d td %p td_phys %p\n",
+	dprintf("usb-ohci: %s pipe %p data_phys %p len %d DIR_IN %d td %p td_phys %lx\n",
 		__func__, pipe, data_phys, datalen, dir, td, td_phys);
 
 	if (!tds) {
@@ -672,7 +674,7 @@ static int ohci_transfer_bulk(struct usb_pipe *pipe, void *td_ptr,
 		| EDA_SKIP | pipe->dev->speed | EDA_EP(pipe->epno);
 	td_next = ohci_get_td_phys(td, tds, td_phys);
 	ohci_fill_ed(ed, td_phys, td_next, attr, 0);
-	dprintf("usb-ohci: %s - tds %p td %p\n", __func__, td_phys, td_next);
+	dprintf("usb-ohci: %s - tds %lx td %lx\n", __func__, td_phys, td_next);
 	mb();
 	ed->attr &= cpu_to_le32(~EDA_SKIP);
 
@@ -778,7 +780,7 @@ static int ohci_get_pipe_intr(struct usb_pipe *pipe, struct ohci_hcd *ohcd,
 		td->attr = cpu_to_le32(TDA_DP_IN | TDA_ROUNDING | TDA_CC);
 		td->next_td = cpu_to_le32(td_next);
 		td->be = cpu_to_le32(PTR_U32(ptr) + mps - 1);
-		dprintf("td %x td++ %x ptr %x be %x\n",
+		dprintf("td %p td++ %x ptr %p be %x\n",
 			td, le32_to_cpu(td->next_td),
 			ptr, (PTR_U32(ptr) + mps - 1));
 	}
diff --git a/qemu/roms/SLOF/lib/libusb/usb-slof.c b/qemu/roms/SLOF/lib/libusb/usb-slof.c
index de841f0fb..ff070559a 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-slof.c
+++ b/qemu/roms/SLOF/lib/libusb/usb-slof.c
@@ -26,7 +26,7 @@
 #define dprintf(_x ...)
 #endif
 
-int slof_usb_handle(struct usb_dev *dev)
+static int slof_usb_handle(struct usb_dev *dev)
 {
 	struct slof_usb_dev sdev;
 	sdev.port = dev->port;
@@ -59,3 +59,35 @@ int slof_usb_handle(struct usb_dev *dev)
 	}
 	return true;
 }
+
+void usb_slof_populate_new_device(struct usb_dev *dev)
+{
+	switch (usb_get_intf_class(dev->class)) {
+	case 3:
+		dprintf("HID found %06X\n", dev->class);
+		slof_usb_handle(dev);
+		break;
+	case 8:
+		dprintf("MASS STORAGE found %d %06X\n", dev->intf_num,
+			dev->class);
+		if ((dev->class & 0x50) != 0x50) { /* Bulk-only supported */
+			printf("Device not supported %06X\n", dev->class);
+			break;
+		}
+
+		if (!usb_msc_reset(dev)) {
+			printf("%s: bulk reset failed\n", __func__);
+			break;
+		}
+		SLOF_msleep(100);
+		slof_usb_handle(dev);
+		break;
+	case 9:
+		dprintf("HUB found\n");
+		slof_usb_handle(dev);
+		break;
+	default:
+		printf("USB Interface class -%x- Not supported\n", dev->class);
+		break;
+	}
+}
diff --git a/qemu/roms/SLOF/lib/libusb/usb-xhci.c b/qemu/roms/SLOF/lib/libusb/usb-xhci.c
index 0c3d6e47f..858cd12f9 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-xhci.c
+++ b/qemu/roms/SLOF/lib/libusb/usb-xhci.c
@@ -225,11 +225,11 @@ static void xhci_handle_cmd_completion(struct xhci_hcd *xhcd,
 		xhcd->slot_id = 0;
 }
 
-static struct xhci_event_trb *xhci_poll_event(struct xhci_hcd *xhcd,
-					uint32_t event_type)
+static uint64_t xhci_poll_event(struct xhci_hcd *xhcd,
+				uint32_t event_type)
 {
 	struct xhci_event_trb *event;
-	uint64_t val;
+	uint64_t val, retval = 0;
 	uint32_t flags, time;
 	int index;
 
@@ -244,7 +244,7 @@ static struct xhci_event_trb *xhci_poll_event(struct xhci_hcd *xhcd,
 		mb();
 		flags = le32_to_cpu(event->flags);
 		if (time < SLOF_GetTimer())
-			return NULL;
+			return 0;
 	}
 
 	mb();
@@ -273,6 +273,7 @@ static struct xhci_event_trb *xhci_poll_event(struct xhci_hcd *xhcd,
 		break;
 	}
 	xhcd->ering.deq = (uint64_t) (event + 1);
+	retval = le64_to_cpu(event->addr);
 
 	event->addr = 0;
 	event->status = 0;
@@ -289,7 +290,11 @@ static struct xhci_event_trb *xhci_poll_event(struct xhci_hcd *xhcd,
 	dprintf("Update start %x deq %x index %d\n",
 		xhcd->ering.trbs_dma, val, index/sizeof(*event));
 	write_reg64(&xhcd->run_regs->irs[0].erdp, val);
-	return event;
+
+	if (retval == 0)
+		return (uint64_t)event;
+	else
+		return retval;
 }
 
 static void xhci_send_cmd(struct xhci_hcd *xhcd, uint32_t field1,
@@ -388,10 +393,12 @@ static void xhci_init_seg(struct xhci_seg *seg, uint32_t size, uint32_t type)
 	seg->deq = (uint64_t)seg->trbs;
 	memset((void *)seg->trbs, 0, size);
 
-	link =(struct xhci_link_trb *) (seg->trbs + seg->size - 1);
-	link->addr = cpu_to_le64(seg->trbs_dma);
-	link->field2 = 0;
-	link->field3 = cpu_to_le32(0x1 | TRB_CMD_TYPE(TRB_LINK));
+	if (type != TYPE_EVENT) {
+		link =(struct xhci_link_trb *) (seg->trbs + seg->size - 1);
+		link->addr = cpu_to_le64(seg->trbs_dma);
+		link->field2 = 0;
+		link->field3 = cpu_to_le32(0x1 | TRB_CMD_TYPE(TRB_LINK));
+	}
 	return;
 }
 
@@ -601,8 +608,10 @@ static bool xhci_alloc_dev(struct xhci_hcd *xhcd, uint32_t slot_id, uint32_t por
 	dev->port = newport;
 	dev->priv = xdev;
 	xdev->dev = dev;
-	if (setup_new_device(dev, newport))
+	if (usb_setup_new_device(dev, newport)) {
+		usb_slof_populate_new_device(dev);
 		return true;
+	}
 
 	xhci_free_ctx(&xdev->out_ctx, XHCI_CTX_BUF_SIZE);
 fail_control_seg:
@@ -616,6 +625,7 @@ static void xhci_free_dev(struct xhci_dev *xdev)
 {
 	xhci_free_seg(&xdev->bulk_in, XHCI_DATA_TRBS_SIZE);
 	xhci_free_seg(&xdev->bulk_out, XHCI_DATA_TRBS_SIZE);
+	xhci_free_seg(&xdev->intr, XHCI_INTR_TRBS_SIZE);
 	xhci_free_seg(&xdev->control, XHCI_CONTROL_TRBS_SIZE);
 	xhci_free_ctx(&xdev->in_ctx, XHCI_CTX_BUF_SIZE);
 	xhci_free_ctx(&xdev->out_ctx, XHCI_CTX_BUF_SIZE);
@@ -637,7 +647,25 @@ static bool usb3_dev_init(struct xhci_hcd *xhcd, uint32_t port)
 	return true;
 }
 
-static int xhci_hub_check_ports(struct xhci_hcd *xhcd)
+static int xhci_device_present(uint32_t portsc, uint32_t usb_ver)
+{
+	if (usb_ver == USB_XHCI) {
+		/* Device present and enabled state */
+		if ((portsc & PORTSC_CCS) &&
+			(portsc & PORTSC_PP) &&
+			(portsc & PORTSC_PED)) {
+			return true;
+		}
+	} else if (usb_ver == USB_EHCI) {
+		/* Device present and in disabled state */
+		if ((portsc & PORTSC_CCS) && (portsc & PORTSC_CSC))
+			return true;
+	}
+	return false;
+}
+
+static int xhci_port_scan(struct xhci_hcd *xhcd,
+			uint32_t usb_ver)
 {
 	uint32_t num_ports, portsc, i;
 	struct xhci_op_regs *op;
@@ -645,7 +673,7 @@ static int xhci_hub_check_ports(struct xhci_hcd *xhcd)
 	struct xhci_cap_regs *cap;
 	uint32_t xecp_off;
 	uint32_t *xecp_addr, *base;
-	uint32_t port_off = 1, port_cnt;
+	uint32_t port_off = 0, port_cnt;
 
 	dprintf("enter\n");
 
@@ -658,14 +686,14 @@ static int xhci_hub_check_ports(struct xhci_hcd *xhcd)
 	base = (uint32_t *)cap;
 	while (xecp_off > 0) {
 		xecp_addr = base + xecp_off;
-		dprintf(stderr, "xecp_off %d %p %p \n", xecp_off, base, xecp_addr);
+		dprintf("xecp_off %d %p %p \n", xecp_off, base, xecp_addr);
 
 		if (XHCI_XECP_CAP_ID(read_reg32(xecp_addr)) == XHCI_XECP_CAP_SP &&
-		    XHCI_XECP_CAP_SP_MJ(read_reg32(xecp_addr)) == 3 &&
+		    XHCI_XECP_CAP_SP_MJ(read_reg32(xecp_addr)) == usb_ver &&
 		    XHCI_XECP_CAP_SP_MN(read_reg32(xecp_addr)) == 0) {
 			port_cnt = XHCI_XECP_CAP_SP_PC(read_reg32(xecp_addr + 2));
 			port_off = XHCI_XECP_CAP_SP_PO(read_reg32(xecp_addr + 2));
-			dprintf(stderr, "PortCount %d Portoffset %d\n", port_cnt, port_off);
+			dprintf("PortCount %d Portoffset %d\n", port_cnt, port_off);
 		}
 		base = xecp_addr;
 		xecp_off = XHCI_XECP_NEXT_PTR(read_reg32(xecp_addr));
@@ -675,10 +703,8 @@ static int xhci_hub_check_ports(struct xhci_hcd *xhcd)
 	for (i = (port_off - 1); i < (port_off + port_cnt - 1); i++) {
 		prs = &op->prs[i];
 		portsc = read_reg32(&prs->portsc);
-		if ((portsc & PORTSC_CCS) &&
-			(portsc & PORTSC_PP) &&
-			(portsc & PORTSC_PED)) {
-			/* Device present and enabled */
+		if (xhci_device_present(portsc, usb_ver)) {
+			/* Device present */
 			dprintf("Device present on port %d\n", i);
 			/* Reset the port */
 			portsc = read_reg32(&prs->portsc);
@@ -701,6 +727,11 @@ static int xhci_hub_check_ports(struct xhci_hcd *xhcd)
 	return true;
 }
 
+static int xhci_hub_check_ports(struct xhci_hcd *xhcd)
+{
+	return xhci_port_scan(xhcd, USB_XHCI) | xhci_port_scan(xhcd, USB_EHCI);
+}
+
 static bool xhci_hcd_init(struct xhci_hcd *xhcd)
 {
 	struct xhci_op_regs *op;
@@ -868,6 +899,18 @@ static bool xhci_hcd_exit(struct xhci_hcd *xhcd)
 		SLOF_dma_map_out(xhcd->dcbaap_dma, (void *)xhcd->dcbaap, XHCI_DCBAAP_MAX_SIZE);
 		SLOF_dma_free((void *)xhcd->dcbaap, XHCI_DCBAAP_MAX_SIZE);
 	}
+
+	/*
+	 * QEMU implementation of XHCI doesn't implement halt
+	 * properly. It basically says that it's halted immediately
+	 * but doesn't actually terminate ongoing activities and
+	 * DMAs. This needs to be fixed in QEMU.
+	 *
+	 * For now, wait for 50ms grace time till qemu stops using
+	 * this device.
+	 */
+	SLOF_msleep(50);
+
 	return true;
 }
 
@@ -1079,18 +1122,17 @@ static inline struct xhci_seg *xhci_pipe_get_seg(struct usb_pipe *pipe)
 static inline void *xhci_get_trb(struct xhci_seg *seg)
 {
 	uint64_t val, enq;
-	uint32_t size;
+	int index;
 	struct xhci_link_trb *link;
 
 	enq = val = seg->enq;
 	val = val + XHCI_TRB_SIZE;
-	size = seg->size * XHCI_TRB_SIZE;
-        /* TRBs being a cyclic buffer, here we cycle back to beginning. */
-	if ((val % size) == 0) {
+	index = (enq - (uint64_t)seg->trbs) / XHCI_TRB_SIZE + 1;
+	dprintf("%s: enq %llx, val %llx %x\n", __func__, enq, val, index);
+	/* TRBs being a cyclic buffer, here we cycle back to beginning. */
+	if (index == (seg->size - 1)) {
+		dprintf("%s: rounding \n", __func__);
 		seg->enq = (uint64_t)seg->trbs;
-		enq = seg->enq;
-		seg->enq = seg->enq + XHCI_TRB_SIZE;
-		val = 0;
 		seg->cycle_state ^= seg->cycle_state;
 		link = (struct xhci_link_trb *) (seg->trbs + seg->size - 1);
 		link->addr = cpu_to_le64(seg->trbs_dma);
@@ -1105,6 +1147,12 @@ static inline void *xhci_get_trb(struct xhci_seg *seg)
 	return (void *)enq;
 }
 
+static uint64_t xhci_get_trb_phys(struct xhci_seg *seg, uint64_t trb)
+{
+	return seg->trbs_dma + (trb - (uint64_t)seg->trbs);
+}
+
+static int usb_kb = false;
 static int xhci_transfer_bulk(struct usb_pipe *pipe, void *td, void *td_phys,
 			void *data, int datalen)
 {
@@ -1114,7 +1162,8 @@ static int xhci_transfer_bulk(struct usb_pipe *pipe, void *td, void *td_phys,
 	struct xhci_transfer_trb *trb;
 	struct xhci_db_regs *dbr;
 	int ret = true;
-	uint32_t slot_id, epno;
+	uint32_t slot_id, epno, time;
+	uint64_t trb_phys, event_phys;
 
 	if (!pipe->dev || !pipe->dev->hcidev) {
 		dprintf(" NULL pointer\n");
@@ -1139,13 +1188,26 @@ static int xhci_transfer_bulk(struct usb_pipe *pipe, void *td, void *td_phys,
 	}
 
 	trb = xhci_get_trb(seg);
+	trb_phys = xhci_get_trb_phys(seg, (uint64_t)trb);
 	fill_normal_trb(trb, (void *)data, datalen);
 
 	epno = xhci_get_epno(pipe);
 	write_reg32(&dbr->db[slot_id], epno);
-	if (!xhci_poll_event(xhcd, 0)) {
-		dprintf("Bulk failed\n");
-		ret = false;
+
+	time = SLOF_GetTimer() + USB_TIMEOUT;
+	while (1) {
+		event_phys = xhci_poll_event(xhcd, 0);
+		if (event_phys == trb_phys) {
+			break;
+		} else if (event_phys == 0) { /* polling timed out */
+			ret = false;
+			break;
+		} else
+			usb_kb = true;
+
+		/* transfer timed out */
+		if (time < SLOF_GetTimer())
+			return false;
 	}
 	trb->addr = 0;
 	trb->len = 0;
@@ -1214,7 +1276,8 @@ static void xhci_init_bulk_ep(struct usb_dev *dev, struct usb_pipe *pipe)
 
 	if (!seg->trbs) {
 		if (!xhci_alloc_seg(seg, XHCI_DATA_TRBS_SIZE, TYPE_BULK)) {
-			dprintf("Failed allocating seg\n");
+			printf("usb-xhci: allocation failed for bulk endpoint\n");
+			return;
 		}
 	} else {
 		xhci_init_seg(seg, XHCI_DATA_TRBS_SIZE, TYPE_BULK);
@@ -1235,6 +1298,61 @@ static void xhci_init_bulk_ep(struct usb_dev *dev, struct usb_pipe *pipe)
 	xpipe->seg = seg;
 }
 
+static int xhci_get_pipe_intr(struct usb_pipe *pipe,
+			struct xhci_hcd *xhcd,
+			char *buf, size_t len)
+{
+	struct xhci_dev *xdev;
+	struct xhci_seg *seg;
+	struct xhci_pipe *xpipe;
+	struct xhci_control_ctx *ctrl;
+	struct xhci_ep_ctx *ep;
+	uint32_t x_epno, val, type;
+	struct usb_dev *dev;
+	struct xhci_transfer_trb *trb;
+
+	dev = pipe->dev;
+	if (dev->class != DEV_HID_KEYB)
+		return false;
+
+	xdev = dev->priv;
+	pipe->mps = 8;
+	seg = xhci_pipe_get_seg(pipe);
+	xpipe = xhci_pipe_get_xpipe(pipe);
+	type = EP_INT_IN;
+	seg = &xdev->intr;
+
+	if (!seg->trbs) {
+		if (!xhci_alloc_seg(seg, XHCI_INTR_TRBS_SIZE, TYPE_BULK)) {
+			printf("usb-xhci: allocation failed for interrupt endpoint\n");
+			return false;
+		}
+	} else {
+		xhci_init_seg(seg, XHCI_EVENT_TRBS_SIZE, TYPE_BULK);
+	}
+
+	xpipe->buf = buf;
+	xpipe->buf_phys = SLOF_dma_map_in(buf, len, false);
+	xpipe->buflen = len;
+
+	ctrl = xhci_get_control_ctx(&xdev->in_ctx);
+	x_epno = xhci_get_epno(pipe);
+	ep = xhci_get_ep_ctx(&xdev->in_ctx, xdev->ctx_size, x_epno);
+	val = EP_TYPE(type) | MAX_BURST(0) | ERROR_COUNT(3) |
+		MAX_PACKET_SIZE(pipe->mps);
+	ep->field2 = cpu_to_le32(val);
+	ep->deq_addr = cpu_to_le64(seg->trbs_dma | seg->cycle_state);
+	ep->field4 = cpu_to_le32(8);
+	ctrl->a_flags = cpu_to_le32(BIT(x_epno) | 0x1);
+	ctrl->d_flags = 0;
+	xhci_configure_ep(xhcd, xdev->slot_id, xdev->in_ctx.dma_addr);
+	xpipe->seg = seg;
+
+	trb = xhci_get_trb(seg);
+	fill_normal_trb(trb, (void *)xpipe->buf_phys, pipe->mps);
+	return true;
+}
+
 static struct usb_pipe* xhci_get_pipe(struct usb_dev *dev, struct usb_ep_descr *ep, char *buf, size_t len)
 {
 	struct xhci_hcd *xhcd;
@@ -1264,6 +1382,12 @@ static struct usb_pipe* xhci_get_pipe(struct usb_dev *dev, struct usb_ep_descr *
 	new->dir = (ep->bEndpointAddress & 0x80) >> 7;
 	new->epno = ep->bEndpointAddress & 0x0f;
 
+	if (new->type == USB_EP_TYPE_INTR) {
+		if (!xhci_get_pipe_intr(new, xhcd, buf, len)) {
+			printf("usb-xhci: %s alloc_intr failed  %p\n",
+				__func__, new);
+		}
+	}
 	if (new->type == USB_EP_TYPE_BULK)
 		xhci_init_bulk_ep(dev, new);
 
@@ -1284,6 +1408,10 @@ static void xhci_put_pipe(struct usb_pipe *pipe)
 	if (pipe->type == USB_EP_TYPE_BULK) {
 		xpipe = xhci_pipe_get_xpipe(pipe);
 		xpipe->seg = NULL;
+	} else if (pipe->type == USB_EP_TYPE_INTR) {
+		xpipe = xhci_pipe_get_xpipe(pipe);
+		SLOF_dma_map_out(xpipe->buf_phys, xpipe->buf, xpipe->buflen);
+		xpipe->seg = NULL;
 	}
 	if (xhcd->end)
 		xhcd->end->next = pipe;
@@ -1298,6 +1426,51 @@ static void xhci_put_pipe(struct usb_pipe *pipe)
 	dprintf("usb-xhci: %s exit\n", __func__);
 }
 
+static int xhci_poll_intr(struct usb_pipe *pipe, uint8_t *data)
+{
+	struct xhci_transfer_trb *trb;
+	struct xhci_seg *seg;
+	struct xhci_pipe *xpipe;
+	struct xhci_dev *xdev;
+	struct xhci_hcd *xhcd;
+	struct xhci_db_regs *dbr;
+	uint32_t x_epno;
+	uint8_t *buf, ret = 1;
+
+	if (!pipe || !pipe->dev || !pipe->dev->hcidev)
+		return 0;
+	xdev = pipe->dev->priv;
+	xhcd = (struct xhci_hcd *)pipe->dev->hcidev->priv;
+	x_epno = xhci_get_epno(pipe);
+	seg = xhci_pipe_get_seg(pipe);
+	xpipe = xhci_pipe_get_xpipe(pipe);
+
+	if (usb_kb == true) {
+		/* This event was consumed by bulk transfer */
+		usb_kb = false;
+		goto skip_poll;
+	}
+	buf = xpipe->buf;
+	memset(buf, 0, 8);
+
+	mb();
+	/* Ring the doorbell - x_epno */
+	dbr = xhcd->db_regs;
+	write_reg32(&dbr->db[xdev->slot_id], x_epno);
+	if (!xhci_poll_event(xhcd, 0)) {
+		printf("poll intr failed\n");
+		return 0;
+	}
+	mb();
+	memcpy(data, buf, 8);
+
+skip_poll:
+	trb = xhci_get_trb(seg);
+	fill_normal_trb(trb, (void *)xpipe->buf_phys, pipe->mps);
+	mb();
+	return ret;
+}
+
 struct usb_hcd_ops xhci_ops = {
 	.name          = "xhci-hcd",
 	.init          = xhci_init,
@@ -1305,6 +1478,7 @@ struct usb_hcd_ops xhci_ops = {
 	.usb_type      = USB_XHCI,
 	.get_pipe      = xhci_get_pipe,
 	.put_pipe      = xhci_put_pipe,
+	.poll_intr     = xhci_poll_intr,
 	.send_ctrl     = xhci_send_ctrl,
 	.transfer_bulk = xhci_transfer_bulk,
 	.next          = NULL,
diff --git a/qemu/roms/SLOF/lib/libusb/usb-xhci.h b/qemu/roms/SLOF/lib/libusb/usb-xhci.h
index faeb07ead..3fc7e7889 100644
--- a/qemu/roms/SLOF/lib/libusb/usb-xhci.h
+++ b/qemu/roms/SLOF/lib/libusb/usb-xhci.h
@@ -266,6 +266,7 @@ struct xhci_seg {
 #define XHCI_EVENT_TRBS_SIZE   4096
 #define XHCI_CONTROL_TRBS_SIZE 4096
 #define XHCI_DATA_TRBS_SIZE    4096
+#define XHCI_INTR_TRBS_SIZE    4096
 #define XHCI_ERST_NUM_SEGS     1
 
 #define XHCI_MAX_BULK_SIZE    0xF000
@@ -349,6 +350,7 @@ struct xhci_dev {
 	struct xhci_ctx in_ctx;
 	struct xhci_ctx out_ctx;
 	struct xhci_seg control;
+	struct xhci_seg intr;
 	struct xhci_seg bulk_in;
 	struct xhci_seg bulk_out;
 	uint32_t ctx_size;
@@ -381,6 +383,9 @@ struct xhci_hcd {
 struct xhci_pipe {
 	struct usb_pipe pipe;
 	struct xhci_seg *seg;
+	void *buf;
+	long buf_phys;
+	uint32_t buflen;
 };
 
 #endif	/* USB_XHCI_H */
diff --git a/qemu/roms/SLOF/lib/libvirtio/p9.c b/qemu/roms/SLOF/lib/libvirtio/p9.c
index a55662994..0e5953031 100644
--- a/qemu/roms/SLOF/lib/libvirtio/p9.c
+++ b/qemu/roms/SLOF/lib/libvirtio/p9.c
@@ -143,7 +143,7 @@ int p9_transaction(p9_connection_t *connection)
 {
 	int rc;
 	int tx_size = GET_SIZE;
-	int rx_size = connection->message_size;
+	uint32_t rx_size = connection->message_size;
 
 	if (transact == NULL) {
 		return P9_NO_TRANSPORT;
diff --git a/qemu/roms/SLOF/lib/libvirtio/p9.h b/qemu/roms/SLOF/lib/libvirtio/p9.h
index 7df9ef441..3a35e80ed 100644
--- a/qemu/roms/SLOF/lib/libvirtio/p9.h
+++ b/qemu/roms/SLOF/lib/libvirtio/p9.h
@@ -33,7 +33,7 @@
 #define P9_PARTIAL_WALK			1
 
 typedef int (*p9_transact_t)(void *opaque, uint8_t *tx, int tx_size,
-		uint8_t *rx, int *rx_size);
+		uint8_t *rx, uint32_t *rx_size);
 
 typedef struct {
 	uint32_t message_size;
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio-9p.c b/qemu/roms/SLOF/lib/libvirtio/virtio-9p.c
index 5a5fd01da..fc5db9154 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio-9p.c
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio-9p.c
@@ -19,6 +19,7 @@
 #include "virtio-9p.h"
 #include "p9.h"
 
+static struct vqs vq;
 
 /**
  * Notes for 9P Server config:
@@ -86,7 +87,7 @@ static void dprint_buffer(const char *name, uint8_t *buffer, int length)
  * @return	0 = success, -ve = error.
  */
 static int virtio_9p_transact(void *opaque, uint8_t *tx, int tx_size, uint8_t *rx,
-			      int *rx_size)
+			      uint32_t *rx_size)
 {
 	struct virtio_device *dev = opaque;
 	struct vring_desc *desc;
@@ -165,6 +166,7 @@ int virtio_9p_init(struct virtio_device *dev, void *tx_buf, void *rx_buf,
 		   int buf_size)
 {
 	struct vring_avail *vq_avail;
+	int status = VIRTIO_STAT_ACKNOWLEDGE;
 
 	/* Check for double open */
 	if (__buf_size)
@@ -174,28 +176,31 @@ int virtio_9p_init(struct virtio_device *dev, void *tx_buf, void *rx_buf,
         dprintf("%s : device at %p\n", __func__, dev->base);
         dprintf("%s : type is %04x\n", __func__, dev->type);
 
-	/* Reset device */
-	// XXX That will clear the virtq base. We need to move
-	//     initializing it to here anyway
-	//
-	//	 virtio_reset_device(dev);
+	/* Keep it disabled until the driver is 1.0 capable */
+	dev->is_modern = false;
+
+	virtio_reset_device(dev);
 
 	/* Acknowledge device. */
-	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE);
+	virtio_set_status(dev, status);
 
 	/* Tell HV that we know how to drive the device. */
-	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE | VIRTIO_STAT_DRIVER);
+	status |= VIRTIO_STAT_DRIVER;
+	virtio_set_status(dev, status);
 
 	/* Device specific setup - we do not support special features */
 	virtio_set_guest_features(dev,  0);
 
+	if (virtio_queue_init_vq(dev, &vq, 0))
+		goto dev_error;
+
 	vq_avail = virtio_get_vring_avail(dev, 0);
 	vq_avail->flags = VRING_AVAIL_F_NO_INTERRUPT;
 	vq_avail->idx = 0;
 
 	/* Tell HV that setup succeeded */
-	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE | VIRTIO_STAT_DRIVER
-			  |VIRTIO_STAT_DRIVER_OK);
+	status |= VIRTIO_STAT_DRIVER_OK;
+	virtio_set_status(dev, status);
 
 	/* Setup 9P library. */
 	p9_reg_transport(virtio_9p_transact, dev,(uint8_t *)tx_buf,
@@ -203,6 +208,12 @@ int virtio_9p_init(struct virtio_device *dev, void *tx_buf, void *rx_buf,
 
 	dprintf("%s : complete\n", __func__);
 	return 0;
+
+dev_error:
+	printf("%s: failed\n", __func__);
+	status |= VIRTIO_STAT_FAILED;
+	virtio_set_status(dev, status);
+	return -1;
 }
 
 /**
@@ -228,7 +239,7 @@ void virtio_9p_shutdown(struct virtio_device *dev)
  * @param buffer[out]	Where to read the file to.
  * @return	+ve = amount of data read, -ve = error.
  */
-int virtio_9p_load(struct virtio_device *dev, const char *file_name, uint8_t *buffer)
+long virtio_9p_load(struct virtio_device *dev, const char *file_name, uint8_t *buffer)
 {
 	int rc;
 	uint16_t tag_len;
@@ -332,5 +343,5 @@ cleanup_connection:
 
 
 	dprintf("%s : complete, read %llu bytes\n", __func__, offset);
-	return rc == 0 ? offset : rc;
+	return rc == 0 ? (long)offset : rc;
 }
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio-9p.h b/qemu/roms/SLOF/lib/libvirtio/virtio-9p.h
index 4bf47d078..db2cf6f11 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio-9p.h
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio-9p.h
@@ -26,7 +26,7 @@ typedef struct {
 int virtio_9p_init(struct virtio_device *dev, void *tx_buf, void *rx_buf,
 		   int buf_size);
 void virtio_9p_shutdown(struct virtio_device *dev);
-int virtio_9p_load(struct virtio_device *dev, const char *file_name, uint8_t *buffer);
+long virtio_9p_load(struct virtio_device *dev, const char *file_name, uint8_t *buffer);
 
 
 #endif /* VIRTIO_9P_H_ */
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio-blk.c b/qemu/roms/SLOF/lib/libvirtio/virtio-blk.c
index 826f2ea0e..07ec1048f 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio-blk.c
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio-blk.c
@@ -13,10 +13,15 @@
 #include <stdio.h>
 #include <cpu.h>
 #include <helpers.h>
+#include <byteorder.h>
 #include "virtio.h"
 #include "virtio-blk.h"
+#include "virtio-internal.h"
 
 #define DEFAULT_SECTOR_SIZE 512
+#define DRIVER_FEATURE_SUPPORT  (VIRTIO_BLK_F_BLK_SIZE | VIRTIO_F_VERSION_1)
+
+static struct vqs vq;
 
 /**
  * Initialize virtio-block device.
@@ -27,39 +32,54 @@ virtioblk_init(struct virtio_device *dev)
 {
 	struct vring_avail *vq_avail;
 	int blk_size = DEFAULT_SECTOR_SIZE;
-	int features;
+	uint64_t features;
+	int status = VIRTIO_STAT_ACKNOWLEDGE;
 
 	/* Reset device */
-	// XXX That will clear the virtq base. We need to move
-	//     initializing it to here anyway
-	//
-	//	 virtio_reset_device(dev);
+	virtio_reset_device(dev);
 
 	/* Acknowledge device. */
-	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE);
+	virtio_set_status(dev, status);
 
 	/* Tell HV that we know how to drive the device. */
-	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER);
+	status |= VIRTIO_STAT_DRIVER;
+	virtio_set_status(dev, status);
+
+	if (dev->is_modern) {
+		/* Negotiate features and sets FEATURES_OK if successful */
+		if (virtio_negotiate_guest_features(dev, DRIVER_FEATURE_SUPPORT))
+			goto dev_error;
+
+		virtio_get_status(dev, &status);
+	} else {
+		/* Device specific setup - we support F_BLK_SIZE */
+		virtio_set_guest_features(dev,  VIRTIO_BLK_F_BLK_SIZE);
+	}
 
-	/* Device specific setup - we support F_BLK_SIZE */
-	virtio_set_guest_features(dev,  VIRTIO_BLK_F_BLK_SIZE);
+	if (virtio_queue_init_vq(dev, &vq, 0))
+		goto dev_error;
 
 	vq_avail = virtio_get_vring_avail(dev, 0);
-	vq_avail->flags = VRING_AVAIL_F_NO_INTERRUPT;
+	vq_avail->flags = virtio_cpu_to_modern16(dev, VRING_AVAIL_F_NO_INTERRUPT);
 	vq_avail->idx = 0;
 
 	/* Tell HV that setup succeeded */
-	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER
-				|VIRTIO_STAT_DRIVER_OK);
+	status |= VIRTIO_STAT_DRIVER_OK;
+	virtio_set_status(dev, status);
 
-	virtio_get_host_features(dev, &features);
+	features = virtio_get_host_features(dev);
 	if (features & VIRTIO_BLK_F_BLK_SIZE) {
 		blk_size = virtio_get_config(dev,
-				offset_of(struct virtio_blk_cfg, blk_size),
-				sizeof(blk_size));
+					     offset_of(struct virtio_blk_cfg, blk_size),
+					     sizeof(blk_size));
 	}
 
 	return blk_size;
+dev_error:
+	printf("%s: failed\n", __func__);
+	status |= VIRTIO_STAT_FAILED;
+	virtio_set_status(dev, status);
+	return 0;
 }
 
 
@@ -77,6 +97,19 @@ virtioblk_shutdown(struct virtio_device *dev)
 	virtio_reset_device(dev);
 }
 
+static void fill_blk_hdr(struct virtio_blk_req *blkhdr, bool is_modern,
+                         uint32_t type, uint32_t ioprio, uint32_t sector)
+{
+	if (is_modern) {
+		blkhdr->type = cpu_to_le32(type);
+		blkhdr->ioprio = cpu_to_le32(ioprio);
+		blkhdr->sector = cpu_to_le64(sector);
+	} else {
+		blkhdr->type = type;
+		blkhdr->ioprio = ioprio;
+		blkhdr->sector = sector;
+	}
+}
 
 /**
  * Read blocks
@@ -87,7 +120,7 @@ virtioblk_shutdown(struct virtio_device *dev)
  * @return number of blocks that have been read successfully
  */
 int
-virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt)
+virtioblk_read(struct virtio_device *dev, char *buf, uint64_t blocknum, long cnt)
 {
 	struct vring_desc *desc;
 	int id;
@@ -100,7 +133,7 @@ virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt)
 	struct vring_used *vq_used;		/* "Used" vring */
 	volatile uint8_t status = -1;
 	volatile uint16_t *current_used_idx;
-	uint16_t last_used_idx;
+	uint16_t last_used_idx, avail_idx;
 	int blk_size = DEFAULT_SECTOR_SIZE;
 
 	//printf("virtioblk_read: dev=%p buf=%p blocknum=%li count=%li\n",
@@ -128,41 +161,38 @@ virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt)
 	vq_avail = virtio_get_vring_avail(dev, 0);
 	vq_used = virtio_get_vring_used(dev, 0);
 
+	avail_idx = virtio_modern16_to_cpu(dev, vq_avail->idx);
+
 	last_used_idx = vq_used->idx;
 	current_used_idx = &vq_used->idx;
 
 	/* Set up header */
-	blkhdr.type = VIRTIO_BLK_T_IN | VIRTIO_BLK_T_BARRIER;
-	blkhdr.ioprio = 1;
-	blkhdr.sector = blocknum * blk_size / DEFAULT_SECTOR_SIZE;
+	fill_blk_hdr(&blkhdr, dev->is_modern, VIRTIO_BLK_T_IN | VIRTIO_BLK_T_BARRIER,
+		     1, blocknum * blk_size / DEFAULT_SECTOR_SIZE);
 
 	/* Determine descriptor index */
-	id = (vq_avail->idx * 3) % vq_size;
+	id = (avail_idx * 3) % vq_size;
 
 	/* Set up virtqueue descriptor for header */
 	desc = &vq_desc[id];
-	desc->addr = (uint64_t)&blkhdr;
-	desc->len = sizeof(struct virtio_blk_req);
-	desc->flags = VRING_DESC_F_NEXT;
-	desc->next = (id + 1) % vq_size;
+	virtio_fill_desc(desc, dev->is_modern, (uint64_t)&blkhdr,
+			 sizeof(struct virtio_blk_req),
+			 VRING_DESC_F_NEXT, (id + 1) % vq_size);
 
 	/* Set up virtqueue descriptor for data */
 	desc = &vq_desc[(id + 1) % vq_size];
-	desc->addr = (uint64_t)buf;
-	desc->len = cnt * blk_size;
-	desc->flags = VRING_DESC_F_NEXT | VRING_DESC_F_WRITE;
-	desc->next = (id + 2) % vq_size;
+	virtio_fill_desc(desc, dev->is_modern, (uint64_t)buf, cnt * blk_size,
+			 VRING_DESC_F_NEXT | VRING_DESC_F_WRITE,
+			 (id + 2) % vq_size);
 
 	/* Set up virtqueue descriptor for status */
 	desc = &vq_desc[(id + 2) % vq_size];
-	desc->addr = (uint64_t)&status;
-	desc->len = 1;
-	desc->flags = VRING_DESC_F_WRITE;
-	desc->next = 0;
+	virtio_fill_desc(desc, dev->is_modern, (uint64_t)&status, 1,
+			 VRING_DESC_F_WRITE, 0);
 
-	vq_avail->ring[vq_avail->idx % vq_size] = id;
+	vq_avail->ring[avail_idx % vq_size] = virtio_cpu_to_modern16 (dev, id);
 	mb();
-	vq_avail->idx += 1;
+	vq_avail->idx = virtio_cpu_to_modern16(dev, avail_idx + 1);
 
 	/* Tell HV that the queue is ready */
 	virtio_queue_notify(dev, 0);
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio-blk.h b/qemu/roms/SLOF/lib/libvirtio/virtio-blk.h
index ac8bf2896..2e7b5926b 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio-blk.h
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio-blk.h
@@ -55,6 +55,6 @@ struct virtio_blk_req {
 
 extern int virtioblk_init(struct virtio_device *dev);
 extern void virtioblk_shutdown(struct virtio_device *dev);
-extern int virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt);
+extern int virtioblk_read(struct virtio_device *dev, char *buf, uint64_t blocknum, long cnt);
 
 #endif  /* _VIRTIO_BLK_H */
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio-internal.h b/qemu/roms/SLOF/lib/libvirtio/virtio-internal.h
new file mode 100644
index 000000000..08662eab7
--- /dev/null
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio-internal.h
@@ -0,0 +1,48 @@
+/******************************************************************************
+ * Copyright (c) 2016 IBM Corporation
+ * All rights reserved.
+ * This program and the accompanying materials
+ * are made available under the terms of the BSD License
+ * which accompanies this distribution, and is available at
+ * http://www.opensource.org/licenses/bsd-license.php
+ *
+ * Contributors:
+ *     IBM Corporation - initial implementation
+ *****************************************************************************/
+
+#ifndef _LIBVIRTIO_INTERNAL_H
+#define _LIBVIRTIO_INTERNAL_H
+
+#include <byteorder.h>
+
+static inline uint16_t virtio_cpu_to_modern16(struct virtio_device *dev, uint16_t val)
+{
+	return dev->is_modern ? cpu_to_le16(val) : val;
+}
+
+static inline uint32_t virtio_cpu_to_modern32(struct virtio_device *dev, uint32_t val)
+{
+	return dev->is_modern ? cpu_to_le32(val) : val;
+}
+
+static inline uint64_t virtio_cpu_to_modern64(struct virtio_device *dev, uint64_t val)
+{
+	return dev->is_modern ? cpu_to_le64(val) : val;
+}
+
+static inline uint16_t virtio_modern16_to_cpu(struct virtio_device *dev, uint16_t val)
+{
+	return dev->is_modern ? le16_to_cpu(val) : val;
+}
+
+static inline uint32_t virtio_modern32_to_cpu(struct virtio_device *dev, uint32_t val)
+{
+	return dev->is_modern ? le32_to_cpu(val) : val;
+}
+
+static inline uint64_t virtio_modern64_to_cpu(struct virtio_device *dev, uint64_t val)
+{
+	return dev->is_modern ? le64_to_cpu(val) : val;
+}
+
+#endif /* _LIBVIRTIO_INTERNAL_H */
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio-net.c b/qemu/roms/SLOF/lib/libvirtio/virtio-net.c
index 99c19d952..fc620a201 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio-net.c
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio-net.c
@@ -26,6 +26,7 @@
 #include <byteorder.h>
 #include "virtio.h"
 #include "virtio-net.h"
+#include "virtio-internal.h"
 
 #undef DEBUG
 //#define DEBUG
@@ -37,22 +38,13 @@
 
 #define sync()  asm volatile (" sync \n" ::: "memory")
 
-/* PCI virtio header offsets */
-#define VIRTIOHDR_DEVICE_FEATURES       0
-#define VIRTIOHDR_GUEST_FEATURES        4
-#define VIRTIOHDR_QUEUE_ADDRESS         8
-#define VIRTIOHDR_QUEUE_SIZE            12
-#define VIRTIOHDR_QUEUE_SELECT          14
-#define VIRTIOHDR_QUEUE_NOTIFY          16
-#define VIRTIOHDR_DEVICE_STATUS         18
-#define VIRTIOHDR_ISR_STATUS            19
-#define VIRTIOHDR_DEVICE_CONFIG         20
-#define VIRTIOHDR_MAC_ADDRESS           20
+#define DRIVER_FEATURE_SUPPORT  (VIRTIO_NET_F_MAC | VIRTIO_F_VERSION_1)
 
 struct virtio_device virtiodev;
-struct vqs vq[2];     /* Information about virtqueues */
+static struct vqs vq_rx;     /* Information about receive virtqueues */
+static struct vqs vq_tx;     /* Information about transmit virtqueues */
 
-/* See Virtio Spec, appendix C, "Device Operation" */ 
+/* See Virtio Spec, appendix C, "Device Operation" */
 struct virtio_net_hdr {
 	uint8_t  flags;
 	uint8_t  gso_type;
@@ -63,6 +55,18 @@ struct virtio_net_hdr {
 	// uint16_t  num_buffers;	/* Only if VIRTIO_NET_F_MRG_RXBUF */
 };
 
+static unsigned int net_hdr_size;
+
+struct virtio_net_hdr_v1 {
+	uint8_t  flags;
+	uint8_t  gso_type;
+	le16  hdr_len;
+	le16  gso_size;
+	le16  csum_start;
+	le16  csum_offset;
+	le16  num_buffers;
+};
+
 static uint16_t last_rx_idx;	/* Last index in RX "used" ring */
 
 /**
@@ -72,15 +76,13 @@ static uint16_t last_rx_idx;	/* Last index in RX "used" ring */
  */
 static int virtionet_init_pci(struct virtio_device *dev)
 {
-	int i;
-
 	dprintf("virtionet: doing virtionet_init_pci!\n");
 
 	if (!dev)
 		return -1;
 
-	virtiodev.base = dev->base;
-	virtiodev.type = dev->type;
+	/* make a copy of the device structure */
+	memcpy(&virtiodev, dev, sizeof(struct virtio_device));
 
 	/* Reset device */
 	virtio_reset_device(&virtiodev);
@@ -90,29 +92,11 @@ static int virtionet_init_pci(struct virtio_device *dev)
 	 * second the transmit queue, and the forth is the control queue for
 	 * networking options.
 	 * We are only interested in the receive and transmit queue here. */
-
-	for (i=VQ_RX; i<=VQ_TX; i++) {
-		/* Select ring (0=RX, 1=TX): */
-		vq[i].id = i-VQ_RX;
-		ci_write_16(virtiodev.base+VIRTIOHDR_QUEUE_SELECT,
-			    cpu_to_le16(vq[i].id));
-
-		vq[i].size = le16_to_cpu(ci_read_16(virtiodev.base+VIRTIOHDR_QUEUE_SIZE));
-		vq[i].desc = SLOF_alloc_mem_aligned(virtio_vring_size(vq[i].size), 4096);
-		if (!vq[i].desc) {
-			printf("memory allocation failed!\n");
-			return -1;
-		}
-		memset(vq[i].desc, 0, virtio_vring_size(vq[i].size));
-		ci_write_32(virtiodev.base+VIRTIOHDR_QUEUE_ADDRESS,
-			    cpu_to_le32((long)vq[i].desc / 4096));
-		vq[i].avail = (void*)vq[i].desc
-				    + vq[i].size * sizeof(struct vring_desc);
-		vq[i].used = (void*)VQ_ALIGN((long)vq[i].avail
-				    + vq[i].size * sizeof(struct vring_avail));
-
-		dprintf("%i: vq.id = %llx\nvq.size =%x\n vq.avail =%p\nvq.used=%p\n",
-			i, vq[i].id, vq[i].size, vq[i].avail, vq[i].used);
+	if (virtio_queue_init_vq(dev, &vq_rx, VQ_RX) ||
+	    virtio_queue_init_vq(dev, &vq_tx, VQ_TX)) {
+		virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER
+				  |VIRTIO_STAT_FAILED);
+		return -1;
 	}
 
 	/* Acknowledge device. */
@@ -129,6 +113,7 @@ static int virtionet_init_pci(struct virtio_device *dev)
 static int virtionet_init(net_driver_t *driver)
 {
 	int i;
+	int status = VIRTIO_STAT_ACKNOWLEDGE | VIRTIO_STAT_DRIVER;
 
 	dprintf("virtionet_init(%02x:%02x:%02x:%02x:%02x:%02x)\n",
 		driver->mac_addr[0], driver->mac_addr[1],
@@ -139,60 +124,69 @@ static int virtionet_init(net_driver_t *driver)
 		return 0;
 
 	/* Tell HV that we know how to drive the device. */
-	virtio_set_status(&virtiodev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER);
-
-	/* Device specific setup - we do not support special features right now */
-	virtio_set_guest_features(&virtiodev,  0);
+	virtio_set_status(&virtiodev, status);
+
+	/* Device specific setup */
+	if (virtiodev.is_modern) {
+		if (virtio_negotiate_guest_features(&virtiodev, DRIVER_FEATURE_SUPPORT))
+			goto dev_error;
+		net_hdr_size = sizeof(struct virtio_net_hdr_v1);
+		virtio_get_status(&virtiodev, &status);
+	} else {
+		net_hdr_size = sizeof(struct virtio_net_hdr);
+		virtio_set_guest_features(&virtiodev,  0);
+	}
 
 	/* Allocate memory for one transmit an multiple receive buffers */
-	vq[VQ_RX].buf_mem = SLOF_alloc_mem((BUFFER_ENTRY_SIZE+sizeof(struct virtio_net_hdr))
+	vq_rx.buf_mem = SLOF_alloc_mem((BUFFER_ENTRY_SIZE+net_hdr_size)
 				   * RX_QUEUE_SIZE);
-	if (!vq[VQ_RX].buf_mem) {
+	if (!vq_rx.buf_mem) {
 		printf("virtionet: Failed to allocate buffers!\n");
-		virtio_set_status(&virtiodev, VIRTIO_STAT_FAILED);
-		return -1;
+		goto dev_error;
 	}
 
 	/* Prepare receive buffer queue */
 	for (i = 0; i < RX_QUEUE_SIZE; i++) {
-		struct vring_desc *desc;
+		uint64_t addr = (uint64_t)vq_rx.buf_mem
+			+ i * (BUFFER_ENTRY_SIZE+net_hdr_size);
+		uint32_t id = i*2;
 		/* Descriptor for net_hdr: */
-		desc = &vq[VQ_RX].desc[i*2];
-		desc->addr = (uint64_t)vq[VQ_RX].buf_mem
-			     + i * (BUFFER_ENTRY_SIZE+sizeof(struct virtio_net_hdr));
-		desc->len = sizeof(struct virtio_net_hdr);
-		desc->flags = VRING_DESC_F_NEXT | VRING_DESC_F_WRITE;
-		desc->next = i*2+1;
+		virtio_fill_desc(&vq_rx.desc[id], virtiodev.is_modern, addr, net_hdr_size,
+				 VRING_DESC_F_NEXT | VRING_DESC_F_WRITE, id + 1);
 
 		/* Descriptor for data: */
-		desc = &vq[VQ_RX].desc[i*2+1];
-		desc->addr = vq[VQ_RX].desc[i*2].addr + sizeof(struct virtio_net_hdr);
-		desc->len = BUFFER_ENTRY_SIZE;
-		desc->flags = VRING_DESC_F_WRITE;
-		desc->next = 0;
+		virtio_fill_desc(&vq_rx.desc[id+1], virtiodev.is_modern, addr + net_hdr_size,
+				 BUFFER_ENTRY_SIZE, VRING_DESC_F_WRITE, 0);
 
-		vq[VQ_RX].avail->ring[i] = i*2;
+		vq_rx.avail->ring[i] = virtio_cpu_to_modern16(&virtiodev, id);
 	}
 	sync();
-	vq[VQ_RX].avail->flags = VRING_AVAIL_F_NO_INTERRUPT;
-	vq[VQ_RX].avail->idx = RX_QUEUE_SIZE;
 
-	last_rx_idx = vq[VQ_RX].used->idx;
+	vq_rx.avail->flags = virtio_cpu_to_modern16(&virtiodev, VRING_AVAIL_F_NO_INTERRUPT);
+	vq_rx.avail->idx = virtio_cpu_to_modern16(&virtiodev, RX_QUEUE_SIZE);
+
+	last_rx_idx = virtio_modern16_to_cpu(&virtiodev, vq_rx.used->idx);
 
-	vq[VQ_TX].avail->flags = VRING_AVAIL_F_NO_INTERRUPT;
-	vq[VQ_TX].avail->idx = 0;
+	vq_tx.avail->flags = virtio_cpu_to_modern16(&virtiodev, VRING_AVAIL_F_NO_INTERRUPT);
+	vq_tx.avail->idx = 0;
 
 	/* Tell HV that setup succeeded */
-	virtio_set_status(&virtiodev, VIRTIO_STAT_ACKNOWLEDGE
-				      |VIRTIO_STAT_DRIVER
-				      |VIRTIO_STAT_DRIVER_OK);
+	status |= VIRTIO_STAT_DRIVER_OK;
+	virtio_set_status(&virtiodev, status);
 
 	/* Tell HV that RX queues are ready */
 	virtio_queue_notify(&virtiodev, VQ_RX);
 
 	driver->running = 1;
-
+	for(i = 0; i < (int)sizeof(driver->mac_addr); i++) {
+		driver->mac_addr[i] = virtio_get_config(&virtiodev, i, 1);
+	}
 	return 0;
+
+dev_error:
+	status |= VIRTIO_STAT_FAILED;
+	virtio_set_status(&virtiodev, status);
+	return -1;
 }
 
 
@@ -225,9 +219,10 @@ static int virtionet_term(net_driver_t *driver)
  */
 static int virtionet_xmit(char *buf, int len)
 {
-	struct vring_desc *desc;
-	int id;
-	static struct virtio_net_hdr nethdr;
+	int id, idx;
+	static struct virtio_net_hdr_v1 nethdr_v1;
+	static struct virtio_net_hdr nethdr_legacy;
+	void *nethdr = &nethdr_legacy;
 
 	if (len > BUFFER_ENTRY_SIZE) {
 		printf("virtionet: Packet too big!\n");
@@ -236,28 +231,25 @@ static int virtionet_xmit(char *buf, int len)
 
 	dprintf("\nvirtionet_xmit(packet at %p, %d bytes)\n", buf, len);
 
-	memset(&nethdr, 0, sizeof(nethdr));
+	if (virtiodev.is_modern)
+		nethdr = &nethdr_v1;
+
+	memset(nethdr, 0, net_hdr_size);
 
 	/* Determine descriptor index */
-	id = (vq[VQ_TX].avail->idx * 2) % vq[VQ_TX].size;
+	idx = virtio_modern16_to_cpu(&virtiodev, vq_tx.avail->idx);
+	id = (idx * 2) % vq_tx.size;
 
 	/* Set up virtqueue descriptor for header */
-	desc = &vq[VQ_TX].desc[id];
-	desc->addr = (uint64_t)&nethdr;
-	desc->len = sizeof(struct virtio_net_hdr);
-	desc->flags = VRING_DESC_F_NEXT;
-	desc->next = id + 1;
+	virtio_fill_desc(&vq_tx.desc[id], virtiodev.is_modern, (uint64_t)nethdr,
+			 net_hdr_size, VRING_DESC_F_NEXT, id + 1);
 
 	/* Set up virtqueue descriptor for data */
-	desc = &vq[VQ_TX].desc[id+1];
-	desc->addr = (uint64_t)buf;
-	desc->len = len;
-	desc->flags = 0;
-	desc->next = 0;
+	virtio_fill_desc(&vq_tx.desc[id+1], virtiodev.is_modern, (uint64_t)buf, len, 0, 0);
 
-	vq[VQ_TX].avail->ring[vq[VQ_TX].avail->idx % vq[VQ_TX].size] = id;
+	vq_tx.avail->ring[idx % vq_tx.size] = virtio_cpu_to_modern16(&virtiodev, id);
 	sync();
-	vq[VQ_TX].avail->idx += 1;
+	vq_tx.avail->idx = virtio_cpu_to_modern16(&virtiodev, idx + 1);
 	sync();
 
 	/* Tell HV that TX queue is ready */
@@ -272,23 +264,24 @@ static int virtionet_xmit(char *buf, int len)
  */
 static int virtionet_receive(char *buf, int maxlen)
 {
-	int len = 0;
-	int id;
+	uint32_t len = 0;
+	uint32_t id, idx;
 
-	if (last_rx_idx == vq[VQ_RX].used->idx) {
+	idx = virtio_modern16_to_cpu(&virtiodev, vq_rx.used->idx);
+
+	if (last_rx_idx == idx) {
 		/* Nothing received yet */
 		return 0;
 	}
 
-	id = (vq[VQ_RX].used->ring[last_rx_idx % vq[VQ_RX].size].id + 1)
-	     % vq[VQ_RX].size;
-	len = vq[VQ_RX].used->ring[last_rx_idx % vq[VQ_RX].size].len
-	      - sizeof(struct virtio_net_hdr);
-
-	dprintf("virtionet_receive() last_rx_idx=%i, vq[VQ_RX].used->idx=%i,"
-		" id=%i len=%i\n", last_rx_idx, vq[VQ_RX].used->idx, id, len);
+	id = (virtio_modern32_to_cpu(&virtiodev, vq_rx.used->ring[last_rx_idx % vq_rx.size].id) + 1)
+		% vq_rx.size;
+	len = virtio_modern32_to_cpu(&virtiodev, vq_rx.used->ring[last_rx_idx % vq_rx.size].len)
+		- net_hdr_size;
+	dprintf("virtionet_receive() last_rx_idx=%i, vq_rx.used->idx=%i,"
+		" id=%i len=%i\n", last_rx_idx, vq_rx.used->idx, id, len);
 
-	if (len > maxlen) {
+	if (len > (uint32_t)maxlen) {
 		printf("virtio-net: Receive buffer not big enough!\n");
 		len = maxlen;
 	}
@@ -298,7 +291,7 @@ static int virtionet_receive(char *buf, int maxlen)
 	printf("\n");
 	int i;
 	for (i=0; i<64; i++) {
-		printf(" %02x", *(uint8_t*)(vq[VQ_RX].desc[id].addr+i));
+		printf(" %02x", *(uint8_t*)(vq_rx.desc[id].addr+i));
 		if ((i%16)==15)
 			printf("\n");
 	}
@@ -306,14 +299,14 @@ static int virtionet_receive(char *buf, int maxlen)
 #endif
 
 	/* Copy data to destination buffer */
-	memcpy(buf, (void*)vq[VQ_RX].desc[id].addr, len);
+	memcpy(buf, (void *)virtio_modern64_to_cpu(&virtiodev, vq_rx.desc[id].addr), len);
 
 	/* Move indices to next entries */
 	last_rx_idx = last_rx_idx + 1;
 
-	vq[VQ_RX].avail->ring[vq[VQ_RX].avail->idx % vq[VQ_RX].size] = id - 1;
+	vq_rx.avail->ring[idx % vq_rx.size] = virtio_cpu_to_modern16(&virtiodev, id - 1);
 	sync();
-	vq[VQ_RX].avail->idx += 1;
+	vq_rx.avail->idx = virtio_cpu_to_modern16(&virtiodev, idx + 1);
 
 	/* Tell HV that RX queue entry is ready */
 	virtio_queue_notify(&virtiodev, VQ_RX);
@@ -321,7 +314,7 @@ static int virtionet_receive(char *buf, int maxlen)
 	return len;
 }
 
-net_driver_t *virtionet_open(char *mac_addr, int len, struct virtio_device *dev)
+net_driver_t *virtionet_open(struct virtio_device *dev)
 {
 	net_driver_t *driver;
 
@@ -331,7 +324,6 @@ net_driver_t *virtionet_open(char *mac_addr, int len, struct virtio_device *dev)
 		return NULL;
 	}
 
-	memcpy(driver->mac_addr, mac_addr, 6);
 	driver->running = 0;
 
 	if (virtionet_init_pci(dev))
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio-net.h b/qemu/roms/SLOF/lib/libvirtio/virtio-net.h
index bc7a189f7..c2d8ee336 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio-net.h
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio-net.h
@@ -23,19 +23,10 @@ enum {
 	VQ_TX = 1,	/* Transmit Queue */
 };
 
-struct vqs {
-	uint64_t id;	/* Queue ID */
-	uint32_t size;
-	void *buf_mem;
-	struct vring_desc *desc;
-	struct vring_avail *avail;
-	struct vring_used *used;
-};
-
-/* Device is identified by RX queue ID: */
-#define DEVICE_ID  vq[0].id
+/* VIRTIO_NET Feature bits */
+#define VIRTIO_NET_F_MAC       (1 << 5)
 
-extern net_driver_t *virtionet_open(char *mac_addr, int len, struct virtio_device *dev);
+extern net_driver_t *virtionet_open(struct virtio_device *dev);
 extern void virtionet_close(net_driver_t *driver);
 extern int virtionet_read(char *buf, int len);
 extern int virtionet_write(char *buf, int len);
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio-scsi.c b/qemu/roms/SLOF/lib/libvirtio/virtio-scsi.c
index 48289289a..04181b06c 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio-scsi.c
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio-scsi.c
@@ -15,6 +15,7 @@
 #include <cpu.h>
 #include <helpers.h>
 #include "virtio.h"
+#include "virtio-internal.h"
 #include "virtio-scsi.h"
 
 int virtioscsi_send(struct virtio_device *dev,
@@ -22,58 +23,54 @@ int virtioscsi_send(struct virtio_device *dev,
 		    struct virtio_scsi_resp_cmd *resp,
 		    int is_read, void *buf, uint64_t buf_len)
 {
-        struct vring_desc *desc;
-        struct vring_desc *vq_desc;		/* Descriptor vring */
-        struct vring_avail *vq_avail;		/* "Available" vring */
-        struct vring_used *vq_used;		/* "Used" vring */
-
-        volatile uint16_t *current_used_idx;
-        uint16_t last_used_idx;
-        int id;
-        uint32_t vq_size, time;
-
-        int vq = VIRTIO_SCSI_REQUEST_VQ;
-
-        vq_size = virtio_get_qsize(dev, vq);
-        vq_desc = virtio_get_vring_desc(dev, vq);
-        vq_avail = virtio_get_vring_avail(dev, vq);
-        vq_used = virtio_get_vring_used(dev, vq);
-
-        last_used_idx = vq_used->idx;
-        current_used_idx = &vq_used->idx;
-
-        /* Determine descriptor index */
-        id = (vq_avail->idx * 3) % vq_size;
-
-        desc = &vq_desc[id];
-        desc->addr = (uint64_t)req;
-        desc->len = sizeof(*req);
-        desc->flags = VRING_DESC_F_NEXT;
-        desc->next = (id + 1) % vq_size;
-
-        /* Set up virtqueue descriptor for data */
-        desc = &vq_desc[(id + 1) % vq_size];
-        desc->addr = (uint64_t)resp;
-        desc->len = sizeof(*resp);
-        desc->flags = VRING_DESC_F_NEXT | VRING_DESC_F_WRITE;
-        desc->next = (id + 2) % vq_size;
-
-        if (buf && buf_len) {
-                /* Set up virtqueue descriptor for status */
-                desc = &vq_desc[(id + 2) % vq_size];
-                desc->addr = (uint64_t)buf;
-                desc->len = buf_len;
-                desc->flags = is_read ? VRING_DESC_F_WRITE : 0;
-                desc->next = 0;
-        } else
-                desc->flags &= ~VRING_DESC_F_NEXT;
-
-        vq_avail->ring[vq_avail->idx % vq_size] = id;
-        mb();
-        vq_avail->idx += 1;
-
-        /* Tell HV that the vq is ready */
-        virtio_queue_notify(dev, vq);
+	struct vring_desc *vq_desc;		/* Descriptor vring */
+	struct vring_avail *vq_avail;		/* "Available" vring */
+	struct vring_used *vq_used;		/* "Used" vring */
+
+	volatile uint16_t *current_used_idx;
+	uint16_t last_used_idx, avail_idx;
+	int id;
+	uint32_t vq_size, time;
+
+	int vq = VIRTIO_SCSI_REQUEST_VQ;
+
+	vq_size = virtio_get_qsize(dev, vq);
+	vq_desc = virtio_get_vring_desc(dev, vq);
+	vq_avail = virtio_get_vring_avail(dev, vq);
+	vq_used = virtio_get_vring_used(dev, vq);
+
+	avail_idx = virtio_modern16_to_cpu(dev, vq_avail->idx);
+
+	last_used_idx = vq_used->idx;
+	current_used_idx = &vq_used->idx;
+
+	/* Determine descriptor index */
+	id = (avail_idx * 3) % vq_size;
+	virtio_fill_desc(&vq_desc[id], dev->is_modern, (uint64_t)req, sizeof(*req), VRING_DESC_F_NEXT,
+			 (id + 1) % vq_size);
+
+	/* Set up virtqueue descriptor for data */
+	if (buf && buf_len) {
+		virtio_fill_desc(&vq_desc[(id + 1) % vq_size], dev->is_modern,
+				 (uint64_t)resp, sizeof(*resp),
+				 VRING_DESC_F_NEXT | VRING_DESC_F_WRITE,
+				 (id + 2) % vq_size);
+		/* Set up virtqueue descriptor for status */
+		virtio_fill_desc(&vq_desc[(id + 2) % vq_size], dev->is_modern,
+				 (uint64_t)buf, buf_len,
+				 (is_read ? VRING_DESC_F_WRITE : 0), 0);
+	} else {
+		virtio_fill_desc(&vq_desc[(id + 1) % vq_size], dev->is_modern,
+				 (uint64_t)resp, sizeof(*resp),
+				 VRING_DESC_F_WRITE, 0);
+	}
+
+	vq_avail->ring[avail_idx % vq_size] = virtio_cpu_to_modern16(dev, id);
+	mb();
+	vq_avail->idx = virtio_cpu_to_modern16(dev, avail_idx + 1);
+
+	/* Tell HV that the vq is ready */
+	virtio_queue_notify(dev, vq);
 
 	/* Wait for host to consume the descriptor */
 	time = SLOF_GetTimer() + VIRTIO_TIMEOUT;
@@ -84,7 +81,7 @@ int virtioscsi_send(struct virtio_device *dev,
 			break;
 	}
 
-        return 0;
+	return 0;
 }
 
 /**
@@ -93,42 +90,55 @@ int virtioscsi_send(struct virtio_device *dev,
  */
 int virtioscsi_init(struct virtio_device *dev)
 {
-        struct vring_avail *vq_avail;
-        unsigned int idx = 0;
-        int qsize = 0;
-
-        /* Reset device */
-        // XXX That will clear the virtq base. We need to move
-        //     initializing it to here anyway
-        //
-        //     virtio_reset_device(dev);
-
-        /* Acknowledge device. */
-        virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE);
+	struct vring_avail *vq_avail;
+	unsigned int idx = 0;
+	int qsize = 0;
+	int status = VIRTIO_STAT_ACKNOWLEDGE;
 
-        /* Tell HV that we know how to drive the device. */
-        virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER);
-
-        /* Device specific setup - we do not support special features right now */
-        virtio_set_guest_features(dev,  0);
+	/* Reset device */
+	// XXX That will clear the virtq base. We need to move
+	//     initializing it to here anyway
+	//
+	//     virtio_reset_device(dev);
+
+	/* Acknowledge device. */
+	virtio_set_status(dev, status);
+
+	/* Tell HV that we know how to drive the device. */
+	status |= VIRTIO_STAT_DRIVER;
+	virtio_set_status(dev, status);
+
+	/* Device specific setup - we do not support special features right now */
+	if (dev->is_modern) {
+		if (virtio_negotiate_guest_features(dev, VIRTIO_F_VERSION_1))
+			goto dev_error;
+		virtio_get_status(dev, &status);
+	} else {
+		virtio_set_guest_features(dev, 0);
+	}
 
-        while(1) {
-                qsize = virtio_get_qsize(dev, idx);
-                if (!qsize)
-                        break;
-                virtio_vring_size(qsize);
+	while(1) {
+		qsize = virtio_get_qsize(dev, idx);
+		if (!qsize)
+			break;
+		virtio_vring_size(qsize);
 
-                vq_avail = virtio_get_vring_avail(dev, 0);
-                vq_avail->flags = VRING_AVAIL_F_NO_INTERRUPT;
-                vq_avail->idx = 0;
-                idx++;
-        }
+		vq_avail = virtio_get_vring_avail(dev, idx);
+		vq_avail->flags = virtio_cpu_to_modern16(dev, VRING_AVAIL_F_NO_INTERRUPT);
+		vq_avail->idx = 0;
+		idx++;
+	}
 
 	/* Tell HV that setup succeeded */
- 	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER
-                          |VIRTIO_STAT_DRIVER_OK);
+	status |= VIRTIO_STAT_DRIVER_OK;
+	virtio_set_status(dev, status);
 
 	return 0;
+dev_error:
+	printf("%s: failed\n", __func__);
+	status |= VIRTIO_STAT_FAILED;
+	virtio_set_status(dev, status);
+	return -1;
 }
 
 /**
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio.c b/qemu/roms/SLOF/lib/libvirtio/virtio.c
index f9c00a67a..f189941c7 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio.c
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio.c
@@ -10,10 +10,16 @@
  *     IBM Corporation - initial implementation
  *****************************************************************************/
 
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stddef.h>
 #include <cpu.h>
 #include <cache.h>
 #include <byteorder.h>
 #include "virtio.h"
+#include "helpers.h"
 
 /* PCI virtio header offsets */
 #define VIRTIOHDR_DEVICE_FEATURES	0
@@ -26,6 +32,166 @@
 #define VIRTIOHDR_ISR_STATUS		19
 #define VIRTIOHDR_DEVICE_CONFIG 	20
 
+/* PCI defines */
+#define PCI_BASE_ADDR_SPACE_IO	0x01
+#define PCI_BASE_ADDR_SPACE_64BIT 0x04
+#define PCI_BASE_ADDR_MEM_MASK	(~0x0fUL)
+#define PCI_BASE_ADDR_IO_MASK	(~0x03UL)
+
+#define PCI_BASE_ADDR_REG_0	0x10
+#define PCI_CONFIG_CAP_REG	0x34
+
+#define PCI_CAP_ID_VNDR		0x9
+
+/* Common configuration */
+#define VIRTIO_PCI_CAP_COMMON_CFG       1
+/* Notifications */
+#define VIRTIO_PCI_CAP_NOTIFY_CFG       2
+/* ISR access */
+#define VIRTIO_PCI_CAP_ISR_CFG          3
+/* Device specific configuration */
+#define VIRTIO_PCI_CAP_DEVICE_CFG       4
+/* PCI configuration access */
+#define VIRTIO_PCI_CAP_PCI_CFG          5
+
+#define VIRTIO_PCI_CAP_VNDR     0	  /* Generic PCI field: PCI_CAP_ID_VNDR */
+#define VIRTIO_PCI_CAP_NEXT     1	  /* Generic PCI field: next ptr. */
+#define VIRTIO_PCI_CAP_LEN      2	  /* Generic PCI field: capability length */
+#define VIRTIO_PCI_CAP_CFG_TYPE 3	  /* Identifies the structure. */
+#define VIRTIO_PCI_CAP_BAR      4	  /* Where to find it. */
+#define VIRTIO_PCI_CAP_OFFSET   8	  /* Offset within bar. */
+#define VIRTIO_PCI_CAP_LENGTH  12	  /* Length of the structure, in bytes. */
+
+struct virtio_dev_common {
+	le32 dev_features_sel;
+	le32 dev_features;
+	le32 drv_features_sel;
+	le32 drv_features;
+	le16 msix_config;
+	le16 num_queues;
+	uint8_t dev_status;
+	uint8_t cfg_generation;
+
+	le16 q_select;
+	le16 q_size;
+	le16 q_msix_vec;
+	le16 q_enable;
+	le16 q_notify_off;
+	le64 q_desc;
+	le64 q_avail;
+	le64 q_used;
+} __attribute__ ((packed));
+
+/* virtio 1.0 Spec: 4.1.3 PCI Device Layout
+ *
+ * Fields of different sizes are present in the device configuration regions.
+ * All 64-bit, 32-bit and 16-bit fields are little-endian. 64-bit fields are to
+ * be treated as two 32-bit fields, with low 32 bit part followed by the high 32
+ * bit part.
+ */
+static void virtio_pci_write64(void *addr, uint64_t val)
+{
+	uint32_t hi = (val >> 32) & 0xFFFFFFFF;
+	uint32_t lo = val & 0xFFFFFFFF;
+
+	ci_write_32(addr, cpu_to_le32(lo));
+	ci_write_32(addr + 4, cpu_to_le32(hi));
+}
+
+static uint64_t virtio_pci_read64(void *addr)
+{
+	uint64_t hi, lo;
+
+	lo = le32_to_cpu(ci_read_32(addr));
+	hi = le32_to_cpu(ci_read_32(addr + 4));
+	return (hi << 32) | lo;
+}
+
+static void virtio_cap_set_base_addr(struct virtio_cap *cap, uint32_t offset)
+{
+	uint64_t addr;
+
+	addr = SLOF_pci_config_read32(PCI_BASE_ADDR_REG_0 + 4 * cap->bar);
+	if (addr & PCI_BASE_ADDR_SPACE_IO) {
+		addr = addr & PCI_BASE_ADDR_IO_MASK;
+		cap->is_io = 1;
+	} else {
+		if (addr & PCI_BASE_ADDR_SPACE_64BIT)
+			addr |= SLOF_pci_config_read32(PCI_BASE_ADDR_REG_0 + 4 * (cap->bar + 1)) << 32;
+		addr = addr & PCI_BASE_ADDR_MEM_MASK;
+		cap->is_io = 0;
+	}
+	addr = (uint64_t)SLOF_translate_my_address((void *)addr);
+	cap->addr = (void *)addr + offset;
+}
+
+static void virtio_process_cap(struct virtio_device *dev, uint8_t cap_ptr)
+{
+	struct virtio_cap *cap;
+	uint8_t cfg_type, bar;
+	uint32_t offset;
+
+	cfg_type = SLOF_pci_config_read8(cap_ptr + VIRTIO_PCI_CAP_CFG_TYPE);
+	bar = SLOF_pci_config_read8(cap_ptr + VIRTIO_PCI_CAP_BAR);
+	offset = SLOF_pci_config_read32(cap_ptr + VIRTIO_PCI_CAP_OFFSET);
+
+	switch(cfg_type) {
+	case VIRTIO_PCI_CAP_COMMON_CFG:
+		cap = &dev->common;
+		break;
+	case VIRTIO_PCI_CAP_NOTIFY_CFG:
+		cap = &dev->notify;
+		dev->notify_off_mul = SLOF_pci_config_read32(cap_ptr + sizeof(struct virtio_cap));
+		break;
+	case VIRTIO_PCI_CAP_ISR_CFG:
+		cap = &dev->isr;
+		break;
+	case VIRTIO_PCI_CAP_DEVICE_CFG:
+		cap = &dev->device;
+		break;
+	default:
+		return;
+	}
+
+	cap->bar = bar;
+	virtio_cap_set_base_addr(cap, offset);
+	cap->cap_id = cfg_type;
+}
+
+/**
+ * Reads the virtio device capabilities, gets called from SLOF routines The
+ * function determines legacy or modern device and sets up driver registers
+ */
+struct virtio_device *virtio_setup_vd(void)
+{
+	uint8_t cap_ptr, cap_vndr;
+	struct virtio_device *dev;
+
+	dev = SLOF_alloc_mem(sizeof(struct virtio_device));
+	if (!dev) {
+		printf("Failed to allocate memory");
+		return NULL;
+	}
+
+	cap_ptr = SLOF_pci_config_read8(PCI_CONFIG_CAP_REG);
+	while (cap_ptr != 0) {
+		cap_vndr = SLOF_pci_config_read8(cap_ptr + VIRTIO_PCI_CAP_VNDR);
+		if (cap_vndr == PCI_CAP_ID_VNDR)
+			virtio_process_cap(dev, cap_ptr);
+		cap_ptr = SLOF_pci_config_read8(cap_ptr+VIRTIO_PCI_CAP_NEXT);
+	}
+
+	if (dev->common.cap_id && dev->notify.cap_id &&
+	    dev->isr.cap_id && dev->device.cap_id) {
+		dev->is_modern = 1;
+	} else {
+		dev->is_modern = 0;
+		dev->legacy.cap_id = 0;
+		dev->legacy.bar = 0;
+		virtio_cap_set_base_addr(&dev->legacy, 0);
+	}
+	return dev;
+}
 
 /**
  * Calculate ring size according to queue size number
@@ -33,9 +199,9 @@
 unsigned long virtio_vring_size(unsigned int qsize)
 {
 	return VQ_ALIGN(sizeof(struct vring_desc) * qsize +
-                        sizeof(struct vring_avail) + sizeof(uint16_t) * qsize) +
-               VQ_ALIGN(sizeof(struct vring_used) +
-                        sizeof(struct vring_used_elem) * qsize);
+			sizeof(struct vring_avail) + sizeof(uint16_t) * qsize) +
+		VQ_ALIGN(sizeof(struct vring_used) +
+			 sizeof(struct vring_used_elem) * qsize);
 }
 
 
@@ -45,15 +211,22 @@ unsigned long virtio_vring_size(unsigned int qsize)
  * @param   queue virtio queue number
  * @return  number of elements
  */
-int virtio_get_qsize(struct virtio_device *dev, int queue)
+unsigned int virtio_get_qsize(struct virtio_device *dev, int queue)
 {
-	int size = 0;
+	unsigned int size = 0;
 
-	if (dev->type == VIRTIO_TYPE_PCI) {
-		ci_write_16(dev->base+VIRTIOHDR_QUEUE_SELECT,
+	if (dev->is_modern) {
+		void *addr = dev->common.addr + offset_of(struct virtio_dev_common, q_select);
+		ci_write_16(addr, cpu_to_le16(queue));
+		eieio();
+		addr = dev->common.addr + offset_of(struct virtio_dev_common, q_size);
+		size = le16_to_cpu(ci_read_16(addr));
+	}
+	else {
+		ci_write_16(dev->legacy.addr+VIRTIOHDR_QUEUE_SELECT,
 			    cpu_to_le16(queue));
 		eieio();
-		size = le16_to_cpu(ci_read_16(dev->base+VIRTIOHDR_QUEUE_SIZE));
+		size = le16_to_cpu(ci_read_16(dev->legacy.addr+VIRTIOHDR_QUEUE_SIZE));
 	}
 
 	return size;
@@ -70,12 +243,19 @@ struct vring_desc *virtio_get_vring_desc(struct virtio_device *dev, int queue)
 {
 	struct vring_desc *desc = 0;
 
-	if (dev->type == VIRTIO_TYPE_PCI) {
-		ci_write_16(dev->base+VIRTIOHDR_QUEUE_SELECT,
+	if (dev->is_modern) {
+		void *q_sel = dev->common.addr + offset_of(struct virtio_dev_common, q_select);
+		void *q_desc = dev->common.addr + offset_of(struct virtio_dev_common, q_desc);
+
+		ci_write_16(q_sel, cpu_to_le16(queue));
+		eieio();
+		desc = (void *)(virtio_pci_read64(q_desc));
+	} else {
+		ci_write_16(dev->legacy.addr+VIRTIOHDR_QUEUE_SELECT,
 			    cpu_to_le16(queue));
 		eieio();
 		desc = (void*)(4096L *
-		   le32_to_cpu(ci_read_32(dev->base+VIRTIOHDR_QUEUE_ADDRESS)));
+			       le32_to_cpu(ci_read_32(dev->legacy.addr+VIRTIOHDR_QUEUE_ADDRESS)));
 	}
 
 	return desc;
@@ -90,8 +270,18 @@ struct vring_desc *virtio_get_vring_desc(struct virtio_device *dev, int queue)
  */
 struct vring_avail *virtio_get_vring_avail(struct virtio_device *dev, int queue)
 {
-	return (void*)((uint64_t)virtio_get_vring_desc(dev, queue)
-			+ virtio_get_qsize(dev, queue) * sizeof(struct vring_desc));
+	if (dev->is_modern) {
+		void *q_sel = dev->common.addr + offset_of(struct virtio_dev_common, q_select);
+		void *q_avail = dev->common.addr + offset_of(struct virtio_dev_common, q_avail);
+
+		ci_write_16(q_sel, cpu_to_le16(queue));
+		eieio();
+		return (void *)(virtio_pci_read64(q_avail));
+	}
+	else {
+		return (void*)((uint64_t)virtio_get_vring_desc(dev, queue) +
+			       virtio_get_qsize(dev, queue) * sizeof(struct vring_desc));
+	}
 }
 
 
@@ -103,20 +293,46 @@ struct vring_avail *virtio_get_vring_avail(struct virtio_device *dev, int queue)
  */
 struct vring_used *virtio_get_vring_used(struct virtio_device *dev, int queue)
 {
-	return (void*)VQ_ALIGN((uint64_t)virtio_get_vring_avail(dev, queue)
-				  + virtio_get_qsize(dev, queue)
-				    * sizeof(struct vring_avail));
+	if (dev->is_modern) {
+		void *q_sel = dev->common.addr + offset_of(struct virtio_dev_common, q_select);
+		void *q_used = dev->common.addr + offset_of(struct virtio_dev_common, q_used);
+
+		ci_write_16(q_sel, cpu_to_le16(queue));
+		eieio();
+		return (void *)(virtio_pci_read64(q_used));
+	} else {
+		return (void*)VQ_ALIGN((uint64_t)virtio_get_vring_avail(dev, queue)
+				       + virtio_get_qsize(dev, queue)
+				       * sizeof(struct vring_avail));
+	}
 }
 
+/**
+ * Fill the virtio ring descriptor depending on the legacy mode or virtio 1.0
+ */
+void virtio_fill_desc(struct vring_desc *desc, bool is_modern,
+                      uint64_t addr, uint32_t len,
+                      uint16_t flags, uint16_t next)
+{
+	if (is_modern) {
+		desc->addr = cpu_to_le64(addr);
+		desc->len = cpu_to_le32(len);
+		desc->flags = cpu_to_le16(flags);
+		desc->next = cpu_to_le16(next);
+	} else {
+		desc->addr = addr;
+		desc->len = len;
+		desc->flags = flags;
+		desc->next = next;
+	}
+}
 
 /**
  * Reset virtio device
  */
 void virtio_reset_device(struct virtio_device *dev)
 {
-	if (dev->type == VIRTIO_TYPE_PCI) {
-		ci_write_8(dev->base+VIRTIOHDR_DEVICE_STATUS, 0);
-	}
+	virtio_set_status(dev, 0);
 }
 
 
@@ -125,25 +341,64 @@ void virtio_reset_device(struct virtio_device *dev)
  */
 void virtio_queue_notify(struct virtio_device *dev, int queue)
 {
-	if (dev->type == VIRTIO_TYPE_PCI) {
-		ci_write_16(dev->base+VIRTIOHDR_QUEUE_NOTIFY, cpu_to_le16(queue));
+	if (dev->is_modern) {
+		void *q_sel = dev->common.addr + offset_of(struct virtio_dev_common, q_select);
+		void *q_ntfy = dev->common.addr + offset_of(struct virtio_dev_common, q_notify_off);
+		void *addr;
+		uint16_t q_notify_off;
+
+		ci_write_16(q_sel, cpu_to_le16(queue));
+		eieio();
+		q_notify_off = le16_to_cpu(ci_read_16(q_ntfy));
+		addr = dev->notify.addr + q_notify_off * dev->notify_off_mul;
+		ci_write_16(addr, cpu_to_le16(queue));
+	} else {
+		ci_write_16(dev->legacy.addr+VIRTIOHDR_QUEUE_NOTIFY, cpu_to_le16(queue));
 	}
 }
 
 /**
  * Set queue address
  */
-void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned int qaddr)
+void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned long qaddr)
+{
+	if (dev->is_modern) {
+		uint64_t q_desc = qaddr;
+		uint64_t q_avail;
+		uint64_t q_used;
+		uint32_t q_size = virtio_get_qsize(dev, queue);
+
+		virtio_pci_write64(dev->common.addr + offset_of(struct virtio_dev_common, q_desc), q_desc);
+		q_avail = q_desc + q_size * sizeof(struct vring_desc);
+		virtio_pci_write64(dev->common.addr + offset_of(struct virtio_dev_common, q_avail), q_avail);
+		q_used = VQ_ALIGN(q_avail + sizeof(struct vring_avail) + sizeof(uint16_t) * q_size);
+		virtio_pci_write64(dev->common.addr + offset_of(struct virtio_dev_common, q_used), q_used);
+		ci_write_16(dev->common.addr + offset_of(struct virtio_dev_common, q_enable), cpu_to_le16(1));
+	} else {
+		uint32_t val = qaddr;
+		val = val >> 12;
+		ci_write_16(dev->legacy.addr+VIRTIOHDR_QUEUE_SELECT,
+			    cpu_to_le16(queue));
+		eieio();
+		ci_write_32(dev->legacy.addr+VIRTIOHDR_QUEUE_ADDRESS,
+			    cpu_to_le32(val));
+	}
+}
+
+int virtio_queue_init_vq(struct virtio_device *dev, struct vqs *vq, unsigned int id)
 {
-        if (dev->type == VIRTIO_TYPE_PCI) {
-                uint32_t val = qaddr;
-                val = val >> 12;
-                ci_write_16(dev->base+VIRTIOHDR_QUEUE_SELECT,
-                            cpu_to_le16(queue));
-                eieio();
-                ci_write_32(dev->base+VIRTIOHDR_QUEUE_ADDRESS,
-                            cpu_to_le32(val));
-        }
+	vq->size = virtio_get_qsize(dev, id);
+	vq->desc = SLOF_alloc_mem_aligned(virtio_vring_size(vq->size), 4096);
+	if (!vq->desc) {
+		printf("memory allocation failed!\n");
+		return -1;
+	}
+	memset(vq->desc, 0, virtio_vring_size(vq->size));
+	virtio_set_qaddr(dev, id, (unsigned long)vq->desc);
+	vq->avail = virtio_get_vring_avail(dev, id);
+	vq->used = virtio_get_vring_used(dev, id);
+	vq->id = id;
+	return 0;
 }
 
 /**
@@ -151,34 +406,109 @@ void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned int qaddr)
  */
 void virtio_set_status(struct virtio_device *dev, int status)
 {
-	if (dev->type == VIRTIO_TYPE_PCI) {
-		ci_write_8(dev->base+VIRTIOHDR_DEVICE_STATUS, status);
+	if (dev->is_modern) {
+		ci_write_8(dev->common.addr +
+			   offset_of(struct virtio_dev_common, dev_status), status);
+	} else {
+		ci_write_8(dev->legacy.addr+VIRTIOHDR_DEVICE_STATUS, status);
 	}
 }
 
+/**
+ * Get device status bits
+ */
+void virtio_get_status(struct virtio_device *dev, int *status)
+{
+	if (dev->is_modern) {
+		*status = ci_read_8(dev->common.addr +
+				    offset_of(struct virtio_dev_common, dev_status));
+	} else {
+		*status = ci_read_8(dev->legacy.addr+VIRTIOHDR_DEVICE_STATUS);
+	}
+}
 
 /**
  * Set guest feature bits
  */
-void virtio_set_guest_features(struct virtio_device *dev, int features)
+void virtio_set_guest_features(struct virtio_device *dev, uint64_t features)
 
 {
-	if (dev->type == VIRTIO_TYPE_PCI) {
-		ci_write_32(dev->base+VIRTIOHDR_GUEST_FEATURES, bswap_32(features));
+	if (dev->is_modern) {
+		uint32_t f1 = (features >> 32) & 0xFFFFFFFF;
+		uint32_t f0 = features & 0xFFFFFFFF;
+		void *addr = dev->common.addr;
+
+		ci_write_32(addr + offset_of(struct virtio_dev_common, drv_features_sel),
+			    cpu_to_le32(1));
+		ci_write_32(addr + offset_of(struct virtio_dev_common, drv_features),
+			    cpu_to_le32(f1));
+
+		ci_write_32(addr + offset_of(struct virtio_dev_common, drv_features_sel),
+			    cpu_to_le32(0));
+		ci_write_32(addr + offset_of(struct virtio_dev_common, drv_features),
+			    cpu_to_le32(f0));
+	} else {
+		ci_write_32(dev->legacy.addr+VIRTIOHDR_GUEST_FEATURES, cpu_to_le32(features));
 	}
 }
 
 /**
  * Get host feature bits
  */
-void virtio_get_host_features(struct virtio_device *dev, int *features)
+uint64_t virtio_get_host_features(struct virtio_device *dev)
 
 {
-	if (dev->type == VIRTIO_TYPE_PCI && features) {
-		*features = bswap_32(ci_read_32(dev->base+VIRTIOHDR_DEVICE_FEATURES));
+	uint64_t features = 0;
+	if (dev->is_modern) {
+		uint32_t f0 = 0, f1 = 0;
+		void *addr = dev->common.addr;
+
+		ci_write_32(addr + offset_of(struct virtio_dev_common, dev_features_sel),
+			    cpu_to_le32(1));
+		f1 = ci_read_32(addr +
+				offset_of(struct virtio_dev_common, dev_features));
+		ci_write_32(addr + offset_of(struct virtio_dev_common, dev_features_sel),
+			    cpu_to_le32(0));
+		f0 = ci_read_32(addr +
+				offset_of(struct virtio_dev_common, dev_features));
+
+		features = ((uint64_t)le32_to_cpu(f1) << 32) | le32_to_cpu(f0);
+	} else {
+		features = le32_to_cpu(ci_read_32(dev->legacy.addr+VIRTIOHDR_DEVICE_FEATURES));
 	}
+	return features;
 }
 
+int virtio_negotiate_guest_features(struct virtio_device *dev, uint64_t features)
+{
+	uint64_t host_features = 0;
+	int status;
+
+	/* Negotiate features */
+	host_features = virtio_get_host_features(dev);
+	if (!(host_features & VIRTIO_F_VERSION_1)) {
+		fprintf(stderr, "Device does not support virtio 1.0 %llx\n", host_features);
+		return -1;
+	}
+
+	virtio_set_guest_features(dev,  features);
+	host_features = virtio_get_host_features(dev);
+	if ((host_features & features) != features) {
+		fprintf(stderr, "Features error %llx\n", features);
+		return -1;
+	}
+
+	virtio_get_status(dev, &status);
+	status |= VIRTIO_STAT_FEATURES_OK;
+	virtio_set_status(dev, status);
+
+	/* Read back to verify the FEATURES_OK bit */
+	virtio_get_status(dev, &status);
+	if ((status & VIRTIO_STAT_FEATURES_OK) != VIRTIO_STAT_FEATURES_OK)
+		return -1;
+
+	return 0;
+}
 
 /**
  * Get additional config values
@@ -186,32 +516,38 @@ void virtio_get_host_features(struct virtio_device *dev, int *features)
 uint64_t virtio_get_config(struct virtio_device *dev, int offset, int size)
 {
 	uint64_t val = ~0ULL;
+	uint32_t hi, lo;
 	void *confbase;
 
-	switch (dev->type) {
-	 case VIRTIO_TYPE_PCI:
-		confbase = dev->base+VIRTIOHDR_DEVICE_CONFIG;
-		break;
-	 default:
-		return ~0ULL;
-	}
+	if (dev->is_modern)
+		confbase = dev->device.addr;
+	else
+		confbase = dev->legacy.addr+VIRTIOHDR_DEVICE_CONFIG;
+
 	switch (size) {
-	 case 1:
+	case 1:
 		val = ci_read_8(confbase+offset);
 		break;
-	 case 2:
+	case 2:
 		val = ci_read_16(confbase+offset);
+		if (dev->is_modern)
+			val = le16_to_cpu(val);
 		break;
-	 case 4:
+	case 4:
 		val = ci_read_32(confbase+offset);
+		if (dev->is_modern)
+			val = le32_to_cpu(val);
 		break;
-	 case 8:
+	case 8:
 		/* We don't support 8 bytes PIO accesses
 		 * in qemu and this is all PIO
 		 */
-		val = ci_read_32(confbase+offset);
-		val <<= 32;
-		val |= ci_read_32(confbase+offset+4);
+		lo = ci_read_32(confbase+offset);
+		hi = ci_read_32(confbase+offset+4);
+		if (dev->is_modern)
+			val = (uint64_t)le32_to_cpu(hi) << 32 | le32_to_cpu(lo);
+		else
+			val = (uint64_t)hi << 32 | lo;
 		break;
 	}
 
@@ -222,20 +558,19 @@ uint64_t virtio_get_config(struct virtio_device *dev, int offset, int size)
  * Get config blob
  */
 int __virtio_read_config(struct virtio_device *dev, void *dst,
-			  int offset, int len)
+			 int offset, int len)
 {
 	void *confbase;
 	unsigned char *buf = dst;
 	int i;
 
-	switch (dev->type) {
-	 case VIRTIO_TYPE_PCI:
-		confbase = dev->base+VIRTIOHDR_DEVICE_CONFIG;
-		break;
-	 default:
-		return 0;
-	}
+	if (dev->is_modern)
+		confbase = dev->device.addr;
+	else
+		confbase = dev->legacy.addr+VIRTIOHDR_DEVICE_CONFIG;
+
 	for (i = 0; i < len; i++)
 		buf[i] = ci_read_8(confbase + offset + i);
+
 	return len;
 }
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio.code b/qemu/roms/SLOF/lib/libvirtio/virtio.code
index 258b9bbda..8eec8f055 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio.code
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio.code
@@ -18,6 +18,11 @@
 
 /******** core virtio ********/
 
+// : virtio-setup-vd  ( -- dev )
+PRIM(virtio_X2d_setup_X2d_vd)
+	PUSH; TOS.a = virtio_setup_vd();
+MIRP
+
 // : virtio-vring-size  ( queuesize -- ringsize )
 PRIM(virtio_X2d_vring_X2d_size)
 	TOS.u = virtio_vring_size(TOS.u);
@@ -122,20 +127,18 @@ MIRP
 
 /******** virtio-net ********/
 
-// : virtio-net-open ( mac-addr-str len dev -- false | [ driver true ] )
+// : virtio-net-open ( dev -- false | [ driver true ] )
 PRIM(virtio_X2d_net_X2d_open)
 {
-	void *dev = TOS.a; POP;
-	int len = TOS.u; POP;
-        char *mac_addr = TOS.a;
+	void *dev = TOS.a;
 
-        net_driver_t *net_driver = virtionet_open(mac_addr, len, dev);
+	net_driver_t *net_driver = virtionet_open(dev);
 
-        if (net_driver) {
-                TOS.u = (unsigned long)net_driver; PUSH;
-                TOS.n = -1;
-        } else
-                TOS.n = 0;
+	if (net_driver) {
+		TOS.u = (unsigned long)net_driver; PUSH;
+		TOS.n = -1;
+	} else
+		TOS.n = 0;
 }
 MIRP
 
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio.h b/qemu/roms/SLOF/lib/libvirtio/virtio.h
index d5759b45a..0fee4baec 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio.h
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio.h
@@ -14,13 +14,23 @@
 #define _LIBVIRTIO_H
 
 #include <stdint.h>
+#include <stdbool.h>
 
 /* Device status bits */
 #define VIRTIO_STAT_ACKNOWLEDGE		1
 #define VIRTIO_STAT_DRIVER		2
 #define VIRTIO_STAT_DRIVER_OK		4
+#define VIRTIO_STAT_FEATURES_OK		8
+#define VIRTIO_STAT_NEEDS_RESET		64
 #define VIRTIO_STAT_FAILED		128
 
+#define BIT(x) (1UL << (x))
+
+/* VIRTIO 1.0 Device independent feature bits */
+#define VIRTIO_F_RING_INDIRECT_DESC	BIT(28)
+#define VIRTIO_F_RING_EVENT_IDX		BIT(29)
+#define VIRTIO_F_VERSION_1		BIT(32)
+
 #define VIRTIO_TIMEOUT		        5000 /* 5 sec timeout */
 
 /* Definitions for vring_desc.flags */
@@ -34,7 +44,7 @@ struct vring_desc {
 	uint32_t len;		/* Length */
 	uint16_t flags;		/* The flags as indicated above */
 	uint16_t next;		/* Next field if flags & NEXT */
-}; 
+};
 
 /* Definitions for vring_avail.flags */
 #define VRING_AVAIL_F_NO_INTERRUPT	1
@@ -44,8 +54,7 @@ struct vring_avail {
 	uint16_t flags;
 	uint16_t idx;
 	uint16_t ring[];
-}; 
-
+};
 
 /* Definitions for vring_used.flags */
 #define VRING_USED_F_NO_NOTIFY		1
@@ -61,27 +70,56 @@ struct vring_used {
 	struct vring_used_elem ring[];
 };
 
-#define VIRTIO_TYPE_PCI 0	/* For virtio-pci interface */
+/* Structure shared with SLOF and is 16bytes */
+struct virtio_cap {
+	void *addr;
+	uint8_t bar;
+	uint8_t is_io;
+	uint8_t cap_id;
+};
+
 struct virtio_device {
-	void *base;		/* base address */
-	int type;		/* VIRTIO_TYPE_PCI or VIRTIO_TYPE_VIO */
+	uint32_t is_modern;     /* Indicates whether to use virtio 1.0 */
+	struct virtio_cap legacy;
+	struct virtio_cap common;
+	struct virtio_cap notify;
+	struct virtio_cap isr;
+	struct virtio_cap device;
+	struct virtio_cap pci;
+	uint32_t notify_off_mul;
+};
+
+struct vqs {
+	uint64_t id;	/* Queue ID */
+	uint32_t size;
+	void *buf_mem;
+	struct vring_desc *desc;
+	struct vring_avail *avail;
+	struct vring_used *used;
 };
 
 /* Parts of the virtqueue are aligned on a 4096 byte page boundary */
 #define VQ_ALIGN(addr)	(((addr) + 0xfff) & ~0xfff)
 
 extern unsigned long virtio_vring_size(unsigned int qsize);
-extern int virtio_get_qsize(struct virtio_device *dev, int queue);
+extern unsigned int virtio_get_qsize(struct virtio_device *dev, int queue);
 extern struct vring_desc *virtio_get_vring_desc(struct virtio_device *dev, int queue);
 extern struct vring_avail *virtio_get_vring_avail(struct virtio_device *dev, int queue);
 extern struct vring_used *virtio_get_vring_used(struct virtio_device *dev, int queue);
+extern void virtio_fill_desc(struct vring_desc *desc, bool is_modern,
+                             uint64_t addr, uint32_t len,
+                             uint16_t flags, uint16_t next);
+extern int virtio_queue_init_vq(struct virtio_device *dev, struct vqs *vq, unsigned int id);
 
+extern struct virtio_device *virtio_setup_vd(void);
 extern void virtio_reset_device(struct virtio_device *dev);
 extern void virtio_queue_notify(struct virtio_device *dev, int queue);
 extern void virtio_set_status(struct virtio_device *dev, int status);
-extern void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned int qaddr);
-extern void virtio_set_guest_features(struct virtio_device *dev, int features);
-extern void virtio_get_host_features(struct virtio_device *dev, int *features);
+extern void virtio_get_status(struct virtio_device *dev, int *status);
+extern void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned long qaddr);
+extern void virtio_set_guest_features(struct virtio_device *dev, uint64_t features);
+extern uint64_t virtio_get_host_features(struct virtio_device *dev);
+extern int virtio_negotiate_guest_features(struct virtio_device *dev, uint64_t features);
 extern uint64_t virtio_get_config(struct virtio_device *dev, int offset, int size);
 extern int __virtio_read_config(struct virtio_device *dev, void *dst,
 				int offset, int len);
diff --git a/qemu/roms/SLOF/lib/libvirtio/virtio.in b/qemu/roms/SLOF/lib/libvirtio/virtio.in
index c36d127c7..195840e0f 100644
--- a/qemu/roms/SLOF/lib/libvirtio/virtio.in
+++ b/qemu/roms/SLOF/lib/libvirtio/virtio.in
@@ -10,6 +10,8 @@
  *     IBM Corporation - initial implementation
  *****************************************************************************/
 
+cod(virtio-setup-vd)
+
 cod(virtio-vring-size)
 cod(virtio-get-qsize)
 cod(virtio-get-config)
diff --git a/qemu/roms/SLOF/make.rules b/qemu/roms/SLOF/make.rules
index aebc4e360..cbc63530a 100644
--- a/qemu/roms/SLOF/make.rules
+++ b/qemu/roms/SLOF/make.rules
@@ -19,8 +19,12 @@
 ARCH := $(shell uname -p)
 
 # Auto-detect ppc64
-ifeq ($(ARCH), ppc64)
-CROSS          = ""
+ifeq (ppc64,$(findstring ppc64,$(ARCH)))
+  ifeq ($(ARCH), ppc64le)
+    EXTRA_CC	= -mbig -mabi=elfv1
+    EXTRA_LD	= -mbig
+  endif
+CROSS		?=
 else
 CROSS		?= powerpc64-linux-
 endif
@@ -31,8 +35,8 @@ HOSTCC		?= gcc
 HOSTCFLAGS	= -g -Wall -W -O2 -I. -I../include
 DD		= dd
 
-ONLY_CC 	= $(CROSS)gcc -m$(CELLSIZE)
-ONLY_AS 	= $(CROSS)as -m$(CELLSIZE)
+ONLY_CC 	= $(CROSS)gcc -m$(CELLSIZE) $(EXTRA_CC)
+ONLY_AS 	= $(CROSS)as -m$(CELLSIZE) $(EXTRA_LD)
 ONLY_LD 	= $(CROSS)ld -melf$(CELLSIZE)ppc
 
 # Verbose level:
diff --git a/qemu/roms/SLOF/rtas/reloc.S b/qemu/roms/SLOF/rtas/reloc.S
index e24d293d4..1b5b59a68 100644
--- a/qemu/roms/SLOF/rtas/reloc.S
+++ b/qemu/roms/SLOF/rtas/reloc.S
@@ -61,7 +61,7 @@ _rtas_start:
 ._rtas_entry_offset:	.quad   rtas_entry-_rtas_start
 ._rtas_config_offset:	.quad   rtas_config-_rtas_start
 ._rtas_stack:	        .quad   .stack-_rtas_start+RTAS_STACKSIZE-0x60
-._rtas_toc:	        .quad   _got-_rtas_start+0x8000
+._rtas_toc:	        .quad   _got-_rtas_start
 
 .over:	
 	mflr r8			# gpr 8 is the base
diff --git a/qemu/roms/SLOF/rtas/rtas.lds b/qemu/roms/SLOF/rtas/rtas.lds
index a5ba1daaf..30b18dd26 100644
--- a/qemu/roms/SLOF/rtas/rtas.lds
+++ b/qemu/roms/SLOF/rtas/rtas.lds
@@ -28,7 +28,8 @@ SECTIONS {
 	}
 	.got :
 	{
-	  _got = .;
+	  . = ALIGN(256);
+	  _got = DEFINED (.TOC.) ? .TOC. : ADDR (.got) + 0x8000;
 	  *(.got .toc)
 	}  	
 	.reloc :
diff --git a/qemu/roms/SLOF/rtas/rtas_entry.S b/qemu/roms/SLOF/rtas/rtas_entry.S
index 74693aa48..424137bf5 100644
--- a/qemu/roms/SLOF/rtas/rtas_entry.S
+++ b/qemu/roms/SLOF/rtas/rtas_entry.S
@@ -39,7 +39,7 @@ rtas_entry:
 	bcl 20,31,.over		# branch to over 
 .base:	
 	.align  3
-..got:			.quad   _got-.base+0x8000
+..got:			.quad   _got-.base
 ..stack:		.quad   .stack+RTAS_STACKSIZE-0x60-.base
 .over:	
 	mflr r8			# gpr 8 is the base
diff --git a/qemu/roms/SLOF/slof/entry.S b/qemu/roms/SLOF/slof/entry.S
index dcff57ba0..5372de357 100644
--- a/qemu/roms/SLOF/slof/entry.S
+++ b/qemu/roms/SLOF/slof/entry.S
@@ -207,4 +207,13 @@ call_client:
 	li 3, -1 # client app return
 	blr
 
+
+	# Call another function via pointer in r6
+	# (arguments can be provided in r3 to r5)
+	# Destination function should jump back to lr
+C_ENTRY(call_c)
+	mtctr	r6
+	bctr
+
+.global the_system_stack
 	.lcomm	the_system_stack, STACKSIZE, 16
diff --git a/qemu/roms/SLOF/slof/fs/base.fs b/qemu/roms/SLOF/slof/fs/base.fs
index e71e087eb..03e77e54f 100644
--- a/qemu/roms/SLOF/slof/fs/base.fs
+++ b/qemu/roms/SLOF/slof/fs/base.fs
@@ -579,8 +579,6 @@ defer cursor-off ( -- )
 #include "debug.fs"
 \ provide 7.5.3.1 Dictionary search
 #include "dictionary.fs"
-\ block data access for IO devices - ought to be implemented in engine
-#include "rmove.fs"
 \ provide a simple run time preprocessor
 #include <preprocessor.fs>
 
diff --git a/qemu/roms/SLOF/slof/fs/boot.fs b/qemu/roms/SLOF/slof/fs/boot.fs
index 9a0ded0c2..e0b628140 100644
--- a/qemu/roms/SLOF/slof/fs/boot.fs
+++ b/qemu/roms/SLOF/slof/fs/boot.fs
@@ -181,17 +181,13 @@ defer go ( -- )
       \ with watchdog timeout.
       4ec set-watchdog
    THEN
+   2dup " HALT" str= IF 2drop 0 EXIT THEN
    my-self >r current-node @ >r         \ Save my-self
    ." Trying to load: " $bootargs type ."  from: " 2dup type ."  ... "
    2dup open-dev dup IF
       dup to my-self
       dup ihandle>phandle set-node
       -rot                              ( ihandle devstr len )
-      my-args nip 0= IF
-	 2dup 1- + c@ [char] : <> IF    \ Add : to device path if missing
-	    1+ strdup 2dup 1- + [char] : swap c!
-	 THEN
-      THEN
       encode-string s" bootpath" set-chosen
       $bootargs encode-string s" bootargs" set-chosen
       get-load-base s" load" 3 pick ['] $call-method CATCH IF
@@ -211,7 +207,7 @@ defer go ( -- )
 
 : parse-load ( "{devlist}" -- success )	\ Parse-execute boot-device list
    cr BEGIN parse-word dup WHILE
-	 ( de-alias ) do-load dup 0< IF drop 0 THEN IF
+	 de-alias do-load dup 0< IF drop 0 THEN IF
 	    state-valid @ IF ."   Successfully loaded" cr THEN
 	    true 0d parse strdup load-list 2! EXIT
 	 THEN
diff --git a/qemu/roms/SLOF/slof/fs/client.fs b/qemu/roms/SLOF/slof/fs/client.fs
index 1b2bb0326..7d537a668 100644
--- a/qemu/roms/SLOF/slof/fs/client.fs
+++ b/qemu/roms/SLOF/slof/fs/client.fs
@@ -282,6 +282,18 @@ ALSO client-voc DEFINITIONS
 ;
 
 \
+\ Standard for Boot, defined in 6.3.2.5:
+\
+: boot  ( zstr -- )
+   zcount
+   debug-client-interface? IF
+      ." ci: boot " 2dup type cr
+   THEN
+   " boot " 2swap $cat " boot-command" $setenv (nvupdate)
+   reset-all
+;
+
+\
 \ User Interface, defined in 6.3.2.6
 \
 : interpret ( ... zstr -- result ... )
diff --git a/qemu/roms/SLOF/slof/fs/fbuffer.fs b/qemu/roms/SLOF/slof/fs/fbuffer.fs
index 756f05a95..47046087d 100644
--- a/qemu/roms/SLOF/slof/fs/fbuffer.fs
+++ b/qemu/roms/SLOF/slof/fs/fbuffer.fs
@@ -19,6 +19,7 @@
 0 VALUE screen-height
 0 VALUE screen-width
 0 VALUE screen-depth
+0 VALUE screen-line-bytes
 0 VALUE window-top
 0 VALUE window-left
 
@@ -54,10 +55,10 @@
 : fb8-background inverse? ;
 : fb8-foreground inverse? invert ;
 
-: fb8-lines2bytes ( #lines -- #bytes ) char-height * screen-width * screen-depth * ;
+: fb8-lines2bytes ( #lines -- #bytes ) char-height * screen-line-bytes * ;
 : fb8-columns2bytes ( #columns -- #bytes ) char-width * screen-depth * ;
 : fb8-line2addr ( line# -- addr )
-	char-height * window-top + screen-width * screen-depth *
+	char-height * window-top + screen-line-bytes *
 	frame-buffer-adr + window-left screen-depth * +
 ;
 
@@ -98,9 +99,10 @@ CREATE bitmap-buffer 400 4 * allot
 
 : fb8-toggle-cursor ( -- )
 	line# fb8-line2addr column# fb8-columns2bytes +
-	char-height 0 ?DO
-		char-width screen-depth * 0 ?DO dup dup rb@ -1 xor swap rb! 1+ LOOP
-		screen-width screen-depth * + char-width screen-depth * -
+	char-height 2 - screen-line-bytes * +
+	2 0 ?DO
+		dup char-width screen-depth * invert-region
+		screen-line-bytes +
 	LOOP drop
 ;
 
@@ -110,7 +112,7 @@ CREATE bitmap-buffer 400 4 * allot
 	line# fb8-line2addr column# fb8-columns2bytes + ( bitmap-buf fb-addr )
 	char-height 0 ?DO
 		2dup char-width screen-depth * mrmove
-		screen-width screen-depth * + >r char-width screen-depth * + r>
+		screen-line-bytes + >r char-width screen-depth * + r>
 	LOOP 2drop
     ELSE 2drop r> 3drop THEN
 ;
@@ -135,12 +137,12 @@ CREATE bitmap-buffer 400 4 * allot
 		fb8-columns2bytes swap fb8-columns2bytes tuck -
 		over r@ tuck + rot char-height 0 ?DO
 			3dup rmove
-			-rot screen-width screen-depth * tuck + -rot + swap rot
+			-rot screen-line-bytes tuck + -rot + swap rot
 		LOOP
 		3drop r>
 	THEN
 	char-height 0 ?DO
-		dup 2 pick fb8-erase-block screen-width screen-depth * +
+		dup 2 pick fb8-erase-block screen-line-bytes +
 	LOOP
 	2drop
 ;
@@ -153,12 +155,12 @@ CREATE bitmap-buffer 400 4 * allot
 		fb8-columns2bytes swap fb8-columns2bytes tuck -
 		over r@ + 2dup + r> swap >r rot char-height 0 ?DO
 			3dup rmove
-			-rot screen-width screen-depth * tuck + -rot + swap rot
+			-rot screen-line-bytes tuck + -rot + swap rot
 		LOOP
 		3drop r> over -
 	THEN
 	char-height 0 ?DO
-		dup 2 pick fb8-erase-block screen-width screen-depth * +
+		dup 2 pick fb8-erase-block screen-line-bytes +
 	LOOP
 	2drop
 ;
@@ -166,13 +168,11 @@ CREATE bitmap-buffer 400 4 * allot
 : fb8-reset-screen ( -- ) ( Left as no-op by design ) ;
 
 : fb8-erase-screen ( -- )
-	frame-buffer-adr screen-height screen-width * screen-depth * fb8-erase-block
+	frame-buffer-adr screen-height screen-line-bytes * fb8-erase-block
 ;
 
 : fb8-invert-screen ( -- )
-	frame-buffer-adr screen-height screen-width * screen-depth * 2dup /x / 0 ?DO
-		dup rx@ -1 xor over rx! xa1+
-	LOOP 3drop
+	frame-buffer-adr screen-height screen-line-bytes * invert-region
 ;
 
 : fb8-blink-screen ( -- ) fb8-invert-screen fb8-invert-screen ;
@@ -180,6 +180,7 @@ CREATE bitmap-buffer 400 4 * allot
 : fb8-install ( width height #columns #lines -- )
 	1 to screen-depth
 	2swap  to screen-height  to screen-width
+	screen-width to screen-line-bytes
 	screen-#rows min to #lines
 	screen-#columns min to #columns
 	screen-height char-height #lines * - 2/ to window-top
@@ -201,6 +202,7 @@ CREATE bitmap-buffer 400 4 * allot
 	>r
 	fb8-install
 	r> to screen-depth
+	screen-width screen-depth * to screen-line-bytes
 ;
 
 
diff --git a/qemu/roms/SLOF/slof/fs/little-endian.fs b/qemu/roms/SLOF/slof/fs/little-endian.fs
index f2e4e8d42..6b4779ee0 100644
--- a/qemu/roms/SLOF/slof/fs/little-endian.fs
+++ b/qemu/roms/SLOF/slof/fs/little-endian.fs
@@ -17,6 +17,9 @@ here c@ ef = CONSTANT ?littleendian
 
 ?bigendian [IF]
 
+: x!-le >r xbflip r> x! ;
+: x@-le x@ xbflip ;
+
 : l!-le  >r lbflip r> l! ;
 : l@-le  l@ lbflip ;
 
@@ -47,6 +50,9 @@ here c@ ef = CONSTANT ?littleendian
 
 [ELSE]
 
+: x!-le x! ;
+: x@-le x@ ;
+
 : l!-le  l! ;
 : l@-le  l@ ;
 
diff --git a/qemu/roms/SLOF/slof/fs/packages/disk-label.fs b/qemu/roms/SLOF/slof/fs/packages/disk-label.fs
index fe1c25e7a..e034d6408 100644
--- a/qemu/roms/SLOF/slof/fs/packages/disk-label.fs
+++ b/qemu/roms/SLOF/slof/fs/packages/disk-label.fs
@@ -20,6 +20,7 @@ false VALUE debug-disk-label?
 \ If we ever want to put a large kernel with initramfs from a PREP partition
 \ we might need to increase this value. The default value is 65536 blocks (32MB)
 d# 65536 value max-prep-partition-blocks
+d# 4096 CONSTANT block-array-size
 
 s" disk-label" device-name
 
@@ -152,8 +153,8 @@ CONSTANT /gpt-part-entry
 : init-block ( -- )
    s" block-size" ['] $call-parent CATCH IF ABORT" parent has no block-size." THEN
    to block-size
-   d# 4096 alloc-mem
-   dup d# 4096 erase
+   block-array-size alloc-mem
+   dup block-array-size erase
    to block
    debug-disk-label? IF
       ." init-block: block-size=" block-size .d ." block=0x" block u. cr
@@ -178,7 +179,8 @@ CONSTANT /gpt-part-entry
 \ This word returns true if the currently loaded block has _NO_ GPT partition id
 : no-gpt? ( -- true|false )
    0 read-sector
-   1 partition>part-entry part-entry>id c@ ee <>
+   1 partition>part-entry part-entry>id c@ ee <> IF true EXIT THEN
+   block mbr>magic w@-le aa55 <>
 ;
 
 : pc-extended-partition? ( part-entry-addr -- true|false )
@@ -266,7 +268,10 @@ CONSTANT /gpt-part-entry
 
 : try-dos-partition ( -- okay? )
    \ Read partition table and check magic.
-   no-mbr? IF cr ." No DOS disk-label found." cr false EXIT THEN
+   no-mbr? IF
+       debug-disk-label? IF cr ." No DOS disk-label found." cr THEN
+       false EXIT
+   THEN
 
    count-dos-logical-partitions TO dos-logical-partitions
 
@@ -320,6 +325,14 @@ CONSTANT /gpt-part-entry
 
 \ Load from first active DOS boot partition.
 
+: fat-bootblock? ( addr -- flag )
+   \ byte 0-2 of the bootblock is a jump instruction in
+   \ all FAT filesystems.
+   \ e9 and eb are jump instructions in x86 assembler.
+   dup c@ e9 = IF drop true EXIT THEN
+   dup c@ eb = swap 2+ c@ 90 = and
+;
+
 \ NOTE: block-size is always 512 bytes for DOS partition tables.
 
 : load-from-dos-boot-partition ( addr -- size )
@@ -352,60 +365,103 @@ CONSTANT /gpt-part-entry
    drop 0
 ;
 
-\ Check for GPT PReP partition GUID
-9E1A2D38     CONSTANT GPT-PREP-PARTITION-1
-C612         CONSTANT GPT-PREP-PARTITION-2
-4316         CONSTANT GPT-PREP-PARTITION-3
-AA26         CONSTANT GPT-PREP-PARTITION-4
-8B49521E5A8B CONSTANT GPT-PREP-PARTITION-5
+\ Check for GPT PReP partition GUID. Only first 3 blocks are
+\ byte-swapped treating last two blocks as contigous for simplifying
+\ comparison
+9E1A2D38            CONSTANT GPT-PREP-PARTITION-1
+C612                CONSTANT GPT-PREP-PARTITION-2
+4316                CONSTANT GPT-PREP-PARTITION-3
+AA268B49521E5A8B    CONSTANT GPT-PREP-PARTITION-4
 
 : gpt-prep-partition? ( -- true|false )
-   block gpt-part-entry>part-type-guid l@-le GPT-PREP-PARTITION-1 = IF
-      block gpt-part-entry>part-type-guid 4 + w@-le
-      GPT-PREP-PARTITION-2 = IF
-         block gpt-part-entry>part-type-guid 6 + w@-le
-         GPT-PREP-PARTITION-3 = IF
-            block gpt-part-entry>part-type-guid 8 + w@
-            GPT-PREP-PARTITION-4 = IF
-               block gpt-part-entry>part-type-guid a + w@
-               block gpt-part-entry>part-type-guid c + l@ swap lxjoin
-               GPT-PREP-PARTITION-5 = IF
-                   TRUE EXIT
-               THEN
-            THEN
-         THEN
-      THEN
+   block gpt-part-entry>part-type-guid
+   dup l@-le     GPT-PREP-PARTITION-1 <> IF drop false EXIT THEN
+   dup 4 + w@-le GPT-PREP-PARTITION-2 <> IF drop false EXIT THEN
+   dup 6 + w@-le GPT-PREP-PARTITION-3 <> IF drop false EXIT THEN
+       8 + x@    GPT-PREP-PARTITION-4 =
+;
+
+\ Check for GPT MSFT BASIC DATA GUID - fat based
+EBD0A0A2            CONSTANT GPT-BASIC-DATA-PARTITION-1
+B9E5                CONSTANT GPT-BASIC-DATA-PARTITION-2
+4433                CONSTANT GPT-BASIC-DATA-PARTITION-3
+87C068B6B72699C7    CONSTANT GPT-BASIC-DATA-PARTITION-4
+
+: gpt-basic-data-partition? ( -- true|false )
+   block gpt-part-entry>part-type-guid
+   dup l@-le     GPT-BASIC-DATA-PARTITION-1 <> IF drop false EXIT THEN
+   dup 4 + w@-le GPT-BASIC-DATA-PARTITION-2 <> IF drop false EXIT THEN
+   dup 6 + w@-le GPT-BASIC-DATA-PARTITION-3 <> IF drop false EXIT THEN
+       8 + x@    GPT-BASIC-DATA-PARTITION-4 =
+;
+
+\
+\ GPT Signature
+\ ("EFI PART", 45h 46h 49h 20h 50h 41h 52h 54h)
+\
+4546492050415254 CONSTANT GPT-SIGNATURE
+
+\ The routine checks whether the protective MBR has GPT ID and then
+\ reads the gpt data from the sector. Also set the seek position and
+\ the partition size used in caller routines.
+
+: get-gpt-partition ( -- true|false )
+   no-gpt? IF false EXIT THEN
+   debug-disk-label? IF cr ." GPT partition found " cr  THEN
+   1 read-sector
+   block gpt>part-entry-lba x@-le
+   block-size * to seek-pos
+   block gpt>part-entry-size l@-le to gpt-part-size
+   gpt-part-size block-array-size > IF
+      cr ." GPT part size exceeds buffer allocated " cr
+      false exit
    THEN
-   FALSE
+   block gpt>signature x@ GPT-SIGNATURE =
 ;
 
 : load-from-gpt-prep-partition ( addr -- size )
-   no-gpt? IF drop FALSE EXIT THEN
-   debug-disk-label? IF
-      cr ." GPT partition found " cr
-   THEN
-   1 read-sector block gpt>part-entry-lba l@-le
-   block-size * to seek-pos
-   block gpt>part-entry-size l@-le to gpt-part-size
-   block gpt>num-part-entry l@-le dup 0= IF FALSE EXIT THEN
+   get-gpt-partition 0= IF false EXIT THEN
+   block gpt>num-part-entry l@-le dup 0= IF false exit THEN
    1+ 1 ?DO
       seek-pos 0 seek drop
       block gpt-part-size read drop gpt-prep-partition? IF
-         debug-disk-label? IF
-            ." GPT PReP partition found " cr
-         THEN
-         block gpt-part-entry>first-lba x@ xbflip
-         block gpt-part-entry>last-lba x@ xbflip
-         over - 1+                 ( addr offset len )
-         swap                      ( addr len offset )
-         block-size * to part-offset
-         0 0 seek drop             ( addr len )
-         block-size * read         ( size )
+         debug-disk-label? IF  ." GPT PReP partition found " cr THEN
+         block gpt-part-entry>first-lba x@-le ( addr first-lba )
+         block gpt-part-entry>last-lba x@-le  ( addr first-lba last-lba)
+         over - 1+                            ( addr first-lba blocks )
+         swap                                 ( addr blocks first-lba )
+         block-size * to part-offset          ( addr blocks )
+         0 0 seek drop                        ( addr blocks )
+         block-size * read                    ( size )
+         UNLOOP EXIT
+     THEN
+     seek-pos gpt-part-size + to seek-pos
+   LOOP
+   false
+;
+
+: try-gpt-dos-partition ( -- true|false )
+   get-gpt-partition 0= IF false EXIT THEN
+   block gpt>num-part-entry l@-le dup 0= IF false EXIT THEN
+   1+ 1 ?DO
+      seek-pos 0 seek drop
+      block gpt-part-size read drop
+      gpt-basic-data-partition? IF
+         debug-disk-label? IF ." GPT BASIC DATA partition found " cr THEN
+         block gpt-part-entry>first-lba x@-le       ( first-lba )
+         dup to part-start                          ( first-lba )
+         block gpt-part-entry>last-lba x@-le        ( first-lba last-lba )
+         over - 1+                                  ( first-lba s1 )
+         block-size * to part-size                  ( first-lba )
+         block-size * to part-offset                ( )
+         0 0 seek drop
+         block block-size read drop
+         block fat-bootblock?                    ( true|false )
          UNLOOP EXIT
       THEN
-      seek-pos gpt-part-size i * + to seek-pos
+      seek-pos gpt-part-size + to seek-pos
    LOOP
-   FALSE
+   false
 ;
 
 \ Extract the boot loader path from a bootinfo.txt file
@@ -493,7 +549,7 @@ AA26         CONSTANT GPT-PREP-PARTITION-4
 
    debug-disk-label? IF ." Trying CHRP boot " .s cr THEN
    1 disk-chrp-boot !
-   dup load-chrp-boot-file ?dup 0 <> IF .s cr nip EXIT THEN
+   dup load-chrp-boot-file ?dup 0 <> IF nip EXIT THEN
    0 disk-chrp-boot !
 
    debug-disk-label? IF ." Trying GPT boot " .s cr THEN
@@ -558,14 +614,7 @@ AA26         CONSTANT GPT-PREP-PARTITION-4
 : try-dos-files ( -- found? )
    no-mbr? IF false EXIT THEN
 
-   \ block 0 byte 0-2 is a jump instruction in all FAT
-   \ filesystems.
-   \ e9 and eb are jump instructions in x86 assembler.
-   block c@ e9 <> IF
-      block c@ eb <>
-      block 2+ c@ 90 <> or
-      IF false EXIT THEN
-   THEN
+   block fat-bootblock? 0= IF false EXIT THEN
    s" fat-files" (interpose-filesystem)
    true
 ;
@@ -600,6 +649,7 @@ AA26         CONSTANT GPT-PREP-PARTITION-4
 
 : try-partitions ( -- found? )
    try-dos-partition IF try-files EXIT THEN
+   try-gpt-dos-partition IF try-files EXIT THEN
    \ try-iso9660-partition IF try-files EXIT THEN
    \ ... more partition types here...
    false
@@ -610,7 +660,7 @@ AA26         CONSTANT GPT-PREP-PARTITION-4
 
 : close ( -- )
    debug-disk-label? IF ." Closing disk-label: block=0x" block u. ." block-size=" block-size .d cr THEN
-   block d# 4096 free-mem
+   block block-array-size free-mem
 ;
 
 
diff --git a/qemu/roms/SLOF/slof/fs/packages/fat-files.fs b/qemu/roms/SLOF/slof/fs/packages/fat-files.fs
index 0cec3664e..d9194527e 100644
--- a/qemu/roms/SLOF/slof/fs/packages/fat-files.fs
+++ b/qemu/roms/SLOF/slof/fs/packages/fat-files.fs
@@ -18,6 +18,7 @@ INSTANCE VARIABLE sectors/cluster
 INSTANCE VARIABLE #reserved-sectors
 INSTANCE VARIABLE #fats
 INSTANCE VARIABLE #root-entries
+INSTANCE VARIABLE fat32-root-cluster
 INSTANCE VARIABLE total-#sectors
 INSTANCE VARIABLE media-descriptor
 INSTANCE VARIABLE sectors/fat
@@ -59,9 +60,18 @@ INSTANCE VARIABLE next-cluster
 : read-cluster ( cluster# -- )
   dup bytes/cluster @ * cluster-offset @ + bytes/cluster @ read-data
   read-fat dup #clusters @ >= IF drop 0 THEN next-cluster ! ;
+
 : read-dir ( cluster# -- )
-  ?dup 0= IF root-offset @ #root-entries @ 20 * read-data 0 next-cluster !
-  ELSE read-cluster THEN ;
+    ?dup 0= IF
+        #root-entries @ 0= IF
+            fat32-root-cluster @ read-cluster
+        ELSE
+            root-offset @ #root-entries @ 20 * read-data 0 next-cluster !
+        THEN
+    ELSE
+        read-cluster
+    THEN
+;
 
 : .time ( x -- )
   base @ >r decimal
@@ -137,6 +147,7 @@ CREATE dos-name b allot
 
   \ For FAT32:
   sectors/fat @ 0= IF data @ 24 + 4c@ bljoin sectors/fat ! THEN
+  #root-entries @ 0= IF data @ 2c + 4c@ bljoin ELSE 0 THEN fat32-root-cluster !
 
   \ XXX add other FAT32 stuff (offsets 28, 2c, 30)
 
diff --git a/qemu/roms/SLOF/slof/fs/pci-config-bridge.fs b/qemu/roms/SLOF/slof/fs/pci-config-bridge.fs
index 689325318..1efbcd804 100644
--- a/qemu/roms/SLOF/slof/fs/pci-config-bridge.fs
+++ b/qemu/roms/SLOF/slof/fs/pci-config-bridge.fs
@@ -81,26 +81,6 @@
    2drop 
 ;
 
-: dma-alloc ( ... size -- virt )
-   \ ." dma-alloc called: " .s cr
-   alloc-mem
-;
-
-: dma-free ( virt size -- )
-   \ ." dma-free called: " .s cr
-   free-mem
-;
-
-: dma-map-in ( ... virt size cacheable? -- devaddr )
-   \ ." dma-map-in called: " .s cr
-   2drop
-;
-
-: dma-map-out ( virt devaddr size -- )
-   \ ." dma-map-out called: " .s cr
-   2drop drop
-;
-
 : dma-sync ( virt devaddr size -- )
    \ XXX should we add at least a memory barrier here?
    \ ." dma-sync called: " .s cr
diff --git a/qemu/roms/SLOF/slof/fs/pci-scan.fs b/qemu/roms/SLOF/slof/fs/pci-scan.fs
index b8b9fe61f..2fdf0e8f5 100644
--- a/qemu/roms/SLOF/slof/fs/pci-scan.fs
+++ b/qemu/roms/SLOF/slof/fs/pci-scan.fs
@@ -110,10 +110,13 @@ here 100 allot CONSTANT pci-device-vec
         dup 100000 + pci-next-mem !             \ and write back with 1MB for bridge
         over 24 + rtas-config-w@                \ check if 64bit support
         1 and IF                                \ IF 64 bit support
-                2dup 20 rshift                  \ | keep upper 32 bits
-                swap 28 + rtas-config-l!        \ | and write it into the Base-Upper32-bits
-                pci-max-mem @ 20 rshift         \ | fetch max Limit address and keep upper 32 bits
-                2 pick 2C + rtas-config-l!      \ | and set the Limit
+                pci-next-mem64 @ 100000000 #aligned \ | read the current Value of 64-bit and align to 4GB boundary
+                dup 100000000 + pci-next-mem64 x!   \ | and write back with 1GB for bridge
+                2 pick swap                         \ |
+                20 rshift                           \ | keep upper 32 bits
+                swap 28 + rtas-config-l!            \ | and write it into the Base-Upper32-bits
+                pci-max-mem64 @ 20 rshift           \ | fetch max Limit address and keep upper 32 bits
+                2 pick 2C + rtas-config-l!          \ | and set the Limit
         THEN                                    \ FI
         10 rshift                               \ keep upper 16 bits
         pci-max-mem @ 1- FFFF0000 and or        \ and Insert mmem Limit (set it to max)
@@ -129,8 +132,12 @@ here 100 allot CONSTANT pci-device-vec
         1-                                      \ make limit one less than boundary
         over 24 + rtas-config-w@                \ check if 64bit support
         1 and IF                                \ IF 64 bit support
-                2dup 20 rshift                  \ | keep upper 32 bits
-                swap 2C + rtas-config-l!        \ | and write it into the Limit-Upper32-bits
+                pci-next-mem64 @ 100000000 #aligned \ | Reat current value of 64-bar and align at 4GB
+                dup pci-next-mem64 x!               \ | and write it back
+                1-                                  \ | make limite one less than boundary
+                2 pick swap                         \ |
+                20 rshift                           \ | keep upper 32 bits
+                swap 2C + rtas-config-l!            \ | and write it into the Limit-Upper32-bits
         THEN                                    \ FI
         FFFF0000 and                            \ keep upper 16 bits
         over 24 + rtas-config-l@ 0000FFFF and   \ fetch original Value
diff --git a/qemu/roms/SLOF/slof/fs/rmove.fs b/qemu/roms/SLOF/slof/fs/rmove.fs
deleted file mode 100644
index c28dba9c4..000000000
--- a/qemu/roms/SLOF/slof/fs/rmove.fs
+++ /dev/null
@@ -1,53 +0,0 @@
-\ *****************************************************************************
-\ * Copyright (c) 2004, 2008 IBM Corporation
-\ * All rights reserved.
-\ * This program and the accompanying materials
-\ * are made available under the terms of the BSD License
-\ * which accompanies this distribution, and is available at
-\ * http://www.opensource.org/licenses/bsd-license.php
-\ *
-\ * Contributors:
-\ *     IBM Corporation - initial implementation
-\ ****************************************************************************/
-
-defer '(r@)
-defer '(r!)
-1 VALUE /(r)
-
-
-\ The rest of the code already implemented in prim.in
-\ In the end all of this should be moved over there and this file terminated
-
-: (rfill) ( addr size pattern 'r! /r -- )
-	to /(r) to '(r!) ff and
-	dup 8 lshift or dup 10 lshift or dup 20 lshift or
-	-rot bounds ?do dup i '(r!) /(r) +loop drop
-;
-
-: (fwrmove) ( src dest size -- )
-	>r 0 -rot r> bounds ?do + dup '(r@) i '(r!) /(r) dup +loop 2drop
-;
-
-\ Move from main to device memory
-: mrmove ( src dest size -- )
-	3dup or or 7 AND CASE
-		0 OF ['] x@ ['] rx! /x ENDOF
-		4 OF ['] l@ ['] rl! /l ENDOF
-		2 OF ['] w@ ['] rw! /w ENDOF
-		dup OF ['] c@ ['] rb! /c ENDOF
-	ENDCASE
-	( We already know that source and destination do not overlap )
-	to /(r) to '(r!) to '(r@) (fwrmove)
-;
-
-: rfill ( addr size pattern -- )
-	3dup drop or 7 AND CASE
-		0 OF ['] rx! /x ENDOF
-		4 OF ['] rl! /l ENDOF
-		2 OF ['] rw! /w ENDOF
-		dup OF ['] rb! /c ENDOF
-	ENDCASE (rfill)
-;
-
-
-
diff --git a/qemu/roms/SLOF/slof/fs/root.fs b/qemu/roms/SLOF/slof/fs/root.fs
index 21c710951..952b00e75 100644
--- a/qemu/roms/SLOF/slof/fs/root.fs
+++ b/qemu/roms/SLOF/slof/fs/root.fs
@@ -77,7 +77,6 @@ finish-device
 
 : open true ;
 : close ;
-#include <archsupport.fs>
 
 \ Finish root
 finish-device
diff --git a/qemu/roms/SLOF/slof/fs/terminal.fs b/qemu/roms/SLOF/slof/fs/terminal.fs
index 582bedeb3..dc82e7bf4 100644
--- a/qemu/roms/SLOF/slof/fs/terminal.fs
+++ b/qemu/roms/SLOF/slof/fs/terminal.fs
@@ -167,6 +167,7 @@ false VALUE stopcsi
 CREATE twtracebuf 4000 allot twtracebuf 4000 erase
 twtracebuf VALUE twbp
 0 VALUE twbc
+0 VALUE twtrace-enabled?
 
 : twtrace
 	twbc 4000 = IF 0 to twbc twtracebuf to twbp THEN
@@ -176,7 +177,7 @@ twtracebuf VALUE twbp
 : terminal-write ( addr len -- actual-len )
  	cursor-off
 	tuck bounds ?DO i c@
-		twtrace
+		twtrace-enabled? IF twtrace THEN
 		esc-on IF esc-process
 		ELSE CASE
 			1B OF true to esc-on ENDOF
diff --git a/qemu/roms/SLOF/slof/helpers.c b/qemu/roms/SLOF/slof/helpers.c
index d7c1888b4..48c34a65c 100644
--- a/qemu/roms/SLOF/slof/helpers.c
+++ b/qemu/roms/SLOF/slof/helpers.c
@@ -114,6 +114,13 @@ long SLOF_pci_config_read16(long offset)
 	return forth_pop();
 }
 
+long SLOF_pci_config_read8(long offset)
+{
+	forth_push(offset);
+	forth_eval("config-b@");
+	return forth_pop();
+}
+
 void SLOF_pci_config_write32(long offset, long value)
 {
 	forth_push(value);
@@ -128,6 +135,13 @@ void SLOF_pci_config_write16(long offset, long value)
 	forth_eval("config-w!");
 }
 
+void SLOF_pci_config_write8(long offset, long value)
+{
+	forth_push(value);
+	forth_push(offset);
+	forth_eval("config-b!");
+}
+
 void *SLOF_translate_my_address(void *addr)
 {
 	forth_push((long)addr);
diff --git a/qemu/roms/SLOF/slof/paflof.c b/qemu/roms/SLOF/slof/paflof.c
index 624955fba..2fc25c81e 100644
--- a/qemu/roms/SLOF/slof/paflof.c
+++ b/qemu/roms/SLOF/slof/paflof.c
@@ -19,6 +19,7 @@
 #undef unix
 
 #include "paflof.h"
+#include <stdio.h>
 #include <string.h>
 #include <stdint.h>
 #include <ctype.h>
@@ -43,6 +44,9 @@ unsigned char hash_table[HASHSIZE*CELLSIZE];
 
 #include ISTR(TARG,c)
 
+static int did_stackwarning;
+extern char the_system_stack[];
+
 // the actual engine
 long engine(int mode, long param_1, long param_2)
 {
@@ -84,6 +88,11 @@ long engine(int mode, long param_1, long param_2)
 		c_return[1].a = &dummy;
 	}
 
+	if ((char *)&ip < the_system_stack && !did_stackwarning) {
+		puts("ERROR: stack overflow in engine()!");
+		did_stackwarning = 1;
+	}
+
 	if (mode & ENGINE_MODE_PARAM_2) {
 		(++dp)->n = param_2;
 	}
diff --git a/qemu/roms/SLOF/slof/ppc64.c b/qemu/roms/SLOF/slof/ppc64.c
index 20d927069..619d95ec7 100644
--- a/qemu/roms/SLOF/slof/ppc64.c
+++ b/qemu/roms/SLOF/slof/ppc64.c
@@ -42,24 +42,7 @@ cell *the_heap_start = &the_heap[0];
 cell *the_heap_end = &the_heap[HEAP_SIZE / CELLSIZE];
 
 extern void io_putchar(unsigned char);
-
-
-static unsigned long __attribute__((noinline))
-call_c(cell arg0, cell arg1, cell arg2, cell entry)
-{
-	register unsigned long r3 asm("r3") = arg0.u;
-	register unsigned long r4 asm("r4") = arg1.u;
-	register unsigned long r5 asm("r5") = arg2.u;
-	register unsigned long r6 = entry.u         ;
-
-	asm volatile("mflr 31 ; mtctr %4 ; bctrl ; mtlr 31"
-		     : "=r" (r3)
-		     : "r" (r3), "r" (r4), "r" (r5), "r" (r6)
-		     : "ctr", "r6", "r7", "r8", "r9", "r10", "r11",
-		       "r12", "r13", "r31", "lr", "cc");
-
-	return r3;
-}
+extern unsigned long call_c(cell arg0, cell arg1, cell arg2, cell entry);
 
 
 long
diff --git a/qemu/roms/SLOF/slof/prim.code b/qemu/roms/SLOF/slof/prim.code
index 9fbed7168..bb9e036a9 100644
--- a/qemu/roms/SLOF/slof/prim.code
+++ b/qemu/roms/SLOF/slof/prim.code
@@ -520,6 +520,19 @@ PRIM(RMOVE)
 
 	MIRP
 
+PRIM(MRMOVE)
+	type_u size = TOS.u; POP;
+	void *d = TOS.a; POP;
+	void *s = TOS.a; POP;
+	FAST_MRMOVE(s, d, size);
+	MIRP
+
+PRIM(RFILL)
+	type_u pat = TOS.u; POP;
+	type_u size = TOS.u; POP;
+	void *dst = TOS.a; POP;
+	FAST_RFILL(dst, size, pat);
+	MIRP
 
 // String compare, case insensitive:
 // : string=ci  ( str1 len1 str2 len2 -- equal? )
diff --git a/qemu/roms/SLOF/slof/prim.in b/qemu/roms/SLOF/slof/prim.in
index 7a0d6a2ed..855f59262 100644
--- a/qemu/roms/SLOF/slof/prim.in
+++ b/qemu/roms/SLOF/slof/prim.in
@@ -104,8 +104,9 @@ cod(SEMICOLON)
 cod(EXECUTE)
 
 cod(MOVE)
-// cod(RMOVE64)
 cod(RMOVE)
+cod(MRMOVE)
+cod(RFILL)
 cod(ZCOUNT)
 con(HASH-SIZE HASHSIZE)
 cod(HASH)