add m1n1HEADmaster

3 files changed, 517 insertions, 0 deletions

diff --git a/tools/proxyclient/m1n1/fw/afk/__init__.py b/tools/proxyclient/m1n1/fw/afk/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/tools/proxyclient/m1n1/fw/afk/__init__.py
diff --git a/tools/proxyclient/m1n1/fw/afk/epic.py b/tools/proxyclient/m1n1/fw/afk/epic.py
new file mode 100644
index 0000000..e95281d
--- /dev/null
+++ b/tools/proxyclient/m1n1/fw/afk/epic.py
@@ -0,0 +1,292 @@
+# SPDX-License-Identifier: MIT
+
+import struct
+from io import BytesIO
+from construct import *
+from ..common import *
+from ...utils import *
+from ..asc import StandardASC
+from ..asc.base import *
+from .rbep import AFKRingBufEndpoint
+
+EPICType = "EPICType" / Enum(Int32ul,
+    NOTIFY = 0,
+    COMMAND = 3,
+    REPLY = 4,
+    NOTIFY_ACK = 8,
+)
+
+EPICCategory = "EPICCategory" / Enum(Int8ul,
+    REPORT = 0x00,
+    NOTIFY = 0x10,
+    REPLY = 0x20,
+    COMMAND = 0x30,
+)
+
+EPICHeader = Struct(
+    "channel" / Int32ul,
+    "type" / EPICType,
+    "version" / Const(2, Int8ul),
+    "seq" / Int16ul,
+    "pad" / Const(0, Int8ul),
+    "unk" / Const(0, Int32ul),
+    "timestamp" / Default(Int64ul, 0),
+)
+
+EPICSubHeader = Struct(
+    "length" / Int32ul,
+    "version" / Default(Int8ul, 4),
+    "category" / EPICCategory,
+    "type" / Hex(Int16ul),
+    "timestamp" / Default(Int64ul, 0),
+    "seq" / Int16ul,
+    "unk" / Default(Hex(Int16ul), 0),
+    "inline_len" / Hex(Int32ul),
+)
+
+EPICAnnounce = Struct(
+    "name" / Padded(32, CString("utf8")),
+    "props" / Optional(OSSerialize())
+)
+
+EPICSetProp = Struct(
+    "name_len" / Int32ul,
+    "name" / Aligned(4, CString("utf8")),
+    "value" / OSSerialize()
+)
+
+EPICCmd = Struct(
+    "retcode" / Default(Hex(Int32ul), 0),
+    "rxbuf" / Hex(Int64ul),
+    "txbuf" / Hex(Int64ul),
+    "rxlen" / Hex(Int32ul),
+    "txlen" / Hex(Int32ul),
+    "rxcookie" / Optional(Default(Bool(Int8ul), False)),
+    "txcookie" / Optional(Default(Bool(Int8ul), False)),
+)
+
+
+class EPICError(Exception):
+    pass
+
+
+class EPICService:
+    RX_BUFSIZE = 0x4000
+    TX_BUFSIZE = 0x4000
+
+    def __init__(self, ep):
+        self.iface = ep.asc.iface
+        self.ep = ep
+        self.ready = False
+        self.chan = None
+        self.seq = 0
+    
+    def log(self, msg):
+        print(f"[{self.ep.name}.{self.SHORT}] {msg}")
+    
+    def init(self, props):
+        self.log(f"Init: {props}")
+        self.props = props
+        self.rxbuf, self.rxbuf_dva = self.ep.asc.ioalloc(self.RX_BUFSIZE)
+        self.txbuf, self.txbuf_dva = self.ep.asc.ioalloc(self.TX_BUFSIZE)
+        self.ready = True
+    
+    def wait(self):
+        while not self.ready:
+            self.ep.asc.work()
+
+    def handle_report(self, category, type, seq, fd):
+        self.log(f"Report {category}/{type} #{seq}")
+        chexdump(fd.read())
+
+    def handle_notify(self, category, type, seq, fd):
+        retcode = struct.unpack("<I", fd.read(4))[0]
+        self.log(f"Notify {category}/{type} #{seq} ({retcode})")
+        data = fd.read()
+        chexdump(data)
+        print("Send ACK")
+
+        data = data[:0x50] + b"\x01\x00\x00\x00" + data[0x54:]
+
+        pkt = struct.pack("<I", 0) + data
+        self.ep.send_epic(self.chan, EPICType.NOTIFY_ACK, EPICCategory.REPLY, type, seq, pkt, len(data))
+
+    def handle_reply(self, category, type, seq, fd):
+        off = fd.tell()
+        data = fd.read()
+        if len(data) == 4:
+            retcode = struct.unpack("<I", data)[0]
+            if retcode:
+                raise EPICError(f"IOP returned errcode {retcode:#x}")
+            else:
+                self.reply = retcode
+                return
+        fd.seek(off)
+        cmd = EPICCmd.parse_stream(fd)
+        payload = fd.read()
+        self.log(f"Response {type:#x} #{seq}: {cmd.retcode:#x}")
+        if cmd.retcode != 0:
+            raise EPICError(f"IOP returned errcode {cmd.retcode:#x}")
+        if payload:
+            self.log("Inline payload:")
+            chexdump(payload)
+        assert cmd.rxbuf == self.rxbuf_dva
+        self.reply = self.iface.readmem(self.rxbuf, cmd.rxlen)
+
+    def handle_cmd(self, category, type, seq, fd):
+        cmd = EPICCmd.parse_stream(fd)
+        self.log(f"Command {type:#x} #{seq}: {cmd.retcode:#x}")
+
+    def send_cmd(self, type, data, retlen=None):
+        if retlen is None:
+            retlen = len(data)
+        cmd = Container()
+        cmd.rxbuf = self.rxbuf_dva
+        cmd.txbuf = self.txbuf_dva
+        cmd.txlen = len(data)
+        cmd.rxlen = retlen
+        self.iface.writemem(self.txbuf, data)
+        self.reply = None
+        pkt = EPICCmd.build(cmd)
+        self.ep.send_epic(self.chan, EPICType.COMMAND, EPICCategory.COMMAND, type, self.seq, pkt)
+        self.seq += 1
+        while self.reply is None:
+            self.ep.asc.work()
+        return self.reply
+
+class EPICStandardService(EPICService):
+    def call(self, group, cmd, data=b'', replen=None):
+        msg = struct.pack("<2xHIII48x", group, cmd, len(data), 0x69706378) + data
+        if replen is not None:
+            replen += 64
+        resp = self.send_cmd(0xc0, msg, replen)
+        if not resp:
+            return
+        rgroup, rcmd, rlen, rmagic = struct.unpack("<2xHIII", resp[:16])
+        assert rmagic == 0x69706378
+        assert rgroup == group
+        assert rcmd == cmd
+        return resp[64:64+rlen]
+
+    def getLocation(self, unk=0):
+        return struct.unpack("<16xI12x", self.call(4, 4, bytes(32)))
+
+    def getUnit(self, unk=0):
+        return struct.unpack("<16xI12x", self.call(4, 5, bytes(32)))
+
+    def open(self, unk=0):
+        self.call(4, 6, struct.pack("<16xI12x", unk))
+
+    def close(self):
+        self.call(4, 7, bytes(16))
+
+class AFKSystemService(EPICService):
+    NAME = "system"
+    SHORT = "system"
+
+    def getProperty(self, prop, val):
+        pass
+        #self.send_cmd(0x40, msg, 0)
+
+    def setProperty(self, prop, val):
+        msg = {
+            "name_len": (len(prop) + 3) & ~3,
+            "name": prop,
+            "value": val,
+        }
+        msg = EPICSetProp.build(msg)
+        self.send_cmd(0x43, msg, 0)
+
+class EPICEndpoint(AFKRingBufEndpoint):
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.serv_map = {}
+        self.chan_map = {}
+        self.serv_names = {}
+        self.hseq = 0
+
+        for i in self.SERVICES:
+            self.serv_names[i.NAME] = i
+
+    def handle_ipc(self, data):
+        fd = BytesIO(data)
+        hdr = EPICHeader.parse_stream(fd)
+        sub = EPICSubHeader.parse_stream(fd)
+
+        if self.verbose > 2:
+            self.log(f"Ch {hdr.channel} Type {hdr.type} Ver {hdr.version} Seq {hdr.seq}")
+            self.log(f"  Len {sub.length} Ver {sub.version} Cat {sub.category} Type {sub.type:#x} Seq {sub.seq}")
+
+        if sub.category == EPICCategory.REPORT:
+            self.handle_report(hdr, sub, fd)
+        if sub.category == EPICCategory.NOTIFY:
+            self.handle_notify(hdr, sub, fd)
+        elif sub.category == EPICCategory.REPLY:
+            self.handle_reply(hdr, sub, fd)
+        elif sub.category == EPICCategory.COMMAND:
+            self.handle_cmd(hdr, sub, fd)
+
+    def wait_for(self, name):
+        while True:
+            srv = getattr(self, name, None)
+            if srv is not None and srv.ready:
+                break
+            self.asc.work()
+
+    def handle_report(self, hdr, sub, fd):
+        if sub.type == 0x30:
+            init = EPICAnnounce.parse_stream(fd)
+            if init.props is None:
+                init.props = {}
+            name = init.name
+            if "EPICName" in init.props:
+                name = init.props["EPICName"]
+            key = name + str(init.props.get("EPICUnit", ""))
+            if name in self.serv_names:
+                srv = self.serv_names[name](self)
+                short = srv.SHORT + str(init.props.get("EPICUnit", ""))
+                setattr(self, short, srv)
+                srv.init(init.props)
+                srv.chan = hdr.channel
+                self.chan_map[hdr.channel] = srv
+                self.serv_map[key] = srv
+                self.log(f"New service: {key} on channel {hdr.channel} (short name: {short})")
+            else:
+                self.log(f"Unknown service {key} on channel {hdr.channel}")
+        else:
+            if hdr.channel not in self.chan_map:
+                self.log(f"Ignoring report on channel {hdr.channel}")
+            else:
+                self.chan_map[hdr.channel].handle_report(sub.category, sub.type, sub.seq, fd)
+
+    def handle_notify(self, hdr, sub, fd):
+        self.chan_map[hdr.channel].handle_notify(sub.category, sub.type, sub.seq, fd)
+
+    def handle_reply(self, hdr, sub, fd):
+        self.chan_map[hdr.channel].handle_reply(sub.category, sub.type, sub.seq, fd)
+
+    def handle_cmd(self, hdr, sub, fd):
+        self.chan_map[hdr.channel].handle_cmd(sub.category, sub.type, sub.seq, fd)
+    
+    def send_epic(self, chan, ptype, category, type, seq, data, inline_len=0):
+        hdr = Container()
+        hdr.channel = chan
+        hdr.type = ptype
+        hdr.seq = self.hseq
+        self.hseq += 1
+
+        sub = Container()
+        sub.length = len(data)
+        sub.category = category
+        sub.type = type
+        sub.seq = seq
+        sub.inline_len = inline_len
+        pkt = EPICHeader.build(hdr) + EPICSubHeader.build(sub) + data
+        super().send_ipc(pkt)
+
+class AFKSystemEndpoint(EPICEndpoint):
+    SHORT = "system"
+
+    SERVICES = [
+        AFKSystemService,
+    ]
diff --git a/tools/proxyclient/m1n1/fw/afk/rbep.py b/tools/proxyclient/m1n1/fw/afk/rbep.py
new file mode 100644
index 0000000..872d75f
--- /dev/null
+++ b/tools/proxyclient/m1n1/fw/afk/rbep.py
@@ -0,0 +1,225 @@
+# SPDX-License-Identifier: MIT
+
+import struct
+
+from ..common import *
+from ...utils import *
+from ..asc.base import *
+
+
+class AFKEPMessage(Register64):
+    TYPE = 63, 48
+
+class AFKEP_GetBuf(AFKEPMessage):
+    TYPE = 63, 48, Constant(0x89)
+    SIZE = 31, 16
+    TAG = 15, 0
+
+class AFKEP_GetBuf_Ack(AFKEPMessage):
+    TYPE = 63, 48, Constant(0xa1)
+    DVA = 47, 0
+
+class AFKEP_InitRB(AFKEPMessage):
+    OFFSET = 47, 32
+    SIZE = 31, 16
+    TAG = 15, 0
+
+class AFKEP_Send(AFKEPMessage):
+    TYPE = 63, 48, Constant(0xa2)
+    WPTR = 31, 0
+
+class AFKEP_Recv(AFKEPMessage):
+    TYPE = 63, 48, Constant(0x85)
+    WPTR = 31, 0
+
+class AFKEP_Init(AFKEPMessage):
+    TYPE = 63, 48, Constant(0x80)
+
+class AFKEP_Init_Ack(AFKEPMessage):
+    TYPE = 63, 48, Constant(0xa0)
+
+class AFKEP_Start(AFKEPMessage):
+    TYPE = 63, 48, Constant(0xa3)
+
+class AFKEP_Start_Ack(AFKEPMessage):
+    TYPE = 63, 48, Constant(0x86)
+
+class AFKEP_Shutdown(AFKEPMessage):
+    TYPE = 63, 48, Constant(0xc0)
+
+class AFKEP_Shutdown_Ack(AFKEPMessage):
+    TYPE = 63, 48, Constant(0xc1)
+
+
+class AFKError(Exception):
+    pass
+
+class AFKRingBuf(Reloadable):
+    BLOCK_SIZE = 0x40
+
+    def __init__(self, ep, base, size):
+        self.ep = ep
+        self.base = base
+
+        bs, unk = struct.unpack("<II", self.read_buf(0, 8))
+        assert (bs + 3 * self.BLOCK_SIZE) == size
+        self.bufsize = bs
+        self.rptr = 0
+        self.wptr = 0
+
+    def read_buf(self, off, size):
+        return self.ep.iface.readmem(self.base + off, size)
+
+    def write_buf(self, off, data):
+        return self.ep.iface.writemem(self.base + off, data)
+    
+    def get_rptr(self):
+        return self.ep.asc.p.read32(self.base + self.BLOCK_SIZE)
+
+    def get_wptr(self):
+        return self.ep.asc.p.read32(self.base + 2 * self.BLOCK_SIZE)
+
+    def update_rptr(self, rptr):
+        self.ep.asc.p.write32(self.base + self.BLOCK_SIZE, self.rptr)
+
+    def update_wptr(self, rptr):
+        self.ep.asc.p.write32(self.base + 2 * self.BLOCK_SIZE, self.wptr)
+
+    def read(self):
+        self.wptr = self.get_wptr()
+
+        while self.wptr != self.rptr:
+            hdr = self.read_buf(3 * self.BLOCK_SIZE + self.rptr, 16)
+            self.rptr += 16
+            magic, size = struct.unpack("<4sI", hdr[:8])
+            assert magic in [b"IOP ", b"AOP "]
+            if size > (self.bufsize - self.rptr):
+                hdr = self.read_buf(3 * self.BLOCK_SIZE, 16)
+                self.rptr = 16
+                magic, size = struct.unpack("<4sI", hdr[:8])
+                assert magic in [b"IOP ", b"AOP "]
+
+            payload = self.read_buf(3 * self.BLOCK_SIZE + self.rptr, size)
+            self.rptr = (align_up(self.rptr + size, self.BLOCK_SIZE)) % self.bufsize
+            self.update_rptr(self.rptr)
+            yield hdr[8:] + payload
+            self.wptr = self.get_wptr()
+
+        self.update_rptr(self.rptr)
+
+    def write(self, data):
+        hdr2, data = data[:8], data[8:]
+        self.rptr = self.get_rptr()
+        
+        if self.wptr < self.rptr and self.wptr + 0x10 >= self.rptr:
+            raise AFKError("Ring buffer is full")
+
+        hdr = struct.pack("<4sI", b"IOP ", len(data)) + hdr2
+        self.write_buf(3 * self.BLOCK_SIZE + self.wptr, hdr)
+
+        if len(data) > (self.bufsize - self.wptr - 16):
+            if self.rptr < 0x10:
+                raise AFKError("Ring buffer is full")
+            self.write_buf(3 * self.BLOCK_SIZE, hdr)
+            self.wptr = 0
+
+        if self.wptr < self.rptr and self.wptr + 0x10 + len(data) >= self.rptr:
+            raise AFKError("Ring buffer is full")
+
+        self.write_buf(3 * self.BLOCK_SIZE + self.wptr + 0x10, data)
+        self.wptr = align_up(self.wptr + 0x10 + len(data), self.BLOCK_SIZE) % self.bufsize
+
+        self.update_wptr(self.wptr)
+        return self.wptr
+
+class AFKRingBufEndpoint(ASCBaseEndpoint):
+    BASE_MESSAGE = AFKEPMessage
+    SHORT = "afkep"
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.txq = None
+        self.rxq = None
+        self.iface = self.asc.iface
+        self.alive = False
+        self.started = False
+        self.iobuffer = None
+        self.verbose = 2
+        self.msgid = 0
+
+    def start(self):
+        self.send(AFKEP_Init())
+
+    @msg_handler(0xa0, AFKEP_Init_Ack)
+    def Init_Ack(self, msg):
+        self.alive = True
+        return True
+
+    @msg_handler(0x89, AFKEP_GetBuf)
+    def GetBuf(self, msg):
+        size = msg.SIZE * AFKRingBuf.BLOCK_SIZE
+
+        if self.iobuffer:
+            print("WARNING: trying to reset iobuffer!")
+
+        self.iobuffer, self.iobuffer_dva = self.asc.ioalloc(size)
+        self.asc.p.write32(self.iobuffer, 0xdeadbeef)
+        self.send(AFKEP_GetBuf_Ack(DVA=self.iobuffer_dva))
+        self.log(f"Buffer: phys={self.iobuffer:#x} dva={self.iobuffer_dva:#x} size={size:#x}")
+        return True
+
+    def stop(self):
+        self.log("Shutting down")
+        self.send(AFKEP_Shutdown())
+        while self.alive:
+            self.asc.work()
+
+    @msg_handler(0xc1, AFKEP_Shutdown_Ack)
+    def Shutdown_Ack(self, msg):
+        self.alive = False
+        self.log("Shutdown ACKed")
+        return True
+
+    @msg_handler(0x8a, AFKEP_InitRB)
+    def InitTX(self, msg):
+        self.txq = self.init_rb(msg)
+        if self.rxq and self.txq:
+            self.start_queues()
+        return True
+
+    @msg_handler(0x8b, AFKEP_InitRB)
+    def InitRX(self, msg):
+        self.rxq = self.init_rb(msg)
+        if self.rxq and self.txq:
+            self.start_queues()
+        return True
+
+    def init_rb(self, msg):
+        off = msg.OFFSET * AFKRingBuf.BLOCK_SIZE
+        size = msg.SIZE * AFKRingBuf.BLOCK_SIZE
+
+        return AFKRingBuf(self, self.iobuffer + off, size)
+
+    def start_queues(self):
+         self.send(AFKEP_Start())
+
+    @msg_handler(0x86, AFKEP_Start_Ack)
+    def Start_Ack(self, msg):
+        self.started = True
+        return True
+
+    @msg_handler(0x85, AFKEP_Recv)
+    def Recv(self, msg):
+        for data in self.rxq.read():
+            if self.verbose >= 3:
+                self.log(f"<RX rptr={self.rxq.rptr:#x}")
+                chexdump(data)
+            self.handle_ipc(data)
+        return True
+
+    def handle_ipc(self, data):
+        pass
+    
+    def send_ipc(self, data):
+        wptr = self.txq.write(data)
+        self.send(AFKEP_Send(WPTR = wptr))