- ARP cache auto lookup feature added
- IGMPv2 capabilities added (report membership, leave group) - ICMP capabilities added (ping) - Tx Message Queue added
This commit is contained in:
parent
923ac4caa4
commit
05288d7a3c
@ -77,7 +77,7 @@ const ArpEntry *arpc_get_ask(ArpCache *arpc, ip4_addr ip) {
|
||||
while (((entry = arpc_get(arpc, ip)) == NULL) && (attemptN < ETHLIB_ARP_RETRY_COUNT)) {
|
||||
arpc_ask(arpc, ip);
|
||||
ETHLIB_SLEEP_MS(20);
|
||||
//attemptN++;
|
||||
attemptN++;
|
||||
}
|
||||
|
||||
return entry;
|
||||
|
||||
@ -26,6 +26,9 @@ EthInterface *ethintf_new(EthIODef * io) {
|
||||
ethIntf->arpc = arpc_new(ethIntf, ETHLIB_ARPCACHE_SIZE);
|
||||
ASSERT_NULL(ethIntf->arpc);
|
||||
ethintf_register(ethIntf);
|
||||
|
||||
ethIntf->txQ = mq_create(ETHLIB_DEF_MQ_SIZE);
|
||||
|
||||
return ethIntf;
|
||||
}
|
||||
|
||||
@ -34,5 +37,7 @@ void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt) {
|
||||
}
|
||||
|
||||
void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt) {
|
||||
intf->ioDef->llTx(intf->ioDef, rawPckt);
|
||||
mq_push(intf->txQ, rawPckt); // push packet onto the message queue
|
||||
|
||||
intf->ioDef->llTxTrigger(intf->ioDef, intf->txQ);
|
||||
}
|
||||
|
||||
@ -6,12 +6,13 @@
|
||||
#include "prefab/packet_parsers/ipv4_types.h"
|
||||
#include "arp_cache.h"
|
||||
#include "connection_block.h"
|
||||
#include "msg_queue.h"
|
||||
|
||||
/**
|
||||
* Ethernet interface low level definition.
|
||||
*/
|
||||
typedef struct EthIODef_ {
|
||||
int (*llTx)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Function pointer to low-level transmit function
|
||||
int (*llTxTrigger)(struct EthIODef_ * io, MsgQueue * mq); ///< Function pointer to low-level transmit function trigger
|
||||
int (*llTxDone)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Transmission done (interrupt) callback
|
||||
int (*llLinkChg)(struct EthIODef_ * io, int linkState); ///< Link change interrupt
|
||||
int (*llRxDone)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Receive done callback
|
||||
@ -33,6 +34,7 @@ typedef struct EthInterface_ {
|
||||
ip4_addr dns; ///< Domain Name Server
|
||||
ArpCache * arpc; ///< ARP cache
|
||||
ConnBlock arpCb; ///< ARP connection block
|
||||
MsgQueue * txQ; ///< Transmit queue
|
||||
} EthInterface;
|
||||
|
||||
/**
|
||||
|
||||
@ -5,6 +5,8 @@
|
||||
#include "prefab/prefab.h"
|
||||
#include "packet_sieve.h"
|
||||
#include "prefab/packet_parsers/arp_packet.h"
|
||||
#include "prefab/packet_parsers/icmp_packet.h"
|
||||
#include "etherlib/prefab/packet_parsers/igmp_packet.h"
|
||||
|
||||
EthState gEthState;
|
||||
|
||||
@ -14,6 +16,7 @@ static void register_packet_parsers() {
|
||||
cdesc.class = 0;
|
||||
cdesc.containerClass = 0;
|
||||
cdesc.procFun = parse_ethernet;
|
||||
cdesc.hdrInsFn = insert_ethernet_header;
|
||||
cdesc.propertySize = sizeof(EthernetProps);
|
||||
packreg_add_class(E.pcktReg, &cdesc);
|
||||
|
||||
@ -21,6 +24,7 @@ static void register_packet_parsers() {
|
||||
cdesc.class = ETH_IPv4_PACKET_CLASS;
|
||||
cdesc.containerClass = 0;
|
||||
cdesc.procFun = parse_ipv4;
|
||||
cdesc.hdrInsFn = insert_ipv4_header;
|
||||
cdesc.propertySize = sizeof(IPv4Props);
|
||||
packreg_add_class(E.pcktReg, &cdesc);
|
||||
|
||||
@ -28,6 +32,7 @@ static void register_packet_parsers() {
|
||||
cdesc.class = ETH_UDP_PACKET_CLASS;
|
||||
cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
|
||||
cdesc.procFun = parse_udp;
|
||||
cdesc.hdrInsFn = insert_udp_header;
|
||||
cdesc.propertySize = sizeof(UdpProps);
|
||||
packreg_add_class(E.pcktReg, &cdesc);
|
||||
|
||||
@ -35,12 +40,30 @@ static void register_packet_parsers() {
|
||||
cdesc.class = ETH_ARP_PACKET_CLASS;
|
||||
cdesc.containerClass = 0;
|
||||
cdesc.procFun = parse_arp;
|
||||
cdesc.hdrInsFn = insert_arp_header;
|
||||
cdesc.propertySize = sizeof(ArpProps);
|
||||
packreg_add_class(E.pcktReg, &cdesc);
|
||||
|
||||
// ICMP packet parser
|
||||
cdesc.class = ETH_ICMP_PACKET_CLASS;
|
||||
cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
|
||||
cdesc.procFun = parse_icmp;
|
||||
cdesc.hdrInsFn = insert_icmp_header;
|
||||
cdesc.propertySize = sizeof(IcmpProps);
|
||||
packreg_add_class(E.pcktReg, &cdesc);
|
||||
|
||||
// IGMP packet parser
|
||||
cdesc.class = ETH_IGMP_PACKET_CLASS;
|
||||
cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
|
||||
cdesc.procFun = parse_igmp;
|
||||
cdesc.hdrInsFn = insert_igmp_header;
|
||||
cdesc.propertySize = sizeof(IgmpProps);
|
||||
packreg_add_class(E.pcktReg, &cdesc);
|
||||
}
|
||||
|
||||
void ethlib_init() {
|
||||
dynmem_init(); // initialize dynamic memory subsystem
|
||||
E.tmr = timer_new(ETHLIB_TMR_SCHED_TABLE_SIZE); // create timer
|
||||
E.pcktReg = packreg_new(); // create new packet registry
|
||||
register_packet_parsers(); // register packet parsers
|
||||
|
||||
|
||||
@ -5,12 +5,14 @@
|
||||
#include "packet_registry.h"
|
||||
#include "packet_sieve.h"
|
||||
#include "eth_interface.h"
|
||||
#include "timer.h"
|
||||
|
||||
/**
|
||||
* Global EtherLib state.
|
||||
*/
|
||||
typedef struct {
|
||||
MP * mp; ///< Memory pool for dynamic allocations
|
||||
Timer * tmr; ///< Timer for internal schedule.
|
||||
PcktRegistry * pcktReg; ///< Packet registry
|
||||
EthInterface * ethIntf; ///< Array of Ethernet interfaces
|
||||
} EthState;
|
||||
|
||||
51
msg_queue.c
51
msg_queue.c
@ -2,4 +2,55 @@
|
||||
// Created by epagris on 2023.01.13..
|
||||
//
|
||||
|
||||
#include <memory.h>
|
||||
#include <stdbool.h>
|
||||
#include "msg_queue.h"
|
||||
#include "dynmem.h"
|
||||
|
||||
MsgQueue *mq_create(uint32_t size) {
|
||||
MsgQueue * mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt));
|
||||
mq->size = size;
|
||||
mq->readIdx = 0;
|
||||
mq->writeIdx = 0;
|
||||
memset(mq->pckts, 0, mq->size * sizeof(RawPckt));
|
||||
|
||||
return mq;
|
||||
}
|
||||
|
||||
void mq_clear(MsgQueue * mq) {
|
||||
mq->readIdx = 0;
|
||||
mq->writeIdx = 0;
|
||||
memset(mq->pckts, 0, mq->size * sizeof(RawPckt));
|
||||
}
|
||||
|
||||
uint32_t mq_avail(const MsgQueue * mq) {
|
||||
return mq->writeIdx - mq->readIdx;
|
||||
}
|
||||
|
||||
#define MQ_NEXT(size,current) (((current)+1)%(size))
|
||||
|
||||
bool mq_push(MsgQueue * mq, const RawPckt * raw) {
|
||||
if (MQ_NEXT(mq->size, mq->writeIdx) == mq->readIdx) { // cannot push, queue is full
|
||||
return false;
|
||||
}
|
||||
|
||||
// can push
|
||||
mq->pckts[mq->writeIdx++] = *raw;
|
||||
|
||||
// advance write pointer
|
||||
mq->writeIdx = MQ_NEXT(mq->size, mq->writeIdx);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
RawPckt mq_top(MsgQueue * mq) {
|
||||
return mq->pckts[mq->readIdx];
|
||||
}
|
||||
|
||||
void mq_pop(MsgQueue * mq) {
|
||||
if (mq_avail(mq) > 0) { // if there's something to pop
|
||||
mq->readIdx = MQ_NEXT(mq->size, mq->readIdx);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
55
msg_queue.h
55
msg_queue.h
@ -1,8 +1,55 @@
|
||||
//
|
||||
// Created by epagris on 2023.01.13..
|
||||
//
|
||||
|
||||
#ifndef ETHERLIB_TEST_MSG_QUEUE_H
|
||||
#define ETHERLIB_TEST_MSG_QUEUE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "packet.h"
|
||||
|
||||
typedef struct {
|
||||
uint32_t writeIdx; ///< Next block to write
|
||||
uint32_t readIdx; ///< Next block to read
|
||||
uint32_t size; ///< Size of circular buffer
|
||||
RawPckt pckts[]; ///< Array of packets
|
||||
} MsgQueue;
|
||||
|
||||
/**
|
||||
* Create Message Queue.
|
||||
* @param size size of circular buffer
|
||||
* @return pointer to MQ instance OR NULL on failure
|
||||
*/
|
||||
MsgQueue * mq_create(uint32_t size);
|
||||
|
||||
/**
|
||||
* Clear circular buffer.
|
||||
* @param mq pointer to Message Queue
|
||||
*/
|
||||
void mq_clear(MsgQueue * mq);
|
||||
|
||||
/**
|
||||
* Get number of available elements.
|
||||
* @param mq pointer to Message Queue
|
||||
* @return number of available elements
|
||||
*/
|
||||
uint32_t mq_avail(const MsgQueue * mq);
|
||||
|
||||
/**
|
||||
* Push element to the Message Queue.
|
||||
* @param mq pointer to MQ
|
||||
* @param raw pointer to raw packet
|
||||
* @return true on success, false on failure (e.g.: queue full)
|
||||
*/
|
||||
bool mq_push(MsgQueue * mq, const RawPckt * raw);
|
||||
|
||||
/**
|
||||
* Get top element.
|
||||
* @param mq pointer to MQ
|
||||
* @return top element (COPY, NOT POINTER!)
|
||||
*/
|
||||
RawPckt mq_top(MsgQueue * mq);
|
||||
|
||||
/**
|
||||
* Pop top element.
|
||||
* @param mq pointer to MQ
|
||||
*/
|
||||
void mq_pop(MsgQueue * mq);
|
||||
|
||||
#endif //ETHERLIB_TEST_MSG_QUEUE_H
|
||||
|
||||
@ -18,9 +18,20 @@ typedef uint8_t bool8_t;
|
||||
* as well. (The size of the packet is divisible by 4.)
|
||||
*/
|
||||
|
||||
struct PcktClassDesc_;
|
||||
//struct PcktClassDesc_;
|
||||
struct EthInterface_;
|
||||
struct PcktHeaderElement_;
|
||||
|
||||
/**
|
||||
* @typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement_ * pcktHdrLe)
|
||||
* @brief Packet header insert function prototype.
|
||||
* @param hdr buffer to insert header into
|
||||
* @param pcktHdrLe linked list of packet headers
|
||||
*/
|
||||
typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement_ * pcktHdrLe);
|
||||
|
||||
#define PcktPropsHeader \
|
||||
PcktHeaderInsertFn hdrInsFn; /**< Header insert function pointer (used only on transmission) */ \
|
||||
uint16_t propSize; /**< Size of this property object */ \
|
||||
uint16_t headerSize; /**< Header size in bytes */ \
|
||||
uint16_t containedPacketClass; /**< Class of contained packet. Zero if no packet contained. */ \
|
||||
@ -28,14 +39,10 @@ struct PcktClassDesc_;
|
||||
uint16_t accumulatedOffset; /** Accumulated offset from the beginning of the packet */ \
|
||||
bool8_t validityOK; /**< Indicates that checksum is OK. */ \
|
||||
|
||||
|
||||
typedef struct {
|
||||
PcktPropsHeader
|
||||
} PcktProps;
|
||||
|
||||
struct EthInterface_;
|
||||
struct PcktHeaderElement_;
|
||||
|
||||
/**
|
||||
* @typedef int (*PcktProcFn)(const uint8_t *pHdr, uint32_t size, uint8_t *pPcktProps);
|
||||
* @brief Pckt processing function template.
|
||||
@ -59,6 +66,7 @@ typedef struct PcktClassDesc_ {
|
||||
uint16_t class; ///< Type identification of the packet 'class' (unique!)
|
||||
uint16_t containerClass; ///< Type of container packet packet (e.g.: IPv4 in case of UDP)
|
||||
PcktProcFn procFun; ///< Pckt processing function
|
||||
PcktHeaderInsertFn hdrInsFn; ///< Header insert function
|
||||
uint16_t propertySize; ///< Size of property structure
|
||||
} PcktClassDesc;
|
||||
|
||||
|
||||
@ -82,7 +82,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
|
||||
const PcktSieveLayer * nodeIter = layer->nodes;
|
||||
found = false;
|
||||
while (nodeIter && !found) {
|
||||
found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props); // specific or general match
|
||||
found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props, intf); // specific or general match
|
||||
if (found) {
|
||||
layer = nodeIter; // advance in the sieve tree
|
||||
const PcktHeaderElement * containedHeader = headerIter->next; // advance on headers
|
||||
@ -204,9 +204,9 @@ bool packsieve_remove_layer(PcktSieveLayer * layer) {
|
||||
|
||||
void packsieve_report(const PcktSieveLayer *layer, uint32_t indent) {
|
||||
if (*layer->infoTag != '\0') {
|
||||
INFO("%*c\\--|%s|---\n", indent, ' ', layer->infoTag);
|
||||
INFO("%*c└─┤%s├───\n", indent, ' ', layer->infoTag);
|
||||
} else {
|
||||
INFO("%*c\\--|%d|---\n", indent, ' ', layer->packetClass);
|
||||
INFO("%*c└─┤%d├───\n", indent, ' ', layer->packetClass);
|
||||
}
|
||||
const PcktSieveLayer * nodeIter = layer->nodes;
|
||||
while(nodeIter) {
|
||||
@ -214,3 +214,18 @@ void packsieve_report(const PcktSieveLayer *layer, uint32_t indent) {
|
||||
nodeIter = nodeIter->next;
|
||||
}
|
||||
}
|
||||
|
||||
void pckthdr_chain_free(PcktHeaderElement *hdr) {
|
||||
// rewind
|
||||
while (hdr->prev != NULL) {
|
||||
hdr = hdr->prev;
|
||||
}
|
||||
|
||||
// free
|
||||
PcktHeaderElement * next;
|
||||
while (hdr != NULL) {
|
||||
next = hdr->next;
|
||||
dynmem_free(hdr);
|
||||
hdr = next;
|
||||
}
|
||||
}
|
||||
|
||||
@ -14,6 +14,12 @@ typedef struct PcktHeaderElement_ {
|
||||
PcktProps props; ///< Properties (allocated to appropriate size)
|
||||
} PcktHeaderElement;
|
||||
|
||||
/**
|
||||
* Free chain of packet header. List is auto-rewound.
|
||||
* @param hdr pointer to header chain element
|
||||
*/
|
||||
void pckthdr_chain_free(PcktHeaderElement * hdr);
|
||||
|
||||
#define ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE (2 * sizeof(PcktHeaderElement *))
|
||||
|
||||
/**
|
||||
@ -44,10 +50,12 @@ bool packfiltcond_cmp(const PcktSieveFilterCondition * c1, const PcktSieveFilter
|
||||
*/
|
||||
void packfiltcond_zero(PcktSieveFilterCondition * cond);
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
/**
|
||||
* Sieve filter function type.
|
||||
*/
|
||||
typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps);
|
||||
typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, struct EthInterface_ * intf);
|
||||
|
||||
struct PcktSieveLayer_;
|
||||
|
||||
@ -79,8 +87,6 @@ typedef struct PcktSieveLayer_ {
|
||||
char infoTag[PCKT_SIEVE_INFOTAG_LEN]; ///< Info tag length
|
||||
} PcktSieveLayer;
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
/**
|
||||
* Packet sieve class.
|
||||
*/
|
||||
@ -105,6 +111,12 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
|
||||
/**
|
||||
* Create a new sieve filter layer.
|
||||
* @param parent parent layer
|
||||
* @param filtCond filter condition passed to the filter callback function
|
||||
* @param matchAny don't test against filter criteria, accept any incoming packet (e.g. pass IP packets with any destination address)
|
||||
* @param filtFn filter callback function, returning boolean
|
||||
* @param cbFn callback function invoked, if packet has passed filter criteria
|
||||
* @param tag arbitrary tag, passed to callbacn function, when invoked
|
||||
* @param pcktClass packet class associated with current sieve layer
|
||||
* @return pointer to new layer object or NULL on failure
|
||||
*/
|
||||
PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass);
|
||||
|
||||
@ -2,4 +2,66 @@
|
||||
// Created by epagris on 2023.01.11..
|
||||
//
|
||||
|
||||
#include <stddef.h>
|
||||
#include "pckt_assembler.h"
|
||||
#include "packet_sieve.h"
|
||||
#include "dynmem.h"
|
||||
#include "eth_interface.h"
|
||||
#include "global_state.h"
|
||||
#include "utils.h"
|
||||
|
||||
int pckt_assemble(RawPckt *raw, const Pckt *cooked) {
|
||||
// calculate frame size
|
||||
uint16_t frameSize = 0;
|
||||
uint16_t headerSize = 0;
|
||||
const PcktHeaderElement *hdrIter = cooked->header;
|
||||
|
||||
// rewind headers
|
||||
while (hdrIter->prev != NULL) {
|
||||
hdrIter = hdrIter->prev;
|
||||
}
|
||||
|
||||
const PcktHeaderElement *lastHdr = hdrIter;
|
||||
while (hdrIter != NULL) {
|
||||
headerSize += hdrIter->props.headerSize;
|
||||
lastHdr = hdrIter;
|
||||
hdrIter = hdrIter->next;
|
||||
}
|
||||
frameSize = headerSize + cooked->payloadSize + 4; // header + payload + CRC32 checksum area
|
||||
|
||||
// calculate padding size
|
||||
uint16_t padding = (frameSize < ETH_FRAME_MIN_SIZE) ? (ETH_FRAME_MIN_SIZE - frameSize) : 0;
|
||||
|
||||
// grow frame to minimum length if needed
|
||||
frameSize = (frameSize < ETH_FRAME_MIN_SIZE) ? ETH_FRAME_MIN_SIZE : frameSize;
|
||||
|
||||
// allocate raw packet data
|
||||
raw->payload = dynmem_alloc(frameSize);
|
||||
raw->size = frameSize;
|
||||
|
||||
// insert pointer
|
||||
uint8_t * payloadInsPtr = raw->payload + headerSize;
|
||||
uint8_t * headerInsPtr = payloadInsPtr - lastHdr->props.headerSize;
|
||||
|
||||
// insert payload
|
||||
memcpy(payloadInsPtr, cooked->payload, cooked->payloadSize);
|
||||
|
||||
// insert zero padding
|
||||
memset(payloadInsPtr + cooked->payloadSize, 0, padding);
|
||||
|
||||
// insert reversely headers (e.g.: UDP -> IPv4 -> Ethernet)
|
||||
hdrIter = lastHdr;
|
||||
while (hdrIter != NULL) {
|
||||
hdrIter->props.hdrInsFn(headerInsPtr, hdrIter); // insert header
|
||||
hdrIter = hdrIter->prev; // step to previous header
|
||||
if (hdrIter != NULL) {
|
||||
headerInsPtr -= hdrIter->props.headerSize; // advance data pointer
|
||||
}
|
||||
}
|
||||
|
||||
// insert CRC32
|
||||
uint32_t crc = crc32(raw->payload, frameSize - 4);
|
||||
memcpy(raw->payload + frameSize - 4, (const uint8_t *)&crc, 4);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -1,8 +1,18 @@
|
||||
//
|
||||
// Created by epagris on 2023.01.11..
|
||||
//
|
||||
|
||||
#ifndef ETHERLIB_TEST_PCKT_ASSEMBLER_H
|
||||
#define ETHERLIB_TEST_PCKT_ASSEMBLER_H
|
||||
|
||||
#include "packet.h"
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
#define ETH_FRAME_MIN_SIZE (64)
|
||||
|
||||
/**
|
||||
* Assemble packet.
|
||||
* @param raw raw packet
|
||||
* @param cooked packet information and headers
|
||||
* @return 0 on success OR -1 on failure
|
||||
*/
|
||||
int pckt_assemble(RawPckt *raw, const Pckt *cooked);
|
||||
|
||||
#endif //ETHERLIB_TEST_PCKT_ASSEMBLER_H
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
#include "../../dynmem.h"
|
||||
#include "../packet_parsers/arp_packet.h"
|
||||
|
||||
static bool filtArp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps) {
|
||||
static bool filtArp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
|
||||
EthernetProps * ethProps = (EthernetProps *) contProps;
|
||||
ArpProps * arpProps = (ArpProps *) ownProps;
|
||||
|
||||
|
||||
@ -1,5 +1,79 @@
|
||||
//
|
||||
// Created by epagris on 2023.01.04..
|
||||
//
|
||||
|
||||
#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
|
||||
|
||||
switch (icmpProps->type) {
|
||||
case ICMP_MT_ECHO_REQUEST: {
|
||||
icmpProps->type = ICMP_MT_ECHO_REPLY; // replace request with reply
|
||||
icmpProps->hdrInsFn = insert_icmp_header;
|
||||
|
||||
// swap source an destination IP addresses
|
||||
IPv4Props * iPv4Props = HEADER_FETCH_PROPS(IPv4Props, pckt->header->prev);
|
||||
ip4_addr tmpIp = iPv4Props->SourceIPAddr;
|
||||
iPv4Props->SourceIPAddr = iPv4Props->DestIPAddr;
|
||||
iPv4Props->DestIPAddr = tmpIp;
|
||||
iPv4Props->hdrInsFn = insert_ipv4_header;
|
||||
|
||||
// swap Ethernet fields
|
||||
EthernetAddress tmpEth;
|
||||
EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, pckt->header->prev->prev);
|
||||
HWACPY(tmpEth, ethProps->sourceAddr);
|
||||
HWACPY(ethProps->sourceAddr, ethProps->destAddr);
|
||||
HWACPY(ethProps->destAddr, tmpEth);
|
||||
ethProps->hdrInsFn = insert_ethernet_header;
|
||||
|
||||
// payload is the same...
|
||||
|
||||
// assemble packet
|
||||
RawPckt raw;
|
||||
pckt_assemble(&raw, pckt);
|
||||
|
||||
// release headers
|
||||
pckthdr_chain_free(pckt->header);
|
||||
|
||||
ethinf_transmit(intf, &raw);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
ConnBlock icmp_new_connblock(EthInterface * intf) {
|
||||
ConnBlock 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.intf = intf;
|
||||
|
||||
SNPRINTF(icmpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "ICMP (ping)");
|
||||
|
||||
return icmpConnB;
|
||||
}
|
||||
@ -1,8 +1,15 @@
|
||||
//
|
||||
// Created by epagris on 2023.01.04..
|
||||
//
|
||||
|
||||
#ifndef ETHERLIB_TEST_ICMP_CONNBLOCK_H
|
||||
#define ETHERLIB_TEST_ICMP_CONNBLOCK_H
|
||||
|
||||
#include "../../connection_block.h"
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
/**
|
||||
* Create ICMP connection block.
|
||||
* @param intf associated Ethernet interface
|
||||
* @return connection block
|
||||
*/
|
||||
ConnBlock icmp_new_connblock(struct EthInterface_ * intf);
|
||||
|
||||
#endif //ETHERLIB_TEST_ICMP_CONNBLOCK_H
|
||||
|
||||
@ -1,5 +1,120 @@
|
||||
//
|
||||
// Created by epagris on 2023.01.14..
|
||||
//
|
||||
|
||||
#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 "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;
|
||||
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.intf = intf;
|
||||
|
||||
SNPRINTF(igmpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IGMP");
|
||||
|
||||
return igmpConnB;
|
||||
}
|
||||
|
||||
static void igmp_send(ConnBlock * connBlock, ip4_addr ga, int mt) {
|
||||
// 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;
|
||||
ipProps->IHL = ETH_IPv4_HEADER_SIZE / 4;
|
||||
ipProps->Version = 4;
|
||||
ipProps->DSF = 0x00;
|
||||
ipProps->TotalLength = ETH_IPv4_HEADER_SIZE + ETH_IGMP_HEADER_SIZE;
|
||||
// Identification is filled on header insertion
|
||||
ipProps->Flags = 0x00;
|
||||
ipProps->FragmentOffset = 0x00;
|
||||
ipProps->TTL = 255;
|
||||
ipProps->Protocol = ETH_IGMP_PACKET_CLASS;
|
||||
// HeaderChecksum is calculated on header insertion
|
||||
ipProps->SourceIPAddr = connBlock->intf->ip;
|
||||
ipProps->DestIPAddr = ga;
|
||||
|
||||
// common fields for packet assembly
|
||||
ipProps->hdrInsFn = insert_ipv4_header;
|
||||
ipProps->headerSize = ETH_IPv4_HEADER_SIZE;
|
||||
|
||||
// Ethernet
|
||||
ethmc_from_ipmc(ethProps->destAddr, ga);
|
||||
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 = NULL;
|
||||
cooked.payloadSize = 0;
|
||||
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
|
||||
pckthdr_chain_free(ethHeader);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
@ -1,8 +1,30 @@
|
||||
//
|
||||
// Created by epagris on 2023.01.14..
|
||||
//
|
||||
|
||||
#ifndef ETHERLIB_TEST_IGMP_CONNBLOCK_H
|
||||
#define ETHERLIB_TEST_IGMP_CONNBLOCK_H
|
||||
|
||||
#include "../../connection_block.h"
|
||||
#include "../packet_parsers/ipv4_types.h"
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
/**
|
||||
* Create IGMP connection block.
|
||||
* @param intf associated Ethernet interface
|
||||
* @return connection block
|
||||
*/
|
||||
ConnBlock igmp_new_connblock(struct EthInterface_ * intf);
|
||||
|
||||
/**
|
||||
* Report IGMP membership.
|
||||
* @param connBlock pointer to IGMP connblock
|
||||
* @param ga IPv4 group address
|
||||
*/
|
||||
void igmp_report_membership(ConnBlock * connBlock, ip4_addr ga);
|
||||
|
||||
/**
|
||||
* Leave IGMP group.
|
||||
* @param connBlock pointer to IGMP connblock
|
||||
* @param ga IPv4 group address
|
||||
*/
|
||||
void igmp_leave_group(ConnBlock * connBlock, ip4_addr ga);
|
||||
|
||||
#endif //ETHERLIB_TEST_IGMP_CONNBLOCK_H
|
||||
|
||||
@ -7,11 +7,12 @@
|
||||
#include "../packet_parsers/packet_parsers.h"
|
||||
#include "../../utils.h"
|
||||
|
||||
static bool filtIPv4(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps) {
|
||||
static bool filtIPv4(const PcktSieveFilterCondition *filtCond, const PcktProps *contProps, const PcktProps *ownProps, EthInterface * intf) {
|
||||
EthernetProps *etherProps = (EthernetProps *) contProps;
|
||||
IPv4Props *ipProps = (IPv4Props *) ownProps;
|
||||
|
||||
return etherProps->length_type == ETH_IPv4_PACKET_CLASS && IP_ADDR_FROM_FILTCOND(filtCond) == ipProps->DestIPAddr;
|
||||
return etherProps->length_type == ETH_IPv4_PACKET_CLASS && ((IP_ADDR_FROM_FILTCOND(filtCond) == ipProps->DestIPAddr) ||
|
||||
((IP_ADDR_FROM_FILTCOND(filtCond) == IPv4_IF_ADDR) && (ipProps->DestIPAddr == intf->ip) && (intf->ip != 0)));
|
||||
}
|
||||
|
||||
ConnBlock ipv4_new_connblock(EthInterface *intf, ip4_addr ipAddr, SieveCallBackFn cbFn) {
|
||||
@ -28,8 +29,12 @@ ConnBlock ipv4_new_connblock(EthInterface *intf, ip4_addr ipAddr, SieveCallBackF
|
||||
connb.intf = intf;
|
||||
|
||||
if (!matchAny) {
|
||||
if (ipAddr != IPv4_IF_ADDR) {
|
||||
SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: %u.%u.%u.%u",
|
||||
(ipAddr & 0xFF), ((ipAddr >> 8) & 0xFF), ((ipAddr >> 16) & 0xFF), ((ipAddr >> 24) & 0xFF));
|
||||
} else {
|
||||
SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: INTF. ADDR.");
|
||||
}
|
||||
} else {
|
||||
SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: ANY");
|
||||
}
|
||||
|
||||
@ -9,8 +9,9 @@
|
||||
#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) {
|
||||
static bool filtUdp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
|
||||
IPv4Props * ipProps = (IPv4Props *) contProps;
|
||||
UdpProps * udpProps = (UdpProps *) ownProps;
|
||||
|
||||
@ -60,6 +61,10 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
|
||||
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;
|
||||
@ -75,6 +80,10 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
|
||||
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) {
|
||||
@ -87,42 +96,32 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
|
||||
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);
|
||||
// common fields for packet assembly
|
||||
ethProps->hdrInsFn = insert_ethernet_header;
|
||||
ethProps->headerSize = ETH_ETHERNET_HEADER_SIZE;
|
||||
|
||||
// copy payload
|
||||
memcpy(txBuf + txHeaderSize, data, size);
|
||||
Pckt cooked;
|
||||
cooked.payload = data;
|
||||
cooked.payloadSize = size;
|
||||
cooked.header = ethHeader;
|
||||
|
||||
// insert UDP header
|
||||
insert_udp_header(txBuf + ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE, udpHeader);
|
||||
// NOT FILLED FIELDS
|
||||
cooked.headerSize = 0;
|
||||
cooked.time_s = 0;
|
||||
cooked.time_ns = 0;
|
||||
|
||||
// insert IPv4 header
|
||||
insert_ipv4_header(txBuf + ETH_ETHERNET_HEADER_SIZE, ipHeader);
|
||||
RawPckt raw;
|
||||
pckt_assemble(&raw, &cooked);
|
||||
|
||||
// insert Ethernet header
|
||||
insert_ethernet_header(txBuf, ethHeader);
|
||||
ethinf_transmit(connBlock->intf, &raw);
|
||||
|
||||
// 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);
|
||||
// release payload
|
||||
// dynmem_free(raw.payload);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -43,8 +43,21 @@ typedef struct {
|
||||
ip4_addr TPA;
|
||||
} ArpProps;
|
||||
|
||||
/**
|
||||
* Parse ARP packet.
|
||||
* @param hdr pointer to the packet buffer beginning with ARP header
|
||||
* @param size size of processable buffer area
|
||||
* @param pcktHdrLe linked list of packet headers
|
||||
* @param intf Ethernet interface
|
||||
* @return always 0 OR -1 on failure
|
||||
*/
|
||||
int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
|
||||
|
||||
/**
|
||||
* Insert ARP header into packet.
|
||||
* @param hdr pointer to the packet buffer where the ARP header is to be written
|
||||
* @param headers linked list of packet headers
|
||||
*/
|
||||
void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers);
|
||||
|
||||
#endif //ETHERLIB_TEST_ARP_PACKET_H
|
||||
|
||||
@ -8,6 +8,7 @@
|
||||
#include "../../utils.h"
|
||||
#include "../../connection_block.h"
|
||||
#include "../conn_blocks/udp_connblock.h"
|
||||
#include "etherlib/global_state.h"
|
||||
|
||||
static struct {
|
||||
DhcpState state;
|
||||
@ -238,7 +239,7 @@ void dhcp_request(ip4_addr reqAddr, ip4_addr dhcpServerAddr) {
|
||||
memcpy(props.chaddr, s.intf->mac, ETH_HW_ADDR_LEN);
|
||||
|
||||
DhcpOption optEnd = {DHCP_OPT_End, 0, NULL};
|
||||
DhcpOption paramReq = {DHCP_OPT_ParamReqList, 3, &optEnd, {1, 3, 6}}; // TODO...
|
||||
DhcpOption paramReq = {DHCP_OPT_ParamReqList, 4, &optEnd, {1, 3, 6, 51}}; // TODO...
|
||||
DhcpOption reqIp = {DHCP_OPT_RequestedIpAddress, 4, ¶mReq, {IPv4_ADDR_TO_BE_BYTES(reqAddr)}};
|
||||
DhcpOption clId = {DHCP_OPT_ClientIdentifier, 7, &reqIp, {DHCP_HW_TYPE_ETHERNET, HWADDR_TO_BE_BYTES(s.intf->mac)}};
|
||||
DhcpOption msgType = {DHCP_OPT_MsgType, 1, &clId, {DHCPREQUEST}};
|
||||
@ -282,6 +283,13 @@ static void dhcp_process(DhcpProps *props, DhcpOption *opts) {
|
||||
opt = dhcp_get_option(opts, DHCP_OPT_DomainNameServer); // get DNS
|
||||
FETCH_DWORD(&(s.intf->dns), opt->value);
|
||||
|
||||
opt = dhcp_get_option(opts, DHCP_OPT_IPAddrLeaseTime); // fetch Lease Time
|
||||
uint32_t dhcpLeaseTime_s;
|
||||
FETCH_DWORD_H2N(&dhcpLeaseTime_s, opt->value);
|
||||
|
||||
AlarmUserData params = { 0 };
|
||||
timer_sched_rel(E.tmr, ((int64_t)dhcpLeaseTime_s) * 1000000, NULL, params);
|
||||
|
||||
MSG("DHCP done!\n");
|
||||
MSG("IP: ");
|
||||
PRINT_IPv4(s.intf->ip);
|
||||
@ -291,6 +299,7 @@ static void dhcp_process(DhcpProps *props, DhcpOption *opts) {
|
||||
PRINT_IPv4(s.intf->netmask);
|
||||
MSG("\nDNS: ");
|
||||
PRINT_IPv4(s.intf->dns);
|
||||
MSG("\nLease time: %u s\n", dhcpLeaseTime_s);
|
||||
MSG("\n");
|
||||
|
||||
s.state = DHCP_BOUND;
|
||||
|
||||
@ -53,6 +53,9 @@ typedef enum {
|
||||
*/
|
||||
void dhcp_initiate(EthInterface * intf);
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
void dhcp_start();
|
||||
|
||||
#endif //ETHERLIB_TEST_DHCP_H
|
||||
|
||||
@ -10,6 +10,8 @@
|
||||
#define ETH_ETHERTYPE_LENGTH_THRESHOLD (1500)
|
||||
#define ETH_ETHERNET_HEADER_SIZE (14)
|
||||
|
||||
EthernetAddress ETH_ETHERNET_IPMC_HWADDR = { 0x01, 0x00, 0x5E, 0x00, 0x00, 0x00 };
|
||||
|
||||
int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
|
||||
EthernetProps * frameProps = (EthernetProps *) &pcktHdrLe->props;
|
||||
FETCH_ADVANCE(frameProps->destAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
|
||||
|
||||
@ -16,16 +16,20 @@
|
||||
|
||||
#define ALLOC_HEADER_ELEMENT(T) (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(T))
|
||||
|
||||
#define HWACPY(dst,src) memcpy((dst),(src), ETH_HW_ADDR_LEN)
|
||||
|
||||
typedef uint8_t EthernetAddress[ETH_HW_ADDR_LEN];
|
||||
|
||||
extern EthernetAddress ETH_ETHERNET_IPMC_HWADDR;
|
||||
|
||||
/**
|
||||
* @struct EthernetProps
|
||||
* Ethernet frame properties
|
||||
*/
|
||||
typedef struct {
|
||||
PcktPropsHeader;
|
||||
EthernetAddress destAddr[ETH_HW_ADDR_LEN]; ///< destination address
|
||||
EthernetAddress sourceAddr[ETH_HW_ADDR_LEN]; ///< source address
|
||||
EthernetAddress destAddr; ///< destination address
|
||||
EthernetAddress sourceAddr; ///< source address
|
||||
uint16_t length_type; ///< frame length_type
|
||||
} EthernetProps;
|
||||
|
||||
|
||||
@ -3,3 +3,40 @@
|
||||
//
|
||||
|
||||
#include "icmp_packet.h"
|
||||
#include "../../utils.h"
|
||||
#include "ipv4_packet.h"
|
||||
|
||||
int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
|
||||
// parse header
|
||||
IcmpProps *icmpProps = HEADER_FETCH_PROPS(IcmpProps, pcktHdrLe);
|
||||
FETCH_BYTE_ADVANCE(&icmpProps->type, hdr);
|
||||
FETCH_BYTE_ADVANCE(&icmpProps->code, hdr);
|
||||
FETCH_WORD_ADVANCE(&icmpProps->checksum, hdr);
|
||||
FETCH_WORD_ADVANCE(&icmpProps->identifier, hdr);
|
||||
FETCH_WORD_ADVANCE(&icmpProps->sequenceNumber, hdr);
|
||||
|
||||
// fill-in common fields
|
||||
icmpProps->validityOK = true; // TODO...
|
||||
icmpProps->containedPacketClass = 0;
|
||||
icmpProps->headerSize = ETH_ICMP_HEADER_SIZE;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
|
||||
uint8_t * hdrStart = hdr;
|
||||
IcmpProps *icmpProps = HEADER_FETCH_PROPS(IcmpProps, headers);
|
||||
FILL_BYTE_ADVANCE(hdr, &icmpProps->type);
|
||||
FILL_BYTE_ADVANCE(hdr, &icmpProps->code);
|
||||
uint8_t * ChkSumPtr = hdr;
|
||||
icmpProps->checksum = 0;
|
||||
FILL_ADVANCE(hdr, &icmpProps->checksum, 2);
|
||||
FILL_ADVANCE(hdr, &icmpProps->identifier, 2);
|
||||
FILL_ADVANCE(hdr, &icmpProps->sequenceNumber, 2);
|
||||
|
||||
IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, headers->prev);
|
||||
icmpProps->checksum = chksum(hdrStart, ipProps->TotalLength - ETH_IPv4_HEADER_SIZE);
|
||||
FILL_WORD_H2N_ADVANCE(ChkSumPtr, icmpProps->checksum);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1,8 +1,46 @@
|
||||
//
|
||||
// Created by epagris on 2022.12.25..
|
||||
//
|
||||
|
||||
#ifndef ETHERLIB_TEST_ICMP_PACKET_H
|
||||
#define ETHERLIB_TEST_ICMP_PACKET_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "ipv4_types.h"
|
||||
#include "ethernet_frame.h"
|
||||
|
||||
#define ETH_ICMP_PACKET_CLASS (0x0001)
|
||||
#define ETH_ICMP_HEADER_SIZE (8)
|
||||
|
||||
typedef enum {
|
||||
ICMP_MT_ECHO_REPLY = 0x00,
|
||||
ICMP_MT_ECHO_REQUEST = 0x08
|
||||
} IcmpMsgType;
|
||||
|
||||
/**
|
||||
* ICMP packet properties.
|
||||
*/
|
||||
typedef struct {
|
||||
PcktPropsHeader
|
||||
|
||||
uint8_t type; ///< ICMP message type
|
||||
uint8_t code; ///< ICMP message code (~subtype)
|
||||
uint16_t checksum; ///< Checksum for the entire packet
|
||||
uint16_t identifier; ///< Identifier
|
||||
uint16_t sequenceNumber; ///< Sequence number
|
||||
} IcmpProps;
|
||||
|
||||
/**
|
||||
* Parse ICMP packet header
|
||||
* @param hdr pointer to the beginning of ICMP header
|
||||
* @param size packet size
|
||||
* @param pcktHdrLe linked list of packet headers
|
||||
* @param intf Ethernet interface through which transmission and reception is carried out
|
||||
* @return always 0
|
||||
*/
|
||||
int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
|
||||
|
||||
/**
|
||||
* Insert ICMP header.
|
||||
* @param hdr pointer to message header
|
||||
* @param headers linked list of recursively encapsulated packet headers
|
||||
*/
|
||||
void insert_icmp_header(uint8_t * hdr, const PcktHeaderElement * headers);
|
||||
|
||||
#endif //ETHERLIB_TEST_ICMP_PACKET_H
|
||||
|
||||
@ -3,3 +3,56 @@
|
||||
//
|
||||
|
||||
#include "igmp_packet.h"
|
||||
#include "../../utils.h"
|
||||
|
||||
//static uint16_t igmp_chksum(const uint8_t * hdr) {
|
||||
// // sum fields
|
||||
// uint32_t sum = 0;
|
||||
// const uint16_t *pField = (const uint16_t *) hdr;
|
||||
// for (uint8_t i = 0; i < (ETH_IGMP_HEADER_SIZE / sizeof(uint16_t)); i++) {
|
||||
// uint16_t field = pField[i];
|
||||
// if (i != 1) { // 6. 16-bit long field is the checksum itself, do not count in
|
||||
// sum += field;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// while (sum >> 16) {
|
||||
// sum = (sum & 0xFFFF) + (sum >> 16);
|
||||
// }
|
||||
//
|
||||
// // invert result
|
||||
// uint16_t sum16 = sum ^ 0xFFFF;
|
||||
// return sum16;
|
||||
//}
|
||||
|
||||
void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
|
||||
IgmpProps *igmpProps = HEADER_FETCH_PROPS(IgmpProps, headers);
|
||||
uint8_t * hdrBeg = hdr;
|
||||
|
||||
FILL_ADVANCE(hdr, &igmpProps->type, 1);
|
||||
FILL_ADVANCE(hdr, &igmpProps->maxRespTime, 1);
|
||||
uint8_t * ChkSumPtr = hdr;
|
||||
igmpProps->checksum = 0;
|
||||
FILL_WORD_ADVANCE(hdr, igmpProps->checksum);
|
||||
FILL_DWORD_ADVANCE(hdr, igmpProps->groupAddr);
|
||||
|
||||
igmpProps->checksum = chksum(hdrBeg, ETH_IGMP_HEADER_SIZE);
|
||||
memcpy(ChkSumPtr, &igmpProps->checksum, 2);
|
||||
}
|
||||
|
||||
int parse_igmp(const uint8_t * hdr, uint32_t size, PcktHeaderElement * pcktHdrLe, struct EthInterface_ * intf) {
|
||||
// parse header
|
||||
IgmpProps *igmpProps = HEADER_FETCH_PROPS(IgmpProps, pcktHdrLe);
|
||||
FETCH_BYTE_ADVANCE(&igmpProps->type, hdr);
|
||||
FETCH_BYTE_ADVANCE(&igmpProps->maxRespTime, hdr);
|
||||
FETCH_WORD_ADVANCE(&igmpProps->checksum, hdr);
|
||||
FETCH_WORD_ADVANCE(&igmpProps->groupAddr, hdr);
|
||||
|
||||
// fill-in common fields
|
||||
uint16_t calcChkSum = chksum(hdr, ETH_IGMP_HEADER_SIZE);
|
||||
igmpProps->validityOK = (calcChkSum == 0);
|
||||
igmpProps->containedPacketClass = 0;
|
||||
igmpProps->headerSize = ETH_IGMP_HEADER_SIZE;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,8 +1,48 @@
|
||||
//
|
||||
// Created by epagris on 2022.12.25..
|
||||
//
|
||||
|
||||
#ifndef ETHERLIB_TEST_IGMP_PACKET_H
|
||||
#define ETHERLIB_TEST_IGMP_PACKET_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "ipv4_types.h"
|
||||
#include "ethernet_frame.h"
|
||||
|
||||
#define ETH_IGMP_PACKET_CLASS (0x0002)
|
||||
#define ETH_IGMP_HEADER_SIZE (8)
|
||||
|
||||
// IGMPv2 is implemented!
|
||||
|
||||
typedef enum {
|
||||
IGMP_MT_MEMBERSHIP_QUERY = 0x11,
|
||||
IGMP_MT_MEMBERSHIP_REPORT = 0x16,
|
||||
IGMP_MT_LEAVE_GROUP = 0x17,
|
||||
} IgmpMsgType;
|
||||
|
||||
/**
|
||||
* IGMP packet properties.
|
||||
*/
|
||||
typedef struct {
|
||||
PcktPropsHeader
|
||||
|
||||
uint8_t type; ///< IGMP message type
|
||||
uint8_t maxRespTime; ///< Maximum response time in 1/10 seconds
|
||||
uint16_t checksum; ///< Checksum for the entire packet
|
||||
ip4_addr groupAddr; ///< Group address
|
||||
} IgmpProps;
|
||||
|
||||
/**
|
||||
* Insert IGMP header.
|
||||
* @param hdr pointer to message header
|
||||
* @param headers linked list of recursively encapsulated packet headers
|
||||
*/
|
||||
void insert_igmp_header(uint8_t * hdr, const PcktHeaderElement * headers);
|
||||
|
||||
/**
|
||||
* Parse IGMP packet.
|
||||
* @param hdr pointer to the beginning of the IGMP header
|
||||
* @param size size of processable buffer area
|
||||
* @param pcktHdrLe linket list of packet headers
|
||||
* @param intf Ethernet interface
|
||||
* @return always 0
|
||||
*/
|
||||
int parse_igmp(const uint8_t * hdr, uint32_t size, PcktHeaderElement * pcktHdrLe, struct EthInterface_ * intf);
|
||||
|
||||
#endif //ETHERLIB_TEST_IGMP_PACKET_H
|
||||
|
||||
@ -9,33 +9,13 @@
|
||||
|
||||
#include "../../eth_interface.h"
|
||||
|
||||
static uint16_t ip_checksum(const uint8_t * hdr) {
|
||||
// sum fields
|
||||
uint32_t sum = 0;
|
||||
const uint16_t *pField = (const uint16_t *) hdr;
|
||||
for (uint8_t i = 0; i < (ETH_IPv4_HEADER_SIZE / sizeof(uint16_t)); i++) {
|
||||
uint16_t field = pField[i];
|
||||
if (i != 5) { // 6. 16-bit long field is the checksum itself, do not count in
|
||||
sum += field;
|
||||
}
|
||||
}
|
||||
|
||||
while (sum >> 16) {
|
||||
sum = (sum & 0xFFFF) + (sum >> 16);
|
||||
}
|
||||
|
||||
// invert result
|
||||
uint16_t sum16 = sum ^ 0xFFFF;
|
||||
return sum16;
|
||||
}
|
||||
|
||||
#define ETH_IP_HEADER_LENGTH (20)
|
||||
|
||||
static bool check_ipv4_validity(const uint8_t * hdr, const IPv4Props *ipProps) {
|
||||
bool valid =
|
||||
(ipProps->Version == 4) &&
|
||||
(ipProps->IHL == (ETH_IP_HEADER_LENGTH / 4)) &&
|
||||
(ntohs(ipProps->HeaderChecksum) == ip_checksum(hdr));
|
||||
(ntohs(ipProps->HeaderChecksum) == chksum(hdr, ETH_IPv4_HEADER_SIZE));
|
||||
return valid;
|
||||
}
|
||||
|
||||
@ -97,6 +77,14 @@ void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) {
|
||||
FILL_ADVANCE(hdr, &ipProps->DestIPAddr, 4);
|
||||
|
||||
// calculate checksum after filling header
|
||||
uint16_t checksum = ip_checksum(hdrOrig);
|
||||
memcpy(ChkSumPtr, &checksum, 2);
|
||||
ipProps->HeaderChecksum = chksum(hdrOrig, ETH_IPv4_HEADER_SIZE);
|
||||
memcpy(ChkSumPtr, &ipProps->HeaderChecksum, 2);
|
||||
}
|
||||
|
||||
void ethmc_from_ipmc(uint8_t *hwa, ip4_addr ipa) {
|
||||
memcpy(hwa, ETH_ETHERNET_IPMC_HWADDR, ETH_HW_ADDR_LEN);
|
||||
const uint8_t * ipab = (const uint8_t *) &ipa;
|
||||
hwa[3] = (hwa[3] & 0x80) | (ipab[1] & 0x7F);
|
||||
hwa[4] = ipab[2];
|
||||
hwa[5] = ipab[3];
|
||||
}
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
#include <stdint.h>
|
||||
#include "../../packet_registry.h"
|
||||
#include "../../packet_sieve.h"
|
||||
#include "ipv4_types.h"
|
||||
|
||||
#define ETH_IPv4_PACKET_CLASS (0x0800)
|
||||
#define ETH_IPv4_HEADER_SIZE (20)
|
||||
@ -53,4 +54,11 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe,
|
||||
*/
|
||||
void insert_ipv4_header(uint8_t * hdr, const PcktHeaderElement * headers);
|
||||
|
||||
/**
|
||||
* Calculate Ethernet IPv4 multicast address from multicast
|
||||
* @param hwa
|
||||
* @param ipa
|
||||
*/
|
||||
void ethmc_from_ipmc(uint8_t * hwa, ip4_addr ipa);
|
||||
|
||||
#endif //ETHERLIB_IPV4_PACKET_H
|
||||
|
||||
@ -6,5 +6,6 @@
|
||||
typedef uint32_t ip4_addr;
|
||||
|
||||
#define IPv4_ANY_ADDR (0xFFFFFFFF)
|
||||
#define IPv4_IF_ADDR (0x00000000)
|
||||
|
||||
#endif //ETHERLIB_TEST_IPV4_TYPES_H
|
||||
|
||||
@ -15,35 +15,11 @@ typedef struct {
|
||||
} UdpPseudoHeader;
|
||||
|
||||
static uint16_t udp_checksum(const UdpPseudoHeader *pseudoHeader, const uint8_t * hdr, uint32_t size) {
|
||||
uint32_t chksum = 0;
|
||||
|
||||
// pseudoheader
|
||||
for (uint16_t i = 0; i < sizeof(UdpPseudoHeader) / 2; i++) {
|
||||
uint16_t field = ((uint16_t *) pseudoHeader)[i];
|
||||
field = htons(field);
|
||||
chksum += field;
|
||||
uint32_t sum = chksum((const uint8_t *)pseudoHeader, sizeof(UdpPseudoHeader)) + chksum(hdr, size);
|
||||
while (sum >> 16) {
|
||||
sum = (sum & 0xFFFF) + (sum >> 16);
|
||||
}
|
||||
|
||||
// UDP header and hdr
|
||||
for (uint16_t i = 0; i < size / 2; i++) {
|
||||
if (i != 3) { // skip Checksum field
|
||||
uint16_t field = ((uint16_t *) hdr)[i];
|
||||
field = htons(field);
|
||||
chksum += field;
|
||||
}
|
||||
}
|
||||
|
||||
// pad if packet size is odd
|
||||
if (size % 2) {
|
||||
chksum += ((uint16_t) (hdr[size - 1] << 8));
|
||||
}
|
||||
|
||||
while (chksum >> 16) {
|
||||
chksum = (chksum & 0xFFFF) + (chksum >> 16);
|
||||
}
|
||||
|
||||
uint16_t sum16 = ~(chksum & 0xFFFF);
|
||||
return sum16;
|
||||
return sum;
|
||||
}
|
||||
|
||||
int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
|
||||
@ -67,12 +43,15 @@ void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->SourcePort);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->DestinationPort);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->Length);
|
||||
uint8_t *ChkSumPtr = hdr;
|
||||
udpProps->Checksum = 0;
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum);
|
||||
|
||||
// calculate checksum
|
||||
const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props;
|
||||
UdpPseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, htons(udpProps->Length)};
|
||||
udpProps->Checksum = udp_checksum(&ph, hdrBegin, udpProps->Length);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum);
|
||||
memcpy(ChkSumPtr, &udpProps->Checksum, 2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
181
timer.c
181
timer.c
@ -2,21 +2,196 @@
|
||||
// Created by epagris on 2022.12.21..
|
||||
//
|
||||
|
||||
#include <stdlib.h>
|
||||
#include "timer.h"
|
||||
#include "dynmem.h"
|
||||
#include "utils.h"
|
||||
|
||||
int64_t time_to_us(const TimePoint * t) {
|
||||
return (int64_t)t->s * 1000000 + (int64_t)t->us;
|
||||
}
|
||||
|
||||
void time_from_us(TimePoint * t, int64_t us) {
|
||||
t->s = us / 1000000;
|
||||
t->us = us - ((int64_t)t->s * 1000000);
|
||||
}
|
||||
|
||||
void time_add_us(TimePoint * t, int64_t us) {
|
||||
time_from_us(t, time_to_us(t) + us);
|
||||
}
|
||||
|
||||
// ------------------------------
|
||||
|
||||
Timer *timer_new(uint32_t maxSched) {
|
||||
uint32_t tmrHdrSize = sizeof(TimePoint) + sizeof(AlarmAssignment *) + 2 * sizeof(uint32_t);
|
||||
Timer * tmr = (Timer *) dynmem_alloc(tmrHdrSize + maxSched * sizeof(AlarmAssignment));
|
||||
Timer * tmr = (Timer *) dynmem_alloc(sizeof(Timer) + maxSched * sizeof(AlarmAssignment));
|
||||
ASSERT_NULL(tmr);
|
||||
|
||||
tmr->maxSched = maxSched;
|
||||
tmr->nSched = 0;
|
||||
tmr->nextAlarm = NULL;
|
||||
tmr->time.s = 0;
|
||||
tmr->time.ns = 0;
|
||||
tmr->time.us = 0;
|
||||
memset(tmr->alarms, 0, maxSched * sizeof(AlarmAssignment));
|
||||
|
||||
return tmr;
|
||||
}
|
||||
|
||||
static AlarmAssignment * timer_get_alarm_by_id(Timer * tmr, uint32_t id) {
|
||||
AlarmAssignment * slot = NULL;
|
||||
for (uint32_t i = 0; (i < tmr->maxSched) && (slot == NULL); i++) {
|
||||
if (tmr->alarms[i].id == id) {
|
||||
slot = (tmr->alarms) + i;
|
||||
}
|
||||
}
|
||||
return slot;
|
||||
}
|
||||
|
||||
static uint32_t timer_generate_id(Timer * tmr) {
|
||||
uint32_t newId = 0;
|
||||
do {
|
||||
newId = ((uint32_t) rand()) | 0x01; // ID cannot be 0
|
||||
|
||||
} while (timer_get_alarm_by_id(tmr, newId) != NULL);
|
||||
return newId;
|
||||
}
|
||||
|
||||
static void timer_update_nearest_alarm(Timer * tmr) {
|
||||
if (tmr->nSched == 0) {
|
||||
tmr->nextAlarm = NULL;
|
||||
return;
|
||||
}
|
||||
|
||||
int64_t t_c_us = time_to_us(&tmr->time);
|
||||
int64_t min_delta_t_us = 0;
|
||||
|
||||
AlarmAssignment * nearest = NULL;
|
||||
for (uint32_t i = 0; i < tmr->maxSched; i++) {
|
||||
int64_t t_i_us = time_to_us(&(tmr->alarms[i].time));
|
||||
if ((nearest == NULL) && (tmr->alarms[i].id != 0)) {
|
||||
nearest = tmr->alarms + i;
|
||||
min_delta_t_us = t_i_us - t_c_us;
|
||||
} else {
|
||||
int64_t delta_t_us = t_i_us - t_c_us;
|
||||
if (delta_t_us < min_delta_t_us) {
|
||||
min_delta_t_us = delta_t_us;
|
||||
nearest = tmr->alarms + i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
tmr->nextAlarm = nearest;
|
||||
}
|
||||
|
||||
uint32_t timer_sched(Timer * tmr, const TimePoint * t, TimerAlarmCb cb, AlarmUserData params) {
|
||||
if (tmr->nSched == tmr->maxSched) { // if cannot schedule more alarm
|
||||
return TIMER_SCHED_FAILED;
|
||||
}
|
||||
|
||||
// if we can schedule get the first unused block
|
||||
AlarmAssignment * slot = timer_get_alarm_by_id(tmr, 0);
|
||||
|
||||
// allocate on the first free slot
|
||||
slot->id = timer_generate_id(tmr);
|
||||
slot->cb = cb;
|
||||
slot->time = *t;
|
||||
slot->params = params;
|
||||
|
||||
// increase allocation count
|
||||
tmr->nSched++;
|
||||
|
||||
// replace nearest if needed
|
||||
if (tmr->nSched > 1) {
|
||||
timer_update_nearest_alarm(tmr);
|
||||
} else {
|
||||
tmr->nextAlarm = slot;
|
||||
}
|
||||
|
||||
return slot->id;
|
||||
}
|
||||
|
||||
uint32_t timer_sched_rel(Timer * tmr, int64_t us, TimerAlarmCb cb, AlarmUserData params) {
|
||||
TimePoint t = tmr->time;
|
||||
time_add_us(&t, us);
|
||||
return timer_sched(tmr, &t, cb, params);
|
||||
}
|
||||
|
||||
void timer_unsched(Timer * tmr, uint32_t id) {
|
||||
AlarmAssignment * alarm = timer_get_alarm_by_id(tmr, id);
|
||||
if (alarm != NULL) {
|
||||
memset(alarm, 0, sizeof(AlarmAssignment));
|
||||
tmr->nSched--;
|
||||
}
|
||||
}
|
||||
|
||||
void timer_set_time(Timer *tmr, const TimePoint *t) {
|
||||
tmr->time = *t;
|
||||
}
|
||||
|
||||
int64_t timer_get_time_us(const Timer * tmr) {
|
||||
return time_to_us(&tmr->time);
|
||||
}
|
||||
|
||||
TimePoint timer_get_time(const Timer * tmr) {
|
||||
return tmr->time;
|
||||
}
|
||||
|
||||
void timer_tick(Timer * tmr, int64_t us) {
|
||||
// advance time
|
||||
time_add_us(&tmr->time, us);
|
||||
/*t_us += us;
|
||||
time_from_us(&tmr->time, t_us);*/
|
||||
|
||||
int64_t t_us = timer_get_time_us(tmr);
|
||||
|
||||
if ((tmr->nSched > 0) && (tmr->nextAlarm != NULL)) {
|
||||
int64_t t_alarm = time_to_us(&(tmr->nextAlarm->time));
|
||||
while (((t_alarm - t_us) <= 0) && (tmr->nSched > 0)) {
|
||||
// invoke callback
|
||||
if (tmr->nextAlarm->cb != NULL) {
|
||||
tmr->nextAlarm->cb(tmr, tmr->nextAlarm->params);
|
||||
}
|
||||
|
||||
// clear alarm descriptor
|
||||
memset(tmr->nextAlarm, 0, sizeof(AlarmAssignment));
|
||||
|
||||
// decrease scheduled alarm count
|
||||
tmr->nSched--;
|
||||
|
||||
// update nearest alarm
|
||||
timer_update_nearest_alarm(tmr);
|
||||
if (tmr->nextAlarm != NULL) {
|
||||
t_alarm = time_to_us(&(tmr->nextAlarm->time));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void timer_report(const Timer * tmr) {
|
||||
MSG("Time: %u.%06u\n\n", tmr->time.s, tmr->time.us);
|
||||
|
||||
int64_t t_c_us = timer_get_time_us(tmr);
|
||||
if (tmr->nSched > 0) {
|
||||
for (uint32_t i = 0; i < tmr->maxSched; i++) {
|
||||
|
||||
if (tmr->alarms[i].id == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
int64_t t_delta_us = time_to_us(&tmr->alarms[i].time) - t_c_us;
|
||||
if (t_delta_us < 1000000) {
|
||||
MSG("─ alarm (#%X) in %li us", tmr->alarms[i].id, t_delta_us);
|
||||
} else {
|
||||
TimePoint dt;
|
||||
time_from_us(&dt, t_delta_us);
|
||||
MSG("─ alarm (#%X) in %i.%06i s", tmr->alarms[i].id, dt.s, dt.us);
|
||||
}
|
||||
|
||||
if ((tmr->alarms + i) == tmr->nextAlarm) {
|
||||
MSG(" <-- NEXT ALARM\n");
|
||||
} else {
|
||||
MSG("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
MSG("\nSchedules: %u/%u\n", tmr->nSched, tmr->maxSched);
|
||||
}
|
||||
|
||||
91
timer.h
91
timer.h
@ -2,18 +2,44 @@
|
||||
#define ETHERLIB_TEST_TIMER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/**
|
||||
* A single point in time
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t s; ///< Seconds
|
||||
uint32_t ns; ///< Nanoseconds
|
||||
uint32_t us; ///< Microseconds
|
||||
} TimePoint;
|
||||
|
||||
/**
|
||||
* Convert TimePoint to microseconds.
|
||||
* @param t time
|
||||
* @return time in microseconds
|
||||
*/
|
||||
int64_t time_to_us(const TimePoint * t);
|
||||
|
||||
/**
|
||||
* Convert microseconds to TimePoint.
|
||||
* @param t pointer to TimePoint structure
|
||||
* @param us time in microseconds
|
||||
*/
|
||||
void time_from_us(TimePoint * t, int64_t us);
|
||||
|
||||
/**
|
||||
* Add microseconds to TimePoint.
|
||||
* @param t pointer to TimePoint structure
|
||||
* @param us time in microseconds
|
||||
*/
|
||||
void time_add_us(TimePoint * t, int64_t us);
|
||||
|
||||
#define TIMER_SCHED_FAILED (-1)
|
||||
|
||||
struct Timer_;
|
||||
|
||||
/**
|
||||
* Alarm user data.
|
||||
*/
|
||||
typedef union {
|
||||
void * ptr;
|
||||
uint32_t u;
|
||||
@ -21,7 +47,11 @@ typedef union {
|
||||
|
||||
typedef void (*TimerAlarmCb)(struct Timer_ * timer, AlarmUserData user);
|
||||
|
||||
/**
|
||||
* Alarm assignment.
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t id; ///< Timing ID
|
||||
TimePoint time; ///< Alarm time
|
||||
TimerAlarmCb cb; ///< Pointer to callback function
|
||||
AlarmUserData params; ///< User data passed to callback function
|
||||
@ -45,6 +75,65 @@ typedef struct Timer_ {
|
||||
*/
|
||||
Timer * timer_new(uint32_t maxSched);
|
||||
|
||||
/**
|
||||
* Schedule new alarm.
|
||||
* @param tmr pointer to Timer instance
|
||||
* @param t alarm time
|
||||
* @param cb alarm callback function
|
||||
* @param params user-defined params passed to callback function
|
||||
* @return ID of new schedule OR TIMER_SCHED_FAILED
|
||||
*/
|
||||
uint32_t timer_sched(Timer * tmr, const TimePoint * t, TimerAlarmCb cb, AlarmUserData params);
|
||||
|
||||
/**
|
||||
* Schedule new alarm by specifying time relatively from the current time point.
|
||||
* @param tmr pointer to Timer instance
|
||||
* @param us relative alarm time
|
||||
* @param cb alarm callback function
|
||||
* @param params user-defined params passed to callback function
|
||||
* @return ID of new schedule OR TIMER_SCHED_FAILED
|
||||
*/
|
||||
uint32_t timer_sched_rel(Timer * tmr, int64_t us, TimerAlarmCb cb, AlarmUserData params);
|
||||
|
||||
/**
|
||||
* Unschedule alarm.
|
||||
* @param tmr pointer to Timer instance
|
||||
* @param id schedule ID returned by timer_sched() or timer_sched_rel()
|
||||
*/
|
||||
void timer_unsched(Timer * tmr, uint32_t id);
|
||||
|
||||
/**
|
||||
* Set time.
|
||||
* @param tmr pointer to Timer instance
|
||||
* @param t time
|
||||
*/
|
||||
void timer_set_time(Timer * tmr, const TimePoint * t);
|
||||
|
||||
/**
|
||||
* Get current time in microseconds.
|
||||
* @param tmr pointer to Timer instance
|
||||
* @return current time in microseconds
|
||||
*/
|
||||
int64_t timer_get_time_us(const Timer * tmr);
|
||||
|
||||
/**
|
||||
* Get current time.
|
||||
* @param tmr pointer to Timer instance
|
||||
* @return current time
|
||||
*/
|
||||
TimePoint timer_get_time(const Timer * tmr);
|
||||
|
||||
/**
|
||||
* Advance timer.
|
||||
* @param tmr pointer to Timer instance
|
||||
* @param us time advancement in microseconds
|
||||
*/
|
||||
void timer_tick(Timer * tmr, int64_t us);
|
||||
|
||||
/**
|
||||
* Print Timer report.
|
||||
* @param tmr pointer to Timer instance
|
||||
*/
|
||||
void timer_report(const Timer * tmr);
|
||||
|
||||
#endif //ETHERLIB_TEST_TIMER_H
|
||||
|
||||
22
utils.c
22
utils.c
@ -59,3 +59,25 @@ uint32_t crc32(const uint8_t * data, uint32_t size) {
|
||||
checksum = ~checksum;
|
||||
return checksum;
|
||||
}
|
||||
|
||||
uint16_t chksum(const uint8_t * data, uint32_t size) {
|
||||
// sum fields
|
||||
uint32_t sum = 0;
|
||||
const uint16_t *pField = (const uint16_t *) data;
|
||||
for (uint8_t i = 0; i < (size / sizeof(uint16_t)); i++) {
|
||||
uint16_t field = pField[i];
|
||||
sum += field;
|
||||
}
|
||||
|
||||
if (size % 2) {
|
||||
sum += ((uint16_t) (data[size - 1] << 8));
|
||||
}
|
||||
|
||||
while (sum >> 16) {
|
||||
sum = (sum & 0xFFFF) + (sum >> 16);
|
||||
}
|
||||
|
||||
// invert result
|
||||
uint16_t sum16 = sum ^ 0xFFFF;
|
||||
return sum16;
|
||||
}
|
||||
14
utils.h
14
utils.h
@ -50,15 +50,17 @@
|
||||
#define FETCH_BYTE_ADVANCE(dst,src) FETCH_ADVANCE(dst,src,1)
|
||||
#define FILL_BYTE_ADVANCE(dst,src) FILL_ADVANCE(dst,src,1)
|
||||
#define FETCH_WORD_H2N_ADVANCE(dst,w) { uint16_t u; memcpy(&u, w, 2); u = htons(u); memcpy((dst), &u, 2); (w) += 2; }
|
||||
#define FETCH_WORD_ADVANCE(dst,w) { memcpy(dst, w, 2); (w) += 2; }
|
||||
#define FETCH_WORD_H2N(dst,w) { uint16_t u; memcpy(&u, w, 2); u = htons(u); memcpy((dst), &u, 2) }
|
||||
#define FETCH_WORD(dst,w) { uint16_t u; memcpy(&u, w, 2); memcpy((dst), &u, 2) }
|
||||
#define FETCH_WORD(dst,w) memcpy(dst, w, 2)
|
||||
#define FILL_WORD_ADVANCE(dst,w) { memcpy((dst), &(w), 2); (dst) += 2; }
|
||||
#define FILL_WORD_H2N_ADVANCE(dst,w) { uint16_t u = htons(w); memcpy((dst), &u, 2); (dst) += 2; }
|
||||
#define FETCH_DWORD_H2N_ADVANCE(dst,dw) { uint32_t du; memcpy(&du, dw, 4); du = htonl(du); memcpy((dst), &du, 4); (dw) += 4; }
|
||||
#define FETCH_DWORD_H2N(dst,dw) { uint32_t du; memcpy(&du, dw, 4); du = htonl(du); memcpy((dst), &du, 4); }
|
||||
#define FETCH_DWORD(dst,dw) { uint32_t du; memcpy(&du, dw, 4); memcpy((dst), &du, 4); }
|
||||
#define FETCH_DWORD_ADVANCE(dst,dw) { uint32_t du; memcpy(&du, dw, 4); memcpy((dst), &du, 4); (dw) += 4; }
|
||||
#define FILL_DWORD_H2N_ADVANCE(dst,dw) { uint32_t du = htonl(dw); memcpy(dst, &du, 4); (dst) += 4; }
|
||||
#define FILL_DWORD_ADVANCE(dst,dw) { memcpy(dst, &dw, 4); (dst) += 4; }
|
||||
#define FILL_DWORD_ADVANCE(dst,dw) { memcpy(dst, &(dw), 4); (dst) += 4; }
|
||||
|
||||
#define MIN(a,b) (((a) < (b)) ? (a) : (b))
|
||||
#define MAX(a,b) (((a) > (b)) ? (a) : (b))
|
||||
@ -73,4 +75,12 @@
|
||||
*/
|
||||
uint32_t crc32(const uint8_t * data, uint32_t size);
|
||||
|
||||
/**
|
||||
* Compute IP/UDP/ICMP/TCP etc. 16-bit checksum.
|
||||
* @param data pointer to data block to process
|
||||
* @param size size of data block
|
||||
* @return checksum
|
||||
*/
|
||||
uint16_t chksum(const uint8_t * data, uint32_t size);
|
||||
|
||||
#endif //ETHERLIB_UTILS_H
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user