target/linux/ar71xx/files/drivers/net/phy/rtl8366_smi.c

   1 /*
   2  * Platform driver for the Realtek RTL8366 ethernet switch
   3  *
   4  * Copyright (C) 2009 Gabor Juhos <juhosg@openwrt.org>
   5  * Copyright (C) 2010 Antti Seppälä <a.seppala@gmail.com>
   6  *
   7  * This program is free software; you can redistribute it and/or modify it
   8  * under the terms of the GNU General Public License version 2 as published
   9  * by the Free Software Foundation.
  10  */
  11
  12 #include <linux/kernel.h>
  13 #include <linux/module.h>
  14 #include <linux/init.h>
  15 #include <linux/platform_device.h>
  16 #include <linux/delay.h>
  17 #include <linux/gpio.h>
  18 #include <linux/spinlock.h>
  19 #include <linux/skbuff.h>
  20 #include <linux/switch.h>
  21 #include <linux/phy.h>
  22 #include <linux/rtl8366_smi.h>
  23
  24 /* #define DEBUG 1 */
  25
  26 #ifdef DEBUG
  27 #include <linux/debugfs.h>
  28 #endif
  29
  30 #define RTL8366_SMI_DRIVER_NAME "rtl8366-smi"
  31 #define RTL8366_SMI_DRIVER_DESC "Realtek RTL8366 switch driver"
  32 #define RTL8366_SMI_DRIVER_VER  "0.1.1"
  33
  34 #define RTL8366S_PHY_NO_MAX                 4
  35 #define RTL8366S_PHY_PAGE_MAX               7
  36 #define RTL8366S_PHY_ADDR_MAX               31
  37
  38 #define RTL8366_CHIP_GLOBAL_CTRL_REG        0x0000
  39 #define RTL8366_CHIP_CTRL_VLAN              (1 << 13)
  40
  41 #define RTL8366_RESET_CTRL_REG              0x0100
  42 #define RTL8366_CHIP_CTRL_RESET_HW          1
  43 #define RTL8366_CHIP_CTRL_RESET_SW          (1 << 1)
  44
  45 #define RTL8366S_CHIP_VERSION_CTRL_REG      0x0104
  46 #define RTL8366S_CHIP_VERSION_MASK          0xf
  47 #define RTL8366S_CHIP_ID_REG                0x0105
  48 #define RTL8366S_CHIP_ID_8366               0x8366
  49
  50 /* PHY registers control */
  51 #define RTL8366S_PHY_ACCESS_CTRL_REG        0x8028
  52 #define RTL8366S_PHY_ACCESS_DATA_REG        0x8029
  53
  54 #define RTL8366S_PHY_CTRL_READ              1
  55 #define RTL8366S_PHY_CTRL_WRITE             0
  56
  57 #define RTL8366S_PHY_REG_MASK               0x1f
  58 #define RTL8366S_PHY_PAGE_OFFSET            5
  59 #define RTL8366S_PHY_PAGE_MASK              (0x7 << 5)
  60 #define RTL8366S_PHY_NO_OFFSET              9
  61 #define RTL8366S_PHY_NO_MASK                (0x1f << 9)
  62
  63 #define RTL8366_SMI_ACK_RETRY_COUNT         5
  64 #define RTL8366_SMI_CLK_DELAY               10 /* nsec */
  65
  66 /* LED control registers */
  67 #define RTL8366_LED_BLINKRATE_REG           0x0420
  68 #define RTL8366_LED_BLINKRATE_BIT           0
  69 #define RTL8366_LED_BLINKRATE_MASK          0x0007
  70
  71 #define RTL8366_LED_CTRL_REG                0x0421
  72 #define RTL8366_LED_0_1_CTRL_REG            0x0422
  73 #define RTL8366_LED_2_3_CTRL_REG            0x0423
  74
  75 #define RTL8366S_MIB_COUNT                  33
  76 #define RTL8366S_GLOBAL_MIB_COUNT           1
  77 #define RTL8366S_MIB_COUNTER_PORT_OFFSET    0x0040
  78 #define RTL8366S_MIB_COUNTER_BASE           0x1000
  79 #define RTL8366S_MIB_CTRL_REG               0x11F0
  80 #define RTL8366S_MIB_CTRL_USER_MASK         0x01FF
  81 #define RTL8366S_MIB_CTRL_BUSY_MASK         0x0001
  82 #define RTL8366S_MIB_CTRL_RESET_MASK        0x0002
  83
  84 #define RTL8366S_MIB_CTRL_GLOBAL_RESET_MASK 0x0004
  85 #define RTL8366S_MIB_CTRL_PORT_RESET_BIT    0x0003
  86 #define RTL8366S_MIB_CTRL_PORT_RESET_MASK   0x01FC
  87
  88
  89 #define RTL8366S_PORT_VLAN_CTRL_BASE        0x0058
  90 #define RTL8366S_VLAN_TABLE_READ_BASE       0x018B
  91 #define RTL8366S_VLAN_TABLE_WRITE_BASE      0x0185
  92
  93 #define RTL8366S_VLAN_TB_CTRL_REG           0x010F
  94
  95 #define RTL8366S_TABLE_ACCESS_CTRL_REG      0x0180
  96 #define RTL8366S_TABLE_VLAN_READ_CTRL       0x0E01
  97 #define RTL8366S_TABLE_VLAN_WRITE_CTRL      0x0F01
  98
  99 #define RTL8366S_VLAN_MEMCONF_BASE          0x0016
 100
 101
 102 #define RTL8366S_PORT_LINK_STATUS_BASE      0x0060
 103 #define RTL8366S_PORT_STATUS_SPEED_MASK     0x0003
 104 #define RTL8366S_PORT_STATUS_DUPLEX_MASK    0x0004
 105 #define RTL8366S_PORT_STATUS_LINK_MASK      0x0010
 106 #define RTL8366S_PORT_STATUS_TXPAUSE_MASK   0x0020
 107 #define RTL8366S_PORT_STATUS_RXPAUSE_MASK   0x0040
 108 #define RTL8366S_PORT_STATUS_AN_MASK        0x0080
 109
 110
 111 #define RTL8366_PORT_NUM_CPU                5
 112 #define RTL8366_NUM_PORTS                   6
 113 #define RTL8366_NUM_VLANS                   16
 114 #define RTL8366_NUM_LEDGROUPS               4
 115 #define RTL8366_NUM_VIDS                    4096
 116 #define RTL8366S_PRIORITYMAX                7
 117 #define RTL8366S_FIDMAX                     7
 118
 119
 120 #define RTL8366_PORT_1                      (1 << 0) /* In userspace port 0 */
 121 #define RTL8366_PORT_2                      (1 << 1) /* In userspace port 1 */
 122 #define RTL8366_PORT_3                      (1 << 2) /* In userspace port 2 */
 123 #define RTL8366_PORT_4                      (1 << 3) /* In userspace port 3 */
 124
 125 #define RTL8366_PORT_UNKNOWN                (1 << 4) /* No known connection */
 126 #define RTL8366_PORT_CPU                    (1 << 5) /* CPU port */
 127
 128 #define RTL8366_PORT_ALL                    (RTL8366_PORT_1 |       \
 129                                              RTL8366_PORT_2 |       \
 130                                              RTL8366_PORT_3 |       \
 131                                              RTL8366_PORT_4 |       \
 132                                              RTL8366_PORT_UNKNOWN | \
 133                                              RTL8366_PORT_CPU)
 134
 135 #define RTL8366_PORT_ALL_BUT_CPU            (RTL8366_PORT_1 |       \
 136                                              RTL8366_PORT_2 |       \
 137                                              RTL8366_PORT_3 |       \
 138                                              RTL8366_PORT_4 |       \
 139                                              RTL8366_PORT_UNKNOWN)
 140
 141 #define RTL8366_PORT_ALL_EXTERNAL           (RTL8366_PORT_1 |       \
 142                                              RTL8366_PORT_2 |       \
 143                                              RTL8366_PORT_3 |       \
 144                                              RTL8366_PORT_4)
 145
 146 #define RTL8366_PORT_ALL_INTERNAL           (RTL8366_PORT_UNKNOWN | \
 147                                              RTL8366_PORT_CPU)
 148
 149 struct rtl8366s_vlanconfig {
 150         u16     reserved2:1;
 151         u16     priority:3;
 152         u16     vid:12;
 153
 154         u16     reserved1:1;
 155         u16     fid:3;
 156         u16     untag:6;
 157         u16     member:6;
 158 };
 159
 160 struct rtl8366s_vlan4kentry {
 161         u16     reserved1:4;
 162         u16     vid:12;
 163
 164         u16     reserved2:1;
 165         u16     fid:3;
 166         u16     untag:6;
 167         u16     member:6;
 168 };
 169
 170 static const char *MIBCOUNTERS[] = { "IfInOctets                        ",
 171                                      "EtherStatsOctets                  ",
 172                                      "EtherStatsUnderSizePkts           ",
 173                                      "EtherFregament                    ",
 174                                      "EtherStatsPkts64Octets            ",
 175                                      "EtherStatsPkts65to127Octets       ",
 176                                      "EtherStatsPkts128to255Octets      ",
 177                                      "EtherStatsPkts256to511Octets      ",
 178                                      "EtherStatsPkts512to1023Octets     ",
 179                                      "EtherStatsPkts1024to1518Octets    ",
 180                                      "EtherOversizeStats                ",
 181                                      "EtherStatsJabbers                 ",
 182                                      "IfInUcastPkts                     ",
 183                                      "EtherStatsMulticastPkts           ",
 184                                      "EtherStatsBroadcastPkts           ",
 185                                      "EtherStatsDropEvents              ",
 186                                      "Dot3StatsFCSErrors                ",
 187                                      "Dot3StatsSymbolErrors             ",
 188                                      "Dot3InPauseFrames                 ",
 189                                      "Dot3ControlInUnknownOpcodes       ",
 190                                      "IfOutOctets                       ",
 191                                      "Dot3StatsSingleCollisionFrames    ",
 192                                      "Dot3StatMultipleCollisionFrames   ",
 193                                      "Dot3sDeferredTransmissions        ",
 194                                      "Dot3StatsLateCollisions           ",
 195                                      "EtherStatsCollisions              ",
 196                                      "Dot3StatsExcessiveCollisions      ",
 197                                      "Dot3OutPauseFrames                ",
 198                                      "Dot1dBasePortDelayExceededDiscards",
 199                                      "Dot1dTpPortInDiscards             ",
 200                                      "IfOutUcastPkts                    ",
 201                                      "IfOutMulticastPkts                ",
 202                                      "IfOutBroadcastPkts                ",
 203                                      NULL };
 204
 205 struct rtl8366_smi {
 206         struct platform_device             *pdev;
 207         struct rtl8366_smi_platform_data   *pdata;
 208         spinlock_t                         lock;
 209         struct mii_bus                     *mii_bus;
 210         struct switch_dev                  dev;
 211         int                                mii_irq[PHY_MAX_ADDR];
 212         char buf[4096];
 213 #ifdef DEBUG
 214         struct dentry                      *debugfs_root;
 215 #endif
 216 };
 217
 218 #ifdef DEBUG
 219 u16 g_dbg_reg;
 220 #endif
 221
 222 static inline struct rtl8366_smi *sw_to_rtl8366(struct switch_dev *sw)
 223 {
 224         return container_of(sw, struct rtl8366_smi, dev);
 225 }
 226
 227 static inline void rtl8366_smi_clk_delay(struct rtl8366_smi *smi)
 228 {
 229         ndelay(RTL8366_SMI_CLK_DELAY);
 230 }
 231
 232 static void rtl8366_smi_start(struct rtl8366_smi *smi)
 233 {
 234         unsigned int sda = smi->pdata->gpio_sda;
 235         unsigned int sck = smi->pdata->gpio_sck;
 236
 237         /*
 238          * Set GPIO pins to output mode, with initial state:
 239          * SCK = 0, SDA = 1
 240          */
 241         gpio_direction_output(sck, 0);
 242         gpio_direction_output(sda, 1);
 243         rtl8366_smi_clk_delay(smi);
 244
 245         /* CLK 1: 0 -> 1, 1 -> 0 */
 246         gpio_set_value(sck, 1);
 247         rtl8366_smi_clk_delay(smi);
 248         gpio_set_value(sck, 0);
 249         rtl8366_smi_clk_delay(smi);
 250
 251         /* CLK 2: */
 252         gpio_set_value(sck, 1);
 253         rtl8366_smi_clk_delay(smi);
 254         gpio_set_value(sda, 0);
 255         rtl8366_smi_clk_delay(smi);
 256         gpio_set_value(sck, 0);
 257         rtl8366_smi_clk_delay(smi);
 258         gpio_set_value(sda, 1);
 259 }
 260
 261 static void rtl8366_smi_stop(struct rtl8366_smi *smi)
 262 {
 263         unsigned int sda = smi->pdata->gpio_sda;
 264         unsigned int sck = smi->pdata->gpio_sck;
 265
 266         rtl8366_smi_clk_delay(smi);
 267         gpio_set_value(sda, 0);
 268         gpio_set_value(sck, 1);
 269         rtl8366_smi_clk_delay(smi);
 270         gpio_set_value(sda, 1);
 271         rtl8366_smi_clk_delay(smi);
 272         gpio_set_value(sck, 1);
 273         rtl8366_smi_clk_delay(smi);
 274         gpio_set_value(sck, 0);
 275         rtl8366_smi_clk_delay(smi);
 276         gpio_set_value(sck, 1);
 277
 278         /* add a click */
 279         rtl8366_smi_clk_delay(smi);
 280         gpio_set_value(sck, 0);
 281         rtl8366_smi_clk_delay(smi);
 282         gpio_set_value(sck, 1);
 283
 284         /* set GPIO pins to input mode */
 285         gpio_direction_input(sda);
 286         gpio_direction_input(sck);
 287 }
 288
 289 static void rtl8366_smi_write_bits(struct rtl8366_smi *smi, u32 data, u32 len)
 290 {
 291         unsigned int sda = smi->pdata->gpio_sda;
 292         unsigned int sck = smi->pdata->gpio_sck;
 293
 294         for (; len > 0; len--) {
 295                 rtl8366_smi_clk_delay(smi);
 296
 297                 /* prepare data */
 298                 if ( data & ( 1 << (len - 1)) )
 299                         gpio_set_value(sda, 1);
 300                 else
 301                         gpio_set_value(sda, 0);
 302                 rtl8366_smi_clk_delay(smi);
 303
 304                 /* clocking */
 305                 gpio_set_value(sck, 1);
 306                 rtl8366_smi_clk_delay(smi);
 307                 gpio_set_value(sck, 0);
 308         }
 309 }
 310
 311 static void rtl8366_smi_read_bits(struct rtl8366_smi *smi, u32 len, u32 *data)
 312 {
 313         unsigned int sda = smi->pdata->gpio_sda;
 314         unsigned int sck = smi->pdata->gpio_sck;
 315
 316         gpio_direction_input(sda);
 317
 318         for (*data = 0; len > 0; len--) {
 319                 u32 u;
 320
 321                 rtl8366_smi_clk_delay(smi);
 322
 323                 /* clocking */
 324                 gpio_set_value(sck, 1);
 325                 rtl8366_smi_clk_delay(smi);
 326                 u = gpio_get_value(sda);
 327                 gpio_set_value(sck, 0);
 328
 329                 *data |= (u << (len - 1));
 330         }
 331
 332         gpio_direction_output(sda, 0);
 333 }
 334
 335 static int rtl8366_smi_wait_for_ack(struct rtl8366_smi *smi)
 336 {
 337         int retry_cnt;
 338
 339         retry_cnt = 0;
 340         do {
 341                 u32 ack;
 342
 343                 rtl8366_smi_read_bits(smi, 1, &ack);
 344                 if (ack == 0)
 345                         break;
 346
 347                 if (++retry_cnt > RTL8366_SMI_ACK_RETRY_COUNT)
 348                         return -EIO;
 349         } while (1);
 350
 351         return 0;
 352 }
 353
 354 static int rtl8366_smi_write_byte(struct rtl8366_smi *smi, u8 data)
 355 {
 356         rtl8366_smi_write_bits(smi, data, 8);
 357         return rtl8366_smi_wait_for_ack(smi);
 358 }
 359
 360 static int rtl8366_smi_read_byte0(struct rtl8366_smi *smi, u8 *data)
 361 {
 362         u32 t;
 363
 364         /* read data */
 365         rtl8366_smi_read_bits(smi, 8, &t);
 366         *data = (t & 0xff);
 367
 368         /* send an ACK */
 369         rtl8366_smi_write_bits(smi, 0x00, 1);
 370
 371         return 0;
 372 }
 373
 374 static int rtl8366_smi_read_byte1(struct rtl8366_smi *smi, u8 *data)
 375 {
 376         u32 t;
 377
 378         /* read data */
 379         rtl8366_smi_read_bits(smi, 8, &t);
 380         *data = (t & 0xff);
 381
 382         /* send an ACK */
 383         rtl8366_smi_write_bits(smi, 0x01, 1);
 384
 385         return 0;
 386 }
 387
 388 static int rtl8366_smi_read_reg(struct rtl8366_smi *smi, u32 addr, u32 *data)
 389 {
 390         unsigned long flags;
 391         u8 lo = 0;
 392         u8 hi = 0;
 393         int ret;
 394
 395         spin_lock_irqsave(&smi->lock, flags);
 396
 397         rtl8366_smi_start(smi);
 398
 399         /* send READ command */
 400         ret = rtl8366_smi_write_byte(smi, 0x0a << 4 | 0x04 << 1 | 0x01);
 401         if (ret)
 402                 goto out;
 403
 404         /* set ADDR[7:0] */
 405         ret = rtl8366_smi_write_byte(smi, addr & 0xff);
 406         if (ret)
 407                 goto out;
 408
 409         /* set ADDR[15:8] */
 410         ret = rtl8366_smi_write_byte(smi, addr >> 8);
 411         if (ret)
 412                 goto out;
 413
 414         /* read DATA[7:0] */
 415         rtl8366_smi_read_byte0(smi, &lo);
 416         /* read DATA[15:8] */
 417         rtl8366_smi_read_byte1(smi, &hi);
 418
 419         *data = ((u32) lo) | (((u32) hi) << 8);
 420
 421         ret = 0;
 422
 423  out:
 424         rtl8366_smi_stop(smi);
 425         spin_unlock_irqrestore(&smi->lock, flags);
 426
 427         return ret;
 428 }
 429
 430 static int rtl8366_smi_write_reg(struct rtl8366_smi *smi, u32 addr, u32 data)
 431 {
 432         unsigned long flags;
 433         int ret;
 434
 435         spin_lock_irqsave(&smi->lock, flags);
 436
 437         rtl8366_smi_start(smi);
 438
 439         /* send WRITE command */
 440         ret = rtl8366_smi_write_byte(smi, 0x0a << 4 | 0x04 << 1 | 0x00);
 441         if (ret)
 442                 goto out;
 443
 444         /* set ADDR[7:0] */
 445         ret = rtl8366_smi_write_byte(smi, addr & 0xff);
 446         if (ret)
 447                 goto out;
 448
 449         /* set ADDR[15:8] */
 450         ret = rtl8366_smi_write_byte(smi, addr >> 8);
 451         if (ret)
 452                 goto out;
 453
 454         /* write DATA[7:0] */
 455         ret = rtl8366_smi_write_byte(smi, data & 0xff);
 456         if (ret)
 457                 goto out;
 458
 459         /* write DATA[15:8] */
 460         ret = rtl8366_smi_write_byte(smi, data >> 8);
 461         if (ret)
 462                 goto out;
 463
 464         ret = 0;
 465
 466  out:
 467         rtl8366_smi_stop(smi);
 468         spin_unlock_irqrestore(&smi->lock, flags);
 469
 470         return ret;
 471 }
 472
 473 static int rtl8366_smi_read_phy_reg(struct rtl8366_smi *smi,
 474                                     u32 phy_no, u32 page, u32 addr, u32 *data)
 475 {
 476         u32 reg;
 477         int ret;
 478
 479         if (phy_no > RTL8366S_PHY_NO_MAX)
 480                 return -EINVAL;
 481
 482         if (page > RTL8366S_PHY_PAGE_MAX)
 483                 return -EINVAL;
 484
 485         if (addr > RTL8366S_PHY_ADDR_MAX)
 486                 return -EINVAL;
 487
 488         ret = rtl8366_smi_write_reg(smi, RTL8366S_PHY_ACCESS_CTRL_REG,
 489                                     RTL8366S_PHY_CTRL_READ);
 490         if (ret)
 491                 return ret;
 492
 493         reg = 0x8000 | (1 << (phy_no + RTL8366S_PHY_NO_OFFSET)) |
 494               ((page << RTL8366S_PHY_PAGE_OFFSET) & RTL8366S_PHY_PAGE_MASK) |
 495               (addr & RTL8366S_PHY_REG_MASK);
 496
 497         ret = rtl8366_smi_write_reg(smi, reg, 0);
 498         if (ret)
 499                 return ret;
 500
 501         ret = rtl8366_smi_read_reg(smi, RTL8366S_PHY_ACCESS_DATA_REG, data);
 502         if (ret)
 503                 return ret;
 504
 505         return 0;
 506 }
 507
 508 static int rtl8366_smi_write_phy_reg(struct rtl8366_smi *smi,
 509                                      u32 phy_no, u32 page, u32 addr, u32 data)
 510 {
 511         u32 reg;
 512         int ret;
 513
 514         if (phy_no > RTL8366S_PHY_NO_MAX)
 515                 return -EINVAL;
 516
 517         if (page > RTL8366S_PHY_PAGE_MAX)
 518                 return -EINVAL;
 519
 520         if (addr > RTL8366S_PHY_ADDR_MAX)
 521                 return -EINVAL;
 522
 523         ret = rtl8366_smi_write_reg(smi, RTL8366S_PHY_ACCESS_CTRL_REG,
 524                                     RTL8366S_PHY_CTRL_WRITE);
 525         if (ret)
 526                 return ret;
 527
 528         reg = 0x8000 | (1 << (phy_no + RTL8366S_PHY_NO_OFFSET)) |
 529               ((page << RTL8366S_PHY_PAGE_OFFSET) & RTL8366S_PHY_PAGE_MASK) |
 530               (addr & RTL8366S_PHY_REG_MASK);
 531
 532         ret = rtl8366_smi_write_reg(smi, reg, data);
 533         if (ret)
 534                 return ret;
 535
 536         return 0;
 537 }
 538
 539 static int rtl8366_get_mib_counter(struct rtl8366_smi *smi, int counter,
 540                                    int port, unsigned long long *val)
 541 {
 542         int i;
 543         int err;
 544         u32 addr, data, regoffset;
 545         u64 mibvalue;
 546
 547         /* address offset to MIBs counter */
 548         const u16 mibLength[RTL8366S_MIB_COUNT] = {4, 4, 2, 2, 2, 2, 2, 2, 2,
 549                                                    2, 2, 2, 2, 2, 2, 2, 2, 2,
 550                                                    2, 2, 4, 2, 2, 2, 2, 2, 2,
 551                                                    2, 2, 2, 2, 2, 2};
 552
 553         if (port > RTL8366_NUM_PORTS || counter >= RTL8366S_MIB_COUNT)
 554                 return -EINVAL;
 555
 556         i = 0;
 557         regoffset = RTL8366S_MIB_COUNTER_PORT_OFFSET * (port);
 558
 559         while (i < counter) {
 560                 regoffset += mibLength[i];
 561                 i++;
 562         }
 563
 564         addr = RTL8366S_MIB_COUNTER_BASE + regoffset;
 565
 566
 567         /* writing access counter address first */
 568         /* then ASIC will prepare 64bits counter wait for being retrived */
 569         data = 0;/* writing data will be discard by ASIC */
 570         err = rtl8366_smi_write_reg(smi, addr, data);
 571         if (err)
 572                 return err;
 573
 574         /* read MIB control register */
 575         err =  rtl8366_smi_read_reg(smi, RTL8366S_MIB_CTRL_REG, &data);
 576         if (err)
 577                 return err;
 578
 579         if (data & RTL8366S_MIB_CTRL_BUSY_MASK)
 580                 return -EBUSY;
 581
 582         if (data & RTL8366S_MIB_CTRL_RESET_MASK)
 583                 return -EIO;
 584
 585         mibvalue = 0;
 586         addr = addr + mibLength[counter] - 1;
 587         i = mibLength[counter];
 588
 589         while (i) {
 590                 err = rtl8366_smi_read_reg(smi, addr, &data);
 591                 if (err)
 592                         return err;
 593
 594                 mibvalue = (mibvalue << 16) | (data & 0xFFFF);
 595
 596                 addr--;
 597                 i--;
 598         }
 599
 600         *val = mibvalue;
 601         return 0;
 602 }
 603
 604 static int rtl8366s_get_vlan_4k_entry(struct rtl8366_smi *smi, u32 vid,
 605                                       struct rtl8366s_vlan4kentry *vlan4k)
 606 {
 607         int err;
 608         u32 data;
 609         u16 *tableaddr;
 610
 611         memset(vlan4k, '\0', sizeof(struct rtl8366s_vlan4kentry));
 612         vlan4k->vid = vid;
 613
 614         if (vid >= RTL8366_NUM_VIDS)
 615                 return -EINVAL;
 616
 617         tableaddr = (u16 *)vlan4k;
 618
 619         /* write VID */
 620         data = *tableaddr;
 621         err = rtl8366_smi_write_reg(smi, RTL8366S_VLAN_TABLE_WRITE_BASE, data);
 622         if (err)
 623                 return err;
 624
 625         /* write table access control word */
 626         err = rtl8366_smi_write_reg(smi, RTL8366S_TABLE_ACCESS_CTRL_REG,
 627                                     RTL8366S_TABLE_VLAN_READ_CTRL);
 628         if (err)
 629                 return err;
 630
 631         err = rtl8366_smi_read_reg(smi, RTL8366S_VLAN_TABLE_READ_BASE, &data);
 632         if (err)
 633                 return err;
 634
 635         *tableaddr = data;
 636         tableaddr++;
 637
 638         err = rtl8366_smi_read_reg(smi, RTL8366S_VLAN_TABLE_READ_BASE + 1,
 639                                    &data);
 640         if (err)
 641                 return err;
 642
 643         *tableaddr = data;
 644         vlan4k->vid = vid;
 645
 646         return 0;
 647 }
 648
 649 static int rtl8366s_set_vlan_4k_entry(struct rtl8366_smi *smi,
 650                                       const struct rtl8366s_vlan4kentry *vlan4k)
 651 {
 652         int err;
 653         u32 data;
 654         u16 *tableaddr;
 655
 656         if (vlan4k->vid >= RTL8366_NUM_VIDS ||
 657             vlan4k->member > RTL8366_PORT_ALL ||
 658             vlan4k->untag > RTL8366_PORT_ALL ||
 659             vlan4k->fid > RTL8366S_FIDMAX)
 660                 return -EINVAL;
 661
 662         tableaddr = (u16 *)vlan4k;
 663
 664         data = *tableaddr;
 665
 666         err = rtl8366_smi_write_reg(smi, RTL8366S_VLAN_TABLE_WRITE_BASE, data);
 667         if (err)
 668                 return err;
 669
 670         tableaddr++;
 671
 672         data = *tableaddr;
 673
 674         rtl8366_smi_write_reg(smi, RTL8366S_VLAN_TABLE_WRITE_BASE + 1, data);
 675
 676
 677         /* write table access control word */
 678         err = rtl8366_smi_write_reg(smi, RTL8366S_TABLE_ACCESS_CTRL_REG,
 679                                     RTL8366S_TABLE_VLAN_WRITE_CTRL);
 680         if (err)
 681                 return err;
 682
 683         return 0;
 684 }
 685
 686 static int rtl8366s_get_vlan_member_config(struct rtl8366_smi *smi, u32 index,
 687                                            struct rtl8366s_vlanconfig *vlanmc)
 688 {
 689         int err;
 690         u32 addr;
 691         u32 data;
 692         u16 *tableaddr;
 693
 694         memset(vlanmc, '\0', sizeof(struct rtl8366s_vlanconfig));
 695
 696         if (index >= RTL8366_NUM_VLANS)
 697                 return -EINVAL;
 698
 699         tableaddr = (u16 *)vlanmc;
 700
 701         addr = RTL8366S_VLAN_MEMCONF_BASE + (index << 1);
 702
 703         err = rtl8366_smi_read_reg(smi, addr, &data);
 704         if (err)
 705                 return err;
 706
 707         *tableaddr = data;
 708         tableaddr++;
 709
 710         addr = RTL8366S_VLAN_MEMCONF_BASE + 1 + (index << 1);
 711
 712         err = rtl8366_smi_read_reg(smi, addr, &data);
 713         if (err)
 714                 return err;
 715
 716         *tableaddr = data;
 717
 718         return 0;
 719 }
 720
 721 static int rtl8366s_set_vlan_member_config(struct rtl8366_smi *smi, u32 index,
 722                                            const struct rtl8366s_vlanconfig
 723                                            *vlanmc)
 724 {
 725         int err;
 726         u32 addr;
 727         u32 data;
 728         u16 *tableaddr;
 729
 730         if (index >= RTL8366_NUM_VLANS ||
 731            vlanmc->vid >= RTL8366_NUM_VIDS ||
 732            vlanmc->priority > RTL8366S_PRIORITYMAX ||
 733            vlanmc->member > RTL8366_PORT_ALL ||
 734            vlanmc->untag > RTL8366_PORT_ALL ||
 735            vlanmc->fid > RTL8366S_FIDMAX)
 736                 return -EINVAL;
 737
 738         addr = RTL8366S_VLAN_MEMCONF_BASE + (index << 1);
 739
 740
 741         tableaddr = (u16 *)vlanmc;
 742         data = *tableaddr;
 743
 744         err = rtl8366_smi_write_reg(smi, addr, data);
 745         if (err)
 746                 return err;
 747
 748         addr = RTL8366S_VLAN_MEMCONF_BASE + 1 + (index << 1);
 749
 750         tableaddr++;
 751         data = *tableaddr;
 752
 753         err = rtl8366_smi_write_reg(smi, addr, data);
 754         if (err)
 755                 return err;
 756
 757         return 0;
 758 }
 759
 760 static int rtl8366_get_port_vlan_index(struct rtl8366_smi *smi, int port,
 761                                        int *val)
 762 {
 763         int err;
 764         u32 addr;
 765         u32 data;
 766
 767         /* bits mapping to port vlan control register of port n */
 768         const u16 bits[RTL8366_NUM_PORTS] = { 0x000F, 0x00F0, 0x0F00,
 769                                              0xF000, 0x000F, 0x00F0 };
 770         /* bits offset to port vlan control register of port n */
 771         const u16 bitoffset[RTL8366_NUM_PORTS] = { 0, 4, 8, 12, 0, 4 };
 772         /* address offset to port vlan control register of port n */
 773         const u16 addroffset[RTL8366_NUM_PORTS] = { 0, 0, 0, 0, 1, 1 };
 774
 775         if (port >= RTL8366_NUM_PORTS)
 776                 return -EINVAL;
 777
 778         addr = RTL8366S_PORT_VLAN_CTRL_BASE + addroffset[port];
 779
 780         err = rtl8366_smi_read_reg(smi, addr, &data);
 781         if (err)
 782                 return err;
 783
 784         *val = (data & bits[port]) >> bitoffset[port];
 785
 786         return 0;
 787
 788 }
 789
 790 static int rtl8366_get_vlan_port_pvid(struct rtl8366_smi *smi, int port,
 791                                       int *val)
 792 {
 793         int err;
 794         int index;
 795         struct rtl8366s_vlanconfig vlanmc;
 796
 797         err = rtl8366_get_port_vlan_index(smi, port, &index);
 798         if (err)
 799                 return err;
 800
 801         err = rtl8366s_get_vlan_member_config(smi, index, &vlanmc);
 802         if (err)
 803                 return err;
 804
 805         *val = vlanmc.vid;
 806         return 0;
 807 }
 808
 809 static int rtl8366_set_port_vlan_index(struct rtl8366_smi *smi, int port,
 810                                        int index)
 811 {
 812         int err;
 813         u32 addr;
 814         u32 data;
 815         u32 vlan_data;
 816         u32 bits;
 817
 818         /* bits mapping to port vlan control register of port n */
 819         const u16 bitmasks[6] = { 0x000F, 0x00F0, 0x0F00,
 820                                  0xF000, 0x000F, 0x00F0 };
 821         /* bits offset to port vlan control register of port n */
 822         const u16 bitOff[6] = { 0, 4, 8, 12, 0, 4 };
 823         /* address offset to port vlan control register of port n */
 824         const u16 addrOff[6] = { 0, 0, 0, 0, 1, 1 };
 825
 826         if (port >= RTL8366_NUM_PORTS || index >= RTL8366_NUM_VLANS)
 827                 return -EINVAL;
 828
 829         addr = RTL8366S_PORT_VLAN_CTRL_BASE + addrOff[port];
 830
 831         bits = bitmasks[port];
 832
 833         data = (index << bitOff[port]) & bits;
 834
 835         err = rtl8366_smi_read_reg(smi, addr, &vlan_data);
 836         if (err)
 837                 return err;
 838
 839         vlan_data &= ~(vlan_data & bits);
 840         vlan_data |= data;
 841
 842         err = rtl8366_smi_write_reg(smi, addr, vlan_data);
 843         if (err)
 844                 return err;
 845
 846         return 0;
 847 }
 848
 849 static int rtl8366_set_vlan_port_pvid(struct rtl8366_smi *smi, int port,
 850                                       int val)
 851 {
 852         int i;
 853         struct rtl8366s_vlanconfig vlanmc;
 854         struct rtl8366s_vlan4kentry vlan4k;
 855
 856         if (port >= RTL8366_NUM_PORTS || val >= RTL8366_NUM_VIDS)
 857                 return -EINVAL;
 858
 859
 860
 861         /* Updating the 4K entry; lookup it and change the port member set */
 862         rtl8366s_get_vlan_4k_entry(smi, val, &vlan4k);
 863         vlan4k.member |= ((1 << port) | RTL8366_PORT_CPU);
 864         vlan4k.untag = RTL8366_PORT_ALL_BUT_CPU;
 865         rtl8366s_set_vlan_4k_entry(smi, &vlan4k);
 866
 867         /* For the 16 entries more work needs to be done. First see if such
 868            VID is already there and change it */
 869         for (i = 0; i < RTL8366_NUM_VLANS; ++i) {
 870                 rtl8366s_get_vlan_member_config(smi, i, &vlanmc);
 871
 872                 /* Try to find an existing vid and update port member set */
 873                 if (val == vlanmc.vid) {
 874                         vlanmc.member |= ((1 << port) | RTL8366_PORT_CPU);
 875                         rtl8366s_set_vlan_member_config(smi, i, &vlanmc);
 876
 877                         /* Now update PVID register settings */
 878                         rtl8366_set_port_vlan_index(smi, port, i);
 879
 880                         return 0;
 881                 }
 882         }
 883
 884         /* PVID could not be found from vlan table. Replace unused (one that
 885            has no member ports) with new one */
 886         for (i = 0; i < RTL8366_NUM_VLANS; ++i) {
 887                 rtl8366s_get_vlan_member_config(smi, i, &vlanmc);
 888
 889                 /* See if this vlan member configuration is unused. It is
 890                    unused if member set contains no ports or CPU port only */
 891                 if (!vlanmc.member || vlanmc.member == RTL8366_PORT_CPU) {
 892                         vlanmc.vid = val;
 893                         vlanmc.priority = 0;
 894                         vlanmc.untag = RTL8366_PORT_ALL_BUT_CPU;
 895                         vlanmc.member = ((1 << port) | RTL8366_PORT_CPU);
 896                         vlanmc.fid = 0;
 897
 898                         rtl8366s_set_vlan_member_config(smi, i, &vlanmc);
 899
 900                         /* Now update PVID register settings */
 901                         rtl8366_set_port_vlan_index(smi, port, i);
 902
 903                         return 0;
 904                 }
 905         }
 906
 907         dev_err(&smi->pdev->dev, "All 16 vlan member configurations are in "
 908                 "use\n");
 909         return -EINVAL;
 910 }
 911
 912
 913 static int rtl8366_vlan_set_vlan(struct rtl8366_smi *smi, int enable)
 914 {
 915         u32 data = 0;
 916         rtl8366_smi_read_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, &data);
 917
 918         data &= ~(data & RTL8366_CHIP_CTRL_VLAN);
 919         if (enable)
 920                 data |= RTL8366_CHIP_CTRL_VLAN;
 921
 922         return rtl8366_smi_write_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, data);
 923 }
 924
 925 static int rtl8366_vlan_set_4ktable(struct rtl8366_smi *smi, int enable)
 926 {
 927         u32 data = 0;
 928         rtl8366_smi_read_reg(smi, RTL8366S_VLAN_TB_CTRL_REG, &data);
 929
 930         data &= ~(data & 1);
 931         if (enable)
 932                 data |= 1;
 933
 934         return rtl8366_smi_write_reg(smi, RTL8366S_VLAN_TB_CTRL_REG, data);
 935 }
 936
 937 static int rtl8366s_reset_vlan(struct rtl8366_smi *smi)
 938 {
 939         int i;
 940         struct rtl8366s_vlan4kentry vlan4k;
 941         struct rtl8366s_vlanconfig vlanmc;
 942
 943         /* clear 16 VLAN member configuration */
 944         for (i = 0; i < RTL8366_NUM_VLANS; i++) {
 945                 vlanmc.vid = 0;
 946                 vlanmc.priority = 0;
 947                 vlanmc.member = 0;
 948                 vlanmc.untag = 0;
 949                 vlanmc.fid = 0;
 950                 if (rtl8366s_set_vlan_member_config(smi, i, &vlanmc) != 0)
 951                         return -EIO;
 952         }
 953
 954         /* Set a default VLAN with vid 1 to 4K table for all ports */
 955         vlan4k.vid = 1;
 956         vlan4k.member = RTL8366_PORT_ALL;
 957         vlan4k.untag = RTL8366_PORT_ALL;
 958         vlan4k.fid = 0;
 959         if (rtl8366s_set_vlan_4k_entry(smi, &vlan4k) != 0)
 960                 return -EIO;
 961
 962         /* Set all ports PVID to default VLAN */
 963         for (i = 0; i < RTL8366_NUM_PORTS; i++) {
 964                 if (rtl8366_set_vlan_port_pvid(smi, i, 0) != 0)
 965                         return -EIO;
 966         }
 967
 968         return 0;
 969 }
 970
 971 #ifdef DEBUG
 972 static int rtl8366_debugfs_open(struct inode *inode, struct file *file)
 973 {
 974         file->private_data = inode->i_private;
 975         return 0;
 976 }
 977
 978 static ssize_t rtl8366_read_debugfs_mibs(struct file *file,
 979                                          char __user *user_buf,
 980                                          size_t count, loff_t *ppos)
 981 {
 982         int i, j, len = 0;
 983         struct rtl8366_smi *smi = (struct rtl8366_smi *)file->private_data;
 984         char *buf = smi->buf;
 985
 986         len += snprintf(buf + len, sizeof(smi->buf) - len, "MIB Counters:\n");
 987         len += snprintf(buf + len, sizeof(smi->buf) - len, "Counter"
 988                         "                            "
 989                         "Port 0 \t\t Port 1 \t\t Port 2 \t\t Port 3 \t\t "
 990                         "Port 4\n");
 991
 992         for (i = 0; i < 33; ++i) {
 993
 994                 len += snprintf(buf + len, sizeof(smi->buf) - len, "%d:%s ",
 995                                 i, MIBCOUNTERS[i]);
 996                 for (j = 0; j < RTL8366_NUM_PORTS; ++j) {
 997                         unsigned long long counter = 0;
 998
 999                         if (!rtl8366_get_mib_counter(smi, i, j, &counter))
1000                                 len += snprintf(buf + len, sizeof(smi->buf) - len,
1001                                                 "[%llu]", counter);
1002                         else
1003                                 len += snprintf(buf + len, sizeof(smi->buf) - len,
1004                                                 "[error]");
1005
1006                         if (j != RTL8366_NUM_PORTS - 1) {
1007                                 if (counter < 100000)
1008                                         len += snprintf(buf + len,
1009                                                         sizeof(smi->buf) - len,
1010                                                         "\t");
1011
1012                                 len += snprintf(buf + len, sizeof(smi->buf) - len,
1013                                                 "\t");
1014                         }
1015                 }
1016                 len += snprintf(buf + len, sizeof(smi->buf) - len, "\n");
1017         }
1018
1019         len += snprintf(buf + len, sizeof(smi->buf) - len, "\n");
1020
1021         return simple_read_from_buffer(user_buf, count, ppos, buf, len);
1022 }
1023
1024 static ssize_t rtl8366_read_debugfs_vlan(struct file *file,
1025                                          char __user *user_buf,
1026                                          size_t count, loff_t *ppos)
1027 {
1028         int i, j, len = 0;
1029         struct rtl8366_smi *smi = (struct rtl8366_smi *)file->private_data;
1030         char *buf = smi->buf;
1031
1032         len += snprintf(buf + len, sizeof(smi->buf) - len, "VLAN Member Config:\n");
1033         len += snprintf(buf + len, sizeof(smi->buf) - len,
1034                         "\t id \t vid \t prio \t member \t untag  \t fid "
1035                         "\tports\n");
1036
1037         for (i = 0; i < RTL8366_NUM_VLANS; ++i) {
1038                 struct rtl8366s_vlanconfig vlanmc;
1039
1040                 rtl8366s_get_vlan_member_config(smi, i, &vlanmc);
1041
1042                 len += snprintf(buf + len, sizeof(smi->buf) - len,
1043                                 "\t[%d] \t %d \t %d \t 0x%04x \t 0x%04x \t %d "
1044                                 "\t", i, vlanmc.vid, vlanmc.priority,
1045                                 vlanmc.member, vlanmc.untag, vlanmc.fid);
1046
1047                 for (j = 0; j < RTL8366_NUM_PORTS; ++j) {
1048                         int index = 0;
1049                         if (!rtl8366_get_port_vlan_index(smi, j, &index)) {
1050                                 if (index == i)
1051                                         len += snprintf(buf + len,
1052                                                         sizeof(smi->buf) - len,
1053                                                         "%d", j);
1054                         }
1055                 }
1056                 len += snprintf(buf + len, sizeof(smi->buf) - len, "\n");
1057         }
1058
1059         return simple_read_from_buffer(user_buf, count, ppos, buf, len);
1060 }
1061
1062 static ssize_t rtl8366_read_debugfs_reg(struct file *file,
1063                                         char __user *user_buf,
1064                                         size_t count, loff_t *ppos)
1065 {
1066         u32 t, reg = g_dbg_reg;
1067         int err, len = 0;
1068         struct rtl8366_smi *smi = (struct rtl8366_smi *)file->private_data;
1069         char *buf = smi->buf;
1070
1071         memset(buf, '\0', sizeof(smi->buf));
1072
1073         err = rtl8366_smi_read_reg(smi, reg, &t);
1074         if (err) {
1075                 len += snprintf(buf, sizeof(smi->buf),
1076                                 "Read failed (reg: 0x%04x)\n", reg);
1077                 return simple_read_from_buffer(user_buf, count, ppos, buf, len);
1078         }
1079
1080         len += snprintf(buf, sizeof(smi->buf), "reg = 0x%04x, val = 0x%04x\n",
1081                         reg, t);
1082
1083         return simple_read_from_buffer(user_buf, count, ppos, buf, len);
1084 }
1085
1086 static ssize_t rtl8366_write_debugfs_reg(struct file *file,
1087                                          const char __user *user_buf,
1088                                          size_t count, loff_t *ppos)
1089 {
1090         unsigned long data;
1091         u32 reg = g_dbg_reg;
1092         int err;
1093         size_t len;
1094         struct rtl8366_smi *smi = (struct rtl8366_smi *)file->private_data;
1095         char *buf = smi->buf;
1096
1097         len = min(count, sizeof(smi->buf) - 1);
1098         if (copy_from_user(buf, user_buf, len)) {
1099                 dev_err(&smi->pdev->dev, "copy from user failed\n");
1100                 return -EFAULT;
1101         }
1102
1103         buf[len] = '\0';
1104         if (len > 0 && buf[len - 1] == '\n')
1105                 buf[len - 1] = '\0';
1106
1107
1108         if (strict_strtoul(buf, 16, &data)) {
1109                 dev_err(&smi->pdev->dev, "Invalid reg value %s\n", buf);
1110         } else {
1111                 err = rtl8366_smi_write_reg(smi, reg, data);
1112                 if (err) {
1113                         dev_err(&smi->pdev->dev,
1114                                 "writing reg 0x%04x val 0x%04lx failed\n",
1115                                 reg, data);
1116                 }
1117         }
1118
1119         return count;
1120 }
1121
1122 static const struct file_operations fops_rtl8366_regs = {
1123         .read = rtl8366_read_debugfs_reg,
1124         .write = rtl8366_write_debugfs_reg,
1125         .open = rtl8366_debugfs_open,
1126         .owner = THIS_MODULE
1127 };
1128
1129 static const struct file_operations fops_rtl8366_vlan = {
1130         .read = rtl8366_read_debugfs_vlan,
1131         .open = rtl8366_debugfs_open,
1132         .owner = THIS_MODULE
1133 };
1134
1135 static const struct file_operations fops_rtl8366_mibs = {
1136         .read = rtl8366_read_debugfs_mibs,
1137         .open = rtl8366_debugfs_open,
1138         .owner = THIS_MODULE
1139 };
1140
1141 static void rtl8366_debugfs_init(struct rtl8366_smi *smi)
1142 {
1143         struct dentry *node;
1144         struct dentry *root;
1145
1146         if (!smi->debugfs_root)
1147                 smi->debugfs_root = debugfs_create_dir("rtl8366s", NULL);
1148
1149         if (!smi->debugfs_root) {
1150                 dev_err(&smi->pdev->dev, "Unable to create debugfs dir\n");
1151                 return;
1152         }
1153         root = smi->debugfs_root;
1154
1155         node = debugfs_create_x16("reg", S_IRUGO | S_IWUSR, root, &g_dbg_reg);
1156         if (!node) {
1157                 dev_err(&smi->pdev->dev, "Creating debugfs file reg failed\n");
1158                 return;
1159         }
1160
1161         node = debugfs_create_file("val", S_IRUGO | S_IWUSR, root, smi,
1162                                    &fops_rtl8366_regs);
1163         if (!node) {
1164                 dev_err(&smi->pdev->dev, "Creating debugfs file val failed\n");
1165                 return;
1166         }
1167
1168         node = debugfs_create_file("vlan", S_IRUSR, root, smi,
1169                                    &fops_rtl8366_vlan);
1170         if (!node) {
1171                 dev_err(&smi->pdev->dev, "Creating debugfs file vlan "
1172                         "failed\n");
1173                 return;
1174         }
1175
1176         node = debugfs_create_file("mibs", S_IRUSR, root, smi,
1177                                    &fops_rtl8366_mibs);
1178         if (!node) {
1179                 dev_err(&smi->pdev->dev, "Creating debugfs file mibs "
1180                         "xfailed\n");
1181                 return;
1182         }
1183 }
1184
1185 static void rtl8366_debugfs_remove(struct rtl8366_smi *smi)
1186 {
1187         if (smi->debugfs_root) {
1188                 debugfs_remove_recursive(smi->debugfs_root);
1189                 smi->debugfs_root = NULL;
1190         }
1191 }
1192
1193 #else
1194 static inline void rtl8366_debugfs_init(struct rtl8366_smi *smi) {}
1195 static inline void rtl8366_debugfs_remove(struct rtl8366_smi *smi) {}
1196 #endif
1197
1198 static int rtl8366_global_reset_mibs(struct switch_dev *dev,
1199                                      const struct switch_attr *attr,
1200                                      struct switch_val *val)
1201 {
1202         u32 data = 0;
1203         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1204
1205         if (val->value.i == 1) {
1206                 rtl8366_smi_read_reg(smi, RTL8366S_MIB_CTRL_REG, &data);
1207                 data |= (1 << 2);
1208                 rtl8366_smi_write_reg(smi, RTL8366S_MIB_CTRL_REG, data);
1209         }
1210
1211         return 0;
1212 }
1213
1214 static int rtl8366_get_vlan(struct switch_dev *dev,
1215                             const struct switch_attr *attr,
1216                             struct switch_val *val)
1217 {
1218         u32 data;
1219         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1220
1221         if (attr->ofs == 1) {
1222                 rtl8366_smi_read_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, &data);
1223
1224                 if (data & RTL8366_CHIP_CTRL_VLAN)
1225                         val->value.i = 1;
1226                 else
1227                         val->value.i = 0;
1228         } else if (attr->ofs == 2) {
1229                 rtl8366_smi_read_reg(smi, RTL8366S_VLAN_TB_CTRL_REG, &data);
1230
1231                 if (data & 0x0001)
1232                         val->value.i = 1;
1233                 else
1234                         val->value.i = 0;
1235         }
1236
1237         return 0;
1238 }
1239
1240 static int rtl8366_global_get_blinkrate(struct switch_dev *dev,
1241                                         const struct switch_attr *attr,
1242                                         struct switch_val *val)
1243 {
1244         u32 data;
1245         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1246         rtl8366_smi_read_reg(smi, RTL8366_LED_BLINKRATE_REG, &data);
1247
1248         val->value.i = (data & (RTL8366_LED_BLINKRATE_MASK));
1249
1250         return 0;
1251 }
1252
1253 static int rtl8366_global_set_blinkrate(struct switch_dev *dev,
1254                                         const struct switch_attr *attr,
1255                                         struct switch_val *val)
1256 {
1257         u32 data;
1258         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1259
1260         if (val->value.i >= 6)
1261                 return -EINVAL;
1262
1263         rtl8366_smi_read_reg(smi, RTL8366_LED_BLINKRATE_REG, &data);
1264
1265         data &= ~(data & RTL8366_LED_BLINKRATE_MASK);
1266         data |= val->value.i;
1267
1268         rtl8366_smi_write_reg(smi, RTL8366_LED_BLINKRATE_REG, data);
1269
1270         return 0;
1271 }
1272
1273 static int rtl8366_set_vlan(struct switch_dev *dev,
1274                             const struct switch_attr *attr,
1275                             struct switch_val *val)
1276 {
1277         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1278
1279         if (attr->ofs == 1)
1280                 return rtl8366_vlan_set_vlan(smi, val->value.i);
1281         else
1282                 return rtl8366_vlan_set_4ktable(smi, val->value.i);
1283 }
1284
1285 static int rtl8366_attr_get_port_link(struct switch_dev *dev,
1286                                       const struct switch_attr *attr,
1287                                       struct switch_val *val)
1288 {
1289         u32 len = 0, data = 0;
1290         int speed, duplex, link, txpause, rxpause, nway;
1291         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1292         char *buf = smi->buf;
1293
1294         if (val->port_vlan >= RTL8366_NUM_PORTS)
1295                 return -EINVAL;
1296
1297         memset(buf, '\0', sizeof(smi->buf));
1298         rtl8366_smi_read_reg(smi, RTL8366S_PORT_LINK_STATUS_BASE +
1299                              (val->port_vlan >> 1),
1300                              &data);
1301
1302         if (val->port_vlan & 0x1)
1303                 data = data >> 8;
1304
1305         speed = (data & RTL8366S_PORT_STATUS_SPEED_MASK);
1306         duplex = (data & RTL8366S_PORT_STATUS_DUPLEX_MASK) >> 2;
1307         link = (data & RTL8366S_PORT_STATUS_LINK_MASK) >> 4;
1308         txpause = (data & RTL8366S_PORT_STATUS_TXPAUSE_MASK) >> 5;
1309         rxpause = (data & RTL8366S_PORT_STATUS_RXPAUSE_MASK) >> 6;
1310         nway = (data & RTL8366S_PORT_STATUS_AN_MASK) >> 7;
1311
1312         len += snprintf(buf + len, sizeof(smi->buf) - len, "Port %d: ",
1313                         val->port_vlan);
1314
1315         if (link)
1316                 len += snprintf(buf + len, sizeof(smi->buf) - len,
1317                                 "Link UP, Speed: ");
1318         else
1319                 len += snprintf(buf + len, sizeof(smi->buf) - len,
1320                                 "Link DOWN, Speed: ");
1321
1322         if (speed == 0)
1323                 len += snprintf(buf + len, sizeof(smi->buf) - len, "10Base-TX ");
1324         else if (speed == 1)
1325                 len += snprintf(buf + len, sizeof(smi->buf) - len, "100Base-TX ");
1326         else if (speed == 2)
1327                 len += snprintf(buf + len, sizeof(smi->buf) - len, "1000Base-TX ");
1328
1329         if (duplex)
1330                 len += snprintf(buf + len, sizeof(smi->buf) - len, "Full-Duplex, ");
1331         else
1332                 len += snprintf(buf + len, sizeof(smi->buf) - len, "Half-Duplex, ");
1333
1334         if (txpause)
1335                 len += snprintf(buf + len, sizeof(smi->buf) - len, "TX-Pause ");
1336         if (rxpause)
1337                 len += snprintf(buf + len, sizeof(smi->buf) - len, "RX-Pause ");
1338         if (nway)
1339                 len += snprintf(buf + len, sizeof(smi->buf) - len, "nway ");
1340
1341         val->value.s = buf;
1342         val->len = len;
1343
1344         return 0;
1345 }
1346
1347 static int rtl8366_attr_get_vlan_info(struct switch_dev *dev,
1348                                       const struct switch_attr *attr,
1349                                       struct switch_val *val)
1350 {
1351         int i;
1352         u32 len = 0;
1353         struct rtl8366s_vlanconfig vlanmc;
1354         struct rtl8366s_vlan4kentry vlan4k;
1355         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1356         char *buf = smi->buf;
1357
1358         if (val->port_vlan >= RTL8366_NUM_PORTS)
1359                 return -EINVAL;
1360
1361         memset(buf, '\0', sizeof(smi->buf));
1362
1363         rtl8366s_get_vlan_member_config(smi, val->port_vlan, &vlanmc);
1364         rtl8366s_get_vlan_4k_entry(smi, vlanmc.vid, &vlan4k);
1365
1366         len += snprintf(buf + len, sizeof(smi->buf) - len, "VLAN %d: Ports: ",
1367                         val->port_vlan);
1368
1369         for (i = 0; i < RTL8366_NUM_PORTS; ++i) {
1370                 int index = 0;
1371                 if (!rtl8366_get_port_vlan_index(smi, i, &index) &&
1372                     index == val->port_vlan)
1373                         len += snprintf(buf + len, sizeof(smi->buf) - len, "%d", i);
1374         }
1375         len += snprintf(buf + len, sizeof(smi->buf) - len, "\n");
1376
1377         len += snprintf(buf + len, sizeof(smi->buf) - len,
1378                         "\t\t vid \t prio \t member \t untag \t fid\n");
1379         len += snprintf(buf + len, sizeof(smi->buf) - len, "\tMC:\t");
1380         len += snprintf(buf + len, sizeof(smi->buf) - len,
1381                         "%d \t %d \t 0x%04x \t 0x%04x \t %d\n",
1382                         vlanmc.vid, vlanmc.priority, vlanmc.member,
1383                         vlanmc.untag, vlanmc.fid);
1384         len += snprintf(buf + len, sizeof(smi->buf) - len, "\t4K:\t");
1385         len += snprintf(buf + len, sizeof(smi->buf) - len,
1386                         "%d \t  \t 0x%04x \t 0x%04x \t %d",
1387                         vlan4k.vid, vlan4k.member, vlan4k.untag, vlan4k.fid);
1388
1389         val->value.s = buf;
1390         val->len = len;
1391
1392         return 0;
1393 }
1394
1395 static int rtl8366_set_port_led(struct switch_dev *dev,
1396                                 const struct switch_attr *attr,
1397                                 struct switch_val *val)
1398 {
1399         u32 data = 0;
1400         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1401         if (val->port_vlan >= RTL8366_NUM_PORTS ||
1402             (1 << val->port_vlan) == RTL8366_PORT_UNKNOWN)
1403                 return -EINVAL;
1404
1405         if (val->port_vlan == RTL8366_PORT_NUM_CPU) {
1406                 rtl8366_smi_read_reg(smi, RTL8366_LED_BLINKRATE_REG, &data);
1407                 data = (data & (~(0xF << 4))) | (val->value.i << 4);
1408                 rtl8366_smi_write_reg(smi, RTL8366_LED_BLINKRATE_REG, data);
1409         } else {
1410                 rtl8366_smi_read_reg(smi, RTL8366_LED_CTRL_REG, &data);
1411                 data = (data & (~(0xF << (val->port_vlan * 4)))) |
1412                         (val->value.i << (val->port_vlan * 4));
1413                 rtl8366_smi_write_reg(smi, RTL8366_LED_CTRL_REG, data);
1414         }
1415
1416         return 0;
1417 }
1418
1419 static int rtl8366_get_port_led(struct switch_dev *dev,
1420                                 const struct switch_attr *attr,
1421                                 struct switch_val *val)
1422 {
1423         u32 data = 0;
1424         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1425         if (val->port_vlan >= RTL8366_NUM_LEDGROUPS)
1426                 return -EINVAL;
1427
1428         rtl8366_smi_read_reg(smi, RTL8366_LED_CTRL_REG, &data);
1429         val->value.i = (data >> (val->port_vlan * 4)) & 0x000F;
1430
1431         return 0;
1432 }
1433
1434 static int rtl8366_reset_port_mibs(struct switch_dev *dev,
1435                                    const struct switch_attr *attr,
1436                                    struct switch_val *val)
1437 {
1438         u32 data = 0;
1439         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1440         if (val->port_vlan >= RTL8366_NUM_PORTS)
1441                 return -EINVAL;
1442
1443         rtl8366_smi_read_reg(smi, RTL8366S_MIB_CTRL_REG, &data);
1444         data |= (1 << (val->port_vlan + 3));
1445         rtl8366_smi_write_reg(smi, RTL8366S_MIB_CTRL_REG, data);
1446
1447         return 0;
1448 }
1449
1450 static int rtl8366_get_port_mib(struct switch_dev *dev,
1451                                 const struct switch_attr *attr,
1452                                 struct switch_val *val)
1453 {
1454         int i, len = 0;
1455         unsigned long long counter = 0;
1456         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1457         char *buf = smi->buf;
1458
1459         if (val->port_vlan >= RTL8366_NUM_PORTS)
1460                 return -EINVAL;
1461
1462         len += snprintf(buf + len, sizeof(smi->buf) - len, "Port %d MIB counters\n",
1463                         val->port_vlan);
1464         for (i = 0; i < RTL8366S_MIB_COUNT; ++i) {
1465
1466                 len += snprintf(buf + len, sizeof(smi->buf) - len,
1467                                 "%d:%s\t", i, MIBCOUNTERS[i]);
1468                 if (!rtl8366_get_mib_counter(smi, i, val->port_vlan, &counter))
1469                         len += snprintf(buf + len, sizeof(smi->buf) - len,
1470                                         "[%llu]\n", counter);
1471                 else
1472                         len += snprintf(buf + len, sizeof(smi->buf) - len,
1473                                         "[error]\n");
1474         }
1475
1476         val->value.s = buf;
1477         val->len = len;
1478         return 0;
1479 }
1480
1481 static int rtl8366_get_ports(struct switch_dev *dev,
1482                              struct switch_val *val)
1483 {
1484         struct rtl8366s_vlanconfig vlanmc;
1485         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1486         struct switch_port *port;
1487         int i;
1488
1489         if (val->port_vlan >= RTL8366_NUM_VLANS)
1490                 return -EINVAL;
1491
1492         rtl8366s_get_vlan_member_config(smi, val->port_vlan, &vlanmc);
1493
1494         port = &val->value.ports[0];
1495         val->len = 0;
1496         for (i = 0; i < RTL8366_NUM_PORTS; i++) {
1497                 if (!(vlanmc.member & BIT(i)))
1498                         continue;
1499
1500                 port->id = i;
1501                 port->flags = (vlanmc.untag & BIT(i)) ?
1502                                         0 : BIT(SWITCH_PORT_FLAG_TAGGED);
1503                 val->len++;
1504                 port++;
1505         }
1506         return 0;
1507 }
1508
1509 static int rtl8366_set_ports(struct switch_dev *dev,
1510                              struct switch_val *val)
1511 {
1512         struct rtl8366s_vlanconfig vlanmc;
1513         struct rtl8366s_vlan4kentry vlan4k;
1514         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1515         struct switch_port *port;
1516         int i;
1517
1518         if (val->port_vlan >= RTL8366_NUM_VLANS)
1519                 return -EINVAL;
1520
1521         rtl8366s_get_vlan_member_config(smi, val->port_vlan, &vlanmc);
1522         rtl8366s_get_vlan_4k_entry(smi, vlanmc.vid, &vlan4k);
1523
1524         vlanmc.untag = 0;
1525         vlanmc.member = 0;
1526
1527         port = &val->value.ports[0];
1528         for (i = 0; i < val->len; i++, port++) {
1529                 vlanmc.member |= BIT(port->id);
1530
1531                 if (!(port->flags & BIT(SWITCH_PORT_FLAG_TAGGED)))
1532                         vlanmc.untag |= BIT(port->id);
1533         }
1534
1535         vlan4k.member = vlanmc.member;
1536         vlan4k.untag = vlanmc.untag;
1537
1538         rtl8366s_set_vlan_member_config(smi, val->port_vlan, &vlanmc);
1539         rtl8366s_set_vlan_4k_entry(smi, &vlan4k);
1540         return 0;
1541 }
1542
1543 static int rtl8366_get_port_pvid(struct switch_dev *dev, int port, int *val)
1544 {
1545         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1546         return rtl8366_get_vlan_port_pvid(smi, port, val);
1547 }
1548
1549 static int rtl8366_set_port_pvid(struct switch_dev *dev, int port, int val)
1550 {
1551         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1552         return rtl8366_set_vlan_port_pvid(smi, port, val);
1553 }
1554
1555 static int rtl8366_reset_switch(struct switch_dev *dev)
1556 {
1557         struct rtl8366_smi *smi = sw_to_rtl8366(dev);
1558         int timeout = 10;
1559         u32 data;
1560
1561         rtl8366_smi_write_reg(smi, RTL8366_RESET_CTRL_REG,
1562                               RTL8366_CHIP_CTRL_RESET_HW);
1563         do {
1564                 msleep(1);
1565                 if (rtl8366_smi_read_reg(smi, RTL8366_RESET_CTRL_REG, &data))
1566                         return -EIO;
1567
1568                 if (!(data & RTL8366_CHIP_CTRL_RESET_HW))
1569                         break;
1570         } while (--timeout);
1571
1572         if (!timeout) {
1573                 printk("Timeout waiting for the switch to reset\n");
1574                 return -EIO;
1575         }
1576
1577         return rtl8366s_reset_vlan(smi);
1578 }
1579
1580 static struct switch_attr rtl8366_globals[] = {
1581         {
1582                 .type = SWITCH_TYPE_INT,
1583                 .name = "enable_vlan",
1584                 .description = "Enable VLAN mode",
1585                 .set = rtl8366_set_vlan,
1586                 .get = rtl8366_get_vlan,
1587                 .max = 1,
1588                 .ofs = 1
1589         },
1590         {
1591                 .type = SWITCH_TYPE_INT,
1592                 .name = "enable_vlan4k",
1593                 .description = "Enable VLAN 4K mode",
1594                 .set = rtl8366_set_vlan,
1595                 .get = rtl8366_get_vlan,
1596                 .max = 1,
1597                 .ofs = 2
1598         },
1599         {
1600                 .type = SWITCH_TYPE_INT,
1601                 .name = "reset_mibs",
1602                 .description = "Reset all MIB counters",
1603                 .set = rtl8366_global_reset_mibs,
1604                 .get = NULL,
1605                 .max = 1
1606         },
1607         {
1608                 .type = SWITCH_TYPE_INT,
1609                 .name = "blinkrate",
1610                 .description = "Get/Set LED blinking rate (0 = 43ms, 1 = 84ms,"
1611                 " 2 = 120ms, 3 = 170ms, 4 = 340ms, 5 = 670ms)",
1612                 .set = rtl8366_global_set_blinkrate,
1613                 .get = rtl8366_global_get_blinkrate,
1614                 .max = 5
1615         },
1616 };
1617
1618 static struct switch_attr rtl8366_port[] = {
1619         {
1620                 .type = SWITCH_TYPE_STRING,
1621                 .name = "link",
1622                 .description = "Get port link information",
1623                 .max = 1,
1624                 .set = NULL,
1625                 .get = rtl8366_attr_get_port_link
1626         },
1627         {
1628                 .type = SWITCH_TYPE_INT,
1629                 .name = "reset_mib",
1630                 .description = "Reset single port MIB counters",
1631                 .max = 1,
1632                 .set = rtl8366_reset_port_mibs,
1633                 .get = NULL
1634         },
1635         {
1636                 .type = SWITCH_TYPE_STRING,
1637                 .name = "mib",
1638                 .description = "Get MIB counters for port",
1639                 .max = 33,
1640                 .set = NULL,
1641                 .get = rtl8366_get_port_mib
1642         },
1643         {
1644                 .type = SWITCH_TYPE_INT,
1645                 .name = "led",
1646                 .description = "Get/Set port group (0 - 3) led mode (0 - 15)",
1647                 .max = 15,
1648                 .set = rtl8366_set_port_led,
1649                 .get = rtl8366_get_port_led
1650         },
1651 };
1652
1653 static struct switch_attr rtl8366_vlan[] = {
1654         {
1655                 .type = SWITCH_TYPE_STRING,
1656                 .name = "info",
1657                 .description = "Get vlan information",
1658                 .max = 1,
1659                 .set = NULL,
1660                 .get = rtl8366_attr_get_vlan_info
1661         },
1662 };
1663
1664
1665 /* template */
1666 static struct switch_dev rtldev = {
1667         .name = "RTL8366S",
1668         .cpu_port = RTL8366_PORT_NUM_CPU,
1669         .ports = RTL8366_NUM_PORTS,
1670         .vlans = RTL8366_NUM_VLANS,
1671         .attr_global = {
1672                 .attr = rtl8366_globals,
1673                 .n_attr = ARRAY_SIZE(rtl8366_globals),
1674         },
1675         .attr_port = {
1676                 .attr = rtl8366_port,
1677                 .n_attr = ARRAY_SIZE(rtl8366_port),
1678         },
1679         .attr_vlan = {
1680                 .attr = rtl8366_vlan,
1681                 .n_attr = ARRAY_SIZE(rtl8366_vlan),
1682         },
1683
1684         .get_vlan_ports = rtl8366_get_ports,
1685         .set_vlan_ports = rtl8366_set_ports,
1686         .get_port_pvid = rtl8366_get_port_pvid,
1687         .set_port_pvid = rtl8366_set_port_pvid,
1688         .reset_switch = rtl8366_reset_switch,
1689 };
1690
1691 static int rtl8366_smi_switch_init(struct rtl8366_smi *smi)
1692 {
1693         struct switch_dev *dev = &smi->dev;
1694         int err;
1695
1696         memcpy(dev, &rtldev, sizeof(struct switch_dev));
1697         dev->priv = smi;
1698         dev->devname = dev_name(&smi->pdev->dev);
1699
1700         err = register_switch(dev, NULL);
1701         if (err)
1702                 dev_err(&smi->pdev->dev, "switch registration failed\n");
1703
1704         return err;
1705 }
1706
1707 static void rtl8366_smi_switch_cleanup(struct rtl8366_smi *smi)
1708 {
1709         unregister_switch(&smi->dev);
1710 }
1711
1712 static int rtl8366_smi_mii_read(struct mii_bus *bus, int addr, int reg)
1713 {
1714         struct rtl8366_smi *smi = bus->priv;
1715         u32 val = 0;
1716         int err;
1717
1718         err = rtl8366_smi_read_phy_reg(smi, addr, 0, reg, &val);
1719         if (err)
1720                 return 0xffff;
1721
1722         return val;
1723 }
1724
1725 static int rtl8366_smi_mii_write(struct mii_bus *bus, int addr, int reg,
1726                                      u16 val)
1727 {
1728         struct rtl8366_smi *smi = bus->priv;
1729         u32 t;
1730         int err;
1731
1732         err = rtl8366_smi_write_phy_reg(smi, addr, 0, reg, val);
1733         /* flush write */
1734         (void) rtl8366_smi_read_phy_reg(smi, addr, 0, reg, &t);
1735
1736         return err;
1737 }
1738
1739 static int rtl8366_smi_mii_init(struct rtl8366_smi *smi)
1740 {
1741         int ret;
1742         int i;
1743
1744         smi->mii_bus = mdiobus_alloc();
1745         if (smi->mii_bus == NULL) {
1746                 ret = -ENOMEM;
1747                 goto err;
1748         }
1749
1750         spin_lock_init(&smi->lock);
1751         smi->mii_bus->priv = (void *) smi;
1752         smi->mii_bus->name = "rtl8366-smi";
1753         smi->mii_bus->read = rtl8366_smi_mii_read;
1754         smi->mii_bus->write = rtl8366_smi_mii_write;
1755         snprintf(smi->mii_bus->id, MII_BUS_ID_SIZE, "%s",
1756                         dev_name(&smi->pdev->dev));
1757         smi->mii_bus->parent = &smi->pdev->dev;
1758         smi->mii_bus->phy_mask = ~(0x1f);
1759         smi->mii_bus->irq = smi->mii_irq;
1760         for (i = 0; i < PHY_MAX_ADDR; i++)
1761                 smi->mii_irq[i] = PHY_POLL;
1762
1763         ret = mdiobus_register(smi->mii_bus);
1764         if (ret)
1765                 goto err_free;
1766
1767         return 0;
1768
1769  err_free:
1770         mdiobus_free(smi->mii_bus);
1771  err:
1772         return ret;
1773 }
1774
1775 static void rtl8366_smi_mii_cleanup(struct rtl8366_smi *smi)
1776 {
1777         mdiobus_unregister(smi->mii_bus);
1778         mdiobus_free(smi->mii_bus);
1779 }
1780
1781 static int rtl8366_smi_mii_bus_match(struct mii_bus *bus)
1782 {
1783         return (bus->read == rtl8366_smi_mii_read &&
1784                 bus->write == rtl8366_smi_mii_write);
1785 }
1786
1787 static int rtl8366_smi_setup(struct rtl8366_smi *smi)
1788 {
1789         u32 chip_id = 0;
1790         u32 chip_ver = 0;
1791         int ret;
1792
1793         ret = rtl8366_smi_read_reg(smi, RTL8366S_CHIP_ID_REG, &chip_id);
1794         if (ret) {
1795                 dev_err(&smi->pdev->dev, "unable to read chip id\n");
1796                 return ret;
1797         }
1798
1799         switch (chip_id) {
1800         case RTL8366S_CHIP_ID_8366:
1801                 break;
1802         default:
1803                 dev_err(&smi->pdev->dev, "unknown chip id (%04x)\n", chip_id);
1804                 return -ENODEV;
1805         }
1806
1807         ret = rtl8366_smi_read_reg(smi, RTL8366S_CHIP_VERSION_CTRL_REG,
1808                                    &chip_ver);
1809         if (ret) {
1810                 dev_err(&smi->pdev->dev, "unable to read chip version\n");
1811                 return ret;
1812         }
1813
1814         dev_info(&smi->pdev->dev, "RTL%04x ver. %u chip found\n",
1815                  chip_id, chip_ver & RTL8366S_CHIP_VERSION_MASK);
1816
1817         rtl8366_debugfs_init(smi);
1818
1819         return 0;
1820 }
1821
1822 static int __init rtl8366_smi_probe(struct platform_device *pdev)
1823 {
1824         static int rtl8366_smi_version_printed;
1825         struct rtl8366_smi_platform_data *pdata;
1826         struct rtl8366_smi *smi;
1827         int err;
1828
1829         if (!rtl8366_smi_version_printed++)
1830                 printk(KERN_NOTICE RTL8366_SMI_DRIVER_DESC
1831                        " version " RTL8366_SMI_DRIVER_VER"\n");
1832
1833         pdata = pdev->dev.platform_data;
1834         if (!pdata) {
1835                 dev_err(&pdev->dev, "no platform data specified\n");
1836                 err = -EINVAL;
1837                 goto err_out;
1838         }
1839
1840         smi = kzalloc(sizeof(struct rtl8366_smi), GFP_KERNEL);
1841         if (!smi) {
1842                 dev_err(&pdev->dev, "no memory for private data\n");
1843                 err = -ENOMEM;
1844                 goto err_out;
1845         }
1846
1847         err = gpio_request(pdata->gpio_sda, dev_name(&pdev->dev));
1848         if (err) {
1849                 dev_err(&pdev->dev, "gpio_request failed for %u, err=%d\n",
1850                         pdata->gpio_sda, err);
1851                 goto err_free_smi;
1852         }
1853
1854         err = gpio_request(pdata->gpio_sck, dev_name(&pdev->dev));
1855         if (err) {
1856                 dev_err(&pdev->dev, "gpio_request failed for %u, err=%d\n",
1857                         pdata->gpio_sck, err);
1858                 goto err_free_sda;
1859         }
1860
1861         smi->pdev = pdev;
1862         smi->pdata = pdata;
1863         spin_lock_init(&smi->lock);
1864
1865         platform_set_drvdata(pdev, smi);
1866
1867         dev_info(&pdev->dev, "using GPIO pins %u (SDA) and %u (SCK)\n",
1868                  pdata->gpio_sda, pdata->gpio_sck);
1869
1870         err = rtl8366_smi_setup(smi);
1871         if (err)
1872                 goto err_clear_drvdata;
1873
1874         err = rtl8366_smi_mii_init(smi);
1875         if (err)
1876                 goto err_clear_drvdata;
1877
1878         err = rtl8366_smi_switch_init(smi);
1879         if (err)
1880                 goto err_mii_cleanup;
1881
1882         return 0;
1883
1884  err_mii_cleanup:
1885         rtl8366_smi_mii_cleanup(smi);
1886  err_clear_drvdata:
1887         platform_set_drvdata(pdev, NULL);
1888         gpio_free(pdata->gpio_sck);
1889  err_free_sda:
1890         gpio_free(pdata->gpio_sda);
1891  err_free_smi:
1892         kfree(smi);
1893  err_out:
1894         return err;
1895 }
1896
1897 int rtl8366_phy_config_init(struct phy_device *phydev)
1898 {
1899         if (!rtl8366_smi_mii_bus_match(phydev->bus))
1900                 return -EINVAL;
1901
1902         return 0;
1903 }
1904
1905 int rtl8366_phy_config_aneg(struct phy_device *phydev)
1906 {
1907         return 0;
1908 }
1909
1910 static struct phy_driver rtl8366_smi_phy_driver = {
1911         .phy_id         = 0x001cc960,
1912         .name           = "Realtek RTL8366",
1913         .phy_id_mask    = 0x1ffffff0,
1914         .features       = PHY_GBIT_FEATURES,
1915         .config_aneg    = rtl8366_phy_config_aneg,
1916         .config_init    = rtl8366_phy_config_init,
1917         .read_status    = genphy_read_status,
1918         .driver         = {
1919                 .owner = THIS_MODULE,
1920         },
1921 };
1922
1923 static int __devexit rtl8366_smi_remove(struct platform_device *pdev)
1924 {
1925         struct rtl8366_smi *smi = platform_get_drvdata(pdev);
1926
1927         if (smi) {
1928                 struct rtl8366_smi_platform_data *pdata;
1929
1930                 pdata = pdev->dev.platform_data;
1931
1932                 rtl8366_smi_switch_cleanup(smi);
1933                 rtl8366_debugfs_remove(smi);
1934                 rtl8366_smi_mii_cleanup(smi);
1935                 platform_set_drvdata(pdev, NULL);
1936                 gpio_free(pdata->gpio_sck);
1937                 gpio_free(pdata->gpio_sda);
1938                 kfree(smi);
1939         }
1940
1941         return 0;
1942 }
1943
1944 static struct platform_driver rtl8366_smi_driver = {
1945         .driver = {
1946                 .name           = RTL8366_SMI_DRIVER_NAME,
1947                 .owner          = THIS_MODULE,
1948         },
1949         .probe          = rtl8366_smi_probe,
1950         .remove         = __devexit_p(rtl8366_smi_remove),
1951 };
1952
1953 static int __init rtl8366_smi_init(void)
1954 {
1955         int ret;
1956         ret = platform_driver_register(&rtl8366_smi_driver);
1957         if (ret)
1958                 return ret;
1959
1960         ret = phy_driver_register(&rtl8366_smi_phy_driver);
1961         if (ret)
1962                 goto err_platform_unregister;
1963
1964         return 0;
1965
1966  err_platform_unregister:
1967         platform_driver_unregister(&rtl8366_smi_driver);
1968         return ret;
1969 }
1970 module_init(rtl8366_smi_init);
1971
1972 static void __exit rtl8366_smi_exit(void)
1973 {
1974         phy_driver_unregister(&rtl8366_smi_phy_driver);
1975         platform_driver_unregister(&rtl8366_smi_driver);
1976 }
1977 module_exit(rtl8366_smi_exit);
1978
1979 MODULE_DESCRIPTION(RTL8366_SMI_DRIVER_DESC);
1980 MODULE_VERSION(RTL8366_SMI_DRIVER_VER);
1981 MODULE_AUTHOR("Gabor Juhos <juhosg@openwrt.org>");
1982 MODULE_AUTHOR("Antti Seppälä <a.seppala@gmail.com>");
1983 MODULE_LICENSE("GPL v2");
1984 MODULE_ALIAS("platform:" RTL8366_SMI_DRIVER_NAME);