--- /dev/null
+\ *****************************************************************************
+\ * 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
+\ ****************************************************************************/
+
+0 VALUE fdt-debug
+TRUE VALUE fdt-cas-fix?
+
+\ Bail out if no fdt
+fdt-start 0 = IF -1 throw THEN
+
+struct
+ 4 field >fdth_magic
+ 4 field >fdth_tsize
+ 4 field >fdth_struct_off
+ 4 field >fdth_string_off
+ 4 field >fdth_rsvmap_off
+ 4 field >fdth_version
+ 4 field >fdth_compat_vers
+ 4 field >fdth_boot_cpu
+ 4 field >fdth_string_size
+ 4 field >fdth_struct_size
+drop
+
+h# d00dfeed constant OF_DT_HEADER
+h# 1 constant OF_DT_BEGIN_NODE
+h# 2 constant OF_DT_END_NODE
+h# 3 constant OF_DT_PROP
+h# 4 constant OF_DT_NOP
+h# 9 constant OF_DT_END
+
+\ Create some variables early
+0 value fdt-start-addr
+0 value fdt-struct
+0 value fdt-strings
+
+: fdt-init ( fdt-start -- )
+ dup to fdt-start-addr
+ dup dup >fdth_struct_off l@ + to fdt-struct
+ dup dup >fdth_string_off l@ + to fdt-strings
+ drop
+;
+fdt-start fdt-init
+
+\ Dump fdt header for all to see and check FDT validity
+: fdt-check-header ( -- )
+ fdt-start-addr dup 0 = IF
+ ." No flat device tree !" cr drop -1 throw EXIT THEN
+ hex
+ fdt-debug IF
+ ." Flat device tree header at 0x" dup . s" :" type cr
+ ." magic : 0x" dup >fdth_magic l@ . cr
+ ." total size : 0x" dup >fdth_tsize l@ . cr
+ ." offset to struct : 0x" dup >fdth_struct_off l@ . cr
+ ." offset to strings: 0x" dup >fdth_string_off l@ . cr
+ ." offset to rsvmap : 0x" dup >fdth_rsvmap_off l@ . cr
+ ." version : " dup >fdth_version l@ decimal . hex cr
+ ." last compat vers : " dup >fdth_compat_vers l@ decimal . hex cr
+ dup >fdth_version l@ 2 >= IF
+ ." boot CPU : 0x" dup >fdth_boot_cpu l@ . cr
+ THEN
+ dup >fdth_version l@ 3 >= IF
+ ." strings size : 0x" dup >fdth_string_size l@ . cr
+ THEN
+ dup >fdth_version l@ 17 >= IF
+ ." struct size : 0x" dup >fdth_struct_size l@ . cr
+ THEN
+ THEN
+ dup >fdth_magic l@ OF_DT_HEADER <> IF
+ ." Flat device tree has incorrect magic value !" cr
+ drop -1 throw EXIT
+ THEN
+ dup >fdth_version l@ 10 < IF
+ ." Flat device tree has usupported version !" cr
+ drop -1 throw EXIT
+ THEN
+
+ drop
+;
+fdt-check-header
+
+\ Fetch next tag, skip nops and increment address
+: fdt-next-tag ( addr -- nextaddr tag )
+ 0 ( dummy tag on stack for loop )
+ BEGIN
+ drop ( drop previous tag )
+ dup l@ ( read new tag )
+ swap 4 + swap ( increment addr )
+ dup OF_DT_NOP <> UNTIL ( loop until not nop )
+;
+
+\ Parse unit name and advance addr
+: fdt-fetch-unit ( addr -- addr $name )
+ dup from-cstring \ get string size
+ 2dup + 1 + 3 + fffffffc and -rot
+;
+
+\ Update unit with information from the reg property...
+\ ... this is required for the PCI nodes for example.
+: fdt-reg-unit ( prop-addr prop-len -- )
+ decode-phys ( prop-addr' prop-len' phys.lo ... phys.hi )
+ set-unit ( prop-addr' prop-len' )
+ 2drop
+;
+
+\ Lookup a string by index
+: fdt-fetch-string ( index -- str-addr str-len )
+ fdt-strings + dup from-cstring
+;
+
+: fdt-create-dec s" decode-unit" $CREATE , DOES> @ hex64-decode-unit ;
+: fdt-create-enc s" encode-unit" $CREATE , DOES> @ hex64-encode-unit ;
+
+\ Check whether array contains an zero-terminated ASCII string:
+: fdt-prop-is-string? ( addr len -- string? )
+ dup 1 < IF 2drop FALSE EXIT THEN \ Check for valid length
+ 1-
+ 2dup + c@ 0<> IF 2drop FALSE EXIT THEN \ Check zero-termination
+ test-string
+;
+
+\ Encode fdt property to OF property
+: fdt-encode-prop ( addr len -- )
+ 2dup fdt-prop-is-string? IF
+ 1- encode-string
+ ELSE
+ encode-bytes
+ THEN
+;
+
+\ Method to unflatten a node
+: fdt-unflatten-node ( start -- end )
+ \ this can and will recurse
+ recursive
+
+ \ Get & check first tag of node ( addr -- addr)
+ fdt-next-tag dup OF_DT_BEGIN_NODE <> IF
+ s" Weird tag 0x" type . " at start of node" type cr
+ -1 throw
+ THEN drop
+
+ new-device
+
+ \ Parse name, split unit address
+ fdt-fetch-unit
+ dup 0 = IF drop drop " /" THEN
+ 40 left-parse-string
+ \ Set name
+ device-name
+
+ \ Set preliminary unit address - might get overwritten by reg property
+ dup IF
+ " #address-cells" get-parent get-package-property IF
+ 2drop
+ ELSE
+ decode-int nip nip
+ hex-decode-unit
+ set-unit
+ THEN
+ ELSE 2drop THEN
+
+ \ Iterate sub tags
+ BEGIN
+ fdt-next-tag dup OF_DT_END_NODE <>
+ WHILE
+ dup OF_DT_PROP = IF
+ \ Found property
+ drop dup ( drop tag, dup addr : a1 a1 )
+ dup l@ dup rot 4 + ( fetch size, stack is : a1 s s a2)
+ dup l@ swap 4 + ( fetch nameid, stack is : a1 s s i a3 )
+ rot ( we now have: a1 s i a3 s )
+ fdt-encode-prop rot ( a1 s pa ps i)
+ fdt-fetch-string ( a1 s pa ps na ns )
+ 2dup s" reg" str= IF
+ 2swap 2dup fdt-reg-unit 2swap
+ THEN
+ property
+ + 8 + 3 + fffffffc and
+ ELSE dup OF_DT_BEGIN_NODE = IF
+ drop ( drop tag )
+ 4 -
+ fdt-unflatten-node
+ ELSE
+ drop -1 throw
+ THEN THEN
+ REPEAT drop \ drop tag
+
+ \ Create encode/decode unit
+ " #address-cells" get-node get-package-property IF ELSE
+ decode-int dup fdt-create-dec fdt-create-enc 2drop
+ THEN
+
+ finish-device
+;
+
+\ Start unflattening
+: fdt-unflatten-tree
+ fdt-debug IF
+ ." Unflattening device tree..." cr THEN
+ fdt-struct fdt-unflatten-node drop
+ fdt-debug IF
+ ." Done !" cr THEN
+;
+fdt-unflatten-tree
+
+\ Find memory size
+: fdt-parse-memory
+ \ XXX FIXME Handle more than one memory node, and deal
+ \ with RMA vs. full access
+ " /memory@0" find-device
+ " reg" get-node get-package-property IF throw -1 THEN
+
+ \ XXX FIXME Assume one entry only in "reg" property for now
+ decode-phys 2drop decode-phys
+ my-#address-cells 1 > IF 20 << or THEN
+
+ fdt-debug IF
+ dup ." Memory size: " . cr
+ THEN
+ \ claim.fs already released the memory between 0 and MIN-RAM-SIZE,
+ \ so we've got only to release the remaining memory now:
+ MIN-RAM-SIZE swap MIN-RAM-SIZE - release
+ 2drop device-end
+;
+fdt-parse-memory
+
+
+\ Claim fdt memory and reserve map
+: fdt-claim-reserve
+ fdt-start-addr
+ dup dup >fdth_tsize l@ 0 claim drop
+ dup >fdth_rsvmap_off l@ +
+ BEGIN
+ dup dup x@ swap 8 + x@
+ dup 0 <>
+ WHILE
+ fdt-debug IF
+ 2dup swap ." Reserve map entry: " . ." : " . cr
+ THEN
+ 0 claim drop
+ 10 +
+ REPEAT drop drop drop
+;
+fdt-claim-reserve
+
+
+\ The following functions are use to replace the FDT phandle and
+\ linux,phandle properties with our own OF1275 phandles...
+
+\ This is used to check whether we successfully replaced a phandle value
+0 VALUE (fdt-phandle-replaced)
+
+\ Replace phandle value in "interrupt-map" property
+: fdt-replace-interrupt-map ( old new prop-addr prop-len -- old new )
+ BEGIN
+ dup ( old new prop-addr prop-len prop-len )
+ WHILE
+ \ This is a little bit ugly ... we're accessing the property at
+ \ hard-coded offsets instead of analyzing it completely...
+ swap dup 10 + ( old new prop-len prop-addr prop-addr+10 )
+ dup l@ 5 pick = IF
+ \ it matches the old phandle value!
+ 3 pick swap l!
+ TRUE TO (fdt-phandle-replaced)
+ ELSE
+ drop
+ THEN
+ ( old new prop-len prop-addr )
+ 1c + swap 1c -
+ ( old new new-prop-addr new-prop-len )
+ REPEAT
+ 2drop
+;
+
+\ Replace one FDT phandle "old" with a OF1275 phandle "new" in the
+\ whole tree:
+: fdt-replace-all-phandles ( old new node -- )
+ \ ." Replacing in " dup node>path type cr
+ >r
+ s" interrupt-map" r@ get-property 0= IF
+ ( old new prop-addr prop-len R: node )
+ fdt-replace-interrupt-map
+ THEN
+ s" interrupt-parent" r@ get-property 0= IF
+ ( old new prop-addr prop-len R: node )
+ decode-int -rot 2drop ( old new val R: node )
+ 2 pick = IF ( old new R: node )
+ dup encode-int s" interrupt-parent" r@ set-property
+ TRUE TO (fdt-phandle-replaced)
+ THEN
+ THEN
+ \ ... add more properties that have to be fixed here ...
+ r>
+ \ Now recurse over all child nodes: ( old new node )
+ child BEGIN
+ dup
+ WHILE
+ 3dup RECURSE
+ PEER
+ REPEAT
+ 3drop
+;
+
+\ Check whether a node has "phandle" or "linux,phandle" properties
+\ and replace them:
+: fdt-fix-node-phandle ( node -- )
+ >r
+ FALSE TO (fdt-phandle-replaced)
+ s" phandle" r@ get-property 0= IF
+ decode-int ( p-addr2 p-len2 val )
+ \ ." found phandle: " dup . cr
+ r@ s" /" find-node ( p-addr2 p-len2 val node root )
+ fdt-replace-all-phandles ( p-addr2 p-len2 )
+ 2drop
+ (fdt-phandle-replaced) IF
+ r@ set-node
+ s" phandle" delete-property
+ s" linux,phandle" delete-property
+ ELSE
+ diagnostic-mode? IF
+ cr ." Warning: Did not replace phandle in " r@ node>path type cr
+ THEN
+ THEN
+ THEN
+ r> drop
+;
+
+\ Recursively walk through all nodes to fix their phandles:
+: fdt-fix-phandles ( node -- )
+ \ ." fixing phandles of " dup node>path type cr
+ dup fdt-fix-node-phandle
+ child BEGIN
+ dup
+ WHILE
+ dup RECURSE
+ PEER
+ REPEAT
+ drop
+ device-end
+;
+
+: fdt-create-cas-node ( name -- )
+ 2dup
+ 2dup " memory@" find-substr 0 = IF
+ fdt-debug IF ." Creating memory@ " cr THEN
+ new-device
+ 2dup " @" find-substr nip device-name \ Parse the node name
+ 2dup
+ 2dup " @" find-substr rot over + 1 + -rot - 1 - \ Jump to addr afte "@"
+ parse-2int nip xlsplit set-unit \ Parse and set unit
+ finish-device
+ ELSE
+ 2dup " ibm,dynamic-reconfiguration-memory" find-substr 0 = IF
+ fdt-debug IF ." Creating ibm,dynamic-reconfiguration-memory " cr THEN
+ new-device
+ device-name
+ finish-device
+ ELSE
+ 2drop 2drop
+ false to fdt-cas-fix?
+ ." Node not supported " cr
+ EXIT
+ THEN
+ THEN
+
+ find-node ?dup 0 <> IF set-node THEN
+;
+
+: fdt-fix-cas-node ( start -- end )
+ recursive
+ fdt-next-tag dup OF_DT_BEGIN_NODE <> IF
+ ." Error " cr
+ false to fdt-cas-fix?
+ EXIT
+ THEN drop
+ fdt-fetch-unit
+ dup 0 = IF drop drop " /" THEN
+ 40 left-parse-string
+ 2swap ?dup 0 <> IF
+ nip
+ 1 + + \ Add the string len +@
+ ELSE
+ drop
+ THEN
+ fdt-debug IF ." Setting node: " 2dup type cr THEN
+ 2dup find-node ?dup 0 <> IF
+ set-node 2drop
+ ELSE
+ fdt-debug IF ." Node not found, creating " 2dup type cr THEN
+ fdt-create-cas-node
+ THEN
+ fdt-debug IF ." Current now: " pwd cr THEN
+ BEGIN
+ fdt-next-tag dup OF_DT_END_NODE <>
+ WHILE
+ dup OF_DT_PROP = IF
+ fdt-debug IF ." Found property " cr THEN
+ drop dup ( drop tag, dup addr : a1 a1 )
+ dup l@ dup rot 4 + ( fetch size, stack is : a1 s s a2)
+ dup l@ swap 4 + ( fetch nameid, stack is : a1 s s i a3 )
+ rot ( we now have: a1 s i a3 s )
+ fdt-encode-prop rot ( a1 s pa ps i)
+ fdt-fetch-string ( a1 s pa ps na ns )
+ property
+ fdt-debug IF ." Setting property done " cr THEN
+ + 8 + 3 + fffffffc and
+ ELSE dup OF_DT_BEGIN_NODE = IF
+ drop ( drop tag )
+ 4 -
+ fdt-fix-cas-node
+ get-parent set-node
+ fdt-debug IF ." Returning back " pwd cr THEN
+ ELSE
+ ." Error " cr
+ drop
+ false to fdt-cas-fix?
+ EXIT
+ THEN
+ THEN
+ REPEAT
+ drop \ drop tag
+;
+
+: fdt-fix-cas-success
+ fdt-cas-fix?
+;
+
+s" /" find-node fdt-fix-phandles
Code Review / kvmfornfv.git / blobdiff