drivers/qtn/ruby/iputil.c - kernel/quantenna - Git at Google

 /**
  * Copyright (c) 2012 Quantenna Communications, Inc.
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
  * as published by the Free Software Foundation; either version 2
  * of the License, or (at your option) any later version.
  *
  * 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.
  *
  * You should have received a copy of the GNU General Public License
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  **/

 #include <compat.h>
 #include <net/ip.h>
 #ifdef CONFIG_IPV6
 #include <net/ipv6.h>
 #endif

 #include <qtn/iputil.h>
 #include <asm/board/platform.h>

 #ifdef CONFIG_IPV6
 /* optimisation of the ipv6_skip_exthdr() kernel function */
 int __sram_text
 iputil_v6_skip_exthdr(const struct ipv6hdr *ipv6h, int start, uint8_t *nexthdrp,
 			int total_len, __be32 *ip_id, uint8_t *more_frags)
 {
 	uint8_t nexthdr = ipv6h->nexthdr;
 	struct frag_hdr *frag_hdrp;
 	struct ipv6_opt_hdr *hp;
 	int hdrlen;

 	while ((start < total_len) && ipv6_ext_hdr(nexthdr) && (nexthdr != NEXTHDR_NONE)) {
 		hp = (struct ipv6_opt_hdr *)((char *)ipv6h + start);

 		if (unlikely(nexthdr == NEXTHDR_FRAGMENT)) {
 			frag_hdrp = (struct frag_hdr *)hp;

 			if (ip_id != NULL) {
 				*ip_id = ntohl(get_unaligned(&frag_hdrp->identification));
 			}

 			KASSERT((((int)frag_hdrp) & 0x1) == 0,
 				("iputil: frag hdr is not on 2-octet boundary"));

 			if (more_frags != NULL) {
 				*more_frags = IPUTIL_V6_FRAG_MF(frag_hdrp);
 			}
 			if (IPUTIL_V6_FRAG_OFFSET(frag_hdrp)) {
 				/* not start of packet - does not contain protocol hdr */
 				break;
 			}
 			hdrlen = 8;
 		} else if (unlikely(nexthdr == NEXTHDR_AUTH)) {
 			hdrlen = (hp->hdrlen + 2) << 2;
 		} else {
 			hdrlen = ipv6_optlen(hp);
 		}

 		if ((start + hdrlen) > total_len) {
 			nexthdr = NEXTHDR_NONE;
 			break;
 		}
 		nexthdr = hp->nexthdr;
 		start += hdrlen;
 	}

 	*nexthdrp = nexthdr;

 	return start;
 }
 EXPORT_SYMBOL(iputil_v6_skip_exthdr);

 int iputil_v6_ntop(char *buf, const struct in6_addr *addr)
 {
 	char *p = buf;
 	int i = 0;
 	int zstart = 0;
 	int bestzstart = 0;
 	int bestzend = 0;
 	int bestzlen = 0;
 	int inz = 0;
 	int donez = 0;
 	const int addr16len = (int)(sizeof(struct in6_addr) / sizeof(uint16_t));

 	/* parse address, looking for longest substring of 0s */
 	for (i = 0; i < addr16len; i++) {
 		if (addr->s6_addr16[i] == 0) {
 			if (!inz) {
 				zstart = i;
 			} else {
 				int zlen;
 				int zend;
 				zend = i;
 				zlen = zend - zstart + 1;
 				if (zlen > bestzlen) {
 					bestzlen = zlen;
 					bestzstart = zstart;
 					bestzend = i;
 				}
 			}
 			inz = 1;
 		} else {
 			inz = 0;
 		}
 	}

 	/* when only the last 32 bits contain an address, format as an ::ipv4 */
 	if (bestzstart == 0 && bestzlen == 6) {
 		p += sprintf(p, "::%d.%d.%d.%d",
 				addr->s6_addr[12], addr->s6_addr[13],
 				addr->s6_addr[14], addr->s6_addr[15]);
 		return p - buf;
 	}

 	/* otherwise format as normal ipv6 */
 	for (i = 0; i < addr16len; i++) {
 		uint16_t s = ntohs(addr->s6_addr16[i]);
 		if ((bestzlen == 0) || (i < bestzstart) || (i > bestzend) || s) {
 			const char *colon;
 			if ((i == (addr16len - 1)) || (i == (bestzstart - 1))) {
 				colon = "";
 			} else {
 				colon = ":";
 			}
 			p += sprintf(p, "%x%s", s, colon);
 		} else if (bestzlen && (i == bestzstart)) {
 			inz = 1;
 		} else if (bestzlen && (i == bestzend)) {
 			p += sprintf(p, "::");
 			inz = 0;
 			donez = 1;
 		} else if (inz) {
 		} else {
 			WARN_ONCE(1, "%s: implementation error", __FUNCTION__);
 		}
 	}

 	return p - buf;
 }
 EXPORT_SYMBOL(iputil_v6_ntop);

 int iputil_v6_ntop_port(char *buf, const struct in6_addr *addr, __be16 port)
 {
 	char *p = buf;
 	p += sprintf(p, "[");
 	p += iputil_v6_ntop(p, addr);
 	p += sprintf(p, "]:%u", ntohs(port));
 	return p - buf;
 }
 EXPORT_SYMBOL(iputil_v6_ntop_port);

 int iputil_eth_is_v6_mld(void *iphdr, uint32_t data_len)
 {
 	struct ipv6hdr *ip6hdr_p = iphdr;
 	uint8_t nexthdr;
 	int nhdr_off;
 	struct icmp6hdr *icmp6hdr;
 	int is_ipv6_mld = 0;

 	nhdr_off = iputil_v6_skip_exthdr(ip6hdr_p, sizeof(struct ipv6hdr),
 		&nexthdr, data_len, NULL, NULL);

 	if (unlikely(nexthdr == IPPROTO_ICMPV6)) {
 		icmp6hdr = (struct icmp6hdr*)((__u8 *)ip6hdr_p + nhdr_off);

 		if (icmp6hdr->icmp6_type == ICMPV6_MGM_QUERY ||
 			icmp6hdr->icmp6_type == ICMPV6_MGM_REPORT ||
 			icmp6hdr->icmp6_type == ICMPV6_MGM_REDUCTION ||
 			icmp6hdr->icmp6_type == ICMPV6_MLD2_REPORT) {
 			is_ipv6_mld = 1;
 		}
 	}

 	return is_ipv6_mld;
 }
 EXPORT_SYMBOL(iputil_eth_is_v6_mld);
 #endif /* CONFIG_IPV6 */

 /*
  * Return IP packet protocol information
  */
 uint8_t __sram_text
 iputil_proto_info(void *iph, void *data, void **proto_data, uint32_t *ip_id, uint8_t *more_frags)
 {
 	const struct iphdr *ipv4h = iph;
 	uint16_t frag_off;
 #ifdef CONFIG_IPV6
 	u_int8_t nexthdr;
 	int start;
 	uint32_t data_len;
 #endif
 	struct sk_buff *skb = data;

 	if (ipv4h->version == 4) {
 		if (skb->len < (ipv4h->ihl << 2))
 			return 0;
 		frag_off = ntohs(get_unaligned((u16 *)&ipv4h->frag_off));
 		if (ip_id && (frag_off & (IP_OFFSET | IP_MF)) != 0) {
 			*ip_id = (uint32_t)ntohs(ipv4h->id);
 		}
 		if (more_frags) {
 			*more_frags = !!(frag_off & IP_MF);
 		}
 		*proto_data = (char *)iph + (ipv4h->ihl << 2);

 		if ((frag_off & IP_OFFSET) != 0) {
 			/* not start of packet - does not contain protocol hdr */
 			return NEXTHDR_FRAGMENT;
 		}

 		return ipv4h->protocol;
 	}

 #ifdef CONFIG_IPV6
 	if (ipv4h->version == 6) {
 		data_len = skb->len - ((uint8_t*)iph - (skb->data));
 		start = iputil_v6_skip_exthdr((struct ipv6hdr *)iph, sizeof(struct ipv6hdr),
 				&nexthdr, data_len, ip_id, more_frags);
 		*proto_data = (char *)iph + start;

 		return nexthdr;
 	}
 #endif

 	return 0;
 }
 EXPORT_SYMBOL(iputil_proto_info);

 int iputil_v4_ntop_port(char *buf, __be32 addr, __be16 port)
 {
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,7,0)
 	return sprintf(buf, "%pI4:%u", &addr, ntohs(port));
 #else
 	return sprintf(buf, NIPQUAD_FMT ":%u", NIPQUAD(addr), ntohs(port));
 #endif
 }
 EXPORT_SYMBOL(iputil_v4_ntop_port);

 int iputil_v4_pton(const char *ip_str, __be32 *ipaddr)
 {
 	int i;
 	uint32_t tmp_array[IPUTIL_V4_ADDR_LEN];
 	uint8_t *ipaddr_p = (uint8_t *)ipaddr;

 	if (ip_str == NULL)
 		return -1;

 	if (sscanf(ip_str, "%d.%d.%d.%d",
 			&tmp_array[0],
 			&tmp_array[1],
 			&tmp_array[2],
 			&tmp_array[3]) != 4) {
 		return -1;
 	}

 	for (i = 0; i < IPUTIL_V4_ADDR_LEN; i++) {
 		if (tmp_array[i] > 0xff) {
 			return -1;
 		}
 	}

 	ipaddr_p[0] = tmp_array[0];
 	ipaddr_p[1] = tmp_array[1];
 	ipaddr_p[2] = tmp_array[2];
 	ipaddr_p[3] = tmp_array[3];

 	return 0;
 }
 EXPORT_SYMBOL(iputil_v4_pton);

 #ifdef CONFIG_IPV6
 int iputil_ipv6_is_neigh_msg(struct ipv6hdr *ipv6, struct icmp6hdr *icmpv6)
 {
 	if (ipv6->hop_limit != 255) {
 		return 0;
 	}

 	if (icmpv6->icmp6_code != 0) {
 		return 0;
 	}

 	return 1;
 }
 EXPORT_SYMBOL(iputil_ipv6_is_neigh_msg);

 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,7,0)
 int iputil_ipv6_is_neigh_sol_msg(uint8_t dup_addr_detect, const struct in6_addr *target, const struct in6_addr *daddr)
 #else
 int iputil_ipv6_is_neigh_sol_msg(uint8_t dup_addr_detect, struct nd_msg *msg, struct ipv6hdr *ipv6)
 #endif
 {

 #if LINUX_VERSION_CODE < KERNEL_VERSION(4,7,0)
 	const struct in6_addr *daddr = &ipv6->daddr;
 #endif

 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,7,0)
 	if (ipv6_addr_is_multicast(target)) {
 #else
 	if (ipv6_addr_is_multicast(&msg->target)) {
 #endif

 		return 0;
 	}

 	if (dup_addr_detect && !(daddr->s6_addr32[0] == htonl(0xff020000) &&
 			daddr->s6_addr32[1] == htonl(0x00000000) &&
 			daddr->s6_addr32[2] == htonl(0x00000001) &&
 			daddr->s6_addr [12] == 0xff )) {
 		return 0;
 	}

 	return 1;
 }
 EXPORT_SYMBOL(iputil_ipv6_is_neigh_sol_msg);
 #endif
	/**
	* Copyright (c) 2012 Quantenna Communications, Inc.
	* All rights reserved.
	*
	* This program is free software; you can redistribute it and/or
	* modify it under the terms of the GNU General Public License
	* as published by the Free Software Foundation; either version 2
	* of the License, or (at your option) any later version.
	*
	* 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.
	*
	* You should have received a copy of the GNU General Public License
	* along with this program; if not, write to the Free Software
	* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
	**/

	#include <compat.h>
	#include <net/ip.h>
	#ifdef CONFIG_IPV6
	#include <net/ipv6.h>
	#endif

	#include <qtn/iputil.h>
	#include <asm/board/platform.h>

	#ifdef CONFIG_IPV6
	/* optimisation of the ipv6_skip_exthdr() kernel function */
	int __sram_text
	iputil_v6_skip_exthdr(const struct ipv6hdr ipv6h, int start, uint8_t nexthdrp,
	int total_len, __be32 ip_id, uint8_t more_frags)
	{
	uint8_t nexthdr = ipv6h->nexthdr;
	struct frag_hdr *frag_hdrp;
	struct ipv6_opt_hdr *hp;
	int hdrlen;

	while ((start < total_len) && ipv6_ext_hdr(nexthdr) && (nexthdr != NEXTHDR_NONE)) {
	hp = (struct ipv6_opt_hdr )((char )ipv6h + start);

	if (unlikely(nexthdr == NEXTHDR_FRAGMENT)) {
	frag_hdrp = (struct frag_hdr *)hp;

	if (ip_id != NULL) {
	*ip_id = ntohl(get_unaligned(&frag_hdrp->identification));
	}

	KASSERT((((int)frag_hdrp) & 0x1) == 0,
	("iputil: frag hdr is not on 2-octet boundary"));

	if (more_frags != NULL) {
	*more_frags = IPUTIL_V6_FRAG_MF(frag_hdrp);
	}
	if (IPUTIL_V6_FRAG_OFFSET(frag_hdrp)) {
	/* not start of packet - does not contain protocol hdr */
	break;
	}
	hdrlen = 8;
	} else if (unlikely(nexthdr == NEXTHDR_AUTH)) {
	hdrlen = (hp->hdrlen + 2) << 2;
	} else {
	hdrlen = ipv6_optlen(hp);
	}

	if ((start + hdrlen) > total_len) {
	nexthdr = NEXTHDR_NONE;
	break;
	}
	nexthdr = hp->nexthdr;
	start += hdrlen;
	}

	*nexthdrp = nexthdr;

	return start;
	}
	EXPORT_SYMBOL(iputil_v6_skip_exthdr);

	int iputil_v6_ntop(char buf, const struct in6_addr addr)
	{
	char *p = buf;
	int i = 0;
	int zstart = 0;
	int bestzstart = 0;
	int bestzend = 0;
	int bestzlen = 0;
	int inz = 0;
	int donez = 0;
	const int addr16len = (int)(sizeof(struct in6_addr) / sizeof(uint16_t));

	/* parse address, looking for longest substring of 0s */
	for (i = 0; i < addr16len; i++) {
	if (addr->s6_addr16[i] == 0) {
	if (!inz) {
	zstart = i;
	} else {
	int zlen;
	int zend;
	zend = i;
	zlen = zend - zstart + 1;
	if (zlen > bestzlen) {
	bestzlen = zlen;
	bestzstart = zstart;
	bestzend = i;
	}
	}
	inz = 1;
	} else {
	inz = 0;
	}
	}

	/* when only the last 32 bits contain an address, format as an ::ipv4 */
	if (bestzstart == 0 && bestzlen == 6) {
	p += sprintf(p, "::%d.%d.%d.%d",
	addr->s6_addr[12], addr->s6_addr[13],
	addr->s6_addr[14], addr->s6_addr[15]);
	return p - buf;
	}

	/* otherwise format as normal ipv6 */
	for (i = 0; i < addr16len; i++) {
	uint16_t s = ntohs(addr->s6_addr16[i]);
	if ((bestzlen == 0) \|\| (i < bestzstart) \|\| (i > bestzend) \|\| s) {
	const char *colon;
	if ((i == (addr16len - 1)) \|\| (i == (bestzstart - 1))) {
	colon = "";
	} else {
	colon = ":";
	}
	p += sprintf(p, "%x%s", s, colon);
	} else if (bestzlen && (i == bestzstart)) {
	inz = 1;
	} else if (bestzlen && (i == bestzend)) {
	p += sprintf(p, "::");
	inz = 0;
	donez = 1;
	} else if (inz) {
	} else {
	WARN_ONCE(1, "%s: implementation error", __FUNCTION__);
	}
	}

	return p - buf;
	}
	EXPORT_SYMBOL(iputil_v6_ntop);

	int iputil_v6_ntop_port(char buf, const struct in6_addr addr, __be16 port)
	{
	char *p = buf;
	p += sprintf(p, "[");
	p += iputil_v6_ntop(p, addr);
	p += sprintf(p, "]:%u", ntohs(port));
	return p - buf;
	}
	EXPORT_SYMBOL(iputil_v6_ntop_port);

	int iputil_eth_is_v6_mld(void *iphdr, uint32_t data_len)
	{
	struct ipv6hdr *ip6hdr_p = iphdr;
	uint8_t nexthdr;
	int nhdr_off;
	struct icmp6hdr *icmp6hdr;
	int is_ipv6_mld = 0;

	nhdr_off = iputil_v6_skip_exthdr(ip6hdr_p, sizeof(struct ipv6hdr),
	&nexthdr, data_len, NULL, NULL);

	if (unlikely(nexthdr == IPPROTO_ICMPV6)) {
	icmp6hdr = (struct icmp6hdr)((__u8 )ip6hdr_p + nhdr_off);

	if (icmp6hdr->icmp6_type == ICMPV6_MGM_QUERY \|\|
	icmp6hdr->icmp6_type == ICMPV6_MGM_REPORT \|\|
	icmp6hdr->icmp6_type == ICMPV6_MGM_REDUCTION \|\|
	icmp6hdr->icmp6_type == ICMPV6_MLD2_REPORT) {
	is_ipv6_mld = 1;
	}
	}

	return is_ipv6_mld;
	}
	EXPORT_SYMBOL(iputil_eth_is_v6_mld);
	#endif /* CONFIG_IPV6 */

	/*
	* Return IP packet protocol information
	*/
	uint8_t __sram_text
	iputil_proto_info(void iph, void data, void *proto_data, uint32_t ip_id, uint8_t *more_frags)
	{
	const struct iphdr *ipv4h = iph;
	uint16_t frag_off;
	#ifdef CONFIG_IPV6
	u_int8_t nexthdr;
	int start;
	uint32_t data_len;
	#endif
	struct sk_buff *skb = data;

	if (ipv4h->version == 4) {
	if (skb->len < (ipv4h->ihl << 2))
	return 0;
	frag_off = ntohs(get_unaligned((u16 *)&ipv4h->frag_off));
	if (ip_id && (frag_off & (IP_OFFSET \| IP_MF)) != 0) {
	*ip_id = (uint32_t)ntohs(ipv4h->id);
	}
	if (more_frags) {
	*more_frags = !!(frag_off & IP_MF);
	}
	proto_data = (char )iph + (ipv4h->ihl << 2);

	if ((frag_off & IP_OFFSET) != 0) {
	/* not start of packet - does not contain protocol hdr */
	return NEXTHDR_FRAGMENT;
	}

	return ipv4h->protocol;
	}

	#ifdef CONFIG_IPV6
	if (ipv4h->version == 6) {
	data_len = skb->len - ((uint8_t*)iph - (skb->data));
	start = iputil_v6_skip_exthdr((struct ipv6hdr *)iph, sizeof(struct ipv6hdr),
	&nexthdr, data_len, ip_id, more_frags);
	proto_data = (char )iph + start;

	return nexthdr;
	}
	#endif

	return 0;
	}
	EXPORT_SYMBOL(iputil_proto_info);

	int iputil_v4_ntop_port(char *buf, __be32 addr, __be16 port)
	{
	#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,7,0)
	return sprintf(buf, "%pI4:%u", &addr, ntohs(port));
	#else
	return sprintf(buf, NIPQUAD_FMT ":%u", NIPQUAD(addr), ntohs(port));
	#endif
	}
	EXPORT_SYMBOL(iputil_v4_ntop_port);

	int iputil_v4_pton(const char ip_str, __be32 ipaddr)
	{
	int i;
	uint32_t tmp_array[IPUTIL_V4_ADDR_LEN];
	uint8_t ipaddr_p = (uint8_t )ipaddr;

	if (ip_str == NULL)
	return -1;

	if (sscanf(ip_str, "%d.%d.%d.%d",
	&tmp_array[0],
	&tmp_array[1],
	&tmp_array[2],
	&tmp_array[3]) != 4) {
	return -1;
	}

	for (i = 0; i < IPUTIL_V4_ADDR_LEN; i++) {
	if (tmp_array[i] > 0xff) {
	return -1;
	}
	}

	ipaddr_p[0] = tmp_array[0];
	ipaddr_p[1] = tmp_array[1];
	ipaddr_p[2] = tmp_array[2];
	ipaddr_p[3] = tmp_array[3];

	return 0;
	}
	EXPORT_SYMBOL(iputil_v4_pton);

	#ifdef CONFIG_IPV6
	int iputil_ipv6_is_neigh_msg(struct ipv6hdr ipv6, struct icmp6hdr icmpv6)
	{
	if (ipv6->hop_limit != 255) {
	return 0;
	}

	if (icmpv6->icmp6_code != 0) {
	return 0;
	}

	return 1;
	}
	EXPORT_SYMBOL(iputil_ipv6_is_neigh_msg);

	#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,7,0)
	int iputil_ipv6_is_neigh_sol_msg(uint8_t dup_addr_detect, const struct in6_addr target, const struct in6_addr daddr)
	#else
	int iputil_ipv6_is_neigh_sol_msg(uint8_t dup_addr_detect, struct nd_msg msg, struct ipv6hdr ipv6)
	#endif
	{

	#if LINUX_VERSION_CODE < KERNEL_VERSION(4,7,0)
	const struct in6_addr *daddr = &ipv6->daddr;
	#endif

	#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,7,0)
	if (ipv6_addr_is_multicast(target)) {
	#else
	if (ipv6_addr_is_multicast(&msg->target)) {
	#endif

	return 0;
	}

	if (dup_addr_detect && !(daddr->s6_addr32[0] == htonl(0xff020000) &&
	daddr->s6_addr32[1] == htonl(0x00000000) &&
	daddr->s6_addr32[2] == htonl(0x00000001) &&
	daddr->s6_addr [12] == 0xff )) {
	return 0;
	}

	return 1;
	}
	EXPORT_SYMBOL(iputil_ipv6_is_neigh_sol_msg);
	#endif