[PATCH net-next v2 7/7] net: dsa: mv88e6xxx: Add per port devlink regions

[Andrew Lunn]

Add a devlink region to return the per port registers.

Signed-off-by: Andrew Lunn <and...@lunn.ch>
---
 drivers/net/dsa/mv88e6xxx/devlink.c | 109 +++++++++++++++++++++++++++-
 1 file changed, 105 insertions(+), 4 deletions(-)

diff --git a/drivers/net/dsa/mv88e6xxx/devlink.c 
b/drivers/net/dsa/mv88e6xxx/devlink.c
index 81e1560db206..ed74b075de84 100644
--- a/drivers/net/dsa/mv88e6xxx/devlink.c
+++ b/drivers/net/dsa/mv88e6xxx/devlink.c
@@ -415,6 +415,36 @@ static int mv88e6xxx_region_atu_snapshot(struct devlink 
*dl,
        return err;
 }
 
+static int mv88e6xxx_region_port_snapshot(struct devlink_port *devlink_port,
+                                         const struct devlink_port_region_ops 
*ops,
+                                         struct netlink_ext_ack *extack,
+                                         u8 **data)
+{
+       struct dsa_switch *ds = dsa_devlink_port_to_ds(devlink_port);
+       int port = dsa_devlink_port_to_port(devlink_port);
+       struct mv88e6xxx_chip *chip = ds->priv;
+       u16 *registers;
+       int i, err;
+
+       registers = kmalloc_array(32, sizeof(u16), GFP_KERNEL);
+       if (!registers)
+               return -ENOMEM;
+
+       mv88e6xxx_reg_lock(chip);
+       for (i = 0; i < 32; i++) {
+               err = mv88e6xxx_port_read(chip, port, i, &registers[i]);
+               if (err) {
+                       kfree(registers);
+                       goto out;
+               }
+       }
+       *data = (u8 *)registers;
+out:
+       mv88e6xxx_reg_unlock(chip);
+
+       return err;
+}
+
 static struct mv88e6xxx_region_priv mv88e6xxx_region_global1_priv = {
        .id = MV88E6XXX_REGION_GLOBAL1,
 };
@@ -443,6 +473,12 @@ static struct devlink_region_ops mv88e6xxx_region_atu_ops 
= {
        .destructor = kfree,
 };
 
+static const struct devlink_port_region_ops mv88e6xxx_region_port_ops = {
+       .name = "port",
+       .snapshot = mv88e6xxx_region_port_snapshot,
+       .destructor = kfree,
+};
+
 struct mv88e6xxx_region {
        struct devlink_region_ops *ops;
        u64 size;
@@ -471,11 +507,59 @@ mv88e6xxx_teardown_devlink_regions_global(struct 
mv88e6xxx_chip *chip)
                dsa_devlink_region_destroy(chip->regions[i]);
 }
 
-void mv88e6xxx_teardown_devlink_regions(struct dsa_switch *ds)
+static void
+mv88e6xxx_teardown_devlink_regions_port(struct mv88e6xxx_chip *chip,
+                                       int port)
 {
-       struct mv88e6xxx_chip *chip = ds->priv;
+       dsa_devlink_region_destroy(chip->ports[port].region);
+}
 
-       mv88e6xxx_teardown_devlink_regions_global(chip);
+static int mv88e6xxx_setup_devlink_regions_port(struct dsa_switch *ds,
+                                               struct mv88e6xxx_chip *chip,
+                                               int port)
+{
+       struct devlink_region *region;
+
+       region = dsa_devlink_port_region_create(ds,
+                                               port,
+                                               &mv88e6xxx_region_port_ops, 1,
+                                               32 * sizeof(u16));
+       if (IS_ERR(region))
+               return PTR_ERR(region);
+
+       chip->ports[port].region = region;
+
+       return 0;
+}
+
+static void
+mv88e6xxx_teardown_devlink_regions_ports(struct mv88e6xxx_chip *chip)
+{
+       int port;
+
+       for (port = 0; port < mv88e6xxx_num_ports(chip); port++)
+               mv88e6xxx_teardown_devlink_regions_port(chip, port);
+}
+
+static int mv88e6xxx_setup_devlink_regions_ports(struct dsa_switch *ds,
+                                                struct mv88e6xxx_chip *chip)
+{
+       int port, port_err;
+       int err;
+
+       for (port = 0; port < mv88e6xxx_num_ports(chip); port++) {
+               err = mv88e6xxx_setup_devlink_regions_port(ds, chip, port);
+               if (err)
+                       goto out;
+       }
+
+       return 0;
+
+out:
+       for (port_err = 0; port_err < port; port_err++)
+               mv88e6xxx_teardown_devlink_regions_port(chip, port_err);
+
+       return err;
 }
 
 static int mv88e6xxx_setup_devlink_regions_global(struct dsa_switch *ds,
@@ -511,8 +595,25 @@ static int mv88e6xxx_setup_devlink_regions_global(struct 
dsa_switch *ds,
 int mv88e6xxx_setup_devlink_regions(struct dsa_switch *ds)
 {
        struct mv88e6xxx_chip *chip = ds->priv;
+       int err;
+
+       err = mv88e6xxx_setup_devlink_regions_global(ds, chip);
+       if (err)
+               return err;
+
+       err = mv88e6xxx_setup_devlink_regions_ports(ds, chip);
+       if (err)
+               mv88e6xxx_teardown_devlink_regions_global(chip);
 
-       return mv88e6xxx_setup_devlink_regions_global(ds, chip);
+       return err;
+}
+
+void mv88e6xxx_teardown_devlink_regions(struct dsa_switch *ds)
+{
+       struct mv88e6xxx_chip *chip = ds->priv;
+
+       mv88e6xxx_teardown_devlink_regions_ports(chip);
+       mv88e6xxx_teardown_devlink_regions_global(chip);
 }
 
 int mv88e6xxx_devlink_info_get(struct dsa_switch *ds,
-- 
2.28.0