/* * Copyright (C) 2011 Samsung Electronics. * * This software is licensed under the terms of the GNU General Public * License version 2, as published by the Free Software Foundation, and * may be copied, distributed, and modified under those terms. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "modem_prj.h" #include "modem_variation.h" #include "modem_utils.h" #define CMD_SUSPEND ((unsigned short)(0x00CA)) #define CMD_RESUME ((unsigned short)(0x00CB)) #define TX_SEPARATOR "mif: >>>>>>>>>> Outgoing packet " #define RX_SEPARATOR "mif: Incoming packet <<<<<<<<<<" #define LINE_SEPARATOR \ "mif: ------------------------------------------------------------" #define LINE_BUFF_SIZE 80 static const char *hex = "0123456789abcdef"; void ts2utc(struct timespec *ts, struct utc_time *utc) { struct tm tm; time_to_tm((ts->tv_sec - (sys_tz.tz_minuteswest * 60)), 0, &tm); utc->year = 1900 + tm.tm_year; utc->mon = 1 + tm.tm_mon; utc->day = tm.tm_mday; utc->hour = tm.tm_hour; utc->min = tm.tm_min; utc->sec = tm.tm_sec; utc->msec = (ts->tv_nsec > 0) ? (ts->tv_nsec / 1000000) : 0; } void get_utc_time(struct utc_time *utc) { struct timespec ts; getnstimeofday(&ts); ts2utc(&ts, utc); } #ifdef CONFIG_LINK_DEVICE_DPRAM #include "modem_link_device_dpram.h" int mif_dump_dpram(struct io_device *iod) { struct link_device *ld = get_current_link(iod); struct dpram_link_device *dpld = to_dpram_link_device(ld); u32 size = dpld->size; unsigned long read_len = 0; struct sk_buff *skb; char *buff; buff = kzalloc(size, GFP_ATOMIC); if (!buff) { mif_err("ERR! kzalloc fail\n"); return -ENOMEM; } else { dpld->dpram_dump(ld, buff); } while (read_len < size) { skb = alloc_skb(MAX_IPC_SKB_SIZE, GFP_ATOMIC); if (!skb) { mif_err("ERR! alloc_skb fail\n"); kfree(buff); return -ENOMEM; } memcpy(skb_put(skb, MAX_IPC_SKB_SIZE), buff + read_len, MAX_IPC_SKB_SIZE); skb_queue_tail(&iod->sk_rx_q, skb); read_len += MAX_IPC_SKB_SIZE; wake_up(&iod->wq); } kfree(buff); return 0; } #endif int mif_dump_log(struct modem_shared *msd, struct io_device *iod) { struct sk_buff *skb; unsigned long read_len = 0; unsigned long int flags; spin_lock_irqsave(&msd->lock, flags); while (read_len < MAX_MIF_BUFF_SIZE) { skb = alloc_skb(MAX_IPC_SKB_SIZE, GFP_ATOMIC); if (!skb) { mif_err("ERR! alloc_skb fail\n"); spin_unlock_irqrestore(&msd->lock, flags); return -ENOMEM; } memcpy(skb_put(skb, MAX_IPC_SKB_SIZE), msd->storage.addr + read_len, MAX_IPC_SKB_SIZE); skb_queue_tail(&iod->sk_rx_q, skb); read_len += MAX_IPC_SKB_SIZE; wake_up(&iod->wq); } spin_unlock_irqrestore(&msd->lock, flags); return 0; } static unsigned long long get_kernel_time(void) { int this_cpu; unsigned long flags; unsigned long long time; preempt_disable(); raw_local_irq_save(flags); this_cpu = smp_processor_id(); time = cpu_clock(this_cpu); preempt_enable(); raw_local_irq_restore(flags); return time; } void mif_ipc_log(enum mif_log_id id, struct modem_shared *msd, const char *data, size_t len) { struct mif_ipc_block *block; unsigned long int flags; spin_lock_irqsave(&msd->lock, flags); block = (struct mif_ipc_block *) (msd->storage.addr + (MAX_LOG_SIZE * msd->storage.cnt)); msd->storage.cnt = ((msd->storage.cnt + 1) < MAX_LOG_CNT) ? msd->storage.cnt + 1 : 0; spin_unlock_irqrestore(&msd->lock, flags); block->id = id; block->time = get_kernel_time(); block->len = (len > MAX_IPC_LOG_SIZE) ? MAX_IPC_LOG_SIZE : len; memcpy(block->buff, data, block->len); } void _mif_irq_log(enum mif_log_id id, struct modem_shared *msd, struct mif_irq_map map, const char *data, size_t len) { struct mif_irq_block *block; unsigned long int flags; spin_lock_irqsave(&msd->lock, flags); block = (struct mif_irq_block *) (msd->storage.addr + (MAX_LOG_SIZE * msd->storage.cnt)); msd->storage.cnt = ((msd->storage.cnt + 1) < MAX_LOG_CNT) ? msd->storage.cnt + 1 : 0; spin_unlock_irqrestore(&msd->lock, flags); block->id = id; block->time = get_kernel_time(); memcpy(&(block->map), &map, sizeof(struct mif_irq_map)); if (data) memcpy(block->buff, data, (len > MAX_IRQ_LOG_SIZE) ? MAX_IRQ_LOG_SIZE : len); } void _mif_com_log(enum mif_log_id id, struct modem_shared *msd, const char *format, ...) { struct mif_common_block *block; unsigned long int flags; va_list args; int ret; spin_lock_irqsave(&msd->lock, flags); block = (struct mif_common_block *) (msd->storage.addr + (MAX_LOG_SIZE * msd->storage.cnt)); msd->storage.cnt = ((msd->storage.cnt + 1) < MAX_LOG_CNT) ? msd->storage.cnt + 1 : 0; spin_unlock_irqrestore(&msd->lock, flags); block->id = id; block->time = get_kernel_time(); va_start(args, format); ret = vsnprintf(block->buff, MAX_COM_LOG_SIZE, format, args); va_end(args); } void _mif_time_log(enum mif_log_id id, struct modem_shared *msd, struct timespec epoch, const char *data, size_t len) { struct mif_time_block *block; unsigned long int flags; spin_lock_irqsave(&msd->lock, flags); block = (struct mif_time_block *) (msd->storage.addr + (MAX_LOG_SIZE * msd->storage.cnt)); msd->storage.cnt = ((msd->storage.cnt + 1) < MAX_LOG_CNT) ? msd->storage.cnt + 1 : 0; spin_unlock_irqrestore(&msd->lock, flags); block->id = id; block->time = get_kernel_time(); memcpy(&block->epoch, &epoch, sizeof(struct timespec)); if (data) memcpy(block->buff, data, (len > MAX_IRQ_LOG_SIZE) ? MAX_IRQ_LOG_SIZE : len); } /* dump2hex * dump data to hex as fast as possible. * the length of @buff must be greater than "@len * 3" * it need 3 bytes per one data byte to print. */ static inline int dump2hex(char *buff, const char *data, size_t len) { char *dest = buff; int i; for (i = 0; i < len; i++) { *dest++ = hex[(data[i] >> 4) & 0xf]; *dest++ = hex[data[i] & 0xf]; *dest++ = ' '; } if (likely(len > 0)) dest--; /* last space will be overwrited with null */ *dest = '\0'; return dest - buff; } void pr_ipc(const char *tag, const char *data, size_t len) { struct utc_time utc; unsigned char str[128]; get_utc_time(&utc); dump2hex(str, data, (len > 32 ? 32 : len)); pr_info("%s: %s: [%02d:%02d:%02d.%03d] %s\n", MIF_TAG, tag, utc.hour, utc.min, utc.sec, utc.msec, str); } /* print buffer as hex string */ int pr_buffer(const char *tag, const char *data, size_t data_len, size_t max_len) { size_t len = min(data_len, max_len); unsigned char str[len ? len * 3 : 1]; /* 1 <= sizeof <= max_len*3 */ dump2hex(str, data, len); /* don't change this printk to mif_debug for print this as level7 */ return printk(KERN_INFO "%s: %s(%u): %s%s\n", MIF_TAG, tag, data_len, str, (len == data_len) ? "" : " ..."); } /* flow control CM from CP, it use in serial devices */ int link_rx_flowctl_cmd(struct link_device *ld, const char *data, size_t len) { struct modem_shared *msd = ld->msd; unsigned short *cmd, *end = (unsigned short *)(data + len); mif_debug("flow control cmd: size=%d\n", len); for (cmd = (unsigned short *)data; cmd < end; cmd++) { switch (*cmd) { case CMD_SUSPEND: iodevs_for_each(msd, iodev_netif_stop, 0); ld->raw_tx_suspended = true; mif_info("flowctl CMD_SUSPEND(%04X)\n", *cmd); break; case CMD_RESUME: iodevs_for_each(msd, iodev_netif_wake, 0); ld->raw_tx_suspended = false; complete_all(&ld->raw_tx_resumed_by_cp); mif_info("flowctl CMD_RESUME(%04X)\n", *cmd); break; default: mif_err("flowctl BAD CMD: %04X\n", *cmd); break; } } return 0; } struct io_device *get_iod_with_channel(struct modem_shared *msd, unsigned channel) { struct rb_node *n = msd->iodevs_tree_chan.rb_node; struct io_device *iodev; while (n) { iodev = rb_entry(n, struct io_device, node_chan); if (channel < iodev->id) n = n->rb_left; else if (channel > iodev->id) n = n->rb_right; else return iodev; } return NULL; } struct io_device *get_iod_with_format(struct modem_shared *msd, enum dev_format format) { struct rb_node *n = msd->iodevs_tree_fmt.rb_node; struct io_device *iodev; while (n) { iodev = rb_entry(n, struct io_device, node_fmt); if (format < iodev->format) n = n->rb_left; else if (format > iodev->format) n = n->rb_right; else return iodev; } return NULL; } struct io_device *insert_iod_with_channel(struct modem_shared *msd, unsigned channel, struct io_device *iod) { struct rb_node **p = &msd->iodevs_tree_chan.rb_node; struct rb_node *parent = NULL; struct io_device *iodev; while (*p) { parent = *p; iodev = rb_entry(parent, struct io_device, node_chan); if (channel < iodev->id) p = &(*p)->rb_left; else if (channel > iodev->id) p = &(*p)->rb_right; else return iodev; } rb_link_node(&iod->node_chan, parent, p); rb_insert_color(&iod->node_chan, &msd->iodevs_tree_chan); return NULL; } struct io_device *insert_iod_with_format(struct modem_shared *msd, enum dev_format format, struct io_device *iod) { struct rb_node **p = &msd->iodevs_tree_fmt.rb_node; struct rb_node *parent = NULL; struct io_device *iodev; while (*p) { parent = *p; iodev = rb_entry(parent, struct io_device, node_fmt); if (format < iodev->format) p = &(*p)->rb_left; else if (format > iodev->format) p = &(*p)->rb_right; else return iodev; } rb_link_node(&iod->node_fmt, parent, p); rb_insert_color(&iod->node_fmt, &msd->iodevs_tree_fmt); return NULL; } void iodevs_for_each(struct modem_shared *msd, action_fn action, void *args) { struct io_device *iod; struct rb_node *node = rb_first(&msd->iodevs_tree_chan); for (; node; node = rb_next(node)) { iod = rb_entry(node, struct io_device, node_chan); action(iod, args); } } void iodev_netif_wake(struct io_device *iod, void *args) { if (iod->io_typ == IODEV_NET && iod->ndev) { netif_wake_queue(iod->ndev); mif_info("%s\n", iod->name); } } void iodev_netif_stop(struct io_device *iod, void *args) { if (iod->io_typ == IODEV_NET && iod->ndev) { netif_stop_queue(iod->ndev); mif_info("%s\n", iod->name); } } static void iodev_set_tx_link(struct io_device *iod, void *args) { struct link_device *ld = (struct link_device *)args; if (iod->format == IPC_RAW && IS_CONNECTED(iod, ld)) { set_current_link(iod, ld); mif_err("%s -> %s\n", iod->name, ld->name); } } void rawdevs_set_tx_link(struct modem_shared *msd, enum modem_link link_type) { struct link_device *ld = find_linkdev(msd, link_type); if (ld) iodevs_for_each(msd, iodev_set_tx_link, ld); } void mif_netif_stop(struct link_device *ld) { struct io_device *iod; if (ld->ipc_version < SIPC_VER_50) iod = link_get_iod_with_channel(ld, 0x20 | RMNET0_CH_ID); else iod = link_get_iod_with_channel(ld, RMNET0_CH_ID); if (iod) iodevs_for_each(iod->msd, iodev_netif_stop, 0); } void mif_netif_wake(struct link_device *ld) { struct io_device *iod; /** * If ld->suspend_netif_tx is true, this means that there was a SUSPEND * flow control command from CP so MIF must wait for a RESUME command * from CP. */ if (ld->suspend_netif_tx) { mif_info("%s: waiting for FLOW_CTRL_RESUME\n", ld->name); return; } if (ld->ipc_version < SIPC_VER_50) iod = link_get_iod_with_channel(ld, 0x20 | RMNET0_CH_ID); else iod = link_get_iod_with_channel(ld, RMNET0_CH_ID); if (iod) iodevs_for_each(iod->msd, iodev_netif_wake, 0); } /** * ipv4str_to_be32 - ipv4 string to be32 (big endian 32bits integer) * @return: return zero when errors occurred */ __be32 ipv4str_to_be32(const char *ipv4str, size_t count) { unsigned char ip[4]; char ipstr[16]; /* == strlen("xxx.xxx.xxx.xxx") + 1 */ char *next = ipstr; char *p; int i; strncpy(ipstr, ipv4str, ARRAY_SIZE(ipstr)); for (i = 0; i < 4; i++) { p = strsep(&next, "."); if (kstrtou8(p, 10, &ip[i]) < 0) return 0; /* == 0.0.0.0 */ } return *((__be32 *)ip); } void mif_add_timer(struct timer_list *timer, unsigned long expire, void (*function)(unsigned long), unsigned long data) { if (timer_pending(timer)) return; init_timer(timer); timer->expires = get_jiffies_64() + expire; timer->function = function; timer->data = data; add_timer(timer); } void mif_print_data(const char *buff, int len) { int words = len >> 4; int residue = len - (words << 4); int i; char *b; char last[80]; char tb[8]; /* Make the last line, if ((len % 16) > 0) */ if (residue > 0) { memset(last, 0, sizeof(last)); memset(tb, 0, sizeof(tb)); b = (char *)buff + (words << 4); sprintf(last, "%04X: ", (words << 4)); for (i = 0; i < residue; i++) { sprintf(tb, "%02x ", b[i]); strcat(last, tb); if ((i & 0x3) == 0x3) { sprintf(tb, " "); strcat(last, tb); } } } for (i = 0; i < words; i++) { b = (char *)buff + (i << 4); mif_err("%04X: " "%02x %02x %02x %02x %02x %02x %02x %02x " "%02x %02x %02x %02x %02x %02x %02x %02x\n", (i << 4), b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7], b[8], b[9], b[10], b[11], b[12], b[13], b[14], b[15]); } /* Print the last line */ if (residue > 0) mif_err("%s\n", last); } void mif_dump2format16(const char *data, int len, char *buff, char *tag) { char *d; int i; int words = len >> 4; int residue = len - (words << 4); char line[LINE_BUFF_SIZE]; char tb[8]; for (i = 0; i < words; i++) { memset(line, 0, LINE_BUFF_SIZE); d = (char *)data + (i << 4); if (tag) sprintf(line, "%s%04X| " "%02x %02x %02x %02x " "%02x %02x %02x %02x " "%02x %02x %02x %02x " "%02x %02x %02x %02x\n", tag, (i << 4), d[0], d[1], d[2], d[3], d[4], d[5], d[6], d[7], d[8], d[9], d[10], d[11], d[12], d[13], d[14], d[15]); else sprintf(line, "%04X| " "%02x %02x %02x %02x " "%02x %02x %02x %02x " "%02x %02x %02x %02x " "%02x %02x %02x %02x\n", (i << 4), d[0], d[1], d[2], d[3], d[4], d[5], d[6], d[7], d[8], d[9], d[10], d[11], d[12], d[13], d[14], d[15]); strcat(buff, line); } /* Make the last line, if (len % 16) > 0 */ if (residue > 0) { memset(line, 0, LINE_BUFF_SIZE); memset(tb, 0, sizeof(tb)); d = (char *)data + (words << 4); if (tag) sprintf(line, "%s%04X|", tag, (words << 4)); else sprintf(line, "%04X|", (words << 4)); for (i = 0; i < residue; i++) { sprintf(tb, " %02x", d[i]); strcat(line, tb); if ((i & 0x3) == 0x3) { sprintf(tb, " "); strcat(line, tb); } } strcat(line, "\n"); strcat(buff, line); } } void mif_dump2format4(const char *data, int len, char *buff, char *tag) { char *d; int i; int words = len >> 2; int residue = len - (words << 2); char line[LINE_BUFF_SIZE]; char tb[8]; for (i = 0; i < words; i++) { memset(line, 0, LINE_BUFF_SIZE); d = (char *)data + (i << 2); if (tag) sprintf(line, "%s%04X| %02x %02x %02x %02x\n", tag, (i << 2), d[0], d[1], d[2], d[3]); else sprintf(line, "%04X| %02x %02x %02x %02x\n", (i << 2), d[0], d[1], d[2], d[3]); strcat(buff, line); } /* Make the last line, if (len % 4) > 0 */ if (residue > 0) { memset(line, 0, LINE_BUFF_SIZE); memset(tb, 0, sizeof(tb)); d = (char *)data + (words << 2); if (tag) sprintf(line, "%s%04X|", tag, (words << 2)); else sprintf(line, "%04X|", (words << 2)); for (i = 0; i < residue; i++) { sprintf(tb, " %02x", d[i]); strcat(line, tb); } strcat(line, "\n"); strcat(buff, line); } } void mif_print_dump(const char *data, int len, int width) { char *buff; buff = kzalloc(len << 3, GFP_ATOMIC); if (!buff) { mif_err("ERR! kzalloc fail\n"); return; } if (width == 16) mif_dump2format16(data, len, buff, LOG_TAG); else mif_dump2format4(data, len, buff, LOG_TAG); pr_info("%s", buff); kfree(buff); } void print_sipc4_hdlc_fmt_frame(const u8 *psrc) { u8 *frm; /* HDLC Frame */ struct fmt_hdr *hh; /* HDLC Header */ struct sipc_fmt_hdr *fh; /* IPC Header */ u16 hh_len = sizeof(struct fmt_hdr); u16 fh_len = sizeof(struct sipc_fmt_hdr); u8 *data; int dlen; /* Actual HDLC header starts from after START flag (0x7F) */ frm = (u8 *)(psrc + 1); /* Point HDLC header and IPC header */ hh = (struct fmt_hdr *)(frm); fh = (struct sipc_fmt_hdr *)(frm + hh_len); /* Point IPC data */ data = frm + (hh_len + fh_len); dlen = hh->len - (hh_len + fh_len); mif_err("--------------------HDLC & FMT HEADER----------------------\n"); mif_err("HDLC: length %d, control 0x%02x\n", hh->len, hh->control); mif_err("(M)0x%02X, (S)0x%02X, (T)0x%02X, mseq %d, aseq %d, len %d\n", fh->main_cmd, fh->sub_cmd, fh->cmd_type, fh->msg_seq, fh->ack_seq, fh->len); mif_err("-----------------------IPC FMT DATA------------------------\n"); if (dlen > 0) { if (dlen > 64) dlen = 64; mif_print_data(data, dlen); } mif_err("-----------------------------------------------------------\n"); } void print_sipc4_fmt_frame(const u8 *psrc) { struct sipc_fmt_hdr *fh = (struct sipc_fmt_hdr *)psrc; u16 fh_len = sizeof(struct sipc_fmt_hdr); u8 *data; int dlen; /* Point IPC data */ data = (u8 *)(psrc + fh_len); dlen = fh->len - fh_len; mif_err("----------------------IPC FMT HEADER-----------------------\n"); mif_err("(M)0x%02X, (S)0x%02X, (T)0x%02X, mseq:%d, aseq:%d, len:%d\n", fh->main_cmd, fh->sub_cmd, fh->cmd_type, fh->msg_seq, fh->ack_seq, fh->len); mif_err("-----------------------IPC FMT DATA------------------------\n"); if (dlen > 0) mif_print_data(data, dlen); mif_err("-----------------------------------------------------------\n"); } void print_sipc5_link_fmt_frame(const u8 *psrc) { u8 *lf; /* Link Frame */ struct sipc5_link_hdr *lh; /* Link Header */ struct sipc_fmt_hdr *fh; /* IPC Header */ u16 lh_len; u16 fh_len; u8 *data; int dlen; lf = (u8 *)psrc; /* Point HDLC header and IPC header */ lh = (struct sipc5_link_hdr *)lf; if (lh->cfg & SIPC5_CTL_FIELD_EXIST) lh_len = SIPC5_HEADER_SIZE_WITH_CTL_FLD; else lh_len = SIPC5_MIN_HEADER_SIZE; fh = (struct sipc_fmt_hdr *)(lf + lh_len); fh_len = sizeof(struct sipc_fmt_hdr); /* Point IPC data */ data = lf + (lh_len + fh_len); dlen = lh->len - (lh_len + fh_len); mif_err("--------------------LINK & FMT HEADER----------------------\n"); mif_err("LINK: cfg 0x%02X, ch %d, len %d\n", lh->cfg, lh->ch, lh->len); mif_err("(M)0x%02X, (S)0x%02X, (T)0x%02X, mseq:%d, aseq:%d, len:%d\n", fh->main_cmd, fh->sub_cmd, fh->cmd_type, fh->msg_seq, fh->ack_seq, fh->len); mif_err("-----------------------IPC FMT DATA------------------------\n"); if (dlen > 0) { if (dlen > 64) dlen = 64; mif_print_data(data, dlen); } mif_err("-----------------------------------------------------------\n"); } static void strcat_tcp_header(char *buff, u8 *pkt) { struct tcphdr *tcph = (struct tcphdr *)pkt; int eol; char line[LINE_BUFF_SIZE]; char flag_str[32]; /*------------------------------------------------------------------------- TCP Header Format +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Source Port | Destination Port | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Sequence Number | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Acknowledgment Number | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Data | |C|E|U|A|P|R|S|F| | | Offset| Rsvd |W|C|R|C|S|S|Y|I| Window | | | |R|E|G|K|H|T|N|N| | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Checksum | Urgent Pointer | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Options | Padding | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | data | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -------------------------------------------------------------------------*/ memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: TCP:: Src.Port %u, Dst.Port %u\n", MIF_TAG, ntohs(tcph->source), ntohs(tcph->dest)); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: TCP:: SEQ 0x%08X(%u), ACK 0x%08X(%u)\n", MIF_TAG, ntohs(tcph->seq), ntohs(tcph->seq), ntohs(tcph->ack_seq), ntohs(tcph->ack_seq)); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); memset(flag_str, 0, sizeof(flag_str)); if (tcph->cwr) strcat(flag_str, "CWR "); if (tcph->ece) strcat(flag_str, "ECE"); if (tcph->urg) strcat(flag_str, "URG "); if (tcph->ack) strcat(flag_str, "ACK "); if (tcph->psh) strcat(flag_str, "PSH "); if (tcph->rst) strcat(flag_str, "RST "); if (tcph->syn) strcat(flag_str, "SYN "); if (tcph->fin) strcat(flag_str, "FIN "); eol = strlen(flag_str) - 1; if (eol > 0) flag_str[eol] = 0; snprintf(line, LINE_BUFF_SIZE, "%s: TCP:: Flags {%s}\n", MIF_TAG, flag_str); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: TCP:: Window %u, Checksum 0x%04X, Urgent %u\n", MIF_TAG, ntohs(tcph->window), ntohs(tcph->check), ntohs(tcph->urg_ptr)); strcat(buff, line); } static void strcat_udp_header(char *buff, u8 *pkt) { struct udphdr *udph = (struct udphdr *)pkt; char line[LINE_BUFF_SIZE]; /*------------------------------------------------------------------------- UDP Header Format +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Source Port | Destination Port | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Length | Checksum | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | data | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -------------------------------------------------------------------------*/ memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: UDP:: Src.Port %u, Dst.Port %u\n", MIF_TAG, ntohs(udph->source), ntohs(udph->dest)); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: UDP:: Length %u, Checksum 0x%04X\n", MIF_TAG, ntohs(udph->len), ntohs(udph->check)); strcat(buff, line); if (ntohs(udph->dest) == 53) { memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: UDP:: DNS query!!!\n", MIF_TAG); strcat(buff, line); } if (ntohs(udph->source) == 53) { memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: UDP:: DNS response!!!\n", MIF_TAG); strcat(buff, line); } } void print_ip4_packet(const u8 *ip_pkt, bool tx) { char *buff; struct iphdr *iph = (struct iphdr *)ip_pkt; u8 *pkt = (u8 *)ip_pkt + (iph->ihl << 2); u16 flags = (ntohs(iph->frag_off) & 0xE000); u16 frag_off = (ntohs(iph->frag_off) & 0x1FFF); int eol; char line[LINE_BUFF_SIZE]; char flag_str[16]; /*--------------------------------------------------------------------------- IPv4 Header Format +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ |Version| IHL |Type of Service| Total Length | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Identification |C|D|M| Fragment Offset | | |E|F|F| | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Time to Live | Protocol | Header Checksum | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Source Address | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Destination Address | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Options | Padding | +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ IHL - Header Length Flags - Consist of 3 bits The 1st bit is "Congestion" bit. The 2nd bit is "Dont Fragment" bit. The 3rd bit is "More Fragments" bit. ---------------------------------------------------------------------------*/ if (iph->version != 4) return; buff = kzalloc(4096, GFP_ATOMIC); if (!buff) return; memset(line, 0, LINE_BUFF_SIZE); if (tx) snprintf(line, LINE_BUFF_SIZE, "\n%s\n", TX_SEPARATOR); else snprintf(line, LINE_BUFF_SIZE, "\n%s\n", RX_SEPARATOR); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s\n", LINE_SEPARATOR); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: IP4:: Version %u, Header Length %u, TOS %u, Length %u\n", MIF_TAG, iph->version, (iph->ihl << 2), iph->tos, ntohs(iph->tot_len)); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: IP4:: ID %u, Fragment Offset %u\n", MIF_TAG, ntohs(iph->id), frag_off); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); memset(flag_str, 0, sizeof(flag_str)); if (flags & IP_CE) strcat(flag_str, "CE "); if (flags & IP_DF) strcat(flag_str, "DF "); if (flags & IP_MF) strcat(flag_str, "MF "); eol = strlen(flag_str) - 1; if (eol > 0) flag_str[eol] = 0; snprintf(line, LINE_BUFF_SIZE, "%s: IP4:: Flags {%s}\n", MIF_TAG, flag_str); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: IP4:: TTL %u, Protocol %u, Header Checksum 0x%04X\n", MIF_TAG, iph->ttl, iph->protocol, ntohs(iph->check)); strcat(buff, line); memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s: IP4:: Src.IP %u.%u.%u.%u, Dst.IP %u.%u.%u.%u\n", MIF_TAG, ip_pkt[12], ip_pkt[13], ip_pkt[14], ip_pkt[15], ip_pkt[16], ip_pkt[17], ip_pkt[18], ip_pkt[19]); strcat(buff, line); switch (iph->protocol) { case 6: /* TCP */ strcat_tcp_header(buff, pkt); break; case 17: /* UDP */ strcat_udp_header(buff, pkt); break; default: break; } memset(line, 0, LINE_BUFF_SIZE); snprintf(line, LINE_BUFF_SIZE, "%s\n", LINE_SEPARATOR); strcat(buff, line); pr_info("%s", buff); kfree(buff); } bool is_dns_packet(const u8 *ip_pkt) { struct iphdr *iph = (struct iphdr *)ip_pkt; struct udphdr *udph = (struct udphdr *)(ip_pkt + (iph->ihl << 2)); /* If this packet is not a UDP packet, return here. */ if (iph->protocol != 17) return false; if (ntohs(udph->dest) == 53 || ntohs(udph->source) == 53) return true; else return false; } bool is_syn_packet(const u8 *ip_pkt) { struct iphdr *iph = (struct iphdr *)ip_pkt; struct tcphdr *tcph = (struct tcphdr *)(ip_pkt + (iph->ihl << 2)); /* If this packet is not a TCP packet, return here. */ if (iph->protocol != 6) return false; if (tcph->syn || tcph->fin) return true; else return false; } int memcmp16_to_io(const void __iomem *to, void *from, int size) { u16 *d = (u16 *)to; u16 *s = (u16 *)from; int count = size >> 1; int diff = 0; int i; u16 d1; u16 s1; for (i = 0; i < count; i++) { d1 = ioread16(d); s1 = *s; if (d1 != s1) { diff++; mif_err("ERR! [%d] d:0x%04X != s:0x%04X\n", i, d1, s1); } d++; s++; } return diff; } int mif_test_dpram(char *dp_name, u8 __iomem *start, u32 size) { u8 __iomem *dst; int i; u16 val; mif_info("%s: start = 0x%p, size = %d\n", dp_name, start, size); dst = start; for (i = 0; i < (size >> 1); i++) { iowrite16((i & 0xFFFF), dst); dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { val = ioread16(dst); if (val != (i & 0xFFFF)) { mif_info("%s: ERR! dst[%d] 0x%04X != 0x%04X\n", dp_name, i, val, (i & 0xFFFF)); return -EINVAL; } dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { iowrite16(0x00FF, dst); dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { val = ioread16(dst); if (val != 0x00FF) { mif_info("%s: ERR! dst[%d] 0x%04X != 0x00FF\n", dp_name, i, val); return -EINVAL; } dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { iowrite16(0x0FF0, dst); dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { val = ioread16(dst); if (val != 0x0FF0) { mif_info("%s: ERR! dst[%d] 0x%04X != 0x0FF0\n", dp_name, i, val); return -EINVAL; } dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { iowrite16(0xFF00, dst); dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { val = ioread16(dst); if (val != 0xFF00) { mif_info("%s: ERR! dst[%d] 0x%04X != 0xFF00\n", dp_name, i, val); return -EINVAL; } dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { iowrite16(0, dst); dst += 2; } dst = start; for (i = 0; i < (size >> 1); i++) { val = ioread16(dst); if (val != 0) { mif_info("%s: ERR! dst[%d] 0x%04X != 0\n", dp_name, i, val); return -EINVAL; } dst += 2; } mif_info("%s: PASS!!!\n", dp_name); return 0; } struct file *mif_open_file(const char *path) { struct file *fp; mm_segment_t old_fs; old_fs = get_fs(); set_fs(get_ds()); fp = filp_open(path, O_RDWR|O_CREAT|O_APPEND, 0666); set_fs(old_fs); if (IS_ERR(fp)) return NULL; return fp; } void mif_save_file(struct file *fp, const char *buff, size_t size) { int ret; mm_segment_t old_fs; old_fs = get_fs(); set_fs(get_ds()); ret = fp->f_op->write(fp, buff, size, &fp->f_pos); if (ret < 0) mif_err("ERR! write fail\n"); set_fs(old_fs); } void mif_close_file(struct file *fp) { mm_segment_t old_fs; old_fs = get_fs(); set_fs(get_ds()); filp_close(fp, NULL); set_fs(old_fs); }