]> cvs.zerfleddert.de Git - proxmark3-svn/blob - client/ui.c
FIXED: lf t55xx fsk now demods but only to binary.
[proxmark3-svn] / client / ui.c
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
22 //#include <liquid/liquid.h>
23 #define M_PI 3.14159265358979323846264338327
24
25 double CursorScaleFactor;
26 int PlotGridX, PlotGridY, PlotGridXdefault= 64, PlotGridYdefault= 64;
27 int offline;
28 int flushAfterWrite = 0; //buzzy
29 extern pthread_mutex_t print_lock;
30
31 static char *logfilename = "proxmark3.log";
32
33 void PrintAndLog(char *fmt, ...)
34 {
35 char *saved_line;
36 int saved_point;
37 va_list argptr, argptr2;
38 static FILE *logfile = NULL;
39 static int logging=1;
40
41 // lock this section to avoid interlacing prints from different threats
42 pthread_mutex_lock(&print_lock);
43
44 if (logging && !logfile) {
45 logfile=fopen(logfilename, "a");
46 if (!logfile) {
47 fprintf(stderr, "Can't open logfile, logging disabled!\n");
48 logging=0;
49 }
50 }
51
52 int need_hack = (rl_readline_state & RL_STATE_READCMD) > 0;
53
54 if (need_hack) {
55 saved_point = rl_point;
56 saved_line = rl_copy_text(0, rl_end);
57 rl_save_prompt();
58 rl_replace_line("", 0);
59 rl_redisplay();
60 }
61
62 va_start(argptr, fmt);
63 va_copy(argptr2, argptr);
64 vprintf(fmt, argptr);
65 printf(" "); // cleaning prompt
66 va_end(argptr);
67 printf("\n");
68
69 if (need_hack) {
70 rl_restore_prompt();
71 rl_replace_line(saved_line, 0);
72 rl_point = saved_point;
73 rl_redisplay();
74 free(saved_line);
75 }
76
77 if (logging && logfile) {
78 vfprintf(logfile, fmt, argptr2);
79 fprintf(logfile,"\n");
80 fflush(logfile);
81 }
82 va_end(argptr2);
83
84 if (flushAfterWrite == 1) //buzzy
85 {
86 fflush(NULL);
87 }
88 //release lock
89 pthread_mutex_unlock(&print_lock);
90 }
91
92 void SetLogFilename(char *fn)
93 {
94 logfilename = fn;
95 }
96
97 int manchester_decode( int * data, const size_t len, uint8_t * dataout){
98
99 int bitlength = 0;
100 int i, clock, high, low, startindex;
101 low = startindex = 0;
102 high = 1;
103 uint8_t bitStream[len];
104
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 PrintAndLog(" startindex : %d", startindex);
121
122 if (high != 1)
123 bitlength = ManchesterConvertFrom255(data, len, bitStream, high, low, clock, startindex);
124 else
125 bitlength= ManchesterConvertFrom1(data, len, bitStream, clock, startindex);
126
127 memcpy(dataout, bitStream, bitlength);
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 {
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 void iceFsk(int * data, const size_t len){
396
397 //34359738 == 125khz (2^32 / 125) =
398
399 // parameters
400 float phase_offset = 0.00f; // carrier phase offset
401 float frequency_offset = 0.30f; // carrier frequency offset
402 float wn = 0.01f; // pll bandwidth
403 float zeta = 0.707f; // pll damping factor
404 float K = 1000; // pll loop gain
405 size_t n = len; // number of samples
406
407 // generate loop filter parameters (active PI design)
408 float t1 = K/(wn*wn); // tau_1
409 float t2 = 2*zeta/wn; // tau_2
410
411 // feed-forward coefficients (numerator)
412 float b0 = (4*K/t1)*(1.+t2/2.0f);
413 float b1 = (8*K/t1);
414 float b2 = (4*K/t1)*(1.-t2/2.0f);
415
416 // feed-back coefficients (denominator)
417 // a0 = 1.0 is implied
418 float a1 = -2.0f;
419 float a2 = 1.0f;
420
421 // filter buffer
422 float v0=0.0f, v1=0.0f, v2=0.0f;
423
424 // initialize states
425 float phi = phase_offset; // input signal's initial phase
426 float phi_hat = 0.0f; // PLL's initial phase
427
428 unsigned int i;
429 float complex x,y;
430 float complex output[n];
431
432 for (i=0; i<n; i++) {
433 // INPUT SIGNAL
434 x = data[i];
435 phi += frequency_offset;
436
437 // generate complex sinusoid
438 y = cosf(phi_hat) + _Complex_I*sinf(phi_hat);
439
440 output[i] = y;
441
442 // compute error estimate
443 float delta_phi = cargf( x * conjf(y) );
444
445
446 // print results to standard output
447 printf(" %6u %12.8f %12.8f %12.8f %12.8f %12.8f\n",
448 i,
449 crealf(x), cimagf(x),
450 crealf(y), cimagf(y),
451 delta_phi);
452
453 // push result through loop filter, updating phase estimate
454
455 // advance buffer
456 v2 = v1; // shift center register to upper register
457 v1 = v0; // shift lower register to center register
458
459 // compute new lower register
460 v0 = delta_phi - v1*a1 - v2*a2;
461
462 // compute new output
463 phi_hat = v0*b0 + v1*b1 + v2*b2;
464
465 }
466
467 for (i=0; i<len; ++i){
468 data[i] = (int)crealf(output[i]);
469 }
470 }
471
472 /* Sliding DFT
473 Smooths out
474 */
475 void iceFsk2(int * data, const size_t len){
476
477 int i, j;
478 int output[len];
479
480 // for (i=0; i<len-5; ++i){
481 // for ( j=1; j <=5; ++j) {
482 // output[i] += data[i*j];
483 // }
484 // output[i] /= 5;
485 // }
486 int rest = 127;
487 int tmp =0;
488 for (i=0; i<len; ++i){
489 if ( data[i] < 127)
490 output[i] = 0;
491 else {
492 tmp = (100 * (data[i]-rest)) / rest;
493 output[i] = (tmp > 60)? 100:0;
494 }
495 }
496
497 for (j=0; j<len; ++j)
498 data[j] = output[j];
499 }
500
501 void iceFsk3(int * data, const size_t len){
502
503 int i,j;
504 int output[len];
505 float fc = 0.1125f; // center frequency
506
507 // create very simple low-pass filter to remove images (2nd-order Butterworth)
508 float complex iir_buf[3] = {0,0,0};
509 float b[3] = {0.003621681514929, 0.007243363029857, 0.003621681514929};
510 float a[3] = {1.000000000000000, -1.822694925196308, 0.837181651256023};
511
512 // process entire input file one sample at a time
513 float sample = 0; // input sample read from file
514 float complex x_prime = 1.0f; // save sample for estimating frequency
515 float complex x;
516
517 for (i=0; i<len; ++i) {
518
519 sample = data[i];
520
521 // remove DC offset and mix to complex baseband
522 x = (sample - 127.5f) * cexpf( _Complex_I * 2 * M_PI * fc * i );
523
524 // apply low-pass filter, removing spectral image (IIR using direct-form II)
525 iir_buf[2] = iir_buf[1];
526 iir_buf[1] = iir_buf[0];
527 iir_buf[0] = x - a[1]*iir_buf[1] - a[2]*iir_buf[2];
528 x = b[0]*iir_buf[0] +
529 b[1]*iir_buf[1] +
530 b[2]*iir_buf[2];
531
532 // compute instantaneous frequency by looking at phase difference
533 // between adjacent samples
534 float freq = cargf(x*conjf(x_prime));
535 x_prime = x; // retain this sample for next iteration
536
537 output[i] =(freq > 0)? 10 : -10;
538 }
539
540 // show data
541 for (j=0; j<len; ++j)
542 data[j] = output[j];
543
544 CmdLtrim("30");
545
546 // zero crossings.
547 for (j=0; j<len; ++j){
548 if ( data[j] == 10) break;
549 }
550 int startOne =j;
551
552 for (;j<len; ++j){
553 if ( data[j] == -10 ) break;
554 }
555 int stopOne = j-1;
556
557 int fieldlen = stopOne-startOne;
558
559 fieldlen = (fieldlen == 39 || fieldlen == 41)? 40 : fieldlen;
560 fieldlen = (fieldlen == 59 || fieldlen == 51)? 50 : fieldlen;
561 if ( fieldlen != 40 && fieldlen != 50){
562 printf("Detected field Length: %d \n", fieldlen);
563 printf("Can only handle len 40 or 50. Aborting...");
564 return;
565 }
566
567 // FSK sequence start == 000111
568 int startPos = 0;
569 for (i =0; i<len; ++i){
570 int dec = 0;
571 for ( j = 0; j < 6*fieldlen; ++j){
572 dec += data[i + j];
573 }
574 if (dec == 0) {
575 startPos = i;
576 break;
577 }
578 }
579
580 printf("000111 position: %d \n", startPos);
581
582 startPos += 6*fieldlen+5;
583
584 int bit =0;
585 printf("BINARY\n");
586 printf("R/40 : ");
587 for (i =startPos ; i < len; i += 40){
588 bit = data[i]>0 ? 1:0;
589 printf("%d", bit );
590 }
591 printf("\n");
592
593 printf("R/50 : ");
594 for (i =startPos ; i < len; i += 50){
595 bit = data[i]>0 ? 1:0;
596 printf("%d", bit ); }
597 printf("\n");
598
599 }
600
601 float complex cexpf (float complex Z)
602 {
603 float complex Res;
604 double rho = exp (__real__ Z);
605 __real__ Res = rho * cosf(__imag__ Z);
606 __imag__ Res = rho * sinf(__imag__ Z);
607 return Res;
608 }
Impressum, Datenschutz