packet.c 7.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291
  1. /* vi: set sw=4 ts=4: */
  2. /*
  3. * Packet ops
  4. *
  5. * Rewrite by Russ Dill <Russ.Dill@asu.edu> July 2001
  6. *
  7. * Licensed under GPLv2, see file LICENSE in this tarball for details.
  8. */
  9. #include <netinet/in.h>
  10. #if (defined(__GLIBC__) && __GLIBC__ >= 2 && __GLIBC_MINOR__ >= 1) || defined _NEWLIB_VERSION
  11. # include <netpacket/packet.h>
  12. # include <net/ethernet.h>
  13. #else
  14. # include <asm/types.h>
  15. # include <linux/if_packet.h>
  16. # include <linux/if_ether.h>
  17. #endif
  18. #include "common.h"
  19. #include "dhcpd.h"
  20. void FAST_FUNC udhcp_init_header(struct dhcp_packet *packet, char type)
  21. {
  22. memset(packet, 0, sizeof(*packet));
  23. packet->op = BOOTREQUEST; /* if client to a server */
  24. switch (type) {
  25. case DHCPOFFER:
  26. case DHCPACK:
  27. case DHCPNAK:
  28. packet->op = BOOTREPLY; /* if server to client */
  29. }
  30. packet->htype = 1; /* ethernet */
  31. packet->hlen = 6;
  32. packet->cookie = htonl(DHCP_MAGIC);
  33. if (DHCP_END != 0)
  34. packet->options[0] = DHCP_END;
  35. udhcp_add_simple_option(packet, DHCP_MESSAGE_TYPE, type);
  36. }
  37. #if defined CONFIG_UDHCP_DEBUG && CONFIG_UDHCP_DEBUG >= 2
  38. void FAST_FUNC udhcp_dump_packet(struct dhcp_packet *packet)
  39. {
  40. char buf[sizeof(packet->chaddr)*2 + 1];
  41. if (dhcp_verbose < 2)
  42. return;
  43. bb_info_msg(
  44. //" op %x"
  45. //" htype %x"
  46. " hlen %x"
  47. //" hops %x"
  48. " xid %x"
  49. //" secs %x"
  50. //" flags %x"
  51. " ciaddr %x"
  52. " yiaddr %x"
  53. " siaddr %x"
  54. " giaddr %x"
  55. //" chaddr %s"
  56. //" sname %s"
  57. //" file %s"
  58. //" cookie %x"
  59. //" options %s"
  60. //, packet->op
  61. //, packet->htype
  62. , packet->hlen
  63. //, packet->hops
  64. , packet->xid
  65. //, packet->secs
  66. //, packet->flags
  67. , packet->ciaddr
  68. , packet->yiaddr
  69. , packet->siaddr_nip
  70. , packet->gateway_nip
  71. //, packet->chaddr[16]
  72. //, packet->sname[64]
  73. //, packet->file[128]
  74. //, packet->cookie
  75. //, packet->options[]
  76. );
  77. *bin2hex(buf, (void *) packet->chaddr, sizeof(packet->chaddr)) = '\0';
  78. bb_info_msg(" chaddr %s", buf);
  79. }
  80. #endif
  81. /* Read a packet from socket fd, return -1 on read error, -2 on packet error */
  82. int FAST_FUNC udhcp_recv_kernel_packet(struct dhcp_packet *packet, int fd)
  83. {
  84. int bytes;
  85. unsigned char *vendor;
  86. memset(packet, 0, sizeof(*packet));
  87. bytes = safe_read(fd, packet, sizeof(*packet));
  88. if (bytes < 0) {
  89. log1("Packet read error, ignoring");
  90. return bytes; /* returns -1 */
  91. }
  92. if (packet->cookie != htonl(DHCP_MAGIC)) {
  93. bb_info_msg("Packet with bad magic, ignoring");
  94. return -2;
  95. }
  96. log1("Received a packet");
  97. udhcp_dump_packet(packet);
  98. if (packet->op == BOOTREQUEST) {
  99. vendor = udhcp_get_option(packet, DHCP_VENDOR);
  100. if (vendor) {
  101. #if 0
  102. static const char broken_vendors[][8] = {
  103. "MSFT 98",
  104. ""
  105. };
  106. int i;
  107. for (i = 0; broken_vendors[i][0]; i++) {
  108. if (vendor[OPT_LEN - OPT_DATA] == (uint8_t)strlen(broken_vendors[i])
  109. && strncmp((char*)vendor, broken_vendors[i], vendor[OPT_LEN - OPT_DATA]) == 0
  110. ) {
  111. log1("Broken client (%s), forcing broadcast replies",
  112. broken_vendors[i]);
  113. packet->flags |= htons(BROADCAST_FLAG);
  114. }
  115. }
  116. #else
  117. if (vendor[OPT_LEN - OPT_DATA] == (uint8_t)(sizeof("MSFT 98")-1)
  118. && memcmp(vendor, "MSFT 98", sizeof("MSFT 98")-1) == 0
  119. ) {
  120. log1("Broken client (%s), forcing broadcast replies", "MSFT 98");
  121. packet->flags |= htons(BROADCAST_FLAG);
  122. }
  123. #endif
  124. }
  125. }
  126. return bytes;
  127. }
  128. uint16_t FAST_FUNC udhcp_checksum(void *addr, int count)
  129. {
  130. /* Compute Internet Checksum for "count" bytes
  131. * beginning at location "addr".
  132. */
  133. int32_t sum = 0;
  134. uint16_t *source = (uint16_t *) addr;
  135. while (count > 1) {
  136. /* This is the inner loop */
  137. sum += *source++;
  138. count -= 2;
  139. }
  140. /* Add left-over byte, if any */
  141. if (count > 0) {
  142. /* Make sure that the left-over byte is added correctly both
  143. * with little and big endian hosts */
  144. uint16_t tmp = 0;
  145. *(uint8_t*)&tmp = *(uint8_t*)source;
  146. sum += tmp;
  147. }
  148. /* Fold 32-bit sum to 16 bits */
  149. while (sum >> 16)
  150. sum = (sum & 0xffff) + (sum >> 16);
  151. return ~sum;
  152. }
  153. /* Construct a ip/udp header for a packet, send packet */
  154. int FAST_FUNC udhcp_send_raw_packet(struct dhcp_packet *dhcp_pkt,
  155. uint32_t source_ip, int source_port,
  156. uint32_t dest_ip, int dest_port, const uint8_t *dest_arp,
  157. int ifindex)
  158. {
  159. struct sockaddr_ll dest;
  160. struct ip_udp_dhcp_packet packet;
  161. int fd;
  162. int result = -1;
  163. const char *msg;
  164. enum {
  165. IP_UPD_DHCP_SIZE = sizeof(struct ip_udp_dhcp_packet) - CONFIG_UDHCPC_SLACK_FOR_BUGGY_SERVERS,
  166. UPD_DHCP_SIZE = IP_UPD_DHCP_SIZE - offsetof(struct ip_udp_dhcp_packet, udp),
  167. };
  168. fd = socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_IP));
  169. if (fd < 0) {
  170. msg = "socket(%s)";
  171. goto ret_msg;
  172. }
  173. memset(&dest, 0, sizeof(dest));
  174. memset(&packet, 0, sizeof(packet));
  175. packet.data = *dhcp_pkt; /* struct copy */
  176. dest.sll_family = AF_PACKET;
  177. dest.sll_protocol = htons(ETH_P_IP);
  178. dest.sll_ifindex = ifindex;
  179. dest.sll_halen = 6;
  180. memcpy(dest.sll_addr, dest_arp, 6);
  181. if (bind(fd, (struct sockaddr *)&dest, sizeof(dest)) < 0) {
  182. msg = "bind(%s)";
  183. goto ret_close;
  184. }
  185. packet.ip.protocol = IPPROTO_UDP;
  186. packet.ip.saddr = source_ip;
  187. packet.ip.daddr = dest_ip;
  188. packet.udp.source = htons(source_port);
  189. packet.udp.dest = htons(dest_port);
  190. /* size, excluding IP header: */
  191. packet.udp.len = htons(UPD_DHCP_SIZE);
  192. /* for UDP checksumming, ip.len is set to UDP packet len */
  193. packet.ip.tot_len = packet.udp.len;
  194. packet.udp.check = udhcp_checksum(&packet, IP_UPD_DHCP_SIZE);
  195. /* but for sending, it is set to IP packet len */
  196. packet.ip.tot_len = htons(IP_UPD_DHCP_SIZE);
  197. packet.ip.ihl = sizeof(packet.ip) >> 2;
  198. packet.ip.version = IPVERSION;
  199. packet.ip.ttl = IPDEFTTL;
  200. packet.ip.check = udhcp_checksum(&packet.ip, sizeof(packet.ip));
  201. /* Currently we send full-sized DHCP packets (zero padded).
  202. * If you need to change this: last byte of the packet is
  203. * packet.data.options[udhcp_end_option(packet.data.options)]
  204. */
  205. udhcp_dump_packet(dhcp_pkt);
  206. result = sendto(fd, &packet, IP_UPD_DHCP_SIZE, 0,
  207. (struct sockaddr *) &dest, sizeof(dest));
  208. msg = "sendto";
  209. ret_close:
  210. close(fd);
  211. /* FIXME: and if result >= 0 but != IP_UPD_DHCP_SIZE? */
  212. if (result < 0) {
  213. ret_msg:
  214. bb_perror_msg(msg, "PACKET");
  215. }
  216. return result;
  217. }
  218. /* Let the kernel do all the work for packet generation */
  219. int FAST_FUNC udhcp_send_kernel_packet(struct dhcp_packet *dhcp_pkt,
  220. uint32_t source_ip, int source_port,
  221. uint32_t dest_ip, int dest_port)
  222. {
  223. struct sockaddr_in client;
  224. int fd;
  225. int result = -1;
  226. const char *msg;
  227. enum {
  228. DHCP_SIZE = sizeof(struct dhcp_packet) - CONFIG_UDHCPC_SLACK_FOR_BUGGY_SERVERS,
  229. };
  230. fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
  231. if (fd < 0) {
  232. msg = "socket(%s)";
  233. goto ret_msg;
  234. }
  235. setsockopt_reuseaddr(fd);
  236. memset(&client, 0, sizeof(client));
  237. client.sin_family = AF_INET;
  238. client.sin_port = htons(source_port);
  239. client.sin_addr.s_addr = source_ip;
  240. if (bind(fd, (struct sockaddr *)&client, sizeof(client)) == -1) {
  241. msg = "bind(%s)";
  242. goto ret_close;
  243. }
  244. memset(&client, 0, sizeof(client));
  245. client.sin_family = AF_INET;
  246. client.sin_port = htons(dest_port);
  247. client.sin_addr.s_addr = dest_ip;
  248. if (connect(fd, (struct sockaddr *)&client, sizeof(client)) == -1) {
  249. msg = "connect";
  250. goto ret_close;
  251. }
  252. /* Currently we send full-sized DHCP packets (see above) */
  253. udhcp_dump_packet(dhcp_pkt);
  254. result = safe_write(fd, dhcp_pkt, DHCP_SIZE);
  255. msg = "write";
  256. ret_close:
  257. close(fd);
  258. /* FIXME: and if result >= 0 but != DHCP_SIZE? */
  259. if (result < 0) {
  260. ret_msg:
  261. bb_perror_msg(msg, "UDP");
  262. }
  263. return result;
  264. }