Hamlib/rigs/commradio/frame.c

/*
 * Hamlib CommRadio backend
 * idk, copyright and GPL here
 */

#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <hamlib/rig.h>
#include <serial.h>

#include "frame.h"


/*
 * Brute-force determined to be polynomial = 0x1021.
 * Seems to be the same CRC16 as Kermit. Initialized with 0.
 */
uint16_t crc16tab[] =
{
    0,      4489,  8978, 12955, 17956, 22445, 25910, 29887, 35912, 40385,
    44890, 48851, 51820, 56293, 59774, 63735,  4225,   264, 13203,  8730,
    22181, 18220, 30135, 25662, 40137, 36160, 49115, 44626, 56045, 52068,
    63999, 59510,  8450, 12427,   528,  5017, 26406, 30383, 17460, 21949,
    44362, 48323, 36440, 40913, 60270, 64231, 51324, 55797, 12675,  8202,
    4753,   792, 30631, 26158, 21685, 17724, 48587, 44098, 40665, 36688,
    64495, 60006, 55549, 51572, 16900, 21389, 24854, 28831,  1056,  5545,
    10034, 14011, 52812, 57285, 60766, 64727, 34920, 39393, 43898, 47859,
    21125, 17164, 29079, 24606,  5281,  1320, 14259,  9786, 57037, 53060,
    64991, 60502, 39145, 35168, 48123, 43634, 25350, 29327, 16404, 20893,
    9506, 13483,  1584,  6073, 61262, 65223, 52316, 56789, 43370, 47331,
    35448, 39921, 29575, 25102, 20629, 16668, 13731,  9258,  5809,  1848,
    65487, 60998, 56541, 52564, 47595, 43106, 39673, 35696, 33800, 38273,
    42778, 46739, 49708, 54181, 57662, 61623,  2112,  6601, 11090, 15067,
    20068, 24557, 28022, 31999, 38025, 34048, 47003, 42514, 53933, 49956,
    61887, 57398,  6337,  2376, 15315, 10842, 24293, 20332, 32247, 27774,
    42250, 46211, 34328, 38801, 58158, 62119, 49212, 53685, 10562, 14539,
    2640,  7129, 28518, 32495, 19572, 24061, 46475, 41986, 38553, 34576,
    62383, 57894, 53437, 49460, 14787, 10314,  6865,  2904, 32743, 28270,
    23797, 19836, 50700, 55173, 58654, 62615, 32808, 37281, 41786, 45747,
    19012, 23501, 26966, 30943,  3168,  7657, 12146, 16123, 54925, 50948,
    62879, 58390, 37033, 33056, 46011, 41522, 23237, 19276, 31191, 26718,
    7393,  3432, 16371, 11898, 59150, 63111, 50204, 54677, 41258, 45219,
    33336, 37809, 27462, 31439, 18516, 23005, 11618, 15595,  3696,  8185,
    63375, 58886, 54429, 50452, 45483, 40994, 37561, 33584, 31687, 27214,
    22741, 18780, 15843, 11370,  7921,  3960
};

static uint16_t crc16byte(const unsigned char byte, uint16_t crc)
{
    return crc16tab[(crc & 0xFF) ^ byte] ^ (crc >> 8);
}

static uint16_t crc16(const unsigned char *data, int data_len, uint16_t crc)
{
    for (int i = 0; i < data_len; i++)
    {
        crc = crc16byte(data[i], crc);
    }

    return crc;
}

static int esc_append(unsigned char frame[], int idx, const unsigned char a)
{
    switch (a)
    {
    case CR_SOF:
    case CR_EOF:
    case CR_ESC:
        frame[idx] = CR_ESC;
        frame[idx + 1] = a ^ 20;
        return idx + 2;
        break;

    default:
        frame[idx] = a;
        return idx + 1;
    }
}

/*
 * frame[] might be 2x the size of data and crc +3 bytes if it is just a long
 * string of 0xFC, 0xFD, or 0xFE for some insane reason.
 *
 */
int frame_message(unsigned char frame[], const unsigned char *data,
                  const int data_len)
{
    uint16_t crc = 0;
    frame[0] = CR_SOF;
    frame[1] = 0x21; /* Messages to the radio are always 0x21 */
    crc = crc16byte(frame[1], crc);
    frame[2] = data[0];
    crc = crc16byte(frame[2], crc);
    int frame_len = 3;

    for (int i = 1; i < data_len; i++)
    {
        crc = crc16byte(data[i], crc);
        frame_len = esc_append(frame, frame_len, data[i]);
    }

    frame_len = esc_append(frame, frame_len, crc >> 8);
    frame_len = esc_append(frame, frame_len, crc & 0xFF);
    frame[frame_len] = CR_EOF;
    frame_len++;
    return frame_len;
}

int commradio_unpack_frame(unsigned char msg[], const unsigned char *frame,
                           const int frame_len)
{
    if (frame_len < 5)
    {
        rig_debug(RIG_DEBUG_ERR,
                  "%s Got a frame that was too small (<5) to be valid\n",
                  __func__);
        return -RIG_ETRUNC;
    }

    if ((frame[0] != CR_SOF) || (frame[frame_len - 1] != CR_EOF))
    {
        rig_debug(RIG_DEBUG_ERR,
                  "%s Tried to unpack a frame without start or end\n", __func__);
        return -RIG_EPROTO;
    }

    if (frame[1] != 0x11)
    {
        rig_debug(RIG_DEBUG_ERR,
                  "%s Message address is not for host (0x11)\n", __func__);
        return -RIG_EPROTO;
    }

    int msg_len = 0;

    for (int i = 2; i < frame_len; i++)
    {
        switch (frame[i])
        {
        case CR_SOF:
            return -RIG_EPROTO;
            break;

        case CR_EOF:
            i = frame_len;
            break;

        case CR_ESC:
            i++;
            msg[msg_len] = frame[i] ^ 20;
            msg_len++;
            break;

        default:
            msg[msg_len] = frame[i];
            msg_len++;
        }
    }

    uint16_t msg_crc = (msg[msg_len - 2] << 8) | msg[msg_len - 1];
    msg_len = msg_len - 2;
    uint16_t crc = crc16(msg, msg_len, crc16byte(frame[1], 0));

    if (msg_crc != crc)
    {
        rig_debug(RIG_DEBUG_ERR, "%s CRC check failed. msg_crc=%x, crc=%x\n", __func__,
                  msg_crc, crc);
    }

    return msg_len;
}