EtherLib/prefab/conn_blocks/igmp_connblock.c

#include "igmp_connblock.h"

#include <stddef.h>

#include "../packet_parsers/packet_parsers.h"
#include "../../utils.h"
#include "../../dynmem.h"
#include "../../pckt_assembler.h"
#include "../../eth_interface.h"
#include "etherlib/prefab/packet_parsers/ipv4_packet.h"
#include "ipv4_connblock.h"
#include "../packet_parsers/igmp_packet.h"

static bool filtIgmp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
    IPv4Props * ipProps = (IPv4Props *) contProps;
    IgmpProps * icmpProps = (IgmpProps *) ownProps;

    return ipProps->Protocol == ETH_IGMP_PACKET_CLASS;
}

ConnBlock igmp_new_connblock(struct EthInterface_ *intf) {
    ConnBlock igmpConnB;
    connb_init_defaults(&igmpConnB);

    ConnBlock ipConnB = ipv4_new_connblock(intf, IPv4_IF_ADDR, NULL); // create new IPv4 connection block

    PcktSieveFilterCondition filtCond;
    packfiltcond_zero(&filtCond);
    PcktSieveLayerTag tag;
    tag.p = intf; // Ethernet-interface passed in 'tag.p'
    igmpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, false, filtIgmp, NULL, tag, ETH_IGMP_PACKET_CLASS);
    ASSERT_NULL(igmpConnB.sieveLayer);

    igmpConnB.sieve = &intf->sieve;
    igmpConnB.sieveLayer->connBReportFn = igmp_print_report;

    return igmpConnB;
}

static void igmp_send(ConnBlock * connBlock, ip4_addr ga, int mt) {
    // fetch interface
    EthInterface * intf = connBlock->sieve->intf;

    // allocate headers
    PcktHeaderElement * igmpHeader = ALLOC_HEADER_ELEMENT(IgmpProps);
    PcktHeaderElement * ipHeader = ALLOC_HEADER_ELEMENT(IPv4Props);
    PcktHeaderElement * ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps);

    igmpHeader->next = NULL;
    igmpHeader->prev = ipHeader;
    ipHeader->next = igmpHeader;
    ipHeader->prev = ethHeader;
    ethHeader->next = ipHeader;
    ethHeader->prev = NULL;

    // prepare headers
    IgmpProps * igmpProps = HEADER_FETCH_PROPS(IgmpProps, igmpHeader);
    IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, ipHeader);
    EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, ethHeader);

    // fetch sieve layers and fill transmit headers
    PcktSieveLayer * layer = connBlock->sieveLayer; // UDP layer
    igmpProps->type = mt;
    igmpProps->maxRespTime = 0;
    igmpProps->checksum = 0;
    igmpProps->groupAddr = ga;

    // common fields for packet assembly
    igmpProps->headerSize = ETH_IGMP_HEADER_SIZE;
    igmpProps->hdrInsFn = insert_igmp_header;

    // IP
    layer = layer->parent;
    ipv4_fill_props(ipProps, ETH_IGMP_HEADER_SIZE, ETH_IGMP_PACKET_CLASS, intf->ip, ga);

    // Ethernet
    ethmc_from_ipmc(ethProps->destAddr, ga);
    memcpy(ethProps->sourceAddr, intf->mac, ETH_HW_ADDR_LEN);
    ethProps->length_type = ETH_IPv4_PACKET_CLASS;

    // common fields for packet assembly
    ethProps->hdrInsFn = insert_ethernet_header;
    ethProps->headerSize = ETH_ETHERNET_HEADER_SIZE;

    // allocate transmit buffer
    uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE + ETH_IGMP_HEADER_SIZE;
    uint32_t txBufSize = MAX(txHeaderSize + 4, ETH_FRAME_MIN_SIZE);
    uint8_t *txBuf = dynmem_alloc(txBufSize);
    memset(txBuf, 0, txBufSize);

    // insert Ethernet header
    insert_ethernet_header(txBuf, ethHeader, NULL);

    // insert IPv4 header
    insert_ipv4_header(txBuf + ETH_ETHERNET_HEADER_SIZE, ipHeader, intf);
    ipv4_fill_in_chksum(txBuf + ETH_ETHERNET_HEADER_SIZE); // fill-in IPv4 checksum

    // insert IGMP header
    insert_igmp_header(txBuf + ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE, igmpHeader, intf);

    // release headers
    dynmem_free(ethHeader);
    dynmem_free(ipHeader);
    dynmem_free(igmpHeader);

    // calculate CRC
    uint32_t crc = crc32(txBuf, txBufSize - 4);
    memcpy(txBuf + txBufSize - 4, &crc, 4);

    // send packet
    RawPckt rpckt;
    memset(&rpckt, 0, sizeof(RawPckt));
    rpckt.size = txBufSize;
    rpckt.payload = txBuf;
    rpckt.ext.tx.txTsCb = NULL;
    ethinf_transmit(intf, &rpckt);
}

void igmp_report_membership(ConnBlock * connBlock, ip4_addr ga) {
    igmp_send(connBlock, ga, IGMP_MT_MEMBERSHIP_REPORT);
}

void igmp_leave_group(ConnBlock *connBlock, ip4_addr ga) {
    igmp_send(connBlock, ga, IGMP_MT_LEAVE_GROUP);
}

void igmp_print_report(const ConnBlock* connBlock) {
    INFO("IGMP");
}