]>
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[len]; | |
105 | ||
106 | memset(bitStream, 0x00, len); | |
107 | ||
108 | /* Detect high and lows */ | |
109 | for (i = 0; i < len; i++) { | |
110 | if (data[i] > high) | |
111 | high = data[i]; | |
112 | else if (data[i] < low) | |
113 | low = data[i]; | |
114 | } | |
115 | ||
116 | /* get clock */ | |
117 | clock = GetT55x7Clock( data, len, high ); | |
118 | startindex = DetectFirstTransition(data, len, high); | |
119 | ||
120 | PrintAndLog(" Clock : %d", clock); | |
121 | //PrintAndLog(" startindex : %d", startindex); | |
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 | ||
128 | memcpy(dataout, bitStream, bitlength); | |
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) { | |
148 | /* if this is the beginning of a peak */ | |
149 | if ( data[i-1] != data[i] && data[i] == peak) { | |
150 | /* find lowest difference between peaks */ | |
151 | if (lastpeak && i - lastpeak < clock) | |
152 | clock = i - lastpeak; | |
153 | lastpeak = i; | |
154 | } | |
155 | } | |
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 | } | |
171 | ||
172 | //PrintAndLog(" Found Clock : %d - trying to adjust", clock); | |
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; | |
182 | } | |
183 | ||
184 | int DetectFirstTransition(const int * data, const size_t len, int threshold){ | |
185 | ||
186 | int i =0; | |
187 | /* now look for the first threshold */ | |
188 | for (; i < len; ++i) { | |
189 | if (data[i] == threshold) { | |
190 | break; | |
191 | } | |
192 | } | |
193 | return i; | |
194 | } | |
195 | ||
196 | int ManchesterConvertFrom255(const int * data, const size_t len, uint8_t * dataout, int high, int low, int clock, int startIndex){ | |
197 | ||
198 | int i, j, z, hithigh, hitlow, bitIndex, startType; | |
199 | i = 0; | |
200 | bitIndex = 0; | |
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; | |
207 | ||
208 | // i = clock frame of data | |
209 | for (; i < (int)(len / clock); i++) | |
210 | { | |
211 | hithigh = 0; | |
212 | hitlow = 0; | |
213 | startType = -1; | |
214 | z = startIndex + (i*clock); | |
215 | isDamp = 0; | |
216 | ||
217 | /* Find out if we hit both high and low peaks */ | |
218 | for (j = 0; j < clock; j++) | |
219 | { | |
220 | if (data[z+j] == high){ | |
221 | hithigh = 1; | |
222 | if ( startType == -1) | |
223 | startType = 1; | |
224 | } | |
225 | ||
226 | if (data[z+j] == low ){ | |
227 | hitlow = 1; | |
228 | if ( startType == -1) | |
229 | startType = 0; | |
230 | } | |
231 | ||
232 | if (hithigh && hitlow) | |
233 | break; | |
234 | } | |
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); | |
239 | for (j = 0; j < clock; j++) { | |
240 | if ( | |
241 | (data[z+j] <= dampHi && data[z+j] >= dampLow) | |
242 | ){ | |
243 | isDamp++; | |
244 | } | |
245 | } | |
246 | } | |
247 | ||
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 | ||
259 | if ( isDamp > clock/2 ) { | |
260 | firstST++; | |
261 | } | |
262 | ||
263 | if ( firstST == 4) | |
264 | break; | |
265 | } | |
266 | return bitIndex; | |
267 | } | |
268 | ||
269 | int ManchesterConvertFrom1(const int * data, const size_t len, uint8_t * dataout, int clock, int startIndex){ | |
270 | ||
271 | PrintAndLog(" Path B"); | |
272 | ||
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 | ||
281 | /* Detect duration between 2 successive transitions */ | |
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; | |
309 | } | |
310 | } | |
311 | } | |
312 | } | |
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 { | |
325 | i++; | |
326 | warnings++; | |
327 | PrintAndLog("Unsynchronized, resync..."); | |
328 | PrintAndLog("(too many of those messages mean the stream is not Manchester encoded)"); | |
329 | ||
330 | if (warnings > 10) { | |
331 | PrintAndLog("Error: too many decode errors, aborting."); | |
332 | return 0; | |
333 | } | |
334 | } | |
335 | } | |
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]; | |
348 | ||
349 | j = warnings = 0; | |
350 | ||
351 | uint8_t lastbit = 0; | |
352 | ||
353 | for (i = 0; i < len; i += 2) { | |
354 | ||
355 | uint8_t first = bitstream[i]; | |
356 | uint8_t second = bitstream[i+1]; | |
357 | ||
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 | ||
378 | void PrintPaddedManchester( uint8_t* bitStream, size_t len, size_t blocksize){ | |
379 | ||
380 | PrintAndLog(" Manchester decoded : %d bits", len); | |
381 | ||
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) { | |
388 | PrintAndLog(" %s", sprint_bin(bitStream+i,blocksize) ); | |
389 | } | |
390 | ||
391 | if ( mod > 0 ) | |
392 | PrintAndLog(" %s", sprint_bin(bitStream+i, mod) ); | |
393 | } | |
394 | ||
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]; | |
428 | float fc = 0.1125f; // center frequency | |
429 | size_t adjustedLen = len; | |
430 | ||
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 | ||
436 | float sample = 0; // input sample read from file | |
437 | float complex x_prime = 1.0f; // save sample for estimating frequency | |
438 | float complex x; | |
439 | ||
440 | for (i=0; i<adjustedLen; ++i) { | |
441 | ||
442 | sample = data[i]+128; | |
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 | |
464 | for (j=0; j<adjustedLen; ++j) | |
465 | data[j] = output[j]; | |
466 | ||
467 | CmdLtrim("30"); | |
468 | adjustedLen -= 30; | |
469 | ||
470 | // zero crossings. | |
471 | for (j=0; j<adjustedLen; ++j){ | |
472 | if ( data[j] == 10) break; | |
473 | } | |
474 | int startOne =j; | |
475 | ||
476 | for (;j<adjustedLen; ++j){ | |
477 | if ( data[j] == -10 ) break; | |
478 | } | |
479 | int stopOne = j-1; | |
480 | ||
481 | int fieldlen = stopOne-startOne; | |
482 | ||
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); | |
487 | printf("Can only handle 40 or 50. Aborting...\n"); | |
488 | return; | |
489 | } | |
490 | ||
491 | // FSK sequence start == 000111 | |
492 | int startPos = 0; | |
493 | for (i =0; i<adjustedLen; ++i){ | |
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 | ||
506 | startPos += 6*fieldlen+5; | |
507 | ||
508 | int bit =0; | |
509 | printf("BINARY\n"); | |
510 | printf("R/40 : "); | |
511 | for (i =startPos ; i < adjustedLen; i += 40){ | |
512 | bit = data[i]>0 ? 1:0; | |
513 | printf("%d", bit ); | |
514 | } | |
515 | printf("\n"); | |
516 | ||
517 | printf("R/50 : "); | |
518 | for (i =startPos ; i < adjustedLen; i += 50){ | |
519 | bit = data[i]>0 ? 1:0; | |
520 | printf("%d", bit ); } | |
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 | } |