diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b150859 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +*.pyc +.env +.3nv +*egg-info* diff --git a/src/escapelib/game.py b/src/escapelib/game.py index c8872ed..ca3903d 100644 --- a/src/escapelib/game.py +++ b/src/escapelib/game.py @@ -25,7 +25,7 @@ class Game(six.with_metaclass(IOMeta)): # Configure Serial if self.__bus_port__ is not None: self.__services__.append( - SerialService(self.__bus_port__, self.__bus_baud__, + SerialService(self, self.__bus_port__, self.__bus_baud__, self.__bus_power_enable__, self.__bus_tx_enable__)) diff --git a/src/escapelib/hdlc/__init__.py b/src/escapelib/hdlc/__init__.py new file mode 100644 index 0000000..6014917 --- /dev/null +++ b/src/escapelib/hdlc/__init__.py @@ -0,0 +1 @@ +from .protocol import HDLCProtocol diff --git a/src/escapelib/hdlc/command.py b/src/escapelib/hdlc/command.py new file mode 100644 index 0000000..e27385d --- /dev/null +++ b/src/escapelib/hdlc/command.py @@ -0,0 +1,36 @@ +class HDLCCommand(object): + """Contains up to seven HDLCFrames.""" + def __init__(self, bus_id, data): + self._frames = [] + self._last_ack_idx = 0 + self._peer_state = None + self._response = [] + self.deferred = None + num_frames = ceil_div(len(data), FRAME_DATA_CAPACITY) + if num_frames is 0: + num_frames = 1 + for i in range(num_frames): + start = i*FRAME_DATA_CAPACITY + end = start+FRAME_DATA_CAPACITY + self._frames.append( + HDLCFrame.iframe(bus_id, data[start:end], i, poll)) + + def rx_frame(self, frame): + """Adjust peer-recv value, accept data.""" + num_ackd_frames = (frame.r_seq - self._peer_state.recv) % 8 + self._last_ack_idx += num_ackd_frames + self._peer_state.recv = frame.r_seq + if frame.payload is not None: + self._response.append(frame.payload) + else: + self._response.append(frame.is_rr()) + + def do_callback(self): + self.deferred.callback(self._response) + + def register_peerstate(self, peer_state): + """Link this command to remote state on peer to track s_seq, r_seq.""" + self._peer_state = peer_state + + def unacked_frames(self, frame): + return self._frames[self._last_ack_idx:] diff --git a/src/escapelib/hdlc/exceptions.py b/src/escapelib/hdlc/exceptions.py new file mode 100644 index 0000000..d272656 --- /dev/null +++ b/src/escapelib/hdlc/exceptions.py @@ -0,0 +1,5 @@ +class HDLCError(Exception): + pass + +class HDLCInvalidFrame(HDLCError): + pass diff --git a/src/escapelib/hdlc/exchange.py b/src/escapelib/hdlc/exchange.py new file mode 100644 index 0000000..50e3518 --- /dev/null +++ b/src/escapelib/hdlc/exchange.py @@ -0,0 +1,42 @@ +from escapelib.util import ceil_div + +class HDLCExchange(object): + """Contains one or more HDLCTransactions.""" + def __init__(self, bus_id, data): + self.protocol = None + self.deferred = None + self._rseq = 0 # Last rx'd packet from remote + self._transactions = [] + num_transactions = ceil_div(len(data), + TRANSACTION_DATA_CAPACITY) + if num_transactions is 0: + num_transactions = 1 + for i in range(num_transactions): + start = i*TRANSACTION_DATA_CAPACITY + end = start+TRANSACTION_DATA_CAPACITY + self._transactions.append( + HDLCTransaction(bus_id, data[start:end])) + + def do_frame(self, data): + frame = HDLCFrame.parse(data) + self._rseq = frame.r_seq + if (self._rseq >= len(self._transactions[self._cur_trans]) or + self._rseq == 0): # _rseq is 0 when max packets in-flight + # are ack'd + self._cur_trans += 1 + if self._cur_trans >= len(self._trasactions): + self.deferred.callback(self. + if frame.final: + self.protocol._lock.release() + + + def __len__(self): + return len(self._transactions) + + def __iter__(self): + self._cur_trans = 0 + self._rseq = 0 + return self._transactions.__iter__(self) + + def __next__(self): + return self._transactions[self._cur_trans][self._next_frame:] diff --git a/src/escapelib/hdlc/frame.py b/src/escapelib/hdlc/frame.py new file mode 100644 index 0000000..62858fd --- /dev/null +++ b/src/escapelib/hdlc/frame.py @@ -0,0 +1,177 @@ +from binascii import hexlify + +from .util import hdlc_escape, hdlc_crc + +CLIENT_MAX_BUFFER = 128 +HDLC_FRAME_OVERHEAD = 6 # Flag, Addr, Ctrl, FCS1, FCS2, Flag +FRAME_DATA_CAPACITY = CLIENT_MAX_BUFFER - HDLC_FRAME_OVERHEAD +TRANSACTION_DATA_CAPACITY = FRAME_DATA_CAPACITY * 7 +FLAG = b'\x7e' + +S_FRAME_TYPE_MASK = (0x3 << 2) +S_FRAME_TYPE_RR = 0 +S_FRAME_TYPE_REJ = 1 +S_FRAME_TYPE_RNR = 2 +S_FRAME_TYPE_SREJ = 3 + +POLL = FINAL = 1 << 4 + +S_SEQ_MASK = (0x7 << 1) +S_SEQ_OFFSET = 1 +R_SEQ_MASK = (0x7 << 5) +R_SEQ_OFFSET = 5 + +S_FRAME_MASK = 1 +U_FRAME_MASK = 3 + +class HDLCFrame(object): + + def __init__(self, frame_data=None, has_fcs=False): + if frame_data is None: + self._data = bytearray(2) + else: + self._data = bytearray(frame_data).strip(FLAG) + if has_fcs: + self._data = self._data[:-2] + + @classmethod + def iframe(cls, address, payload, s_seq=0, poll=True): + frame = cls() + frame.is_iframe(True) + frame.address = address + frame.payload = payload + frame.s_seq = s_seq + frame.poll = poll + return frame + + @classmethod + def sframe(cls, address, stype=S_FRAME_TYPE_RR, r_seq=0, poll=True): + frame = cls() + frame.is_sframe(True) + frame.address = address + frame.r_seq = r_seq + frame.poll = poll + frame.s_frame_type = stype + return frame + + @classmethod + def uframe(cls, *args, **kwargs): + raise NotImplementedError + + @classmethod + def parse(cls, frame_data): + frame_data = hdlc_unescape(frame_data) + if hdlc_check_fcs(frame_data): + return HDLCFrame(frame_data, has_fcs=True) + raise HDLCInvalidFrame("FCS Check Failed") + + @property + def address(self): + return self._data[0] + + @address.setter + def address(self, val): + self._data[0] = val & 0xff + + @property + def control(self): + return self._data[1] + + @control.setter + def control(self, val): + self._data[1] = val & 0xff + + @property + def r_seq(self): + if self.is_uframe(): + raise HDLCError("U-Frames have no N(R)") + return self.control & R_SEQ_MASK + + @r_seq.setter + def r_seq(self, val): + self.control &= ~R_SEQ_MASK + self.control |= (val & 0x7) << R_SEQ_OFFSET + + @property + def final(self): + return self.control & FINAL + + @final.setter + def final(self, val): + if val: + self.control |= FINAL + else: + self.control &= ~FINAL + + poll = final + + @property + def s_seq(self): + if not self.is_iframe(): + raise HDLCError("Only I-frames have N(S)") + self.control & S_SEQ_MASK + + @s_seq.setter + def s_seq(self, val): + self.control &= ~S_SEQ_MASK + self.control |= (val & 0x7) << S_SEQ_OFFSET + + def is_iframe(self, force=False): + if force: + self.control &= ~U_FRAME_MASK + return not self.control & S_FRAME_MASK + + def is_sframe(self, force=False): + if force: + self.control &= ~U_FRAME_MASK + self.control |= S_FRAME_MASK + return (self.control & U_FRAME_MASK == S_FRAME_MASK) + + def is_uframe(self): + return self.control & U_FRAME_MASK == U_FRAME_MASK + + def is_rr(self): + return self.is_sframe and self.sframe_type == S_FRAME_TYPE_RR + + def is_rnr(self): + return self.is_sframe and self.sframe_type == S_FRAME_TYPE_RNR + + @property + def sframe_type(self): + if not self.is_sframe(): + raise HDLCError("Non-sframe has no sframe type") + return self.control & S_FRAME_TYPE_MASK + + @sframe_type.setter + def sframe_type(self, val): + self.control &= ~S_FRAME_TYPE_MASK + self.control |= val & 0x3 + + @property + def payload(self): + if self.is_sframe(): + raise HDLCError("S-frame has no payload") + return self._data[2:] + + @payload.setter + def payload(self, val): + if self.is_sframe(): + raise HDLCError("S-frame has no payload") + new = bytearray(self._data[:2]) + new.extend(val) + self._data = new + + @property + def fcs(self): + return hdlc_crc(self._data) + + def raw(self): + raw_frame = bytearray() + raw_frame.append(FLAG) + raw_frame.extend(hdlc_escape(self._data)) + raw_frame.extend(hdlc_escape(self.fcs)) + raw_frame.append(FLAG) + return raw_frame + + def __repr__(self): + return hexlify(self.raw()) diff --git a/src/escapelib/hdlc/peer.py b/src/escapelib/hdlc/peer.py new file mode 100644 index 0000000..10eebb1 --- /dev/null +++ b/src/escapelib/hdlc/peer.py @@ -0,0 +1,7 @@ +import attr + +@attr.s +class HDLCPeerState(object): + send = attr.ib(default=0) + recv = attr.ib(default=0) + pending = attr.ib(default=attr.Factory(list)) diff --git a/src/escapelib/hdlc/protocol.py b/src/escapelib/hdlc/protocol.py new file mode 100644 index 0000000..de34551 --- /dev/null +++ b/src/escapelib/hdlc/protocol.py @@ -0,0 +1,64 @@ +from twisted.internet import defer, protocol +from twisted.internet.serialport import SerialPort + +from .command import HDLCCommand +from .exchange import HDLCExchange +from .frame import FLAG + +COMMAND_QUEUE = defer.DeferredQueue() + +def hdlc_submit_command(bus_id, packet=None): + command = HDLCCommand(bus_id, packet=packet) + command.deferred = defer.deferred() + COMMAND_QUEUE.put(ex) + return command.deferred + +class HDLCProtocol(protocol.Protocol): + + def __init__(self, bus_port, **kwargs): + self._serial = SerialPort(bus_port, **kwargs) + self._token_lock = defer.DeferredLock() + self._command_lock = defer.DeferredLock() + self._rx_buf = bytearray() + self._rx_frames = [] + self._peer_state = {} + + def dataReceived(self, data): + print("RX:", hexlify(data), data) + if not FLAG in data: + self._rx_buf.extend(data) + else: + for byte in data: + if byte == b'\x7e': + if len(self._buf) > 0: + self.service_rx_frame(self._buf) + self._buf = bytearray() + else: + self._buf.append(byte) + + def service_rx_frame(self, data): + frame = HDLCFrame.parse(data) + if frame is None: + return + self._cmd.rx_frame(frame) + if frame.final: + self._token_lock.release() + + @defer.inlineCallbacks + def service_command(bus_id, data): + while True: + self._cmd = yield COMMAND_QUEUE.get() + peer_state = self._peer_state[cmd.bus_id] + self._cmd.register_peerstate(peer_state) + while True: + yield self._token_lock.acquire() + frames = self._cmd.unacked_frames()[:7] + if not frames: + cmd.do_callback() + break + frames[-1].poll = True # Give up the token on last frame + for frame in frames: + frame.s_seq = peer_state.send + frame.r_seq = peer_state.recv + self.sendSomeData(frame) + peer_state.send += 1 diff --git a/src/escapelib/hdlc/util.py b/src/escapelib/hdlc/util.py new file mode 100644 index 0000000..b890c91 --- /dev/null +++ b/src/escapelib/hdlc/util.py @@ -0,0 +1,34 @@ +import crcmod + +def hdlc_check_fcs(frame): + frame = frame.strip(FLAG) + l_crc = hdlc_crc(frame[:-2]) + r_crc = frame[-2:] + return l_crc == r_crc + +def hdlc_crc(val): + crc16 = crcmod.predefined.Crc('xmodem') + crc16.update(val) + return crc16.digest() + +def hdlc_escape(data): # type: (bytearray) -> bytearray + r = bytearray() + for i in data: + if i in [0x7e, 0x7d]: + r.append(0x7d) + r.append(i & ~(1 << 5)) + else: + r.append(i) + return r + +def hdlc_unescape(data): # type: (bytearray) -> bytearray + r = bytearray() + i = 0 + while i < len(data): + if data[i] == 0x7d: + r.append(data[i+1] | (1 << 5)) + i += 2 + else: + r.append(data[i]) + i += 1 + return r diff --git a/src/escapelib/io.py b/src/escapelib/io.py index c20a08b..e96692b 100644 --- a/src/escapelib/io.py +++ b/src/escapelib/io.py @@ -39,6 +39,7 @@ class IOptions(object): def __init__(self): self.inputs = [] self.outputs = [] + self.props = [] def add_input(self, i): self.inputs.append(i) @@ -46,6 +47,15 @@ class IOptions(object): def add_output(self, o): self.outputs.append(o) + def add_prop(self, p): + self.props.append(p) + + def find_prop(self, b_id): + for p in self.props: + if p.bus_id == b_id: + return p + return None + def find_output(self, o_str): for o in self.outputs: if o.name == o_str: @@ -69,6 +79,8 @@ class IOMeta(type): cls._io.add_output(attr) elif isinstance(attr, Input): cls._io.add_input(attr) + elif isinstance(attr, Prop): + cls._io.add_prop(attr) class IO(object): @@ -101,3 +113,6 @@ class Output(IO): class Sensor(IO): pass + +class Prop(object): + pass diff --git a/src/escapelib/nbus.py b/src/escapelib/nbus.py index 4155106..754a7d3 100644 --- a/src/escapelib/nbus.py +++ b/src/escapelib/nbus.py @@ -1,5 +1,6 @@ """Serial Bus fuctionality for Trespassed.""" +from binascii import hexlify from enum import IntEnum from functools import wraps from time import time @@ -8,7 +9,9 @@ import attr import crcmod.predefined import six -from escapelib.io import Input, Output, Sensor, IOMeta +from escapelib import io +#from escapelib.hdlc import hdlc_query_frame, HDLCFrame +from escapelib.packet import process_packet from escapelib.status import set_status from escapelib.util import delay @@ -21,17 +24,100 @@ from twisted.python import log WATCHDOG = {} WATCHDOG_TIMEOUT = 30 -__all__ = ('SerialService', 'on_input', 'on_puzzle', 'Prop', 'Input', 'Output') +__all__ = ('SerialService', 'Prop', 'Input', 'Output') -class PacketType(IntEnum): - Keepalive = 0 - Input = 1 - Output = 2 - Solved = 3 - Failed = 4 - Log = 5 -class Prop(six.with_metaclass(IOMeta)): +class NBusProtocol(protocol.Protocol): + _buf = b'' # type: bytes + _escape = False + + def __init__(self, service): + self._service = service + + def dataReceived(self, data): + print("RX:", hexlify(data), data) + for byte in data: + if self._escape: + escaped_byte = ord(byte) & ~(1 << 5) + if escaped_byte not in b'\x7d\x7e': + log.msg( + 'Unnecessary escaped value: {}'.format( + escaped_byte)) + self._buf += escaped_byte + self._escape = False + elif byte == b'\x7d': + self._escape = True + elif byte == b'\x7e': + if len(self._buf) is 0: + return + frame = HDLCFrame.parse(self._buf) + if frame: + print("Packet: ", hexlify(frame.payload)) + self._clear_buf() + self._service.mutex.release() + else: + self._buf += byte + + def _clear_buf(self): + self._buf = b'' + + +NBUS_QUEUE = defer.DeferredQueue() + +class SerialService(service.Service): + """Service wrapper for twisted.internet.serialport.""" + + _serial = None # type: SerialPort + _port = None # type: str + bypass = False # type: bool + mutex = defer.DeferredLock() + + def __init__(self, game, port='/dev/ttyS0', baudrate=1000000, + power_gpio=None, tx_enable_gpio=None): + # type: (str, int) -> None + """Instanatiate SerialPort handling service.""" + self._port = port + self._baud = baudrate + self._game = game + + def startService(self): + """Start the NBus serial handling service.""" + try: + self._serial = SerialPort( + NBusProtocol(self), self._port, reactor, baudrate=self._baud) + reactor.callLater(0, self.do_bus_write) + reactor.callLater(0, self.queue_poll) + except IOError as e: + log.msg( + "Failed to initialize serial. NBUS disabled: {}".format(e)) + + def stopService(self): + """Stop the NBus serial handling service.""" + self._serial.loseConnection() + + @defer.inlineCallbacks + def do_bus_write(self): + while True: + frame = yield NBUS_QUEUE.get() + d = self.mutex.acquire() + def _release_mutex_on_timeout(garbage, timeout): + print("Timed out after {} seconds".format(timeout)) + self.mutex.release() + d.addTimeout(1, reactor, onTimeoutCancel=_release_mutex_on_timeout) + yield d + print("Sending: ", hexlify(frame)) + self._serial.writeSomeData(frame) + + def queue_poll(self): + """Add a query frame for every registered prop to the queue.""" + for prop in self._game._io.props: + pass + #print("Added ", hdlc_query_frame(prop.bus_id)) + #NBUS_QUEUE.put(hdlc_query_frame(prop.bus_id)) + reactor.callLater(5, self.queue_poll) + + +class Prop(six.with_metaclass(io.IOMeta, io.Prop)): def __init__(self, bus_id, title=None): self.bus_id = bus_id @@ -43,84 +129,14 @@ class Prop(six.with_metaclass(IOMeta)): if self.title is None: self.title = name.replace('_', ' ').capitalize() -class Input(Input): + +class Input(io.Input): pass -class Output(Output): + +class Output(io.Output): pass -class Analog(Sensor): + +class Analog(io.Sensor): pass - -@attr.s -class HDLCFrame(object): - _frame_data = attr.ib() - - def is_valid(self): - r_crc = self._frame_data[-2:] - crc16 = crcmod.predefined.Crc('xmodem') - crc16.update(self._frame_data[:-2]) - crc = crc16.digest() - return crc == r_crc - -class NBusProtocol(protocol.Protocol): - _buf = b'' # type: bytes - _escape = False - - def dataReceived(self, data): - #log.msg("RX: {}".format(hexlify(data))) - for byte in data: - if self._escape: - escaped_byte = byte & ~(1 << 5) - if escaped_byte not in b'\x7d\x7e': - log.msg( - 'Warning: Unnecessary escaped value: {}'.format(escaped_byte)) - self._buf += escaped_byte - self._escape = False - elif byte == b'\x7d': - self._escape = True - elif byte == b'\x7e' and len(self._buf) > 0: - self._check_crc() - self._buf = b'' - else: - self._buf += byte - - def _check_crc(self): - r_crc = self._buf[-2:] - crc16 = crcmod.predefined.Crc('xmodem') - crc16.update(packet[:-2]) - crc = crc16.digest() - if crc != r_crc: - log.err("CRC Mismatch: {}".format(packet)) - return - - def _filter(self, packet): - pass - - -class SerialService(service.Service): - """Service wrapper for twisted.internet.serialport.""" - - _serial = None # type: SerialPort - _port = None # type: str - bypass = False # type: bool - - def __init__(self, port='/dev/ttyS0', baudrate=1000000, - power_gpio=None, tx_enable_gpio=None): - # type: (str, int) -> None - """Instanatiate SerialPort handling service.""" - self._port = port - self._baud = baudrate - - def startService(self): - """Start the NBus serial handling service.""" - try: - self._serial = SerialPort(NBusProtocol(), self._port, - reactor, baudrate=self._baud) - except IOError as e: - log.msg( - "Failed to initialize serial. NBUS disabled: {}".format(e)) - - def stopService(self): - """Stop the NBus serial handling service.""" - self._serial.loseConnection() diff --git a/src/escapelib/packet.py b/src/escapelib/packet.py new file mode 100644 index 0000000..f995f8a --- /dev/null +++ b/src/escapelib/packet.py @@ -0,0 +1,28 @@ +from enum import IntEnum + +import attr + +class PacketType(IntEnum): + Keepalive = 0 + InputChange = 1 + InputRequest = 2 + SensorRequest = 3 + PuzzleSolved = 4 + PuzzleFailed = 5 + Log = 6 + +@attr.s +class Packet(object): + p_type = attr.ib() + payload = attr.ib() + + @classmethod + def parse(cls, binary): + return cls(binary[0], binary[1:]) + +def process_packet(game, b_id, data): + packet = Packet.parse(data) + if packet.p_type == PacketType.InputChange: + pass + elif packet.p_type == PacketType.PuzzleSolved: + pass diff --git a/src/escapelib/util.py b/src/escapelib/util.py index aa34ced..e7464aa 100644 --- a/src/escapelib/util.py +++ b/src/escapelib/util.py @@ -1,5 +1,7 @@ """Utility Functions for Trespassed.""" +from __future__ import division + from twisted.internet import reactor, task def delay(sec): @@ -16,3 +18,10 @@ def quit_game(): if not skip: reactor.stop() skip = False + +def ceil_div(dividend, divisor): + """Return ceiling division of divident/divisor.""" + + r = dividend // divisor + r += 1 if dividend % divisor else 0 + return r