From f508e07c14873c5829838a4e59f9425c27fa833f Mon Sep 17 00:00:00 2001 From: Damjan Marion Date: Tue, 27 Aug 2024 18:21:02 +0200 Subject: dev: add platform bus and devicetree support Change-Id: Ief8e159b25d4fc4859c7116da6ff22c15bd3fff0 Type: feature Signed-off-by: Damjan Marion --- src/vnet/CMakeLists.txt | 4 +- src/vnet/dev/bus/platform.c | 172 ++++++++++++++++++++++ src/vnet/dev/bus/platform.h | 27 ++++ src/vppinfra/CMakeLists.txt | 2 + src/vppinfra/devicetree.c | 347 ++++++++++++++++++++++++++++++++++++++++++++ src/vppinfra/devicetree.h | 72 +++++++++ 6 files changed, 623 insertions(+), 1 deletion(-) create mode 100644 src/vnet/dev/bus/platform.c create mode 100644 src/vnet/dev/bus/platform.h create mode 100644 src/vppinfra/devicetree.c create mode 100644 src/vppinfra/devicetree.h diff --git a/src/vnet/CMakeLists.txt b/src/vnet/CMakeLists.txt index b7a1bbe58c9..1af2d40eff1 100644 --- a/src/vnet/CMakeLists.txt +++ b/src/vnet/CMakeLists.txt @@ -78,8 +78,8 @@ list(APPEND VNET_HEADERS dev/args.h dev/bus/pci.h dev/counters.h - dev/dev.h dev/dev_funcs.h + dev/dev.h dev/errors.h dev/log.h dev/mgmt.h @@ -960,12 +960,14 @@ list(APPEND VNET_API_FILES if("${CMAKE_SYSTEM_NAME}" STREQUAL "Linux") list(APPEND VNET_SOURCES + dev/bus/platform.c devices/tap/cli.c devices/tap/tap.c devices/tap/tapv2_api.c ) list(APPEND VNET_HEADERS + dev/bus/platform.h devices/tap/tap.h ) diff --git a/src/vnet/dev/bus/platform.c b/src/vnet/dev/bus/platform.c new file mode 100644 index 00000000000..56a1f9c762c --- /dev/null +++ b/src/vnet/dev/bus/platform.c @@ -0,0 +1,172 @@ +/* SPDX-License-Identifier: Apache-2.0 + * Copyright (c) 2024 Cisco Systems, Inc. + */ + +#include +#include +#include + +#include +#include +#include +#include + +VLIB_REGISTER_LOG_CLASS (dev_log, static) = { + .class_name = "dev", + .subclass_name = "platform", +}; + +#define log_debug(dev, f, ...) \ + vlib_log (VLIB_LOG_LEVEL_DEBUG, dev_log.class, "%U" f, format_vnet_dev_log, \ + dev, \ + clib_string_skip_prefix (__func__, "vnet_dev_bus_platform_dt_"), \ + ##__VA_ARGS__) +#define log_err(dev, f, ...) \ + vlib_log (VLIB_LOG_LEVEL_ERR, dev_log.class, "%U" f, format_vnet_dev_log, \ + dev, 0, ##__VA_ARGS__) + +#define PLATFORM_DEV_PATH "/sys/bus/platform/devices" + +clib_dt_main_t vnet_dev_bus_platform_dt_main; + +vnet_dev_rv_t +vnet_dev_bus_platform_dt_node_from_device_id (clib_dt_node_t **nodep, + char *device_id) +{ + clib_dt_main_t *dm = &vnet_dev_bus_platform_dt_main; + clib_dt_node_t *n; + char *name = device_id + sizeof (PLATFORM_BUS_NAME); + char path[PATH_MAX]; + int r; + u8 *link; + + if (dm->root == 0) + { + clib_error_t *err; + err = clib_dt_read_from_sysfs (&vnet_dev_bus_platform_dt_main); + if (err) + { + log_err (0, "cannot read devicetree: %U", format_clib_error, err); + clib_error_free (err); + return VNET_DEV_ERR_NOT_FOUND; + } + } + + link = format (0, PLATFORM_DEV_PATH "/%s/of_node%c", name, 0); + r = readlink ((char *) link, path, sizeof (path) - 1); + + if (r < 1) + { + log_err (0, "of_node doesn't exist for '%s'", name); + vec_free (link); + return VNET_DEV_ERR_NOT_FOUND; + } + + path[r] = 0; + vec_reset_length (link); + link = format (link, PLATFORM_DEV_PATH "/%s/%s%c", name, path, 0); + if (!realpath ((char *) link, path)) + { + log_err (0, "cannot find realpath for '%s'", link); + vec_free (link); + return VNET_DEV_ERR_NOT_FOUND; + } + + vec_free (link); + + if (strncmp (CLIB_DT_LINUX_PREFIX, path, + sizeof (CLIB_DT_LINUX_PREFIX) - 1) != 0) + return VNET_DEV_ERR_BUG; + + n = clib_dt_get_node_with_path (dm, "%s", + path + sizeof (CLIB_DT_LINUX_PREFIX) - 1); + + if (n) + { + *nodep = n; + return VNET_DEV_OK; + } + + return VNET_DEV_ERR_NOT_FOUND; +} + +static void * +vnet_dev_bus_platform_get_device_info (vlib_main_t *vm, char *device_id) +{ + clib_dt_node_t *n = 0; + vnet_dev_bus_platform_device_info_t *di; + + vnet_dev_bus_platform_dt_node_from_device_id (&n, device_id); + + if (n) + { + clib_dt_property_t *compatible; + compatible = clib_dt_get_node_property_by_name (n, "compatible"); + log_debug (0, "node found, is compatible %U", + format_clib_dt_property_data, compatible); + di = clib_mem_alloc (sizeof (*di)); + di->node = n; + return di; + } + + return 0; +} + +static void +vnet_dev_bus_platform_free_device_info (vlib_main_t *vm, void *p) +{ + clib_mem_free (p); +} + +static void +vnet_dev_bus_platform_close (vlib_main_t *vm, vnet_dev_t *dev) +{ + log_debug (dev, ""); +} + +static vnet_dev_rv_t +vnet_dev_bus_platform_open (vlib_main_t *vm, vnet_dev_t *dev) +{ + clib_dt_node_t *n = 0; + vnet_dev_bus_platform_device_data_t *dd = vnet_dev_get_bus_data (dev); + vnet_dev_rv_t rv; + + log_debug (dev, ""); + + rv = vnet_dev_bus_platform_dt_node_from_device_id (&n, dev->device_id); + if (rv != VNET_DEV_OK) + return rv; + + dd->node = n; + return VNET_DEV_OK; +} + +static u8 * +format_dev_bus_platform_device_info (u8 *s, va_list *args) +{ + vnet_dev_format_args_t __clib_unused *a = + va_arg (*args, vnet_dev_format_args_t *); + vnet_dev_t *dev = va_arg (*args, vnet_dev_t *); + vnet_dev_bus_platform_device_data_t *dd = vnet_dev_get_bus_data (dev); + return format (s, "device-tree path is '%v'", dd->node->path); +} + +static u8 * +format_dev_bus_platform_device_addr (u8 *s, va_list *args) +{ + vnet_dev_t *dev = va_arg (*args, vnet_dev_t *); + return format (s, "%s", dev->device_id + sizeof (PLATFORM_BUS_NAME)); +} + +VNET_DEV_REGISTER_BUS (pp2) = { + .name = PLATFORM_BUS_NAME, + .device_data_size = sizeof (vnet_dev_bus_platform_device_info_t), + .ops = { + .get_device_info = vnet_dev_bus_platform_get_device_info, + .free_device_info = vnet_dev_bus_platform_free_device_info, + .device_open = vnet_dev_bus_platform_open, + .device_close = vnet_dev_bus_platform_close, + .format_device_info = format_dev_bus_platform_device_info, + .format_device_addr = format_dev_bus_platform_device_addr, + }, +}; diff --git a/src/vnet/dev/bus/platform.h b/src/vnet/dev/bus/platform.h new file mode 100644 index 00000000000..3492aad57ed --- /dev/null +++ b/src/vnet/dev/bus/platform.h @@ -0,0 +1,27 @@ +/* SPDX-License-Identifier: Apache-2.0 + * Copyright (c) 2023 Cisco Systems, Inc. + */ + +#ifndef _VNET_DEV_BUS_PLATFORM_H_ +#define _VNET_DEV_BUS_PLATFORM_H_ + +#include +#include +#include +#include + +#define PLATFORM_BUS_NAME "platform" + +extern clib_dt_main_t vnet_dev_bus_platform_dt_main; + +typedef struct +{ + clib_dt_node_t *node; +} vnet_dev_bus_platform_device_info_t; + +typedef struct +{ + clib_dt_node_t *node; +} vnet_dev_bus_platform_device_data_t; + +#endif /* _VNET_DEV_BUS_PLATFORM_H_ */ diff --git a/src/vppinfra/CMakeLists.txt b/src/vppinfra/CMakeLists.txt index 0b8b6ae1226..83a8b2a7e57 100644 --- a/src/vppinfra/CMakeLists.txt +++ b/src/vppinfra/CMakeLists.txt @@ -69,6 +69,7 @@ set(VPPINFRA_SRCS bitmap.c bihash_all_vector.c cpu.c + devicetree.c dlmalloc.c elf.c elog.c @@ -151,6 +152,7 @@ set(VPPINFRA_HEADERS crypto/aes_ctr.h crypto/aes_gcm.h crypto/poly1305.h + devicetree.h dlist.h dlmalloc.h elf_clib.h diff --git a/src/vppinfra/devicetree.c b/src/vppinfra/devicetree.c new file mode 100644 index 00000000000..9bf8eeeac6c --- /dev/null +++ b/src/vppinfra/devicetree.c @@ -0,0 +1,347 @@ +/* SPDX-License-Identifier: Apache-2.0 + * Copyright (c) 2024 Cisco Systems, Inc. + */ + +#include +#include + +#ifdef __linux +#include +#include +#include +#include + +#endif + +static_always_inline clib_dt_node_t * +clib_dt_node_add_child (clib_dt_main_t *dm, clib_dt_node_t *n, char *name) +{ + clib_dt_node_t *cn; + + cn = clib_mem_alloc (sizeof (clib_dt_node_t)); + *cn = (clib_dt_node_t){ .parent = n, .depth = n ? n->depth + 1 : 0 }; + vec_add1 (dm->nodes, cn); + + if (n == 0) + { + ASSERT (dm->root == 0); + dm->root = cn; + return cn; + } + + vec_add1 (n->child_nodes, cn); + cn->path = format (0, "%v/%s", n->path, name); + cn->dt_main = dm; + hash_set_mem (dm->node_by_path, cn->path, cn); + if (vec_len (n->child_nodes) > 1) + { + clib_dt_node_t *prev = n->child_nodes[vec_len (n->child_nodes) - 2]; + prev->next = cn; + cn->prev = prev; + } + + return cn; +} + +void +clib_dt_main_free (clib_dt_main_t *dm) +{ + vec_foreach_pointer (n, dm->nodes) + { + vec_foreach_pointer (p, n->properties) + clib_mem_free (p); + vec_free (n->child_nodes); + vec_free (n->path); + vec_free (n->properties); + } + + vec_free (dm->nodes); + hash_free (dm->node_by_path); + hash_free (dm->node_by_phandle); +} + +#ifdef __linux +__clib_export clib_error_t * +clib_dt_read_from_sysfs (clib_dt_main_t *dm) +{ + DIR *dir, **dir_stack = 0; + struct dirent *e; + clib_dt_node_t *n; + u8 *path = 0; + u32 path_prefix_len; + clib_error_t *err = 0; + + path = format (0, CLIB_DT_LINUX_PREFIX); + path_prefix_len = vec_len (path); + vec_add1 (path, 0); + + dir = opendir ((char *) path); + if (!dir) + { + err = clib_error_return (0, "'%s' opendir failed", path); + goto done; + } + + dm->node_by_path = hash_create_vec (0, sizeof (u8), sizeof (uword)); + dm->node_by_phandle = hash_create (0, sizeof (uword)); + vec_set_len (path, path_prefix_len); + n = clib_dt_node_add_child (dm, 0, 0); + + while (1) + { + e = readdir (dir); + + if (!e) + { + closedir (dir); + if (vec_len (dir_stack) == 0) + break; + + dir = dir_stack[vec_len (dir_stack) - 1]; + vec_pop (dir_stack); + n = n->parent; + continue; + } + + if (e->d_type == DT_REG) + { + path = format (path, "%v/%s%c", n->path, e->d_name, 0); + int fd = open ((char *) path, 0); + if (fd >= 0) + { + struct stat st; + if (fstat (fd, &st) == 0) + { + u32 sz = sizeof (clib_dt_property_t) + st.st_size; + clib_dt_property_t *p = clib_mem_alloc (sz); + clib_memset (p, 0, sz); + + if (read (fd, p->data, st.st_size) == st.st_size) + { + strncpy (p->name, e->d_name, sizeof (p->name)); + p->size = st.st_size; + vec_add1 (n->properties, p); + if (strncmp ("name", p->name, 5) == 0) + n->name = p; + if ((strncmp ("phandle", p->name, 8) == 0) && + (p->size == 4)) + { + u32 phandle = + clib_net_to_host_u32 (*(u32u *) p->data); + hash_set (dm->node_by_phandle, phandle, n); + } + } + else + { + clib_mem_free (p); + err = clib_error_return (0, "'%s' read failed", path); + close (fd); + goto done; + } + } + else + { + err = clib_error_return (0, "'%s' fstat failed", path); + close (fd); + goto done; + } + close (fd); + } + else + { + err = clib_error_return (0, "'%s' open failed", path); + goto done; + } + + vec_set_len (path, path_prefix_len); + } + else if (e->d_type == DT_DIR) + { + DIR *subdir; + if (strncmp (".", e->d_name, 2) == 0 || + strncmp ("..", e->d_name, 3) == 0) + continue; + + path = format (path, "%v/%s%c", n->path, e->d_name, 0); + subdir = opendir ((char *) path); + vec_set_len (path, path_prefix_len); + if (subdir) + { + vec_add1 (dir_stack, dir); + dir = subdir; + n = clib_dt_node_add_child (dm, n, e->d_name); + } + else + { + err = clib_error_return (0, "'%s' opendir failed", path); + goto done; + } + } + else + err = + clib_error_return (0, "unknown entry %s [%u]", e->d_name, e->d_type); + } + +done: + if (err) + clib_dt_main_free (dm); + while (vec_len (dir_stack)) + closedir (vec_pop (dir_stack)); + vec_free (dir_stack); + vec_free (path); + return err; +} +#endif + +clib_dt_node_t * +clib_dt_get_child_node (clib_dt_node_t *n, char *name) +{ + vec_foreach_pointer (cn, n->child_nodes) + { + u8 *p = cn->path + vec_len (cn->path) - 1; + u32 i = 0; + + while (p > cn->path && p[-1] != '/') + p--; + + if (p[-1] != '/') + continue; + + while (p[i] == name[i] && name[i] != 0) + i++; + + if (name[i] != 0) + continue; + + return cn; + } + + return 0; +} + +__clib_export clib_dt_node_t * +clib_dt_get_node_with_path (clib_dt_main_t *dm, char *fmt, ...) +{ + u8 *s; + uword *p; + + va_list va; + va_start (va, fmt); + s = va_format (0, fmt, &va); + va_end (va); + + if (s[0] != '/') + return 0; + + p = hash_get_mem (dm->node_by_path, s); + if (p) + return (clib_dt_node_t *) p[0]; + + return 0; +} + +__clib_export clib_dt_property_t * +clib_dt_get_node_property_by_name (clib_dt_node_t *n, char *name) +{ + vec_foreach_pointer (p, n->properties) + if (strncmp (name, p->name, sizeof (p->name)) == 0) + return p; + return 0; +} + +__clib_export int +clib_dt_node_is_compatible (clib_dt_node_t *n, char *comp) +{ + clib_dt_property_t *p; + char *s; + + p = clib_dt_get_node_property_by_name (n, "compatible"); + + if (!p) + return 0; + + s = (char *) p->data; + for (u32 i = 1, len = 1; i <= p->size; i++) + { + if (p->data[i - 1] == 0) + { + if (strncmp (comp, s, len) == 0) + return 1; + s = (char *) p->data + i; + len = 1; + } + else + len++; + } + + return 0; +} + +__clib_export u8 * +format_clib_dt_property_data (u8 *s, va_list *args) +{ + clib_dt_property_t *p = va_arg (*args, clib_dt_property_t *); + u32 sz = p->size, is_printable = 0; + u32 n_nulls = 0; + + if (sz > 2 && p->data[sz - 1] == 0 && p->data[0] != 0) + { + is_printable = 1; + for (u32 i = 1; i < sz - 1; i++) + { + u8 c = p->data[i]; + if (c == 0) + { + if (p->data[i - 1] == 0) + { + is_printable = 0; + break; + } + n_nulls++; + } + else if ((c < 0x20) || (c > 0x7f)) + { + is_printable = 0; + break; + } + } + } + + if (is_printable) + { + s = format (s, "'%s'", p->data); + if (n_nulls) + { + for (u32 i = 2; i < p->size; i++) + if (((u8 *) p->data)[i - 1] == 0) + s = format (s, ", '%s'", ((u8 *) p->data) + i); + } + } + else + { + s = format (s, "< %02x", p->data[0]); + for (u32 i = 0; i < p->size; i++) + s = format (s, " %02x", p->data[i]); + s = format (s, " >"); + } + return s; +} + +__clib_export clib_dt_node_t * +clib_dt_dereference_node (clib_dt_node_t *n, char *name) +{ + clib_dt_property_t *p; + uword *h; + + p = clib_dt_get_node_property_by_name (n, name); + if (!p || (p->size != sizeof (u32))) + return 0; + + h = hash_get (n->dt_main->node_by_phandle, + clib_net_to_host_u32 (*(u32u *) p->data)); + + if (h) + return (clib_dt_node_t *) h[0]; + + return 0; +} diff --git a/src/vppinfra/devicetree.h b/src/vppinfra/devicetree.h new file mode 100644 index 00000000000..21c2e0f7006 --- /dev/null +++ b/src/vppinfra/devicetree.h @@ -0,0 +1,72 @@ +/* SPDX-License-Identifier: Apache-2.0 + * Copyright (c) 2024 Cisco Systems, Inc. + */ + +#ifndef CLIB_DEVICETREE_H_ +#define CLIB_DEVICETREE_H_ + +#include +#include + +#ifdef __linux +#define CLIB_DT_LINUX_PREFIX "/sys/firmware/devicetree/base" +#endif + +typedef struct +{ + char name[32]; + u32 size; + u8 data[]; +} clib_dt_property_t; + +typedef struct clib_dt_main clib_dt_main_t; + +typedef struct clib_dt_node +{ + u8 *path; + struct clib_dt_node *parent; + struct clib_dt_node *prev; + struct clib_dt_node *next; + struct clib_dt_node **child_nodes; + u8 depth; + clib_dt_property_t *name; + clib_dt_property_t **properties; + clib_dt_main_t *dt_main; +} clib_dt_node_t; + +typedef struct clib_dt_main +{ + clib_dt_node_t **nodes; + clib_dt_node_t *root; + uword *node_by_path; + uword *node_by_phandle; +} clib_dt_main_t; + +clib_dt_node_t *clib_dt_get_node_with_path (clib_dt_main_t *dm, char *fmt, + ...); +clib_dt_property_t *clib_dt_get_node_property_by_name (clib_dt_node_t *, + char *); +int clib_dt_node_is_compatible (clib_dt_node_t *, char *); +clib_dt_node_t *clib_dt_dereference_node (clib_dt_node_t *, char *); +#ifdef __linux +clib_error_t *clib_dt_read_from_sysfs (clib_dt_main_t *dm); +#endif + +format_function_t format_clib_dt_desc; +format_function_t format_clib_dt_property_data; + +static_always_inline int +clib_dt_proprerty_is_u32 (clib_dt_property_t *p) +{ + if (p == 0 || p->size != 4) + return 0; + return 1; +} + +static_always_inline u32 +clib_dt_proprerty_get_u32 (clib_dt_property_t *p) +{ + return clib_net_to_host_u32 (*(u32u *) p->data); +} + +#endif /* CLIB_DEVICETREE_H_ */ -- cgit 1.2.3-korg