From bd6ec63be4dd7a03f5e0e76331d0a60fd74adb5b Mon Sep 17 00:00:00 2001
From: Florent Kermarrec <florent@enjoy-digital.fr>
Date: Fri, 6 Sep 2019 11:57:18 +0200
Subject: [PATCH] build/openocd: add stream method for JTAG UART

---
 litex/build/openocd.py | 108 ++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 107 insertions(+), 1 deletion(-)

diff --git a/litex/build/openocd.py b/litex/build/openocd.py
index 8f1cfb06..04b5a7f0 100644
--- a/litex/build/openocd.py
+++ b/litex/build/openocd.py
@@ -1,9 +1,11 @@
-# This file is Copyright (c) 2015 Robert Jordens <jordens@gmail.com>
+# This file is Copyright (c) 2015-2017 Robert Jordens <jordens@gmail.com>
 # This file is Copyright (c) 2015 Sebastien Bourdeauducq <sb@m-labs.hk>
+# This file is Copyright (c) 2019 Florent Kermarrec <florent@enjoy-digital.fr>
 # License: BSD
 
 import subprocess
 
+from litex.build.tools import write_to_file
 from litex.build.generic_programmer import GenericProgrammer
 
 
@@ -32,3 +34,107 @@ class OpenOCD(GenericProgrammer):
             "exit"
         ])
         subprocess.call(["openocd", "-f", self.config, "-c", script])
+
+
+    def stream(self, port=20000):
+        """
+        Create a Telnet server to stream data to/from the internal JTAG TAP of the FPGA
+
+        Wire format: 10 bits LSB first
+        Host to Target:
+          - TX ready : bit 0
+          - RX data: : bit 1 to 8
+          - RX valid : bit 9
+
+        Target to Host:
+          - RX ready : bit 0
+          - TX data  : bit 1 to 8
+          - TX valid : bit 9
+        """
+        cfg = """
+proc jtagstream_poll {tap tx n} {
+    set m [string length $tx]
+    set n [expr ($m>$n)?$m:$n]
+    set txi [lrepeat $n {10 0x001}]
+    set i 0
+    foreach txj [split $tx ""] {
+        lset txi $i 1 [format 0x%4.4X [expr 0x201 | ([scan $txj %c] << 1)]]
+        incr i
+    }
+    set txi [concat {*}$txi]
+    set rxi [split [drscan $tap {*}$txi -endstate DRPAUSE] " "]
+    #echo $txi:$rxi
+    set rx ""
+    set writable 1
+    foreach {rxj} $rxi {
+        set readable [expr 0x$rxj & 0x200]
+        set writable [expr 0x$rxj & $writable]
+        if {$readable} {
+            append rx [format %c [expr (0x$rxj >> 1) & 0xff]]
+        }
+    }
+    return [list $rx $readable $writable]
+}
+
+proc jtagstream_drain {tap tx chunk_rx max_rx} {
+    lassign [jtagstream_poll $tap $tx $chunk_rx] rx readable writable
+    while {[expr $writable && ($readable > 0) && ([string length $rx] < $max_rx)]} {
+        lassign [jtagstream_poll $tap "" $chunk_rx] rxi readable writable
+        append rx $rxi
+    }
+    if {!$writable} {
+        echo "write overflow"
+    }
+    return $rx
+}
+
+proc jtagstream_rxtx {tap client is_poll} {
+    if {![$client eof]} {
+        if {!$is_poll} {
+            set tx [$client gets]
+        } else {
+            set tx ""
+        }
+        set rx [jtagstream_drain $tap $tx 64 4096]
+        if {[string length $rx]} {
+            $client puts -nonewline $rx
+        }
+        if {$is_poll} {
+            after 1 [list jtagstream_rxtx $tap $client 1]
+        }
+    } else {
+        $client readable {}
+        $client onexception {}
+        $client close
+    }
+}
+
+proc jtagstream_client {tap sock} {
+    set client [$sock accept]
+    fconfigure $client -buffering none
+    $client readable [list jtagstream_rxtx $tap $client 0]
+    $client onexception [list $client close]
+    after 1 [list jtagstream_rxtx $tap $client 1]
+}
+
+proc jtagstream_exit {sock} {
+    stdin readable {}
+    $sock readable {}
+}
+
+proc jtagstream_serve {tap port} {
+    set sock [socket stream.server $port]
+    $sock readable [list jtagstream_client $tap $sock]
+    stdin readable [list jtagstream_exit $sock]
+    vwait forever
+    $sock close
+}
+"""
+        write_to_file("stream.cfg", cfg)
+        script = "; ".join([
+            "init",
+            "irscan $_CHIPNAME.tap $_USER1",
+            "jtagstream_serve $_CHIPNAME.tap {:d}".format(port),
+            "exit",
+        ])
+        subprocess.call(["openocd", "-f", self.config, "-f", "stream.cfg", "-c", script])
-- 
2.30.2