Merge pull request #61 from vwidor/codex/tesla-key-persist-pairing

fix: persist private key and allow BLE whitelist pairing flow

Author: yoziru

include/client.h

@@ -50,6 +50,7 @@ class Client {
   int create_private_key();
   int load_private_key(const uint8_t *private_key_buffer, size_t private_key_length);
   int get_private_key(pb_byte_t *output_buffer, size_t output_buffer_length, size_t *output_length);
+  bool has_private_key() const;
 
   // Message building
   int build_white_list_message(Keys_Role role, VCSEC_KeyFormFactor form_factor, pb_byte_t *output_buffer,

include/vehicle.h

@@ -248,6 +248,7 @@ class Vehicle {
   void send_infotainment_poll_(const std::string &name, int32_t data_type, bool force_wake = false);
   void initiate_auth_for_domain_(const std::shared_ptr<Command> &command, UniversalMessage_Domain domain,
                                  CommandState waiting_state, const std::string &domain_name);
+  bool persist_private_key_();
   template<typename T> void send_infotainment_action_with_value_(const std::string &name, int32_t action_tag, T value) {
     send_command(UniversalMessage_Domain_DOMAIN_INFOTAINMENT, name,
                  [action_tag, value](Client *client, uint8_t *buff, size_t *len) {

src/client.cpp

@@ -97,6 +97,8 @@ int Client::get_private_key(pb_byte_t *output_buffer, size_t output_buffer_lengt
   return crypto_context_.get_private_key(output_buffer, output_buffer_length, output_length);
 }
 
+bool Client::has_private_key() const { return crypto_context_.is_private_key_initialized(); }
+
 int Client::generate_public_key_data_() {
   // Set the buffer size to maximum capacity before calling generate_public_key
   public_key_size_ = public_key_.size();

src/vehicle.cpp

@@ -156,7 +156,8 @@ void TeslaBLE::Vehicle::process_command_queue_() {
       break;
     case CommandState::WAITING_FOR_RESPONSE: {
       auto tx_duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - command->last_tx_at);
-      if (tx_duration > CLOCK_SYNC_MAX_LATENCY) {
+      const auto timeout = (command->name == "Whitelist Add Key") ? COMMAND_TIMEOUT : CLOCK_SYNC_MAX_LATENCY;
+      if (tx_duration > timeout) {
         if (command->name == "Wake" && is_vehicle_awake_) {
           LOG_INFO("Wake response timeout but vehicle is awake - proceeding");
           mark_command_completed_(command);
@@ -181,6 +182,15 @@ std::shared_ptr<TeslaBLE::Command> TeslaBLE::Vehicle::peek_command_() const {
 
 void TeslaBLE::Vehicle::process_idle_command_(const std::shared_ptr<Command> &command) {
   command->started_at = std::chrono::steady_clock::now();
+
+  // Pairing starts with an untrusted key, so VCSEC session auth will return
+  // KEY_NOT_ON_WHITELIST. This command must be sent without prior session auth.
+  if (command->name == "Whitelist Add Key") {
+    LOG_INFO("Bypassing session auth for Whitelist Add Key");
+    command->state = CommandState::READY;
+    return;
+  }
+
   switch (command->domain) {
     case UniversalMessage_Domain_DOMAIN_BROADCAST:
       command->state = CommandState::READY;
@@ -1216,11 +1226,48 @@ void TeslaBLE::Vehicle::close_windows() {
 // Pairing and Key Management
 // =============================================================================
 
+bool TeslaBLE::Vehicle::persist_private_key_() {
+  if (!client_ || !storage_adapter_) {
+    LOG_ERROR("Client or storage adapter unavailable");
+    return false;
+  }
+
+  std::array<uint8_t, 2048> key_buf{};
+  size_t key_len = 0;
+  if (client_->get_private_key(key_buf.data(), key_buf.size(), &key_len) != 0) {
+    LOG_ERROR("Failed to export private key");
+    return false;
+  }
+
+  if (key_len == 0 || key_len > key_buf.size()) {
+    LOG_ERROR("Invalid private key length: %zu", key_len);
+    return false;
+  }
+
+  std::vector<uint8_t> key_vec(key_buf.begin(), key_buf.begin() + key_len);
+  if (!storage_adapter_->save("private_key", key_vec)) {
+    LOG_ERROR("Failed to save private key to storage");
+    return false;
+  }
+
+  return true;
+}
+
 void TeslaBLE::Vehicle::pair(Keys_Role role) {
   LOG_INFO("Initiating pairing sequence...");
-  if (client_->create_private_key() != 0) {
-    LOG_WARNING("Could not check/create private key, proceeding anyway");
+  if (!client_->has_private_key()) {
+    LOG_INFO("No private key loaded, creating a new one");
+    if (client_->create_private_key() != 0) {
+      LOG_ERROR("Failed to create private key for pairing");
+      return;
+    }
   }
+
+  if (!persist_private_key_()) {
+    LOG_ERROR("Cannot start pairing without persisted private key");
+    return;
+  }
+
   send_command(UniversalMessage_Domain_DOMAIN_VEHICLE_SECURITY, "Whitelist Add Key",
                [role_copy = role](Client *client, uint8_t *buf, size_t *len) {
                  return client->build_white_list_message(role_copy, VCSEC_KeyFormFactor_KEY_FORM_FACTOR_NFC_CARD, buf,
@@ -1230,23 +1277,17 @@ void TeslaBLE::Vehicle::pair(Keys_Role role) {
 
 void TeslaBLE::Vehicle::regenerate_key() {
   LOG_INFO("Regenerating private key...");
-  if (client_->create_private_key() == 0) {
-    // NOLINTNEXTLINE(readability-math-missing-parentheses) - macro from external library
-    uint8_t key_buf[MBEDTLS_ECP_MAX_PT_LEN];
-    size_t key_len = 0;
-    size_t buf_len = sizeof(key_buf);
-    if (client_->get_private_key(key_buf, buf_len, &key_len) == 0) {
-      std::vector<uint8_t> key_vec(key_buf, key_buf + key_len);
-      if (storage_adapter_->save("private_key", key_vec)) {
-        LOG_INFO("New private key saved to storage");
-      } else {
-        LOG_ERROR("Failed to save new private key");
-      }
-    } else {
-      LOG_ERROR("Failed to create private key");
-    }
+  if (client_->create_private_key() != 0) {
+    LOG_ERROR("Failed to create private key");
+    return;
   }
-}  // namespace TeslaBLE
+
+  if (persist_private_key_()) {
+    LOG_INFO("New private key saved to storage");
+  } else {
+    LOG_ERROR("Failed to save new private key");
+  }
+}
 
 void TeslaBLE::Vehicle::handle_signed_message_error_(const UniversalMessage_RoutableMessage &msg,
                                                      bool &has_session_error) {