From f02c5a5d7e7714a18ceaada26058fd1c0c127ad8 Mon Sep 17 00:00:00 2001
From: Jiuzhu Dong <dongjiuzhu1@xiaomi.com>
Date: Sat, 21 May 2022 18:07:00 +0800
Subject: [PATCH] system/uorb: support new api

orb_advertise_multi_queue_persist

Signed-off-by: Jiuzhu Dong <dongjiuzhu1@xiaomi.com>
---
 system/uorb/uORB/uORB.c | 61 +++++++++++++++++++++++++++--------------
 system/uorb/uORB/uORB.h | 29 +++++++++++++++++++-
 2 files changed, 68 insertions(+), 22 deletions(-)

diff --git a/system/uorb/uORB/uORB.c b/system/uorb/uORB/uORB.c
index 2d02378a4..953e3b26e 100644
--- a/system/uorb/uORB/uORB.c
+++ b/system/uorb/uORB/uORB.c
@@ -73,6 +73,7 @@ static int orb_advsub_open(FAR const struct orb_metadata *meta, int flags,
       reginfo.path    = path;
       reginfo.esize   = meta->o_size;
       reginfo.nbuffer = queue_size;
+      reginfo.persist = !!(flags & SENSOR_PERSIST);
 
       fd = open(ORB_USENSOR_PATH, O_WRONLY);
       if (fd < 0)
@@ -111,26 +112,10 @@ static int orb_advsub_open(FAR const struct orb_metadata *meta, int flags,
   return fd;
 }
 
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-int orb_open(FAR const char *name, int instance, int flags)
-{
-  char path[ORB_PATH_MAX];
-
-  snprintf(path, ORB_PATH_MAX, ORB_SENSOR_PATH"%s%d", name, instance);
-  return open(path, O_CLOEXEC | flags);
-}
-
-int orb_close(int fd)
-{
-  return close(fd);
-}
-
-int orb_advertise_multi_queue(FAR const struct orb_metadata *meta,
-                              FAR const void *data, FAR int *instance,
-                              unsigned int queue_size)
+static int
+orb_advertise_multi_queue_flags(FAR const struct orb_metadata *meta,
+                                FAR const void *data, FAR int *instance,
+                                unsigned int queue_size, int flags)
 {
   int inst;
   int fd;
@@ -139,7 +124,7 @@ int orb_advertise_multi_queue(FAR const struct orb_metadata *meta,
 
   inst = instance ? *instance : orb_group_count(meta);
 
-  fd = orb_advsub_open(meta, O_WRONLY, inst, queue_size);
+  fd = orb_advsub_open(meta, flags, inst, queue_size);
   if (fd < 0)
     {
       uorberr("%s advertise failed (%i)", meta->o_name, fd);
@@ -165,6 +150,40 @@ int orb_advertise_multi_queue(FAR const struct orb_metadata *meta,
   return fd;
 }
 
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+int orb_open(FAR const char *name, int instance, int flags)
+{
+  char path[ORB_PATH_MAX];
+
+  snprintf(path, ORB_PATH_MAX, ORB_SENSOR_PATH"%s%d", name, instance);
+  return open(path, O_CLOEXEC | flags);
+}
+
+int orb_close(int fd)
+{
+  return close(fd);
+}
+
+int orb_advertise_multi_queue(FAR const struct orb_metadata *meta,
+                              FAR const void *data, FAR int *instance,
+                              unsigned int queue_size)
+{
+  return orb_advertise_multi_queue_flags(meta, data, instance,
+                                         queue_size, O_WRONLY);
+}
+
+int orb_advertise_multi_queue_persist(FAR const struct orb_metadata *meta,
+                                      FAR const void *data,
+                                      FAR int *instance,
+                                      unsigned int queue_size)
+{
+  return orb_advertise_multi_queue_flags(meta, data, instance, queue_size,
+                                         O_WRONLY | SENSOR_PERSIST);
+}
+
 ssize_t orb_publish_multi(int fd, const void *data, size_t len)
 {
   return write(fd, data, len);
diff --git a/system/uorb/uORB/uORB.h b/system/uorb/uORB/uORB.h
index 4cfe6b29d..f5713521d 100644
--- a/system/uorb/uORB/uORB.h
+++ b/system/uorb/uORB/uORB.h
@@ -256,6 +256,33 @@ static inline int orb_advertise_multi(FAR const struct orb_metadata *meta,
   return orb_advertise_multi_queue(meta, data, instance, 1);
 }
 
+/****************************************************************************
+ * Name: orb_advertise_multi_queue_persist
+ *
+ * Description:
+ *   orb_advertise_multi_queue_persist is similar to orb_advertise_mult and
+ *   it can ensures that every subscriber has access to current and
+ *   future data.
+ *
+ * Input Parameters:
+ *   meta         The uORB metadata (usually from the ORB_ID() macro)
+ *   data         A pointer to the initial data to be published.
+ *   instance     Pointer to an integer which yield the instance ID,
+ *                (has default 0 if pointer is NULL).
+ *
+ * Returned Value:
+ *   -1 on error, otherwise returns an file descriptor
+ *   that can be used to publish to the topic.
+ *   If the topic in question is not known (due to an
+ *   ORB_DEFINE with no corresponding ORB_DECLARE)
+ *   this function will return -1 and set errno to ENOENT.
+ ****************************************************************************/
+
+int orb_advertise_multi_queue_persist(FAR const struct orb_metadata *meta,
+                                      FAR const void *data,
+                                      FAR int *instance,
+                                      unsigned int queue_size);
+
 /****************************************************************************
  * Name: orb_unadvertise
  *
@@ -316,7 +343,7 @@ static inline int orb_publish_auto(FAR const struct orb_metadata *meta,
     {
       int tmp;
 
-      tmp = orb_advertise_multi(meta, data, instance);
+      tmp = orb_advertise_multi_queue_persist(meta, data, instance, 1);
       if (tmp < 0)
         {
           return tmp;