1 // SPDX-License-Identifier: GPL-2.0 2 /* Copyright 2019 NXP Semiconductors 3 */ 4 #include <uapi/linux/if_bridge.h> 5 #include <soc/mscc/ocelot_qsys.h> 6 #include <soc/mscc/ocelot_sys.h> 7 #include <soc/mscc/ocelot_dev.h> 8 #include <soc/mscc/ocelot_ana.h> 9 #include <soc/mscc/ocelot.h> 10 #include <linux/packing.h> 11 #include <linux/module.h> 12 #include <linux/of_net.h> 13 #include <linux/pci.h> 14 #include <linux/of.h> 15 #include <net/dsa.h> 16 #include "felix.h" 17 18 static enum dsa_tag_protocol felix_get_tag_protocol(struct dsa_switch *ds, 19 int port, 20 enum dsa_tag_protocol mp) 21 { 22 return DSA_TAG_PROTO_OCELOT; 23 } 24 25 static int felix_set_ageing_time(struct dsa_switch *ds, 26 unsigned int ageing_time) 27 { 28 struct ocelot *ocelot = ds->priv; 29 30 ocelot_set_ageing_time(ocelot, ageing_time); 31 32 return 0; 33 } 34 35 static int felix_fdb_dump(struct dsa_switch *ds, int port, 36 dsa_fdb_dump_cb_t *cb, void *data) 37 { 38 struct ocelot *ocelot = ds->priv; 39 40 return ocelot_fdb_dump(ocelot, port, cb, data); 41 } 42 43 static int felix_fdb_add(struct dsa_switch *ds, int port, 44 const unsigned char *addr, u16 vid) 45 { 46 struct ocelot *ocelot = ds->priv; 47 bool vlan_aware; 48 49 vlan_aware = dsa_port_is_vlan_filtering(dsa_to_port(ds, port)); 50 51 return ocelot_fdb_add(ocelot, port, addr, vid, vlan_aware); 52 } 53 54 static int felix_fdb_del(struct dsa_switch *ds, int port, 55 const unsigned char *addr, u16 vid) 56 { 57 struct ocelot *ocelot = ds->priv; 58 59 return ocelot_fdb_del(ocelot, port, addr, vid); 60 } 61 62 static void felix_bridge_stp_state_set(struct dsa_switch *ds, int port, 63 u8 state) 64 { 65 struct ocelot *ocelot = ds->priv; 66 67 return ocelot_bridge_stp_state_set(ocelot, port, state); 68 } 69 70 static int felix_bridge_join(struct dsa_switch *ds, int port, 71 struct net_device *br) 72 { 73 struct ocelot *ocelot = ds->priv; 74 75 return ocelot_port_bridge_join(ocelot, port, br); 76 } 77 78 static void felix_bridge_leave(struct dsa_switch *ds, int port, 79 struct net_device *br) 80 { 81 struct ocelot *ocelot = ds->priv; 82 83 ocelot_port_bridge_leave(ocelot, port, br); 84 } 85 86 /* This callback needs to be present */ 87 static int felix_vlan_prepare(struct dsa_switch *ds, int port, 88 const struct switchdev_obj_port_vlan *vlan) 89 { 90 return 0; 91 } 92 93 static int felix_vlan_filtering(struct dsa_switch *ds, int port, bool enabled) 94 { 95 struct ocelot *ocelot = ds->priv; 96 97 ocelot_port_vlan_filtering(ocelot, port, enabled); 98 99 return 0; 100 } 101 102 static void felix_vlan_add(struct dsa_switch *ds, int port, 103 const struct switchdev_obj_port_vlan *vlan) 104 { 105 struct ocelot *ocelot = ds->priv; 106 u16 vid; 107 int err; 108 109 for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) { 110 err = ocelot_vlan_add(ocelot, port, vid, 111 vlan->flags & BRIDGE_VLAN_INFO_PVID, 112 vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED); 113 if (err) { 114 dev_err(ds->dev, "Failed to add VLAN %d to port %d: %d\n", 115 vid, port, err); 116 return; 117 } 118 } 119 } 120 121 static int felix_vlan_del(struct dsa_switch *ds, int port, 122 const struct switchdev_obj_port_vlan *vlan) 123 { 124 struct ocelot *ocelot = ds->priv; 125 u16 vid; 126 int err; 127 128 for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) { 129 err = ocelot_vlan_del(ocelot, port, vid); 130 if (err) { 131 dev_err(ds->dev, "Failed to remove VLAN %d from port %d: %d\n", 132 vid, port, err); 133 return err; 134 } 135 } 136 return 0; 137 } 138 139 static int felix_port_enable(struct dsa_switch *ds, int port, 140 struct phy_device *phy) 141 { 142 struct ocelot *ocelot = ds->priv; 143 144 ocelot_port_enable(ocelot, port, phy); 145 146 return 0; 147 } 148 149 static void felix_port_disable(struct dsa_switch *ds, int port) 150 { 151 struct ocelot *ocelot = ds->priv; 152 153 return ocelot_port_disable(ocelot, port); 154 } 155 156 static void felix_phylink_validate(struct dsa_switch *ds, int port, 157 unsigned long *supported, 158 struct phylink_link_state *state) 159 { 160 struct ocelot *ocelot = ds->priv; 161 struct ocelot_port *ocelot_port = ocelot->ports[port]; 162 __ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, }; 163 164 if (state->interface != PHY_INTERFACE_MODE_NA && 165 state->interface != ocelot_port->phy_mode) { 166 bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS); 167 return; 168 } 169 170 /* No half-duplex. */ 171 phylink_set_port_modes(mask); 172 phylink_set(mask, Autoneg); 173 phylink_set(mask, Pause); 174 phylink_set(mask, Asym_Pause); 175 phylink_set(mask, 10baseT_Full); 176 phylink_set(mask, 100baseT_Full); 177 phylink_set(mask, 1000baseT_Full); 178 179 /* The internal ports that run at 2.5G are overclocked GMII */ 180 if (state->interface == PHY_INTERFACE_MODE_GMII || 181 state->interface == PHY_INTERFACE_MODE_2500BASEX || 182 state->interface == PHY_INTERFACE_MODE_USXGMII) { 183 phylink_set(mask, 2500baseT_Full); 184 phylink_set(mask, 2500baseX_Full); 185 } 186 187 bitmap_and(supported, supported, mask, 188 __ETHTOOL_LINK_MODE_MASK_NBITS); 189 bitmap_and(state->advertising, state->advertising, mask, 190 __ETHTOOL_LINK_MODE_MASK_NBITS); 191 } 192 193 static int felix_phylink_mac_pcs_get_state(struct dsa_switch *ds, int port, 194 struct phylink_link_state *state) 195 { 196 struct ocelot *ocelot = ds->priv; 197 struct felix *felix = ocelot_to_felix(ocelot); 198 199 if (felix->info->pcs_link_state) 200 felix->info->pcs_link_state(ocelot, port, state); 201 202 return 0; 203 } 204 205 static void felix_phylink_mac_config(struct dsa_switch *ds, int port, 206 unsigned int link_an_mode, 207 const struct phylink_link_state *state) 208 { 209 struct ocelot *ocelot = ds->priv; 210 struct ocelot_port *ocelot_port = ocelot->ports[port]; 211 struct felix *felix = ocelot_to_felix(ocelot); 212 u32 mac_fc_cfg; 213 214 /* Take port out of reset by clearing the MAC_TX_RST, MAC_RX_RST and 215 * PORT_RST bits in CLOCK_CFG 216 */ 217 ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(state->speed), 218 DEV_CLOCK_CFG); 219 220 /* Flow control. Link speed is only used here to evaluate the time 221 * specification in incoming pause frames. 222 */ 223 mac_fc_cfg = SYS_MAC_FC_CFG_FC_LINK_SPEED(state->speed); 224 225 /* handle Rx pause in all cases, with 2500base-X this is used for rate 226 * adaptation. 227 */ 228 mac_fc_cfg |= SYS_MAC_FC_CFG_RX_FC_ENA; 229 230 if (state->pause & MLO_PAUSE_TX) 231 mac_fc_cfg |= SYS_MAC_FC_CFG_TX_FC_ENA | 232 SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) | 233 SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) | 234 SYS_MAC_FC_CFG_ZERO_PAUSE_ENA; 235 ocelot_write_rix(ocelot, mac_fc_cfg, SYS_MAC_FC_CFG, port); 236 237 ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port); 238 239 if (felix->info->pcs_init) 240 felix->info->pcs_init(ocelot, port, link_an_mode, state); 241 } 242 243 static void felix_phylink_mac_an_restart(struct dsa_switch *ds, int port) 244 { 245 struct ocelot *ocelot = ds->priv; 246 struct felix *felix = ocelot_to_felix(ocelot); 247 248 if (felix->info->pcs_an_restart) 249 felix->info->pcs_an_restart(ocelot, port); 250 } 251 252 static void felix_phylink_mac_link_down(struct dsa_switch *ds, int port, 253 unsigned int link_an_mode, 254 phy_interface_t interface) 255 { 256 struct ocelot *ocelot = ds->priv; 257 struct ocelot_port *ocelot_port = ocelot->ports[port]; 258 259 ocelot_port_writel(ocelot_port, 0, DEV_MAC_ENA_CFG); 260 ocelot_rmw_rix(ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA, 261 QSYS_SWITCH_PORT_MODE, port); 262 } 263 264 static void felix_phylink_mac_link_up(struct dsa_switch *ds, int port, 265 unsigned int link_an_mode, 266 phy_interface_t interface, 267 struct phy_device *phydev) 268 { 269 struct ocelot *ocelot = ds->priv; 270 struct ocelot_port *ocelot_port = ocelot->ports[port]; 271 272 /* Enable MAC module */ 273 ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA | 274 DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG); 275 276 /* Enable receiving frames on the port, and activate auto-learning of 277 * MAC addresses. 278 */ 279 ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO | 280 ANA_PORT_PORT_CFG_RECV_ENA | 281 ANA_PORT_PORT_CFG_PORTID_VAL(port), 282 ANA_PORT_PORT_CFG, port); 283 284 /* Core: Enable port for frame transfer */ 285 ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE | 286 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) | 287 QSYS_SWITCH_PORT_MODE_PORT_ENA, 288 QSYS_SWITCH_PORT_MODE, port); 289 } 290 291 static void felix_get_strings(struct dsa_switch *ds, int port, 292 u32 stringset, u8 *data) 293 { 294 struct ocelot *ocelot = ds->priv; 295 296 return ocelot_get_strings(ocelot, port, stringset, data); 297 } 298 299 static void felix_get_ethtool_stats(struct dsa_switch *ds, int port, u64 *data) 300 { 301 struct ocelot *ocelot = ds->priv; 302 303 ocelot_get_ethtool_stats(ocelot, port, data); 304 } 305 306 static int felix_get_sset_count(struct dsa_switch *ds, int port, int sset) 307 { 308 struct ocelot *ocelot = ds->priv; 309 310 return ocelot_get_sset_count(ocelot, port, sset); 311 } 312 313 static int felix_get_ts_info(struct dsa_switch *ds, int port, 314 struct ethtool_ts_info *info) 315 { 316 struct ocelot *ocelot = ds->priv; 317 318 return ocelot_get_ts_info(ocelot, port, info); 319 } 320 321 static int felix_parse_ports_node(struct felix *felix, 322 struct device_node *ports_node, 323 phy_interface_t *port_phy_modes) 324 { 325 struct ocelot *ocelot = &felix->ocelot; 326 struct device *dev = felix->ocelot.dev; 327 struct device_node *child; 328 329 for_each_available_child_of_node(ports_node, child) { 330 phy_interface_t phy_mode; 331 u32 port; 332 int err; 333 334 /* Get switch port number from DT */ 335 if (of_property_read_u32(child, "reg", &port) < 0) { 336 dev_err(dev, "Port number not defined in device tree " 337 "(property \"reg\")\n"); 338 of_node_put(child); 339 return -ENODEV; 340 } 341 342 /* Get PHY mode from DT */ 343 err = of_get_phy_mode(child, &phy_mode); 344 if (err) { 345 dev_err(dev, "Failed to read phy-mode or " 346 "phy-interface-type property for port %d\n", 347 port); 348 of_node_put(child); 349 return -ENODEV; 350 } 351 352 err = felix->info->prevalidate_phy_mode(ocelot, port, phy_mode); 353 if (err < 0) { 354 dev_err(dev, "Unsupported PHY mode %s on port %d\n", 355 phy_modes(phy_mode), port); 356 return err; 357 } 358 359 port_phy_modes[port] = phy_mode; 360 } 361 362 return 0; 363 } 364 365 static int felix_parse_dt(struct felix *felix, phy_interface_t *port_phy_modes) 366 { 367 struct device *dev = felix->ocelot.dev; 368 struct device_node *switch_node; 369 struct device_node *ports_node; 370 int err; 371 372 switch_node = dev->of_node; 373 374 ports_node = of_get_child_by_name(switch_node, "ports"); 375 if (!ports_node) { 376 dev_err(dev, "Incorrect bindings: absent \"ports\" node\n"); 377 return -ENODEV; 378 } 379 380 err = felix_parse_ports_node(felix, ports_node, port_phy_modes); 381 of_node_put(ports_node); 382 383 return err; 384 } 385 386 static int felix_init_structs(struct felix *felix, int num_phys_ports) 387 { 388 struct ocelot *ocelot = &felix->ocelot; 389 phy_interface_t *port_phy_modes; 390 resource_size_t switch_base; 391 int port, i, err; 392 393 ocelot->num_phys_ports = num_phys_ports; 394 ocelot->ports = devm_kcalloc(ocelot->dev, num_phys_ports, 395 sizeof(struct ocelot_port *), GFP_KERNEL); 396 if (!ocelot->ports) 397 return -ENOMEM; 398 399 ocelot->map = felix->info->map; 400 ocelot->stats_layout = felix->info->stats_layout; 401 ocelot->num_stats = felix->info->num_stats; 402 ocelot->shared_queue_sz = felix->info->shared_queue_sz; 403 ocelot->ops = felix->info->ops; 404 405 port_phy_modes = kcalloc(num_phys_ports, sizeof(phy_interface_t), 406 GFP_KERNEL); 407 if (!port_phy_modes) 408 return -ENOMEM; 409 410 err = felix_parse_dt(felix, port_phy_modes); 411 if (err) { 412 kfree(port_phy_modes); 413 return err; 414 } 415 416 switch_base = pci_resource_start(felix->pdev, 417 felix->info->switch_pci_bar); 418 419 for (i = 0; i < TARGET_MAX; i++) { 420 struct regmap *target; 421 struct resource *res; 422 423 if (!felix->info->target_io_res[i].name) 424 continue; 425 426 res = &felix->info->target_io_res[i]; 427 res->flags = IORESOURCE_MEM; 428 res->start += switch_base; 429 res->end += switch_base; 430 431 target = ocelot_regmap_init(ocelot, res); 432 if (IS_ERR(target)) { 433 dev_err(ocelot->dev, 434 "Failed to map device memory space\n"); 435 kfree(port_phy_modes); 436 return PTR_ERR(target); 437 } 438 439 ocelot->targets[i] = target; 440 } 441 442 err = ocelot_regfields_init(ocelot, felix->info->regfields); 443 if (err) { 444 dev_err(ocelot->dev, "failed to init reg fields map\n"); 445 kfree(port_phy_modes); 446 return err; 447 } 448 449 for (port = 0; port < num_phys_ports; port++) { 450 struct ocelot_port *ocelot_port; 451 void __iomem *port_regs; 452 struct resource *res; 453 454 ocelot_port = devm_kzalloc(ocelot->dev, 455 sizeof(struct ocelot_port), 456 GFP_KERNEL); 457 if (!ocelot_port) { 458 dev_err(ocelot->dev, 459 "failed to allocate port memory\n"); 460 kfree(port_phy_modes); 461 return -ENOMEM; 462 } 463 464 res = &felix->info->port_io_res[port]; 465 res->flags = IORESOURCE_MEM; 466 res->start += switch_base; 467 res->end += switch_base; 468 469 port_regs = devm_ioremap_resource(ocelot->dev, res); 470 if (IS_ERR(port_regs)) { 471 dev_err(ocelot->dev, 472 "failed to map registers for port %d\n", port); 473 kfree(port_phy_modes); 474 return PTR_ERR(port_regs); 475 } 476 477 ocelot_port->phy_mode = port_phy_modes[port]; 478 ocelot_port->ocelot = ocelot; 479 ocelot_port->regs = port_regs; 480 ocelot->ports[port] = ocelot_port; 481 } 482 483 kfree(port_phy_modes); 484 485 if (felix->info->mdio_bus_alloc) { 486 err = felix->info->mdio_bus_alloc(ocelot); 487 if (err < 0) 488 return err; 489 } 490 491 return 0; 492 } 493 494 /* Hardware initialization done here so that we can allocate structures with 495 * devm without fear of dsa_register_switch returning -EPROBE_DEFER and causing 496 * us to allocate structures twice (leak memory) and map PCI memory twice 497 * (which will not work). 498 */ 499 static int felix_setup(struct dsa_switch *ds) 500 { 501 struct ocelot *ocelot = ds->priv; 502 struct felix *felix = ocelot_to_felix(ocelot); 503 int port, err; 504 505 err = felix_init_structs(felix, ds->num_ports); 506 if (err) 507 return err; 508 509 ocelot_init(ocelot); 510 511 for (port = 0; port < ds->num_ports; port++) { 512 ocelot_init_port(ocelot, port); 513 514 if (dsa_is_cpu_port(ds, port)) 515 ocelot_set_cpu_port(ocelot, port, 516 OCELOT_TAG_PREFIX_NONE, 517 OCELOT_TAG_PREFIX_LONG); 518 } 519 520 /* It looks like the MAC/PCS interrupt register - PM0_IEVENT (0x8040) 521 * isn't instantiated for the Felix PF. 522 * In-band AN may take a few ms to complete, so we need to poll. 523 */ 524 ds->pcs_poll = true; 525 526 return 0; 527 } 528 529 static void felix_teardown(struct dsa_switch *ds) 530 { 531 struct ocelot *ocelot = ds->priv; 532 struct felix *felix = ocelot_to_felix(ocelot); 533 534 if (felix->info->mdio_bus_free) 535 felix->info->mdio_bus_free(ocelot); 536 537 /* stop workqueue thread */ 538 ocelot_deinit(ocelot); 539 } 540 541 static int felix_hwtstamp_get(struct dsa_switch *ds, int port, 542 struct ifreq *ifr) 543 { 544 struct ocelot *ocelot = ds->priv; 545 546 return ocelot_hwstamp_get(ocelot, port, ifr); 547 } 548 549 static int felix_hwtstamp_set(struct dsa_switch *ds, int port, 550 struct ifreq *ifr) 551 { 552 struct ocelot *ocelot = ds->priv; 553 554 return ocelot_hwstamp_set(ocelot, port, ifr); 555 } 556 557 static bool felix_rxtstamp(struct dsa_switch *ds, int port, 558 struct sk_buff *skb, unsigned int type) 559 { 560 struct skb_shared_hwtstamps *shhwtstamps; 561 struct ocelot *ocelot = ds->priv; 562 u8 *extraction = skb->data - ETH_HLEN - OCELOT_TAG_LEN; 563 u32 tstamp_lo, tstamp_hi; 564 struct timespec64 ts; 565 u64 tstamp, val; 566 567 ocelot_ptp_gettime64(&ocelot->ptp_info, &ts); 568 tstamp = ktime_set(ts.tv_sec, ts.tv_nsec); 569 570 packing(extraction, &val, 116, 85, OCELOT_TAG_LEN, UNPACK, 0); 571 tstamp_lo = (u32)val; 572 573 tstamp_hi = tstamp >> 32; 574 if ((tstamp & 0xffffffff) < tstamp_lo) 575 tstamp_hi--; 576 577 tstamp = ((u64)tstamp_hi << 32) | tstamp_lo; 578 579 shhwtstamps = skb_hwtstamps(skb); 580 memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps)); 581 shhwtstamps->hwtstamp = tstamp; 582 return false; 583 } 584 585 static bool felix_txtstamp(struct dsa_switch *ds, int port, 586 struct sk_buff *clone, unsigned int type) 587 { 588 struct ocelot *ocelot = ds->priv; 589 struct ocelot_port *ocelot_port = ocelot->ports[port]; 590 591 if (!ocelot_port_add_txtstamp_skb(ocelot_port, clone)) 592 return true; 593 594 return false; 595 } 596 597 static const struct dsa_switch_ops felix_switch_ops = { 598 .get_tag_protocol = felix_get_tag_protocol, 599 .setup = felix_setup, 600 .teardown = felix_teardown, 601 .set_ageing_time = felix_set_ageing_time, 602 .get_strings = felix_get_strings, 603 .get_ethtool_stats = felix_get_ethtool_stats, 604 .get_sset_count = felix_get_sset_count, 605 .get_ts_info = felix_get_ts_info, 606 .phylink_validate = felix_phylink_validate, 607 .phylink_mac_link_state = felix_phylink_mac_pcs_get_state, 608 .phylink_mac_config = felix_phylink_mac_config, 609 .phylink_mac_an_restart = felix_phylink_mac_an_restart, 610 .phylink_mac_link_down = felix_phylink_mac_link_down, 611 .phylink_mac_link_up = felix_phylink_mac_link_up, 612 .port_enable = felix_port_enable, 613 .port_disable = felix_port_disable, 614 .port_fdb_dump = felix_fdb_dump, 615 .port_fdb_add = felix_fdb_add, 616 .port_fdb_del = felix_fdb_del, 617 .port_bridge_join = felix_bridge_join, 618 .port_bridge_leave = felix_bridge_leave, 619 .port_stp_state_set = felix_bridge_stp_state_set, 620 .port_vlan_prepare = felix_vlan_prepare, 621 .port_vlan_filtering = felix_vlan_filtering, 622 .port_vlan_add = felix_vlan_add, 623 .port_vlan_del = felix_vlan_del, 624 .port_hwtstamp_get = felix_hwtstamp_get, 625 .port_hwtstamp_set = felix_hwtstamp_set, 626 .port_rxtstamp = felix_rxtstamp, 627 .port_txtstamp = felix_txtstamp, 628 }; 629 630 static struct felix_info *felix_instance_tbl[] = { 631 [FELIX_INSTANCE_VSC9959] = &felix_info_vsc9959, 632 }; 633 634 static irqreturn_t felix_irq_handler(int irq, void *data) 635 { 636 struct ocelot *ocelot = (struct ocelot *)data; 637 638 /* The INTB interrupt is used for both PTP TX timestamp interrupt 639 * and preemption status change interrupt on each port. 640 * 641 * - Get txtstamp if have 642 * - TODO: handle preemption. Without handling it, driver may get 643 * interrupt storm. 644 */ 645 646 ocelot_get_txtstamp(ocelot); 647 648 return IRQ_HANDLED; 649 } 650 651 static int felix_pci_probe(struct pci_dev *pdev, 652 const struct pci_device_id *id) 653 { 654 enum felix_instance instance = id->driver_data; 655 struct dsa_switch *ds; 656 struct ocelot *ocelot; 657 struct felix *felix; 658 int err; 659 660 err = pci_enable_device(pdev); 661 if (err) { 662 dev_err(&pdev->dev, "device enable failed\n"); 663 goto err_pci_enable; 664 } 665 666 /* set up for high or low dma */ 667 err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64)); 668 if (err) { 669 err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32)); 670 if (err) { 671 dev_err(&pdev->dev, 672 "DMA configuration failed: 0x%x\n", err); 673 goto err_dma; 674 } 675 } 676 677 felix = kzalloc(sizeof(struct felix), GFP_KERNEL); 678 if (!felix) { 679 err = -ENOMEM; 680 dev_err(&pdev->dev, "Failed to allocate driver memory\n"); 681 goto err_alloc_felix; 682 } 683 684 pci_set_drvdata(pdev, felix); 685 ocelot = &felix->ocelot; 686 ocelot->dev = &pdev->dev; 687 felix->pdev = pdev; 688 felix->info = felix_instance_tbl[instance]; 689 690 pci_set_master(pdev); 691 692 err = devm_request_threaded_irq(&pdev->dev, pdev->irq, NULL, 693 &felix_irq_handler, IRQF_ONESHOT, 694 "felix-intb", ocelot); 695 if (err) { 696 dev_err(&pdev->dev, "Failed to request irq\n"); 697 goto err_alloc_irq; 698 } 699 700 ocelot->ptp = 1; 701 702 ds = kzalloc(sizeof(struct dsa_switch), GFP_KERNEL); 703 if (!ds) { 704 err = -ENOMEM; 705 dev_err(&pdev->dev, "Failed to allocate DSA switch\n"); 706 goto err_alloc_ds; 707 } 708 709 ds->dev = &pdev->dev; 710 ds->num_ports = felix->info->num_ports; 711 ds->ops = &felix_switch_ops; 712 ds->priv = ocelot; 713 felix->ds = ds; 714 715 err = dsa_register_switch(ds); 716 if (err) { 717 dev_err(&pdev->dev, "Failed to register DSA switch: %d\n", err); 718 goto err_register_ds; 719 } 720 721 return 0; 722 723 err_register_ds: 724 kfree(ds); 725 err_alloc_ds: 726 err_alloc_irq: 727 err_alloc_felix: 728 kfree(felix); 729 err_dma: 730 pci_disable_device(pdev); 731 err_pci_enable: 732 return err; 733 } 734 735 static void felix_pci_remove(struct pci_dev *pdev) 736 { 737 struct felix *felix; 738 739 felix = pci_get_drvdata(pdev); 740 741 dsa_unregister_switch(felix->ds); 742 743 kfree(felix->ds); 744 kfree(felix); 745 746 pci_disable_device(pdev); 747 } 748 749 static struct pci_device_id felix_ids[] = { 750 { 751 /* NXP LS1028A */ 752 PCI_DEVICE(PCI_VENDOR_ID_FREESCALE, 0xEEF0), 753 .driver_data = FELIX_INSTANCE_VSC9959, 754 }, 755 { 0, } 756 }; 757 MODULE_DEVICE_TABLE(pci, felix_ids); 758 759 static struct pci_driver felix_pci_driver = { 760 .name = KBUILD_MODNAME, 761 .id_table = felix_ids, 762 .probe = felix_pci_probe, 763 .remove = felix_pci_remove, 764 }; 765 766 module_pci_driver(felix_pci_driver); 767 768 MODULE_DESCRIPTION("Felix Switch driver"); 769 MODULE_LICENSE("GPL v2"); 770