lora_aprs_node_pico/src/kiss.cpp

#include "kiss.h"

/*
 * KISS encoder uses two steps:
 * 
 * Step 1: encodes the LoRa APRS frame into an AX.25 frame. DONE
 * Step 2: encapsulates the AX.25 frame into a KISS frame. TODO
 * 
 *  struct aprs_frame {
 *      uint8_t source_address[10];
 *      uint8_t digi_path[255];
 *      uint8_t data_field[255];
 *      uint8_t message[255];
 *      uint8_t digis[10][10];
 *      uint8_t acknowledge_number[255];
 *      bool acknowledge_request = false;
 *      uint16_t server_command = 0;
 *      uint16_t number_of_digipeaters = 0;
 *      
 *      uint8_t valid_apsr_data = false;
 *  };
 *
 */
uint16_t KissClass::EncodeFrame(struct aprs_frame *aprsframe, struct ax25_frame *ax25frame)
{
    uint8_t *encoded_call;
    uint16_t cnt=0;
    uint16_t position = 0;
    uint8_t digi_cnt = 0;
    
    // Destination call
    //printf("Destination: ");
    encoded_call = EncodeCall(aprsframe->digis[0]);
    cnt = 0;
    while (cnt < 7)
    {
        ax25frame->complete[position] = *(encoded_call+cnt);
        ////printf("0x%X ", *(encoded_call+cnt));
        //printf("0x%X ", ax25frame->complete[position]);
        cnt++;
        position++;
    }
    //printf("\n");
    
    // Source call
    //printf("Source: ");
    cnt=0;
    encoded_call = EncodeCall(aprsframe->source_address);
    while (cnt < 7)
    {
        ax25frame->complete[position] = *(encoded_call+cnt);
        ////printf("0x%X ", *(encoded_call+cnt));
        //printf("0x%X ", ax25frame->complete[position]);
        cnt++;
        position++;
    }
    //printf("\n");
    
    // No digipeaters in path, so destination is the last call and therefore the 'last' flag should be set
    if (aprsframe->number_of_digipeaters == 0)
    {
        ax25frame->complete[position-1] |= 0x01;
    }
    else
    {

        // Path (digipeaters)
        //printf("Path: ");
        cnt=0;
        digi_cnt = 1;       // Start at position 1 as position 0 contains destination (which we already encoded)
        while (aprsframe->number_of_digipeaters-- != 0) {
            
            encoded_call = EncodeCall(aprsframe->digis[digi_cnt]);            
            while (cnt < 7) {
                ax25frame->complete[position] = *(encoded_call+cnt);
                ////printf("0x%X ", *(encoded_call+cnt));
                //printf("0x%X ", ax25frame->complete[position]);
                cnt++;
                position++;
            }
            digi_cnt++;
            cnt = 0;
            
        }
        //printf("\n");
        
        // Set 'last' flag
        ax25frame->complete[position-1] |= 0x01;
    }
    
    // Add control fields
    ax25frame->complete[position++] = 0x03;
    ax25frame->complete[position++] = 0xF0;
    
    // All the dificult bits are done, now we just add the payload.
    //printf("Data:");
    cnt = 0;
    while (aprsframe->data_field[cnt] != 0 && cnt < 256) {
        ax25frame->complete[position] = aprsframe->data_field[cnt];
        //printf("0x%X ", ax25frame->complete[position]);
        cnt++;
        position++;
    }
    //printf( "\n");
    
    // Store length of AX25 frame
    ax25frame->lenght = position;
    
    // Encapsulate AX.25 frame in KISS frame (including escaping FEND codes)    
    putchar(FEND);
    putchar(CMD_DATA);
    cnt=0;
    position=ax25frame->lenght;
    while (position-- != 0)
    {
        // Escape FESC and TFEND
        if (ax25frame->complete[cnt] == FEND) {
            putchar(FESC);
            putchar(TFEND);
        } else if (ax25frame->complete[cnt] == FESC) {
            putchar(FESC);
            putchar(TFESC);
        } else {
            putchar(ax25frame->complete[cnt]);
        }
        cnt++;
    }
    putchar(FEND);
    //printf("\n");
    

    return 0;
}

/*
 * Decodes a KISS frame from the usb serial port
 * 
 * Input : string starting with FEND and ending with FEND
 * Output: ax25frame->decoded_kiss_frame terminated with NULL
 * Return: 0 = OK, 1 = ERROR, 2 = EXIT KISS MODE
 */
uint16_t KissClass::DecodeFrame(uint8_t string[], struct ax25_frame *ax25frame)
{
    uint16_t position  =0 ;
    uint16_t cnt  =0 ;
    
    // Not a valid frame
    if (string[position] != FEND)
        return 1;
    
    position++;
    
    // Is data frame: we de-escape the string and put it back in the same string with a NULL at the end as terminator.
    if (string[position] == CMD_DATA)
    {
        position++;
        while (string[position] != FEND)
        {
            // De-escape codes
            if (string[position] == FESC && string[position+1] == TFEND)
            {
                string[cnt] = FEND;
                position++;
            }
            else if (string[position] == FESC && string[position+1] == TFESC)
            {
                string[cnt] = FESC;
                position++;
            }
            else {
                string[cnt] = string[position];
            }
            cnt++;
            position++;
        }
       string[cnt] = 0;             //Terminate string
        
    }
    // Is command to exit KISS MODE
    else if (string[position] == CMD_EXIT_KISS) {
        return 2;
    }
    
    // If we are here we have a valid AX.25 frame in 'string' which is decapsulated from its KISS frame
    
    return 0;
}

/*
 * Encodes call according to AX.25 specs.
 * 
 * input : string with call and ssid as readable characters
 * output: pointer to memory location (7 bytes with the encoded call)
 * 
 */
uint8_t * KissClass::EncodeCall(uint8_t string[])
{
    uint8_t position = 0;
    uint8_t cnt = 0;
    
    static uint8_t call[7] = { 0,0,0,0,0,0,0} ;
    uint8_t ssid = 0;
    
    // extract call
    while( string[position] != 0 || cnt < 6)
    {
        if ( string[position] == '-') {
            position++;
            break;
        } else {
            call[cnt] = string[position] << 1;            
        }
        cnt++;
        position++;
    }
    // pad with spaces to a length of 6
    while( cnt < 6 )
    {
        call[cnt++] = ' ' << 1;
    }
    
    // extract ssid
    cnt=0;
    while( string[position] != 0 )
    {
        if (string[position] >= 48 && string[position] <= 57) {
            ssid = 10*ssid + string[position]-48;
            position++;
        }
    }
    
    // if asterix is pressent in string, than the message has been repeated, so set the 'has been repeated' flag in the ssid register
    position = 0;
    while( string[position] != 0)
    {
        if (string[position] == '*')
            ssid = 0b10000000;
        
        position++;
    }
    
    ssid = (ssid << 1) | 0b01100000;
    
    // add encoded ssid to encoded call array
    call[6] = ssid;
    
    /*
    //printf("SSID: 0x%X\n", ssid);
    
    cnt = 0;
    while (cnt < 7)
    {
        //printf("0x%X ", call[cnt++]);
    }
    //printf("\n");
    */
    
    return call;
    
}