drivers/net/ethernet/mscc/ocelot_board.c

   1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2 /*
   3  * Microsemi Ocelot Switch driver
   4  *
   5  * Copyright (c) 2017 Microsemi Corporation
   6  */
   7 #include <linux/interrupt.h>
   8 #include <linux/module.h>
   9 #include <linux/of_net.h>
  10 #include <linux/netdevice.h>
  11 #include <linux/of_mdio.h>
  12 #include <linux/of_platform.h>
  13 #include <linux/mfd/syscon.h>
  14 #include <linux/skbuff.h>
  15 #include <net/switchdev.h>
  16
  17 #include <soc/mscc/ocelot_vcap.h>
  18 #include "ocelot.h"
  19
  20 #define IFH_EXTRACT_BITFIELD64(x, o, w) (((x) >> (o)) & GENMASK_ULL((w) - 1, 0))
  21 #define VSC7514_VCAP_IS2_CNT 64
  22 #define VSC7514_VCAP_IS2_ENTRY_WIDTH 376
  23 #define VSC7514_VCAP_IS2_ACTION_WIDTH 99
  24 #define VSC7514_VCAP_PORT_CNT 11
  25
  26 static int ocelot_parse_ifh(u32 *_ifh, struct frame_info *info)
  27 {
  28         u8 llen, wlen;
  29         u64 ifh[2];
  30
  31         ifh[0] = be64_to_cpu(((__force __be64 *)_ifh)[0]);
  32         ifh[1] = be64_to_cpu(((__force __be64 *)_ifh)[1]);
  33
  34         wlen = IFH_EXTRACT_BITFIELD64(ifh[0], 7,  8);
  35         llen = IFH_EXTRACT_BITFIELD64(ifh[0], 15,  6);
  36
  37         info->len = OCELOT_BUFFER_CELL_SZ * wlen + llen - 80;
  38
  39         info->timestamp = IFH_EXTRACT_BITFIELD64(ifh[0], 21, 32);
  40
  41         info->port = IFH_EXTRACT_BITFIELD64(ifh[1], 43, 4);
  42
  43         info->tag_type = IFH_EXTRACT_BITFIELD64(ifh[1], 16,  1);
  44         info->vid = IFH_EXTRACT_BITFIELD64(ifh[1], 0,  12);
  45
  46         return 0;
  47 }
  48
  49 static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
  50                                 u32 *rval)
  51 {
  52         u32 val;
  53         u32 bytes_valid;
  54
  55         val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
  56         if (val == XTR_NOT_READY) {
  57                 if (ifh)
  58                         return -EIO;
  59
  60                 do {
  61                         val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
  62                 } while (val == XTR_NOT_READY);
  63         }
  64
  65         switch (val) {
  66         case XTR_ABORT:
  67                 return -EIO;
  68         case XTR_EOF_0:
  69         case XTR_EOF_1:
  70         case XTR_EOF_2:
  71         case XTR_EOF_3:
  72         case XTR_PRUNED:
  73                 bytes_valid = XTR_VALID_BYTES(val);
  74                 val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
  75                 if (val == XTR_ESCAPE)
  76                         *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
  77                 else
  78                         *rval = val;
  79
  80                 return bytes_valid;
  81         case XTR_ESCAPE:
  82                 *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
  83
  84                 return 4;
  85         default:
  86                 *rval = val;
  87
  88                 return 4;
  89         }
  90 }
  91
  92 static irqreturn_t ocelot_xtr_irq_handler(int irq, void *arg)
  93 {
  94         struct ocelot *ocelot = arg;
  95         int i = 0, grp = 0;
  96         int err = 0;
  97
  98         if (!(ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp)))
  99                 return IRQ_NONE;
 100
 101         do {
 102                 struct skb_shared_hwtstamps *shhwtstamps;
 103                 struct ocelot_port_private *priv;
 104                 struct ocelot_port *ocelot_port;
 105                 u64 tod_in_ns, full_ts_in_ns;
 106                 struct frame_info info = {};
 107                 struct net_device *dev;
 108                 u32 ifh[4], val, *buf;
 109                 struct timespec64 ts;
 110                 int sz, len, buf_len;
 111                 struct sk_buff *skb;
 112
 113                 for (i = 0; i < OCELOT_TAG_LEN / 4; i++) {
 114                         err = ocelot_rx_frame_word(ocelot, grp, true, &ifh[i]);
 115                         if (err != 4)
 116                                 break;
 117                 }
 118
 119                 if (err != 4)
 120                         break;
 121
 122                 /* At this point the IFH was read correctly, so it is safe to
 123                  * presume that there is no error. The err needs to be reset
 124                  * otherwise a frame could come in CPU queue between the while
 125                  * condition and the check for error later on. And in that case
 126                  * the new frame is just removed and not processed.
 127                  */
 128                 err = 0;
 129
 130                 ocelot_parse_ifh(ifh, &info);
 131
 132                 ocelot_port = ocelot->ports[info.port];
 133                 priv = container_of(ocelot_port, struct ocelot_port_private,
 134                                     port);
 135                 dev = priv->dev;
 136
 137                 skb = netdev_alloc_skb(dev, info.len);
 138
 139                 if (unlikely(!skb)) {
 140                         netdev_err(dev, "Unable to allocate sk_buff\n");
 141                         err = -ENOMEM;
 142                         break;
 143                 }
 144                 buf_len = info.len - ETH_FCS_LEN;
 145                 buf = (u32 *)skb_put(skb, buf_len);
 146
 147                 len = 0;
 148                 do {
 149                         sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
 150                         *buf++ = val;
 151                         len += sz;
 152                 } while (len < buf_len);
 153
 154                 /* Read the FCS */
 155                 sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
 156                 /* Update the statistics if part of the FCS was read before */
 157                 len -= ETH_FCS_LEN - sz;
 158
 159                 if (unlikely(dev->features & NETIF_F_RXFCS)) {
 160                         buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
 161                         *buf = val;
 162                 }
 163
 164                 if (sz < 0) {
 165                         err = sz;
 166                         break;
 167                 }
 168
 169                 if (ocelot->ptp) {
 170                         ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
 171
 172                         tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
 173                         if ((tod_in_ns & 0xffffffff) < info.timestamp)
 174                                 full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) |
 175                                                 info.timestamp;
 176                         else
 177                                 full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) |
 178                                                 info.timestamp;
 179
 180                         shhwtstamps = skb_hwtstamps(skb);
 181                         memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
 182                         shhwtstamps->hwtstamp = full_ts_in_ns;
 183                 }
 184
 185                 /* Everything we see on an interface that is in the HW bridge
 186                  * has already been forwarded.
 187                  */
 188                 if (ocelot->bridge_mask & BIT(info.port))
 189                         skb->offload_fwd_mark = 1;
 190
 191                 skb->protocol = eth_type_trans(skb, dev);
 192                 if (!skb_defer_rx_timestamp(skb))
 193                         netif_rx(skb);
 194                 dev->stats.rx_bytes += len;
 195                 dev->stats.rx_packets++;
 196         } while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp));
 197
 198         if (err)
 199                 while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp))
 200                         ocelot_read_rix(ocelot, QS_XTR_RD, grp);
 201
 202         return IRQ_HANDLED;
 203 }
 204
 205 static irqreturn_t ocelot_ptp_rdy_irq_handler(int irq, void *arg)
 206 {
 207         struct ocelot *ocelot = arg;
 208
 209         ocelot_get_txtstamp(ocelot);
 210
 211         return IRQ_HANDLED;
 212 }
 213
 214 static const struct of_device_id mscc_ocelot_match[] = {
 215         { .compatible = "mscc,vsc7514-switch" },
 216         { }
 217 };
 218 MODULE_DEVICE_TABLE(of, mscc_ocelot_match);
 219
 220 static int ocelot_reset(struct ocelot *ocelot)
 221 {
 222         int retries = 100;
 223         u32 val;
 224
 225         regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_INIT], 1);
 226         regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_ENA], 1);
 227
 228         do {
 229                 msleep(1);
 230                 regmap_field_read(ocelot->regfields[SYS_RESET_CFG_MEM_INIT],
 231                                   &val);
 232         } while (val && --retries);
 233
 234         if (!retries)
 235                 return -ETIMEDOUT;
 236
 237         regmap_field_write(ocelot->regfields[SYS_RESET_CFG_MEM_ENA], 1);
 238         regmap_field_write(ocelot->regfields[SYS_RESET_CFG_CORE_ENA], 1);
 239
 240         return 0;
 241 }
 242
 243 static const struct ocelot_ops ocelot_ops = {
 244         .reset                  = ocelot_reset,
 245 };
 246
 247 static const struct vcap_field vsc7514_vcap_is2_keys[] = {
 248         /* Common: 46 bits */
 249         [VCAP_IS2_TYPE]                         = {  0,   4},
 250         [VCAP_IS2_HK_FIRST]                     = {  4,   1},
 251         [VCAP_IS2_HK_PAG]                       = {  5,   8},
 252         [VCAP_IS2_HK_IGR_PORT_MASK]             = { 13,  12},
 253         [VCAP_IS2_HK_RSV2]                      = { 25,   1},
 254         [VCAP_IS2_HK_HOST_MATCH]                = { 26,   1},
 255         [VCAP_IS2_HK_L2_MC]                     = { 27,   1},
 256         [VCAP_IS2_HK_L2_BC]                     = { 28,   1},
 257         [VCAP_IS2_HK_VLAN_TAGGED]               = { 29,   1},
 258         [VCAP_IS2_HK_VID]                       = { 30,  12},
 259         [VCAP_IS2_HK_DEI]                       = { 42,   1},
 260         [VCAP_IS2_HK_PCP]                       = { 43,   3},
 261         /* MAC_ETYPE / MAC_LLC / MAC_SNAP / OAM common */
 262         [VCAP_IS2_HK_L2_DMAC]                   = { 46,  48},
 263         [VCAP_IS2_HK_L2_SMAC]                   = { 94,  48},
 264         /* MAC_ETYPE (TYPE=000) */
 265         [VCAP_IS2_HK_MAC_ETYPE_ETYPE]           = {142,  16},
 266         [VCAP_IS2_HK_MAC_ETYPE_L2_PAYLOAD0]     = {158,  16},
 267         [VCAP_IS2_HK_MAC_ETYPE_L2_PAYLOAD1]     = {174,   8},
 268         [VCAP_IS2_HK_MAC_ETYPE_L2_PAYLOAD2]     = {182,   3},
 269         /* MAC_LLC (TYPE=001) */
 270         [VCAP_IS2_HK_MAC_LLC_L2_LLC]            = {142,  40},
 271         /* MAC_SNAP (TYPE=010) */
 272         [VCAP_IS2_HK_MAC_SNAP_L2_SNAP]          = {142,  40},
 273         /* MAC_ARP (TYPE=011) */
 274         [VCAP_IS2_HK_MAC_ARP_SMAC]              = { 46,  48},
 275         [VCAP_IS2_HK_MAC_ARP_ADDR_SPACE_OK]     = { 94,   1},
 276         [VCAP_IS2_HK_MAC_ARP_PROTO_SPACE_OK]    = { 95,   1},
 277         [VCAP_IS2_HK_MAC_ARP_LEN_OK]            = { 96,   1},
 278         [VCAP_IS2_HK_MAC_ARP_TARGET_MATCH]      = { 97,   1},
 279         [VCAP_IS2_HK_MAC_ARP_SENDER_MATCH]      = { 98,   1},
 280         [VCAP_IS2_HK_MAC_ARP_OPCODE_UNKNOWN]    = { 99,   1},
 281         [VCAP_IS2_HK_MAC_ARP_OPCODE]            = {100,   2},
 282         [VCAP_IS2_HK_MAC_ARP_L3_IP4_DIP]        = {102,  32},
 283         [VCAP_IS2_HK_MAC_ARP_L3_IP4_SIP]        = {134,  32},
 284         [VCAP_IS2_HK_MAC_ARP_DIP_EQ_SIP]        = {166,   1},
 285         /* IP4_TCP_UDP / IP4_OTHER common */
 286         [VCAP_IS2_HK_IP4]                       = { 46,   1},
 287         [VCAP_IS2_HK_L3_FRAGMENT]               = { 47,   1},
 288         [VCAP_IS2_HK_L3_FRAG_OFS_GT0]           = { 48,   1},
 289         [VCAP_IS2_HK_L3_OPTIONS]                = { 49,   1},
 290         [VCAP_IS2_HK_IP4_L3_TTL_GT0]            = { 50,   1},
 291         [VCAP_IS2_HK_L3_TOS]                    = { 51,   8},
 292         [VCAP_IS2_HK_L3_IP4_DIP]                = { 59,  32},
 293         [VCAP_IS2_HK_L3_IP4_SIP]                = { 91,  32},
 294         [VCAP_IS2_HK_DIP_EQ_SIP]                = {123,   1},
 295         /* IP4_TCP_UDP (TYPE=100) */
 296         [VCAP_IS2_HK_TCP]                       = {124,   1},
 297         [VCAP_IS2_HK_L4_SPORT]                  = {125,  16},
 298         [VCAP_IS2_HK_L4_DPORT]                  = {141,  16},
 299         [VCAP_IS2_HK_L4_RNG]                    = {157,   8},
 300         [VCAP_IS2_HK_L4_SPORT_EQ_DPORT]         = {165,   1},
 301         [VCAP_IS2_HK_L4_SEQUENCE_EQ0]           = {166,   1},
 302         [VCAP_IS2_HK_L4_URG]                    = {167,   1},
 303         [VCAP_IS2_HK_L4_ACK]                    = {168,   1},
 304         [VCAP_IS2_HK_L4_PSH]                    = {169,   1},
 305         [VCAP_IS2_HK_L4_RST]                    = {170,   1},
 306         [VCAP_IS2_HK_L4_SYN]                    = {171,   1},
 307         [VCAP_IS2_HK_L4_FIN]                    = {172,   1},
 308         [VCAP_IS2_HK_L4_1588_DOM]               = {173,   8},
 309         [VCAP_IS2_HK_L4_1588_VER]               = {181,   4},
 310         /* IP4_OTHER (TYPE=101) */
 311         [VCAP_IS2_HK_IP4_L3_PROTO]              = {124,   8},
 312         [VCAP_IS2_HK_L3_PAYLOAD]                = {132,  56},
 313         /* IP6_STD (TYPE=110) */
 314         [VCAP_IS2_HK_IP6_L3_TTL_GT0]            = { 46,   1},
 315         [VCAP_IS2_HK_L3_IP6_SIP]                = { 47, 128},
 316         [VCAP_IS2_HK_IP6_L3_PROTO]              = {175,   8},
 317         /* OAM (TYPE=111) */
 318         [VCAP_IS2_HK_OAM_MEL_FLAGS]             = {142,   7},
 319         [VCAP_IS2_HK_OAM_VER]                   = {149,   5},
 320         [VCAP_IS2_HK_OAM_OPCODE]                = {154,   8},
 321         [VCAP_IS2_HK_OAM_FLAGS]                 = {162,   8},
 322         [VCAP_IS2_HK_OAM_MEPID]                 = {170,  16},
 323         [VCAP_IS2_HK_OAM_CCM_CNTS_EQ0]          = {186,   1},
 324         [VCAP_IS2_HK_OAM_IS_Y1731]              = {187,   1},
 325 };
 326
 327 static const struct vcap_field vsc7514_vcap_is2_actions[] = {
 328         [VCAP_IS2_ACT_HIT_ME_ONCE]              = {  0,  1},
 329         [VCAP_IS2_ACT_CPU_COPY_ENA]             = {  1,  1},
 330         [VCAP_IS2_ACT_CPU_QU_NUM]               = {  2,  3},
 331         [VCAP_IS2_ACT_MASK_MODE]                = {  5,  2},
 332         [VCAP_IS2_ACT_MIRROR_ENA]               = {  7,  1},
 333         [VCAP_IS2_ACT_LRN_DIS]                  = {  8,  1},
 334         [VCAP_IS2_ACT_POLICE_ENA]               = {  9,  1},
 335         [VCAP_IS2_ACT_POLICE_IDX]               = { 10,  9},
 336         [VCAP_IS2_ACT_POLICE_VCAP_ONLY]         = { 19,  1},
 337         [VCAP_IS2_ACT_PORT_MASK]                = { 20, 11},
 338         [VCAP_IS2_ACT_REW_OP]                   = { 31,  9},
 339         [VCAP_IS2_ACT_SMAC_REPLACE_ENA]         = { 40,  1},
 340         [VCAP_IS2_ACT_RSV]                      = { 41,  2},
 341         [VCAP_IS2_ACT_ACL_ID]                   = { 43,  6},
 342         [VCAP_IS2_ACT_HIT_CNT]                  = { 49, 32},
 343 };
 344
 345 static const struct vcap_props vsc7514_vcap_props[] = {
 346         [VCAP_IS2] = {
 347                 .tg_width = 2,
 348                 .sw_count = 4,
 349                 .entry_count = VSC7514_VCAP_IS2_CNT,
 350                 .entry_width = VSC7514_VCAP_IS2_ENTRY_WIDTH,
 351                 .action_count = VSC7514_VCAP_IS2_CNT +
 352                                 VSC7514_VCAP_PORT_CNT + 2,
 353                 .action_width = 99,
 354                 .action_type_width = 1,
 355                 .action_table = {
 356                         [IS2_ACTION_TYPE_NORMAL] = {
 357                                 .width = 49,
 358                                 .count = 2
 359                         },
 360                         [IS2_ACTION_TYPE_SMAC_SIP] = {
 361                                 .width = 6,
 362                                 .count = 4
 363                         },
 364                 },
 365                 .counter_words = 4,
 366                 .counter_width = 32,
 367         },
 368 };
 369
 370 static struct ptp_clock_info ocelot_ptp_clock_info = {
 371         .owner          = THIS_MODULE,
 372         .name           = "ocelot ptp",
 373         .max_adj        = 0x7fffffff,
 374         .n_alarm        = 0,
 375         .n_ext_ts       = 0,
 376         .n_per_out      = OCELOT_PTP_PINS_NUM,
 377         .n_pins         = OCELOT_PTP_PINS_NUM,
 378         .pps            = 0,
 379         .gettime64      = ocelot_ptp_gettime64,
 380         .settime64      = ocelot_ptp_settime64,
 381         .adjtime        = ocelot_ptp_adjtime,
 382         .adjfine        = ocelot_ptp_adjfine,
 383         .verify         = ocelot_ptp_verify,
 384         .enable         = ocelot_ptp_enable,
 385 };
 386
 387 static int mscc_ocelot_probe(struct platform_device *pdev)
 388 {
 389         struct device_node *np = pdev->dev.of_node;
 390         struct device_node *ports, *portnp;
 391         int err, irq_xtr, irq_ptp_rdy;
 392         struct ocelot *ocelot;
 393         struct regmap *hsio;
 394         unsigned int i;
 395
 396         struct {
 397                 enum ocelot_target id;
 398                 char *name;
 399                 u8 optional:1;
 400         } io_target[] = {
 401                 { SYS, "sys" },
 402                 { REW, "rew" },
 403                 { QSYS, "qsys" },
 404                 { ANA, "ana" },
 405                 { QS, "qs" },
 406                 { S2, "s2" },
 407                 { PTP, "ptp", 1 },
 408         };
 409
 410         if (!np && !pdev->dev.platform_data)
 411                 return -ENODEV;
 412
 413         ocelot = devm_kzalloc(&pdev->dev, sizeof(*ocelot), GFP_KERNEL);
 414         if (!ocelot)
 415                 return -ENOMEM;
 416
 417         platform_set_drvdata(pdev, ocelot);
 418         ocelot->dev = &pdev->dev;
 419
 420         for (i = 0; i < ARRAY_SIZE(io_target); i++) {
 421                 struct regmap *target;
 422                 struct resource *res;
 423
 424                 res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 425                                                    io_target[i].name);
 426
 427                 target = ocelot_regmap_init(ocelot, res);
 428                 if (IS_ERR(target)) {
 429                         if (io_target[i].optional) {
 430                                 ocelot->targets[io_target[i].id] = NULL;
 431                                 continue;
 432                         }
 433                         return PTR_ERR(target);
 434                 }
 435
 436                 ocelot->targets[io_target[i].id] = target;
 437         }
 438
 439         hsio = syscon_regmap_lookup_by_compatible("mscc,ocelot-hsio");
 440         if (IS_ERR(hsio)) {
 441                 dev_err(&pdev->dev, "missing hsio syscon\n");
 442                 return PTR_ERR(hsio);
 443         }
 444
 445         ocelot->targets[HSIO] = hsio;
 446
 447         err = ocelot_chip_init(ocelot, &ocelot_ops);
 448         if (err)
 449                 return err;
 450
 451         irq_xtr = platform_get_irq_byname(pdev, "xtr");
 452         if (irq_xtr < 0)
 453                 return -ENODEV;
 454
 455         err = devm_request_threaded_irq(&pdev->dev, irq_xtr, NULL,
 456                                         ocelot_xtr_irq_handler, IRQF_ONESHOT,
 457                                         "frame extraction", ocelot);
 458         if (err)
 459                 return err;
 460
 461         irq_ptp_rdy = platform_get_irq_byname(pdev, "ptp_rdy");
 462         if (irq_ptp_rdy > 0 && ocelot->targets[PTP]) {
 463                 err = devm_request_threaded_irq(&pdev->dev, irq_ptp_rdy, NULL,
 464                                                 ocelot_ptp_rdy_irq_handler,
 465                                                 IRQF_ONESHOT, "ptp ready",
 466                                                 ocelot);
 467                 if (err)
 468                         return err;
 469
 470                 /* Both the PTP interrupt and the PTP bank are available */
 471                 ocelot->ptp = 1;
 472         }
 473
 474         ports = of_get_child_by_name(np, "ethernet-ports");
 475         if (!ports) {
 476                 dev_err(&pdev->dev, "no ethernet-ports child node found\n");
 477                 return -ENODEV;
 478         }
 479
 480         ocelot->num_phys_ports = of_get_child_count(ports);
 481
 482         ocelot->ports = devm_kcalloc(&pdev->dev, ocelot->num_phys_ports,
 483                                      sizeof(struct ocelot_port *), GFP_KERNEL);
 484
 485         ocelot->vcap_is2_keys = vsc7514_vcap_is2_keys;
 486         ocelot->vcap_is2_actions = vsc7514_vcap_is2_actions;
 487         ocelot->vcap = vsc7514_vcap_props;
 488
 489         ocelot_init(ocelot);
 490         if (ocelot->ptp) {
 491                 err = ocelot_init_timestamp(ocelot, &ocelot_ptp_clock_info);
 492                 if (err) {
 493                         dev_err(ocelot->dev,
 494                                 "Timestamp initialization failed\n");
 495                         ocelot->ptp = 0;
 496                 }
 497         }
 498
 499         /* No NPI port */
 500         ocelot_configure_cpu(ocelot, -1, OCELOT_TAG_PREFIX_NONE,
 501                              OCELOT_TAG_PREFIX_NONE);
 502
 503         for_each_available_child_of_node(ports, portnp) {
 504                 struct ocelot_port_private *priv;
 505                 struct ocelot_port *ocelot_port;
 506                 struct device_node *phy_node;
 507                 phy_interface_t phy_mode;
 508                 struct phy_device *phy;
 509                 struct resource *res;
 510                 struct phy *serdes;
 511                 void __iomem *regs;
 512                 char res_name[8];
 513                 u32 port;
 514
 515                 if (of_property_read_u32(portnp, "reg", &port))
 516                         continue;
 517
 518                 snprintf(res_name, sizeof(res_name), "port%d", port);
 519
 520                 res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 521                                                    res_name);
 522                 regs = devm_ioremap_resource(&pdev->dev, res);
 523                 if (IS_ERR(regs))
 524                         continue;
 525
 526                 phy_node = of_parse_phandle(portnp, "phy-handle", 0);
 527                 if (!phy_node)
 528                         continue;
 529
 530                 phy = of_phy_find_device(phy_node);
 531                 of_node_put(phy_node);
 532                 if (!phy)
 533                         continue;
 534
 535                 err = ocelot_probe_port(ocelot, port, regs, phy);
 536                 if (err) {
 537                         of_node_put(portnp);
 538                         goto out_put_ports;
 539                 }
 540
 541                 ocelot_port = ocelot->ports[port];
 542                 priv = container_of(ocelot_port, struct ocelot_port_private,
 543                                     port);
 544
 545                 of_get_phy_mode(portnp, &phy_mode);
 546
 547                 ocelot_port->phy_mode = phy_mode;
 548
 549                 switch (ocelot_port->phy_mode) {
 550                 case PHY_INTERFACE_MODE_NA:
 551                         continue;
 552                 case PHY_INTERFACE_MODE_SGMII:
 553                         break;
 554                 case PHY_INTERFACE_MODE_QSGMII:
 555                         /* Ensure clock signals and speed is set on all
 556                          * QSGMII links
 557                          */
 558                         ocelot_port_writel(ocelot_port,
 559                                            DEV_CLOCK_CFG_LINK_SPEED
 560                                            (OCELOT_SPEED_1000),
 561                                            DEV_CLOCK_CFG);
 562                         break;
 563                 default:
 564                         dev_err(ocelot->dev,
 565                                 "invalid phy mode for port%d, (Q)SGMII only\n",
 566                                 port);
 567                         of_node_put(portnp);
 568                         err = -EINVAL;
 569                         goto out_put_ports;
 570                 }
 571
 572                 serdes = devm_of_phy_get(ocelot->dev, portnp, NULL);
 573                 if (IS_ERR(serdes)) {
 574                         err = PTR_ERR(serdes);
 575                         if (err == -EPROBE_DEFER)
 576                                 dev_dbg(ocelot->dev, "deferring probe\n");
 577                         else
 578                                 dev_err(ocelot->dev,
 579                                         "missing SerDes phys for port%d\n",
 580                                         port);
 581
 582                         of_node_put(portnp);
 583                         goto out_put_ports;
 584                 }
 585
 586                 priv->serdes = serdes;
 587         }
 588
 589         register_netdevice_notifier(&ocelot_netdevice_nb);
 590         register_switchdev_notifier(&ocelot_switchdev_nb);
 591         register_switchdev_blocking_notifier(&ocelot_switchdev_blocking_nb);
 592
 593         dev_info(&pdev->dev, "Ocelot switch probed\n");
 594
 595 out_put_ports:
 596         of_node_put(ports);
 597         return err;
 598 }
 599
 600 static int mscc_ocelot_remove(struct platform_device *pdev)
 601 {
 602         struct ocelot *ocelot = platform_get_drvdata(pdev);
 603
 604         ocelot_deinit_timestamp(ocelot);
 605         ocelot_deinit(ocelot);
 606         unregister_switchdev_blocking_notifier(&ocelot_switchdev_blocking_nb);
 607         unregister_switchdev_notifier(&ocelot_switchdev_nb);
 608         unregister_netdevice_notifier(&ocelot_netdevice_nb);
 609
 610         return 0;
 611 }
 612
 613 static struct platform_driver mscc_ocelot_driver = {
 614         .probe = mscc_ocelot_probe,
 615         .remove = mscc_ocelot_remove,
 616         .driver = {
 617                 .name = "ocelot-switch",
 618                 .of_match_table = mscc_ocelot_match,
 619         },
 620 };
 621
 622 module_platform_driver(mscc_ocelot_driver);
 623
 624 MODULE_DESCRIPTION("Microsemi Ocelot switch driver");
 625 MODULE_AUTHOR("Alexandre Belloni <alexandre.belloni@bootlin.com>");
 626 MODULE_LICENSE("Dual MIT/GPL");