\ *****************************************************************************
\ * 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