\ ***************************************************************************** \ * 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? 0 VALUE fdt-cas-pass 0 VALUE fdt-generation# : fdt-update-from-fdt ( -- ) fdt-generation# encode-int s" slof,from-fdt" property ; \ 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 constant /fdth 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@ 11 >= 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 unsupported 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 -- pa ps ) 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 fdt-update-from-fdt 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 ; : (fdt-replace-phandles) ( old new propname propnamelen node -- ) get-property IF 2drop EXIT THEN BEGIN dup WHILE ( old new prop-addr prop-len ) over l@ 4 pick = IF 2 pick 2 pick l! \ replace old with new in place TRUE TO (fdt-phandle-replaced) THEN 4 - swap 4 + swap REPEAT 2drop 2drop ; : (phandle>node) ( phandle current -- node|0 ) dup s" phandle" rot get-property 0= IF decode-int nip nip ( phandle current phandle-prop ) 2 pick = IF fdt-debug IF ." Found phandle; " dup . ." <= " over . cr THEN nip ( current ) EXIT THEN ELSE dup s" linux-phandle" rot get-property 0= IF decode-int nip nip ( phandle current phandle-prop ) 2 pick = IF fdt-debug IF ." Found linux-phandle; " dup . ." <= " over . cr THEN nip ( current ) EXIT THEN THEN THEN child BEGIN dup WHILE 2dup RECURSE ?dup 0<> IF nip nip EXIT THEN PEER REPEAT 2drop 0 ; : phandle>node ( phandle -- node ) s" /" find-node (phandle>node) ; : (fdt-patch-phandles) ( prop-addr prop-len -- ) BEGIN dup WHILE ( prop-addr prop-len ) over l@ phandle>node ?dup 0<> IF fdt-debug IF ." ### Patching phandle=" 2 pick l@ . cr THEN 2 pick l! TRUE TO (fdt-phandle-replaced) THEN 4 - swap 4 + swap REPEAT 2drop ; : (fdt-patch-interrupt-map) ( prop-addr prop-len -- ) \ interrupt-controller phandle is expected to be the same across the map over 10 + l@ phandle>node ?dup 0= IF 2drop EXIT THEN -rot fdt-debug IF ." ### Patching interrupt-map: " over 10 + l@ . ." => " 2 pick . cr THEN TRUE TO (fdt-phandle-replaced) BEGIN dup WHILE ( newph prop-addr prop-len ) 2 pick 2 pick 10 + l! 1c - swap 1c + swap REPEAT 3drop ; : fdt-patch-phandles ( prop-addr prop-len nameadd namelen -- ) 2dup s" interrupt-map" str= IF 2drop (fdt-patch-interrupt-map) EXIT THEN 2dup s" interrupt-parent" str= IF 2drop (fdt-patch-phandles) EXIT THEN 2dup s" ibm,gpu" str= IF 2drop (fdt-patch-phandles) EXIT THEN 2dup s" ibm,npu" str= IF 2drop (fdt-patch-phandles) EXIT THEN 2dup s" ibm,nvlink" str= IF 2drop (fdt-patch-phandles) EXIT THEN 2dup s" memory-region" str= IF 2drop (fdt-patch-phandles) EXIT THEN 4drop ; \ Replace one phandle "old" with a phandle "new" in "node" and recursively \ in its child nodes: : 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 2dup s" interrupt-parent" r@ (fdt-replace-phandles) 2dup s" ibm,gpu" r@ (fdt-replace-phandles) 2dup s" ibm,npu" r@ (fdt-replace-phandles) 2dup s" ibm,nvlink" r@ (fdt-replace-phandles) 2dup s" memory-region" r@ (fdt-replace-phandles) \ ... 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 ; \ Replace one FDT phandle "val" with a OF1275 phandle "node" in the \ whole tree: : fdt-update-phandle ( val node -- ) >r FALSE TO (fdt-phandle-replaced) r@ s" /" find-node ( val node root ) fdt-replace-all-phandles (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 r> drop ; \ Check whether a node has "phandle" or "linux,phandle" properties \ and replace them: : fdt-fix-node-phandle ( node -- ) >r s" phandle" r@ get-property 0= IF decode-int nip nip \ ." found phandle: " dup . cr r@ fdt-update-phandle 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 ; : str=phandle? ( s len -- true|false ) 2dup s" phandle" str= >r s" linux,phandle" str= r> or ; : fdt-cas-finish-device ( -- ) " reg" get-node get-package-property IF ELSE fdt-reg-unit THEN get-node finish-device set-node ; : (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 ( a1 $name ) dup 0 = IF drop drop " /" THEN 40 left-parse-string 2swap ?dup 0 <> IF nip 1 + + \ Add the string len +@ ELSE drop THEN fdt-cas-pass 0= IF \ The guest might have asked to change the interrupt controller \ type. It doesn't make sense to merge the new node and the \ existing "interrupt-controller" node in this case. Delete the \ latter. A brand new one will be created with the appropriate \ properties and unit name. 2dup " interrupt-controller" find-substr 0= IF " interrupt-controller" find-node ?dup 0 <> IF fdt-debug IF ." Deleting existing node: " dup .node cr THEN delete-node THEN THEN THEN 2dup find-node ?dup 0 <> IF set-node fdt-debug IF ." Setting node: " 2dup type cr THEN 2drop \ newnode?=0: updating the existing node, i.e. pass1 adds only phandles 0 ELSE fdt-cas-pass 0 <> IF \ We could not find the node added in the previous pass, \ most likely because it is hotplug-under-hotplug case \ (such as PCI bridge under bridge) when missing new node methods \ such as "decode-unit" are critical. \ Reboot when detect such case which is expected as it is a part of \ ibm,client-architecture-support. ." Cannot handle FDT update for the " 2dup type ." node, rebooting" cr reset-all THEN fdt-debug IF ." Creating node: " 2dup type cr THEN new-device 2dup " @" find-substr nip device-name \ newnode?=1: adding new node, i.e. pass1 adds all properties, \ most importantly "reg". After reading properties, we call \ "fdt-cas-finish-device" which sets the unit address from "reg". 1 THEN swap ( newnode? a1 ) fdt-debug IF ." Current now: " pwd get-node ." = " . cr THEN fdt-cas-pass 0= IF fdt-update-from-fdt THEN BEGIN fdt-next-tag dup OF_DT_END_NODE <> WHILE ( newnode? a1 tag ) dup OF_DT_PROP = IF drop dup ( newnode? a1 a1 ) dup l@ dup rot 4 + ( newnode? a1 s s a2) dup l@ swap 4 + ( newnode? a1 s s i a3 ) rot ( newnode? a1 s i a3 s ) fdt-encode-prop rot ( newnode? a1 s pa ps i) fdt-fetch-string ( newnode? a1 s pa ps na ns ) fdt-cas-pass CASE 0 OF 2dup str=phandle? 7 pick or IF fdt-debug IF 4dup ." Property: " type ." =" swap ." @" . ." " .d ." bytes" cr THEN property ELSE 4drop THEN ENDOF 1 OF 2dup str=phandle? not IF fdt-debug IF 4dup ." Property: " type ." =" swap ." @" . ." " .d ." bytes" cr THEN 4dup fdt-patch-phandles property ELSE 4drop THEN ENDOF 2 OF 2dup str=phandle? IF fdt-debug IF 4dup ." Deleting: " type ." =" swap ." @" . ." " .d ." bytes" cr THEN delete-property 2drop ELSE 4drop THEN ENDOF ENDCASE + 8 + 3 + fffffffc and ELSE ( newnode? a1 tag ) dup OF_DT_BEGIN_NODE = IF 2 pick IF rot drop 0 -rot fdt-cas-finish-device fdt-debug IF ." Finished node: " pwd get-node ." = " . cr THEN THEN drop ( a1 ) 4 - (fdt-fix-cas-node) get-parent set-node ELSE ." Error " cr drop false to fdt-cas-fix? EXIT THEN THEN REPEAT ( newnode? a1 tag ) drop swap ( a1 newnode? ) IF fdt-cas-finish-device fdt-debug IF ." Finished subnode: " pwd get-node ." = " . cr THEN THEN ; : alias-dev-path ( xt -- dev-path len ) link> execute decode-string 2swap 2drop ; : alias-name ( xt -- alias-name len ) link> >name name>string ; : fdt-cas-alias-obsolete? ( xt -- true|false ) alias-dev-path find-node 0= ; : (fdt-cas-delete-obsolete-aliases) ( xt -- ) dup IF dup @ recurse dup alias-name s" name" str= IF ELSE dup fdt-cas-alias-obsolete? IF fdt-debug IF ." Deleting obsolete alias: " dup alias-name type ." -> " dup alias-dev-path type cr THEN dup alias-name delete-property THEN THEN THEN drop ; : fdt-cas-delete-obsolete-aliases ( -- ) s" /aliases" find-device get-node node>properties @ cell+ @ (fdt-cas-delete-obsolete-aliases) device-end ; : fdt-cas-node-obsolete? ( node -- true|false) s" slof,from-fdt" rot get-package-property IF \ Not a QEMU originated node false ELSE decode-int nip nip fdt-generation# < THEN ; : (fdt-cas-search-obsolete-nodes) ( node -- ) dup child BEGIN dup WHILE dup recurse peer REPEAT drop dup fdt-cas-node-obsolete? IF fdt-debug IF dup ." Deleting obsolete node: " dup .node ." = " . cr THEN dup delete-node THEN drop ; : fdt-cas-delete-obsolete-nodes ( -- ) s" /" find-device get-node (fdt-cas-search-obsolete-nodes) fdt-cas-delete-obsolete-aliases ; : fdt-fix-cas-node ( start -- ) fdt-generation# 1+ to fdt-generation# 0 to fdt-cas-pass dup (fdt-fix-cas-node) drop \ Add phandles fdt-cas-delete-obsolete-nodes \ Delete removed devices 1 to fdt-cas-pass dup (fdt-fix-cas-node) drop \ Patch+add other properties 2 to fdt-cas-pass dup (fdt-fix-cas-node) drop \ Delete phandles from pass 0 drop ; : fdt-fix-cas-success fdt-cas-fix? ; s" /" find-node fdt-fix-phandles