diff options
Diffstat (limited to 'drivers/misc/modem_if/modem_utils.c')
| -rw-r--r-- | drivers/misc/modem_if/modem_utils.c | 600 |
1 files changed, 519 insertions, 81 deletions
diff --git a/drivers/misc/modem_if/modem_utils.c b/drivers/misc/modem_if/modem_utils.c index 24a9d19..d62aaa6 100644 --- a/drivers/misc/modem_if/modem_utils.c +++ b/drivers/misc/modem_if/modem_utils.c @@ -19,6 +19,8 @@ #include <linux/ip.h> #include <linux/tcp.h> #include <linux/udp.h> +#include <linux/rtc.h> +#include <linux/time.h> #include <net/ip.h> #include "modem_prj.h" @@ -27,6 +29,183 @@ #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 + +#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->dp_size; + unsigned long read_len = 0; + struct sk_buff *skb; + char *buff; + + buff = kzalloc(size, GFP_ATOMIC); + if (!buff) { + pr_err("[MIF] <%s> alloc dpram buff failed\n", __func__); + return -ENOMEM; + } else { + dpld->dpram_dump(ld, buff); + } + + while (read_len < size) { + skb = alloc_skb(MAX_IPC_SKB_SIZE, GFP_ATOMIC); + if (!skb) { + pr_err("[MIF] <%s> alloc skb failed\n", __func__); + 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) { + pr_err("[MIF] <%s> alloc skb failed\n", __func__); + 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. @@ -52,6 +231,23 @@ static inline int dump2hex(char *buf, const char *data, size_t len) return dest - buf; } +int pr_ipc(const char *str, const char *data, size_t len) +{ + struct timeval tv; + struct tm date; + unsigned char hexstr[128]; + + do_gettimeofday(&tv); + time_to_tm((tv.tv_sec - sys_tz.tz_minuteswest * 60), 0, &date); + + dump2hex(hexstr, data, (len > 40 ? 40 : len)); + + return pr_info("mif: %s: [%02d-%02d %02d:%02d:%02d.%03ld] %s\n", + str, date.tm_mon + 1, date.tm_mday, + date.tm_hour, date.tm_min, date.tm_sec, + (tv.tv_usec > 0 ? tv.tv_usec / 1000 : 0), hexstr); +} + /* print buffer as hex string */ int pr_buffer(const char *tag, const char *data, size_t data_len, size_t max_len) @@ -61,14 +257,14 @@ int pr_buffer(const char *tag, const char *data, size_t data_len, dump2hex(hexstr, data, len); /* don't change this printk to mif_debug for print this as level7 */ - return printk(KERN_DEBUG "%s(%u): %s%s\n", tag, data_len, hexstr, + return printk(KERN_INFO "mif: %s(%u): %s%s\n", tag, data_len, hexstr, len == data_len ? "" : " ..."); } -/* flow control CMfrom CP, it use in serial devices */ +/* 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 mif_common *commons = &ld->mc->commons; + struct modem_shared *msd = ld->msd; unsigned short *cmd, *end = (unsigned short *)(data + len); mif_debug("flow control cmd: size=%d\n", len); @@ -76,13 +272,13 @@ int link_rx_flowctl_cmd(struct link_device *ld, const char *data, size_t len) for (cmd = (unsigned short *)data; cmd < end; cmd++) { switch (*cmd) { case CMD_SUSPEND: - iodevs_for_each(commons, iodev_netif_stop, 0); + 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(commons, iodev_netif_wake, 0); + 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); @@ -97,10 +293,10 @@ int link_rx_flowctl_cmd(struct link_device *ld, const char *data, size_t len) return 0; } -struct io_device *get_iod_with_channel(struct mif_common *commons, +struct io_device *get_iod_with_channel(struct modem_shared *msd, unsigned channel) { - struct rb_node *n = commons->iodevs_tree_chan.rb_node; + 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); @@ -114,10 +310,10 @@ struct io_device *get_iod_with_channel(struct mif_common *commons, return NULL; } -struct io_device *get_iod_with_format(struct mif_common *commons, +struct io_device *get_iod_with_format(struct modem_shared *msd, enum dev_format format) { - struct rb_node *n = commons->iodevs_tree_fmt.rb_node; + 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); @@ -131,10 +327,10 @@ struct io_device *get_iod_with_format(struct mif_common *commons, return NULL; } -struct io_device *insert_iod_with_channel(struct mif_common *commons, +struct io_device *insert_iod_with_channel(struct modem_shared *msd, unsigned channel, struct io_device *iod) { - struct rb_node **p = &commons->iodevs_tree_chan.rb_node; + struct rb_node **p = &msd->iodevs_tree_chan.rb_node; struct rb_node *parent = NULL; struct io_device *iodev; while (*p) { @@ -148,14 +344,14 @@ struct io_device *insert_iod_with_channel(struct mif_common *commons, return iodev; } rb_link_node(&iod->node_chan, parent, p); - rb_insert_color(&iod->node_chan, &commons->iodevs_tree_chan); + rb_insert_color(&iod->node_chan, &msd->iodevs_tree_chan); return NULL; } -struct io_device *insert_iod_with_format(struct mif_common *commons, +struct io_device *insert_iod_with_format(struct modem_shared *msd, enum dev_format format, struct io_device *iod) { - struct rb_node **p = &commons->iodevs_tree_fmt.rb_node; + struct rb_node **p = &msd->iodevs_tree_fmt.rb_node; struct rb_node *parent = NULL; struct io_device *iodev; while (*p) { @@ -169,14 +365,14 @@ struct io_device *insert_iod_with_format(struct mif_common *commons, return iodev; } rb_link_node(&iod->node_fmt, parent, p); - rb_insert_color(&iod->node_fmt, &commons->iodevs_tree_fmt); + rb_insert_color(&iod->node_fmt, &msd->iodevs_tree_fmt); return NULL; } -void iodevs_for_each(struct mif_common *commons, action_fn action, void *args) +void iodevs_for_each(struct modem_shared *msd, action_fn action, void *args) { struct io_device *iod; - struct rb_node *node = rb_first(&commons->iodevs_tree_chan); + 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); @@ -202,22 +398,48 @@ void iodev_netif_stop(struct io_device *iod, void *args) static void iodev_set_tx_link(struct io_device *iod, void *args) { struct link_device *ld = (struct link_device *)args; - if (iod->io_typ == IODEV_NET && IS_CONNECTED(iod, ld)) { + 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 mif_common *commons, enum modem_link link_type) +void rawdevs_set_tx_link(struct modem_shared *msd, enum modem_link link_type) { - struct link_device *ld = find_linkdev(commons, link_type); + struct link_device *ld = find_linkdev(msd, link_type); if (ld) - iodevs_for_each(commons, iodev_set_tx_link, ld); + iodevs_for_each(msd, iodev_set_tx_link, ld); +} + +/** + * 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; @@ -375,13 +597,12 @@ void print_sipc5_link_fmt_frame(const u8 *psrc) mif_err("-----------------------------------------------------------\n"); } -static void print_tcp_header(u8 *pkt) +static void strcat_tcp_header(char *buff, u8 *pkt) { - int i; - char tcp_flags[32]; struct tcphdr *tcph = (struct tcphdr *)pkt; - u8 *opt = pkt + TCP_HDR_SIZE; - unsigned opt_len = (tcph->doff << 2) - TCP_HDR_SIZE; + int eol; + char line[LINE_BUFF_SIZE]; + char flag_str[32]; /*------------------------------------------------------------------------- @@ -409,44 +630,54 @@ static void print_tcp_header(u8 *pkt) -------------------------------------------------------------------------*/ - memset(tcp_flags, 0, sizeof(tcp_flags)); + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, + "mif: TCP:: Src.Port %u, Dst.Port %u\n", + ntohs(tcph->source), ntohs(tcph->dest)); + strcat(buff, line); + + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, + "mif: TCP:: SEQ 0x%08X(%u), ACK 0x%08X(%u)\n", + 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(tcp_flags, "CWR "); + strcat(flag_str, "CWR "); if (tcph->ece) - strcat(tcp_flags, "EC"); + strcat(flag_str, "ECE"); if (tcph->urg) - strcat(tcp_flags, "URG "); + strcat(flag_str, "URG "); if (tcph->ack) - strcat(tcp_flags, "ACK "); + strcat(flag_str, "ACK "); if (tcph->psh) - strcat(tcp_flags, "PSH "); + strcat(flag_str, "PSH "); if (tcph->rst) - strcat(tcp_flags, "RST "); + strcat(flag_str, "RST "); if (tcph->syn) - strcat(tcp_flags, "SYN "); + strcat(flag_str, "SYN "); if (tcph->fin) - strcat(tcp_flags, "FIN "); - - mif_err("TCP:: Src.Port %u, Dst.Port %u\n", - ntohs(tcph->source), ntohs(tcph->dest)); - mif_err("TCP:: SEQ 0x%08X(%u), ACK 0x%08X(%u)\n", - ntohs(tcph->seq), ntohs(tcph->seq), - ntohs(tcph->ack_seq), ntohs(tcph->ack_seq)); - mif_err("TCP:: Flags {%s}\n", tcp_flags); - mif_err("TCP:: Window %u, Checksum 0x%04X, Urg Pointer %u\n", + strcat(flag_str, "FIN "); + eol = strlen(flag_str) - 1; + if (eol > 0) + flag_str[eol] = 0; + snprintf(line, LINE_BUFF_SIZE, "mif: TCP:: Flags {%s}\n", flag_str); + strcat(buff, line); + + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, + "mif: TCP:: Window %u, Checksum 0x%04X, Urg Pointer %u\n", ntohs(tcph->window), ntohs(tcph->check), ntohs(tcph->urg_ptr)); - - if (opt_len > 0) { - mif_err("TCP:: Options {"); - for (i = 0; i < opt_len; i++) - mif_err("%02X ", opt[i]); - mif_err("}\n"); - } + strcat(buff, line); } -static void print_udp_header(u8 *pkt) +static void strcat_udp_header(char *buff, u8 *pkt) { struct udphdr *udph = (struct udphdr *)pkt; + char line[LINE_BUFF_SIZE]; /*------------------------------------------------------------------------- @@ -462,30 +693,43 @@ static void print_udp_header(u8 *pkt) -------------------------------------------------------------------------*/ - mif_err("UDP:: Src.Port %u, Dst.Port %u\n", + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, + "mif: UDP:: Src.Port %u, Dst.Port %u\n", ntohs(udph->source), ntohs(udph->dest)); - mif_err("UDP:: Length %u, Checksum 0x%04X\n", + strcat(buff, line); + + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, + "mif: UDP:: Length %u, Checksum 0x%04X\n", ntohs(udph->len), ntohs(udph->check)); + strcat(buff, line); - if (ntohs(udph->dest) == 53) - mif_err("UDP:: DNS query!!!\n"); + if (ntohs(udph->dest) == 53) { + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, "mif: UDP:: DNS query!!!\n"); + strcat(buff, line); + } - if (ntohs(udph->source) == 53) - mif_err("UDP:: DNS response!!!\n"); + if (ntohs(udph->source) == 53) { + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, "mif: UDP:: DNS response!!!\n"); + strcat(buff, line); + } } -void print_ip4_packet(u8 *ip_pkt) +void print_ip4_packet(u8 *ip_pkt, bool tx) { - char ip_flags[16]; + char *buff; struct iphdr *iph = (struct iphdr *)ip_pkt; u8 *pkt = ip_pkt + (iph->ihl << 2); u16 flags = (ntohs(iph->frag_off) & 0xE000); u16 frag_off = (ntohs(iph->frag_off) & 0x1FFF); - - mif_err("-----------------------------------------------------------\n"); + int eol; + char line[LINE_BUFF_SIZE]; + char flag_str[16]; /*--------------------------------------------------------------------------- - IPv4 Header Format +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ @@ -513,40 +757,234 @@ void print_ip4_packet(u8 *ip_pkt) ---------------------------------------------------------------------------*/ - memset(ip_flags, 0, sizeof(ip_flags)); - if (flags & IP_CE) - strcat(ip_flags, "C"); - if (flags & IP_DF) - strcat(ip_flags, "D"); - if (flags & IP_MF) - strcat(ip_flags, "M"); + if (iph->version != 4) + return; - mif_err("IP4:: Version %u, Header Length %u, TOS %u, Length %u\n", + 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, + "mif: IP4:: Version %u, Header Length %u, TOS %u, Length %u\n", iph->version, (iph->ihl << 2), iph->tos, ntohs(iph->tot_len)); - mif_err("IP4:: I%u, Fragment Offset %u\n", + strcat(buff, line); + + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, + "mif: IP4:: ID %u, Fragment Offset %u\n", ntohs(iph->id), frag_off); - mif_err("IP4:: Flags {%s}\n", ip_flags); - mif_err("IP4:: TTL %u, Protocol %u, Header Checksum 0x%04X\n", + 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, "mif: IP4:: Flags {%s}\n", flag_str); + strcat(buff, line); + + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, + "mif: IP4:: TTL %u, Protocol %u, Header Checksum 0x%04X\n", iph->ttl, iph->protocol, ntohs(iph->check)); - mif_err("IP4:: Src.IP %u.%u.%u.%u, Dst.IP %u.%u.%u.%u\n", + strcat(buff, line); + + memset(line, 0, LINE_BUFF_SIZE); + snprintf(line, LINE_BUFF_SIZE, + "mif: IP4:: Src.IP %u.%u.%u.%u, Dst.IP %u.%u.%u.%u\n", 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 */ - print_tcp_header(pkt); + case 6: /* TCP */ + strcat_tcp_header(buff, pkt); break; - case 17: - /* UDP */ - print_udp_header(pkt); + case 17: /* UDP */ + strcat_udp_header(buff, pkt); break; default: break; } - mif_err("-----------------------------------------------------------\n"); + 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(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(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; } |