From 5d943a609dbbfb1e09be8a266689265b73bcb99e Mon Sep 17 00:00:00 2001
From: Laurens Valk <laurens@pybricks.com>
Date: Thu, 7 May 2020 11:12:04 +0200
Subject: [PATCH] ev3/snippets/bluetooth_pc: copy pybricks bluetooth

This copies the Pybricks MicroPython bluetooth implementation. As-is, it only runs in Pybricks MicroPython.

The next commits will adapt it to work with MicroPython and then Python3. We only need the Python3 end result, but this staged approach makes it easy to keep track of what will be changed.
---
 .../ev3/bluetooth_pc/pybricks/bluetooth.py    | 187 ++++++++++
 .../ev3/bluetooth_pc/pybricks/messaging.py    | 327 ++++++++++++++++++
 2 files changed, 514 insertions(+)
 create mode 100644 snippets/ev3/bluetooth_pc/pybricks/bluetooth.py
 create mode 100644 snippets/ev3/bluetooth_pc/pybricks/messaging.py

diff --git a/snippets/ev3/bluetooth_pc/pybricks/bluetooth.py b/snippets/ev3/bluetooth_pc/pybricks/bluetooth.py
new file mode 100644
index 0000000..a1c6cc2
--- /dev/null
+++ b/snippets/ev3/bluetooth_pc/pybricks/bluetooth.py
@@ -0,0 +1,187 @@
+# SPDX-License-Identifier: MIT
+# Copyright (C) 2020 The Pybricks Authors
+
+"""
+:class:`RFCOMMServer` can be used to communicate with other Bluetooth RFCOMM
+devices that don't support the EV3 mailbox protocol.
+
+It is based on the standard library ``socketserver`` module and attempts to
+remain a strict subset of that implementation when it comes to low-level
+implementation details.
+"""
+
+from _thread import start_new_thread
+from uctypes import addressof, sizeof, struct, ARRAY, UINT8, UINT16
+from usocket import socket, SOCK_STREAM
+
+from bluetooth_c import resolve
+
+# stuff from bluetooth/bluetooth.h
+
+AF_BLUETOOTH = 31
+BTPROTO_RFCOMM = 3
+BDADDR_ANY = '00:00:00:00:00:00'
+
+sa_family_t = UINT16
+
+bd_addr_t = {
+    'b': (ARRAY | 0, UINT8 | 6)
+}
+
+sockaddr_rc = {
+    'rc_family': sa_family_t | 0,
+    'rc_bdaddr': (2, bd_addr_t),
+    'rc_channel': UINT8 | 8,
+}
+
+
+def str2ba(string, ba):
+    """Convert string to Bluetooth address"""
+    for i, v in enumerate(string.split(':')):
+        ba.b[5-i] = int(v, 16)
+
+
+def ba2str(ba):
+    """Convert Bluetooth address to string"""
+    string = []
+    for b in ba.b:
+        string.append('{:02X}'.format(b))
+    string.reverse()
+    return ':'.join(string).upper()
+
+
+class RFCOMMServer:
+    """Object that simplifies setting up an RFCOMM socket server.
+
+    This is based on the ``socketserver.SocketServer`` class in the Python
+    standard library.
+    """
+    request_queue_size = 1
+
+    def __init__(self, server_address, RequestHandlerClass):
+        self.server_address = server_address
+        self.RequestHandlerClass = RequestHandlerClass
+
+        self.socket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM)
+
+        try:
+            addr_data = bytearray(sizeof(sockaddr_rc))
+            addr = struct(addressof(addr_data), sockaddr_rc)
+            addr.rc_family = AF_BLUETOOTH
+            str2ba(server_address[0], addr.rc_bdaddr)
+            addr.rc_channel = server_address[1]
+            self.socket.bind(addr_data)
+            # self.server_address = self.socket.getsockname()
+            self.socket.listen(self.request_queue_size)
+        except:
+            self.server_close()
+            raise
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, type, value, traceback):
+        self.server_close()
+
+    def handle_request(self):
+        try:
+            request, addr_data = self.socket.accept()
+        except OSError:
+            return
+
+        try:
+            addr = struct(addressof(addr_data), sockaddr_rc)
+            client_address = (ba2str(addr.rc_bdaddr), addr.rc_channel)
+            self.process_request(request, client_address)
+        except:
+            request.close()
+            raise
+
+    def process_request(self, request, client_address):
+        self.finish_request(request, client_address)
+        request.close()
+
+    def finish_request(self, request, client_address):
+        self.RequestHandlerClass(request, client_address, self)
+
+    def server_close(self):
+        self.socket.close()
+
+
+class ThreadingMixIn:
+    def process_request_thread(self, request, client_address):
+        try:
+            self.finish_request(request, client_address)
+        finally:
+            request.close()
+
+    def process_request(self, request, client_address):
+        start_new_thread(self.process_request_thread, (request, client_address))
+
+
+class ThreadingRFCOMMServer(ThreadingMixIn, RFCOMMServer):
+    """Version of :class:`RFCOMMServer` that handles connections in a new
+    thread.
+    """
+    pass
+
+
+class StreamRequestHandler:
+    """Class that handles incoming requests.
+
+    This is based on ``socketserver.StreamRequestHandler`` from the Python
+    standard library.
+    """
+    def __init__(self, request, client_address, server):
+        self.request = request
+        self.client_address = client_address
+        self.server = server
+        self.setup()
+        try:
+            self.handle()
+        finally:
+            self.finish()
+
+    def setup(self):
+        self.wfile = self.request
+        self.rfile = self.request
+
+    def handle(self):
+        pass
+
+    def finish(self):
+        pass
+
+
+class RFCOMMClient:
+    def __init__(self, client_address, RequestHandlerClass):
+        self.client_address = client_address
+        self.RequestHandlerClass = RequestHandlerClass
+        self.socket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM)
+
+    def handle_request(self):
+        addr_data = bytearray(sizeof(sockaddr_rc))
+        addr = struct(addressof(addr_data), sockaddr_rc)
+        addr.rc_family = AF_BLUETOOTH
+        str2ba(self.client_address[0], addr.rc_bdaddr)
+        addr.rc_channel = self.client_address[1]
+        self.socket.connect(addr_data)
+        try:
+            self.process_request(self.socket, self.client_address)
+        except:
+            self.socket.close()
+            raise
+
+    def process_request(self, request, client_address):
+        self.finish_request(request, client_address)
+        request.close()
+
+    def finish_request(self, request, client_address):
+        self.RequestHandlerClass(request, client_address, self)
+
+    def client_close(self):
+        self.socket.close()
+
+
+class ThreadingRFCOMMClient(ThreadingMixIn, RFCOMMClient):
+    pass
diff --git a/snippets/ev3/bluetooth_pc/pybricks/messaging.py b/snippets/ev3/bluetooth_pc/pybricks/messaging.py
new file mode 100644
index 0000000..093ba1e
--- /dev/null
+++ b/snippets/ev3/bluetooth_pc/pybricks/messaging.py
@@ -0,0 +1,327 @@
+# SPDX-License-Identifier: MIT
+# Copyright (C) 2020 The Pybricks Authors
+
+from _thread import allocate_lock
+from uerrno import ECONNRESET
+from ustruct import pack, unpack
+
+from pybricks.bluetooth import (resolve, BDADDR_ANY, ThreadingRFCOMMServer,
+                                ThreadingRFCOMMClient, StreamRequestHandler)
+
+
+class Mailbox:
+    def __init__(self, name, connection, encode=None, decode=None):
+        """Object that represents a mailbox for sending an receiving messages
+        from other connected devices.
+
+        Arguments:
+            name (str):
+                The name of this mailbox.
+            connection:
+                A connection object that implements the mailbox connection
+                interface.
+            encode:
+                A function that encodes an object into a bytes-like object.
+            decode:
+                A function that decodes an object from a bytes-like object.
+        """
+        self.name = name
+        self._connection = connection
+
+        if encode:
+            self.encode = encode
+
+        if decode:
+            self.decode = decode
+
+    def encode(self, value):
+        return value
+
+    def decode(self, payload):
+        return payload
+
+    def read(self):
+        """Reads the current value of the mailbox.
+
+        Returns:
+            The decoded value or ``None`` if the mailbox has never received
+            a value.
+        """
+        data = self._connection.read_from_mailbox(self.name)
+        if data is None:
+            return None
+        return self.decode(data)
+
+    def send(self, value, destination=None):
+        """Sends a value to remote mailboxes with the same name as this
+        mailbox.
+
+        Arguments:
+            value: The value to send.
+            destination: The name or address of a specific device or ``None``
+                to broadcast to all connected devices.
+        """
+        data = self.encode(value)
+        self._connection.send_to_mailbox(destination, self.name, data)
+
+    def wait(self):
+        """Waits for the mailbox to receive a message."""
+        self._connection.wait_for_mailbox_update(self.name)
+
+    def wait_new(self):
+        """Waits for the mailbox to receive a message that is different from
+        the current contents of the mailbox.
+
+        Returns:
+            The new value. (Same as return value of :meth:`read`.)
+        """
+        old = self.read()
+        while True:
+            self.wait()
+            new = self.read()
+            if new != old:
+                return new
+
+
+class LogicMailbox(Mailbox):
+    """:class:`Mailbox` that holds a logic or boolean value.
+
+    This is compatible with the "logic" message blocks in the standard
+    EV3 firmware.
+    """
+
+    def encode(self, value):
+        return b'\x01' if value else b'\x00'
+
+    def decode(self, payload):
+        return bool(payload[0])
+
+
+class NumericMailbox(Mailbox):
+    """:class:`Mailbox` that holds a numeric or floating point value.
+
+    This is compatible with the "numeric" message blocks in the standard
+    EV3 firmware.
+    """
+
+    def encode(self, value):
+        return pack('<f', value)
+
+    def decode(self, payload):
+        return unpack('<f', payload)[0]
+
+
+class TextMailbox(Mailbox):
+    """:class:`Text` that holds a text or string point value.
+
+    This is compatible with the "text" message blocks in the standard
+    EV3 firmware.
+    """
+
+    def encode(self, value):
+        return '{}\0'.format(value)
+
+    def decode(self, payload):
+        return payload.decode().strip('\0')
+
+
+# EV3 standard firmware is hard-coded to use channel 1
+EV3_RFCOMM_CHANNEL = 1
+
+# EV3 VM bytecodes
+SYSTEM_COMMAND_NO_REPLY = 0x81
+WRITEMAILBOX = 0x9E
+
+
+class MailboxHandler(StreamRequestHandler):
+    def handle(self):
+        with self.server._lock:
+            self.server._clients[self.client_address[0]] = self.request
+        while True:
+            try:
+                buf = self.rfile.read(2)
+            except OSError as ex:
+                # The client disconnected the connection
+                if ex.args[0] == ECONNRESET:
+                    break
+                raise
+            size, = unpack('<H', buf)
+            buf = self.rfile.read(size)
+            msg_count, cmd_type, cmd, name_size = unpack('<HBBB', buf)
+            if cmd_type != SYSTEM_COMMAND_NO_REPLY:
+                raise ValueError('Bad message type')
+            if cmd != WRITEMAILBOX:
+                raise ValueError('Bad command')
+            mbox = buf[5:5+name_size].decode().strip('\0')
+            data_size, = unpack('<H', buf[5+name_size:7+name_size])
+            data = buf[7+name_size:7+name_size+data_size]
+
+            with self.server._lock:
+                self.server._mailboxes[mbox] = data
+                update_lock = self.server._updates.get(mbox)
+                if update_lock:
+                    update_lock.release()
+
+
+class MailboxHandlerMixIn:
+    def __init__(self):
+        # protects against concurrent access of other attributes
+        self._lock = allocate_lock()
+        # map of mailbox name to raw data
+        self._mailboxes = {}
+        # map of device name/address to object with send() method
+        self._clients = {}
+        # map of mailbox name to mutex lock
+        self._updates = {}
+        # map of names to addresses
+        self._addresses = {}
+
+    def read_from_mailbox(self, mbox):
+        """Reads the current raw data from a mailbox.
+
+        Arguments:
+            mbox (str):
+                The name of the mailbox.
+
+        Returns:
+            bytes:
+                The current mailbox raw data or ``None`` if nothing has ever
+                been delivered to the mailbox.
+        """
+        with self._lock:
+            return self._mailboxes.get(mbox)
+
+    def send_to_mailbox(self, brick, mbox, payload):
+        """Sends a mailbox value using raw bytes data.
+
+        Arguments:
+            brick (str):
+                The name or address of the brick or ``None``` to broadcast to
+                all connected devices
+            mbox (str):
+                The name of the mailbox.
+            payload (bytes):
+                A bytes-like object that will be sent to the mailbox.
+        """
+        mbox_len = len(mbox) + 1
+        payload_len = len(payload)
+        send_len = 7 + mbox_len + payload_len
+        fmt = '<HHBBB{}sH{}s'.format(mbox_len, payload_len)
+        data = pack(fmt, send_len, 1, SYSTEM_COMMAND_NO_REPLY, WRITEMAILBOX,
+                    mbox_len, mbox, payload_len, payload)
+        with self._lock:
+            if brick is None:
+                for client in self._clients.values():
+                    client.send(data)
+            else:
+                addr = self._addresses.get(brick)
+                if addr is None:
+                    addr = resolve(brick)
+                    self._addresses[brick] = addr
+                if addr is None:
+                    raise ValueError('no paired devices matching "{}"'.format(brick))
+                self._clients[addr].send(data)
+
+    def wait_for_mailbox_update(self, mbox):
+        """Waits until ``mbox`` receives a value."""
+        lock = allocate_lock()
+        lock.acquire()
+        with self._lock:
+            self._updates[mbox] = lock
+        try:
+            return lock.acquire()
+        finally:
+            with self._lock:
+                del self._updates[mbox]
+
+
+class BluetoothMailboxServer(MailboxHandlerMixIn, ThreadingRFCOMMServer):
+    def __init__(self):
+        """Object that represents an incoming Bluetooth connection from another
+        EV3.
+
+        The remote EV3 can either be running MicroPython or the standard EV3
+        firmare.
+        """
+        super().__init__()
+        super(ThreadingRFCOMMServer, self).__init__(
+            (BDADDR_ANY, EV3_RFCOMM_CHANNEL), MailboxHandler)
+
+    def wait_for_connection(self, count=1):
+        """Waits for a :class:`BluetoothMailboxClient` on a remote device to
+        connect.
+
+        Arguments:
+            count (int):
+                The number of remote connections to wait for.
+
+        Raises:
+            OSError:
+                There was a problem establishing the connection.
+        """
+        for _ in range(count):
+            self.handle_request()
+
+
+class MailboxRFCOMMClient(ThreadingRFCOMMClient):
+    def __init__(self, parent, bdaddr):
+        self.parent = parent
+        super().__init__((bdaddr, EV3_RFCOMM_CHANNEL), MailboxHandler)
+
+    def send(self, data):
+        self.socket.send(data)
+
+    def finish_request(self, request, client_address):
+        self.RequestHandlerClass(request, client_address, self.parent)
+
+
+class BluetoothMailboxClient(MailboxHandlerMixIn):
+    """Object that represents outgoing Bluetooth connections to one or more
+    remote EV3s.
+
+    The remote EV3s can either be running MicroPython or the standard EV3
+    firmare.
+    """
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, type, value, traceback):
+        self.close()
+
+    def connect(self, brick):
+        """Connects to a :class:`BluetoothMailboxServer` on another device.
+
+        The remote device must be paired and waiting for a connection. See
+        :meth:`BluetoothMailboxServer.wait_for_connection`.
+
+        Arguments:
+            brick (str):
+                The name or address of the remote EV3 to connect to.
+
+        Raises:
+            TypeError:
+                ``brick`` is not a string
+            ValueError:
+                There are no paired Bluetooth devices that match ``brick``
+                or connection to ``brick`` already exists.
+            OSError:
+                There was a problem establishing the connection.
+        """
+        addr = resolve(brick)
+        if addr is None:
+            raise ValueError('no paired devices matching "{}"'.format(brick))
+        client = MailboxRFCOMMClient(self, addr)
+        if self._clients.setdefault(addr, client) is not client:
+            raise ValueError('connection with this address already exists')
+        try:
+            client.handle_request()
+        except:
+            del self._clients[addr]
+            raise
+
+    def close(self):
+        """Closes the connections."""
+        for client in self._clients.values():
+            client.client_close()
+        self._clients.clear()