modem_utils.c 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678
  1. /*
  2. * Copyright (C) 2012 Samsung Electronics.
  3. *
  4. * This software is licensed under the terms of the GNU General Public
  5. * License version 2, as published by the Free Software Foundation, and
  6. * may be copied, distributed, and modified under those terms.
  7. *
  8. * This program is distributed in the hope that it will be useful,
  9. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. * GNU General Public License for more details.
  12. *
  13. */
  14. #include <linux/netdevice.h>
  15. #include <linux/platform_data/modem.h>
  16. #include <linux/platform_device.h>
  17. #include <linux/skbuff.h>
  18. #include <linux/ip.h>
  19. #include <linux/tcp.h>
  20. #include <linux/udp.h>
  21. #include <linux/rtc.h>
  22. #include <linux/time.h>
  23. #include <net/ip.h>
  24. #include "modem_prj.h"
  25. #include "modem_utils.h"
  26. #define CMD_SUSPEND ((unsigned short)(0x00CA))
  27. #define CMD_RESUME ((unsigned short)(0x00CB))
  28. int mif_dump_log(struct modem_shared *msd, struct io_device *iod)
  29. {
  30. struct sk_buff *skb;
  31. unsigned long read_len = 0;
  32. unsigned long int flags;
  33. spin_lock_irqsave(&msd->lock, flags);
  34. while (read_len < MAX_MIF_BUFF_SIZE) {
  35. skb = alloc_skb(MAX_IPC_SKB_SIZE, GFP_ATOMIC);
  36. if (!skb) {
  37. pr_err("[MIF] <%s> alloc skb failed\n", __func__);
  38. spin_unlock_irqrestore(&msd->lock, flags);
  39. return -ENOMEM;
  40. }
  41. memcpy(skb_put(skb, MAX_IPC_SKB_SIZE),
  42. msd->storage.addr + read_len, MAX_IPC_SKB_SIZE);
  43. skb_queue_tail(&iod->sk_rx_q, skb);
  44. read_len += MAX_IPC_SKB_SIZE;
  45. wake_up(&iod->wq);
  46. }
  47. spin_unlock_irqrestore(&msd->lock, flags);
  48. return 0;
  49. }
  50. static unsigned long long get_kernel_time(void)
  51. {
  52. int this_cpu;
  53. unsigned long flags;
  54. unsigned long long time;
  55. preempt_disable();
  56. raw_local_irq_save(flags);
  57. this_cpu = smp_processor_id();
  58. time = cpu_clock(this_cpu);
  59. preempt_enable();
  60. raw_local_irq_restore(flags);
  61. return time;
  62. }
  63. void mif_ipc_log(enum mif_log_id id,
  64. struct modem_shared *msd, const char *data, size_t len)
  65. {
  66. struct mif_ipc_block *block;
  67. unsigned long int flags;
  68. spin_lock_irqsave(&msd->lock, flags);
  69. block = (struct mif_ipc_block *)
  70. (msd->storage.addr + (MAX_LOG_SIZE * msd->storage.cnt));
  71. msd->storage.cnt = ((msd->storage.cnt + 1) < MAX_LOG_CNT) ?
  72. msd->storage.cnt + 1 : 0;
  73. spin_unlock_irqrestore(&msd->lock, flags);
  74. block->id = id;
  75. block->time = get_kernel_time();
  76. block->len = (len > MAX_IPC_LOG_SIZE) ? MAX_IPC_LOG_SIZE : len;
  77. memcpy(block->buff, data, block->len);
  78. }
  79. void _mif_irq_log(enum mif_log_id id, struct modem_shared *msd,
  80. struct mif_irq_map map, const char *data, size_t len)
  81. {
  82. struct mif_irq_block *block;
  83. unsigned long int flags;
  84. spin_lock_irqsave(&msd->lock, flags);
  85. block = (struct mif_irq_block *)
  86. (msd->storage.addr + (MAX_LOG_SIZE * msd->storage.cnt));
  87. msd->storage.cnt = ((msd->storage.cnt + 1) < MAX_LOG_CNT) ?
  88. msd->storage.cnt + 1 : 0;
  89. spin_unlock_irqrestore(&msd->lock, flags);
  90. block->id = id;
  91. block->time = get_kernel_time();
  92. memcpy(&(block->map), &map, sizeof(struct mif_irq_map));
  93. if (data)
  94. memcpy(block->buff, data,
  95. (len > MAX_IRQ_LOG_SIZE) ? MAX_IRQ_LOG_SIZE : len);
  96. }
  97. void _mif_com_log(enum mif_log_id id,
  98. struct modem_shared *msd, const char *format, ...)
  99. {
  100. struct mif_common_block *block;
  101. unsigned long int flags;
  102. va_list args;
  103. int ret;
  104. spin_lock_irqsave(&msd->lock, flags);
  105. block = (struct mif_common_block *)
  106. (msd->storage.addr + (MAX_LOG_SIZE * msd->storage.cnt));
  107. msd->storage.cnt = ((msd->storage.cnt + 1) < MAX_LOG_CNT) ?
  108. msd->storage.cnt + 1 : 0;
  109. spin_unlock_irqrestore(&msd->lock, flags);
  110. block->id = id;
  111. block->time = get_kernel_time();
  112. va_start(args, format);
  113. ret = vsnprintf(block->buff, MAX_COM_LOG_SIZE, format, args);
  114. va_end(args);
  115. }
  116. void _mif_time_log(enum mif_log_id id, struct modem_shared *msd,
  117. struct timespec epoch, const char *data, size_t len)
  118. {
  119. struct mif_time_block *block;
  120. unsigned long int flags;
  121. spin_lock_irqsave(&msd->lock, flags);
  122. block = (struct mif_time_block *)
  123. (msd->storage.addr + (MAX_LOG_SIZE * msd->storage.cnt));
  124. msd->storage.cnt = ((msd->storage.cnt + 1) < MAX_LOG_CNT) ?
  125. msd->storage.cnt + 1 : 0;
  126. spin_unlock_irqrestore(&msd->lock, flags);
  127. block->id = id;
  128. block->time = get_kernel_time();
  129. memcpy(&block->epoch, &epoch, sizeof(struct timespec));
  130. if (data)
  131. memcpy(block->buff, data,
  132. (len > MAX_IRQ_LOG_SIZE) ? MAX_IRQ_LOG_SIZE : len);
  133. }
  134. /* dump2hex
  135. * dump data to hex as fast as possible.
  136. * the length of @buf must be greater than "@len * 3"
  137. * it need 3 bytes per one data byte to print.
  138. */
  139. static inline int dump2hex(char *buf, const char *data, size_t len)
  140. {
  141. static const char *hex = "0123456789abcdef";
  142. char *dest = buf;
  143. int i;
  144. for (i = 0; i < len; i++) {
  145. *dest++ = hex[(data[i] >> 4) & 0xf];
  146. *dest++ = hex[data[i] & 0xf];
  147. *dest++ = ' ';
  148. }
  149. if (likely(len > 0))
  150. dest--; /* last space will be overwrited with null */
  151. *dest = '\0';
  152. return dest - buf;
  153. }
  154. /* print buffer as hex string */
  155. int pr_buffer(const char *tag, const char *data, size_t data_len,
  156. size_t max_len)
  157. {
  158. size_t len = min(data_len, max_len);
  159. unsigned char hexstr[len ? len * 3 : 1]; /* 1 <= sizeof <= max_len*3 */
  160. dump2hex(hexstr, data, len);
  161. /* don't change this printk to mif_debug for print this as level7 */
  162. return printk(KERN_INFO "%s(%u): %s%s\n", tag, data_len, hexstr,
  163. len == data_len ? "" : " ...");
  164. }
  165. /* flow control CM from CP, it use in serial devices */
  166. int link_rx_flowctl_cmd(struct link_device *ld, const char *data, size_t len)
  167. {
  168. struct modem_shared *msd = ld->msd;
  169. unsigned short *cmd, *end = (unsigned short *)(data + len);
  170. mif_debug("flow control cmd: size=%d\n", len);
  171. for (cmd = (unsigned short *)data; cmd < end; cmd++) {
  172. switch (*cmd) {
  173. case CMD_SUSPEND:
  174. iodevs_for_each(msd, iodev_netif_stop, 0);
  175. ld->raw_tx_suspended = true;
  176. mif_info("flowctl CMD_SUSPEND(%04X)\n", *cmd);
  177. break;
  178. case CMD_RESUME:
  179. iodevs_for_each(msd, iodev_netif_wake, 0);
  180. ld->raw_tx_suspended = false;
  181. complete_all(&ld->raw_tx_resumed_by_cp);
  182. mif_info("flowctl CMD_RESUME(%04X)\n", *cmd);
  183. break;
  184. default:
  185. mif_err("flowctl BACMD: %04X\n", *cmd);
  186. break;
  187. }
  188. }
  189. return 0;
  190. }
  191. struct io_device *get_iod_with_channel(struct modem_shared *msd,
  192. unsigned channel)
  193. {
  194. struct rb_node *n = msd->iodevs_tree_chan.rb_node;
  195. struct io_device *iodev;
  196. while (n) {
  197. iodev = rb_entry(n, struct io_device, node_chan);
  198. if (channel < iodev->id)
  199. n = n->rb_left;
  200. else if (channel > iodev->id)
  201. n = n->rb_right;
  202. else
  203. return iodev;
  204. }
  205. return NULL;
  206. }
  207. struct io_device *get_iod_with_format(struct modem_shared *msd,
  208. enum dev_format format)
  209. {
  210. struct rb_node *n = msd->iodevs_tree_fmt.rb_node;
  211. struct io_device *iodev;
  212. while (n) {
  213. iodev = rb_entry(n, struct io_device, node_fmt);
  214. if (format < iodev->format)
  215. n = n->rb_left;
  216. else if (format > iodev->format)
  217. n = n->rb_right;
  218. else
  219. return iodev;
  220. }
  221. return NULL;
  222. }
  223. struct io_device *insert_iod_with_channel(struct modem_shared *msd,
  224. unsigned channel, struct io_device *iod)
  225. {
  226. struct rb_node **p = &msd->iodevs_tree_chan.rb_node;
  227. struct rb_node *parent = NULL;
  228. struct io_device *iodev;
  229. while (*p) {
  230. parent = *p;
  231. iodev = rb_entry(parent, struct io_device, node_chan);
  232. if (channel < iodev->id)
  233. p = &(*p)->rb_left;
  234. else if (channel > iodev->id)
  235. p = &(*p)->rb_right;
  236. else
  237. return iodev;
  238. }
  239. rb_link_node(&iod->node_chan, parent, p);
  240. rb_insert_color(&iod->node_chan, &msd->iodevs_tree_chan);
  241. return NULL;
  242. }
  243. struct io_device *insert_iod_with_format(struct modem_shared *msd,
  244. enum dev_format format, struct io_device *iod)
  245. {
  246. struct rb_node **p = &msd->iodevs_tree_fmt.rb_node;
  247. struct rb_node *parent = NULL;
  248. struct io_device *iodev;
  249. while (*p) {
  250. parent = *p;
  251. iodev = rb_entry(parent, struct io_device, node_fmt);
  252. if (format < iodev->format)
  253. p = &(*p)->rb_left;
  254. else if (format > iodev->format)
  255. p = &(*p)->rb_right;
  256. else
  257. return iodev;
  258. }
  259. rb_link_node(&iod->node_fmt, parent, p);
  260. rb_insert_color(&iod->node_fmt, &msd->iodevs_tree_fmt);
  261. return NULL;
  262. }
  263. void iodevs_for_each(struct modem_shared *msd, action_fn action, void *args)
  264. {
  265. struct io_device *iod;
  266. struct rb_node *node = rb_first(&msd->iodevs_tree_chan);
  267. for (; node; node = rb_next(node)) {
  268. iod = rb_entry(node, struct io_device, node_chan);
  269. action(iod, args);
  270. }
  271. }
  272. void iodev_netif_wake(struct io_device *iod, void *args)
  273. {
  274. if (iod->io_typ == IODEV_NET && iod->ndev) {
  275. netif_wake_queue(iod->ndev);
  276. mif_info("%s\n", iod->name);
  277. }
  278. }
  279. void iodev_netif_stop(struct io_device *iod, void *args)
  280. {
  281. if (iod->io_typ == IODEV_NET && iod->ndev) {
  282. netif_stop_queue(iod->ndev);
  283. mif_info("%s\n", iod->name);
  284. }
  285. }
  286. static void iodev_set_tx_link(struct io_device *iod, void *args)
  287. {
  288. struct link_device *ld = (struct link_device *)args;
  289. if (iod->io_typ == IODEV_NET && IS_CONNECTED(iod, ld)) {
  290. set_current_link(iod, ld);
  291. mif_err("%s -> %s\n", iod->name, ld->name);
  292. }
  293. }
  294. void rawdevs_set_tx_link(struct modem_shared *msd, enum modem_link link_type)
  295. {
  296. struct link_device *ld = find_linkdev(msd, link_type);
  297. if (ld)
  298. iodevs_for_each(msd, iodev_set_tx_link, ld);
  299. }
  300. void mif_add_timer(struct timer_list *timer, unsigned long expire,
  301. void (*function)(unsigned long), unsigned long data)
  302. {
  303. if (timer_pending(timer))
  304. return;
  305. init_timer(timer);
  306. timer->expires = get_jiffies_64() + expire;
  307. timer->function = function;
  308. timer->data = data;
  309. add_timer(timer);
  310. }
  311. void mif_print_data(char *buf, int len)
  312. {
  313. int words = len >> 4;
  314. int residue = len - (words << 4);
  315. int i;
  316. char *b;
  317. char last[80];
  318. char tb[8];
  319. /* Make the last line, if ((len % 16) > 0) */
  320. if (residue > 0) {
  321. memset(last, 0, sizeof(last));
  322. memset(tb, 0, sizeof(tb));
  323. b = buf + (words << 4);
  324. sprintf(last, "%04X: ", (words << 4));
  325. for (i = 0; i < residue; i++) {
  326. sprintf(tb, "%02x ", b[i]);
  327. strcat(last, tb);
  328. if ((i & 0x3) == 0x3) {
  329. sprintf(tb, " ");
  330. strcat(last, tb);
  331. }
  332. }
  333. }
  334. for (i = 0; i < words; i++) {
  335. b = buf + (i << 4);
  336. mif_err("%04X: "
  337. "%02x %02x %02x %02x %02x %02x %02x %02x "
  338. "%02x %02x %02x %02x %02x %02x %02x %02x\n",
  339. (i << 4),
  340. b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7],
  341. b[8], b[9], b[10], b[11], b[12], b[13], b[14], b[15]);
  342. }
  343. /* Print the last line */
  344. if (residue > 0)
  345. mif_err("%s\n", last);
  346. }
  347. void print_sipc4_hdlc_fmt_frame(const u8 *psrc)
  348. {
  349. u8 *frm; /* HDLC Frame */
  350. struct fmt_hdr *hh; /* HDLC Header */
  351. struct sipc_fmt_hdr *fh; /* IPC Header */
  352. u16 hh_len = sizeof(struct fmt_hdr);
  353. u16 fh_len = sizeof(struct sipc_fmt_hdr);
  354. u8 *data;
  355. int dlen;
  356. /* Actual HDLC header starts from after START flag (0x7F) */
  357. frm = (u8 *)(psrc + 1);
  358. /* Point HDLC header and IPC header */
  359. hh = (struct fmt_hdr *)(frm);
  360. fh = (struct sipc_fmt_hdr *)(frm + hh_len);
  361. /* Point IPC data */
  362. data = frm + (hh_len + fh_len);
  363. dlen = hh->len - (hh_len + fh_len);
  364. mif_err("--------------------HDLC & FMT HEADER----------------------\n");
  365. mif_err("HDLC: length %d, control 0x%02x\n", hh->len, hh->control);
  366. mif_err("(M)0x%02X, (S)0x%02X, (T)0x%02X, mseq %d, aseq %d, len %d\n",
  367. fh->main_cmd, fh->sub_cmd, fh->cmd_type,
  368. fh->msg_seq, fh->ack_seq, fh->len);
  369. /* mif_err("-----------------------IPC FMT DATA------------------------\n");
  370. if (dlen > 0) {
  371. if (dlen > 64)
  372. dlen = 64;
  373. mif_print_data(data, dlen);
  374. }*/
  375. mif_err("-----------------------------------------------------------\n");
  376. }
  377. void print_sipc4_fmt_frame(const u8 *psrc)
  378. {
  379. struct sipc_fmt_hdr *fh = (struct sipc_fmt_hdr *)psrc;
  380. u16 fh_len = sizeof(struct sipc_fmt_hdr);
  381. u8 *data;
  382. int dlen;
  383. /* Point IPC data */
  384. data = (u8 *)(psrc + fh_len);
  385. dlen = fh->len - fh_len;
  386. mif_err("----------------------IPC FMT HEADER-----------------------\n");
  387. mif_err("(M)0x%02X, (S)0x%02X, (T)0x%02X, mseq:%d, aseq:%d, len:%d\n",
  388. fh->main_cmd, fh->sub_cmd, fh->cmd_type,
  389. fh->msg_seq, fh->ack_seq, fh->len);
  390. /* mif_err("-----------------------IPC FMT DATA------------------------\n");
  391. if (dlen > 0)
  392. mif_print_data(data, dlen);*/
  393. mif_err("-----------------------------------------------------------\n");
  394. }
  395. static void print_tcp_header(u8 *pkt)
  396. {
  397. int i;
  398. char tcp_flags[32];
  399. struct tcphdr *tcph = (struct tcphdr *)pkt;
  400. u8 *opt = pkt + TCP_HDR_SIZE;
  401. unsigned opt_len = (tcph->doff << 2) - TCP_HDR_SIZE;
  402. /*-------------------------------------------------------------------------
  403. TCP Header Format
  404. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  405. | Source Port | Destination Port |
  406. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  407. | Sequence Number |
  408. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  409. | Acknowledgment Number |
  410. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  411. | Data | |C|E|U|A|P|R|S|F| |
  412. | Offset| Rsvd |W|C|R|C|S|S|Y|I| Window |
  413. | | |R|E|G|K|H|T|N|N| |
  414. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  415. | Checksum | Urgent Pointer |
  416. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  417. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  418. | Options | Padding |
  419. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  420. | data |
  421. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  422. -------------------------------------------------------------------------*/
  423. memset(tcp_flags, 0, sizeof(tcp_flags));
  424. if (tcph->cwr)
  425. strcat(tcp_flags, "CWR ");
  426. if (tcph->ece)
  427. strcat(tcp_flags, "ECE");
  428. if (tcph->urg)
  429. strcat(tcp_flags, "URG ");
  430. if (tcph->ack)
  431. strcat(tcp_flags, "ACK ");
  432. if (tcph->psh)
  433. strcat(tcp_flags, "PSH ");
  434. if (tcph->rst)
  435. strcat(tcp_flags, "RST ");
  436. if (tcph->syn)
  437. strcat(tcp_flags, "SYN ");
  438. if (tcph->fin)
  439. strcat(tcp_flags, "FIN ");
  440. mif_err("TCP:: Src.Port %u, Dst.Port %u\n",
  441. ntohs(tcph->source), ntohs(tcph->dest));
  442. mif_err("TCP:: SEQ 0x%08X(%u), ACK 0x%08X(%u)\n",
  443. ntohs(tcph->seq), ntohs(tcph->seq),
  444. ntohs(tcph->ack_seq), ntohs(tcph->ack_seq));
  445. mif_err("TCP:: Flags {%s}\n", tcp_flags);
  446. mif_err("TCP:: Window %u, Checksum 0x%04X, Urg Pointer %u\n",
  447. ntohs(tcph->window), ntohs(tcph->check), ntohs(tcph->urg_ptr));
  448. if (opt_len > 0) {
  449. mif_err("TCP:: Options {");
  450. for (i = 0; i < opt_len; i++)
  451. mif_err("%02X ", opt[i]);
  452. mif_err("}\n");
  453. }
  454. }
  455. static void print_udp_header(u8 *pkt)
  456. {
  457. struct udphdr *udph = (struct udphdr *)pkt;
  458. /*-------------------------------------------------------------------------
  459. UDP Header Format
  460. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  461. | Source Port | Destination Port |
  462. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  463. | Length | Checksum |
  464. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  465. | data |
  466. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  467. -------------------------------------------------------------------------*/
  468. mif_err("UDP:: Src.Port %u, Dst.Port %u\n",
  469. ntohs(udph->source), ntohs(udph->dest));
  470. mif_err("UDP:: Length %u, Checksum 0x%04X\n",
  471. ntohs(udph->len), ntohs(udph->check));
  472. if (ntohs(udph->dest) == 53)
  473. mif_err("UDP:: DNS query!!!\n");
  474. if (ntohs(udph->source) == 53)
  475. mif_err("UDP:: DNS response!!!\n");
  476. }
  477. void print_ip4_packet(u8 *ip_pkt)
  478. {
  479. char ip_flags[16];
  480. struct iphdr *iph = (struct iphdr *)ip_pkt;
  481. u8 *pkt = ip_pkt + (iph->ihl << 2);
  482. u16 flags = (ntohs(iph->frag_off) & 0xE000);
  483. u16 frag_off = (ntohs(iph->frag_off) & 0x1FFF);
  484. mif_err("-----------------------------------------------------------\n");
  485. /*---------------------------------------------------------------------------
  486. IPv4 Header Format
  487. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  488. |Version| IHL |Type of Service| Total Length |
  489. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  490. | Identification |C|D|M| Fragment Offset |
  491. | |E|F|F| |
  492. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  493. | Time to Live | Protocol | Header Checksum |
  494. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  495. | Source Address |
  496. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  497. | Destination Address |
  498. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  499. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  500. | Options | Padding |
  501. +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  502. IHL - Header Length
  503. Flags - Consist of 3 bits
  504. The 1st bit is "Congestion" bit.
  505. The 2nd bit is "Dont Fragment" bit.
  506. The 3rd bit is "More Fragments" bit.
  507. ---------------------------------------------------------------------------*/
  508. memset(ip_flags, 0, sizeof(ip_flags));
  509. if (flags & IP_CE)
  510. strcat(ip_flags, "C");
  511. if (flags & IP_DF)
  512. strcat(ip_flags, "D");
  513. if (flags & IP_MF)
  514. strcat(ip_flags, "M");
  515. mif_err("IP4:: Version %u, Header Length %u, TOS %u, Length %u\n",
  516. iph->version, (iph->ihl << 2), iph->tos, ntohs(iph->tot_len));
  517. mif_err("IP4:: I%u, Fragment Offset %u\n",
  518. ntohs(iph->id), frag_off);
  519. mif_err("IP4:: Flags {%s}\n", ip_flags);
  520. mif_err("IP4:: TTL %u, Protocol %u, Header Checksum 0x%04X\n",
  521. iph->ttl, iph->protocol, ntohs(iph->check));
  522. mif_err("IP4:: Src.IP %u.%u.%u.%u, Dst.IP %u.%u.%u.%u\n",
  523. ip_pkt[12], ip_pkt[13], ip_pkt[14], ip_pkt[15],
  524. ip_pkt[16], ip_pkt[17], ip_pkt[18], ip_pkt[19]);
  525. switch (iph->protocol) {
  526. case 6:
  527. /* TCP */
  528. print_tcp_header(pkt);
  529. break;
  530. case 17:
  531. /* UDP */
  532. print_udp_header(pkt);
  533. break;
  534. default:
  535. break;
  536. }
  537. mif_err("-----------------------------------------------------------\n");
  538. }
  539. bool is_dns_packet(u8 *ip_pkt)
  540. {
  541. struct iphdr *iph = (struct iphdr *)ip_pkt;
  542. struct udphdr *udph = (struct udphdr *)(ip_pkt + (iph->ihl << 2));
  543. /* If this packet is not a UDP packet, return here. */
  544. if (iph->protocol != 17)
  545. return false;
  546. if (ntohs(udph->dest) == 53 || ntohs(udph->source) == 53)
  547. return true;
  548. else
  549. return false;
  550. }
  551. bool is_syn_packet(u8 *ip_pkt)
  552. {
  553. struct iphdr *iph = (struct iphdr *)ip_pkt;
  554. struct tcphdr *tcph = (struct tcphdr *)(ip_pkt + (iph->ihl << 2));
  555. /* If this packet is not a TCP packet, return here. */
  556. if (iph->protocol != 6)
  557. return false;
  558. if (tcph->syn)
  559. return true;
  560. else
  561. return false;
  562. }