refactor parsing to in-place unwrap

Tomer27cz · Tomer27cz · commit 849cc949e677 · 2026-03-28T16:27:10.000+01:00
replace recursive logic with in-place unwrap

ApduHandler::parse now requires mutable buffer and calls unwrap_in_place

reducing allocations
simplifying reassembly/decryption
diff --git a/src/dlms_parser/apdu_handler.cpp b/src/dlms_parser/apdu_handler.cpp
@@ -5,169 +5,15 @@
 
 namespace dlms_parser {
 
-bool ApduHandler::parse(const uint8_t* buf, size_t len, AxdrPayloadCallback cb) const {
-  for (size_t pos = 0; pos < len; pos++) {
-    const uint8_t tag = buf[pos];
-    const uint8_t* rest = buf + pos + 1;
-    const size_t rest_len = len - pos - 1;
-
-    if (tag == DLMS_APDU_GENERAL_BLOCK_TRANSFER) {
-      Logger::log(LogLevel::DEBUG, "Found General-Block-Transfer (0xE0) at offset %zu", pos);
-      return parse_general_block_transfer_(buf + pos, len - pos, cb);
-    }
-    if (tag == DLMS_APDU_DATA_NOTIFICATION) {
-      Logger::log(LogLevel::DEBUG, "Found DATA-NOTIFICATION (0x0F) at offset %zu", pos);
-      return parse_data_notification_(rest, rest_len, cb);
-    }
-    if (tag == DLMS_APDU_GENERAL_GLO_CIPHERING || tag == DLMS_APDU_GENERAL_DED_CIPHERING) {
-      Logger::log(LogLevel::DEBUG, "Found ciphered APDU (0x%02X) at offset %zu", tag, pos);
-      return parse_ciphered_apdu_(rest, rest_len, tag, cb);
-    }
-    if (tag == DLMS_DATA_TYPE_ARRAY || tag == DLMS_DATA_TYPE_STRUCTURE) {
-      Logger::log(LogLevel::DEBUG, "Found raw AXDR %s (0x%02X) at offset %zu - no APDU wrapper",
-                  tag == DLMS_DATA_TYPE_ARRAY ? "ARRAY" : "STRUCTURE", tag, pos);
-      cb(buf + pos, len - pos);
-      return true;
-    }
+bool ApduHandler::parse(uint8_t* buf, size_t len, AxdrPayloadCallback cb) const {
+  UnwrapResult result = unwrap_in_place(buf, len);
+  if (result.length > 0) {
+    cb(buf + result.offset, result.length);
+    return true;
   }
-
-  Logger::log(LogLevel::WARNING, "No supported APDU tag found in buffer");
   return false;
 }
 
-bool ApduHandler::parse_data_notification_(const uint8_t* buf, size_t len,
-                                           AxdrPayloadCallback cb) const {
-  if (len < 5) {
-    Logger::log(LogLevel::WARNING, "DATA-NOTIFICATION payload too short (%zu bytes)", len);
-    return false;
-  }
-
-  size_t pos = 0;
-
-  // Long-Invoke-ID-And-Priority (4 bytes)
-  pos += 4;
-
-  // Date-Time presence flag (1 byte): 0x00 = absent, anything else = 12-byte datetime follows
-  const uint8_t has_datetime = buf[pos++];
-  if (has_datetime != 0x00) {
-    Logger::log(LogLevel::VERBOSE, "Datetime flag 0x%02X - skipping 12-byte datetime", has_datetime);
-    if (pos + 12 > len) {
-      Logger::log(LogLevel::WARNING, "Buffer too short to skip datetime object");
-      return false;
-    }
-    pos += 12;
-  }
-
-  if (pos >= len) {
-    Logger::log(LogLevel::WARNING, "No AXDR payload after DATA-NOTIFICATION header");
-    return false;
-  }
-
-  cb(buf + pos, len - pos);
-  return true;
-}
-
-bool ApduHandler::parse_ciphered_apdu_(const uint8_t* buf, size_t len, uint8_t /*tag*/,
-                                       AxdrPayloadCallback cb) const {
-  if (!decryptor_ || !decryptor_->has_key()) {
-    Logger::log(LogLevel::WARNING, "Encrypted APDU received but no decryption key is set");
-    return false;
-  }
-
-  size_t pos = 0;
-
-  // System title length byte (must be 8)
-  if (pos >= len || buf[pos] != DLMS_SYSTITLE_LENGTH) {
-    Logger::log(LogLevel::WARNING, "Unexpected system title length byte: 0x%02X",
-                pos < len ? buf[pos] : 0xFF);
-    return false;
-  }
-  pos++;
-
-  // System title (8 bytes) → first 8 bytes of the 12-byte IV
-  if (pos + DLMS_SYSTITLE_LENGTH > len) return false;
-  uint8_t iv[DLMS_IV_LENGTH];
-  std::memcpy(iv, buf + pos, DLMS_SYSTITLE_LENGTH);
-  pos += DLMS_SYSTITLE_LENGTH;
-
-  // BER length
-  const uint32_t cipher_len = utils::read_ber_length(buf, pos, len);
-  if (cipher_len == 0) return false;
-
-  // Security control byte (1 byte, informational — skip)
-  if (pos >= len) return false;
-  pos++;
-
-  // Frame counter (4 bytes) → last 4 bytes of IV
-  if (pos + DLMS_FRAME_COUNTER_LENGTH > len) return false;
-  std::memcpy(iv + DLMS_SYSTITLE_LENGTH, buf + pos, DLMS_FRAME_COUNTER_LENGTH);
-  pos += DLMS_FRAME_COUNTER_LENGTH;
-
-  // The length field includes the security control byte and frame counter (5 bytes total)
-  if (cipher_len < DLMS_LENGTH_CORRECTION) {
-    Logger::log(LogLevel::WARNING, "Cipher length field too small: %u", cipher_len);
-    return false;
-  }
-  const uint32_t payload_len = cipher_len - DLMS_LENGTH_CORRECTION;
-
-  if (pos + payload_len > len) {
-    Logger::log(LogLevel::WARNING, "Buffer too short for ciphertext (need %u, have %zu)", payload_len,
-                len - pos);
-    return false;
-  }
-
-  std::vector<uint8_t> plain;
-  if (!decryptor_->decrypt(iv, buf + pos, payload_len, plain)) {
-    Logger::log(LogLevel::ERROR, "Decryption failed");
-    return false;
-  }
-
-  // Decrypted payload begins with the inner APDU tag (typically 0x0F) — recurse
-  return parse(plain.data(), plain.size(), cb);
-}
-
-bool ApduHandler::parse_general_block_transfer_(const uint8_t* buf, size_t len,
-                                                 AxdrPayloadCallback cb) const {
-  // GBT block format: E0 [ctrl:1] [block_num:2] [block_num_ack:2] [BER_len] [data...]
-  // Reassemble all blocks, then recurse into parse().
-  std::vector<uint8_t> reassembled;
-  size_t pos = 0;
-
-  while (pos < len && buf[pos] == DLMS_APDU_GENERAL_BLOCK_TRANSFER) {
-    if (pos + 6 > len) {
-      Logger::log(LogLevel::WARNING, "GBT: truncated block header at offset %zu", pos);
-      return false;
-    }
-    const uint8_t ctrl = buf[pos + 1];
-    const bool is_last = (ctrl & 0x80U) != 0;
-    const uint16_t block_num = static_cast<uint16_t>(buf[pos + 2] << 8 | buf[pos + 3]);
-    // pos+4..pos+5: block_num_ack (skip)
-    size_t ber_pos = pos + 6;
-    const uint32_t block_len = utils::read_ber_length(buf, ber_pos, len);
-
-    if (ber_pos + block_len > len) {
-      Logger::log(LogLevel::WARNING, "GBT: block %u truncated (need %u, have %zu)",
-                  block_num, block_len, len - ber_pos);
-      return false;
-    }
-
-    Logger::log(LogLevel::DEBUG, "GBT block %u: %u bytes%s", block_num, block_len,
-                is_last ? " (last)" : "");
-    reassembled.insert(reassembled.end(), buf + ber_pos, buf + ber_pos + block_len);
-    pos = ber_pos + block_len;
-
-    if (is_last) break;
-  }
-
-  if (reassembled.empty()) {
-    Logger::log(LogLevel::WARNING, "GBT: no payload after reassembly");
-    return false;
-  }
-
-  Logger::log(LogLevel::DEBUG, "GBT: reassembled %zu bytes from blocks", reassembled.size());
-  return parse(reassembled.data(), reassembled.size(), cb);
-}
-
 // ---------------------------------------------------------------------------
 // In-place unwrap: sequential pipeline replacing recursive parse().
 // Scans for known APDU tag, transforms in-place, loops until AXDR is exposed.
diff --git a/src/dlms_parser/apdu_handler.h b/src/dlms_parser/apdu_handler.h
@@ -12,17 +12,18 @@ using AxdrPayloadCallback = std::function<void(const uint8_t* axdr, size_t len)>
 
 // Scans a buffer byte-by-byte for the first recognized DLMS APDU tag.
 // Unknown leading bytes are skipped. Recognized tags:
-//   0xE0  General-Block-Transfer   : reassembles numbered blocks, then recurses
+//   0xE0  General-Block-Transfer   : reassembles numbered blocks
 //   0x0F  DATA-NOTIFICATION        : strips Long-Invoke-ID and optional datetime header
-//   0xDB  General-Glo-Ciphering    : decrypts with GcmDecryptor, then recurses
-//   0xDF  General-Ded-Ciphering    : decrypts with GcmDecryptor, then recurses
+//   0xDB  General-Glo-Ciphering    : decrypts with GcmDecryptor
+//   0xDF  General-Ded-Ciphering    : decrypts with GcmDecryptor
 //   0x01 / 0x02  raw ARRAY/STRUCT  : no APDU wrapper (e.g. HDLC/Aidon)
 class ApduHandler {
  public:
   void set_decryptor(GcmDecryptor* d) { decryptor_ = d; }
 
   // Fires cb exactly once on success with the raw AXDR payload span.
-  bool parse(const uint8_t* buf, size_t len, AxdrPayloadCallback cb) const;
+  // Requires a mutable buffer for in-place decryption and reassembly.
+  bool parse(uint8_t* buf, size_t len, AxdrPayloadCallback cb) const;
 
   // In-place unwrap: transforms buf in a loop (GBT→decrypt→strip header).
   // Returns the offset and length of the AXDR payload within buf.
@@ -31,10 +32,6 @@ class ApduHandler {
   UnwrapResult unwrap_in_place(uint8_t* buf, size_t len) const;
 
  private:
-  bool parse_data_notification_(const uint8_t* buf, size_t len, AxdrPayloadCallback cb) const;
-  bool parse_ciphered_apdu_(const uint8_t* buf, size_t len, uint8_t tag, AxdrPayloadCallback cb) const;
-  bool parse_general_block_transfer_(const uint8_t* buf, size_t len, AxdrPayloadCallback cb) const;
-
   GcmDecryptor* decryptor_{nullptr};
 };
 
diff --git a/src/dlms_parser/gcm_decryptor.cpp b/src/dlms_parser/gcm_decryptor.cpp
@@ -26,73 +26,8 @@ void GcmDecryptor::set_key(const std::vector<uint8_t>& key) {
   }
 }
 
-bool GcmDecryptor::decrypt(const uint8_t* iv, const uint8_t* cipher, size_t cipher_len,
-                           std::vector<uint8_t>& plain_out) const {
-  plain_out.resize(cipher_len);
-
-#if defined(USE_ESP8266_FRAMEWORK_ARDUINO) || defined(USE_ESP8266) || defined(ESP8266)
-
-  std::memcpy(plain_out.data(), cipher, cipher_len);
-  br_gcm_context gcm_ctx;
-  br_aes_ct_ctr_keys bc;
-  br_aes_ct_ctr_init(&bc, key_.data(), key_.size());
-  br_gcm_init(&gcm_ctx, &bc.vtable, br_ghash_ctmul32);
-  br_gcm_reset(&gcm_ctx, iv, 12);
-  br_gcm_flip(&gcm_ctx);
-  br_gcm_run(&gcm_ctx, 0, plain_out.data(), cipher_len);
-  return true;
-
-// ESP_IDF_VERSION_VAL(6,0,0) == 0x60000; written as literal to avoid expansion errors when the macro is undefined
-#elif defined(USE_ESP32) && defined(ESP_IDF_VERSION) && (ESP_IDF_VERSION >= 0x60000)
-
-  psa_key_attributes_t attributes = PSA_KEY_ATTRIBUTES_INIT;
-  psa_set_key_type(&attributes, PSA_KEY_TYPE_AES);
-  psa_set_key_bits(&attributes, static_cast<psa_key_bits_t>(key_.size() * 8));
-  psa_set_key_usage_flags(&attributes, PSA_KEY_USAGE_DECRYPT);
-  psa_set_key_algorithm(&attributes, PSA_ALG_GCM);
-
-  mbedtls_svc_key_id_t key_id;
-  bool ok = false;
-  if (psa_import_key(&attributes, key_.data(), key_.size(), &key_id) == PSA_SUCCESS) {
-    psa_aead_operation_t op = PSA_AEAD_OPERATION_INIT;
-    if (psa_aead_decrypt_setup(&op, key_id, PSA_ALG_GCM) == PSA_SUCCESS &&
-        psa_aead_set_nonce(&op, iv, 12) == PSA_SUCCESS) {
-      size_t outlen = 0;
-      if (psa_aead_update(&op, cipher, cipher_len, plain_out.data(), cipher_len, &outlen) == PSA_SUCCESS &&
-          outlen == cipher_len) {
-        ok = true;
-      }
-    }
-    psa_aead_abort(&op);
-    psa_destroy_key(key_id);
-  }
-  if (!ok) {
-    Logger::log(LogLevel::ERROR, "PSA decryption failed");
-  }
-  return ok;
-
-#else
-
-  mbedtls_gcm_context gcm_ctx;
-  mbedtls_gcm_init(&gcm_ctx);
-  mbedtls_gcm_setkey(&gcm_ctx, MBEDTLS_CIPHER_ID_AES, key_.data(),
-                     static_cast<unsigned int>(key_.size() * 8));
-  mbedtls_gcm_starts(&gcm_ctx, MBEDTLS_GCM_DECRYPT, iv, 12);
-  size_t outlen = 0;
-  const int ret = mbedtls_gcm_update(&gcm_ctx, cipher, cipher_len,
-                                     plain_out.data(), cipher_len, &outlen);
-  mbedtls_gcm_free(&gcm_ctx);
-  if (ret != 0) {
-    Logger::log(LogLevel::ERROR, "mbedTLS decryption failed: %d", ret);
-    return false;
-  }
-  return true;
-
-#endif
-}
-
 size_t GcmDecryptor::decrypt_in_place(const uint8_t* iv, uint8_t* buf,
-                                       size_t cipher_offset, size_t cipher_len) const {
+                                      size_t cipher_offset, size_t cipher_len) const {
 
 #if defined(USE_ESP8266_FRAMEWORK_ARDUINO) || defined(USE_ESP8266) || defined(ESP8266)
 
diff --git a/src/dlms_parser/gcm_decryptor.h b/src/dlms_parser/gcm_decryptor.h
@@ -17,11 +17,6 @@ class GcmDecryptor {
 
   bool has_key() const { return has_key_; }
 
-  // Decrypts AES-128-GCM ciphertext. iv must be exactly 12 bytes.
-  // On success, plain_out is filled with cipher_len decrypted bytes and true is returned.
-  bool decrypt(const uint8_t* iv, const uint8_t* cipher, size_t cipher_len,
-               std::vector<uint8_t>& plain_out) const;
-
   // In-place decrypt: reads ciphertext from buf[cipher_offset..], writes plaintext to buf[0..].
   // Returns plaintext length, or 0 on failure. iv must be exactly 12 bytes.
   size_t decrypt_in_place(const uint8_t* iv, uint8_t* buf, size_t cipher_offset, size_t cipher_len) const;
