meshcore-wch/User/meshcore/meshframing.c

#include "meshframing.h"
#include "lib/config.h"
#include "string.h"
#include "stdio.h"
#include "sx1262.h"
#include "util/hexdump.h"

FrameStruct decodeFrame (unsigned char *data, unsigned char dataLen) {
    hexdump ("RxDump", data, dataLen);
    FrameStruct frame;
    memset (&frame, 0, sizeof (frame));
    unsigned char index = 0;
    frame.header = data[index++];
    if ((frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
        (frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
        memcpy (frame.transportCodes, data + index, 4);
        index += 4;
    }
    frame.path.pathLen = data[index++];

    memcpy (frame.path.path, data + index, frame.path.pathLen);
    index += frame.path.pathLen;
    frame.payloadLen = dataLen - index;
    memcpy (frame.payload, data + index, frame.payloadLen);

    return frame;
}

void printFrameHeader (FrameStruct frame) {
    switch (frame.header & ROUTE_TYPE_MASK) {
    case ROUTE_TYPE_TRANSPORT_FLOOD:
        printf ("transport flood");
        break;

    case ROUTE_TYPE_FLOOD:
        printf ("flood");
        break;

    case ROUTE_TYPE_DIRECT:
        printf ("direct");
        break;

    case ROUTE_TYPE_TRANSPORT_DIRECT:
        printf ("transport direct");
        break;
    }

    printf (", payload type is ");

    switch (frame.header & PAYLOAD_TYPE_MASK) {
    case PAYLOAD_TYPE_REQ:
        printf ("request");
        break;

    case PAYLOAD_TYPE_RESPONSE:
        printf ("response");
        break;

    case PAYLOAD_TYPE_TXT_MSG:
        printf ("text message");
        break;

    case PAYLOAD_TYPE_ACK:
        printf ("acknowledgement");
        break;

    case PAYLOAD_TYPE_ADVERT:
        printf ("advert");
        break;

    case PAYLOAD_TYPE_GRP_TXT:
        printf ("group text");
        break;

    case PAYLOAD_TYPE_GRP_DATA:
        printf ("group data");
        break;

    case PAYLOAD_TYPE_ANON_REQ:
        printf ("anon request");
        break;

    case PAYLOAD_TYPE_PATH:
        printf ("path");
        break;

    case PAYLOAD_TYPE_TRACE:
        printf ("trace");
        break;

    case PAYLOAD_TYPE_MULTIPART:
        printf ("multipart");
        break;

    case PAYLOAD_TYPE_CONTROL:
        printf ("control");
        break;

    case PAYLOAD_TYPE_RAW_CUSTOM:
        printf ("raw");
        break;
    }
    char version[2];
    version[0] = (frame.header >> 6) + '0';
    version[1] = 0;

    printf (", payload version is %s ", version);

    if ((frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
        (frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
        printf ("Transport codes: %d %d\n", *((uint16_t *)frame.transportCodes),
                *((uint16_t *)&(frame.transportCodes[2])));
    }
    printf ("Path is %d nodes long", frame.path.pathLen);

    for (uint8_t pathIndex = 0; pathIndex < frame.path.pathLen; pathIndex++) {
        printf ("node %d - %02X, ", pathIndex, frame.path.path[pathIndex]);
    }
    putchar ('\n');
}

void sendFrame (FrameStruct frame) {
    uint8_t txBuf[256];
    size_t offset = 0;

    txBuf[offset++] = frame.header;

    if ((frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
        (frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
        memcpy (txBuf + offset, frame.transportCodes, 4);
        offset += 4;
    }

    if (frame.path.pathLen > 64) {
        frame.path.pathLen = 64;
    }

    txBuf[offset++] = frame.path.pathLen;

    memcpy (txBuf + offset, frame.path.path, frame.path.pathLen);
    offset += frame.path.pathLen;

    uint16_t maxPayloadLen = 256 - offset;

    uint16_t payloadLen = frame.payloadLen > maxPayloadLen ? maxPayloadLen : frame.payloadLen;

    memcpy (txBuf + offset, frame.payload, payloadLen);
    offset += payloadLen;

    hexdump ("TxDump", txBuf, offset);
    LoRaSend (txBuf, offset, SX126x_TXMODE_SYNC);
}

void retransmitFrame (FrameStruct frame) {
    if (frame.header & ROUTE_TYPE_FLOOD || frame.header & ROUTE_TYPE_TRANSPORT_FLOOD) {
        if (frame.header != DONT_RETRANSMIT_HEADER && frame.path.pathLen + 1 < MAX_FLOOD_TTL) {
            frame.path.path[frame.path.pathLen++] = persistent.pubkey[0];
        }
    }

    if (frame.header & ROUTE_TYPE_DIRECT || frame.header & ROUTE_TYPE_TRANSPORT_DIRECT) {
    }
}


// Verify MAC + Decrypt

int encrypt_then_mac (const uint8_t *aes_key, const uint8_t keySize, const uint8_t *plaintext, size_t plen, uint8_t *output, size_t *olen) {
    if (plen == 0)
        return -1;

    size_t padded_len = ((plen + 15) / 16) * 16;

    // prepare padded buffer
    uint8_t padded[padded_len];
    memset(padded, 0, padded_len);       // zero padding
    memcpy(padded, plaintext, plen);     // copy plaintext

    // ciphertext will go right after HMAC
    uint8_t *ciphertext = output + HMAC_SIZE;

    // encrypt plaintext
    aes_encrypt_ecb (aes_key, 16, padded, padded_len, ciphertext);

    // compute HMAC over ciphertext
    uint8_t mac[32];  // full SHA-256
    hmac_sha256 (aes_key, keySize, ciphertext, padded_len, mac);

    // copy only HMAC_SIZE bytes of MAC
    memcpy (output, mac, HMAC_SIZE);

    // return total length = HMAC + ciphertext
    *olen = HMAC_SIZE + padded_len;
    return 0;
}

int mac_then_decrypt (const uint8_t *aes_key, const uint8_t keySize, const uint8_t *input, size_t ilen, uint8_t *plaintext) {
    if (ilen <= HMAC_SIZE)
        return -1;

    const uint8_t *mac = input;
    const uint8_t *ciphertext = input + HMAC_SIZE;
    size_t clen = ilen - HMAC_SIZE;

    if (clen % 16 != 0) return -2; // must be multiple of block size

    uint8_t calc_mac[32];  // full SHA-256
    hmac_sha256 (aes_key, keySize, ciphertext, clen, calc_mac);

    if (memcmp (mac, calc_mac, HMAC_SIZE) != 0)
        return -2;

    return aes_decrypt_ecb (aes_key, 16, ciphertext, clen, plaintext);
}