121 lines
4.3 KiB
C
121 lines
4.3 KiB
C
#include "icmp_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 "../packet_parsers/icmp_packet.h"
|
|
#include "ipv4_connblock.h"
|
|
|
|
static bool filtIcmp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
|
|
IPv4Props * ipProps = (IPv4Props *) contProps;
|
|
IcmpProps * icmpProps = (IcmpProps *) ownProps;
|
|
|
|
return ipProps->Protocol == ETH_ICMP_PACKET_CLASS;
|
|
}
|
|
|
|
static int icmp_recv_cb(const Pckt * pckt, PcktSieveLayerTag tag) {
|
|
IcmpProps * icmpProps = HEADER_FETCH_PROPS(IcmpProps, pckt->header);
|
|
EthInterface * intf = (EthInterface *) tag.p; // icmp_new_connblock() puts pointer to intf into tag field
|
|
|
|
static PcktHeaderElement * txIcmpHdr = NULL;
|
|
static PcktHeaderElement * txIpHdr = NULL;
|
|
static PcktHeaderElement * txEthHdr = NULL;
|
|
|
|
static IcmpProps * txIcmpProps = NULL;
|
|
static IPv4Props * txIpProps = NULL;
|
|
static EthernetProps * txEthProps = NULL;
|
|
|
|
if (txIcmpHdr == NULL) {
|
|
txIcmpHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(IcmpProps));
|
|
txIcmpHdr->next = NULL;
|
|
txIcmpProps = HEADER_FETCH_PROPS(IcmpProps, txIcmpHdr);
|
|
}
|
|
if (txIpHdr == NULL) {
|
|
txIpHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(IPv4Props));
|
|
txIcmpHdr->prev = txIpHdr;
|
|
txIpHdr->next = txIcmpHdr;
|
|
txIpProps = HEADER_FETCH_PROPS(IPv4Props, txIpHdr);
|
|
}
|
|
if (txEthHdr == NULL) {
|
|
txEthHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(EthernetProps));
|
|
txIpHdr->prev = txEthHdr;
|
|
txEthHdr->next = txIpHdr;
|
|
txEthHdr->prev = NULL;
|
|
txEthProps = HEADER_FETCH_PROPS(EthernetProps , txEthHdr);
|
|
}
|
|
|
|
switch (icmpProps->type) {
|
|
case ICMP_MT_ECHO_REQUEST: {
|
|
*txIcmpProps = *icmpProps;
|
|
txIcmpProps->type = ICMP_MT_ECHO_REPLY; // replace request with reply
|
|
txIcmpProps->hdrInsFn = insert_icmp_header;
|
|
txIcmpProps->checksum = 0;
|
|
|
|
// swap source an destination IP addresses
|
|
IPv4Props * iPv4Props = HEADER_FETCH_PROPS(IPv4Props, pckt->header->prev);
|
|
*txIpProps = *iPv4Props;
|
|
ip4_addr tmpIp = txIpProps->SourceIPAddr;
|
|
txIpProps->SourceIPAddr = txIpProps->DestIPAddr;
|
|
txIpProps->DestIPAddr = tmpIp;
|
|
txIpProps->hdrInsFn = insert_ipv4_header;
|
|
|
|
// swap Ethernet fields
|
|
EthernetAddress tmpEth;
|
|
EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, pckt->header->prev->prev);
|
|
*txEthProps = *ethProps;
|
|
HWACPY(tmpEth, txEthProps->sourceAddr);
|
|
HWACPY(txEthProps->sourceAddr, txEthProps->destAddr);
|
|
HWACPY(txEthProps->destAddr, tmpEth);
|
|
txEthProps->hdrInsFn = insert_ethernet_header;
|
|
|
|
// payload is the same...
|
|
Pckt reply;
|
|
reply.header = txEthHdr;
|
|
reply.payload = pckt->payload;
|
|
reply.payloadSize = pckt->payloadSize;
|
|
reply.headerSize = pckt->headerSize;
|
|
|
|
// assemble packet
|
|
RawPckt raw;
|
|
pckt_assemble(&raw, &reply, intf);
|
|
|
|
// release headers
|
|
//pckthdr_chain_free(pckt->header);
|
|
|
|
ethinf_transmit(intf, &raw);
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
ConnBlock icmp_new_connblock(EthInterface * intf) {
|
|
ConnBlock icmpConnB;
|
|
connb_init_defaults(&icmpConnB);
|
|
|
|
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'
|
|
icmpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, false, filtIcmp, icmp_recv_cb, tag, ETH_ICMP_PACKET_CLASS);
|
|
ASSERT_NULL(icmpConnB.sieveLayer);
|
|
|
|
icmpConnB.sieve = &intf->sieve;
|
|
icmpConnB.sieveLayer->connBReportFn = icmp_print_report;
|
|
|
|
return icmpConnB;
|
|
}
|
|
|
|
void icmp_print_report(const ConnBlock *connBlock) {
|
|
INFO("ICMP (ping)");
|
|
}
|