Magellan Linux

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

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

    Parent Directory Parent Directory | Revision Log Revision Log

    Revision 532 - (show annotations) (download)
    Sat Sep 1 22:45:15 2007 UTC (16 years, 8 months ago) by niro
    File MIME type: text/plain
    File size: 5664 byte(s) 
    -import if magellan mkinitrd; it is a fork of redhats mkinitrd-5.0.8 with all magellan patches and features; deprecates magellan-src/mkinitrd
    1 /*
    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 if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one)) == -1) {
    48 perror("SO_BROADCAST");
    49 close(fd);
    50 fd = -1;
    51 }
    52
    53 pkt_fd = fd;
    54
    55 return fd;
    56 }
    57
    58 void packet_close(void)
    59 {
    60 close(pkt_fd);
    61 pkt_fd = -1;
    62 }
    63
    64 static unsigned int ip_checksum(uint16_t *hdr, int len)
    65 {
    66 unsigned int chksum = 0;
    67
    68 while (len) {
    69 chksum += *hdr++;
    70 chksum += *hdr++;
    71 len--;
    72 }
    73 chksum = (chksum & 0xffff) + (chksum >> 16);
    74 chksum = (chksum & 0xffff) + (chksum >> 16);
    75 return (~chksum) & 0xffff;
    76 }
    77
    78 struct header {
    79 struct iphdr ip;
    80 struct udphdr udp;
    81 } __attribute__ ((packed));
    82
    83 static struct header ipudp_hdrs = {
    84 .ip = {
    85 .ihl = 5,
    86 .version = IPVERSION,
    87 .frag_off = __constant_htons(IP_DF),
    88 .ttl = 64,
    89 .protocol = IPPROTO_UDP,
    90 .saddr = INADDR_ANY,
    91 .daddr = INADDR_BROADCAST,
    92 },
    93 .udp = {
    94 .source = __constant_htons(LOCAL_PORT),
    95 .dest = __constant_htons(REMOTE_PORT),
    96 .len = 0,
    97 .check = 0,
    98 },
    99 };
    100
    101 #ifdef IPC_DEBUG /* Only used by DEBUG(()) */
    102 static char *ntoa(uint32_t addr)
    103 {
    104 struct in_addr in = { addr };
    105 return inet_ntoa(in);
    106 }
    107 #endif
    108
    109 /*
    110 * Send a packet. The options are listed in iov[1...iov_len].
    111 * iov[0] is reserved for the bootp packet header.
    112 */
    113 int packet_send(struct netdev *dev, struct iovec *iov, int iov_len)
    114 {
    115 struct sockaddr_ll sll;
    116 struct msghdr msg = {
    117 .msg_name = &sll,
    118 .msg_namelen = sizeof(sll),
    119 .msg_iov = iov,
    120 .msg_iovlen = iov_len,
    121 .msg_control = NULL,
    122 .msg_controllen = 0,
    123 .msg_flags = 0
    124 };
    125 int i, len = 0;
    126
    127 if (cfg_local_port != LOCAL_PORT) {
    128 ipudp_hdrs.udp.source = htons(cfg_local_port);
    129 ipudp_hdrs.udp.dest = htons(cfg_remote_port);
    130 }
    131
    132 DEBUG(("\n udp src %d dst %d", ntohs(ipudp_hdrs.udp.source),
    133 ntohs(ipudp_hdrs.udp.dest)));
    134
    135 DEBUG(("\n ip src %s ", ntoa(ipudp_hdrs.ip.saddr)));
    136 DEBUG(("dst %s ", ntoa(ipudp_hdrs.ip.daddr)));
    137
    138 /*
    139 * Glue in the ip+udp header iovec
    140 */
    141 iov[0].iov_base = &ipudp_hdrs;
    142 iov[0].iov_len = sizeof(struct header);
    143
    144 for (i = 0; i < iov_len; i++)
    145 len += iov[i].iov_len;
    146
    147 sll.sll_family = AF_PACKET;
    148 sll.sll_protocol = htons(ETH_P_IP);
    149 sll.sll_ifindex = dev->ifindex;
    150 sll.sll_hatype = dev->hwtype;
    151 sll.sll_pkttype = PACKET_BROADCAST;
    152 sll.sll_halen = dev->hwlen;
    153 memcpy(sll.sll_addr, dev->hwbrd, dev->hwlen);
    154
    155 ipudp_hdrs.ip.tot_len = htons(len);
    156 ipudp_hdrs.ip.check = 0;
    157 ipudp_hdrs.ip.check = ip_checksum((uint16_t *) & ipudp_hdrs.ip,
    158 ipudp_hdrs.ip.ihl);
    159
    160 ipudp_hdrs.udp.len = htons(len - sizeof(struct iphdr));
    161
    162 DEBUG(("\n bytes %d\n", len));
    163
    164 return sendmsg(pkt_fd, &msg, 0);
    165 }
    166
    167 int packet_peek(int *ifindex)
    168 {
    169 struct sockaddr_ll sll;
    170 struct iphdr iph;
    171 int ret, sllen = sizeof(struct sockaddr_ll);
    172
    173 /*
    174 * Peek at the IP header.
    175 */
    176 ret = recvfrom(pkt_fd, &iph, sizeof(struct iphdr),
    177 MSG_PEEK, (struct sockaddr *)&sll, &sllen);
    178 if (ret == -1)
    179 return -1;
    180
    181 if (sll.sll_family != AF_PACKET)
    182 goto discard_pkt;
    183
    184 if (iph.ihl < 5 || iph.version != IPVERSION)
    185 goto discard_pkt;
    186
    187 *ifindex = sll.sll_ifindex;
    188
    189 return 0;
    190
    191 discard_pkt:
    192 packet_discard();
    193 return 0;
    194 }
    195
    196 void packet_discard(void)
    197 {
    198 struct iphdr iph;
    199 struct sockaddr_ll sll;
    200 socklen_t sllen = sizeof(sll);
    201
    202 recvfrom(pkt_fd, &iph, sizeof(iph), 0, (struct sockaddr *)&sll, &sllen);
    203 }
    204
    205 /*
    206 * Receive a bootp packet. The options are listed in iov[1...iov_len].
    207 * iov[0] must point to the bootp packet header.
    208 */
    209 int packet_recv(struct iovec *iov, int iov_len)
    210 {
    211 struct iphdr *ip, iph;
    212 struct udphdr *udp;
    213 struct msghdr msg = {
    214 .msg_name = NULL,
    215 .msg_namelen = 0,
    216 .msg_iov = iov,
    217 .msg_iovlen = iov_len,
    218 .msg_control = NULL,
    219 .msg_controllen = 0,
    220 .msg_flags = 0
    221 };
    222 int ret, iphl;
    223
    224 ret = recvfrom(pkt_fd, &iph, sizeof(struct iphdr),
    225 MSG_PEEK, NULL, NULL);
    226 if (ret == -1)
    227 return -1;
    228
    229 if (iph.ihl < 5 || iph.version != IPVERSION)
    230 goto discard_pkt;
    231
    232 iphl = iph.ihl * 4;
    233
    234 ip = malloc(iphl + sizeof(struct udphdr));
    235 if (!ip)
    236 goto discard_pkt;
    237
    238 udp = (struct udphdr *)((char *)ip + iphl);
    239
    240 iov[0].iov_base = ip;
    241 iov[0].iov_len = iphl + sizeof(struct udphdr);
    242
    243 ret = recvmsg(pkt_fd, &msg, 0);
    244 if (ret == -1)
    245 goto free_pkt;
    246
    247 DEBUG(("<- bytes %d ", ret));
    248
    249 if (ip_checksum((uint16_t *) ip, ip->ihl) != 0)
    250 goto free_pkt;
    251
    252 DEBUG(("\n ip src %s ", ntoa(ip->saddr)));
    253 DEBUG(("dst %s ", ntoa(ip->daddr)));
    254
    255 if (ntohs(ip->tot_len) > ret || ip->protocol != IPPROTO_UDP)
    256 goto free_pkt;
    257
    258 ret -= 4 * ip->ihl;
    259
    260 DEBUG(("\n udp src %d dst %d ", ntohs(udp->source),
    261 ntohs(udp->dest)));
    262
    263 if (udp->source != htons(cfg_remote_port) ||
    264 udp->dest != htons(cfg_local_port))
    265 goto free_pkt;
    266
    267 if (ntohs(udp->len) > ret)
    268 goto free_pkt;
    269
    270 ret -= sizeof(struct udphdr);
    271
    272 free(ip);
    273
    274 return ret;
    275
    276 free_pkt:
    277 free(ip);
    278 return 0;
    279
    280 discard_pkt:
    281 DEBUG(("discarded\n"));
    282 packet_discard();
    283 return 0;
    284 }