]>
cvs.zerfleddert.de Git - proxmark3-svn/blob - client/util.c
7e2304888c7021206bd2d7ff03096c068e99e37c
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 //-----------------------------------------------------------------------------
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 );
59 struct termios Otty
, Ntty
;
60 if ( tcgetattr(STDIN_FILENO
, &Otty
) == -1 ) return -1;
62 Ntty
.c_lflag
&= ~ICANON
; /* disable buffered i/o */
63 if (0 == (error
= tcsetattr(STDIN_FILENO
, TCSANOW
, &Ntty
))) { // set new attributes
65 error
+= tcsetattr(STDIN_FILENO
, TCSANOW
, &Otty
); // reset attributes
67 return ( error
== 0 ? c
: -1 );
78 // log files functions
79 void AddLogLine(char *file
, char *extData
, char *c
) {
81 char filename
[FILE_PATH_SIZE
] = {0x00};
85 if (len
> FILE_PATH_SIZE
) len
= FILE_PATH_SIZE
;
86 memcpy(filename
, file
, len
);
88 fLog
= fopen(filename
, "a");
90 printf("Could not append log file %s", filename
);
94 fprintf(fLog
, "%s", extData
);
95 fprintf(fLog
, "%s\n", c
);
99 void AddLogHex(char *fileName
, char *extData
, const uint8_t * data
, const size_t len
){
100 AddLogLine(fileName
, extData
, sprint_hex(data
, len
));
103 void AddLogUint64(char *fileName
, char *extData
, const uint64_t data
) {
105 sprintf(buf
, "%x%x", (unsigned int)((data
& 0xFFFFFFFF00000000) >> 32), (unsigned int)(data
& 0xFFFFFFFF));
106 AddLogLine(fileName
, extData
, buf
);
109 void AddLogCurrentDT(char *fileName
) {
113 time_t now
= time(0);
114 curTime
= gmtime(&now
);
116 strftime (buff
, sizeof(buff
), "%Y-%m-%d %H:%M:%S", curTime
);
117 AddLogLine(fileName
, "\nanticollision: ", buff
);
120 void FillFileNameByUID(char *fileName
, uint8_t *uid
, char *ext
, int byteCount
) {
121 char * fnameptr
= fileName
;
123 for (int j
= 0; j
< byteCount
; j
++, fnameptr
+= 2)
124 sprintf(fnameptr
, "%02x", (unsigned int) uid
[j
]);
125 sprintf(fnameptr
, "%s", ext
);
128 // fill buffer from structure [{uint8_t data, size_t length},...]
129 int FillBuffer(uint8_t *data
, size_t maxDataLength
, size_t *dataLength
, ...) {
132 va_start(valist
, dataLength
);
134 uint8_t *vdata
= NULL
;
137 vdata
= va_arg(valist
, uint8_t *);
141 vlength
= va_arg(valist
, size_t);
142 if (*dataLength
+ vlength
> maxDataLength
) {
147 memcpy(&data
[*dataLength
], vdata
, vlength
);
148 *dataLength
+= vlength
;
157 bool CheckStringIsHEXValue(const char *value
) {
158 for (int i
= 0; i
< strlen(value
); i
++)
159 if (!isxdigit(value
[i
]))
162 if (strlen(value
) % 2)
168 void hex_to_buffer(const uint8_t *buf
, const uint8_t *hex_data
, const size_t hex_len
, const size_t hex_max_len
,
169 const size_t min_str_len
, const size_t spaces_between
, bool uppercase
) {
171 char *tmp
= (char *)buf
;
173 memset(tmp
, 0x00, hex_max_len
);
175 int maxLen
= ( hex_len
> hex_max_len
) ? hex_max_len
: hex_len
;
177 for (i
= 0; i
< maxLen
; ++i
, tmp
+= 2 + spaces_between
) {
178 sprintf(tmp
, (uppercase
) ? "%02X" : "%02x", (unsigned int) hex_data
[i
]);
180 for (int j
= 0; j
< spaces_between
; j
++)
181 sprintf(tmp
+ 2 + j
, " ");
184 i
*= (2 + spaces_between
);
185 int minStrLen
= min_str_len
> i
? min_str_len
: 0;
186 if (minStrLen
> hex_max_len
)
187 minStrLen
= hex_max_len
;
188 for(; i
< minStrLen
; i
++, tmp
+= 1)
194 // printing and converting functions
196 char *sprint_hex(const uint8_t *data
, const size_t len
) {
197 static char buf
[4097] = {0};
199 hex_to_buffer((uint8_t *)buf
, data
, len
, sizeof(buf
) - 1, 0, 1, false);
204 char *sprint_hex_inrow_ex(const uint8_t *data
, const size_t len
, const size_t min_str_len
) {
205 static char buf
[4097] = {0};
207 hex_to_buffer((uint8_t *)buf
, data
, len
, sizeof(buf
) - 1, min_str_len
, 0, false);
212 char *sprint_hex_inrow(const uint8_t *data
, const size_t len
) {
213 return sprint_hex_inrow_ex(data
, len
, 0);
216 char *sprint_bin_break(const uint8_t *data
, const size_t len
, const uint8_t breaks
) {
217 // make sure we don't go beyond our char array memory
220 max_len
= ( len
> MAX_BIN_BREAK_LENGTH
) ? MAX_BIN_BREAK_LENGTH
: len
;
222 max_len
= ( len
+(len
/breaks
) > MAX_BIN_BREAK_LENGTH
) ? MAX_BIN_BREAK_LENGTH
: len
+(len
/breaks
);
224 static char buf
[MAX_BIN_BREAK_LENGTH
]; // 3072 + end of line characters if broken at 8 bits
226 memset(buf
, 0x00, sizeof(buf
));
230 // loop through the out_index to make sure we don't go too far
231 for (size_t out_index
=0; out_index
< max_len
; out_index
++) {
232 // set character - (should be binary but verify it isn't more than 1 digit)
233 if (data
[in_index
]<10)
234 sprintf(tmp
++, "%u", (unsigned int) data
[in_index
]);
235 // check if a line break is needed and we have room to print it in our array
236 if ( (breaks
> 0) && !((in_index
+1) % breaks
) && (out_index
+1 < max_len
) ) {
237 // increment and print line break
239 sprintf(tmp
++, "%s","\n");
247 char *sprint_bin(const uint8_t *data
, const size_t len
) {
248 return sprint_bin_break(data
, len
, 0);
251 char *sprint_ascii_ex(const uint8_t *data
, const size_t len
, const size_t min_str_len
) {
252 static char buf
[1024];
254 memset(buf
, 0x00, 1024);
255 size_t max_len
= (len
> 1010) ? 1010 : len
;
259 tmp
[i
] = ((c
< 32) || (c
== 127)) ? '.' : c
;
263 int minStrLen
= min_str_len
> i
? min_str_len
: 0;
264 for(; i
< minStrLen
; ++i
)
270 char *sprint_ascii(const uint8_t *data
, const size_t len
) {
271 return sprint_ascii_ex(data
, len
, 0);
274 void num_to_bytes(uint64_t n
, size_t len
, uint8_t* dest
)
277 dest
[len
] = (uint8_t) n
;
282 uint64_t bytes_to_num(uint8_t* src
, size_t len
)
287 num
= (num
<< 8) | (*src
);
293 void num_to_bytebits(uint64_t n
, size_t len
, uint8_t *dest
) {
300 //least significant bit first
301 void num_to_bytebitsLSBF(uint64_t n
, size_t len
, uint8_t *dest
) {
302 for(int i
= 0 ; i
< len
; ++i
) {
308 // Swap bit order on a uint32_t value. Can be limited by nrbits just use say 8bits reversal
309 // And clears the rest of the bits.
310 uint32_t SwapBits(uint32_t value
, int nrbits
) {
311 uint32_t newvalue
= 0;
312 for(int i
= 0; i
< nrbits
; i
++) {
313 newvalue
^= ((value
>> i
) & 1) << (nrbits
- 1 - i
);
318 // aa,bb,cc,dd,ee,ff,gg,hh, ii,jj,kk,ll,mm,nn,oo,pp
320 // hh,gg,ff,ee,dd,cc,bb,aa, pp,oo,nn,mm,ll,kk,jj,ii
321 // up to 64 bytes or 512 bits
322 uint8_t *SwapEndian64(const uint8_t *src
, const size_t len
, const uint8_t blockSize
){
323 static uint8_t buf
[64];
324 memset(buf
, 0x00, 64);
325 for (uint8_t block
=0; block
< (uint8_t)(len
/blockSize
); block
++){
326 for (size_t i
= 0; i
< blockSize
; i
++){
327 buf
[i
+(blockSize
*block
)] = src
[(blockSize
-1-i
)+(blockSize
*block
)];
333 //assumes little endian
334 char *printBits(size_t const size
, void const * const ptr
)
336 unsigned char *b
= (unsigned char*) ptr
;
338 static char buf
[1024];
342 for (i
=size
-1;i
>=0;i
--)
346 byte
= b
[i
] & (1<<j
);
348 sprintf(tmp
, "%u", (unsigned int)byte
);
355 char *printBitsPar(const uint8_t *b
, size_t len
) {
356 static char buf1
[512] = {0};
357 static char buf2
[512] = {0};
363 memset(buf
, 0x00, 512);
365 for (int i
= 0; i
< len
; i
++) {
366 buf
[i
] = ((b
[i
/ 8] << (i
% 8)) & 0x80) ? '1':'0';
372 // -------------------------------------------------------------------------
373 // string parameters lib
374 // -------------------------------------------------------------------------
376 // -------------------------------------------------------------------------
378 // bg, en - symbol numbers in param line of beginning and ending parameter
379 // paramnum - param number (from 0)
380 // -------------------------------------------------------------------------
381 int param_getptr(const char *line
, int *bg
, int *en
, int paramnum
)
384 int len
= strlen(line
);
390 while (line
[*bg
] ==' ' || line
[*bg
]=='\t') (*bg
)++;
395 for (i
= 0; i
< paramnum
; i
++) {
396 while (line
[*bg
]!=' ' && line
[*bg
]!='\t' && line
[*bg
] != '\0') (*bg
)++;
397 while (line
[*bg
]==' ' || line
[*bg
]=='\t') (*bg
)++;
399 if (line
[*bg
] == '\0') return 1;
403 while (line
[*en
] != ' ' && line
[*en
] != '\t' && line
[*en
] != '\0') (*en
)++;
411 int param_getlength(const char *line
, int paramnum
)
415 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 0;
420 char param_getchar(const char *line
, int paramnum
) {
421 return param_getchar_indx(line
, 0, paramnum
);
424 char param_getchar_indx(const char *line
, int indx
, int paramnum
) {
427 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 0x00;
432 return line
[bg
+ indx
];
435 uint8_t param_get8(const char *line
, int paramnum
)
437 return param_get8ex(line
, paramnum
, 0, 10);
441 * @brief Reads a decimal integer (actually, 0-254, not 255)
444 * @return -1 if error
446 uint8_t param_getdec(const char *line
, int paramnum
, uint8_t *destination
)
448 uint8_t val
= param_get8ex(line
, paramnum
, 255, 10);
449 if( (int8_t) val
== -1) return 1;
450 (*destination
) = val
;
454 * @brief Checks if param is decimal
459 uint8_t param_isdec(const char *line
, int paramnum
)
462 //TODO, check more thorougly
463 if (!param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
464 // return strtoul(&line[bg], NULL, 10) & 0xff;
469 uint8_t param_get8ex(const char *line
, int paramnum
, int deflt
, int base
)
473 if (!param_getptr(line
, &bg
, &en
, paramnum
))
474 return strtoul(&line
[bg
], NULL
, base
) & 0xff;
479 uint32_t param_get32ex(const char *line
, int paramnum
, int deflt
, int base
)
483 if (!param_getptr(line
, &bg
, &en
, paramnum
))
484 return strtoul(&line
[bg
], NULL
, base
);
489 uint64_t param_get64ex(const char *line
, int paramnum
, int deflt
, int base
)
493 if (!param_getptr(line
, &bg
, &en
, paramnum
))
494 return strtoull(&line
[bg
], NULL
, base
);
499 int param_gethex(const char *line
, int paramnum
, uint8_t *data
, int hexcnt
)
506 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
508 if (en
- bg
+ 1 != hexcnt
)
511 for(i
= 0; i
< hexcnt
; i
+= 2) {
512 if (!(isxdigit((unsigned char)line
[bg
+ i
]) && isxdigit((unsigned char)line
[bg
+ i
+ 1])) ) return 1;
514 sscanf((char[]){line
[bg
+ i
], line
[bg
+ i
+ 1], 0}, "%X", &temp
);
515 data
[i
/ 2] = temp
& 0xff;
521 int param_gethex_ex(const char *line
, int paramnum
, uint8_t *data
, int *hexcnt
)
528 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
530 if (en
- bg
+ 1 > *hexcnt
) return 1;
532 *hexcnt
= en
- bg
+ 1;
533 if (*hexcnt
% 2) //error if not complete hex bytes
536 for(i
= 0; i
< *hexcnt
; i
+= 2) {
537 if (!(isxdigit((unsigned char)line
[bg
+ i
]) && isxdigit((unsigned char)line
[bg
+ i
+ 1])) ) return 1;
539 sscanf((char[]){line
[bg
+ i
], line
[bg
+ i
+ 1], 0}, "%X", &temp
);
540 data
[i
/ 2] = temp
& 0xff;
546 int param_gethex_to_eol(const char *line
, int paramnum
, uint8_t * data
, int maxdatalen
, int *datalen
) {
551 if (param_getptr(line
, &bg
, &en
, paramnum
)) return 1;
557 if (line
[indx
] == '\t' || line
[indx
] == ' ') {
562 if (isxdigit((unsigned char)line
[indx
])) {
563 buf
[strlen(buf
) + 1] = 0x00;
564 buf
[strlen(buf
)] = line
[indx
];
566 // if we have symbols other than spaces and hex
570 if (*datalen
>= maxdatalen
) {
571 // if we dont have space in buffer and have symbols to translate
575 if (strlen(buf
) >= 2) {
576 sscanf(buf
, "%x", &temp
);
577 data
[*datalen
] = (uint8_t)(temp
& 0xff);
586 //error when not completed hex bytes
592 int param_getstr(const char *line
, int paramnum
, char * str
, size_t buffersize
)
596 if (param_getptr(line
, &bg
, &en
, paramnum
)) {
600 // Prevent out of bounds errors
601 if (en
- bg
+ 1 >= buffersize
) {
602 printf("out of bounds error: want %d bytes have %zd bytes\n", en
- bg
+ 1 + 1, buffersize
);
606 memcpy(str
, line
+ bg
, en
- bg
+ 1);
607 str
[en
- bg
+ 1] = 0;
613 The following methods comes from Rfidler sourcecode.
614 https://github.com/ApertureLabsLtd/RFIDler/blob/master/firmware/Pic32/RFIDler.X/src/
617 // convert hex to sequence of 0/1 bit values
618 // returns number of bits converted
619 int hextobinarray(char *target
, char *source
)
621 int length
, i
, count
= 0;
622 char* start
= source
;
625 length
= strlen(source
);
626 // process 4 bits (1 hex digit) at a time
631 if (x
>= 'a' && x
<= 'f')
633 // convert to numeric value
634 if (x
>= '0' && x
<= '9')
636 else if (x
>= 'A' && x
<= 'F')
639 printf("Discovered unknown character %c %d at idx %tu of %s\n", x
, x
, source
- start
, start
);
643 for(i
= 0 ; i
< 4 ; ++i
, ++count
)
644 *(target
++)= (x
>> (3 - i
)) & 1;
650 // convert binary array of 0x00/0x01 values to hex (safe to do in place as target will always be shorter than source)
651 // return number of bits converted
652 int binarraytohex(char *target
,char *source
, int length
)
662 for(i
= x
= 0 ; i
< 4 ; ++i
)
663 x
+= ( source
[i
] << (3 - i
));
664 sprintf(target
,"%X", (unsigned int)x
);
672 // return parity bit required to match type
673 uint8_t GetParity( uint8_t *bits
, uint8_t type
, int length
)
677 for(x
= 0 ; length
> 0 ; --length
)
678 x
+= bits
[length
- 1];
684 // add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half
685 void wiegand_add_parity(uint8_t *target
, uint8_t *source
, uint8_t length
)
687 *(target
++)= GetParity(source
, EVEN
, length
/ 2);
688 memcpy(target
, source
, length
);
690 *(target
)= GetParity(source
+ length
/ 2, ODD
, length
/ 2);
693 // xor two arrays together for len items. The dst array contains the new xored values.
694 void xor(unsigned char *dst
, unsigned char *src
, size_t len
) {
695 for( ; len
> 0; len
--,dst
++,src
++)
699 // RotateLeft - Ultralight, Desfire, works on byte level
700 // 00-01-02 >> 01-02-00
701 void rol(uint8_t *data
, const size_t len
){
702 uint8_t first
= data
[0];
703 for (size_t i
= 0; i
< len
-1; i
++) {
710 // Replace unprintable characters with a dot in char buffer
711 void clean_ascii(unsigned char *buf
, size_t len
) {
712 for (size_t i
= 0; i
< len
; i
++) {
713 if (!isprint(buf
[i
]))
718 // replace \r \n to \0
719 void strcleanrn(char *buf
, size_t len
) {
720 strcreplace(buf
, len
, '\n', '\0');
721 strcreplace(buf
, len
, '\r', '\0');
724 // replace char in buffer
725 void strcreplace(char *buf
, size_t len
, char from
, char to
) {
726 for (size_t i
= 0; i
< len
; i
++) {
732 char *strmcopy(char *buf
) {
734 if ((str
= (char*) malloc(strlen(buf
) + 1)) != NULL
) {
735 memset(str
, 0, strlen(buf
) + 1);
742 // determine number of logical CPU cores (use for multithreaded functions)
743 extern int num_CPUs(void)
746 #include <sysinfoapi.h>
748 GetSystemInfo(&sysinfo
);
749 return sysinfo
.dwNumberOfProcessors
;
750 #elif defined(__linux__) || defined(__APPLE__)
752 return sysconf(_SC_NPROCESSORS_ONLN
);