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 1122 - (show annotations) (download)
    Wed Aug 18 21:11:40 2010 UTC (13 years, 8 months ago) by niro
    File MIME type: text/plain
    File size: 5951 byte(s) 
    -updated to klibc-1.5.19
    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,
    48 sizeof(one)) == -1) {
    49 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 #ifdef DEBUG /* Only used with dprintf() */
    103 static char *ntoa(uint32_t addr)
    104 {
    105 struct in_addr in = { addr };
    106 return inet_ntoa(in);
    107 }
    108 #endif /* DEBUG */
    109
    110 /*
    111 * Send a packet. The options are listed in iov[1...iov_len-1].
    112 * 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 struct msghdr msg;
    118 int i, len = 0;
    119
    120 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 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 dprintf("\n udp src %d dst %d", ntohs(ipudp_hdrs.udp.source),
    135 ntohs(ipudp_hdrs.udp.dest));
    136
    137 dprintf("\n ip src %s ", ntoa(ipudp_hdrs.ip.saddr));
    138 dprintf("dst %s ", ntoa(ipudp_hdrs.ip.daddr));
    139
    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 dprintf("\n bytes %d\n", len);
    165
    166 return sendmsg(pkt_fd, &msg, 0);
    167 }
    168
    169 /*
    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 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 return 0;
    188
    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 return ret;
    198
    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 recvfrom(pkt_fd, &iph, sizeof(iph), 0,
    211 (struct sockaddr *)&sll, &sllen);
    212 }
    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 * Returns:
    218 * 0 = Error, try again later
    219 * >0 = Size of packet
    220 */
    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 return 0;
    240
    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 dprintf("<- bytes %d ", ret);
    260
    261 if (ip_checksum((uint16_t *) ip, ip->ihl) != 0)
    262 goto free_pkt;
    263
    264 dprintf("\n ip src %s ", ntoa(ip->saddr));
    265 dprintf("dst %s ", ntoa(ip->daddr));
    266
    267 if (ntohs(ip->tot_len) > ret || ip->protocol != IPPROTO_UDP)
    268 goto free_pkt;
    269
    270 ret -= 4 * ip->ihl;
    271
    272 dprintf("\n udp src %d dst %d ", ntohs(udp->source),
    273 ntohs(udp->dest));
    274
    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 dprintf("freed\n");
    290 free(ip);
    291 return 0;
    292
    293 discard_pkt:
    294 dprintf("discarded\n");
    295 packet_discard();
    296 return 0;
    297 }