- int i, id_bit;
- uint64_t id = EM410X_HEADER;
- uint64_t rev_id = 0; // reversed ID
- int c_parity[4]; // column parity
- int r_parity = 0; // row parity
- uint32_t clock = 0;
-
- // Reverse ID bits given as parameter (for simpler operations)
- for (i = 0; i < EM410X_ID_LENGTH; ++i) {
- if (i < 32) {
- rev_id = (rev_id << 1) | (id_lo & 1);
- id_lo >>= 1;
- } else {
- rev_id = (rev_id << 1) | (id_hi & 1);
- id_hi >>= 1;
- }
- }
-
- for (i = 0; i < EM410X_ID_LENGTH; ++i) {
- id_bit = rev_id & 1;
-
- if (i % 4 == 0) {
- // Don't write row parity bit at start of parsing
- if (i)
- id = (id << 1) | r_parity;
- // Start counting parity for new row
- r_parity = id_bit;
- } else {
- // Count row parity
- r_parity ^= id_bit;
- }
-
- // First elements in column?
- if (i < 4)
- // Fill out first elements
- c_parity[i] = id_bit;
- else
- // Count column parity
- c_parity[i % 4] ^= id_bit;
-
- // Insert ID bit
- id = (id << 1) | id_bit;
- rev_id >>= 1;
- }
-
- // Insert parity bit of last row
- id = (id << 1) | r_parity;
-
- // Fill out column parity at the end of tag
- for (i = 0; i < 4; ++i)
- id = (id << 1) | c_parity[i];
-
- // Add stop bit
- id <<= 1;
-
- Dbprintf("Started writing %s tag ...", card ? "T55x7":"T5555");
- LED_D_ON();
-
- // Write EM410x ID
- T55xxWriteBlock((uint32_t)(id >> 32), 1, 0, 0);
- T55xxWriteBlock((uint32_t)id, 2, 0, 0);
-
- // Config for EM410x (RF/64, Manchester, Maxblock=2)
- if (card) {
- // Clock rate is stored in bits 8-15 of the card value
- clock = (card & 0xFF00) >> 8;
- Dbprintf("Clock rate: %d", clock);
- switch (clock)
- {
- case 32:
- clock = T55x7_BITRATE_RF_32;
- break;
- case 16:
- clock = T55x7_BITRATE_RF_16;
- break;
- case 0:
- // A value of 0 is assumed to be 64 for backwards-compatibility
- // Fall through...
- case 64:
- clock = T55x7_BITRATE_RF_64;
- break;
- default:
- Dbprintf("Invalid clock rate: %d", clock);
- return;
- }
-
- // Writing configuration for T55x7 tag
- T55xxWriteBlock(clock |
- T55x7_MODULATION_MANCHESTER |
- 2 << T55x7_MAXBLOCK_SHIFT,
- 0, 0, 0);
- }
- else
- // Writing configuration for T5555(Q5) tag
- T55xxWriteBlock(0x1F << T5555_BITRATE_SHIFT |
- T5555_MODULATION_MANCHESTER |
- 2 << T5555_MAXBLOCK_SHIFT,
- 0, 0, 0);
-
- LED_D_OFF();
- Dbprintf("Tag %s written with 0x%08x%08x\n", card ? "T55x7":"T5555",
- (uint32_t)(id >> 32), (uint32_t)id);
-}
-
-// Clone Indala 64-bit tag by UID to T55x7
-void CopyIndala64toT55x7(int hi, int lo)
-{
-
- //Program the 2 data blocks for supplied 64bit UID
- // and the block 0 for Indala64 format
- T55xxWriteBlock(hi,1,0,0);
- T55xxWriteBlock(lo,2,0,0);
- //Config for Indala (RF/32;PSK1 with RF/2;Maxblock=2)
- T55xxWriteBlock(T55x7_BITRATE_RF_32 |
- T55x7_MODULATION_PSK1 |
- 2 << T55x7_MAXBLOCK_SHIFT,
- 0, 0, 0);
- //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=2;Inverse data)
- // T5567WriteBlock(0x603E1042,0);
-
- DbpString("DONE!");
-
-}
-
-void CopyIndala224toT55x7(int uid1, int uid2, int uid3, int uid4, int uid5, int uid6, int uid7)
-{
-
- //Program the 7 data blocks for supplied 224bit UID
- // and the block 0 for Indala224 format
- T55xxWriteBlock(uid1,1,0,0);
- T55xxWriteBlock(uid2,2,0,0);
- T55xxWriteBlock(uid3,3,0,0);
- T55xxWriteBlock(uid4,4,0,0);
- T55xxWriteBlock(uid5,5,0,0);
- T55xxWriteBlock(uid6,6,0,0);
- T55xxWriteBlock(uid7,7,0,0);
- //Config for Indala (RF/32;PSK1 with RF/2;Maxblock=7)
- T55xxWriteBlock(T55x7_BITRATE_RF_32 |
- T55x7_MODULATION_PSK1 |
- 7 << T55x7_MAXBLOCK_SHIFT,
- 0,0,0);
- //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=7;Inverse data)
- // T5567WriteBlock(0x603E10E2,0);
-
- DbpString("DONE!");
-
-}
-
-
-#define abs(x) ( ((x)<0) ? -(x) : (x) )
-#define max(x,y) ( x<y ? y:x)
-
-int DemodPCF7931(uint8_t **outBlocks) {
- uint8_t BitStream[256];
- uint8_t Blocks[8][16];
- uint8_t *GraphBuffer = BigBuf_get_addr();
- int GraphTraceLen = BigBuf_max_traceLen();
- int i, j, lastval, bitidx, half_switch;
- int clock = 64;
- int tolerance = clock / 8;
- int pmc, block_done;
- int lc, warnings = 0;
- int num_blocks = 0;
- int lmin=128, lmax=128;
- uint8_t dir;
-
- LFSetupFPGAForADC(95, true);
- DoAcquisition_default(0, 0);
-
-
- lmin = 64;
- lmax = 192;
-
- i = 2;
-
- /* Find first local max/min */
- if(GraphBuffer[1] > GraphBuffer[0]) {
- while(i < GraphTraceLen) {
- if( !(GraphBuffer[i] > GraphBuffer[i-1]) && GraphBuffer[i] > lmax)
- break;
- i++;
- }
- dir = 0;
- }
- else {
- while(i < GraphTraceLen) {
- if( !(GraphBuffer[i] < GraphBuffer[i-1]) && GraphBuffer[i] < lmin)
- break;
- i++;
- }
- dir = 1;
- }
-
- lastval = i++;
- half_switch = 0;
- pmc = 0;
- block_done = 0;
-
- for (bitidx = 0; i < GraphTraceLen; i++)
- {
- if ( (GraphBuffer[i-1] > GraphBuffer[i] && dir == 1 && GraphBuffer[i] > lmax) || (GraphBuffer[i-1] < GraphBuffer[i] && dir == 0 && GraphBuffer[i] < lmin))
- {
- lc = i - lastval;
- lastval = i;
-
- // Switch depending on lc length:
- // Tolerance is 1/8 of clock rate (arbitrary)
- if (abs(lc-clock/4) < tolerance) {
- // 16T0
- if((i - pmc) == lc) { /* 16T0 was previous one */
- /* It's a PMC ! */
- i += (128+127+16+32+33+16)-1;
- lastval = i;
- pmc = 0;
- block_done = 1;
- }
- else {
- pmc = i;
- }
- } else if (abs(lc-clock/2) < tolerance) {
- // 32TO
- if((i - pmc) == lc) { /* 16T0 was previous one */
- /* It's a PMC ! */
- i += (128+127+16+32+33)-1;
- lastval = i;
- pmc = 0;
- block_done = 1;
- }
- else if(half_switch == 1) {
- BitStream[bitidx++] = 0;
- half_switch = 0;
- }
- else
- half_switch++;
- } else if (abs(lc-clock) < tolerance) {
- // 64TO
- BitStream[bitidx++] = 1;
- } else {
- // Error
- warnings++;
- if (warnings > 10)
- {
- Dbprintf("Error: too many detection errors, aborting.");
- return 0;
- }
- }
-
- if(block_done == 1) {
- if(bitidx == 128) {
- for(j=0; j<16; j++) {
- Blocks[num_blocks][j] = 128*BitStream[j*8+7]+
- 64*BitStream[j*8+6]+
- 32*BitStream[j*8+5]+
- 16*BitStream[j*8+4]+
- 8*BitStream[j*8+3]+
- 4*BitStream[j*8+2]+
- 2*BitStream[j*8+1]+
- BitStream[j*8];
- }
- num_blocks++;
- }
- bitidx = 0;
- block_done = 0;
- half_switch = 0;
- }
- if(i < GraphTraceLen)
- {
- if (GraphBuffer[i-1] > GraphBuffer[i]) dir=0;
- else dir = 1;
- }
- }
- if(bitidx==255)
- bitidx=0;
- warnings = 0;
- if(num_blocks == 4) break;
- }
- memcpy(outBlocks, Blocks, 16*num_blocks);
- return num_blocks;
-}
-
-int IsBlock0PCF7931(uint8_t *Block) {
- // Assume RFU means 0 :)
- if((memcmp(Block, "\x00\x00\x00\x00\x00\x00\x00\x01", 8) == 0) && memcmp(Block+9, "\x00\x00\x00\x00\x00\x00\x00", 7) == 0) // PAC enabled
- return 1;
- if((memcmp(Block+9, "\x00\x00\x00\x00\x00\x00\x00", 7) == 0) && Block[7] == 0) // PAC disabled, can it *really* happen ?
- return 1;
- return 0;
-}
-
-int IsBlock1PCF7931(uint8_t *Block) {
- // Assume RFU means 0 :)
- if(Block[10] == 0 && Block[11] == 0 && Block[12] == 0 && Block[13] == 0)
- if((Block[14] & 0x7f) <= 9 && Block[15] <= 9)
- return 1;
-
- return 0;
+ int i, id_bit;
+ uint64_t id = EM410X_HEADER;
+ uint64_t rev_id = 0; // reversed ID
+ int c_parity[4]; // column parity
+ int r_parity = 0; // row parity
+ uint32_t clock = 0;
+
+ // Reverse ID bits given as parameter (for simpler operations)
+ for (i = 0; i < EM410X_ID_LENGTH; ++i) {
+ if (i < 32) {
+ rev_id = (rev_id << 1) | (id_lo & 1);
+ id_lo >>= 1;
+ } else {
+ rev_id = (rev_id << 1) | (id_hi & 1);
+ id_hi >>= 1;
+ }
+ }
+
+ for (i = 0; i < EM410X_ID_LENGTH; ++i) {
+ id_bit = rev_id & 1;
+
+ if (i % 4 == 0) {
+ // Don't write row parity bit at start of parsing
+ if (i)
+ id = (id << 1) | r_parity;
+ // Start counting parity for new row
+ r_parity = id_bit;
+ } else {
+ // Count row parity
+ r_parity ^= id_bit;
+ }
+
+ // First elements in column?
+ if (i < 4)
+ // Fill out first elements
+ c_parity[i] = id_bit;
+ else
+ // Count column parity
+ c_parity[i % 4] ^= id_bit;
+
+ // Insert ID bit
+ id = (id << 1) | id_bit;
+ rev_id >>= 1;
+ }
+
+ // Insert parity bit of last row
+ id = (id << 1) | r_parity;
+
+ // Fill out column parity at the end of tag
+ for (i = 0; i < 4; ++i)
+ id = (id << 1) | c_parity[i];
+
+ // Add stop bit
+ id <<= 1;
+
+ Dbprintf("Started writing %s tag ...", card ? "T55x7":"T5555");
+ LED_D_ON();
+
+ // Write EM410x ID
+ uint32_t data[] = {0, id>>32, id & 0xFFFF};
+ if (card) {
+ clock = (card & 0xFF00) >> 8;
+ clock = (clock == 0) ? 64 : clock;
+ Dbprintf("Clock rate: %d", clock);
+ clock = GetT55xxClockBit(clock);
+ if (clock == 0) {
+ Dbprintf("Invalid clock rate: %d", clock);
+ return;
+ }
+
+ data[0] = clock | T55x7_MODULATION_MANCHESTER | (2 << T55x7_MAXBLOCK_SHIFT);
+ } else {
+ data[0] = (0x1F << T5555_BITRATE_SHIFT) | T5555_MODULATION_MANCHESTER | (2 << T5555_MAXBLOCK_SHIFT);
+ }
+
+ WriteT55xx(data, 0, 3);
+
+ LED_D_OFF();
+ Dbprintf("Tag %s written with 0x%08x%08x\n", card ? "T55x7":"T5555",
+ (uint32_t)(id >> 32), (uint32_t)id);
projects / proxmark3-svn / blobdiff