mirror of
https://github.com/markqvist/OpenModem.git
synced 2024-12-29 01:16:17 -05:00
12-byte interleaver
This commit is contained in:
Mark Qvist
parent
35a92e167e
commit
2462188597
@ -3,8 +3,8 @@
|
||||
#define FSK_CFG
|
||||
|
||||
// Debug & test options
|
||||
#define SERIAL_DEBUG false
|
||||
#define PASSALL false
|
||||
#define SERIAL_DEBUG true
|
||||
#define PASSALL true
|
||||
#define AUTOREPLY false
|
||||
|
||||
// Modem options
|
||||
@ -17,7 +17,7 @@
|
||||
#define CONFIG_AFSK_RXTIMEOUT 0 // How long a read operation from the modem
|
||||
// will wait for data before timing out.
|
||||
|
||||
#define CONFIG_AFSK_PREAMBLE_LEN 250UL // The length of the packet preamble in milliseconds
|
||||
#define CONFIG_AFSK_PREAMBLE_LEN 450UL // The length of the packet preamble in milliseconds
|
||||
#define CONFIG_AFSK_TRAILER_LEN 20UL // The length of the packet tail in milliseconds
|
||||
|
||||
#endif
|
||||
@ -68,15 +68,18 @@ static void mp1Decode(MP1 *mp1) {
|
||||
|
||||
// If header indicates a padded packet, remove
|
||||
// padding
|
||||
uint8_t padding = header >> 4;
|
||||
if (header & MP1_HEADER_PADDED) {
|
||||
buffer++;
|
||||
for (int i = 0; i < padding; i++) {
|
||||
buffer++;
|
||||
}
|
||||
}
|
||||
|
||||
if (SERIAL_DEBUG) kprintf("[TS=%d] ", mp1->packetLength);
|
||||
|
||||
// Set the payload length of the packet to the counted
|
||||
// length minus 1, so we remove the checksum
|
||||
packet.dataLength = mp1->packetLength - 2 - (header & 0x01);
|
||||
packet.dataLength = mp1->packetLength - 2 - (header & 0x01)*padding;
|
||||
|
||||
// Check if we have received a compressed packet
|
||||
if (header & MP1_HEADER_COMPRESSION) {
|
||||
@ -121,101 +124,128 @@ void mp1Poll(MP1 *mp1) {
|
||||
// HDLC_FLAG), we will start looking at the
|
||||
// incoming data and perform forward error
|
||||
// correction on it.
|
||||
if ((mp1->reading && (byte != AX25_ESC )) || (mp1->reading && (mp1->escape && byte == AX25_ESC))) {
|
||||
|
||||
|
||||
if ((mp1->reading && (byte != AX25_ESC )) || (mp1->reading && (mp1->escape && (byte == AX25_ESC || byte == HDLC_FLAG || byte == HDLC_RESET)))) {
|
||||
mp1->readLength++;
|
||||
|
||||
// Check if we have read three bytes. If we
|
||||
// have, we should now have a block of two
|
||||
// data bytes and a parity byte. This block
|
||||
if (mp1->readLength % 3 == 0) {
|
||||
if (mp1->readLength % MP1_INTERLEAVE_SIZE == 0) {
|
||||
// If the last character in the block
|
||||
// looks like a control character, we
|
||||
// need to set the escape indicator to
|
||||
// false, since the next byte will be
|
||||
// read immediately after the FEC
|
||||
// routine, and thus, the normal reading
|
||||
// code will not reset the indicator.
|
||||
if (byte == AX25_ESC || byte == HDLC_FLAG || byte == HDLC_RESET) mp1->escape = false;
|
||||
|
||||
// The block is interleaved, so we will
|
||||
// first put the received bytes in the
|
||||
// deinterleaving buffer
|
||||
mp1->interleaveIn[0] = mp1->buffer[mp1->packetLength-2];
|
||||
mp1->interleaveIn[1] = mp1->buffer[mp1->packetLength-1];
|
||||
mp1->interleaveIn[2] = byte;
|
||||
for (int i = 1; i < MP1_INTERLEAVE_SIZE; i++) {
|
||||
mp1->interleaveIn[i-1] = mp1->buffer[mp1->packetLength-(MP1_INTERLEAVE_SIZE-i)];
|
||||
}
|
||||
mp1->interleaveIn[MP1_INTERLEAVE_SIZE-1] = byte;
|
||||
|
||||
// We then deinterleave the block
|
||||
mp1Deinterleave(mp1);
|
||||
|
||||
// And write the deinterleaved data
|
||||
// back into the buffer
|
||||
mp1->buffer[mp1->packetLength-2] = mp1->interleaveIn[0];
|
||||
mp1->buffer[mp1->packetLength-1] = mp1->interleaveIn[1];
|
||||
// Adjust the packet length, since we will get
|
||||
// parity bytes in the data buffer with block
|
||||
// sizes larger than 3
|
||||
mp1->packetLength -= MP1_INTERLEAVE_SIZE/3 - 1;
|
||||
|
||||
// We now calculate a parity byte on the
|
||||
// received data.
|
||||
mp1->calculatedParity = mp1ParityBlock(mp1->buffer[mp1->packetLength-2], mp1->buffer[mp1->packetLength-1]);
|
||||
// For each 3-byte block in the deinterleaved
|
||||
// bytes, we apply forward error correction
|
||||
for (int i = 0; i < MP1_INTERLEAVE_SIZE; i+=3) {
|
||||
// We now calculate a parity byte on the
|
||||
// received data.
|
||||
|
||||
// By XORing the calculated parity byte
|
||||
// with the received parity byte, we get
|
||||
// what is called the "syndrome". This
|
||||
// number will tell us if we had any
|
||||
// errors during transmission, and if so
|
||||
// where they are. Using Hamming code, we
|
||||
// can only detect single bit errors in a
|
||||
// byte though, which is why we interleave
|
||||
// the data, since most errors will usually
|
||||
// occur in bursts of more than one bit.
|
||||
// With 2 data byte interleaving we can
|
||||
// correct 2 consecutive bit errors.
|
||||
uint8_t syndrome = mp1->calculatedParity ^ mp1->interleaveIn[2];
|
||||
if (syndrome == 0x00) {
|
||||
// If the syndrome equals 0, we either
|
||||
// don't have any errors, or the error
|
||||
// is unrecoverable, so we don't do
|
||||
// anything
|
||||
} else {
|
||||
// If the syndrome is not equal to 0,
|
||||
// there is a problem, and we will try
|
||||
// to correct it. We first need to split
|
||||
// the syndrome byte up into the two
|
||||
// actual syndrome numbers, one for
|
||||
// each data byte.
|
||||
uint8_t syndromes[2];
|
||||
syndromes[0] = syndrome & 0x0f;
|
||||
syndromes[1] = (syndrome & 0xf0) >> 4;
|
||||
// Deinterleaved data bytes
|
||||
uint8_t a = mp1->interleaveIn[i];
|
||||
uint8_t b = mp1->interleaveIn[i+1];
|
||||
|
||||
// Then we look at each syndrome number
|
||||
// to determine what bit in the data
|
||||
// bytes to correct.
|
||||
for (int i = 0; i < 2; i++) {
|
||||
uint8_t s = syndromes[i];
|
||||
uint8_t correction = 0x00;
|
||||
if (s == 1 || s == 2 || s == 4 || s == 8) {
|
||||
// This signifies an error in the
|
||||
// parity block, so we actually
|
||||
// don't need any correction
|
||||
continue;
|
||||
// Deinterleaved parity byte
|
||||
uint8_t p = mp1->interleaveIn[i+2];
|
||||
|
||||
mp1->calculatedParity = mp1ParityBlock(a, b);
|
||||
|
||||
// By XORing the calculated parity byte
|
||||
// with the received parity byte, we get
|
||||
// what is called the "syndrome". This
|
||||
// number will tell us if we had any
|
||||
// errors during transmission, and if so
|
||||
// where they are. Using Hamming code, we
|
||||
// can only detect single bit errors in a
|
||||
// byte though, which is why we interleave
|
||||
// the data, since most errors will usually
|
||||
// occur in bursts of more than one bit.
|
||||
// With 2 data byte interleaving we can
|
||||
// correct 2 consecutive bit errors.
|
||||
uint8_t syndrome = mp1->calculatedParity ^ p;
|
||||
if (syndrome == 0x00) {
|
||||
// If the syndrome equals 0, we either
|
||||
// don't have any errors, or the error
|
||||
// is unrecoverable, so we don't do
|
||||
// anything
|
||||
} else {
|
||||
// If the syndrome is not equal to 0,
|
||||
// there is a problem, and we will try
|
||||
// to correct it. We first need to split
|
||||
// the syndrome byte up into the two
|
||||
// actual syndrome numbers, one for
|
||||
// each data byte.
|
||||
uint8_t syndromes[2];
|
||||
syndromes[0] = syndrome & 0x0f;
|
||||
syndromes[1] = (syndrome & 0xf0) >> 4;
|
||||
|
||||
// Then we look at each syndrome number
|
||||
// to determine what bit in the data
|
||||
// bytes to correct.
|
||||
for (int i = 0; i < 2; i++) {
|
||||
uint8_t s = syndromes[i];
|
||||
uint8_t correction = 0x00;
|
||||
if (s == 1 || s == 2 || s == 4 || s == 8) {
|
||||
// This signifies an error in the
|
||||
// parity block, so we actually
|
||||
// don't need any correction
|
||||
continue;
|
||||
}
|
||||
|
||||
// The following determines what
|
||||
// bit to correct according to
|
||||
// the syndrome value.
|
||||
if (s == 3) correction = 0x01;
|
||||
if (s == 5) correction = 0x02;
|
||||
if (s == 6) correction = 0x04;
|
||||
if (s == 7) correction = 0x08;
|
||||
if (s == 9) correction = 0x10;
|
||||
if (s == 10) correction = 0x20;
|
||||
if (s == 11) correction = 0x40;
|
||||
if (s == 12) correction = 0x80;
|
||||
|
||||
// And finally we apply the correction
|
||||
if (i == 1) a ^= correction;
|
||||
if (i == 0) b ^= correction;
|
||||
|
||||
// This is just for testing purposes.
|
||||
// Nice to know when corrections were
|
||||
// actually made.
|
||||
if (s != 0) mp1->correctionsMade += 1;
|
||||
}
|
||||
|
||||
// The following determines what
|
||||
// bit to correct according to
|
||||
// the syndrome value.
|
||||
if (s == 3) correction = 0x01;
|
||||
if (s == 5) correction = 0x02;
|
||||
if (s == 6) correction = 0x04;
|
||||
if (s == 7) correction = 0x08;
|
||||
if (s == 9) correction = 0x10;
|
||||
if (s == 10) correction = 0x20;
|
||||
if (s == 11) correction = 0x40;
|
||||
if (s == 12) correction = 0x80;
|
||||
|
||||
// And finally we apply the correction
|
||||
mp1->buffer[mp1->packetLength-(2-i)] ^= correction;
|
||||
|
||||
// This is just for testing purposes.
|
||||
// Nice to know when corrections were
|
||||
// actually made.
|
||||
if (s != 0) mp1->correctionsMade += 1;
|
||||
}
|
||||
}
|
||||
|
||||
// We now update the checksum of the packet
|
||||
// with the deinterleaved and possibly
|
||||
// corrected bytes.
|
||||
mp1->checksum_in ^= mp1->buffer[mp1->packetLength-2];
|
||||
mp1->checksum_in ^= mp1->buffer[mp1->packetLength-1];
|
||||
// We now update the checksum of the packet
|
||||
// with the deinterleaved and possibly
|
||||
// corrected bytes.
|
||||
mp1->checksum_in ^= a;
|
||||
mp1->checksum_in ^= b;
|
||||
mp1->buffer[mp1->packetLength-(MP1_DATA_BLOCK_SIZE)+((i/3)*2)] = a;
|
||||
mp1->buffer[mp1->packetLength-(MP1_DATA_BLOCK_SIZE-1)+((i/3)*2)] = b;
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
@ -234,7 +264,7 @@ void mp1Poll(MP1 *mp1) {
|
||||
if (!mp1->escape && byte == HDLC_FLAG) {
|
||||
// We are not in an escape sequence and we
|
||||
// found a HDLC_FLAG. This can mean two things:
|
||||
if (mp1->packetLength >= MP1_MIN_FRAME_LENGTH) {
|
||||
if (mp1->readLength >= MP1_MIN_FRAME_LENGTH) {
|
||||
// We already have more data than the minimum
|
||||
// frame length, which means the flag signifies
|
||||
// the end of the packet. Pass control to the
|
||||
@ -279,6 +309,7 @@ void mp1Poll(MP1 *mp1) {
|
||||
// the escape seqeunce indicator so we don't
|
||||
// interpret the next byte as a reset or flag
|
||||
mp1->escape = true;
|
||||
|
||||
// We then continue reading the next byte.
|
||||
continue;
|
||||
}
|
||||
@ -400,36 +431,43 @@ void mp1Send(MP1 *mp1, void *_buffer, size_t length) {
|
||||
// since we are sending a parity byte for every
|
||||
// two data bytes sent, and because interleaving
|
||||
// happens in blocks of three bytes.
|
||||
uint8_t header = 0xf0;
|
||||
uint8_t header = 0x00;
|
||||
|
||||
// If we are using compression, set the
|
||||
// appropriate header flag to true.
|
||||
if (packetCompression) header ^= MP1_HEADER_COMPRESSION;
|
||||
|
||||
// We check if the data length is even
|
||||
if (length % 2 != 0) {
|
||||
// If it is not, we set the appropriate
|
||||
// We check if the data length matches our
|
||||
// required block size
|
||||
uint8_t padding = (length+2) % MP1_DATA_BLOCK_SIZE;
|
||||
|
||||
if (padding != 0) {
|
||||
// If it does not, we set the appropriate
|
||||
// header flag to indicate that we are
|
||||
// padding this packet with one byte.
|
||||
// padding this packet.
|
||||
header ^= MP1_HEADER_PADDED;
|
||||
|
||||
// And calculate how much padding we need
|
||||
padding = MP1_DATA_BLOCK_SIZE - padding;
|
||||
|
||||
// And put the amount of padding we are
|
||||
// going to append in the header
|
||||
header ^= (padding << 4);
|
||||
|
||||
// We then update the checksum with the
|
||||
// header byte and queue it for transmit
|
||||
mp1->checksum_out = mp1->checksum_out ^ header;
|
||||
mp1Putbyte(mp1, header);
|
||||
|
||||
// We now update the checksum with the
|
||||
// padding byte, and queue that for
|
||||
// transmission as well. At this point,
|
||||
// we will have pushed out two bytes of
|
||||
// data. The output function will detect
|
||||
// this, and a parity byte will be
|
||||
// calculated. The 3-byte block is then
|
||||
// actually transmitted.
|
||||
mp1->checksum_out = mp1->checksum_out ^ MP1_PADDING;
|
||||
mp1Putbyte(mp1, MP1_PADDING);
|
||||
// padding bytes, and queue these for
|
||||
// transmission as well.
|
||||
for (int i = 0; i < padding; i++) {
|
||||
mp1->checksum_out = mp1->checksum_out ^ MP1_PADDING;
|
||||
mp1Putbyte(mp1, MP1_PADDING);
|
||||
}
|
||||
} else {
|
||||
// If the length was already even, we
|
||||
// If the length already matches, we
|
||||
// just update the checksum with the
|
||||
// header byte and queue it.
|
||||
mp1->checksum_out = mp1->checksum_out ^ header;
|
||||
@ -478,132 +516,6 @@ int freeRam(void) {
|
||||
return FREE_RAM;
|
||||
}
|
||||
|
||||
// Following is the functions responsible
|
||||
// for interleaving and deinterleaving
|
||||
// blocks of data. The interleaving table
|
||||
// is also included.
|
||||
|
||||
///////////////////////////////
|
||||
// Interleave-table //
|
||||
///////////////////////////////
|
||||
//
|
||||
// Non-interleaved:
|
||||
// aaaaaaaa bbbbbbbb cccccccc
|
||||
// 12345678 12345678 12345678
|
||||
// M L
|
||||
// S S
|
||||
// B B
|
||||
//
|
||||
// Interleaved:
|
||||
// abcabcab cabcabca bcabcabc
|
||||
// 11144477 22255578 63336688
|
||||
//
|
||||
//
|
||||
// 3bit burst error patterns:
|
||||
// X||||||| X||||||| X|||||||
|
||||
// |||X|||| X||||||| X|||||||
|
||||
// |||X|||| |||X|||| X|||||||
|
||||
// |||X|||| |||X|||| |||X||||
|
||||
// ||||||X| |||X|||| |||X||||
|
||||
// ||||||X| ||||||X| |||X||||
|
||||
// ||||||X| ||||||X| |X||||||
|
||||
// |X|||||| ||||||X| |X||||||
|
||||
// |X|||||| |X|||||| |X||||||
|
||||
// |X|||||| |X|||||| ||||X|||
|
||||
// ||||X||| |X|||||| ||||X|||
|
||||
// ||||X||| ||||X||| ||||X|||
|
||||
// ||||X||| ||||X||| ||||||X|
|
||||
// |||||||X ||||X||| ||||||X|
|
||||
// |||||||X |||||X|| ||||||X|
|
||||
// |||||||X |||||X|| ||X|||||
|
||||
// ||X||||| |||||X|| ||X|||||
|
||||
// ||X||||| ||X||||| ||X|||||
|
||||
// ||X||||| ||X||||| |||||X||
|
||||
// |||||X|| ||X||||| |||||X||
|
||||
// |||||X|| |||||||X |||||X||
|
||||
// |||||X|| |||||||X |||||||X
|
||||
//
|
||||
///////////////////////////////
|
||||
|
||||
void mp1Interleave(MP1 *mp1, uint8_t byte) {
|
||||
mp1->interleaveOut[mp1->interleaveCounter] = byte;
|
||||
mp1->interleaveCounter++;
|
||||
if (mp1->interleaveCounter == 3) {
|
||||
// We have three bytes in the buffer and
|
||||
// are ready to interleave them.
|
||||
|
||||
uint8_t a = (GET_BIT(mp1->interleaveOut[0], 1) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 1) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 1) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 4) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 4) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 4) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 7));
|
||||
mp1WriteByte(mp1, a);
|
||||
|
||||
uint8_t b = (GET_BIT(mp1->interleaveOut[2], 2) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 2) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 2) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 5) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 5) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 5) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 8));
|
||||
mp1WriteByte(mp1, b);
|
||||
|
||||
uint8_t c = (GET_BIT(mp1->interleaveOut[1], 6) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 3) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 3) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 3) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 6) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 6) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 8) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 8));
|
||||
|
||||
mp1WriteByte(mp1, c);
|
||||
// mp1WriteByte(mp1, a);
|
||||
// mp1WriteByte(mp1, b);
|
||||
// mp1WriteByte(mp1, c);
|
||||
|
||||
mp1->interleaveCounter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void mp1Deinterleave(MP1 *mp1) {
|
||||
uint8_t a = (GET_BIT(mp1->interleaveIn[0], 1) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 2) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 3) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 4) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 5) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 6) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 8));
|
||||
|
||||
uint8_t b = (GET_BIT(mp1->interleaveIn[0], 2) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 3) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 4) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 5) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 6) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 1) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 8) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 7));
|
||||
|
||||
uint8_t c = (GET_BIT(mp1->interleaveIn[0], 3) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 1) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 2) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 6) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 4) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 5) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 8));
|
||||
|
||||
mp1->interleaveIn[0] = a;
|
||||
mp1->interleaveIn[1] = b;
|
||||
mp1->interleaveIn[2] = c;
|
||||
}
|
||||
|
||||
// This function compresses data using
|
||||
// the Heatshrink library
|
||||
size_t compress(uint8_t *input, size_t length) {
|
||||
@ -657,3 +569,353 @@ size_t decompress(uint8_t *input, size_t length) {
|
||||
heatshrink_decoder_free(hsd);
|
||||
return written;
|
||||
}
|
||||
|
||||
|
||||
// Following is the functions responsible
|
||||
// for interleaving and deinterleaving
|
||||
// blocks of data. The interleaving table
|
||||
// for 3-byte interleaving is also included.
|
||||
// The table for 12-byte is much simpler,
|
||||
// and should be inferable from looking
|
||||
// at the function.
|
||||
|
||||
///////////////////////////////
|
||||
// Interleave-table (3-byte) //
|
||||
///////////////////////////////
|
||||
//
|
||||
// Non-interleaved:
|
||||
// aaaaaaaa bbbbbbbb cccccccc
|
||||
// 12345678 12345678 12345678
|
||||
// M L
|
||||
// S S
|
||||
// B B
|
||||
//
|
||||
// Interleaved:
|
||||
// abcabcab cabcabca bcabcabc
|
||||
// 11144477 22255578 63336688
|
||||
//
|
||||
///////////////////////////////
|
||||
|
||||
void mp1Interleave(MP1 *mp1, uint8_t byte) {
|
||||
mp1->interleaveOut[mp1->interleaveCounter] = byte;
|
||||
mp1->interleaveCounter++;
|
||||
if (mp1->interleaveCounter == MP1_INTERLEAVE_SIZE) {
|
||||
// We have the bytes we need for interleaving
|
||||
// in the buffer and are ready to interleave them.
|
||||
#if MP1_INTERLEAVE_SIZE == 3
|
||||
// This is for 3-byte interleaving
|
||||
uint8_t a = (GET_BIT(mp1->interleaveOut[0], 1) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 1) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 1) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 4) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 4) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 4) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 7));
|
||||
mp1WriteByte(mp1, a);
|
||||
|
||||
uint8_t b = (GET_BIT(mp1->interleaveOut[2], 2) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 2) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 2) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 5) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 5) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 5) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 8));
|
||||
mp1WriteByte(mp1, b);
|
||||
|
||||
uint8_t c = (GET_BIT(mp1->interleaveOut[1], 6) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 3) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 3) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 3) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 6) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[0], 6) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 8) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 8));
|
||||
|
||||
mp1WriteByte(mp1, c);
|
||||
#elif MP1_INTERLEAVE_SIZE == 12
|
||||
// This is for 12-byte interleaving
|
||||
uint8_t a = (GET_BIT(mp1->interleaveOut[0], 1) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 1) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[3], 1) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[4], 1) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[6], 1) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[7], 1) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[9], 1) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[10],1));
|
||||
mp1WriteByte(mp1, a);
|
||||
|
||||
uint8_t b = (GET_BIT(mp1->interleaveOut[0], 2) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 2) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[3], 2) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[4], 2) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[6], 2) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[7], 2) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[9], 2) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[10],2));
|
||||
mp1WriteByte(mp1, b);
|
||||
|
||||
uint8_t c = (GET_BIT(mp1->interleaveOut[0], 3) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 3) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[3], 3) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[4], 3) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[6], 3) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[7], 3) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[9], 3) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[10],3));
|
||||
mp1WriteByte(mp1, c);
|
||||
|
||||
uint8_t d = (GET_BIT(mp1->interleaveOut[0], 4) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 4) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[3], 4) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[4], 4) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[6], 4) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[7], 4) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[9], 4) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[10],4));
|
||||
mp1WriteByte(mp1, d);
|
||||
|
||||
uint8_t e = (GET_BIT(mp1->interleaveOut[0], 5) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 5) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[3], 5) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[4], 5) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[6], 5) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[7], 5) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[9], 5) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[10],5));
|
||||
mp1WriteByte(mp1, e);
|
||||
|
||||
uint8_t f = (GET_BIT(mp1->interleaveOut[0], 6) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 6) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[3], 6) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[4], 6) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[6], 6) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[7], 6) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[9], 6) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[10],6));
|
||||
mp1WriteByte(mp1, f);
|
||||
|
||||
uint8_t g = (GET_BIT(mp1->interleaveOut[0], 7) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 7) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[3], 7) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[4], 7) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[6], 7) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[7], 7) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[9], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[10],7));
|
||||
mp1WriteByte(mp1, g);
|
||||
|
||||
uint8_t h = (GET_BIT(mp1->interleaveOut[0], 8) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[1], 8) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[3], 8) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[4], 8) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[6], 8) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[7], 8) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[9], 8) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[10],8));
|
||||
mp1WriteByte(mp1, h);
|
||||
|
||||
uint8_t p = (GET_BIT(mp1->interleaveOut[2], 1) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 5) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[5], 1) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[5], 5) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[8], 1) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[8], 5) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[11],1) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[11],5));
|
||||
mp1WriteByte(mp1, p);
|
||||
|
||||
uint8_t q = (GET_BIT(mp1->interleaveOut[2], 2) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 6) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[5], 2) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[5], 6) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[8], 2) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[8], 6) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[11],2) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[11],6));
|
||||
mp1WriteByte(mp1, q);
|
||||
|
||||
uint8_t s = (GET_BIT(mp1->interleaveOut[2], 3) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 7) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[5], 3) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[5], 7) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[8], 3) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[8], 7) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[11],3) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[11],7));
|
||||
mp1WriteByte(mp1, s);
|
||||
|
||||
uint8_t t = (GET_BIT(mp1->interleaveOut[2], 4) << 7) +
|
||||
(GET_BIT(mp1->interleaveOut[2], 8) << 6) +
|
||||
(GET_BIT(mp1->interleaveOut[5], 4) << 5) +
|
||||
(GET_BIT(mp1->interleaveOut[5], 8) << 4) +
|
||||
(GET_BIT(mp1->interleaveOut[8], 4) << 3) +
|
||||
(GET_BIT(mp1->interleaveOut[8], 8) << 2) +
|
||||
(GET_BIT(mp1->interleaveOut[11],4) << 1) +
|
||||
(GET_BIT(mp1->interleaveOut[11],8));
|
||||
mp1WriteByte(mp1, t);
|
||||
|
||||
#endif
|
||||
|
||||
mp1->interleaveCounter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void mp1Deinterleave(MP1 *mp1) {
|
||||
#if MP1_INTERLEAVE_SIZE == 3
|
||||
uint8_t a = (GET_BIT(mp1->interleaveIn[0], 1) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 2) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 3) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 4) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 5) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 6) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 8));
|
||||
|
||||
uint8_t b = (GET_BIT(mp1->interleaveIn[0], 2) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 3) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 4) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 5) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 6) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 1) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 8) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 7));
|
||||
|
||||
uint8_t c = (GET_BIT(mp1->interleaveIn[0], 3) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 1) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 2) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[0], 6) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 4) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 5) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 8));
|
||||
|
||||
mp1->interleaveIn[0] = a;
|
||||
mp1->interleaveIn[1] = b;
|
||||
mp1->interleaveIn[2] = c;
|
||||
#elif MP1_INTERLEAVE_SIZE == 12
|
||||
uint8_t a = (GET_BIT(mp1->interleaveIn[0], 1) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 1) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 1) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[3], 1) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[4], 1) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[5], 1) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[6], 1) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[7], 1));
|
||||
|
||||
uint8_t b = (GET_BIT(mp1->interleaveIn[0], 2) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 2) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 2) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[3], 2) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[4], 2) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[5], 2) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[6], 2) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[7], 2));
|
||||
|
||||
uint8_t p = (GET_BIT(mp1->interleaveIn[8], 1) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[9], 1) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[10],1) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[11],1) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[8], 2) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[9], 2) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[10],2) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[11],2));
|
||||
|
||||
uint8_t c = (GET_BIT(mp1->interleaveIn[0], 3) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 3) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 3) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[3], 3) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[4], 3) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[5], 3) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[6], 3) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[7], 3));
|
||||
|
||||
uint8_t d = (GET_BIT(mp1->interleaveIn[0], 4) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 4) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 4) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[3], 4) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[4], 4) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[5], 4) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[6], 4) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[7], 4));
|
||||
|
||||
uint8_t q = (GET_BIT(mp1->interleaveIn[8], 3) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[9], 3) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[10],3) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[11],3) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[8], 4) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[9], 4) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[10],4) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[11],4));
|
||||
|
||||
uint8_t e = (GET_BIT(mp1->interleaveIn[0], 5) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 5) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 5) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[3], 5) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[4], 5) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[5], 5) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[6], 5) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[7], 5));
|
||||
|
||||
uint8_t f = (GET_BIT(mp1->interleaveIn[0], 6) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 6) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 6) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[3], 6) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[4], 6) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[5], 6) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[6], 6) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[7], 6));
|
||||
|
||||
uint8_t s = (GET_BIT(mp1->interleaveIn[8], 5) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[9], 5) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[10],5) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[11],5) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[8], 6) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[9], 6) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[10],6) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[11],6));
|
||||
|
||||
uint8_t g = (GET_BIT(mp1->interleaveIn[0], 7) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 7) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 7) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[3], 7) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[4], 7) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[5], 7) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[6], 7) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[7], 7));
|
||||
|
||||
uint8_t h = (GET_BIT(mp1->interleaveIn[0], 8) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[1], 8) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[2], 8) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[3], 8) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[4], 8) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[5], 8) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[6], 8) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[7], 8));
|
||||
|
||||
uint8_t t = (GET_BIT(mp1->interleaveIn[8], 7) << 7) +
|
||||
(GET_BIT(mp1->interleaveIn[9], 7) << 6) +
|
||||
(GET_BIT(mp1->interleaveIn[10],7) << 5) +
|
||||
(GET_BIT(mp1->interleaveIn[11],7) << 4) +
|
||||
(GET_BIT(mp1->interleaveIn[8], 8) << 3) +
|
||||
(GET_BIT(mp1->interleaveIn[9], 8) << 2) +
|
||||
(GET_BIT(mp1->interleaveIn[10],8) << 1) +
|
||||
(GET_BIT(mp1->interleaveIn[11],8));
|
||||
|
||||
mp1->interleaveIn[0] = a;
|
||||
mp1->interleaveIn[1] = b;
|
||||
mp1->interleaveIn[2] = p;
|
||||
mp1->interleaveIn[3] = c;
|
||||
mp1->interleaveIn[4] = d;
|
||||
mp1->interleaveIn[5] = q;
|
||||
mp1->interleaveIn[6] = e;
|
||||
mp1->interleaveIn[7] = f;
|
||||
mp1->interleaveIn[8] = s;
|
||||
mp1->interleaveIn[9] = g;
|
||||
mp1->interleaveIn[10] = h;
|
||||
mp1->interleaveIn[11] = t;
|
||||
|
||||
#endif
|
||||
}
|
||||
@ -5,9 +5,10 @@
|
||||
#include <io/kfile.h>
|
||||
|
||||
// Frame sizing & checksum
|
||||
#define MP1_MIN_FRAME_LENGTH 3
|
||||
#define MP1_INTERLEAVE_SIZE 12
|
||||
#define MP1_MIN_FRAME_LENGTH MP1_INTERLEAVE_SIZE
|
||||
#define MP1_DATA_BLOCK_SIZE ((MP1_INTERLEAVE_SIZE/3)*2)
|
||||
#define MP1_MAX_FRAME_LENGTH 250
|
||||
#define MP1_INTERLEAVE_SIZE 3
|
||||
#define MP1_CHECKSUM_INIT 0xAA
|
||||
|
||||
// We need to know some basic HDLC flag bytes
|
||||
@ -33,7 +34,7 @@ typedef void (*mp1_callback_t)(struct MP1Packet *packet);
|
||||
// Struct for a protocol context
|
||||
typedef struct MP1 {
|
||||
uint8_t buffer[MP1_MAX_FRAME_LENGTH]; // A buffer for incoming packets
|
||||
uint8_t fecBuffer[3]; // FEC buffer
|
||||
uint8_t fecBuffer[3]; // Forward Error Correction buffer
|
||||
KFile *modem; // KFile access to the modem
|
||||
size_t packetLength; // Counter for received packet length
|
||||
size_t readLength; // This is the full read length, including parity bytes
|
||||
|
||||
@ -1,2 +1,2 @@
|
||||
#define VERS_BUILD 1323
|
||||
#define VERS_BUILD 1373
|
||||
#define VERS_HOST "vixen"
|
||||
|
||||
Loading…
Reference in New Issue
Block a user