From 40fa7418636da5e03b0126e38c221b143b24b5a4 Mon Sep 17 00:00:00 2001
From: Jaz Chen <jazchen.tw@gmail.com>
Date: Sat, 15 Jan 2022 01:58:37 +0800
Subject: [PATCH] Add inCommunicateThru() and add check status for
 mifareclassic_WriteDataBlock()

---
 PN532/PN532.cpp | 61 ++++++++++++++++++++++++++++++++++++++++++++++++-
 PN532/PN532.h   |  1 +
 2 files changed, 61 insertions(+), 1 deletion(-)

diff --git a/PN532/PN532.cpp b/PN532/PN532.cpp
index 17701f1..46152ea 100644
--- a/PN532/PN532.cpp
+++ b/PN532/PN532.cpp
@@ -629,7 +629,19 @@ uint8_t PN532::mifareclassic_WriteDataBlock (uint8_t blockNumber, uint8_t *data)
     }
 
     /* Read the response packet */
-    return (0 < HAL(readResponse)(pn532_packetbuffer, sizeof(pn532_packetbuffer)));
+    if (0 > HAL(readResponse)(pn532_packetbuffer, sizeof(pn532_packetbuffer))) {
+        return 0;
+    }
+
+    /* Check status */
+    if (pn532_packetbuffer[0] != 0x00) {
+    	DMSG("Status code indicates an error: ");
+    	DMSG_HEX(pn532_packetbuffer[0]);
+    	DMSG("\n");
+        return 0;
+    }
+
+    return 1;
 }
 
 /**************************************************************************/
@@ -858,6 +870,53 @@ bool PN532::inDataExchange(uint8_t *send, uint8_t sendLength, uint8_t *response,
     return true;
 }
 
+/**************************************************************************/
+/*!
+    This command is used to support basic data exchanges
+    between the PN532 and a target.
+
+    @param  send            Pointer to the command buffer
+    @param  sendLength      Command length in bytes
+    @param  response        Pointer to response data
+    @param  responseLength  Pointer to the response data length
+*/
+/**************************************************************************/
+bool PN532::inCommunicateThru(uint8_t *send, uint8_t sendLength, uint8_t *response, uint8_t *responseLength)
+{
+  pn532_packetbuffer[0] = PN532_COMMAND_INCOMMUNICATETHRU;
+
+  if (HAL(writeCommand)(pn532_packetbuffer, 1, send, sendLength)) {
+    return false;
+  }
+
+  int16_t status = HAL(readResponse)(response, *responseLength, 1000);
+  if (status < 0) {
+    return false;
+  }
+
+  // check status code
+  if (response[0] != 0x0) {
+      DMSG("Status code indicates an error : 0x");
+      DMSG_HEX(pn532_packetbuffer[0]);
+      DMSG("\n");
+      return false;
+  }
+
+  uint8_t length = status;
+  length -= 1;
+
+  if (length > *responseLength) {
+      length = *responseLength; // silent truncation...
+  }
+
+  for (uint8_t i = 0; i < length; i++) {
+    response[i] = response[i + 1];
+  }
+  *responseLength = length;
+
+  return true;
+}
+
 /**************************************************************************/
 /*!
     @brief  'InLists' a passive target. PN532 acting as reader/initiator,
diff --git a/PN532/PN532.h b/PN532/PN532.h
index 946b316..5d956fa 100644
--- a/PN532/PN532.h
+++ b/PN532/PN532.h
@@ -162,6 +162,7 @@ class PN532
     bool startPassiveTargetIDDetection(uint8_t cardbaudrate);
     bool readPassiveTargetID(uint8_t cardbaudrate, uint8_t *uid, uint8_t *uidLength, uint16_t timeout = 1000, bool inlist = false);
     bool inDataExchange(uint8_t *send, uint8_t sendLength, uint8_t *response, uint8_t *responseLength);
+    bool inCommunicateThru(uint8_t *send, uint8_t sendLength, uint8_t *response, uint8_t *responseLength);
 
     // Mifare Classic functions
     bool mifareclassic_IsFirstBlock (uint32_t uiBlock);