1. fixed send manchester

2. emulator commands select, authenticate, read block, write block works
3. nested authentication - not working (maybe next release)
4. small bugfixes
5. mifare1ksim - in alpha state!!! code not so clear!!!

diff --git a/armsrc/iso14443a.c b/armsrc/iso14443a.c
index cc63093eb75f3872bd3f2bda4eccae7e3fe07534..ce8467bfe37ed1bf78d7136a03002a90360eb03f 100644 (file)
--- a/armsrc/iso14443a.c
+++ b/armsrc/iso14443a.c
@@ -26,11 +26,11 @@ static int rsamples = 0;
 static int tracing = TRUE;
 static uint32_t iso14a_timeout;
 
 static int tracing = TRUE;
 static uint32_t iso14a_timeout;
 

-// CARD TO READER
+// CARD TO READER - manchester

 // Sequence D: 11110000 modulation with subcarrier during first half
 // Sequence E: 00001111 modulation with subcarrier during second half
 // Sequence F: 00000000 no modulation with subcarrier
 // Sequence D: 11110000 modulation with subcarrier during first half
 // Sequence E: 00001111 modulation with subcarrier during second half
 // Sequence F: 00000000 no modulation with subcarrier

-// READER TO CARD
+// READER TO CARD - miller

 // Sequence X: 00001100 drop after half a period
 // Sequence Y: 00000000 no drop
 // Sequence Z: 11000000 drop at start
 // Sequence X: 00001100 drop after half a period
 // Sequence Y: 00000000 no drop
 // Sequence Z: 11000000 drop at start


@@ -60,16 +60,6 @@ static const uint8_t OddByteParity[256] = {
   1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1
 };
 
   1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1
 };
 

-// BIG CHANGE - UNDERSTAND THIS BEFORE WE COMMIT
-#define RECV_CMD_OFFSET   3032
-#define RECV_RES_OFFSET   3096
-#define DMA_BUFFER_OFFSET 3160
-#define DMA_BUFFER_SIZE   4096
-#define TRACE_LENGTH      3000
-// card emulator memory
-#define CARD_MEMORY       7260
-#define CARD_MEMORY_LEN   1024
-

 uint8_t trigger = 0;
 void iso14a_set_trigger(int enable) {
        trigger = enable;
 uint8_t trigger = 0;
 void iso14a_set_trigger(int enable) {
        trigger = enable;


@@ -787,12 +777,12 @@ done:
 //-----------------------------------------------------------------------------
 // Prepare tag messages
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 // Prepare tag messages
 //-----------------------------------------------------------------------------

-static void CodeIso14443aAsTag(const uint8_t *cmd, int len)
+static void CodeIso14443aAsTagPar(const uint8_t *cmd, int len, uint32_t dwParity)

 {
 {

-    int i;
-       int oddparity;
+       int i;
+//     int oddparity;

 
 

-    ToSendReset();
+       ToSendReset();

 
        // Correction bit, might be removed when not needed
        ToSendStuffBit(0);
 
        // Correction bit, might be removed when not needed
        ToSendStuffBit(0);


@@ -803,55 +793,68 @@ static void CodeIso14443aAsTag(const uint8_t *cmd, int len)
        ToSendStuffBit(0);
        ToSendStuffBit(0);
        ToSendStuffBit(0);
        ToSendStuffBit(0);
        ToSendStuffBit(0);
        ToSendStuffBit(0);

-
+       

        // Send startbit
        ToSend[++ToSendMax] = SEC_D;
 
        // Send startbit
        ToSend[++ToSendMax] = SEC_D;
 

-    for(i = 0; i < len; i++) {
-        int j;
-        uint8_t b = cmd[i];
+       for(i = 0; i < len; i++) {
+               int j;
+               uint8_t b = cmd[i];

 
                // Data bits
 
                // Data bits

-        oddparity = 0x01;
+//             oddparity = 0x01;

                for(j = 0; j < 8; j++) {
                for(j = 0; j < 8; j++) {

-            oddparity ^= (b & 1);
+//                     oddparity ^= (b & 1);

                        if(b & 1) {
                                ToSend[++ToSendMax] = SEC_D;
                        } else {
                                ToSend[++ToSendMax] = SEC_E;
                        if(b & 1) {
                                ToSend[++ToSendMax] = SEC_D;
                        } else {
                                ToSend[++ToSendMax] = SEC_E;

-            }
-            b >>= 1;
-        }
+                       }
+                       b >>= 1;
+               }

 
 

-        // Parity bit
-        if(oddparity) {
-               ToSend[++ToSendMax] = SEC_D;
+       // Get the parity bit
+               if ((dwParity >> i) & 0x01) {
+                       ToSend[++ToSendMax] = SEC_D;

                } else {
                        ToSend[++ToSendMax] = SEC_E;
                }
                } else {
                        ToSend[++ToSendMax] = SEC_E;
                }

-    }
+               
+                       // Parity bit
+//                     if(oddparity) {
+//                             ToSend[++ToSendMax] = SEC_D;
+//                     } else {
+//                             ToSend[++ToSendMax] = SEC_E;
+//                     }
+
+//             if (oddparity != ((dwParity >> i) & 0x01))
+//               Dbprintf("par error. i=%d", i);
+       }

 
 

-    // Send stopbit
-    ToSend[++ToSendMax] = SEC_F;
+       // Send stopbit
+       ToSend[++ToSendMax] = SEC_F;

 
        // Flush the buffer in FPGA!!
        for(i = 0; i < 5; i++) {
                ToSend[++ToSendMax] = SEC_F;
        }
 
 
        // Flush the buffer in FPGA!!
        for(i = 0; i < 5; i++) {
                ToSend[++ToSendMax] = SEC_F;
        }
 

-    // Convert from last byte pos to length
-    ToSendMax++;
+       // Convert from last byte pos to length
+       ToSendMax++;

 
     // Add a few more for slop
 
     // Add a few more for slop

-    ToSend[ToSendMax++] = 0x00;
-       ToSend[ToSendMax++] = 0x00;
-    //ToSendMax += 2;
+//    ToSend[ToSendMax++] = 0x00;
+//     ToSend[ToSendMax++] = 0x00;
+}
+
+static void CodeIso14443aAsTag(const uint8_t *cmd, int len){
+       CodeIso14443aAsTagPar(cmd, len, GetParity(cmd, len));

 }
 
 //-----------------------------------------------------------------------------
 // This is to send a NACK kind of answer, its only 3 bits, I know it should be 4
 //-----------------------------------------------------------------------------
 }
 
 //-----------------------------------------------------------------------------
 // This is to send a NACK kind of answer, its only 3 bits, I know it should be 4
 //-----------------------------------------------------------------------------

-static void CodeStrangeAnswer()
+static void CodeStrangeAnswerAsTag()

 {
        int i;
 
 {
        int i;
 


@@ -889,11 +892,47 @@ static void CodeStrangeAnswer()
 
     // Convert from last byte pos to length
     ToSendMax++;
 
     // Convert from last byte pos to length
     ToSendMax++;

+}

 
 

-    // Add a few more for slop
-    ToSend[ToSendMax++] = 0x00;
-       ToSend[ToSendMax++] = 0x00;
-    //ToSendMax += 2;
+static void Code4bitAnswerAsTag(uint8_t cmd)
+{
+       int i;
+
+    ToSendReset();
+
+       // Correction bit, might be removed when not needed
+       ToSendStuffBit(0);
+       ToSendStuffBit(0);
+       ToSendStuffBit(0);
+       ToSendStuffBit(0);
+       ToSendStuffBit(1);  // 1
+       ToSendStuffBit(0);
+       ToSendStuffBit(0);
+       ToSendStuffBit(0);
+
+       // Send startbit
+       ToSend[++ToSendMax] = SEC_D;
+
+       uint8_t b = cmd;
+       for(i = 0; i < 4; i++) {
+               if(b & 1) {
+                       ToSend[++ToSendMax] = SEC_D;
+               } else {
+                       ToSend[++ToSendMax] = SEC_E;
+               }
+               b >>= 1;
+       }
+
+       // Send stopbit
+       ToSend[++ToSendMax] = SEC_F;
+
+       // Flush the buffer in FPGA!!
+       for(i = 0; i < 5; i++) {
+               ToSend[++ToSendMax] = SEC_F;
+       }
+
+    // Convert from last byte pos to length
+    ToSendMax++;

 }
 
 //-----------------------------------------------------------------------------
 }
 
 //-----------------------------------------------------------------------------


@@ -1066,7 +1105,7 @@ ComputeCrc14443(CRC_14443_A, response3a, 1, &response3a[1], &response3a[2]);
     memcpy(resp3a, ToSend, ToSendMax); resp3aLen = ToSendMax;
 
        // Strange answer is an example of rare message size (3 bits)
     memcpy(resp3a, ToSend, ToSendMax); resp3aLen = ToSendMax;
 
        // Strange answer is an example of rare message size (3 bits)

-       CodeStrangeAnswer();
+       CodeStrangeAnswerAsTag();

        memcpy(resp4, ToSend, ToSendMax); resp4Len = ToSendMax;
 
        // Authentication answer (random nonce)
        memcpy(resp4, ToSend, ToSendMax); resp4Len = ToSendMax;
 
        // Authentication answer (random nonce)


@@ -1472,12 +1511,12 @@ static int EmGetCmd(uint8_t *received, int *len, int maxLen)
                        volatile uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
                        if(MillerDecoding((b & 0xf0) >> 4)) {
                                *len = Uart.byteCnt;
                        volatile uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
                        if(MillerDecoding((b & 0xf0) >> 4)) {
                                *len = Uart.byteCnt;

-                               if (tracing) LogTrace(received, *len, 0, GetParity(received, *len), TRUE);
+                               if (tracing) LogTrace(received, *len, GetDeltaCountUS(), Uart.parityBits, TRUE);

                                return 0;
                        }
                        if(MillerDecoding(b & 0x0f)) {
                                *len = Uart.byteCnt;
                                return 0;
                        }
                        if(MillerDecoding(b & 0x0f)) {
                                *len = Uart.byteCnt;

-                               if (tracing) LogTrace(received, *len, 0, GetParity(received, *len), TRUE);
+                               if (tracing) LogTrace(received, *len, GetDeltaCountUS(), Uart.parityBits, TRUE);

                                return 0;
                        }
                }
                                return 0;
                        }
                }


@@ -1509,7 +1548,7 @@ static int EmSendCmd14443aRaw(uint8_t *resp, int respLen, int correctionNeeded)
                }
                if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
                        if(i > respLen) {
                }
                if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
                        if(i > respLen) {

-                               b = 0x00;
+                               b = 0xff; // was 0x00

                                u++;
                        } else {
                                b = resp[i];
                                u++;
                        } else {
                                b = resp[i];


@@ -1527,15 +1566,34 @@ static int EmSendCmd14443aRaw(uint8_t *resp, int respLen, int correctionNeeded)
        return 0;
 }
 
        return 0;
 }
 

-static int EmSendCmdEx(uint8_t *resp, int respLen, int correctionNeeded){
-  CodeIso14443aAsTag(resp, respLen);
+int EmSend4bitEx(uint8_t resp, int correctionNeeded){
+  Code4bitAnswerAsTag(resp);

        int res = EmSendCmd14443aRaw(ToSend, ToSendMax, correctionNeeded);
        int res = EmSendCmd14443aRaw(ToSend, ToSendMax, correctionNeeded);

-  if (tracing) LogTrace(resp, respLen, 0, GetParity(resp, respLen), FALSE);
+  if (tracing) LogTrace(&resp, 1, GetDeltaCountUS(), GetParity(&resp, 1), FALSE);

        return res;
 }
 
        return res;
 }
 

-static int EmSendCmd(uint8_t *resp, int respLen){
-       return EmSendCmdEx(resp, respLen, 0);
+int EmSend4bit(uint8_t resp){
+       return EmSend4bitEx(resp, 0);
+}
+
+int EmSendCmdExPar(uint8_t *resp, int respLen, int correctionNeeded, uint32_t par){
+  CodeIso14443aAsTagPar(resp, respLen, par);
+       int res = EmSendCmd14443aRaw(ToSend, ToSendMax, correctionNeeded);
+  if (tracing) LogTrace(resp, respLen, GetDeltaCountUS(), par, FALSE);
+       return res;
+}
+
+int EmSendCmdEx(uint8_t *resp, int respLen, int correctionNeeded){
+       return EmSendCmdExPar(resp, respLen, correctionNeeded, GetParity(resp, respLen));
+}
+
+int EmSendCmd(uint8_t *resp, int respLen){
+       return EmSendCmdExPar(resp, respLen, 0, GetParity(resp, respLen));
+}
+
+int EmSendCmdPar(uint8_t *resp, int respLen, uint32_t par){
+       return EmSendCmdExPar(resp, respLen, 0, par);

 }
 
 //-----------------------------------------------------------------------------
 }
 
 //-----------------------------------------------------------------------------


@@ -1993,10 +2051,7 @@ void MifareReadBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
        if (MF_DBGLEVEL >= 2)   DbpString("READ BLOCK FINISHED");
 
        // add trace trailer
        if (MF_DBGLEVEL >= 2)   DbpString("READ BLOCK FINISHED");
 
        // add trace trailer

-       uid[0] = 0xff;
-       uid[1] = 0xff;
-       uid[2] = 0xff;
-       uid[3] = 0xff;
+       memset(uid, 0x44, 4);

        LogTrace(uid, 4, 0, 0, TRUE);
 
        UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
        LogTrace(uid, 4, 0, 0, TRUE);
 
        UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};


@@ -2088,10 +2143,7 @@ void MifareReadSector(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
        if (MF_DBGLEVEL >= 2) DbpString("READ SECTOR FINISHED");
 
        // add trace trailer
        if (MF_DBGLEVEL >= 2) DbpString("READ SECTOR FINISHED");
 
        // add trace trailer

-       uid[0] = 0xff;
-       uid[1] = 0xff;
-       uid[2] = 0xff;
-       uid[3] = 0xff;
+       memset(uid, 0x44, 4);

        LogTrace(uid, 4, 0, 0, TRUE);
 
        UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
        LogTrace(uid, 4, 0, 0, TRUE);
 
        UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};


@@ -2177,10 +2229,7 @@ void MifareWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
        if (MF_DBGLEVEL >= 2)   DbpString("WRITE BLOCK FINISHED");
 
        // add trace trailer
        if (MF_DBGLEVEL >= 2)   DbpString("WRITE BLOCK FINISHED");
 
        // add trace trailer

-       uid[0] = 0xff;
-       uid[1] = 0xff;
-       uid[2] = 0xff;
-       uid[3] = 0xff;
+       memset(uid, 0x44, 4);

        LogTrace(uid, 4, 0, 0, TRUE);
 
        UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
        LogTrace(uid, 4, 0, 0, TRUE);
 
        UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};


@@ -2392,10 +2441,7 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain)
        crypto1_destroy(pcs);
        
        // add trace trailer
        crypto1_destroy(pcs);
        
        // add trace trailer

-       uid[0] = 0xff;
-       uid[1] = 0xff;
-       uid[2] = 0xff;
-       uid[3] = 0xff;
+       memset(uid, 0x44, 4);

        LogTrace(uid, 4, 0, 0, TRUE);
 
        for (i = 0; i < NES_MAX_INFO; i++) {
        LogTrace(uid, 4, 0, 0, TRUE);
 
        for (i = 0; i < NES_MAX_INFO; i++) {


@@ -2502,10 +2548,7 @@ void MifareChkKeys(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
        crypto1_destroy(pcs);
        
        // add trace trailer
        crypto1_destroy(pcs);
        
        // add trace trailer

-       uid[0] = 0xff;
-       uid[1] = 0xff;
-       uid[2] = 0xff;
-       uid[3] = 0xff;
+       memset(uid, 0x44, 4);

        LogTrace(uid, 4, 0, 0, TRUE);
 
        UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
        LogTrace(uid, 4, 0, 0, TRUE);
 
        UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};


@@ -2531,13 +2574,14 @@ void Mifare1ksim(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 {
        int cardSTATE = MFEMUL_NOFIELD;
        int vHf = 0;    // in mV
 {
        int cardSTATE = MFEMUL_NOFIELD;
        int vHf = 0;    // in mV

-       int res, i;
+       int nextCycleTimeout = 0;
+       int res;

        uint32_t timer = 0;
        uint32_t selTimer = 0;
        uint32_t authTimer = 0;
        uint32_t par = 0;
        int len = 0;
        uint32_t timer = 0;
        uint32_t selTimer = 0;
        uint32_t authTimer = 0;
        uint32_t par = 0;
        int len = 0;

-       uint8_t bt;
+       uint8_t cardWRBL = 0;

        uint8_t cardAUTHSC = 0;
        uint8_t cardAUTHKEY = 0xff;  // no authentication
        uint32_t cuid = 0;
        uint8_t cardAUTHSC = 0;
        uint8_t cardAUTHKEY = 0xff;  // no authentication
        uint32_t cuid = 0;


@@ -2547,7 +2591,8 @@ void Mifare1ksim(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
        
        uint64_t key64 = 0xffffffffffffULL;
        
        
        uint64_t key64 = 0xffffffffffffULL;
        

-       uint8_t* receivedCmd = mifare_get_bigbufptr();
+       uint8_t* receivedCmd = eml_get_bigbufptr_recbuf();
+       uint8_t *response = eml_get_bigbufptr_sendbuf();

        
        static uint8_t rATQA[] = {0x04, 0x00}; // Mifare classic 1k
 
        
        static uint8_t rATQA[] = {0x04, 0x00}; // Mifare classic 1k
 


@@ -2558,19 +2603,23 @@ void Mifare1ksim(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 
        static uint8_t rAUTH_NT[] = {0x1a, 0xac, 0xff, 0x4f};
        static uint8_t rAUTH_AT[] = {0x00, 0x00, 0x00, 0x00};
 
        static uint8_t rAUTH_NT[] = {0x1a, 0xac, 0xff, 0x4f};
        static uint8_t rAUTH_AT[] = {0x00, 0x00, 0x00, 0x00};

-       static uint8_t cmdBuf[18];

        
        // clear trace
        traceLen = 0;
        tracing = true;
        
        
        // clear trace
        traceLen = 0;
        tracing = true;
        

+       // emulator memory
+       emlClearMem();
+       emlGetMemBt(rUIDBCC1, 0, 4);
+       rUIDBCC1[4] = rUIDBCC1[0] ^ rUIDBCC1[1] ^ rUIDBCC1[2] ^ rUIDBCC1[3];
+       

 // --------------------------------------      test area
 
    // Authenticate response - nonce
 // --------------------------------------      test area
 
    // Authenticate response - nonce

-    uint8_t *resp1 = (((uint8_t *)BigBuf) + CARD_MEMORY);
+    uint8_t *resp1 = (((uint8_t *)BigBuf) + EML_RESPONSES);

     int resp1Len;
     int resp1Len;

-    uint8_t *resp2 = (((uint8_t *)BigBuf) + CARD_MEMORY + 200);
-    int resp2Len;
+//    uint8_t *resp2 = (((uint8_t *)BigBuf) + EML_RESPONSES + 200);
+//    int resp2Len;

        CodeIso14443aAsTag(rAUTH_NT, sizeof(rAUTH_NT));
     memcpy(resp1, ToSend, ToSendMax); resp1Len = ToSendMax;
                
        CodeIso14443aAsTag(rAUTH_NT, sizeof(rAUTH_NT));
     memcpy(resp1, ToSend, ToSendMax); resp1Len = ToSendMax;
                


@@ -2579,7 +2628,7 @@ void Mifare1ksim(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
        uint32_t rn_enc = 0x98d76b77; // !!!!!!!!!!!!!!!!!
        uint32_t ans = 0;
        cuid = bytes_to_num(rUIDBCC1, 4);
        uint32_t rn_enc = 0x98d76b77; // !!!!!!!!!!!!!!!!!
        uint32_t ans = 0;
        cuid = bytes_to_num(rUIDBCC1, 4);

-       
+/*     

        crypto1_create(pcs, key64);
   crypto1_word(pcs, cuid ^ nonce, 0);
   crypto1_word(pcs, rn_enc , 1);
        crypto1_create(pcs, key64);
   crypto1_word(pcs, cuid ^ nonce, 0);
   crypto1_word(pcs, rn_enc , 1);


@@ -2589,8 +2638,10 @@ void Mifare1ksim(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
        CodeIso14443aAsTag(rAUTH_AT, sizeof(rAUTH_AT));
   memcpy(resp2, ToSend, ToSendMax); resp2Len = ToSendMax;
        Dbprintf("crypto auth time: %d", GetTickCount() - timer);
        CodeIso14443aAsTag(rAUTH_AT, sizeof(rAUTH_AT));
   memcpy(resp2, ToSend, ToSendMax); resp2Len = ToSendMax;
        Dbprintf("crypto auth time: %d", GetTickCount() - timer);

-
+*/

 // --------------------------------------      END test area
 // --------------------------------------      END test area

+       // start mkseconds counter
+       StartCountUS();

 
        // We need to listen to the high-frequency, peak-detected path.
        SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 
        // We need to listen to the high-frequency, peak-detected path.
        SetAdcMuxFor(GPIO_MUXSEL_HIPKD);


@@ -2599,10 +2650,16 @@ void Mifare1ksim(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
   FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_LISTEN);
        SpinDelay(200);
 
   FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_LISTEN);
        SpinDelay(200);
 

-Dbprintf("--> start");
+       Dbprintf("--> start");
+       // calibrate mkseconds counter
+       GetDeltaCountUS();

        while (true) {
                WDT_HIT();
 
        while (true) {
                WDT_HIT();
 

+               if(BUTTON_PRESS()) {
+                       break;
+               }
+

                // find reader field
                // Vref = 3300mV, and an 10:1 voltage divider on the input
                // can measure voltages up to 33000 mV
                // find reader field
                // Vref = 3300mV, and an 10:1 voltage divider on the input
                // can measure voltages up to 33000 mV


@@ -2615,7 +2672,7 @@ Dbprintf("--> start");
                } 
 
                if (cardSTATE != MFEMUL_NOFIELD) {
                } 
 
                if (cardSTATE != MFEMUL_NOFIELD) {

-                       res = EmGetCmd(receivedCmd, &len, 100);
+                       res = EmGetCmd(receivedCmd, &len, 100); // (+ nextCycleTimeout)

                        if (res == 2) {
                                cardSTATE = MFEMUL_NOFIELD;
                                LEDsoff();
                        if (res == 2) {
                                cardSTATE = MFEMUL_NOFIELD;
                                LEDsoff();


@@ -2624,13 +2681,12 @@ Dbprintf("--> start");
                        if(res) break;
                }
                
                        if(res) break;
                }
                

-               if(BUTTON_PRESS()) {
-                       break;
-               }
+               nextCycleTimeout = 0;
+               

 //             if (len) Dbprintf("len:%d cmd: %02x %02x %02x %02x", len, receivedCmd[0], receivedCmd[1], receivedCmd[2], receivedCmd[3]);
 
                if (len != 4 && cardSTATE != MFEMUL_NOFIELD) { // len != 4 <---- speed up the code 4 authentication
 //             if (len) Dbprintf("len:%d cmd: %02x %02x %02x %02x", len, receivedCmd[0], receivedCmd[1], receivedCmd[2], receivedCmd[3]);
 
                if (len != 4 && cardSTATE != MFEMUL_NOFIELD) { // len != 4 <---- speed up the code 4 authentication

-                               // REQ or WUP request in ANY state and WUP in HALTED state
+                       // REQ or WUP request in ANY state and WUP in HALTED state

                        if (len == 1 && ((receivedCmd[0] == 0x26 && cardSTATE != MFEMUL_HALTED) || receivedCmd[0] == 0x52)) {
                                selTimer = GetTickCount();
                                EmSendCmdEx(rATQA, sizeof(rATQA), (receivedCmd[0] == 0x52));
                        if (len == 1 && ((receivedCmd[0] == 0x26 && cardSTATE != MFEMUL_HALTED) || receivedCmd[0] == 0x52)) {
                                selTimer = GetTickCount();
                                EmSendCmdEx(rATQA, sizeof(rATQA), (receivedCmd[0] == 0x52));


@@ -2683,12 +2739,11 @@ Dbprintf("--> start");
                                cuid = bytes_to_num(rUIDBCC2, 4);
                                cardSTATE = MFEMUL_WORK;
                                LED_B_ON();
                                cuid = bytes_to_num(rUIDBCC2, 4);
                                cardSTATE = MFEMUL_WORK;
                                LED_B_ON();

-Dbprintf("--> WORK. anticol2 time: %d", GetTickCount() - timer);
+Dbprintf("--> WORK. anticol2 time: %d", GetTickCount() - selTimer);

                                break;
                        }
                        case MFEMUL_AUTH1:{
                                if (len == 8) {
                                break;
                        }
                        case MFEMUL_AUTH1:{
                                if (len == 8) {

-timer = GetTickCount();

 // ---------------------------------
        rn_enc = bytes_to_num(receivedCmd, 4);
        crypto1_create(pcs, key64);
 // ---------------------------------
        rn_enc = bytes_to_num(receivedCmd, 4);
        crypto1_create(pcs, key64);


@@ -2699,7 +2754,6 @@ timer = GetTickCount();
        num_to_bytes(ans, 4, rAUTH_AT);
 // ---------------------------------
                                EmSendCmd(rAUTH_AT, sizeof(rAUTH_AT));
        num_to_bytes(ans, 4, rAUTH_AT);
 // ---------------------------------
                                EmSendCmd(rAUTH_AT, sizeof(rAUTH_AT));

-//                                     EmSendCmd14443aRaw(resp2, resp2Len, 0);

                                        cardSTATE = MFEMUL_AUTH2;
                                } else {
                                        cardSTATE = MFEMUL_IDLE;
                                        cardSTATE = MFEMUL_AUTH2;
                                } else {
                                        cardSTATE = MFEMUL_IDLE;


@@ -2713,16 +2767,16 @@ timer = GetTickCount();
 
                                LED_C_ON();
                                cardSTATE = MFEMUL_WORK;
 
                                LED_C_ON();
                                cardSTATE = MFEMUL_WORK;

-Dbprintf("AUTH COMPLETED. sec=%d, key=%d time=%d a=%d", cardAUTHSC, cardAUTHKEY, GetTickCount() - authTimer, GetTickCount() - timer);
+Dbprintf("AUTH COMPLETED. sec=%d, key=%d time=%d", cardAUTHSC, cardAUTHKEY, GetTickCount() - authTimer);

                                break;
                        }
                        case MFEMUL_WORK:{
                                // auth
                                if (len == 4 && (receivedCmd[0] == 0x60 || receivedCmd[0] == 0x61)) {
                                break;
                        }
                        case MFEMUL_WORK:{
                                // auth
                                if (len == 4 && (receivedCmd[0] == 0x60 || receivedCmd[0] == 0x61)) {

-                                       authTimer = GetTickCount();
+authTimer = GetTickCount();

 //                                     EmSendCmd(rAUTH_NT, sizeof(rAUTH_NT));
 //                                     EmSendCmd(rAUTH_NT, sizeof(rAUTH_NT));

-//SpinDelayUs(30);

                                        EmSendCmd14443aRaw(resp1, resp1Len, 0);
                                        EmSendCmd14443aRaw(resp1, resp1Len, 0);

+LogTrace(NULL, 0, GetDeltaCountUS(), 0, TRUE);

 //                                     crypto1_create(pcs, key64);
 //                                     if (cardAUTHKEY == 0xff) { // first auth
 //                                     crypto1_word(pcs, cuid ^ bytes_to_num(rAUTH_NT, 4), 0); // uid ^ nonce
 //                                     crypto1_create(pcs, key64);
 //                                     if (cardAUTHKEY == 0xff) { // first auth
 //                                     crypto1_word(pcs, cuid ^ bytes_to_num(rAUTH_NT, 4), 0); // uid ^ nonce


@@ -2732,50 +2786,54 @@ Dbprintf("AUTH COMPLETED. sec=%d, key=%d time=%d a=%d", cardAUTHSC, cardAUTHKEY,
                                        cardAUTHSC = receivedCmd[1] / 4;  // received block num
                                        cardAUTHKEY = receivedCmd[0] - 0x60;
                                        cardSTATE = MFEMUL_AUTH1;
                                        cardAUTHSC = receivedCmd[1] / 4;  // received block num
                                        cardAUTHKEY = receivedCmd[0] - 0x60;
                                        cardSTATE = MFEMUL_AUTH1;

+                                       nextCycleTimeout = 10;

                                        break;
                                }
                                
                                if (len == 0) break;
                                
                                // decrypt seqence
                                        break;
                                }
                                
                                if (len == 0) break;
                                
                                // decrypt seqence

-                               if (cardAUTHKEY != 0xff){
-                                       if (len != 1) {
-                                               for (i = 0; i < len; i++)
-                                                       receivedCmd[i] = crypto1_byte(pcs, 0x00, 0) ^ receivedCmd[i];
-                                       } else {
-                                               bt = 0;
-                                               for (i = 0; i < 4; i++)
-                                                       bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(receivedCmd[0], i)) << i;
+                               if (cardAUTHKEY != 0xff) mf_crypto1_decrypt(pcs, receivedCmd, len);

                                
                                

-                                               receivedCmd[0] = bt;
-                                       }
+                               // rule 13 of 7.5.3. in ISO 14443-4. chaining shall be continued
+                               // BUT... ACK --> NACK
+                               if (len == 1 && receivedCmd[0] == CARD_ACK) {
+                                       EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_NACK_NA));
+                                       break;
+                               }
+                               
+                               // rule 12 of 7.5.3. in ISO 14443-4. R(NAK) --> R(ACK)
+                               if (len == 1 && receivedCmd[0] == CARD_NACK_NA) {
+                                       EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_ACK));
+                                       break;

                                }
                                
                                // read block
                                if (len == 4 && receivedCmd[0] == 0x30) {
                                }
                                
                                // read block
                                if (len == 4 && receivedCmd[0] == 0x30) {

-                               cmdBuf[0] = 0;
-                               par = 0;
-/*                                     memcpy(cmdBuf, blockData, 16);
-                                       AppendCrc14443a(cmdBuf, 16);
-       
-       // crypto
-                                       par = 0;
-                                       for (i = 0; i < 18; i++) {
-                                               d_block_enc[pos] = crypto1_byte(pcs, 0x00, 0) ^ cmdBuf[pos];
-                                               par = (par >> 1) | ( ((filter(pcs->odd) ^ oddparity(cmdBuf[pos])) & 0x01) * 0x20000 );
-                                       }       
-*/
-                                       //ReaderTransmitPar(d_block_enc, sizeof(d_block_enc), par);
-Dbprintf("read block: %d", receivedCmd[1]);
+                                       if (receivedCmd[1] >= 16 * 4) {
+                                               EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_NACK_NA));
+                                               break;
+                                       }
+                                       emlGetMem(response, receivedCmd[1], 1);
+                                       AppendCrc14443a(response, 16);
+                                       mf_crypto1_encrypt(pcs, response, 18, &par);
+                                       EmSendCmdPar(response, 18, par);

                                        break;
                                }
                                
                                // write block
                                if (len == 4 && receivedCmd[0] == 0xA0) {
                                        break;
                                }
                                
                                // write block
                                if (len == 4 && receivedCmd[0] == 0xA0) {

-Dbprintf("write block: %d", receivedCmd[1]);
+                                       if (receivedCmd[1] >= 16 * 4) {
+                                               EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_NACK_NA));
+                                               break;
+                                       }
+                                       EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_ACK));
+                                       nextCycleTimeout = 50;
+                                       cardSTATE = MFEMUL_WRITEBL2;
+                                       cardWRBL = receivedCmd[1];

                                        break;
                                }
                                        break;
                                }

-                               
+                       

                                // halt
                                if (len == 4 && (receivedCmd[0] == 0x50 && receivedCmd[1] == 0x00)) {
                                        cardSTATE = MFEMUL_HALTED;
                                // halt
                                if (len == 4 && (receivedCmd[0] == 0x50 && receivedCmd[1] == 0x00)) {
                                        cardSTATE = MFEMUL_HALTED;


@@ -2785,6 +2843,23 @@ Dbprintf("write block: %d", receivedCmd[1]);
                                        break;
                                }
                                break;
                                        break;
                                }
                                break;

+
+                               // command not allowed
+                               if (len == 4) {
+                                       EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_NACK_NA));
+                                       break;
+                               }
+                       }
+                       case MFEMUL_WRITEBL2:{
+                               if (len == 18){
+                                       mf_crypto1_decrypt(pcs, receivedCmd, len);
+                                       emlSetMem(receivedCmd, cardWRBL, 1);
+                                       EmSend4bit(mf_crypto1_encrypt4bit(pcs, CARD_ACK));
+                                       cardSTATE = MFEMUL_WORK;
+                                       break;
+                               }
+Dbprintf("err write block: %d len:%d", cardWRBL, len);
+                               break;

                        }
                
                }
                        }
                
                }


@@ -2795,6 +2870,7 @@ Dbprintf("write block: %d", receivedCmd[1]);
        LEDsoff();
 
        // add trace trailer
        LEDsoff();
 
        // add trace trailer

+       memset(rAUTH_NT, 0x44, 4);

        LogTrace(rAUTH_NT, 4, 0, 0, TRUE);
 
        DbpString("Emulator stopped.");
        LogTrace(rAUTH_NT, 4, 0, 0, TRUE);
 
        DbpString("Emulator stopped.");

diff --git a/armsrc/iso14443a.h b/armsrc/iso14443a.h
index ba86acc522869c0f9a6b9e15c42c75bbc729aa84..9ee20c8e36f7f615c78a302bb5b7a54dbeaea308 100644 (file)
--- a/armsrc/iso14443a.h
+++ b/armsrc/iso14443a.h
@@ -14,6 +14,19 @@
 #define __ISO14443A_H
 #include "common.h"
 
 #define __ISO14443A_H
 #include "common.h"
 

+// BIG CHANGE - UNDERSTAND THIS BEFORE WE COMMIT
+#define RECV_CMD_OFFSET    3032
+#define RECV_RES_OFFSET    3096
+#define DMA_BUFFER_OFFSET  3160
+#define DMA_BUFFER_SIZE    4096
+#define TRACE_LENGTH       3000
+// mifare reader                      over DMA buffer (SnoopIso14443a())!!!
+#define MIFARE_BUFF_OFFSET 3560  //              \/   \/   \/
+// card emulator memory
+#define EML_RESPONSES      4000
+#define CARD_MEMORY        6000
+#define CARD_MEMORY_LEN    1024
+

 
 typedef struct nestedVector { uint32_t nt, ks1; } nestedVector;
 
 
 typedef struct nestedVector { uint32_t nt, ks1; } nestedVector;
 

diff --git a/armsrc/mifareutil.c b/armsrc/mifareutil.c
index fc95de72666c0e75981106f179cf27fa148bbaa2..8970813567d2a5ede69d2a43e20a5418b2771aca 100644 (file)
--- a/armsrc/mifareutil.c
+++ b/armsrc/mifareutil.c
@@ -1,12 +1,12 @@
 //-----------------------------------------------------------------------------\r
 // Merlok, May 2011\r
 //-----------------------------------------------------------------------------\r
 // Merlok, May 2011\r

-// Many authors, that makes it possible\r
+// Many authors, whom made it possible\r

 //\r
 // This code is licensed to you under the terms of the GNU GPL, version 2 or,\r
 // at your option, any later version. See the LICENSE.txt file for the text of\r
 // the license.\r
 //-----------------------------------------------------------------------------\r
 //\r
 // This code is licensed to you under the terms of the GNU GPL, version 2 or,\r
 // at your option, any later version. See the LICENSE.txt file for the text of\r
 // the license.\r
 //-----------------------------------------------------------------------------\r

-// code for work with mifare cards.\r
+// Work with mifare cards.\r

 //-----------------------------------------------------------------------------\r
 \r
 #include "proxmark3.h"\r
 //-----------------------------------------------------------------------------\r
 \r
 #include "proxmark3.h"\r


@@ -21,10 +21,62 @@
 \r
 int MF_DBGLEVEL = MF_DBG_ALL;\r
 \r
 \r
 int MF_DBGLEVEL = MF_DBG_ALL;\r
 \r

+// memory management\r

 uint8_t* mifare_get_bigbufptr(void) {\r
 uint8_t* mifare_get_bigbufptr(void) {\r

-       return (((uint8_t *)BigBuf) + 3560);    // was 3560 - tied to other size changes\r
+       return (((uint8_t *)BigBuf) + MIFARE_BUFF_OFFSET);      // was 3560 - tied to other size changes\r
+}\r
+uint8_t* eml_get_bigbufptr_sendbuf(void) {\r
+       return (((uint8_t *)BigBuf) + RECV_CMD_OFFSET); \r
+}\r
+uint8_t* eml_get_bigbufptr_recbuf(void) {\r
+       return (((uint8_t *)BigBuf) + MIFARE_BUFF_OFFSET);\r
+}\r
+uint8_t* eml_get_bigbufptr_cardmem(void) {\r
+       return (((uint8_t *)BigBuf) + CARD_MEMORY);\r
+}\r
+\r
+// crypto1 helpers\r
+void mf_crypto1_decrypt(struct Crypto1State *pcs, uint8_t *data, int len){\r
+       uint8_t bt = 0;\r
+       int i;\r
+       \r
+       if (len != 1) {\r
+               for (i = 0; i < len; i++)\r
+                       data[i] = crypto1_byte(pcs, 0x00, 0) ^ data[i];\r
+       } else {\r
+               bt = 0;\r
+               for (i = 0; i < 4; i++)\r
+                       bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data[0], i)) << i;\r
+                               \r
+               data[0] = bt;\r
+       }\r
+       return;\r
+}\r
+\r
+void mf_crypto1_encrypt(struct Crypto1State *pcs, uint8_t *data, int len, uint32_t *par) {\r
+       uint8_t bt = 0;\r
+       int i;\r
+       uint32_t mltpl = 1 << (len - 1); // for len=18 it=0x20000\r
+       *par = 0;\r
+       for (i = 0; i < len; i++) {\r
+               bt = data[i];\r
+               data[i] = crypto1_byte(pcs, 0x00, 0) ^ data[i];\r
+               *par = (*par >> 1) | ( ((filter(pcs->odd) ^ oddparity(bt)) & 0x01) * mltpl );\r
+       }       \r
+       return;\r
+}\r
+\r
+uint8_t mf_crypto1_encrypt4bit(struct Crypto1State *pcs, uint8_t data) {\r
+       uint8_t bt = 0;\r
+       int i;\r
+\r
+       for (i = 0; i < 4; i++)\r
+               bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data, i)) << i;\r
+               \r
+       return bt;\r

 }\r
 \r
 }\r
 \r

+// send commands\r

 int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t data, uint8_t* answer)\r
 {\r
        return mifare_sendcmd_shortex(pcs, crypted, cmd, data, answer, NULL);\r
 int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t data, uint8_t* answer)\r
 {\r
        return mifare_sendcmd_shortex(pcs, crypted, cmd, data, answer, NULL);\r


@@ -78,6 +130,7 @@ int mifare_sendcmd_shortex(struct Crypto1State *pcs, uint8_t crypted, uint8_t cm
        return len;\r
 }\r
 \r
        return len;\r
 }\r
 \r

+// mifare commands\r

 int mifare_classic_auth(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint64_t isNested) \r
 {\r
        return mifare_classic_authex(pcs, uid, blockNo, keyType, ui64Key, isNested, NULL);\r
 int mifare_classic_auth(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint64_t isNested) \r
 {\r
        return mifare_classic_authex(pcs, uid, blockNo, keyType, ui64Key, isNested, NULL);\r


@@ -267,3 +320,40 @@ int mifare_classic_halt(struct Crypto1State *pcs, uint32_t uid)
        return 0;\r
 }\r
 \r
        return 0;\r
 }\r
 \r

+// work with emulator memory\r
+void emlSetMem(uint8_t *data, int blockNum, int blocksCount) {\r
+       uint8_t* emCARD = eml_get_bigbufptr_cardmem();\r
+       \r
+       memcpy(emCARD + blockNum * 16, data, blocksCount * 16);\r
+}\r
+\r
+void emlGetMem(uint8_t *data, int blockNum, int blocksCount) {\r
+       uint8_t* emCARD = eml_get_bigbufptr_cardmem();\r
+       \r
+       memcpy(data, emCARD + blockNum * 16, blocksCount * 16);\r
+}\r
+\r
+void emlGetMemBt(uint8_t *data, int bytePtr, int byteCount) {\r
+       uint8_t* emCARD = eml_get_bigbufptr_cardmem();\r
+       \r
+       memcpy(data, emCARD + bytePtr, byteCount);\r
+}\r
+\r
+void emlClearMem(void) {\r
+       int i;\r
+       \r
+       const uint8_t trailer[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x07, 0x80, 0x69, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};\r
+       const uint8_t empty[] =   {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};\r
+       const uint8_t uid[]   =   {0xe6, 0x84, 0x87, 0xf3, 0x16, 0x88, 0x04, 0x00, 0x46, 0x8e, 0x45, 0x55, 0x4d, 0x70, 0x41, 0x04};\r
+       // fill sectors data\r
+       for(i = 0; i < 16; i++) {\r
+               emlSetMem((uint8_t *)empty,   i * 4 + 0, 1);\r
+               emlSetMem((uint8_t *)empty,   i * 4 + 1, 1);\r
+               emlSetMem((uint8_t *)empty,   i * 4 + 2, 1);\r
+               emlSetMem((uint8_t *)trailer, i * 4 + 3, 1);\r
+       }\r
+\r
+       // uid\r
+       emlSetMem((uint8_t *)uid, 0, 1);\r
+       return;\r
+}\r

diff --git a/armsrc/mifareutil.h b/armsrc/mifareutil.h
index 4433fe65a952f3133dc571aff747b09f0495b6f7..51ff25922f08751007dc8dcdeb996ebe6457d9b0 100644 (file)
--- a/armsrc/mifareutil.h
+++ b/armsrc/mifareutil.h
@@ -19,6 +19,11 @@
 #define AUTH_FIRST    0\r
 #define AUTH_NESTED   2\r
 \r
 #define AUTH_FIRST    0\r
 #define AUTH_NESTED   2\r
 \r

+// mifare 4bit card answers\r
+#define CARD_ACK      0x0A  // 1010 - ACK\r
+#define CARD_NACK_NA  0x04  // 0100 - NACK, not allowed (command not allowed)\r
+#define CARD_NACK_TR  0x05  // 0101 - NACK, transmission error\r
+\r

 // reader voltage field detector\r
 #define MF_MINFIELDV      4000\r
 \r
 // reader voltage field detector\r
 #define MF_MINFIELDV      4000\r
 \r


@@ -46,7 +51,8 @@ extern int MF_DBGLEVEL;
 #define MFEMUL_AUTH1    4\r
 #define MFEMUL_AUTH2    5\r
 #define MFEMUL_WORK                    6\r
 #define MFEMUL_AUTH1    4\r
 #define MFEMUL_AUTH2    5\r
 #define MFEMUL_WORK                    6\r

-#define MFEMUL_HALTED   7\r
+#define MFEMUL_WRITEBL2 7\r
+#define MFEMUL_HALTED   8\r

 \r
 //functions\r
 uint8_t* mifare_get_bigbufptr(void);\r
 \r
 //functions\r
 uint8_t* mifare_get_bigbufptr(void);\r


@@ -61,4 +67,20 @@ int mifare_classic_readblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blo
 int mifare_classic_writeblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData);\r
 int mifare_classic_halt(struct Crypto1State *pcs, uint32_t uid); \r
 \r
 int mifare_classic_writeblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData);\r
 int mifare_classic_halt(struct Crypto1State *pcs, uint32_t uid); \r
 \r

+// crypto functions\r
+void mf_crypto1_decrypt(struct Crypto1State *pcs, uint8_t *receivedCmd, int len);\r
+void mf_crypto1_encrypt(struct Crypto1State *pcs, uint8_t *data, int len, uint32_t *par);\r
+uint8_t mf_crypto1_encrypt4bit(struct Crypto1State *pcs, uint8_t data);\r
+\r
+// memory management\r
+uint8_t* mifare_get_bigbufptr(void);\r
+uint8_t* eml_get_bigbufptr_sendbuf(void);\r
+uint8_t* eml_get_bigbufptr_recbuf(void);\r
+\r
+// emulator functions\r
+void emlClearMem(void);\r
+void emlSetMem(uint8_t *data, int blockNum, int blocksCount);\r
+void emlGetMem(uint8_t *data, int blockNum, int blocksCount);\r
+void emlGetMemBt(uint8_t *data, int bytePtr, int byteCount);\r
+\r

 #endif
\ No newline at end of file
 #endif
\ No newline at end of file

diff --git a/armsrc/util.c b/armsrc/util.c
index 0172754e283a950b47606103719d7a6d8478e261..00dbdbb83e29f176792ba9e7eb3352ca34faa3ed 100644 (file)
--- a/armsrc/util.c
+++ b/armsrc/util.c
@@ -259,6 +259,45 @@ void StartTickCount()
 * Get the current count.
 */
 uint32_t RAMFUNC GetTickCount(){
 * Get the current count.
 */
 uint32_t RAMFUNC GetTickCount(){

-       return AT91C_BASE_RTTC->RTTC_RTVR;// * 2;
+       return AT91C_BASE_RTTC->RTTC_RTVR;// was * 2;

 }
 
 }
 

+//  -------------------------------------------------------------------------
+//  microseconds timer 
+//  -------------------------------------------------------------------------
+void StartCountUS()
+{
+       AT91C_BASE_PMC->PMC_PCER |= (0x1 << 12) | (0x1 << 13) | (0x1 << 14);
+//     AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC1XC1S_TIOA0;
+       AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_NONE | AT91C_TCB_TC1XC1S_TIOA0 | AT91C_TCB_TC2XC2S_NONE;
+
+       // fast clock
+       AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS; // timer disable
+       AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | // MCK(48MHz)/32 -- tick=1.5mks
+                                                                                                               AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO | AT91C_TC_ACPA_CLEAR |
+                                                                                                               AT91C_TC_ACPC_SET | AT91C_TC_ASWTRG_SET;
+       AT91C_BASE_TC0->TC_RA = 1;
+       AT91C_BASE_TC0->TC_RC = 0xBFFF + 1; // 0xC000
+       
+       AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; // timer disable  
+       AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_XC1; // from timer 0
+
+       AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN;
+       AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN;
+       AT91C_BASE_TCB->TCB_BCR = 1;
+}
+
+uint32_t RAMFUNC GetCountUS(){
+       return (AT91C_BASE_TC1->TC_CV * 0x8000) + ((AT91C_BASE_TC0->TC_CV / 15) * 10);
+}
+
+static uint32_t GlobalUsCounter = 0;
+
+uint32_t RAMFUNC GetDeltaCountUS(){
+       uint32_t g_cnt = GetCountUS();
+       uint32_t g_res = g_cnt - GlobalUsCounter;
+       GlobalUsCounter = g_cnt;
+       return g_res;
+}
+
+

diff --git a/armsrc/util.h b/armsrc/util.h
index 080dac6b5ebab17675dfe2ae4cb4de15fe5e46f0..144032324d62e74b77bba24bc2a3669045328208 100644 (file)
--- a/armsrc/util.h
+++ b/armsrc/util.h
@@ -42,4 +42,8 @@ void FormatVersionInformation(char *dst, int len, const char *prefix, void *vers
 void StartTickCount();
 uint32_t RAMFUNC GetTickCount();
 
 void StartTickCount();
 uint32_t RAMFUNC GetTickCount();
 

+void StartCountUS();
+uint32_t RAMFUNC GetCountUS();
+uint32_t RAMFUNC GetDeltaCountUS();
+

 #endif
 #endif