summaryrefslogtreecommitdiffstats
path: root/usr/kinit/ipconfig/packet.c
diff options
context:
space:
mode:
Diffstat (limited to 'usr/kinit/ipconfig/packet.c')
-rw-r--r--usr/kinit/ipconfig/packet.c278
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;
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-11 01:12:13 +0000