[OpenBIOS] [PATCH] Add user resolutions to QEMU, VGA node's fb-modes property.

[Programmingkid]

Implement the add-resolutions word. It will copy the user resolutions in the
options node's resolutions property to the QEMU,VGA node's fb-modes property.

Signed-off-by: John Arbuckle <programmingkidx at gmail.com>
---
 arch/ppc/qemu/init.c  |  2 +-
 arch/ppc/qemu/qemu.fs | 68 +++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 69 insertions(+), 1 deletion(-)

diff --git a/arch/ppc/qemu/init.c b/arch/ppc/qemu/init.c
index 6f21814..df873ce 100644
--- a/arch/ppc/qemu/init.c
+++ b/arch/ppc/qemu/init.c
@@ -631,7 +631,7 @@ go(void)
     if (find_dev("/rom/macos")) {
         fword("insert-copyright-property");
     }
-    
+    feval("add-resolutions");
     feval("saved-program-state >sps.entry @");
     addr = POP();
 
diff --git a/arch/ppc/qemu/qemu.fs b/arch/ppc/qemu/qemu.fs
index 3d99a34..10df618 100644
--- a/arch/ppc/qemu/qemu.fs
+++ b/arch/ppc/qemu/qemu.fs
@@ -138,3 +138,71 @@ variable keyboard-phandle 0 keyboard-phandle !
     3drop 0
   then
 ;
+
+\ Adds user's resolutions from /Options node's resolutions property to
+\ /pci/QEMU,VGA node's fb-modes property.
+\ Turns a string of "widthxheight,..." into an array of ints.
+
+: add-resolutions { ; fb-mode-addr fb-mode-len value res-addr res-len } ( -- )
+    decimal
+
+    " /options" find-device  ( -- )
+    " resolutions" active-package get-package-property  ( -- res-addr res-len false )
+    abort" "  \ Fail quietly because user resolutions are optional
+    -> res-len    
+    -> res-addr
+
+    \ Add the first value to the array
+    0 0 res-addr res-len 
+    >number
+    -> res-len -> res-addr
+    drop encode-int
+    -> fb-mode-len -> fb-mode-addr
+    res-addr res-len 1 /string
+    -> res-len -> res-addr
+
+    begin
+        res-len 0 = not
+    while
+        0 0
+        res-addr res-len
+        >number  ( ud1 c-addr1 u1 -- ud2 0 c-addr2 u2 )
+        -> res-len
+        -> res-addr
+        drop
+        -> value
+
+        \ go to the next number after 'x' or ','
+        res-addr res-len 1 /string
+        -> res-len
+        -> res-addr
+        
+        \ Encode and add value to array
+        fb-mode-addr fb-mode-len value  ( -- fb-mode-addr fb-mode-len value )
+        encode-int  ( -- new-fb-mode-addr new-fb-mode-len )
+        encode+  ( -- new-fb-mode-addr new-fb-mode-len )
+        -> fb-mode-len
+        -> fb-mode-addr        
+    repeat
+    
+    \ Get the original resolutions
+    " /pci/QEMU,VGA" find-device  ( -- )
+    " fb-modes" active-package get-package-property  ( -- prop-addr prop-len false )
+    abort" Could not find fb-modes property in QEMU,VGA node!"  ( -- prop-addr prop-len )
+
+    \ Needed to fix a limit of encode+
+    encode-bytes  ( -- new-prop-addr new-prop-len )
+    
+    fb-mode-addr fb-mode-len  ( -- new-prop-addr new-prop-len fb-mode-addr fb-mode-len )
+    2swap  ( --  fb-mode-addr fb-mode-len new-prop-addr new-prop-len )
+    
+    \ Add user resolutions first then add built-in resolutions if space permits
+    encode+ ( -- new-addr new-len )
+    -> fb-mode-len
+    -> fb-mode-addr
+    
+    \ Update fb-modes property
+    " /pci/QEMU,VGA" find-device
+    fb-mode-addr fb-mode-len
+    " fb-modes" property
+;
-- 
2.7.2