Wiesner András
05288d7a3c
- IGMPv2 capabilities added (report membership, leave group) - ICMP capabilities added (ping) - Tx Message Queue added
128 lines
4.1 KiB
C
128 lines
4.1 KiB
C
//
|
|
// Created by epagris on 2022.11.07..
|
|
//
|
|
|
|
#include "udp_connblock.h"
|
|
|
|
#include <stddef.h>
|
|
|
|
#include "../packet_parsers/packet_parsers.h"
|
|
#include "../../utils.h"
|
|
#include "../../dynmem.h"
|
|
#include "etherlib/pckt_assembler.h"
|
|
|
|
static bool filtUdp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
|
|
IPv4Props * ipProps = (IPv4Props *) contProps;
|
|
UdpProps * udpProps = (UdpProps *) ownProps;
|
|
|
|
return ipProps->Protocol == ETH_UDP_PACKET_CLASS && (UDP_PORT_FROM_FILTCOND(filtCond) == udpProps->DestinationPort);
|
|
}
|
|
|
|
ConnBlock udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn) {
|
|
ConnBlock udpConnB;
|
|
ConnBlock ipConnB = ipv4_new_connblock(intf, ipAddr, NULL); // create new IPv4 connection block
|
|
|
|
PcktSieveFilterCondition filtCond;
|
|
packfiltcond_zero(&filtCond);
|
|
UDP_PORT_TO_FILTCOND(&filtCond, port);
|
|
PcktSieveLayerTag tag;
|
|
tag.u = 0;
|
|
udpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, false, filtUdp, cbFn, tag, ETH_UDP_PACKET_CLASS);
|
|
ASSERT_NULL(udpConnB.sieveLayer);
|
|
|
|
udpConnB.intf = intf;
|
|
|
|
SNPRINTF(udpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "UDP port: %d", port);
|
|
|
|
return udpConnB;
|
|
}
|
|
|
|
int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_t port) {
|
|
// allocate headers
|
|
PcktHeaderElement * udpHeader = ALLOC_HEADER_ELEMENT(UdpProps);
|
|
PcktHeaderElement * ipHeader = ALLOC_HEADER_ELEMENT(IPv4Props);
|
|
PcktHeaderElement * ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps);
|
|
udpHeader->next = NULL;
|
|
udpHeader->prev = ipHeader;
|
|
ipHeader->next = udpHeader;
|
|
ipHeader->prev = ethHeader;
|
|
ethHeader->next = ipHeader;
|
|
ethHeader->prev = NULL;
|
|
|
|
// prepare headers
|
|
UdpProps * udpProps = HEADER_FETCH_PROPS(UdpProps, udpHeader);
|
|
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
|
|
udpProps->SourcePort = UDP_PORT_FROM_FILTCOND(&layer->filtCond);
|
|
udpProps->DestinationPort = port;
|
|
udpProps->Length = ETH_UDP_HEADER_SIZE + size;
|
|
udpProps->Checksum = 0;
|
|
|
|
// common fields for packet assembly
|
|
udpProps->hdrInsFn = insert_udp_header;
|
|
udpProps->headerSize = ETH_UDP_HEADER_SIZE;
|
|
|
|
// IP
|
|
layer = layer->parent;
|
|
ipProps->IHL = ETH_IPv4_HEADER_SIZE / 4;
|
|
ipProps->Version = 4;
|
|
ipProps->DSF = 0x00;
|
|
ipProps->TotalLength = ETH_IPv4_HEADER_SIZE + ETH_UDP_HEADER_SIZE + size;
|
|
// Identification is filled on header insertion
|
|
ipProps->Flags = 0x00;
|
|
ipProps->FragmentOffset = 0x00;
|
|
ipProps->TTL = 255;
|
|
ipProps->Protocol = ETH_UDP_PACKET_CLASS;
|
|
// HeaderChecksum is calculated on header insertion
|
|
ipProps->SourceIPAddr = connBlock->intf->ip;
|
|
ipProps->DestIPAddr = addr;
|
|
|
|
// common fields for packet assembly
|
|
ipProps->hdrInsFn = insert_ipv4_header;
|
|
ipProps->headerSize = ETH_IPv4_HEADER_SIZE;
|
|
|
|
// Ethernet
|
|
layer = layer->parent;
|
|
if (addr != 0xFFFFFFFF) {
|
|
ArpCache * arpc = connBlock->intf->arpc;
|
|
const ArpEntry * entry = arpc_get_ask(arpc, addr);
|
|
memcpy(ethProps->destAddr, entry, ETH_HW_ADDR_LEN);
|
|
} else {
|
|
memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN);
|
|
}
|
|
memcpy(ethProps->sourceAddr, connBlock->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;
|
|
|
|
Pckt cooked;
|
|
cooked.payload = data;
|
|
cooked.payloadSize = size;
|
|
cooked.header = ethHeader;
|
|
|
|
// NOT FILLED FIELDS
|
|
cooked.headerSize = 0;
|
|
cooked.time_s = 0;
|
|
cooked.time_ns = 0;
|
|
|
|
RawPckt raw;
|
|
pckt_assemble(&raw, &cooked);
|
|
|
|
ethinf_transmit(connBlock->intf, &raw);
|
|
|
|
// free headers
|
|
dynmem_free(udpHeader);
|
|
dynmem_free(ipHeader);
|
|
dynmem_free(ethHeader);
|
|
|
|
// release payload
|
|
// dynmem_free(raw.payload);
|
|
|
|
return 0;
|
|
}
|