Skip to content

S
Software for White Rabbit PTP Core
Commit f8e1de8a authored by Tomasz Wlostowski's avatar Tomasz Wlostowski
Browse files
Options

imported Wesley's ARP/ICMP/IP handling code from etherbone-demo

parent b8d71cb2
Show whitespace changes
Inline Side-by-side
Showing with 185 additions and 37 deletions
lib/arp.c
View file @ f8e1de8a
static wr_socket_t arp_socket;
static uint8_t myMAC[6];
static uint8_t myIP[4];
#include <string.h>
#include "ipv4.h"
#include "ptpd_netif.h"
static wr_socket_t* arp_socket;
#define ARP_HTYPE 0
#define ARP_PTYPE (ARP_HTYPE+2)
......@@ -13,23 +16,8 @@ static uint8_t myIP[4];
#define ARP_TPA (ARP_THA+6)
#define ARP_END (ARP_TPA+4)
unsigned int ipv4_checksum(unsigned short* buf, int shorts) {
int i;
unsigned int sum;
sum = 0;
for (i = 0; i < shorts; ++i)
sum += buf[i];
sum = (sum >> 16) + (sum & 0xffff);
sum += (sum >> 16);
return (~sum & 0xffff);
}
void arp_init(const char* if_name, uint32_t ip) {
void arp_init(const char* if_name) {
wr_sockaddr_t saddr;
uint32_t ip_bigendian;
/* Configure socket filter */
memset(&saddr, 0, sizeof(saddr));
......@@ -38,26 +26,23 @@ void arp_init(const char* if_name, uint32_t ip) {
saddr.ethertype = htons(0x0806); /* ARP */
saddr.family = PTPD_SOCK_RAW_ETHERNET;
/* Configure ARP parameters */
ip_bigendian = htonl(ip);
memcpy(myIP, &ip_bigendian, 4);
get_mac_addr(myMAC);
arp_socket = ptpd_netif_create_socket(PTPD_SOCK_RAW_ETHERNET, 0, &saddr);
}
static int process_arp(char* buffer) {
static int process_arp(uint8_t* buf, int len) {
uint8_t hisMAC[6];
uint8_t hisIP[4];
if (len < ARP_END) return 0;
/* Is it ARP request targetting our IP? */
if (buffer[ARP_OPER+0] != 0 ||
buffer[ARP_OPER+1] != 1 ||
memcmp(buffer+ARP_TPA, myIP, 4))
if (buf[ARP_OPER+0] != 0 ||
buf[ARP_OPER+1] != 1 ||
memcmp(buf+ARP_TPA, myIP, 4))
return 0;
memcpy(hisMAC, buffer+ARP_SHA, 6);
memcpy(hisIP, buffer+ARP_SPA, 4);
memcpy(hisMAC, buf+ARP_SHA, 6);
memcpy(hisIP, buf+ARP_SPA, 4);
// ------------- ARP ------------
// HW ethernet
......@@ -79,15 +64,15 @@ static int process_arp(char* buffer) {
memcpy(buf+ARP_THA, hisMAC, 6);
memcpy(buf+ARP_TPA, hisIP, 4);
return 1;
return ARP_END;
}
void arp_poll() {
char buffer[ARP_END];
void arp_poll(void) {
uint8_t buf[ARP_END+100];
wr_sockaddr_t addr;
int len;
if ((len = ptpd_netif_recvfrom(&arp_socket, &addr, buffer, sizeof(buffer), 0)) > 0)
if (process_arp(buffer) > 0)
ptp_netif_sendto(&arp_socket, &addr, buffer, sizeof(buffer), 0);
if ((len = ptpd_netif_recvfrom(arp_socket, &addr, buf, sizeof(buf), 0)) > 0)
if ((len = process_arp(buf, len)) > 0)
ptpd_netif_sendto(arp_socket, &addr, buf, len, 0);
}
lib/icmp.c 0 → 100644
View file @ f8e1de8a
#include <string.h>
#include "ipv4.h"
#include "ptpd_netif.h"
static wr_socket_t* icmp_socket;
#define IP_VERSION 0
#define IP_TOS (IP_VERSION+1)
#define IP_LEN (IP_TOS+1)
#define IP_ID (IP_LEN+2)
#define IP_FLAGS (IP_ID+2)
#define IP_TTL (IP_FLAGS+2)
#define IP_PROTOCOL (IP_TTL+1)
#define IP_CHECKSUM (IP_PROTOCOL+1)
#define IP_SOURCE (IP_CHECKSUM+2)
#define IP_DEST (IP_SOURCE+4)
#define IP_END (IP_DEST+4)
#define ICMP_TYPE (IP_END)
#define ICMP_CODE (ICMP_TYPE+1)
#define ICMP_CHECKSUM (ICMP_CODE+1)
#define ICMP_END (ICMP_CHECKSUM+2)
void icmp_init(const char* if_name) {
wr_sockaddr_t saddr;
/* Configure socket filter */
memset(&saddr, 0, sizeof(saddr));
strcpy(saddr.if_name, if_name);
memcpy(&saddr.mac, myMAC, 6); /* Unicast */
saddr.ethertype = htons(0x0800); /* IPv4 */
saddr.family = PTPD_SOCK_RAW_ETHERNET;
icmp_socket = ptpd_netif_create_socket(PTPD_SOCK_RAW_ETHERNET, 0, &saddr);
}
static int process_icmp(uint8_t* buf, int len) {
int iplen, hisBodyLen;
uint8_t hisIP[4];
uint16_t sum;
/* Is it IP targetting us? */
if (buf[IP_VERSION] != 0x45 ||
memcmp(buf+IP_DEST, myIP, 4))
return 0;
iplen = (buf[IP_LEN+0] << 8 | buf[IP_LEN+1]);
/* An ICMP ECHO request? */
if (buf[IP_PROTOCOL] != 0x01 || buf[ICMP_TYPE] != 0x08)
return 0;
hisBodyLen = iplen - 24;
if (hisBodyLen > 64) hisBodyLen = 64;
memcpy(hisIP, buf+IP_SOURCE, 4);
// ------------ IP --------------
buf[IP_VERSION] = 0x45;
buf[IP_TOS] = 0;
buf[IP_LEN+0] = (hisBodyLen+24) >> 8;
buf[IP_LEN+1] = (hisBodyLen+24) & 0xff;
buf[IP_ID+0] = 0;
buf[IP_ID+1] = 0;
buf[IP_FLAGS+0] = 0;
buf[IP_FLAGS+1] = 0;
buf[IP_TTL] = 63;
buf[IP_PROTOCOL] = 1; /* ICMP */
buf[IP_CHECKSUM+0] = 0;
buf[IP_CHECKSUM+1] = 0;
memcpy(buf+IP_SOURCE, myIP, 4);
memcpy(buf+IP_DEST, hisIP, 4);
// ------------ ICMP ---------
buf[ICMP_TYPE] = 0x0; // echo reply
buf[ICMP_CODE] = 0;
buf[ICMP_CHECKSUM+0] = 0;
buf[ICMP_CHECKSUM+1] = 0;
// No need to copy payload; we modified things in-place
sum = ipv4_checksum((unsigned short*)(buf+ICMP_TYPE), (hisBodyLen+4+1)/2);
buf[ICMP_CHECKSUM+0] = sum >> 8;
buf[ICMP_CHECKSUM+1] = sum & 0xff;
sum = ipv4_checksum((unsigned short*)(buf+IP_VERSION), 10);
buf[IP_CHECKSUM+0] = sum >> 8;
buf[IP_CHECKSUM+1] = sum & 0xff;
return 24+hisBodyLen;
}
void icmp_poll(void) {
uint8_t buf[ICMP_END+136];
wr_sockaddr_t addr;
int len;
if ((len = ptpd_netif_recvfrom(icmp_socket, &addr, buf, sizeof(buf), 0)) > 0)
if ((len = process_icmp(buf, len)) > 0)
ptpd_netif_sendto(icmp_socket, &addr, buf, len, 0);
}
lib/ipv4.c 0 → 100644
View file @ f8e1de8a
#include <string.h>
#include "endpoint.h"
#include "ipv4.h"
uint8_t myMAC[6];
uint8_t myIP[4];
unsigned int ipv4_checksum(unsigned short* buf, int shorts) {
int i;
unsigned int sum;
sum = 0;
for (i = 0; i < shorts; ++i)
sum += buf[i];
sum = (sum >> 16) + (sum & 0xffff);
sum += (sum >> 16);
return (~sum & 0xffff);
}
void ipv4_init(const char* if_name, uint32_t ip) {
uint32_t ip_bigendian;
ip_bigendian = htonl(ip);
memcpy(myIP, &ip_bigendian, 4);
get_mac_addr(myMAC);
arp_init(if_name);
icmp_init(if_name);
TRACE_DEV("My IP: %d.%d.%d.%d\n", myIP[0], myIP[1], myIP[2], myIP[3]);
}
void ipv4_poll(void) {
arp_poll();
icmp_poll();
}
lib/ipv4.h 0 → 100644
View file @ f8e1de8a
#ifndef IPV4_H
#define IPV4_H
void ipv4_init(const char* if_name, uint32_t ip);
void ipv4_poll(void);
/* Internal to IP stack: */
unsigned int ipv4_checksum(unsigned short* buf, int shorts);
void arp_init(const char* if_name);
void icmp_init(const char* if_name);
void arp_poll(void);
void icmp_poll(void);
extern uint8_t myMAC[6];
extern uint8_t myIP[4];
#endif
lib/lib.mk
View file @ f8e1de8a
OBJS_LIB= lib/mprintf.o \
lib/util.o
ifneq ($(WITH_ETHERBONE), 0)
OBJS_LIB += lib/arp.o lib/icmp.o lib/ipv4.o
endif
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment