#! /bin/bash
# \
export RLWRAP_ #\
exec rlwrap -C lm32-ctl -I /opt/quartus/quartus/bin/quartus_stp --64bit -t "$0" "$@"

########################### LOW LEVEL ACCESS #########################
proc jtag_put {val} {
  device_virtual_dr_shift -instance_index 0 -length 11 -dr_value "[format %03X $val]" -value_in_hex -no_captured_dr_value
}

proc jtag_get {} {
  return 0x[device_virtual_dr_shift -instance_index 0 -length 11 -dr_value 000 -value_in_hex]
}

############################ SAFE-ISHL ACCESS #########################

proc jtag_val {idx val} {
  set v [expr {($val << 3) | $idx}]
  jtag_put "$v"
}

proc jtag_cmd {idx cmd} {
  set val [expr {$cmd << 4}]
  jtag_val "$idx" "$val"
}

proc jtag_low {i} {
  set high 1
  while {$high >= 1} {
    set val [jtag_get]
    set high [expr {($val >> $i) & 1}]
  }
  return [expr {$val >> 3}]
}

proc jtag_high {i} {
  set high 0
  while {$high < 1} {
    set val [jtag_get]
    set high [expr {($val >> $i) & 1}]
  }
  return [expr {$val >> 3}]
}

############################## COMMANDS ###############################

proc jtag_read_addr {addr} {
  jtag_cmd 0 1
  jtag_val 0 "[expr {($addr >> 24) & 0xff}]"
  jtag_val 0 "[expr {($addr >> 16) & 0xff}]"
  jtag_val 0 "[expr {($addr >>  8) & 0xff}]"
  jtag_val 0 "[expr {($addr >>  0) & 0xff}]"
  
  return [jtag_low 2]
}

proc jtag_read_next {} {
  jtag_cmd 0 3
  return [jtag_low 2]
}

proc jtag_read_memory {addr len} {
  set out [list]
  
  if {$len > 0} {
    lappend out "[format %02X [jtag_read_addr $addr]]"
  }
  
  for {set i 1} {$i < $len} {incr i} {
    #set x [expr {$addr+$i}]
    #lappend out "[format %02X [jtag_read_addr $x]]"
    lappend out "[format %02X [jtag_read_next]]"
  }
  return "$out"
}

proc jtag_write_addr {addr val} {
  jtag_cmd 0 2
  jtag_val 0 "[expr {($addr >> 24) & 0xff}]"
  jtag_val 0 "[expr {($addr >> 16) & 0xff}]"
  jtag_val 0 "[expr {($addr >>  8) & 0xff}]"
  jtag_val 0 "[expr {($addr >>  0) & 0xff}]"
  jtag_val 0 "$val"
  
  return [jtag_low 2]
}

proc jtag_write_next {val} {
  jtag_cmd 0 4
  jtag_val 0 "$val"
  return [jtag_low 2]
}

proc jtag_write_memory {addr data} {
  set first 1
  foreach j $data {
    if {$first == 1} {
      set first 0
      jtag_write_addr "$addr" "$j"
    } else {
      jtag_write_next "$j"
    }
  }
}

proc jtag_uart_write {val} {
  jtag_low 1
  jtag_val 1 "$val"
}

proc jtag_uart_read {} {
  set val [jtag_get]
  while {($val & 1) == 1} {
    jtag_val 2 0
    set val [jtag_get]
    set inb [expr {$val >> 3}]
    puts -nonewline "[format %02X $inb] "
  }
  puts "."
}

proc jtag_write_csr {csr val} {
  jtag_cmd 0 5
  jtag_val 0 "[expr {($val >> 24) & 0xff}]"
  jtag_val 0 "[expr {($val >> 16) & 0xff}]"
  jtag_val 0 "[expr {($val >>  8) & 0xff}]"
  jtag_val 0 "[expr {($val >>  0) & 0xff}]"
  jtag_val 0 "$csr"
  
  return [jtag_low 2]
}

proc jtag_break {} {
  jtag_cmd 0 6
}

proc jtag_reset {} {
  jtag_cmd 0 7
}

# Move back to idle state
proc jtag_sync {} {
  for {set i 0} {$i < 10} {incr i} {
    jtag_cmd 0 0
    after 20
  }
}

################################# ASM #################################

proc opcode {val} {
  switch $val {
     0 { return "srui"    }
     1 { return "nori"    }
     2 { return "muli"    }
     3 { return "sh"      }
     4 { return "lb"      }
     5 { return "sri"     }
     6 { return "xori"    }
     7 { return "lh"      }
     8 { return "andi"    }
     9 { return "xnori"   }
    10 { return "lw"      }
    11 { return "lhu"     }
    12 { return "sb"      }
    13 { return "addi"    }
    14 { return "ori"     }
    15 { return "sli"     }
    16 { return "lbu"     }
    17 { return "be"      }
    18 { return "bg"      }
    19 { return "bge"     }
    20 { return "bgeu"    }
    21 { return "bgu"     }
    22 { return "sw"      }
    23 { return "bne"     }
    24 { return "andhi"   }
    25 { return "cmpei"   }
    26 { return "cmpgi"   }
    27 { return "cmpgei"  }
    28 { return "cmpgeui" }
    29 { return "cmpgui"  }
    30 { return "orhi"    }
    31 { return "cmpnei"  }
    32 { return "sru"     }
    33 { return "nor"     }
    34 { return "mul"     }
    35 { return "divu"    }
    36 { return "rcsr"    }
    37 { return "sr"      }
    38 { return "xor"     }
    39 { return "div"     }
    40 { return "and"     }
    41 { return "xnor"    }
    42 { return "??"      }
    43 { return "raise"   }
    44 { return "sextb"   }
    45 { return "add"     }
    46 { return "or"      }
    47 { return "sl"      }
    48 { return "b"       }
    49 { return "modu"    }
    50 { return "sub"     }
    51 { return "??"      }
    52 { return "wcsr"    }
    53 { return "mod"     }
    54 { return "call"    }
    55 { return "sexth"   }
    56 { return "bi"      }
    57 { return "cmpe"    }
    58 { return "cmpg"    }
    59 { return "cmpge"   }
    60 { return "cmpgeu"  }
    61 { return "cmpgu"   }
    62 { return "calli"   }
    63 { return "cmpne"   }
  }
}

proc reg {i} {
  switch $i {
    26 { return "gp" }
    27 { return "fp" }
    28 { return "sp" }
    29 { return "ra" }
    30 { return "ea" }
    31 { return "ba" }
    default { return "r$i" }
  }
}

proc csr {i} {
  switch $i {
    0 { return "IE" }
    1 { return "IM" }
    2 { return "IP" }
    3 { return "ICC" }
    4 { return "DCC" }
    5 { return "CC" }
    6 { return "CFG" }
    7 { return "EBA" }
    8 { return "DC" }
    9 { return "DEBA" }
   14 { return "JTX" }
   15 { return "JRX" }
   16 { return "BP0" }
   17 { return "BP1" }
   18 { return "BP2" }
   19 { return "BP3" }
   24 { return "WP0" }
   25 { return "WP1" }
   26 { return "WP2" }
   27 { return "WP3" }
  }
}

proc imm16 {i} {
  if {$i >= 32768} {
    return "-[expr {65536 - $i}]"
  } else {
    return "+$i"
  }
}

proc imm26 {i} {
  if {$i >= 33554432} {
    return "-[expr {67108864 - $i}]"
  } else {
    return "+$i"
  }
}

proc opfmt {op} {
  set code [expr {$op >> 26}]
  set r0 [expr {($op >> 21) & 31}]
  set r1 [expr {($op >> 16) & 31}]
  set r2 [expr {($op >> 11) & 31}]
  set i16 [expr {$op & 0xffff}]
  set i26 [expr {$op & 0x3ffffff}]
  
  if {$code == 4 || $code == 7 || $code == 10 || $code == 11 || $code == 16} {
    # lb, lh, lw, lhu, lbu
    return "[opcode $code] [reg $r1], ([reg $r0][imm16 $i16])"
  } elseif {$code == 3 || $code == 12 || $code == 22} { # sh, sb, sw
    return "[opcode $code] ([reg $r0][imm16 $i16]), [reg $r1]"
  } elseif {$code <= 32} { # (op, op, imm) instruction
    return "[opcode $code] [reg $r1], [reg $r0], [imm16 $i16]"
  } elseif {$code == 48 || $code == 54} { # b, call
    return "[opcode $code] [reg $r0]"
  } elseif {$code == 36} { # rcsr
    return "[opcode $code] [reg $r2], [csr $r0]"
  } elseif {$code == 52} { # wcsr
    return "[opcode $code] [csr $r0], [reg $r1]"
  } elseif {$code == 56 || $code == 62 || $code == 43} { # bi, calli, raise
    return "[opcode $code] [imm26 $i26]"
  } elseif {$code == 44 || $code == 55} { # sextb, sexth
    return "[opcode $code] [reg $r2], [reg $r0]"
  } else {
    return "[opcode $code] [reg $r2], [reg $r1], [reg $r0]"
  }
}

################################ CMDS #################################

proc read_memory {addr len} {
  if {$addr == ""} {set addr 0}
  if {$len == ""} {set len 64}
  
  # Align read to 16-byte boundary
  set a_addr [expr {$addr & ~0xf}]
  set a_len [expr {($len + 15) & ~0xf}]
  
  set vals [jtag_read_memory $a_addr $a_len]
  
  for {set i 0} {$i < $a_len} {set i [expr {$i + 16}]} {
    puts -nonewline [format %08X: $a_addr]
    set a_addr [expr {$a_addr + 16}]
    for {set j 0} {$j < 4} {incr j} {
      set vals [lassign $vals b0 b1 b2 b3]
      puts -nonewline " $b0$b1$b2$b3"
    }
    puts ""
  }
  
  set nextcmd [list]
  lappend nextcmd "read"
  lappend nextcmd $a_addr
  lappend nextcmd $a_len
}

proc write_memory {addr val} {
  set data [list]
  lappend data [expr {($val >> 24) & 0xff}]
  lappend data [expr {($val >> 16) & 0xff}]
  lappend data [expr {($val >>  8) & 0xff}]
  lappend data [expr {($val >>  0) & 0xff}]
  jtag_write_memory $addr $data
}

proc dump_memory {addr len} {
  if {$addr == ""} {set addr 0}
  if {$len == ""} {set len 16}
      
  # Align read to 4-byte boundary
  set a_addr [expr {$addr & ~0x3}]
  set a_len [expr {$len * 4}]
  set a_end [expr {$a_addr + $a_len}]
  
  set vals [jtag_read_memory $a_addr $a_len]
  
  for {set a $a_addr} {$a < $a_end} {set a [expr {$a + 4}]} {
    set vals [lassign $vals b0 b1 b2 b3]
    puts "[format %08X $a]: [opfmt 0x$b0$b1$b2$b3]"
  }
  
  set nextcmd [list]
  lappend nextcmd "dump"
  lappend nextcmd [expr {$a_addr + $a_len}]
  lappend nextcmd [expr {$a_len / 4}]
}

proc send {data} {
  foreach j $data {
    jtag_uart_write $j
  }
}

proc transfer {prompt file offset len target} {
  set data [open $file]
  fconfigure $data -translation binary
  seek $data $offset
  
  set progress 0
  for {set done 0} {$done < $len} {set done [expr {$done+$did}]} {
    puts -nonewline "\r$prompt$done bytes"
    
    set rest [expr {$len - $done}]
    if {$rest > 100} { set do 100 } else { set do $rest }
    
    set bytes [read $data $do]
    set chunk [list]
    for {set i 0} {$i < $do} {incr i} {
      scan [string index $bytes $i] %c v
      lappend chunk $v
    }
    
    #puts "$chunk = [llength $chunk]"
    set did [llength $chunk]
    if {$did != $do} {
      puts "\n -- Short transfer error!"
      break
    }
    
    jtag_write_memory $target $chunk
    set target [expr {$target+$did}]
  }
  
  if {$done == $len} {
    puts "\r$prompt[format %d $len] bytes - complete"
  }
  
  close $data
}

proc load {file} {
  if {$file == ""} {
    puts "Specify file to load!"
    return
  }
  
  puts -nonewline "Capturing the CPU at address 0x0: "
  for {set i 0} {$i < 20} {incr i} {
    # bi +0 (CPU trap)
    write_memory 0x0 0xE0000000
    # flush instruction cache
    jtag_write_csr 0x3 0x0
    # Position CPU on this trap
    jtag_reset
  }
  # Wait a bit to be sure the CPU has the trap good and cached
  after 20
  puts "done"
  
  set sections [list]
  set sf [open "| readelf -S $file" "r"]
  while {[gets $sf line] >= 0} {
    if {[regexp {^\s+\[..\]\s+(\.\w+)\s+[^0-9a-f]*\s[0-9a-f]{4,}\s+([0-9a-f]{4,})\s+([0-9a-f]{4,})\s} $line nil section offset size] == 0} continue
    lappend sections "$section 0x$offset 0x$size"
  }
  close $sf
  
  # We can safely overwrite all of the instruction bus (even 0x0) now.
  # The trap is certainly cached and the CPU will not see the new values.
  set lf [open "| readelf -l $file" "r"]
  while {[gets $lf line] >= 0} {
    if {[regexp {^\s*LOAD\s+(\w+)\s+(\w+)\s+(\w+)\s+(\w+)\s+} $line nil offset vaddr paddr len] == 0} continue
    puts "Loading $offset+$len to $paddr"
    if {$paddr != $vaddr} {
      puts " Physical and virtual address mismatch! - Skipping"
      continue
    }
    foreach j $sections {
      lassign [split $j " "] section off size
      if {$offset <= $off && $off+$size <= $offset+$len} {
        transfer " section $section: " $file $off $size [expr {$paddr+$off-$offset}]
      } elseif {$offset <= $off && $off < $offset+$len} {
        puts " section $section: only half contained??"
      }
    }
  }
  close $lf
  
  # The CPU is spinning at address 0, so no need to reset it.
  # First flush the dcache and then release the CPU by flushing the icache
  puts -nonewline "Releasing CPU: "
  jtag_write_csr 0x4 0x0
  after 20
  jtag_write_csr 0x3 0x0
  puts done
}

################################ MAIN #################################


# List all available programming hardwares, and select the USBBlaster.
# (Note: this example assumes only one USBBlaster connected.)
puts "Programming Hardwares:"
foreach hardware_name [get_hardware_names] {
	puts $hardware_name
	if { [string match "USB_Blaster*" $hardware_name] } {
		set usbblaster_name $hardware_name
	}
}
puts "\nSelect JTAG chain connected to $usbblaster_name.\n";

# List all devices on the chain, and select the first device on the chain.
puts "\nDevices on the JTAG chain:"
foreach device_name [get_device_names -hardware_name $usbblaster_name] {
	puts $device_name
	if { [string match "@1*" $device_name] } {
		set test_device $device_name
	}
}
puts "\nSelect device: $test_device.\n";

# Open device 
open_device -hardware_name $usbblaster_name -device_name $test_device

device_lock -timeout 10000
device_virtual_ir_shift -instance_index 0 -ir_value 1 -no_captured_ir_value

jtag_sync
while {$cmd != "quit"} {
  puts -nonewline "\nlm32> "

  if {[gets stdin line] < 0} break
  
  if {$line eq ""} { set line $nextcmd }
  
  set parts [split $line " "]
  set args [lassign $parts cmd]
  set tail [lassign $args arg1 arg2 arg3 arg4]
  
  set nextcmd ""
  switch $cmd {
    "" { }
    "break" { jtag_break }
    "reset" { jtag_reset }
    "sync" { jtag_sync }
    "read" { set nextcmd [read_memory $arg1 $arg2] }
    "dump" { set nextcmd [dump_memory $arg1 $arg2] }
    "write" { write_memory $arg1 $arg2 }
    "csr" { jtag_write_csr $arg1 $arg2 }
    "recv" { jtag_uart_read }
    "send" { send $args }
    "load" { load $arg1 }
    "quit" { }
    default { puts "Unknown command" }
  }
}

# Close device
device_unlock
puts "Bye!"
close_device
