epagris/EtherLib
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

DHCP initially working; ARP basics OK; memory leaks handled

Browse Source
This commit is contained in:
Wiesner András 2022-12-19 18:12:33 +01:00
parent 3efcde1c4b
commit 0ae4bc9526
20 changed files with 497 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
arp_cache.c
View File

@ -6,11 +6,34 @@
#include "arp_cache.h"
#include "dynmem.h"
#include "utils.h"
#include "eth_interface.h"
#include "prefab/conn_blocks/arp_connblock.h"
#include "prefab/packet_parsers/arp_packet.h"
#include "etherlib_options.h"
ArpCache *arpc_new(uint16_t size) {
ArpCache * arcp = (ArpCache *) dynmem_alloc(2 * sizeof(uint16_t) + size * sizeof(ArpEntry));
static int arpc_recv_cb(const Pckt * pckt, PcktSieveLayerTag tag) {
ArpProps * arpProps = HEADER_FETCH_PROPS(ArpProps, pckt->header);
EthInterface * intf = (EthInterface *) tag.p; // arp_new_connblock() puts pointer to intf into tag field
bool weAreCalled = (arpProps->OPER == ARPOP_REQ) && (arpProps->PTYPE == ETH_IPv4_PACKET_CLASS) &&
(arpProps->TPA == intf->ip) && (intf->ip != 0);
ArpCache * arpc = intf->arpc;
if (weAreCalled) {
arpc_respond(arpc, arpProps->SHA, arpProps->SPA);
}
return 0;
}
ArpCache *arpc_new(EthInterface * intf, uint16_t size) {
// allocate table
ArpCache * arcp = (ArpCache *) dynmem_alloc(2 * sizeof(uint16_t) + sizeof(ConnBlock) + size * sizeof(ArpEntry));
arcp->size = size;
arcp->fill = 0;
// create connblock
arcp->cb = arp_new_connblock(intf, arpc_recv_cb);
return arcp;
}
@ -32,6 +55,7 @@ void arpc_learn(ArpCache *arpc, const ArpEntry *newEntry) {
}
const ArpEntry *arpc_get(ArpCache *arpc, ip4_addr ip) {
// search in the cache
for (uint16_t i = 0; i < arpc->fill; i++) {
ArpEntry * entry = arpc->entries + i;
if (entry->ip == ip) {
@ -42,6 +66,23 @@ const ArpEntry *arpc_get(ArpCache *arpc, ip4_addr ip) {
return NULL;
}
const ArpEntry *arpc_get_ask(ArpCache *arpc, ip4_addr ip) {
const ArpEntry * entry = arpc_get(arpc, ip);
if (entry != NULL) {
return entry;
}
// not found in the cache, probe the network
uint32_t attemptN = 0;
while (((entry = arpc_get(arpc, ip)) == NULL) && (attemptN < ETHLIB_ARP_RETRY_COUNT)) {
arpc_ask(arpc, ip);
ETHLIB_SLEEP(20);
//attemptN++;
}
return entry;
}
void arpc_dump(ArpCache *arpc) {
for (uint16_t i = 0; i < arpc->fill; i++) {
ArpEntry * entry = arpc->entries + i;
@ -50,3 +91,37 @@ void arpc_dump(ArpCache *arpc) {
}
MSG("\n");
}
void arpc_ask(ArpCache *arpc, ip4_addr addr) {
ArpProps arpProps;
arpProps.HTYPE = 1;
arpProps.PTYPE = ETH_IPv4_PACKET_CLASS;
arpProps.HLEN = ETH_HW_ADDR_LEN;
arpProps.PLEN = sizeof(ip4_addr);
arpProps.OPER = ARPOP_REQ;
memcpy(arpProps.SHA, arpc->cb.intf->mac, ETH_HW_ADDR_LEN);
arpProps.SPA = arpc->cb.intf->ip;
memset(arpProps.THA, 0x00, ETH_HW_ADDR_LEN);
arpProps.TPA = addr;
arp_send(&arpc->cb, &arpProps);
}
void arpc_respond(ArpCache *arpc, const EthernetAddress hwAddr, ip4_addr ipAddr) {
ArpProps arpProps;
arpProps.HTYPE = 1;
arpProps.PTYPE = ETH_IPv4_PACKET_CLASS;
arpProps.HLEN = ETH_HW_ADDR_LEN;
arpProps.PLEN = sizeof(ip4_addr);
arpProps.OPER = ARPOP_REP;
memcpy(arpProps.SHA, arpc->cb.intf->mac, ETH_HW_ADDR_LEN);
arpProps.SPA = arpc->cb.intf->ip;
memcpy(arpProps.THA, hwAddr, ETH_HW_ADDR_LEN);
arpProps.TPA = ipAddr;
arp_send(&arpc->cb, &arpProps);
}

14
arp_cache.h
View File

@ -3,9 +3,13 @@
#include "prefab/packet_parsers/ethernet_frame.h"
#include "prefab/packet_parsers/ipv4_types.h"
#include "connection_block.h"
#define ETH_ARP_PACKET_CLASS (0x0806)
struct EthInterface_;
/**
* ARP cache entry record.
*/
@ -20,15 +24,17 @@ typedef struct {
typedef struct {
uint16_t size; ///< Number of cache entries
uint16_t fill; ///< Fill level
ConnBlock cb; ///< Connection block for sending and receiving ARP requests
ArpEntry entries[]; ///< Cache entries
} ArpCache;
/**
* Create new ARP cache table.
* @param intf Related Ethernet interface
* @param size Number of available entries
* @return Pointer to newly allocated ARP cache
*/
ArpCache * arpc_new(uint16_t size);
ArpCache * arpc_new(struct EthInterface_ * intf, uint16_t size);
/**
* Learn new assignment.
@ -50,6 +56,12 @@ const ArpEntry *arpc_get(ArpCache *arpc, ip4_addr ip);
*/
void arpc_free(ArpCache * aprc);
void arpc_ask(ArpCache *arpc, ip4_addr addr);
void arpc_respond(ArpCache *arpc, const EthernetAddress hwAddr, ip4_addr ipAddr);
const ArpEntry *arpc_get_ask(ArpCache *arpc, ip4_addr ip);
/**
* Dump ARP cache contents.
* @param arpc

1
connection_block.c
View File

@ -3,6 +3,7 @@
//
#include "connection_block.h"
#include "eth_interface.h"
#include <stddef.h>

7
connection_block.h
View File

@ -2,19 +2,18 @@
#define ETHERLIB_CONNECTION_BLOCK_H
#include "packet_sieve.h"
#include "eth_interface.h"
struct ConnBlock_;
typedef int (*ConnBlockTransmitFn)(EthInterface * intf, const uint8_t * data, uint32_t size, const struct ConnBlock_ * connBlock);
typedef int (*ConnBlockTransmitFn)(struct EthInterface_ * intf, const uint8_t * data, uint32_t size, const struct ConnBlock_ * connBlock);
/**
* Connection block.
*/
typedef struct ConnBlock_ {
EthInterface * intf; ///< Ethernet interface
struct EthInterface_ * intf; ///< Ethernet interface
PcktSieveLayer * sieveLayer; ///< Sieve layer
ConnBlockTransmitFn transmitFn; ///< Conn-block related transmit function
void * tag; ///< Arbitrary tagging
} ConnBlock;
/**

4
dynmem.c
View File

@ -13,10 +13,10 @@ void dynmem_init() {
ASSERT_NULL(E.mp);
}
void * dynmem_alloc(uint32_t size) {
void * dynmem_alloc_(uint32_t size) {
return mp_alloc(E.mp, size);
}
void dynmem_free(void * ptr) {
void dynmem_free_(void * ptr) {
mp_free(E.mp, ptr);
}

16
dynmem.h
View File

@ -14,12 +14,24 @@ void dynmem_init();
* @param size requested size
* @return pointer to allocated area or NULL on failure
*/
void * dynmem_alloc(uint32_t size);
void * dynmem_alloc_(uint32_t size);
#ifdef DYNMEM_DEBUG
#define dynmem_alloc(size) dynmem_alloc_(size); MSG("ALLOC: %d %s() in %s:%d\n", size, __FUNCTION__, __FILE__, __LINE__)
#else
#define dynmem_alloc(size) dynmem_alloc_(size)
#endif
/**
* Release allocated block.
* @param ptr pointer to allocated area
*/
void dynmem_free(void * ptr);
void dynmem_free_(void * ptr);
#ifdef DYNMEM_DEBUG
#define dynmem_free(ptr) MSG("FREE: %s() in %s:%d\n", __FUNCTION__, __FILE__, __LINE__), dynmem_free_(ptr)
#else
#define dynmem_free(ptr) dynmem_free_(ptr)
#endif
#endif //ETHERLIB_DYNMEM_H

2
eth_interface.c
View File

@ -23,7 +23,7 @@ EthInterface *ethintf_new(EthIODef * io) {
memset(&ethIntf->sieve.layer0, 0, sizeof(PcktSieveLayer));
ethIntf->ioDef = io;
ethIntf->ip = 0;
ethIntf->arpc = arpc_new(ETHLIB_ARPCACHE_SIZE);
ethIntf->arpc = arpc_new(ethIntf, ETHLIB_ARPCACHE_SIZE);
ASSERT_NULL(ethIntf->arpc);
ethintf_register(ethIntf);
return ethIntf;

5
eth_interface.h
View File

@ -5,6 +5,7 @@
#include "prefab/packet_parsers/packet_parsers.h"
#include "prefab/packet_parsers/ipv4_types.h"
#include "arp_cache.h"
#include "connection_block.h"
/**
* Ethernet interface low level definition.
@ -27,7 +28,11 @@ typedef struct EthInterface_ {
EthIODef * ioDef; ///< Low-level IO definitions
EthernetAddress mac; ///< Ethernet address
ip4_addr ip; ///< IP address
ip4_addr router; ///< Router IP address
ip4_addr netmask; ///< Subnet mask
ip4_addr dns; ///< Domain Name Server
ArpCache * arpc; ///< ARP cache
ConnBlock arpCb; ///< ARP connection block
} EthInterface;
/**

4
memory_pool.c
View File

@ -65,7 +65,7 @@ uint8_t *mp_alloc(MP *mp, uint32_t size) {
rec++;
}
// store information on allocated block
// store information on allocated
MPAllocRecord *allocated = bestBlock - 1;
allocated->type = MPRT_ALLOCATED;
allocated->size = size;
@ -105,6 +105,7 @@ static void mp_join_free_blocks(MP *mp) {
joinIter--;
}
mp->freeSpace += sizeof(MPAllocRecord);
mp->blockRecCnt--;
} else {
recIter--;
}
@ -119,6 +120,7 @@ void mp_free(MP *mp, const uint8_t *p) {
if ((recIter->type == MPRT_ALLOCATED) &&
((recIter->addrStart <= p) && ((recIter->addrStart + recIter->size) > p))) { // ...block found
recIter->type = MPRT_FREE;
mp->freeSpace += recIter->size;
success = true;
break;
}

16
packet_sieve.c
View File

@ -41,7 +41,9 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
}
// allocate property object
PcktHeaderElement *header = (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize);
uint32_t hdrSize = ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize;
PcktHeaderElement *header = (PcktHeaderElement *) dynmem_alloc(hdrSize);
memset(header, 0, hdrSize);
header->props.ownPacketClass = ownClass;
header->props.propSize = cdesc->propertySize;
header->prev = lastHeader;
@ -49,7 +51,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
lastHeader->next = header;
}
if (outermostHeader == NULL) {
outermostHeader = lastHeader;
outermostHeader = header;
}
// call parsing function
@ -90,7 +92,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
packet.payload = data + offset;
packet.headerSize = offset;
packet.payloadSize = size - offset;
layer->cbFn(&packet);
layer->cbFn(&packet, layer->tag);
}
headerIter = containedHeader;
} else {
@ -105,7 +107,13 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
// INFO("Packet headers not fully processed!\n");
// }
return;
// release header chain blocks
PcktHeaderElement * iter = outermostHeader;
while (iter != NULL) {
PcktHeaderElement * next = iter->next;
dynmem_free(iter);
iter = next;
}
}
PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass) {

10
packet_sieve.h
View File

@ -51,16 +51,16 @@ typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const P
struct PcktSieveLayer_;
/**
* Callback function type for packet sieve match.
*/
typedef int (*SieveCallBackFn)(const Pckt * pckt);
typedef union {
void * p;
uint32_t u;
} PcktSieveLayerTag;
/**
* Callback function type for packet sieve match.
*/
typedef int (*SieveCallBackFn)(const Pckt * pckt, PcktSieveLayerTag tag);
#define PCKT_SIEVE_INFOTAG_LEN (24)
/**

58
prefab/conn_blocks/arp_connblock.c
View File

@ -14,14 +14,14 @@ static bool filtArp(const PcktSieveFilterCondition * filtCond, const PcktProps *
return ethProps->length_type == ETH_ARP_PACKET_CLASS;
}
ConnBlock arp_new_connblock(EthInterface * intf) {
ConnBlock arp_new_connblock(EthInterface * intf, SieveCallBackFn cb) {
ConnBlock arpConnB; // create ARP connblock
PcktSieveFilterCondition filtCond;
packfiltcond_zero(&filtCond);
PcktSieveLayerTag tag;
tag.u = 0;
arpConnB.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, false, filtArp, NULL, tag, ETH_ARP_PACKET_CLASS);
tag.p = intf;
arpConnB.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, false, filtArp, cb, tag, ETH_ARP_PACKET_CLASS);
ASSERT_NULL(arpConnB.sieveLayer);
arpConnB.intf = intf;
@ -29,4 +29,56 @@ ConnBlock arp_new_connblock(EthInterface * intf) {
SNPRINTF(arpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "ARP");
return arpConnB;
}
void arp_send(const ConnBlock * connBlock, const ArpProps * props) {
// allocate header chain
PcktHeaderElement * arpHeader = ALLOC_HEADER_ELEMENT(ArpProps);
PcktHeaderElement * ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps);
arpHeader->next = NULL;
arpHeader->prev = ethHeader;
ethHeader->next = arpHeader;
ethHeader->prev = NULL;
// fetch props
ArpProps * arpProps = HEADER_FETCH_PROPS(ArpProps, arpHeader);
EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, ethHeader);
// ARP
*arpProps = *props;
// Ethernet
ethProps->length_type = ETH_ARP_PACKET_CLASS;
memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN); // broadcast destination
memcpy(ethProps->sourceAddr, connBlock->intf->mac, ETH_HW_ADDR_LEN); // source
// allocate transmit buffer
uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_ARP_HEADER_SIZE;
uint32_t txBufSize = MAX(txHeaderSize + 4, 60);
uint8_t * txBuf = dynmem_alloc(txBufSize);
// insert Ethernet header
insert_ethernet_header(txBuf, ethHeader);
// insert ARP header
insert_arp_header(txBuf + ETH_ETHERNET_HEADER_SIZE, arpHeader);
// release headers
dynmem_free(arpHeader);
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);
}

6
prefab/conn_blocks/arp_connblock.h
View File

@ -6,10 +6,14 @@
#include <stdint.h>
#include "../../connection_block.h"
#include "../../eth_interface.h"
#include "../packet_parsers/arp_packet.h"
/**
* Create new ARP connection block.
* @param intf associated Ethernet interface
* @param cbFn callback function
* @return ARP connection block
*/
ConnBlock arp_new_connblock(EthInterface * intf);
ConnBlock arp_new_connblock(EthInterface * intf, SieveCallBackFn cb);
void arp_send(const ConnBlock * connBlock, const ArpProps * props);

14
prefab/conn_blocks/udp_connblock.c
View File

@ -36,8 +36,6 @@ ConnBlock udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port,
return udpConnB;
}
#define ALLOC_HEADER_ELEMENT(T) (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(T))
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);
@ -80,7 +78,9 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
// Ethernet
layer = layer->parent;
if (addr != 0xFFFFFFFF) {
memset(ethProps->destAddr, 0x00, ETH_HW_ADDR_LEN); // TODO...
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);
}
@ -104,6 +104,11 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
// 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);
@ -116,5 +121,8 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
rpckt.time_ns = 0;
ethinf_transmit(connBlock->intf, &rpckt);
// release transmit buffer
dynmem_free(txBuf);
return 0;
}

9
prefab/packet_parsers/arp_packet.c
View File

@ -12,12 +12,12 @@ static EthernetAddress emptyEthAddr = { 0x00 };
static void arp_fetch_mapping(const ArpProps * arpProps, ArpCache * arpc) {
ArpEntry entry;
if ((memcmp(arpProps->SHA, emptyEthAddr, ETH_HW_ADDR_LEN)) && (arpProps->SPA != 0)) {
if ((memcmp(arpProps->SHA, emptyEthAddr, ETH_HW_ADDR_LEN) != 0) && (arpProps->SPA != 0)) {
memcpy(entry.eth, arpProps->SHA, ETH_HW_ADDR_LEN);
entry.ip = arpProps->SPA;
arpc_learn(arpc, &entry);
}
if ((memcmp(arpProps->THA, emptyEthAddr, ETH_HW_ADDR_LEN)) && (arpProps->TPA != 0)) {
if ((memcmp(arpProps->THA, emptyEthAddr, ETH_HW_ADDR_LEN) != 0) && (arpProps->TPA != 0)) {
memcpy(entry.eth, arpProps->THA, ETH_HW_ADDR_LEN);
entry.ip = arpProps->TPA;
arpc_learn(arpc, &entry);
@ -37,6 +37,11 @@ int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s
FETCH_ADVANCE(&arpProps->THA, hdr, ETH_HW_ADDR_LEN); // target HW address
FETCH_ADVANCE(&arpProps->TPA, hdr, sizeof(ip4_addr)); // target protocol address
// fill-in common packet header fields
arpProps->validityOK = true;
arpProps->containedPacketClass = 0;
arpProps->headerSize = ETH_ARP_HEADER_SIZE;
// learn new mapping
arp_fetch_mapping(arpProps, intf->arpc);
return 0;

6
prefab/packet_parsers/arp_packet.h
View File

@ -12,6 +12,7 @@
#include "../../packet_sieve.h"
#define ETH_ARP_PACKET_CLASS (0x0806)
#define ETH_ARP_HEADER_SIZE (28)
/**
* Arp operations
@ -29,6 +30,8 @@ typedef enum {
* ARP properties.
*/
typedef struct {
PcktPropsHeader
uint16_t HTYPE; ///< Hardware type (Ethernet: 1)
uint16_t PTYPE; ///< Protocol type (IP: 0x0800)
uint8_t HLEN; ///< Hardware address length
@ -42,4 +45,7 @@ typedef struct {
int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers);
#endif //ETHERLIB_TEST_ARP_PACKET_H

306
prefab/packet_parsers/dhcp.c
View File

@ -10,13 +10,14 @@
#include "../conn_blocks/udp_connblock.h"
static struct {
DhcpState state;
void * buf;
ConnBlock connb;
uint32_t tranId;
DhcpState state;
void *buf;
ConnBlock connb;
uint32_t tranId;
EthInterface *intf;
} s;
static const uint8_t DHCP_MAGIC_COOKIE[] = { 99, 130, 83, 99 };
static const uint8_t DHCP_MAGIC_COOKIE[] = {99, 130, 83, 99};
#define SNAME_LEN (64)
#define FILE_LEN (128)
@ -32,14 +33,41 @@ typedef enum {
DHCPRELEASE = 7
} DhcpMsgType;
static void dhcp_option_insert_msg_type(uint8_t ** bufPtr, int msgType) {
// DHCP option IDs
typedef enum {
DHCP_OPT_SubnetMask = 0x1,
DHCP_OPT_Router = 0x3,
DHCP_OPT_DomainNameServer = 0x6,
DHCP_OPT_RequestedIpAddress = 0x32,
DHCP_OPT_IPAddrLeaseTime = 0x33,
DHCP_OPT_MsgType = 0x35,
DHCP_OPT_ServerId = 0x36,
DHCP_OPT_ParamReqList = 0x37,
DHCP_OPT_MaxMsgSize = 0x39,
DHCP_OPT_ClientIdentifier = 0x3d,
DHCP_OPT_End = 0xFF,
} DhcpOptionId;
#define MAX_DHCP_OPTION_LENGTH (16)
#define DHCP_HW_TYPE_ETHERNET (1)
// DHCP options
typedef struct DhcpOption_ {
uint8_t id; ///< Options
uint8_t length; ///< Option data length
struct DhcpOption_ *next; /// next DHCP option
uint8_t value[MAX_DHCP_OPTION_LENGTH]; ///< Option value
} DhcpOption;
static void dhcp_option_insert_msg_type(uint8_t **bufPtr, int msgType) {
(*bufPtr)[0] = 0x35;
(*bufPtr)[1] = 1;
(*bufPtr)[2] = msgType;
(*bufPtr) += 3;
}
static void dhcp_option_insert_max_msg_size(uint8_t ** bufPtr, uint16_t maxSize) {
static void dhcp_option_insert_max_msg_size(uint8_t **bufPtr, uint16_t maxSize) {
(*bufPtr)[0] = 0x39;
(*bufPtr)[1] = 2;
(*bufPtr)[2] = (maxSize >> 8) & 0xFF;
@ -47,67 +75,253 @@ static void dhcp_option_insert_max_msg_size(uint8_t ** bufPtr, uint16_t maxSize)
(*bufPtr) += 4;
}
static void dhcp_option_insert_end(uint8_t ** bufPtr) {
static void dhcp_option_insert_end(uint8_t **bufPtr) {
(*bufPtr)[0] = 0xFF;
(*bufPtr) += 1;
}
static void dhcp_send(EthInterface * intf, DhcpOps * ops) {
// construct message
uint8_t * buf = (uint8_t *)s.buf;
static void dhcp_read_next_option(const uint8_t **buf, DhcpOption *opt) {
opt->id = (*buf)[0];
if (opt->id != DHCP_OPT_End) {
opt->length = (*buf)[1];
memcpy(opt->value, (*buf) + 2, MIN(opt->length, MAX_DHCP_OPTION_LENGTH));
(*buf) += 2 + opt->length;
} else {
(*buf) += 1;
}
}
static void dhcp_insert_option(uint8_t **buf, const DhcpOption *opt) {
(*buf)[0] = opt->id;
if (opt->id != DHCP_OPT_End) {
(*buf)[1] = opt->length;
memcpy((*buf) + 2, opt->value, MIN(opt->length, MAX_DHCP_OPTION_LENGTH));
(*buf) += 2 + opt->length;
} else {
(*buf) += 1;
}
}
static void dhcp_free_opt_chain(DhcpOption *opt) {
DhcpOption *iter = opt;
while (iter) {
DhcpOption *next = iter->next;
dynmem_free(iter);
iter = next;
}
}
static const DhcpOption *dhcp_get_option(const DhcpOption *opts, DhcpOptionId id) {
const DhcpOption *iter = opts;
while (iter) {
if (iter->id == id) {
return iter;
}
iter = iter->next;
}
return NULL;
}
static void dhcp_send(const DhcpProps *props, const DhcpOption *opts) {
// construct body
uint8_t *buf = (uint8_t *) s.buf;
memset(buf, 0, DHCP_MIN_PACKET_SIZE);
FILL_BYTE_ADVANCE(buf, &(ops->op));
FILL_BYTE_ADVANCE(buf, &(ops->htype));
FILL_BYTE_ADVANCE(buf, &(ops->hlen));
FILL_BYTE_ADVANCE(buf, &(ops->hops));
FILL_DWORD_H2N_ADVANCE(buf, ops->xid);
FILL_WORD_H2N_ADVANCE(buf, ops->secs);
FILL_WORD_H2N_ADVANCE(buf, ops->flags);
FILL_DWORD_H2N_ADVANCE(buf, ops->ciaddr);
FILL_DWORD_H2N_ADVANCE(buf, ops->yiaddr);
FILL_DWORD_H2N_ADVANCE(buf, ops->siaddr);
FILL_DWORD_H2N_ADVANCE(buf, ops->giaddr);
FILL_ADVANCE(buf, ops->chaddr, 16);
FILL_BYTE_ADVANCE(buf, &(props->op));
FILL_BYTE_ADVANCE(buf, &(props->htype));
FILL_BYTE_ADVANCE(buf, &(props->hlen));
FILL_BYTE_ADVANCE(buf, &(props->hops));
FILL_DWORD_H2N_ADVANCE(buf, props->xid);
FILL_WORD_H2N_ADVANCE(buf, props->secs);
FILL_WORD_H2N_ADVANCE(buf, props->flags);
FILL_ADVANCE(buf, &(props->ciaddr), 4);
FILL_ADVANCE(buf, &(props->yiaddr), 4);
FILL_ADVANCE(buf, &(props->siaddr), 4);
FILL_ADVANCE(buf, &(props->giaddr), 4);
FILL_ADVANCE(buf, props->chaddr, 16);
buf += SNAME_LEN + FILE_LEN;
FILL_ADVANCE(buf, DHCP_MAGIC_COOKIE, 4); // DHCP magic cookie
// insert options
dhcp_option_insert_msg_type(&buf, DHCPDISCOVER);
dhcp_option_insert_max_msg_size(&buf, 1500);
dhcp_option_insert_end(&buf);
const DhcpOption *iter = opts;
while (iter) {
dhcp_insert_option(&buf, iter);
iter = iter->next;
}
// dhcp_option_insert_msg_type(&buf, DHCPDISCOVER);
// dhcp_option_insert_max_msg_size(&buf, 1500);
// dhcp_option_insert_end(&buf);
// send packet
udp_sendto(&s.connb, s.buf, DHCP_MIN_PACKET_SIZE, IPv4_ANY_ADDR, DHCP_SERVER_PORT);
}
static void dhcp_discover(EthInterface * intf) {
s.tranId = rand();
DhcpOps ops = { 0 };
static void dhcp_parse(const uint8_t *buf, DhcpProps *props, DhcpOption **opts) {
// parse body
FETCH_BYTE_ADVANCE(&(props->op), buf);
FETCH_BYTE_ADVANCE(&(props->htype), buf);
FETCH_BYTE_ADVANCE(&(props->hlen), buf);
FETCH_BYTE_ADVANCE(&(props->hops), buf);
FETCH_DWORD_H2N_ADVANCE(&(props->xid), buf);
FETCH_WORD_H2N_ADVANCE(&(props->secs), buf);
FETCH_WORD_H2N_ADVANCE(&(props->flags), buf);
FETCH_ADVANCE(&(props->ciaddr), buf, 4);
FETCH_ADVANCE(&(props->yiaddr), buf, 4);
FETCH_ADVANCE(&(props->siaddr), buf, 4);
FETCH_ADVANCE(&(props->giaddr), buf, 4);
FETCH_ADVANCE(props->chaddr, buf, 16);
buf += SNAME_LEN + FILE_LEN;
uint8_t magicCookie[sizeof(DHCP_MAGIC_COOKIE)];
FETCH_ADVANCE(magicCookie, buf, 4); // DHCP magic cookie
ops.op = DHCP_BOOTREQUEST;
ops.htype = 1;
ops.hlen = 6;
ops.hops = 0;
ops.xid = s.tranId;
ops.secs = 0;
ops.flags = 0;
ops.ciaddr = 0;
ops.yiaddr = 0;
ops.siaddr = 0;
ops.giaddr = 0;
// parse options
*opts = dynmem_alloc(sizeof(DhcpOption));
(*opts)->next = NULL;
dhcp_read_next_option(&buf, (*opts));
DhcpOption *iter = *opts;
memcpy(ops.chaddr, intf->mac, ETH_HW_ADDR_LEN);
while (iter->id != DHCP_OPT_End) {
iter->next = dynmem_alloc(sizeof(DhcpOption));
iter = iter->next;
dhcp_read_next_option(&buf, iter);
iter->next = NULL;
}
dhcp_send(intf, &ops);
}
static int dhcp_resp_cb(const Pckt * pckt) {
#define UINT16_TO_BE_BYTES(u) ((u) >> 8) & 0xFF, ((u) & 0xFF),
#define UINT32_TO_BE_BYTES(u) ((u) >> 24) & 0xFF, ((u) >> 16) & 0xFF, ((u) >> 8) & 0xFF, ((u) & 0xFF),
#define IPv4_ADDR_TO_BE_BYTES(addr) ((addr) & 0xFF), (((addr) >> 8) & 0xFF), (((addr) >> 16) & 0xFF), (((addr) >> 24) & 0xFF),
#define HWADDR_TO_BE_BYTES(hwa) (hwa)[0], (hwa)[1], (hwa)[2], (hwa)[3], (hwa)[4], (hwa)[5],
static void dhcp_discover() {
s.tranId = rand();
DhcpProps props = {0};
props.op = DHCP_BOOTREQUEST;
props.htype = DHCP_HW_TYPE_ETHERNET;
props.hlen = 6;
props.hops = 0;
props.xid = s.tranId;
props.secs = 0;
props.flags = 0;
props.ciaddr = 0;
props.yiaddr = 0;
props.siaddr = 0;
props.giaddr = 0;
memcpy(props.chaddr, s.intf->mac, ETH_HW_ADDR_LEN);
DhcpOption optEnd = {DHCP_OPT_End, 0, NULL};
uint16_t maxSize = 1500;
DhcpOption maxMsgSize = {DHCP_OPT_MaxMsgSize, 2, &optEnd, {UINT16_TO_BE_BYTES(maxSize)}};
DhcpOption msgType = {DHCP_OPT_MsgType, 1, &maxMsgSize, {DHCPDISCOVER}};
dhcp_send(&props, &msgType);
}
void dhcp_request(ip4_addr reqAddr, ip4_addr dhcpServerAddr) {
DhcpProps props = {0};
props.op = DHCP_BOOTREQUEST;
props.htype = DHCP_HW_TYPE_ETHERNET;
props.hlen = 6;
props.hops = 0;
props.xid = s.tranId;
props.secs = 0;
props.flags = 0;
props.ciaddr = 0;
props.yiaddr = 0;
props.siaddr = 0;
props.giaddr = 0;
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 reqIp = {DHCP_OPT_RequestedIpAddress, 4, &paramReq, {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}};
dhcp_send(&props, &msgType);
}
static void dhcp_process(DhcpProps *props, DhcpOption *opts) {
switch (s.state) {
case DHCP_INIT:
dhcp_discover(); // send discover message
s.state = DHCP_SELECTING;
break;
case DHCP_SELECTING: {
const DhcpOption *msgType = dhcp_get_option(opts, DHCP_OPT_MsgType);
if (msgType->value[0] == DHCPOFFER) {
ip4_addr addrOffer = props->yiaddr;
ip4_addr serverAddr = props->siaddr;
dhcp_request(addrOffer, serverAddr);
s.state = DHCP_REQUESTING;
}
}
break;
case DHCP_REQUESTING: {
const DhcpOption *opt = dhcp_get_option(opts, DHCP_OPT_MsgType);
uint8_t msgType = opt->value[0];
if (msgType == DHCPNAK) {
//dhcp_discover();
//s.state = DHCP_SELECTING;
} else if (msgType == DHCPACK) {
s.intf->ip = props->yiaddr; // fetch ip address
opt = dhcp_get_option(opts, DHCP_OPT_Router); // get gateway/router address
FETCH_DWORD(&(s.intf->router), opt->value);
opt = dhcp_get_option(opts, DHCP_OPT_SubnetMask); // get subnet mask
FETCH_DWORD(&(s.intf->netmask), opt->value);
opt = dhcp_get_option(opts, DHCP_OPT_DomainNameServer); // get DNS
FETCH_DWORD(&(s.intf->dns), opt->value);
MSG("DHCP done!\n");
MSG("IP: ");
PRINT_IPv4(s.intf->ip);
MSG("\nRouter: ");
PRINT_IPv4(s.intf->router);
MSG("\nNetmask: ");
PRINT_IPv4(s.intf->netmask);
MSG("\nDNS: ");
PRINT_IPv4(s.intf->dns);
MSG("\n");
s.state = DHCP_BOUND;
}
}
break;
default:
break;
}
}
static int dhcp_resp_cb(const Pckt *pckt, PcktSieveLayerTag tag) {
DhcpProps props;
DhcpOption *opts = NULL;
dhcp_parse(pckt->payload, &props, &opts);
dhcp_process(&props, opts);
dhcp_free_opt_chain(opts);
return 0;
}
void dhcp_start() {
s.state = DHCP_INIT;
dhcp_process(NULL, NULL);
}
void dhcp_initiate(EthInterface *intf) {
s.state = DHCP_INIT_REBOOT;
s.state = DHCP_INIT;
s.buf = dynmem_alloc(DHCP_MIN_PACKET_SIZE);
s.connb = udp_new_connblock(intf, IPv4_ANY_ADDR, DHCP_CLIENT_PORT, dhcp_resp_cb);
dhcp_discover(intf);
s.intf = intf;
}

10
prefab/packet_parsers/dhcp.h
View File

@ -14,12 +14,12 @@ typedef struct {
uint16_t flags; ///< Flags
uint32_t ciaddr; ///< Client IP address (filled in DHCPREQUEST)
uint32_t yiaddr; ///< 'Your' client IP address
uint32_t siaddr; ///< ...
uint32_t giaddr; ///< ...
uint32_t siaddr; ///< DHCP server IP address
uint32_t giaddr; ///< Relay agent IP address
uint8_t chaddr[16]; ///< Client hardware address
char * sname; ///< Optional server host name
char * file; ///< Boot file name
} DhcpOps;
} DhcpProps;
/**
* DHCP state.
@ -36,7 +36,7 @@ typedef enum {
} DhcpState;
/**
* DHCP op codes.
* DHCP id codes.
*/
typedef enum {
DHCP_BOOTREQUEST = 1,
@ -53,4 +53,6 @@ typedef enum {
*/
void dhcp_initiate(EthInterface * intf);
void dhcp_start();
#endif //ETHERLIB_TEST_DHCP_H

2
prefab/packet_parsers/ethernet_frame.h
View File

@ -14,6 +14,8 @@
#define HEADER_FETCH_PROPS(T,h) (T *)(&((h)->props))
#define ALLOC_HEADER_ELEMENT(T) (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(T))
typedef uint8_t EthernetAddress[ETH_HW_ADDR_LEN];
/**

9
utils.h
View File

@ -38,6 +38,7 @@
#define IPv4(a,b,c,d) ((a) | (b << 8) | (c << 16) | (d << 24))
#define PRINT_IPv4(ip) MSG("%u.%u.%u.%u", (ip & 0xFF), ((ip >> 8) & 0xFF), ((ip >> 16) & 0xFF), ((ip >> 24) & 0xFF))
#define PRINT_HWADDR(hwaddr) MSG("%02x:%02x:%02x:%02x:%02x:%02x", (hwaddr)[0], (hwaddr)[1], (hwaddr)[2], (hwaddr)[3], (hwaddr)[4], (hwaddr)[5]);
#define ASSERT_BAD_ALIGN(p) if ((size_t)(p) & 0b11) ERROR("Bad memory alignment in function '%s' in file '%s' on line %d!\n", __func__, __FILE__, __LINE__)
#define ASSERT_NULL(p) if ((p) == NULL) ERROR("NULL in function '%s' in file '%s' on line %d!\n", __func__, __FILE__, __LINE__)
@ -49,10 +50,18 @@
#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_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 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 MIN(a,b) (((a) < (b)) ? (a) : (b))
#define MAX(a,b) (((a) > (b)) ? (a) : (b))
// ------------------