cleaning up endless copy-paste of trace functionality

diff --git a/armsrc/iso14443a.c b/armsrc/iso14443a.c
index dbada07678f727f3ae801a200ee7dfe6c1544923..a02d7d42d4f7cfeb5264789205607303f00cbef1 100644 (file)
--- a/armsrc/iso14443a.c
+++ b/armsrc/iso14443a.c
@@ -7,6 +7,10 @@
 #include "apps.h"\r
 #include "../common/iso14443_crc.c"\r
 \r
+static BYTE *trace = (BYTE *) BigBuf;\r
+static int traceLen = 0;\r
+static int rsamples = 0;\r
+\r
 typedef enum {\r
        SEC_D = 1,\r
        SEC_E = 2,\r
@@ -16,6 +20,42 @@ typedef enum {
        SEC_Z = 6\r
 } SecType;\r
 \r
+static const BYTE OddByteParity[256] = {\r
+  1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,\r
+  0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,\r
+  0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,\r
+  1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,\r
+  0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,\r
+  1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,\r
+  1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,\r
+  0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,\r
+  0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,\r
+  1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,\r
+  1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,\r
+  0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,\r
+  1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,\r
+  0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,\r
+  0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,\r
+  1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1\r
+};\r
+\r
+//-----------------------------------------------------------------------------\r
+// Generate the parity value for a byte sequence\r
+// \r
+//-----------------------------------------------------------------------------\r
+DWORD GetParity(const BYTE * pbtCmd, int iLen)\r
+{\r
+  int i;\r
+  DWORD dwPar = 0;\r
+  \r
+  // Generate the encrypted data\r
+  for (i = 0; i < iLen; i++) {\r
+    // Save the encrypted parity bit\r
+    dwPar |= ((OddByteParity[pbtCmd[i]]) << i);\r
+  }\r
+  return dwPar;\r
+}\r
+\r
 //-----------------------------------------------------------------------------\r
 // The software UART that receives commands from the reader, and its state\r
 // variables.\r
@@ -538,8 +578,8 @@ void SnoopIso14443a(void)
 \r
     // As we receive stuff, we copy it from receivedCmd or receivedResponse\r
     // into trace, along with its length and other annotations.\r
-    BYTE *trace = (BYTE *)BigBuf;\r
-    int traceLen = 0;\r
+    //BYTE *trace = (BYTE *)BigBuf;\r
+    //int traceLen = 0;\r
 \r
     // The DMA buffer, used to stream samples from the FPGA\r
     SBYTE *dmaBuf = ((SBYTE *)BigBuf) + DMA_BUFFER_OFFSET;\r
@@ -834,6 +874,27 @@ static void CodeStrangeAnswer()
     //ToSendMax += 2;\r
 }\r
 \r
+int LogTrace(const BYTE * btBytes, int iLen, int iSamples, DWORD dwParity, BOOL bReader)\r
+{\r
+  // Trace the random, i'm curious\r
+  rsamples += iSamples;\r
+  trace[traceLen++] = ((rsamples >> 0) & 0xff);\r
+  trace[traceLen++] = ((rsamples >> 8) & 0xff);\r
+  trace[traceLen++] = ((rsamples >> 16) & 0xff);\r
+  trace[traceLen++] = ((rsamples >> 24) & 0xff);\r
+  if (!bReader) {\r
+    trace[traceLen - 1] |= 0x80;\r
+  }\r
+  trace[traceLen++] = ((dwParity >> 0) & 0xff);\r
+  trace[traceLen++] = ((dwParity >> 8) & 0xff);\r
+  trace[traceLen++] = ((dwParity >> 16) & 0xff);\r
+  trace[traceLen++] = ((dwParity >> 24) & 0xff);\r
+  trace[traceLen++] = iLen;\r
+  memcpy(trace + traceLen, btBytes, iLen);\r
+  traceLen += iLen;\r
+  return (traceLen < TRACE_LENGTH);\r
+}\r
+\r
 //-----------------------------------------------------------------------------\r
 // Wait for commands from reader\r
 // Stop when button is pressed\r
@@ -1473,6 +1534,8 @@ static BOOL GetIso14443aAnswerFromTag(BYTE *receivedResponse, int maxLen, int *s
     }\r
 }\r
 \r
+\r
+\r
 //-----------------------------------------------------------------------------\r
 // Read an ISO 14443a tag. Send out commands and store answers.\r
 //\r
@@ -1531,9 +1594,10 @@ void ReaderIso14443a(DWORD parameter)
 \r
        BYTE *receivedAnswer = (((BYTE *)BigBuf) + 3560);       // was 3560 - tied to other size changes\r
 \r
-       BYTE *trace = (BYTE *)BigBuf;\r
-       int traceLen = 0;\r
-       int rsamples = 0;\r
+       //BYTE *trace = (BYTE *)BigBuf;\r
+       //int traceLen = 0;\r
+       //int rsamples = 0;\r
+  traceLen = 0;\r
 \r
        memset(trace, 0x44, 2000);                              // was 2000 - tied to oter size chnages\r
        // setting it to 3000 causes no tag responses to be detected (2900 is ok)\r
@@ -1588,229 +1652,105 @@ void ReaderIso14443a(DWORD parameter)
        int wait = 0;\r
        int elapsed = 0;\r
 \r
-       for(;;) {\r
+       while(1) {\r
                // Send WUPA (or REQA)\r
                TransmitFor14443a(req1, req1Len, &tsamples, &wait);\r
-               // Store answer in buffer\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 1;\r
-               memcpy(trace+traceLen, cmd1, 1);\r
-               traceLen += 1;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-\r
-               while(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) {\r
-                       if(BUTTON_PRESS()) goto done;\r
-\r
-                       // No answer, just continue polling\r
-                       TransmitFor14443a(req1, req1Len, &tsamples, &wait);\r
-                       // Store answer in buffer\r
-                       trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-                       trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-                       trace[traceLen++] = 1;\r
-                       memcpy(trace+traceLen, cmd1, 1);\r
-                       traceLen += 1;\r
-                       if(traceLen > TRACE_LENGTH) goto done;\r
-               }\r
 \r
-               // Store answer in buffer\r
-               rsamples = rsamples + (samples - Demod.samples);\r
-               trace[traceLen++] = ((rsamples >>  0) & 0xff);\r
-               trace[traceLen++] = ((rsamples >>  8) & 0xff);\r
-               trace[traceLen++] = ((rsamples >> 16) & 0xff);\r
-               trace[traceLen++] = 0x80 | ((rsamples >> 24) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  0) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  8) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 16) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 24) & 0xff);\r
-               trace[traceLen++] = Demod.len;\r
-               memcpy(trace+traceLen, receivedAnswer, Demod.len);\r
-               traceLen += Demod.len;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-\r
-               // Ask for card UID\r
-               TransmitFor14443a(req2, req2Len, &tsamples, &wait);\r
-               // Store answer in buffer\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 2;\r
-               memcpy(trace+traceLen, cmd2, 2);\r
-               traceLen += 2;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-\r
-               if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) {\r
-                       continue;\r
-               }\r
+    // Store reader command in buffer\r
+    if (!LogTrace(cmd1,1,0,GetParity(cmd1,1),TRUE)) break;\r
+    \r
+    // Test if the action was cancelled\r
+    if(BUTTON_PRESS()) {\r
+      break;\r
+    }\r
+    \r
+               if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) continue;\r
+    \r
+    // Log the ATQA\r
+    if (!LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE)) break;\r
 \r
-               // Store answer in buffer\r
-               rsamples = rsamples + (samples - Demod.samples);\r
-               trace[traceLen++] = ((rsamples >>  0) & 0xff);\r
-               trace[traceLen++] = ((rsamples >>  8) & 0xff);\r
-               trace[traceLen++] = ((rsamples >> 16) & 0xff);\r
-               trace[traceLen++] = 0x80 | ((rsamples >> 24) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  0) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  8) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 16) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 24) & 0xff);\r
-               trace[traceLen++] = Demod.len;\r
-               memcpy(trace+traceLen, receivedAnswer, Demod.len);\r
-               traceLen += Demod.len;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
+    // Store reader command in buffer\r
+    if (!LogTrace(cmd2,2,0,GetParity(cmd2,2),TRUE)) break;\r
+    TransmitFor14443a(req2, req2Len, &samples, &wait);\r
 \r
+               if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) continue;\r
+\r
+    // Log the uid\r
+    if (!LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE)) break;\r
+    \r
                // Construct SELECT UID command\r
                // First copy the 5 bytes (Mifare Classic) after the 93 70\r
                memcpy(cmd3+2,receivedAnswer,5);\r
                // Secondly compute the two CRC bytes at the end\r
                ComputeCrc14443(CRC_14443_A, cmd3, 7, &cmd3[7], &cmd3[8]);\r
-               // Prepare the bit sequence to modulate the subcarrier\r
-               // Store answer in buffer\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 9;\r
-               memcpy(trace+traceLen, cmd3, 9);\r
-               traceLen += 9;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
+\r
+               // Store reader command in buffer\r
+    if (!LogTrace(cmd3,9,0,GetParity(cmd5,9),TRUE)) break;\r
+               \r
                CodeIso14443aAsReader(cmd3, sizeof(cmd3));\r
                memcpy(req3, ToSend, ToSendMax); req3Len = ToSendMax;\r
 \r
                // Select the card\r
                TransmitFor14443a(req3, req3Len, &samples, &wait);\r
-               if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) {\r
-                       continue;\r
-               }\r
+               if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) continue;\r
 \r
-               // Store answer in buffer\r
-               rsamples = rsamples + (samples - Demod.samples);\r
-               trace[traceLen++] = ((rsamples >>  0) & 0xff);\r
-               trace[traceLen++] = ((rsamples >>  8) & 0xff);\r
-               trace[traceLen++] = ((rsamples >> 16) & 0xff);\r
-               trace[traceLen++] = 0x80 | ((rsamples >> 24) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  0) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  8) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 16) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 24) & 0xff);\r
-               trace[traceLen++] = Demod.len;\r
-               memcpy(trace+traceLen, receivedAnswer, Demod.len);\r
-               traceLen += Demod.len;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-\r
-// OK we have selected at least at cascade 1, lets see if first byte of UID was 0x88 in\r
-// which case we need to make a cascade 2 request and select - this is a long UID\r
+    // Log the SAK\r
+    if (!LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE)) break;\r
+\r
+    // OK we have selected at least at cascade 1, lets see if first byte of UID was 0x88 in\r
+    // which case we need to make a cascade 2 request and select - this is a long UID\r
                if (receivedAnswer[0] == 0x88)\r
                {\r
-               // Do cascade level 2 stuff\r
-               ///////////////////////////////////////////////////////////////////\r
-               // First issue a '95 20' identify request\r
-               // Ask for card UID (part 2)\r
-               TransmitFor14443a(req4, req4Len, &tsamples, &wait);\r
-               // Store answer in buffer\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 2;\r
-               memcpy(trace+traceLen, cmd4, 2);\r
-               traceLen += 2;\r
-               if(traceLen > TRACE_LENGTH) {\r
-               DbpString("Bugging out, just popped tracelength");\r
-               goto done;}\r
-\r
-               if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) {\r
-                       continue;\r
-               }\r
-               // Store answer in buffer\r
-               rsamples = rsamples + (samples - Demod.samples);\r
-               trace[traceLen++] = ((rsamples >>  0) & 0xff);\r
-               trace[traceLen++] = ((rsamples >>  8) & 0xff);\r
-               trace[traceLen++] = ((rsamples >> 16) & 0xff);\r
-               trace[traceLen++] = 0x80 | ((rsamples >> 24) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  0) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  8) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 16) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 24) & 0xff);\r
-               trace[traceLen++] = Demod.len;\r
-               memcpy(trace+traceLen, receivedAnswer, Demod.len);\r
-               traceLen += Demod.len;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-               //////////////////////////////////////////////////////////////////\r
-               // Then Construct SELECT UID (cascasde 2) command\r
-               DbpString("Just about to copy the UID out of the cascade 2 id req");\r
-               // First copy the 5 bytes (Mifare Classic) after the 95 70\r
-               memcpy(cmd5+2,receivedAnswer,5);\r
-               // Secondly compute the two CRC bytes at the end\r
-               ComputeCrc14443(CRC_14443_A, cmd4, 7, &cmd5[7], &cmd5[8]);\r
-               // Prepare the bit sequence to modulate the subcarrier\r
-               // Store answer in buffer\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 9;\r
-               memcpy(trace+traceLen, cmd5, 9);\r
-               traceLen += 9;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-               CodeIso14443aAsReader(cmd5, sizeof(cmd5));\r
-               memcpy(req5, ToSend, ToSendMax); req5Len = ToSendMax;\r
-\r
-               // Select the card\r
-               TransmitFor14443a(req4, req4Len, &samples, &wait);\r
-               if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) {\r
-                       continue;\r
-               }\r
-\r
-               // Store answer in buffer\r
-               rsamples = rsamples + (samples - Demod.samples);\r
-               trace[traceLen++] = ((rsamples >>  0) & 0xff);\r
-               trace[traceLen++] = ((rsamples >>  8) & 0xff);\r
-               trace[traceLen++] = ((rsamples >> 16) & 0xff);\r
-               trace[traceLen++] = 0x80 | ((rsamples >> 24) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  0) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  8) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 16) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 24) & 0xff);\r
-               trace[traceLen++] = Demod.len;\r
-               memcpy(trace+traceLen, receivedAnswer, Demod.len);\r
-               traceLen += Demod.len;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-\r
+      // Do cascade level 2 stuff\r
+      ///////////////////////////////////////////////////////////////////\r
+      // First issue a '95 20' identify request\r
+      // Ask for card UID (part 2)\r
+      TransmitFor14443a(req4, req4Len, &tsamples, &wait);\r
+\r
+      // Store reader command in buffer\r
+      if (!LogTrace(cmd4,2,0,GetParity(cmd4,2),TRUE)) break;\r
+\r
+      if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) continue;\r
+\r
+      //////////////////////////////////////////////////////////////////\r
+      // Then Construct SELECT UID (cascasde 2) command\r
+      DbpString("Just about to copy the UID out of the cascade 2 id req");\r
+      // First copy the 5 bytes (Mifare Classic) after the 95 70\r
+      memcpy(cmd5+2,receivedAnswer,5);\r
+      // Secondly compute the two CRC bytes at the end\r
+      ComputeCrc14443(CRC_14443_A, cmd4, 7, &cmd5[7], &cmd5[8]);\r
+\r
+      // Store reader command in buffer\r
+      if (!LogTrace(cmd5,9,0,GetParity(cmd5,9),TRUE)) break;\r
+\r
+      CodeIso14443aAsReader(cmd5, sizeof(cmd5));\r
+      memcpy(req5, ToSend, ToSendMax); req5Len = ToSendMax;\r
+      \r
+      // Select the card\r
+      TransmitFor14443a(req4, req4Len, &samples, &wait);\r
+      if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) continue;\r
+      \r
+      // Log the SAK\r
+      if (!LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE)) break;\r
                }\r
 \r
                // Secondly compute the two CRC bytes at the end\r
                ComputeCrc14443(CRC_14443_A, cmd7, 2, &cmd7[2], &cmd7[3]);\r
                CodeIso14443aAsReader(cmd7, sizeof(cmd7));\r
                memcpy(req7, ToSend, ToSendMax); req7Len = ToSendMax;\r
+\r
                // Send authentication request (Mifare Classic)\r
                TransmitFor14443a(req7, req7Len, &samples, &wait);\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0; trace[traceLen++] = 0;\r
-               trace[traceLen++] = 4;\r
-               memcpy(trace+traceLen, cmd7, 4);\r
-               traceLen += 4;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-               if(GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) {\r
-                       rsamples++;\r
-                       // We received probably a random, continue and trace!\r
-               }\r
-               else {\r
-                       // Received nothing\r
-                       continue;\r
-               }\r
+    // Store reader command in buffer\r
+    if (!LogTrace(cmd7,4,0,GetParity(cmd7,4),TRUE)) break;\r
 \r
-               // Trace the random, i'm curious\r
-               rsamples = rsamples + (samples - Demod.samples);\r
-               trace[traceLen++] = ((rsamples >>  0) & 0xff);\r
-               trace[traceLen++] = ((rsamples >>  8) & 0xff);\r
-               trace[traceLen++] = ((rsamples >> 16) & 0xff);\r
-               trace[traceLen++] = 0x80 | ((rsamples >> 24) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  0) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >>  8) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 16) & 0xff);\r
-               trace[traceLen++] = ((Demod.parityBits >> 24) & 0xff);\r
-               trace[traceLen++] = Demod.len;\r
-               memcpy(trace+traceLen, receivedAnswer, Demod.len);\r
-               traceLen += Demod.len;\r
-               if(traceLen > TRACE_LENGTH) goto done;\r
-\r
-               // Thats it...\r
+               if(!GetIso14443aAnswerFromTag(receivedAnswer, 100, &samples, &elapsed)) continue;\r
+\r
+    // We received probably a random, continue and trace!\r
+    if (!LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE)) break;\r
        }\r
 \r
-done:\r
+  // Thats it...\r
        FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);\r
        LEDsoff();\r
        DbpIntegers(rsamples, 0xCC, 0xCC);\r