package/kernel/mfp/files/fp_cm.c

/*
 *	Fastpath Cm Interface
 *
 *	This program is free software; you can redistribute it and/or
 *	modify it under the terms of the GNU FP_ERR( Public License
 *	as published by the Free Software Foundation; either version
 *	2 of the License, or (at your option) any later version.
 */

#include <linux/if_vlan.h>
#include "fp_common.h"
#include "fp_database.h"
#include "fp_device.h"
#include "fp_core.h"
#include "../linux/drivers/marvell/toev2/toe.h"
#include "../linux/drivers/marvell/toev2/toe_464xlat.h"


#define MAXLEN 256
#define DEVICE_NAME_MAXSIZE 64
#define NLA_DATA(na) ((void *)((char *)(na) + NLA_HDRLEN))

static u32 g_cm_nlpid = -1;
static u32 speed_thresh = 1000; /* kbps */

static int fp_cm_genl_get_tuple(struct sk_buff *skb, struct genl_info *info);
static int fp_cm_genl_del_tuple(struct sk_buff *skb, struct genl_info *info);
static int fp_cm_genl_set_tuple(struct sk_buff *skb, struct genl_info *info);
static int fp_cm_genl_set_pid(struct sk_buff *skb, struct genl_info *info);
static void fp_cm_update_genl_pid(u32 pid);
static u32 fp_cm_get_genl_pid(void);

/* attribute type */
enum fp_cm_genl_attrs {
	CM_ATTR_UNSPEC,
	CM_ATTR_PID,
	CM_ATTR_SRC_IP,
	CM_ATTR_DST_IP,
	CM_ATTR_SRC_IP6,		/* struct in6_addr */
	CM_ATTR_DST_IP6,		/* struct in6_addr */
	CM_ATTR_SRC_PORT,
	CM_ATTR_DST_PORT,
	CM_ATTR_PROTO,
	CM_ATTR_SRC_MAC,
	CM_ATTR_DST_MAC,
	CM_ATTR_SNAT,
	CM_ATTR_FWD,
	CM_ATTR_NAT_PORT,
	CM_ATTR_NAT_IP,
	CM_ATTR_DEVICE_NAME,
	CM_ATTR_MCID,
	CM_ATTR_RBID,
	CM_ATTR_QFI,
	CM_ATTR_PDU,
	CM_ATTR_IN_PKT,
	CM_ATTR_OUT_PKT,
	CM_ATTR_VLAN_EN,
	CM_ATTR_VLANID,
	CM_ATTR_XLAT_EN,
	CM_ATTR_XLAT_INSTANCE,
	CM_ATTR_UPDATE_TUPLE,
/* private: internal use only */
	__FP_CM_ATTR_AFTER_LAST
};
#define FP_CM_ATTR_MAX (__FP_CM_ATTR_AFTER_LAST - 1)

/* commands */
enum fp_cm_commands {
	CM_CMD_UNSPEC,
	CM_CMD_SET_PID,
	CM_CMD_GET_TUPLE,
	CM_CMD_SET_TUPLE,
	CM_CMD_DEL_TUPLE,
	__FP_CM_CMD_AFTER_LAST,
};
#define FP_CM_CMD_MAX (__FP_CM_CMD_AFTER_LAST - 1)

#define ETH_TYPE_LEN		2
#define FP_CM_NLMSG_DEFAULT_SIZE  256


/* attribute policy */
static struct nla_policy fp_cm_genl_policy[FP_CM_ATTR_MAX + 1] = {
	[CM_ATTR_PID] = { .type = NLA_U32 },
	[CM_ATTR_SRC_IP] = { .type = NLA_U32 },
	[CM_ATTR_DST_IP] = { .type = NLA_U32 },
	[CM_ATTR_SRC_IP6] = {
		.type = NLA_BINARY,
		.len = sizeof(struct in6_addr),
	},
	[CM_ATTR_DST_IP6] = {
		.type = NLA_BINARY,
		.len = sizeof(struct in6_addr),
	},
	[CM_ATTR_SRC_PORT] = { .type = NLA_U16 },
	[CM_ATTR_DST_PORT] = { .type = NLA_U16 },
	[CM_ATTR_PROTO] = { .type = NLA_U8 },
	[CM_ATTR_SNAT] = { .type = NLA_U8 },
	[CM_ATTR_FWD] = { .type = NLA_U8 },
	[CM_ATTR_NAT_PORT] = { .type = NLA_U16 },
	[CM_ATTR_NAT_IP] = { .type = NLA_U32 },
	[CM_ATTR_SRC_MAC] = { .type = NLA_STRING},
	[CM_ATTR_DST_MAC] = { .type = NLA_STRING},
	[CM_ATTR_DEVICE_NAME] = { .type = NLA_STRING,
			.len = DEVICE_NAME_MAXSIZE },
	[CM_ATTR_MCID] = { .type = NLA_U8 },
	[CM_ATTR_RBID] = { .type = NLA_U8 },
	[CM_ATTR_QFI] = { .type = NLA_U8 },
	[CM_ATTR_PDU] = { .type = NLA_U8 },
	[CM_ATTR_IN_PKT] = { .type = NLA_U8 },
	[CM_ATTR_OUT_PKT] = { .type = NLA_U8 },
	[CM_ATTR_VLAN_EN] = { .type = NLA_U8 },
	[CM_ATTR_VLANID] = { .type = NLA_U16 },
	[CM_ATTR_XLAT_EN] = { .type = NLA_U32 },
	[CM_ATTR_XLAT_INSTANCE] = { .type = NLA_STRING },
	[CM_ATTR_UPDATE_TUPLE] = { .type = NLA_U8 },
};

/* operation definition */
struct genl_ops fp_cm_genl_ops[] = {
	{
		.cmd = CM_CMD_SET_PID,
		.flags = 0,
		.doit = fp_cm_genl_set_pid,
	},
	{
		.cmd = CM_CMD_GET_TUPLE,
		.flags = 0,
		.doit = fp_cm_genl_get_tuple,
	},
	{
		.cmd = CM_CMD_SET_TUPLE,
		.flags = 0,
		.doit = fp_cm_genl_set_tuple,
	},
	{
		.cmd = CM_CMD_DEL_TUPLE,
		.flags = 0,
		.doit = fp_cm_genl_del_tuple,
	}
};

static struct genl_family fp_cm_genl_family = {
	.hdrsize = 0,
	.name = "fp_cm",
	.version = 1,
	.maxattr = FP_CM_ATTR_MAX,
	.policy = fp_cm_genl_policy,
	.ops	 = fp_cm_genl_ops,
	.n_ops	 = ARRAY_SIZE(fp_cm_genl_ops),
};

static void fp_cm_update_genl_pid(u32 pid)
{
	g_cm_nlpid = pid;
}

static u32 fp_cm_get_genl_pid(void)
{
	return g_cm_nlpid;
}

static int fp_cm_genl_set_pid(struct sk_buff *skb, struct genl_info *info)
{
	u32 pid;

	if (!info->attrs[CM_ATTR_PID])
		return -EINVAL;

	pid = nla_get_u32(info->attrs[CM_ATTR_PID]);
	printk("%s, get cm pid: %d\n", __func__, pid);
	fp_cm_update_genl_pid(pid);
	return 0;
}

static int __fp_cm_genl_fill_tuple_info(struct sk_buff *msg, struct nf_conntrack_tuple *tuple,
				struct fpdb_entry *el, u32 portid, u32 seq, int flags, int add)
{
	void *hdr;
	struct hh_cache *hh;
	int hh_len;
	u8 proto = 0, in_pkt = 0, out_pkt = 0, fwd = 0, nat = 0;
	u16 nat_port = 0;
	u32 nat_ip = 0;
	char src_mac[ETH_ALEN]={0}, dst_mac[ETH_ALEN]={0};
	struct fp_net_device *dst, *src;
	struct vlan_dev_priv *vlan;
	struct net_device *src_dev, *dst_dev;

	hh = &el->hh;
	hh_len = hh->hh_len;

	if (add)
		hdr = genlmsg_put(msg, portid, seq, &fp_cm_genl_family, flags,
				  CM_CMD_GET_TUPLE);
	else
		hdr = genlmsg_put(msg, portid, seq, &fp_cm_genl_family, flags,
				  CM_CMD_DEL_TUPLE);
	if (!hdr)
		return -EMSGSIZE;

	if (tuple->src.l3num == AF_INET) {
		if (nla_put_u32(msg, CM_ATTR_SRC_IP, tuple->src.u3.ip) ||
			nla_put_u32(msg, CM_ATTR_DST_IP, tuple->dst.u3.ip))
			goto nla_put_failure;
	} else if (tuple->src.l3num == AF_INET6) {
		if (nla_put(msg, CM_ATTR_SRC_IP6, sizeof(struct in6_addr), &tuple->src.u3.in6) ||
			nla_put(msg, CM_ATTR_DST_IP6, sizeof(struct in6_addr), &tuple->dst.u3.in6))
			goto nla_put_failure;
	}

	if (tuple->dst.protonum == IPPROTO_UDP)
		proto = TOE_UDP;
	else if (tuple->dst.protonum == IPPROTO_TCP)
		proto = TOE_TCP;
	else
		proto = TOE_MAX;

	if (nla_put_u16(msg, CM_ATTR_SRC_PORT, ntohs(tuple->src.u.all)) ||
		nla_put_u16(msg, CM_ATTR_DST_PORT, ntohs(tuple->dst.u.all)) ||
		nla_put_u8(msg, CM_ATTR_PROTO, proto))
		goto nla_put_failure;

	src = rcu_dereference_bh(el->in_dev);
	dst = rcu_dereference_bh(el->out_dev);

	if (is_vlan_dev(src->dev)) {
		vlan = vlan_dev_priv(src->dev);
		src_dev = vlan->real_dev;
		nla_put_u8(msg, CM_ATTR_VLAN_EN, 1);
		nla_put_u16(msg, CM_ATTR_VLANID, vlan->vlan_id);
	} else
		src_dev = src->dev;

	if (!strncasecmp(src_dev->name, "ccinet", 6))
		in_pkt = PDU_PKT;
	else if (!strncasecmp(src_dev->name, "usbnet", 6))
		in_pkt = USB_PKT;
	else if (!strncasecmp(src_dev->name, "wlan", 4))
		in_pkt = WIFI_PKT;
	else if (!strncasecmp(src_dev->name, "eth", 3))
		in_pkt = ETH_PKT;
	else
		in_pkt = AP_PKT;

	if (is_vlan_dev(dst->dev)) {
		vlan = vlan_dev_priv(dst->dev);
		dst_dev = vlan->real_dev;
		nla_put_u8(msg, CM_ATTR_VLAN_EN, 1);
		nla_put_u16(msg, CM_ATTR_VLANID, vlan->vlan_id);
	} else
		dst_dev = dst->dev;

	if (!strncasecmp(dst_dev->name, "ccinet", 6))
		out_pkt = PDU_PKT;
	else if (!strncasecmp(dst_dev->name, "usbnet", 6))
		out_pkt = USB_PKT;
	else if (!strncasecmp(dst_dev->name, "wlan", 4))
		out_pkt = WIFI_PKT;
	else if (!strncasecmp(dst_dev->name, "eth", 3))
		out_pkt = ETH_PKT;
	else
		out_pkt = AP_PKT;

	fwd = (in_pkt != AP_PKT) && (out_pkt != AP_PKT);
	if (fwd && (tuple->src.l3num == AF_INET)) {
		if (in_pkt == PDU_PKT && (out_pkt == USB_PKT || out_pkt == WIFI_PKT || out_pkt == ETH_PKT)) {
			nat = 1;
			nat_ip = el->out_tuple.src.u3.ip;
			nat_port = ntohs(el->out_tuple.src.u.all);
		} else if ((in_pkt == USB_PKT || in_pkt == WIFI_PKT || in_pkt == ETH_PKT) && out_pkt == PDU_PKT) {
			nat = 1;
			nat_ip = el->out_tuple.dst.u3.ip;
			nat_port = ntohs(el->out_tuple.dst.u.all);
		} else
			/* CP TOE WIFI/WIFI TOE CP no need nat */
			nat = 0;
	}

	if (nla_put_u8(msg, CM_ATTR_IN_PKT, in_pkt) ||
		nla_put_u8(msg, CM_ATTR_OUT_PKT, out_pkt) ||
		nla_put_u8(msg, CM_ATTR_FWD, fwd) ||
		nla_put_string(msg, CM_ATTR_DEVICE_NAME, dst->dev->name))
		goto nla_put_failure;

	if (tuple->src.l3num == AF_INET) {
		if (nla_put_u8(msg, CM_ATTR_SNAT, nat) ||
			nla_put_u16(msg, CM_ATTR_NAT_PORT, nat_port) ||
			nla_put_u32(msg, CM_ATTR_NAT_IP, nat_ip))
			goto nla_put_failure;
	}

	if (hh_len) {
		if (likely(hh_len <= HH_DATA_MOD)) {
			/* this is inlined by gcc */
			char mac_header[HH_DATA_MOD];
			memcpy(mac_header, hh->hh_data, HH_DATA_MOD);
			memcpy(src_mac, &mac_header[HH_DATA_MOD-ETH_TYPE_LEN-ETH_ALEN], ETH_ALEN);
			memcpy(dst_mac, &mac_header[HH_DATA_MOD-ETH_TYPE_LEN-ETH_ALEN*2], ETH_ALEN);
		} else {
			int hh_alen = HH_DATA_ALIGN(hh_len);
			char *mac_header = kmalloc(hh_alen, GFP_ATOMIC);

			memcpy(mac_header, hh->hh_data, hh_alen);
			memcpy(src_mac, mac_header+(hh_alen-ETH_TYPE_LEN-ETH_ALEN), ETH_ALEN);
			memcpy(dst_mac, mac_header+(hh_alen-ETH_TYPE_LEN-ETH_ALEN*2), ETH_ALEN);
			kfree(mac_header);
		}
	}

	if (nla_put(msg, CM_ATTR_SRC_MAC, ETH_ALEN, src_mac) ||
		nla_put(msg, CM_ATTR_DST_MAC, ETH_ALEN, dst_mac))
			goto nla_put_failure;

	pr_debug("%s:\n in:%d, out:%d\n src_ip:0x%x\n dst_ip:0x%x\n src_port:%d\n dst_prot:%d\n"
			" protocol:%d\n nat_port:%d\n nat_ip:0x%x\n fwd:%d\n snat:%d\n",
			__func__, in_pkt, out_pkt, ntohl(tuple->src.u3.ip), ntohl(tuple->dst.u3.ip), ntohs(tuple->src.u.all),
			ntohs(tuple->dst.u.all), proto, nat_port, ntohl(nat_ip), fwd, nat);

	genlmsg_end(msg, hdr);
	if (add)
		el->nl_flag = 1;

	return 0;

nla_put_failure:
	genlmsg_cancel(msg, hdr);
	return -EMSGSIZE;
}

static int __fp_cm_genl_fill_464xlat_info(struct sk_buff *msg, struct nf_conntrack_tuple *tuple,
				struct fpdb_entry *el, u32 portid, u32 seq, int flags, int add)
{
	void *hdr;
	struct hh_cache *hh;
	int hh_len;
	u8 proto = 0, in_pkt = 0, out_pkt = 0, fwd = 0, nat = 0;
	u16 nat_port = 0;
	u32 nat_ip = 0;
	char src_mac[ETH_ALEN]={0}, dst_mac[ETH_ALEN]={0};
	struct fp_net_device *dst, *src;
	struct vlan_dev_priv *vlan;
	struct net_device *src_dev, *dst_dev;
	nat46_instance_t *nat46;
	nat46_netdev_priv_t *dev_priv;

	hh = &el->hh;
	hh_len = hh->hh_len;

	if (tuple->src.l3num == AF_INET6) {
		el->nl_flag = 1;
		return 0;
	}

	if (add)
		hdr = genlmsg_put(msg, portid, seq, &fp_cm_genl_family, flags,
				  CM_CMD_GET_TUPLE);
	else
		hdr = genlmsg_put(msg, portid, seq, &fp_cm_genl_family, flags,
				  CM_CMD_DEL_TUPLE);
	if (!hdr)
		return -EMSGSIZE;

	if (tuple->src.l3num == AF_INET) {
		if (nla_put_u32(msg, CM_ATTR_SRC_IP, tuple->src.u3.ip) ||
			nla_put_u32(msg, CM_ATTR_DST_IP, tuple->dst.u3.ip))
		goto nla_put_failure;
	}

	if (tuple->dst.protonum == IPPROTO_UDP)
		proto = TOE_UDP;
	else if (tuple->dst.protonum == IPPROTO_TCP)
		proto = TOE_TCP;
	else
		proto = TOE_MAX;

	if (nla_put_u16(msg, CM_ATTR_SRC_PORT, ntohs(tuple->src.u.all)) ||
		nla_put_u16(msg, CM_ATTR_DST_PORT, ntohs(tuple->dst.u.all)) ||
		nla_put_u8(msg, CM_ATTR_PROTO, proto))
		goto nla_put_failure;

	src = rcu_dereference_bh(el->in_dev);
	dst = rcu_dereference_bh(el->out_dev);

	if (is_vlan_dev(src->dev)) {
		vlan = vlan_dev_priv(src->dev);
		src_dev = vlan->real_dev;
		nla_put_u8(msg, CM_ATTR_VLAN_EN, 1);
		nla_put_u16(msg, CM_ATTR_VLANID, vlan->vlan_id);
	} else
		src_dev = src->dev;

	if (is_nat46_dev(src_dev) && is_valid_nat46_instance(src_dev)) {
		//RX:
		in_pkt = PDU_PKT;
		dev_priv = netdev_priv(src_dev);
		nat46 = dev_priv->nat46;
		pr_debug("%s:\nDL xlat enable\n, src:%pI6c, dst:%pI6c\n", __func__, nat46->pairs[0].remote.v6_pref.s6_addr32,
				nat46->pairs[0].local.v6_pref.s6_addr32);

		if (nla_put_u32(msg, CM_ATTR_XLAT_EN, 1) ||
			nla_put(msg, CM_ATTR_SRC_IP6, sizeof(struct in6_addr), &nat46->pairs[0].remote.v6_pref) ||
			nla_put(msg, CM_ATTR_DST_IP6, sizeof(struct in6_addr), &nat46->pairs[0].local.v6_pref))
			goto nla_put_failure;
	} else if (!strncasecmp(src_dev->name, "ccinet", 6))
		in_pkt = PDU_PKT;
	else if (!strncasecmp(src_dev->name, "usbnet", 6))
		in_pkt = USB_PKT;
	else if (!strncasecmp(src_dev->name, "wlan", 4))
		in_pkt = WIFI_PKT;
	else if (!strncasecmp(src_dev->name, "eth", 3))
		in_pkt = ETH_PKT;
	else
		in_pkt = AP_PKT;

	if (is_vlan_dev(dst->dev)) {
		vlan = vlan_dev_priv(dst->dev);
		dst_dev = vlan->real_dev;
		nla_put_u8(msg, CM_ATTR_VLAN_EN, 1);
		nla_put_u16(msg, CM_ATTR_VLANID, vlan->vlan_id);
	} else
		dst_dev = dst->dev;

	if (is_nat46_dev(dst_dev) && is_valid_nat46_instance(dst_dev)) {
		//TX
		out_pkt = PDU_PKT;
		dev_priv = netdev_priv(dst_dev);
		nat46 = dev_priv->nat46;
		pr_debug("%s:\nUL xlat enable\n, xlat instance: %s, src:%pI6c, dst:%pI6c\n", __func__, dst_dev->name,
				nat46->pairs[0].local.v6_pref.s6_addr32, nat46->pairs[0].remote.v6_pref.s6_addr32);

		if (nla_put_u32(msg, CM_ATTR_XLAT_EN, 1) ||
			nla_put_string(msg, CM_ATTR_XLAT_INSTANCE, dst_dev->name) ||
			nla_put(msg, CM_ATTR_SRC_IP6, sizeof(struct in6_addr), &nat46->pairs[0].local.v6_pref) ||
			nla_put(msg, CM_ATTR_DST_IP6, sizeof(struct in6_addr), &nat46->pairs[0].remote.v6_pref))
			goto nla_put_failure;
	} else if (!strncasecmp(dst_dev->name, "ccinet", 6))
		out_pkt = PDU_PKT;
	else if (!strncasecmp(dst_dev->name, "usbnet", 6))
		out_pkt = USB_PKT;
	else if (!strncasecmp(dst_dev->name, "wlan", 4))
		out_pkt = WIFI_PKT;
	else if (!strncasecmp(dst_dev->name, "eth", 3))
		out_pkt = ETH_PKT;
	else
		out_pkt = AP_PKT;

	fwd = (in_pkt != AP_PKT) && (out_pkt != AP_PKT);
	if (fwd && (tuple->src.l3num == AF_INET)) {
		if (in_pkt == PDU_PKT && (out_pkt == USB_PKT || out_pkt == WIFI_PKT || out_pkt == ETH_PKT)) {
			nat = 1;
			nat_ip = el->out_tuple.src.u3.ip;
			nat_port = ntohs(el->out_tuple.src.u.all);
		} else if ((in_pkt == USB_PKT || in_pkt == WIFI_PKT || in_pkt == ETH_PKT) && out_pkt == PDU_PKT) {
			nat = 1;
			nat_ip = el->out_tuple.dst.u3.ip;
			nat_port = ntohs(el->out_tuple.dst.u.all);
		} else
			/* not support*/
			goto nla_put_failure;
	}

	if (nla_put_u8(msg, CM_ATTR_IN_PKT, in_pkt) ||
		nla_put_u8(msg, CM_ATTR_OUT_PKT, out_pkt) ||
		nla_put_u8(msg, CM_ATTR_FWD, fwd) ||
		nla_put_string(msg, CM_ATTR_DEVICE_NAME, dst_dev->name))
		goto nla_put_failure;

	if (tuple->src.l3num == AF_INET) {
		if (nla_put_u8(msg, CM_ATTR_SNAT, nat) ||
			nla_put_u16(msg, CM_ATTR_NAT_PORT, nat_port) ||
			nla_put_u32(msg, CM_ATTR_NAT_IP, nat_ip))
			goto nla_put_failure;
	}

	if (hh_len) {
		if (likely(hh_len <= HH_DATA_MOD)) {
			/* this is inlined by gcc */
			char mac_header[HH_DATA_MOD];
			memcpy(mac_header, hh->hh_data, HH_DATA_MOD);
			memcpy(src_mac, &mac_header[HH_DATA_MOD-ETH_TYPE_LEN-ETH_ALEN], ETH_ALEN);
			memcpy(dst_mac, &mac_header[HH_DATA_MOD-ETH_TYPE_LEN-ETH_ALEN*2], ETH_ALEN);
		} else {
			int hh_alen = HH_DATA_ALIGN(hh_len);
			char *mac_header = kmalloc(hh_alen, GFP_ATOMIC);

			memcpy(mac_header, hh->hh_data, hh_alen);
			memcpy(src_mac, mac_header+(hh_alen-ETH_TYPE_LEN-ETH_ALEN), ETH_ALEN);
			memcpy(dst_mac, mac_header+(hh_alen-ETH_TYPE_LEN-ETH_ALEN*2), ETH_ALEN);
			kfree(mac_header);
		}
	}

	if (nla_put(msg, CM_ATTR_SRC_MAC, ETH_ALEN, src_mac) ||
		nla_put(msg, CM_ATTR_DST_MAC, ETH_ALEN, dst_mac))
			goto nla_put_failure;

	pr_debug("%s:\n in:%d, out:%d\n src_ip:0x%x\n dst_ip:0x%x\n src_port:%d\n dst_prot:%d\n"
			" protocol:%d\n nat_port:%d\n nat_ip:0x%x\n fwd:%d\n snat:%d\n\n",
			__func__, in_pkt, out_pkt, ntohl(tuple->src.u3.ip), ntohl(tuple->dst.u3.ip), ntohs(tuple->src.u.all),
			ntohs(tuple->dst.u.all), proto, nat_port, ntohl(nat_ip), fwd, nat);

	genlmsg_end(msg, hdr);
	el->nl_flag = 1;

	return 0;

nla_put_failure:
	genlmsg_cancel(msg, hdr);
	return -EMSGSIZE;
}

static int fp_cm_genl_fill_tuple_info(struct sk_buff *msg, struct nf_conntrack_tuple *tuple,
				struct fpdb_entry *el, u32 portid, int add)
{
	struct fp_net_device *dst, *src;

	if (unlikely(!tuple) || unlikely(!el))
		return -EMSGSIZE;

	src = rcu_dereference_bh(el->in_dev);
	dst = rcu_dereference_bh(el->out_dev);
	if (!src || !dst)
		return -EMSGSIZE;

	if (is_nat46_dev(src->dev) || is_nat46_dev(dst->dev)) {
		return __fp_cm_genl_fill_464xlat_info(msg, tuple, el, portid, 0, 0, add);
	} else
		return __fp_cm_genl_fill_tuple_info(msg, tuple, el, portid, 0, 0, add);
}

static int fp_cm_genl_fill_tuple_info_for_test(struct sk_buff *msg, u32 portid, u32 seq,
				int flags, int add)
{
	void *hdr;
	struct in6_addr addr;
	char mac[ETH_ALEN] = {0x0, 0x2, 0x3, 0x4, 0x5, 0x6};

	if (add)
		hdr = genlmsg_put(msg, portid, seq, &fp_cm_genl_family, flags,
				  CM_CMD_GET_TUPLE);
	else
		hdr = genlmsg_put(msg, portid, seq, &fp_cm_genl_family, flags,
				  CM_CMD_DEL_TUPLE);
	if (!hdr)
		return -EMSGSIZE;

	memset(&addr.s6_addr, 6, sizeof(struct in6_addr));

	if (nla_put_u32(msg, CM_ATTR_SRC_IP, 0xC0A80101) ||
		nla_put_u32(msg, CM_ATTR_DST_IP, 0xC0A80102) ||
		/* nla_put(msg, CM_ATTR_SRC_IP6, sizeof(struct in6_addr), &addr) ||
		nla_put(msg, CM_ATTR_DST_IP6, sizeof(struct in6_addr), &addr) || */
		nla_put_u16(msg, CM_ATTR_SRC_PORT, 0x64) ||
		nla_put_u16(msg, CM_ATTR_DST_PORT, 0xC8) ||
		nla_put_u8(msg, CM_ATTR_PROTO, TOE_TCP) ||
		nla_put_u16(msg, CM_ATTR_NAT_PORT, 0x64) ||
		nla_put_u32(msg, CM_ATTR_NAT_IP, 0xC0A8010A) ||
		nla_put(msg, CM_ATTR_SRC_MAC, ETH_ALEN, mac) ||
		nla_put(msg, CM_ATTR_DST_MAC, ETH_ALEN, mac) ||
		nla_put_string(msg, CM_ATTR_DEVICE_NAME, "ccinet0") ||
		nla_put_u8(msg, CM_ATTR_IN_PKT, PDU_PKT) ||
		nla_put_u8(msg, CM_ATTR_OUT_PKT, USB_PKT) ||
		nla_put_u8(msg, CM_ATTR_FWD, 1) ||
		nla_put_u8(msg, CM_ATTR_SNAT, 1) ||
		nla_put_u8(msg, CM_ATTR_VLAN_EN, 1) ||
		nla_put_u16(msg, CM_ATTR_VLANID, 0x64))
		goto nla_put_failure;

	genlmsg_end(msg, hdr);
	return 0;

nla_put_failure:
	genlmsg_cancel(msg, hdr);
	return -EMSGSIZE;
}

static int fp_cm_genl_set_tuple(struct sk_buff *skb, struct genl_info *info)
{
	struct toe_tuple_buff toe_tuple, toe_tuple_tmp;
	char dev_name[DEVICE_NAME_MAXSIZE] = {0};
	char src_mac[ETH_ALEN]={0}, dst_mac[ETH_ALEN]={0};
	char xlat_instance[16] = {0};
	struct in6_addr *src_ip6 = NULL;
	struct in6_addr *dst_ip6 = NULL;
	u32 src_ip, dst_ip, nat_ip, xlat_en = 0;
	u16 src_port, dst_port, nat_port, vlanid;
	u8 rx_tx, prot, fwd, nat, in_pkt, out_pkt, pdu, qfi, rbid, mcid, vlan_en;
	u8 update = 0;

	if (!info->attrs[CM_ATTR_MCID] ||
		!info->attrs[CM_ATTR_RBID] ||
		!info->attrs[CM_ATTR_QFI] ||
		!info->attrs[CM_ATTR_PDU])
		return -EINVAL;

	memset(&toe_tuple, 0, sizeof(struct toe_tuple_buff));
	memset(&toe_tuple_tmp, 0, sizeof(struct toe_tuple_buff));

	if (info->attrs[CM_ATTR_SRC_IP])
		src_ip = nla_get_u32(info->attrs[CM_ATTR_SRC_IP]);

	if (info->attrs[CM_ATTR_DST_IP])
		dst_ip = nla_get_u32(info->attrs[CM_ATTR_DST_IP]);

	if (info->attrs[CM_ATTR_SRC_IP6]) {
		src_ip6 = nla_data(info->attrs[CM_ATTR_SRC_IP6]);
		pr_debug("%s, src_ip6=%pI6c\n", __func__, src_ip6->s6_addr32);
	}

	if (info->attrs[CM_ATTR_DST_IP6]) {
		dst_ip6 = nla_data(info->attrs[CM_ATTR_DST_IP6]);
		pr_debug("%s, dst_ip6=%pI6c\n", __func__, dst_ip6->s6_addr32);
	}

	if (info->attrs[CM_ATTR_SRC_PORT])
		src_port = nla_get_u16(info->attrs[CM_ATTR_SRC_PORT]);

	if (info->attrs[CM_ATTR_DST_PORT])
		dst_port = nla_get_u16(info->attrs[CM_ATTR_DST_PORT]);

	if (info->attrs[CM_ATTR_PROTO])
		prot = nla_get_u8(info->attrs[CM_ATTR_PROTO]);

	if (info->attrs[CM_ATTR_SRC_MAC]) {
		memcpy(src_mac, nla_data(info->attrs[CM_ATTR_SRC_MAC]), ETH_ALEN);
		pr_debug("%s, src_mac: %02x%02x-%02x%02x-%02x%02x\n", __func__,
			src_mac[0], src_mac[1], src_mac[2], src_mac[3], src_mac[4], src_mac[5]);
	}
	if (info->attrs[CM_ATTR_DST_MAC]) {
		memcpy(dst_mac, nla_data(info->attrs[CM_ATTR_DST_MAC]), ETH_ALEN);
		pr_debug("%s, dst_mac: %02x%02x-%02x%02x-%02x%02x\n", __func__,
			dst_mac[0], dst_mac[1], dst_mac[2], dst_mac[3], dst_mac[4], dst_mac[5]);
	}

	if (info->attrs[CM_ATTR_SNAT])
		nat = nla_get_u8(info->attrs[CM_ATTR_SNAT]);

	if (info->attrs[CM_ATTR_FWD])
		fwd = nla_get_u8(info->attrs[CM_ATTR_FWD]);

	if (info->attrs[CM_ATTR_NAT_PORT])
		nat_port = nla_get_u16(info->attrs[CM_ATTR_NAT_PORT]);

	if (info->attrs[CM_ATTR_NAT_IP])
		nat_ip = nla_get_u32(info->attrs[CM_ATTR_NAT_IP]);

	if (info->attrs[CM_ATTR_DEVICE_NAME]) {
		/*nla_len = strlen(dev_name) + 1 + NLA_HDRLEN;*/
		memcpy(dev_name, (char *)nla_data(info->attrs[CM_ATTR_DEVICE_NAME]),
					info->attrs[CM_ATTR_DEVICE_NAME]->nla_len - NLA_HDRLEN -1);
		pr_debug("%s, dev_name: %s\n", __func__, dev_name);
	}

	if (info->attrs[CM_ATTR_MCID])
		mcid = nla_get_u8(info->attrs[CM_ATTR_MCID]);

	if (info->attrs[CM_ATTR_RBID])
		rbid = nla_get_u8(info->attrs[CM_ATTR_RBID]);

	if (info->attrs[CM_ATTR_QFI])
		qfi = nla_get_u8(info->attrs[CM_ATTR_QFI]);

	if (info->attrs[CM_ATTR_PDU])
		pdu = nla_get_u8(info->attrs[CM_ATTR_PDU]);

	if (info->attrs[CM_ATTR_IN_PKT])
		in_pkt = nla_get_u8(info->attrs[CM_ATTR_IN_PKT]);

	if (info->attrs[CM_ATTR_OUT_PKT])
		out_pkt = nla_get_u8(info->attrs[CM_ATTR_OUT_PKT]);

	if (info->attrs[CM_ATTR_VLAN_EN])
		vlan_en = nla_get_u8(info->attrs[CM_ATTR_VLAN_EN]);

	if (info->attrs[CM_ATTR_VLANID])
		vlanid = nla_get_u16(info->attrs[CM_ATTR_VLANID]);

	if (info->attrs[CM_ATTR_XLAT_EN])
		xlat_en = nla_get_u32(info->attrs[CM_ATTR_XLAT_EN]);

	if (info->attrs[CM_ATTR_XLAT_INSTANCE]) {
		memcpy(xlat_instance, (char *)nla_data(info->attrs[CM_ATTR_XLAT_INSTANCE]),
					info->attrs[CM_ATTR_XLAT_INSTANCE]->nla_len - NLA_HDRLEN -1);
		pr_debug("%s, xlat_instance: %s\n", __func__, xlat_instance);
	}

	if (info->attrs[CM_ATTR_UPDATE_TUPLE])
		update = nla_get_u8(info->attrs[CM_ATTR_UPDATE_TUPLE]);

	/* rx: cp -> ap, usb, wifi */
	if (in_pkt == PDU_PKT)
		rx_tx = 1;
	/* rx: ap -> usb, ap -> wifi */
	else if ((in_pkt == AP_PKT) && (out_pkt != PDU_PKT))
		rx_tx = 1;
	/*
	 * tx:
	 * ap -> cp
	 * usb/wifi -> ap/cp */
	else
		rx_tx = 0;

	if (src_ip6 && dst_ip6 && !xlat_en) {
		memcpy(toe_tuple.src_ip6, src_ip6->s6_addr32, sizeof(toe_tuple.src_ip6));
		memcpy(toe_tuple.dst_ip6, dst_ip6->s6_addr32, sizeof(toe_tuple.src_ip6));
		toe_tuple.ip6 = 1;
	} else {
		toe_tuple.src_ip = ntohl(src_ip);
		toe_tuple.dst_ip = ntohl(dst_ip);
		toe_tuple.ip6 = 0;
		toe_tuple.nat = nat;
		toe_tuple.nat_port = nat_port;
		toe_tuple.nat_ip = ntohl(nat_ip);
	}

	if (vlan_en) {
		toe_tuple.vlan_en = vlan_en;
		toe_tuple.vlanid = vlanid;
	}

	toe_tuple.src_port = src_port;
	toe_tuple.dst_port = dst_port;
	toe_tuple.prot = prot;
	toe_tuple.urg = 0;
	toe_tuple.fwd = fwd;
	toe_tuple.crc = 1;
	toe_tuple.rxtx = rx_tx;
	toe_tuple.out_pkt = out_pkt;
	toe_tuple.pdu = pdu;
	toe_tuple.qfi = qfi ;
	toe_tuple.rbid = rbid ;
	toe_tuple.mcid = mcid;
	toe_tuple.xlat_en = xlat_en;
	toe_tuple.in_pkt = in_pkt;
	memcpy(toe_tuple.smac, src_mac, sizeof(toe_tuple.smac));
	memcpy(toe_tuple.dmac, dst_mac, sizeof(toe_tuple.dmac));
	memcpy(toe_tuple.xlat_instance, xlat_instance, sizeof(xlat_instance));

	pr_debug("%s:\n in:%d, out:%d, src_port:%d, dst_prot:%d,"
			" protocol:%d, nat_port:%d, nat_ip:0x%x,fwd:%d,snat:%d, xlat instance:%s\n\n",
			__func__, in_pkt, out_pkt, src_port, dst_port, prot,
			nat_port, nat_ip, fwd, nat, xlat_instance);

	if (update) {
		memcpy(&toe_tuple_tmp, &toe_tuple, sizeof(struct toe_tuple_buff));
		toe_del_connection(&toe_tuple_tmp);
	}
	toe_add_connection(&toe_tuple);

	return 0;
}

static int fp_cm_genl_get_tuple(struct sk_buff *skb, struct genl_info *info)
{
	struct sk_buff *msg;
	int rc;

	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
	if (!msg)
		return -ENOMEM;

	rc = fp_cm_genl_fill_tuple_info_for_test(msg, info->snd_portid, info->snd_seq, 0, 1);
	if (rc < 0)
		goto out_free;

	return genlmsg_reply(msg, info);

out_free:
	nlmsg_free(msg);
	return rc;
}

static int fp_cm_genl_del_tuple(struct sk_buff *skb, struct genl_info *info)
{
	struct sk_buff *msg;
	int rc;

	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
	if (!msg)
		return -ENOMEM;

	rc = fp_cm_genl_fill_tuple_info_for_test(msg, info->snd_portid, info->snd_seq, 0, 0);
	if (rc < 0)
		goto out_free;

	return genlmsg_reply(msg, info);

out_free:
	nlmsg_free(msg);
	return rc;
}

static int __fp_eth_wan_set_tuple(struct nf_conntrack_tuple *tuple,
				struct fpdb_entry *el, int add)
{
	struct hh_cache *hh;
	int hh_len;

	struct fp_net_device *dst, *src;
	struct net_device *src_dev, *dst_dev;
	struct toe_tuple_buff toe_tuple_ul; /* eth -> cp */
	struct toe_tuple_buff toe_tuple_dl; /* cp->usb/wifi/eth */
	struct vlan_dev_priv *vlan;
	u8 src_vlan_en = 0, dst_vlan_en = 0;
	u16 src_vlanid, dst_vlanid;

	hh = &el->hh;
	hh_len = hh->hh_len;

	src = rcu_dereference_bh(el->in_dev);
	dst = rcu_dereference_bh(el->out_dev);

	if (is_vlan_dev(src->dev)) {
		vlan = vlan_dev_priv(src->dev);
		src_dev = vlan->real_dev;
		src_vlan_en = 1;
		src_vlanid = vlan->vlan_id;
	} else
		src_dev = src->dev;

	if (is_vlan_dev(dst->dev)) {
		vlan = vlan_dev_priv(dst->dev);
		dst_dev = vlan->real_dev;
		dst_vlan_en = 1;
		dst_vlanid = vlan->vlan_id;
	} else
		dst_dev = dst->dev;

	if (src->br) {
		/* if src dev is under bridge such as usb/eth(lan)/wifi */

		if (strncasecmp(dst_dev->name, "eth", 3))
			/* dst dev is not eth */
			return -1;
		else {
			if (!dst->br) {
				/* usb/eth(lan)/wifi -> eth(wan)
				 * don't add the ul path to toe,
				 * and no need to send tuple to cm.
				*/
				return 0;
			} else {
				/* dst is eth lan */
				return -1;
			}
		}
	} else {
		if (strncasecmp(src_dev->name, "eth", 3))
			/* src dev is not eth */
			return -1;
	}

	/* only eth wan as input go here */
	printk(KERN_DEBUG "%s: %s -> %s\n", __func__,
		src_dev->name, dst_dev->name);

	memset(&toe_tuple_ul, 0, sizeof(toe_tuple_ul));
	memset(&toe_tuple_dl, 0, sizeof(toe_tuple_dl));
	toe_tuple_ul.in_pkt = ETH_PKT;
	toe_tuple_ul.out_pkt = PDU_PKT;
	toe_tuple_dl.in_pkt = PDU_PKT;

	if (tuple->dst.protonum == IPPROTO_UDP)
		toe_tuple_ul.prot = TOE_UDP;
	else if (tuple->dst.protonum == IPPROTO_TCP)
		toe_tuple_ul.prot = TOE_TCP;
	else
		return 1;
	toe_tuple_dl.prot = toe_tuple_ul.prot;

	if (!strncasecmp(dst_dev->name, "usbnet", 6))
		toe_tuple_dl.out_pkt = USB_PKT;
	else if (!strncasecmp(dst_dev->name, "wlan", 4))
		toe_tuple_dl.out_pkt = WIFI_PKT;
	else if (!strncasecmp(dst_dev->name, "eth", 3))
		toe_tuple_dl.out_pkt = ETH_PKT;
	else
		return 2;

	if (tuple->src.l3num == AF_INET) {
		toe_tuple_ul.src_ip = ntohl(tuple->src.u3.ip);
		toe_tuple_ul.dst_ip = ntohl(tuple->dst.u3.ip);

		toe_tuple_ul.nat_ip = ntohl(tuple->dst.u3.ip);
		toe_tuple_ul.nat_port = ntohs(el->out_tuple.dst.u.all);
		toe_tuple_ul.nat = 0;

		toe_tuple_dl.src_ip = toe_tuple_ul.src_ip;
		toe_tuple_dl.dst_ip = toe_tuple_ul.dst_ip;
		toe_tuple_dl.nat_ip = ntohl(el->out_tuple.src.u3.ip);
		toe_tuple_dl.nat_port = ntohs(el->out_tuple.src.u.all);
		toe_tuple_dl.nat = 1;
	} else if (tuple->src.l3num == AF_INET6) {
		memcpy(toe_tuple_ul.src_ip6,
			tuple->src.u3.in6.s6_addr32, sizeof(toe_tuple_ul.src_ip6));
		memcpy(toe_tuple_ul.dst_ip6,
			tuple->dst.u3.in6.s6_addr32, sizeof(toe_tuple_ul.dst_ip6));
		toe_tuple_dl.ip6 = toe_tuple_ul.ip6 = 1;
	}

	toe_tuple_dl.src_port = toe_tuple_ul.src_port = ntohs(tuple->src.u.all);
	toe_tuple_dl.dst_port = toe_tuple_ul.dst_port = ntohs(tuple->dst.u.all);
	toe_tuple_dl.crc = toe_tuple_ul.crc = 1;
	toe_tuple_dl.fwd = toe_tuple_ul.fwd = 1;
	toe_tuple_ul.rxtx = 0; /* ul is tx */
	toe_tuple_dl.rxtx = 1; /* dl is rx */
	toe_tuple_ul.pdu = 0xff;
	toe_tuple_ul.qfi = 0xff;
	toe_tuple_ul.rbid = 0xff;
	toe_tuple_ul.mcid = 0xff;

	if (src_vlan_en) {
		toe_tuple_ul.vlan_en = 1;
		toe_tuple_ul.vlanid = src_vlanid;
	}

	if (dst_vlan_en) {
		toe_tuple_dl.vlan_en = 1;
		toe_tuple_dl.vlanid = dst_vlanid;
	}

	if (hh_len) {
		if (likely(hh_len <= HH_DATA_MOD)) {
			/* this is inlined by gcc */
			char mac_header[HH_DATA_MOD];
			memcpy(mac_header, hh->hh_data, HH_DATA_MOD);
			memcpy(toe_tuple_ul.smac,
				&mac_header[HH_DATA_MOD-ETH_TYPE_LEN-ETH_ALEN], ETH_ALEN);
			memcpy(toe_tuple_ul.dmac,
				&mac_header[HH_DATA_MOD-ETH_TYPE_LEN-ETH_ALEN*2], ETH_ALEN);
		} else {
			int hh_alen = HH_DATA_ALIGN(hh_len);
			char *mac_header = kmalloc(hh_alen, GFP_ATOMIC);

			memcpy(mac_header, hh->hh_data, hh_alen);
			memcpy(toe_tuple_ul.smac,
				mac_header+(hh_alen-ETH_TYPE_LEN-ETH_ALEN), ETH_ALEN);
			memcpy(toe_tuple_ul.dmac,
				mac_header+(hh_alen-ETH_TYPE_LEN-ETH_ALEN*2), ETH_ALEN);
			kfree(mac_header);
		}

		memcpy(toe_tuple_dl.smac, toe_tuple_ul.smac, ETH_ALEN);
		memcpy(toe_tuple_dl.dmac, toe_tuple_ul.dmac, ETH_ALEN);
	}

	if (add) {
		if (toe_add_connection(&toe_tuple_ul) >= 0 &&
			toe_add_connection(&toe_tuple_dl) >= 0) {
			el->nl_flag = 1;
			return 0;
		}
	} else {
		toe_del_connection(&toe_tuple_ul);
		toe_del_connection(&toe_tuple_dl);
		el->nl_flag = 0;
		return 0;
	}

	return -1;
}

int fp_cm_genl_send_tuple(struct nf_conntrack_tuple *tuple, struct fpdb_entry *el,
									 int add, int len)
{
	struct sk_buff *msg;
	int rc;
	u32 pid, ms;

	if (add) {
		if (!el->detect_speed_jiffies)
			el->detect_speed_jiffies = jiffies;
		el->detect_speed_bytes += len;
		ms = jiffies_to_msecs(jiffies - el->detect_speed_jiffies);
		if (ms >= 1000) {
			el->speed = (el->detect_speed_bytes / ms) << 3; /* kbps */
			el->detect_speed_jiffies = 0;
			el->detect_speed_bytes = 0;
		}

		if (el->speed < speed_thresh)
			return 0;
	}

	rc = __fp_eth_wan_set_tuple(tuple, el, add);
	if (rc >= 0)
		return rc;

	pid = fp_cm_get_genl_pid();
	if (pid == -1)
		return -1;

	msg = nlmsg_new(FP_CM_NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
	if (!msg)
		return -ENOMEM;

	rc = fp_cm_genl_fill_tuple_info(msg, tuple, el, pid, add);
	if (rc < 0)
		goto out_free;

	rc = genlmsg_unicast(&init_net, msg, pid);
	if (rc) {
		pr_err_ratelimited("%s genlmsg_unicast fail, rc: %d", __func__, rc);
		el->nl_flag = 0;
	}

	return rc;
out_free:
	nlmsg_free(msg);
	return rc;
}

EXPORT_SYMBOL(fp_cm_genl_send_tuple);

static ssize_t speed_thresh_show(struct fastpath_module *m, char *buf)
{
	return sprintf(buf, "speed_thresh: %dKbps\n", speed_thresh);
}

static ssize_t speed_thresh_store(struct fastpath_module *m, const char *buf,
			      size_t count)
{
	sscanf(buf, "%u", &speed_thresh);
	return count;
}

static FP_ATTR(speed_thresh, S_IRUGO|S_IWUSR, speed_thresh_show, speed_thresh_store);

static struct attribute *fp_cm_attrs[] = {
	&fp_attr_speed_thresh.attr,
	NULL, /* need to NULL terminate the list of attributes */
};

void fp_cm_release(struct kobject *kobj)
{
	struct fastpath_module *module = to_fpmod(kobj);
#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 12, 0)
			int i;
#endif

#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 12, 0)
	for (i = ARRAY_SIZE(fp_cm_genl_ops) - 1; i >= 0; i--)
		genl_unregister_ops(&fp_cm_genl_family, &fp_cm_genl_ops[i]);
#endif
		genl_unregister_family(&fp_cm_genl_family);

		printk(KERN_ERR "fp_cm gennetlink unregister.....\n");
		kfree(module);
}

static struct kobj_type ktype_fp_cm = {
	.sysfs_ops	= &fp_sysfs_ops,
	.default_attrs	= fp_cm_attrs,
	.release	= fp_cm_release,
};

static int fp_cm_probe(struct fastpath_module *module)
{
	int ret;

	module->priv = NULL;
	snprintf(module->name, sizeof(module->name), "fp_cm");

	ret = genl_register_family(&fp_cm_genl_family);

	kobject_init(&module->kobj, &ktype_fp_cm);
	ret = kobject_add(&module->kobj, module->fastpath->kobj, "%s", module->name);
	if (ret < 0) {
		pr_err("kobject_add failed (%d)\n", ret);
		goto err_kobject_add;
	}

	kobject_uevent(&module->kobj, KOBJ_ADD);

	pr_debug("fp_cm probed\n");
	pr_info("fp_cm gennetlink register success!!!\n");

	return 0;

err_kobject_add:
	kobject_put(&module->kobj);
	genl_unregister_family(&fp_cm_genl_family);

	return ret;
}

static int fp_cm_remove(struct fastpath_module *module)
{
	kobject_put(&module->kobj);

	printk(KERN_ERR "fp_cm removed\n");
	return 0;
}

struct fastpath_module_ops fp_cm_ops = {
	.probe = fp_cm_probe,
	.remove = fp_cm_remove,
};