net.c 19 KB

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