Magellan Linux

  • Back to homepage
    • /[magellan-source]/trunk/mkinitrd-magellan/klibc/usr/kinit/ipconfig/packet.c

    Annotation of /trunk/mkinitrd-magellan/klibc/usr/kinit/ipconfig/packet.c

    Parent Directory Parent Directory | Revision Log Revision Log

    Revision 1122 - (hide annotations) (download)
    Wed Aug 18 21:11:40 2010 UTC (13 years, 9 months ago) by niro
    File MIME type: text/plain
    File size: 5951 byte(s) 
    -updated to klibc-1.5.19
    1 niro 532 /*
    2     * Packet socket handling glue.
    3     */
    4     #include <sys/types.h>
    5     #include <sys/socket.h>
    6     #include <stdio.h>
    7     #include <stdlib.h>
    8     #include <string.h>
    9     #include <unistd.h>
    10     #include <net/if_packet.h>
    11     #include <netinet/if_ether.h>
    12     #include <netinet/in.h>
    13     #include <netpacket/packet.h>
    14     #include <asm/byteorder.h>
    15     #include <arpa/inet.h>
    16     #include <netinet/ip.h>
    17     #include <netinet/udp.h>
    18    
    19     #include "ipconfig.h"
    20     #include "netdev.h"
    21     #include "packet.h"
    22    
    23     static int pkt_fd = -1;
    24    
    25     uint16_t cfg_local_port = LOCAL_PORT;
    26     uint16_t cfg_remote_port = REMOTE_PORT;
    27    
    28     int packet_open(void)
    29     {
    30     int fd, one = 1;
    31    
    32     if (pkt_fd != -1)
    33     return pkt_fd;
    34    
    35     /*
    36     * Get a PACKET socket for IP traffic.
    37     */
    38     fd = socket(AF_PACKET, SOCK_DGRAM, htons(ETH_P_IP));
    39     if (fd == -1) {
    40     perror("socket");
    41     return -1;
    42     }
    43    
    44     /*
    45     * We want to broadcast
    46     */
    47 niro 815 if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &one,
    48     sizeof(one)) == -1) {
    49 niro 532 perror("SO_BROADCAST");
    50     close(fd);
    51     fd = -1;
    52     }
    53    
    54     pkt_fd = fd;
    55    
    56     return fd;
    57     }
    58    
    59     void packet_close(void)
    60     {
    61     close(pkt_fd);
    62     pkt_fd = -1;
    63     }
    64    
    65     static unsigned int ip_checksum(uint16_t *hdr, int len)
    66     {
    67     unsigned int chksum = 0;
    68    
    69     while (len) {
    70     chksum += *hdr++;
    71     chksum += *hdr++;
    72     len--;
    73     }
    74     chksum = (chksum & 0xffff) + (chksum >> 16);
    75     chksum = (chksum & 0xffff) + (chksum >> 16);
    76     return (~chksum) & 0xffff;
    77     }
    78    
    79     struct header {
    80     struct iphdr ip;
    81     struct udphdr udp;
    82     } __attribute__ ((packed));
    83    
    84     static struct header ipudp_hdrs = {
    85     .ip = {
    86     .ihl = 5,
    87     .version = IPVERSION,
    88     .frag_off = __constant_htons(IP_DF),
    89     .ttl = 64,
    90     .protocol = IPPROTO_UDP,
    91     .saddr = INADDR_ANY,
    92     .daddr = INADDR_BROADCAST,
    93     },
    94     .udp = {
    95     .source = __constant_htons(LOCAL_PORT),
    96     .dest = __constant_htons(REMOTE_PORT),
    97     .len = 0,
    98     .check = 0,
    99     },
    100     };
    101    
    102 niro 1122 #ifdef DEBUG /* Only used with dprintf() */
    103 niro 532 static char *ntoa(uint32_t addr)
    104     {
    105     struct in_addr in = { addr };
    106     return inet_ntoa(in);
    107     }
    108 niro 1122 #endif /* DEBUG */
    109 niro 532
    110     /*
    111 niro 1122 * Send a packet. The options are listed in iov[1...iov_len-1].
    112 niro 532 * iov[0] is reserved for the bootp packet header.
    113     */
    114     int packet_send(struct netdev *dev, struct iovec *iov, int iov_len)
    115     {
    116     struct sockaddr_ll sll;
    117 niro 1122 struct msghdr msg;
    118 niro 532 int i, len = 0;
    119    
    120 niro 1122 memset(&sll, 0, sizeof(sll));
    121     msg.msg_name = &sll;
    122     msg.msg_namelen = sizeof(sll);
    123     msg.msg_iov = iov;
    124     msg.msg_iovlen = iov_len;
    125     msg.msg_control = NULL;
    126     msg.msg_controllen = 0;
    127     msg.msg_flags = 0;
    128    
    129 niro 532 if (cfg_local_port != LOCAL_PORT) {
    130     ipudp_hdrs.udp.source = htons(cfg_local_port);
    131     ipudp_hdrs.udp.dest = htons(cfg_remote_port);
    132     }
    133    
    134 niro 1122 dprintf("\n udp src %d dst %d", ntohs(ipudp_hdrs.udp.source),
    135     ntohs(ipudp_hdrs.udp.dest));
    136 niro 532
    137 niro 1122 dprintf("\n ip src %s ", ntoa(ipudp_hdrs.ip.saddr));
    138     dprintf("dst %s ", ntoa(ipudp_hdrs.ip.daddr));
    139 niro 532
    140     /*
    141     * Glue in the ip+udp header iovec
    142     */
    143     iov[0].iov_base = &ipudp_hdrs;
    144     iov[0].iov_len = sizeof(struct header);
    145    
    146     for (i = 0; i < iov_len; i++)
    147     len += iov[i].iov_len;
    148    
    149     sll.sll_family = AF_PACKET;
    150     sll.sll_protocol = htons(ETH_P_IP);
    151     sll.sll_ifindex = dev->ifindex;
    152     sll.sll_hatype = dev->hwtype;
    153     sll.sll_pkttype = PACKET_BROADCAST;
    154     sll.sll_halen = dev->hwlen;
    155     memcpy(sll.sll_addr, dev->hwbrd, dev->hwlen);
    156    
    157     ipudp_hdrs.ip.tot_len = htons(len);
    158     ipudp_hdrs.ip.check = 0;
    159     ipudp_hdrs.ip.check = ip_checksum((uint16_t *) & ipudp_hdrs.ip,
    160     ipudp_hdrs.ip.ihl);
    161    
    162     ipudp_hdrs.udp.len = htons(len - sizeof(struct iphdr));
    163    
    164 niro 1122 dprintf("\n bytes %d\n", len);
    165 niro 532
    166     return sendmsg(pkt_fd, &msg, 0);
    167     }
    168    
    169 niro 1122 /*
    170     * Fetches a bootp packet, but doesn't remove it.
    171     * Returns:
    172     * 0 = Error
    173     * >0 = A packet of size "ret" is available for interface ifindex
    174     */
    175 niro 532 int packet_peek(int *ifindex)
    176     {
    177     struct sockaddr_ll sll;
    178     struct iphdr iph;
    179     int ret, sllen = sizeof(struct sockaddr_ll);
    180    
    181     /*
    182     * Peek at the IP header.
    183     */
    184     ret = recvfrom(pkt_fd, &iph, sizeof(struct iphdr),
    185     MSG_PEEK, (struct sockaddr *)&sll, &sllen);
    186     if (ret == -1)
    187 niro 1122 return 0;
    188 niro 532
    189     if (sll.sll_family != AF_PACKET)
    190     goto discard_pkt;
    191    
    192     if (iph.ihl < 5 || iph.version != IPVERSION)
    193     goto discard_pkt;
    194    
    195     *ifindex = sll.sll_ifindex;
    196    
    197 niro 1122 return ret;
    198 niro 532
    199     discard_pkt:
    200     packet_discard();
    201     return 0;
    202     }
    203    
    204     void packet_discard(void)
    205     {
    206     struct iphdr iph;
    207     struct sockaddr_ll sll;
    208     socklen_t sllen = sizeof(sll);
    209    
    210 niro 815 recvfrom(pkt_fd, &iph, sizeof(iph), 0,
    211     (struct sockaddr *)&sll, &sllen);
    212 niro 532 }
    213    
    214     /*
    215     * Receive a bootp packet. The options are listed in iov[1...iov_len].
    216     * iov[0] must point to the bootp packet header.
    217 niro 1122 * Returns:
    218     * 0 = Error, try again later
    219     * >0 = Size of packet
    220 niro 532 */
    221     int packet_recv(struct iovec *iov, int iov_len)
    222     {
    223     struct iphdr *ip, iph;
    224     struct udphdr *udp;
    225     struct msghdr msg = {
    226     .msg_name = NULL,
    227     .msg_namelen = 0,
    228     .msg_iov = iov,
    229     .msg_iovlen = iov_len,
    230     .msg_control = NULL,
    231     .msg_controllen = 0,
    232     .msg_flags = 0
    233     };
    234     int ret, iphl;
    235    
    236     ret = recvfrom(pkt_fd, &iph, sizeof(struct iphdr),
    237     MSG_PEEK, NULL, NULL);
    238     if (ret == -1)
    239 niro 1122 return 0;
    240 niro 532
    241     if (iph.ihl < 5 || iph.version != IPVERSION)
    242     goto discard_pkt;
    243    
    244     iphl = iph.ihl * 4;
    245    
    246     ip = malloc(iphl + sizeof(struct udphdr));
    247     if (!ip)
    248     goto discard_pkt;
    249    
    250     udp = (struct udphdr *)((char *)ip + iphl);
    251    
    252     iov[0].iov_base = ip;
    253     iov[0].iov_len = iphl + sizeof(struct udphdr);
    254    
    255     ret = recvmsg(pkt_fd, &msg, 0);
    256     if (ret == -1)
    257     goto free_pkt;
    258    
    259 niro 1122 dprintf("<- bytes %d ", ret);
    260 niro 532
    261     if (ip_checksum((uint16_t *) ip, ip->ihl) != 0)
    262     goto free_pkt;
    263    
    264 niro 1122 dprintf("\n ip src %s ", ntoa(ip->saddr));
    265     dprintf("dst %s ", ntoa(ip->daddr));
    266 niro 532
    267     if (ntohs(ip->tot_len) > ret || ip->protocol != IPPROTO_UDP)
    268     goto free_pkt;
    269    
    270     ret -= 4 * ip->ihl;
    271    
    272 niro 1122 dprintf("\n udp src %d dst %d ", ntohs(udp->source),
    273     ntohs(udp->dest));
    274 niro 532
    275     if (udp->source != htons(cfg_remote_port) ||
    276     udp->dest != htons(cfg_local_port))
    277     goto free_pkt;
    278    
    279     if (ntohs(udp->len) > ret)
    280     goto free_pkt;
    281    
    282     ret -= sizeof(struct udphdr);
    283    
    284     free(ip);
    285    
    286     return ret;
    287    
    288     free_pkt:
    289 niro 1122 dprintf("freed\n");
    290 niro 532 free(ip);
    291     return 0;
    292    
    293     discard_pkt:
    294 niro 1122 dprintf("discarded\n");
    295 niro 532 packet_discard();
    296     return 0;
    297     }