net.c 7.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342
  1. /* Minimal network stack: ARP + ICMP echo (ping) over the NE2000 driver.
  2. Freestanding, no malloc; fixed-size buffers and a single configured IP. */
  3. #include "io.h"
  4. #include "ne2000.h"
  5. #include "net.h"
  6. static unsigned char my_ip[4] = { 192, 168, 0, 99 };
  7. /* ---- small print helpers (framebuffer console) ---- */
  8. static void print_hex2(unsigned int v)
  9. {
  10. static const char hex[] = "0123456789ABCDEF";
  11. io_putchar(hex[(v >> 4) & 0xF]);
  12. io_putchar(hex[v & 0xF]);
  13. }
  14. static void print_hex4(unsigned int v)
  15. {
  16. print_hex2((v >> 8) & 0xFF);
  17. print_hex2(v & 0xFF);
  18. }
  19. static void print_dec(unsigned int v)
  20. {
  21. char b[12];
  22. int i = 0;
  23. if (v == 0) {
  24. io_putchar('0');
  25. return;
  26. }
  27. while (v) {
  28. b[i++] = (char)('0' + (v % 10));
  29. v /= 10;
  30. }
  31. while (i)
  32. io_putchar(b[--i]);
  33. }
  34. static void print_mac(const unsigned char *m)
  35. {
  36. unsigned int i;
  37. for (i = 0; i < 6; i++) {
  38. if (i)
  39. io_putchar(':');
  40. print_hex2(m[i]);
  41. }
  42. }
  43. static void print_ip(const unsigned char *p)
  44. {
  45. unsigned int i;
  46. for (i = 0; i < 4; i++) {
  47. if (i)
  48. io_putchar('.');
  49. print_dec(p[i]);
  50. }
  51. }
  52. static int ip_eq(const unsigned char *a, const unsigned char *b)
  53. {
  54. return a[0] == b[0] && a[1] == b[1] && a[2] == b[2] && a[3] == b[3];
  55. }
  56. /* Internet checksum (16-bit one's complement, network byte order). */
  57. static unsigned short checksum(const void *data, unsigned int len)
  58. {
  59. const unsigned char *p = (const unsigned char *)data;
  60. unsigned int sum = 0;
  61. while (len > 1) {
  62. sum += ((unsigned int)p[0] << 8) | p[1];
  63. p += 2;
  64. len -= 2;
  65. }
  66. if (len)
  67. sum += (unsigned int)p[0] << 8;
  68. while (sum >> 16)
  69. sum = (sum & 0xFFFFu) + (sum >> 16);
  70. return (unsigned short)(~sum);
  71. }
  72. /* ---- IP address config ---- */
  73. void net_set_ip(const unsigned char ip[4])
  74. {
  75. unsigned int i;
  76. for (i = 0; i < 4; i++)
  77. my_ip[i] = ip[i];
  78. }
  79. void net_get_ip(unsigned char ip[4])
  80. {
  81. unsigned int i;
  82. for (i = 0; i < 4; i++)
  83. ip[i] = my_ip[i];
  84. }
  85. int net_parse_ip(const char *s, unsigned char ip[4])
  86. {
  87. unsigned int i, val = 0, nparts = 0;
  88. int have_digit = 0;
  89. for (i = 0; ; i++) {
  90. char c = s[i];
  91. if (c >= '0' && c <= '9') {
  92. val = val * 10 + (unsigned int)(c - '0');
  93. if (val > 255)
  94. return -1;
  95. have_digit = 1;
  96. } else if (c == '.' || c == '\0') {
  97. if (!have_digit || nparts >= 4)
  98. return -1;
  99. ip[nparts++] = (unsigned char)val;
  100. val = 0;
  101. have_digit = 0;
  102. if (c == '\0')
  103. break;
  104. } else {
  105. return -1;
  106. }
  107. }
  108. return (nparts == 4) ? 0 : -1;
  109. }
  110. void net_print_ip(void)
  111. {
  112. print_ip(my_ip);
  113. }
  114. /* ---- ethertype naming ---- */
  115. const char *net_ethertype_name(unsigned short et)
  116. {
  117. switch (et) {
  118. case ET_IPV4: return "IPv4";
  119. case ET_ARP: return "ARP";
  120. case ET_IPV6: return "IPv6";
  121. default: return 0;
  122. }
  123. }
  124. /* ---- frame decoding ---- */
  125. void net_decode_frame(const unsigned char *f, unsigned int len)
  126. {
  127. unsigned short et;
  128. const char *name;
  129. if (len < 14) {
  130. io_putstr("(short frame len=");
  131. print_dec(len);
  132. io_putstr(")");
  133. return;
  134. }
  135. et = ((unsigned short)f[12] << 8) | f[13];
  136. print_mac(f + 6);
  137. io_putstr(" -> ");
  138. print_mac(f);
  139. io_putstr(" ");
  140. name = net_ethertype_name(et);
  141. if (name)
  142. io_putstr(name);
  143. else {
  144. io_putstr("0x");
  145. print_hex4(et);
  146. }
  147. io_putstr(" len=");
  148. print_dec(len);
  149. if (et == ET_ARP && len >= 14 + 28) {
  150. const unsigned char *a = f + 14;
  151. unsigned short op = ((unsigned short)a[6] << 8) | a[7];
  152. if (op == 1) {
  153. io_putstr(" who-has ");
  154. print_ip(a + 24);
  155. io_putstr(" tell ");
  156. print_ip(a + 14);
  157. } else if (op == 2) {
  158. io_putstr(" ");
  159. print_ip(a + 14);
  160. io_putstr(" is-at ");
  161. print_mac(a + 8);
  162. }
  163. } else if (et == ET_IPV4 && len >= 14 + 20) {
  164. const unsigned char *ip = f + 14;
  165. unsigned int ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  166. unsigned char proto = ip[9];
  167. io_putstr(" ");
  168. print_ip(ip + 12);
  169. io_putstr(" -> ");
  170. print_ip(ip + 16);
  171. io_putstr(" proto=");
  172. print_dec(proto);
  173. if (proto == IPPROTO_ICMP && ihl >= 20 && len >= 14 + ihl + 8) {
  174. const unsigned char *ic = ip + ihl;
  175. io_putstr(" ICMP type=");
  176. print_dec(ic[0]);
  177. io_putstr(" code=");
  178. print_dec(ic[1]);
  179. }
  180. }
  181. }
  182. /* ---- reply generation ---- */
  183. int net_build_reply(const unsigned char *frame, unsigned int len,
  184. unsigned char *out, unsigned int maxout)
  185. {
  186. unsigned char mac[6];
  187. unsigned short et;
  188. unsigned int i;
  189. if (len < 14)
  190. return 0;
  191. et = ((unsigned short)frame[12] << 8) | frame[13];
  192. ne2000_get_mac(mac);
  193. /* ARP request for our IP -> ARP reply. */
  194. if (et == ET_ARP && len >= 14 + 28) {
  195. const unsigned char *a = frame + 14;
  196. unsigned short op = ((unsigned short)a[6] << 8) | a[7];
  197. if (op != 1)
  198. return 0;
  199. if ((((unsigned short)a[0] << 8) | a[1]) != 1) /* htype Ethernet */
  200. return 0;
  201. if ((((unsigned short)a[2] << 8) | a[3]) != ET_IPV4)
  202. return 0;
  203. if (a[4] != 6 || a[5] != 4)
  204. return 0;
  205. if (!ip_eq(a + 24, my_ip)) /* target != us */
  206. return 0;
  207. for (i = 0; i < 6; i++)
  208. out[i] = frame[6 + i]; /* dst = requester */
  209. for (i = 0; i < 6; i++)
  210. out[6 + i] = mac[i]; /* src = us */
  211. out[12] = 0x08; out[13] = 0x06; /* ethertype ARP */
  212. {
  213. unsigned char *r = out + 14;
  214. r[0] = 0x00; r[1] = 0x01; /* htype Ethernet */
  215. r[2] = 0x08; r[3] = 0x00; /* ptype IPv4 */
  216. r[4] = 6; r[5] = 4;
  217. r[6] = 0x00; r[7] = 0x02; /* opcode reply */
  218. for (i = 0; i < 6; i++)
  219. r[8 + i] = mac[i]; /* sha */
  220. for (i = 0; i < 4; i++)
  221. r[14 + i] = my_ip[i]; /* spa */
  222. for (i = 0; i < 6; i++)
  223. r[18 + i] = a[8 + i]; /* tha = requester MAC */
  224. for (i = 0; i < 4; i++)
  225. r[24 + i] = a[14 + i]; /* tpa = requester IP */
  226. }
  227. return 42;
  228. }
  229. /* ICMP echo request for our IP -> ICMP echo reply. */
  230. if (et == ET_IPV4 && len >= 14 + 20) {
  231. const unsigned char *ip = frame + 14;
  232. unsigned int ihl, icmp_len, reply_len;
  233. unsigned short ip_len;
  234. const unsigned char *ic;
  235. unsigned char *rip, *ric;
  236. if ((ip[0] >> 4) != 4)
  237. return 0;
  238. ihl = (unsigned int)(ip[0] & 0x0F) * 4;
  239. if (ihl < 20)
  240. return 0;
  241. if (ip[9] != IPPROTO_ICMP)
  242. return 0;
  243. if (!ip_eq(ip + 16, my_ip))
  244. return 0;
  245. ip_len = ((unsigned short)ip[2] << 8) | ip[3];
  246. if (ip_len < ihl || ip_len > len - 14)
  247. return 0;
  248. if (len < 14 + ihl + 8)
  249. return 0;
  250. ic = ip + ihl;
  251. if (ic[0] != ICMP_ECHO_REQUEST)
  252. return 0;
  253. icmp_len = ip_len - ihl;
  254. reply_len = 14 + ip_len;
  255. if (reply_len > maxout)
  256. return 0;
  257. /* Ethernet header. */
  258. for (i = 0; i < 6; i++)
  259. out[i] = frame[6 + i];
  260. for (i = 0; i < 6; i++)
  261. out[6 + i] = mac[i];
  262. out[12] = 0x08; out[13] = 0x00; /* ethertype IPv4 */
  263. /* IPv4 header (IHL 5, no options). */
  264. rip = out + 14;
  265. rip[0] = 0x45;
  266. rip[1] = 0x00;
  267. rip[2] = ip[2]; rip[3] = ip[3]; /* total length (same) */
  268. rip[4] = ip[4]; rip[5] = ip[5]; /* id */
  269. rip[6] = 0x00; rip[7] = 0x00; /* flags/frag */
  270. rip[8] = 64; /* TTL */
  271. rip[9] = IPPROTO_ICMP;
  272. rip[10] = 0; rip[11] = 0; /* checksum (computed below) */
  273. for (i = 0; i < 4; i++)
  274. rip[12 + i] = my_ip[i]; /* src = us */
  275. for (i = 0; i < 4; i++)
  276. rip[16 + i] = ip[12 + i]; /* dst = requester */
  277. /* ICMP: copy the whole request, then fix type/code/checksum. */
  278. ric = rip + ihl;
  279. for (i = 0; i < icmp_len; i++)
  280. ric[i] = ic[i];
  281. ric[0] = ICMP_ECHO_REPLY;
  282. ric[1] = 0;
  283. ric[2] = 0; ric[3] = 0;
  284. {
  285. unsigned short cs = checksum(rip, ihl);
  286. rip[10] = (unsigned char)(cs >> 8);
  287. rip[11] = (unsigned char)(cs & 0xFF);
  288. }
  289. {
  290. unsigned short cs = checksum(ric, icmp_len);
  291. ric[2] = (unsigned char)(cs >> 8);
  292. ric[3] = (unsigned char)(cs & 0xFF);
  293. }
  294. return (int)reply_len;
  295. }
  296. return 0;
  297. }