am ae2234d1: add recovery mechanism on CORE_RESET_NTF

* commit 'ae2234d1cdd7c35543a795dcb51a6537ffce2007':
  add recovery mechanism on CORE_RESET_NTF

4 files changed, 126 insertions, 7 deletions

diff --git a/halimpl/pn54x/hal/phNxpNciHal.c b/halimpl/pn54x/hal/phNxpNciHal.c
index c32f33e..eb928a1 100644
--- a/halimpl/pn54x/hal/phNxpNciHal.c
+++ b/halimpl/pn54x/hal/phNxpNciHal.c
@@ -56,6 +56,8 @@ extern int send_to_upper_kovio;
 extern int kovio_detected;
 extern int disable_kovio;
 static uint8_t Rx_data[NCI_MAX_DATA_LEN];
+uint8_t discovery_cmd[50] = { 0 };
+uint8_t discovery_cmd_len = 0;
 extern bool_t rf_deactive_cmd;
 
 uint32_t timeoutTimerId = 0;
@@ -461,6 +463,9 @@ int phNxpNciHal_open(nfc_stack_callback_t *p_cback, nfc_stack_data_callback_t *p
     tOsalConfig.pLogFile = NULL;
     tTmlConfig.dwGetMsgThreadId = (uintptr_t) nxpncihal_ctrl.gDrvCfg.nClientId;
 
+    memset (discovery_cmd, 0, sizeof(discovery_cmd));
+    discovery_cmd_len = 0;
+
     /* Initialize TML layer */
     wConfigStatus = phTmlNfc_Init(&tTmlConfig);
     if (wConfigStatus != NFCSTATUS_SUCCESS)
@@ -2444,3 +2449,78 @@ static void phNxpNciHal_print_res_status( uint8_t *p_rx_data, uint16_t *p_len)
         }
     }
 }
+
+NFCSTATUS phNxpNciHal_core_reset_recovery ()
+{
+    NFCSTATUS status = NFCSTATUS_FAILED;
+
+    uint8_t buffer[260];
+    long bufflen = 260;
+
+    /*NCI_INIT_CMD*/
+    static uint8_t cmd_init_nci[] = {0x20, 0x01, 0x00};
+    /*NCI_RESET_CMD*/
+    static uint8_t cmd_reset_nci[] = {0x20, 0x00, 0x01, 0x00}; //keep configuration
+
+    /* reset config cache */
+    uint8_t retry_core_init_cnt = 0;
+
+    if (discovery_cmd_len == 0)
+    {
+        goto FAILURE;
+    }
+    NXPLOG_NCIHAL_D ("%s: recovery", __FUNCTION__);
+
+retry_core_init:
+    if (retry_core_init_cnt > 3)
+    {
+        goto FAILURE;
+    }
+
+    status = phTmlNfc_IoCtl (phTmlNfc_e_ResetDevice);
+    if (status != NFCSTATUS_SUCCESS)
+    {
+        NXPLOG_NCIHAL_D("PN54X Reset - FAILED\n");
+        goto FAILURE;
+    }
+    status = phNxpNciHal_send_ext_cmd (sizeof(cmd_reset_nci), cmd_reset_nci);
+    if ((status != NFCSTATUS_SUCCESS) && (nxpncihal_ctrl.retry_cnt >= MAX_RETRY_COUNT))
+    {
+        retry_core_init_cnt++;
+        goto retry_core_init;
+    }
+    else if (status != NFCSTATUS_SUCCESS)
+    {
+        NXPLOG_NCIHAL_D ("NCI_CORE_RESET: Failed");
+        retry_core_init_cnt++;
+        goto retry_core_init;
+    }
+    status = phNxpNciHal_send_ext_cmd (sizeof(cmd_init_nci), cmd_init_nci);
+    if (status != NFCSTATUS_SUCCESS)
+    {
+        NXPLOG_NCIHAL_D ("NCI_CORE_INIT : Failed");
+        retry_core_init_cnt++;
+        goto retry_core_init;
+    }
+
+    status = phNxpNciHal_send_ext_cmd (discovery_cmd_len, discovery_cmd);
+    if (status != NFCSTATUS_SUCCESS)
+    {
+        NXPLOG_NCIHAL_D ("RF_DISCOVERY : Failed");
+        retry_core_init_cnt++;
+        goto retry_core_init;
+    }
+    return NFCSTATUS_SUCCESS;
+FAILURE:
+    abort ();
+}
+
+void phNxpNciHal_discovery_cmd_ext (uint8_t *p_cmd_data, uint16_t cmd_len)
+{
+    NXPLOG_NCIHAL_D ("phNxpNciHal_discovery_cmd_ext");
+    if (cmd_len > 0 && cmd_len <= sizeof(discovery_cmd))
+    {
+        memcpy (discovery_cmd, p_cmd_data, cmd_len);
+        discovery_cmd_len = cmd_len;
+    }
+}
diff --git a/halimpl/pn54x/hal/phNxpNciHal.h b/halimpl/pn54x/hal/phNxpNciHal.h
index 6c97cd2..ba094c9 100644
--- a/halimpl/pn54x/hal/phNxpNciHal.h
+++ b/halimpl/pn54x/hal/phNxpNciHal.h
@@ -129,5 +129,7 @@ typedef struct phNxpNciProfile_Control
 void phNxpNciHal_request_control (void);
 void phNxpNciHal_release_control (void);
 int phNxpNciHal_write_unlocked (uint16_t data_len, const uint8_t *p_data);
+NFCSTATUS phNxpNciHal_core_reset_recovery ();
+void phNxpNciHal_discovery_cmd_ext (uint8_t *p_cmd_data, uint16_t cmd_len);
 
 #endif /* _PHNXPNCIHAL_H_ */
diff --git a/halimpl/pn54x/hal/phNxpNciHal_ext.c b/halimpl/pn54x/hal/phNxpNciHal_ext.c
index bcb82d2..2e28efb 100644
--- a/halimpl/pn54x/hal/phNxpNciHal_ext.c
+++ b/halimpl/pn54x/hal/phNxpNciHal_ext.c
@@ -36,6 +36,7 @@ extern int send_to_upper_kovio;
 extern uint32_t cleanup_timer;
 static uint8_t icode_detected = 0x00;
 uint8_t icode_send_eof = 0x00;
+uint8_t nfcdep_detected = 0x00;
 static uint8_t ee_disc_done = 0x00;
 uint8_t EnableP2P_PrioLogic = FALSE;
 static uint32_t RfDiscID = 1;
@@ -162,6 +163,11 @@ NFCSTATUS phNxpNciHal_process_ext_rsp (uint8_t *p_ntf, uint16_t *p_len)
     if (p_ntf[0] == 0x61 &&
             p_ntf[1] == 0x05)
     {
+        if (nfcdep_detected)
+        {
+            nfcdep_detected = 0x00;
+        }
+
         switch (p_ntf[4])
         {
         case 0x00:
@@ -175,6 +181,7 @@ NFCSTATUS phNxpNciHal_process_ext_rsp (uint8_t *p_ntf, uint16_t *p_len)
             break;
         case 0x03:
             NXPLOG_NCIHAL_D("NxpNci: RF Interface = NFC-DEP");
+            nfcdep_detected = 0x01;
             break;
         case 0x80:
             NXPLOG_NCIHAL_D("NxpNci: RF Interface = MIFARE");
@@ -386,8 +393,19 @@ NFCSTATUS phNxpNciHal_process_ext_rsp (uint8_t *p_ntf, uint16_t *p_len)
     }
     else if(p_ntf[0] == 0x60 && p_ntf[1] == 0x00)
     {
-        NXPLOG_NCIHAL_E("CORE_RESET_NTF received!");
-        phNxpNciHal_emergency_recovery();
+        NXPLOG_NCIHAL_E ("CORE_RESET_NTF received!");
+        if ( nfcdep_detected &&
+               !(p_ntf[2] == 0x06 && p_ntf[3] == 0xA0 && p_ntf[4] == 0x00
+                     && ((p_ntf[5] == 0xC9 && p_ntf[6] == 0x95
+                     && p_ntf[7] == 0x00 && p_ntf[8] == 0x00)
+                     || (p_ntf[5] == 0x07 && p_ntf[6] == 0x39
+                     && p_ntf[7] == 0xF2 && p_ntf[8] == 0x00)) ))
+        {
+            nfcdep_detected = 0x00;
+        }
+        phNxpNciHal_emergency_recovery ();
+        status = NFCSTATUS_FAILED;
+        return status;
     }
 #if(NFC_NXP_CHIP_TYPE == PN547C2)
     else if(p_ntf[0] == 0x61 && p_ntf[1] == 0x05
@@ -721,6 +739,8 @@ NFCSTATUS phNxpNciHal_write_ext(uint16_t *cmd_len, uint8_t *p_cmd_data,
         NXPLOG_NCIHAL_D ("> Polling Loop Started");
         icode_detected = 0;
         icode_send_eof = 0;
+        // Cache discovery cmd for recovery
+        phNxpNciHal_discovery_cmd_ext (p_cmd_data, *cmd_len);
     }
     //22000100
     else if (p_cmd_data[0] == 0x22 &&
diff --git a/halimpl/pn54x/utils/phNxpNciHal_utils.c b/halimpl/pn54x/utils/phNxpNciHal_utils.c
index 57dcc88..86be571 100644
--- a/halimpl/pn54x/utils/phNxpNciHal_utils.c
+++ b/halimpl/pn54x/utils/phNxpNciHal_utils.c
@@ -15,10 +15,16 @@
  *  limitations under the License.
  *
  ******************************************************************************/
-
-#include <phNxpNciHal_utils.h>
 #include <errno.h>
+#include <pthread.h>
+
 #include <phNxpLog.h>
+#include <phNxpNciHal.h>
+#include <phNxpNciHal_utils.h>
+
+extern uint8_t discovery_cmd[50];
+extern uint8_t discovery_cmd_len;
+extern uint8_t nfcdep_detected;
 
 /*********************** Link list functions **********************************/
 
@@ -499,8 +505,19 @@ void phNxpNciHal_print_packet(const char *pString, const uint8_t *p_data,
 **
 *******************************************************************************/
 
-void phNxpNciHal_emergency_recovery(void)
+void phNxpNciHal_emergency_recovery (void)
 {
-    NXPLOG_NCIHAL_E("%s: abort()", __FUNCTION__);
-    abort();
+    if (nfcdep_detected && discovery_cmd_len != 0)
+    {
+        pthread_t pthread;
+        pthread_attr_t attr;
+        pthread_attr_init (&attr);
+        pthread_attr_setdetachstate (&attr, PTHREAD_CREATE_DETACHED);
+        if (pthread_create (&pthread, &attr, phNxpNciHal_core_reset_recovery, NULL) == 0)
+        {
+            return;
+        }
+    }
+    NXPLOG_NCIHAL_E ("%s: abort()", __FUNCTION__);
+    abort ();
 }