Contents of /trunk/mkinitrd-magellan/klibc/usr/kinit/ipconfig/packet.c
Parent Directory | Revision Log
Revision 1122 -
(show annotations)
(download)
Wed Aug 18 21:11:40 2010 UTC (14 years, 1 month ago) by niro
File MIME type: text/plain
File size: 5951 byte(s)
Wed Aug 18 21:11:40 2010 UTC (14 years, 1 month 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 | } |