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() {
    PcktSieve *sieve = (PcktSieve *) dynmem_alloc(sizeof(PcktSieve));
    ASSERT_NULL(sieve);
    memset(&sieve->layer0, 0, sizeof(PcktSieveLayer)); // clear layer0 data
    return sieve;
}

void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthInterface_ *intf) {
    // extract fields
    uint8_t * data = rawPckt->payload;
    uint32_t size = rawPckt->size;

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

    do {
        // get packet descriptor
        const 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;
        PcktHeaderElement *header = (PcktHeaderElement *) dynmem_alloc(hdrSize);
        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
        cdesc->procFun(data + offset, size - offset, header, intf);
        uint16_t containedClass = header->props.containedPacketClass;
        if (containedClass != 0) {
            containerClass = ownClass;
            //dynmem_free(props);
        }
        offset += header->props.headerSize;
        header->props.accumulatedOffset = offset;

        ownClass = containedClass;
        lastHeader = header;
    } while (ownClass != 0);

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

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

    // lookup headers in the sieve
    const PcktHeaderElement * headerIter = outermostHeader;
    const 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, intf); // specific or general match
            if (found) {
                layer = nodeIter; // advance in the sieve tree
                const PcktHeaderElement * containedHeader = headerIter->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;
                    layer->cbFn(&packet, layer->tag);
                }
                headerIter = containedHeader;
            } else {
                nodeIter = nodeIter->next; // advance on linked list and countinue 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
    PcktHeaderElement * iter = outermostHeader;
    while (iter != NULL) {
        PcktHeaderElement * next = iter->next;
        dynmem_free(iter);
        iter = next;
    }
}

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->infoTag[0] = '\0';
        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 PcktSieveLayer *layer, uint32_t indent) {
    if (*layer->infoTag != '\0') {
        INFO("%*c└─┤%s├───\n", indent, ' ', layer->infoTag);
    } else {
        INFO("%*c└─┤%d├───\n", indent, ' ', layer->packetClass);
    }
    const PcktSieveLayer * nodeIter = layer->nodes;
    while(nodeIter) {
        packsieve_report(nodeIter, indent + ETH_SIEVE_LAYER_INDENT_PER_LEVEL);
        nodeIter = nodeIter->next;
    }
}

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;
    }
}