129 lines
4.3 KiB
C
129 lines
4.3 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"
|
|
|
|
static bool filtUdp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps) {
|
|
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;
|
|
|
|
// 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;
|
|
|
|
// 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;
|
|
|
|
// allocate transmit buffer
|
|
uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE + ETH_UDP_HEADER_SIZE;
|
|
uint32_t txBufSize = txHeaderSize + size + 4;
|
|
uint8_t * txBuf = dynmem_alloc(txBufSize);
|
|
|
|
// copy payload
|
|
memcpy(txBuf + txHeaderSize, data, size);
|
|
|
|
// insert UDP header
|
|
insert_udp_header(txBuf + ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE, udpHeader);
|
|
|
|
// insert IPv4 header
|
|
insert_ipv4_header(txBuf + ETH_ETHERNET_HEADER_SIZE, ipHeader);
|
|
|
|
// insert Ethernet header
|
|
insert_ethernet_header(txBuf, ethHeader);
|
|
|
|
// free headers
|
|
dynmem_free(udpHeader);
|
|
dynmem_free(ipHeader);
|
|
dynmem_free(ethHeader);
|
|
|
|
// append CRC at the end
|
|
uint32_t crc = crc32(txBuf, txBufSize - 4);
|
|
memcpy(txBuf + txBufSize - 4, &crc, 4);
|
|
|
|
// send packet
|
|
RawPckt rpckt;
|
|
rpckt.size = txBufSize;
|
|
rpckt.payload = txBuf;
|
|
rpckt.time_s = 0;
|
|
rpckt.time_ns = 0;
|
|
ethinf_transmit(connBlock->intf, &rpckt);
|
|
|
|
// release transmit buffer
|
|
dynmem_free(txBuf);
|
|
|
|
return 0;
|
|
}
|