EtherLib/packet_sieve.c

//
// Created by epagris on 2022.11.06..
//

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

bool packfiltcond_cmp(const PcktSieveFilterCondition *c1, const PcktSieveFilterCondition *c2) {
    return !memcmp(c1, c2, sizeof(PcktSieveFilterCondition));
}

void packfiltcond_zero(PcktSieveFilterCondition *cond) {
    memset(cond, 0, sizeof(PcktSieveFilterCondition));
}

PcktSieve* packsieve_new(EthInterface *intf) {
    PcktSieve *sieve = (PcktSieve*) dynmem_alloc(sizeof(PcktSieve));
    ASSERT_NULL(sieve);
    memset(&sieve->layer0, 0, sizeof(PcktSieveLayer)); // clear layer0 data
    sieve->intf = intf;
    return sieve;
}

void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) {
    // extract fields
    uint8_t *data = rawPckt->payload;
    uint32_t size = rawPckt->size;
    bool mrd = true; // 'Must Release Data': at the end of processing not only release headers but data buffer as well

    restart: ;
    // process payload, fetch packet class etc.
    uint16_t ownClass = 0, containerClass = 0; // Ethernet...
    uint16_t offset = 0;
    PcktHeaderElement *lastHeader = NULL, *outermostHeader = NULL;
    int procRet;

    do {
        // get packet descriptor
        PcktClassDesc *cdesc = packreg_get_by_class(E.pcktReg, ownClass, containerClass);
        if (cdesc == NULL) {
            break;
        }

        // allocate property object
        uint32_t hdrSize = ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize;
        // look for possible former allocated cache area
        PcktHeaderElement *header = NULL;
        if (cdesc->cacheSize < hdrSize) { // allocate cache area if previously allocated area is too small
            dynmem_free(cdesc->cacheArea); // does nothing if cacheArea is NULL
            cdesc->cacheArea = (uint8_t*) dynmem_alloc(hdrSize);
            ASSERT_NULL(cdesc->cacheArea);
            cdesc->cacheSize = hdrSize; // retain cache size
        }

        header = (PcktHeaderElement*) cdesc->cacheArea;

        memset(header, 0, hdrSize);
        header->props.ownPacketClass = ownClass;
        header->props.propSize = cdesc->propertySize;
        header->prev = lastHeader;
        if (lastHeader) {
            lastHeader->next = header;
        }
        if (outermostHeader == NULL) {
            outermostHeader = header;
        }

        // call parsing function
        PcktProcFnPassbackData pb;
        procRet = cdesc->procFun(data + offset, size - offset, header, sieve->intf, &pb);
        switch (procRet) {
        case PROC_FN_RET_REPRST:
            dynmem_free(data); // release previous packet data
            data = pb.p; // store new packet data
            size = pb.u;
            mrd = pb.b;
            // NO BREAK!
        case PROC_FN_RET_ABORT:
            goto header_release;
            // GOTO :D!
            break;
        case PROC_FN_RET_OK:
            default:
            break;
        }

        uint16_t containedClass = header->props.containedPacketClass;
        if (containedClass != 0) {
            containerClass = ownClass;
            //dynmem_free(props);
        }
        offset += header->props.headerSize;
        header->props.accumulatedOffset = offset;
        header->props.bytesToEnd = size - header->props.accumulatedOffset;
        header->props.hdrInsFn = cdesc->hdrInsFn;

        ownClass = containedClass;
        lastHeader = header;
    } while ((ownClass != 0) && lastHeader->props.validityOK);

    if (lastHeader == NULL) { // if found nothing, don't attempt to process
        goto data_release;
    }

    lastHeader->next = NULL;

    // ------------------------------------

    if (!lastHeader->props.validityOK) { // if packet is not valid, then drop
        goto header_release;
        // GOTO here!
    }

    Pckt packet;
    packet.time_s = rawPckt->ext.rx.time_s;
    packet.time_ns = rawPckt->ext.rx.time_ns;

    // lookup headers in the sieve
    const PcktHeaderElement *headerIter = outermostHeader;
    PcktSieveLayer *layer = &sieve->layer0; // innermost matched sieve layer
    bool found = true; // first structure is always an Ethernet-frame
    while (found && headerIter) {
        const PcktSieveLayer *nodeIter = layer->nodes;
        found = false;
        while (nodeIter && !found) {
            found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props, sieve->intf); // specific or general match
            if (found) {
                layer = nodeIter; // advance in the sieve tree
                PcktHeaderElement *containedHeader = headerIter;
                if (headerIter->next != NULL) {
                    containedHeader = containedHeader->next; // advance on headers
                }
                if (layer->cbFn != NULL) { // if defined, invoke layer callback function
                    offset = containedHeader->props.accumulatedOffset; // accumulated offset + own header size
                    packet.header = containedHeader;
                    packet.payload = data + offset;
                    packet.headerSize = offset;
                    packet.payloadSize = size - offset;
                    int action = layer->cbFn(&packet, layer->tag);

                    // execute special return action
                    switch (action) {
                    case SIEVE_LAYER_REMOVE_THIS:
                        packsieve_remove_layer(layer);
                        break;
                    default:
                        break;
                    }
                }
                headerIter = containedHeader;
            } else {
                nodeIter = nodeIter->next; // advance on linked list and continue search
            }
        }
    }

    // if there are no more sieve layers (cannot process headers further) BUT
    // meaningful headers have been left unprocessed
//    if (layer->next == NULL && headerIter) {
//        INFO("Packet headers not fully processed!\n");
//    }

    // release header chain blocks
    header_release: ; // empty line, solely for label placement
//    PcktHeaderElement *iter = outermostHeader;
//    while (iter != NULL) {
//        PcktHeaderElement *next = iter->next;
//        dynmem_free(iter);
//        iter = next;
//    }
    if (procRet == PROC_FN_RET_REPRST) { // if a restart was requested, then run everything again!
        goto restart;
    }
    data_release: ;
    if (mrd) {
        dynmem_free(data);
    }
}

PcktSieveLayer* packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag,
        uint16_t pcktClass) {
    // search for matching layer
    PcktSieveLayer *nodeIter = parent->nodes;
    bool alreadyExists = false;
    while (nodeIter != NULL && !alreadyExists) {
        if ((packfiltcond_cmp(&nodeIter->filtCond, filtCond) || (nodeIter->matchAny && matchAny)) && (nodeIter->filtFn == filtFn)) { // if matching... [search for specific match OR any match]
            alreadyExists = true;
        } else {
            nodeIter = nodeIter->next;
        }
    }

    // if found, then return with the pointer to the existing layer
    if (alreadyExists) {
        return nodeIter; // OK
    } else { // if allocation of a new layer is required
        PcktSieveLayer *layer = (PcktSieveLayer*) dynmem_alloc(sizeof(PcktSieveLayer));
        ASSERT_NULL(layer);

        PcktSieveLayer *oldListFirst = parent->nodes;
        layer->packetClass = pcktClass;
        layer->parent = parent;

        if (!matchAny || (oldListFirst == NULL)) { // for specific match or on first node insertion...
            layer->prev = NULL;
            parent->nodes = layer; // ...replace first element (it's the fastest way of new element insertion, since element position does not carry any meaning in general case)
            layer->next = oldListFirst;
            if (oldListFirst != NULL) {
                layer->prev = layer;
            }
        } else { // for 'any' match if at least a single node is already present
            PcktSieveLayer *iter = parent->nodes;
            while (iter->next != NULL) { // find the last node
                iter = iter->next;
            }
            iter->next = layer;
            layer->prev = iter;
            layer->next = NULL;
        }

        layer->nodes = NULL;
        layer->filtCond = *filtCond;
        layer->matchAny = matchAny;
        layer->filtFn = filtFn;
        layer->cbFn = cbFn;
        layer->tag = tag;
        layer->connBReportFn = NULL;
        layer->txTsCb = NULL;
        return layer;
    }
}

bool packsieve_remove_layer(PcktSieveLayer *layer) {
    // avoid NULL-operations
    if (layer == NULL) {
        return true;
    }

    // remove parent elements if their only subnode is the one we're deleting
    PcktSieveLayer *parent;
    while (layer != NULL && layer->nodes == NULL) {
        parent = layer->parent; // store parent

        // chain out our layer
        if (layer->next == NULL && layer->prev == NULL) { // we are the only subnode
            parent->nodes = NULL;
        } else { // there are multiple subnodes, just chain us out
            if (layer->next != NULL) {
                layer->next->prev = layer->prev;
            }
            if (layer->prev != NULL) {
                layer->prev->next = layer->next;
            }
            if (parent->nodes == layer) {
                parent->nodes = layer->next;
            }
        }

        dynmem_free(layer); // deallocate layer
        layer = parent; // advance in the tree
    }

    return layer == NULL;
}

#define ETH_SIEVE_LAYER_INDENT_PER_LEVEL (4)

void packsieve_report(const PcktSieve *sieve, const PcktSieveLayer *layer, uint32_t indent) {
    if (layer->connBReportFn != NULL) {
        INFO("%*c└─┤", indent, ' ');
        ConnBlock connBlock = { sieve, layer }; // tag not required
        layer->connBReportFn(&connBlock);
        INFO("├───\n");
    } else {
        INFO("%*c└─┤%d├───\n", indent, ' ', layer->packetClass);
    }
    const PcktSieveLayer *nodeIter = layer->nodes;
    while (nodeIter) {
        packsieve_report(sieve, nodeIter, indent + ETH_SIEVE_LAYER_INDENT_PER_LEVEL);
        nodeIter = nodeIter->next;
    }
}

void packsieve_layer_info(const PcktSieve *sieve, const PcktSieveLayer *layer) {
    const PcktSieveLayer *iter = layer;
    while (iter != NULL) { // climb up to the top in the sieve tree
        if (iter->connBReportFn != NULL) {
            ConnBlock connBlock = { sieve, iter }; // tag not required
            iter->connBReportFn(&connBlock);
        } else {
            INFO("[%d]", layer->packetClass);
        }
        if (iter->parent != NULL) {
            INFO(", ");
        }
        iter = iter->parent;
    }
}

void pckthdr_chain_free(PcktHeaderElement *hdr) {
    // rewind
    while (hdr->prev != NULL) {
        hdr = hdr->prev;
    }

    // free
    PcktHeaderElement *next;
    while (hdr != NULL) {
        next = hdr->next;
        dynmem_free(hdr);
        hdr = next;
    }
}