237 lines · c
1// SPDX-License-Identifier: (GPL-2.0 OR MIT)2/* Microsemi Ocelot Switch driver3 *4 * Copyright (c) 2017, 2019 Microsemi Corporation5 * Copyright 2020-2021 NXP6 */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 mask85 * doesn't cover the LSB86 */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