efi: add multifs support

The system firmware will expose device handles of all MBR/GPT partitions
which support BlockIo and/or DiskIo protocols, so find them and pass
through multifs driver.

Even in EFI environment ldlinux will be loaded up, so to avoid
initialise multifs twice call init_multifs() earlier.

Cc: Gene Cumm <gene.cumm@gmail.com>
Signed-off-by: Paulo Alcantara <pcacjr@zytor.com>

9 files changed, 387 insertions, 23 deletions

diff --git a/com32/elflink/ldlinux/execute.c b/com32/elflink/ldlinux/execute.c
index 39555715..fdfe88c1 100644
--- a/com32/elflink/ldlinux/execute.c
+++ b/com32/elflink/ldlinux/execute.c
@@ -30,6 +30,7 @@
 #include <syslinux/movebits.h>
 #include <syslinux/config.h>
 #include <syslinux/boot.h>
+#include <syslinux/multifs_utils.h>
 
 const struct image_types image_boot_types[] = {
     { "localboot", IMAGE_TYPE_LOCALBOOT },
@@ -149,6 +150,8 @@ __export void execute(const char *cmdline, uint32_t type, bool sysappend)
 		if (!config)
 			goto out;
 
+		init_multifs();
+
 		realpath(config, kernel, FILENAME_MAX);
 
 		/* If we got anything on the command line, do a chdir */
diff --git a/com32/elflink/ldlinux/ldlinux.c b/com32/elflink/ldlinux/ldlinux.c
index 369e97b3..0172117b 100644
--- a/com32/elflink/ldlinux/ldlinux.c
+++ b/com32/elflink/ldlinux/ldlinux.c
@@ -306,8 +306,6 @@ __export int main(int argc __unused, char **argv)
 	size_t count = 0;
 	int retval;
 
-	init_multifs(); /* Init MultiFS support */
-	
 	ldlinux_console_init();
 
 	parse_configs(&argv[1]);
diff --git a/core/fs/fs.c b/core/fs/fs.c
index a5d8db4b..1bea6b59 100644
--- a/core/fs/fs.c
+++ b/core/fs/fs.c
@@ -371,7 +371,6 @@ __export int open_file(const char *name, int flags, struct com32_filedata *filed
     filedata->handle	= rv;
 
     restore_fs();
-    restore_chdir_start();
 
     return rv;
 }
diff --git a/core/fs/readdir.c b/core/fs/readdir.c
index 2a1efded..07fae5da 100644
--- a/core/fs/readdir.c
+++ b/core/fs/readdir.c
@@ -29,7 +29,6 @@ __export DIR *opendir(const char *path)
     }
 
     restore_fs();
-    restore_chdir_start();
 
     return (DIR *)file;
 }
diff --git a/core/multifs.c b/core/multifs.c
index 8951ef76..aafce571 100644
--- a/core/multifs.c
+++ b/core/multifs.c
@@ -71,6 +71,5 @@ int switch_fs(const char **path)
     }
 ret:
     this_fs = fs;
-    restore_chdir_start();
     return 0;
 }
diff --git a/efi/diskio.c b/efi/diskio.c
index d6a160e1..64014fe4 100644
--- a/efi/diskio.c
+++ b/efi/diskio.c
@@ -43,7 +43,7 @@ static int efi_rdwr_sectors(struct disk *disk, void *buf,
 
 struct disk *efi_disk_init(void *private)
 {
-    static struct disk disk;
+    struct disk *disk;
     struct efi_disk_private *priv = (struct efi_disk_private *)private;
     EFI_HANDLE handle = priv->dev_handle;
     EFI_BLOCK_IO *bio;
@@ -60,33 +60,37 @@ struct disk *efi_disk_init(void *private)
     if (status != EFI_SUCCESS)
 	    return NULL;
 
+    disk = malloc(sizeof(*disk));
+    if (!disk)
+        return NULL;
+
     /*
      * XXX Do we need to map this to a BIOS disk number?
      */
-    disk.disk_number   = bio->Media->MediaId;
+    disk->disk_number   = bio->Media->MediaId;
 
-    disk.sector_size   = bio->Media->BlockSize;
-    disk.rdwr_sectors  = efi_rdwr_sectors;
-    disk.sector_shift  = ilog2(disk.sector_size);
+    disk->sector_size   = bio->Media->BlockSize;
+    disk->rdwr_sectors  = efi_rdwr_sectors;
+    disk->sector_shift  = ilog2(disk->sector_size);
 
-    dprintf("sector_size=%d, disk_number=%d\n", disk.sector_size,
-	    disk.disk_number);
+    dprintf("sector_size=%d, disk_number=%d\n", disk->sector_size,
+	    disk->disk_number);
 
     priv->bio = bio;
     priv->dio = dio;
-    disk.private = private;
+    disk->private = private;
 #if 0
 
-    disk.part_start    = part_start;
-    disk.secpercyl     = disk.h * disk.s;
+    disk->part_start    = part_start;
+    disk->secpercyl     = disk->h * disk->s;
 
 
-    disk.maxtransfer   = MaxTransfer;
+    disk->maxtransfer   = MaxTransfer;
 
     dprintf("disk %02x cdrom %d type %d sector %u/%u offset %llu limit %u\n",
-	    media_id, cdrom, ebios, sector_size, disk.sector_shift,
-	    part_start, disk.maxtransfer);
+	    media_id, cdrom, ebios, sector_size, disk->sector_shift,
+	    part_start, disk->maxtransfer);
 #endif
 
-    return &disk;
+    return disk;
 }
diff --git a/efi/main.c b/efi/main.c
index fd95f5c8..43b920b5 100644
--- a/efi/main.c
+++ b/efi/main.c
@@ -16,6 +16,7 @@
 #include "fio.h"
 #include "version.h"
 #include "efi_pxe.h"
+#include "multifs_utils.h"
 
 __export uint16_t PXERetry;
 __export char copyright_str[] = "Copyright (C) 2011-" YEAR_STR "\n";
@@ -1267,6 +1268,11 @@ static inline void syslinux_register_efi(void)
 
 extern void init(void);
 extern const struct fs_ops vfat_fs_ops;
+extern const struct fs_ops ext2_fs_ops;
+extern const struct fs_ops ntfs_fs_ops;
+extern const struct fs_ops xfs_fs_ops;
+extern const struct fs_ops btrfs_fs_ops;
+extern const struct fs_ops ufs_fs_ops;
 extern const struct fs_ops pxe_fs_ops;
 
 char free_high_memory[4096];
@@ -1274,6 +1280,11 @@ char free_high_memory[4096];
 extern char __bss_start[];
 extern char __bss_end[];
 
+static const struct fs_ops *fs_ops[] = {
+	&vfat_fs_ops, &ext2_fs_ops, &ntfs_fs_ops, &xfs_fs_ops, &btrfs_fs_ops,
+	&ufs_fs_ops, &pxe_fs_ops, NULL,
+};
+
 static void efi_setcwd(CHAR16 *dp)
 {
 	CHAR16 *c16;
@@ -1310,7 +1321,6 @@ EFI_STATUS efi_main(EFI_HANDLE image, EFI_SYSTEM_TABLE *table)
 	EFI_PXE_BASE_CODE *pxe;
 	EFI_LOADED_IMAGE *info;
 	EFI_STATUS status = EFI_SUCCESS;
-	const struct fs_ops *ops[] = { NULL, NULL };
 	unsigned long len = (unsigned long)__bss_end - (unsigned long)__bss_start;
 	static struct efi_disk_private priv;
 	SIMPLE_INPUT_INTERFACE *in;
@@ -1347,10 +1357,14 @@ EFI_STATUS efi_main(EFI_HANDLE image, EFI_SYSTEM_TABLE *table)
 		}
 
 		efi_derivative(SYSLINUX_FS_SYSLINUX);
-		ops[0] = &vfat_fs_ops;
+		status = init_multifs();
+		if (EFI_ERROR(status)) {
+			Print(L"Failed to initialise multifs support: %r\n",
+			      status);
+			goto out;
+		}
 	} else {
 		efi_derivative(SYSLINUX_FS_PXELINUX);
-		ops[0] = &pxe_fs_ops;
 		image_device_handle = info->DeviceHandle;
 	}
 
@@ -1371,7 +1385,7 @@ EFI_STATUS efi_main(EFI_HANDLE image, EFI_SYSTEM_TABLE *table)
 	 */
 	efi_setcwd(DevicePathToStr(info->FilePath));
 
-	fs_init(ops, (void *)&priv);
+	fs_init(fs_ops, (void *)&priv);
 
 	/*
 	 * There may be pending user input that wasn't processed by
diff --git a/efi/multifs_utils.c b/efi/multifs_utils.c
new file mode 100644
index 00000000..a4e3ff9c
--- /dev/null
+++ b/efi/multifs_utils.c
@@ -0,0 +1,296 @@
+/*
+ * Copyright (c) 2015 Paulo Alcantara <pcacjr@zytor.com>
+ * Copyright (C) 2012 Andre Ericson <de.ericson@gmail.com>
+ * Copyright (C) 2013 Raphael S. Carvalho <raphael.scarv@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+*/
+#include <fs.h>
+#include <ilog2.h>
+#include <disk.h>
+
+#include "cache.h"
+#include "minmax.h"
+#include "multifs_utils.h"
+#include "efi.h"
+
+#define DISKS_MAX 0xff
+
+static EFI_HANDLE *_logical_parts = NULL;
+static unsigned int _logical_parts_no = 0;
+static struct queue_head *parts_info[DISKS_MAX];
+
+/* Find all BlockIo device handles which is a logical partition */
+static EFI_STATUS find_all_logical_parts(void)
+{
+    EFI_STATUS status;
+    unsigned long len = 0;
+    EFI_HANDLE *handles = NULL;
+    unsigned long i;
+    EFI_BLOCK_IO *bio;
+
+    if (_logical_parts) {
+        status = EFI_SUCCESS;
+        goto out;
+    }
+
+    status = uefi_call_wrapper(BS->LocateHandle, 5, ByProtocol,
+                               &BlockIoProtocol, NULL, &len, NULL);
+    if (EFI_ERROR(status) && status != EFI_BUFFER_TOO_SMALL)
+        goto out;
+
+    handles = malloc(len);
+    if (!handles) {
+        status = EFI_OUT_OF_RESOURCES;
+        goto out;
+    }
+
+    _logical_parts = malloc(len);
+    if (!_logical_parts) {
+        status = EFI_OUT_OF_RESOURCES;
+        goto out_free;
+    }
+
+    status = uefi_call_wrapper(BS->LocateHandle, 5, ByProtocol,
+                               &BlockIoProtocol, NULL, &len,
+                               (void **)handles);
+    if (EFI_ERROR(status))
+        goto out_free;
+
+    for (i = 0; i < len / sizeof(EFI_HANDLE); i++) {
+        status = uefi_call_wrapper(BS->HandleProtocol, 3, handles[i],
+                                   &BlockIoProtocol, (void **)&bio);
+        if (EFI_ERROR(status))
+            goto out_free;
+        if (bio->Media->LogicalPartition) {
+            _logical_parts[_logical_parts_no++] = handles[i];
+        }
+    }
+
+    free(handles);
+    return status;
+
+out_free:
+    if (handles)
+        free(handles);
+    if (_logical_parts)
+        free(_logical_parts);
+out:
+    return status;
+}
+
+static inline EFI_HANDLE get_logical_part(unsigned int partno)
+{
+    if (!_logical_parts || partno > _logical_parts_no)
+        return NULL;
+    return _logical_parts[partno - 1];
+}
+
+static int add_fs(struct fs_info *fs, uint8_t disk, uint8_t partition)
+{
+    struct queue_head *head = parts_info[disk];
+    struct part_node *node;
+
+    node = malloc(sizeof(struct part_node));
+    if (!node)
+        return -1;
+    node->fs = fs;
+    node->next = NULL;
+    node->partition = partition;
+
+    if (!head) {
+        head = malloc(sizeof(struct queue_head));
+        if (!head) {
+            free(node);
+            return -1;
+        }
+        head->first = head->last = node;
+        parts_info[disk] = head;
+        return 0;
+    }
+    head->last->next = node;
+    head->last = node;
+    return 0;
+}
+
+static struct fs_info *get_fs(uint8_t disk, uint8_t partition)
+{
+    struct part_node *i;
+
+    for (i = parts_info[disk]->first; i; i = i->next) {
+        if (i->partition == partition)
+            return i->fs;
+    }
+    return NULL;
+}
+
+static EFI_HANDLE find_partition(unsigned int diskno, unsigned int partno)
+{
+    return get_logical_part(partno);
+}
+
+static const char *get_num(const char *p, char delimiter, unsigned int *data)
+{
+    uint32_t n = 0;
+
+    while (*p) {
+        if (*p < '0' || *p > '9')
+            break;
+        n = (n * 10) + (*p - '0');
+        p++;
+        if (*p == delimiter) {
+            p++; /* skip delimiter */
+            *data = min(n, UINT8_MAX); /* avoid overflow */
+            return p;
+        }
+    }
+    return NULL;
+}
+
+static int parse_multifs_path(const char **path, unsigned int *hdd,
+                              unsigned int *partition)
+{
+    const char *p = *path;
+    static const char *cwd = ".";
+
+    *hdd = *partition = 0;
+    p++; /* Skip open parentheses */
+
+    /* Get hd number (Range: 0 - (DISKS_MAX - 1)) */
+    if (*p != 'h' || *(p + 1) != 'd')
+        return -1;
+
+    p += 2; /* Skip 'h' and 'd' */
+    p = get_num(p, ',', hdd);
+    if (!p)
+        return -1;
+    if (*hdd >= DISKS_MAX) {
+        printf("MultiFS: hdd is out of range: 0-%d\n", DISKS_MAX - 1);
+        return -1;
+    }
+
+    /* Get partition number (Range: 0 - 0xFF) */
+    p = get_num(p, ')', partition);
+    if (!p)
+        return -1;
+
+    if (*p == '\0') {
+        /* Assume it's a cwd request */
+        p = cwd;
+    }
+
+    *path = p;
+    dprintf("MultiFS: hdd: %u partition: %u path: %s\n",
+            *hdd, *partition, *path);
+    return 0;
+}
+
+static inline void *get_private(EFI_HANDLE lpart)
+{
+    static struct efi_disk_private priv;
+    priv.dev_handle = lpart;
+    return (void *)&priv;
+}
+
+static struct fs_info *get_fs_info(const char **path)
+{
+    const struct fs_ops **ops;
+    struct fs_info *fsp;
+    EFI_HANDLE dh;
+    struct device *dev = NULL;
+    void *private;
+    int blk_shift = -1;
+    unsigned int hdd, partition;
+
+    if (parse_multifs_path(path, &hdd, &partition)) {
+        return NULL;
+    }
+
+    fsp = get_fs(hdd, partition - 1);
+    if (fsp)
+        return fsp;
+
+    fsp = malloc(sizeof(struct fs_info));
+    if (!fsp)
+        return NULL;
+
+    dh = find_partition(hdd, partition);
+    if (!dh)
+        goto bail;
+    dprintf("\nMultiFS: found partition %d\n", partition);
+    private = get_private(dh);
+
+    /* set default name for the root directory */
+    fsp->cwd_name[0] = '/';
+    fsp->cwd_name[1] = '\0';
+
+    ops = p_ops;
+    while ((blk_shift < 0) && *ops) {
+        /* set up the fs stucture */
+        fsp->fs_ops = *ops;
+
+        /*
+         * This boldly assumes that we don't mix FS_NODEV filesystems
+         * with FS_DEV filesystems...
+         */
+        if (fsp->fs_ops->fs_flags & FS_NODEV) {
+            fsp->fs_dev = NULL;
+        } else {
+            if (!dev) {
+                dev = device_init(private);
+                if (!dev)
+                    goto bail;
+            }
+            fsp->fs_dev = dev;
+        }
+        /* invoke the fs-specific init code */
+        blk_shift = fsp->fs_ops->fs_init(fsp);
+        ops++;
+    }
+    if (blk_shift < 0) {
+        dprintf("MultiFS: No valid file system found!\n");
+        goto free_dev;
+    }
+
+    if (add_fs(fsp, hdd, partition - 1))
+        goto free_dev;
+    if (fsp->fs_dev && fsp->fs_dev->cache_data && !fsp->fs_dev->cache_init)
+        cache_init(fsp->fs_dev, blk_shift);
+    if (fsp->fs_ops->iget_root) {
+        fsp->root = fsp->fs_ops->iget_root(fsp);
+        fsp->cwd = get_inode(fsp->root);
+    }
+    return fsp;
+
+free_dev:
+    free(dev->disk);
+    free(dev->cache_data);
+    free(dev);
+bail:
+    free(fsp);
+    return NULL;
+}
+
+EFI_STATUS init_multifs(void)
+{
+    EFI_STATUS status;
+
+    status = find_all_logical_parts();
+    enable_multifs(get_fs_info);
+    dprintf("MultiFS: initialised\n");
+
+    return status;
+}
diff --git a/efi/multifs_utils.h b/efi/multifs_utils.h
new file mode 100644
index 00000000..7ae5b43e
--- /dev/null
+++ b/efi/multifs_utils.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2012-2015 Paulo Alcantara <pcacjr@zytor.com>
+ * Copyright (C) 2012 Andre Ericson <de.ericson@gmail.com>
+ * Copyright (C) 2013 Raphael S. Carvalho <raphael.scarv@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+*/
+
+#ifndef MULTIDISK_UTILS_H
+#define MULTIDISK_UTILS_H
+
+#include <fs.h>
+
+#include "efi.h"
+
+struct part_node {
+    int partition;
+    struct fs_info *fs;
+    struct part_node *next;
+};
+
+struct queue_head {
+    struct part_node *first;
+    struct part_node *last;
+};
+
+/*
+ * Needs to keep ROOT_FS_OPS after fs_init()
+ * to be used by multidisk
+ */
+extern const struct fs_ops **p_ops;
+
+/*
+ * Used to initialize MultiFS support
+ */
+extern void enable_multifs(void *);
+extern EFI_STATUS init_multifs(void);
+
+#endif /* MULTIDISK_UTILS_H */