ucmd_ping.c 9.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353
  1. /* ping: ring-3 command. Active ARP + ICMP echo (via the raw frame syscalls). */
  2. #include "ucmd_lib.h"
  3. #define ET_IPV4 0x0800
  4. #define ET_ARP 0x0806
  5. #define IPPROTO_ICMP 1
  6. #define ICMP_ECHO_REPLY 0
  7. #define ICMP_ECHO_REQUEST 8
  8. #define NET_FRAME_MAX 1518
  9. #define PING_COUNT 4
  10. #define PING_ID 0x4E42 /* "NB" */
  11. #define PING_WAIT_MS 1000
  12. static unsigned char rx_buf[NET_FRAME_MAX];
  13. static unsigned int ping_sent_ms[PING_COUNT];
  14. static unsigned short net_checksum(const void *data, unsigned int len)
  15. {
  16. const unsigned char *p = (const unsigned char *)data;
  17. unsigned int sum = 0;
  18. while (len > 1) {
  19. sum += ((unsigned int)p[0] << 8) | p[1];
  20. p += 2;
  21. len -= 2;
  22. }
  23. if (len)
  24. sum += (unsigned int)p[0] << 8;
  25. while (sum >> 16)
  26. sum = (sum & 0xFFFFu) + (sum >> 16);
  27. return (unsigned short)(~sum);
  28. }
  29. static int ip_eq(const unsigned char *a, const unsigned char *b)
  30. {
  31. return a[0] == b[0] && a[1] == b[1] && a[2] == b[2] && a[3] == b[3];
  32. }
  33. /* ARP request "who has target? tell my_ip" -> 42-byte frame. */
  34. static unsigned int build_arp_request(const unsigned char *my_mac,
  35. const unsigned char *my_ip,
  36. const unsigned char *target_ip,
  37. unsigned char *out)
  38. {
  39. unsigned char *a = out + 14;
  40. unsigned int i;
  41. for (i = 0; i < 6; i++)
  42. out[i] = 0xFF; /* broadcast */
  43. for (i = 0; i < 6; i++)
  44. out[6 + i] = my_mac[i];
  45. out[12] = 0x08;
  46. out[13] = 0x06;
  47. a[0] = 0; a[1] = 1; /* htype Ethernet */
  48. a[2] = 0x08; a[3] = 0; /* ptype IPv4 */
  49. a[4] = 6; a[5] = 4;
  50. a[6] = 0; a[7] = 1; /* opcode request */
  51. for (i = 0; i < 6; i++) /* sha */
  52. a[8 + i] = my_mac[i];
  53. for (i = 0; i < 4; i++) /* spa */
  54. a[14 + i] = my_ip[i];
  55. for (i = 0; i < 6; i++) /* tha */
  56. a[18 + i] = 0;
  57. for (i = 0; i < 4; i++) /* tpa */
  58. a[24 + i] = target_ip[i];
  59. return 42;
  60. }
  61. /* 1 + peer MAC if frame is an ARP reply from target_ip. */
  62. static int parse_arp_reply(const unsigned char *f, unsigned int len,
  63. const unsigned char *target_ip, unsigned char *mac)
  64. {
  65. const unsigned char *a = f + 14;
  66. unsigned int i;
  67. if (len < 14 + 28)
  68. return 0;
  69. if ((((unsigned int)f[12] << 8) | f[13]) != ET_ARP)
  70. return 0;
  71. if ((((unsigned int)a[6] << 8) | a[7]) != 2) /* opcode reply */
  72. return 0;
  73. if (a[4] != 6 || a[5] != 4)
  74. return 0;
  75. if (!ip_eq(a + 14, target_ip)) /* sender IP == target */
  76. return 0;
  77. for (i = 0; i < 6; i++)
  78. mac[i] = a[8 + i];
  79. return 1;
  80. }
  81. /* ICMP echo request (14 + 20 + 16 bytes; the driver pads to 60). */
  82. static unsigned int build_icmp_echo(const unsigned char *src_mac,
  83. const unsigned char *dst_mac,
  84. const unsigned char *src_ip,
  85. const unsigned char *dst_ip,
  86. unsigned short seq, unsigned char *out)
  87. {
  88. unsigned char *ip = out + 14;
  89. unsigned char *ic = ip + 20;
  90. unsigned char *pl = ic + 8;
  91. unsigned short cs;
  92. unsigned int i;
  93. for (i = 0; i < 6; i++)
  94. out[i] = dst_mac[i];
  95. for (i = 0; i < 6; i++)
  96. out[6 + i] = src_mac[i];
  97. out[12] = 0x08;
  98. out[13] = 0x00;
  99. for (i = 0; i < 20; i++)
  100. ip[i] = 0;
  101. ip[0] = 0x45;
  102. ip[2] = 0; ip[3] = 36; /* total length */
  103. ip[8] = 64; /* TTL */
  104. ip[9] = IPPROTO_ICMP;
  105. for (i = 0; i < 4; i++)
  106. ip[12 + i] = src_ip[i];
  107. for (i = 0; i < 4; i++)
  108. ip[16 + i] = dst_ip[i];
  109. cs = net_checksum(ip, 20);
  110. ip[10] = (unsigned char)(cs >> 8);
  111. ip[11] = (unsigned char)(cs & 0xFF);
  112. ic[0] = ICMP_ECHO_REQUEST;
  113. ic[1] = 0;
  114. ic[2] = 0; ic[3] = 0;
  115. ic[4] = (unsigned char)(PING_ID >> 8);
  116. ic[5] = (unsigned char)(PING_ID & 0xFF);
  117. ic[6] = (unsigned char)(seq >> 8);
  118. ic[7] = (unsigned char)(seq & 0xFF);
  119. pl[0] = 'n'; pl[1] = 'e'; pl[2] = 'b'; pl[3] = 'o';
  120. pl[4] = 's'; pl[5] = 0; pl[6] = 0; pl[7] = 0;
  121. cs = net_checksum(ic, 16);
  122. ic[2] = (unsigned char)(cs >> 8);
  123. ic[3] = (unsigned char)(cs & 0xFF);
  124. return 14 + 20 + 16;
  125. }
  126. /* 1 + ICMP echo-reply seq for our IP from peer_ip. */
  127. static int parse_icmp_reply(const unsigned char *f, unsigned int len,
  128. const unsigned char *my_ip,
  129. const unsigned char *peer_ip,
  130. unsigned short *seq)
  131. {
  132. const unsigned char *ip = f + 14;
  133. const unsigned char *ic;
  134. unsigned int ihl;
  135. unsigned short ip_len, id;
  136. if (len < 14 + 20)
  137. return 0;
  138. if ((((unsigned int)f[12] << 8) | f[13]) != ET_IPV4)
  139. return 0;
  140. if ((ip[0] >> 4) != 4)
  141. return 0;
  142. ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  143. if (ihl < 20 || ip[9] != IPPROTO_ICMP)
  144. return 0;
  145. ip_len = ((unsigned short)ip[2] << 8) | ip[3];
  146. if (ip_len < ihl + 8 || (unsigned int)ip_len > len - 14)
  147. return 0;
  148. if (!ip_eq(ip + 12, peer_ip)) /* src == peer */
  149. return 0;
  150. if (!ip_eq(ip + 16, my_ip)) /* dst == us */
  151. return 0;
  152. ic = ip + ihl;
  153. if (ic[0] != ICMP_ECHO_REPLY || ic[1] != 0)
  154. return 0;
  155. id = ((unsigned short)ic[4] << 8) | ic[5];
  156. if (id != PING_ID)
  157. return 0;
  158. *seq = ((unsigned short)ic[6] << 8) | ic[7];
  159. if (*seq < 1 || *seq > PING_COUNT)
  160. return 0;
  161. return 1;
  162. }
  163. /* Resolve target_ip's MAC by ARP (3 tries, 1 s each).
  164. * Returns 0 = resolved, -1 = no reply, -2 = transmit failed.
  165. * Prints the frames seen while waiting (capped) so a silent peer, a filtered
  166. * reply and a reply we refuse to parse can be told apart on screen. */
  167. static int ping_arp(const unsigned char *my_mac, const unsigned char *my_ip,
  168. const unsigned char *target_ip, unsigned char *target_mac)
  169. {
  170. unsigned char frame[64];
  171. unsigned int tries, seen = 0;
  172. for (tries = 0; tries < 3; tries++) {
  173. unsigned int deadline = u_ticks_ms() + PING_WAIT_MS;
  174. unsigned int flen = build_arp_request(my_mac, my_ip, target_ip, frame);
  175. if (u_syscall2(SYS_NET_TX, (int)frame, flen) != 0)
  176. return -2;
  177. u_puts("ping: ARP request sent (try ");
  178. u_putdec(tries + 1);
  179. u_puts("/3)\n");
  180. while ((int)(u_ticks_ms() - deadline) < 0) {
  181. int len = u_syscall2(SYS_NET_RX, (int)rx_buf, NET_FRAME_MAX);
  182. if (len < 0)
  183. continue;
  184. if (seen < 8) {
  185. u_puts("ping: rx ");
  186. u_syscall2(SYS_NET_DECODE, (int)rx_buf,
  187. (unsigned int)len);
  188. u_putc('\n');
  189. seen++;
  190. }
  191. if (parse_arp_reply(rx_buf, (unsigned int)len,
  192. target_ip, target_mac))
  193. return 0;
  194. }
  195. }
  196. u_puts("ping: saw ");
  197. u_putdec(seen);
  198. u_puts(" frame(s) while waiting\n");
  199. return -1;
  200. }
  201. void _start(const char *arg)
  202. {
  203. unsigned char target[4], my_ip[4], my_mac[6], target_mac[6];
  204. unsigned char mask[4], gw[4], hop[4];
  205. unsigned int i, replies = 0;
  206. if (!arg || !arg[0]) {
  207. u_puts("ping: usage ping <host|a.b.c.d>\n");
  208. u_exit(1);
  209. }
  210. if (u_syscall0(SYS_NET_ACQUIRE) != 0) {
  211. u_puts("ping: no NIC found\n");
  212. u_exit(1);
  213. }
  214. /* Dotted IP, or a name to resolve first. */
  215. if (u_parse_ip(arg, target) != 0) {
  216. if (u_syscall2(SYS_DNS_RESOLVE, (int)arg, (int)target) != 0) {
  217. u_syscall0(SYS_NET_RELEASE);
  218. u_exit(1);
  219. }
  220. }
  221. u_syscall1(SYS_NET_MAC, (int)my_mac);
  222. u_syscall1(SYS_NET_GET_IP, (int)my_ip);
  223. /* With a mask and gateway known, off-subnet targets go via the router;
  224. * we can only ARP for the next hop. */
  225. u_syscall1(SYS_NET_GET_MASK, (int)mask);
  226. u_syscall1(SYS_NET_GET_GW, (int)gw);
  227. {
  228. unsigned int j, off = 0;
  229. for (j = 0; j < 4; j++)
  230. hop[j] = target[j];
  231. if ((mask[0] || mask[1] || mask[2] || mask[3]) &&
  232. (gw[0] || gw[1] || gw[2] || gw[3])) {
  233. for (j = 0; j < 4; j++) {
  234. if ((target[j] & mask[j]) != (my_ip[j] & mask[j]))
  235. off = 1;
  236. }
  237. }
  238. if (off) {
  239. for (j = 0; j < 4; j++)
  240. hop[j] = gw[j];
  241. u_puts("ping: ");
  242. u_put_ip(target);
  243. u_puts(" via ");
  244. u_put_ip(hop);
  245. u_putc('\n');
  246. }
  247. {
  248. int r = ping_arp(my_mac, my_ip, hop, target_mac);
  249. if (r == -2) {
  250. u_puts("ping: transmit failed (see netregs)\n");
  251. u_syscall0(SYS_NET_RELEASE);
  252. u_exit(1);
  253. }
  254. if (r != 0) {
  255. u_puts("ping: no ARP reply from ");
  256. u_put_ip(hop);
  257. u_putc('\n');
  258. u_syscall0(SYS_NET_RELEASE);
  259. u_exit(1);
  260. }
  261. }
  262. }
  263. u_puts("ping: ");
  264. u_put_ip(target);
  265. u_puts(" is-at ");
  266. u_put_mac(target_mac);
  267. u_putc('\n');
  268. for (i = 0; i < PING_COUNT; i++) {
  269. unsigned char frame[64];
  270. unsigned int flen, deadline;
  271. int got = 0;
  272. flen = build_icmp_echo(my_mac, target_mac, my_ip, target,
  273. (unsigned short)(i + 1), frame);
  274. ping_sent_ms[i] = u_ticks_ms();
  275. if (u_syscall2(SYS_NET_TX, (int)frame, flen) != 0) {
  276. u_puts("ping: transmit failed\n");
  277. u_syscall0(SYS_NET_RELEASE);
  278. u_exit(1);
  279. }
  280. deadline = u_ticks_ms() + PING_WAIT_MS;
  281. while ((int)(u_ticks_ms() - deadline) < 0 && !got) {
  282. unsigned short seq;
  283. int len = u_syscall2(SYS_NET_RX, (int)rx_buf, NET_FRAME_MAX);
  284. unsigned int rtt;
  285. if (len < 0)
  286. continue;
  287. if (!parse_icmp_reply(rx_buf, (unsigned int)len,
  288. my_ip, target, &seq))
  289. continue;
  290. rtt = u_ticks_ms() - ping_sent_ms[seq - 1];
  291. u_puts("reply: seq=");
  292. u_putdec(seq);
  293. u_puts(" rtt=");
  294. if (rtt == 0)
  295. u_puts("<1");
  296. else
  297. u_putdec(rtt);
  298. u_puts("ms\n");
  299. got = 1;
  300. replies++;
  301. }
  302. if (!got) {
  303. u_puts("ping: timeout, seq=");
  304. u_putdec(i + 1);
  305. u_putc('\n');
  306. }
  307. }
  308. u_puts("ping: ");
  309. u_putdec(replies);
  310. u_puts("/");
  311. u_putdec(PING_COUNT);
  312. u_puts(" replies from ");
  313. u_put_ip(target);
  314. u_putc('\n');
  315. u_syscall0(SYS_NET_RELEASE);
  316. u_exit(replies ? 0 : 1);
  317. }