[linux-block.git] / net / bridge / br_cfm.c

// SPDX-License-Identifier: GPL-2.0-or-later

#include <linux/cfm_bridge.h>
#include <uapi/linux/cfm_bridge.h>
#include "br_private_cfm.h"

static struct br_cfm_mep *br_mep_find(struct net_bridge *br, u32 instance)
{
	struct br_cfm_mep *mep;

	hlist_for_each_entry(mep, &br->mep_list, head)
		if (mep->instance == instance)
			return mep;

	return NULL;
}

static struct br_cfm_mep *br_mep_find_ifindex(struct net_bridge *br,
					      u32 ifindex)
{
	struct br_cfm_mep *mep;

	hlist_for_each_entry_rcu(mep, &br->mep_list, head,
				 lockdep_rtnl_is_held())
		if (mep->create.ifindex == ifindex)
			return mep;

	return NULL;
}

static struct br_cfm_peer_mep *br_peer_mep_find(struct br_cfm_mep *mep,
						u32 mepid)
{
	struct br_cfm_peer_mep *peer_mep;

	hlist_for_each_entry_rcu(peer_mep, &mep->peer_mep_list, head,
				 lockdep_rtnl_is_held())
		if (peer_mep->mepid == mepid)
			return peer_mep;

	return NULL;
}

static struct net_bridge_port *br_mep_get_port(struct net_bridge *br,
					       u32 ifindex)
{
	struct net_bridge_port *port;

	list_for_each_entry(port, &br->port_list, list)
		if (port->dev->ifindex == ifindex)
			return port;

	return NULL;
}

/* Calculate the CCM interval in us. */
static u32 interval_to_us(enum br_cfm_ccm_interval interval)
{
	switch (interval) {
	case BR_CFM_CCM_INTERVAL_NONE:
		return 0;
	case BR_CFM_CCM_INTERVAL_3_3_MS:
		return 3300;
	case BR_CFM_CCM_INTERVAL_10_MS:
		return 10 * 1000;
	case BR_CFM_CCM_INTERVAL_100_MS:
		return 100 * 1000;
	case BR_CFM_CCM_INTERVAL_1_SEC:
		return 1000 * 1000;
	case BR_CFM_CCM_INTERVAL_10_SEC:
		return 10 * 1000 * 1000;
	case BR_CFM_CCM_INTERVAL_1_MIN:
		return 60 * 1000 * 1000;
	case BR_CFM_CCM_INTERVAL_10_MIN:
		return 10 * 60 * 1000 * 1000;
	}
	return 0;
}

/* Convert the interface interval to CCM PDU value. */
static u32 interval_to_pdu(enum br_cfm_ccm_interval interval)
{
	switch (interval) {
	case BR_CFM_CCM_INTERVAL_NONE:
		return 0;
	case BR_CFM_CCM_INTERVAL_3_3_MS:
		return 1;
	case BR_CFM_CCM_INTERVAL_10_MS:
		return 2;
	case BR_CFM_CCM_INTERVAL_100_MS:
		return 3;
	case BR_CFM_CCM_INTERVAL_1_SEC:
		return 4;
	case BR_CFM_CCM_INTERVAL_10_SEC:
		return 5;
	case BR_CFM_CCM_INTERVAL_1_MIN:
		return 6;
	case BR_CFM_CCM_INTERVAL_10_MIN:
		return 7;
	}
	return 0;
}

static struct sk_buff *ccm_frame_build(struct br_cfm_mep *mep,
				       const struct br_cfm_cc_ccm_tx_info *const tx_info)

{
	struct br_cfm_common_hdr *common_hdr;
	struct net_bridge_port *b_port;
	struct br_cfm_maid *maid;
	u8 *itu_reserved, *e_tlv;
	struct ethhdr *eth_hdr;
	struct sk_buff *skb;
	__be32 *status_tlv;
	__be32 *snumber;
	__be16 *mepid;

	skb = dev_alloc_skb(CFM_CCM_MAX_FRAME_LENGTH);
	if (!skb)
		return NULL;

	rcu_read_lock();
	b_port = rcu_dereference(mep->b_port);
	if (!b_port) {
		kfree_skb(skb);
		rcu_read_unlock();
		return NULL;
	}
	skb->dev = b_port->dev;
	rcu_read_unlock();
	/* The device cannot be deleted until the work_queue functions has
	 * completed. This function is called from ccm_tx_work_expired()
	 * that is a work_queue functions.
	 */

	skb->protocol = htons(ETH_P_CFM);
	skb->priority = CFM_FRAME_PRIO;

	/* Ethernet header */
	eth_hdr = skb_put(skb, sizeof(*eth_hdr));
	ether_addr_copy(eth_hdr->h_dest, tx_info->dmac.addr);
	ether_addr_copy(eth_hdr->h_source, mep->config.unicast_mac.addr);
	eth_hdr->h_proto = htons(ETH_P_CFM);

	/* Common CFM Header */
	common_hdr = skb_put(skb, sizeof(*common_hdr));
	common_hdr->mdlevel_version = mep->config.mdlevel << 5;
	common_hdr->opcode = BR_CFM_OPCODE_CCM;
	common_hdr->flags = (mep->rdi << 7) |
			    interval_to_pdu(mep->cc_config.exp_interval);
	common_hdr->tlv_offset = CFM_CCM_TLV_OFFSET;

	/* Sequence number */
	snumber = skb_put(skb, sizeof(*snumber));
	if (tx_info->seq_no_update) {
		*snumber = cpu_to_be32(mep->ccm_tx_snumber);
		mep->ccm_tx_snumber += 1;
	} else {
		*snumber = 0;
	}

	mepid = skb_put(skb, sizeof(*mepid));
	*mepid = cpu_to_be16((u16)mep->config.mepid);

	maid = skb_put(skb, sizeof(*maid));
	memcpy(maid->data, mep->cc_config.exp_maid.data, sizeof(maid->data));

	/* ITU reserved (CFM_CCM_ITU_RESERVED_SIZE octets) */
	itu_reserved = skb_put(skb, CFM_CCM_ITU_RESERVED_SIZE);
	memset(itu_reserved, 0, CFM_CCM_ITU_RESERVED_SIZE);

	/* Generel CFM TLV format:
	 * TLV type:		one byte
	 * TLV value length:	two bytes
	 * TLV value:		'TLV value length' bytes
	 */

	/* Port status TLV. The value length is 1. Total of 4 bytes. */
	if (tx_info->port_tlv) {
		status_tlv = skb_put(skb, sizeof(*status_tlv));
		*status_tlv = cpu_to_be32((CFM_PORT_STATUS_TLV_TYPE << 24) |
					  (1 << 8) |	/* Value length */
					  (tx_info->port_tlv_value & 0xFF));
	}

	/* Interface status TLV. The value length is 1. Total of 4 bytes. */
	if (tx_info->if_tlv) {
		status_tlv = skb_put(skb, sizeof(*status_tlv));
		*status_tlv = cpu_to_be32((CFM_IF_STATUS_TLV_TYPE << 24) |
					  (1 << 8) |	/* Value length */
					  (tx_info->if_tlv_value & 0xFF));
	}

	/* End TLV */
	e_tlv = skb_put(skb, sizeof(*e_tlv));
	*e_tlv = CFM_ENDE_TLV_TYPE;

	return skb;
}

static void ccm_frame_tx(struct sk_buff *skb)
{
	skb_reset_network_header(skb);
	dev_queue_xmit(skb);
}

/* This function is called with the configured CC 'expected_interval'
 * in order to drive CCM transmission when enabled.
 */
static void ccm_tx_work_expired(struct work_struct *work)
{
	struct delayed_work *del_work;
	struct br_cfm_mep *mep;
	struct sk_buff *skb;
	u32 interval_us;

	del_work = to_delayed_work(work);
	mep = container_of(del_work, struct br_cfm_mep, ccm_tx_dwork);

	if (time_before_eq(mep->ccm_tx_end, jiffies)) {
		/* Transmission period has ended */
		mep->cc_ccm_tx_info.period = 0;
		return;
	}

	skb = ccm_frame_build(mep, &mep->cc_ccm_tx_info);
	if (skb)
		ccm_frame_tx(skb);

	interval_us = interval_to_us(mep->cc_config.exp_interval);
	queue_delayed_work(system_wq, &mep->ccm_tx_dwork,
			   usecs_to_jiffies(interval_us));
}

int br_cfm_mep_create(struct net_bridge *br,
		      const u32 instance,
		      struct br_cfm_mep_create *const create,
		      struct netlink_ext_ack *extack)
{
	struct net_bridge_port *p;
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	if (create->domain == BR_CFM_VLAN) {
		NL_SET_ERR_MSG_MOD(extack,
				   "VLAN domain not supported");
		return -EINVAL;
	}
	if (create->domain != BR_CFM_PORT) {
		NL_SET_ERR_MSG_MOD(extack,
				   "Invalid domain value");
		return -EINVAL;
	}
	if (create->direction == BR_CFM_MEP_DIRECTION_UP) {
		NL_SET_ERR_MSG_MOD(extack,
				   "Up-MEP not supported");
		return -EINVAL;
	}
	if (create->direction != BR_CFM_MEP_DIRECTION_DOWN) {
		NL_SET_ERR_MSG_MOD(extack,
				   "Invalid direction value");
		return -EINVAL;
	}
	p = br_mep_get_port(br, create->ifindex);
	if (!p) {
		NL_SET_ERR_MSG_MOD(extack,
				   "Port is not related to bridge");
		return -EINVAL;
	}
	mep = br_mep_find(br, instance);
	if (mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "MEP instance already exists");
		return -EEXIST;
	}

	/* In PORT domain only one instance can be created per port */
	if (create->domain == BR_CFM_PORT) {
		mep = br_mep_find_ifindex(br, create->ifindex);
		if (mep) {
			NL_SET_ERR_MSG_MOD(extack,
					   "Only one Port MEP on a port allowed");
			return -EINVAL;
		}
	}

	mep = kzalloc(sizeof(*mep), GFP_KERNEL);
	if (!mep)
		return -ENOMEM;

	mep->create = *create;
	mep->instance = instance;
	rcu_assign_pointer(mep->b_port, p);

	INIT_HLIST_HEAD(&mep->peer_mep_list);
	INIT_DELAYED_WORK(&mep->ccm_tx_dwork, ccm_tx_work_expired);

	hlist_add_tail_rcu(&mep->head, &br->mep_list);

	return 0;
}

static void mep_delete_implementation(struct net_bridge *br,
				      struct br_cfm_mep *mep)
{
	struct br_cfm_peer_mep *peer_mep;
	struct hlist_node *n_store;

	ASSERT_RTNL();

	/* Empty and free peer MEP list */
	hlist_for_each_entry_safe(peer_mep, n_store, &mep->peer_mep_list, head) {
		hlist_del_rcu(&peer_mep->head);
		kfree_rcu(peer_mep, rcu);
	}

	cancel_delayed_work_sync(&mep->ccm_tx_dwork);

	RCU_INIT_POINTER(mep->b_port, NULL);
	hlist_del_rcu(&mep->head);
	kfree_rcu(mep, rcu);
}

int br_cfm_mep_delete(struct net_bridge *br,
		      const u32 instance,
		      struct netlink_ext_ack *extack)
{
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	mep = br_mep_find(br, instance);
	if (!mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "MEP instance does not exists");
		return -ENOENT;
	}

	mep_delete_implementation(br, mep);

	return 0;
}

int br_cfm_mep_config_set(struct net_bridge *br,
			  const u32 instance,
			  const struct br_cfm_mep_config *const config,
			  struct netlink_ext_ack *extack)
{
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	mep = br_mep_find(br, instance);
	if (!mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "MEP instance does not exists");
		return -ENOENT;
	}

	mep->config = *config;

	return 0;
}

int br_cfm_cc_config_set(struct net_bridge *br,
			 const u32 instance,
			 const struct br_cfm_cc_config *const config,
			 struct netlink_ext_ack *extack)
{
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	mep = br_mep_find(br, instance);
	if (!mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "MEP instance does not exists");
		return -ENOENT;
	}

	/* Check for no change in configuration */
	if (memcmp(config, &mep->cc_config, sizeof(*config)) == 0)
		return 0;

	mep->cc_config = *config;
	mep->ccm_tx_snumber = 1;

	return 0;
}

int br_cfm_cc_peer_mep_add(struct net_bridge *br, const u32 instance,
			   u32 mepid,
			   struct netlink_ext_ack *extack)
{
	struct br_cfm_peer_mep *peer_mep;
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	mep = br_mep_find(br, instance);
	if (!mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "MEP instance does not exists");
		return -ENOENT;
	}

	peer_mep = br_peer_mep_find(mep, mepid);
	if (peer_mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "Peer MEP-ID already exists");
		return -EEXIST;
	}

	peer_mep = kzalloc(sizeof(*peer_mep), GFP_KERNEL);
	if (!peer_mep)
		return -ENOMEM;

	peer_mep->mepid = mepid;
	peer_mep->mep = mep;

	hlist_add_tail_rcu(&peer_mep->head, &mep->peer_mep_list);

	return 0;
}

int br_cfm_cc_peer_mep_remove(struct net_bridge *br, const u32 instance,
			      u32 mepid,
			      struct netlink_ext_ack *extack)
{
	struct br_cfm_peer_mep *peer_mep;
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	mep = br_mep_find(br, instance);
	if (!mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "MEP instance does not exists");
		return -ENOENT;
	}

	peer_mep = br_peer_mep_find(mep, mepid);
	if (!peer_mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "Peer MEP-ID does not exists");
		return -ENOENT;
	}

	hlist_del_rcu(&peer_mep->head);
	kfree_rcu(peer_mep, rcu);

	return 0;
}

int br_cfm_cc_rdi_set(struct net_bridge *br, const u32 instance,
		      const bool rdi, struct netlink_ext_ack *extack)
{
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	mep = br_mep_find(br, instance);
	if (!mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "MEP instance does not exists");
		return -ENOENT;
	}

	mep->rdi = rdi;

	return 0;
}

int br_cfm_cc_ccm_tx(struct net_bridge *br, const u32 instance,
		     const struct br_cfm_cc_ccm_tx_info *const tx_info,
		     struct netlink_ext_ack *extack)
{
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	mep = br_mep_find(br, instance);
	if (!mep) {
		NL_SET_ERR_MSG_MOD(extack,
				   "MEP instance does not exists");
		return -ENOENT;
	}

	if (memcmp(tx_info, &mep->cc_ccm_tx_info, sizeof(*tx_info)) == 0) {
		/* No change in tx_info. */
		if (mep->cc_ccm_tx_info.period == 0)
			/* Transmission is not enabled - just return */
			return 0;

		/* Transmission is ongoing, the end time is recalculated */
		mep->ccm_tx_end = jiffies +
				  usecs_to_jiffies(tx_info->period * 1000000);
		return 0;
	}

	if (tx_info->period == 0 && mep->cc_ccm_tx_info.period == 0)
		/* Some change in info and transmission is not ongoing */
		goto save;

	if (tx_info->period != 0 && mep->cc_ccm_tx_info.period != 0) {
		/* Some change in info and transmission is ongoing
		 * The end time is recalculated
		 */
		mep->ccm_tx_end = jiffies +
				  usecs_to_jiffies(tx_info->period * 1000000);

		goto save;
	}

	if (tx_info->period == 0 && mep->cc_ccm_tx_info.period != 0) {
		cancel_delayed_work_sync(&mep->ccm_tx_dwork);
		goto save;
	}

	/* Start delayed work to transmit CCM frames. It is done with zero delay
	 * to send first frame immediately
	 */
	mep->ccm_tx_end = jiffies + usecs_to_jiffies(tx_info->period * 1000000);
	queue_delayed_work(system_wq, &mep->ccm_tx_dwork, 0);

save:
	mep->cc_ccm_tx_info = *tx_info;

	return 0;
}

/* Deletes the CFM instances on a specific bridge port
 */
void br_cfm_port_del(struct net_bridge *br, struct net_bridge_port *port)
{
	struct hlist_node *n_store;
	struct br_cfm_mep *mep;

	ASSERT_RTNL();

	hlist_for_each_entry_safe(mep, n_store, &br->mep_list, head)
		if (mep->create.ifindex == port->dev->ifindex)
			mep_delete_implementation(br, mep);
}
Commit	Line	Data
86a14b79 HB	1	// SPDX-License-Identifier: GPL-2.0-or-later
	2
	3	#include <linux/cfm_bridge.h>
	4	#include <uapi/linux/cfm_bridge.h>
	5	#include "br_private_cfm.h"
	6
	7	static struct br_cfm_mep br_mep_find(struct net_bridge br, u32 instance)
	8	{
	9	struct br_cfm_mep *mep;
	10
	11	hlist_for_each_entry(mep, &br->mep_list, head)
	12	if (mep->instance == instance)
	13	return mep;
	14
	15	return NULL;
	16	}
	17
	18	static struct br_cfm_mep br_mep_find_ifindex(struct net_bridge br,
	19	u32 ifindex)
	20	{
	21	struct br_cfm_mep *mep;
	22
	23	hlist_for_each_entry_rcu(mep, &br->mep_list, head,
	24	lockdep_rtnl_is_held())
	25	if (mep->create.ifindex == ifindex)
	26	return mep;
	27
	28	return NULL;
	29	}
	30
	31	static struct br_cfm_peer_mep br_peer_mep_find(struct br_cfm_mep mep,
	32	u32 mepid)
	33	{
	34	struct br_cfm_peer_mep *peer_mep;
	35
	36	hlist_for_each_entry_rcu(peer_mep, &mep->peer_mep_list, head,
	37	lockdep_rtnl_is_held())
	38	if (peer_mep->mepid == mepid)
	39	return peer_mep;
	40
	41	return NULL;
	42	}
	43
	44	static struct net_bridge_port br_mep_get_port(struct net_bridge br,
	45	u32 ifindex)
	46	{
	47	struct net_bridge_port *port;
	48
	49	list_for_each_entry(port, &br->port_list, list)
	50	if (port->dev->ifindex == ifindex)
	51	return port;
	52
	53	return NULL;
	54	}
	55
a806ad8e HB	56	/* Calculate the CCM interval in us. */
	57	static u32 interval_to_us(enum br_cfm_ccm_interval interval)
	58	{
	59	switch (interval) {
	60	case BR_CFM_CCM_INTERVAL_NONE:
	61	return 0;
	62	case BR_CFM_CCM_INTERVAL_3_3_MS:
	63	return 3300;
	64	case BR_CFM_CCM_INTERVAL_10_MS:
	65	return 10 * 1000;
	66	case BR_CFM_CCM_INTERVAL_100_MS:
	67	return 100 * 1000;
	68	case BR_CFM_CCM_INTERVAL_1_SEC:
	69	return 1000 * 1000;
	70	case BR_CFM_CCM_INTERVAL_10_SEC:
	71	return 10 * 1000 * 1000;
	72	case BR_CFM_CCM_INTERVAL_1_MIN:
	73	return 60 * 1000 * 1000;
	74	case BR_CFM_CCM_INTERVAL_10_MIN:
	75	return 10 * 60 * 1000 * 1000;
	76	}
	77	return 0;
	78	}
	79
	80	/* Convert the interface interval to CCM PDU value. */
	81	static u32 interval_to_pdu(enum br_cfm_ccm_interval interval)
	82	{
	83	switch (interval) {
	84	case BR_CFM_CCM_INTERVAL_NONE:
	85	return 0;
	86	case BR_CFM_CCM_INTERVAL_3_3_MS:
	87	return 1;
	88	case BR_CFM_CCM_INTERVAL_10_MS:
	89	return 2;
	90	case BR_CFM_CCM_INTERVAL_100_MS:
	91	return 3;
	92	case BR_CFM_CCM_INTERVAL_1_SEC:
	93	return 4;
	94	case BR_CFM_CCM_INTERVAL_10_SEC:
	95	return 5;
	96	case BR_CFM_CCM_INTERVAL_1_MIN:
	97	return 6;
	98	case BR_CFM_CCM_INTERVAL_10_MIN:
	99	return 7;
	100	}
	101	return 0;
	102	}
	103
	104	static struct sk_buff ccm_frame_build(struct br_cfm_mep mep,
	105	const struct br_cfm_cc_ccm_tx_info *const tx_info)
	106
	107	{
	108	struct br_cfm_common_hdr *common_hdr;
	109	struct net_bridge_port *b_port;
	110	struct br_cfm_maid *maid;
	111	u8 itu_reserved, e_tlv;
	112	struct ethhdr *eth_hdr;
	113	struct sk_buff *skb;
	114	__be32 *status_tlv;
	115	__be32 *snumber;
	116	__be16 *mepid;
	117
	118	skb = dev_alloc_skb(CFM_CCM_MAX_FRAME_LENGTH);
	119	if (!skb)
120	return NULL;
121
122	rcu_read_lock();
123	b_port = rcu_dereference(mep->b_port);
124	if (!b_port) {
125	kfree_skb(skb);
126	rcu_read_unlock();
127	return NULL;
128	}
129	skb->dev = b_port->dev;
130	rcu_read_unlock();
131	/* The device cannot be deleted until the work_queue functions has
132	* completed. This function is called from ccm_tx_work_expired()
133	* that is a work_queue functions.
134	*/
135
136	skb->protocol = htons(ETH_P_CFM);
137	skb->priority = CFM_FRAME_PRIO;
138
139	/* Ethernet header */
140	eth_hdr = skb_put(skb, sizeof(*eth_hdr));
141	ether_addr_copy(eth_hdr->h_dest, tx_info->dmac.addr);
142	ether_addr_copy(eth_hdr->h_source, mep->config.unicast_mac.addr);
143	eth_hdr->h_proto = htons(ETH_P_CFM);
144
145	/* Common CFM Header */
146	common_hdr = skb_put(skb, sizeof(*common_hdr));
147	common_hdr->mdlevel_version = mep->config.mdlevel << 5;
148	common_hdr->opcode = BR_CFM_OPCODE_CCM;
149	common_hdr->flags = (mep->rdi << 7) \|
150	interval_to_pdu(mep->cc_config.exp_interval);
151	common_hdr->tlv_offset = CFM_CCM_TLV_OFFSET;
152
153	/* Sequence number */
154	snumber = skb_put(skb, sizeof(*snumber));
155	if (tx_info->seq_no_update) {
156	*snumber = cpu_to_be32(mep->ccm_tx_snumber);
157	mep->ccm_tx_snumber += 1;
158	} else {
159	*snumber = 0;
160	}
161
162	mepid = skb_put(skb, sizeof(*mepid));
163	*mepid = cpu_to_be16((u16)mep->config.mepid);
164
165	maid = skb_put(skb, sizeof(*maid));
166	memcpy(maid->data, mep->cc_config.exp_maid.data, sizeof(maid->data));
167
168	/* ITU reserved (CFM_CCM_ITU_RESERVED_SIZE octets) */
169	itu_reserved = skb_put(skb, CFM_CCM_ITU_RESERVED_SIZE);
170	memset(itu_reserved, 0, CFM_CCM_ITU_RESERVED_SIZE);
171
172	/* Generel CFM TLV format:
173	* TLV type: one byte
174	* TLV value length: two bytes
175	* TLV value: 'TLV value length' bytes
176	*/
177
178	/* Port status TLV. The value length is 1. Total of 4 bytes. */
179	if (tx_info->port_tlv) {
180	status_tlv = skb_put(skb, sizeof(*status_tlv));
181	*status_tlv = cpu_to_be32((CFM_PORT_STATUS_TLV_TYPE << 24) \|
182	(1 << 8) \| /* Value length */
183	(tx_info->port_tlv_value & 0xFF));
184	}
185
186	/* Interface status TLV. The value length is 1. Total of 4 bytes. */
187	if (tx_info->if_tlv) {
188	status_tlv = skb_put(skb, sizeof(*status_tlv));
189	*status_tlv = cpu_to_be32((CFM_IF_STATUS_TLV_TYPE << 24) \|
190	(1 << 8) \| /* Value length */
191	(tx_info->if_tlv_value & 0xFF));
192	}
193
194	/* End TLV */
195	e_tlv = skb_put(skb, sizeof(*e_tlv));
196	*e_tlv = CFM_ENDE_TLV_TYPE;
197
198	return skb;
199	}
200
201	static void ccm_frame_tx(struct sk_buff *skb)
202	{
203	skb_reset_network_header(skb);
204	dev_queue_xmit(skb);
205	}
206
207	/* This function is called with the configured CC 'expected_interval'
208	* in order to drive CCM transmission when enabled.
209	*/
210	static void ccm_tx_work_expired(struct work_struct *work)
211	{
212	struct delayed_work *del_work;
213	struct br_cfm_mep *mep;
214	struct sk_buff *skb;
215	u32 interval_us;
216
217	del_work = to_delayed_work(work);
218	mep = container_of(del_work, struct br_cfm_mep, ccm_tx_dwork);
219
220	if (time_before_eq(mep->ccm_tx_end, jiffies)) {
221	/* Transmission period has ended */
222	mep->cc_ccm_tx_info.period = 0;
223	return;
224	}
225
226	skb = ccm_frame_build(mep, &mep->cc_ccm_tx_info);
227	if (skb)
228	ccm_frame_tx(skb);
229
230	interval_us = interval_to_us(mep->cc_config.exp_interval);
231	queue_delayed_work(system_wq, &mep->ccm_tx_dwork,
232	usecs_to_jiffies(interval_us));
233	}
234
86a14b79 HB	235	int br_cfm_mep_create(struct net_bridge *br,
	236	const u32 instance,
	237	struct br_cfm_mep_create *const create,
	238	struct netlink_ext_ack *extack)
	239	{
	240	struct net_bridge_port *p;
	241	struct br_cfm_mep *mep;
	242
	243	ASSERT_RTNL();
	244
	245	if (create->domain == BR_CFM_VLAN) {
	246	NL_SET_ERR_MSG_MOD(extack,
	247	"VLAN domain not supported");
	248	return -EINVAL;
	249	}
	250	if (create->domain != BR_CFM_PORT) {
	251	NL_SET_ERR_MSG_MOD(extack,
	252	"Invalid domain value");
	253	return -EINVAL;
	254	}
	255	if (create->direction == BR_CFM_MEP_DIRECTION_UP) {
	256	NL_SET_ERR_MSG_MOD(extack,
	257	"Up-MEP not supported");
	258	return -EINVAL;
	259	}
	260	if (create->direction != BR_CFM_MEP_DIRECTION_DOWN) {
	261	NL_SET_ERR_MSG_MOD(extack,
	262	"Invalid direction value");
	263	return -EINVAL;
	264	}
	265	p = br_mep_get_port(br, create->ifindex);
	266	if (!p) {
	267	NL_SET_ERR_MSG_MOD(extack,
	268	"Port is not related to bridge");
	269	return -EINVAL;
	270	}
	271	mep = br_mep_find(br, instance);
	272	if (mep) {
	273	NL_SET_ERR_MSG_MOD(extack,
	274	"MEP instance already exists");
	275	return -EEXIST;
	276	}
	277
	278	/* In PORT domain only one instance can be created per port */
	279	if (create->domain == BR_CFM_PORT) {
	280	mep = br_mep_find_ifindex(br, create->ifindex);
	281	if (mep) {
	282	NL_SET_ERR_MSG_MOD(extack,
	283	"Only one Port MEP on a port allowed");
	284	return -EINVAL;
	285	}
	286	}
	287
	288	mep = kzalloc(sizeof(*mep), GFP_KERNEL);
	289	if (!mep)
	290	return -ENOMEM;
	291
	292	mep->create = *create;
	293	mep->instance = instance;
	294	rcu_assign_pointer(mep->b_port, p);
	295
	296	INIT_HLIST_HEAD(&mep->peer_mep_list);
a806ad8e	297	INIT_DELAYED_WORK(&mep->ccm_tx_dwork, ccm_tx_work_expired);
86a14b79 HB	298
	299	hlist_add_tail_rcu(&mep->head, &br->mep_list);
	300
	301	return 0;
	302	}
	303
	304	static void mep_delete_implementation(struct net_bridge *br,
	305	struct br_cfm_mep *mep)
	306	{
	307	struct br_cfm_peer_mep *peer_mep;
	308	struct hlist_node *n_store;
	309
	310	ASSERT_RTNL();
	311
	312	/* Empty and free peer MEP list */
	313	hlist_for_each_entry_safe(peer_mep, n_store, &mep->peer_mep_list, head) {
	314	hlist_del_rcu(&peer_mep->head);
	315	kfree_rcu(peer_mep, rcu);
	316	}
	317
a806ad8e HB	318	cancel_delayed_work_sync(&mep->ccm_tx_dwork);
a806ad8e HB	319
86a14b79 HB	320	RCU_INIT_POINTER(mep->b_port, NULL);
	321	hlist_del_rcu(&mep->head);
	322	kfree_rcu(mep, rcu);
	323	}
	324
	325	int br_cfm_mep_delete(struct net_bridge *br,
	326	const u32 instance,
	327	struct netlink_ext_ack *extack)
	328	{
	329	struct br_cfm_mep *mep;
	330
	331	ASSERT_RTNL();
	332
	333	mep = br_mep_find(br, instance);
	334	if (!mep) {
	335	NL_SET_ERR_MSG_MOD(extack,
	336	"MEP instance does not exists");
	337	return -ENOENT;
	338	}
	339
	340	mep_delete_implementation(br, mep);
	341
	342	return 0;
	343	}
	344
	345	int br_cfm_mep_config_set(struct net_bridge *br,
	346	const u32 instance,
	347	const struct br_cfm_mep_config *const config,
	348	struct netlink_ext_ack *extack)
	349	{
	350	struct br_cfm_mep *mep;
	351
	352	ASSERT_RTNL();
	353
	354	mep = br_mep_find(br, instance);
	355	if (!mep) {
	356	NL_SET_ERR_MSG_MOD(extack,
	357	"MEP instance does not exists");
	358	return -ENOENT;
	359	}
	360
	361	mep->config = *config;
	362
	363	return 0;
	364	}
	365
a806ad8e HB	366	int br_cfm_cc_config_set(struct net_bridge *br,
	367	const u32 instance,
	368	const struct br_cfm_cc_config *const config,
	369	struct netlink_ext_ack *extack)
	370	{
	371	struct br_cfm_mep *mep;
	372
	373	ASSERT_RTNL();
	374
	375	mep = br_mep_find(br, instance);
	376	if (!mep) {
	377	NL_SET_ERR_MSG_MOD(extack,
	378	"MEP instance does not exists");
	379	return -ENOENT;
	380	}
	381
	382	/* Check for no change in configuration */
	383	if (memcmp(config, &mep->cc_config, sizeof(*config)) == 0)
	384	return 0;
	385
	386	mep->cc_config = *config;
	387	mep->ccm_tx_snumber = 1;
	388
	389	return 0;
	390	}
	391
86a14b79 HB	392	int br_cfm_cc_peer_mep_add(struct net_bridge *br, const u32 instance,
	393	u32 mepid,
	394	struct netlink_ext_ack *extack)
	395	{
	396	struct br_cfm_peer_mep *peer_mep;
	397	struct br_cfm_mep *mep;
	398
	399	ASSERT_RTNL();
	400
	401	mep = br_mep_find(br, instance);
	402	if (!mep) {
	403	NL_SET_ERR_MSG_MOD(extack,
	404	"MEP instance does not exists");
	405	return -ENOENT;
	406	}
	407
	408	peer_mep = br_peer_mep_find(mep, mepid);
	409	if (peer_mep) {
	410	NL_SET_ERR_MSG_MOD(extack,
	411	"Peer MEP-ID already exists");
	412	return -EEXIST;
	413	}
	414
	415	peer_mep = kzalloc(sizeof(*peer_mep), GFP_KERNEL);
	416	if (!peer_mep)
	417	return -ENOMEM;
	418
	419	peer_mep->mepid = mepid;
	420	peer_mep->mep = mep;
	421
	422	hlist_add_tail_rcu(&peer_mep->head, &mep->peer_mep_list);
	423
	424	return 0;
	425	}
	426
	427	int br_cfm_cc_peer_mep_remove(struct net_bridge *br, const u32 instance,
	428	u32 mepid,
	429	struct netlink_ext_ack *extack)
	430	{
	431	struct br_cfm_peer_mep *peer_mep;
	432	struct br_cfm_mep *mep;
	433
	434	ASSERT_RTNL();
	435
	436	mep = br_mep_find(br, instance);
	437	if (!mep) {
	438	NL_SET_ERR_MSG_MOD(extack,
	439	"MEP instance does not exists");
	440	return -ENOENT;
	441	}
	442
	443	peer_mep = br_peer_mep_find(mep, mepid);
	444	if (!peer_mep) {
	445	NL_SET_ERR_MSG_MOD(extack,
	446	"Peer MEP-ID does not exists");
	447	return -ENOENT;
	448	}
	449
	450	hlist_del_rcu(&peer_mep->head);
	451	kfree_rcu(peer_mep, rcu);
	452
	453	return 0;
	454	}
	455
a806ad8e HB	456	int br_cfm_cc_rdi_set(struct net_bridge *br, const u32 instance,
	457	const bool rdi, struct netlink_ext_ack *extack)
	458	{
	459	struct br_cfm_mep *mep;
	460
	461	ASSERT_RTNL();
	462
	463	mep = br_mep_find(br, instance);
	464	if (!mep) {
	465	NL_SET_ERR_MSG_MOD(extack,
	466	"MEP instance does not exists");
	467	return -ENOENT;
	468	}
	469
	470	mep->rdi = rdi;
	471
	472	return 0;
	473	}
	474
	475	int br_cfm_cc_ccm_tx(struct net_bridge *br, const u32 instance,
	476	const struct br_cfm_cc_ccm_tx_info *const tx_info,
	477	struct netlink_ext_ack *extack)
	478	{
	479	struct br_cfm_mep *mep;
	480
	481	ASSERT_RTNL();
	482
	483	mep = br_mep_find(br, instance);
	484	if (!mep) {
	485	NL_SET_ERR_MSG_MOD(extack,
	486	"MEP instance does not exists");
	487	return -ENOENT;
	488	}
	489
	490	if (memcmp(tx_info, &mep->cc_ccm_tx_info, sizeof(*tx_info)) == 0) {
	491	/* No change in tx_info. */
	492	if (mep->cc_ccm_tx_info.period == 0)
	493	/* Transmission is not enabled - just return */
	494	return 0;
	495
	496	/* Transmission is ongoing, the end time is recalculated */
	497	mep->ccm_tx_end = jiffies +
	498	usecs_to_jiffies(tx_info->period * 1000000);
	499	return 0;
	500	}
	501
	502	if (tx_info->period == 0 && mep->cc_ccm_tx_info.period == 0)
	503	/* Some change in info and transmission is not ongoing */
	504	goto save;
	505
	506	if (tx_info->period != 0 && mep->cc_ccm_tx_info.period != 0) {
	507	/* Some change in info and transmission is ongoing
	508	* The end time is recalculated
	509	*/
	510	mep->ccm_tx_end = jiffies +
	511	usecs_to_jiffies(tx_info->period * 1000000);
	512
	513	goto save;
	514	}
	515
	516	if (tx_info->period == 0 && mep->cc_ccm_tx_info.period != 0) {
	517	cancel_delayed_work_sync(&mep->ccm_tx_dwork);
	518	goto save;
	519	}
520
521	/* Start delayed work to transmit CCM frames. It is done with zero delay
522	* to send first frame immediately
523	*/
524	mep->ccm_tx_end = jiffies + usecs_to_jiffies(tx_info->period * 1000000);
525	queue_delayed_work(system_wq, &mep->ccm_tx_dwork, 0);
526
527	save:
528	mep->cc_ccm_tx_info = *tx_info;
529
530	return 0;
531	}
532
86a14b79 HB	533	/* Deletes the CFM instances on a specific bridge port
	534	*/
	535	void br_cfm_port_del(struct net_bridge br, struct net_bridge_port port)
	536	{
	537	struct hlist_node *n_store;
	538	struct br_cfm_mep *mep;
	539
	540	ASSERT_RTNL();
	541
	542	hlist_for_each_entry_safe(mep, n_store, &br->mep_list, head)
	543	if (mep->create.ifindex == port->dev->ifindex)
	544	mep_delete_implementation(br, mep);
	545	}