pkt.c 6.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342
  1. /*
  2. * pkt.c
  3. *
  4. * Copyright (c) 2001 Dug Song <dugsong@monkey.org>
  5. *
  6. * $Id$
  7. */
  8. #include "config.h"
  9. #include "common/err.h"
  10. #include <sys/types.h>
  11. #include <stdio.h>
  12. #include <stdlib.h>
  13. #include <string.h>
  14. #include "bget.h"
  15. #include "pkt.h"
  16. static struct pkt **pvbase;
  17. static int pvlen;
  18. void
  19. pkt_init(int size)
  20. {
  21. bectl(NULL, malloc, free, sizeof(struct pkt) * size);
  22. }
  23. void
  24. pkt_close(void)
  25. {
  26. if (pvbase) {
  27. pvlen = 0;
  28. free (pvbase);
  29. }
  30. }
  31. struct pkt *
  32. pkt_new(void)
  33. {
  34. struct pkt *pkt;
  35. if ((pkt = bget(sizeof(*pkt))) == NULL)
  36. return (NULL);
  37. timerclear(&pkt->pkt_ts);
  38. // memset(&pkt->pkt_ev, 0, sizeof(pkt->pkt_ev));
  39. pkt->pkt_data = pkt->pkt_buf + PKT_BUF_ALIGN;
  40. pkt->pkt_eth = (struct eth_hdr *)pkt->pkt_data;
  41. pkt->pkt_eth_data = pkt->pkt_data + ETH_HDR_LEN;
  42. pkt->pkt_ip_data = pkt->pkt_data + ETH_HDR_LEN + IP_HDR_LEN;
  43. pkt->pkt_tcp_data = NULL;
  44. pkt->pkt_end = pkt->pkt_ip_data;
  45. return (pkt);
  46. }
  47. struct pkt *
  48. pkt_dup(struct pkt *pkt)
  49. {
  50. struct pkt *new;
  51. off_t off;
  52. if ((new = bget(sizeof(*new))) == NULL)
  53. return (NULL);
  54. off = new->pkt_buf - pkt->pkt_buf;
  55. new->pkt_ts = pkt->pkt_ts;
  56. // memset(&new->pkt_ev, 0, sizeof(new->pkt_ev));
  57. new->pkt_data = pkt->pkt_data + off;
  58. new->pkt_eth = (pkt->pkt_eth != NULL) ?
  59. (struct eth_hdr *)new->pkt_data : NULL;
  60. new->pkt_eth_data = (pkt->pkt_eth_data != NULL) ?
  61. pkt->pkt_eth_data + off : NULL;
  62. new->pkt_ip_data = (pkt->pkt_ip_data != NULL) ?
  63. pkt->pkt_ip_data + off : NULL;
  64. new->pkt_tcp_data = (pkt->pkt_tcp_data != NULL) ?
  65. pkt->pkt_tcp_data + off : NULL;
  66. memcpy(new->pkt_data, pkt->pkt_data, pkt->pkt_end - pkt->pkt_data);
  67. new->pkt_end = pkt->pkt_end + off;
  68. return (new);
  69. }
  70. #define IP6_IS_EXT(n) \
  71. ((n) == IP_PROTO_HOPOPTS || (n) == IP_PROTO_DSTOPTS || \
  72. (n) == IP_PROTO_ROUTING || (n) == IP_PROTO_FRAGMENT)
  73. void
  74. pkt_decorate(struct pkt *pkt)
  75. {
  76. u_char *p;
  77. uint16_t eth_type;
  78. int hl, len, off;
  79. uint8_t next_hdr;
  80. struct ip6_ext_hdr *ext;
  81. pkt->pkt_data = pkt->pkt_buf + PKT_BUF_ALIGN;
  82. pkt->pkt_eth = NULL;
  83. pkt->pkt_ip = NULL;
  84. pkt->pkt_ip_data = NULL;
  85. pkt->pkt_tcp_data = NULL;
  86. p = pkt->pkt_data;
  87. if (p + ETH_HDR_LEN > pkt->pkt_end)
  88. return;
  89. pkt->pkt_eth = (struct eth_hdr *)p;
  90. p += ETH_HDR_LEN;
  91. eth_type = htons(pkt->pkt_eth->eth_type);
  92. if (eth_type == ETH_TYPE_IP) {
  93. if (p + IP_HDR_LEN > pkt->pkt_end)
  94. return;
  95. pkt->pkt_eth_data = p;
  96. /* If IP header length is longer than packet length, stop. */
  97. hl = pkt->pkt_ip->ip_hl << 2;
  98. if (p + hl > pkt->pkt_end) {
  99. pkt->pkt_ip = NULL;
  100. return;
  101. }
  102. len = ntohs(pkt->pkt_ip->ip_len);
  103. /* If IP length is 0, this packet was generated by wireshark
  104. on systems with TCP Segmentation offloading to NIC enabled.
  105. Calculate the IP length from packet end and packet data pointers.
  106. */
  107. if (0 == len)
  108. len = (pkt->pkt_end - (pkt->pkt_data + ETH_HDR_LEN));
  109. /* If IP length is longer than packet length, stop.
  110. */
  111. if (p + len > pkt->pkt_end)
  112. return;
  113. /* If IP fragment, stop. */
  114. off = ntohs(pkt->pkt_ip->ip_off);
  115. if ((off & IP_OFFMASK) != 0 || (off & IP_MF) != 0)
  116. return;
  117. pkt->pkt_end = p + len;
  118. p += hl;
  119. next_hdr = pkt->pkt_ip->ip_p;
  120. } else if (eth_type == ETH_TYPE_IPV6) {
  121. if (p + IP6_HDR_LEN > pkt->pkt_end)
  122. return;
  123. pkt->pkt_eth_data = p;
  124. p += IP6_HDR_LEN;
  125. next_hdr = pkt->pkt_ip6->ip6_nxt;
  126. for (; IP6_IS_EXT(next_hdr); p += (ext->ext_len + 1) << 3) {
  127. if (p > pkt->pkt_end)
  128. return;
  129. ext = (struct ip6_ext_hdr *)p;
  130. next_hdr = ext->ext_nxt;
  131. }
  132. } else {
  133. return;
  134. }
  135. /* If transport layer header is longer than packet length, stop. */
  136. switch (next_hdr) {
  137. case IP_PROTO_ICMP:
  138. case IP_PROTO_ICMPV6:
  139. hl = ICMP_HDR_LEN;
  140. break;
  141. case IP_PROTO_TCP:
  142. if (p + TCP_HDR_LEN > pkt->pkt_end)
  143. return;
  144. hl = ((struct tcp_hdr *)p)->th_off << 2;
  145. break;
  146. case IP_PROTO_UDP:
  147. hl = UDP_HDR_LEN;
  148. break;
  149. default:
  150. return;
  151. }
  152. if (p + hl > pkt->pkt_end)
  153. return;
  154. pkt->pkt_ip_data = p;
  155. p += hl;
  156. /* Check for transport layer data. */
  157. switch (next_hdr) {
  158. case IP_PROTO_ICMP:
  159. pkt->pkt_icmp_msg = (union icmp_msg *)p;
  160. switch (pkt->pkt_icmp->icmp_type) {
  161. case ICMP_ECHO:
  162. case ICMP_ECHOREPLY:
  163. hl = sizeof(pkt->pkt_icmp_msg->echo);
  164. break;
  165. case ICMP_UNREACH:
  166. if (pkt->pkt_icmp->icmp_code == ICMP_UNREACH_NEEDFRAG)
  167. hl = sizeof(pkt->pkt_icmp_msg->needfrag);
  168. else
  169. hl = sizeof(pkt->pkt_icmp_msg->unreach);
  170. break;
  171. case ICMP_SRCQUENCH:
  172. case ICMP_REDIRECT:
  173. case ICMP_TIMEXCEED:
  174. case ICMP_PARAMPROB:
  175. hl = sizeof(pkt->pkt_icmp_msg->srcquench);
  176. break;
  177. case ICMP_RTRADVERT:
  178. hl = sizeof(pkt->pkt_icmp_msg->rtradvert);
  179. break;
  180. case ICMP_RTRSOLICIT:
  181. hl = sizeof(pkt->pkt_icmp_msg->rtrsolicit);
  182. break;
  183. case ICMP_TSTAMP:
  184. case ICMP_TSTAMPREPLY:
  185. hl = sizeof(pkt->pkt_icmp_msg->tstamp);
  186. break;
  187. case ICMP_INFO:
  188. case ICMP_INFOREPLY:
  189. case ICMP_DNS:
  190. hl = sizeof(pkt->pkt_icmp_msg->info);
  191. break;
  192. case ICMP_MASK:
  193. case ICMP_MASKREPLY:
  194. hl = sizeof(pkt->pkt_icmp_msg->mask);
  195. break;
  196. case ICMP_DNSREPLY:
  197. hl = sizeof(pkt->pkt_icmp_msg->dnsreply);
  198. break;
  199. default:
  200. hl = pkt->pkt_end - p + 1;
  201. break;
  202. }
  203. if (p + hl > pkt->pkt_end)
  204. pkt->pkt_icmp_msg = NULL;
  205. break;
  206. case IP_PROTO_ICMPV6:
  207. pkt->pkt_icmp_msg = (union icmp_msg *)p;
  208. break;
  209. case IP_PROTO_TCP:
  210. if (p < pkt->pkt_end)
  211. pkt->pkt_tcp_data = p;
  212. break;
  213. case IP_PROTO_UDP:
  214. if (pkt->pkt_ip_data + ntohs(pkt->pkt_udp->uh_ulen) <=
  215. pkt->pkt_end)
  216. pkt->pkt_udp_data = p;
  217. break;
  218. }
  219. }
  220. void
  221. pkt_free(struct pkt *pkt)
  222. {
  223. brel(pkt);
  224. }
  225. void
  226. pktq_reverse(struct pktq *pktq)
  227. {
  228. struct pktq tmpq;
  229. struct pkt *pkt, *next;
  230. TAILQ_INIT(&tmpq);
  231. for (pkt = TAILQ_FIRST(pktq); pkt != TAILQ_END(pktq); pkt = next) {
  232. next = TAILQ_NEXT(pkt, pkt_next);
  233. TAILQ_INSERT_HEAD(&tmpq, pkt, pkt_next);
  234. }
  235. TAILQ_COPY(pktq, &tmpq, pkt_next);
  236. }
  237. void
  238. pktq_shuffle(rand_t *r, struct pktq *pktq)
  239. {
  240. struct pkt *pkt;
  241. int i;
  242. i = 0;
  243. TAILQ_FOREACH(pkt, pktq, pkt_next) {
  244. i++;
  245. }
  246. if (i > 0 && i > pvlen) {
  247. pvlen = i;
  248. if (pvbase == NULL)
  249. pvbase = malloc(sizeof(pkt) * pvlen);
  250. else
  251. pvbase = realloc(pvbase, sizeof(pkt) * pvlen);
  252. }
  253. if (!pvbase)
  254. err(-1, "out of memory\n");
  255. i = 0;
  256. TAILQ_FOREACH(pkt, pktq, pkt_next) {
  257. pvbase[i++] = pkt;
  258. }
  259. TAILQ_INIT(pktq);
  260. rand_shuffle(r, pvbase, i, sizeof(pkt));
  261. while (--i >= 0) {
  262. TAILQ_INSERT_TAIL(pktq, pvbase[i], pkt_next);
  263. }
  264. }
  265. struct pkt *
  266. pktq_random(rand_t *r, struct pktq *pktq)
  267. {
  268. struct pkt *pkt;
  269. unsigned int i;
  270. i = 0;
  271. TAILQ_FOREACH(pkt, pktq, pkt_next) {
  272. i++;
  273. }
  274. if (i)
  275. --i;
  276. if (i)
  277. i = rand_uint32(r) % i;
  278. pkt = TAILQ_FIRST(pktq);
  279. while (pkt && ((int)--i) >= 0) {
  280. pkt = TAILQ_NEXT(pkt, pkt_next);
  281. }
  282. return (pkt);
  283. }