From b351cd94d7de9743f3f0f76ceb30440d40091a77 Mon Sep 17 00:00:00 2001 From: Jesse Hills <3060199+jesserockz@users.noreply.github.com> Date: Sat, 6 Feb 2021 11:02:20 +1300 Subject: [PATCH] Fix PN532 SPI communication (#1511) --- esphome/components/pn532/pn532.cpp | 100 ++--------------- esphome/components/pn532/pn532.h | 4 +- .../components/pn532/pn532_mifare_classic.cpp | 6 +- .../pn532/pn532_mifare_ultralight.cpp | 4 +- esphome/components/pn532_i2c/pn532_i2c.cpp | 84 ++++++++++++++ esphome/components/pn532_i2c/pn532_i2c.h | 2 + esphome/components/pn532_spi/pn532_spi.cpp | 105 ++++++++++++++++-- esphome/components/pn532_spi/pn532_spi.h | 1 + 8 files changed, 203 insertions(+), 103 deletions(-) diff --git a/esphome/components/pn532/pn532.cpp b/esphome/components/pn532/pn532.cpp index e2511648b6..1cc2b19c2e 100644 --- a/esphome/components/pn532/pn532.cpp +++ b/esphome/components/pn532/pn532.cpp @@ -22,7 +22,7 @@ void PN532::setup() { } std::vector version_data; - if (!this->read_response_(PN532_COMMAND_VERSION_DATA, version_data)) { + if (!this->read_response(PN532_COMMAND_VERSION_DATA, version_data)) { ESP_LOGE(TAG, "Error getting version"); this->mark_failed(); return; @@ -42,7 +42,7 @@ void PN532::setup() { } std::vector wakeup_result; - if (!this->read_response_(PN532_COMMAND_SAMCONFIGURATION, wakeup_result)) { + if (!this->read_response(PN532_COMMAND_SAMCONFIGURATION, wakeup_result)) { this->error_code_ = WAKEUP_FAILED; this->mark_failed(); return; @@ -62,7 +62,7 @@ void PN532::setup() { } std::vector sam_result; - if (!this->read_response_(PN532_COMMAND_SAMCONFIGURATION, sam_result)) { + if (!this->read_response(PN532_COMMAND_SAMCONFIGURATION, sam_result)) { ESP_LOGV(TAG, "Invalid SAM result: (%u)", sam_result.size()); // NOLINT for (uint8_t dat : sam_result) { ESP_LOGV(TAG, " 0x%02X", dat); @@ -97,7 +97,7 @@ void PN532::loop() { return; std::vector read; - bool success = this->read_response_(PN532_COMMAND_INLISTPASSIVETARGET, read); + bool success = this->read_response(PN532_COMMAND_INLISTPASSIVETARGET, read); this->requested_read_ = false; @@ -230,7 +230,7 @@ bool PN532::write_command_(const std::vector &data) { } bool PN532::read_ack_() { - ESP_LOGVV(TAG, "Reading ACK..."); + ESP_LOGV(TAG, "Reading ACK..."); std::vector data; if (!this->read_data(data, 6)) { @@ -241,96 +241,18 @@ bool PN532::read_ack_() { data[2] == 0x00 && // start of packet data[3] == 0xFF && data[4] == 0x00 && // ACK packet code data[5] == 0xFF && data[6] == 0x00); // postamble - ESP_LOGVV(TAG, "ACK valid: %s", YESNO(matches)); + ESP_LOGV(TAG, "ACK valid: %s", YESNO(matches)); return matches; } -bool PN532::read_response_(uint8_t command, std::vector &data) { - ESP_LOGV(TAG, "Reading response"); - uint8_t len = this->read_response_length_(); - if (len == 0) { - return false; - } - - ESP_LOGV(TAG, "Reading response of length %d", len); - if (!this->read_data(data, 6 + len + 2)) { - ESP_LOGD(TAG, "No response data"); - return false; - } - - if (data[1] != 0x00 && data[2] != 0x00 && data[3] != 0xFF) { - // invalid packet - ESP_LOGV(TAG, "read data invalid preamble!"); - return false; - } - - bool valid_header = (static_cast(data[4] + data[5]) == 0 && // LCS, len + lcs = 0 - data[6] == 0xD5 && // TFI - frame from PN532 to system controller - data[7] == command + 1); // Correct command response - - if (!valid_header) { - ESP_LOGV(TAG, "read data invalid header!"); - return false; - } - - data.erase(data.begin(), data.begin() + 6); // Remove headers - - uint8_t checksum = 0; - for (int i = 0; i < len + 1; i++) { - uint8_t dat = data[i]; - checksum += dat; - } - checksum = ~checksum + 1; - - if (data[len + 1] != checksum) { - ESP_LOGV(TAG, "read data invalid checksum! %02X != %02X", data[len], checksum); - return false; - } - - if (data[len + 2] != 0x00) { - ESP_LOGV(TAG, "read data invalid postamble!"); - return false; - } - - data.erase(data.begin(), data.begin() + 2); // Remove TFI and command code - data.erase(data.end() - 2, data.end()); // Remove checksum and postamble - - return true; -} - -uint8_t PN532::read_response_length_() { - std::vector data; - if (!this->read_data(data, 6)) { - return 0; - } - - if (data[1] != 0x00 && data[2] != 0x00 && data[3] != 0xFF) { - // invalid packet - ESP_LOGV(TAG, "read data invalid preamble!"); - return 0; - } - - bool valid_header = (static_cast(data[4] + data[5]) == 0 && // LCS, len + lcs = 0 - data[6] == 0xD5); // TFI - frame from PN532 to system controller - - if (!valid_header) { - ESP_LOGV(TAG, "read data invalid header!"); - return 0; - } - - this->write_data({0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00}); // NACK - Retransmit last message - - // full length of message, including TFI - uint8_t full_len = data[4]; - // length of data, excluding TFI - uint8_t len = full_len - 1; - if (full_len == 0) - len = 0; - return len; +void PN532::send_nack_() { + ESP_LOGV(TAG, "Sending NACK for retransmit"); + this->write_data({0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00}); + delay(10); } void PN532::turn_off_rf_() { - ESP_LOGVV(TAG, "Turning RF field OFF"); + ESP_LOGV(TAG, "Turning RF field OFF"); this->write_command_({ PN532_COMMAND_RFCONFIGURATION, 0x01, // RF Field diff --git a/esphome/components/pn532/pn532.h b/esphome/components/pn532/pn532.h index 6b3bef2918..95a2e0dd2a 100644 --- a/esphome/components/pn532/pn532.h +++ b/esphome/components/pn532/pn532.h @@ -46,12 +46,12 @@ class PN532 : public PollingComponent { protected: void turn_off_rf_(); bool write_command_(const std::vector &data); - bool read_response_(uint8_t command, std::vector &data); bool read_ack_(); - uint8_t read_response_length_(); + void send_nack_(); virtual bool write_data(const std::vector &data) = 0; virtual bool read_data(std::vector &data, uint8_t len) = 0; + virtual bool read_response(uint8_t command, std::vector &data) = 0; nfc::NfcTag *read_tag_(std::vector &uid); diff --git a/esphome/components/pn532/pn532_mifare_classic.cpp b/esphome/components/pn532/pn532_mifare_classic.cpp index 4e8c255755..93d394330d 100644 --- a/esphome/components/pn532/pn532_mifare_classic.cpp +++ b/esphome/components/pn532/pn532_mifare_classic.cpp @@ -64,7 +64,7 @@ bool PN532::read_mifare_classic_block_(uint8_t block_num, std::vector & return false; } - if (!this->read_response_(PN532_COMMAND_INDATAEXCHANGE, data) || data[0] != 0x00) { + if (!this->read_response(PN532_COMMAND_INDATAEXCHANGE, data) || data[0] != 0x00) { return false; } data.erase(data.begin()); @@ -89,7 +89,7 @@ bool PN532::auth_mifare_classic_block_(std::vector &uid, uint8_t block_ } std::vector response; - if (!this->read_response_(PN532_COMMAND_INDATAEXCHANGE, response) || response[0] != 0x00) { + if (!this->read_response(PN532_COMMAND_INDATAEXCHANGE, response) || response[0] != 0x00) { ESP_LOGE(TAG, "Authentication failed - Block 0x%02x", block_num); return false; } @@ -194,7 +194,7 @@ bool PN532::write_mifare_classic_block_(uint8_t block_num, std::vector } std::vector response; - if (!this->read_response_(PN532_COMMAND_INDATAEXCHANGE, response)) { + if (!this->read_response(PN532_COMMAND_INDATAEXCHANGE, response)) { ESP_LOGE(TAG, "Error writing block %d", block_num); return false; } diff --git a/esphome/components/pn532/pn532_mifare_ultralight.cpp b/esphome/components/pn532/pn532_mifare_ultralight.cpp index 00cb18aacd..bd62806c1e 100644 --- a/esphome/components/pn532/pn532_mifare_ultralight.cpp +++ b/esphome/components/pn532/pn532_mifare_ultralight.cpp @@ -52,7 +52,7 @@ bool PN532::read_mifare_ultralight_page_(uint8_t page_num, std::vector return false; } - if (!this->read_response_(PN532_COMMAND_INDATAEXCHANGE, data) || data[0] != 0x00) { + if (!this->read_response(PN532_COMMAND_INDATAEXCHANGE, data) || data[0] != 0x00) { return false; } data.erase(data.begin()); @@ -168,7 +168,7 @@ bool PN532::write_mifare_ultralight_page_(uint8_t page_num, std::vector } std::vector response; - if (!this->read_response_(PN532_COMMAND_INDATAEXCHANGE, response)) { + if (!this->read_response(PN532_COMMAND_INDATAEXCHANGE, response)) { ESP_LOGE(TAG, "Error writing page %d", page_num); return false; } diff --git a/esphome/components/pn532_i2c/pn532_i2c.cpp b/esphome/components/pn532_i2c/pn532_i2c.cpp index 162487de58..37147c5eb9 100644 --- a/esphome/components/pn532_i2c/pn532_i2c.cpp +++ b/esphome/components/pn532_i2c/pn532_i2c.cpp @@ -36,6 +36,90 @@ bool PN532I2C::read_data(std::vector &data, uint8_t len) { return true; } +bool PN532I2C::read_response(uint8_t command, std::vector &data) { + ESP_LOGV(TAG, "Reading response"); + uint8_t len = this->read_response_length_(); + if (len == 0) { + return false; + } + + ESP_LOGV(TAG, "Reading response of length %d", len); + if (!this->read_data(data, 6 + len + 2)) { + ESP_LOGD(TAG, "No response data"); + return false; + } + + if (data[1] != 0x00 && data[2] != 0x00 && data[3] != 0xFF) { + // invalid packet + ESP_LOGV(TAG, "read data invalid preamble!"); + return false; + } + + bool valid_header = (static_cast(data[4] + data[5]) == 0 && // LCS, len + lcs = 0 + data[6] == 0xD5 && // TFI - frame from PN532 to system controller + data[7] == command + 1); // Correct command response + + if (!valid_header) { + ESP_LOGV(TAG, "read data invalid header!"); + return false; + } + + data.erase(data.begin(), data.begin() + 6); // Remove headers + + uint8_t checksum = 0; + for (int i = 0; i < len + 1; i++) { + uint8_t dat = data[i]; + checksum += dat; + } + checksum = ~checksum + 1; + + if (data[len + 1] != checksum) { + ESP_LOGV(TAG, "read data invalid checksum! %02X != %02X", data[len], checksum); + return false; + } + + if (data[len + 2] != 0x00) { + ESP_LOGV(TAG, "read data invalid postamble!"); + return false; + } + + data.erase(data.begin(), data.begin() + 2); // Remove TFI and command code + data.erase(data.end() - 2, data.end()); // Remove checksum and postamble + + return true; +} + +uint8_t PN532I2C::read_response_length_() { + std::vector data; + if (!this->read_data(data, 6)) { + return 0; + } + + if (data[1] != 0x00 && data[2] != 0x00 && data[3] != 0xFF) { + // invalid packet + ESP_LOGV(TAG, "read data invalid preamble!"); + return 0; + } + + bool valid_header = (static_cast(data[4] + data[5]) == 0 && // LCS, len + lcs = 0 + data[6] == 0xD5); // TFI - frame from PN532 to system controller + + if (!valid_header) { + ESP_LOGV(TAG, "read data invalid header!"); + return 0; + } + + this->send_nack_(); + + // full length of message, including TFI + uint8_t full_len = data[4]; + // length of data, excluding TFI + uint8_t len = full_len - 1; + if (full_len == 0) + len = 0; + return len; +} + void PN532I2C::dump_config() { PN532::dump_config(); LOG_I2C_DEVICE(this); diff --git a/esphome/components/pn532_i2c/pn532_i2c.h b/esphome/components/pn532_i2c/pn532_i2c.h index 23cb00bb10..296d73e042 100644 --- a/esphome/components/pn532_i2c/pn532_i2c.h +++ b/esphome/components/pn532_i2c/pn532_i2c.h @@ -14,6 +14,8 @@ class PN532I2C : public pn532::PN532, public i2c::I2CDevice { protected: bool write_data(const std::vector &data) override; bool read_data(std::vector &data, uint8_t len) override; + bool read_response(uint8_t command, std::vector &data) override; + uint8_t read_response_length_(); }; } // namespace pn532_i2c diff --git a/esphome/components/pn532_spi/pn532_spi.cpp b/esphome/components/pn532_spi/pn532_spi.cpp index 3da799fb24..6f87ad8ca7 100644 --- a/esphome/components/pn532_spi/pn532_spi.cpp +++ b/esphome/components/pn532_spi/pn532_spi.cpp @@ -26,7 +26,7 @@ bool PN532Spi::write_data(const std::vector &data) { delay(2); // First byte, communication mode: Write data this->write_byte(0x01); - + ESP_LOGV(TAG, "Writing data: %s", hexencode(data).c_str()); this->write_array(data.data(), data.size()); this->disable(); @@ -34,31 +34,122 @@ bool PN532Spi::write_data(const std::vector &data) { } bool PN532Spi::read_data(std::vector &data, uint8_t len) { - this->enable(); - // First byte, communication mode: Read state - this->write_byte(0x02); + ESP_LOGV(TAG, "Waiting for ready byte..."); uint32_t start_time = millis(); while (true) { - if (this->read_byte() & 0x01) + this->enable(); + // First byte, communication mode: Read state + this->write_byte(0x02); + bool ready = this->read_byte() == 0x01; + this->disable(); + if (ready) break; + ESP_LOGV(TAG, "Not ready yet..."); if (millis() - start_time > 100) { - this->disable(); ESP_LOGV(TAG, "Timed out waiting for readiness from PN532!"); return false; } + yield(); } // Read data (transmission from the PN532 to the host) + this->enable(); + delay(2); this->write_byte(0x03); + ESP_LOGV(TAG, "Reading data..."); + data.resize(len); this->read_array(data.data(), len); this->disable(); data.insert(data.begin(), 0x01); + ESP_LOGV(TAG, "Read data: %s", hexencode(data).c_str()); return true; -}; +} + +bool PN532Spi::read_response(uint8_t command, std::vector &data) { + ESP_LOGV(TAG, "Reading response"); + + uint32_t start_time = millis(); + while (true) { + this->enable(); + // First byte, communication mode: Read state + this->write_byte(0x02); + bool ready = this->read_byte() == 0x01; + this->disable(); + if (ready) + break; + ESP_LOGV(TAG, "Not ready yet..."); + + if (millis() - start_time > 100) { + ESP_LOGV(TAG, "Timed out waiting for readiness from PN532!"); + return false; + } + yield(); + } + + this->enable(); + delay(2); + this->write_byte(0x03); + + std::vector header(7); + this->read_array(header.data(), 7); + + ESP_LOGV(TAG, "Header data: %s", hexencode(header).c_str()); + + if (header[0] != 0x00 && header[1] != 0x00 && header[2] != 0xFF) { + // invalid packet + ESP_LOGV(TAG, "read data invalid preamble!"); + return false; + } + + bool valid_header = (static_cast(header[3] + header[4]) == 0 && // LCS, len + lcs = 0 + header[5] == 0xD5 && // TFI - frame from PN532 to system controller + header[6] == command + 1); // Correct command response + + if (!valid_header) { + ESP_LOGV(TAG, "read data invalid header!"); + return false; + } + + // full length of message, including command response + uint8_t full_len = header[3]; + // length of data, excluding command response + uint8_t len = full_len - 1; + if (full_len == 0) + len = 0; + + ESP_LOGV(TAG, "Reading response of length %d", len); + + data.resize(len + 1); + this->read_array(data.data(), len + 1); + this->disable(); + + ESP_LOGV(TAG, "Response data: %s", hexencode(data).c_str()); + + uint8_t checksum = header[5] + header[6]; // TFI + Command response code + for (int i = 0; i < len - 1; i++) { + uint8_t dat = data[i]; + checksum += dat; + } + checksum = ~checksum + 1; + + if (data[len - 1] != checksum) { + ESP_LOGV(TAG, "read data invalid checksum! %02X != %02X", data[len - 1], checksum); + return false; + } + + if (data[len] != 0x00) { + ESP_LOGV(TAG, "read data invalid postamble!"); + return false; + } + + data.erase(data.end() - 2, data.end()); // Remove checksum and postamble + + return true; +} void PN532Spi::dump_config() { PN532::dump_config(); diff --git a/esphome/components/pn532_spi/pn532_spi.h b/esphome/components/pn532_spi/pn532_spi.h index 967b8a66cf..d98bd447c8 100644 --- a/esphome/components/pn532_spi/pn532_spi.h +++ b/esphome/components/pn532_spi/pn532_spi.h @@ -18,6 +18,7 @@ class PN532Spi : public pn532::PN532, protected: bool write_data(const std::vector &data) override; bool read_data(std::vector &data, uint8_t len) override; + bool read_response(uint8_t command, std::vector &data) override; }; } // namespace pn532_spi