mirror of
https://github.com/DarkFlippers/unleashed-firmware
synced 2024-12-26 20:53:07 +00:00
151 lines
4.2 KiB
C++
151 lines
4.2 KiB
C++
|
#include "protocol-emmarin.h"
|
||
|
#include <furi.h>
|
||
|
|
||
|
#define EM_HEADER_POS 55
|
||
|
#define EM_HEADER_MASK (0x1FFLLU << EM_HEADER_POS)
|
||
|
|
||
|
#define EM_FIRST_ROW_POS 50
|
||
|
|
||
|
#define EM_ROW_COUNT 10
|
||
|
#define EM_COLUMN_COUNT 4
|
||
|
#define EM_BITS_PER_ROW_COUNT (EM_COLUMN_COUNT + 1)
|
||
|
|
||
|
#define EM_COLUMN_POS 4
|
||
|
#define EM_STOP_POS 0
|
||
|
#define EM_STOP_MASK (0x1LLU << EM_STOP_POS)
|
||
|
|
||
|
#define EM_HEADER_AND_STOP_MASK (EM_HEADER_MASK | EM_STOP_MASK)
|
||
|
#define EM_HEADER_AND_STOP_DATA (EM_HEADER_MASK)
|
||
|
|
||
|
typedef uint64_t EMMarinCardData;
|
||
|
|
||
|
void write_nibble(bool low_nibble, uint8_t data, EMMarinCardData* card_data) {
|
||
|
uint8_t parity_sum = 0;
|
||
|
uint8_t start = 0;
|
||
|
if(!low_nibble) start = 4;
|
||
|
|
||
|
for(int8_t i = (start + 3); i >= start; i--) {
|
||
|
parity_sum += (data >> i) & 1;
|
||
|
*card_data = (*card_data << 1) | ((data >> i) & 1);
|
||
|
}
|
||
|
|
||
|
*card_data = (*card_data << 1) | ((parity_sum % 2) & 1);
|
||
|
}
|
||
|
|
||
|
uint8_t ProtocolEMMarin::get_encoded_data_size() {
|
||
|
return sizeof(EMMarinCardData);
|
||
|
}
|
||
|
|
||
|
uint8_t ProtocolEMMarin::get_decoded_data_size() {
|
||
|
return 5;
|
||
|
}
|
||
|
|
||
|
void ProtocolEMMarin::encode(
|
||
|
const uint8_t* decoded_data,
|
||
|
const uint8_t decoded_data_size,
|
||
|
uint8_t* encoded_data,
|
||
|
const uint8_t encoded_data_size) {
|
||
|
furi_check(decoded_data_size >= get_decoded_data_size());
|
||
|
furi_check(encoded_data_size >= get_encoded_data_size());
|
||
|
|
||
|
EMMarinCardData card_data;
|
||
|
|
||
|
// header
|
||
|
card_data = 0b111111111;
|
||
|
|
||
|
// data
|
||
|
for(uint8_t i = 0; i < get_decoded_data_size(); i++) {
|
||
|
write_nibble(false, decoded_data[i], &card_data);
|
||
|
write_nibble(true, decoded_data[i], &card_data);
|
||
|
}
|
||
|
|
||
|
// column parity and stop bit
|
||
|
uint8_t parity_sum;
|
||
|
|
||
|
for(uint8_t c = 0; c < EM_COLUMN_COUNT; c++) {
|
||
|
parity_sum = 0;
|
||
|
for(uint8_t i = 1; i <= EM_ROW_COUNT; i++) {
|
||
|
uint8_t parity_bit = (card_data >> (i * EM_BITS_PER_ROW_COUNT - 1)) & 1;
|
||
|
parity_sum += parity_bit;
|
||
|
}
|
||
|
card_data = (card_data << 1) | ((parity_sum % 2) & 1);
|
||
|
}
|
||
|
|
||
|
// stop bit
|
||
|
card_data = (card_data << 1) | 0;
|
||
|
|
||
|
memcpy(encoded_data, &card_data, get_encoded_data_size());
|
||
|
}
|
||
|
|
||
|
void ProtocolEMMarin::decode(
|
||
|
const uint8_t* encoded_data,
|
||
|
const uint8_t encoded_data_size,
|
||
|
uint8_t* decoded_data,
|
||
|
const uint8_t decoded_data_size) {
|
||
|
furi_check(decoded_data_size >= get_decoded_data_size());
|
||
|
furi_check(encoded_data_size >= get_encoded_data_size());
|
||
|
|
||
|
uint8_t decoded_data_index = 0;
|
||
|
EMMarinCardData card_data = *(reinterpret_cast<const EMMarinCardData*>(encoded_data));
|
||
|
|
||
|
// clean result
|
||
|
memset(decoded_data, 0, decoded_data_size);
|
||
|
|
||
|
// header
|
||
|
for(uint8_t i = 0; i < 9; i++) {
|
||
|
card_data = card_data << 1;
|
||
|
}
|
||
|
|
||
|
// nibbles
|
||
|
uint8_t value = 0;
|
||
|
for(uint8_t r = 0; r < EM_ROW_COUNT; r++) {
|
||
|
uint8_t nibble = 0;
|
||
|
for(uint8_t i = 0; i < 5; i++) {
|
||
|
if(i < 4) nibble = (nibble << 1) | (card_data & (1LLU << 63) ? 1 : 0);
|
||
|
card_data = card_data << 1;
|
||
|
}
|
||
|
value = (value << 4) | nibble;
|
||
|
if(r % 2) {
|
||
|
decoded_data[decoded_data_index] |= value;
|
||
|
decoded_data_index++;
|
||
|
value = 0;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
bool ProtocolEMMarin::can_be_decoded(const uint8_t* encoded_data, const uint8_t encoded_data_size) {
|
||
|
furi_check(encoded_data_size >= get_encoded_data_size());
|
||
|
const EMMarinCardData* card_data = reinterpret_cast<const EMMarinCardData*>(encoded_data);
|
||
|
|
||
|
// check header and stop bit
|
||
|
if((*card_data & EM_HEADER_AND_STOP_MASK) != EM_HEADER_AND_STOP_DATA) return false;
|
||
|
|
||
|
// check row parity
|
||
|
for(uint8_t i = 0; i < EM_ROW_COUNT; i++) {
|
||
|
uint8_t parity_sum = 0;
|
||
|
|
||
|
for(uint8_t j = 0; j < EM_BITS_PER_ROW_COUNT; j++) {
|
||
|
parity_sum += (*card_data >> (EM_FIRST_ROW_POS - i * EM_BITS_PER_ROW_COUNT + j)) & 1;
|
||
|
}
|
||
|
|
||
|
if((parity_sum % 2)) {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// check columns parity
|
||
|
for(uint8_t i = 0; i < EM_COLUMN_COUNT; i++) {
|
||
|
uint8_t parity_sum = 0;
|
||
|
|
||
|
for(uint8_t j = 0; j < EM_ROW_COUNT + 1; j++) {
|
||
|
parity_sum += (*card_data >> (EM_COLUMN_POS - i + j * EM_BITS_PER_ROW_COUNT)) & 1;
|
||
|
}
|
||
|
|
||
|
if((parity_sum % 2)) {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
return true;
|
||
|
}
|