diff --git a/so3/so3/devices/device.c b/so3/so3/devices/device.c index 787636d6c5..7323968d17 100644 --- a/so3/so3/devices/device.c +++ b/so3/so3/devices/device.c @@ -1,5 +1,6 @@ /* - * Copyright (C) 2017-2026 Daniel Rossier + * Copyright (c) 2017-2026 REDS Institute, HEIG-VD + * Author: Daniel Rossier * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as @@ -23,6 +24,7 @@ #include #include #include +#include #include @@ -99,6 +101,15 @@ bool fdt_device_is_available(void *fdt_addr, int node_offset) return false; } +/* + * Descriptor stored in the driver hash map: it ties a compatible string (the + * map key) to the matching driver's init function and its initcall level. + */ +struct driver_match { + int (*init)(dev_t *dev, int fdt_offset); + int level; +}; + /* * Read the content of a device tree and associate a generic device info structure to each * relevant entry. @@ -111,11 +122,13 @@ void parse_dtb(void *fdt_addr) { unsigned int drivers_count[INITCALLS_LEVELS]; driver_initcall_t *driver_entries[INITCALLS_LEVELS]; - dev_t *dev; - int i, level; - int ret; + struct list_head pending[INITCALLS_LEVELS]; + struct driver_match *matches, *match; + struct hashmap *driver_map; + dev_t *dev, *tmp; + int i, level, ret, m; int offset, new_off; - bool found; + int total_drivers; drivers_count[CORE] = ll_entry_count(driver_initcall_t, core); driver_entries[CORE] = ll_entry_start(driver_initcall_t, core); @@ -127,51 +140,96 @@ void parse_dtb(void *fdt_addr) LOG_DEBUG("%s: # entries for postcore drivers : %d\n", __func__, drivers_count[POSTCORE]); LOG_DEBUG("Now scanning the device tree to retrieve all devices...\n"); - for (level = 0; level < INITCALLS_LEVELS; level++) { + total_drivers = drivers_count[CORE] + drivers_count[POSTCORE]; + + /* + * Index every registered driver by its compatible string so that each + * device tree node can be matched in O(1). Building the map is O(M) over + * the M drivers and the single device tree scan below is O(N) over the N + * nodes, turning the former O(N * M) nested loop into an O(N + M) parsing. + */ + driver_map = hashmap_create(total_drivers); + ASSERT(driver_map != NULL); + + matches = total_drivers ? (struct driver_match *) malloc(total_drivers * sizeof(*matches)) : NULL; + ASSERT(total_drivers == 0 || matches != NULL); + + for (level = 0; level < INITCALLS_LEVELS; level++) + INIT_LIST_HEAD(&pending[level]); + + /* + * Insert postcore drivers before core ones: hashmap_put() overwrites on a + * duplicate key, so a compatible shared by both levels ends up mapped to + * its core driver, preserving the core-before-postcore precedence. + */ + m = 0; + for (level = INITCALLS_LEVELS - 1; level >= 0; level--) { + for (i = 0; i < drivers_count[level]; i++) { + matches[m].init = driver_entries[level][i].init; + matches[m].level = level; + hashmap_put(driver_map, driver_entries[level][i].compatible, &matches[m]); + m++; + } + } + + /* + * Scan the device tree once. A single scratch entry probes every node -- + * get_dev_info() resets it on each call -- and is only kept (and replaced + * by a fresh allocation) when the node matches a driver. Matched nodes are + * queued on the list of their initcall level so that the initialization + * below can honour the core-before-postcore ordering. + */ + dev = (dev_t *) malloc(sizeof(dev_t)); + ASSERT(dev != NULL); + + offset = 0; + while ((new_off = get_dev_info(fdt_addr, offset, "*", dev)) != -1) { + offset = new_off; + + if (!fdt_device_is_available(fdt_addr, new_off)) + continue; + + match = (struct driver_match *) hashmap_get(driver_map, dev->compatible); + if (!match) + continue; + + list_add_tail(&dev->list, &pending[match->level]); + dev = (dev_t *) malloc(sizeof(dev_t)); ASSERT(dev != NULL); - memset(dev, 0, sizeof(dev_t)); - - found = false; - offset = 0; - - while ((new_off = get_dev_info(fdt_addr, offset, "*", dev)) != -1) { - if (fdt_device_is_available(fdt_addr, new_off)) { - for (i = 0; i < drivers_count[level]; i++) { - if (!strcmp(dev->compatible, driver_entries[level][i].compatible)) { - found = true; - - LOG_DEBUG("Found compatible: %s\n", driver_entries[level][i].compatible); - LOG_DEBUG(" Compatible: %s\n", dev->compatible); - LOG_DEBUG(" Status: %s\n", dev_state_str(dev->status)); - LOG_DEBUG(" Initcall level: %d\n", level); - - if (dev->status == STATUS_INIT_PENDING) { - ret = driver_entries[level][i].init(dev, new_off); - BUG_ON(ret); - - dev->status = STATUS_INITIALIZED; - list_add_tail(&dev->list, &devices); - } - break; - } - } - } - if (!found) - free(dev); + } + + /* The last scratch buffer was never consumed. */ + free(dev); + + /* + * Initialize the matched devices, core drivers before postcore ones and in + * device tree order within each level, then move them to the global list. + */ + for (level = 0; level < INITCALLS_LEVELS; level++) { + list_for_each_entry_safe(dev, tmp, &pending[level], list) { + match = (struct driver_match *) hashmap_get(driver_map, dev->compatible); - offset = new_off; + LOG_DEBUG("Found compatible: %s\n", dev->compatible); + LOG_DEBUG(" Status: %s\n", dev_state_str(dev->status)); + LOG_DEBUG(" Initcall level: %d\n", level); - dev = (dev_t *) malloc(sizeof(dev_t)); - ASSERT(dev != NULL); - memset(dev, 0, sizeof(dev_t)); + if (dev->status == STATUS_INIT_PENDING) { + ret = match->init(dev, dev->offset_dts); + BUG_ON(ret); - found = false; + dev->status = STATUS_INITIALIZED; + } + + list_del(&dev->list); + list_add_tail(&dev->list, &devices); } } - /* We have always the last allocation which will not be used */ - free(dev); + if (matches) + free(matches); + + hashmap_free(driver_map); } /* Register a device. Usually called from the device driver. */ diff --git a/so3/so3/include/hashmap.h b/so3/so3/include/hashmap.h new file mode 100644 index 0000000000..a72e5d477f --- /dev/null +++ b/so3/so3/include/hashmap.h @@ -0,0 +1,55 @@ +/* + * Copyright (c) 2017-2026 REDS Institute, HEIG-VD + * Author: Daniel Rossier + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifndef HASHMAP_H +#define HASHMAP_H + +#include + +/* + * A generic hash map associating string keys to void * values. + * + * It uses open addressing with linear probing and grows automatically to keep + * the load factor -- and therefore the lookup cost -- bounded. Keys are copied + * and owned by the map; values are owned by the caller and left untouched. + */ +struct hashmap; + +/* + * Create a map pre-sized to hold at least `capacity_hint` keys without growing. + * Returns NULL on allocation failure. + */ +struct hashmap *hashmap_create(size_t capacity_hint); + +/* Release the map and its copied keys. Values are not freed. */ +void hashmap_free(struct hashmap *map); + +/* + * Associate `value` with `key`, replacing any value previously stored for that + * key. The key string is copied. Returns 0 on success, -1 on allocation failure. + */ +int hashmap_put(struct hashmap *map, const char *key, void *value); + +/* Return the value associated with `key`, or NULL if the key is absent. */ +void *hashmap_get(const struct hashmap *map, const char *key); + +/* Return the number of entries currently stored in the map. */ +size_t hashmap_count(const struct hashmap *map); + +#endif /* HASHMAP_H */ diff --git a/so3/so3/include/math.h b/so3/so3/include/math.h new file mode 100644 index 0000000000..7844149ddd --- /dev/null +++ b/so3/so3/include/math.h @@ -0,0 +1,39 @@ +/* + * Copyright (c) 2017-2026 REDS Institute, HEIG-VD + * Author: Daniel Rossier + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifndef MATH_H +#define MATH_H + +#include + +/* + * Round @n up to the next power of two. Returns @n unchanged when it already is + * a power of two, and 1 for @n == 0. + */ +static inline size_t round_up_pow2(size_t n) +{ + size_t p = 1; + + while (p < n) + p <<= 1; + + return p; +} + +#endif /* MATH_H */ diff --git a/so3/so3/lib/Makefile b/so3/so3/lib/Makefile index 37dc955925..42ae8963d8 100644 --- a/so3/so3/lib/Makefile +++ b/so3/so3/lib/Makefile @@ -7,4 +7,5 @@ lib-y += ctype.o lib-y += vsprintf.o printk.o lib-y += strtod.o lib-y += list_sort.o +lib-y += hashmap.o diff --git a/so3/so3/lib/hashmap.c b/so3/so3/lib/hashmap.c new file mode 100644 index 0000000000..a6b611a7dc --- /dev/null +++ b/so3/so3/lib/hashmap.c @@ -0,0 +1,176 @@ +/* + * Copyright (c) 2017-2026 REDS Institute, HEIG-VD + * Author: Daniel Rossier + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include +#include +#include +#include + +#include + +/* Smallest table size; must be a power of two. */ +#define HASHMAP_MIN_CAPACITY 8u + +/* + * One table slot. An empty slot has a NULL key. There is no removal operation, + * so no tombstones are needed and a probe simply stops at the first empty slot. + */ +struct hashmap_slot { + char *key; /* owned copy of the key, NULL when the slot is empty */ + void *value; +}; + +struct hashmap { + struct hashmap_slot *slots; + size_t capacity; /* always a power of two */ + size_t count; /* number of live entries */ +}; + +/* FNV-1a hash over a NUL-terminated string. */ +static unsigned int str_hash(const char *s) +{ + unsigned int h = 2166136261u; + + while (*s) { + h ^= (unsigned char) *s++; + h *= 16777619u; + } + + return h; +} + +struct hashmap *hashmap_create(size_t capacity_hint) +{ + struct hashmap *map = (struct hashmap *) malloc(sizeof(*map)); + + if (!map) + return NULL; + + /* Size the table so `capacity_hint` keys stay below a 0.5 load factor. */ + map->capacity = round_up_pow2(capacity_hint * 2); + if (map->capacity < HASHMAP_MIN_CAPACITY) + map->capacity = HASHMAP_MIN_CAPACITY; + map->count = 0; + map->slots = (struct hashmap_slot *) calloc(map->capacity, sizeof(*map->slots)); + + if (!map->slots) { + free(map); + return NULL; + } + + return map; +} + +void hashmap_free(struct hashmap *map) +{ + size_t i; + + if (!map) + return; + + for (i = 0; i < map->capacity; i++) + if (map->slots[i].key) + free(map->slots[i].key); + + free(map->slots); + free(map); +} + +/* Insert an already-owned key/value pair; assumes a free slot exists and the + * key is not already present (used for fresh insertions and rehashing). */ +static void hashmap_place(struct hashmap_slot *slots, size_t capacity, char *key, void *value) +{ + size_t idx = str_hash(key) & (capacity - 1); + + while (slots[idx].key) + idx = (idx + 1) & (capacity - 1); + + slots[idx].key = key; + slots[idx].value = value; +} + +/* Double the table capacity and re-insert the existing entries. */ +static int hashmap_grow(struct hashmap *map) +{ + size_t new_capacity = map->capacity << 1; + struct hashmap_slot *new_slots; + size_t i; + + new_slots = (struct hashmap_slot *) calloc(new_capacity, sizeof(*new_slots)); + if (!new_slots) + return -1; + + for (i = 0; i < map->capacity; i++) + if (map->slots[i].key) + hashmap_place(new_slots, new_capacity, map->slots[i].key, map->slots[i].value); + + free(map->slots); + map->slots = new_slots; + map->capacity = new_capacity; + + return 0; +} + +int hashmap_put(struct hashmap *map, const char *key, void *value) +{ + size_t idx = str_hash(key) & (map->capacity - 1); + char *copy; + + /* Replace the value if the key is already present. */ + while (map->slots[idx].key) { + if (!strcmp(map->slots[idx].key, key)) { + map->slots[idx].value = value; + return 0; + } + idx = (idx + 1) & (map->capacity - 1); + } + + /* Keep the load factor below 0.5 to bound the probe length. */ + if ((map->count + 1) * 2 > map->capacity) { + if (hashmap_grow(map) < 0) + return -1; + } + + copy = strdup(key); + if (!copy) + return -1; + + hashmap_place(map->slots, map->capacity, copy, value); + map->count++; + + return 0; +} + +void *hashmap_get(const struct hashmap *map, const char *key) +{ + size_t idx = str_hash(key) & (map->capacity - 1); + + while (map->slots[idx].key) { + if (!strcmp(map->slots[idx].key, key)) + return map->slots[idx].value; + idx = (idx + 1) & (map->capacity - 1); + } + + return NULL; +} + +size_t hashmap_count(const struct hashmap *map) +{ + return map->count; +}