board/CZ.NIC/turris_mox/turris_mox.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (C) 2018 Marek Behun <marek.behun@nic.cz>
   4  */
   5
   6 #include <common.h>
   7 #include <asm/arch/cpu.h>
   8 #include <asm/arch/soc.h>
   9 #include <asm/io.h>
  10 #include <asm/gpio.h>
  11 #include <clk.h>
  12 #include <dm.h>
  13 #include <env.h>
  14 #include <fdt_support.h>
  15 #include <init.h>
  16 #include <linux/libfdt.h>
  17 #include <linux/string.h>
  18 #include <miiphy.h>
  19 #include <mvebu/comphy.h>
  20 #include <spi.h>
  21
  22 #include "mox_sp.h"
  23
  24 #define MAX_MOX_MODULES         10
  25
  26 #define MOX_MODULE_SFP          0x1
  27 #define MOX_MODULE_PCI          0x2
  28 #define MOX_MODULE_TOPAZ        0x3
  29 #define MOX_MODULE_PERIDOT      0x4
  30 #define MOX_MODULE_USB3         0x5
  31 #define MOX_MODULE_PASSPCI      0x6
  32
  33 #define ARMADA_37XX_NB_GPIO_SEL (MVEBU_REGISTER(0x13830))
  34 #define ARMADA_37XX_SPI_CTRL    (MVEBU_REGISTER(0x10600))
  35 #define ARMADA_37XX_SPI_CFG     (MVEBU_REGISTER(0x10604))
  36 #define ARMADA_37XX_SPI_DOUT    (MVEBU_REGISTER(0x10608))
  37 #define ARMADA_37XX_SPI_DIN     (MVEBU_REGISTER(0x1060c))
  38
  39 #define ETH1_PATH       "/soc/internal-regs@d0000000/ethernet@40000"
  40 #define MDIO_PATH       "/soc/internal-regs@d0000000/mdio@32004"
  41 #define SFP_GPIO_PATH   "/soc/internal-regs@d0000000/spi@10600/moxtet@1/gpio@0"
  42 #define PCIE_PATH       "/soc/pcie@d0070000"
  43 #define SFP_PATH        "/sfp"
  44
  45 DECLARE_GLOBAL_DATA_PTR;
  46
  47 #if defined(CONFIG_OF_BOARD_FIXUP)
  48 int board_fix_fdt(void *blob)
  49 {
  50         u8 topology[MAX_MOX_MODULES];
  51         int i, size, node;
  52         bool enable;
  53
  54         /*
  55          * SPI driver is not loaded in driver model yet, but we have to find out
  56          * if pcie should be enabled in U-Boot's device tree. Therefore we have
  57          * to read SPI by reading/writing SPI registers directly
  58          */
  59
  60         writel(0x10df, ARMADA_37XX_SPI_CFG);
  61         /* put pin from GPIO to SPI mode */
  62         clrbits_le32(ARMADA_37XX_NB_GPIO_SEL, BIT(12));
  63         /* enable SPI CS1 */
  64         setbits_le32(ARMADA_37XX_SPI_CTRL, BIT(17));
  65
  66         while (!(readl(ARMADA_37XX_SPI_CTRL) & 0x2))
  67                 udelay(1);
  68
  69         for (i = 0; i < MAX_MOX_MODULES; ++i) {
  70                 writel(0x0, ARMADA_37XX_SPI_DOUT);
  71
  72                 while (!(readl(ARMADA_37XX_SPI_CTRL) & 0x2))
  73                         udelay(1);
  74
  75                 topology[i] = readl(ARMADA_37XX_SPI_DIN) & 0xff;
  76                 if (topology[i] == 0xff)
  77                         break;
  78
  79                 topology[i] &= 0xf;
  80         }
  81
  82         size = i;
  83
  84         /* disable SPI CS1 */
  85         clrbits_le32(ARMADA_37XX_SPI_CTRL, BIT(17));
  86
  87         if (size > 1 && (topology[1] == MOX_MODULE_PCI ||
  88                          topology[1] == MOX_MODULE_USB3 ||
  89                          topology[1] == MOX_MODULE_PASSPCI))
  90                 enable = true;
  91         else
  92                 enable = false;
  93
  94         node = fdt_path_offset(blob, PCIE_PATH);
  95
  96         if (node < 0) {
  97                 printf("Cannot find PCIe node in U-Boot's device tree!\n");
  98                 return 0;
  99         }
 100
 101         if (fdt_setprop_string(blob, node, "status",
 102                                enable ? "okay" : "disabled") < 0) {
 103                 printf("Cannot %s PCIe in U-Boot's device tree!\n",
 104                        enable ? "enable" : "disable");
 105                 return 0;
 106         }
 107
 108         if (a3700_fdt_fix_pcie_regions(blob) < 0) {
 109                 printf("Cannot fix PCIe regions in U-Boot's device tree!\n");
 110                 return 0;
 111         }
 112
 113         return 0;
 114 }
 115 #endif
 116
 117 int board_init(void)
 118 {
 119         /* address of boot parameters */
 120         gd->bd->bi_boot_params = CONFIG_SYS_SDRAM_BASE + 0x100;
 121
 122         return 0;
 123 }
 124
 125 static int mox_do_spi(u8 *in, u8 *out, size_t size)
 126 {
 127         struct spi_slave *slave;
 128         struct udevice *dev;
 129         int ret;
 130
 131         ret = spi_get_bus_and_cs(0, 1, 1000000, SPI_CPHA | SPI_CPOL,
 132                                  "spi_generic_drv", "moxtet@1", &dev,
 133                                  &slave);
 134         if (ret)
 135                 goto fail;
 136
 137         ret = spi_claim_bus(slave);
 138         if (ret)
 139                 goto fail_free;
 140
 141         ret = spi_xfer(slave, size * 8, out, in, SPI_XFER_ONCE);
 142
 143         spi_release_bus(slave);
 144 fail_free:
 145         spi_free_slave(slave);
 146 fail:
 147         return ret;
 148 }
 149
 150 static int mox_get_topology(const u8 **ptopology, int *psize, int *pis_sd)
 151 {
 152         static int is_sd;
 153         static u8 topology[MAX_MOX_MODULES - 1];
 154         static int size;
 155         u8 din[MAX_MOX_MODULES], dout[MAX_MOX_MODULES];
 156         int ret, i;
 157
 158         if (size) {
 159                 if (ptopology)
 160                         *ptopology = topology;
 161                 if (psize)
 162                         *psize = size;
 163                 if (pis_sd)
 164                         *pis_sd = is_sd;
 165                 return 0;
 166         }
 167
 168         memset(din, 0, MAX_MOX_MODULES);
 169         memset(dout, 0, MAX_MOX_MODULES);
 170
 171         ret = mox_do_spi(din, dout, MAX_MOX_MODULES);
 172         if (ret)
 173                 return ret;
 174
 175         if (din[0] == 0x10)
 176                 is_sd = 1;
 177         else if (din[0] == 0x00)
 178                 is_sd = 0;
 179         else
 180                 return -ENODEV;
 181
 182         for (i = 1; i < MAX_MOX_MODULES && din[i] != 0xff; ++i)
 183                 topology[i - 1] = din[i] & 0xf;
 184         size = i - 1;
 185
 186         if (ptopology)
 187                 *ptopology = topology;
 188         if (psize)
 189                 *psize = size;
 190         if (pis_sd)
 191                 *pis_sd = is_sd;
 192
 193         return 0;
 194 }
 195
 196 int comphy_update_map(struct comphy_map *serdes_map, int count)
 197 {
 198         int ret, i, size, sfpindex = -1, swindex = -1;
 199         const u8 *topology;
 200
 201         ret = mox_get_topology(&topology, &size, NULL);
 202         if (ret)
 203                 return ret;
 204
 205         for (i = 0; i < size; ++i) {
 206                 if (topology[i] == MOX_MODULE_SFP && sfpindex == -1)
 207                         sfpindex = i;
 208                 else if ((topology[i] == MOX_MODULE_TOPAZ ||
 209                           topology[i] == MOX_MODULE_PERIDOT) &&
 210                          swindex == -1)
 211                         swindex = i;
 212         }
 213
 214         if (sfpindex >= 0 && swindex >= 0) {
 215                 if (sfpindex < swindex)
 216                         serdes_map[0].speed = PHY_SPEED_1_25G;
 217                 else
 218                         serdes_map[0].speed = PHY_SPEED_3_125G;
 219         } else if (sfpindex >= 0) {
 220                 serdes_map[0].speed = PHY_SPEED_1_25G;
 221         } else if (swindex >= 0) {
 222                 serdes_map[0].speed = PHY_SPEED_3_125G;
 223         }
 224
 225         return 0;
 226 }
 227
 228 #define SW_SMI_CMD_R(d, r)      (0x9800 | (((d) & 0x1f) << 5) | ((r) & 0x1f))
 229 #define SW_SMI_CMD_W(d, r)      (0x9400 | (((d) & 0x1f) << 5) | ((r) & 0x1f))
 230
 231 static int sw_multi_read(struct mii_dev *bus, int sw, int dev, int reg)
 232 {
 233         bus->write(bus, sw, 0, 0, SW_SMI_CMD_R(dev, reg));
 234         mdelay(5);
 235         return bus->read(bus, sw, 0, 1);
 236 }
 237
 238 static void sw_multi_write(struct mii_dev *bus, int sw, int dev, int reg,
 239                            u16 val)
 240 {
 241         bus->write(bus, sw, 0, 1, val);
 242         bus->write(bus, sw, 0, 0, SW_SMI_CMD_W(dev, reg));
 243         mdelay(5);
 244 }
 245
 246 static int sw_scratch_read(struct mii_dev *bus, int sw, int reg)
 247 {
 248         sw_multi_write(bus, sw, 0x1c, 0x1a, (reg & 0x7f) << 8);
 249         return sw_multi_read(bus, sw, 0x1c, 0x1a) & 0xff;
 250 }
 251
 252 static void sw_led_write(struct mii_dev *bus, int sw, int port, int reg,
 253                          u16 val)
 254 {
 255         sw_multi_write(bus, sw, port, 0x16, 0x8000 | ((reg & 7) << 12)
 256                                             | (val & 0x7ff));
 257 }
 258
 259 static void sw_blink_leds(struct mii_dev *bus, int peridot, int topaz)
 260 {
 261         int i, p;
 262         struct {
 263                 int port;
 264                 u16 val;
 265                 int wait;
 266         } regs[] = {
 267                 { 2, 0xef, 1 }, { 2, 0xfe, 1 }, { 2, 0x33, 0 },
 268                 { 4, 0xef, 1 }, { 4, 0xfe, 1 }, { 4, 0x33, 0 },
 269                 { 3, 0xfe, 1 }, { 3, 0xef, 1 }, { 3, 0x33, 0 },
 270                 { 1, 0xfe, 1 }, { 1, 0xef, 1 }, { 1, 0x33, 0 }
 271         };
 272
 273         for (i = 0; i < 12; ++i) {
 274                 for (p = 0; p < peridot; ++p) {
 275                         sw_led_write(bus, 0x10 + p, regs[i].port, 0,
 276                                      regs[i].val);
 277                         sw_led_write(bus, 0x10 + p, regs[i].port + 4, 0,
 278                                      regs[i].val);
 279                 }
 280                 if (topaz) {
 281                         sw_led_write(bus, 0x2, 0x10 + regs[i].port, 0,
 282                                      regs[i].val);
 283                 }
 284
 285                 if (regs[i].wait)
 286                         mdelay(75);
 287         }
 288 }
 289
 290 static void check_switch_address(struct mii_dev *bus, int addr)
 291 {
 292         if (sw_scratch_read(bus, addr, 0x70) >> 3 != addr)
 293                 printf("Check of switch MDIO address failed for 0x%02x\n",
 294                        addr);
 295 }
 296
 297 static int sfp, pci, topaz, peridot, usb, passpci;
 298 static int sfp_pos, peridot_pos[3];
 299 static int module_count;
 300
 301 static int configure_peridots(struct gpio_desc *reset_gpio)
 302 {
 303         int i, ret;
 304         u8 dout[MAX_MOX_MODULES];
 305
 306         memset(dout, 0, MAX_MOX_MODULES);
 307
 308         /* set addresses of Peridot modules */
 309         for (i = 0; i < peridot; ++i)
 310                 dout[module_count - peridot_pos[i]] = (~i) & 3;
 311
 312         /*
 313          * if there is a SFP module connected to the last Peridot module, set
 314          * the P10_SMODE to 1 for the Peridot module
 315          */
 316         if (sfp)
 317                 dout[module_count - peridot_pos[i - 1]] |= 1 << 3;
 318
 319         dm_gpio_set_value(reset_gpio, 1);
 320         mdelay(10);
 321
 322         ret = mox_do_spi(NULL, dout, module_count + 1);
 323
 324         mdelay(10);
 325         dm_gpio_set_value(reset_gpio, 0);
 326
 327         mdelay(50);
 328
 329         return ret;
 330 }
 331
 332 static int get_reset_gpio(struct gpio_desc *reset_gpio)
 333 {
 334         int node;
 335
 336         node = fdt_node_offset_by_compatible(gd->fdt_blob, 0, "cznic,moxtet");
 337         if (node < 0) {
 338                 printf("Cannot find Moxtet bus device node!\n");
 339                 return -1;
 340         }
 341
 342         gpio_request_by_name_nodev(offset_to_ofnode(node), "reset-gpios", 0,
 343                                    reset_gpio, GPIOD_IS_OUT);
 344
 345         if (!dm_gpio_is_valid(reset_gpio)) {
 346                 printf("Cannot find reset GPIO for Moxtet bus!\n");
 347                 return -1;
 348         }
 349
 350         return 0;
 351 }
 352
 353 int misc_init_r(void)
 354 {
 355         int ret;
 356         u8 mac1[6], mac2[6];
 357
 358         ret = mbox_sp_get_board_info(NULL, mac1, mac2, NULL, NULL);
 359         if (ret < 0) {
 360                 printf("Cannot read data from OTP!\n");
 361                 return 0;
 362         }
 363
 364         if (is_valid_ethaddr(mac1) && !env_get("ethaddr"))
 365                 eth_env_set_enetaddr("ethaddr", mac1);
 366
 367         if (is_valid_ethaddr(mac2) && !env_get("eth1addr"))
 368                 eth_env_set_enetaddr("eth1addr", mac2);
 369
 370         return 0;
 371 }
 372
 373 static void mox_print_info(void)
 374 {
 375         int ret, board_version, ram_size;
 376         u64 serial_number;
 377         const char *pub_key;
 378
 379         ret = mbox_sp_get_board_info(&serial_number, NULL, NULL, &board_version,
 380                                      &ram_size);
 381         if (ret < 0)
 382                 return;
 383
 384         printf("Turris Mox:\n");
 385         printf("  Board version: %i\n", board_version);
 386         printf("  RAM size: %i MiB\n", ram_size);
 387         printf("  Serial Number: %016llX\n", serial_number);
 388
 389         pub_key = mox_sp_get_ecdsa_public_key();
 390         if (pub_key)
 391                 printf("  ECDSA Public Key: %s\n", pub_key);
 392         else
 393                 printf("Cannot read ECDSA Public Key\n");
 394 }
 395
 396 int last_stage_init(void)
 397 {
 398         int ret, i;
 399         const u8 *topology;
 400         int is_sd;
 401         struct mii_dev *bus;
 402         struct gpio_desc reset_gpio = {};
 403
 404         mox_print_info();
 405
 406         ret = mox_get_topology(&topology, &module_count, &is_sd);
 407         if (ret) {
 408                 printf("Cannot read module topology!\n");
 409                 return 0;
 410         }
 411
 412         printf("  SD/eMMC version: %s\n", is_sd ? "SD" : "eMMC");
 413
 414         if (module_count)
 415                 printf("Module Topology:\n");
 416
 417         for (i = 0; i < module_count; ++i) {
 418                 switch (topology[i]) {
 419                 case MOX_MODULE_SFP:
 420                         printf("% 4i: SFP Module\n", i + 1);
 421                         break;
 422                 case MOX_MODULE_PCI:
 423                         printf("% 4i: Mini-PCIe Module\n", i + 1);
 424                         break;
 425                 case MOX_MODULE_TOPAZ:
 426                         printf("% 4i: Topaz Switch Module (4-port)\n", i + 1);
 427                         break;
 428                 case MOX_MODULE_PERIDOT:
 429                         printf("% 4i: Peridot Switch Module (8-port)\n", i + 1);
 430                         break;
 431                 case MOX_MODULE_USB3:
 432                         printf("% 4i: USB 3.0 Module (4 ports)\n", i + 1);
 433                         break;
 434                 case MOX_MODULE_PASSPCI:
 435                         printf("% 4i: Passthrough Mini-PCIe Module\n", i + 1);
 436                         break;
 437                 default:
 438                         printf("% 4i: unknown (ID %i)\n", i + 1, topology[i]);
 439                 }
 440         }
 441
 442         /* now check if modules are connected in supported mode */
 443
 444         for (i = 0; i < module_count; ++i) {
 445                 switch (topology[i]) {
 446                 case MOX_MODULE_SFP:
 447                         if (sfp) {
 448                                 printf("Error: Only one SFP module is supported!\n");
 449                         } else if (topaz) {
 450                                 printf("Error: SFP module cannot be connected after Topaz Switch module!\n");
 451                         } else {
 452                                 sfp_pos = i;
 453                                 ++sfp;
 454                         }
 455                         break;
 456                 case MOX_MODULE_PCI:
 457                         if (pci)
 458                                 printf("Error: Only one Mini-PCIe module is supported!\n");
 459                         else if (usb)
 460                                 printf("Error: Mini-PCIe module cannot come after USB 3.0 module!\n");
 461                         else if (i && (i != 1 || !passpci))
 462                                 printf("Error: Mini-PCIe module should be the first connected module or come right after Passthrough Mini-PCIe module!\n");
 463                         else
 464                                 ++pci;
 465                         break;
 466                 case MOX_MODULE_TOPAZ:
 467                         if (topaz)
 468                                 printf("Error: Only one Topaz module is supported!\n");
 469                         else if (peridot >= 3)
 470                                 printf("Error: At most two Peridot modules can come before Topaz module!\n");
 471                         else
 472                                 ++topaz;
 473                         break;
 474                 case MOX_MODULE_PERIDOT:
 475                         if (sfp || topaz) {
 476                                 printf("Error: Peridot module must come before SFP or Topaz module!\n");
 477                         } else if (peridot >= 3) {
 478                                 printf("Error: At most three Peridot modules are supported!\n");
 479                         } else {
 480                                 peridot_pos[peridot] = i;
 481                                 ++peridot;
 482                         }
 483                         break;
 484                 case MOX_MODULE_USB3:
 485                         if (pci)
 486                                 printf("Error: USB 3.0 module cannot come after Mini-PCIe module!\n");
 487                         else if (usb)
 488                                 printf("Error: Only one USB 3.0 module is supported!\n");
 489                         else if (i && (i != 1 || !passpci))
 490                                 printf("Error: USB 3.0 module should be the first connected module or come right after Passthrough Mini-PCIe module!\n");
 491                         else
 492                                 ++usb;
 493                         break;
 494                 case MOX_MODULE_PASSPCI:
 495                         if (passpci)
 496                                 printf("Error: Only one Passthrough Mini-PCIe module is supported!\n");
 497                         else if (i != 0)
 498                                 printf("Error: Passthrough Mini-PCIe module should be the first connected module!\n");
 499                         else
 500                                 ++passpci;
 501                 }
 502         }
 503
 504         /* now configure modules */
 505
 506         if (get_reset_gpio(&reset_gpio) < 0)
 507                 return 0;
 508
 509         if (peridot > 0) {
 510                 if (configure_peridots(&reset_gpio) < 0) {
 511                         printf("Cannot configure Peridot modules!\n");
 512                         peridot = 0;
 513                 }
 514         } else {
 515                 dm_gpio_set_value(&reset_gpio, 1);
 516                 mdelay(50);
 517                 dm_gpio_set_value(&reset_gpio, 0);
 518                 mdelay(50);
 519         }
 520
 521         if (peridot || topaz) {
 522                 /*
 523                  * now check if the addresses are set by reading Scratch & Misc
 524                  * register 0x70 of Peridot (and potentially Topaz) modules
 525                  */
 526
 527                 bus = miiphy_get_dev_by_name("neta@30000");
 528                 if (!bus) {
 529                         printf("Cannot get MDIO bus device!\n");
 530                 } else {
 531                         for (i = 0; i < peridot; ++i)
 532                                 check_switch_address(bus, 0x10 + i);
 533
 534                         if (topaz)
 535                                 check_switch_address(bus, 0x2);
 536
 537                         sw_blink_leds(bus, peridot, topaz);
 538                 }
 539         }
 540
 541         printf("\n");
 542
 543         return 0;
 544 }
 545
 546 #if defined(CONFIG_OF_BOARD_SETUP)
 547
 548 static int vnode_by_path(void *blob, const char *fmt, va_list ap)
 549 {
 550         char path[128];
 551
 552         vsnprintf(path, 128, fmt, ap);
 553         return fdt_path_offset(blob, path);
 554 }
 555
 556 static int node_by_path(void *blob, const char *fmt, ...)
 557 {
 558         va_list ap;
 559         int res;
 560
 561         va_start(ap, fmt);
 562         res = vnode_by_path(blob, fmt, ap);
 563         va_end(ap);
 564
 565         return res;
 566 }
 567
 568 static int phandle_by_path(void *blob, const char *fmt, ...)
 569 {
 570         va_list ap;
 571         int node, phandle, res;
 572
 573         va_start(ap, fmt);
 574         node = vnode_by_path(blob, fmt, ap);
 575         va_end(ap);
 576
 577         if (node < 0)
 578                 return node;
 579
 580         phandle = fdt_get_phandle(blob, node);
 581         if (phandle > 0)
 582                 return phandle;
 583
 584         phandle = fdt_get_max_phandle(blob);
 585         if (phandle < 0)
 586                 return phandle;
 587
 588         phandle += 1;
 589
 590         res = fdt_setprop_u32(blob, node, "linux,phandle", phandle);
 591         if (res < 0)
 592                 return res;
 593
 594         res = fdt_setprop_u32(blob, node, "phandle", phandle);
 595         if (res < 0)
 596                 return res;
 597
 598         return phandle;
 599 }
 600
 601 static int enable_by_path(void *blob, const char *fmt, ...)
 602 {
 603         va_list ap;
 604         int node;
 605
 606         va_start(ap, fmt);
 607         node = vnode_by_path(blob, fmt, ap);
 608         va_end(ap);
 609
 610         if (node < 0)
 611                 return node;
 612
 613         return fdt_setprop_string(blob, node, "status", "okay");
 614 }
 615
 616 static bool is_topaz(int id)
 617 {
 618         return topaz && id == peridot + topaz - 1;
 619 }
 620
 621 static int switch_addr(int id)
 622 {
 623         return is_topaz(id) ? 0x2 : 0x10 + id;
 624 }
 625
 626 static int setup_switch(void *blob, int id)
 627 {
 628         int res, addr, i, node, phandle;
 629
 630         addr = switch_addr(id);
 631
 632         /* first enable the switch by setting status = "okay" */
 633         res = enable_by_path(blob, MDIO_PATH "/switch%i@%x", id, addr);
 634         if (res < 0)
 635                 return res;
 636
 637         /*
 638          * now if there are more switches or a SFP module coming after,
 639          * enable corresponding ports
 640          */
 641         if (id < peridot + topaz - 1) {
 642                 res = enable_by_path(blob,
 643                                      MDIO_PATH "/switch%i@%x/ports/port@a",
 644                                      id, addr);
 645         } else if (id == peridot - 1 && !topaz && sfp) {
 646                 res = enable_by_path(blob,
 647                                      MDIO_PATH "/switch%i@%x/ports/port-sfp@a",
 648                                      id, addr);
 649         } else {
 650                 res = 0;
 651         }
 652         if (res < 0)
 653                 return res;
 654
 655         if (id >= peridot + topaz - 1)
 656                 return 0;
 657
 658         /* finally change link property if needed */
 659         node = node_by_path(blob, MDIO_PATH "/switch%i@%x/ports/port@a", id,
 660                             addr);
 661         if (node < 0)
 662                 return node;
 663
 664         for (i = id + 1; i < peridot + topaz; ++i) {
 665                 phandle = phandle_by_path(blob,
 666                                           MDIO_PATH "/switch%i@%x/ports/port@%x",
 667                                           i, switch_addr(i),
 668                                           is_topaz(i) ? 5 : 9);
 669                 if (phandle < 0)
 670                         return phandle;
 671
 672                 if (i == id + 1)
 673                         res = fdt_setprop_u32(blob, node, "link", phandle);
 674                 else
 675                         res = fdt_appendprop_u32(blob, node, "link", phandle);
 676                 if (res < 0)
 677                         return res;
 678         }
 679
 680         return 0;
 681 }
 682
 683 static int remove_disabled_nodes(void *blob)
 684 {
 685         while (1) {
 686                 int res, offset;
 687
 688                 offset = fdt_node_offset_by_prop_value(blob, -1, "status",
 689                                                        "disabled", 9);
 690                 if (offset < 0)
 691                         break;
 692
 693                 res = fdt_del_node(blob, offset);
 694                 if (res < 0)
 695                         return res;
 696         }
 697
 698         return 0;
 699 }
 700
 701 int ft_board_setup(void *blob, bd_t *bd)
 702 {
 703         int node, phandle, res;
 704
 705         /*
 706          * If MOX B (PCI), MOX F (USB) or MOX G (Passthrough PCI) modules are
 707          * connected, enable the PCIe node.
 708          */
 709         if (pci || usb || passpci) {
 710                 node = fdt_path_offset(blob, PCIE_PATH);
 711                 if (node < 0)
 712                         return node;
 713
 714                 res = fdt_setprop_string(blob, node, "status", "okay");
 715                 if (res < 0)
 716                         return res;
 717
 718                 /* Fix PCIe regions for devices with 4 GB RAM */
 719                 res = a3700_fdt_fix_pcie_regions(blob);
 720                 if (res < 0)
 721                         return res;
 722         }
 723
 724         /*
 725          * If MOX C (Topaz switch) and/or MOX E (Peridot switch) are connected,
 726          * enable the eth1 node and setup the switches.
 727          */
 728         if (peridot || topaz) {
 729                 int i;
 730
 731                 res = enable_by_path(blob, ETH1_PATH);
 732                 if (res < 0)
 733                         return res;
 734
 735                 for (i = 0; i < peridot + topaz; ++i) {
 736                         res = setup_switch(blob, i);
 737                         if (res < 0)
 738                                 return res;
 739                 }
 740         }
 741
 742         /*
 743          * If MOX D (SFP cage module) is connected, enable the SFP node and eth1
 744          * node. If there is no Peridot switch between MOX A and MOX D, add link
 745          * to the SFP node to eth1 node.
 746          * Also enable and configure SFP GPIO controller node.
 747          */
 748         if (sfp) {
 749                 res = enable_by_path(blob, SFP_PATH);
 750                 if (res < 0)
 751                         return res;
 752
 753                 res = enable_by_path(blob, ETH1_PATH);
 754                 if (res < 0)
 755                         return res;
 756
 757                 if (!peridot) {
 758                         phandle = phandle_by_path(blob, SFP_PATH);
 759                         if (phandle < 0)
 760                                 return res;
 761
 762                         node = node_by_path(blob, ETH1_PATH);
 763                         if (node < 0)
 764                                 return node;
 765
 766                         res = fdt_setprop_u32(blob, node, "sfp", phandle);
 767                         if (res < 0)
 768                                 return res;
 769
 770                         res = fdt_setprop_string(blob, node, "phy-mode",
 771                                                  "sgmii");
 772                         if (res < 0)
 773                                 return res;
 774                 }
 775
 776                 res = enable_by_path(blob, SFP_GPIO_PATH);
 777                 if (res < 0)
 778                         return res;
 779
 780                 if (sfp_pos) {
 781                         char newname[16];
 782
 783                         /* moxtet-sfp is on non-zero position, change default */
 784                         node = node_by_path(blob, SFP_GPIO_PATH);
 785                         if (node < 0)
 786                                 return node;
 787
 788                         res = fdt_setprop_u32(blob, node, "reg", sfp_pos);
 789                         if (res < 0)
 790                                 return res;
 791
 792                         sprintf(newname, "gpio@%x", sfp_pos);
 793
 794                         res = fdt_set_name(blob, node, newname);
 795                         if (res < 0)
 796                                 return res;
 797                 }
 798         }
 799
 800         fdt_fixup_ethernet(blob);
 801
 802         /* Finally remove disabled nodes, as per Rob Herring's request. */
 803         remove_disabled_nodes(blob);
 804
 805         return 0;
 806 }
 807
 808 #endif