drivers/net/ethernet/mscc/ocelot_mrp.c

   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 Semiconductors
   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
  15 static const u8 mrp_test_dmac[] = { 0x01, 0x15, 0x4e, 0x00, 0x00, 0x01 };
  16 static const u8 mrp_control_dmac[] = { 0x01, 0x15, 0x4e, 0x00, 0x00, 0x02 };
  17
  18 static 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
  36 static 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
  50 static 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 = 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
  80 static int ocelot_mrp_copy_add_vcap(struct ocelot *ocelot, int port,
  81                                     int prio, unsigned long cookie)
  82 {
  83         const u8 mrp_mask[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0x00 };
  84         struct ocelot_vcap_filter *filter;
  85         int err;
  86
  87         filter = kzalloc(sizeof(*filter), GFP_KERNEL);
  88         if (!filter)
  89                 return -ENOMEM;
  90
  91         filter->key_type = OCELOT_VCAP_KEY_ETYPE;
  92         filter->prio = prio;
  93         filter->id.cookie = cookie;
  94         filter->id.tc_offload = false;
  95         filter->block_id = VCAP_IS2;
  96         filter->type = OCELOT_VCAP_FILTER_OFFLOAD;
  97         filter->ingress_port_mask = BIT(port);
  98         /* Here is possible to use control or test dmac because the mask
  99          * doesn't cover the LSB
 100          */
 101         ether_addr_copy(filter->key.etype.dmac.value, mrp_test_dmac);
 102         ether_addr_copy(filter->key.etype.dmac.mask, mrp_mask);
 103         filter->action.mask_mode = OCELOT_MASK_MODE_PERMIT_DENY;
 104         filter->action.port_mask = 0x0;
 105         filter->action.cpu_copy_ena = true;
 106         filter->action.cpu_qu_num = OCELOT_MRP_CPUQ;
 107
 108         err = ocelot_vcap_filter_add(ocelot, filter, NULL);
 109         if (err)
 110                 kfree(filter);
 111
 112         return err;
 113 }
 114
 115 static void ocelot_mrp_save_mac(struct ocelot *ocelot,
 116                                 struct ocelot_port *port)
 117 {
 118         ocelot_mact_learn(ocelot, PGID_BLACKHOLE, mrp_test_dmac,
 119                           port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
 120         ocelot_mact_learn(ocelot, PGID_BLACKHOLE, mrp_control_dmac,
 121                           port->pvid_vlan.vid, ENTRYTYPE_LOCKED);
 122 }
 123
 124 static void ocelot_mrp_del_mac(struct ocelot *ocelot,
 125                                struct ocelot_port *port)
 126 {
 127         ocelot_mact_forget(ocelot, mrp_test_dmac, port->pvid_vlan.vid);
 128         ocelot_mact_forget(ocelot, mrp_control_dmac, port->pvid_vlan.vid);
 129 }
 130
 131 int ocelot_mrp_add(struct ocelot *ocelot, int port,
 132                    const struct switchdev_obj_mrp *mrp)
 133 {
 134         struct ocelot_port *ocelot_port = ocelot->ports[port];
 135         struct ocelot_port_private *priv;
 136         struct net_device *dev;
 137
 138         if (!ocelot_port)
 139                 return -EOPNOTSUPP;
 140
 141         priv = container_of(ocelot_port, struct ocelot_port_private, port);
 142         dev = priv->dev;
 143
 144         if (mrp->p_port != dev && mrp->s_port != dev)
 145                 return 0;
 146
 147         ocelot_port->mrp_ring_id = mrp->ring_id;
 148
 149         return 0;
 150 }
 151 EXPORT_SYMBOL(ocelot_mrp_add);
 152
 153 int ocelot_mrp_del(struct ocelot *ocelot, int port,
 154                    const struct switchdev_obj_mrp *mrp)
 155 {
 156         struct ocelot_port *ocelot_port = ocelot->ports[port];
 157
 158         if (!ocelot_port)
 159                 return -EOPNOTSUPP;
 160
 161         if (ocelot_port->mrp_ring_id != mrp->ring_id)
 162                 return 0;
 163
 164         ocelot_port->mrp_ring_id = 0;
 165
 166         return 0;
 167 }
 168 EXPORT_SYMBOL(ocelot_mrp_del);
 169
 170 int ocelot_mrp_add_ring_role(struct ocelot *ocelot, int port,
 171                              const struct switchdev_obj_ring_role_mrp *mrp)
 172 {
 173         struct ocelot_port *ocelot_port = ocelot->ports[port];
 174         int dst_port;
 175         int err;
 176
 177         if (!ocelot_port)
 178                 return -EOPNOTSUPP;
 179
 180         if (mrp->ring_role != BR_MRP_RING_ROLE_MRC && !mrp->sw_backup)
 181                 return -EOPNOTSUPP;
 182
 183         if (ocelot_port->mrp_ring_id != mrp->ring_id)
 184                 return 0;
 185
 186         ocelot_mrp_save_mac(ocelot, ocelot_port);
 187
 188         if (mrp->ring_role != BR_MRP_RING_ROLE_MRC)
 189                 return ocelot_mrp_copy_add_vcap(ocelot, port, 1, port);
 190
 191         dst_port = ocelot_mrp_find_partner_port(ocelot, ocelot_port);
 192         if (dst_port == -1)
 193                 return -EINVAL;
 194
 195         err = ocelot_mrp_redirect_add_vcap(ocelot, port, dst_port);
 196         if (err)
 197                 return err;
 198
 199         err = ocelot_mrp_copy_add_vcap(ocelot, port, 2,
 200                                        port + ocelot->num_phys_ports);
 201         if (err) {
 202                 ocelot_mrp_del_vcap(ocelot, port);
 203                 return err;
 204         }
 205
 206         return 0;
 207 }
 208 EXPORT_SYMBOL(ocelot_mrp_add_ring_role);
 209
 210 int ocelot_mrp_del_ring_role(struct ocelot *ocelot, int port,
 211                              const struct switchdev_obj_ring_role_mrp *mrp)
 212 {
 213         struct ocelot_port *ocelot_port = ocelot->ports[port];
 214         int i;
 215
 216         if (!ocelot_port)
 217                 return -EOPNOTSUPP;
 218
 219         if (mrp->ring_role != BR_MRP_RING_ROLE_MRC && !mrp->sw_backup)
 220                 return -EOPNOTSUPP;
 221
 222         if (ocelot_port->mrp_ring_id != mrp->ring_id)
 223                 return 0;
 224
 225         ocelot_mrp_del_vcap(ocelot, port);
 226         ocelot_mrp_del_vcap(ocelot, port + ocelot->num_phys_ports);
 227
 228         for (i = 0; i < ocelot->num_phys_ports; ++i) {
 229                 ocelot_port = ocelot->ports[i];
 230
 231                 if (!ocelot_port)
 232                         continue;
 233
 234                 if (ocelot_port->mrp_ring_id != 0)
 235                         goto out;
 236         }
 237
 238         ocelot_mrp_del_mac(ocelot, ocelot->ports[port]);
 239 out:
 240         return 0;
 241 }
 242 EXPORT_SYMBOL(ocelot_mrp_del_ring_role);