Implement Bluetooth receive queue

Author: Bre77

tesla_fleet_api/tesla/vehicle/bluetooth.py

@@ -1,7 +1,7 @@
 from __future__ import annotations
 
 import hashlib
-from asyncio import Future, get_running_loop
+import asyncio
 from typing import TYPE_CHECKING
 from google.protobuf.message import DecodeError
 
@@ -85,7 +85,7 @@ class VehicleBluetooth(Commands):
 
     ble_name: str
     client: BleakClient
-    _futures: dict[Domain, Future]
+    _queues: dict[Domain, asyncio.Queue]
     _ekey: ec.EllipticCurvePublicKey
     _recv: bytearray = bytearray()
     _recv_len: int = 0
@@ -96,7 +96,10 @@ def __init__(
     ):
         super().__init__(parent, vin, key)
         self.ble_name = "S" + hashlib.sha1(vin.encode('utf-8')).hexdigest()[:16] + "C"
-        self._futures = {}
+        self._queues = {
+            Domain.DOMAIN_VEHICLE_SECURITY: asyncio.Queue(),
+            Domain.DOMAIN_INFOTAINMENT: asyncio.Queue(),
+        }
         if device is not None:
             self.client = BleakClient(device, services=[SERVICE_UUID])
 
@@ -135,7 +138,7 @@ async def __aexit__(self, exc_type, exc_val, exc_tb) -> None:
         """Exit the async context."""
         await self.disconnect()
 
-    def _on_notify(self,sender: BleakGATTCharacteristic,data : bytearray) -> None:
+    async def _on_notify(self,sender: BleakGATTCharacteristic,data : bytearray) -> None:
         """Receive data from the Tesla BLE device."""
         if self._recv_len:
             self._recv += data
@@ -145,16 +148,16 @@ def _on_notify(self,sender: BleakGATTCharacteristic,data : bytearray) -> None:
         LOGGER.debug(f"Received {len(self._recv)} of {self._recv_len} bytes")
         while len(self._recv) > self._recv_len:
             LOGGER.warn(f"Received more data than expected: {len(self._recv)} > {self._recv_len}")
-            self._on_message(bytes(self._recv[:self._recv_len]))
+            await self._on_message(bytes(self._recv[:self._recv_len]))
             self._recv_len = int.from_bytes(self._recv[self._recv_len:self._recv_len+2], 'big')
             self._recv = self._recv[self._recv_len+2:]
             continue
         if len(self._recv) == self._recv_len:
-            self._on_message(bytes(self._recv))
+            await self._on_message(bytes(self._recv))
             self._recv = bytearray()
             self._recv_len = 0
 
-    def _on_message(self, data:bytes) -> None:
+    async def _on_message(self, data:bytes) -> None:
         """Receive messages from the Tesla BLE data."""
         try:
             msg = RoutableMessage.FromString(data)
@@ -164,57 +167,36 @@ def _on_message(self, data:bytes) -> None:
             self._recv_len = 0
             return
 
-        # Update Session
-        if(msg.session_info):
-            info = SessionInfo.FromString(msg.session_info)
-            # maybe dont?
-            if(info.status == Session_Info_Status.SESSION_INFO_STATUS_KEY_NOT_ON_WHITELIST):
-                 self._futures[msg.from_destination.domain].set_exception(NotOnWhitelistFault())
-                 return
-            self._sessions[msg.from_destination.domain].update(info)
-
         if(msg.to_destination.routing_address != self._from_destination):
-            # Get the ephemeral key here and save to self._ekey
-            return
-
-        if(msg.from_destination.domain in self._futures):
-            LOGGER.debug(f"Received response for request {msg.request_uuid}")
-            self._futures[msg.from_destination.domain].set_result(msg)
-            del self._futures[msg.from_destination.domain]
+            # Ignore ephemeral key broadcasts
             return
 
-        if msg.from_destination.domain == Domain.DOMAIN_VEHICLE_SECURITY:
-            submsg = FromVCSECMessage.FromString(msg.protobuf_message_as_bytes)
-            LOGGER.warning(f"Received orphaned VCSEC response: {submsg}")
-        elif msg.from_destination.domain == Domain.DOMAIN_INFOTAINMENT:
-            submsg = Response.FromString(msg.protobuf_message_as_bytes)
-            LOGGER.warning(f"Received orphaned INFOTAINMENT response: {submsg}")
-        else:
-            LOGGER.warning(f"Received orphaned response: {msg}")
-
-    async def _create_future(self, domain: Domain) -> Future:
-        if(not self._sessions[domain].lock.locked):
-            raise ValueError("Session is not locked")
-        self._futures[domain] = get_running_loop().create_future()
-        return self._futures[domain]
+        LOGGER.info(f"Received response: {msg}")
+        await self._queues[msg.from_destination.domain].put(msg)
 
-    async def _send(self, msg: RoutableMessage) -> RoutableMessage:
+    async def _send(self, msg: RoutableMessage, requires: str) -> RoutableMessage:
         """Serialize a message and send to the vehicle and wait for a response."""
         domain = msg.to_destination.domain
         async with self._sessions[domain].lock:
             LOGGER.debug(f"Sending message {msg}")
-            future = await self._create_future(domain)
+
             payload = prependLength(msg.SerializeToString())
 
+            # Empty the queue before sending the message
+            while not self._queues[domain].empty():
+                await self._queues[domain].get()
             await self.client.write_gatt_char(WRITE_UUID, payload, True)
 
-            resp = await future
-            LOGGER.debug(f"Received message {resp}")
+            # Process the response
+            async with asyncio.timeout(10):
+                while True:
+                    resp = await self._queues[domain].get()
+                    LOGGER.debug(f"Received message {resp}")
 
-            if resp.signedMessageStatus.signed_message_fault > 0:
-                raise MESSAGE_FAULTS[resp.signedMessageStatus.signed_message_fault]
+                    self.validate_msg(resp)
 
-            return resp
+                    if resp.HasField(requires):
+                        return resp
 
     async def pair(self, role: Role = Role.ROLE_OWNER, form: KeyFormFactor = KeyFormFactor.KEY_FORM_FACTOR_CLOUD_KEY):
         """Pair the key."""

tesla_fleet_api/tesla/vehicle/commands.py

@@ -14,7 +14,9 @@
 from asyncio import Lock, sleep
 
 from tesla_fleet_api.exceptions import (
+    MESSAGE_FAULTS,
     SIGNED_MESSAGE_INFORMATION_FAULTS,
+    NotOnWhitelistFault,
     #TeslaFleetMessageFaultInvalidSignature,
     TeslaFleetMessageFaultIncorrectEpoch,
     TeslaFleetMessageFaultInvalidTokenOrCounter,
@@ -38,6 +40,7 @@
     Response,
 )
 from tesla_fleet_api.tesla.vehicle.proto.signatures_pb2 import (
+    Session_Info_Status,
     SignatureType,
     Tag,
     AES_GCM_Personalized_Signature_Data,
@@ -251,10 +254,21 @@ def shared_key(self, vehicleKey: bytes) -> bytes:
 
 
     @abstractmethod
-    async def _send(self, msg: RoutableMessage) -> RoutableMessage:
+    async def _send(self, msg: RoutableMessage, requires: str) -> RoutableMessage:
         """Transmit the message to the vehicle."""
         raise NotImplementedError
 
+    def validate_msg(self, msg: RoutableMessage) -> None:
+        """Validate the message."""
+        if(msg.session_info):
+            info = SessionInfo.FromString(msg.session_info)
+            if(info.status == Session_Info_Status.SESSION_INFO_STATUS_KEY_NOT_ON_WHITELIST):
+                raise NotOnWhitelistFault
+            self._sessions[msg.from_destination.domain].update(info)
+
+        if msg.signedMessageStatus.signed_message_fault > 0:
+            raise MESSAGE_FAULTS[msg.signedMessageStatus.signed_message_fault]
+
     @abstractmethod
     async def _command(self, domain: Domain, command: bytes, attempt: int = 0) -> dict[str, Any]:
         """Serialize a message and send to the signed command endpoint."""
@@ -270,7 +284,7 @@ async def _command(self, domain: Domain, command: bytes, attempt: int = 0) -> di
             raise ValueError(f"Unknown auth method: {self._auth_method}")
 
         try:
-            resp = await self._send(msg)
+            resp = await self._send(msg, "protobuf_message_as_bytes")
         except (
             #TeslaFleetMessageFaultInvalidSignature,
             TeslaFleetMessageFaultIncorrectEpoch,
@@ -334,7 +348,11 @@ async def _command(self, domain: Domain, command: bytes, attempt: int = 0) -> di
                 resp.protobuf_message_as_bytes = aesgcm.decrypt(resp.signature_data.AES_GCM_Response_data.nonce, resp.protobuf_message_as_bytes + resp.signature_data.AES_GCM_Response_data.tag, aad.finalize())
 
             if(resp.from_destination.domain == Domain.DOMAIN_VEHICLE_SECURITY):
-                vcsec = FromVCSECMessage.FromString(resp.protobuf_message_as_bytes)
+                try:
+                    vcsec = FromVCSECMessage.FromString(resp.protobuf_message_as_bytes)
+                except Exception as e:
+                    LOGGER.error("Failed to parse VCSEC message: %s %s", e, resp)
+                    raise e
                 LOGGER.debug("VCSEC Response: %s", vcsec)
                 if vcsec.HasField("nominalError"):
                     LOGGER.error("Command failed with reason: %s", vcsec.nominalError.genericError)
@@ -344,6 +362,10 @@ async def _command(self, domain: Domain, command: bytes, attempt: int = 0) -> di
                             "reason": GenericError_E.Name(vcsec.nominalError.genericError)
                         }
                     }
+                elif vcsec.HasField("vehicleStatus"):
+                    return {
+                        "response": vcsec.vehicleStatus
+                    }
                 elif vcsec.commandStatus.operationStatus == OperationStatus_E.OPERATIONSTATUS_OK:
                     return {"response": {"result": True, "reason": ""}}
                 elif vcsec.commandStatus.operationStatus == OperationStatus_E.OPERATIONSTATUS_WAIT:
@@ -359,7 +381,11 @@ async def _command(self, domain: Domain, command: bytes, attempt: int = 0) -> di
                         raise SIGNED_MESSAGE_INFORMATION_FAULTS[vcsec.commandStatus.signedMessageStatus.signedMessageInformation]
 
             elif(resp.from_destination.domain == Domain.DOMAIN_INFOTAINMENT):
-                response = Response.FromString(resp.protobuf_message_as_bytes)
+                try:
+                    response = Response.FromString(resp.protobuf_message_as_bytes)
+                except Exception as e:
+                    LOGGER.error("Failed to parse Infotainment Response: %s %s", e, resp)
+                    raise e
                 LOGGER.debug("Infotainment Response: %s", response)
                 if (response.HasField("ping")):
                     return {
@@ -534,7 +560,7 @@ async def _handshake(self, domain: Domain) -> bool:
             uuid=randbytes(16)
         )
 
-        await self._send(msg)
+        await self._send(msg, "session_info")
         return self._sessions[domain].ready
 
     async def ping(self) -> dict[str, Any]:

tesla_fleet_api/tesla/vehicle/signed.py

@@ -7,8 +7,10 @@
 from tesla_fleet_api.tesla.vehicle.commands import Commands
 from tesla_fleet_api.exceptions import (
     MESSAGE_FAULTS,
+    NotOnWhitelistFault,
 )
 from tesla_fleet_api.tesla.vehicle.proto.signatures_pb2 import (
+    Session_Info_Status,
     SessionInfo,
 )
 from tesla_fleet_api.tesla.vehicle.proto.universal_message_pb2 import (
@@ -31,25 +33,17 @@ def __init__(self, parent: TeslaFleetApi, vin: str):
         super(Commands, self).__init__(parent, vin)
 
 
-    async def _send(self, msg: RoutableMessage) -> RoutableMessage:
+    async def _send(self, msg: RoutableMessage, requires: str) -> RoutableMessage:
         """Serialize a message and send to the signed command endpoint."""
+        # requires isnt used because Fleet API messages are singular
 
         async with self._sessions[msg.to_destination.domain].lock:
-            resp = await self.signed_command(
+            json = await self.signed_command(
                 base64.b64encode(msg.SerializeToString()).decode()
             )
 
-            resp_msg = RoutableMessage.FromString(base64.b64decode(resp["response"]))
+            resp = RoutableMessage.FromString(base64.b64decode(json["response"]))
 
-            # Check UUID?
-            # Check RoutingAdress?
+            self.validate_msg(resp)
 
-            if resp_msg.session_info:
-                self._sessions[resp_msg.from_destination.domain].update(
-                    SessionInfo.FromString(resp_msg.session_info), self.private_key
-                )
-
-            if resp_msg.signedMessageStatus.signed_message_fault:
-                raise MESSAGE_FAULTS[resp_msg.signedMessageStatus.signed_message_fault]
-
-            return resp_msg
+            return resp