6923d3f1 |
1 | //----------------------------------------------------------------------------- |
2 | // |
3 | // This code is licensed to you under the terms of the GNU GPL, version 2 or, |
4 | // at your option, any later version. See the LICENSE.txt file for the text of |
5 | // the license. |
6 | //----------------------------------------------------------------------------- |
7 | // Low frequency Farpoint / Pyramid tag commands |
b9957414 |
8 | // FSK2a, rf/50, 128 bits (complete) |
6923d3f1 |
9 | //----------------------------------------------------------------------------- |
ad939de5 |
10 | |
11 | #include "cmdlfpyramid.h" |
12 | |
6923d3f1 |
13 | #include <string.h> |
14 | #include <inttypes.h> |
acf0582d |
15 | #include <stdio.h> |
ad939de5 |
16 | #include "comms.h" |
29ada8fc |
17 | #include "ui.h" |
18 | #include "util.h" |
19 | #include "graph.h" |
20 | #include "cmdparser.h" |
cf4640b9 |
21 | #include "cmddata.h" // setDemodBuf + |
29ada8fc |
22 | #include "cmdmain.h" |
23 | #include "cmdlf.h" |
24 | #include "protocols.h" // for T55xx config register definitions |
25 | #include "lfdemod.h" // parityTest |
26 | #include "crc.h" |
27 | |
6923d3f1 |
28 | static int CmdHelp(const char *Cmd); |
29 | |
30 | int usage_lf_pyramid_clone(void){ |
31 | PrintAndLog("clone a Farpointe/Pyramid tag to a T55x7 tag."); |
32 | PrintAndLog("The facility-code is 8-bit and the card number is 16-bit. Larger values are truncated. "); |
cf4640b9 |
33 | PrintAndLog("Currently only works on 26bit"); |
6923d3f1 |
34 | PrintAndLog(""); |
35 | PrintAndLog("Usage: lf pyramid clone <Facility-Code> <Card-Number>"); |
36 | PrintAndLog("Options :"); |
37 | PrintAndLog(" <Facility-Code> : 8-bit value facility code"); |
38 | PrintAndLog(" <Card Number> : 16-bit value card number"); |
29ada8fc |
39 | PrintAndLog(" Q5 : optional - clone to Q5 (T5555) instead of T55x7 chip"); |
6923d3f1 |
40 | PrintAndLog(""); |
41 | PrintAndLog("Sample : lf pyramid clone 123 11223"); |
42 | return 0; |
43 | } |
44 | |
45 | int usage_lf_pyramid_sim(void) { |
46 | PrintAndLog("Enables simulation of Farpointe/Pyramid card with specified card number."); |
47 | PrintAndLog("Simulation runs until the button is pressed or another USB command is issued."); |
48 | PrintAndLog("The facility-code is 8-bit and the card number is 16-bit. Larger values are truncated."); |
49 | PrintAndLog("Currently work only on 26bit"); |
50 | PrintAndLog(""); |
51 | PrintAndLog("Usage: lf pyramid sim <Card-Number>"); |
52 | PrintAndLog("Options :"); |
53 | PrintAndLog(" <Facility-Code> : 8-bit value facility code"); |
54 | PrintAndLog(" <Card Number> : 16-bit value card number"); |
55 | PrintAndLog(""); |
56 | PrintAndLog("Sample : lf pyramid sim 123 11223"); |
57 | return 0; |
58 | } |
59 | |
60 | // Works for 26bits. |
61 | int GetPyramidBits(uint32_t fc, uint32_t cn, uint8_t *pyramidBits) { |
62 | |
63 | uint8_t pre[128]; |
64 | memset(pre, 0x00, sizeof(pre)); |
65 | |
66 | // format start bit |
67 | pre[79] = 1; |
68 | |
69 | // Get 26 wiegand from FacilityCode, CardNumber |
70 | uint8_t wiegand[24]; |
71 | memset(wiegand, 0x00, sizeof(wiegand)); |
72 | num_to_bytebits(fc, 8, wiegand); |
73 | num_to_bytebits(cn, 16, wiegand+8); |
74 | |
75 | // add wiegand parity bits (dest, source, len) |
76 | wiegand_add_parity(pre+80, wiegand, 24); |
77 | |
78 | // add paritybits (bitsource, dest, sourcelen, paritylen, parityType (odd, even,) |
79 | addParity(pre+8, pyramidBits+8, 102, 8, 1); |
80 | |
81 | // add checksum |
82 | uint8_t csBuff[13]; |
83 | for (uint8_t i = 0; i < 13; i++) |
84 | csBuff[i] = bytebits_to_byte(pyramidBits + 16 + (i*8), 8); |
85 | |
86 | uint32_t crc = CRC8Maxim(csBuff, 13); |
87 | num_to_bytebits(crc, 8, pyramidBits+120); |
88 | return 1; |
89 | } |
90 | |
cf4640b9 |
91 | //by marshmellow |
92 | //Pyramid Prox demod - FSK RF/50 with preamble of 0000000000000001 (always a 128 bit data stream) |
93 | //print full Farpointe Data/Pyramid Prox ID and some bit format details if found |
94 | int CmdFSKdemodPyramid(const char *Cmd) |
95 | { |
96 | //raw fsk demod no manchester decoding no start bit finding just get binary from wave |
97 | uint8_t BitStream[MAX_GRAPH_TRACE_LEN]={0}; |
98 | size_t size = getFromGraphBuf(BitStream); |
99 | if (size==0) return 0; |
100 | |
1c70664a |
101 | int waveIdx=0; |
cf4640b9 |
102 | //get binary from fsk wave |
1c70664a |
103 | int idx = PyramiddemodFSK(BitStream, &size, &waveIdx); |
cf4640b9 |
104 | if (idx < 0){ |
105 | if (g_debugMode){ |
106 | if (idx == -5) |
107 | PrintAndLog("DEBUG: Error - not enough samples"); |
108 | else if (idx == -1) |
109 | PrintAndLog("DEBUG: Error - only noise found"); |
110 | else if (idx == -2) |
111 | PrintAndLog("DEBUG: Error - problem during FSK demod"); |
112 | else if (idx == -3) |
113 | PrintAndLog("DEBUG: Error - Size not correct: %d", size); |
114 | else if (idx == -4) |
115 | PrintAndLog("DEBUG: Error - Pyramid preamble not found"); |
116 | else |
117 | PrintAndLog("DEBUG: Error - idx: %d",idx); |
118 | } |
119 | return 0; |
120 | } |
121 | // Index map |
122 | // 0 10 20 30 40 50 60 |
123 | // | | | | | | | |
124 | // 0123456 7 8901234 5 6789012 3 4567890 1 2345678 9 0123456 7 8901234 5 6789012 3 |
125 | // ----------------------------------------------------------------------------- |
126 | // 0000000 0 0000000 1 0000000 1 0000000 1 0000000 1 0000000 1 0000000 1 0000000 1 |
127 | // premable xxxxxxx o xxxxxxx o xxxxxxx o xxxxxxx o xxxxxxx o xxxxxxx o xxxxxxx o |
128 | |
129 | // 64 70 80 90 100 110 120 |
130 | // | | | | | | | |
131 | // 4567890 1 2345678 9 0123456 7 8901234 5 6789012 3 4567890 1 2345678 9 0123456 7 |
132 | // ----------------------------------------------------------------------------- |
133 | // 0000000 1 0000000 1 0000000 1 0110111 0 0011000 1 0000001 0 0001100 1 1001010 0 |
134 | // xxxxxxx o xxxxxxx o xxxxxxx o xswffff o ffffccc o ccccccc o ccccccw o ppppppp o |
135 | // |---115---||---------71---------| |
136 | // s = format start bit, o = odd parity of last 7 bits |
137 | // f = facility code, c = card number |
138 | // w = wiegand parity, x = extra space for other formats |
139 | // p = unknown checksum |
140 | // (26 bit format shown) |
141 | |
142 | //get bytes for checksum calc |
143 | uint8_t checksum = bytebits_to_byte(BitStream + idx + 120, 8); |
144 | uint8_t csBuff[14] = {0x00}; |
145 | for (uint8_t i = 0; i < 13; i++){ |
146 | csBuff[i] = bytebits_to_byte(BitStream + idx + 16 + (i*8), 8); |
147 | } |
148 | //check checksum calc |
149 | //checksum calc thanks to ICEMAN!! |
150 | uint32_t checkCS = CRC8Maxim(csBuff,13); |
151 | |
152 | //get raw ID before removing parities |
153 | uint32_t rawLo = bytebits_to_byte(BitStream+idx+96,32); |
154 | uint32_t rawHi = bytebits_to_byte(BitStream+idx+64,32); |
155 | uint32_t rawHi2 = bytebits_to_byte(BitStream+idx+32,32); |
156 | uint32_t rawHi3 = bytebits_to_byte(BitStream+idx,32); |
157 | setDemodBuf(BitStream,128,idx); |
1c70664a |
158 | setClockGrid(50, waveIdx + (idx*50)); |
cf4640b9 |
159 | |
160 | size = removeParity(BitStream, idx+8, 8, 1, 120); |
161 | if (size != 105){ |
162 | if (g_debugMode) |
163 | PrintAndLog("DEBUG: Error at parity check - tag size does not match Pyramid format, SIZE: %d, IDX: %d, hi3: %x",size, idx, rawHi3); |
164 | return 0; |
165 | } |
166 | |
167 | // ok valid card found! |
168 | |
169 | // Index map |
170 | // 0 10 20 30 40 50 60 70 |
171 | // | | | | | | | | |
172 | // 01234567890123456789012345678901234567890123456789012345678901234567890 |
173 | // ----------------------------------------------------------------------- |
174 | // 00000000000000000000000000000000000000000000000000000000000000000000000 |
175 | // xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx |
176 | |
177 | // 71 80 90 100 |
178 | // | | | | |
179 | // 1 2 34567890 1234567890123456 7 8901234 |
180 | // --------------------------------------- |
181 | // 1 1 01110011 0000000001000110 0 1001010 |
182 | // s w ffffffff cccccccccccccccc w ppppppp |
183 | // |--115-| |------71------| |
184 | // s = format start bit, o = odd parity of last 7 bits |
185 | // f = facility code, c = card number |
186 | // w = wiegand parity, x = extra space for other formats |
187 | // p = unknown checksum |
188 | // (26 bit format shown) |
189 | |
190 | //find start bit to get fmtLen |
191 | int j; |
192 | for (j=0; j<size; j++){ |
193 | if(BitStream[j]) break; |
194 | } |
195 | uint8_t fmtLen = size-j-8; |
196 | uint32_t fc = 0; |
197 | uint32_t cardnum = 0; |
198 | uint32_t code1 = 0; |
199 | if (fmtLen==26){ |
200 | fc = bytebits_to_byte(BitStream+73, 8); |
201 | cardnum = bytebits_to_byte(BitStream+81, 16); |
202 | code1 = bytebits_to_byte(BitStream+72,fmtLen); |
203 | PrintAndLog("Pyramid ID Found - BitLength: %d, FC: %d, Card: %d - Wiegand: %x, Raw: %08x%08x%08x%08x", fmtLen, fc, cardnum, code1, rawHi3, rawHi2, rawHi, rawLo); |
204 | } else if (fmtLen==45){ |
205 | fmtLen=42; //end = 10 bits not 7 like 26 bit fmt |
206 | fc = bytebits_to_byte(BitStream+53, 10); |
207 | cardnum = bytebits_to_byte(BitStream+63, 32); |
208 | PrintAndLog("Pyramid ID Found - BitLength: %d, FC: %d, Card: %d - Raw: %08x%08x%08x%08x", fmtLen, fc, cardnum, rawHi3, rawHi2, rawHi, rawLo); |
209 | } else { |
210 | cardnum = bytebits_to_byte(BitStream+81, 16); |
211 | if (fmtLen>32){ |
212 | //code1 = bytebits_to_byte(BitStream+(size-fmtLen),fmtLen-32); |
213 | //code2 = bytebits_to_byte(BitStream+(size-32),32); |
214 | PrintAndLog("Pyramid ID Found - BitLength: %d -unknown BitLength- (%d), Raw: %08x%08x%08x%08x", fmtLen, cardnum, rawHi3, rawHi2, rawHi, rawLo); |
215 | } else{ |
216 | //code1 = bytebits_to_byte(BitStream+(size-fmtLen),fmtLen); |
217 | PrintAndLog("Pyramid ID Found - BitLength: %d -unknown BitLength- (%d), Raw: %08x%08x%08x%08x", fmtLen, cardnum, rawHi3, rawHi2, rawHi, rawLo); |
218 | } |
219 | } |
220 | if (checksum == checkCS) |
221 | PrintAndLog("Checksum %02x passed", checksum); |
222 | else |
223 | PrintAndLog("Checksum %02x failed - should have been %02x", checksum, checkCS); |
224 | |
225 | if (g_debugMode){ |
226 | PrintAndLog("DEBUG: idx: %d, Len: %d, Printing Demod Buffer:", idx, 128); |
227 | printDemodBuff(); |
228 | } |
229 | return 1; |
230 | } |
231 | |
6923d3f1 |
232 | int CmdPyramidRead(const char *Cmd) { |
b9957414 |
233 | lf_read(true, 15000); |
6923d3f1 |
234 | return CmdFSKdemodPyramid(""); |
235 | } |
236 | |
237 | int CmdPyramidClone(const char *Cmd) { |
238 | |
239 | char cmdp = param_getchar(Cmd, 0); |
240 | if (strlen(Cmd) == 0 || cmdp == 'h' || cmdp == 'H') return usage_lf_pyramid_clone(); |
241 | |
242 | uint32_t facilitycode=0, cardnumber=0, fc = 0, cn = 0; |
243 | uint32_t blocks[5]; |
244 | uint8_t i; |
245 | uint8_t bs[128]; |
246 | memset(bs, 0x00, sizeof(bs)); |
247 | |
248 | if (sscanf(Cmd, "%u %u", &fc, &cn ) != 2) return usage_lf_pyramid_clone(); |
249 | |
250 | facilitycode = (fc & 0x000000FF); |
251 | cardnumber = (cn & 0x0000FFFF); |
252 | |
253 | if ( !GetPyramidBits(facilitycode, cardnumber, bs)) { |
254 | PrintAndLog("Error with tag bitstream generation."); |
255 | return 1; |
29ada8fc |
256 | } |
6923d3f1 |
257 | |
258 | //Pyramid - compat mode, FSK2a, data rate 50, 4 data blocks |
259 | blocks[0] = T55x7_MODULATION_FSK2a | T55x7_BITRATE_RF_50 | 4<<T55x7_MAXBLOCK_SHIFT; |
29ada8fc |
260 | |
261 | if (param_getchar(Cmd, 3) == 'Q' || param_getchar(Cmd, 3) == 'q') |
262 | blocks[0] = T5555_MODULATION_FSK2 | T5555_INVERT_OUTPUT | 50<<T5555_BITRATE_SHIFT | 4<<T5555_MAXBLOCK_SHIFT; |
263 | |
6923d3f1 |
264 | blocks[1] = bytebits_to_byte(bs,32); |
265 | blocks[2] = bytebits_to_byte(bs+32,32); |
266 | blocks[3] = bytebits_to_byte(bs+64,32); |
267 | blocks[4] = bytebits_to_byte(bs+96,32); |
268 | |
269 | PrintAndLog("Preparing to clone Farpointe/Pyramid to T55x7 with Facility Code: %u, Card Number: %u", facilitycode, cardnumber); |
270 | PrintAndLog("Blk | Data "); |
271 | PrintAndLog("----+------------"); |
272 | for ( i = 0; i<5; ++i ) |
273 | PrintAndLog(" %02d | %08" PRIx32, i, blocks[i]); |
274 | |
275 | UsbCommand resp; |
276 | UsbCommand c = {CMD_T55XX_WRITE_BLOCK, {0,0,0}}; |
277 | |
278 | for ( i = 0; i<5; ++i ) { |
279 | c.arg[0] = blocks[i]; |
280 | c.arg[1] = i; |
281 | clearCommandBuffer(); |
282 | SendCommand(&c); |
283 | if (!WaitForResponseTimeout(CMD_ACK, &resp, 1000)){ |
284 | PrintAndLog("Error occurred, device did not respond during write operation."); |
285 | return -1; |
286 | } |
287 | } |
29ada8fc |
288 | return 0; |
6923d3f1 |
289 | } |
290 | |
291 | int CmdPyramidSim(const char *Cmd) { |
292 | |
293 | char cmdp = param_getchar(Cmd, 0); |
294 | if (strlen(Cmd) == 0 || cmdp == 'h' || cmdp == 'H') return usage_lf_pyramid_sim(); |
295 | |
296 | uint32_t facilitycode = 0, cardnumber = 0, fc = 0, cn = 0; |
29ada8fc |
297 | |
6923d3f1 |
298 | uint8_t bs[128]; |
299 | size_t size = sizeof(bs); |
300 | memset(bs, 0x00, size); |
29ada8fc |
301 | |
6923d3f1 |
302 | // Pyramid uses: fcHigh: 10, fcLow: 8, clk: 50, invert: 0 |
303 | uint64_t arg1, arg2; |
304 | arg1 = (10 << 8) + 8; |
305 | arg2 = 50 | 0; |
306 | |
307 | if (sscanf(Cmd, "%u %u", &fc, &cn ) != 2) return usage_lf_pyramid_sim(); |
308 | |
309 | facilitycode = (fc & 0x000000FF); |
310 | cardnumber = (cn & 0x0000FFFF); |
29ada8fc |
311 | |
6923d3f1 |
312 | if ( !GetPyramidBits(facilitycode, cardnumber, bs)) { |
313 | PrintAndLog("Error with tag bitstream generation."); |
314 | return 1; |
29ada8fc |
315 | } |
6923d3f1 |
316 | |
317 | PrintAndLog("Simulating Farpointe/Pyramid - Facility Code: %u, CardNumber: %u", facilitycode, cardnumber ); |
29ada8fc |
318 | |
6923d3f1 |
319 | UsbCommand c = {CMD_FSK_SIM_TAG, {arg1, arg2, size}}; |
320 | memcpy(c.d.asBytes, bs, size); |
321 | clearCommandBuffer(); |
322 | SendCommand(&c); |
323 | return 0; |
324 | } |
325 | |
326 | static command_t CommandTable[] = { |
cf4640b9 |
327 | {"help", CmdHelp, 1, "This help"}, |
328 | {"demod", CmdFSKdemodPyramid, 1, "Demodulate a Pyramid FSK tag from the GraphBuffer"}, |
329 | {"read", CmdPyramidRead, 0, "Attempt to read and extract tag data"}, |
330 | {"clone", CmdPyramidClone, 0, "<Facility-Code> <Card Number> clone pyramid tag"}, |
331 | {"sim", CmdPyramidSim, 0, "<Facility-Code> <Card Number> simulate pyramid tag"}, |
29ada8fc |
332 | {NULL, NULL, 0, NULL} |
6923d3f1 |
333 | }; |
334 | |
335 | int CmdLFPyramid(const char *Cmd) { |
336 | clearCommandBuffer(); |
29ada8fc |
337 | CmdsParse(CommandTable, Cmd); |
338 | return 0; |
6923d3f1 |
339 | } |
340 | |
341 | int CmdHelp(const char *Cmd) { |
29ada8fc |
342 | CmdsHelp(CommandTable); |
343 | return 0; |
6923d3f1 |
344 | } |