From 851d035588f2eee3a846d15c2d9953f9c90f1e15 Mon Sep 17 00:00:00 2001
From: Romuald Conty <romuald@libnfc.org>
Date: Thu, 10 May 2012 19:37:55 +0000
Subject: [PATCH] drivers/pn532_uart: flush uart input on error to resync uart
 port

---
 libnfc/drivers/pn532_uart.c | 39 ++++++++++++++++++++-----------------
 1 file changed, 21 insertions(+), 18 deletions(-)

diff --git a/libnfc/drivers/pn532_uart.c b/libnfc/drivers/pn532_uart.c
index a3e5482..05bbb3d 100644
--- a/libnfc/drivers/pn532_uart.c
+++ b/libnfc/drivers/pn532_uart.c
@@ -367,16 +367,15 @@ pn532_uart_receive (nfc_device *pnd, uint8_t *pbtData, const size_t szDataLen, i
     return pn532_uart_ack (pnd);
   }
 
-  if (pnd->last_error != 0) {
-    log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Unable to receive data. (RX)");
-    return pnd->last_error;
+  if (pnd->last_error < 0) {
+    goto error;
   }
 
   const uint8_t pn53x_preamble[3] = { 0x00, 0x00, 0xff };
   if (0 != (memcmp (abtRxBuf, pn53x_preamble, 3))) {
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Frame preamble+start code mismatch");
     pnd->last_error = NFC_EIO;
-    return pnd->last_error;
+    goto error;
   }
 
   if ((0x01 == abtRxBuf[3]) && (0xff == abtRxBuf[4])) {
@@ -384,19 +383,20 @@ pn532_uart_receive (nfc_device *pnd, uint8_t *pbtData, const size_t szDataLen, i
     uart_receive (DRIVER_DATA (pnd)->port, abtRxBuf, 3, 0, timeout);
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Application level error detected");
     pnd->last_error = NFC_EIO;
-    return pnd->last_error;
+    goto error;
   } else if ((0xff == abtRxBuf[3]) && (0xff == abtRxBuf[4])) {
     // Extended frame
     pnd->last_error = uart_receive (DRIVER_DATA(pnd)->port, abtRxBuf, 3, 0, timeout);
-    if (pnd->last_error) return pnd->last_error;
-
+    if (pnd->last_error != 0) {
+      log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Unable to receive data. (RX)");
+      goto error;
+    }
     // (abtRxBuf[0] << 8) + abtRxBuf[1] (LEN) include TFI + (CC+1)
     len = (abtRxBuf[0] << 8) + abtRxBuf[1] - 2;
     if (((abtRxBuf[0] + abtRxBuf[1] + abtRxBuf[2]) % 256) != 0) {
-      // TODO: Retry
       log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Length checksum mismatch");
       pnd->last_error = NFC_EIO;
-      return pnd->last_error;
+      goto error;
     }
   } else {
     // Normal frame
@@ -404,7 +404,7 @@ pn532_uart_receive (nfc_device *pnd, uint8_t *pbtData, const size_t szDataLen, i
       // TODO: Retry
       log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Length checksum mismatch");
       pnd->last_error = NFC_EIO;
-      return pnd->last_error;
+      goto error;
     }
 
     // abtRxBuf[3] (LEN) include TFI + (CC+1)
@@ -414,40 +414,40 @@ pn532_uart_receive (nfc_device *pnd, uint8_t *pbtData, const size_t szDataLen, i
   if (len > szDataLen) {
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "Unable to receive data: buffer too small. (szDataLen: %zu, len: %zu)", szDataLen, len);
     pnd->last_error = NFC_EIO;
-    return pnd->last_error;
+    goto error;
   }
 
   // TFI + PD0 (CC+1)
   pnd->last_error = uart_receive (DRIVER_DATA (pnd)->port, abtRxBuf, 2, 0, timeout);
   if (pnd->last_error != 0) {
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Unable to receive data. (RX)");
-    return pnd->last_error;
+    goto error;
   }
 
   if (abtRxBuf[0] != 0xD5) {
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "TFI Mismatch");
     pnd->last_error = NFC_EIO;
-    return pnd->last_error;
+    goto error;
   }
 
   if (abtRxBuf[1] != CHIP_DATA (pnd)->last_command + 1) {
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Command Code verification failed");
     pnd->last_error = NFC_EIO;
-    return pnd->last_error;
+    goto error;
   }
 
   if (len) {
     pnd->last_error = uart_receive (DRIVER_DATA (pnd)->port, pbtData, len, 0, timeout);
     if (pnd->last_error != 0) {
       log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Unable to receive data. (RX)");
-      return pnd->last_error;
+      goto error;
     }
   }
 
   pnd->last_error = uart_receive (DRIVER_DATA (pnd)->port, abtRxBuf, 2, 0, timeout);
   if (pnd->last_error != 0) {
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Unable to receive data. (RX)");
-    return pnd->last_error;
+    goto error;
   }
 
   uint8_t btDCS = (256 - 0xD5);
@@ -459,16 +459,19 @@ pn532_uart_receive (nfc_device *pnd, uint8_t *pbtData, const size_t szDataLen, i
   if (btDCS != abtRxBuf[0]) {
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Data checksum mismatch");
     pnd->last_error = NFC_EIO;
-    return pnd->last_error;
+    goto error;
   }
 
   if (0x00 != abtRxBuf[1]) {
     log_put (LOG_CATEGORY, NFC_PRIORITY_ERROR, "%s", "Frame postamble mismatch");
     pnd->last_error = NFC_EIO;
-    return pnd->last_error;
+    goto error;
   }
   // The PN53x command is done and we successfully received the reply
   return len;
+error:
+  uart_flush_input (DRIVER_DATA (pnd)->port);
+  return pnd->last_error;
 }
 
 int