Adding upstream version 2.0.13.upstream/2.0.13upstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

1 files changed, 278 insertions, 0 deletions

diff --git a/usr/kinit/ipconfig/packet.c b/usr/kinit/ipconfig/packet.c
new file mode 100644
index 0000000..2e1487d
--- /dev/null
+++ b/usr/kinit/ipconfig/packet.c
@@ -0,0 +1,278 @@
+#include <errno.h>/*XXX*/
+/*
+ * Packet socket handling glue.
+ */
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <net/if_packet.h>
+#include <netinet/if_ether.h>
+#include <netinet/in.h>
+#include <netpacket/packet.h>
+#include <asm/byteorder.h>
+#include <arpa/inet.h>
+#include <netinet/ip.h>
+#include <netinet/udp.h>
+
+#include "ipconfig.h"
+#include "netdev.h"
+#include "packet.h"
+
+uint16_t cfg_local_port = LOCAL_PORT;
+uint16_t cfg_remote_port = REMOTE_PORT;
+
+int packet_open(struct netdev *dev)
+{
+	struct sockaddr_ll sll;
+	int fd, rv, one = 1;
+
+	/*
+	 * Get a PACKET socket for IP traffic.
+	 */
+	fd = socket(AF_PACKET, SOCK_DGRAM, htons(ETH_P_IP));
+	if (fd == -1) {
+		perror("socket");
+		return -1;
+	}
+
+	/*
+	 * We want to broadcast
+	 */
+	if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &one,
+		       sizeof(one)) == -1) {
+		perror("SO_BROADCAST");
+		close(fd);
+		return -1;
+	}
+
+	memset(&sll, 0, sizeof(sll));
+	sll.sll_family = AF_PACKET;
+	sll.sll_ifindex = dev->ifindex;
+
+	rv = bind(fd, (struct sockaddr *)&sll, sizeof(sll));
+	if (-1 == rv) {
+		perror("bind");
+		close(fd);
+		return -1;
+	}
+
+	dev->pkt_fd = fd;
+	return fd;
+}
+
+void packet_close(struct netdev *dev)
+{
+	close(dev->pkt_fd);
+	dev->pkt_fd = -1;
+}
+
+static unsigned int ip_checksum(uint16_t *hdr, int len)
+{
+	unsigned int chksum = 0;
+
+	while (len) {
+		chksum += *hdr++;
+		chksum += *hdr++;
+		len--;
+	}
+	chksum = (chksum & 0xffff) + (chksum >> 16);
+	chksum = (chksum & 0xffff) + (chksum >> 16);
+	return (~chksum) & 0xffff;
+}
+
+struct header {
+	struct iphdr ip;
+	struct udphdr udp;
+} __attribute__ ((packed, aligned(4)));
+
+static struct header ipudp_hdrs = {
+	.ip = {
+	       .ihl		= 5,
+	       .version		= IPVERSION,
+	       .frag_off	= __constant_htons(IP_DF),
+	       .ttl		= 64,
+	       .protocol	= IPPROTO_UDP,
+	       .saddr		= INADDR_ANY,
+	       .daddr		= INADDR_BROADCAST,
+	       },
+	.udp = {
+		.source		= __constant_htons(LOCAL_PORT),
+		.dest		= __constant_htons(REMOTE_PORT),
+		.len		= 0,
+		.check		= 0,
+		},
+};
+
+#ifdef DEBUG /* Only used with dprintf() */
+static char *ntoa(uint32_t addr)
+{
+	struct in_addr in = { addr };
+	return inet_ntoa(in);
+}
+#endif /* DEBUG */
+
+/*
+ * Send a packet.  The options are listed in iov[1...iov_len-1].
+ * iov[0] is reserved for the bootp packet header.
+ */
+int packet_send(struct netdev *dev, struct iovec *iov, int iov_len)
+{
+	struct sockaddr_ll sll;
+	struct msghdr msg;
+	int i, len = 0;
+
+	memset(&sll, 0, sizeof(sll));
+	msg.msg_name = &sll;
+	msg.msg_namelen = sizeof(sll);
+	msg.msg_iov = iov;
+	msg.msg_iovlen = iov_len;
+	msg.msg_control = NULL;
+	msg.msg_controllen = 0;
+	msg.msg_flags = 0;
+
+	if (cfg_local_port != LOCAL_PORT) {
+		ipudp_hdrs.udp.source = htons(cfg_local_port);
+		ipudp_hdrs.udp.dest = htons(cfg_remote_port);
+	}
+
+	dprintf("\n   udp src %d dst %d", ntohs(ipudp_hdrs.udp.source),
+		ntohs(ipudp_hdrs.udp.dest));
+
+	dprintf("\n   ip src %s ", ntoa(ipudp_hdrs.ip.saddr));
+	dprintf("dst %s ", ntoa(ipudp_hdrs.ip.daddr));
+
+	/*
+	 * Glue in the ip+udp header iovec
+	 */
+	iov[0].iov_base = &ipudp_hdrs;
+	iov[0].iov_len = sizeof(struct header);
+
+	for (i = 0; i < iov_len; i++)
+		len += iov[i].iov_len;
+
+	sll.sll_family	 = AF_PACKET;
+	sll.sll_protocol = htons(ETH_P_IP);
+	sll.sll_ifindex	 = dev->ifindex;
+	sll.sll_hatype	 = dev->hwtype;
+	sll.sll_pkttype	 = PACKET_BROADCAST;
+	sll.sll_halen	 = dev->hwlen;
+	memcpy(sll.sll_addr, dev->hwbrd, dev->hwlen);
+
+	ipudp_hdrs.ip.tot_len = htons(len);
+	ipudp_hdrs.ip.check   = 0;
+	ipudp_hdrs.ip.check   = ip_checksum((uint16_t *) &ipudp_hdrs.ip,
+					    ipudp_hdrs.ip.ihl);
+
+	ipudp_hdrs.udp.len    = htons(len - sizeof(struct iphdr));
+
+	dprintf("\n   bytes %d\n", len);
+
+	return sendmsg(dev->pkt_fd, &msg, 0);
+}
+
+void packet_discard(struct netdev *dev)
+{
+	struct iphdr iph;
+	struct sockaddr_ll sll;
+	socklen_t sllen = sizeof(sll);
+
+	sll.sll_ifindex = dev->ifindex;
+
+	recvfrom(dev->pkt_fd, &iph, sizeof(iph), 0,
+		 (struct sockaddr *)&sll, &sllen);
+}
+
+/*
+ * Receive a bootp packet.  The options are listed in iov[1...iov_len].
+ * iov[0] must point to the bootp packet header.
+ * Returns:
+ * -1 = Error, try again later
+*   0 = Discarded packet (non-DHCP/BOOTP traffic)
+ * >0 = Size of packet
+ */
+int packet_recv(struct netdev *dev, struct iovec *iov, int iov_len)
+{
+	struct iphdr *ip, iph;
+	struct udphdr *udp;
+	struct msghdr msg = {
+		.msg_name	= NULL,
+		.msg_namelen	= 0,
+		.msg_iov	= iov,
+		.msg_iovlen	= iov_len,
+		.msg_control	= NULL,
+		.msg_controllen = 0,
+		.msg_flags	= 0
+	};
+	int ret, iphl;
+	struct sockaddr_ll sll;
+	socklen_t sllen = sizeof(sll);
+
+	sll.sll_ifindex = dev->ifindex;
+	msg.msg_name = &sll;
+	msg.msg_namelen = sllen;
+
+	ret = recvfrom(dev->pkt_fd, &iph, sizeof(struct iphdr),
+		       MSG_PEEK, (struct sockaddr *)&sll, &sllen);
+	if (ret == -1)
+		return -1;
+
+	if (iph.ihl < 5 || iph.version != IPVERSION)
+		goto discard_pkt;
+
+	iphl = iph.ihl * 4;
+
+	ip = malloc(iphl + sizeof(struct udphdr));
+	if (!ip)
+		goto discard_pkt;
+
+	udp = (struct udphdr *)((char *)ip + iphl);
+
+	iov[0].iov_base = ip;
+	iov[0].iov_len = iphl + sizeof(struct udphdr);
+
+	ret = recvmsg(dev->pkt_fd, &msg, 0);
+	if (ret == -1)
+		goto free_pkt;
+
+	dprintf("<- bytes %d ", ret);
+
+	if (ip_checksum((uint16_t *) ip, ip->ihl) != 0)
+		goto free_pkt;
+
+	dprintf("\n   ip src %s ", ntoa(ip->saddr));
+	dprintf("dst %s ", ntoa(ip->daddr));
+
+	if (ntohs(ip->tot_len) > ret || ip->protocol != IPPROTO_UDP)
+		goto free_pkt;
+
+	ret -= 4 * ip->ihl;
+
+	dprintf("\n   udp src %d dst %d ", ntohs(udp->source),
+		ntohs(udp->dest));
+
+	if (udp->source != htons(cfg_remote_port) ||
+	    udp->dest != htons(cfg_local_port))
+		goto free_pkt;
+
+	if (ntohs(udp->len) > ret)
+		goto free_pkt;
+
+	ret -= sizeof(struct udphdr);
+
+	free(ip);
+
+	return ret;
+
+free_pkt:
+	dprintf("freed\n");
+	free(ip);
+	return 0;
+
+discard_pkt:
+	dprintf("discarded\n");
+	packet_discard(dev);
+	return 0;
+}