From c937e8d178ce08306b1546599bc8ba42143ee1da Mon Sep 17 00:00:00 2001
From: Philippe Teuwen <yobibe@gmail.com>
Date: Fri, 1 Oct 2010 17:42:16 +0000
Subject: [PATCH] pn53x: handle properly ReadRegister of PN533

---
 libnfc/chips/pn53x.c       | 19 +++++++++++++++----
 libnfc/drivers/pn53x_usb.c |  8 --------
 2 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/libnfc/chips/pn53x.c b/libnfc/chips/pn53x.c
index a1557ed..5da4916 100644
--- a/libnfc/chips/pn53x.c
+++ b/libnfc/chips/pn53x.c
@@ -164,13 +164,24 @@ pn53x_transceive (nfc_device_t * pnd, const byte_t * pbtTx, const size_t szTxLen
 bool
 pn53x_get_reg (nfc_device_t * pnd, uint16_t ui16Reg, uint8_t * ui8Value)
 {
-  size_t  szValueLen = 1;
+  size_t  szValueLen;
   byte_t  abtCmd[sizeof (pncmd_get_register)];
   memcpy (abtCmd, pncmd_get_register, sizeof (pncmd_get_register));
 
   abtCmd[2] = ui16Reg >> 8;
   abtCmd[3] = ui16Reg & 0xff;
-  return pn53x_transceive (pnd, abtCmd, 4, ui8Value, &szValueLen);
+  if (pn53x_transceive (pnd, abtCmd, sizeof (pncmd_get_register), ui8Value, &szValueLen)) {
+    if (pnd->nc == NC_PN533) {
+       // PN533 prepends its answer by a status byte
+       if (ui8Value[0] == 0) {
+         ui8Value[0] = ui8Value[1];
+       } else {
+         return false;
+       }
+    }
+    return true;
+  }
+  return false;
 }
 
 bool
@@ -186,7 +197,7 @@ pn53x_set_reg (nfc_device_t * pnd, uint16_t ui16Reg, uint8_t ui8SymbolMask, uint
     return false;
 
   abtCmd[4] = ui8Value | (ui8Current & (~ui8SymbolMask));
-  return pn53x_transceive (pnd, abtCmd, 5, NULL, NULL);
+  return pn53x_transceive (pnd, abtCmd, sizeof (pncmd_set_register), NULL, NULL);
 }
 
 bool
@@ -196,7 +207,7 @@ pn53x_set_parameters (nfc_device_t * pnd, uint8_t ui8Value)
   memcpy (abtCmd, pncmd_set_parameters, sizeof (pncmd_set_parameters));
 
   abtCmd[2] = ui8Value;
-  return pn53x_transceive (pnd, abtCmd, 3, NULL, NULL);
+  return pn53x_transceive (pnd, abtCmd, sizeof (pncmd_set_parameters), NULL, NULL);
 }
 
 bool
diff --git a/libnfc/drivers/pn53x_usb.c b/libnfc/drivers/pn53x_usb.c
index 857f88f..61751d2 100644
--- a/libnfc/drivers/pn53x_usb.c
+++ b/libnfc/drivers/pn53x_usb.c
@@ -367,14 +367,6 @@ pn53x_usb_transceive (nfc_device_t * pnd, const byte_t * pbtTx, const size_t szT
   // Remove the preceding and appending bytes 00 00 FF xx Fx .. .. .. xx 00 (x = variable)
   *pszRxLen = ret - 7 - 2;
 
-  // Get register: nuke extra byte (awful hack)
-  if ((abtRx[5] == 0xd5) && (abtRx[6] == 0x07) && (*pszRxLen == 2)) {
-    // DBG("awful hack: abtRx[7]=%02x, abtRx[8]=%02x, we only keep abtRx[8]=%02x", abtRx[7], abtRx[8], abtRx[8]);
-    *pszRxLen = (*pszRxLen) - 1;
-    memcpy (pbtRx, abtRx + 8, *pszRxLen);
-    return true;
-  }
-
   memcpy (pbtRx, abtRx + 7, *pszRxLen);
 
   return true;