44 lines
1.5 KiB
C
44 lines
1.5 KiB
C
#include "decode.h"
|
|
|
|
uint8_t decode_combine_nibbles(uint8_t high, uint8_t low){
|
|
uint8_t byte;
|
|
uint8_t high_nibble;
|
|
uint8_t low_nibble;
|
|
decode_byte(high,&high_nibble);
|
|
decode_byte(low,&low_nibble);
|
|
byte = (high_nibble << 4) | (low_nibble);
|
|
return byte;
|
|
}
|
|
|
|
void decode_byte(uint8_t in, uint8_t* nibble){
|
|
uint8_t p0 = (in >> 0) & 0x01;
|
|
uint8_t p1 = (in >> 1) & 0x01;
|
|
uint8_t p2 = (in >> 2) & 0x01;
|
|
uint8_t d0 = (in >> 3) & 0x01;
|
|
uint8_t d1 = (in >> 4) & 0x01;
|
|
uint8_t d2 = (in >> 5) & 0x01;
|
|
uint8_t d3 = (in >> 6) & 0x01;
|
|
uint8_t MSB = (in >> 7) & 0x01;
|
|
|
|
// Check parity bits
|
|
uint8_t error = 0;
|
|
if ((p0 ^ d0 ^ d1 ^ d2) != 0) error |= 0x01; // Error in p0 group
|
|
if ((p1 ^ d0 ^ d1 ^ d3) != 0) error |= 0x02; // Error in p1 group // 0000111
|
|
if ((p2 ^ d0 ^ d2 ^ d3) != 0) error |= 0x04; // Error in p2 group
|
|
|
|
if (MSB == 0 && error != 0) {
|
|
switch(error) {
|
|
case 0x07: d0 ^= 1; break; // Error in d0 0x7 = 0111
|
|
case 0x03: d1 ^= 1; break; // Error in d1 0x3 = 0011
|
|
case 0x05: d2 ^= 1; break; // Error in d2 0x5 = 0101
|
|
case 0x06: d3 ^= 1; break; // Error in d3 0x6 = 0110
|
|
|
|
case 0x01: p0 ^= 1; break; // Error in p0 0x1 = 0001
|
|
case 0x02: p1 ^= 1; break; // Error in p1 0x2 = 0010
|
|
case 0x04: p2 ^= 1; break; // Error in p2 0x4 = 0100
|
|
}
|
|
} else if (MSB == 1) MSB ^= 1; // If MSB is 1, flip it
|
|
|
|
// create nibble from data bits
|
|
*nibble = (d3 << 3) | (d2 << 2) | (d1 << 1) | d0;
|
|
} |