EtherLib/pckt_assembler.c

//
// Created by epagris on 2023.01.11..
//

#include <stddef.h>
#include "pckt_assembler.h"
#include "packet_sieve.h"
#include "dynmem.h"
#include "eth_interface.h"
#include "global_state.h"
#include "utils.h"

int pckt_assemble(RawPckt *raw, Pckt *cooked, EthInterface *intf) {
    // calculate frame size
    uint16_t frameSize = 0;
    uint16_t headerSize = 0;
    PcktHeaderElement *hdrIter = cooked->header;

    // rewind headers
    while (hdrIter->prev != NULL) {
        hdrIter = hdrIter->prev;
    }

    PcktHeaderElement *lastHdr = hdrIter;
    while (hdrIter != NULL) {
        headerSize += hdrIter->props.headerSize;
        lastHdr = hdrIter;
        hdrIter = hdrIter->next;
    }
    frameSize = headerSize + cooked->payloadSize;// + 4; // header + payload
    bool computeCRC = (!(intf->capabilities & ETHINF_CAP_TX_CRC_OFFLOAD)) || raw->opts.calculate_ethernet_CRC;
    if (computeCRC) { // + CRC32 checksum area if needed
        frameSize += 4;
    }

    // calculate padding size
    uint16_t padding = (frameSize < ETH_FRAME_MIN_SIZE) ? (ETH_FRAME_MIN_SIZE - frameSize) : 0;

    // grow frame to minimum length if needed
    frameSize = (frameSize < ETH_FRAME_MIN_SIZE) ? ETH_FRAME_MIN_SIZE : frameSize;

    // allocate raw packet data
    raw->payload = dynmem_alloc(frameSize);
    raw->size = frameSize;

    // insert pointer
    uint8_t * payloadInsPtr = raw->payload + headerSize;
    uint8_t * headerInsPtr = payloadInsPtr - lastHdr->props.headerSize;

    // insert payload
    if (cooked->payloadSize > 0 && cooked->payload != NULL) {
        memcpy(payloadInsPtr, cooked->payload, cooked->payloadSize);
    }

    // insert zero padding
    memset(payloadInsPtr + cooked->payloadSize, 0, padding);

    // insert reversely headers (e.g.: UDP -> IPv4 -> Ethernet)
    hdrIter = lastHdr;
    if (lastHdr != NULL) {
        lastHdr->props.bytesToEnd = cooked->payloadSize;
    }

    while (hdrIter != NULL) {
        hdrIter->props.hdrInsFn(headerInsPtr, hdrIter, intf); // insert header
        hdrIter = hdrIter->prev; // step to previous header
        if (hdrIter != NULL) {
            headerInsPtr -= hdrIter->props.headerSize; // advance data pointer

            // store bytes left until the packet end
            PcktHeaderElement * next = hdrIter->next;
            hdrIter->props.bytesToEnd = next->props.bytesToEnd + next->props.headerSize;
        }
    }

    // insert CRC32 if interface cannot handle CRC calculation or if explicitly requested
    if (computeCRC) {
        uint32_t crc = crc32(raw->payload, frameSize - 4);
        memcpy(raw->payload + frameSize - 4, &crc, 4);
    }

    // turn off TX timestamping by default
    raw->ext.tx.txTsCb = NULL;

    return 0;
}