--- /dev/null
+/* SPDX-License-Identifier: Apache-2.0
+ * Copyright (c) 2024 Cisco Systems, Inc.
+ */
+
+#include <vnet/vnet.h>
+#include <vnet/dev/dev.h>
+#include <vnet/dev/bus/platform.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <dirent.h>
+
+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,
+ },
+};
--- /dev/null
+/* SPDX-License-Identifier: Apache-2.0
+ * Copyright (c) 2024 Cisco Systems, Inc.
+ */
+
+#include <vppinfra/clib.h>
+#include <vppinfra/devicetree.h>
+
+#ifdef __linux
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <dirent.h>
+
+#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;
+}
--- /dev/null
+/* SPDX-License-Identifier: Apache-2.0
+ * Copyright (c) 2024 Cisco Systems, Inc.
+ */
+
+#ifndef CLIB_DEVICETREE_H_
+#define CLIB_DEVICETREE_H_
+
+#include <vppinfra/clib.h>
+#include <vlib/vlib.h>
+
+#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_ */
Code Review / vpp.git / commitdiff