1// SPDX-License-Identifier: (GPL-2.0 OR MIT)
2/* Microsemi Ocelot Switch driver
3 *
4 * Copyright (c) 2017, 2019 Microsemi Corporation
5 * Copyright 2020-2021 NXP
6 */
7
8#include <linux/if_bridge.h>
9#include <linux/mrp_bridge.h>
10#include <soc/mscc/ocelot_vcap.h>
11#include <uapi/linux/mrp_bridge.h>
12#include "ocelot.h"
13#include "ocelot_vcap.h"
14
15static const u8 mrp_test_dmac[] = { 0x01, 0x15, 0x4e, 0x00, 0x00, 0x01 };
16static const u8 mrp_control_dmac[] = { 0x01, 0x15, 0x4e, 0x00, 0x00, 0x02 };
17
18static int ocelot_mrp_find_partner_port(struct ocelot *ocelot,
19					struct ocelot_port *p)
20{
21	int i;
22
23	for (i = 0; i < ocelot->num_phys_ports; ++i) {
24		struct ocelot_port *ocelot_port = ocelot->ports[i];
25
26		if (!ocelot_port || p == ocelot_port)
27			continue;
28
29		if (ocelot_port->mrp_ring_id == p->mrp_ring_id)
30			return i;
31	}
32
33	return -1;
34}
35
36static int ocelot_mrp_del_vcap(struct ocelot *ocelot, int id)
37{
38	struct ocelot_vcap_block *block_vcap_is2;
39	struct ocelot_vcap_filter *filter;
40
41	block_vcap_is2 = &ocelot->block[VCAP_IS2];
42	filter = ocelot_vcap_block_find_filter_by_id(block_vcap_is2, id,
43						     false);
44	if (!filter)
45		return 0;
46
47	return ocelot_vcap_filter_del(ocelot, filter);
48}
49
50static int ocelot_mrp_redirect_add_vcap(struct ocelot *ocelot, int src_port,
51					int dst_port)
52{
53	const u8 mrp_test_mask[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
54	struct ocelot_vcap_filter *filter;
55	int err;
56
57	filter = kzalloc(sizeof(*filter), GFP_KERNEL);
58	if (!filter)
59		return -ENOMEM;
60
61	filter->key_type = OCELOT_VCAP_KEY_ETYPE;
62	filter->prio = 1;
63	filter->id.cookie = OCELOT_VCAP_IS2_MRP_REDIRECT(ocelot, src_port);
64	filter->id.tc_offload = false;
65	filter->block_id = VCAP_IS2;
66	filter->type = OCELOT_VCAP_FILTER_OFFLOAD;
67	filter->ingress_port_mask = BIT(src_port);
68	ether_addr_copy(filter->key.etype.dmac.value, mrp_test_dmac);
69	ether_addr_copy(filter->key.etype.dmac.mask, mrp_test_mask);
70	filter->action.mask_mode = OCELOT_MASK_MODE_REDIRECT;
71	filter->action.port_mask = BIT(dst_port);
72
73	err = ocelot_vcap_filter_add(ocelot, filter, NULL);
74	if (err)
75		kfree(filter);
76
77	return err;
78}
79
80static void ocelot_populate_mrp_trap_key(struct ocelot_vcap_filter *filter)
81{
82	const u8 mrp_mask[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0x00 };
83
84	/* Here is possible to use control or test dmac because the mask
85	 * doesn't cover the LSB
86	 */
87	ether_addr_copy(filter->key.etype.dmac.value, mrp_test_dmac);
88	ether_addr_copy(filter->key.etype.dmac.mask, mrp_mask);
89}
90
91static int ocelot_mrp_trap_add(struct ocelot *ocelot, int port)
92{
93	unsigned long cookie = OCELOT_VCAP_IS2_MRP_TRAP(ocelot);
94
95	return ocelot_trap_add(ocelot, port, cookie, false,
96			       ocelot_populate_mrp_trap_key);
97}
98
99static int ocelot_mrp_trap_del(struct ocelot *ocelot, int port)
100{
101	unsigned long cookie = OCELOT_VCAP_IS2_MRP_TRAP(ocelot);
102
103	return ocelot_trap_del(ocelot, port, cookie);
104}
105
106static void ocelot_mrp_save_mac(struct ocelot *ocelot,
107				struct ocelot_port *port)
108{
109	ocelot_mact_learn(ocelot, PGID_BLACKHOLE, mrp_test_dmac,
110			  OCELOT_STANDALONE_PVID, ENTRYTYPE_LOCKED);
111	ocelot_mact_learn(ocelot, PGID_BLACKHOLE, mrp_control_dmac,
112			  OCELOT_STANDALONE_PVID, ENTRYTYPE_LOCKED);
113}
114
115static void ocelot_mrp_del_mac(struct ocelot *ocelot,
116			       struct ocelot_port *port)
117{
118	ocelot_mact_forget(ocelot, mrp_test_dmac, OCELOT_STANDALONE_PVID);
119	ocelot_mact_forget(ocelot, mrp_control_dmac, OCELOT_STANDALONE_PVID);
120}
121
122int ocelot_mrp_add(struct ocelot *ocelot, int port,
123		   const struct switchdev_obj_mrp *mrp)
124{
125	struct ocelot_port *ocelot_port = ocelot->ports[port];
126	struct ocelot_port_private *priv;
127	struct net_device *dev;
128
129	if (!ocelot_port)
130		return -EOPNOTSUPP;
131
132	priv = container_of(ocelot_port, struct ocelot_port_private, port);
133	dev = priv->dev;
134
135	if (mrp->p_port != dev && mrp->s_port != dev)
136		return 0;
137
138	ocelot_port->mrp_ring_id = mrp->ring_id;
139
140	return 0;
141}
142EXPORT_SYMBOL(ocelot_mrp_add);
143
144int ocelot_mrp_del(struct ocelot *ocelot, int port,
145		   const struct switchdev_obj_mrp *mrp)
146{
147	struct ocelot_port *ocelot_port = ocelot->ports[port];
148
149	if (!ocelot_port)
150		return -EOPNOTSUPP;
151
152	if (ocelot_port->mrp_ring_id != mrp->ring_id)
153		return 0;
154
155	ocelot_port->mrp_ring_id = 0;
156
157	return 0;
158}
159EXPORT_SYMBOL(ocelot_mrp_del);
160
161int ocelot_mrp_add_ring_role(struct ocelot *ocelot, int port,
162			     const struct switchdev_obj_ring_role_mrp *mrp)
163{
164	struct ocelot_port *ocelot_port = ocelot->ports[port];
165	int dst_port;
166	int err;
167
168	if (!ocelot_port)
169		return -EOPNOTSUPP;
170
171	if (mrp->ring_role != BR_MRP_RING_ROLE_MRC && !mrp->sw_backup)
172		return -EOPNOTSUPP;
173
174	if (ocelot_port->mrp_ring_id != mrp->ring_id)
175		return 0;
176
177	ocelot_mrp_save_mac(ocelot, ocelot_port);
178
179	if (mrp->ring_role != BR_MRP_RING_ROLE_MRC)
180		return ocelot_mrp_trap_add(ocelot, port);
181
182	dst_port = ocelot_mrp_find_partner_port(ocelot, ocelot_port);
183	if (dst_port == -1)
184		return -EINVAL;
185
186	err = ocelot_mrp_redirect_add_vcap(ocelot, port, dst_port);
187	if (err)
188		return err;
189
190	err = ocelot_mrp_trap_add(ocelot, port);
191	if (err) {
192		ocelot_mrp_del_vcap(ocelot,
193				    OCELOT_VCAP_IS2_MRP_REDIRECT(ocelot, port));
194		return err;
195	}
196
197	return 0;
198}
199EXPORT_SYMBOL(ocelot_mrp_add_ring_role);
200
201int ocelot_mrp_del_ring_role(struct ocelot *ocelot, int port,
202			     const struct switchdev_obj_ring_role_mrp *mrp)
203{
204	struct ocelot_port *ocelot_port = ocelot->ports[port];
205	int err, i;
206
207	if (!ocelot_port)
208		return -EOPNOTSUPP;
209
210	if (mrp->ring_role != BR_MRP_RING_ROLE_MRC && !mrp->sw_backup)
211		return -EOPNOTSUPP;
212
213	if (ocelot_port->mrp_ring_id != mrp->ring_id)
214		return 0;
215
216	err = ocelot_mrp_trap_del(ocelot, port);
217	if (err)
218		return err;
219
220	ocelot_mrp_del_vcap(ocelot, OCELOT_VCAP_IS2_MRP_REDIRECT(ocelot, port));
221
222	for (i = 0; i < ocelot->num_phys_ports; ++i) {
223		ocelot_port = ocelot->ports[i];
224
225		if (!ocelot_port)
226			continue;
227
228		if (ocelot_port->mrp_ring_id != 0)
229			goto out;
230	}
231
232	ocelot_mrp_del_mac(ocelot, ocelot->ports[port]);
233out:
234	return 0;
235}
236EXPORT_SYMBOL(ocelot_mrp_del_ring_role);
237