81 lines
2.5 KiB
C
81 lines
2.5 KiB
C
#include <stddef.h>
|
|
#include "global_state.h"
|
|
#include "dynmem.h"
|
|
|
|
#include "prefab/prefab.h"
|
|
|
|
EthState gEthState;
|
|
|
|
static void register_packet_parsers() {
|
|
PcktClassDesc cdesc;
|
|
// Ethernet frame parser
|
|
cdesc.class = 0;
|
|
cdesc.containerClass = 0;
|
|
cdesc.procFun = parse_ethernet;
|
|
cdesc.propertySize = sizeof(EthernetProps);
|
|
packreg_add(E.pcktReg, &cdesc);
|
|
|
|
// IPv4 packet parser
|
|
cdesc.class = ETH_IPv4_PACKET_CLASS;
|
|
cdesc.containerClass = 0;
|
|
cdesc.procFun = parse_ipv4;
|
|
cdesc.propertySize = sizeof(IPv4Props);
|
|
packreg_add(E.pcktReg, &cdesc);
|
|
|
|
// UDP packet parser
|
|
cdesc.class = ETH_UDP_PACKET_CLASS;
|
|
cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
|
|
cdesc.procFun = parse_udp;
|
|
cdesc.propertySize = sizeof(UDPProps);
|
|
packreg_add(E.pcktReg, &cdesc);
|
|
}
|
|
|
|
void ethlib_init() {
|
|
dynmem_init(); // initialize dynamic memory subsystem
|
|
E.pcktReg = packreg_new(); // create new packet registry
|
|
register_packet_parsers(); // register packet parsers
|
|
}
|
|
|
|
typedef struct PcktHeaderElement_ {
|
|
struct PcktHeaderElement_ * next, * prev; ///< Next and previous header in the linked list
|
|
PcktProps props; ///< Properties (allocated to appropriate size)
|
|
} PcktHeaderElement;
|
|
|
|
#define ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE (2 * sizeof(PcktHeaderElement *))
|
|
|
|
void process_raw_packet(const uint8_t * data, uint32_t size) {
|
|
uint16_t ownClass = 0, containerClass = 0; // Ethernet...
|
|
uint16_t offset = 0;
|
|
PcktHeaderElement * lastHeader = NULL;
|
|
do {
|
|
// get packet descriptor
|
|
const PcktClassDesc * cdesc = packreg_get_by_class(E.pcktReg, ownClass, containerClass);
|
|
if (cdesc == NULL) {
|
|
break;
|
|
}
|
|
|
|
// allocate property object
|
|
PcktHeaderElement * header = (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize);
|
|
header->props.ownPacketClass = ownClass;
|
|
header->props.propSize = cdesc->propertySize;
|
|
header->prev = lastHeader;
|
|
if (lastHeader) {
|
|
lastHeader->next = header;
|
|
}
|
|
|
|
// call parsing function
|
|
cdesc->procFun(data + offset, size - offset, &header->props);
|
|
uint16_t containedClass = header->props.containedPacketClass;
|
|
if (containedClass != 0) {
|
|
containerClass = ownClass;
|
|
offset += header->props.headerSize;
|
|
//dynmem_free(props);
|
|
}
|
|
ownClass = containedClass;
|
|
lastHeader = header;
|
|
} while (ownClass != 0);
|
|
|
|
// TODO process innermost packet
|
|
|
|
return;
|
|
} |