pn71xx_mifare_classic.cpp

Go to the documentation of this file.

#include <algorithm>
#include <array>
#include <cinttypes>
#include <memory>
 
#include "pn71xx.h"
#include "esphome/core/log.h"
 
namespace esphome::pn71xx {
 
ESPHOME_LOG_TAG(TAG, "pn71xx.mifare_classic");
 
uint8_t PN71xx::read_mifare_classic_tag_(nfc::NfcTag &tag) {
  uint8_t current_block = 4;
  uint8_t message_start_index = 0;
  uint32_t message_length = 0;
 
  if (this->auth_mifare_classic_block_(current_block, nfc::MIFARE_CMD_AUTH_A, nfc::NDEF_KEY) != nfc::STATUS_OK) {
    ESP_LOGE(TAG, "Tag auth failed while attempting to read tag data");
    return nfc::STATUS_FAILED;
  }
  std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> block_data;
 
  if (this->read_mifare_classic_block_(current_block, block_data) == nfc::STATUS_OK) {
    if (!nfc::decode_mifare_classic_tlv(block_data, message_length, message_start_index)) {
      return nfc::STATUS_FAILED;
    }
  } else {
    ESP_LOGE(TAG, "Failed to read block %u", current_block);
    return nfc::STATUS_FAILED;
  }
  if (message_length > MIFARE_CLASSIC_MAX_NDEF_SIZE) {
    ESP_LOGE(TAG, "NDEF message too long: %" PRIu32 " bytes", message_length);
    return nfc::STATUS_FAILED;
  }
 
  const uint32_t buffer_size = nfc::get_mifare_classic_buffer_size(message_length);
  FixedVector<uint8_t> buffer;
  if (!buffer.try_init(buffer_size)) {
    ESP_LOGE(TAG, "Out of memory reading NDEF message of %" PRIu32 " bytes", buffer_size);
    return nfc::STATUS_FAILED;
  }
 
  while (buffer.size() < buffer_size) {
    if (nfc::mifare_classic_is_first_block(current_block)) {
      if (this->auth_mifare_classic_block_(current_block, nfc::MIFARE_CMD_AUTH_A, nfc::NDEF_KEY) != nfc::STATUS_OK) {
        ESP_LOGE(TAG, "Block authentication failed for %u", current_block);
        return nfc::STATUS_FAILED;
      }
    }
    if (this->read_mifare_classic_block_(current_block, block_data) != nfc::STATUS_OK) {
      ESP_LOGE(TAG, "Error reading block %u", current_block);
      return nfc::STATUS_FAILED;
    }
    for (const uint8_t byte : block_data) {
      buffer.push_back(byte);
    }
 
    current_block++;
    if (nfc::mifare_classic_is_trailer_block(current_block)) {
      current_block++;
    }
  }
 
  if (message_start_index >= buffer.size()) {
    return nfc::STATUS_FAILED;
  }
  tag.set_ndef_message(make_unique<nfc::NdefMessage>(std::span<const uint8_t>(buffer).subspan(message_start_index)));
 
  return nfc::STATUS_OK;
}
 
uint8_t PN71xx::read_mifare_classic_block_(uint8_t block_num,
                                           std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> &data) {
  nfc::NciMessage rx;
  nfc::NciMessage tx(nfc::NCI_PKT_MT_DATA, {XCHG_DATA_OID, nfc::MIFARE_CMD_READ, block_num});
  char buf[nfc::FORMAT_BYTES_BUFFER_SIZE];
 
  ESP_LOGVV(TAG, "Read XCHG_DATA_REQ: %s", nfc::format_bytes_to(buf, tx.get_message()));
  if (this->transceive_(tx, rx) != nfc::STATUS_OK) {
    ESP_LOGE(TAG, "Timeout reading tag data");
    return nfc::STATUS_FAILED;
  }
 
  if ((!rx.message_type_is(nfc::NCI_PKT_MT_DATA)) || (!rx.simple_status_response_is(XCHG_DATA_OID)) ||
      (!rx.message_length_is(18))) {
    ESP_LOGE(TAG, "MFC read block failed - block 0x%02x", block_num);
    ESP_LOGV(TAG, "Read response: %s", nfc::format_bytes_to(buf, rx.get_message()));
    return nfc::STATUS_FAILED;
  }
 
  // payload: XCHG_DATA status byte, 16 block bytes, one trailing status byte
  std::copy_n(rx.get_message().begin() + 4, data.size(), data.begin());
 
  ESP_LOGVV(TAG, " Block %u: %s", block_num, nfc::format_bytes_to(buf, data));
  return nfc::STATUS_OK;
}
 
uint8_t PN71xx::auth_mifare_classic_block_(uint8_t block_num, uint8_t key_num, const uint8_t *key) {
  uint8_t key_select = key_num;
  switch (key_num) {
    case nfc::MIFARE_CMD_AUTH_A:
      key_select = MFC_AUTHENTICATE_PARAM_KS_A;
      break;
 
    case nfc::MIFARE_CMD_AUTH_B:
      key_select = MFC_AUTHENTICATE_PARAM_KS_B;
      break;
 
    default:
      break;
  }
  if (key != nullptr) {
    key_select |= MFC_AUTHENTICATE_PARAM_EMBED_KEY;
  }
 
  nfc::NciMessage rx;
  nfc::NciMessage tx(nfc::NCI_PKT_MT_DATA, {MFC_AUTHENTICATE_OID, this->sect_to_auth_(block_num), key_select});
  if (key != nullptr) {
    tx.append(std::span<const uint8_t>(key, 6));
  }
 
  char buf[nfc::FORMAT_BYTES_BUFFER_SIZE];
  ESP_LOGVV(TAG, "MFC_AUTHENTICATE_REQ: %s", nfc::format_bytes_to(buf, tx.get_message()));
  if (this->transceive_(tx, rx) != nfc::STATUS_OK) {
    ESP_LOGE(TAG, "Sending MFC_AUTHENTICATE_REQ failed");
    return nfc::STATUS_FAILED;
  }
  if ((!rx.message_type_is(nfc::NCI_PKT_MT_DATA)) || (!rx.simple_status_response_is(MFC_AUTHENTICATE_OID)) ||
      (rx.get_message()[4] != nfc::STATUS_OK)) {
    ESP_LOGE(TAG, "MFC authentication failed - block 0x%02x", block_num);
    ESP_LOGVV(TAG, "MFC_AUTHENTICATE_RSP: %s", nfc::format_bytes_to(buf, rx.get_message()));
    return nfc::STATUS_FAILED;
  }
 
  ESP_LOGV(TAG, "MFC block %u authentication succeeded", block_num);
  return nfc::STATUS_OK;
}
 
uint8_t PN71xx::sect_to_auth_(const uint8_t block_num) {
  const uint8_t first_high_block = nfc::MIFARE_CLASSIC_BLOCKS_PER_SECT_LOW * nfc::MIFARE_CLASSIC_16BLOCK_SECT_START;
  if (block_num >= first_high_block) {
    return ((block_num - first_high_block) / nfc::MIFARE_CLASSIC_BLOCKS_PER_SECT_HIGH) +
           nfc::MIFARE_CLASSIC_16BLOCK_SECT_START;
  }
  return block_num / nfc::MIFARE_CLASSIC_BLOCKS_PER_SECT_LOW;
}
 
uint8_t PN71xx::format_mifare_classic_mifare_() {
  static constexpr std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> BLANK_BUFFER = {
      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  static constexpr std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> TRAILER_BUFFER = {
      0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x07, 0x80, 0x69, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 
  auto status = nfc::STATUS_OK;
 
  for (int block = 0; block < 64; block += 4) {
    if (this->auth_mifare_classic_block_(block + 3, nfc::MIFARE_CMD_AUTH_B, nfc::DEFAULT_KEY) != nfc::STATUS_OK) {
      continue;
    }
    if (block != 0) {
      if (this->write_mifare_classic_block_(block, BLANK_BUFFER.data(), BLANK_BUFFER.size()) != nfc::STATUS_OK) {
        ESP_LOGE(TAG, "Unable to write block %u", block);
        status = nfc::STATUS_FAILED;
      }
    }
    if (this->write_mifare_classic_block_(block + 1, BLANK_BUFFER.data(), BLANK_BUFFER.size()) != nfc::STATUS_OK) {
      ESP_LOGE(TAG, "Unable to write block %u", block + 1);
      status = nfc::STATUS_FAILED;
    }
    if (this->write_mifare_classic_block_(block + 2, BLANK_BUFFER.data(), BLANK_BUFFER.size()) != nfc::STATUS_OK) {
      ESP_LOGE(TAG, "Unable to write block %u", block + 2);
      status = nfc::STATUS_FAILED;
    }
    if (this->write_mifare_classic_block_(block + 3, TRAILER_BUFFER.data(), TRAILER_BUFFER.size()) != nfc::STATUS_OK) {
      ESP_LOGE(TAG, "Unable to write block %u", block + 3);
      status = nfc::STATUS_FAILED;
    }
  }
 
  return status;
}
 
uint8_t PN71xx::format_mifare_classic_ndef_() {
  static constexpr std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> EMPTY_NDEF_MESSAGE = {
      0x03, 0x03, 0xD0, 0x00, 0x00, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  static constexpr std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> BLANK_BLOCK = {
      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  static constexpr std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> BLOCK_1_DATA = {
      0x14, 0x01, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1};
  static constexpr std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> BLOCK_2_DATA = {
      0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1, 0x03, 0xE1};
  static constexpr std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> BLOCK_3_TRAILER = {
      0xA0, 0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0x78, 0x77, 0x88, 0xC1, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
  static constexpr std::array<uint8_t, nfc::MIFARE_CLASSIC_BLOCK_SIZE> NDEF_TRAILER = {
      0xD3, 0xF7, 0xD3, 0xF7, 0xD3, 0xF7, 0x7F, 0x07, 0x88, 0x40, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 
  if (this->auth_mifare_classic_block_(0, nfc::MIFARE_CMD_AUTH_B, nfc::DEFAULT_KEY) != nfc::STATUS_OK) {
    ESP_LOGE(TAG, "Unable to authenticate block 0 for formatting");
    return nfc::STATUS_FAILED;
  }
  if (this->write_mifare_classic_block_(1, BLOCK_1_DATA.data(), BLOCK_1_DATA.size()) != nfc::STATUS_OK) {
    return nfc::STATUS_FAILED;
  }
  if (this->write_mifare_classic_block_(2, BLOCK_2_DATA.data(), BLOCK_2_DATA.size()) != nfc::STATUS_OK) {
    return nfc::STATUS_FAILED;
  }
  if (this->write_mifare_classic_block_(3, BLOCK_3_TRAILER.data(), BLOCK_3_TRAILER.size()) != nfc::STATUS_OK) {
    return nfc::STATUS_FAILED;
  }
 
  ESP_LOGD(TAG, "Sector 0 formatted with NDEF");
 
  auto status = nfc::STATUS_OK;
 
  for (int block = 4; block < 64; block += 4) {
    if (this->auth_mifare_classic_block_(block + 3, nfc::MIFARE_CMD_AUTH_B, nfc::DEFAULT_KEY) != nfc::STATUS_OK) {
      return nfc::STATUS_FAILED;
    }
    if (block == 4) {
      if (this->write_mifare_classic_block_(block, EMPTY_NDEF_MESSAGE.data(), EMPTY_NDEF_MESSAGE.size()) !=
          nfc::STATUS_OK) {
        ESP_LOGE(TAG, "Unable to write block %u", block);
        status = nfc::STATUS_FAILED;
      }
    } else {
      if (this->write_mifare_classic_block_(block, BLANK_BLOCK.data(), BLANK_BLOCK.size()) != nfc::STATUS_OK) {
        ESP_LOGE(TAG, "Unable to write block %u", block);
        status = nfc::STATUS_FAILED;
      }
    }
    if (this->write_mifare_classic_block_(block + 1, BLANK_BLOCK.data(), BLANK_BLOCK.size()) != nfc::STATUS_OK) {
      ESP_LOGE(TAG, "Unable to write block %u", block + 1);
      status = nfc::STATUS_FAILED;
    }
    if (this->write_mifare_classic_block_(block + 2, BLANK_BLOCK.data(), BLANK_BLOCK.size()) != nfc::STATUS_OK) {
      ESP_LOGE(TAG, "Unable to write block %u", block + 2);
      status = nfc::STATUS_FAILED;
    }
    if (this->write_mifare_classic_block_(block + 3, NDEF_TRAILER.data(), NDEF_TRAILER.size()) != nfc::STATUS_OK) {
      ESP_LOGE(TAG, "Unable to write trailer block %u", block + 3);
      status = nfc::STATUS_FAILED;
    }
  }
  return status;
}
 
uint8_t PN71xx::write_mifare_classic_block_(uint8_t block_num, const uint8_t *data, size_t len) {
  nfc::NciMessage rx;
  nfc::NciMessage tx(nfc::NCI_PKT_MT_DATA, {XCHG_DATA_OID, nfc::MIFARE_CMD_WRITE, block_num});
  char buf[nfc::FORMAT_BYTES_BUFFER_SIZE];
 
  ESP_LOGVV(TAG, "Write XCHG_DATA_REQ 1: %s", nfc::format_bytes_to(buf, tx.get_message()));
  if (this->transceive_(tx, rx) != nfc::STATUS_OK) {
    ESP_LOGE(TAG, "Sending XCHG_DATA_REQ failed");
    return nfc::STATUS_FAILED;
  }
  // write command part two
  tx.set_payload({XCHG_DATA_OID});
  tx.append(std::span<const uint8_t>(data, len));
 
  ESP_LOGVV(TAG, "Write XCHG_DATA_REQ 2: %s", nfc::format_bytes_to(buf, tx.get_message()));
  if (this->transceive_(tx, rx, NFCC_TAG_WRITE_TIMEOUT) != nfc::STATUS_OK) {
    ESP_LOGE(TAG, "MFC XCHG_DATA timed out waiting for XCHG_DATA_RSP during block write");
    return nfc::STATUS_FAILED;
  }
 
  if ((!rx.message_type_is(nfc::NCI_PKT_MT_DATA)) || (!rx.simple_status_response_is(XCHG_DATA_OID)) ||
      (rx.get_message()[4] != nfc::MIFARE_CMD_ACK)) {
    ESP_LOGE(TAG, "MFC write block failed - block 0x%02x", block_num);
    ESP_LOGV(TAG, "Write response: %s", nfc::format_bytes_to(buf, rx.get_message()));
    return nfc::STATUS_FAILED;
  }
 
  return nfc::STATUS_OK;
}
 
uint8_t PN71xx::write_mifare_classic_tag_(const std::shared_ptr<nfc::NdefMessage> &message) {
  const auto encoded = message->encode();
  const uint32_t buffer_length = nfc::get_mifare_classic_buffer_size(encoded.size());
  FixedVector<uint8_t> buffer;
  nfc::fill_ndef_tlv(encoded, buffer_length, buffer);
 
  uint32_t index = 0;
  uint8_t current_block = 4;
 
  while (index < buffer_length) {
    if (nfc::mifare_classic_is_first_block(current_block)) {
      if (this->auth_mifare_classic_block_(current_block, nfc::MIFARE_CMD_AUTH_A, nfc::NDEF_KEY) != nfc::STATUS_OK) {
        return nfc::STATUS_FAILED;
      }
    }
 
    if (this->write_mifare_classic_block_(current_block, &buffer[index], nfc::MIFARE_CLASSIC_BLOCK_SIZE) !=
        nfc::STATUS_OK) {
      return nfc::STATUS_FAILED;
    }
    index += nfc::MIFARE_CLASSIC_BLOCK_SIZE;
    current_block++;
 
    if (nfc::mifare_classic_is_trailer_block(current_block)) {
      // Skipping as cannot write to trailer
      current_block++;
    }
  }
  return nfc::STATUS_OK;
}
 
uint8_t PN71xx::halt_mifare_classic_tag_() {
  nfc::NciMessage rx;
  nfc::NciMessage tx(nfc::NCI_PKT_MT_DATA, {XCHG_DATA_OID, nfc::MIFARE_CMD_HALT, 0});
 
  char buf[nfc::FORMAT_BYTES_BUFFER_SIZE];
  ESP_LOGVV(TAG, "Halt XCHG_DATA_REQ: %s", nfc::format_bytes_to(buf, tx.get_message()));
  if (this->transceive_(tx, rx, NFCC_TAG_WRITE_TIMEOUT) != nfc::STATUS_OK) {
    ESP_LOGE(TAG, "Sending halt XCHG_DATA_REQ failed");
    return nfc::STATUS_FAILED;
  }
  return nfc::STATUS_OK;
}
 
}  // namespace esphome::pn71xx