acid-drop

- Hacking the planet from a LilyGo T-Deck using custom firmware
git clone git://git.acid.vegas/acid-drop.git
AX25.cpp (15265B)
      1 #include "AX25.h"
      2 #if !defined(RADIOLIB_EXCLUDE_AX25)
      3 
      4 AX25Frame::AX25Frame(const char* destCallsign, uint8_t destSSID, const char* srcCallsign, uint8_t srcSSID, uint8_t control)
      5 : AX25Frame(destCallsign, destSSID, srcCallsign, srcSSID, control, 0, NULL, 0) {
      6 
      7 }
      8 
      9 AX25Frame::AX25Frame(const char* destCallsign, uint8_t destSSID, const char* srcCallsign, uint8_t srcSSID, uint8_t control, uint8_t protocolID, const char* info)
     10   : AX25Frame(destCallsign, destSSID, srcCallsign, srcSSID, control, protocolID, (uint8_t*)info, strlen(info)) {
     11 
     12 }
     13 
     14 AX25Frame::AX25Frame(const char* destCallsign, uint8_t destSSID, const char* srcCallsign, uint8_t srcSSID, uint8_t control, uint8_t protocolID, uint8_t* info, uint16_t infoLen) {
     15   // destination callsign/SSID
     16   memcpy(this->destCallsign, destCallsign, strlen(destCallsign));
     17   this->destCallsign[strlen(destCallsign)] = '\0';
     18   this->destSSID = destSSID;
     19 
     20   // source callsign/SSID
     21   memcpy(this->srcCallsign, srcCallsign, strlen(srcCallsign));
     22   this->srcCallsign[strlen(srcCallsign)] = '\0';
     23   this->srcSSID = srcSSID;
     24 
     25   // set repeaters
     26   this->numRepeaters = 0;
     27   #if !defined(RADIOLIB_STATIC_ONLY)
     28     this->repeaterCallsigns = NULL;
     29     this->repeaterSSIDs = NULL;
     30   #endif
     31 
     32   // control field
     33   this->control = control;
     34 
     35   // sequence numbers
     36   this->rcvSeqNumber = 0;
     37   this->sendSeqNumber = 0;
     38 
     39   // PID field
     40   this->protocolID = protocolID;
     41 
     42   // info field
     43   this->infoLen = infoLen;
     44   if(infoLen > 0) {
     45     #if !defined(RADIOLIB_STATIC_ONLY)
     46       this->info = new uint8_t[infoLen];
     47     #endif
     48     memcpy(this->info, info, infoLen);
     49   }
     50 }
     51 
     52 AX25Frame::AX25Frame(const AX25Frame& frame) {
     53   *this = frame;
     54 }
     55 
     56 AX25Frame::~AX25Frame() {
     57   #if !defined(RADIOLIB_STATIC_ONLY)
     58     // deallocate info field
     59     if(infoLen > 0) {
     60       delete[] this->info;
     61     }
     62 
     63     // deallocate repeaters
     64     if(this->numRepeaters > 0) {
     65       for(uint8_t i = 0; i < this->numRepeaters; i++) {
     66         delete[] this->repeaterCallsigns[i];
     67       }
     68       delete[] this->repeaterCallsigns;
     69       delete[] this->repeaterSSIDs;
     70     }
     71   #endif
     72 }
     73 
     74 AX25Frame& AX25Frame::operator=(const AX25Frame& frame) {
     75   // destination callsign/SSID
     76   memcpy(this->destCallsign, frame.destCallsign, strlen(frame.destCallsign));
     77   this->destCallsign[strlen(frame.destCallsign)] = '\0';
     78   this->destSSID = frame.destSSID;
     79 
     80   // source callsign/SSID
     81   memcpy(this->srcCallsign, frame.srcCallsign, strlen(frame.srcCallsign));
     82   this->srcCallsign[strlen(frame.srcCallsign)] = '\0';
     83   this->srcSSID = frame.srcSSID;
     84 
     85   // repeaters
     86   this->numRepeaters = frame.numRepeaters;
     87   for(uint8_t i = 0; i < this->numRepeaters; i++) {
     88     memcpy(this->repeaterCallsigns[i], frame.repeaterCallsigns[i], strlen(frame.repeaterCallsigns[i]));
     89   }
     90   memcpy(this->repeaterSSIDs, frame.repeaterSSIDs, this->numRepeaters);
     91 
     92   // control field
     93   this->control = frame.control;
     94 
     95   // sequence numbers
     96   this->rcvSeqNumber = frame.rcvSeqNumber;
     97   this->sendSeqNumber = frame.sendSeqNumber;
     98 
     99   // PID field
    100   this->protocolID = frame.protocolID;
    101 
    102   // info field
    103   this->infoLen = frame.infoLen;
    104   memcpy(this->info, frame.info, this->infoLen);
    105 
    106   return(*this);
    107 }
    108 
    109 int16_t AX25Frame::setRepeaters(char** repeaterCallsigns, uint8_t* repeaterSSIDs, uint8_t numRepeaters) {
    110   // check number of repeaters
    111   if((numRepeaters < 1) || (numRepeaters > 8)) {
    112     return(RADIOLIB_ERR_INVALID_NUM_REPEATERS);
    113   }
    114 
    115   // check repeater configuration
    116   if((repeaterCallsigns == NULL) || (repeaterSSIDs == NULL)) {
    117     return(RADIOLIB_ERR_INVALID_NUM_REPEATERS);
    118   }
    119   for(uint16_t i = 0; i < numRepeaters; i++) {
    120     if(strlen(repeaterCallsigns[i]) > RADIOLIB_AX25_MAX_CALLSIGN_LEN) {
    121       return(RADIOLIB_ERR_INVALID_REPEATER_CALLSIGN);
    122     }
    123   }
    124 
    125   // create buffers
    126   #if !defined(RADIOLIB_STATIC_ONLY)
    127     this->repeaterCallsigns = new char*[numRepeaters];
    128     for(uint8_t i = 0; i < numRepeaters; i++) {
    129       this->repeaterCallsigns[i] = new char[strlen(repeaterCallsigns[i]) + 1];
    130     }
    131     this->repeaterSSIDs = new uint8_t[numRepeaters];
    132   #endif
    133 
    134   // copy data
    135   this->numRepeaters = numRepeaters;
    136   for(uint8_t i = 0; i < numRepeaters; i++) {
    137     memcpy(this->repeaterCallsigns[i], repeaterCallsigns[i], strlen(repeaterCallsigns[i]));
    138     this->repeaterCallsigns[i][strlen(repeaterCallsigns[i])] = '\0';
    139   }
    140   memcpy(this->repeaterSSIDs, repeaterSSIDs, numRepeaters);
    141 
    142   return(RADIOLIB_ERR_NONE);
    143 }
    144 
    145 void AX25Frame::setRecvSequence(uint8_t seqNumber) {
    146   this->rcvSeqNumber = seqNumber;
    147 }
    148 
    149 void AX25Frame::setSendSequence(uint8_t seqNumber) {
    150   this->sendSeqNumber = seqNumber;
    151 }
    152 
    153 AX25Client::AX25Client(PhysicalLayer* phy) {
    154   _phy = phy;
    155   #if !defined(RADIOLIB_EXCLUDE_AFSK)
    156   _audio = nullptr;
    157   #endif
    158 }
    159 
    160 #if !defined(RADIOLIB_EXCLUDE_AFSK)
    161 AX25Client::AX25Client(AFSKClient* audio) {
    162   _phy = audio->_phy;
    163   _audio = audio;
    164   _afskMark = RADIOLIB_AX25_AFSK_MARK;
    165   _afskSpace = RADIOLIB_AX25_AFSK_SPACE;
    166 }
    167 
    168 int16_t AX25Client::setCorrection(int16_t mark, int16_t space) {
    169   _afskMark = RADIOLIB_AX25_AFSK_MARK + mark;
    170   _afskSpace = RADIOLIB_AX25_AFSK_SPACE + space;
    171   return(RADIOLIB_ERR_NONE);
    172 }
    173 #endif
    174 
    175 int16_t AX25Client::begin(const char* srcCallsign, uint8_t srcSSID, uint8_t preambleLen) {
    176   // set source SSID
    177   _srcSSID = srcSSID;
    178 
    179   // check source callsign length (6 characters max)
    180   if(strlen(srcCallsign) > RADIOLIB_AX25_MAX_CALLSIGN_LEN) {
    181     return(RADIOLIB_ERR_INVALID_CALLSIGN);
    182   }
    183 
    184   // copy callsign
    185   memcpy(_srcCallsign, srcCallsign, strlen(srcCallsign));
    186   _srcCallsign[strlen(srcCallsign)] = '\0';
    187 
    188   // save preamble length
    189   _preambleLen = preambleLen;
    190 
    191   // configure for direct mode
    192   return(_phy->startDirect());
    193 }
    194 
    195 int16_t AX25Client::transmit(const char* str, const char* destCallsign, uint8_t destSSID) {
    196   // create control field
    197   uint8_t controlField = RADIOLIB_AX25_CONTROL_U_UNNUMBERED_INFORMATION | RADIOLIB_AX25_CONTROL_POLL_FINAL_DISABLED | RADIOLIB_AX25_CONTROL_UNNUMBERED_FRAME;
    198 
    199   // build the frame
    200   AX25Frame frame(destCallsign, destSSID, _srcCallsign, _srcSSID, controlField, RADIOLIB_AX25_PID_NO_LAYER_3, (uint8_t*)str, strlen(str));
    201 
    202   // send Unnumbered Information frame
    203   return(sendFrame(&frame));
    204 }
    205 
    206 int16_t AX25Client::sendFrame(AX25Frame* frame) {
    207   // check destination callsign length (6 characters max)
    208   if(strlen(frame->destCallsign) > RADIOLIB_AX25_MAX_CALLSIGN_LEN) {
    209     return(RADIOLIB_ERR_INVALID_CALLSIGN);
    210   }
    211 
    212   // check repeater configuration
    213   #if !defined(RADIOLIB_STATIC_ONLY)
    214     if(!(((frame->repeaterCallsigns == NULL) && (frame->repeaterSSIDs == NULL) && (frame->numRepeaters == 0)) ||
    215          ((frame->repeaterCallsigns != NULL) && (frame->repeaterSSIDs != NULL) && (frame->numRepeaters != 0)))) {
    216       return(RADIOLIB_ERR_INVALID_NUM_REPEATERS);
    217     }
    218     for(uint16_t i = 0; i < frame->numRepeaters; i++) {
    219       if(strlen(frame->repeaterCallsigns[i]) > RADIOLIB_AX25_MAX_CALLSIGN_LEN) {
    220         return(RADIOLIB_ERR_INVALID_REPEATER_CALLSIGN);
    221       }
    222     }
    223   #endif
    224 
    225   // calculate frame length without FCS (destination address, source address, repeater addresses, control, PID, info)
    226   size_t frameBuffLen = ((2 + frame->numRepeaters)*(RADIOLIB_AX25_MAX_CALLSIGN_LEN + 1)) + 1 + 1 + frame->infoLen;
    227   // create frame buffer without preamble, start or stop flags
    228   #if !defined(RADIOLIB_STATIC_ONLY)
    229     uint8_t* frameBuff = new uint8_t[frameBuffLen + 2];
    230   #else
    231     uint8_t frameBuff[RADIOLIB_STATIC_ARRAY_SIZE];
    232   #endif
    233   uint8_t* frameBuffPtr = frameBuff;
    234 
    235   // set destination callsign - all address field bytes are shifted by one bit to make room for HDLC address extension bit
    236   memset(frameBuffPtr, ' ' << 1, RADIOLIB_AX25_MAX_CALLSIGN_LEN);
    237   for(size_t i = 0; i < strlen(frame->destCallsign); i++) {
    238     *(frameBuffPtr + i) = frame->destCallsign[i] << 1;
    239   }
    240   frameBuffPtr += RADIOLIB_AX25_MAX_CALLSIGN_LEN;
    241 
    242   // set destination SSID
    243   *(frameBuffPtr++) = RADIOLIB_AX25_SSID_RESPONSE_DEST | RADIOLIB_AX25_SSID_RESERVED_BITS | (frame->destSSID & 0x0F) << 1 | RADIOLIB_AX25_SSID_HDLC_EXTENSION_CONTINUE;
    244 
    245   // set source callsign - all address field bytes are shifted by one bit to make room for HDLC address extension bit
    246   memset(frameBuffPtr, ' ' << 1, RADIOLIB_AX25_MAX_CALLSIGN_LEN);
    247   for(size_t i = 0; i < strlen(frame->srcCallsign); i++) {
    248     *(frameBuffPtr + i) = frame->srcCallsign[i] << 1;
    249   }
    250   frameBuffPtr += RADIOLIB_AX25_MAX_CALLSIGN_LEN;
    251 
    252   // set source SSID
    253   *(frameBuffPtr++) = RADIOLIB_AX25_SSID_COMMAND_SOURCE | RADIOLIB_AX25_SSID_RESERVED_BITS | (frame->srcSSID & 0x0F) << 1 | RADIOLIB_AX25_SSID_HDLC_EXTENSION_CONTINUE;
    254 
    255   // set repeater callsigns
    256   for(uint16_t i = 0; i < frame->numRepeaters; i++) {
    257     memset(frameBuffPtr, ' ' << 1, RADIOLIB_AX25_MAX_CALLSIGN_LEN);
    258     for(size_t j = 0; j < strlen(frame->repeaterCallsigns[i]); j++) {
    259       *(frameBuffPtr + j) = frame->repeaterCallsigns[i][j] << 1;
    260     }
    261     frameBuffPtr += RADIOLIB_AX25_MAX_CALLSIGN_LEN;
    262     *(frameBuffPtr++) = RADIOLIB_AX25_SSID_HAS_NOT_BEEN_REPEATED | RADIOLIB_AX25_SSID_RESERVED_BITS | (frame->repeaterSSIDs[i] & 0x0F) << 1 | RADIOLIB_AX25_SSID_HDLC_EXTENSION_CONTINUE;
    263   }
    264 
    265   // set HDLC extension end bit
    266   *(frameBuffPtr - 1) |= RADIOLIB_AX25_SSID_HDLC_EXTENSION_END;
    267 
    268   // set sequence numbers of the frames that have it
    269   uint8_t controlField = frame->control;
    270   if((frame->control & 0x01) == 0) {
    271     // information frame, set both sequence numbers
    272     controlField |= frame->rcvSeqNumber << 5;
    273     controlField |= frame->sendSeqNumber << 1;
    274   } else if((frame->control & 0x02) == 0) {
    275     // supervisory frame, set only receive sequence number
    276     controlField |= frame->rcvSeqNumber << 5;
    277   }
    278 
    279   // set control field
    280   *(frameBuffPtr++) = controlField;
    281 
    282   // set PID field of the frames that have it
    283   if(frame->protocolID != 0x00) {
    284     *(frameBuffPtr++) = frame->protocolID;
    285   }
    286 
    287   // set info field of the frames that have it
    288   if(frame->infoLen > 0) {
    289     memcpy(frameBuffPtr, frame->info, frame->infoLen);
    290     frameBuffPtr += frame->infoLen;
    291   }
    292 
    293   // flip bit order
    294   for(size_t i = 0; i < frameBuffLen; i++) {
    295     frameBuff[i] = Module::flipBits(frameBuff[i]);
    296   }
    297 
    298   // calculate FCS
    299   uint16_t fcs = getFrameCheckSequence(frameBuff, frameBuffLen);
    300   *(frameBuffPtr++) = (uint8_t)((fcs >> 8) & 0xFF);
    301   *(frameBuffPtr++) = (uint8_t)(fcs & 0xFF);
    302 
    303   // prepare buffer for the final frame (stuffed, with added preamble + flags and NRZI-encoded)
    304   #if !defined(RADIOLIB_STATIC_ONLY)
    305     // worst-case scenario: sequence of 1s, will have 120% of the original length, stuffed frame also includes both flags
    306     uint8_t* stuffedFrameBuff = new uint8_t[_preambleLen + 1 + (6*frameBuffLen)/5 + 2];
    307   #else
    308     uint8_t stuffedFrameBuff[RADIOLIB_STATIC_ARRAY_SIZE];
    309   #endif
    310 
    311   // initialize buffer to all zeros
    312   memset(stuffedFrameBuff, 0x00, _preambleLen + 1 + (6*frameBuffLen)/5 + 2);
    313 
    314   // stuff bits (skip preamble and both flags)
    315   uint16_t stuffedFrameBuffLenBits = 8*(_preambleLen + 1);
    316   uint8_t count = 0;
    317   for(size_t i = 0; i < frameBuffLen + 2; i++) {
    318     for(int8_t shift = 7; shift >= 0; shift--) {
    319       uint16_t stuffedFrameBuffPos = stuffedFrameBuffLenBits + 7 - 2*(stuffedFrameBuffLenBits%8);
    320       if((frameBuff[i] >> shift) & 0x01) {
    321         // copy 1 and increment counter
    322         SET_BIT_IN_ARRAY(stuffedFrameBuff, stuffedFrameBuffPos);
    323         stuffedFrameBuffLenBits++;
    324         count++;
    325 
    326         // check 5 consecutive 1s
    327         if(count == 5) {
    328           // get the new position in stuffed frame
    329           stuffedFrameBuffPos = stuffedFrameBuffLenBits + 7 - 2*(stuffedFrameBuffLenBits%8);
    330 
    331           // insert 0 and reset counter
    332           CLEAR_BIT_IN_ARRAY(stuffedFrameBuff, stuffedFrameBuffPos);
    333           stuffedFrameBuffLenBits++;
    334           count = 0;
    335         }
    336 
    337       } else {
    338         // copy 0 and reset counter
    339         CLEAR_BIT_IN_ARRAY(stuffedFrameBuff, stuffedFrameBuffPos);
    340         stuffedFrameBuffLenBits++;
    341         count = 0;
    342       }
    343 
    344     }
    345   }
    346 
    347   // deallocate memory
    348   #if !defined(RADIOLIB_STATIC_ONLY)
    349     delete[] frameBuff;
    350   #endif
    351 
    352   // set preamble bytes and start flag field
    353   for(uint16_t i = 0; i < _preambleLen + 1; i++) {
    354     stuffedFrameBuff[i] = RADIOLIB_AX25_FLAG;
    355   }
    356 
    357   // get stuffed frame length in bytes
    358   size_t stuffedFrameBuffLen = stuffedFrameBuffLenBits/8 + 1;
    359   uint8_t trailingLen = stuffedFrameBuffLenBits % 8;
    360 
    361   // set end flag field (may be split into two bytes due to misalignment caused by extra stuffing bits)
    362   if(trailingLen != 0) {
    363     stuffedFrameBuffLen++;
    364     stuffedFrameBuff[stuffedFrameBuffLen - 2] |= RADIOLIB_AX25_FLAG >> trailingLen;
    365     stuffedFrameBuff[stuffedFrameBuffLen - 1] = RADIOLIB_AX25_FLAG << (8 - trailingLen);
    366   } else {
    367     stuffedFrameBuff[stuffedFrameBuffLen - 1] = RADIOLIB_AX25_FLAG;
    368   }
    369 
    370   // convert to NRZI
    371   for(size_t i = _preambleLen + 1; i < stuffedFrameBuffLen*8; i++) {
    372     size_t currBitPos = i + 7 - 2*(i%8);
    373     size_t prevBitPos = (i - 1) + 7 - 2*((i - 1)%8);
    374     if(TEST_BIT_IN_ARRAY(stuffedFrameBuff, currBitPos)) {
    375       // bit is 1, no change, copy previous bit
    376       if(TEST_BIT_IN_ARRAY(stuffedFrameBuff, prevBitPos)) {
    377         SET_BIT_IN_ARRAY(stuffedFrameBuff, currBitPos);
    378       } else {
    379         CLEAR_BIT_IN_ARRAY(stuffedFrameBuff, currBitPos);
    380       }
    381 
    382     } else {
    383       // bit is 0, transition, copy inversion of the previous bit
    384       if(TEST_BIT_IN_ARRAY(stuffedFrameBuff, prevBitPos)) {
    385         CLEAR_BIT_IN_ARRAY(stuffedFrameBuff, currBitPos);
    386       } else {
    387         SET_BIT_IN_ARRAY(stuffedFrameBuff, currBitPos);
    388       }
    389     }
    390   }
    391 
    392   // transmit
    393   int16_t state = RADIOLIB_ERR_NONE;
    394   #if !defined(RADIOLIB_EXCLUDE_AFSK)
    395   if(_audio != nullptr) {
    396     Module* mod = _phy->getMod();
    397     _phy->transmitDirect();
    398 
    399     // iterate over all bytes in the buffer
    400     for(uint32_t i = 0; i < stuffedFrameBuffLen; i++) {
    401 
    402       // check each bit
    403       for(uint16_t mask = 0x80; mask >= 0x01; mask >>= 1) {
    404         uint32_t start = mod->micros();
    405         if(stuffedFrameBuff[i] & mask) {
    406           _audio->tone(_afskMark, false);
    407         } else {
    408           _audio->tone(_afskSpace, false);
    409         }
    410         while(mod->micros() - start < RADIOLIB_AX25_AFSK_TONE_DURATION) {
    411           mod->yield();
    412         }
    413       }
    414 
    415     }
    416 
    417     _audio->noTone();
    418 
    419   } else {
    420   #endif
    421     state = _phy->transmit(stuffedFrameBuff, stuffedFrameBuffLen);
    422   #if !defined(RADIOLIB_EXCLUDE_AFSK)
    423   }
    424   #endif
    425 
    426   // deallocate memory
    427   #if !defined(RADIOLIB_STATIC_ONLY)
    428     delete[] stuffedFrameBuff;
    429   #endif
    430 
    431   return(state);
    432 }
    433 
    434 void AX25Client::getCallsign(char* buff) {
    435   strncpy(buff, _srcCallsign, RADIOLIB_AX25_MAX_CALLSIGN_LEN);
    436 }
    437 
    438 uint8_t AX25Client::getSSID() {
    439   return(_srcSSID);
    440 }
    441 
    442 /*
    443   CCITT CRC implementation based on https://github.com/kicksat/ax25
    444 
    445   Licensed under Creative Commons Attribution-ShareAlike 4.0 International
    446   https://creativecommons.org/licenses/by-sa/4.0/
    447 */
    448 uint16_t AX25Client::getFrameCheckSequence(uint8_t* buff, size_t len) {
    449   uint8_t outBit;
    450   uint16_t mask;
    451   uint16_t shiftReg = CRC_CCITT_INIT;
    452 
    453   for(size_t i = 0; i < len; i++) {
    454     for(uint8_t b = 0x80; b > 0x00; b /= 2) {
    455       outBit = (shiftReg & 0x01) ? 0x01 : 0x00;
    456       shiftReg >>= 1;
    457       mask = XOR((buff[i] & b), outBit) ? CRC_CCITT_POLY_REVERSED : 0x0000;
    458       shiftReg ^= mask;
    459     }
    460   }
    461 
    462   return(Module::flipBits16(~shiftReg));
    463 }
    464 
    465 #endif