| /* |
| * drivers/soc/asr/toe/toe_sim.c: toe simulator source file |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License version 2 and |
| * only version 2 as published by the Free Software Foundation. |
| * |
| * Copyright (C) 2019, ASR Microelectronics(Shanghai) LTD. Co. |
| * All rights reserved. |
| * |
| */ |
| |
| #include <net/tcp.h> |
| #include <linux/netdevice.h> |
| #include <linux/skbuff.h> |
| #include <linux/ipv6.h> |
| |
| static inline unsigned short from32to16(unsigned int x) |
| { |
| /* add up 16-bit and 16-bit for 16+c bit */ |
| x = (x & 0xffff) + (x >> 16); |
| /* add up carry.. */ |
| x = (x & 0xffff) + (x >> 16); |
| return x; |
| } |
| |
| static inline unsigned int toe_do_csum(const unsigned char *buff, int len) |
| { |
| int odd; |
| unsigned int result = 0; |
| |
| if (len <= 0) |
| goto out; |
| odd = 1 & (unsigned long) buff; |
| if (odd) { |
| #ifdef __LITTLE_ENDIAN |
| result += (*buff << 8); |
| #else |
| result = *buff; |
| #endif |
| len--; |
| buff++; |
| } |
| if (len >= 2) { |
| if (2 & (unsigned long) buff) { |
| result += *(unsigned short *) buff; |
| len -= 2; |
| buff += 2; |
| } |
| if (len >= 4) { |
| const unsigned char *end = buff + ((unsigned)len & ~3); |
| unsigned int carry = 0; |
| do { |
| unsigned int w = *(unsigned int *) buff; |
| buff += 4; |
| result += carry; |
| result += w; |
| carry = (w > result); |
| } while (buff < end); |
| result += carry; |
| result = (result & 0xffff) + (result >> 16); |
| } |
| if (len & 2) { |
| result += *(unsigned short *) buff; |
| buff += 2; |
| } |
| } |
| if (len & 1) |
| #ifdef __LITTLE_ENDIAN |
| result += *buff; |
| #else |
| result += (*buff << 8); |
| #endif |
| result = from32to16(result); |
| if (odd) |
| result = ((result >> 8) & 0xff) | ((result & 0xff) << 8); |
| out: |
| return result; |
| } |
| |
| /* |
| * return: |
| * true: ip packet checksum is right |
| * false: ip packet checksum is wrong |
| */ |
| bool toe_ip_csum_v4(struct iphdr *ip_hdr) |
| { |
| if (!ip_hdr) |
| return false; |
| |
| return !ip_fast_csum((const void *)ip_hdr, ip_hdr->ihl); |
| } |
| EXPORT_SYMBOL(toe_ip_csum_v4); |
| |
| __sum16 toe_tcpudp_csum_v4(struct iphdr *ip_hdr, |
| const void *pkt_buf) |
| { |
| __wsum wdsum; |
| int len; |
| |
| if (!ip_hdr || !pkt_buf) |
| return -1; |
| |
| len = ntohs(ip_hdr->tot_len) - ip_hdr->ihl * 4; |
| wdsum = csum_tcpudp_nofold(ip_hdr->saddr, |
| ip_hdr->daddr, |
| len, |
| ip_hdr->protocol, |
| 0); |
| wdsum = csum_add(wdsum, toe_do_csum(pkt_buf, len)); |
| |
| return csum_fold(wdsum); |
| } |
| |
| __sum16 toe_tcpudp_csum_v6(struct ipv6hdr *ipv6_hdr, const void *pkt_buf) |
| { |
| __wsum wdsum; |
| int len; |
| |
| if (!ipv6_hdr || !pkt_buf) |
| return -1; |
| |
| len = ntohs(ipv6_hdr->payload_len); |
| wdsum = csum_tcpudp_nofold(0, 0, len, ipv6_hdr->nexthdr, 0); |
| wdsum = csum_add(wdsum, toe_do_csum((void *)&ipv6_hdr->saddr, 16)); |
| wdsum = csum_add(wdsum, toe_do_csum((void *)&ipv6_hdr->daddr, 16)); |
| wdsum = csum_add(wdsum, toe_do_csum(pkt_buf, len)); |
| |
| return csum_fold(wdsum); |
| } |
| |
| bool toe_tcp_csum_v4(struct iphdr *ip_hdr, struct tcphdr *tcp_hdr) |
| { |
| if (!ip_hdr || !tcp_hdr || ip_hdr->protocol != IPPROTO_TCP) |
| return false; |
| |
| return !toe_tcpudp_csum_v4(ip_hdr, (void *)tcp_hdr); |
| } |
| EXPORT_SYMBOL(toe_tcp_csum_v4); |
| |
| bool toe_tcp_csum_v6(struct ipv6hdr *ipv6_hdr, struct tcphdr *tcp_hdr) |
| { |
| if (!ipv6_hdr || !tcp_hdr || ipv6_hdr->nexthdr != IPPROTO_TCP) |
| return false; |
| |
| return !toe_tcpudp_csum_v6(ipv6_hdr, (void *)tcp_hdr); |
| } |
| EXPORT_SYMBOL(toe_tcp_csum_v6); |
| |
| bool toe_udp_csum_v4(struct iphdr *ip_hdr, struct udphdr *udp_hdr) |
| { |
| if (!ip_hdr || !udp_hdr || ip_hdr->protocol != IPPROTO_UDP) |
| return false; |
| |
| return !toe_tcpudp_csum_v4(ip_hdr, (void *)udp_hdr); |
| } |
| EXPORT_SYMBOL(toe_udp_csum_v4); |
| |
| bool toe_udp_csum_v6(struct ipv6hdr *ipv6_hdr, struct udphdr *udp_hdr) |
| { |
| if (!ipv6_hdr || !udp_hdr || ipv6_hdr->nexthdr != IPPROTO_UDP) |
| return false; |
| |
| return !toe_tcpudp_csum_v6(ipv6_hdr, (void *)udp_hdr); |
| } |
| EXPORT_SYMBOL(toe_udp_csum_v6); |
| |
| int toe_crc(void *pkt_buf) |
| { |
| int ret = -1; |
| struct iphdr *ip_hdr = (struct iphdr *)pkt_buf; |
| struct ipv6hdr *ipv6_hdr = (struct ipv6hdr *)pkt_buf; |
| |
| /* toe checksum */ |
| if (ip_hdr->version == 4) { |
| if (!toe_ip_csum_v4(ip_hdr)) { |
| pr_info("%s: toe ipv4 checksum fail\n", __func__); |
| return ret; |
| } |
| |
| if ((ip_hdr->protocol == IPPROTO_TCP) || |
| (ip_hdr->protocol == IPPROTO_UDP)) { |
| /* |
| * UDP: if the sender did not do chechsum, we |
| * do not need to do. |
| */ |
| if (ip_hdr->protocol == IPPROTO_UDP) { |
| struct udphdr *udp_hdr = (struct udphdr *)(pkt_buf + ip_hdr->ihl * 4); |
| |
| if (udp_hdr->check == 0) { |
| pr_info("%s upd no checksum\n", __func__); |
| return 0; |
| } |
| } |
| |
| if (toe_tcpudp_csum_v4(ip_hdr, pkt_buf + ip_hdr->ihl * 4)) { |
| pr_info("%s: toe tcpudp v4 csum fail\n", __func__); |
| return ret; |
| } |
| pr_debug("%s tcp/udp protocol:%d checksum pass\n", |
| __func__, ip_hdr->protocol); |
| } |
| } else if (ip_hdr->version == 6) { |
| u8 nexthdr = ipv6_hdr->nexthdr; |
| int iphdr_len = 40, exthdr_len; |
| |
| /* skip the extension header, refer: ipv6_skip_exthdr */ |
| while (ipv6_ext_hdr(nexthdr)) { |
| struct ipv6_opt_hdr *opthdr; |
| |
| if (nexthdr == NEXTHDR_NONE) |
| break; |
| |
| opthdr = (struct ipv6_opt_hdr *)(pkt_buf + iphdr_len); |
| if (!opthdr) |
| break; |
| |
| if (nexthdr == NEXTHDR_FRAGMENT) |
| exthdr_len = 8; |
| else if (nexthdr == NEXTHDR_AUTH) |
| exthdr_len = (opthdr->hdrlen + 2) << 2; |
| else |
| exthdr_len = ipv6_optlen(opthdr); |
| |
| iphdr_len += exthdr_len; |
| nexthdr = opthdr->nexthdr; |
| } |
| |
| if ((nexthdr == IPPROTO_TCP) || (nexthdr == IPPROTO_UDP)) { |
| if (nexthdr == IPPROTO_UDP) { |
| struct udphdr *udp_hdr = (struct udphdr *)(pkt_buf + iphdr_len); |
| |
| if (udp_hdr->check == 0) |
| return 0; |
| } |
| |
| |
| if (toe_tcpudp_csum_v6(ipv6_hdr, pkt_buf + iphdr_len)) { |
| pr_info("%s: toe tcpudp v6 csum fail\n", __func__); |
| return ret; |
| } |
| pr_debug("%s tcp/udp protocol:%d checksum pass\n", |
| __func__, nexthdr); |
| } |
| } else |
| pr_err("%s pkt not IPV4/IPV6\n", __func__); |
| |
| return 0; |
| } |
| |
| int toe_hash_offset_check(void *pkt_buf, u8 offset) |
| { |
| struct iphdr *ip_hdr = (struct iphdr *)pkt_buf; |
| struct ipv6hdr *ipv6_hdr = (struct ipv6hdr *)pkt_buf; |
| |
| /* toe checksum */ |
| if (ip_hdr->version == 4) { |
| if (ip_hdr->protocol == IPPROTO_TCP) { |
| if (offset == (ip_hdr->ihl * 4)) { |
| pr_info("%s tcp: pass\n", __func__); |
| return 0; |
| } else |
| return -1; |
| |
| } else if (ip_hdr->protocol == IPPROTO_UDP) { |
| if (offset == (ip_hdr->ihl *4 + sizeof(struct udphdr))) { |
| pr_info("%s udp: pass\n", __func__); |
| return 0; |
| } else |
| return -1; |
| } |
| } else if (ip_hdr->version == 6) { |
| u8 nexthdr = ipv6_hdr->nexthdr; |
| int iphdr_len = 40, exthdr_len; |
| |
| /* skip the extension header, refer: ipv6_skip_exthdr */ |
| while (ipv6_ext_hdr(nexthdr)) { |
| struct ipv6_opt_hdr *opthdr; |
| |
| if (nexthdr == NEXTHDR_NONE) |
| break; |
| |
| opthdr = (struct ipv6_opt_hdr *)(pkt_buf + iphdr_len); |
| if (!opthdr) |
| break; |
| |
| if (nexthdr == NEXTHDR_FRAGMENT) |
| exthdr_len = 8; |
| else if (nexthdr == NEXTHDR_AUTH) |
| exthdr_len = (opthdr->hdrlen + 2) << 2; |
| else |
| exthdr_len = ipv6_optlen(opthdr); |
| |
| iphdr_len += exthdr_len; |
| nexthdr = opthdr->nexthdr; |
| } |
| |
| if (nexthdr == IPPROTO_TCP) { |
| if (offset == iphdr_len) { |
| pr_info("%s ipv6 tcp pass\n", __func__); |
| return 0; |
| } else |
| return -1; |
| |
| } else if (nexthdr == IPPROTO_UDP) { |
| if (offset == iphdr_len + sizeof(struct udphdr)) { |
| pr_info("%s ipv6 udp pass\n", __func__); |
| return 0; |
| } else |
| return -1; |
| } |
| } else |
| pr_err("%s pkt not IPV4/IPV6\n", __func__); |
| |
| return 0; |
| } |