net.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496
  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. static unsigned char my_ip[4] = { 192, 168, 178, 240 };
  7. static unsigned char my_mask[4];
  8. static unsigned char my_gw[4];
  9. /* ---- small print helpers (framebuffer console) ---- */
  10. static void print_hex2(unsigned int v)
  11. {
  12. static const char hex[] = "0123456789ABCDEF";
  13. io_putchar(hex[(v >> 4) & 0xF]);
  14. io_putchar(hex[v & 0xF]);
  15. }
  16. static void print_hex4(unsigned int v)
  17. {
  18. print_hex2((v >> 8) & 0xFF);
  19. print_hex2(v & 0xFF);
  20. }
  21. static void print_dec(unsigned int v)
  22. {
  23. char b[12];
  24. int i = 0;
  25. if (v == 0) {
  26. io_putchar('0');
  27. return;
  28. }
  29. while (v) {
  30. b[i++] = (char)('0' + (v % 10));
  31. v /= 10;
  32. }
  33. while (i)
  34. io_putchar(b[--i]);
  35. }
  36. static void print_mac(const unsigned char *m)
  37. {
  38. unsigned int i;
  39. for (i = 0; i < 6; i++) {
  40. if (i)
  41. io_putchar(':');
  42. print_hex2(m[i]);
  43. }
  44. }
  45. static void print_ip(const unsigned char *p)
  46. {
  47. unsigned int i;
  48. for (i = 0; i < 4; i++) {
  49. if (i)
  50. io_putchar('.');
  51. print_dec(p[i]);
  52. }
  53. }
  54. static int ip_eq(const unsigned char *a, const unsigned char *b)
  55. {
  56. return a[0] == b[0] && a[1] == b[1] && a[2] == b[2] && a[3] == b[3];
  57. }
  58. /* Internet checksum (16-bit one's complement, network byte order). */
  59. unsigned short net_checksum(const void *data, unsigned int len)
  60. {
  61. const unsigned char *p = (const unsigned char *)data;
  62. unsigned int sum = 0;
  63. while (len > 1) {
  64. sum += ((unsigned int)p[0] << 8) | p[1];
  65. p += 2;
  66. len -= 2;
  67. }
  68. if (len)
  69. sum += (unsigned int)p[0] << 8;
  70. while (sum >> 16)
  71. sum = (sum & 0xFFFFu) + (sum >> 16);
  72. return (unsigned short)(~sum);
  73. }
  74. /* ---- IP address config ---- */
  75. void net_set_ip(const unsigned char ip[4])
  76. {
  77. unsigned int i;
  78. /* Manual override: subnet knowledge is dropped, so the stack keeps
  79. * treating everything as directly reachable. */
  80. for (i = 0; i < 4; i++) {
  81. my_ip[i] = ip[i];
  82. my_mask[i] = 0;
  83. my_gw[i] = 0;
  84. }
  85. }
  86. void net_configure(const unsigned char ip[4], const unsigned char mask[4],
  87. const unsigned char gw[4])
  88. {
  89. unsigned int i;
  90. for (i = 0; i < 4; i++) {
  91. my_ip[i] = ip[i];
  92. my_mask[i] = mask ? mask[i] : 0;
  93. my_gw[i] = gw ? gw[i] : 0;
  94. }
  95. }
  96. void net_get_ip(unsigned char ip[4])
  97. {
  98. unsigned int i;
  99. for (i = 0; i < 4; i++)
  100. ip[i] = my_ip[i];
  101. }
  102. void net_get_mask(unsigned char mask[4])
  103. {
  104. unsigned int i;
  105. for (i = 0; i < 4; i++)
  106. mask[i] = my_mask[i];
  107. }
  108. void net_get_gateway(unsigned char gw[4])
  109. {
  110. unsigned int i;
  111. for (i = 0; i < 4; i++)
  112. gw[i] = my_gw[i];
  113. }
  114. int net_is_configured(void)
  115. {
  116. return my_ip[0] || my_ip[1] || my_ip[2] || my_ip[3];
  117. }
  118. int net_parse_ip(const char *s, unsigned char ip[4])
  119. {
  120. unsigned int i, val = 0, nparts = 0;
  121. int have_digit = 0;
  122. for (i = 0; ; i++) {
  123. char c = s[i];
  124. if (c >= '0' && c <= '9') {
  125. val = val * 10 + (unsigned int)(c - '0');
  126. if (val > 255)
  127. return -1;
  128. have_digit = 1;
  129. } else if (c == '.' || c == '\0') {
  130. if (!have_digit || nparts >= 4)
  131. return -1;
  132. ip[nparts++] = (unsigned char)val;
  133. val = 0;
  134. have_digit = 0;
  135. if (c == '\0')
  136. break;
  137. } else {
  138. return -1;
  139. }
  140. }
  141. return (nparts == 4) ? 0 : -1;
  142. }
  143. void net_print_ip(void)
  144. {
  145. print_ip(my_ip);
  146. }
  147. /* ---- UDP send/parse (used by netlog, netkexec and the DHCP client) ---- */
  148. static unsigned char udp_frame[14 + 20 + 8 + NET_UDP_MAX];
  149. int net_udp_send_from(const unsigned char *src_ip,
  150. const unsigned char *dst_mac, const unsigned char *dst_ip,
  151. unsigned int src_port, unsigned int dst_port,
  152. const void *payload, unsigned int len)
  153. {
  154. const struct netdev *nd = netdev_active();
  155. unsigned char my_mac[6];
  156. unsigned char *ip = udp_frame + 14;
  157. unsigned char *udp = ip + 20;
  158. unsigned short cs;
  159. unsigned int i, total = 20 + 8 + len;
  160. const unsigned char *p = (const unsigned char *)payload;
  161. if (!nd || len > NET_UDP_MAX)
  162. return -1;
  163. nd->get_mac(my_mac);
  164. for (i = 0; i < 6; i++)
  165. udp_frame[i] = dst_mac[i];
  166. for (i = 0; i < 6; i++)
  167. udp_frame[6 + i] = my_mac[i];
  168. udp_frame[12] = 0x08;
  169. udp_frame[13] = 0x00;
  170. for (i = 0; i < 20; i++)
  171. ip[i] = 0;
  172. ip[0] = 0x45;
  173. ip[2] = (unsigned char)(total >> 8);
  174. ip[3] = (unsigned char)(total & 0xFF);
  175. ip[8] = 64;
  176. ip[9] = IPPROTO_UDP;
  177. for (i = 0; i < 4; i++)
  178. ip[12 + i] = src_ip[i];
  179. for (i = 0; i < 4; i++)
  180. ip[16 + i] = dst_ip[i];
  181. cs = net_checksum(ip, 20);
  182. ip[10] = (unsigned char)(cs >> 8);
  183. ip[11] = (unsigned char)(cs & 0xFF);
  184. udp[0] = (unsigned char)(src_port >> 8);
  185. udp[1] = (unsigned char)(src_port & 0xFF);
  186. udp[2] = (unsigned char)(dst_port >> 8);
  187. udp[3] = (unsigned char)(dst_port & 0xFF);
  188. udp[4] = (unsigned char)((8 + len) >> 8);
  189. udp[5] = (unsigned char)((8 + len) & 0xFF);
  190. udp[6] = 0; /* checksum disabled (allowed for IPv4) */
  191. udp[7] = 0;
  192. for (i = 0; i < len; i++)
  193. udp[8 + i] = p[i];
  194. return nd->tx(udp_frame, 14 + total);
  195. }
  196. int net_udp_send(const unsigned char *dst_mac, const unsigned char *dst_ip,
  197. unsigned int src_port, unsigned int dst_port,
  198. const void *payload, unsigned int len)
  199. {
  200. unsigned char my_ip[4];
  201. net_get_ip(my_ip);
  202. return net_udp_send_from(my_ip, dst_mac, dst_ip, src_port, dst_port,
  203. payload, len);
  204. }
  205. int net_udp_parse(const unsigned char *f, unsigned int len,
  206. unsigned char src_ip[4], unsigned char dst_ip[4],
  207. unsigned int *src_port, unsigned int *dst_port,
  208. const unsigned char **payload, unsigned int *plen)
  209. {
  210. const unsigned char *ip = f + 14;
  211. const unsigned char *udp;
  212. unsigned char my_ip[4];
  213. unsigned int i, ihl, ip_len;
  214. int dst_ok = 0, dst_bcast = 1;
  215. if (len < 14 + 20 + 8)
  216. return 0;
  217. if ((((unsigned int)f[12] << 8) | f[13]) != ET_IPV4)
  218. return 0;
  219. if ((ip[0] >> 4) != 4)
  220. return 0;
  221. ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  222. if (ihl < 20 || ip[9] != IPPROTO_UDP)
  223. return 0;
  224. ip_len = ((unsigned int)ip[2] << 8) | ip[3];
  225. if (ip_len < ihl + 8 || 14 + ip_len > len)
  226. return 0;
  227. /* Accept datagrams for us or for the broadcast address (a DHCP client
  228. * has no address yet). */
  229. net_get_ip(my_ip);
  230. dst_ok = 1;
  231. for (i = 0; i < 4; i++) {
  232. if (ip[16 + i] != my_ip[i])
  233. dst_ok = 0;
  234. if (ip[16 + i] != 0xFF)
  235. dst_bcast = 0;
  236. }
  237. if (!dst_ok && !dst_bcast)
  238. return 0;
  239. udp = ip + ihl;
  240. for (i = 0; i < 4; i++) {
  241. src_ip[i] = ip[12 + i];
  242. dst_ip[i] = ip[16 + i];
  243. }
  244. *src_port = ((unsigned int)udp[0] << 8) | udp[1];
  245. *dst_port = ((unsigned int)udp[2] << 8) | udp[3];
  246. *payload = udp + 8;
  247. *plen = ip_len - ihl - 8;
  248. return 1;
  249. }
  250. /* ---- ethertype naming ---- */
  251. const char *net_ethertype_name(unsigned short et)
  252. {
  253. switch (et) {
  254. case ET_IPV4: return "IPv4";
  255. case ET_ARP: return "ARP";
  256. case ET_IPV6: return "IPv6";
  257. default: return 0;
  258. }
  259. }
  260. /* ---- frame decoding ---- */
  261. void net_decode_frame(const unsigned char *f, unsigned int len)
  262. {
  263. unsigned short et;
  264. const char *name;
  265. if (len < 14) {
  266. io_putstr("(short frame len=");
  267. print_dec(len);
  268. io_putstr(")");
  269. return;
  270. }
  271. et = ((unsigned short)f[12] << 8) | f[13];
  272. print_mac(f + 6);
  273. io_putstr(" -> ");
  274. print_mac(f);
  275. io_putstr(" ");
  276. name = net_ethertype_name(et);
  277. if (name)
  278. io_putstr(name);
  279. else {
  280. io_putstr("0x");
  281. print_hex4(et);
  282. }
  283. io_putstr(" len=");
  284. print_dec(len);
  285. if (et == ET_ARP && len >= 14 + 28) {
  286. const unsigned char *a = f + 14;
  287. unsigned short op = ((unsigned short)a[6] << 8) | a[7];
  288. if (op == 1) {
  289. io_putstr(" who-has ");
  290. print_ip(a + 24);
  291. io_putstr(" tell ");
  292. print_ip(a + 14);
  293. } else if (op == 2) {
  294. io_putstr(" ");
  295. print_ip(a + 14);
  296. io_putstr(" is-at ");
  297. print_mac(a + 8);
  298. }
  299. } else if (et == ET_IPV4 && len >= 14 + 20) {
  300. const unsigned char *ip = f + 14;
  301. unsigned int ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  302. unsigned char proto = ip[9];
  303. io_putstr(" ");
  304. print_ip(ip + 12);
  305. io_putstr(" -> ");
  306. print_ip(ip + 16);
  307. io_putstr(" proto=");
  308. print_dec(proto);
  309. if (proto == IPPROTO_ICMP && ihl >= 20 && len >= 14 + ihl + 8) {
  310. const unsigned char *ic = ip + ihl;
  311. io_putstr(" ICMP type=");
  312. print_dec(ic[0]);
  313. io_putstr(" code=");
  314. print_dec(ic[1]);
  315. }
  316. }
  317. }
  318. /* ---- reply generation ---- */
  319. int net_build_reply(const unsigned char *frame, unsigned int len,
  320. unsigned char *out, unsigned int maxout)
  321. {
  322. unsigned char mac[6];
  323. unsigned short et;
  324. unsigned int i;
  325. if (len < 14)
  326. return 0;
  327. et = ((unsigned short)frame[12] << 8) | frame[13];
  328. netdev_get_mac(mac);
  329. /* ARP request for our IP -> ARP reply. */ if (et == ET_ARP && len >= 14 + 28) {
  330. const unsigned char *a = frame + 14;
  331. unsigned short op = ((unsigned short)a[6] << 8) | a[7];
  332. if (op != 1)
  333. return 0;
  334. if ((((unsigned short)a[0] << 8) | a[1]) != 1) /* htype Ethernet */
  335. return 0;
  336. if ((((unsigned short)a[2] << 8) | a[3]) != ET_IPV4)
  337. return 0;
  338. if (a[4] != 6 || a[5] != 4)
  339. return 0;
  340. if (!ip_eq(a + 24, my_ip)) /* target != us */
  341. return 0;
  342. for (i = 0; i < 6; i++)
  343. out[i] = frame[6 + i]; /* dst = requester */
  344. for (i = 0; i < 6; i++)
  345. out[6 + i] = mac[i]; /* src = us */
  346. out[12] = 0x08; out[13] = 0x06; /* ethertype ARP */
  347. {
  348. unsigned char *r = out + 14;
  349. r[0] = 0x00; r[1] = 0x01; /* htype Ethernet */
  350. r[2] = 0x08; r[3] = 0x00; /* ptype IPv4 */
  351. r[4] = 6; r[5] = 4;
  352. r[6] = 0x00; r[7] = 0x02; /* opcode reply */
  353. for (i = 0; i < 6; i++)
  354. r[8 + i] = mac[i]; /* sha */
  355. for (i = 0; i < 4; i++)
  356. r[14 + i] = my_ip[i]; /* spa */
  357. for (i = 0; i < 6; i++)
  358. r[18 + i] = a[8 + i]; /* tha = requester MAC */
  359. for (i = 0; i < 4; i++)
  360. r[24 + i] = a[14 + i]; /* tpa = requester IP */
  361. }
  362. return 42;
  363. }
  364. /* ICMP echo request for our IP -> ICMP echo reply. */
  365. if (et == ET_IPV4 && len >= 14 + 20) {
  366. const unsigned char *ip = frame + 14;
  367. unsigned int ihl, icmp_len, reply_len;
  368. unsigned short ip_len;
  369. const unsigned char *ic;
  370. unsigned char *rip, *ric;
  371. if ((ip[0] >> 4) != 4)
  372. return 0;
  373. ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  374. if (ihl < 20)
  375. return 0;
  376. if (ip[9] != IPPROTO_ICMP)
  377. return 0;
  378. if (!ip_eq(ip + 16, my_ip))
  379. return 0;
  380. ip_len = ((unsigned short)ip[2] << 8) | ip[3];
  381. if (ip_len < ihl || ip_len > len - 14)
  382. return 0;
  383. if (len < 14 + ihl + 8)
  384. return 0;
  385. ic = ip + ihl;
  386. if (ic[0] != ICMP_ECHO_REQUEST)
  387. return 0;
  388. icmp_len = ip_len - ihl;
  389. reply_len = 14 + ip_len;
  390. if (reply_len > maxout)
  391. return 0;
  392. /* Ethernet header. */
  393. for (i = 0; i < 6; i++)
  394. out[i] = frame[6 + i];
  395. for (i = 0; i < 6; i++)
  396. out[6 + i] = mac[i];
  397. out[12] = 0x08; out[13] = 0x00; /* ethertype IPv4 */
  398. /* IPv4 header (IHL 5, no options). */
  399. rip = out + 14;
  400. rip[0] = 0x45;
  401. rip[1] = 0x00;
  402. rip[2] = ip[2]; rip[3] = ip[3]; /* total length (same) */
  403. rip[4] = ip[4]; rip[5] = ip[5]; /* id */
  404. rip[6] = 0x00; rip[7] = 0x00; /* flags/frag */
  405. rip[8] = 64; /* TTL */
  406. rip[9] = IPPROTO_ICMP;
  407. rip[10] = 0; rip[11] = 0; /* checksum (computed below) */
  408. for (i = 0; i < 4; i++)
  409. rip[12 + i] = my_ip[i]; /* src = us */
  410. for (i = 0; i < 4; i++)
  411. rip[16 + i] = ip[12 + i]; /* dst = requester */
  412. /* ICMP: copy the whole request, then fix type/code/checksum. */
  413. ric = rip + ihl;
  414. for (i = 0; i < icmp_len; i++)
  415. ric[i] = ic[i];
  416. ric[0] = ICMP_ECHO_REPLY;
  417. ric[1] = 0;
  418. ric[2] = 0; ric[3] = 0;
  419. {
  420. unsigned short cs = net_checksum(rip, ihl);
  421. rip[10] = (unsigned char)(cs >> 8);
  422. rip[11] = (unsigned char)(cs & 0xFF);
  423. }
  424. {
  425. unsigned short cs = net_checksum(ric, icmp_len);
  426. ric[2] = (unsigned char)(cs >> 8);
  427. ric[3] = (unsigned char)(cs & 0xFF);
  428. }
  429. return (int)reply_len;
  430. }
  431. return 0;
  432. }