From 598185f8067b9b2837a2d312064a99e566d762b4 Mon Sep 17 00:00:00 2001 From: Vladimir Serbinenko Date: Sun, 21 Feb 2016 00:04:18 +0100 Subject: [PATCH] fdtbus --- grub-core/bus/fdt.c | 154 ++++++++++++++++++++++++++++++++++++------ include/grub/fdtbus.h | 20 ++++-- 2 files changed, 149 insertions(+), 25 deletions(-) diff --git a/grub-core/bus/fdt.c b/grub-core/bus/fdt.c index 065035ca1..c0b031f76 100644 --- a/grub-core/bus/fdt.c +++ b/grub-core/bus/fdt.c @@ -21,15 +21,19 @@ #include static const void *dtb; +static grub_size_t root_address_cells, root_size_cells; +/* Pointer to this symbol signals invalid mapping. */ +char grub_fdtbus_invalid_mapping[1]; -struct grub_fdtbus_device +struct grub_fdtbus_dev { - struct grub_fdtbus_device *next; + struct grub_fdtbus_dev *next; + struct grub_fdtbus_dev *parent; int node; struct grub_fdtbus_driver *driver; }; -struct grub_fdtbus_device *devs; +struct grub_fdtbus_dev *devs; struct grub_fdtbus_driver *drivers; static int @@ -49,14 +53,14 @@ is_compatible (struct grub_fdtbus_driver *driver, return 0; } -void -grub_fdtbus_scan (int parent) +static void +fdtbus_scan (struct grub_fdtbus_dev *parent) { int node; - for (node = grub_fdt_first_node (dtb, parent); node >= 0; + for (node = grub_fdt_first_node (dtb, parent ? parent->node : 0); node >= 0; node = grub_fdt_next_node (dtb, node)) { - struct grub_fdtbus_device *dev; + struct grub_fdtbus_dev *dev; struct grub_fdtbus_driver *driver; dev = grub_zalloc (sizeof (*dev)); if (!dev) @@ -66,30 +70,32 @@ grub_fdtbus_scan (int parent) } dev->node = node; dev->next = devs; + dev->parent = parent; devs = dev; FOR_LIST_ELEMENTS(driver, drivers) - if (is_compatible (driver, node)) + if (!dev->driver && is_compatible (driver, node)) { - if (driver->attach(dtb, dev->node) == GRUB_ERR_NONE) + if (driver->attach(dev) == GRUB_ERR_NONE) { dev->driver = driver; break; } grub_print_error (); } + fdtbus_scan (dev); } } void grub_fdtbus_register (struct grub_fdtbus_driver *driver) { - struct grub_fdtbus_device *dev; + struct grub_fdtbus_dev *dev; grub_list_push (GRUB_AS_LIST_P (&drivers), GRUB_AS_LIST (driver)); - FOR_LIST_ELEMENTS(dev, devs) - if (is_compatible (driver, dev->node)) + for (dev = devs; dev; dev = dev->next) + if (!dev->driver && is_compatible (driver, dev->node)) { - if (driver->attach(dtb, dev->node) == GRUB_ERR_NONE) + if (driver->attach(dev) == GRUB_ERR_NONE) { dev->driver = driver; break; @@ -102,14 +108,12 @@ void grub_fdtbus_unregister (struct grub_fdtbus_driver *driver) { grub_list_remove (GRUB_AS_LIST (driver)); - struct grub_fdtbus_device *dev; - grub_list_push (GRUB_AS_LIST_P (&drivers), - GRUB_AS_LIST (driver)); - FOR_LIST_ELEMENTS(dev, devs) + struct grub_fdtbus_dev *dev; + for (dev = devs; dev; dev = dev->next) if (dev->driver == driver) { if (driver->detach) - driver->detach(dtb, dev->node); + driver->detach(dev); dev->driver = 0; } } @@ -118,7 +122,117 @@ void grub_fdtbus_init (const void *dtb_in, grub_size_t size) { if (!dtb_in || grub_fdt_check_header (dtb_in, size) < 0) - return; + grub_fatal ("invalid FDT"); dtb = dtb_in; - grub_fdtbus_scan (0); + const grub_uint32_t *prop = grub_fdt_get_prop (dtb, 0, "#address-cells", 0); + if (prop) + root_address_cells = grub_be_to_cpu32 (*prop); + else + root_address_cells = 1; + + prop = grub_fdt_get_prop (dtb, 0, "#size-cells", 0); + if (prop) + root_size_cells = grub_be_to_cpu32 (*prop); + else + root_size_cells = 1; + + fdtbus_scan (0); +} + +static int +get_address_cells (const struct grub_fdtbus_dev *dev) +{ + const grub_uint32_t *prop; + if (!dev) + return root_address_cells; + prop = grub_fdt_get_prop (dtb, dev->node, "#address-cells", 0); + if (prop) + return grub_be_to_cpu32 (*prop); + return 1; +} + +static int +get_size_cells (const struct grub_fdtbus_dev *dev) +{ + const grub_uint32_t *prop; + if (!dev) + return root_size_cells; + prop = grub_fdt_get_prop (dtb, dev->node, "#size-cells", 0); + if (prop) + return grub_be_to_cpu32 (*prop); + return 1; +} + +static grub_uint64_t +get64 (const grub_uint32_t *reg, grub_size_t cells) +{ + grub_uint64_t val = 0; + if (cells >= 1) + val = grub_be_to_cpu32 (reg[cells - 1]); + if (cells >= 2) + val |= ((grub_uint64_t) grub_be_to_cpu32 (reg[cells - 2])) << 32; + return val; +} + +static volatile void * +translate (const struct grub_fdtbus_dev *dev, const grub_uint32_t *reg) +{ + volatile void *ret; + const grub_uint32_t *ranges; + grub_size_t ranges_size, cells_per_mapping; + grub_size_t parent_address_cells, child_address_cells, child_size_cells; + grub_size_t nmappings, i; + if (dev == 0) + { + grub_uint64_t val; + val = get64 (reg, root_address_cells); + if (sizeof (void *) == 4 && (val >> 32)) + return grub_fdtbus_invalid_mapping; + return (void *) (grub_addr_t) val; + } + ranges = grub_fdt_get_prop (dtb, dev->node, "ranges", &ranges_size); + if (!ranges) + return grub_fdtbus_invalid_mapping; + if (ranges_size == 0) + return translate (dev->parent, reg); + parent_address_cells = get_address_cells (dev->parent); + child_address_cells = get_address_cells (dev); + child_size_cells = get_size_cells (dev); + cells_per_mapping = parent_address_cells + child_address_cells + child_size_cells; + nmappings = ranges_size / 4 / cells_per_mapping; + for (i = 0; i < nmappings; i++) + { + const grub_uint32_t *child_addr = &ranges[i * cells_per_mapping]; + const grub_uint32_t *parent_addr = child_addr + child_address_cells; + grub_uint64_t child_size = get64 (parent_addr + parent_address_cells, child_size_cells); + + if (child_address_cells > 2 && grub_memcmp (reg, child_addr, (child_address_cells - 2) * 4) != 0) + continue; + if (get64 (reg, child_address_cells) < get64 (child_addr, child_address_cells)) + continue; + + grub_uint64_t offset = get64 (reg, child_address_cells) - get64 (child_addr, child_address_cells); + if (offset >= child_size) + continue; + + ret = translate (dev->parent, parent_addr); + if (grub_fdtbus_is_mapping_valid (ret)) + ret = (volatile char *) ret + offset; + return ret; + } + return grub_fdtbus_invalid_mapping; +} + +volatile void * +grub_fdtbus_map_reg (const struct grub_fdtbus_dev *dev, int regno, grub_size_t *size) +{ + grub_size_t address_cells, size_cells; + address_cells = get_address_cells (dev->parent); + size_cells = get_size_cells (dev->parent); + const grub_uint32_t *reg = grub_fdt_get_prop (dtb, dev->node, "reg", 0); + if (size && size_cells) + *size = reg[(address_cells + size_cells) * regno + address_cells]; + if (size && !size_cells) + *size = 0; + return translate (dev->parent, reg + (address_cells + size_cells) * regno); } diff --git a/include/grub/fdtbus.h b/include/grub/fdtbus.h index 8d80802eb..4ebcd3bda 100644 --- a/include/grub/fdtbus.h +++ b/include/grub/fdtbus.h @@ -22,6 +22,8 @@ #include #include +struct grub_fdtbus_dev; + struct grub_fdtbus_driver { struct grub_fdtbus_driver *next; @@ -29,10 +31,21 @@ struct grub_fdtbus_driver const char *compatible; - grub_err_t (*attach) (const void *fdt, unsigned int nodeoffset); - void (*detach) (const void *fdt, unsigned int nodeoffset); + grub_err_t (*attach) (const struct grub_fdtbus_dev *dev); + void (*detach) (const struct grub_fdtbus_dev *dev); }; +extern char grub_fdtbus_invalid_mapping[1]; + +static inline int +grub_fdtbus_is_mapping_valid (volatile void *m) +{ + return m != grub_fdtbus_invalid_mapping; +} + +volatile void * +grub_fdtbus_map_reg (const struct grub_fdtbus_dev *dev, int reg, grub_size_t *size); + void grub_fdtbus_register (struct grub_fdtbus_driver *driver); @@ -46,7 +59,4 @@ grub_fdtbus_unregister (struct grub_fdtbus_driver *driver); void grub_fdtbus_init (const void *dtb, grub_size_t size); -void -grub_fdtbus_scan (int nodeoffset); - #endif -- 2.47.2