arm: mvebu: turris_mox: start blinking PHY LEDs when entering rescue

[platform/kernel/u-boot.git] / common / iomux.c

diff --git a/common/iomux.c b/common/iomux.c
index a8be1ac..c428f71 100644 (file)
--- a/common/iomux.c
+++ b/common/iomux.c
@@ -15,10 +15,8 @@ void iomux_printdevs(const int console)
        int i;
        struct stdio_dev *dev;
 
-       for (i = 0; i < cd_count[console]; i++) {
-               dev = console_devices[console][i];
+       for_each_console_dev(i, console, dev)
                printf("%s ", dev->name);
-       }
        printf("\n");
 }
 
@@ -141,4 +139,41 @@ int iomux_doenv(const int console, const char *arg)
        free(old_set);
        return 0;
 }
+
+int iomux_replace_device(const int console, const char *old, const char *new)
+{
+       struct stdio_dev *dev;
+       char *arg = NULL;       /* Initial empty list */
+       int size = 1;           /* For NUL terminator */
+       int i, ret;
+
+       for_each_console_dev(i, console, dev) {
+               const char *name = strcmp(dev->name, old) ? dev->name : new;
+               char *tmp;
+
+               /* Append name with a ',' (comma) separator */
+               tmp = realloc(arg, size + strlen(name) + 1);
+               if (!tmp) {
+                       free(arg);
+                       return -ENOMEM;
+               }
+
+               if (arg) {
+                       strcat(tmp, ",");
+                       strcat(tmp, name);
+               }
+               else
+                       strcpy(tmp, name);
+
+               arg = tmp;
+               size = strlen(tmp) + 1;
+       }
+
+       ret = iomux_doenv(console, arg);
+       if (ret)
+               ret = -EINVAL;
+
+       free(arg);
+       return ret;
+}
 #endif /* CONSOLE_MUX */