]> git.zerfleddert.de Git - proxmark3-svn/blame - client/ui.c
Some more nasty bugs fixed in the lf t55xx manchester_decode method.
[proxmark3-svn] / client / ui.c
CommitLineData
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
26double CursorScaleFactor;
7ddb9900 27int PlotGridX, PlotGridY, PlotGridXdefault= 64, PlotGridYdefault= 64;
7fe9b0b7 28int offline;
ed77aabe 29int flushAfterWrite = 0; //buzzy
9492e0b0 30extern pthread_mutex_t print_lock;
31
7fe9b0b7 32static char *logfilename = "proxmark3.log";
33
34void 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
93void SetLogFilename(char *fn)
94{
95 logfilename = fn;
96}
f38a1528 97
c6be64da 98int manchester_decode( int * data, const size_t len, uint8_t * dataout, size_t dataoutlen){
f38a1528 99
b44e5233 100 int bitlength = 0;
101 int i, clock, high, low, startindex;
102 low = startindex = 0;
f38a1528 103 high = 1;
c6be64da 104 uint8_t * bitStream = (uint8_t* ) malloc(sizeof(uint8_t) * dataoutlen);
105 memset(bitStream, 0x00, dataoutlen);
b44e5233 106
f38a1528 107 /* Detect high and lows */
b44e5233 108 for (i = 0; i < len; i++) {
f38a1528 109 if (data[i] > high)
110 high = data[i];
111 else if (data[i] < low)
112 low = data[i];
113 }
114
115 /* get clock */
b44e5233 116 clock = GetT55x7Clock( data, len, high );
f6c18637 117 startindex = DetectFirstTransition(data, len, high);
b44e5233 118
c6be64da 119 //PrintAndLog(" Clock : %d", clock);
149aeada 120
b44e5233 121 if (high != 1)
c6be64da 122 bitlength = ManchesterConvertFrom255(data, len, bitStream, dataoutlen, high, low, clock, startindex);
b44e5233 123 else
c6be64da 124 bitlength= ManchesterConvertFrom1(data, len, bitStream, dataoutlen, clock, startindex);
b44e5233 125
b44e5233 126 memcpy(dataout, bitStream, bitlength);
149aeada 127 free(bitStream);
b44e5233 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) {
f38a1528 147 /* if this is the beginning of a peak */
b44e5233 148 if ( data[i-1] != data[i] && data[i] == peak) {
f38a1528 149 /* find lowest difference between peaks */
150 if (lastpeak && i - lastpeak < clock)
151 clock = i - lastpeak;
152 lastpeak = i;
153 }
154 }
b44e5233 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 }
f6c18637 170
77376577 171 //PrintAndLog(" Found Clock : %d - trying to adjust", clock);
f6c18637 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;
b44e5233 181 }
182
f6c18637 183 int DetectFirstTransition(const int * data, const size_t len, int threshold){
b44e5233 184
f6c18637 185 int i =0;
186 /* now look for the first threshold */
187 for (; i < len; ++i) {
188 if (data[i] == threshold) {
f38a1528 189 break;
190 }
f6c18637 191 }
192 return i;
b44e5233 193 }
194
c6be64da 195 int ManchesterConvertFrom255(const int * data, const size_t len, uint8_t * dataout, int dataoutlen, int high, int low, int clock, int startIndex){
b44e5233 196
f6c18637 197 int i, j, z, hithigh, hitlow, bitIndex, startType;
198 i = 0;
b44e5233 199 bitIndex = 0;
f6c18637 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;
b44e5233 206
f6c18637 207 // i = clock frame of data
c6be64da 208 for (; i < (int)(len/clock); i++)
f38a1528 209 {
f38a1528 210 hithigh = 0;
211 hitlow = 0;
f6c18637 212 startType = -1;
213 z = startIndex + (i*clock);
214 isDamp = 0;
77376577 215
f38a1528 216 /* Find out if we hit both high and low peaks */
217 for (j = 0; j < clock; j++)
f6c18637 218 {
219 if (data[z+j] == high){
f38a1528 220 hithigh = 1;
f6c18637 221 if ( startType == -1)
222 startType = 1;
223 }
224
225 if (data[z+j] == low ){
f38a1528 226 hitlow = 1;
f6c18637 227 if ( startType == -1)
228 startType = 0;
229 }
230
f38a1528 231 if (hithigh && hitlow)
232 break;
b44e5233 233 }
f6c18637 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);
081151ea 238 for (j = 0; j < clock; j++) {
f6c18637 239 if (
240 (data[z+j] <= dampHi && data[z+j] >= dampLow)
241 ){
77376577 242 isDamp++;
f6c18637 243 }
f6c18637 244 }
245 }
f38a1528 246
f6c18637 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
77376577 258 if ( isDamp > clock/2 ) {
f6c18637 259 firstST++;
260 }
261
262 if ( firstST == 4)
263 break;
c6be64da 264 if ( bitIndex >= dataoutlen-1 )
265 break;
f38a1528 266 }
b44e5233 267 return bitIndex;
268 }
269
c6be64da 270 int ManchesterConvertFrom1(const int * data, const size_t len, uint8_t * dataout,int dataoutlen, int clock, int startIndex){
b44e5233 271
f6c18637 272 PrintAndLog(" Path B");
273
b44e5233 274 int i,j, bitindex, lc, tolerance, warnings;
275 warnings = 0;
276 int upperlimit = len*2/clock+8;
277 i = startIndex;
278 j = 0;
279 tolerance = clock/4;
280 uint8_t decodedArr[len];
281
f6c18637 282 /* Detect duration between 2 successive transitions */
b44e5233 283 for (bitindex = 1; i < len; i++) {
284
285 if (data[i-1] != data[i]) {
286 lc = i - startIndex;
287 startIndex = i;
288
289 // Error check: if bitindex becomes too large, we do not
290 // have a Manchester encoded bitstream or the clock is really wrong!
291 if (bitindex > upperlimit ) {
292 PrintAndLog("Error: the clock you gave is probably wrong, aborting.");
293 return 0;
294 }
295 // Then switch depending on lc length:
296 // Tolerance is 1/4 of clock rate (arbitrary)
297 if (abs((lc-clock)/2) < tolerance) {
298 // Short pulse : either "1" or "0"
299 decodedArr[bitindex++] = data[i-1];
300 } else if (abs(lc-clock) < tolerance) {
301 // Long pulse: either "11" or "00"
302 decodedArr[bitindex++] = data[i-1];
303 decodedArr[bitindex++] = data[i-1];
304 } else {
305 ++warnings;
306 PrintAndLog("Warning: Manchester decode error for pulse width detection.");
307 if (warnings > 10) {
308 PrintAndLog("Error: too many detection errors, aborting.");
309 return 0;
f38a1528 310 }
311 }
312 }
313 }
b44e5233 314
315 /*
316 * We have a decodedArr of "01" ("1") or "10" ("0")
317 * parse it into final decoded dataout
318 */
319 for (i = 0; i < bitindex; i += 2) {
320
321 if ((decodedArr[i] == 0) && (decodedArr[i+1] == 1)) {
322 dataout[j++] = 1;
323 } else if ((decodedArr[i] == 1) && (decodedArr[i+1] == 0)) {
324 dataout[j++] = 0;
325 } else {
f38a1528 326 i++;
327 warnings++;
328 PrintAndLog("Unsynchronized, resync...");
b44e5233 329 PrintAndLog("(too many of those messages mean the stream is not Manchester encoded)");
330
331 if (warnings > 10) {
f38a1528 332 PrintAndLog("Error: too many decode errors, aborting.");
333 return 0;
334 }
335 }
336 }
b44e5233 337
338 PrintAndLog("%s", sprint_hex(dataout, j));
339 return j;
340 }
341
342 void ManchesterDiffDecodedString(const uint8_t* bitstream, size_t len, uint8_t invert){
343 /*
344 * We have a bitstream of "01" ("1") or "10" ("0")
345 * parse it into final decoded bitstream
346 */
347 int i, j, warnings;
348 uint8_t decodedArr[(len/2)+1];
f38a1528 349
b44e5233 350 j = warnings = 0;
f38a1528 351
b44e5233 352 uint8_t lastbit = 0;
f38a1528 353
b44e5233 354 for (i = 0; i < len; i += 2) {
355
356 uint8_t first = bitstream[i];
357 uint8_t second = bitstream[i+1];
f38a1528 358
b44e5233 359 if ( first == second ) {
360 ++i;
361 ++warnings;
362 if (warnings > 10) {
363 PrintAndLog("Error: too many decode errors, aborting.");
364 return;
365 }
366 }
367 else if ( lastbit != first ) {
368 decodedArr[j++] = 0 ^ invert;
369 }
370 else {
371 decodedArr[j++] = 1 ^ invert;
372 }
373 lastbit = second;
374 }
375
376 PrintAndLog("%s", sprint_hex(decodedArr, j));
377}
378
f38a1528 379void PrintPaddedManchester( uint8_t* bitStream, size_t len, size_t blocksize){
380
f6c18637 381 PrintAndLog(" Manchester decoded : %d bits", len);
f38a1528 382
f6c18637 383 uint8_t mod = len % blocksize;
384 uint8_t div = len / blocksize;
385 int i;
386
387 // Now output the bitstream to the scrollback by line of 16 bits
388 for (i = 0; i < div*blocksize; i+=blocksize) {
f38a1528 389 PrintAndLog(" %s", sprint_bin(bitStream+i,blocksize) );
f6c18637 390 }
391
392 if ( mod > 0 )
393 PrintAndLog(" %s", sprint_bin(bitStream+i, mod) );
7bd30f12 394}
395
7bd30f12 396/* Sliding DFT
397 Smooths out
398*/
399void iceFsk2(int * data, const size_t len){
400
401 int i, j;
149aeada 402 int * output = (int* ) malloc(sizeof(int) * len);
403 memset(output, 0x00, len);
404
7bd30f12 405 // for (i=0; i<len-5; ++i){
406 // for ( j=1; j <=5; ++j) {
407 // output[i] += data[i*j];
408 // }
409 // output[i] /= 5;
410 // }
411 int rest = 127;
412 int tmp =0;
413 for (i=0; i<len; ++i){
414 if ( data[i] < 127)
415 output[i] = 0;
416 else {
417 tmp = (100 * (data[i]-rest)) / rest;
418 output[i] = (tmp > 60)? 100:0;
419 }
420 }
421
422 for (j=0; j<len; ++j)
423 data[j] = output[j];
149aeada 424
425 free(output);
7bd30f12 426}
427
428void iceFsk3(int * data, const size_t len){
429
430 int i,j;
149aeada 431
432 int * output = (int* ) malloc(sizeof(int) * len);
433 memset(output, 0x00, len);
434 float fc = 0.1125f; // center frequency
081151ea 435 size_t adjustedLen = len;
436
7bd30f12 437 // create very simple low-pass filter to remove images (2nd-order Butterworth)
438 float complex iir_buf[3] = {0,0,0};
439 float b[3] = {0.003621681514929, 0.007243363029857, 0.003621681514929};
440 float a[3] = {1.000000000000000, -1.822694925196308, 0.837181651256023};
441
081151ea 442 float sample = 0; // input sample read from file
443 float complex x_prime = 1.0f; // save sample for estimating frequency
7bd30f12 444 float complex x;
445
081151ea 446 for (i=0; i<adjustedLen; ++i) {
7bd30f12 447
081151ea 448 sample = data[i]+128;
7bd30f12 449
450 // remove DC offset and mix to complex baseband
451 x = (sample - 127.5f) * cexpf( _Complex_I * 2 * M_PI * fc * i );
452
453 // apply low-pass filter, removing spectral image (IIR using direct-form II)
454 iir_buf[2] = iir_buf[1];
455 iir_buf[1] = iir_buf[0];
456 iir_buf[0] = x - a[1]*iir_buf[1] - a[2]*iir_buf[2];
457 x = b[0]*iir_buf[0] +
458 b[1]*iir_buf[1] +
459 b[2]*iir_buf[2];
460
461 // compute instantaneous frequency by looking at phase difference
462 // between adjacent samples
463 float freq = cargf(x*conjf(x_prime));
464 x_prime = x; // retain this sample for next iteration
465
466 output[i] =(freq > 0)? 10 : -10;
467 }
468
469 // show data
081151ea 470 for (j=0; j<adjustedLen; ++j)
7bd30f12 471 data[j] = output[j];
472
473 CmdLtrim("30");
081151ea 474 adjustedLen -= 30;
7bd30f12 475
476 // zero crossings.
081151ea 477 for (j=0; j<adjustedLen; ++j){
7bd30f12 478 if ( data[j] == 10) break;
479 }
480 int startOne =j;
481
081151ea 482 for (;j<adjustedLen; ++j){
7bd30f12 483 if ( data[j] == -10 ) break;
484 }
485 int stopOne = j-1;
486
487 int fieldlen = stopOne-startOne;
7bd30f12 488
fbceacc5 489 fieldlen = (fieldlen == 39 || fieldlen == 41)? 40 : fieldlen;
490 fieldlen = (fieldlen == 59 || fieldlen == 51)? 50 : fieldlen;
491 if ( fieldlen != 40 && fieldlen != 50){
492 printf("Detected field Length: %d \n", fieldlen);
081151ea 493 printf("Can only handle 40 or 50. Aborting...\n");
fbceacc5 494 return;
495 }
7bd30f12 496
497 // FSK sequence start == 000111
498 int startPos = 0;
081151ea 499 for (i =0; i<adjustedLen; ++i){
7bd30f12 500 int dec = 0;
501 for ( j = 0; j < 6*fieldlen; ++j){
502 dec += data[i + j];
503 }
504 if (dec == 0) {
505 startPos = i;
506 break;
507 }
508 }
509
510 printf("000111 position: %d \n", startPos);
511
72e930ef 512 startPos += 6*fieldlen+5;
7bd30f12 513
72e930ef 514 int bit =0;
7bd30f12 515 printf("BINARY\n");
516 printf("R/40 : ");
081151ea 517 for (i =startPos ; i < adjustedLen; i += 40){
72e930ef 518 bit = data[i]>0 ? 1:0;
519 printf("%d", bit );
7bd30f12 520 }
521 printf("\n");
522
523 printf("R/50 : ");
081151ea 524 for (i =startPos ; i < adjustedLen; i += 50){
72e930ef 525 bit = data[i]>0 ? 1:0;
526 printf("%d", bit ); }
7bd30f12 527 printf("\n");
528
149aeada 529 free(output);
7bd30f12 530}
531
532float complex cexpf (float complex Z)
533{
534 float complex Res;
535 double rho = exp (__real__ Z);
536 __real__ Res = rho * cosf(__imag__ Z);
537 __imag__ Res = rho * sinf(__imag__ Z);
538 return Res;
539}
Impressum, Datenschutz