target/linux/adm5120/files/drivers/net/adm5120sw.c

   1 /*
   2  *  ADM5120 built-in ethernet switch driver
   3  *
   4  *  Copyright (C) 2007,2008 Gabor Juhos <juhosg at openwrt.org>
   5  *
   6  *  This code was based on a driver for Linux 2.6.xx by Jeroen Vreeken.
   7  *    Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2005
   8  *  NAPI extension for the Jeroen's driver
   9  *    Copyright Thomas Langer (Thomas.Langer@infineon.com), 2007
  10  *    Copyright Friedrich Beckmann (Friedrich.Beckmann@infineon.com), 2007
  11  *  Inspiration for the Jeroen's driver came from the ADMtek 2.4 driver.
  12  *    Copyright ADMtek Inc.
  13  *
  14  *  This program is free software; you can redistribute it and/or modify it
  15  *  under the terms of the GNU General Public License version 2  as published
  16  *  by the Free Software Foundation.
  17  *
  18  */
  19
  20 #include <linux/kernel.h>
  21 #include <linux/module.h>
  22 #include <linux/errno.h>
  23 #include <linux/interrupt.h>
  24 #include <linux/ioport.h>
  25 #include <linux/spinlock.h>
  26 #include <linux/platform_device.h>
  27
  28 #include <linux/netdevice.h>
  29 #include <linux/etherdevice.h>
  30 #include <linux/skbuff.h>
  31
  32 #include <linux/io.h>
  33 #include <linux/irq.h>
  34
  35 #include <asm/mipsregs.h>
  36
  37 #include <adm5120_info.h>
  38 #include <adm5120_defs.h>
  39 #include <adm5120_irq.h>
  40 #include <adm5120_switch.h>
  41
  42 #include "adm5120sw.h"
  43
  44 #define DRV_NAME        "adm5120-switch"
  45 #define DRV_DESC        "ADM5120 built-in ethernet switch driver"
  46 #define DRV_VERSION     "0.1.1"
  47
  48 #define CONFIG_ADM5120_SWITCH_NAPI      1
  49 #undef CONFIG_ADM5120_SWITCH_DEBUG
  50
  51 /* ------------------------------------------------------------------------ */
  52
  53 #ifdef CONFIG_ADM5120_SWITCH_DEBUG
  54 #define SW_DBG(f, a...)         printk(KERN_DBG "%s: " f, DRV_NAME , ## a)
  55 #else
  56 #define SW_DBG(f, a...)         do {} while (0)
  57 #endif
  58 #define SW_ERR(f, a...)         printk(KERN_ERR "%s: " f, DRV_NAME , ## a)
  59 #define SW_INFO(f, a...)        printk(KERN_INFO "%s: " f, DRV_NAME , ## a)
  60
  61 #define SWITCH_NUM_PORTS        6
  62 #define ETH_CSUM_LEN            4
  63
  64 #define RX_MAX_PKTLEN   1550
  65 #define RX_RING_SIZE    64
  66
  67 #define TX_RING_SIZE    32
  68 #define TX_QUEUE_LEN    28      /* Limit ring entries actually used. */
  69 #define TX_TIMEOUT      HZ*400
  70
  71 #define RX_DESCS_SIZE   (RX_RING_SIZE * sizeof(struct dma_desc *))
  72 #define RX_SKBS_SIZE    (RX_RING_SIZE * sizeof(struct sk_buff *))
  73 #define TX_DESCS_SIZE   (TX_RING_SIZE * sizeof(struct dma_desc *))
  74 #define TX_SKBS_SIZE    (TX_RING_SIZE * sizeof(struct sk_buff *))
  75
  76 #define SKB_ALLOC_LEN           (RX_MAX_PKTLEN + 32)
  77 #define SKB_RESERVE_LEN         (NET_IP_ALIGN + NET_SKB_PAD)
  78
  79 #define SWITCH_INTS_HIGH (SWITCH_INT_SHD | SWITCH_INT_RHD | SWITCH_INT_HDF)
  80 #define SWITCH_INTS_LOW (SWITCH_INT_SLD | SWITCH_INT_RLD | SWITCH_INT_LDF)
  81 #define SWITCH_INTS_ERR (SWITCH_INT_RDE | SWITCH_INT_SDE | SWITCH_INT_CPUH)
  82 #define SWITCH_INTS_Q (SWITCH_INT_P0QF | SWITCH_INT_P1QF | SWITCH_INT_P2QF | \
  83                         SWITCH_INT_P3QF | SWITCH_INT_P4QF | SWITCH_INT_P5QF | \
  84                         SWITCH_INT_CPQF | SWITCH_INT_GQF)
  85
  86 #define SWITCH_INTS_ALL (SWITCH_INTS_HIGH | SWITCH_INTS_LOW | \
  87                         SWITCH_INTS_ERR | SWITCH_INTS_Q | \
  88                         SWITCH_INT_MD | SWITCH_INT_PSC)
  89
  90 #define SWITCH_INTS_USED (SWITCH_INTS_LOW | SWITCH_INT_PSC)
  91 #define SWITCH_INTS_POLL (SWITCH_INT_RLD | SWITCH_INT_LDF | SWITCH_INT_SLD)
  92
  93 /* ------------------------------------------------------------------------ */
  94
  95 struct adm5120_if_priv {
  96         struct net_device *dev;
  97
  98         unsigned int    vlan_no;
  99         unsigned int    port_mask;
 100
 101 #ifdef CONFIG_ADM5120_SWITCH_NAPI
 102         struct napi_struct napi;
 103 #endif
 104 };
 105
 106 struct dma_desc {
 107         __u32                   buf1;
 108 #define DESC_OWN                (1UL << 31)     /* Owned by the switch */
 109 #define DESC_EOR                (1UL << 28)     /* End of Ring */
 110 #define DESC_ADDR_MASK          0x1FFFFFF
 111 #define DESC_ADDR(x)    ((__u32)(x) & DESC_ADDR_MASK)
 112         __u32                   buf2;
 113 #define DESC_BUF2_EN            (1UL << 31)     /* Buffer 2 enable */
 114         __u32                   buflen;
 115         __u32                   misc;
 116 /* definitions for tx/rx descriptors */
 117 #define DESC_PKTLEN_SHIFT       16
 118 #define DESC_PKTLEN_MASK        0x7FF
 119 /* tx descriptor specific part */
 120 #define DESC_CSUM               (1UL << 31)     /* Append checksum */
 121 #define DESC_DSTPORT_SHIFT      8
 122 #define DESC_DSTPORT_MASK       0x3F
 123 #define DESC_VLAN_MASK          0x3F
 124 /* rx descriptor specific part */
 125 #define DESC_SRCPORT_SHIFT      12
 126 #define DESC_SRCPORT_MASK       0x7
 127 #define DESC_DA_MASK            0x3
 128 #define DESC_DA_SHIFT           4
 129 #define DESC_IPCSUM_FAIL        (1UL << 3)      /* IP checksum fail */
 130 #define DESC_VLAN_TAG           (1UL << 2)      /* VLAN tag present */
 131 #define DESC_TYPE_MASK          0x3             /* mask for Packet type */
 132 #define DESC_TYPE_IP            0x0             /* IP packet */
 133 #define DESC_TYPE_PPPoE         0x1             /* PPPoE packet */
 134 } __attribute__ ((aligned(16)));
 135
 136 /* ------------------------------------------------------------------------ */
 137
 138 static int adm5120_nrdevs;
 139
 140 static struct net_device *adm5120_devs[SWITCH_NUM_PORTS];
 141 /* Lookup table port -> device */
 142 static struct net_device *adm5120_port[SWITCH_NUM_PORTS];
 143
 144 static struct dma_desc *txl_descs;
 145 static struct dma_desc *rxl_descs;
 146
 147 static dma_addr_t txl_descs_dma;
 148 static dma_addr_t rxl_descs_dma;
 149
 150 static struct sk_buff **txl_skbuff;
 151 static struct sk_buff **rxl_skbuff;
 152
 153 static unsigned int cur_rxl, dirty_rxl; /* producer/consumer ring indices */
 154 static unsigned int cur_txl, dirty_txl;
 155
 156 static unsigned int sw_used;
 157
 158 static spinlock_t tx_lock = SPIN_LOCK_UNLOCKED;
 159
 160 /* ------------------------------------------------------------------------ */
 161
 162 static inline u32 sw_read_reg(u32 reg)
 163 {
 164         return __raw_readl((void __iomem *)KSEG1ADDR(ADM5120_SWITCH_BASE)+reg);
 165 }
 166
 167 static inline void sw_write_reg(u32 reg, u32 val)
 168 {
 169         __raw_writel(val, (void __iomem *)KSEG1ADDR(ADM5120_SWITCH_BASE)+reg);
 170 }
 171
 172 static inline void sw_int_mask(u32 mask)
 173 {
 174         u32     t;
 175
 176         t = sw_read_reg(SWITCH_REG_INT_MASK);
 177         t |= mask;
 178         sw_write_reg(SWITCH_REG_INT_MASK, t);
 179 }
 180
 181 static inline void sw_int_unmask(u32 mask)
 182 {
 183         u32     t;
 184
 185         t = sw_read_reg(SWITCH_REG_INT_MASK);
 186         t &= ~mask;
 187         sw_write_reg(SWITCH_REG_INT_MASK, t);
 188 }
 189
 190 static inline void sw_int_ack(u32 mask)
 191 {
 192         sw_write_reg(SWITCH_REG_INT_STATUS, mask);
 193 }
 194
 195 static inline u32 sw_int_status(void)
 196 {
 197         u32     t;
 198
 199         t = sw_read_reg(SWITCH_REG_INT_STATUS);
 200         t &= ~sw_read_reg(SWITCH_REG_INT_MASK);
 201         return t;
 202 }
 203
 204 static inline u32 desc_get_srcport(struct dma_desc *desc)
 205 {
 206         return (desc->misc >> DESC_SRCPORT_SHIFT) & DESC_SRCPORT_MASK;
 207 }
 208
 209 static inline u32 desc_get_pktlen(struct dma_desc *desc)
 210 {
 211         return (desc->misc >> DESC_PKTLEN_SHIFT) & DESC_PKTLEN_MASK;
 212 }
 213
 214 static inline int desc_ipcsum_fail(struct dma_desc *desc)
 215 {
 216         return ((desc->misc & DESC_IPCSUM_FAIL) != 0);
 217 }
 218
 219 /* ------------------------------------------------------------------------ */
 220
 221 static void sw_dump_desc(char *label, struct dma_desc *desc, int tx)
 222 {
 223         u32 t;
 224
 225         SW_DBG("%s %s desc/%p\n", label, tx ? "tx" : "rx", desc);
 226
 227         t = desc->buf1;
 228         SW_DBG("    buf1 %08X addr=%08X; len=%08X %s%s\n", t,
 229                 t & DESC_ADDR_MASK,
 230                 desc->buflen,
 231                 (t & DESC_OWN) ? "SWITCH" : "CPU",
 232                 (t & DESC_EOR) ? " RE" : "");
 233
 234         t = desc->buf2;
 235         SW_DBG("    buf2 %08X addr=%08X%s\n", desc->buf2,
 236                 t & DESC_ADDR_MASK,
 237                 (t & DESC_BUF2_EN) ? " EN" : "" );
 238
 239         t = desc->misc;
 240         if (tx)
 241                 SW_DBG("    misc %08X%s pktlen=%04X ports=%02X vlan=%02X\n", t,
 242                         (t & DESC_CSUM) ? " CSUM" : "",
 243                         (t >> DESC_PKTLEN_SHIFT) & DESC_PKTLEN_MASK,
 244                         (t >> DESC_DSTPORT_SHIFT) & DESC_DSTPORT_MASK,
 245                         t & DESC_VLAN_MASK);
 246         else
 247                 SW_DBG("    misc %08X pktlen=%04X port=%d DA=%d%s%s type=%d\n",
 248                         t,
 249                         (t >> DESC_PKTLEN_SHIFT) & DESC_PKTLEN_MASK,
 250                         (t >> DESC_SRCPORT_SHIFT) & DESC_SRCPORT_MASK,
 251                         (t >> DESC_DA_SHIFT) & DESC_DA_MASK,
 252                         (t & DESC_IPCSUM_FAIL) ? " IPCF" : "",
 253                         (t & DESC_VLAN_TAG) ? " VLAN" : "",
 254                         (t & DESC_TYPE_MASK));
 255 }
 256
 257 static void sw_dump_intr_mask(char *label, u32 mask)
 258 {
 259         SW_DBG("%s %08X%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 260                 label, mask,
 261                 (mask & SWITCH_INT_SHD) ? " SHD" : "",
 262                 (mask & SWITCH_INT_SLD) ? " SLD" : "",
 263                 (mask & SWITCH_INT_RHD) ? " RHD" : "",
 264                 (mask & SWITCH_INT_RLD) ? " RLD" : "",
 265                 (mask & SWITCH_INT_HDF) ? " HDF" : "",
 266                 (mask & SWITCH_INT_LDF) ? " LDF" : "",
 267                 (mask & SWITCH_INT_P0QF) ? " P0QF" : "",
 268                 (mask & SWITCH_INT_P1QF) ? " P1QF" : "",
 269                 (mask & SWITCH_INT_P2QF) ? " P2QF" : "",
 270                 (mask & SWITCH_INT_P3QF) ? " P3QF" : "",
 271                 (mask & SWITCH_INT_P4QF) ? " P4QF" : "",
 272                 (mask & SWITCH_INT_CPQF) ? " CPQF" : "",
 273                 (mask & SWITCH_INT_GQF) ? " GQF" : "",
 274                 (mask & SWITCH_INT_MD) ? " MD" : "",
 275                 (mask & SWITCH_INT_BCS) ? " BCS" : "",
 276                 (mask & SWITCH_INT_PSC) ? " PSC" : "",
 277                 (mask & SWITCH_INT_ID) ? " ID" : "",
 278                 (mask & SWITCH_INT_W0TE) ? " W0TE" : "",
 279                 (mask & SWITCH_INT_W1TE) ? " W1TE" : "",
 280                 (mask & SWITCH_INT_RDE) ? " RDE" : "",
 281                 (mask & SWITCH_INT_SDE) ? " SDE" : "",
 282                 (mask & SWITCH_INT_CPUH) ? " CPUH" : "");
 283 }
 284
 285 static void sw_dump_regs(void)
 286 {
 287         u32 t;
 288
 289         t = sw_read_reg(SWITCH_REG_PHY_STATUS);
 290         SW_DBG("phy_status: %08X\n", t);
 291
 292         t = sw_read_reg(SWITCH_REG_CPUP_CONF);
 293         SW_DBG("cpup_conf: %08X%s%s%s\n", t,
 294                 (t & CPUP_CONF_DCPUP) ? " DCPUP" : "",
 295                 (t & CPUP_CONF_CRCP) ? " CRCP" : "",
 296                 (t & CPUP_CONF_BTM) ? " BTM" : "");
 297
 298         t = sw_read_reg(SWITCH_REG_PORT_CONF0);
 299         SW_DBG("port_conf0: %08X\n", t);
 300         t = sw_read_reg(SWITCH_REG_PORT_CONF1);
 301         SW_DBG("port_conf1: %08X\n", t);
 302         t = sw_read_reg(SWITCH_REG_PORT_CONF2);
 303         SW_DBG("port_conf2: %08X\n", t);
 304
 305         t = sw_read_reg(SWITCH_REG_VLAN_G1);
 306         SW_DBG("vlan g1: %08X\n", t);
 307         t = sw_read_reg(SWITCH_REG_VLAN_G2);
 308         SW_DBG("vlan g2: %08X\n", t);
 309
 310         t = sw_read_reg(SWITCH_REG_BW_CNTL0);
 311         SW_DBG("bw_cntl0: %08X\n", t);
 312         t = sw_read_reg(SWITCH_REG_BW_CNTL1);
 313         SW_DBG("bw_cntl1: %08X\n", t);
 314
 315         t = sw_read_reg(SWITCH_REG_PHY_CNTL0);
 316         SW_DBG("phy_cntl0: %08X\n", t);
 317         t = sw_read_reg(SWITCH_REG_PHY_CNTL1);
 318         SW_DBG("phy_cntl1: %08X\n", t);
 319         t = sw_read_reg(SWITCH_REG_PHY_CNTL2);
 320         SW_DBG("phy_cntl2: %08X\n", t);
 321         t = sw_read_reg(SWITCH_REG_PHY_CNTL3);
 322         SW_DBG("phy_cntl3: %08X\n", t);
 323         t = sw_read_reg(SWITCH_REG_PHY_CNTL4);
 324         SW_DBG("phy_cntl4: %08X\n", t);
 325
 326         t = sw_read_reg(SWITCH_REG_INT_STATUS);
 327         sw_dump_intr_mask("int_status: ", t);
 328
 329         t = sw_read_reg(SWITCH_REG_INT_MASK);
 330         sw_dump_intr_mask("int_mask: ", t);
 331
 332         t = sw_read_reg(SWITCH_REG_SHDA);
 333         SW_DBG("shda: %08X\n", t);
 334         t = sw_read_reg(SWITCH_REG_SLDA);
 335         SW_DBG("slda: %08X\n", t);
 336         t = sw_read_reg(SWITCH_REG_RHDA);
 337         SW_DBG("rhda: %08X\n", t);
 338         t = sw_read_reg(SWITCH_REG_RLDA);
 339         SW_DBG("rlda: %08X\n", t);
 340 }
 341
 342 /* ------------------------------------------------------------------------ */
 343
 344 static inline void adm5120_rx_dma_update(struct dma_desc *desc,
 345         struct sk_buff *skb, int end)
 346 {
 347         desc->misc = 0;
 348         desc->buf2 = 0;
 349         desc->buflen = RX_MAX_PKTLEN;
 350         desc->buf1 = DESC_ADDR(skb->data) |
 351                 DESC_OWN | (end ? DESC_EOR : 0);
 352 }
 353
 354 static void adm5120_switch_rx_refill(void)
 355 {
 356         unsigned int entry;
 357
 358         for (; cur_rxl - dirty_rxl > 0; dirty_rxl++) {
 359                 struct dma_desc *desc;
 360                 struct sk_buff *skb;
 361
 362                 entry = dirty_rxl % RX_RING_SIZE;
 363                 desc = &rxl_descs[entry];
 364
 365                 skb = rxl_skbuff[entry];
 366                 if (skb == NULL) {
 367                         skb = alloc_skb(SKB_ALLOC_LEN, GFP_ATOMIC);
 368                         if (skb) {
 369                                 skb_reserve(skb, SKB_RESERVE_LEN);
 370                                 rxl_skbuff[entry] = skb;
 371                         } else {
 372                                 SW_ERR("no memory for skb\n");
 373                                 desc->buflen = 0;
 374                                 desc->buf2 = 0;
 375                                 desc->misc = 0;
 376                                 desc->buf1 = (desc->buf1 & DESC_EOR) | DESC_OWN;
 377                                 break;
 378                         }
 379                 }
 380
 381                 desc->buf2 = 0;
 382                 desc->buflen = RX_MAX_PKTLEN;
 383                 desc->misc = 0;
 384                 desc->buf1 = (desc->buf1 & DESC_EOR) | DESC_OWN |
 385                                 DESC_ADDR(skb->data);
 386         }
 387 }
 388
 389 static int adm5120_switch_rx(int limit)
 390 {
 391         unsigned int done = 0;
 392
 393         SW_DBG("rx start, limit=%d, cur_rxl=%u, dirty_rxl=%u\n",
 394                                 limit, cur_rxl, dirty_rxl);
 395
 396         while (done < limit) {
 397                 int entry = cur_rxl % RX_RING_SIZE;
 398                 struct dma_desc *desc = &rxl_descs[entry];
 399                 struct net_device *rdev;
 400                 unsigned int port;
 401
 402                 if (desc->buf1 & DESC_OWN)
 403                         break;
 404
 405                 if (dirty_rxl + RX_RING_SIZE == cur_rxl)
 406                         break;
 407
 408                 port = desc_get_srcport(desc);
 409                 rdev = adm5120_port[port];
 410
 411                 SW_DBG("rx descriptor %u, desc=%p, skb=%p\n", entry, desc,
 412                                 rxl_skbuff[entry]);
 413
 414                 if ((rdev) && netif_running(rdev)) {
 415                         struct sk_buff *skb = rxl_skbuff[entry];
 416                         int pktlen;
 417
 418                         pktlen = desc_get_pktlen(desc);
 419                         pktlen -= ETH_CSUM_LEN;
 420
 421                         if ((pktlen == 0) || desc_ipcsum_fail(desc)) {
 422                                 rdev->stats.rx_errors++;
 423                                 if (pktlen == 0)
 424                                         rdev->stats.rx_length_errors++;
 425                                 if (desc_ipcsum_fail(desc))
 426                                         rdev->stats.rx_crc_errors++;
 427                                 SW_DBG("rx error, recycling skb %u\n", entry);
 428                         } else {
 429                                 skb_put(skb, pktlen);
 430
 431                                 skb->dev = rdev;
 432                                 skb->protocol = eth_type_trans(skb, rdev);
 433                                 skb->ip_summed = CHECKSUM_UNNECESSARY;
 434
 435                                 dma_cache_wback_inv((unsigned long)skb->data,
 436                                         skb->len);
 437
 438 #ifdef CONFIG_ADM5120_SWITCH_NAPI
 439                                 netif_receive_skb(skb);
 440 #else
 441                                 netif_rx(skb);
 442 #endif
 443
 444                                 rdev->last_rx = jiffies;
 445                                 rdev->stats.rx_packets++;
 446                                 rdev->stats.rx_bytes += pktlen;
 447
 448                                 rxl_skbuff[entry] = NULL;
 449                                 done++;
 450                         }
 451                 } else {
 452                         SW_DBG("no rx device, recycling skb %u\n", entry);
 453                 }
 454
 455                 cur_rxl++;
 456                 if (cur_rxl - dirty_rxl > RX_RING_SIZE / 4)
 457                         adm5120_switch_rx_refill();
 458         }
 459
 460         adm5120_switch_rx_refill();
 461
 462         SW_DBG("rx finished, cur_rxl=%u, dirty_rxl=%u, processed %d\n",
 463                                 cur_rxl, dirty_rxl, done);
 464
 465         return done;
 466 }
 467
 468 static void adm5120_switch_tx(void)
 469 {
 470         unsigned int entry;
 471
 472         spin_lock(&tx_lock);
 473         entry = dirty_txl % TX_RING_SIZE;
 474         while (dirty_txl != cur_txl) {
 475                 struct dma_desc *desc = &txl_descs[entry];
 476                 struct sk_buff *skb = txl_skbuff[entry];
 477
 478                 if (desc->buf1 & DESC_OWN)
 479                         break;
 480
 481                 if (netif_running(skb->dev)) {
 482                         skb->dev->stats.tx_bytes += skb->len;
 483                         skb->dev->stats.tx_packets++;
 484                 }
 485
 486                 dev_kfree_skb_irq(skb);
 487                 txl_skbuff[entry] = NULL;
 488                 entry = (++dirty_txl) % TX_RING_SIZE;
 489         }
 490
 491         if ((cur_txl - dirty_txl) < TX_QUEUE_LEN - 4) {
 492                 int i;
 493                 for (i = 0; i < SWITCH_NUM_PORTS; i++) {
 494                         if (!adm5120_devs[i])
 495                                 continue;
 496                         netif_wake_queue(adm5120_devs[i]);
 497                 }
 498         }
 499         spin_unlock(&tx_lock);
 500 }
 501
 502 #ifdef CONFIG_ADM5120_SWITCH_NAPI
 503 static int adm5120_if_poll(struct napi_struct *napi, int limit)
 504 {
 505         struct adm5120_if_priv *priv = container_of(napi,
 506                                 struct adm5120_if_priv, napi);
 507         struct net_device *dev = priv->dev;
 508         int done;
 509         u32 status;
 510
 511         sw_int_ack(SWITCH_INTS_POLL);
 512
 513         SW_DBG("%s: processing TX ring\n", dev->name);
 514         adm5120_switch_tx();
 515
 516         SW_DBG("%s: processing RX ring\n", dev->name);
 517         done = adm5120_switch_rx(limit);
 518
 519         status = sw_int_status() & SWITCH_INTS_POLL;
 520         if ((done < limit) && (!status)) {
 521                 SW_DBG("disable polling mode for %s\n", dev->name);
 522                 netif_rx_complete(dev, napi);
 523                 sw_int_unmask(SWITCH_INTS_POLL);
 524                 return 0;
 525         }
 526
 527         SW_DBG("%s still in polling mode, done=%d, status=%x\n",
 528                         dev->name, done, status);
 529         return 1;
 530 }
 531 #endif /* CONFIG_ADM5120_SWITCH_NAPI */
 532
 533
 534 static irqreturn_t adm5120_switch_irq(int irq, void *dev_id)
 535 {
 536         u32 status;
 537
 538         status = sw_int_status();
 539         status &= SWITCH_INTS_ALL;
 540         if (!status)
 541                 return IRQ_NONE;
 542
 543 #ifdef CONFIG_ADM5120_SWITCH_NAPI
 544         sw_int_ack(status & ~SWITCH_INTS_POLL);
 545
 546         if (status & SWITCH_INTS_POLL) {
 547                 struct net_device *dev = dev_id;
 548                 struct adm5120_if_priv *priv = netdev_priv(dev);
 549
 550                 sw_dump_intr_mask("poll ints", status);
 551                 SW_DBG("enable polling mode for %s\n", dev->name);
 552                 sw_int_mask(SWITCH_INTS_POLL);
 553                 netif_rx_schedule(dev, &priv->napi);
 554         }
 555 #else
 556         sw_int_ack(status);
 557
 558         if (status & (SWITCH_INT_RLD | SWITCH_INT_LDF)) {
 559                 adm5120_switch_rx(RX_RING_SIZE);
 560         }
 561
 562         if (status & SWITCH_INT_SLD) {
 563                 adm5120_switch_tx();
 564         }
 565 #endif
 566
 567         return IRQ_HANDLED;
 568 }
 569
 570 static void adm5120_set_bw(char *matrix)
 571 {
 572         unsigned long val;
 573
 574         /* Port 0 to 3 are set using the bandwidth control 0 register */
 575         val = matrix[0] + (matrix[1]<<8) + (matrix[2]<<16) + (matrix[3]<<24);
 576         sw_write_reg(SWITCH_REG_BW_CNTL0, val);
 577
 578         /* Port 4 and 5 are set using the bandwidth control 1 register */
 579         val = matrix[4];
 580         if (matrix[5] == 1)
 581                 sw_write_reg(SWITCH_REG_BW_CNTL1, val | 0x80000000);
 582         else
 583                 sw_write_reg(SWITCH_REG_BW_CNTL1, val & ~0x8000000);
 584
 585         SW_DBG("D: ctl0 0x%ux, ctl1 0x%ux\n", sw_read_reg(SWITCH_REG_BW_CNTL0),
 586                 sw_read_reg(SWITCH_REG_BW_CNTL1));
 587 }
 588
 589 static void adm5120_switch_tx_ring_reset(struct dma_desc *desc,
 590                 struct sk_buff **skbl, int num)
 591 {
 592         memset(desc, 0, num * sizeof(*desc));
 593         desc[num-1].buf1 |= DESC_EOR;
 594         memset(skbl, 0, sizeof(struct skb*)*num);
 595
 596         cur_txl = 0;
 597         dirty_txl = 0;
 598 }
 599
 600 static void adm5120_switch_rx_ring_reset(struct dma_desc *desc,
 601                 struct sk_buff **skbl, int num)
 602 {
 603         int i;
 604
 605         memset(desc, 0, num * sizeof(*desc));
 606         for (i = 0; i < num; i++) {
 607                 skbl[i] = dev_alloc_skb(SKB_ALLOC_LEN);
 608                 if (!skbl[i]) {
 609                         i = num;
 610                         break;
 611                 }
 612                 skb_reserve(skbl[i], SKB_RESERVE_LEN);
 613                 adm5120_rx_dma_update(&desc[i], skbl[i], (num-1==i));
 614         }
 615
 616         cur_rxl = 0;
 617         dirty_rxl = 0;
 618 }
 619
 620 static int adm5120_switch_tx_ring_alloc(void)
 621 {
 622         int err;
 623
 624         txl_descs = dma_alloc_coherent(NULL, TX_DESCS_SIZE, &txl_descs_dma,
 625                                         GFP_ATOMIC);
 626         if (!txl_descs) {
 627                 err = -ENOMEM;
 628                 goto err;
 629         }
 630
 631         txl_skbuff = kzalloc(TX_SKBS_SIZE, GFP_KERNEL);
 632         if (!txl_skbuff) {
 633                 err = -ENOMEM;
 634                 goto err;
 635         }
 636
 637         return 0;
 638
 639 err:
 640         return err;
 641 }
 642
 643 static void adm5120_switch_tx_ring_free(void)
 644 {
 645         int i;
 646
 647         if (txl_skbuff) {
 648                 for (i = 0; i < TX_RING_SIZE; i++)
 649                         if (txl_skbuff[i])
 650                                 kfree_skb(txl_skbuff[i]);
 651                 kfree(txl_skbuff);
 652         }
 653
 654         if (txl_descs)
 655                 dma_free_coherent(NULL, TX_DESCS_SIZE, txl_descs,
 656                         txl_descs_dma);
 657 }
 658
 659 static int adm5120_switch_rx_ring_alloc(void)
 660 {
 661         int err;
 662         int i;
 663
 664         /* init RX ring */
 665         rxl_descs = dma_alloc_coherent(NULL, RX_DESCS_SIZE, &rxl_descs_dma,
 666                                         GFP_ATOMIC);
 667         if (!rxl_descs) {
 668                 err = -ENOMEM;
 669                 goto err;
 670         }
 671
 672         rxl_skbuff = kzalloc(RX_SKBS_SIZE, GFP_KERNEL);
 673         if (!rxl_skbuff) {
 674                 err = -ENOMEM;
 675                 goto err;
 676         }
 677
 678         for (i = 0; i < RX_RING_SIZE; i++) {
 679                 struct sk_buff *skb;
 680                 skb = alloc_skb(SKB_ALLOC_LEN, GFP_ATOMIC);
 681                 if (!skb) {
 682                         err = -ENOMEM;
 683                         goto err;
 684                 }
 685                 rxl_skbuff[i] = skb;
 686                 skb_reserve(skb, SKB_RESERVE_LEN);
 687         }
 688
 689         return 0;
 690
 691 err:
 692         return err;
 693 }
 694
 695 static void adm5120_switch_rx_ring_free(void)
 696 {
 697         int i;
 698
 699         if (rxl_skbuff) {
 700                 for (i = 0; i < RX_RING_SIZE; i++)
 701                         if (rxl_skbuff[i])
 702                                 kfree_skb(rxl_skbuff[i]);
 703                 kfree(rxl_skbuff);
 704         }
 705
 706         if (rxl_descs)
 707                 dma_free_coherent(NULL, RX_DESCS_SIZE, rxl_descs,
 708                         rxl_descs_dma);
 709 }
 710
 711 static void adm5120_write_mac(struct net_device *dev)
 712 {
 713         struct adm5120_if_priv *priv = netdev_priv(dev);
 714         unsigned char *mac = dev->dev_addr;
 715         u32 t;
 716
 717         t = mac[2] | (mac[3] << MAC_WT1_MAC3_SHIFT) |
 718                 (mac[4] << MAC_WT1_MAC4_SHIFT) | (mac[5] << MAC_WT1_MAC5_SHIFT);
 719         sw_write_reg(SWITCH_REG_MAC_WT1, t);
 720
 721         t = (mac[0] << MAC_WT0_MAC0_SHIFT) | (mac[1] << MAC_WT0_MAC1_SHIFT) |
 722                 MAC_WT0_MAWC | MAC_WT0_WVE | (priv->vlan_no<<3);
 723
 724         sw_write_reg(SWITCH_REG_MAC_WT0, t);
 725
 726         while (!(sw_read_reg(SWITCH_REG_MAC_WT0) & MAC_WT0_MWD));
 727 }
 728
 729 static void adm5120_set_vlan(char *matrix)
 730 {
 731         unsigned long val;
 732         int vlan_port, port;
 733
 734         val = matrix[0] + (matrix[1]<<8) + (matrix[2]<<16) + (matrix[3]<<24);
 735         sw_write_reg(SWITCH_REG_VLAN_G1, val);
 736         val = matrix[4] + (matrix[5]<<8);
 737         sw_write_reg(SWITCH_REG_VLAN_G2, val);
 738
 739         /* Now set/update the port vs. device lookup table */
 740         for (port=0; port<SWITCH_NUM_PORTS; port++) {
 741                 for (vlan_port=0; vlan_port<SWITCH_NUM_PORTS && !(matrix[vlan_port] & (0x00000001 << port)); vlan_port++);
 742                 if (vlan_port <SWITCH_NUM_PORTS)
 743                         adm5120_port[port] = adm5120_devs[vlan_port];
 744                 else
 745                         adm5120_port[port] = NULL;
 746         }
 747 }
 748
 749 static void adm5120_switch_set_vlan_mac(unsigned int vlan, unsigned char *mac)
 750 {
 751         u32 t;
 752
 753         t = mac[2] | (mac[3] << MAC_WT1_MAC3_SHIFT)
 754                 | (mac[4] << MAC_WT1_MAC4_SHIFT)
 755                 | (mac[5] << MAC_WT1_MAC5_SHIFT);
 756         sw_write_reg(SWITCH_REG_MAC_WT1, t);
 757
 758         t = (mac[0] << MAC_WT0_MAC0_SHIFT) | (mac[1] << MAC_WT0_MAC1_SHIFT) |
 759                 MAC_WT0_MAWC | MAC_WT0_WVE | (vlan << MAC_WT0_WVN_SHIFT) |
 760                 (MAC_WT0_WAF_STATIC << MAC_WT0_WAF_SHIFT);
 761         sw_write_reg(SWITCH_REG_MAC_WT0, t);
 762
 763         do {
 764                 t = sw_read_reg(SWITCH_REG_MAC_WT0);
 765         } while ((t & MAC_WT0_MWD) == 0);
 766 }
 767
 768 static void adm5120_switch_set_vlan_ports(unsigned int vlan, u32 ports)
 769 {
 770         unsigned int reg;
 771         u32 t;
 772
 773         if (vlan < 4)
 774                 reg = SWITCH_REG_VLAN_G1;
 775         else {
 776                 vlan -= 4;
 777                 reg = SWITCH_REG_VLAN_G2;
 778         }
 779
 780         t = sw_read_reg(reg);
 781         t &= ~(0xFF << (vlan*8));
 782         t |= (ports << (vlan*8));
 783         sw_write_reg(reg, t);
 784 }
 785
 786 /* ------------------------------------------------------------------------ */
 787
 788 #ifdef CONFIG_ADM5120_SWITCH_NAPI
 789 static inline void adm5120_if_napi_enable(struct net_device *dev)
 790 {
 791         struct adm5120_if_priv *priv = netdev_priv(dev);
 792         napi_enable(&priv->napi);
 793 }
 794
 795 static inline void adm5120_if_napi_disable(struct net_device *dev)
 796 {
 797         struct adm5120_if_priv *priv = netdev_priv(dev);
 798         napi_disable(&priv->napi);
 799 }
 800 #else
 801 static inline void adm5120_if_napi_enable(struct net_device *dev) {}
 802 static inline void adm5120_if_napi_disable(struct net_device *dev) {}
 803 #endif /* CONFIG_ADM5120_SWITCH_NAPI */
 804
 805 static int adm5120_if_open(struct net_device *dev)
 806 {
 807         u32 t;
 808         int err;
 809         int i;
 810
 811         adm5120_if_napi_enable(dev);
 812
 813         err = request_irq(dev->irq, adm5120_switch_irq,
 814                 (IRQF_SHARED | IRQF_DISABLED), dev->name, dev);
 815         if (err) {
 816                 SW_ERR("unable to get irq for %s\n", dev->name);
 817                 goto err;
 818         }
 819
 820         if (!sw_used++)
 821                 /* enable interrupts on first open */
 822                 sw_int_unmask(SWITCH_INTS_USED);
 823
 824         /* enable (additional) port */
 825         t = sw_read_reg(SWITCH_REG_PORT_CONF0);
 826         for (i = 0; i < SWITCH_NUM_PORTS; i++) {
 827                 if (dev == adm5120_devs[i])
 828                         t &= ~adm5120_eth_vlans[i];
 829         }
 830         sw_write_reg(SWITCH_REG_PORT_CONF0, t);
 831
 832         netif_start_queue(dev);
 833
 834         return 0;
 835
 836 err:
 837         adm5120_if_napi_disable(dev);
 838         return err;
 839 }
 840
 841 static int adm5120_if_stop(struct net_device *dev)
 842 {
 843         u32 t;
 844         int i;
 845
 846         netif_stop_queue(dev);
 847         adm5120_if_napi_disable(dev);
 848
 849         /* disable port if not assigned to other devices */
 850         t = sw_read_reg(SWITCH_REG_PORT_CONF0);
 851         t |= SWITCH_PORTS_NOCPU;
 852         for (i = 0; i < SWITCH_NUM_PORTS; i++) {
 853                 if ((dev != adm5120_devs[i]) && netif_running(adm5120_devs[i]))
 854                         t &= ~adm5120_eth_vlans[i];
 855         }
 856         sw_write_reg(SWITCH_REG_PORT_CONF0, t);
 857
 858         if (!--sw_used)
 859                 sw_int_mask(SWITCH_INTS_USED);
 860
 861         free_irq(dev->irq, dev);
 862
 863         return 0;
 864 }
 865
 866 static int adm5120_if_hard_start_xmit(struct sk_buff *skb,
 867                 struct net_device *dev)
 868 {
 869         struct dma_desc *desc;
 870         struct adm5120_if_priv *priv = netdev_priv(dev);
 871         unsigned int entry;
 872         unsigned long data;
 873         int i;
 874
 875         /* lock switch irq */
 876         spin_lock_irq(&tx_lock);
 877
 878         /* calculate the next TX descriptor entry. */
 879         entry = cur_txl % TX_RING_SIZE;
 880
 881         desc = &txl_descs[entry];
 882         if (desc->buf1 & DESC_OWN) {
 883                 /* We want to write a packet but the TX queue is still
 884                  * occupied by the DMA. We are faster than the DMA... */
 885                 SW_DBG("%s unable to transmit, packet dopped\n", dev->name);
 886                 dev_kfree_skb(skb);
 887                 dev->stats.tx_dropped++;
 888                 return 0;
 889         }
 890
 891         txl_skbuff[entry] = skb;
 892         data = (desc->buf1 & DESC_EOR);
 893         data |= DESC_ADDR(skb->data);
 894
 895         desc->misc =
 896             ((skb->len<ETH_ZLEN?ETH_ZLEN:skb->len) << DESC_PKTLEN_SHIFT) |
 897             (0x1 << priv->vlan_no);
 898
 899         desc->buflen = skb->len < ETH_ZLEN ? ETH_ZLEN : skb->len;
 900
 901         desc->buf1 = data | DESC_OWN;
 902         sw_write_reg(SWITCH_REG_SEND_TRIG, SEND_TRIG_STL);
 903
 904         cur_txl++;
 905         if (cur_txl == dirty_txl + TX_QUEUE_LEN) {
 906                 for (i = 0; i < SWITCH_NUM_PORTS; i++) {
 907                         if (!adm5120_devs[i])
 908                                 continue;
 909                         netif_stop_queue(adm5120_devs[i]);
 910                 }
 911         }
 912
 913         dev->trans_start = jiffies;
 914
 915         spin_unlock_irq(&tx_lock);
 916
 917         return 0;
 918 }
 919
 920 static void adm5120_if_tx_timeout(struct net_device *dev)
 921 {
 922         SW_INFO("TX timeout on %s\n",dev->name);
 923 }
 924
 925 static void adm5120_if_set_multicast_list(struct net_device *dev)
 926 {
 927         struct adm5120_if_priv *priv = netdev_priv(dev);
 928         u32 ports;
 929         u32 t;
 930
 931         ports = adm5120_eth_vlans[priv->vlan_no] & SWITCH_PORTS_NOCPU;
 932
 933         t = sw_read_reg(SWITCH_REG_CPUP_CONF);
 934         if (dev->flags & IFF_PROMISC)
 935                 /* enable unknown packets */
 936                 t &= ~(ports << CPUP_CONF_DUNP_SHIFT);
 937         else
 938                 /* disable unknown packets */
 939                 t |= (ports << CPUP_CONF_DUNP_SHIFT);
 940
 941         if (dev->flags & IFF_PROMISC || dev->flags & IFF_ALLMULTI ||
 942                                         dev->mc_count)
 943                 /* enable multicast packets */
 944                 t &= ~(ports << CPUP_CONF_DMCP_SHIFT);
 945         else
 946                 /* disable multicast packets */
 947                 t |= (ports << CPUP_CONF_DMCP_SHIFT);
 948
 949         /* If there is any port configured to be in promiscuous mode, then the */
 950         /* Bridge Test Mode has to be activated. This will result in           */
 951         /* transporting also packets learned in another VLAN to be forwarded   */
 952         /* to the CPU.                                                         */
 953         /* The difficult scenario is when we want to build a bridge on the CPU.*/
 954         /* Assume we have port0 and the CPU port in VLAN0 and port1 and the    */
 955         /* CPU port in VLAN1. Now we build a bridge on the CPU between         */
 956         /* VLAN0 and VLAN1. Both ports of the VLANs are set in promisc mode.   */
 957         /* Now assume a packet with ethernet source address 99 enters port 0   */
 958         /* It will be forwarded to the CPU because it is unknown. Then the     */
 959         /* bridge in the CPU will send it to VLAN1 and it goes out at port 1.  */
 960         /* When now a packet with ethernet destination address 99 comes in at  */
 961         /* port 1 in VLAN1, then the switch has learned that this address is   */
 962         /* located at port 0 in VLAN0. Therefore the switch will drop          */
 963         /* this packet. In order to avoid this and to send the packet still    */
 964         /* to the CPU, the Bridge Test Mode has to be activated.               */
 965
 966         /* Check if there is any vlan in promisc mode. */
 967         if (t & (SWITCH_PORTS_NOCPU << CPUP_CONF_DUNP_SHIFT))
 968                 t &= ~CPUP_CONF_BTM; /* Disable Bridge Testing Mode */
 969         else
 970                 t |= CPUP_CONF_BTM;  /* Enable Bridge Testing Mode */
 971
 972         sw_write_reg(SWITCH_REG_CPUP_CONF, t);
 973
 974 }
 975
 976 static int adm5120_if_set_mac_address(struct net_device *dev, void *p)
 977 {
 978         struct sockaddr *addr = p;
 979
 980         memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
 981         adm5120_write_mac(dev);
 982         return 0;
 983 }
 984
 985 static int adm5120_if_do_ioctl(struct net_device *dev, struct ifreq *rq,
 986                 int cmd)
 987 {
 988         int err;
 989         struct adm5120_sw_info info;
 990         struct adm5120_if_priv *priv = netdev_priv(dev);
 991
 992         switch(cmd) {
 993         case SIOCGADMINFO:
 994                 info.magic = 0x5120;
 995                 info.ports = adm5120_nrdevs;
 996                 info.vlan = priv->vlan_no;
 997                 err = copy_to_user(rq->ifr_data, &info, sizeof(info));
 998                 if (err)
 999                         return -EFAULT;
1000                 break;
1001         case SIOCSMATRIX:
1002                 if (!capable(CAP_NET_ADMIN))
1003                         return -EPERM;
1004                 err = copy_from_user(adm5120_eth_vlans, rq->ifr_data,
1005                                         sizeof(adm5120_eth_vlans));
1006                 if (err)
1007                         return -EFAULT;
1008                 adm5120_set_vlan(adm5120_eth_vlans);
1009                 break;
1010         case SIOCGMATRIX:
1011                 err = copy_to_user(rq->ifr_data, adm5120_eth_vlans,
1012                                         sizeof(adm5120_eth_vlans));
1013                 if (err)
1014                         return -EFAULT;
1015                 break;
1016         default:
1017                 return -EOPNOTSUPP;
1018         }
1019         return 0;
1020 }
1021
1022 static struct net_device *adm5120_if_alloc(void)
1023 {
1024         struct net_device *dev;
1025         struct adm5120_if_priv *priv;
1026
1027         dev = alloc_etherdev(sizeof(*priv));
1028         if (!dev)
1029                 return NULL;
1030
1031         priv = netdev_priv(dev);
1032         priv->dev = dev;
1033
1034         dev->irq                = ADM5120_IRQ_SWITCH;
1035         dev->open               = adm5120_if_open;
1036         dev->hard_start_xmit    = adm5120_if_hard_start_xmit;
1037         dev->stop               = adm5120_if_stop;
1038         dev->set_multicast_list = adm5120_if_set_multicast_list;
1039         dev->do_ioctl           = adm5120_if_do_ioctl;
1040         dev->tx_timeout         = adm5120_if_tx_timeout;
1041         dev->watchdog_timeo     = TX_TIMEOUT;
1042         dev->set_mac_address    = adm5120_if_set_mac_address;
1043
1044 #ifdef CONFIG_ADM5120_SWITCH_NAPI
1045         netif_napi_add(dev, &priv->napi, adm5120_if_poll, 64);
1046 #endif
1047
1048         return dev;
1049 }
1050
1051 /* ------------------------------------------------------------------------ */
1052
1053 static void adm5120_switch_cleanup(void)
1054 {
1055         int i;
1056
1057         /* disable interrupts */
1058         sw_int_mask(SWITCH_INTS_ALL);
1059
1060         for (i = 0; i < SWITCH_NUM_PORTS; i++) {
1061                 struct net_device *dev = adm5120_devs[i];
1062                 if (dev) {
1063                         unregister_netdev(dev);
1064                         free_netdev(dev);
1065                 }
1066         }
1067
1068         adm5120_switch_tx_ring_free();
1069         adm5120_switch_rx_ring_free();
1070 }
1071
1072 static int __init adm5120_switch_probe(struct platform_device *pdev)
1073 {
1074         u32 t;
1075         int i, err;
1076
1077         adm5120_nrdevs = adm5120_eth_num_ports;
1078
1079         t = CPUP_CONF_DCPUP | CPUP_CONF_CRCP |
1080                 SWITCH_PORTS_NOCPU << CPUP_CONF_DUNP_SHIFT |
1081                 SWITCH_PORTS_NOCPU << CPUP_CONF_DMCP_SHIFT ;
1082         sw_write_reg(SWITCH_REG_CPUP_CONF, t);
1083
1084         t = (SWITCH_PORTS_NOCPU << PORT_CONF0_EMCP_SHIFT) |
1085                 (SWITCH_PORTS_NOCPU << PORT_CONF0_BP_SHIFT) |
1086                 (SWITCH_PORTS_NOCPU);
1087         sw_write_reg(SWITCH_REG_PORT_CONF0, t);
1088
1089         /* setup ports to Autoneg/100M/Full duplex/Auto MDIX */
1090         t =  SWITCH_PORTS_PHY |
1091                 (SWITCH_PORTS_PHY << PHY_CNTL2_SC_SHIFT) |
1092                 (SWITCH_PORTS_PHY << PHY_CNTL2_DC_SHIFT) |
1093                 (SWITCH_PORTS_PHY << PHY_CNTL2_PHYR_SHIFT) |
1094                 (SWITCH_PORTS_PHY << PHY_CNTL2_AMDIX_SHIFT) |
1095                 PHY_CNTL2_RMAE;
1096         sw_write_reg(SWITCH_REG_PHY_CNTL2, t);
1097
1098         t = sw_read_reg(SWITCH_REG_PHY_CNTL3);
1099         t |= PHY_CNTL3_RNT;
1100         sw_write_reg(SWITCH_REG_PHY_CNTL3, t);
1101
1102         /* Force all the packets from all ports are low priority */
1103         sw_write_reg(SWITCH_REG_PRI_CNTL, 0);
1104
1105         sw_int_mask(SWITCH_INTS_ALL);
1106         sw_int_ack(SWITCH_INTS_ALL);
1107
1108         err = adm5120_switch_rx_ring_alloc();
1109         if (err)
1110                 goto err;
1111
1112         err = adm5120_switch_tx_ring_alloc();
1113         if (err)
1114                 goto err;
1115
1116         adm5120_switch_tx_ring_reset(txl_descs, txl_skbuff, TX_RING_SIZE);
1117         adm5120_switch_rx_ring_reset(rxl_descs, rxl_skbuff, RX_RING_SIZE);
1118
1119         sw_write_reg(SWITCH_REG_SHDA, 0);
1120         sw_write_reg(SWITCH_REG_SLDA, KSEG1ADDR(txl_descs));
1121         sw_write_reg(SWITCH_REG_RHDA, 0);
1122         sw_write_reg(SWITCH_REG_RLDA, KSEG1ADDR(rxl_descs));
1123
1124         for (i = 0; i < SWITCH_NUM_PORTS; i++) {
1125                 struct net_device *dev;
1126                 struct adm5120_if_priv *priv;
1127
1128                 dev = adm5120_if_alloc();
1129                 if (!dev) {
1130                         err = -ENOMEM;
1131                         goto err;
1132                 }
1133
1134                 adm5120_devs[i] = dev;
1135                 priv = netdev_priv(dev);
1136
1137                 priv->vlan_no = i;
1138                 priv->port_mask = adm5120_eth_vlans[i];
1139
1140                 memcpy(dev->dev_addr, adm5120_eth_macs[i], 6);
1141                 adm5120_write_mac(dev);
1142
1143                 err = register_netdev(dev);
1144                 if (err) {
1145                         SW_INFO("%s register failed, error=%d\n",
1146                                         dev->name, err);
1147                         goto err;
1148                 }
1149         }
1150
1151         /* setup vlan/port mapping after devs are filled up */
1152         adm5120_set_vlan(adm5120_eth_vlans);
1153
1154         /* enable CPU port */
1155         t = sw_read_reg(SWITCH_REG_CPUP_CONF);
1156         t &= ~CPUP_CONF_DCPUP;
1157         sw_write_reg(SWITCH_REG_CPUP_CONF, t);
1158
1159         return 0;
1160
1161 err:
1162         adm5120_switch_cleanup();
1163
1164         SW_ERR("init failed\n");
1165         return err;
1166 }
1167
1168 static int adm5120_switch_remove(struct platform_device *dev)
1169 {
1170         adm5120_switch_cleanup();
1171         return 0;
1172 }
1173
1174 static struct platform_driver adm5120_switch_driver = {
1175         .probe          = adm5120_switch_probe,
1176         .remove         = adm5120_switch_remove,
1177         .driver         = {
1178                 .name   = DRV_NAME,
1179         },
1180 };
1181
1182 /* -------------------------------------------------------------------------- */
1183
1184 static int __init adm5120_switch_mod_init(void)
1185 {
1186         int err;
1187
1188         pr_info(DRV_DESC " version " DRV_VERSION "\n");
1189         err = platform_driver_register(&adm5120_switch_driver);
1190
1191         return err;
1192 }
1193
1194 static void __exit adm5120_switch_mod_exit(void)
1195 {
1196         platform_driver_unregister(&adm5120_switch_driver);
1197 }
1198
1199 module_init(adm5120_switch_mod_init);
1200 module_exit(adm5120_switch_mod_exit);
1201
1202 MODULE_LICENSE("GPL v2");
1203 MODULE_AUTHOR("Gabor Juhos <juhosg at openwrt.org>");
1204 MODULE_DESCRIPTION(DRV_DESC);
1205 MODULE_VERSION(DRV_VERSION);