diff options
| -rw-r--r-- | libtwn3.c | 100 | ||||
| -rw-r--r-- | libtwn3.h | 1 | ||||
| -rw-r--r-- | twn3_test.c | 25 |
3 files changed, 102 insertions, 24 deletions
@@ -37,6 +37,27 @@ typedef struct /*_twn3_trl_t*/ { } twn3_trl_t; #pragma pack(pop) +static inline uint8_t hex_nibble(char in){ + if(in <= 0x39){ + return in & 0x0F; + } + return (in & ~(0x20)) - 55; +} + +static inline size_t decode_ascii_inplace(uint8_t* data, size_t bytes){ + size_t u; + for(u = 0; bytes > 2 * u; u++){ + if(!isprint(data[u * 2])){ + break; + } + if(!isxdigit(data[u * 2]) || !isxdigit(data[u * 2 + 1])){ + return -1; + } + data[u] = (hex_nibble(data[u * 2]) << 4) | hex_nibble(data[u * 2 + 1]); + } + return u; +} + int twn3_open(char* device){ int fd = open(device, O_RDWR | O_CLOEXEC); struct termios tty; @@ -247,39 +268,80 @@ int twn3_sync_antenna(int fd, uint8_t flags, uint8_t enable){ return (bytes > 2 && data[0] == 'P') ? 0 : -1; } -int twn3_sync_select_first(int fd, uint8_t flags, uint8_t* type, uint8_t* serial, size_t* length); +int twn3_sync_select_first(int fd, uint8_t flags, uint8_t* type, uint8_t* serial, size_t* length){ + char data[30]; + size_t u = 0; + ssize_t bytes = twn3_sync_command(fd, flags, (uint8_t*) "s", 1, (uint8_t*) data, sizeof(data)); + + *type = tag_none; + if(bytes > 2 && data[0] != 'N'){ + *type = tag_unknown; + + u = decode_ascii_inplace((uint8_t*) data, bytes - 2); + + //TBD + if(u % 2){ + //type tag prepended + *type = data[0]; + memcpy(serial, data + 1, MIN(*length, u - 1)); + *length = u - 1; + } + else{ + memcpy(serial, data, MIN(*length, u)); + *length = u; + } + } + return (bytes > 0) ? 0 : -1; +} + int twn3_sync_list(int fd, uint8_t flags, uint8_t* type, uint8_t* serial, size_t* length); int twn3_sync_select(int fd, uint8_t flags, uint8_t* type, uint8_t* serial, size_t* length); -int twn3_sync_auth(int fd, uint8_t flags, uint8_t key_a, uint8_t block, uint8_t key[6]); -int twn3_sync_auth_stored(int fd, uint8_t flags, uint8_t key_a, uint8_t block, uint8_t key_id); -static inline uint8_t hex_nibble(char in){ - if(in <= 39){ - return in & 0x0F; +int twn3_sync_auth(int fd, uint8_t flags, uint8_t key_a, uint8_t sector, uint8_t key[6]){ + char data[20]; + ssize_t bytes = 0; + + snprintf(data, sizeof(data) - 1, "l%02X%02X%02X%02X%02X%02X%02X%02X\r", sector, key_a ? 0xAA : 0xBB, + key[0], key[1], key[2], key[3], key[4], key[5]); + + if(sector > 0x3F){ + return -1; } - if(in <= 46){ - return in - 55; + + bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 17, (uint8_t*) data, sizeof(data)); + return (bytes > 0 && data[0] == 'L') ? 0 : -1; +} + +int twn3_sync_auth_stored(int fd, uint8_t flags, uint8_t key_a, uint8_t sector, uint8_t key_id){ + char data[10]; + ssize_t bytes = 0; + uint8_t key = (key_a) ? (key_id + 0x10) : (key_id + 0x30); + + snprintf(data, sizeof(data) - 1, "l%02X%02X\r", sector, key); + + if(sector > 0x3F || key_id > 31){ + return -1; } - return in - 87; + + bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 6, (uint8_t*) data, sizeof(data)); + return (bytes > 0 && data[0] == 'L') ? 0 : -1; } int twn3_sync_read(int fd, uint8_t flags, uint8_t block, uint8_t* values, size_t max_len){ char data[40]; - ssize_t bytes; - unsigned u; + ssize_t bytes = 0; + size_t u; snprintf(data, sizeof(data) - 1, "r%02X", block); bytes = twn3_sync_command(fd, flags, (uint8_t*) data, 3, (uint8_t*) data, sizeof(data)); if(bytes > 0){ if(bytes > 10){ - //convert ascii hex representation - for(u = 0; u < max_len && bytes > 2 * u; u++){ - if(!isxdigit(data[u * 2]) || !isxdigit(data[(u * 2) + 1])){ - return -1; - } + u = decode_ascii_inplace((uint8_t*) data, bytes - 2); + memcpy(values, data, MIN(u, max_len)); - values[u] = (hex_nibble(data[u * 2]) << 4) | hex_nibble(data[(u * 2) + 1]); - } + DBG(printf("Decoded %ld bytes of data\n", u)); + DBG(xxd((uint8_t*) values, u)); + DBG(printf("\n")); return u; } else{ @@ -291,7 +353,7 @@ int twn3_sync_read(int fd, uint8_t flags, uint8_t block, uint8_t* values, size_t int twn3_sync_write(int fd, uint8_t flags, uint8_t block, uint8_t value[16]){ char data[40]; - ssize_t bytes; + ssize_t bytes = 0; snprintf(data, sizeof(data) - 1, "w%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", block, value[0], value[1], value[2], value[3], @@ -3,6 +3,7 @@ #include <inttypes.h> typedef enum /*_twn3_tag_type*/ { + tag_none = 0x00, tag_mf_light = 0x01, tag_mf_classic_1k = 0x02, tag_mf_classic_4l = 0x03, diff --git a/twn3_test.c b/twn3_test.c index ca10574..734201e 100644 --- a/twn3_test.c +++ b/twn3_test.c @@ -12,8 +12,8 @@ int main(int argc, char** argv){ char data[1024]; unsigned u; - int device = twn3_open("/dev/ttyACM0"); - + int device = twn3_open("/dev/ttyACM3"); + printf("Device is at fd %d\n", device); if(device < 0){ printf("%s\n", strerror(errno)); @@ -33,7 +33,7 @@ int main(int argc, char** argv){ } //write some userdata - twn3_sync_write_eeprom(device, 0, 19, 0xAA); + //twn3_sync_write_eeprom(device, 0, 19, 0xAA); //set ExtendID bit //twn3_sync_write_eeprom(device, 0, 5, 0x05); @@ -42,8 +42,23 @@ int main(int argc, char** argv){ //twn3_sync_restart(device, 0); //store a key - uint8_t key[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - twn3_sync_storekey(device, 0, 12, key); + //uint8_t key[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + //twn3_sync_storekey(device, 0, 12, key); + + //wait for a single tag + int status; + uint8_t type = tag_none; + uint8_t serial[20]; + size_t serial_length = sizeof(serial); + for(status = twn3_sync_select_first(device, 0, &type, serial, &serial_length); + status == 0; + status = twn3_sync_select_first(device, 0, &type, serial, &serial_length)){ + if(type != tag_none){ + printf("Tag with UID length %lu found, type %02X\n", serial_length, type); + break; + } + serial_length = sizeof(serial); + } close(device); return EXIT_SUCCESS; |