4 files changed, 567 insertions, 0 deletions
diff --git a/sim/gdbstub.py b/sim/gdbstub.py
new file mode 100644
index 0000000..b262971
--- /dev/null
+++ b/sim/gdbstub.py
@@ -0,0 +1,134 @@
+import sys, socket
+
+start_halted = True
+
+def init():
+    global client
+
+    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    sock.bind(('127.0.0.1', 1234))
+    sock.listen()
+    print('Listening for gdb on', sock.getsockname(), file=sys.stderr)
+
+    client, peer = sock.accept()
+    sock.close()
+    print('Accepted connection from', peer, file=sys.stderr)
+
+buffer = b''
+haltFromStop = False
+
+def halt():
+    global buffer, haltFromStop
+
+    if haltFromStop:
+        reply(b'S05')
+        haltFromStop = False
+
+    while True:
+        data = client.recv(4096)
+        if not data:
+            break
+
+        buffer = buffer + data if buffer else data
+
+        try:
+            start = buffer.index(b'$')
+            marker = buffer.index(b'#', start + 1)
+        except ValueError:
+            continue
+
+        if marker + 2 >= len(buffer):
+            continue
+
+        data = buffer[start + 1:marker]
+        cksum = int(buffer[marker + 1:marker + 3], 16)
+
+        if cksum != (sum(data) & 0xff):
+            raise Exception(f'bad packet checksum: {buffer[start:marker + 3]}')
+
+        buffer = buffer[marker + 3:]
+
+        client.send(b'+')
+
+        if data[0] == b'?'[0]:
+            out = b'S05'
+        elif data[0] == b'c'[0]:
+            assert not data[1:] #TODO
+            break
+        elif data[0] == b'D'[0]:
+            out = b'OK'
+        elif data[0] == b'g'[0]:
+            out = hexout(read_reg(gdb_reg(r)) for r in range(16))
+        elif data[0] == b'm'[0]:
+            addr, length = (int(x, 16) for x in data[1:].split(b','))
+            out = hexout(read_mem(addr, length))
+        elif data[0] == b'M'[0]:
+            addrlen, data = data[1:].split(b':')
+            addr, length = (int(x, 16) for x in addrlen.split(b','))
+
+            data = bytes.fromhex(str(data, 'ascii'))
+            assert len(data) == length
+
+            write_mem(addr, data)
+            out = b'OK'
+        elif data[0] == b'p'[0]:
+            reg = gdb_reg(int(data[1:], 16))
+            out = hexout(read_reg(reg) if reg is not None else None)
+        else:
+            print('unhandled packet:', data)
+            out = b''
+
+        reply(out)
+
+    haltFromStop = True
+
+def reply(out):
+    client.send(b'$' + out + b'#' + hexout(sum(out) & 0xff, 1))
+
+def gdb_reg(n):
+    if 0 <= n < 8:
+        return (r0, r1, r2, r3, r4, r5, r6, r7)[n]
+
+    if n == 15:
+        return pc
+
+    if n == 0x19:
+        return cpsr
+
+    mode = read_reg(cpsr) & 0b11111
+    if 8 <= n < 13:
+        if mode == 0b10001:
+            regs = (r8_fiq, r9_fiq, r10_fiq, r11_fiq, r12_fiq)
+        else:
+            regs = (r8_fiq, r9_fiq, r10_fiq, r11_fiq, r12_fiq)
+
+        return regs[n - 8]
+
+    if 13 <= n < 15:
+        if mode == 0b10011:
+            regs = (r13_svc, r14_svc)
+        if mode == 0b10111:
+            regs = (r13_abt, r14_abt)
+        if mode == 0b11011:
+            regs = (r13_und, r14_und)
+        if mode == 0b10010:
+            regs = (r13_irq, r14_irq)
+        if mode == 0b10001:
+            regs = (r13_fiq, r14_fiq)
+        else:
+            regs = (r13_usr, r14_usr)
+
+        return regs[n - 13]
+
+    return None
+
+def hexout(data, size=4):
+    if data is None:
+        return b''
+    elif type(data) is bytes:
+        return data.hex().encode('ascii')
+    elif type(data) is int:
+        data = [data]
+
+    return b''.join(hex(d)[2:].zfill(2 * size)[:2 * size].encode('ascii') for d in data)
+
diff --git a/sim/link.ld b/sim/link.ld
new file mode 100644
index 0000000..d26cb2a
--- /dev/null
+++ b/sim/link.ld
@@ -0,0 +1,32 @@
+MEMORY
+{
+  HPS_SDRAM (rwx) : ORIGIN = 0x00000000, LENGTH = 512M
+}
+
+SECTIONS
+{
+  ._img :
+  {
+    KEEP(*(.interrupt_vector))
+    *(.text)
+    *(.text*)
+    *(.rodata)
+    *(.rodata*)
+    *(.data)
+    *(.data*)
+    *(.bss)
+    *(.bss*)
+    *(COMMON)
+    . = ALIGN(4);
+  } > HPS_SDRAM
+
+  _stack_size = 4096;
+  _stack_end = ORIGIN(HPS_SDRAM) + LENGTH(HPS_SDRAM);
+  _stack_begin = _stack_end - _stack_size;
+  
+  . = _stack_begin;
+  ._stack :
+  {
+    . = . + _stack_size;
+  } > HPS_SDRAM
+}
diff --git a/sim/sim.py b/sim/sim.py
new file mode 100755
index 0000000..16ceb54
--- /dev/null
+++ b/sim/sim.py
@@ -0,0 +1,367 @@
+#!/usr/bin/env python3
+
+import importlib.util, os, pathlib, random, socket, subprocess, sys
+
+module_path, verilated, image = sys.argv[1:]
+test_name = pathlib.Path(module_path).stem
+module = None
+
+seed = os.getenv('SIM_SEED', str(random.randint(0, 0x7fff_ffff)))
+
+all_regs = [
+    ('r0', 'r0'),
+    ('r1', 'r1'),
+    ('r2', 'r2'),
+    ('r3', 'r3'),
+    ('r4', 'r4'),
+    ('r5', 'r5'),
+    ('r6', 'r6'),
+    ('r7', 'r7'),
+    ('r8', 'r8_usr'),
+    ('r8_usr', 'r8_usr'),
+    ('r8_fiq', 'r8_fiq'),
+    ('r9', 'r9_usr'),
+    ('r9_usr', 'r9_usr'),
+    ('r9_fiq', 'r9_fiq'),
+    ('r10', 'r10_usr'),
+    ('r10_usr', 'r10_usr'),
+    ('r10_fiq', 'r10_fiq'),
+    ('r11', 'r11_usr'),
+    ('r11_usr', 'r11_usr'),
+    ('r11_fiq', 'r11_fiq'),
+    ('r12', 'r12_usr'),
+    ('r12_usr', 'r12_usr'),
+    ('r12_fiq', 'r12_fiq'),
+    ('sp', 'r13_usr'),
+    ('sp_usr', 'r13_usr'),
+    ('sp_svc', 'r13_svc'),
+    ('sp_abt', 'r13_abt'),
+    ('sp_und', 'r13_und'),
+    ('sp_irq', 'r13_irq'),
+    ('sp_fiq', 'r13_fiq'),
+    ('r13', 'r13_usr'),
+    ('r13_usr', 'r13_usr'),
+    ('r13_svc', 'r13_svc'),
+    ('r13_abt', 'r13_abt'),
+    ('r13_und', 'r13_und'),
+    ('r13_irq', 'r13_irq'),
+    ('r13_fiq', 'r13_fiq'),
+    ('lr', 'r14_usr'),
+    ('lr_usr', 'r14_usr'),
+    ('lr_svc', 'r14_svc'),
+    ('lr_abt', 'r14_abt'),
+    ('lr_und', 'r14_und'),
+    ('lr_irq', 'r14_irq'),
+    ('lr_fiq', 'r14_fiq'),
+    ('r14', 'r14_usr'),
+    ('r14_usr', 'r14_usr'),
+    ('r14_svc', 'r14_svc'),
+    ('r14_abt', 'r14_abt'),
+    ('r14_und', 'r14_und'),
+    ('r14_irq', 'r14_irq'),
+    ('r14_fiq', 'r14_fiq'),
+    ('pc', 'pc'),
+    ('r15', 'pc'),
+    ('cpsr', 'cpsr'),
+    ('spsr_svc', 'spsr_svc'),
+    ('spsr_abt', 'spsr_abt'),
+    ('spsr_und', 'spsr_und'),
+    ('spsr_irq', 'spsr_irq'),
+    ('spsr_fiq', 'spsr_fiq'),
+    ]
+
+regs = {}
+read_reg = lambda r: regs.setdefault(r, 0)
+
+dumped = []
+halted = False
+
+def recv_mem_dump():
+    dumped.clear()
+    for line in sim_end:
+        line = line.strip()
+        if line == '=== dump-mem ===' or not line:
+            continue
+        elif line == '=== end-mem ===':
+            break
+
+        base, data = line.split()
+        dumped.append((int(base, 16) << 2, bytes.fromhex(data)))
+
+def read_mem(base, length):
+    fragments = []
+    i = 0
+
+    if halted and length > 0:
+        print('dump-mem', base >> 2, (length + base - (base & ~0b11) + 0b11) >> 2, file=sim_end, flush=True)
+        recv_mem_dump()
+
+    while length > 0:
+        assert i < len(dumped), f'memory at 0x{base:08x} not dumped'
+        start, data = dumped[i]
+        delta = base - start
+
+        if delta < 0:
+            i = len(dumped)
+        elif delta < len(data):
+            taken = min(length, len(data) - delta)
+            fragments.append(data[delta:delta + taken])
+
+            base += taken
+            length -= taken
+        else:
+            i += 1
+
+    return b''.join(fragments)
+
+def write_mem(base, data):
+    assert halted
+ 
+    if not data:
+        return
+
+    prefix = read_mem(base & ~0b11, base & 0b11)
+    suffix = read_mem(base + len(data), (4 - ((base + len(data)) & 0b11)) & 0b11)
+    print('patch-mem ', base >> 2, ' ', prefix.hex(), data.hex(), suffix.hex(), sep='', file=sim_end, flush=True)
+
+    #TODO: Invalidate written addresses only
+    dumped.clear()
+
+def hexdump(base, memory):
+    lines = []
+    offset = 0
+
+    while offset < len(memory) > 0:
+        taken = min(16, len(memory) - offset)
+        line_bytes = memory[offset:offset + taken]
+
+        half = lambda rng: ' '.join(f'{line_bytes[i]:02x}' if i < taken else '  ' for i in rng)
+        left, right = half(range(0, 8)), half(range(8, 16))
+
+        ascii = ''.join(c if c.isascii() and c.isprintable() else '.' for c in map(chr, line_bytes))
+        lines.append(f' {base:08x}:  {left}  {right}  | {ascii}')
+
+        base += 16
+        offset += taken
+
+    return '\n'.join(lines)
+
+def module_get(attr, default=None):
+    return getattr(module, attr, default) if module else None
+
+COLOR_RESET  = '\033[0m'
+COLOR_RED    = '\033[31;1m'
+COLOR_GREEN  = '\033[32m'
+COLOR_YELLOW = '\033[33;1m'
+COLOR_BLUE   = '\033[34;1m'
+
+def exit(*, success):
+    global seed
+
+    if not success:
+        while_running()
+        if exec_args:
+            print('cmdline:', subprocess.list2cmdline(exec_args), file=sys.stderr)
+
+    status, color = ('passed', COLOR_GREEN) if success else (f'failed (seed: {seed})', COLOR_RED)
+    print( \
+        f'{color}Test \'{COLOR_YELLOW}{test_name}{COLOR_RESET}{color}\' ' +
+        f'{status}{COLOR_RESET}', file=sys.stderr)
+
+    sys.exit(0 if success else 1)
+
+def dump_regs():
+    order = {item[0]: i for i, item in enumerate(all_regs)}
+    next_col = 0
+
+    for reg, value in sorted(regs.items(), key=lambda item: order[item[0]]):
+        if next_col > 0:
+            print('   ', end='', file=sys.stderr)
+
+        print(f'{reg:<8} = 0x{value:08x}', end='', file=sys.stderr)
+        if next_col == 3:
+            print(file=sys.stderr)
+            next_col = 0
+        else:
+            next_col += 1
+
+    if next_col != 0:
+        print(file=sys.stderr)
+
+printed_while_running = False
+def while_running():
+    global printed_while_running
+
+    if not printed_while_running:
+        print(
+            f'{COLOR_BLUE}While running test \'{COLOR_YELLOW}{test_name}' + \
+            f'{COLOR_RESET}{COLOR_BLUE}\'{COLOR_RESET}')
+
+        printed_while_running = True
+
+def test_assert(condition, message):
+    if not condition:
+        while_running()
+        print(f'{COLOR_RED}{message()}{COLOR_RESET}', file=sys.stderr)
+
+        if regs:
+            dump_regs()
+
+        exit(success=False)
+
+def unsigned(n):
+    assert -0x8000_0000 <= n <= 0xffff_ffff
+    return n + 0x1_0000_0000 if n < 0 else n
+
+def split_dword(n):
+    assert -0x8000_0000_0000_0000 <= n <= 0xffff_ffff_ffff_ffff
+    if n < 0:
+        n += 0x1_0000_0000_0000_0000
+
+    return (n >> 32, n & 0xffff_ffff)
+
+def int_bytes(n):
+    return n.to_bytes(4, 'little', signed=n < 0) if type(n) is int else n
+
+def assert_reg(r, expected):
+    actual = read_reg(r)
+    expected = unsigned(expected)
+
+    test_assert( \
+        actual == expected, \
+        lambda: f'Register {r} = 0x{actual:08x}, expected 0x{expected:08x}')
+
+def assert_mem(base, value):
+    if type(value) is list:
+        value = b''.join(int_bytes(w) for w in value)
+    else:
+        value = int_bytes(value)
+
+    actual = read_mem(base, len(value))
+    test_assert( \
+        actual == value, \
+        lambda: \
+        f'Memory at 0x{base:08x} holds:\n{hexdump(base, actual)}\n' + \
+        f'But this was expected instead:\n{hexdump(base, value)}')
+
+init_regs = {}
+
+def init_reg(r, value):
+    global init_regs 
+    assert init_regs is not None
+    init_regs[r] = unsigned(value)
+
+if test_name in os.getenv('SIM_SKIP', '').split(','):
+    print( \
+        f'{COLOR_BLUE}Test \'{COLOR_YELLOW}{test_name}{COLOR_RESET}' +
+        f'{COLOR_BLUE}\' skipped{COLOR_RESET}', file=sys.stderr)
+
+    exit(success=True)
+
+spec = importlib.util.spec_from_file_location('sim', module_path)
+module = importlib.util.module_from_spec(spec)
+
+prelude = {
+    'read_reg':    read_reg,
+    'read_mem':    read_mem,
+    'write_mem':   write_mem,
+    'assert_reg':  assert_reg,
+    'assert_mem':  assert_mem,
+    'init_reg':    init_reg,
+    'split_dword': split_dword,
+    }
+
+prelude.update({k: v for k, v in all_regs})
+module.__dict__.update(prelude)
+spec.loader.exec_module(module)
+
+cycles = module_get('cycles', 1024)
+mem_dumps = module_get('mem_dumps', [])
+
+if init := module_get('init'):
+    init()
+
+exec_args = [verilated, '--headless', '--no-tty', '--cycles', str(cycles), '--dump-regs']
+
+for rng in mem_dumps:
+    length = rng.stop - rng.start
+    assert rng.start >= 0 and rng.stop > rng.start \
+       and rng.step == 1 and ((rng.start | length) & 3) == 0
+
+    exec_args.extend(['--dump-mem', f'{rng.start >> 2},{length >> 2}'])
+
+for r, value in init_regs.items():
+    exec_args.extend(['--init-reg', f'{r}={value}'])
+
+for addr, const in module_get('consts', {}).items():
+    exec_args.extend(['--const', f'{addr},{const}'])
+
+for addr, filename in module_get('loads', {}).items():
+    exec_args.extend(['--load', f'{addr},{filename}'])
+
+if module_get('start_halted', False):
+    exec_args.append('--start-halted')
+
+sim_end, target_end = socket.socketpair()
+sim_end = sim_end.makefile('rw')
+target_fd = target_end.fileno()
+
+exec_args.extend(['--control-fd', str(target_fd)])
+
+init_regs = None
+exec_args.append(image)
+
+exec_args.append(f'+verilator+seed+{seed}')
+if not os.getenv('SIM_PULLX', 0):
+    exec_args.append('+verilator+rand+reset+2')
+
+process = subprocess.Popen(exec_args, pass_fds=(target_fd,))
+target_end.close()
+
+in_regs = False
+halt = module_get('halt')
+
+while True:
+    for line in sim_end:
+        line = line.strip()
+        if line == '=== halted ===':
+            break
+        if line == '=== dump-regs ===':
+            in_regs = True
+        elif line == '=== end-regs ===':
+            in_regs = False
+        elif line == '=== dump-mem ===':
+            recv_mem_dump()
+        elif not line:
+            continue
+        elif in_regs:
+            value, reg = line.split()
+            regs[reg] = int(value, 16)
+        else:
+            while_running()
+            print(f'{COLOR_BLUE}{line}{COLOR_RESET}')
+    else:
+        break
+
+    halted = True
+    if halt:
+        halt()
+
+    print('continue', file=sim_end, flush=True)
+    if not halt:
+        break
+
+process.wait(timeout=1)
+if process.returncode != 0:
+    exit(success=False)
+
+if final := module_get('final'):
+    final()
+
+if os.getenv('SIM_DUMP', ''):
+    dump_regs()
+    for rng in mem_dumps:
+        print(f'Memory range 0x{rng.start:08x}..0x{rng.stop:08x}')
+        print(hexdump(rng.start, read_mem(rng.start, rng.stop - rng.start)))
+
+exit(success=True)
diff --git a/sim/start.S b/sim/start.S
new file mode 100644
index 0000000..7639513
--- /dev/null
+++ b/sim/start.S
@@ -0,0 +1,34 @@
+.section .interrupt_vector
+
+__reset:
+	b _start
+
+__undefined:
+	b undefined
+
+__swi:
+	b __data_abort
+
+__prefetch_abort:
+	b __prefetch_abort
+
+__data_abort:
+	b __data_abort
+
+__irq:
+	b __irq
+
+__fiq:
+	b __fiq
+
+.text
+
+.global _start
+_start:
+	ldr sp, =_stack_end
+	bl  reset
+	b   .
+
+.weak undefined
+undefined:
+	b undefined