]>
cvs.zerfleddert.de Git - proxmark3-svn/blob - client/util.c
3a7cbac40cc2a3ee0bdb07d9d7c6c7bdf529b4f2
1 //-----------------------------------------------------------------------------
2 // Copyright (C) 2010 iZsh <izsh at fail0verflow.com>
4 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
5 // at your option, any later version. See the LICENSE.txt file for the text of
7 //-----------------------------------------------------------------------------
9 //-----------------------------------------------------------------------------
12 #define MAX_BIN_BREAK_LENGTH (3072+384+1)
18 static struct termios Otty
, Ntty
;
20 if ( tcgetattr( 0, &Otty
) == -1) return -1;
23 Ntty
.c_iflag
= 0; /* input mode */
24 Ntty
.c_oflag
= 0; /* output mode */
25 Ntty
.c_lflag
&= ~ICANON
; /* raw mode */
26 Ntty
.c_cc
[VMIN
] = CMIN
; /* minimum time to wait */
27 Ntty
.c_cc
[VTIME
] = CTIME
; /* minimum characters to wait for */
29 if (0 == (error
= tcsetattr(0, TCSANOW
, &Ntty
))) {
30 error
+= ioctl(0, FIONREAD
, &cnt
);
31 error
+= tcsetattr(0, TCSANOW
, &Otty
);
34 return ( error
== 0 ? cnt
: -1 );
43 // log files functions
44 void AddLogLine(char *file
, char *extData
, char *c
) {
46 char filename
[FILE_PATH_SIZE
] = {0x00};
50 if (len
> FILE_PATH_SIZE
) len
= FILE_PATH_SIZE
;
51 memcpy(filename
, file
, len
);
53 f
= fopen(filename
, "a");
55 printf("Could not append log file %s", filename
);
59 fprintf(f
, "%s", extData
);
60 fprintf(f
, "%s\n", c
);
68 void AddLogHex(char *fileName
, char *extData
, const uint8_t * data
, const size_t len
){
69 AddLogLine(fileName
, extData
, sprint_hex(data
, len
));
72 void AddLogUint64(char *fileName
, char *extData
, const uint64_t data
) {
74 memset(buf
, 0x00, sizeof(buf
));
75 //sprintf(buf, "%X%X", (unsigned int)((data & 0xFFFFFFFF00000000) >> 32), (unsigned int)(data & 0xFFFFFFFF));
76 sprintf(buf
, "%012"llx
"", data
);
77 AddLogLine(fileName
, extData
, buf
);
80 void AddLogCurrentDT(char *fileName
) {
82 memset(buf
, 0x00, sizeof(buf
));
85 curTime
= gmtime(&now
);
86 strftime (buf
, sizeof(buf
), "%Y-%m-%d %H:%M:%S", curTime
);
87 AddLogLine(fileName
, "\nanticollision: ", buf
);
90 void FillFileNameByUID(char *fileName
, uint8_t *uid
, char *ext
, int byteCount
) {
91 if ( fileName
== NULL
|| uid
== NULL
|| ext
== NULL
){
92 printf("error: parameter is NULL\n");
95 char * fnameptr
= fileName
;
96 memset(fileName
, 0x00, FILE_PATH_SIZE
);
98 for (int j
= 0; j
< byteCount
; j
++, fnameptr
+= 2)
99 sprintf(fnameptr
, "%02X", uid
[j
]);
100 sprintf(fnameptr
, "%s", ext
);
103 // printing and converting functions
104 void print_hex(const uint8_t * data
, const size_t len
) {
106 for (i
=0; i
< len
; ++i
)
107 printf("%02x ", data
[i
]);
111 void print_hex_break(const uint8_t *data
, const size_t len
, uint8_t breaks
) {
114 printf("[%02d] | ", rownum
);
115 for (int i
= 0; i
< len
; ++i
) {
117 printf("%02X ", data
[i
]);
119 // check if a line break is needed
120 if ( breaks
> 0 && !((i
+1) % breaks
) && (i
+1 < len
) ) {
122 printf("\n[%02d] | ", rownum
);
128 char *sprint_hex(const uint8_t *data
, const size_t len
) {
130 int maxLen
= ( len
> 1024/3) ? 1024/3 : len
;
131 static char buf
[1024];
132 memset(buf
, 0x00, 1024);
136 for (i
=0; i
< maxLen
; ++i
, tmp
+= 3)
137 sprintf(tmp
, "%02X ", data
[i
]);
141 char *sprint_bin_break(const uint8_t *data
, const size_t len
, const uint8_t breaks
) {
143 // make sure we don't go beyond our char array memory
144 size_t in_index
= 0, out_index
= 0;
147 max_len
= ( len
> MAX_BIN_BREAK_LENGTH
) ? MAX_BIN_BREAK_LENGTH
: len
;
149 max_len
= ( len
+(len
/breaks
) > MAX_BIN_BREAK_LENGTH
) ? MAX_BIN_BREAK_LENGTH
: len
+(len
/breaks
);
151 static char buf
[MAX_BIN_BREAK_LENGTH
]; // 3072 + end of line characters if broken at 8 bits
153 memset(buf
, 0x00, sizeof(buf
));
156 // loop through the out_index to make sure we don't go too far
157 for (out_index
=0; out_index
< max_len
-2; out_index
++) {
159 sprintf(tmp
++, "%u", (unsigned int) data
[in_index
]);
160 // check if a line break is needed and we have room to print it in our array
161 if ( (breaks
> 0) && !((in_index
+1) % breaks
) && (out_index
+1 != max_len
) ) {
162 // increment and print line break
164 sprintf(tmp
++, "%s","\n");
169 sprintf(tmp
++, "%u", (unsigned int) data
[in_index
]);
173 char *sprint_bin(const uint8_t *data
, const size_t len
) {
174 return sprint_bin_break(data
, len
, 0);
177 char *sprint_hex_ascii(const uint8_t *data
, const size_t len
) {
178 static char buf
[1024];
180 memset(buf
, 0x00, 1024);
181 size_t max_len
= (len
> 1010) ? 1010 : len
;
182 sprintf(tmp
, "%s| %s", sprint_hex(data
, max_len
) , data
);
186 void num_to_bytes(uint64_t n
, size_t len
, uint8_t* dest
) {
188 dest
[len
] = (uint8_t) n
;
193 uint64_t bytes_to_num(uint8_t* src
, size_t len
) {
196 num
= (num
<< 8) | (*src
);
202 // takes a number (uint64_t) and creates a binarray in dest.
203 void num_to_bytebits(uint64_t n
, size_t len
, uint8_t *dest
) {
210 //least significant bit first
211 void num_to_bytebitsLSBF(uint64_t n
, size_t len
, uint8_t *dest
) {
212 for(int i
= 0 ; i
< len
; ++i
) {
218 // aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp
220 // hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii
221 // up to 64 bytes or 512 bits
222 uint8_t *SwapEndian64(const uint8_t *src
, const size_t len
, const uint8_t blockSize
){
223 static uint8_t buf
[64];
224 memset(buf
, 0x00, 64);
226 for (uint8_t block
=0; block
< (uint8_t)(len
/blockSize
); block
++){
227 for (size_t i
= 0; i
< blockSize
; i
++){
228 tmp
[i
+(blockSize
*block
)] = src
[(blockSize
-1-i
)+(blockSize
*block
)];
234 // takes a uint8_t src array, for len items and reverses the byte order in blocksizes (8,16,32,64),
235 // returns: the dest array contains the reordered src array.
236 void SwapEndian64ex(const uint8_t *src
, const size_t len
, const uint8_t blockSize
, uint8_t *dest
){
237 for (uint8_t block
=0; block
< (uint8_t)(len
/blockSize
); block
++){
238 for (size_t i
= 0; i
< blockSize
; i
++){
239 dest
[i
+(blockSize
*block
)] = src
[(blockSize
-1-i
)+(blockSize
*block
)];
244 // -------------------------------------------------------------------------
245 // string parameters lib
246 // -------------------------------------------------------------------------
248 // -------------------------------------------------------------------------
250 // bg, en - symbol numbers in param line of beginning an ending parameter
251 // paramnum - param number (from 0)
252 // -------------------------------------------------------------------------
253 int param_getptr(const char *line
, int *bg
, int *en
, int paramnum
)
256 int len
= strlen(line
);
262 while (line
[*bg
] ==' ' || line
[*bg
]=='\t') (*bg
)++;
267 for (i
= 0; i
< paramnum
; i
++) {
268 while (line
[*bg
]!=' ' && line
[*bg
]!='\t' && line
[*bg
] != '\0') (*bg
)++;
269 while (line
[*bg
]==' ' || line
[*bg
]=='\t') (*bg
)++;
271 if (line
[*bg
] == '\0') return 1;
275 while (line
[*en
] != ' ' && line
[*en
] != '\t' && line
[*en
] != '\0') (*en
)++;
282 char param_getchar(const char *line
, int paramnum
)
286 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 0x00;
291 uint8_t param_get8(const char *line
, int paramnum
)
293 return param_get8ex(line
, paramnum
, 0, 10);
297 * @brief Reads a decimal integer (actually, 0-254, not 255)
300 * @return -1 if error
302 uint8_t param_getdec(const char *line
, int paramnum
, uint8_t *destination
)
304 uint8_t val
= param_get8ex(line
, paramnum
, 255, 10);
305 if( (int8_t) val
== -1) return 1;
306 (*destination
) = val
;
310 * @brief Checks if param is decimal
315 uint8_t param_isdec(const char *line
, int paramnum
)
318 //TODO, check more thorougly
319 if (!param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
320 // return strtoul(&line[bg], NULL, 10) & 0xff;
325 uint8_t param_get8ex(const char *line
, int paramnum
, int deflt
, int base
)
329 if (!param_getptr(line
, &bg
, &en
, paramnum
))
330 return strtoul(&line
[bg
], NULL
, base
) & 0xff;
335 uint32_t param_get32ex(const char *line
, int paramnum
, int deflt
, int base
)
339 if (!param_getptr(line
, &bg
, &en
, paramnum
))
340 return strtoul(&line
[bg
], NULL
, base
);
345 uint64_t param_get64ex(const char *line
, int paramnum
, int deflt
, int base
)
349 if (!param_getptr(line
, &bg
, &en
, paramnum
))
350 return strtoull(&line
[bg
], NULL
, base
);
355 int param_gethex(const char *line
, int paramnum
, uint8_t * data
, int hexcnt
)
359 if (hexcnt
& 1) return 1;
361 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
363 if (en
- bg
+ 1 != hexcnt
) return 1;
365 for(i
= 0; i
< hexcnt
; i
+= 2) {
366 if (!(isxdigit(line
[bg
+ i
]) && isxdigit(line
[bg
+ i
+ 1])) ) return 1;
368 sscanf((char[]){line
[bg
+ i
], line
[bg
+ i
+ 1], 0}, "%X", &temp
);
369 data
[i
/ 2] = temp
& 0xff;
374 int param_gethex_ex(const char *line
, int paramnum
, uint8_t * data
, int *hexcnt
)
381 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
383 *hexcnt
= en
- bg
+ 1;
384 if (*hexcnt
% 2) //error if not complete hex bytes
387 for(i
= 0; i
< *hexcnt
; i
+= 2) {
388 if (!(isxdigit(line
[bg
+ i
]) && isxdigit(line
[bg
+ i
+ 1])) ) return 1;
390 sscanf((char[]){line
[bg
+ i
], line
[bg
+ i
+ 1], 0}, "%X", &temp
);
391 data
[i
/ 2] = temp
& 0xff;
396 int param_getstr(const char *line
, int paramnum
, char * str
)
400 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 0;
402 memcpy(str
, line
+ bg
, en
- bg
+ 1);
403 str
[en
- bg
+ 1] = 0;
409 The following methods comes from Rfidler sourcecode.
410 https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/
413 // convert hex to sequence of 0/1 bit values
414 // returns number of bits converted
415 int hextobinarray(char *target
, char *source
)
417 int length
, i
, count
= 0;
420 length
= strlen(source
);
421 // process 4 bits (1 hex digit) at a time
426 if (x
>= 'a' && x
<= 'f')
428 // convert to numeric value
429 if (x
>= '0' && x
<= '9')
431 else if (x
>= 'A' && x
<= 'F')
436 for(i
= 0 ; i
< 4 ; ++i
, ++count
)
437 *(target
++)= (x
>> (3 - i
)) & 1;
443 // convert hex to human readable binary string
444 int hextobinstring(char *target
, char *source
)
448 if(!(length
= hextobinarray(target
, source
)))
450 binarraytobinstring(target
, target
, length
);
454 // convert binary array of 0x00/0x01 values to hex (safe to do in place as target will always be shorter than source)
455 // return number of bits converted
456 int binarraytohex(char *target
, char *source
, int length
)
466 for(i
= x
= 0 ; i
< 4 ; ++i
)
467 x
+= ( source
[i
] << (3 - i
));
468 sprintf(target
,"%X", x
);
476 // convert binary array to human readable binary
477 void binarraytobinstring(char *target
, char *source
, int length
)
481 for(i
= 0 ; i
< length
; ++i
)
482 *(target
++)= *(source
++) + '0';
486 // return parity bit required to match type
487 uint8_t GetParity( uint8_t *bits
, uint8_t type
, int length
)
490 for( x
= 0 ; length
> 0 ; --length
)
491 x
+= bits
[length
- 1];
496 // add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half
497 void wiegand_add_parity(uint8_t *target
, uint8_t *source
, uint8_t length
)
499 *(target
++)= GetParity(source
, EVEN
, length
/ 2);
500 memcpy(target
, source
, length
);
502 *(target
)= GetParity(source
+ length
/ 2, ODD
, length
/ 2);
505 // xor two arrays together for len items. The dst array contains the new xored values.
506 void xor(unsigned char * dst
, unsigned char * src
, size_t len
) {
507 for( ; len
> 0; len
--,dst
++,src
++)
511 int32_t le24toh (uint8_t data
[3]) {
512 return (data
[2] << 16) | (data
[1] << 8) | data
[0];
514 uint32_t le32toh (uint8_t *data
) {
515 return (uint32_t)( (data
[3]<<24) | (data
[2]<<16) | (data
[1]<<8) | data
[0]);
517 // Pack a bitarray into a uint32_t.
518 uint32_t PackBits(uint8_t start
, uint8_t len
, uint8_t* bits
) {
520 if (len
> 32) return 0;
526 for (; j
>= 0; --j
, ++i
)
532 // RotateLeft - Ultralight, Desfire, works on byte level
533 // 00-01-02 >> 01-02-00
534 void rol(uint8_t *data
, const size_t len
){
535 uint8_t first
= data
[0];
536 for (size_t i
= 0; i
< len
-1; i
++) {
542 // Swap bit order on a uint32_t value. Can be limited by nrbits just use say 8bits reversal
543 // And clears the rest of the bits.
544 uint32_t SwapBits(uint32_t value
, int nrbits
) {
545 uint32_t newvalue
= 0;
546 for(int i
= 0; i
< nrbits
; i
++) {
547 newvalue
^= ((value
>> i
) & 1) << (nrbits
- 1 - i
);
552 ref http://www.csm.ornl.gov/~dunigan/crc.html
553 Returns the value v with the bottom b [0,32] bits reflected.
554 Example: reflect(0x3e23L,3) == 0x3e26
556 uint32_t reflect(uint32_t v
, int b
) {
558 for ( int i
= 0; i
< b
; ++i
) {
560 v
|= BITMASK((b
-1)-i
);
562 v
&= ~BITMASK((b
-1)-i
);
568 uint64_t HornerScheme(uint64_t num
, uint64_t divider
, uint64_t factor
) {
569 uint64_t remainder
=0, quotient
=0, result
=0;
570 remainder
= num
% divider
;
571 quotient
= num
/ divider
;
572 if(!(quotient
== 0 && remainder
== 0))
573 result
+= HornerScheme(quotient
, divider
, factor
) * factor
+ remainder
;