[SeaBIOS] [PATCH 2/2] Add functions for boot device path parsing.

[Gleb Natapov]

Add functions that find boot device path for pci/ata/floppy/rom devices.

Signed-off-by: Gleb Natapov 
---
 src/boot.c |  144 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 files changed, 140 insertions(+), 4 deletions(-)

diff --git a/src/boot.c b/src/boot.c
index e43d1a5..a670d3a 100644
--- a/src/boot.c
+++ b/src/boot.c
@@ -13,6 +13,7 @@
 #include "boot.h" // struct ipl_s
 #include "cmos.h" // inb_cmos
 #include "paravirt.h"
+#include "pci.h" //pci_bdf_to_*

struct ipl_s IPL;

@@ -493,22 +494,157 @@ handle_19(void)
     do_boot(0);
 }

-int bootprio_find_pci_device(int bdf)
+/*
+ * function returns string representing firts device path element in 'dp'
+ * and puts pointer to the rest of the device path into 'end'
+ */
+static char *dev_path_get_node(const char *dp, const char **end)
+{
+    int len;
+    char *node;
+
+    dp += 1; /* skip '/' */
+
+    *end = strchr(dp, '/');
+
+    if (*end == NULL) {
+        len = strlen(dp); /* last path element */
+        *end = dp + len;
+    } else
+        len = *end - dp;
+
+    if (len == 0)
+        return NULL;
+
+    node = malloc_tmphigh(len + 1);
+    if (!node) {
+        warn_noalloc();
+        return NULL;
+    }
+    memcpy(node, dp, len);
+    node[len] = '\0';
+
+    return node;
+}
+
+static int match_unit_address(const char *pe, const char *unit_address)
 {
+    char *s = strchr(pe, '@');
+
+    if (s == NULL)
+        return 0;
+
+    return !strcmp(s + 1, unit_address);
+}
+
+static int match_next_node_address(const char *path, const char *addr,
+                                   const char **rest)
+{
+    char *node = dev_path_get_node(path, rest);
+
+    if (!node)
+        return 0;
+
+    int r = match_unit_address(node, addr);
+    free(node);
+
+    return r;
+}
+
+static int match_pci_node(const char *path, int bdf, const char **rest)
+{
+    char pci[7];
+    int dev = pci_bdf_to_dev(bdf), fn = pci_bdf_to_fn(bdf);
+
+    if (!fn)
+        snprintf(pci, sizeof(pci), "%x", dev);
+    else
+        snprintf(pci, sizeof(pci), "%x,%x", dev, fn);
+
+    return match_next_node_address(path, pci, rest);
+}
+
+#define FW_PCI_DOMAIN "/pci@i0cf8"
+
+static int find_pci_device(int bdf, int start_idx, const char **rest)
+{
+    int i;
+
+    for (i = start_idx; i < IPL.fw_bootorder_count; i++) {
+        const char *path = IPL.fw_bootorder[i];
+
+        /* is pci domain? */
+        if (memcmp(path, FW_PCI_DOMAIN, strlen(FW_PCI_DOMAIN)))
+            continue;
+
+        if (match_pci_node(path + strlen(FW_PCI_DOMAIN), bdf, rest))
+            return i;
+    }
+
     return -1;
 }

+int bootprio_find_pci_device(int bdf)
+{
+    const char *rest;
+
+    return find_pci_device(bdf, 0, &rest);
+}
+
 int bootprio_find_ata_device(int bdf, int chanid, int slave)
 {
-    return -1;
+    const char *rest;
+    int idx = 0;
+
+    if (bdf == -1)
+        return -1; /* support only pci machine for now */
+
+    while((idx = find_pci_device(bdf, idx, &rest)) != -1) {
+        if (match_next_node_address(rest, chanid ? "1" : "0", &rest) &&
+            match_next_node_address(rest, slave ? "1" : "0", &rest))
+            break;
+        idx++;
+    }
+
+    return idx;
 }

-int bootprio_find_fdc_device(int bfd, int port, int fdid)
+
+int bootprio_find_fdc_device(int bdf, int port, int fdid)
 {
-    return -1;
+    const char *rest;
+    int idx = 0;
+    char addr[5];
+
+    if (bdf == -1)
+        return -1; /* support only pci machine for now */
+
+    snprintf(addr, sizeof addr, "%04x", port);
+
+    while((idx = find_pci_device(bdf, idx, &rest)) != -1) {
+        if (match_next_node_address(rest, addr, &rest) &&
+            match_next_node_address(rest, fdid ? "1" : "0", &rest))
+            break;
+        idx++;
+    }
+
+    return idx;
 }

int bootprio_find_named_rom(const char *name)
 {
+    u8 i;
+
+    for (i = 0; i < IPL.fw_bootorder_count; i++) {
+        const char *path = IPL.fw_bootorder[i];
+
+        /* is this rom file? */
+        if (memcmp(path, "/rom@", strlen("/rom@")))
+            continue;
+
+        if (strcmp(path + strlen("/rom@"), name))
+            return i;
+    }
+
     return -1;
 }