From 3233b44d3d9a4f0de09684c376634e9eec1a7bb0 Mon Sep 17 00:00:00 2001 From: Martin Milata Date: Wed, 19 Mar 2025 00:53:15 +0100 Subject: [PATCH] WIP: support BLE in emulator --- core/SConscript.unix | 2 +- core/embed/io/ble/inc/io/ble.h | 18 +- core/embed/io/ble/unix/ble.c | 293 +++++++++++++++++++++ core/site_scons/models/T2T1/emulator.py | 7 + core/site_scons/models/T3W1/emulator.py | 11 + python/src/trezorlib/transport/__init__.py | 2 + python/src/trezorlib/transport/emu_ble.py | 261 ++++++++++++++++++ 7 files changed, 585 insertions(+), 9 deletions(-) create mode 100644 core/embed/io/ble/unix/ble.c create mode 100644 python/src/trezorlib/transport/emu_ble.py diff --git a/core/SConscript.unix b/core/SConscript.unix index 52e1b52a95..cf0fe8e565 100644 --- a/core/SConscript.unix +++ b/core/SConscript.unix @@ -21,7 +21,7 @@ if BENCHMARK and PYOPT != '0': print("BENCHMARK=1 works only with PYOPT=0.") exit(1) -FEATURES_WANTED = ["input", "sd_card", "dma2d", "optiga", "tropic"] +FEATURES_WANTED = ["input", "sd_card", "dma2d", "optiga", "tropic", "ble"] if not models.has_emulator(TREZOR_MODEL): # skip unix build diff --git a/core/embed/io/ble/inc/io/ble.h b/core/embed/io/ble/inc/io/ble.h index c5ed59c975..223b5121f6 100644 --- a/core/embed/io/ble/inc/io/ble.h +++ b/core/embed/io/ble/inc/io/ble.h @@ -32,13 +32,14 @@ #define BLE_ADV_NAME_LEN 20 typedef enum { - BLE_SWITCH_OFF = 0, // Turn off BLE advertising, disconnect - BLE_SWITCH_ON = 1, // Turn on BLE advertising - BLE_PAIRING_MODE = 2, // Enter pairing mode - BLE_DISCONNECT = 3, // Disconnect from the connected device - BLE_ERASE_BONDS = 4, // Erase all bonding information - BLE_ALLOW_PAIRING = 5, // Accept pairing request - BLE_REJECT_PAIRING = 6, // Reject pairing request + BLE_SWITCH_OFF = 0, // Turn off BLE advertising, disconnect + BLE_SWITCH_ON = 1, // Turn on BLE advertising + BLE_PAIRING_MODE = 2, // Enter pairing mode + BLE_DISCONNECT = 3, // Disconnect from the connected device + BLE_ERASE_BONDS = 4, // Erase all bonding information + BLE_ALLOW_PAIRING = 5, // Accept pairing request + BLE_REJECT_PAIRING = 6, // Reject pairing request + BLE_EMULATOR_PONG = 255, // Ping reply, emulator only } ble_command_type_t; typedef struct { @@ -63,11 +64,12 @@ typedef enum { BLE_DISCONNECTED = 2, // Disconnected from a device BLE_PAIRING_REQUEST = 3, // Pairing request received BLE_PAIRING_CANCELLED = 4, // Pairing was cancelled by host + BLE_EMULATOR_PING = 255, // Ping, emulator only } ble_event_type_t; typedef struct { ble_event_type_t type; - int connection_id; + int connection_id; // XXX seems unused uint8_t data_len; uint8_t data[6]; } ble_event_t; diff --git a/core/embed/io/ble/unix/ble.c b/core/embed/io/ble/unix/ble.c new file mode 100644 index 0000000000..ef5265bc96 --- /dev/null +++ b/core/embed/io/ble/unix/ble.c @@ -0,0 +1,293 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +static const uint16_t DATA_PORT_OFFSET = 4; // see usb.py +static const uint16_t EVENT_PORT_OFFSET = 5; + +typedef enum { + BLE_MODE_OFF, + BLE_MODE_CONNECTABLE, + BLE_MODE_PAIRING, +} ble_mode_t; + +typedef struct { + ble_mode_t mode_current; + bool connected; + bool initialized; + bool accept_msgs; + bool pairing_requested; + ble_adv_start_cmd_data_t adv_cmd; + + uint16_t data_port; + int data_sock; + struct sockaddr_in data_si_me, data_si_other; + socklen_t data_slen; + + uint16_t event_port; + int event_sock; + struct sockaddr_in event_si_me, event_si_other; + socklen_t event_slen; +} ble_driver_t; + +static ble_driver_t g_ble_driver = {0}; + +// These are called from the kernel only, emulator doesn't have a kernel. +bool ble_init(void) { return true; } + +void ble_deinit(void) {} + +void ble_start(void) { + ble_driver_t *drv = &g_ble_driver; + memset(drv, 0, sizeof(*drv)); + drv->data_sock = -1; + drv->event_sock = -1; + + const char *ip = getenv("TREZOR_UDP_IP"); + const char *port_base_str = getenv("TREZOR_UDP_PORT"); + uint16_t port_base = port_base_str ? atoi(port_base_str) : 21324; + + drv->data_port = port_base + DATA_PORT_OFFSET; + drv->event_port = port_base + EVENT_PORT_OFFSET; + drv->data_sock = socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, IPPROTO_UDP); + drv->event_sock = + socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, + IPPROTO_UDP); // FIXME TCP might make more sense here + + ensure(sectrue * (drv->data_sock >= 0), NULL); + ensure(sectrue * (drv->event_sock >= 0), NULL); + + drv->data_si_me.sin_family = drv->event_si_me.sin_family = AF_INET; + drv->data_si_me.sin_addr.s_addr = ip ? inet_addr(ip) : htonl(INADDR_LOOPBACK); + drv->event_si_me.sin_addr.s_addr = + ip ? inet_addr(ip) : htonl(INADDR_LOOPBACK); + drv->data_si_me.sin_port = htons(drv->data_port); + drv->event_si_me.sin_port = htons(drv->event_port); + + int ret = -1; + ret = bind(drv->data_sock, (struct sockaddr *)&(drv->data_si_me), + sizeof(struct sockaddr_in)); + ensure(sectrue * (ret == 0), NULL); + ret = bind(drv->event_sock, (struct sockaddr *)&(drv->event_si_me), + sizeof(struct sockaddr_in)); + ensure(sectrue * (ret == 0), NULL); + + drv->initialized = true; +} + +void ble_stop(void) { + ble_driver_t *drv = &g_ble_driver; + if (!drv->initialized) { + return; + } + + if (drv->data_sock >= 0) { + close(drv->data_sock); + drv->data_sock = -1; + } + if (drv->event_sock >= 0) { + close(drv->event_sock); + drv->event_sock = -1; + } + drv->initialized = false; +} + +bool ble_issue_command(ble_command_t *command) { + ble_driver_t *drv = &g_ble_driver; + if (!drv->initialized) { + return false; + } + + switch (command->cmd_type) { + case BLE_SWITCH_OFF: + drv->mode_current = BLE_MODE_OFF; + drv->connected = false; + break; + case BLE_SWITCH_ON: + memcpy(&drv->adv_cmd, &command->data.adv_start, sizeof(drv->adv_cmd)); + drv->mode_current = BLE_MODE_CONNECTABLE; + break; + case BLE_PAIRING_MODE: + memcpy(&drv->adv_cmd, &command->data.adv_start, sizeof(drv->adv_cmd)); + drv->mode_current = BLE_MODE_PAIRING; + break; + case BLE_DISCONNECT: + drv->connected = false; + break; + case BLE_ERASE_BONDS: + break; + case BLE_ALLOW_PAIRING: + drv->pairing_requested = false; + drv->connected = true; + break; + case BLE_REJECT_PAIRING: + drv->pairing_requested = false; + break; + default: + printf("unix/ble: unknown command type\n"); + break; + } + + ssize_t r = -2; + if (drv->event_slen > 0) { + r = sendto(drv->event_sock, command, sizeof(*command), MSG_DONTWAIT, + (const struct sockaddr *)&(drv->event_si_other), + drv->event_slen); + } + if (r != sizeof(*command)) { + printf("unix/ble: failed to write command: %d\n", (int)r); + } + + return true; +} + +bool ble_get_event(ble_event_t *event) { + ble_driver_t *drv = &g_ble_driver; + if (!drv->initialized) { + return false; + } + struct sockaddr_in si; + socklen_t sl = sizeof(si); + uint8_t buf[sizeof(ble_event_t)] = {0}; + ssize_t r = recvfrom(drv->event_sock, buf, sizeof(buf), MSG_DONTWAIT, + (struct sockaddr *)&si, &sl); + if (r <= 0) { + return false; + } else if (r > sizeof(ble_event_t)) { + printf("unix/ble: event packet too long\n"); + return false; + } + + drv->event_si_other = si; + drv->event_slen = sl; + + switch (((ble_event_t *)buf)->type) { + case BLE_CONNECTED: + drv->connected = true; + break; + case BLE_DISCONNECTED: + drv->connected = false; + break; + case BLE_PAIRING_REQUEST: + drv->pairing_requested = true; + break; + case BLE_PAIRING_CANCELLED: + drv->pairing_requested = false; + break; + case BLE_EMULATOR_PING: + static const ble_command_t ping_resp = {.cmd_type = BLE_EMULATOR_PONG}; + ssize_t r = sendto( + drv->event_sock, &ping_resp, sizeof(ping_resp), MSG_DONTWAIT, + (const struct sockaddr *)&(drv->event_si_other), drv->event_slen); + ensure(sectrue * (r == sizeof(ping_resp)), NULL); + return false; + break; + default: + printf("unix/ble: unknown event type\n"); + break; + } + + memcpy(event, buf, sizeof(ble_event_t)); + return true; +} + +void ble_get_state(ble_state_t *state) { + const ble_driver_t *drv = &g_ble_driver; + memset(state, 0, sizeof(ble_state_t)); + + if (!drv->initialized) { + return; + } + + state->connected = drv->connected; + state->peer_count = (uint8_t)(drv->connected); + state->pairing = drv->mode_current == BLE_MODE_PAIRING; + state->connectable = drv->mode_current == BLE_MODE_CONNECTABLE; + state->pairing_requested = drv->pairing_requested; + state->state_known = true; +} + +bool ble_can_write(void) { + ble_driver_t *drv = &g_ble_driver; + if (!drv->initialized /* || !drv->connected || !drv->accept_msgs */) { + return false; + } + + struct pollfd fds[] = { + {drv->data_sock, POLLOUT, 0}, + }; + int r = poll(fds, 1, 0); + return (r > 0); +} + +bool ble_write(const uint8_t *data, uint16_t len) { + ble_driver_t *drv = &g_ble_driver; + if (!drv->initialized /* || !drv->connected || !drv->accept_msgs */) { + return false; + } + + ssize_t r = len; + if (drv->data_slen > 0) { + r = sendto(drv->data_sock, data, len, MSG_DONTWAIT, + (const struct sockaddr *)&(drv->data_si_other), drv->data_slen); + } + return r; +} + +bool ble_can_read(void) { + ble_driver_t *drv = &g_ble_driver; + if (!drv->initialized /* || !drv->connected || !drv->accept_msgs */) { + return false; + } + + struct pollfd fds[] = { + {drv->data_sock, POLLIN, 0}, + }; + int r = poll(fds, 1, 0); + return (r > 0); +} + +uint32_t ble_read(uint8_t *data, uint16_t max_len) { + ble_driver_t *drv = &g_ble_driver; + if (!drv->initialized /* || !drv->connected || !drv->accept_msgs */) { + return 0; + } + struct sockaddr_in si; + socklen_t sl = sizeof(si); + uint8_t buf[max_len]; + memset(buf, 0, max_len); + ssize_t r = recvfrom(drv->data_sock, buf, sizeof(buf), MSG_DONTWAIT, + (struct sockaddr *)&si, &sl); + if (r <= 0) { + return 0; + } + + drv->data_si_other = si; + drv->data_slen = sl; + memcpy(data, buf, r); + return r; +} + +bool ble_get_mac(uint8_t *mac, size_t max_len) { + ble_driver_t *drv = &g_ble_driver; + + if (max_len < 6) { + return false; + } + + if (!drv->initialized) { + memset(mac, 0, max_len); + return false; + } + + for (size_t i = 0; i < 6; i++) { + mac[i] = i + 1; + } + return true; +} diff --git a/core/site_scons/models/T2T1/emulator.py b/core/site_scons/models/T2T1/emulator.py index a4ad3f97d9..29fa69f58b 100644 --- a/core/site_scons/models/T2T1/emulator.py +++ b/core/site_scons/models/T2T1/emulator.py @@ -56,6 +56,13 @@ def configure( features_available.append("touch") defines += [("USE_TOUCH", "1")] + # FIXME: do not merge to main + if "ble" in features_wanted: + sources += ["embed/io/ble/unix/ble.c"] + paths += ["embed/io/ble/inc"] + features_available.append("ble") + defines += [("USE_BLE", "1")] + features_available.append("backlight") defines += [("USE_BACKLIGHT", "1")] diff --git a/core/site_scons/models/T3W1/emulator.py b/core/site_scons/models/T3W1/emulator.py index 0aa1eb5beb..5fbe9d3396 100644 --- a/core/site_scons/models/T3W1/emulator.py +++ b/core/site_scons/models/T3W1/emulator.py @@ -82,6 +82,17 @@ def configure( features_available.append("touch") defines += [("USE_TOUCH", "1")] + sources += ["embed/io/button/unix/button.c"] + paths += ["embed/io/button/inc"] + features_available.append("button") + defines += [("USE_BUTTON", "1")] + + if "ble" in features_wanted: + sources += ["embed/io/ble/unix/ble.c"] + paths += ["embed/io/ble/inc"] + features_available.append("ble") + defines += [("USE_BLE", "1")] + features_available.append("backlight") defines += [("USE_BACKLIGHT", "1")] diff --git a/python/src/trezorlib/transport/__init__.py b/python/src/trezorlib/transport/__init__.py index 5cf580932d..60121b6350 100644 --- a/python/src/trezorlib/transport/__init__.py +++ b/python/src/trezorlib/transport/__init__.py @@ -97,12 +97,14 @@ class Transport: def all_transports() -> t.Iterable[t.Type["Transport"]]: from .bridge import BridgeTransport + from .emu_ble import EmuBleTransport from .hid import HidTransport from .udp import UdpTransport from .webusb import WebUsbTransport transports: t.Tuple[t.Type["Transport"], ...] = ( BridgeTransport, + EmuBleTransport, HidTransport, UdpTransport, WebUsbTransport, diff --git a/python/src/trezorlib/transport/emu_ble.py b/python/src/trezorlib/transport/emu_ble.py new file mode 100644 index 0000000000..1cab022148 --- /dev/null +++ b/python/src/trezorlib/transport/emu_ble.py @@ -0,0 +1,261 @@ +# This file is part of the Trezor project. +# +# Copyright (C) 2025 SatoshiLabs and contributors +# +# This library is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License version 3 +# as published by the Free Software Foundation. +# +# This library is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the License along with this library. +# If not, see . + +from __future__ import annotations + +import logging +import socket +import time +from enum import Enum +from typing import TYPE_CHECKING, Iterable, Tuple + +import construct as c +from construct_classes import Struct + +from ..log import DUMP_PACKETS +from ..tools import EnumAdapter +from . import Timeout, Transport, TransportException +from .udp import UdpTransport + +if TYPE_CHECKING: + from ..models import TrezorModel + +SOCKET_TIMEOUT = 0.1 + +LOG = logging.getLogger(__name__) + + +class EventType(Enum): + NONE = 0 + CONNECTED = 1 + DISCONNECTED = 2 + PAIRING_REQUEST = 3 + PAIRING_CANCELLED = 4 + EMULATOR_PING = 255 + + +class CommandType(Enum): + SWITCH_OFF = 0 + SWITCH_ON = 1 + PAIRING_MODE = 2 + DISCONNECT = 3 + ERASE_BONDS = 4 + ALLOW_PAIRING = 5 + REJECT_PAIRING = 6 + EMULATOR_PONG = 255 + + +class Event(Struct): + event_type: EventType + connection_id: int + data: bytes + + # fmt: off + SUBCON = c.Struct( + "event_type" / EnumAdapter(c.Int32ul, EventType), + "connection_id" / c.Int32ul, + "data" / c.Prefixed(c.Int8ul, c.GreedyBytes), + ) + # fmt: on + + @staticmethod + def new( + event_type: EventType, connection_id: int = 0, data: bytes | None = None + ) -> Event: + return Event( + event_type=event_type, connection_id=connection_id, data=data or bytes() + ) + + +class Command(Struct): + command_type: CommandType + data_len: int + raw: bytes # TODO parse advertising data + + # fmt: off + SUBCON = c.Struct( + "command_type" / EnumAdapter(c.Int32ul, CommandType), + "data_len" / c.Int8ul, + "raw" / c.Bytes(32), + ) + # fmt: on + + +class EmuBleTransport(Transport): + + DEFAULT_HOST = "127.0.0.1" + DEFAULT_PORT = 21328 + PATH_PREFIX = "emuble" + ENABLED: bool = True + CHUNK_SIZE = 244 + + def __init__(self, device: str | None = None) -> None: + if not device: + host = EmuBleTransport.DEFAULT_HOST + port = EmuBleTransport.DEFAULT_PORT + else: + devparts = device.split(":") + host = devparts[0] + port = ( + int(devparts[1]) if len(devparts) > 1 else EmuBleTransport.DEFAULT_PORT + ) + self.device: Tuple[str, int] = (host, port) + + self.data_socket: socket.socket | None = None + self.event_socket: socket.socket | None = None + super().__init__() + + @classmethod + def _try_path(cls, path: str) -> "EmuBleTransport": + d = cls(path) + try: + d.open() + if d.ping(): + return d + else: + raise TransportException( + f"No Trezor device found at address {d.get_path()}" + ) + except Exception as e: + raise + raise TransportException(f"Error opening {d.get_path()}") from e + + finally: + d.close() + + @classmethod + def enumerate( + cls, _models: Iterable["TrezorModel"] | None = None + ) -> Iterable["EmuBleTransport"]: + default_path = f"{cls.DEFAULT_HOST}:{cls.DEFAULT_PORT}" + try: + return [cls._try_path(default_path)] + except TransportException: + return [] + + @classmethod + def find_by_path(cls, path: str, prefix_search: bool = False) -> "EmuBleTransport": + try: + address = path.replace(f"{cls.PATH_PREFIX}:", "") + return cls._try_path(address) + except TransportException: + if not prefix_search: + raise + + assert prefix_search # otherwise we would have raised above + return super().find_by_path(path, prefix_search) + + def get_path(self) -> str: + return "{}:{}:{}".format(self.PATH_PREFIX, *self.device) + + def open(self) -> None: + try: + self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.data_socket.connect(self.device) + self.data_socket.settimeout(SOCKET_TIMEOUT) + self.event_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.event_socket.connect((self.device[0], self.device[1] + 1)) + self.event_socket.settimeout(SOCKET_TIMEOUT) + except Exception: + self.close() + raise + + def close(self) -> None: + if self.data_socket is not None: + self.data_socket.close() + self.data_socket = None + if self.event_socket is not None: + self.event_socket.close() + self.event_socket = None + + def write_chunk(self, chunk: bytes) -> None: + assert self.data_socket is not None + if len(chunk) != self.CHUNK_SIZE: + raise TransportException("Unexpected data length") + LOG.log(DUMP_PACKETS, f"sending packet: {chunk.hex()}") + self.data_socket.sendall(chunk) + + def read_chunk(self, timeout: float | None = None) -> bytes: + assert self.data_socket is not None + start = time.time() + while True: + try: + chunk = self.data_socket.recv(64) + break + except socket.timeout: + if timeout is not None and time.time() - start > timeout: + raise Timeout(f"Timeout reading UDP packet ({timeout}s)") + LOG.log(DUMP_PACKETS, f"received packet: {chunk.hex()}") + if len(chunk) != 64: + raise TransportException(f"Unexpected chunk size: {len(chunk)}") + return bytearray(chunk) + + def find_debug(self) -> "UdpTransport": + host, port = self.device + return UdpTransport(f"{host}:{port - 3}") + + def wait_until_ready(self, timeout: float = 10) -> None: + try: + self.open() + start = time.monotonic() + while True: + if self.ping(): + break + elapsed = time.monotonic() - start + if elapsed >= timeout: + raise Timeout("Timed out waiting for connection.") + + time.sleep(0.05) + finally: + self.close() + + def ping(self) -> bool: + """Test if the device is listening.""" + assert self.event_socket is not None + resp = None + try: + self.event_socket.sendall(Event.new(EventType.EMULATOR_PING).build()) + resp = self.read_command() + except Exception: + pass + return (resp is not None) and (resp.command_type == CommandType.EMULATOR_PONG) + + def ble_connected(self) -> None: + assert self.event_socket is not None + self.event_socket.sendall(Event.new(EventType.CONNECTED).build()) + + def ble_disconnected(self) -> None: + assert self.event_socket is not None + self.event_socket.sendall(Event.new(EventType.DISCONNECTED).build()) + + def ble_pairing_request(self, mac: bytes) -> None: + assert self.event_socket is not None + assert len(mac) == 6 + self.event_socket.sendall( + Event.new(EventType.PAIRING_REQUEST, data=mac).build() + ) + + def ble_pairing_cancel(self) -> None: + assert self.event_socket is not None + self.event_socket.sendall(Event.new(EventType.PAIRING_CANCELLED).build()) + + def read_command(self) -> Command | None: + assert self.event_socket is not None + try: + data = self.event_socket.recv(64) + except TimeoutError: + return None + return Command.parse(data)