dev: add platform bus and devicetree support

Change-Id: Ief8e159b25d4fc4859c7116da6ff22c15bd3fff0
Type: feature
Signed-off-by: Damjan Marion <damarion@cisco.com>

6 files changed, 623 insertions, 1 deletions

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 <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,
+  },
+};
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 <vppinfra/clib.h>
+#include <vppinfra/devicetree.h>
+#include <vlib/vlib.h>
+#include <vnet/dev/dev.h>
+
+#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 <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;
+}
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 <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_ */