diff --git a/armsrc/hitagS.c b/armsrc/hitagS.c index 4c4e3d493..7a42fcd81 100644 --- a/armsrc/hitagS.c +++ b/armsrc/hitagS.c @@ -419,7 +419,7 @@ static void hts_init_clock(void) { static int check_select(const uint8_t *rx, uint32_t uid) { // global var? - concatbits((uint8_t *)&reader_selected_uid, 0, rx, 5, 32); + concatbits((uint8_t *)&reader_selected_uid, 0, rx, 5, 32, false); reader_selected_uid = BSWAP_32(reader_selected_uid); if (reader_selected_uid == uid) { @@ -1090,7 +1090,7 @@ static int hts_select_tag(const lf_hitag_data_t *packet, uint8_t *tx, size_t siz protocol_mode = packet->mode; uint8_t cmd = protocol_mode; - txlen = concatbits(tx, txlen, &cmd, 0, 5); + txlen = concatbits(tx, txlen, &cmd, 0, 5, false); hts_send_receive(tx, txlen, rx, sizeofrx, &rxlen, t_wait, ledcontrol, true); if (rxlen != 32) { @@ -1105,10 +1105,10 @@ static int hts_select_tag(const lf_hitag_data_t *packet, uint8_t *tx, size_t siz // select uid txlen = 0; cmd = HITAGS_SELECT; - txlen = concatbits(tx, txlen, &cmd, 0, 5); - txlen = concatbits(tx, txlen, rx, 0, 32); + txlen = concatbits(tx, txlen, &cmd, 0, 5, false); + txlen = concatbits(tx, txlen, rx, 0, 32, false); uint8_t crc = CRC8Hitag1Bits(tx, txlen); - txlen = concatbits(tx, txlen, &crc, 0, 8); + txlen = concatbits(tx, txlen, &crc, 0, 8, false); hts_send_receive(tx, txlen, rx, sizeofrx, &rxlen, HITAG_T_WAIT_SC, ledcontrol, false); @@ -1140,8 +1140,8 @@ static int hts_select_tag(const lf_hitag_data_t *packet, uint8_t *tx, size_t siz } txlen = 0; - txlen = concatbits(tx, txlen, rnd, 0, 32); - txlen = concatbits(tx, txlen, auth_ks, 0, 32); + txlen = concatbits(tx, txlen, rnd, 0, 32, false); + txlen = concatbits(tx, txlen, auth_ks, 0, 32, false); DBG DbpString("Authenticating using key:"); DBG Dbhexdump(6, packet->key, false); @@ -1173,13 +1173,13 @@ static int hts_select_tag(const lf_hitag_data_t *packet, uint8_t *tx, size_t siz // send write page request txlen = 0; cmd = HITAGS_WRITE_PAGE; - txlen = concatbits(tx, txlen, &cmd, 0, 4); + txlen = concatbits(tx, txlen, &cmd, 0, 4, false); uint8_t addr = 64; - txlen = concatbits(tx, txlen, &addr, 0, 8); + txlen = concatbits(tx, txlen, &addr, 0, 8, false); crc = CRC8Hitag1Bits(tx, txlen); - txlen = concatbits(tx, txlen, &crc, 0, 8); + txlen = concatbits(tx, txlen, &crc, 0, 8, false); hts_send_receive(tx, txlen, rx, sizeofrx, &rxlen, HITAG_T_WAIT_SC, ledcontrol, false); @@ -1189,9 +1189,9 @@ static int hts_select_tag(const lf_hitag_data_t *packet, uint8_t *tx, size_t siz } txlen = 0; - txlen = concatbits(tx, txlen, packet->pwd, 0, 32); + txlen = concatbits(tx, txlen, packet->pwd, 0, 32, false); crc = CRC8Hitag1Bits(tx, txlen); - txlen = concatbits(tx, txlen, &crc, 0, 8); + txlen = concatbits(tx, txlen, &crc, 0, 8, false); hts_send_receive(tx, txlen, rx, sizeofrx, &rxlen, HITAG_T_WAIT_SC, ledcontrol, false); @@ -1287,11 +1287,11 @@ void hts_read(const lf_hitag_data_t *payload, bool ledcontrol) { //send read request size_t txlen = 0; uint8_t cmd = HITAGS_READ_PAGE; - txlen = concatbits(tx, txlen, &cmd, 0, 4); + txlen = concatbits(tx, txlen, &cmd, 0, 4, false); uint8_t addr = page_addr; - txlen = concatbits(tx, txlen, &addr, 0, 8); + txlen = concatbits(tx, txlen, &addr, 0, 8, false); uint8_t crc = CRC8Hitag1Bits(tx, txlen); - txlen = concatbits(tx, txlen, &crc, 0, 8); + txlen = concatbits(tx, txlen, &crc, 0, 8, false); hts_send_receive(tx, txlen, rx, ARRAYLEN(rx), &rxlen, HITAG_T_WAIT_SC, ledcontrol, false); @@ -1396,13 +1396,13 @@ void hts_write_page(const lf_hitag_data_t *payload, bool ledcontrol) { txlen = 0; uint8_t cmd = HITAGS_WRITE_PAGE; - txlen = concatbits(tx, txlen, &cmd, 0, 4); + txlen = concatbits(tx, txlen, &cmd, 0, 4, false); uint8_t addr = payload->page; - txlen = concatbits(tx, txlen, &addr, 0, 8); + txlen = concatbits(tx, txlen, &addr, 0, 8, false); uint8_t crc = CRC8Hitag1Bits(tx, txlen); - txlen = concatbits(tx, txlen, &crc, 0, 8); + txlen = concatbits(tx, txlen, &crc, 0, 8, false); hts_send_receive(tx, txlen, rx, ARRAYLEN(rx), &rxlen, HITAG_T_WAIT_SC, ledcontrol, false); @@ -1430,9 +1430,9 @@ void hts_write_page(const lf_hitag_data_t *payload, bool ledcontrol) { // } txlen = 0; - txlen = concatbits(tx, txlen, payload->data, 0, 32); + txlen = concatbits(tx, txlen, payload->data, 0, 32, false); crc = CRC8Hitag1Bits(tx, txlen); - txlen = concatbits(tx, txlen, &crc, 0, 8); + txlen = concatbits(tx, txlen, &crc, 0, 8, false); enable_page_tearoff = g_tearoff_enabled; @@ -1493,7 +1493,7 @@ int hts_read_uid(uint32_t *uid, bool ledcontrol, bool send_answer) { size_t txlen = 0; uint8_t tx[HITAG_FRAME_LEN] = { 0x00 }; - txlen = concatbits(tx, txlen, &cmd, 0, 5); + txlen = concatbits(tx, txlen, &cmd, 0, 5, false); hts_send_receive(tx, txlen, rx, ARRAYLEN(rx), &rxlen, HITAG_T_WAIT_FIRST, ledcontrol, true); diff --git a/client/src/cmdlfem410x.c b/client/src/cmdlfem410x.c index 145392eb7..7f296336c 100644 --- a/client/src/cmdlfem410x.c +++ b/client/src/cmdlfem410x.c @@ -688,11 +688,11 @@ static int CmdEM410xClone(const char *Cmd) { uint8_t r_parity = 0; uint8_t nibble = id >> i & 0xF; - databits = concatbits(data, databits, &nibble, 4, 4); + databits = concatbits(data, databits, &nibble, 4, 4, false); for (size_t j = 0; j < 4; j++) { r_parity ^= nibble >> j & 1; } - databits = concatbits(data, databits, &r_parity, 7, 1); + databits = concatbits(data, databits, &r_parity, 7, 1, false); c_parity ^= nibble; } data[7] |= c_parity << 1; diff --git a/common/commonutil.c b/common/commonutil.c index d4eed608e..35a583988 100644 --- a/common/commonutil.c +++ b/common/commonutil.c @@ -555,9 +555,13 @@ void reverse_arraybytes_copy(uint8_t *arr, uint8_t *dest, size_t len) { } // TODO: Boost performance by copying in chunks of 1, 2, or 4 bytes when feasible. -size_t concatbits(uint8_t *dest, int dest_offset, const uint8_t *src, int src_offset, size_t nbits) { +/** + * @brief Concatenate bits from src to dest, bitstream is stored MSB first + * which means that the dest_offset=0 is the MSB of the dest[0] + * + */ +size_t concatbits(uint8_t *dest, int dest_offset, const uint8_t *src, int src_offset, size_t nbits, bool src_lsb) { int i, end, step; - // overlap if ((src - dest) * 8 + src_offset - dest_offset > 0) { i = 0; @@ -571,8 +575,8 @@ size_t concatbits(uint8_t *dest, int dest_offset, const uint8_t *src, int src_of for (; i != end; i += step) { // equiv of dest_bits[dest_offset + i] = src_bits[src_offset + i] - CLEAR_BIT(dest, dest_offset + i); - if (TEST_BIT(src, src_offset + i)) SET_BIT(dest, dest_offset + i); + CLEAR_BIT_MSB(dest, dest_offset + i); + if (src_lsb ? TEST_BIT_LSB(src, src_offset + i) : TEST_BIT_MSB(src, src_offset + i)) SET_BIT_MSB(dest, dest_offset + i); } return dest_offset + nbits; diff --git a/common/commonutil.h b/common/commonutil.h index 2ab4fbd79..2360f2a57 100644 --- a/common/commonutil.h +++ b/common/commonutil.h @@ -150,7 +150,7 @@ bool hexstr_to_byte_array(const char *hexstr, uint8_t *d, size_t *n); void reverse_arraybytes(uint8_t *arr, size_t len); void reverse_arraybytes_copy(uint8_t *arr, uint8_t *dest, size_t len); -size_t concatbits(uint8_t *dest, int dest_offset, const uint8_t *src, int src_offset, size_t nbits); +size_t concatbits(uint8_t *dest, int dest_offset, const uint8_t *src, int src_offset, size_t nbits, bool src_lsb); int char2int(char c); int hexstr2ByteArr(const char *hexstr, unsigned char *array, size_t asize); #endif diff --git a/include/common.h b/include/common.h index b405b2ac7..086354750 100644 --- a/include/common.h +++ b/include/common.h @@ -202,10 +202,15 @@ extern bool g_tearoff_enabled; #endif // bit stream operations -#define TEST_BIT(data, i) (*((data) + ((i) / 8)) >> (7 - ((i) % 8))) & 1 -#define SET_BIT(data, i) *((data) + ((i) / 8)) |= (1 << (7 - ((i) % 8))) -#define CLEAR_BIT(data, i) *((data) + ((i) / 8)) &= ~(1 << (7 - ((i) % 8))) -#define FLIP_BIT(data, i) *((data) + ((i) / 8)) ^= (1 << (7 - ((i) % 8))) +#define TEST_BIT_MSB(data, i) ((*((data) + ((i) / 8)) >> (7 - ((i) % 8))) & 1) +#define SET_BIT_MSB(data, i) (*((data) + ((i) / 8)) |= (1 << (7 - ((i) % 8)))) +#define CLEAR_BIT_MSB(data, i) (*((data) + ((i) / 8)) &= ~(1 << (7 - ((i) % 8)))) +#define FLIP_BIT_MSB(data, i) (*((data) + ((i) / 8)) ^= (1 << (7 - ((i) % 8)))) + +#define TEST_BIT_LSB(data, i) ((*((data) + ((i) / 8)) >> ((i) % 8)) & 1) +#define SET_BIT_LSB(data, i) (*((data) + ((i) / 8)) |= (1 << ((i) % 8))) +#define CLEAR_BIT_LSB(data, i) (*((data) + ((i) / 8)) &= ~(1 << ((i) % 8))) +#define FLIP_BIT_LSB(data, i) (*((data) + ((i) / 8)) ^= (1 << ((i) % 8))) // time for decompressing and loading the image to the FPGA #define FPGA_LOAD_WAIT_TIME (1500)