#
# TCL to allow the Xilinx PS7 Init TCL code to run in OpenOCD.
#

proc mrd { args } {
    if {[llength $args] == 0} {
        echo "mrd address \[count \[w|h|b\]\]"
        echo "    Read <count> memory locations starting at <address>.  Defaults to one word."
        return
    }
    set addr [lindex $args 0]
    set count  1
    set bits  32
    if {[llength $args] > 1} {
        set count [lindex $args 1]
        if {[llength $args] > 2} {
            switch [lindex $args 2] {
                w       { set bits 32 }
                h       { set bits 16 }
                b       { set bits  8 }
                default { set bits 32 }
            }
        }
    }
    mem2array x $bits $addr $count
    set nibbles [expr {$bits / 4}]
    set bytes   [expr {$bits / 8}]
    set result  {}
    foreach {idx elmt} $x {
        append result [format "%08x:   %0*x\n" [expr {$addr + $idx * $bytes}] $nibbles $elmt]
    }
    return $result
}

proc mwr { args } {
  set addr [lindex $args 1]
  set data [lindex $args 2]
  mww $addr $data
}

proc mask_write { addr mask value } {
    set curval "0x[string range [mrd $addr] end-8 end]"
    set maskedval [expr {$curval & ~$mask}]
    #echo "curval = [format 0x%08x $curval] maskedval = [format 0x%08x $maskedval]"
    set writeval(0) [expr {$maskedval | $value}]
    #echo " $addr <= [format 0x%08x $writeval(0)] ([format 0x%08x $curval]: [format 0x%08x $mask]/[format 0x%08x $value])"
    array2mem writeval 32 $addr 1
}

proc xilinx_ps7_init { } {
   poll off
   reset init
   reset halt
   targets zynq.cpu.0
   sleep 100
   halt
   ps7_debug
   ps7_init
   ps7_post_config
   poll on
}