header.c

// Copyright (c) 2023-2025, Ericsson AB and Ericsson Telecommunication Hungary
// All rights reserved.


#include "header.h"
#include "log.h"
#include "packet.h"
#include "protocol.h"
#include "utils.h"

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

DEFAULT_LOGGING_MODULE(HEADER, WARNING);

struct HeaderField *new_headerfield(unsigned header_idx, const struct ProtocolField *pfield)
{
    struct HeaderField *ret = calloc_struct(HeaderField);
    ret->header_idx = header_idx;
    ret->bitoffset = pfield->bitoffset;
    ret->bitcount = pfield->bitcount;
    return ret;
}

// only full bytes, no loose bits at the beginning or the end
static void write_bytes(void *state, struct Value *value, struct Packet *p)
{
    struct HeaderField *field = (struct HeaderField *)state;
    uint8_t *src = (uint8_t *)value->value + value->bitoffset/8;
    uint8_t *dst = p->buf + p->headers[field->header_idx].start + field->bitoffset/8;
    unsigned len = value->bitcount / 8;
    for (unsigned i=0; i<len; i++) dst[i] = src[i];
}

// set bits in a single byte
static void write_bits(void *state, struct Value *value, struct Packet *p)
{
    struct HeaderField *field = (struct HeaderField *)state;
    uint8_t *src = (uint8_t *)value->value + value->bitoffset/8;
    uint8_t *dst = p->buf + p->headers[field->header_idx].start + field->bitoffset/8;

    unsigned bitoffset = field->bitoffset % 8;
    unsigned bitcount = field->bitcount;
    unsigned shift = 8 - bitoffset - bitcount;
    unsigned char mask = 1 << bitcount; // we know bitcount < 8
    mask -= 1;
    mask <<= shift;
    dst[0] &= ~mask;
    dst[0] |= src[0] & mask;
}


// generic writer of any number of bits over any number of bytes with any starting offset
static void write_generic(void *state, struct Value *value, struct Packet *p)
{
    struct HeaderField *field = (struct HeaderField *)state;
    uint8_t *src = (uint8_t *)value->value + value->bitoffset/8;
    uint8_t *dst = p->buf + p->headers[field->header_idx].start + field->bitoffset/8;

    unsigned bitoffset = field->bitoffset % 8;
    unsigned bitcount = field->bitcount; // total bits to write
    unsigned bitcount1 = MIN(bitcount, 8 - bitoffset); // bits in first byte
    unsigned shift = 8 - bitoffset - bitcount1;
    unsigned char mask = 1 << bitcount1;
    mask -= 1;
    mask <<= shift;
    dst[0] &= ~mask;
    dst[0] |= src[0] & mask;
    unsigned remaining_bits = bitcount - bitcount1;
    unsigned remaining_bytes = remaining_bits / 8;
    unsigned byteoffset = 1;

    if (remaining_bytes) {
        for (unsigned i=0; i<remaining_bytes; i++) dst[1+i] = src[1+i];
        remaining_bits -= remaining_bytes * 8;
        byteoffset += remaining_bytes;
    }

    if (remaining_bits) {
        shift = 8 - remaining_bits;
        mask = 1 << remaining_bits;
        mask -= 1;
        mask <<= shift;
        dst[byteoffset] &= ~mask;
        dst[byteoffset] |= src[byteoffset] & mask;
    }
}

value_consumer *header_get_field_writer(const struct HeaderField *target, const struct Value *source)
{
    if (source->bitcount != target->bitcount) {
        log_error("field writer: source and target has different bit count %u %u",
                source->bitcount, target->bitcount);
        return NULL;
    }
    if ((source->bitoffset % 8) != (target->bitoffset % 8)) {
        log_error("field writer: source and target has different bit offset %u %u",
                (source->bitoffset % 8), (target->bitoffset % 8));
        return NULL;
    }

    // octet-based assignment
    if ((target->bitoffset % 8) == 0 && (target->bitcount % 8) == 0 &&
            (source->bitoffset % 8) == 0 && (source->bitcount % 8) == 0)
        return write_bytes;

    // some bits within a single byte
    if (target->bitoffset % 8 + target->bitcount <= 8)
        return write_bits;

    // anything else
    return write_generic;
}


// points to the first byte of the field
static void read_bytes(void *state, value_consumer *consumer, void *consumer_state, struct Packet *p)
{
    struct HeaderField *source = (struct HeaderField *)state;
    uint8_t *src = p->buf + p->headers[source->header_idx].start + source->bitoffset/8;
    struct Value val = {src, source->bitoffset%8, source->bitcount};
    consumer(consumer_state, &val, p);
}

value_producer *header_get_field_reader(const struct Value *target, const struct HeaderField *source)
{
    if (source->bitcount != target->bitcount) {
        log_error("field reader: source and target has different bit count %u %u",
                source->bitcount, target->bitcount);
        return NULL;
    }
    if ((source->bitoffset % 8) != (target->bitoffset % 8)) {
        log_error("field reader: source and target has different bit offset %u %u",
                (source->bitoffset % 8), (target->bitoffset % 8));
        return NULL;
    }

    // this should be good for all cases
    return read_bytes;
}


// return true if the header field in the packet equals @value
static bool compare_bytes(const void *state, const struct Value *value, const struct Packet *p)
{
    const struct HeaderField *field = (struct HeaderField *)state;
    uint8_t *match_data = (uint8_t *)value->value + value->bitoffset/8;
    uint8_t *hdr_data = p->buf + p->headers[field->header_idx].start + field->bitoffset/8;
    unsigned len = value->bitcount / 8;
    for (unsigned i=0; i<len; i++)
        if (hdr_data[i] != match_data[i]) return false;
    return true;
}

// return true if bits in a single byte at the header equal with the given @value
static bool compare_bits(const void *state, const struct Value *value, const struct Packet *p)
{
    const struct HeaderField *field = (struct HeaderField *)state;
    uint8_t *match_data = (uint8_t *)value->value + value->bitoffset/8;
    uint8_t *hdr_data = p->buf + p->headers[field->header_idx].start + field->bitoffset/8;

    unsigned bitoffset = field->bitoffset % 8;
    unsigned bitcount = field->bitcount;
    unsigned shift = 8 - bitoffset - bitcount;
    unsigned char mask = 1 << bitcount; // we know bitcount < 8
    mask -= 1;
    mask <<= shift;
    return (hdr_data[0] & mask) == (match_data[0] & mask);
}

// return true if the bit sequences (spanning multiple bytes) equal with the given @value
static bool compare_generic(const void *state, const struct Value *value, const struct Packet *p)
{
    const struct HeaderField *field = (struct HeaderField *)state;
    uint8_t *match_data = (uint8_t *)value->value + value->bitoffset/8;
    uint8_t *hdr_data = p->buf + p->headers[field->header_idx].start + field->bitoffset/8;

    unsigned bitoffset = field->bitoffset % 8;
    unsigned bitcount = field->bitcount; // total bits to compare
    unsigned bitcount1 = MIN(bitcount, 8 - bitoffset); // bits in first byte
    unsigned shift = 8 - bitoffset - bitcount1;
    unsigned char mask = 1 << bitcount1;
    mask -= 1;
    mask <<= shift;
    if ((hdr_data[0] & mask) != (match_data[0] & mask))
        return false;

    unsigned remaining_bits = bitcount - bitcount1;
    unsigned remaining_bytes = remaining_bits / 8;
    unsigned byteoffset = 1;

    if (remaining_bytes) {
        for (unsigned i=0; i<remaining_bytes; i++)
            if (hdr_data[1+i] != match_data[1+i]) return false;
        remaining_bits -= remaining_bytes * 8;
        byteoffset += remaining_bytes;
    }

    if (remaining_bits) {
        shift = 8 - remaining_bits;
        mask = 1 << remaining_bits;
        mask -= 1;
        mask <<= shift;
        if ((hdr_data[byteoffset] & mask) != (match_data[byteoffset] & mask))
            return false;
    }

    return true;
}

value_comparator *header_get_field_comprator(const struct HeaderField *target, const struct Value *match)
{
    if (match->bitcount != target->bitcount) {
        log_error("field writer: source and target has different bit count %u %u",
                match->bitcount, target->bitcount);
        return NULL;
    }
    if ((match->bitoffset % 8) != (target->bitoffset % 8)) {
        log_error("field writer: source and target has different bit offset %u %u",
                (match->bitoffset % 8), (target->bitoffset % 8));
        return NULL;
    }

    // octet-based comparison
    if ((target->bitoffset % 8) == 0 && (target->bitcount % 8) == 0 &&
            (match->bitoffset % 8) == 0 && (match->bitcount % 8) == 0)
        return compare_bytes;

    // compare some bits within a single byte
    if (target->bitoffset % 8 + target->bitcount <= 8)
        return compare_bits;

    // anything else
    return compare_generic;
}