diff --git a/Modem/protocol/mp1.c b/Modem/protocol/mp1.c index 612ba05..72f501d 100644 --- a/Modem/protocol/mp1.c +++ b/Modem/protocol/mp1.c @@ -3,21 +3,24 @@ #include #include +// FIXME: Describe these static uint8_t lastByte = 0x00; static bool sendParityBlock = false; -INLINE bool BIT(uint8_t byte, int n) { return (byte & BV(n)); } +// FIXME: Describe this +INLINE bool BIT(uint8_t byte, int n) { return ((byte & BV(n-1))>>(n-1)); } static uint8_t mp1ParityBlock(uint8_t first, uint8_t other) { uint8_t parity = 0x00; - parity ^= (BIT(first, 1) ^ BIT(first, 2) ^ BIT(first, 4) ^ BIT(first, 5) ^ BIT(first, 7)) << 7; - parity ^= (BIT(first, 1) ^ BIT(first, 3) ^ BIT(first, 4) ^ BIT(first, 6) ^ BIT(first, 7)) << 6; - parity ^= (BIT(first, 2) ^ BIT(first, 3) ^ BIT(first, 4) ^ BIT(first, 8)) << 5; - parity ^= (BIT(first, 5) ^ BIT(first, 6) ^ BIT(first, 7) ^ BIT(first, 8)) << 4; - parity ^= BIT(other, 1) ^ BIT(other, 2) ^ BIT(other, 4) ^ BIT(other, 5) ^ BIT(other, 7) << 3; - parity ^= BIT(other, 1) ^ BIT(other, 3) ^ BIT(other, 4) ^ BIT(other, 6) ^ BIT(other, 7) << 2; - parity ^= BIT(other, 2) ^ BIT(other, 3) ^ BIT(other, 4) ^ BIT(other, 8) << 1; - parity ^= BIT(other, 5) ^ BIT(other, 6) ^ BIT(other, 7) ^ BIT(other, 8); + 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); return parity; } @@ -29,10 +32,20 @@ static void mp1Decode(MP1 *mp1) { // for further processing. MP1Packet packet; // A decoded packet struct uint8_t *buffer = mp1->buffer; // Get the buffer from the protocol context + + // Get the header and "remove" it from the buffer + uint8_t header = buffer[0]; + buffer++; + + // If header indicates a padded packet, remove + // padding + if (header & 0x01) { + buffer++; + } // Set the payload length of the packet to the counted // length minus 1, so we remove the checksum - packet.dataLength = mp1->packetLength - 1; + packet.dataLength = mp1->packetLength - 2 - (header & 0x01); packet.data = buffer; // If a callback have been specified, let's @@ -47,93 +60,123 @@ static void mp1Decode(MP1 *mp1) { //////////////////////////////////////////////////////////// void mp1Poll(MP1 *mp1) { int byte; // A place to store our read byte + sendParityBlock = false; // Reset our parity tx indicator // Read bytes from the modem until we reach EOF while ((byte = kfile_getc(mp1->modem)) != EOF) { // We have a byte, increment our read counter - mp1->readLength++; - if (mp1->readLength % 3 != 0) { - // This is not a parity byte - 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) { - // 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. - if ((mp1->checksum_in & 0xff) == 0x00) { - mp1Decode(mp1); - } else { - // Checksum was incorrect, we don't do anything, - // but you can enable the decode anyway, if you - // need it for testing or debugging - // mp1Decode(mp1); + // FIXME: Describe error correction + if (mp1->reading && (byte != AX25_ESC) ) { + mp1->readLength++; + + + if (mp1->readLength % 3 == 0) { + mp1->calculatedParity = mp1ParityBlock(mp1->buffer[mp1->packetLength-2], mp1->buffer[mp1->packetLength-1]); + uint8_t syndrome = mp1->calculatedParity ^ byte; + if (syndrome == 0x00) { + // No problems! + } else { + uint8_t syndromes[2]; + syndromes[0] = syndrome & 0x0f; + syndromes[1] = (syndrome & 0xf0) >> 4; + + 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) { + // Error in parity bit, no correction needed + continue; + } + 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; + + if (correction != 0x00) { + mp1->checksum_in ^= correction; + } + mp1->buffer[mp1->packetLength-(2-i)] ^= correction; } } - // 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; - - // 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; - } - - // This should be a parity byte - if (mp1->readLength % 3 == 0) { - uint8_t calculatedParity = mp1ParityBlock(mp1->buffer[mp1->packetLength-2], mp1->buffer[mp1->packetLength-1]); - if (byte == calculatedParity) { - // Parity match, block is correct - } else { - // Parity differ, transmission error ocurred - kprintf("Parity mismatch"); - } - mp1->readLength = 0; - } - - 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; - continue; - } - - // Now let's get to the actual reading of the data - if (mp1->reading) { - if (mp1->packetLength < MP1_MAX_FRAME_LENGTH) { - // If the length of the current incoming frame is - // still less than our max length, put the incoming - // byte in the buffer. - if (!mp1->escape) mp1->checksum_in = mp1->checksum_in ^ byte; - 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; - } - } - // We need to set the escape sequence indicator back - // to false after each byte. - mp1->escape = false; - } else { - } + // FIXME: Describe error correction ////////// + + 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) { + // 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. + if ((mp1->checksum_in & 0xff) == 0x00) { + kprintf("[OK] "); + mp1Decode(mp1); + } else { + // Checksum was incorrect, we don't do anything, + // but you can enable the decode anyway, if you + // need it for testing or debugging + kprintf("[ER] [%d] ", mp1->checksum_in); + mp1Decode(mp1); + } + } + // 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; + + // 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; + } + + // This should be a parity byte + + 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; + continue; + } + + // Now let's get to the actual reading of the data + if (mp1->reading) { + if (mp1->packetLength < MP1_MAX_FRAME_LENGTH) { + // If the length of the current incoming frame is + // still less than our max length, put the incoming + // byte in the buffer. + mp1->checksum_in = mp1->checksum_in ^ byte; + 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; + } + } + + // We need to set the escape sequence indicator back + // to false after each byte. + mp1->escape = false; } if (kfile_error(mp1->modem)) { @@ -143,6 +186,7 @@ void mp1Poll(MP1 *mp1) { } } +// FIXME: Desribe additions here static void mp1Putbyte(MP1 *mp1, uint8_t byte) { // If we are sending something that looks // like an HDLC special byte, send an escape @@ -152,20 +196,20 @@ static void mp1Putbyte(MP1 *mp1, uint8_t byte) { byte == AX25_ESC) { kfile_putc(AX25_ESC, mp1->modem); lastByte = AX25_ESC; - sendParityBlock ^= true; + //sendParityBlock ^= true; } kfile_putc(byte, mp1->modem); if (sendParityBlock) { - kfile_putc(mp1ParityBlock(lastByte, byte), mp1->modem); + uint8_t p = mp1ParityBlock(lastByte, byte); + kfile_putc(p, mp1->modem); } lastByte = byte; sendParityBlock ^= true; } - void mp1Send(MP1 *mp1, const void *_buffer, size_t length) { // Get the transmit data buffer const uint8_t *buffer = (const uint8_t *)_buffer; @@ -176,12 +220,27 @@ void mp1Send(MP1 *mp1, const void *_buffer, size_t length) { // Transmit the HDLC_FLAG to signify start of TX kfile_putc(HDLC_FLAG, mp1->modem); + // Write header and possibly padding + // Remember we also write a header and + // a checksum. This ensures that we will + // always end our packet with a checksum + // and a parity byte. + if (length % 2 != 0) { + mp1->checksum_out = mp1->checksum_out ^ 0xf1; + mp1Putbyte(mp1, 0xf1); + mp1->checksum_out = mp1->checksum_out ^ 0x55; + mp1Putbyte(mp1, 0x55); + } else { + mp1->checksum_out = mp1->checksum_out ^ 0xf0; + mp1Putbyte(mp1, 0xf0); + } + // Continously increment the pointer address // of the buffer while passing it to the byte // output function while (length--) { - mp1->checksum_out = mp1->checksum_out ^ *buffer; - mp1Putbyte(mp1, *buffer++); + mp1->checksum_out = mp1->checksum_out ^ *buffer; + mp1Putbyte(mp1, *buffer++); } // Write checksum to end of packet diff --git a/Modem/protocol/mp1.h b/Modem/protocol/mp1.h index 0887d4e..6606812 100644 --- a/Modem/protocol/mp1.h +++ b/Modem/protocol/mp1.h @@ -24,14 +24,17 @@ 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 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 + uint8_t calculatedParity; // Calculated parity for incoming data block mp1_callback_t callback; // The function to call when a packet has been received uint8_t checksum_in; // Rolling checksum for incoming packets uint8_t checksum_out; // Rolling checksum for outgoing packets bool reading; // True when we have seen a HDLC flag bool escape; // We need to know if we are in an escape sequence + bool fecEscape; // fec escape } MP1; // A struct encapsulating a network packet @@ -41,6 +44,7 @@ typedef struct MP1Packet { } MP1Packet; void mp1Init(MP1 *mp1, KFile *modem, mp1_callback_t callback); +void mp1Read(MP1 *mp1, int byte); void mp1Poll(MP1 *mp1); void mp1Send(MP1 *mp1, const void *_buffer, size_t length); diff --git a/buildrev.h b/buildrev.h index f77a343..fae7086 100644 --- a/buildrev.h +++ b/buildrev.h @@ -1,2 +1,2 @@ -#define VERS_BUILD 602 +#define VERS_BUILD 829 #define VERS_HOST "vixen"