From a05f68d1c8b987220b8638398b0351d0cf92d92b Mon Sep 17 00:00:00 2001
From: Hector Martin <marcan@marcan.st>
Date: Thu, 9 Dec 2021 01:21:22 +0900
Subject: [PATCH] m1n1.gpiola: New module

Signed-off-by: Hector Martin <marcan@marcan.st>
---
 proxyclient/m1n1/gpiola.py | 201 +++++++++++++++++++++++++++++++++++++
 1 file changed, 201 insertions(+)
 create mode 100644 proxyclient/m1n1/gpiola.py

diff --git a/proxyclient/m1n1/gpiola.py b/proxyclient/m1n1/gpiola.py
new file mode 100644
index 00000000..b25d3fdf
--- /dev/null
+++ b/proxyclient/m1n1/gpiola.py
@@ -0,0 +1,201 @@
+# SPDX-License-Identifier: MIT
+import os, sys, struct, time
+
+from .utils import *
+from . import asm
+from .proxy import REGION_RX_EL1
+from .sysreg import *
+
+class GPIOLogicAnalyzer(object):
+    def __init__(self, u, node, pins, regs=[], div=1, cpu=1, on_pin_change=True):
+        self.u = u
+        self.p = u.proxy
+        self.iface = u.iface
+        self.cpu = cpu
+        self.base = u.adt[node].get_reg(0)[0]
+        self.node = node
+        self.pins = pins
+        self.regs = regs
+        assert len(pins) <= 32
+        assert div > 0
+        self.div = div
+        self.cbuf = self.u.malloc(0x1000)
+        self.dbuf = None
+        self.on_pin_change = on_pin_change
+        self.p.mmu_init_secondary(cpu)
+        self.tfreq = u.mrs(CNTFRQ_EL0)
+
+    def start(self, ticks, bufsize=0x10000):
+        self.bufsize = bufsize
+        if self.dbuf:
+            self.u.free(self.dbuf)
+        self.dbuf = self.u.malloc(bufsize)
+
+        text = f"""
+        trace:
+            add x3, x3, x2
+            mov x10, x2
+            mov x6, #-1
+            mov x7, #0
+            ldr x8, ={self.base}
+            mrs x4, CNTPCT_EL0
+            isb
+        1:
+            add x4, x4, x1
+        2:
+            mrs x5, CNTPCT_EL0
+            isb
+            cmp x5, x4
+            b.lo 2b
+        """
+        
+        for idx, pin in enumerate(self.pins.values()):
+            text += f"""
+            ldr w9, [x8, #{pin * 4}]
+            bfi x7, x9, #{idx}, #1
+            """
+
+        if self.on_pin_change:
+            text += f"""
+                cmp x7, x6
+                b.eq 3f
+                mov x6, x7
+            """
+        text += f"""
+            str w5, [x2], #4
+            str w7, [x2], #4
+        """
+
+        for reg in self.regs.values():
+            if isinstance(reg, tuple):
+                reg = reg[0]
+            text += f"""
+            ldr x9, ={reg}
+            ldr w9, [x9]
+            str w9, [x2], #4
+            """
+
+        text += f"""
+            cmp x2, x3
+            b.hs 2f
+        3:
+            sub x0, x0, #1
+            cbnz x0, 1b
+        2:
+            sub x0, x2, x10
+            ret
+        """
+        
+        code = asm.ARMAsm(text, self.cbuf)
+        self.iface.writemem(self.cbuf, code.data)
+        self.p.dc_cvau(self.cbuf, len(code.data))
+        self.p.ic_ivau(self.cbuf, len(code.data))
+
+        self.p.smp_call(self.cpu, code.trace | REGION_RX_EL1, ticks, self.div, self.dbuf, bufsize - (8 + 4 * len(self.regs)))
+    
+    def complete(self):
+        wrote = self.p.smp_wait(self.cpu)
+        assert wrote <= self.bufsize
+        data = self.iface.readmem(self.dbuf, wrote)
+        self.u.free(self.dbuf)
+        self.dbuf = None
+        
+        stride = 2 + len(self.regs)
+        
+        #chexdump(data)
+        
+        self.data = [struct.unpack("<" + "I" * stride,
+                                   data[i:i + 4 * stride])
+                     for i in range(0, len(data), 4 * stride)]
+
+    def vcd(self):
+        off = self.data[0][0]
+        if len(self.data) > 1:
+            off2 = max(0, ((self.data[1][0] - off) & 0xffffffff) - 5000)
+        else:
+            off2 = 0
+        
+        print(off, off2)
+        
+        vcd = []
+        vcd.append("""
+$timescale 1ns $end
+$scope module gpio $end
+""")
+        sym = 0
+        keys = []
+        rkeys = []
+                                           
+        for name in self.pins:
+            keys.append(f"s{sym}")
+            vcd.append(f"$var wire 1 s{sym} {name} $end\n")
+            sym += 1
+        for name, reg in self.regs.items():
+            vcd.append(f"$var reg 32 s{sym} {name} [31:0] $end\n")
+            if isinstance(reg, tuple):
+                subkeys = {}
+                rcls = reg[1]
+                rkeys.append((f"s{sym}", rcls, subkeys))
+                sym += 1
+                for fname in rcls().fields.keys():
+                    fdef = getattr(rcls, fname)
+                    if isinstance(fdef, tuple):
+                        width = fdef[0] - fdef[1] + 1
+                    else:
+                        width = 1
+                    vcd.append(f"$var reg {width} s{sym} {name}.{fname} [{width-1}:0] $end\n")
+                    subkeys[fname] = (width, f"s{sym}")
+                    sym += 1
+            else:
+                rkeys.append((f"s{sym}", None, None))
+                sym += 1
+        vcd.append("""
+$enddefinitions $end
+$dumpvars
+""")
+
+        for v in self.data:
+            ts = v[0]
+            val = v[1]
+            regs = v[2:]
+            ts = ((ts - off) & 0xffffffff) - off2
+            ns = max(0, 1000000000 * ts // self.tfreq)
+            vcd.append(f"#{ns}\n")
+            vcd.append("\n".join(f"{(val>>i) & 1}{k}" for i, k in enumerate(keys)) + "\n")
+            for (key, rcls, subkeys), v in zip(rkeys, regs):
+                vcd.append(f"b{v:032b} {key}\n")
+                if rcls:
+                    rval = rcls(v)
+                    for field, (width, key) in subkeys.items():
+                        v = getattr(rval, field)
+                        vcd.append(f"b{v:0{width}b} {key}\n")
+                    
+
+        ns += 10000
+        vcd.append(f"#{ns}\n" + "\n".join(f"{(val>>i) & 1}{k}" for i, k in enumerate(keys)) + "\n")
+ 
+        return "".join(vcd)
+    
+    def show(self):
+        with open("/tmp/dump.vcd", "w") as fd:
+            fd.write(self.vcd())
+        
+        gtkw = ("""
+[dumpfile] "/tmp/dump.vcd"
+[timestart] 0
+[size] 3063 1418
+[pos] -1 -1
+*-17.000000 2 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1
+[sst_width] 288
+[signals_width] 197
+[sst_expanded] 1
+[sst_vpaned_height] 421
+@23
+""" +
+        "\n".join("gpio." + k for k in self.pins) + "\n" + 
+        "\n".join("gpio." + k + "[31:0]" for k in self.regs) + "\n")
+
+        with open("/tmp/dump.gtkw", "w") as fd:
+            fd.write(gtkw)
+
+        os.system("gtkwave /tmp/dump.gtkw")