net.c 17 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687
  1. /* Minimal network stack: ARP + ICMP echo (ping) over a netdev NIC.
  2. Freestanding, no malloc; fixed-size buffers and a single configured IP. */
  3. #include "io.h"
  4. #include "netdev.h"
  5. #include "net.h"
  6. #include "timer.h"
  7. static unsigned char my_ip[4] = { 192, 168, 178, 240 };
  8. static unsigned char my_mask[4];
  9. static unsigned char my_gw[4];
  10. static unsigned char my_dns[4];
  11. /* ---- small print helpers (framebuffer console) ---- */
  12. static void print_hex2(unsigned int v)
  13. {
  14. static const char hex[] = "0123456789ABCDEF";
  15. io_putchar(hex[(v >> 4) & 0xF]);
  16. io_putchar(hex[v & 0xF]);
  17. }
  18. static void print_hex4(unsigned int v)
  19. {
  20. print_hex2((v >> 8) & 0xFF);
  21. print_hex2(v & 0xFF);
  22. }
  23. static void print_dec(unsigned int v)
  24. {
  25. char b[12];
  26. int i = 0;
  27. if (v == 0) {
  28. io_putchar('0');
  29. return;
  30. }
  31. while (v) {
  32. b[i++] = (char)('0' + (v % 10));
  33. v /= 10;
  34. }
  35. while (i)
  36. io_putchar(b[--i]);
  37. }
  38. static void print_mac(const unsigned char *m)
  39. {
  40. unsigned int i;
  41. for (i = 0; i < 6; i++) {
  42. if (i)
  43. io_putchar(':');
  44. print_hex2(m[i]);
  45. }
  46. }
  47. static void print_ip(const unsigned char *p)
  48. {
  49. unsigned int i;
  50. for (i = 0; i < 4; i++) {
  51. if (i)
  52. io_putchar('.');
  53. print_dec(p[i]);
  54. }
  55. }
  56. static int ip_eq(const unsigned char *a, const unsigned char *b)
  57. {
  58. return a[0] == b[0] && a[1] == b[1] && a[2] == b[2] && a[3] == b[3];
  59. }
  60. /* Internet checksum (16-bit one's complement, network byte order). */
  61. unsigned short net_checksum(const void *data, unsigned int len)
  62. {
  63. const unsigned char *p = (const unsigned char *)data;
  64. unsigned int sum = 0;
  65. while (len > 1) {
  66. sum += ((unsigned int)p[0] << 8) | p[1];
  67. p += 2;
  68. len -= 2;
  69. }
  70. if (len)
  71. sum += (unsigned int)p[0] << 8;
  72. while (sum >> 16)
  73. sum = (sum & 0xFFFFu) + (sum >> 16);
  74. return (unsigned short)(~sum);
  75. }
  76. /* TCP checksum: the IPv4 pseudo-header plus the segment. */
  77. unsigned short net_checksum_tcp(const unsigned char *src_ip,
  78. const unsigned char *dst_ip,
  79. const unsigned char *tcp, unsigned int len)
  80. {
  81. const unsigned char *p = tcp;
  82. unsigned int sum = 0, i;
  83. for (i = 0; i < 4; i += 2)
  84. sum += ((unsigned int)src_ip[i] << 8) | src_ip[i + 1];
  85. for (i = 0; i < 4; i += 2)
  86. sum += ((unsigned int)dst_ip[i] << 8) | dst_ip[i + 1];
  87. sum += IPPROTO_TCP; /* pseudo-header protocol (0x0006) */
  88. sum += len; /* TCP length */
  89. while (len > 1) {
  90. sum += ((unsigned int)p[0] << 8) | p[1];
  91. p += 2;
  92. len -= 2;
  93. }
  94. if (len)
  95. sum += (unsigned int)p[0] << 8;
  96. while (sum >> 16)
  97. sum = (sum & 0xFFFFu) + (sum >> 16);
  98. return (unsigned short)(~sum);
  99. }
  100. /* ---- IP address config ---- */
  101. void net_set_ip(const unsigned char ip[4])
  102. {
  103. unsigned int i;
  104. /* Manual override: subnet knowledge is dropped, so the stack keeps
  105. * treating everything as directly reachable. */
  106. for (i = 0; i < 4; i++) {
  107. my_ip[i] = ip[i];
  108. my_mask[i] = 0;
  109. my_gw[i] = 0;
  110. }
  111. }
  112. void net_configure(const unsigned char ip[4], const unsigned char mask[4],
  113. const unsigned char gw[4])
  114. {
  115. unsigned int i;
  116. for (i = 0; i < 4; i++) {
  117. my_ip[i] = ip[i];
  118. my_mask[i] = mask ? mask[i] : 0;
  119. my_gw[i] = gw ? gw[i] : 0;
  120. }
  121. }
  122. void net_get_ip(unsigned char ip[4])
  123. {
  124. unsigned int i;
  125. for (i = 0; i < 4; i++)
  126. ip[i] = my_ip[i];
  127. }
  128. void net_get_mask(unsigned char mask[4])
  129. {
  130. unsigned int i;
  131. for (i = 0; i < 4; i++)
  132. mask[i] = my_mask[i];
  133. }
  134. void net_get_gateway(unsigned char gw[4])
  135. {
  136. unsigned int i;
  137. for (i = 0; i < 4; i++)
  138. gw[i] = my_gw[i];
  139. }
  140. int net_is_configured(void)
  141. {
  142. return my_ip[0] || my_ip[1] || my_ip[2] || my_ip[3];
  143. }
  144. void net_get_dns(unsigned char dns[4])
  145. {
  146. unsigned int i;
  147. for (i = 0; i < 4; i++)
  148. dns[i] = my_dns[i];
  149. }
  150. void net_set_dns(const unsigned char dns[4])
  151. {
  152. unsigned int i;
  153. for (i = 0; i < 4; i++)
  154. my_dns[i] = dns ? dns[i] : 0;
  155. }
  156. void net_next_hop(const unsigned char dst[4], unsigned char hop[4])
  157. {
  158. unsigned int i, off = 0;
  159. for (i = 0; i < 4; i++)
  160. hop[i] = dst[i];
  161. /* Without a mask/gateway (manual netip) keep treating the target as
  162. * directly reachable, like ping does. */
  163. if (!(my_mask[0] || my_mask[1] || my_mask[2] || my_mask[3]) ||
  164. !(my_gw[0] || my_gw[1] || my_gw[2] || my_gw[3]))
  165. return;
  166. for (i = 0; i < 4; i++) {
  167. if ((dst[i] & my_mask[i]) != (my_ip[i] & my_mask[i])) {
  168. off = 1;
  169. break;
  170. }
  171. }
  172. if (off)
  173. for (i = 0; i < 4; i++)
  174. hop[i] = my_gw[i];
  175. }
  176. int net_parse_ip(const char *s, unsigned char ip[4])
  177. {
  178. unsigned int i, val = 0, nparts = 0;
  179. int have_digit = 0;
  180. for (i = 0; ; i++) {
  181. char c = s[i];
  182. if (c >= '0' && c <= '9') {
  183. val = val * 10 + (unsigned int)(c - '0');
  184. if (val > 255)
  185. return -1;
  186. have_digit = 1;
  187. } else if (c == '.' || c == '\0') {
  188. if (!have_digit || nparts >= 4)
  189. return -1;
  190. ip[nparts++] = (unsigned char)val;
  191. val = 0;
  192. have_digit = 0;
  193. if (c == '\0')
  194. break;
  195. } else {
  196. return -1;
  197. }
  198. }
  199. return (nparts == 4) ? 0 : -1;
  200. }
  201. void net_print_ip(void)
  202. {
  203. print_ip(my_ip);
  204. }
  205. /* ---- UDP send/parse (used by netlog, netkexec and the DHCP client) ---- */
  206. static unsigned char udp_frame[14 + 20 + 8 + NET_UDP_MAX];
  207. int net_udp_send_from_dev(const struct netdev *nd,
  208. const unsigned char *src_ip,
  209. const unsigned char *dst_mac, const unsigned char *dst_ip,
  210. unsigned int src_port, unsigned int dst_port,
  211. const void *payload, unsigned int len)
  212. {
  213. unsigned char my_mac[6];
  214. unsigned char *ip = udp_frame + 14;
  215. unsigned char *udp = ip + 20;
  216. unsigned short cs;
  217. unsigned int i, total = 20 + 8 + len;
  218. const unsigned char *p = (const unsigned char *)payload;
  219. if (!nd || len > NET_UDP_MAX)
  220. return -1;
  221. nd->get_mac(my_mac);
  222. for (i = 0; i < 6; i++)
  223. udp_frame[i] = dst_mac[i];
  224. for (i = 0; i < 6; i++)
  225. udp_frame[6 + i] = my_mac[i];
  226. udp_frame[12] = 0x08;
  227. udp_frame[13] = 0x00;
  228. for (i = 0; i < 20; i++)
  229. ip[i] = 0;
  230. ip[0] = 0x45;
  231. ip[2] = (unsigned char)(total >> 8);
  232. ip[3] = (unsigned char)(total & 0xFF);
  233. ip[8] = 64;
  234. ip[9] = IPPROTO_UDP;
  235. for (i = 0; i < 4; i++)
  236. ip[12 + i] = src_ip[i];
  237. for (i = 0; i < 4; i++)
  238. ip[16 + i] = dst_ip[i];
  239. cs = net_checksum(ip, 20);
  240. ip[10] = (unsigned char)(cs >> 8);
  241. ip[11] = (unsigned char)(cs & 0xFF);
  242. udp[0] = (unsigned char)(src_port >> 8);
  243. udp[1] = (unsigned char)(src_port & 0xFF);
  244. udp[2] = (unsigned char)(dst_port >> 8);
  245. udp[3] = (unsigned char)(dst_port & 0xFF);
  246. udp[4] = (unsigned char)((8 + len) >> 8);
  247. udp[5] = (unsigned char)((8 + len) & 0xFF);
  248. udp[6] = 0; /* checksum disabled (allowed for IPv4) */
  249. udp[7] = 0;
  250. for (i = 0; i < len; i++)
  251. udp[8 + i] = p[i];
  252. return nd->tx(udp_frame, 14 + total);
  253. }
  254. int net_udp_send_from(const unsigned char *src_ip,
  255. const unsigned char *dst_mac, const unsigned char *dst_ip,
  256. unsigned int src_port, unsigned int dst_port,
  257. const void *payload, unsigned int len)
  258. {
  259. return net_udp_send_from_dev(netdev_active(), src_ip, dst_mac, dst_ip,
  260. src_port, dst_port, payload, len);
  261. }
  262. int net_udp_send(const unsigned char *dst_mac, const unsigned char *dst_ip,
  263. unsigned int src_port, unsigned int dst_port,
  264. const void *payload, unsigned int len)
  265. {
  266. unsigned char my_ip[4];
  267. net_get_ip(my_ip);
  268. return net_udp_send_from(my_ip, dst_mac, dst_ip, src_port, dst_port,
  269. payload, len);
  270. }
  271. static int net_udp_parse_common(const unsigned char *f, unsigned int len,
  272. unsigned char src_ip[4],
  273. unsigned char dst_ip[4],
  274. unsigned int *src_port,
  275. unsigned int *dst_port,
  276. const unsigned char **payload,
  277. unsigned int *plen, int check_dst)
  278. {
  279. const unsigned char *ip = f + 14;
  280. const unsigned char *udp;
  281. unsigned int i, ihl, ip_len;
  282. if (len < 14 + 20 + 8)
  283. return 0;
  284. if ((((unsigned int)f[12] << 8) | f[13]) != ET_IPV4)
  285. return 0;
  286. if ((ip[0] >> 4) != 4)
  287. return 0;
  288. ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  289. if (ihl < 20 || ip[9] != IPPROTO_UDP)
  290. return 0;
  291. ip_len = ((unsigned int)ip[2] << 8) | ip[3];
  292. if (ip_len < ihl + 8 || 14 + ip_len > len)
  293. return 0;
  294. /* Accept datagrams for us or for the broadcast address (a DHCP client
  295. * has no address yet). The DHCP client passes check_dst=0: a server
  296. * may send its OFFER unicast to the address it is offering, which is
  297. * not our (still static) IP. */
  298. if (check_dst) {
  299. unsigned char my_ip[4];
  300. int dst_ok = 1, dst_bcast = 1;
  301. net_get_ip(my_ip);
  302. for (i = 0; i < 4; i++) {
  303. if (ip[16 + i] != my_ip[i])
  304. dst_ok = 0;
  305. if (ip[16 + i] != 0xFF)
  306. dst_bcast = 0;
  307. }
  308. if (!dst_ok && !dst_bcast)
  309. return 0;
  310. }
  311. udp = ip + ihl;
  312. for (i = 0; i < 4; i++) {
  313. src_ip[i] = ip[12 + i];
  314. dst_ip[i] = ip[16 + i];
  315. }
  316. *src_port = ((unsigned int)udp[0] << 8) | udp[1];
  317. *dst_port = ((unsigned int)udp[2] << 8) | udp[3];
  318. *payload = udp + 8;
  319. *plen = ip_len - ihl - 8;
  320. return 1;
  321. }
  322. int net_udp_parse(const unsigned char *f, unsigned int len,
  323. unsigned char src_ip[4], unsigned char dst_ip[4],
  324. unsigned int *src_port, unsigned int *dst_port,
  325. const unsigned char **payload, unsigned int *plen)
  326. {
  327. return net_udp_parse_common(f, len, src_ip, dst_ip, src_port,
  328. dst_port, payload, plen, 1);
  329. }
  330. int net_udp_parse_any(const unsigned char *f, unsigned int len,
  331. unsigned char src_ip[4], unsigned char dst_ip[4],
  332. unsigned int *src_port, unsigned int *dst_port,
  333. const unsigned char **payload, unsigned int *plen)
  334. {
  335. return net_udp_parse_common(f, len, src_ip, dst_ip, src_port,
  336. dst_port, payload, plen, 0);
  337. }
  338. /* ---- ARP resolution (shared by the DNS resolver and the TCP client) ---- */
  339. /* ARP request "who has target? tell my_ip" -> 42-byte frame. */
  340. static unsigned int arp_build_request(const unsigned char *my_mac,
  341. const unsigned char *my_ip,
  342. const unsigned char *target_ip,
  343. unsigned char *out)
  344. {
  345. unsigned char *a = out + 14;
  346. unsigned int i;
  347. for (i = 0; i < 6; i++)
  348. out[i] = 0xFF; /* broadcast */
  349. for (i = 0; i < 6; i++)
  350. out[6 + i] = my_mac[i];
  351. out[12] = 0x08;
  352. out[13] = 0x06;
  353. a[0] = 0; a[1] = 1; /* htype Ethernet */
  354. a[2] = 0x08; a[3] = 0; /* ptype IPv4 */
  355. a[4] = 6; a[5] = 4;
  356. a[6] = 0; a[7] = 1; /* opcode request */
  357. for (i = 0; i < 6; i++) /* sha */
  358. a[8 + i] = my_mac[i];
  359. for (i = 0; i < 4; i++) /* spa */
  360. a[14 + i] = my_ip[i];
  361. for (i = 0; i < 6; i++) /* tha */
  362. a[18 + i] = 0;
  363. for (i = 0; i < 4; i++) /* tpa */
  364. a[24 + i] = target_ip[i];
  365. return 42;
  366. }
  367. /* 1 + peer MAC if frame is an ARP reply from target_ip. */
  368. static int arp_parse_reply(const unsigned char *f, unsigned int len,
  369. const unsigned char *target_ip, unsigned char *mac)
  370. {
  371. const unsigned char *a = f + 14;
  372. unsigned int i;
  373. if (len < 14 + 28)
  374. return 0;
  375. if ((((unsigned int)f[12] << 8) | f[13]) != ET_ARP)
  376. return 0;
  377. if ((((unsigned int)a[6] << 8) | a[7]) != 2) /* opcode reply */
  378. return 0;
  379. if (a[4] != 6 || a[5] != 4)
  380. return 0;
  381. if (!ip_eq(a + 14, target_ip)) /* sender IP == target */
  382. return 0;
  383. for (i = 0; i < 6; i++)
  384. mac[i] = a[8 + i];
  385. return 1;
  386. }
  387. int net_arp_resolve(const unsigned char *dst_ip, unsigned char mac[6])
  388. {
  389. const struct netdev *nd = netdev_active();
  390. unsigned char my_mac[6], my_ip[4];
  391. unsigned char req[64];
  392. unsigned char *rxbuf = netdev_rx_scratch();
  393. unsigned int tries;
  394. if (!nd || !nd->get_mac)
  395. return -2;
  396. nd->get_mac(my_mac);
  397. net_get_ip(my_ip);
  398. for (tries = 0; tries < 3; tries++) {
  399. unsigned int deadline = get_ticks_ms() + 1000;
  400. unsigned int reqlen = arp_build_request(my_mac, my_ip,
  401. dst_ip, req);
  402. if (nd->tx(req, reqlen) != 0)
  403. return -2;
  404. while ((int)(get_ticks_ms() - deadline) < 0) {
  405. int len = nd->rx(rxbuf, NETDEV_FRAME_MAX);
  406. if (len < 0)
  407. continue;
  408. if (arp_parse_reply(rxbuf, (unsigned int)len,
  409. dst_ip, mac))
  410. return 0;
  411. }
  412. }
  413. return -1;
  414. }
  415. /* ---- ethertype naming ---- */
  416. const char *net_ethertype_name(unsigned short et)
  417. {
  418. switch (et) {
  419. case ET_IPV4: return "IPv4";
  420. case ET_ARP: return "ARP";
  421. case ET_IPV6: return "IPv6";
  422. default: return 0;
  423. }
  424. }
  425. /* ---- frame decoding ---- */
  426. void net_decode_frame(const unsigned char *f, unsigned int len)
  427. {
  428. unsigned short et;
  429. const char *name;
  430. if (len < 14) {
  431. io_putstr("(short frame len=");
  432. print_dec(len);
  433. io_putstr(")");
  434. return;
  435. }
  436. et = ((unsigned short)f[12] << 8) | f[13];
  437. print_mac(f + 6);
  438. io_putstr(" -> ");
  439. print_mac(f);
  440. io_putstr(" ");
  441. name = net_ethertype_name(et);
  442. if (name)
  443. io_putstr(name);
  444. else {
  445. io_putstr("0x");
  446. print_hex4(et);
  447. }
  448. io_putstr(" len=");
  449. print_dec(len);
  450. if (et == ET_ARP && len >= 14 + 28) {
  451. const unsigned char *a = f + 14;
  452. unsigned short op = ((unsigned short)a[6] << 8) | a[7];
  453. if (op == 1) {
  454. io_putstr(" who-has ");
  455. print_ip(a + 24);
  456. io_putstr(" tell ");
  457. print_ip(a + 14);
  458. } else if (op == 2) {
  459. io_putstr(" ");
  460. print_ip(a + 14);
  461. io_putstr(" is-at ");
  462. print_mac(a + 8);
  463. }
  464. } else if (et == ET_IPV4 && len >= 14 + 20) {
  465. const unsigned char *ip = f + 14;
  466. unsigned int ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  467. unsigned char proto = ip[9];
  468. io_putstr(" ");
  469. print_ip(ip + 12);
  470. io_putstr(" -> ");
  471. print_ip(ip + 16);
  472. io_putstr(" proto=");
  473. print_dec(proto);
  474. if (proto == IPPROTO_ICMP && ihl >= 20 && len >= 14 + ihl + 8) {
  475. const unsigned char *ic = ip + ihl;
  476. io_putstr(" ICMP type=");
  477. print_dec(ic[0]);
  478. io_putstr(" code=");
  479. print_dec(ic[1]);
  480. }
  481. }
  482. }
  483. /* ---- reply generation ---- */
  484. int net_build_reply(const unsigned char *frame, unsigned int len,
  485. unsigned char *out, unsigned int maxout)
  486. {
  487. unsigned char mac[6];
  488. unsigned short et;
  489. unsigned int i;
  490. if (len < 14)
  491. return 0;
  492. et = ((unsigned short)frame[12] << 8) | frame[13];
  493. netdev_get_mac(mac);
  494. /* ARP request for our IP -> ARP reply. */ if (et == ET_ARP && len >= 14 + 28) {
  495. const unsigned char *a = frame + 14;
  496. unsigned short op = ((unsigned short)a[6] << 8) | a[7];
  497. if (op != 1)
  498. return 0;
  499. if ((((unsigned short)a[0] << 8) | a[1]) != 1) /* htype Ethernet */
  500. return 0;
  501. if ((((unsigned short)a[2] << 8) | a[3]) != ET_IPV4)
  502. return 0;
  503. if (a[4] != 6 || a[5] != 4)
  504. return 0;
  505. if (!ip_eq(a + 24, my_ip)) /* target != us */
  506. return 0;
  507. for (i = 0; i < 6; i++)
  508. out[i] = frame[6 + i]; /* dst = requester */
  509. for (i = 0; i < 6; i++)
  510. out[6 + i] = mac[i]; /* src = us */
  511. out[12] = 0x08; out[13] = 0x06; /* ethertype ARP */
  512. {
  513. unsigned char *r = out + 14;
  514. r[0] = 0x00; r[1] = 0x01; /* htype Ethernet */
  515. r[2] = 0x08; r[3] = 0x00; /* ptype IPv4 */
  516. r[4] = 6; r[5] = 4;
  517. r[6] = 0x00; r[7] = 0x02; /* opcode reply */
  518. for (i = 0; i < 6; i++)
  519. r[8 + i] = mac[i]; /* sha */
  520. for (i = 0; i < 4; i++)
  521. r[14 + i] = my_ip[i]; /* spa */
  522. for (i = 0; i < 6; i++)
  523. r[18 + i] = a[8 + i]; /* tha = requester MAC */
  524. for (i = 0; i < 4; i++)
  525. r[24 + i] = a[14 + i]; /* tpa = requester IP */
  526. }
  527. return 42;
  528. }
  529. /* ICMP echo request for our IP -> ICMP echo reply. */
  530. if (et == ET_IPV4 && len >= 14 + 20) {
  531. const unsigned char *ip = frame + 14;
  532. unsigned int ihl, icmp_len, reply_len;
  533. unsigned short ip_len;
  534. const unsigned char *ic;
  535. unsigned char *rip, *ric;
  536. if ((ip[0] >> 4) != 4)
  537. return 0;
  538. ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  539. if (ihl < 20)
  540. return 0;
  541. if (ip[9] != IPPROTO_ICMP)
  542. return 0;
  543. if (!ip_eq(ip + 16, my_ip))
  544. return 0;
  545. ip_len = ((unsigned short)ip[2] << 8) | ip[3];
  546. if (ip_len < ihl || ip_len > len - 14)
  547. return 0;
  548. if (len < 14 + ihl + 8)
  549. return 0;
  550. ic = ip + ihl;
  551. if (ic[0] != ICMP_ECHO_REQUEST)
  552. return 0;
  553. icmp_len = ip_len - ihl;
  554. reply_len = 14 + ip_len;
  555. if (reply_len > maxout)
  556. return 0;
  557. /* Ethernet header. */
  558. for (i = 0; i < 6; i++)
  559. out[i] = frame[6 + i];
  560. for (i = 0; i < 6; i++)
  561. out[6 + i] = mac[i];
  562. out[12] = 0x08; out[13] = 0x00; /* ethertype IPv4 */
  563. /* IPv4 header (IHL 5, no options). */
  564. rip = out + 14;
  565. rip[0] = 0x45;
  566. rip[1] = 0x00;
  567. rip[2] = ip[2]; rip[3] = ip[3]; /* total length (same) */
  568. rip[4] = ip[4]; rip[5] = ip[5]; /* id */
  569. rip[6] = 0x00; rip[7] = 0x00; /* flags/frag */
  570. rip[8] = 64; /* TTL */
  571. rip[9] = IPPROTO_ICMP;
  572. rip[10] = 0; rip[11] = 0; /* checksum (computed below) */
  573. for (i = 0; i < 4; i++)
  574. rip[12 + i] = my_ip[i]; /* src = us */
  575. for (i = 0; i < 4; i++)
  576. rip[16 + i] = ip[12 + i]; /* dst = requester */
  577. /* ICMP: copy the whole request, then fix type/code/checksum. */
  578. ric = rip + ihl;
  579. for (i = 0; i < icmp_len; i++)
  580. ric[i] = ic[i];
  581. ric[0] = ICMP_ECHO_REPLY;
  582. ric[1] = 0;
  583. ric[2] = 0; ric[3] = 0;
  584. {
  585. unsigned short cs = net_checksum(rip, ihl);
  586. rip[10] = (unsigned char)(cs >> 8);
  587. rip[11] = (unsigned char)(cs & 0xFF);
  588. }
  589. {
  590. unsigned short cs = net_checksum(ric, icmp_len);
  591. ric[2] = (unsigned char)(cs >> 8);
  592. ric[3] = (unsigned char)(cs & 0xFF);
  593. }
  594. return (int)reply_len;
  595. }
  596. return 0;
  597. }