]>
Commit | Line | Data |
---|---|---|
a553f267 | 1 | //----------------------------------------------------------------------------- |
212ef3a0 | 2 | // Copyright (C) 2009 Michael Gernoth <michael at gernoth.net> |
a553f267 | 3 | // Copyright (C) 2010 iZsh <izsh at fail0verflow.com> |
4 | // | |
5 | // This code is licensed to you under the terms of the GNU GPL, version 2 or, | |
6 | // at your option, any later version. See the LICENSE.txt file for the text of | |
7 | // the license. | |
8 | //----------------------------------------------------------------------------- | |
9 | // UI utilities | |
10 | //----------------------------------------------------------------------------- | |
11 | ||
7fe9b0b7 | 12 | #include <stdarg.h> |
51969283 | 13 | #include <stdlib.h> |
7fe9b0b7 | 14 | #include <stdio.h> |
f6c18637 | 15 | #include <stdbool.h> |
7fe9b0b7 | 16 | #include <time.h> |
51969283 | 17 | #include <readline/readline.h> |
9492e0b0 | 18 | #include <pthread.h> |
f6c18637 | 19 | #include "loclass/cipherutils.h" |
7bd30f12 | 20 | #include "ui.h" |
081151ea | 21 | #include "cmdmain.h" |
22 | #include "cmddata.h" | |
7bd30f12 | 23 | //#include <liquid/liquid.h> |
24 | #define M_PI 3.14159265358979323846264338327 | |
7fe9b0b7 | 25 | |
26 | double CursorScaleFactor; | |
7ddb9900 | 27 | int PlotGridX, PlotGridY, PlotGridXdefault= 64, PlotGridYdefault= 64; |
7fe9b0b7 | 28 | int offline; |
ed77aabe | 29 | int flushAfterWrite = 0; //buzzy |
9492e0b0 | 30 | extern pthread_mutex_t print_lock; |
31 | ||
7fe9b0b7 | 32 | static char *logfilename = "proxmark3.log"; |
33 | ||
34 | void PrintAndLog(char *fmt, ...) | |
35 | { | |
51969283 M |
36 | char *saved_line; |
37 | int saved_point; | |
9492e0b0 | 38 | va_list argptr, argptr2; |
39 | static FILE *logfile = NULL; | |
40 | static int logging=1; | |
7fe9b0b7 | 41 | |
9492e0b0 | 42 | // lock this section to avoid interlacing prints from different threats |
43 | pthread_mutex_lock(&print_lock); | |
44 | ||
45 | if (logging && !logfile) { | |
46 | logfile=fopen(logfilename, "a"); | |
47 | if (!logfile) { | |
48 | fprintf(stderr, "Can't open logfile, logging disabled!\n"); | |
49 | logging=0; | |
50 | } | |
51 | } | |
51969283 M |
52 | |
53 | int need_hack = (rl_readline_state & RL_STATE_READCMD) > 0; | |
7fe9b0b7 | 54 | |
51969283 M |
55 | if (need_hack) { |
56 | saved_point = rl_point; | |
57 | saved_line = rl_copy_text(0, rl_end); | |
58 | rl_save_prompt(); | |
59 | rl_replace_line("", 0); | |
60 | rl_redisplay(); | |
61 | } | |
62 | ||
9492e0b0 | 63 | va_start(argptr, fmt); |
64 | va_copy(argptr2, argptr); | |
65 | vprintf(fmt, argptr); | |
66 | printf(" "); // cleaning prompt | |
67 | va_end(argptr); | |
68 | printf("\n"); | |
51969283 M |
69 | |
70 | if (need_hack) { | |
71 | rl_restore_prompt(); | |
72 | rl_replace_line(saved_line, 0); | |
73 | rl_point = saved_point; | |
74 | rl_redisplay(); | |
75 | free(saved_line); | |
76 | } | |
77 | ||
9492e0b0 | 78 | if (logging && logfile) { |
79 | vfprintf(logfile, fmt, argptr2); | |
80 | fprintf(logfile,"\n"); | |
81 | fflush(logfile); | |
82 | } | |
83 | va_end(argptr2); | |
84 | ||
ed77aabe | 85 | if (flushAfterWrite == 1) //buzzy |
86 | { | |
87 | fflush(NULL); | |
88 | } | |
9492e0b0 | 89 | //release lock |
90 | pthread_mutex_unlock(&print_lock); | |
7fe9b0b7 | 91 | } |
92 | ||
93 | void SetLogFilename(char *fn) | |
94 | { | |
95 | logfilename = fn; | |
96 | } | |
f38a1528 | 97 | |
f6c18637 | 98 | int manchester_decode( int * data, const size_t len, uint8_t * dataout){ |
f38a1528 | 99 | |
b44e5233 | 100 | int bitlength = 0; |
101 | int i, clock, high, low, startindex; | |
102 | low = startindex = 0; | |
f38a1528 | 103 | high = 1; |
b44e5233 | 104 | uint8_t bitStream[len]; |
f6c18637 | 105 | |
106 | memset(bitStream, 0x00, len); | |
b44e5233 | 107 | |
f38a1528 | 108 | /* Detect high and lows */ |
b44e5233 | 109 | for (i = 0; i < len; i++) { |
f38a1528 | 110 | if (data[i] > high) |
111 | high = data[i]; | |
112 | else if (data[i] < low) | |
113 | low = data[i]; | |
114 | } | |
115 | ||
116 | /* get clock */ | |
b44e5233 | 117 | clock = GetT55x7Clock( data, len, high ); |
f6c18637 | 118 | startindex = DetectFirstTransition(data, len, high); |
b44e5233 | 119 | |
72e930ef | 120 | PrintAndLog(" Clock : %d", clock); |
081151ea | 121 | //PrintAndLog(" startindex : %d", startindex); |
b44e5233 | 122 | |
123 | if (high != 1) | |
124 | bitlength = ManchesterConvertFrom255(data, len, bitStream, high, low, clock, startindex); | |
125 | else | |
126 | bitlength= ManchesterConvertFrom1(data, len, bitStream, clock, startindex); | |
127 | ||
b44e5233 | 128 | memcpy(dataout, bitStream, bitlength); |
b44e5233 | 129 | return bitlength; |
130 | } | |
131 | ||
132 | int GetT55x7Clock( const int * data, const size_t len, int peak ){ | |
133 | ||
134 | int i,lastpeak,clock; | |
135 | clock = 0xFFFF; | |
136 | lastpeak = 0; | |
137 | ||
138 | /* Detect peak if we don't have one */ | |
139 | if (!peak) { | |
140 | for (i = 0; i < len; ++i) { | |
141 | if (data[i] > peak) { | |
142 | peak = data[i]; | |
143 | } | |
144 | } | |
145 | } | |
146 | ||
147 | for (i = 1; i < len; ++i) { | |
f38a1528 | 148 | /* if this is the beginning of a peak */ |
b44e5233 | 149 | if ( data[i-1] != data[i] && data[i] == peak) { |
f38a1528 | 150 | /* find lowest difference between peaks */ |
151 | if (lastpeak && i - lastpeak < clock) | |
152 | clock = i - lastpeak; | |
153 | lastpeak = i; | |
154 | } | |
155 | } | |
b44e5233 | 156 | //return clock; |
157 | //defaults clock to precise values. | |
158 | switch(clock){ | |
159 | case 8: | |
160 | case 16: | |
161 | case 32: | |
162 | case 40: | |
163 | case 50: | |
164 | case 64: | |
165 | case 100: | |
166 | case 128: | |
167 | return clock; | |
168 | break; | |
169 | default: break; | |
170 | } | |
f6c18637 | 171 | |
77376577 | 172 | //PrintAndLog(" Found Clock : %d - trying to adjust", clock); |
f6c18637 | 173 | |
174 | // When detected clock is 31 or 33 then then return | |
175 | int clockmod = clock%8; | |
176 | if ( clockmod == 7 ) | |
177 | clock += 1; | |
178 | else if ( clockmod == 1 ) | |
179 | clock -= 1; | |
180 | ||
181 | return clock; | |
b44e5233 | 182 | } |
183 | ||
f6c18637 | 184 | int DetectFirstTransition(const int * data, const size_t len, int threshold){ |
b44e5233 | 185 | |
f6c18637 | 186 | int i =0; |
187 | /* now look for the first threshold */ | |
188 | for (; i < len; ++i) { | |
189 | if (data[i] == threshold) { | |
f38a1528 | 190 | break; |
191 | } | |
f6c18637 | 192 | } |
193 | return i; | |
b44e5233 | 194 | } |
195 | ||
196 | int ManchesterConvertFrom255(const int * data, const size_t len, uint8_t * dataout, int high, int low, int clock, int startIndex){ | |
197 | ||
f6c18637 | 198 | int i, j, z, hithigh, hitlow, bitIndex, startType; |
199 | i = 0; | |
b44e5233 | 200 | bitIndex = 0; |
f6c18637 | 201 | |
202 | int isDamp = 0; | |
203 | int damplimit = (int)((high / 2) * 0.3); | |
204 | int dampHi = (high/2)+damplimit; | |
205 | int dampLow = (high/2)-damplimit; | |
206 | int firstST = 0; | |
b44e5233 | 207 | |
f6c18637 | 208 | // i = clock frame of data |
b44e5233 | 209 | for (; i < (int)(len / clock); i++) |
f38a1528 | 210 | { |
f38a1528 | 211 | hithigh = 0; |
212 | hitlow = 0; | |
f6c18637 | 213 | startType = -1; |
214 | z = startIndex + (i*clock); | |
215 | isDamp = 0; | |
77376577 | 216 | |
f38a1528 | 217 | /* Find out if we hit both high and low peaks */ |
218 | for (j = 0; j < clock; j++) | |
f6c18637 | 219 | { |
220 | if (data[z+j] == high){ | |
f38a1528 | 221 | hithigh = 1; |
f6c18637 | 222 | if ( startType == -1) |
223 | startType = 1; | |
224 | } | |
225 | ||
226 | if (data[z+j] == low ){ | |
f38a1528 | 227 | hitlow = 1; |
f6c18637 | 228 | if ( startType == -1) |
229 | startType = 0; | |
230 | } | |
231 | ||
f38a1528 | 232 | if (hithigh && hitlow) |
233 | break; | |
b44e5233 | 234 | } |
f6c18637 | 235 | |
236 | // No high value found, are we in a dampening field? | |
237 | if ( !hithigh ) { | |
238 | //PrintAndLog(" # Entering damp test at index : %d (%d)", z+j, j); | |
081151ea | 239 | for (j = 0; j < clock; j++) { |
f6c18637 | 240 | if ( |
241 | (data[z+j] <= dampHi && data[z+j] >= dampLow) | |
242 | ){ | |
77376577 | 243 | isDamp++; |
f6c18637 | 244 | } |
f6c18637 | 245 | } |
246 | } | |
f38a1528 | 247 | |
f6c18637 | 248 | /* Manchester Switching.. |
249 | 0: High -> Low | |
250 | 1: Low -> High | |
251 | */ | |
252 | if (startType == 0) | |
253 | dataout[bitIndex++] = 1; | |
254 | else if (startType == 1) | |
255 | dataout[bitIndex++] = 0; | |
256 | else | |
257 | dataout[bitIndex++] = 2; | |
258 | ||
77376577 | 259 | if ( isDamp > clock/2 ) { |
f6c18637 | 260 | firstST++; |
261 | } | |
262 | ||
263 | if ( firstST == 4) | |
264 | break; | |
f38a1528 | 265 | } |
b44e5233 | 266 | return bitIndex; |
267 | } | |
268 | ||
269 | int ManchesterConvertFrom1(const int * data, const size_t len, uint8_t * dataout, int clock, int startIndex){ | |
270 | ||
f6c18637 | 271 | PrintAndLog(" Path B"); |
272 | ||
b44e5233 | 273 | int i,j, bitindex, lc, tolerance, warnings; |
274 | warnings = 0; | |
275 | int upperlimit = len*2/clock+8; | |
276 | i = startIndex; | |
277 | j = 0; | |
278 | tolerance = clock/4; | |
279 | uint8_t decodedArr[len]; | |
280 | ||
f6c18637 | 281 | /* Detect duration between 2 successive transitions */ |
b44e5233 | 282 | for (bitindex = 1; i < len; i++) { |
283 | ||
284 | if (data[i-1] != data[i]) { | |
285 | lc = i - startIndex; | |
286 | startIndex = i; | |
287 | ||
288 | // Error check: if bitindex becomes too large, we do not | |
289 | // have a Manchester encoded bitstream or the clock is really wrong! | |
290 | if (bitindex > upperlimit ) { | |
291 | PrintAndLog("Error: the clock you gave is probably wrong, aborting."); | |
292 | return 0; | |
293 | } | |
294 | // Then switch depending on lc length: | |
295 | // Tolerance is 1/4 of clock rate (arbitrary) | |
296 | if (abs((lc-clock)/2) < tolerance) { | |
297 | // Short pulse : either "1" or "0" | |
298 | decodedArr[bitindex++] = data[i-1]; | |
299 | } else if (abs(lc-clock) < tolerance) { | |
300 | // Long pulse: either "11" or "00" | |
301 | decodedArr[bitindex++] = data[i-1]; | |
302 | decodedArr[bitindex++] = data[i-1]; | |
303 | } else { | |
304 | ++warnings; | |
305 | PrintAndLog("Warning: Manchester decode error for pulse width detection."); | |
306 | if (warnings > 10) { | |
307 | PrintAndLog("Error: too many detection errors, aborting."); | |
308 | return 0; | |
f38a1528 | 309 | } |
310 | } | |
311 | } | |
312 | } | |
b44e5233 | 313 | |
314 | /* | |
315 | * We have a decodedArr of "01" ("1") or "10" ("0") | |
316 | * parse it into final decoded dataout | |
317 | */ | |
318 | for (i = 0; i < bitindex; i += 2) { | |
319 | ||
320 | if ((decodedArr[i] == 0) && (decodedArr[i+1] == 1)) { | |
321 | dataout[j++] = 1; | |
322 | } else if ((decodedArr[i] == 1) && (decodedArr[i+1] == 0)) { | |
323 | dataout[j++] = 0; | |
324 | } else { | |
f38a1528 | 325 | i++; |
326 | warnings++; | |
327 | PrintAndLog("Unsynchronized, resync..."); | |
b44e5233 | 328 | PrintAndLog("(too many of those messages mean the stream is not Manchester encoded)"); |
329 | ||
330 | if (warnings > 10) { | |
f38a1528 | 331 | PrintAndLog("Error: too many decode errors, aborting."); |
332 | return 0; | |
333 | } | |
334 | } | |
335 | } | |
b44e5233 | 336 | |
337 | PrintAndLog("%s", sprint_hex(dataout, j)); | |
338 | return j; | |
339 | } | |
340 | ||
341 | void ManchesterDiffDecodedString(const uint8_t* bitstream, size_t len, uint8_t invert){ | |
342 | /* | |
343 | * We have a bitstream of "01" ("1") or "10" ("0") | |
344 | * parse it into final decoded bitstream | |
345 | */ | |
346 | int i, j, warnings; | |
347 | uint8_t decodedArr[(len/2)+1]; | |
f38a1528 | 348 | |
b44e5233 | 349 | j = warnings = 0; |
f38a1528 | 350 | |
b44e5233 | 351 | uint8_t lastbit = 0; |
f38a1528 | 352 | |
b44e5233 | 353 | for (i = 0; i < len; i += 2) { |
354 | ||
355 | uint8_t first = bitstream[i]; | |
356 | uint8_t second = bitstream[i+1]; | |
f38a1528 | 357 | |
b44e5233 | 358 | if ( first == second ) { |
359 | ++i; | |
360 | ++warnings; | |
361 | if (warnings > 10) { | |
362 | PrintAndLog("Error: too many decode errors, aborting."); | |
363 | return; | |
364 | } | |
365 | } | |
366 | else if ( lastbit != first ) { | |
367 | decodedArr[j++] = 0 ^ invert; | |
368 | } | |
369 | else { | |
370 | decodedArr[j++] = 1 ^ invert; | |
371 | } | |
372 | lastbit = second; | |
373 | } | |
374 | ||
375 | PrintAndLog("%s", sprint_hex(decodedArr, j)); | |
376 | } | |
377 | ||
f38a1528 | 378 | void PrintPaddedManchester( uint8_t* bitStream, size_t len, size_t blocksize){ |
379 | ||
f6c18637 | 380 | PrintAndLog(" Manchester decoded : %d bits", len); |
f38a1528 | 381 | |
f6c18637 | 382 | uint8_t mod = len % blocksize; |
383 | uint8_t div = len / blocksize; | |
384 | int i; | |
385 | ||
386 | // Now output the bitstream to the scrollback by line of 16 bits | |
387 | for (i = 0; i < div*blocksize; i+=blocksize) { | |
f38a1528 | 388 | PrintAndLog(" %s", sprint_bin(bitStream+i,blocksize) ); |
f6c18637 | 389 | } |
390 | ||
391 | if ( mod > 0 ) | |
392 | PrintAndLog(" %s", sprint_bin(bitStream+i, mod) ); | |
7bd30f12 | 393 | } |
394 | ||
7bd30f12 | 395 | /* Sliding DFT |
396 | Smooths out | |
397 | */ | |
398 | void iceFsk2(int * data, const size_t len){ | |
399 | ||
400 | int i, j; | |
401 | int output[len]; | |
402 | ||
403 | // for (i=0; i<len-5; ++i){ | |
404 | // for ( j=1; j <=5; ++j) { | |
405 | // output[i] += data[i*j]; | |
406 | // } | |
407 | // output[i] /= 5; | |
408 | // } | |
409 | int rest = 127; | |
410 | int tmp =0; | |
411 | for (i=0; i<len; ++i){ | |
412 | if ( data[i] < 127) | |
413 | output[i] = 0; | |
414 | else { | |
415 | tmp = (100 * (data[i]-rest)) / rest; | |
416 | output[i] = (tmp > 60)? 100:0; | |
417 | } | |
418 | } | |
419 | ||
420 | for (j=0; j<len; ++j) | |
421 | data[j] = output[j]; | |
422 | } | |
423 | ||
424 | void iceFsk3(int * data, const size_t len){ | |
425 | ||
426 | int i,j; | |
427 | int output[len]; | |
081151ea | 428 | float fc = 0.1125f; // center frequency |
429 | size_t adjustedLen = len; | |
430 | ||
7bd30f12 | 431 | // create very simple low-pass filter to remove images (2nd-order Butterworth) |
432 | float complex iir_buf[3] = {0,0,0}; | |
433 | float b[3] = {0.003621681514929, 0.007243363029857, 0.003621681514929}; | |
434 | float a[3] = {1.000000000000000, -1.822694925196308, 0.837181651256023}; | |
435 | ||
081151ea | 436 | float sample = 0; // input sample read from file |
437 | float complex x_prime = 1.0f; // save sample for estimating frequency | |
7bd30f12 | 438 | float complex x; |
439 | ||
081151ea | 440 | for (i=0; i<adjustedLen; ++i) { |
7bd30f12 | 441 | |
081151ea | 442 | sample = data[i]+128; |
7bd30f12 | 443 | |
444 | // remove DC offset and mix to complex baseband | |
445 | x = (sample - 127.5f) * cexpf( _Complex_I * 2 * M_PI * fc * i ); | |
446 | ||
447 | // apply low-pass filter, removing spectral image (IIR using direct-form II) | |
448 | iir_buf[2] = iir_buf[1]; | |
449 | iir_buf[1] = iir_buf[0]; | |
450 | iir_buf[0] = x - a[1]*iir_buf[1] - a[2]*iir_buf[2]; | |
451 | x = b[0]*iir_buf[0] + | |
452 | b[1]*iir_buf[1] + | |
453 | b[2]*iir_buf[2]; | |
454 | ||
455 | // compute instantaneous frequency by looking at phase difference | |
456 | // between adjacent samples | |
457 | float freq = cargf(x*conjf(x_prime)); | |
458 | x_prime = x; // retain this sample for next iteration | |
459 | ||
460 | output[i] =(freq > 0)? 10 : -10; | |
461 | } | |
462 | ||
463 | // show data | |
081151ea | 464 | for (j=0; j<adjustedLen; ++j) |
7bd30f12 | 465 | data[j] = output[j]; |
466 | ||
467 | CmdLtrim("30"); | |
081151ea | 468 | adjustedLen -= 30; |
7bd30f12 | 469 | |
470 | // zero crossings. | |
081151ea | 471 | for (j=0; j<adjustedLen; ++j){ |
7bd30f12 | 472 | if ( data[j] == 10) break; |
473 | } | |
474 | int startOne =j; | |
475 | ||
081151ea | 476 | for (;j<adjustedLen; ++j){ |
7bd30f12 | 477 | if ( data[j] == -10 ) break; |
478 | } | |
479 | int stopOne = j-1; | |
480 | ||
481 | int fieldlen = stopOne-startOne; | |
7bd30f12 | 482 | |
fbceacc5 | 483 | fieldlen = (fieldlen == 39 || fieldlen == 41)? 40 : fieldlen; |
484 | fieldlen = (fieldlen == 59 || fieldlen == 51)? 50 : fieldlen; | |
485 | if ( fieldlen != 40 && fieldlen != 50){ | |
486 | printf("Detected field Length: %d \n", fieldlen); | |
081151ea | 487 | printf("Can only handle 40 or 50. Aborting...\n"); |
fbceacc5 | 488 | return; |
489 | } | |
7bd30f12 | 490 | |
491 | // FSK sequence start == 000111 | |
492 | int startPos = 0; | |
081151ea | 493 | for (i =0; i<adjustedLen; ++i){ |
7bd30f12 | 494 | int dec = 0; |
495 | for ( j = 0; j < 6*fieldlen; ++j){ | |
496 | dec += data[i + j]; | |
497 | } | |
498 | if (dec == 0) { | |
499 | startPos = i; | |
500 | break; | |
501 | } | |
502 | } | |
503 | ||
504 | printf("000111 position: %d \n", startPos); | |
505 | ||
72e930ef | 506 | startPos += 6*fieldlen+5; |
7bd30f12 | 507 | |
72e930ef | 508 | int bit =0; |
7bd30f12 | 509 | printf("BINARY\n"); |
510 | printf("R/40 : "); | |
081151ea | 511 | for (i =startPos ; i < adjustedLen; i += 40){ |
72e930ef | 512 | bit = data[i]>0 ? 1:0; |
513 | printf("%d", bit ); | |
7bd30f12 | 514 | } |
515 | printf("\n"); | |
516 | ||
517 | printf("R/50 : "); | |
081151ea | 518 | for (i =startPos ; i < adjustedLen; i += 50){ |
72e930ef | 519 | bit = data[i]>0 ? 1:0; |
520 | printf("%d", bit ); } | |
7bd30f12 | 521 | printf("\n"); |
522 | ||
523 | } | |
524 | ||
525 | float complex cexpf (float complex Z) | |
526 | { | |
527 | float complex Res; | |
528 | double rho = exp (__real__ Z); | |
529 | __real__ Res = rho * cosf(__imag__ Z); | |
530 | __imag__ Res = rho * sinf(__imag__ Z); | |
531 | return Res; | |
532 | } |