diff --git a/src/protocols/SSDV/SSDV.cpp b/src/protocols/SSDV/SSDV.cpp new file mode 100644 index 00000000..a8ad581f --- /dev/null +++ b/src/protocols/SSDV/SSDV.cpp @@ -0,0 +1,189 @@ +#include "SSDV.h" + +SSDVClient::SSDVClient(RTTYClient* rtty) { + _rtty = rtty; +} + +int16_t SSDVClient::begin(const char* callsign) { + // check source callsign length (6 characters max) + if(strlen(callsign) > SSDV_MAX_CALLSIGN_LENGTH) { + return(ERR_INVALID_CALLSIGN); + } + + // encode callsign + callsignEnc = encodeBase40(callsign); + + return(ERR_NONE); +} + +int16_t SSDVClient::startTransfer(uint8_t mode, uint8_t* image, uint8_t imageID, uint16_t width, uint16_t height, uint8_t quality, uint8_t subsampling) { + // check all parameters + if(!((mode == SSDV_PACKET_TYPE_NORMAL) || (mode == SSDV_PACKET_TYPE_NO_FEC))) { + return(ERR_INVALID_SSDV_MODE); + } + if((width % 16 != 0) || (height % 16 != 0)) { + return(ERR_INVALID_IMAGE_SIZE); + } + RADIOLIB_CHECK_RANGE(quality, 0, 7, ERR_INVALID_IMAGE_QUALITY); + RADIOLIB_CHECK_RANGE(subsampling, 0, 3, ERR_INVALID_SUBSAMPLING); + + // save the parameters + _mode = mode; + _img = image; + _imgID = imageID; + _w = width; + _h = height; + _qual = (quality ^ 4) << 3; + _sub = subsampling; + + // initialize internal counters + _packetID = 0; + + return(ERR_NONE); +} + +void SSDVClient::sendPacket() { + uint8_t buff[SSDV_PACKET_LENGTH]; + uint8_t* buffPtr = buff; + + // sync byte + *buffPtr++ = SSDV_SYNC; + + // mode + *buffPtr++ = _mode; + + // callsign + memcpy(buffPtr, &callsignEnc, sizeof(callsignEnc)); + buffPtr += sizeof(callsignEnc); + + // image ID + *buffPtr++ = _imgID; + + // packet ID + memcpy(buffPtr, &_packetID, sizeof(_packetID)); + buffPtr += sizeof(_packetID); + _packetID++; + + // width and height + *buffPtr++ = _w; + *buffPtr++ = _h; + + // flags - check if this is the last packet + *buffPtr++ = _qual | _sub; + + // MCU offset and index + + // payload data + + // 32-bit CRC and Reed-Solomon FEC (normal mode only) + uint32_t crc; + if(_mode == SSDV_PACKET_TYPE_NORMAL) { + // normal mode, use FEC + crc = getChecksum(buff + SSDV_PAYLOAD_POS, SSDV_PAYLOAD_LEN_NORMAL); + memcpy(buff + SSDV_CHECKSUM_POS_NORMAL, &crc, sizeof(uint32_t)); + + encodeRS8(buff + 1, buff + SSDV_FEC_POS, 0); + + } else { + // no-FEC mode, CRC only + crc = getChecksum(buff + SSDV_PAYLOAD_POS, SSDV_PAYLOAD_LEN_NO_FEC); + memcpy(buff + SSDV_CHECKSUM_POS_NO_FEC, &crc, sizeof(uint32_t)); + + } + + // send the packet + _rtty->write(buff, SSDV_PACKET_LENGTH); +} + +/* + Base-40 encoding implementation based on https://github.com/fsphil/ssd + + Licensed under GNU General Public License v3.0 + https://github.com/fsphil/ssdv/blob/035f920f5c96880bfd89d4469428b934e830c7c9/COPYING +*/ +uint32_t SSDVClient::encodeBase40(char* str) { + // sanity checks + uint8_t len = strlen(str); + if(len == 0) { + return(0x00000000); + } else if(len > 6) { + return(0xFFFFFFFF) + } + + // encode + uint32_t enc = 0; + for(int8_t i = len - 1; i >= 0; i--) { + enc *= 40; + if((str[i] >= 'A') && (str[i] <= 'Z')) { + enc += str[i] - 'A' + 14; + } else if((str[i] >= '0') && (str[i] <= '9')) { + enc += str[i] - '0' + 1; + } + } + + return(enc); +} + +/* + SSDV CRC32 implementation from https://github.com/fsphil/ssd + + Licensed under GNU General Public License v3.0 + https://github.com/fsphil/ssdv/blob/035f920f5c96880bfd89d4469428b934e830c7c9/COPYING +*/ +uint32_t SSDVClient::getChecksum(uint8_t* data, size_t len) { + uint32_t crc, x; + uint8_t i, *d; + + for(d = data, crc = SSDV_CRC32_INITIAL; len; len--) { + x = (crc ^ *(d++)) & 0xFF; + for(i = 8; i > 0; i--) { + if(x & 1) { + x = (x >> 1) ^ SSDV_CRC32_POLYNOMIAL; + } else { + x >>= 1; + } + } + crc = (crc >> 8) ^ x; + } + + return(crc ^ SSDV_CRC32_INITIAL); +} + +int16_t mod255(int16_t val) { + while(val >= 255) { + val -= 255; + val = (val >> 8) + (val & 255); + } + return(val); +} + +/* + SSDV Reed-Solomon forward error correction implementation from https://github.com/fsphil/ssd + + Licensed under GNU General Public License v3.0 + https://github.com/fsphil/ssdv/blob/035f920f5c96880bfd89d4469428b934e830c7c9/COPYING +*/ +void SSDVClient::encodeRS8(uint8_t* data, uint8_t* parity, int16_t pad) { + int16_t i, j; + uint8_t feedback; + + memset(parity, 0, SSDV_RS_FEC_NROOTS * sizeof(uint8_t)); + + for(i = 0; i < SSDV_RS_FEC_NN - SSDV_RS_FEC_NROOTS - pad; i++) { + feedback = pgm_read_byte(&rs8_index_of[data[i] ^ parity[0]]); + + if(feedback != SSDV_RS_FEC_A0) { /* feedback term is non-zero */ + for(j = 1; j < SSDV_RS_FEC_NROOTS; j++) { + parity[j] ^= pgm_read_byte(&rs8_alpha_to[mod255(feedback + pgm_read_byte(&rs8_poly[SSDV_RS_FEC_NROOTS - j]))]); + } + } + + /* Shift */ + memmove(&parity[0], &parity[1], sizeof(uint8_t) * (SSDV_RS_FEC_NROOTS - 1)); + if(feedback != SSDV_RS_FEC_A0) { + parity[SSDV_RS_FEC_NROOTS - 1] = pgm_read_byte(&rs8_alpha_to[mod255(feedback + pgm_read_byte(&poly[0]))]); + } else { + parity[SSDV_RS_FEC_NROOTS - 1] = 0; + } + } +} diff --git a/src/protocols/SSDV/SSDV.h b/src/protocols/SSDV/SSDV.h new file mode 100644 index 00000000..1e3a210e --- /dev/null +++ b/src/protocols/SSDV/SSDV.h @@ -0,0 +1,124 @@ +#ifndef _RADIOLIB_SSDV_H +#define _RADIOLIB_SSDV_H + +#include "../../TypeDef.h" +#include "../RTTY/RTTY.h" + +#define SSDV_MAX_CALLSIGN_LENGTH 6 +#define SSDV_PACKET_LENGTH 256 + +// CRC32 definitions +#define SSDV_CRC32_INITIAL 0xFFFFFFFF +#define SSDV_CRC32_POLYNOMIAL 0xEDB88320 + +// Reed-Solomon FEC +#define SSDV_RS_FEC_MM (8) +#define SSDV_RS_FEC_NN (255) +#define SSDV_RS_FEC_NROOTS (32) +#define SSDV_RS_FEC_FCR (112) +#define SSDV_RS_FEC_PRIM (11) +#define SSDV_RS_FEC_IPRIM (116) +#define SSDV_RS_FEC_A0 (NN) + +// packet field positions/lengths +#define SSDV_PAYLOAD_POS 15 +#define SSDV_PAYLOAD_LEN_NORMAL 205 +#define SSDV_PAYLOAD_LEN_NO_FEC 237 +#define SSDV_CHECKSUM_POS_NORMAL 220 +#define SSDV_CHECKSUM_POS_NO_FEC 252 +#define SSDV_FEC_POS 224 + +// packet fields MSB LSB DESCRIPTION +#define SSDV_SYNC 0x55 // 7 0 sync byte +#define SSDV_PACKET_TYPE_NORMAL 0x66 // 7 0 packet type: normal (224 byte payload including header, 32 byte FEC) +#define SSDV_PACKET_TYPE_NO_FEC 0x67 // 7 0 no-FEC (256 byte payload including header) +#define SSDV_MCU_OFFSET_NONE 0xFF // 7 0 no MCU offset in the current payload +#define SSDV_MCU_INDEX_NONE 0xFFFF // 15 0 + +// flag field +#define SSDV_JPEG_QUALITY 0b00111000 // 5 3 JPEG quality level (0-7 xor 4) +#define SSDV_END_OF_IMAGE_FLAG 0b00000100 // 2 2 end of image flag +#define SSDV_SUBSAMPLING_2X2 0b00000000 // 1 0 subsampling mode: 2x2 +#define SSDV_SUBSAMPLING_1X2 0b00000001 // 1 0 1x2 +#define SSDV_SUBSAMPLING_2X1 0b00000010 // 1 0 2x1 +#define SSDV_SUBSAMPLING_1X1 0b00000011 // 1 0 1x1 + +static const uint8_t rs8_alpha_to[] PROGMEM = { + 0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x87,0x89,0x95,0xAD,0xDD,0x3D,0x7A,0xF4, + 0x6F,0xDE,0x3B,0x76,0xEC,0x5F,0xBE,0xFB,0x71,0xE2,0x43,0x86,0x8B,0x91,0xA5,0xCD, + 0x1D,0x3A,0x74,0xE8,0x57,0xAE,0xDB,0x31,0x62,0xC4,0x0F,0x1E,0x3C,0x78,0xF0,0x67, + 0xCE,0x1B,0x36,0x6C,0xD8,0x37,0x6E,0xDC,0x3F,0x7E,0xFC,0x7F,0xFE,0x7B,0xF6,0x6B, + 0xD6,0x2B,0x56,0xAC,0xDF,0x39,0x72,0xE4,0x4F,0x9E,0xBB,0xF1,0x65,0xCA,0x13,0x26, + 0x4C,0x98,0xB7,0xE9,0x55,0xAA,0xD3,0x21,0x42,0x84,0x8F,0x99,0xB5,0xED,0x5D,0xBA, + 0xF3,0x61,0xC2,0x03,0x06,0x0C,0x18,0x30,0x60,0xC0,0x07,0x0E,0x1C,0x38,0x70,0xE0, + 0x47,0x8E,0x9B,0xB1,0xE5,0x4D,0x9A,0xB3,0xE1,0x45,0x8A,0x93,0xA1,0xC5,0x0D,0x1A, + 0x34,0x68,0xD0,0x27,0x4E,0x9C,0xBF,0xF9,0x75,0xEA,0x53,0xA6,0xCB,0x11,0x22,0x44, + 0x88,0x97,0xA9,0xD5,0x2D,0x5A,0xB4,0xEF,0x59,0xB2,0xE3,0x41,0x82,0x83,0x81,0x85, + 0x8D,0x9D,0xBD,0xFD,0x7D,0xFA,0x73,0xE6,0x4B,0x96,0xAB,0xD1,0x25,0x4A,0x94,0xAF, + 0xD9,0x35,0x6A,0xD4,0x2F,0x5E,0xBC,0xFF,0x79,0xF2,0x63,0xC6,0x0B,0x16,0x2C,0x58, + 0xB0,0xE7,0x49,0x92,0xA3,0xC1,0x05,0x0A,0x14,0x28,0x50,0xA0,0xC7,0x09,0x12,0x24, + 0x48,0x90,0xA7,0xC9,0x15,0x2A,0x54,0xA8,0xD7,0x29,0x52,0xA4,0xCF,0x19,0x32,0x64, + 0xC8,0x17,0x2E,0x5C,0xB8,0xF7,0x69,0xD2,0x23,0x46,0x8C,0x9F,0xB9,0xF5,0x6D,0xDA, + 0x33,0x66,0xCC,0x1F,0x3E,0x7C,0xF8,0x77,0xEE,0x5B,0xB6,0xEB,0x51,0xA2,0xC3,0x00 +}; + +static const uint8_t rs8_index_of[] PROGMEM = { + 0xFF,0x00,0x01,0x63,0x02,0xC6,0x64,0x6A,0x03,0xCD,0xC7,0xBC,0x65,0x7E,0x6B,0x2A, + 0x04,0x8D,0xCE,0x4E,0xC8,0xD4,0xBD,0xE1,0x66,0xDD,0x7F,0x31,0x6C,0x20,0x2B,0xF3, + 0x05,0x57,0x8E,0xE8,0xCF,0xAC,0x4F,0x83,0xC9,0xD9,0xD5,0x41,0xBE,0x94,0xE2,0xB4, + 0x67,0x27,0xDE,0xF0,0x80,0xB1,0x32,0x35,0x6D,0x45,0x21,0x12,0x2C,0x0D,0xF4,0x38, + 0x06,0x9B,0x58,0x1A,0x8F,0x79,0xE9,0x70,0xD0,0xC2,0xAD,0xA8,0x50,0x75,0x84,0x48, + 0xCA,0xFC,0xDA,0x8A,0xD6,0x54,0x42,0x24,0xBF,0x98,0x95,0xF9,0xE3,0x5E,0xB5,0x15, + 0x68,0x61,0x28,0xBA,0xDF,0x4C,0xF1,0x2F,0x81,0xE6,0xB2,0x3F,0x33,0xEE,0x36,0x10, + 0x6E,0x18,0x46,0xA6,0x22,0x88,0x13,0xF7,0x2D,0xB8,0x0E,0x3D,0xF5,0xA4,0x39,0x3B, + 0x07,0x9E,0x9C,0x9D,0x59,0x9F,0x1B,0x08,0x90,0x09,0x7A,0x1C,0xEA,0xA0,0x71,0x5A, + 0xD1,0x1D,0xC3,0x7B,0xAE,0x0A,0xA9,0x91,0x51,0x5B,0x76,0x72,0x85,0xA1,0x49,0xEB, + 0xCB,0x7C,0xFD,0xC4,0xDB,0x1E,0x8B,0xD2,0xD7,0x92,0x55,0xAA,0x43,0x0B,0x25,0xAF, + 0xC0,0x73,0x99,0x77,0x96,0x5C,0xFA,0x52,0xE4,0xEC,0x5F,0x4A,0xB6,0xA2,0x16,0x86, + 0x69,0xC5,0x62,0xFE,0x29,0x7D,0xBB,0xCC,0xE0,0xD3,0x4D,0x8C,0xF2,0x1F,0x30,0xDC, + 0x82,0xAB,0xE7,0x56,0xB3,0x93,0x40,0xD8,0x34,0xB0,0xEF,0x26,0x37,0x0C,0x11,0x44, + 0x6F,0x78,0x19,0x9A,0x47,0x74,0xA7,0xC1,0x23,0x53,0x89,0xFB,0x14,0x5D,0xF8,0x97, + 0x2E,0x4B,0xB9,0x60,0x0F,0xED,0x3E,0xE5,0xF6,0x87,0xA5,0x17,0x3A,0xA3,0x3C,0xB7 +}; + +static const uint8_t rs8_poly[] PROGMEM = { + 0x00,0xF9,0x3B,0x42,0x04,0x2B,0x7E,0xFB,0x61,0x1E,0x03,0xD5,0x32,0x42,0xAA,0x05, + 0x18,0x05,0xAA,0x42,0x32,0xD5,0x03,0x1E,0x61,0xFB,0x7E,0x2B,0x04,0x42,0x3B,0xF9, + 0x00 +}; + +class SSDVClient { + public: + /*! + \brief Default constructor. + + \param rtty Pointer to the RTTY client used for transfer. + */ + SSTVClient(RTTYClient* rtty); + + int16_t begin(const char* callsign); + + int16_t startTransfer(uint8_t mode, uint8_t* image, uint8_t imageID, uint16_t width, uint16_t height, uint8_t quality = SSDV_JPEG_QUALITY, uint8_t subsampling = SSDV_SUBSAMPLING_1X1); + + void sendPacket(); + +#ifndef RADIOLIB_GODMODE + private: +#endif + RTTYClient* _rtty; + + // Base-40 encoded callsign + uint32_t callsignEnc; + + // cached parameters of image currently in transfer + uint16_t _packetID, _w, _h; + uint8_t _mode, _imgID, _qual, _sub; + uint8_t* _img; + + uint32_t encodeBase40(char* str); + uint32_t getChecksum(uint8_t* data, size_t len); + int16_t mod255(int16_t val); + void encodeRS8(uint8_t* data, uint8_t* parity, int16_t pad); +}; + +#endif