Logo Search packages:      
Sourcecode: yaird version File versions  Download package

packet.c

/*
 * ipconfig/packet.c
 *
 * 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"

static int pkt_fd = -1;

__u16 cfg_local_port = LOCAL_PORT;
__u16 cfg_remote_port = REMOTE_PORT;

int packet_open(void)
{
      int fd, one = 1;

      if (pkt_fd != -1)
            return pkt_fd;
      
      /*
       * 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);
            fd = -1;
      }

      pkt_fd = fd;

      return fd;
}

void packet_close(void)
{
      close(pkt_fd);
      pkt_fd = -1;
}

static unsigned int ip_checksum(__u16 *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));

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 IPC_DEBUG        /* Only used by DEBUG(()) */
static char *ntoa(__u32 addr)
{
      struct in_addr in = { addr };
      return inet_ntoa(in);
}
#endif

/*
 * Send a packet.  The options are listed in iov[1...iov_len].
 * 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 = {
            .msg_name   = &sll,
            .msg_namelen      = sizeof(sll),
            .msg_iov    = iov,
            .msg_iovlen = iov_len,
            .msg_control      = NULL,
            .msg_controllen   = 0,
            .msg_flags  = 0
      };
      int i, len = 0;

      if (cfg_local_port != LOCAL_PORT) {
            ipudp_hdrs.udp.source = htons(cfg_local_port);
            ipudp_hdrs.udp.dest = htons(cfg_remote_port);
      }
      
      DEBUG(("\n   udp src %d dst %d", ntohs(ipudp_hdrs.udp.source),
             ntohs(ipudp_hdrs.udp.dest)));

      DEBUG(("\n   ip src %s ", ntoa(ipudp_hdrs.ip.saddr)));
      DEBUG(("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((__u16 *)&ipudp_hdrs.ip,
                                    ipudp_hdrs.ip.ihl);

      ipudp_hdrs.udp.len      = htons(len - sizeof(struct iphdr));

      DEBUG(("\n   bytes %d\n", len));
      
      return sendmsg(pkt_fd, &msg, 0);
}

int packet_peek(int *ifindex)
{
      struct sockaddr_ll sll;
      struct iphdr iph;
      int ret, sllen = sizeof(struct sockaddr_ll);

      /*
       * Peek at the IP header.
       */
      ret = recvfrom(pkt_fd, &iph, sizeof(struct iphdr),
                   MSG_PEEK, (struct sockaddr *)&sll, &sllen);
      if (ret == -1)
            return -1;

      if (sll.sll_family != AF_PACKET)
            goto discard_pkt;

      if (iph.ihl < 5 || iph.version != IPVERSION)
            goto discard_pkt;

      *ifindex = sll.sll_ifindex;

      return 0;

 discard_pkt:
      packet_discard();
      return 0;
}

void packet_discard(void)
{
      struct iphdr iph;
      struct sockaddr_ll sll;
      socklen_t sllen = sizeof(sll);
      
      recvfrom(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.
 */
int packet_recv(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;

      ret = recvfrom(pkt_fd, &iph, sizeof(struct iphdr),
                   MSG_PEEK, NULL, NULL);
      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(pkt_fd, &msg, 0);
      if (ret == -1)
            goto free_pkt;

      DEBUG(("<- bytes %d ", ret));

      if (ip_checksum((__u16 *)ip, ip->ihl) != 0)
            goto free_pkt;

      DEBUG(("\n   ip src %s ", ntoa(ip->saddr)));
      DEBUG(("dst %s ", ntoa(ip->daddr)));

      if (ntohs(ip->tot_len) > ret || ip->protocol != IPPROTO_UDP)
            goto free_pkt;

      ret -= 4 * ip->ihl;

      DEBUG(("\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:
      free(ip);
      return 0;

 discard_pkt:
      DEBUG(("discarded\n"));
      packet_discard();
      return 0;
}

Generated by  Doxygen 1.6.0   Back to index