2014-04-04 03:17:47 -06:00
|
|
|
#include "mp1.h"
|
2014-04-07 14:06:33 -06:00
|
|
|
#include "hardware.h"
|
2014-04-16 06:06:03 -06:00
|
|
|
#include "config.h"
|
2014-04-27 10:54:01 -06:00
|
|
|
#include <stdlib.h> // Used for random
|
2014-04-04 03:17:47 -06:00
|
|
|
#include <string.h>
|
2014-04-04 09:05:53 -06:00
|
|
|
#include <drv/ser.h>
|
2014-04-27 10:54:01 -06:00
|
|
|
#include <drv/timer.h> // Timer driver from BertOS
|
2014-04-04 03:17:47 -06:00
|
|
|
|
2014-04-14 13:03:22 -06:00
|
|
|
#include "compression/heatshrink_encoder.h"
|
|
|
|
#include "compression/heatshrink_decoder.h"
|
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// We need an indicator to tell us whether we
|
|
|
|
// should send a parity byte. This happens
|
|
|
|
// whenever two normal bytes of data has been
|
|
|
|
// sent. We also keep the last sent byte in
|
|
|
|
// memory because we need it to calculate the
|
|
|
|
// parity byte.
|
2014-04-13 09:25:13 -06:00
|
|
|
static bool sendParityBlock = false;
|
2014-04-16 07:10:30 -06:00
|
|
|
static uint8_t lastByte = 0x00;
|
2014-04-13 09:25:13 -06:00
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// We also need a buffer for compressing and
|
|
|
|
// decompressing packet data.
|
2014-04-27 15:06:05 -06:00
|
|
|
#if MP1_ENABLE_COMPRESSION
|
|
|
|
static uint8_t compressionBuffer[MP1_MAX_DATA_SIZE];
|
|
|
|
#endif
|
2014-04-14 13:03:22 -06:00
|
|
|
|
2014-04-27 15:06:05 -06:00
|
|
|
#if SERIAL_DEBUG
|
2014-04-16 09:28:16 -06:00
|
|
|
// An int to hold amount of free RAM updated
|
|
|
|
// by the FREE_RAM function;
|
|
|
|
static int FREE_RAM;
|
2014-04-27 15:06:05 -06:00
|
|
|
#endif
|
2014-04-16 09:28:16 -06:00
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// The GET_BIT macro is used in the interleaver
|
|
|
|
// and deinterleaver to access single bits of a
|
|
|
|
// byte.
|
2014-04-16 06:06:03 -06:00
|
|
|
INLINE bool GET_BIT(uint8_t byte, int n) { return (byte & (1 << (8-n))) == (1 << (8-n)); }
|
2014-04-16 07:10:30 -06:00
|
|
|
|
|
|
|
// This function calculates and returns a parity
|
|
|
|
// byte for two input bytes. The parity byte is
|
|
|
|
// used for correcting errors in the transmission.
|
|
|
|
// The error correction algorithm is a standard
|
|
|
|
// (12,8) Hamming code.
|
2014-04-13 17:23:48 -06:00
|
|
|
INLINE bool BIT(uint8_t byte, int n) { return ((byte & BV(n-1))>>(n-1)); }
|
2014-04-13 09:25:13 -06:00
|
|
|
static uint8_t mp1ParityBlock(uint8_t first, uint8_t other) {
|
|
|
|
uint8_t parity = 0x00;
|
|
|
|
|
2014-04-13 17:23:48 -06:00
|
|
|
parity = ((BIT(first, 1) ^ BIT(first, 2) ^ BIT(first, 4) ^ BIT(first, 5) ^ BIT(first, 7))) +
|
|
|
|
((BIT(first, 1) ^ BIT(first, 3) ^ BIT(first, 4) ^ BIT(first, 6) ^ BIT(first, 7))<<1) +
|
|
|
|
((BIT(first, 2) ^ BIT(first, 3) ^ BIT(first, 4) ^ BIT(first, 8))<<2) +
|
|
|
|
((BIT(first, 5) ^ BIT(first, 6) ^ BIT(first, 7) ^ BIT(first, 8))<<3) +
|
|
|
|
|
|
|
|
((BIT(other, 1) ^ BIT(other, 2) ^ BIT(other, 4) ^ BIT(other, 5) ^ BIT(other, 7))<<4) +
|
|
|
|
((BIT(other, 1) ^ BIT(other, 3) ^ BIT(other, 4) ^ BIT(other, 6) ^ BIT(other, 7))<<5) +
|
|
|
|
((BIT(other, 2) ^ BIT(other, 3) ^ BIT(other, 4) ^ BIT(other, 8))<<6) +
|
|
|
|
((BIT(other, 5) ^ BIT(other, 6) ^ BIT(other, 7) ^ BIT(other, 8))<<7);
|
2014-04-13 09:25:13 -06:00
|
|
|
|
|
|
|
return parity;
|
|
|
|
}
|
|
|
|
|
2014-04-27 10:54:01 -06:00
|
|
|
// This decode function retrieves the buffer of
|
2014-04-16 07:10:30 -06:00
|
|
|
// received, deinterleaved and error-corrected
|
|
|
|
// bytes, inspects the header and determines
|
|
|
|
// whether there is padding to be removed, and
|
|
|
|
// whether the packet is compressed. If it is
|
|
|
|
// it is decompressed before being passed to
|
|
|
|
// the registered callback.
|
2014-04-04 03:17:47 -06:00
|
|
|
static void mp1Decode(MP1 *mp1) {
|
|
|
|
MP1Packet packet; // A decoded packet struct
|
|
|
|
uint8_t *buffer = mp1->buffer; // Get the buffer from the protocol context
|
2014-04-13 17:23:48 -06:00
|
|
|
|
|
|
|
// Get the header and "remove" it from the buffer
|
|
|
|
uint8_t header = buffer[0];
|
|
|
|
buffer++;
|
|
|
|
|
|
|
|
// If header indicates a padded packet, remove
|
|
|
|
// padding
|
2014-04-17 06:25:16 -06:00
|
|
|
uint8_t padding = header >> 4;
|
2014-04-14 13:03:22 -06:00
|
|
|
if (header & MP1_HEADER_PADDED) {
|
2014-04-17 06:25:16 -06:00
|
|
|
for (int i = 0; i < padding; i++) {
|
|
|
|
buffer++;
|
|
|
|
}
|
2014-04-13 17:23:48 -06:00
|
|
|
}
|
2014-04-04 03:17:47 -06:00
|
|
|
|
2014-04-16 09:28:16 -06:00
|
|
|
if (SERIAL_DEBUG) kprintf("[TS=%d] ", mp1->packetLength);
|
|
|
|
|
2014-04-06 09:17:13 -06:00
|
|
|
// Set the payload length of the packet to the counted
|
|
|
|
// length minus 1, so we remove the checksum
|
2014-04-27 10:54:01 -06:00
|
|
|
packet.dataLength = mp1->packetLength - 2 - (header & MP1_HEADER_PADDED)*padding;
|
2014-04-14 13:03:22 -06:00
|
|
|
|
|
|
|
// Check if we have received a compressed packet
|
2014-04-27 15:06:05 -06:00
|
|
|
if (MP1_ENABLE_COMPRESSION && (header & MP1_HEADER_COMPRESSION)) {
|
2014-04-16 07:10:30 -06:00
|
|
|
// If we have, we decompress it and use the
|
|
|
|
// decompressed data for the packet
|
2014-04-27 15:06:05 -06:00
|
|
|
#if MP1_ENABLE_COMPRESSION
|
|
|
|
if (SERIAL_DEBUG) kprintf("[CS=%d] ", packet.dataLength);
|
|
|
|
size_t decompressedSize = decompress(buffer, packet.dataLength);
|
|
|
|
if (SERIAL_DEBUG) kprintf("[DS=%d]", decompressedSize);
|
|
|
|
packet.dataLength = decompressedSize;
|
|
|
|
memcpy(mp1->buffer, compressionBuffer, decompressedSize);
|
|
|
|
#endif
|
2014-04-27 10:54:01 -06:00
|
|
|
} else {
|
|
|
|
// If the packet was not compressed, we shift
|
|
|
|
// the data in our buffer back down to the actual
|
|
|
|
// beginning of the buffer array, since we incremented
|
|
|
|
// the pointer address for removing the header and
|
|
|
|
// padding.
|
|
|
|
for (unsigned long i = 0; i < packet.dataLength; i++) {
|
|
|
|
mp1->buffer[i] = buffer[i];
|
|
|
|
}
|
2014-04-14 13:03:22 -06:00
|
|
|
}
|
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// Set the data field of the packet to our buffer
|
2014-04-27 10:54:01 -06:00
|
|
|
packet.data = mp1->buffer;
|
2014-04-04 03:17:47 -06:00
|
|
|
|
2014-04-06 09:17:13 -06:00
|
|
|
// If a callback have been specified, let's
|
|
|
|
// call it and pass the decoded packet
|
2014-04-04 03:17:47 -06:00
|
|
|
if (mp1->callback) mp1->callback(&packet);
|
|
|
|
}
|
|
|
|
|
2014-04-16 06:06:03 -06:00
|
|
|
|
2014-04-06 09:17:13 -06:00
|
|
|
////////////////////////////////////////////////////////////
|
|
|
|
// The Poll function reads data from the modem, handles //
|
|
|
|
// frame recognition and passes data on to higher layers //
|
|
|
|
// if valid packets are found //
|
|
|
|
////////////////////////////////////////////////////////////
|
2014-04-04 03:17:47 -06:00
|
|
|
void mp1Poll(MP1 *mp1) {
|
2014-04-06 15:39:28 -06:00
|
|
|
int byte; // A place to store our read byte
|
2014-04-04 03:17:47 -06:00
|
|
|
|
|
|
|
// Read bytes from the modem until we reach EOF
|
|
|
|
while ((byte = kfile_getc(mp1->modem)) != EOF) {
|
2014-04-27 10:54:01 -06:00
|
|
|
// We read something from the modem, so we
|
|
|
|
// set the settleTimer
|
|
|
|
mp1->settleTimer = timer_clock();
|
2014-04-04 03:17:47 -06:00
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
/////////////////////////////////////////////
|
|
|
|
// This following block handles forward //
|
|
|
|
// error correction using an interleaved //
|
|
|
|
// (12,8) Hamming code //
|
|
|
|
/////////////////////////////////////////////
|
|
|
|
|
|
|
|
// If we have started reading (received an
|
|
|
|
// HDLC_FLAG), we will start looking at the
|
|
|
|
// incoming data and perform forward error
|
|
|
|
// correction on it.
|
2014-04-17 06:25:16 -06:00
|
|
|
|
|
|
|
|
|
|
|
if ((mp1->reading && (byte != AX25_ESC )) || (mp1->reading && (mp1->escape && (byte == AX25_ESC || byte == HDLC_FLAG || byte == HDLC_RESET)))) {
|
2014-04-27 10:54:01 -06:00
|
|
|
// We have a byte, increment our read counter
|
2014-04-13 17:23:48 -06:00
|
|
|
mp1->readLength++;
|
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// 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
|
2014-04-17 06:25:16 -06:00
|
|
|
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;
|
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// The block is interleaved, so we will
|
|
|
|
// first put the received bytes in the
|
|
|
|
// deinterleaving buffer
|
2014-04-17 06:25:16 -06:00
|
|
|
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;
|
2014-04-16 06:06:03 -06:00
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// We then deinterleave the block
|
2014-04-16 06:06:03 -06:00
|
|
|
mp1Deinterleave(mp1);
|
2014-04-16 07:10:30 -06:00
|
|
|
|
2014-04-17 06:25:16 -06:00
|
|
|
// 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;
|
|
|
|
|
|
|
|
// 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.
|
|
|
|
|
|
|
|
// Deinterleaved data bytes
|
|
|
|
uint8_t a = mp1->interleaveIn[i];
|
|
|
|
uint8_t b = mp1->interleaveIn[i+1];
|
|
|
|
|
|
|
|
// 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;
|
2014-04-13 17:23:48 -06:00
|
|
|
}
|
|
|
|
}
|
2014-04-16 06:06:03 -06:00
|
|
|
|
2014-04-17 06:25:16 -06:00
|
|
|
// 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;
|
|
|
|
}
|
2014-04-16 06:06:03 -06:00
|
|
|
|
2014-04-13 09:25:13 -06:00
|
|
|
continue;
|
|
|
|
}
|
2014-04-13 17:23:48 -06:00
|
|
|
}
|
2014-04-16 07:10:30 -06:00
|
|
|
/////////////////////////////////////////////
|
|
|
|
// End of forward error correction block //
|
|
|
|
/////////////////////////////////////////////
|
2014-04-13 17:23:48 -06:00
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// This next part of the poll function handles
|
|
|
|
// the reading from the modem, and looks for
|
|
|
|
// starts and ends of transmissions. It also
|
|
|
|
// handles escape characters by discarding them
|
|
|
|
// so they don't get put into the output data.
|
|
|
|
|
|
|
|
// Let's first check if we have read an HDLC_FLAG.
|
2014-04-13 17:23:48 -06:00
|
|
|
if (!mp1->escape && byte == HDLC_FLAG) {
|
|
|
|
// We are not in an escape sequence and we
|
|
|
|
// found a HDLC_FLAG. This can mean two things:
|
2014-04-17 06:25:16 -06:00
|
|
|
if (mp1->readLength >= MP1_MIN_FRAME_LENGTH) {
|
2014-04-13 17:23:48 -06:00
|
|
|
// We already have more data than the minimum
|
|
|
|
// frame length, which means the flag signifies
|
|
|
|
// the end of the packet. Pass control to the
|
|
|
|
// decoder.
|
2014-04-27 10:54:01 -06:00
|
|
|
//
|
|
|
|
// We also set the settle timer to indicate
|
|
|
|
// the time the frame completed reading.
|
|
|
|
mp1->settleTimer = timer_clock();
|
2014-04-13 17:23:48 -06:00
|
|
|
if ((mp1->checksum_in & 0xff) == 0x00) {
|
2014-04-16 06:06:03 -06:00
|
|
|
if (SERIAL_DEBUG) kprintf("[CHK-OK] [C=%d] ", mp1->correctionsMade);
|
2014-04-13 17:23:48 -06:00
|
|
|
mp1Decode(mp1);
|
2014-04-13 09:25:13 -06:00
|
|
|
} else {
|
2014-04-13 17:23:48 -06:00
|
|
|
// Checksum was incorrect, we don't do anything,
|
|
|
|
// but you can enable the decode anyway, if you
|
|
|
|
// need it for testing or debugging
|
2014-04-16 06:06:03 -06:00
|
|
|
if (PASSALL) {
|
|
|
|
if (SERIAL_DEBUG) kprintf("[CHK-ER] [C=%d] ", mp1->correctionsMade);
|
|
|
|
mp1Decode(mp1);
|
|
|
|
}
|
2014-04-13 09:25:13 -06:00
|
|
|
}
|
2014-04-04 03:17:47 -06:00
|
|
|
}
|
2014-04-13 17:23:48 -06:00
|
|
|
// If the above is not the case, this must be the
|
|
|
|
// beginning of a frame
|
|
|
|
mp1->reading = true;
|
|
|
|
mp1->packetLength = 0;
|
|
|
|
mp1->readLength = 0;
|
|
|
|
mp1->checksum_in = MP1_CHECKSUM_INIT;
|
2014-04-15 12:18:22 -06:00
|
|
|
mp1->correctionsMade = 0;
|
2014-04-13 17:23:48 -06:00
|
|
|
|
|
|
|
// We have indicated that we are reading,
|
|
|
|
// and reset the length counter. Now we'll
|
|
|
|
// continue to the next byte.
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!mp1->escape && byte == HDLC_RESET) {
|
|
|
|
// Not good, we got a reset. The transmitting
|
|
|
|
// party may have encountered an error. We'll
|
|
|
|
// stop receiving this packet immediately.
|
|
|
|
mp1->reading = false;
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!mp1->escape && byte == AX25_ESC) {
|
|
|
|
// We found an escape character. We'll set
|
|
|
|
// the escape seqeunce indicator so we don't
|
|
|
|
// interpret the next byte as a reset or flag
|
|
|
|
mp1->escape = true;
|
2014-04-17 06:25:16 -06:00
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// We then continue reading the next byte.
|
2014-04-13 17:23:48 -06:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Now let's get to the actual reading of the data
|
|
|
|
if (mp1->reading) {
|
2014-04-28 09:47:32 -06:00
|
|
|
if (mp1->packetLength < MP1_MAX_FRAME_LENGTH + MP1_INTERLEAVE_SIZE) {
|
2014-04-13 17:23:48 -06:00
|
|
|
// If the length of the current incoming frame is
|
|
|
|
// still less than our max length, put the incoming
|
2014-04-16 07:10:30 -06:00
|
|
|
// byte in the buffer. When we have collected 3
|
|
|
|
// bytes, they will be processed by the error
|
|
|
|
// correction part above.
|
2014-04-13 17:23:48 -06:00
|
|
|
mp1->buffer[mp1->packetLength++] = byte;
|
|
|
|
} else {
|
|
|
|
// If not, we have a problem: The buffer has overrun
|
|
|
|
// We need to stop receiving, and the packet will be
|
|
|
|
// dropped :(
|
|
|
|
mp1->reading = false;
|
|
|
|
}
|
2014-04-04 03:17:47 -06:00
|
|
|
}
|
2014-04-13 17:23:48 -06:00
|
|
|
|
|
|
|
// We need to set the escape sequence indicator back
|
|
|
|
// to false after each byte.
|
|
|
|
mp1->escape = false;
|
2014-04-04 03:17:47 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
if (kfile_error(mp1->modem)) {
|
|
|
|
// If there was an error from the modem, we'll be rude
|
|
|
|
// and just reset it. No error handling is done for now.
|
|
|
|
kfile_clearerr(mp1->modem);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-04-16 06:06:03 -06:00
|
|
|
// This is called to actually send the bytes
|
|
|
|
// after they have been interleaved
|
|
|
|
static void mp1WriteByte(MP1 *mp1, uint8_t byte) {
|
2014-04-04 03:40:43 -06:00
|
|
|
// If we are sending something that looks
|
|
|
|
// like an HDLC special byte, send an escape
|
|
|
|
// character first
|
|
|
|
if (byte == HDLC_FLAG ||
|
|
|
|
byte == HDLC_RESET ||
|
|
|
|
byte == AX25_ESC) {
|
|
|
|
kfile_putc(AX25_ESC, mp1->modem);
|
|
|
|
}
|
|
|
|
kfile_putc(byte, mp1->modem);
|
2014-04-16 06:06:03 -06:00
|
|
|
}
|
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// This is an intermediary function that
|
|
|
|
// receives outgoing bytes, and adds
|
|
|
|
// interleaving and a parity byte to the
|
|
|
|
// outgoing data in blocks of two data
|
|
|
|
// bytes. The actual transmitted block will
|
|
|
|
// be 3 bytes long due to the added parity
|
|
|
|
// byte.
|
|
|
|
static void mp1Putbyte(MP1 *mp1, uint8_t byte) {
|
|
|
|
mp1Interleave(mp1, byte);
|
|
|
|
|
|
|
|
if (sendParityBlock) {
|
|
|
|
uint8_t p = mp1ParityBlock(lastByte, byte);
|
|
|
|
mp1Interleave(mp1, p);
|
|
|
|
}
|
|
|
|
|
|
|
|
lastByte = byte;
|
|
|
|
sendParityBlock ^= true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// This function accepts a buffer with data
|
|
|
|
// to be transmitted, and structures it into
|
|
|
|
// a valid packet.
|
2014-04-16 09:28:16 -06:00
|
|
|
void mp1Send(MP1 *mp1, void *_buffer, size_t length) {
|
2014-04-28 09:47:32 -06:00
|
|
|
// Reset our parity tx indicator
|
|
|
|
sendParityBlock = false;
|
|
|
|
|
2014-04-27 10:54:01 -06:00
|
|
|
// Open transmitter and wait for MP1_TXDELAY msecs
|
|
|
|
AFSK_HW_PTT_ON();
|
|
|
|
ticks_t start = timer_clock();
|
2014-04-28 09:53:43 -06:00
|
|
|
#if MP1_USE_TX_QUEUE
|
2014-04-28 09:47:32 -06:00
|
|
|
if (!mp1->queueProcessing) {
|
|
|
|
while (timer_clock() - start < ms_to_ticks(MP1_TXDELAY)) {
|
|
|
|
cpu_relax();
|
|
|
|
}
|
2014-04-27 10:54:01 -06:00
|
|
|
}
|
2014-04-28 09:53:43 -06:00
|
|
|
#else
|
|
|
|
while (timer_clock() - start < ms_to_ticks(MP1_TXDELAY)) {
|
|
|
|
cpu_relax();
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2014-04-27 10:54:01 -06:00
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// Get the transmit data buffer
|
2014-04-16 09:28:16 -06:00
|
|
|
uint8_t *buffer = (uint8_t *)_buffer;
|
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// Initialize checksum to zero
|
|
|
|
mp1->checksum_out = MP1_CHECKSUM_INIT;
|
|
|
|
|
|
|
|
// We also reset the interleave counter to zero
|
|
|
|
mp1->interleaveCounter = 0;
|
|
|
|
|
|
|
|
// We start out assuming we should not use
|
|
|
|
// compression.
|
|
|
|
bool packetCompression = false;
|
|
|
|
|
|
|
|
// We then try to compress the data to see
|
|
|
|
// if we can save some space with compression.
|
2014-04-27 15:06:05 -06:00
|
|
|
#if MP1_ENABLE_COMPRESSION
|
|
|
|
size_t compressedSize = compress(buffer, length);
|
|
|
|
if (compressedSize != 0 && compressedSize < length) {
|
|
|
|
// Compression saved us some space, we'll
|
|
|
|
// send the paket compressed
|
|
|
|
packetCompression = true;
|
|
|
|
// Write the compressed data into the
|
|
|
|
// outgoing data buffer
|
|
|
|
memcpy(buffer, compressionBuffer, compressedSize);
|
|
|
|
|
|
|
|
// Make sure to set the length of the
|
|
|
|
// data to the new (compressed) length
|
|
|
|
length = compressedSize;
|
|
|
|
} else {
|
|
|
|
// We are not going to use compression,
|
|
|
|
// so we don't do anything.
|
|
|
|
}
|
|
|
|
#endif
|
2014-04-16 07:10:30 -06:00
|
|
|
|
2014-04-16 09:28:16 -06:00
|
|
|
// Transmit the HDLC_FLAG to signify start of TX
|
|
|
|
kfile_putc(HDLC_FLAG, mp1->modem);
|
2014-04-16 07:10:30 -06:00
|
|
|
|
|
|
|
// We now need to construct a header, that
|
|
|
|
// can tell the receiving end whether the
|
|
|
|
// packet is compressed. Since a packet must
|
|
|
|
// have an even number of total payload bytes
|
|
|
|
// (including the header), we check the length
|
|
|
|
// of the outgoing data, and if it is not even,
|
|
|
|
// we add a single byte of padding to the
|
|
|
|
// packet. Remember that we also send a single
|
|
|
|
// byte checksum at the end of the packet, so
|
|
|
|
// the header and checksum bytes together don't
|
|
|
|
// change whether the payload length is even
|
|
|
|
// or not. The payload length needs to be even
|
|
|
|
// since we are sending a parity byte for every
|
|
|
|
// two data bytes sent, and because interleaving
|
|
|
|
// happens in blocks of three bytes.
|
2014-04-17 06:25:16 -06:00
|
|
|
uint8_t header = 0x00;
|
2014-04-16 07:10:30 -06:00
|
|
|
|
|
|
|
// If we are using compression, set the
|
|
|
|
// appropriate header flag to true.
|
|
|
|
if (packetCompression) header ^= MP1_HEADER_COMPRESSION;
|
|
|
|
|
2014-04-17 06:25:16 -06:00
|
|
|
// 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
|
2014-04-16 07:10:30 -06:00
|
|
|
// header flag to indicate that we are
|
2014-04-17 06:25:16 -06:00
|
|
|
// padding this packet.
|
2014-04-16 07:10:30 -06:00
|
|
|
header ^= MP1_HEADER_PADDED;
|
|
|
|
|
2014-04-17 06:25:16 -06:00
|
|
|
// 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);
|
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// 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
|
2014-04-17 06:25:16 -06:00
|
|
|
// 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);
|
|
|
|
}
|
2014-04-16 07:10:30 -06:00
|
|
|
} else {
|
2014-04-17 06:25:16 -06:00
|
|
|
// If the length already matches, we
|
2014-04-16 07:10:30 -06:00
|
|
|
// just update the checksum with the
|
|
|
|
// header byte and queue it.
|
|
|
|
mp1->checksum_out = mp1->checksum_out ^ header;
|
|
|
|
mp1Putbyte(mp1, header);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Now we'll transmit the actual data of
|
|
|
|
// the packet. We continously increment the
|
|
|
|
// pointer address of the buffer while
|
|
|
|
// passing it to the intermediary output
|
|
|
|
// function. Everytime the interleaving
|
|
|
|
// counter reaches 3, a block will be
|
|
|
|
// transmitted.
|
|
|
|
while (length--) {
|
2014-04-16 09:28:16 -06:00
|
|
|
mp1->checksum_out = mp1->checksum_out ^ *buffer;
|
|
|
|
mp1Putbyte(mp1, *buffer++);
|
2014-04-16 07:10:30 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
// Finally we write the checksum to the
|
|
|
|
// end of the packet.
|
|
|
|
mp1Putbyte(mp1, mp1->checksum_out);
|
|
|
|
|
|
|
|
// And transmit a HDLC_FLAG to signify
|
|
|
|
// end of the transmission.
|
|
|
|
kfile_putc(HDLC_FLAG, mp1->modem);
|
2014-04-27 10:54:01 -06:00
|
|
|
|
|
|
|
// Turn off manual PTT
|
2014-04-28 09:53:43 -06:00
|
|
|
#if MP1_USE_TX_QUEUE
|
|
|
|
if (!mp1->queueProcessing) AFSK_HW_PTT_OFF();
|
|
|
|
#else
|
|
|
|
AFSK_HW_PTT_OFF();
|
|
|
|
#endif
|
2014-04-16 07:10:30 -06:00
|
|
|
}
|
|
|
|
|
2014-04-28 09:47:32 -06:00
|
|
|
// This function accepts a frame and stores
|
|
|
|
// it in the transmission queue
|
2014-04-28 09:53:43 -06:00
|
|
|
#if MP1_USE_TX_QUEUE
|
|
|
|
void mp1QueueFrame(MP1 *mp1, void *_buffer, size_t length) {
|
|
|
|
if (mp1->queueLength < MP1_TX_QUEUE_LENGTH) {
|
|
|
|
uint8_t *buffer = (uint8_t *)_buffer;
|
|
|
|
mp1->frameLengths[mp1->queueLength] = length;
|
|
|
|
memcpy(mp1->frameQueue[mp1->queueLength++], buffer, length);
|
|
|
|
}
|
2014-04-28 09:47:32 -06:00
|
|
|
}
|
2014-04-28 09:53:43 -06:00
|
|
|
#endif
|
2014-04-28 09:47:32 -06:00
|
|
|
|
|
|
|
// This function processes the transmission
|
|
|
|
// queue.
|
2014-04-28 09:53:43 -06:00
|
|
|
#if MP1_USE_TX_QUEUE
|
|
|
|
void mp1ProcessQueue(MP1 *mp1) {
|
|
|
|
int i = 0;
|
|
|
|
while (mp1->queueLength) {
|
|
|
|
mp1Send(mp1, mp1->frameQueue[i], mp1->frameLengths[i]);
|
|
|
|
i++;
|
|
|
|
mp1->queueLength--;
|
|
|
|
}
|
|
|
|
AFSK_HW_PTT_OFF();
|
2014-04-28 09:47:32 -06:00
|
|
|
}
|
2014-04-28 09:53:43 -06:00
|
|
|
#endif
|
2014-04-27 10:54:01 -06:00
|
|
|
|
|
|
|
// A simple form of P-persistent CSMA.
|
|
|
|
// Everytime we have heard activity
|
|
|
|
// on the channel, we wait at least
|
|
|
|
// MP1_SETTLE_TIME milliseconds after the
|
|
|
|
// activity has ceased. We then pick a random
|
|
|
|
// number, and if it is less than
|
|
|
|
// MP1_P_PERSISTENCE, we transmit.
|
|
|
|
bool mp1CarrierSense(MP1 *mp1) {
|
2014-04-27 13:13:37 -06:00
|
|
|
if (MP1_ENABLE_CSMA) {
|
|
|
|
if (mp1->randomSeed == 0) {
|
|
|
|
mp1->randomSeed = timer_clock();
|
|
|
|
srand(mp1->randomSeed);
|
|
|
|
}
|
2014-04-27 10:54:01 -06:00
|
|
|
|
2014-04-27 13:13:37 -06:00
|
|
|
if (timer_clock() - mp1->settleTimer > ms_to_ticks(MP1_SETTLE_TIME)) {
|
|
|
|
uint8_t r = rand() % 255;
|
|
|
|
if (r < MP1_P_PERSISTENCE) {
|
|
|
|
return false;
|
|
|
|
} else {
|
2014-04-28 09:47:32 -06:00
|
|
|
mp1->settleTimer = timer_clock() - MP1_SETTLE_TIME + MP1_SLOT_TIME;
|
2014-04-27 13:13:37 -06:00
|
|
|
return true;
|
|
|
|
}
|
2014-04-27 10:54:01 -06:00
|
|
|
} else {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
} else {
|
2014-04-27 13:13:37 -06:00
|
|
|
return false;
|
2014-04-27 10:54:01 -06:00
|
|
|
}
|
2014-04-16 07:10:30 -06:00
|
|
|
}
|
|
|
|
|
2014-04-28 09:47:32 -06:00
|
|
|
// This function will simply initialize
|
|
|
|
// the protocol context and allocate the
|
|
|
|
// needed memory.
|
|
|
|
void mp1Init(MP1 *mp1, KFile *modem, mp1_callback_t callback) {
|
|
|
|
// Allocate memory for our protocol "object"
|
|
|
|
memset(mp1, 0, sizeof(*mp1));
|
|
|
|
// Set references to our modem "object" and
|
|
|
|
// a callback for when a packet has been decoded
|
|
|
|
mp1->modem = modem;
|
|
|
|
mp1->callback = callback;
|
|
|
|
mp1->settleTimer = timer_clock();
|
|
|
|
mp1->randomSeed = 0;
|
|
|
|
#if MP1_USE_TX_QUEUE
|
|
|
|
mp1->queueLength = 0;
|
|
|
|
mp1->queueProcessing = false;
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// A handy debug function that can determine
|
|
|
|
// how much available memory we have left.
|
2014-04-27 15:06:05 -06:00
|
|
|
#if SERIAL_DEBUG
|
2014-04-16 07:10:30 -06:00
|
|
|
int freeRam(void) {
|
|
|
|
extern int __heap_start, *__brkval;
|
2014-04-16 09:28:16 -06:00
|
|
|
int v;
|
|
|
|
FREE_RAM = (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
|
|
|
|
return FREE_RAM;
|
2014-04-16 07:10:30 -06:00
|
|
|
}
|
2014-04-27 15:06:05 -06:00
|
|
|
#endif
|
2014-04-16 07:10:30 -06:00
|
|
|
|
|
|
|
// This function compresses data using
|
|
|
|
// the Heatshrink library
|
2014-04-27 15:06:05 -06:00
|
|
|
#if MP1_ENABLE_COMPRESSION
|
2014-04-14 13:03:22 -06:00
|
|
|
size_t compress(uint8_t *input, size_t length) {
|
|
|
|
heatshrink_encoder *hse = heatshrink_encoder_alloc(8, 4);
|
|
|
|
if (hse == NULL) {
|
2014-04-16 09:28:16 -06:00
|
|
|
if (SERIAL_DEBUG) kprintf("Could not allocate compressor\n");
|
2014-04-14 13:03:22 -06:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
size_t written = 0;
|
|
|
|
size_t sunk = 0;
|
|
|
|
heatshrink_encoder_sink(hse, input, length, &sunk);
|
|
|
|
int status = heatshrink_encoder_finish(hse);
|
|
|
|
|
|
|
|
if (sunk < length) {
|
|
|
|
heatshrink_encoder_free(hse);
|
|
|
|
return 0;
|
|
|
|
} else {
|
|
|
|
if (status == HSER_FINISH_MORE) {
|
|
|
|
heatshrink_encoder_poll(hse, compressionBuffer, MP1_MAX_FRAME_LENGTH, &written);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
heatshrink_encoder_free(hse);
|
|
|
|
return written;
|
|
|
|
}
|
2014-04-27 15:06:05 -06:00
|
|
|
#endif
|
2014-04-14 13:03:22 -06:00
|
|
|
|
2014-04-16 07:10:30 -06:00
|
|
|
// This function decompresses data using
|
|
|
|
// the Heatshrink library
|
2014-04-27 15:06:05 -06:00
|
|
|
#if MP1_ENABLE_COMPRESSION
|
2014-04-14 13:03:22 -06:00
|
|
|
size_t decompress(uint8_t *input, size_t length) {
|
|
|
|
heatshrink_decoder *hsd = heatshrink_decoder_alloc(MP1_MAX_FRAME_LENGTH, 8, 4);
|
|
|
|
if (hsd == NULL) {
|
2014-04-16 09:28:16 -06:00
|
|
|
if (SERIAL_DEBUG) kprintf("Could not allocate decompressor\n");
|
2014-04-14 13:03:22 -06:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
size_t written = 0;
|
|
|
|
size_t sunk = 0;
|
|
|
|
heatshrink_decoder_sink(hsd, input, length, &sunk);
|
|
|
|
int status = heatshrink_decoder_finish(hsd);
|
|
|
|
|
|
|
|
if (sunk < length) {
|
|
|
|
heatshrink_decoder_free(hsd);
|
|
|
|
return 0;
|
|
|
|
} else {
|
|
|
|
if (status == HSER_FINISH_MORE) {
|
|
|
|
heatshrink_decoder_poll(hsd, compressionBuffer, MP1_MAX_FRAME_LENGTH, &written);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
heatshrink_decoder_free(hsd);
|
|
|
|
return written;
|
|
|
|
}
|
2014-04-27 15:06:05 -06:00
|
|
|
#endif
|
2014-04-17 06:25:16 -06:00
|
|
|
|
|
|
|
|
|
|
|
// 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
|
|
|
|
}
|