]>
git.zerfleddert.de Git - proxmark3-svn/blob - client/util.c
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 _POSIX_C_SOURCE 199309L // need nanosleep()
25 #define MAX_BIN_BREAK_LENGTH (3072+384+1)
29 #include <sys/ioctl.h>
36 static struct termios Otty
, Ntty
;
38 if ( tcgetattr(STDIN_FILENO
, &Otty
) == -1 ) return -1;
41 Ntty
.c_iflag
= 0x0000; // input mode
42 Ntty
.c_oflag
= 0x0000; // output mode
43 Ntty
.c_lflag
&= ~ICANON
; // control mode = raw
44 Ntty
.c_cc
[VMIN
] = 1; // return if at least 1 character is in the queue
45 Ntty
.c_cc
[VTIME
] = 0; // no timeout. Wait forever
47 if (0 == (error
= tcsetattr(STDIN_FILENO
, TCSANOW
, &Ntty
))) { // set new attributes
48 error
+= ioctl(STDIN_FILENO
, FIONREAD
, &cnt
); // get number of characters availabe
49 error
+= tcsetattr(STDIN_FILENO
, TCSANOW
, &Otty
); // reset attributes
52 return ( error
== 0 ? cnt
: -1 );
63 // log files functions
64 void AddLogLine(char *file
, char *extData
, char *c
) {
66 char filename
[FILE_PATH_SIZE
] = {0x00};
70 if (len
> FILE_PATH_SIZE
) len
= FILE_PATH_SIZE
;
71 memcpy(filename
, file
, len
);
73 fLog
= fopen(filename
, "a");
75 printf("Could not append log file %s", filename
);
79 fprintf(fLog
, "%s", extData
);
80 fprintf(fLog
, "%s\n", c
);
84 void AddLogHex(char *fileName
, char *extData
, const uint8_t * data
, const size_t len
){
85 AddLogLine(fileName
, extData
, sprint_hex(data
, len
));
88 void AddLogUint64(char *fileName
, char *extData
, const uint64_t data
) {
90 sprintf(buf
, "%x%x", (unsigned int)((data
& 0xFFFFFFFF00000000) >> 32), (unsigned int)(data
& 0xFFFFFFFF));
91 AddLogLine(fileName
, extData
, buf
);
94 void AddLogCurrentDT(char *fileName
) {
99 curTime
= gmtime(&now
);
101 strftime (buff
, sizeof(buff
), "%Y-%m-%d %H:%M:%S", curTime
);
102 AddLogLine(fileName
, "\nanticollision: ", buff
);
105 void FillFileNameByUID(char *fileName
, uint8_t * uid
, char *ext
, int byteCount
) {
106 char * fnameptr
= fileName
;
107 memset(fileName
, 0x00, 200);
109 for (int j
= 0; j
< byteCount
; j
++, fnameptr
+= 2)
110 sprintf(fnameptr
, "%02x", (unsigned int) uid
[j
]);
111 sprintf(fnameptr
, "%s", ext
);
114 // printing and converting functions
116 void print_hex(const uint8_t * data
, const size_t len
)
120 for (i
=0; i
< len
; i
++)
121 printf("%02x ", data
[i
]);
126 void print_hex_break(const uint8_t *data
, const size_t len
, uint8_t breaks
) {
129 printf("[%02d] | ", rownum
);
130 for (int i
= 0; i
< len
; ++i
) {
132 printf("%02X ", data
[i
]);
134 // check if a line break is needed
135 if ( breaks
> 0 && !((i
+1) % breaks
) && (i
+1 < len
) ) {
137 printf("\n[%02d] | ", rownum
);
143 char *sprint_hex(const uint8_t *data
, const size_t len
) {
145 int maxLen
= ( len
> 1024/3) ? 1024/3 : len
;
146 static char buf
[1024];
147 memset(buf
, 0x00, 1024);
151 for (i
=0; i
< maxLen
; ++i
, tmp
+= 3)
152 sprintf(tmp
, "%02x ", (unsigned int) data
[i
]);
157 char *sprint_bin_break(const uint8_t *data
, const size_t len
, const uint8_t breaks
) {
158 // make sure we don't go beyond our char array memory
161 max_len
= ( len
> MAX_BIN_BREAK_LENGTH
) ? MAX_BIN_BREAK_LENGTH
: len
;
163 max_len
= ( len
+(len
/breaks
) > MAX_BIN_BREAK_LENGTH
) ? MAX_BIN_BREAK_LENGTH
: len
+(len
/breaks
);
165 static char buf
[MAX_BIN_BREAK_LENGTH
]; // 3072 + end of line characters if broken at 8 bits
167 memset(buf
, 0x00, sizeof(buf
));
171 // loop through the out_index to make sure we don't go too far
172 for (size_t out_index
=0; out_index
< max_len
; out_index
++) {
173 // set character - (should be binary but verify it isn't more than 1 digit)
174 if (data
[in_index
]<10)
175 sprintf(tmp
++, "%u", (unsigned int) data
[in_index
]);
176 // check if a line break is needed and we have room to print it in our array
177 if ( (breaks
> 0) && !((in_index
+1) % breaks
) && (out_index
+1 < max_len
) ) {
178 // increment and print line break
180 sprintf(tmp
++, "%s","\n");
188 char *sprint_bin(const uint8_t *data
, const size_t len
) {
189 return sprint_bin_break(data
, len
, 0);
192 char *sprint_hex_ascii(const uint8_t *data
, const size_t len
) {
193 static char buf
[1024];
195 memset(buf
, 0x00, 1024);
196 size_t max_len
= (len
> 1010) ? 1010 : len
;
198 sprintf(tmp
, "%s| ", sprint_hex(data
, max_len
) );
201 size_t pos
= (max_len
* 3)+2;
204 if ( (c
< 32) || (c
== 127))
206 sprintf(tmp
+pos
+i
, "%c", c
);
212 char *sprint_ascii(const uint8_t *data
, const size_t len
) {
213 static char buf
[1024];
215 memset(buf
, 0x00, 1024);
216 size_t max_len
= (len
> 1010) ? 1010 : len
;
220 tmp
[i
] = ((c
< 32) || (c
== 127)) ? '.' : c
;
226 void num_to_bytes(uint64_t n
, size_t len
, uint8_t* dest
)
229 dest
[len
] = (uint8_t) n
;
234 uint64_t bytes_to_num(uint8_t* src
, size_t len
)
239 num
= (num
<< 8) | (*src
);
245 void num_to_bytebits(uint64_t n
, size_t len
, uint8_t *dest
) {
252 //least significant bit first
253 void num_to_bytebitsLSBF(uint64_t n
, size_t len
, uint8_t *dest
) {
254 for(int i
= 0 ; i
< len
; ++i
) {
260 // Swap bit order on a uint32_t value. Can be limited by nrbits just use say 8bits reversal
261 // And clears the rest of the bits.
262 uint32_t SwapBits(uint32_t value
, int nrbits
) {
263 uint32_t newvalue
= 0;
264 for(int i
= 0; i
< nrbits
; i
++) {
265 newvalue
^= ((value
>> i
) & 1) << (nrbits
- 1 - i
);
270 // aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp
272 // hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii
273 // up to 64 bytes or 512 bits
274 uint8_t *SwapEndian64(const uint8_t *src
, const size_t len
, const uint8_t blockSize
){
275 static uint8_t buf
[64];
276 memset(buf
, 0x00, 64);
278 for (uint8_t block
=0; block
< (uint8_t)(len
/blockSize
); block
++){
279 for (size_t i
= 0; i
< blockSize
; i
++){
280 tmp
[i
+(blockSize
*block
)] = src
[(blockSize
-1-i
)+(blockSize
*block
)];
286 // takes a uint8_t src array, for len items and reverses the byte order in blocksizes (8,16,32,64),
287 // returns: the dest array contains the reordered src array.
288 void SwapEndian64ex(const uint8_t *src
, const size_t len
, const uint8_t blockSize
, uint8_t *dest
){
289 for (uint8_t block
=0; block
< (uint8_t)(len
/blockSize
); block
++){
290 for (size_t i
= 0; i
< blockSize
; i
++){
291 dest
[i
+(blockSize
*block
)] = src
[(blockSize
-1-i
)+(blockSize
*block
)];
296 //assumes little endian
297 char * printBits(size_t const size
, void const * const ptr
)
299 unsigned char *b
= (unsigned char*) ptr
;
301 static char buf
[1024];
305 for (i
=size
-1;i
>=0;i
--)
309 byte
= b
[i
] & (1<<j
);
311 sprintf(tmp
, "%u", (unsigned int)byte
);
318 // -------------------------------------------------------------------------
319 // string parameters lib
320 // -------------------------------------------------------------------------
322 // -------------------------------------------------------------------------
324 // bg, en - symbol numbers in param line of beginning an ending parameter
325 // paramnum - param number (from 0)
326 // -------------------------------------------------------------------------
327 int param_getptr(const char *line
, int *bg
, int *en
, int paramnum
)
330 int len
= strlen(line
);
336 while (line
[*bg
] ==' ' || line
[*bg
]=='\t') (*bg
)++;
341 for (i
= 0; i
< paramnum
; i
++) {
342 while (line
[*bg
]!=' ' && line
[*bg
]!='\t' && line
[*bg
] != '\0') (*bg
)++;
343 while (line
[*bg
]==' ' || line
[*bg
]=='\t') (*bg
)++;
345 if (line
[*bg
] == '\0') return 1;
349 while (line
[*en
] != ' ' && line
[*en
] != '\t' && line
[*en
] != '\0') (*en
)++;
357 char param_getchar(const char *line
, int paramnum
)
361 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 0x00;
366 uint8_t param_get8(const char *line
, int paramnum
)
368 return param_get8ex(line
, paramnum
, 0, 10);
372 * @brief Reads a decimal integer (actually, 0-254, not 255)
375 * @return -1 if error
377 uint8_t param_getdec(const char *line
, int paramnum
, uint8_t *destination
)
379 uint8_t val
= param_get8ex(line
, paramnum
, 255, 10);
380 if( (int8_t) val
== -1) return 1;
381 (*destination
) = val
;
385 * @brief Checks if param is decimal
390 uint8_t param_isdec(const char *line
, int paramnum
)
393 //TODO, check more thorougly
394 if (!param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
395 // return strtoul(&line[bg], NULL, 10) & 0xff;
400 uint8_t param_get8ex(const char *line
, int paramnum
, int deflt
, int base
)
404 if (!param_getptr(line
, &bg
, &en
, paramnum
))
405 return strtoul(&line
[bg
], NULL
, base
) & 0xff;
410 uint32_t param_get32ex(const char *line
, int paramnum
, int deflt
, int base
)
414 if (!param_getptr(line
, &bg
, &en
, paramnum
))
415 return strtoul(&line
[bg
], NULL
, base
);
420 uint64_t param_get64ex(const char *line
, int paramnum
, int deflt
, int base
)
424 if (!param_getptr(line
, &bg
, &en
, paramnum
))
425 return strtoull(&line
[bg
], NULL
, base
);
430 int param_gethex(const char *line
, int paramnum
, uint8_t * data
, int hexcnt
)
437 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
439 if (en
- bg
+ 1 != hexcnt
)
442 for(i
= 0; i
< hexcnt
; i
+= 2) {
443 if (!(isxdigit(line
[bg
+ i
]) && isxdigit(line
[bg
+ i
+ 1])) ) return 1;
445 sscanf((char[]){line
[bg
+ i
], line
[bg
+ i
+ 1], 0}, "%X", &temp
);
446 data
[i
/ 2] = temp
& 0xff;
451 int param_gethex_ex(const char *line
, int paramnum
, uint8_t * data
, int *hexcnt
)
458 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
460 *hexcnt
= en
- bg
+ 1;
461 if (*hexcnt
% 2) //error if not complete hex bytes
464 for(i
= 0; i
< *hexcnt
; i
+= 2) {
465 if (!(isxdigit(line
[bg
+ i
]) && isxdigit(line
[bg
+ i
+ 1])) ) return 1;
467 sscanf((char[]){line
[bg
+ i
], line
[bg
+ i
+ 1], 0}, "%X", &temp
);
468 data
[i
/ 2] = temp
& 0xff;
473 int param_getstr(const char *line
, int paramnum
, char * str
)
477 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 0;
479 memcpy(str
, line
+ bg
, en
- bg
+ 1);
480 str
[en
- bg
+ 1] = 0;
486 The following methods comes from Rfidler sourcecode.
487 https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/
490 // convert hex to sequence of 0/1 bit values
491 // returns number of bits converted
492 int hextobinarray(char *target
, char *source
)
494 int length
, i
, count
= 0;
497 length
= strlen(source
);
498 // process 4 bits (1 hex digit) at a time
503 if (x
>= 'a' && x
<= 'f')
505 // convert to numeric value
506 if (x
>= '0' && x
<= '9')
508 else if (x
>= 'A' && x
<= 'F')
513 for(i
= 0 ; i
< 4 ; ++i
, ++count
)
514 *(target
++)= (x
>> (3 - i
)) & 1;
520 // convert hex to human readable binary string
521 int hextobinstring(char *target
, char *source
)
525 if(!(length
= hextobinarray(target
, source
)))
527 binarraytobinstring(target
, target
, length
);
531 // convert binary array of 0x00/0x01 values to hex (safe to do in place as target will always be shorter than source)
532 // return number of bits converted
533 int binarraytohex(char *target
,char *source
, int length
)
543 for(i
= x
= 0 ; i
< 4 ; ++i
)
544 x
+= ( source
[i
] << (3 - i
));
545 sprintf(target
,"%X", (unsigned int)x
);
553 // convert binary array to human readable binary
554 void binarraytobinstring(char *target
, char *source
, int length
)
558 for(i
= 0 ; i
< length
; ++i
)
559 *(target
++)= *(source
++) + '0';
563 // return parity bit required to match type
564 uint8_t GetParity( uint8_t *bits
, uint8_t type
, int length
)
568 for(x
= 0 ; length
> 0 ; --length
)
569 x
+= bits
[length
- 1];
575 // add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half
576 void wiegand_add_parity(uint8_t *target
, uint8_t *source
, uint8_t length
)
578 *(target
++)= GetParity(source
, EVEN
, length
/ 2);
579 memcpy(target
, source
, length
);
581 *(target
)= GetParity(source
+ length
/ 2, ODD
, length
/ 2);
584 // xor two arrays together for len items. The dst array contains the new xored values.
585 void xor(unsigned char *dst
, unsigned char *src
, size_t len
) {
586 for( ; len
> 0; len
--,dst
++,src
++)
590 int32_t le24toh (uint8_t data
[3]) {
591 return (data
[2] << 16) | (data
[1] << 8) | data
[0];
593 uint32_t le32toh (uint8_t *data
) {
594 return (uint32_t)( (data
[3]<<24) | (data
[2]<<16) | (data
[1]<<8) | data
[0]);
597 // RotateLeft - Ultralight, Desfire, works on byte level
598 // 00-01-02 >> 01-02-00
599 void rol(uint8_t *data
, const size_t len
){
600 uint8_t first
= data
[0];
601 for (size_t i
= 0; i
< len
-1; i
++) {
608 // Replace unprintable characters with a dot in char buffer
609 void clean_ascii(unsigned char *buf
, size_t len
) {
610 for (size_t i
= 0; i
< len
; i
++) {
611 if (!isprint(buf
[i
]))
618 #if !defined (_WIN32)
621 static void nsleep(uint64_t n
) {
622 struct timespec timeout
;
623 timeout
.tv_sec
= n
/1000000000;
624 timeout
.tv_nsec
= n
%1000000000;
625 while (nanosleep(&timeout
, &timeout
) && errno
== EINTR
);
628 void msleep(uint32_t n
) {
634 // a milliseconds timer for performance measurement
637 #include <sys/types.h>
639 // WORKAROUND FOR MinGW (some versions - use if normal code does not compile)
640 // It has no _ftime_s and needs explicit inclusion of timeb.h
641 #include <sys/timeb.h>
644 return 1000 * t
.time
+ t
.millitm
;
646 // NORMAL CODE (use _ftime_s)
648 //if (_ftime_s(&t)) {
651 // return 1000 * t.time + t.millitm;
655 clock_gettime(CLOCK_MONOTONIC
, &t
);
656 return (t
.tv_sec
* 1000 + t
.tv_nsec
/ 1000000);
660 // determine number of logical CPU cores (use for multithreaded functions)
661 extern int num_CPUs(void)
664 #include <sysinfoapi.h>
666 GetSystemInfo(&sysinfo
);
667 return sysinfo
.dwNumberOfProcessors
;
668 #elif defined(__linux__) || defined(__APPLE__)
670 return sysconf(_SC_NPROCESSORS_ONLN
);