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