diff options
Diffstat (limited to 'libtwn3.c')
| -rw-r--r-- | libtwn3.c | 100 |
1 files changed, 81 insertions, 19 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], |