From d1d592fae3483b023f4214094d1d316d7e8ababc Mon Sep 17 00:00:00 2001
From: "Wesley W. Terpstra" <w.terpstra@gsi.de>
Date: Mon, 27 Feb 2012 15:00:25 +0100
Subject: [PATCH] Configure the ep_filter to accept ARP broadcasts to LM32 Hook
a new socket to tap ARP messages Process ARP messages into replies
---
lib/arp.c | 93 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 93 insertions(+)
create mode 100644 lib/arp.c
diff --git a/lib/arp.c b/lib/arp.c
new file mode 100644
index 000000000..b8e76b7d5
--- /dev/null
+++ b/lib/arp.c
@@ -0,0 +1,93 @@
+static wr_socket_t arp_socket;
+static uint8_t myMAC[6];
+static uint8_t myIP[4];
+
+#define ARP_HTYPE 0
+#define ARP_PTYPE (ARP_HTYPE+2)
+#define ARP_HLEN (ARP_PTYPE+2)
+#define ARP_PLEN (ARP_HLEN+1)
+#define ARP_OPER (ARP_PLEN+1)
+#define ARP_SHA (ARP_OPER+2)
+#define ARP_SPA (ARP_SHA+6)
+#define ARP_THA (ARP_SPA+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) {
+ wr_sockaddr_t saddr;
+ uint32_t ip_bigendian;
+
+ /* Configure socket filter */
+ memset(&saddr, 0, sizeof(saddr));
+ strcpy(saddr.if_name, if_name);
+ memset(&saddr.mac, 0xFF, 6); /* Broadcast */
+ 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) {
+ uint8_t hisMAC[6];
+ uint8_t hisIP[4];
+
+ /* Is it ARP request targetting our IP? */
+ if (buffer[ARP_OPER+0] != 0 ||
+ buffer[ARP_OPER+1] != 1 ||
+ memcmp(buffer+ARP_TPA, myIP, 4))
+ return 0;
+
+ memcpy(hisMAC, buffer+ARP_SHA, 6);
+ memcpy(hisIP, buffer+ARP_SPA, 4);
+
+ // ------------- ARP ------------
+ // HW ethernet
+ buf[ARP_HTYPE+0] = 0;
+ buf[ARP_HTYPE+1] = 1;
+ // proto IP
+ buf[ARP_PTYPE+0] = 8;
+ buf[ARP_PTYPE+1] = 0;
+ // lengths
+ buf[ARP_HLEN] = 6;
+ buf[ARP_PLEN] = 4;
+ // Response
+ buf[ARP_OPER+0] = 0;
+ buf[ARP_OPER+1] = 2;
+ // my MAC+IP
+ memcpy(buf+ARP_SHA, myMAC, 6);
+ memcpy(buf+ARP_SPA, myIP, 4);
+ // his MAC+IP
+ memcpy(buf+ARP_THA, hisMAC, 6);
+ memcpy(buf+ARP_TPA, hisIP, 4);
+
+ return 1;
+}
+
+void arp_poll() {
+ char buffer[ARP_END];
+ 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);
+}
--
GitLab