target/linux/generic/backport-5.15/766-v5.18-12-net-dsa-qca8k-add-support-for-phy-read-write-with-mg.patch

   1 From 2cd5485663847d468dc207b3ff85fb1fab44d97f Mon Sep 17 00:00:00 2001
   2 From: Ansuel Smith <ansuelsmth@gmail.com>
   3 Date: Wed, 2 Feb 2022 01:03:31 +0100
   4 Subject: [PATCH 12/16] net: dsa: qca8k: add support for phy read/write with
   5  mgmt Ethernet
   6
   7 Use mgmt Ethernet also for phy read/write if availabale. Use a different
   8 seq number to make sure we receive the correct packet.
   9 On any error, we fallback to the legacy mdio read/write.
  10
  11 Signed-off-by: Ansuel Smith <ansuelsmth@gmail.com>
  12 Signed-off-by: David S. Miller <davem@davemloft.net>
  13 ---
  14  drivers/net/dsa/qca8k.c | 216 ++++++++++++++++++++++++++++++++++++++++
  15  drivers/net/dsa/qca8k.h |   1 +
  16  2 files changed, 217 insertions(+)
  17
  18 --- a/drivers/net/dsa/qca8k.c
  19 +++ b/drivers/net/dsa/qca8k.c
  20 @@ -867,6 +867,199 @@ qca8k_port_set_status(struct qca8k_priv
  21                 regmap_clear_bits(priv->regmap, QCA8K_REG_PORT_STATUS(port), mask);
  22  }
  23
  24 +static int
  25 +qca8k_phy_eth_busy_wait(struct qca8k_mgmt_eth_data *mgmt_eth_data,
  26 +                       struct sk_buff *read_skb, u32 *val)
  27 +{
  28 +       struct sk_buff *skb = skb_copy(read_skb, GFP_KERNEL);
  29 +       bool ack;
  30 +       int ret;
  31 +
  32 +       reinit_completion(&mgmt_eth_data->rw_done);
  33 +
  34 +       /* Increment seq_num and set it in the copy pkt */
  35 +       mgmt_eth_data->seq++;
  36 +       qca8k_mdio_header_fill_seq_num(skb, mgmt_eth_data->seq);
  37 +       mgmt_eth_data->ack = false;
  38 +
  39 +       dev_queue_xmit(skb);
  40 +
  41 +       ret = wait_for_completion_timeout(&mgmt_eth_data->rw_done,
  42 +                                         QCA8K_ETHERNET_TIMEOUT);
  43 +
  44 +       ack = mgmt_eth_data->ack;
  45 +
  46 +       if (ret <= 0)
  47 +               return -ETIMEDOUT;
  48 +
  49 +       if (!ack)
  50 +               return -EINVAL;
  51 +
  52 +       *val = mgmt_eth_data->data[0];
  53 +
  54 +       return 0;
  55 +}
  56 +
  57 +static int
  58 +qca8k_phy_eth_command(struct qca8k_priv *priv, bool read, int phy,
  59 +                     int regnum, u16 data)
  60 +{
  61 +       struct sk_buff *write_skb, *clear_skb, *read_skb;
  62 +       struct qca8k_mgmt_eth_data *mgmt_eth_data;
  63 +       u32 write_val, clear_val = 0, val;
  64 +       struct net_device *mgmt_master;
  65 +       int ret, ret1;
  66 +       bool ack;
  67 +
  68 +       if (regnum >= QCA8K_MDIO_MASTER_MAX_REG)
  69 +               return -EINVAL;
  70 +
  71 +       mgmt_eth_data = &priv->mgmt_eth_data;
  72 +
  73 +       write_val = QCA8K_MDIO_MASTER_BUSY | QCA8K_MDIO_MASTER_EN |
  74 +                   QCA8K_MDIO_MASTER_PHY_ADDR(phy) |
  75 +                   QCA8K_MDIO_MASTER_REG_ADDR(regnum);
  76 +
  77 +       if (read) {
  78 +               write_val |= QCA8K_MDIO_MASTER_READ;
  79 +       } else {
  80 +               write_val |= QCA8K_MDIO_MASTER_WRITE;
  81 +               write_val |= QCA8K_MDIO_MASTER_DATA(data);
  82 +       }
  83 +
  84 +       /* Prealloc all the needed skb before the lock */
  85 +       write_skb = qca8k_alloc_mdio_header(MDIO_WRITE, QCA8K_MDIO_MASTER_CTRL,
  86 +                                           &write_val, QCA8K_ETHERNET_PHY_PRIORITY);
  87 +       if (!write_skb)
  88 +               return -ENOMEM;
  89 +
  90 +       clear_skb = qca8k_alloc_mdio_header(MDIO_WRITE, QCA8K_MDIO_MASTER_CTRL,
  91 +                                           &clear_val, QCA8K_ETHERNET_PHY_PRIORITY);
  92 +       if (!write_skb) {
  93 +               ret = -ENOMEM;
  94 +               goto err_clear_skb;
  95 +       }
  96 +
  97 +       read_skb = qca8k_alloc_mdio_header(MDIO_READ, QCA8K_MDIO_MASTER_CTRL,
  98 +                                          &clear_val, QCA8K_ETHERNET_PHY_PRIORITY);
  99 +       if (!write_skb) {
 100 +               ret = -ENOMEM;
 101 +               goto err_read_skb;
 102 +       }
 103 +
 104 +       /* Actually start the request:
 105 +        * 1. Send mdio master packet
 106 +        * 2. Busy Wait for mdio master command
 107 +        * 3. Get the data if we are reading
 108 +        * 4. Reset the mdio master (even with error)
 109 +        */
 110 +       mutex_lock(&mgmt_eth_data->mutex);
 111 +
 112 +       /* Check if mgmt_master is operational */
 113 +       mgmt_master = priv->mgmt_master;
 114 +       if (!mgmt_master) {
 115 +               mutex_unlock(&mgmt_eth_data->mutex);
 116 +               ret = -EINVAL;
 117 +               goto err_mgmt_master;
 118 +       }
 119 +
 120 +       read_skb->dev = mgmt_master;
 121 +       clear_skb->dev = mgmt_master;
 122 +       write_skb->dev = mgmt_master;
 123 +
 124 +       reinit_completion(&mgmt_eth_data->rw_done);
 125 +
 126 +       /* Increment seq_num and set it in the write pkt */
 127 +       mgmt_eth_data->seq++;
 128 +       qca8k_mdio_header_fill_seq_num(write_skb, mgmt_eth_data->seq);
 129 +       mgmt_eth_data->ack = false;
 130 +
 131 +       dev_queue_xmit(write_skb);
 132 +
 133 +       ret = wait_for_completion_timeout(&mgmt_eth_data->rw_done,
 134 +                                         QCA8K_ETHERNET_TIMEOUT);
 135 +
 136 +       ack = mgmt_eth_data->ack;
 137 +
 138 +       if (ret <= 0) {
 139 +               ret = -ETIMEDOUT;
 140 +               kfree_skb(read_skb);
 141 +               goto exit;
 142 +       }
 143 +
 144 +       if (!ack) {
 145 +               ret = -EINVAL;
 146 +               kfree_skb(read_skb);
 147 +               goto exit;
 148 +       }
 149 +
 150 +       ret = read_poll_timeout(qca8k_phy_eth_busy_wait, ret1,
 151 +                               !(val & QCA8K_MDIO_MASTER_BUSY), 0,
 152 +                               QCA8K_BUSY_WAIT_TIMEOUT * USEC_PER_MSEC, false,
 153 +                               mgmt_eth_data, read_skb, &val);
 154 +
 155 +       if (ret < 0 && ret1 < 0) {
 156 +               ret = ret1;
 157 +               goto exit;
 158 +       }
 159 +
 160 +       if (read) {
 161 +               reinit_completion(&mgmt_eth_data->rw_done);
 162 +
 163 +               /* Increment seq_num and set it in the read pkt */
 164 +               mgmt_eth_data->seq++;
 165 +               qca8k_mdio_header_fill_seq_num(read_skb, mgmt_eth_data->seq);
 166 +               mgmt_eth_data->ack = false;
 167 +
 168 +               dev_queue_xmit(read_skb);
 169 +
 170 +               ret = wait_for_completion_timeout(&mgmt_eth_data->rw_done,
 171 +                                                 QCA8K_ETHERNET_TIMEOUT);
 172 +
 173 +               ack = mgmt_eth_data->ack;
 174 +
 175 +               if (ret <= 0) {
 176 +                       ret = -ETIMEDOUT;
 177 +                       goto exit;
 178 +               }
 179 +
 180 +               if (!ack) {
 181 +                       ret = -EINVAL;
 182 +                       goto exit;
 183 +               }
 184 +
 185 +               ret = mgmt_eth_data->data[0] & QCA8K_MDIO_MASTER_DATA_MASK;
 186 +       } else {
 187 +               kfree_skb(read_skb);
 188 +       }
 189 +exit:
 190 +       reinit_completion(&mgmt_eth_data->rw_done);
 191 +
 192 +       /* Increment seq_num and set it in the clear pkt */
 193 +       mgmt_eth_data->seq++;
 194 +       qca8k_mdio_header_fill_seq_num(clear_skb, mgmt_eth_data->seq);
 195 +       mgmt_eth_data->ack = false;
 196 +
 197 +       dev_queue_xmit(clear_skb);
 198 +
 199 +       wait_for_completion_timeout(&mgmt_eth_data->rw_done,
 200 +                                   QCA8K_ETHERNET_TIMEOUT);
 201 +
 202 +       mutex_unlock(&mgmt_eth_data->mutex);
 203 +
 204 +       return ret;
 205 +
 206 +       /* Error handling before lock */
 207 +err_mgmt_master:
 208 +       kfree_skb(read_skb);
 209 +err_read_skb:
 210 +       kfree_skb(clear_skb);
 211 +err_clear_skb:
 212 +       kfree_skb(write_skb);
 213 +
 214 +       return ret;
 215 +}
 216 +
 217  static u32
 218  qca8k_port_to_phy(int port)
 219  {
 220 @@ -989,6 +1182,12 @@ qca8k_internal_mdio_write(struct mii_bus
 221  {
 222         struct qca8k_priv *priv = slave_bus->priv;
 223         struct mii_bus *bus = priv->bus;
 224 +       int ret;
 225 +
 226 +       /* Use mdio Ethernet when available, fallback to legacy one on error */
 227 +       ret = qca8k_phy_eth_command(priv, false, phy, regnum, data);
 228 +       if (!ret)
 229 +               return 0;
 230
 231         return qca8k_mdio_write(bus, phy, regnum, data);
 232  }
 233 @@ -998,6 +1197,12 @@ qca8k_internal_mdio_read(struct mii_bus
 234  {
 235         struct qca8k_priv *priv = slave_bus->priv;
 236         struct mii_bus *bus = priv->bus;
 237 +       int ret;
 238 +
 239 +       /* Use mdio Ethernet when available, fallback to legacy one on error */
 240 +       ret = qca8k_phy_eth_command(priv, true, phy, regnum, 0);
 241 +       if (ret >= 0)
 242 +               return ret;
 243
 244         return qca8k_mdio_read(bus, phy, regnum);
 245  }
 246 @@ -1006,6 +1211,7 @@ static int
 247  qca8k_phy_write(struct dsa_switch *ds, int port, int regnum, u16 data)
 248  {
 249         struct qca8k_priv *priv = ds->priv;
 250 +       int ret;
 251
 252         /* Check if the legacy mapping should be used and the
 253          * port is not correctly mapped to the right PHY in the
 254 @@ -1014,6 +1220,11 @@ qca8k_phy_write(struct dsa_switch *ds, i
 255         if (priv->legacy_phy_port_mapping)
 256                 port = qca8k_port_to_phy(port) % PHY_MAX_ADDR;
 257
 258 +       /* Use mdio Ethernet when available, fallback to legacy one on error */
 259 +       ret = qca8k_phy_eth_command(priv, false, port, regnum, 0);
 260 +       if (!ret)
 261 +               return ret;
 262 +
 263         return qca8k_mdio_write(priv->bus, port, regnum, data);
 264  }
 265
 266 @@ -1030,6 +1241,11 @@ qca8k_phy_read(struct dsa_switch *ds, in
 267         if (priv->legacy_phy_port_mapping)
 268                 port = qca8k_port_to_phy(port) % PHY_MAX_ADDR;
 269
 270 +       /* Use mdio Ethernet when available, fallback to legacy one on error */
 271 +       ret = qca8k_phy_eth_command(priv, true, port, regnum, 0);
 272 +       if (ret >= 0)
 273 +               return ret;
 274 +
 275         ret = qca8k_mdio_read(priv->bus, port, regnum);
 276
 277         if (ret < 0)
 278 --- a/drivers/net/dsa/qca8k.h
 279 +++ b/drivers/net/dsa/qca8k.h
 280 @@ -14,6 +14,7 @@
 281  #include <linux/dsa/tag_qca.h>
 282
 283  #define QCA8K_ETHERNET_MDIO_PRIORITY                   7
 284 +#define QCA8K_ETHERNET_PHY_PRIORITY                    6
 285  #define QCA8K_ETHERNET_TIMEOUT                         100
 286
 287  #define QCA8K_NUM_PORTS                                        7