From 6f770fb4b176ef2c58b61b7a369a8c129cd3fb0f Mon Sep 17 00:00:00 2001
From: liaoao <liaoao@xiaomi.com>
Date: Wed, 10 Jan 2024 18:12:10 +0800
Subject: [PATCH] coresight: disable device before unregister

coresight_claim_device will fail when unregister device
which has been enabled. device should be the init state when
it is unregistered.

Signed-off-by: liaoao <liaoao@xiaomi.com>
---
 drivers/coresight/coresight_core.c         | 30 ++++++++++++++++++++++
 drivers/coresight/coresight_funnel.c       | 20 +++++++++++++++
 drivers/coresight/coresight_replicator.c   | 19 ++++++++++++++
 drivers/coresight/coresight_tmc_etf.c      | 23 +++++++++++++++++
 include/nuttx/coresight/coresight_funnel.h |  1 +
 5 files changed, 93 insertions(+)

diff --git a/drivers/coresight/coresight_core.c b/drivers/coresight/coresight_core.c
index 5cd889937d..56e056af0e 100644
--- a/drivers/coresight/coresight_core.c
+++ b/drivers/coresight/coresight_core.c
@@ -791,6 +791,36 @@ void coresight_unregister(FAR struct coresight_dev_s *csdev)
         }
     }
 
+  if (csdev->refcnt > 0)
+    {
+      switch (csdev->type)
+        {
+          case CORESIGHT_DEV_TYPE_SINK:
+            if (csdev->ops->sink_ops->disable != NULL)
+              {
+                csdev->ops->sink_ops->disable(csdev);
+              }
+            break;
+
+          case CORESIGHT_DEV_TYPE_SOURCE:
+            if (csdev->ops->source_ops->disable != NULL)
+              {
+                csdev->ops->source_ops->disable(csdev);
+              }
+            break;
+
+          /* Link devices may have multiple inport or outport, it can
+           * not be distinguished here which one of them has been enabled.
+           * so disable inport/outports in its own unregister function.
+           */
+
+          default:
+            break;
+        }
+
+      coresight_disable_clk(csdev);
+    }
+
   list_delete(&csdev->node);
   leave_critical_section(flags);
 
diff --git a/drivers/coresight/coresight_funnel.c b/drivers/coresight/coresight_funnel.c
index 4228847aeb..8fe281f19e 100644
--- a/drivers/coresight/coresight_funnel.c
+++ b/drivers/coresight/coresight_funnel.c
@@ -25,6 +25,7 @@
 #include <errno.h>
 #include <debug.h>
 #include <nuttx/kmalloc.h>
+#include <nuttx/irq.h>
 
 #include <nuttx/coresight/coresight_funnel.h>
 
@@ -223,6 +224,7 @@ funnel_register(FAR const struct coresight_desc_s *desc)
       return NULL;
     }
 
+  fundev->port_num = desc->inport_num;
   csdev = &fundev->csdev;
   csdev->ops = &g_funnel_ops;
   ret = coresight_register(csdev, desc);
@@ -249,6 +251,24 @@ funnel_register(FAR const struct coresight_desc_s *desc)
 
 void funnel_unregister(FAR struct coresight_funnel_dev_s *fundev)
 {
+  irqstate_t flags;
+
+  flags = enter_critical_section();
+  if (fundev->csdev.refcnt > 0)
+    {
+      int i;
+
+      for (i = 0; i < fundev->port_num; i++)
+        {
+          if (fundev->port_refcnt[i] > 0)
+            {
+              funnel_hw_disable(fundev, i);
+            }
+        }
+    }
+
+  leave_critical_section(flags);
   coresight_unregister(&fundev->csdev);
+
   kmm_free(fundev);
 }
diff --git a/drivers/coresight/coresight_replicator.c b/drivers/coresight/coresight_replicator.c
index aa1c354bf8..cdba6f5110 100644
--- a/drivers/coresight/coresight_replicator.c
+++ b/drivers/coresight/coresight_replicator.c
@@ -25,6 +25,7 @@
 #include <errno.h>
 #include <debug.h>
 #include <nuttx/kmalloc.h>
+#include <nuttx/irq.h>
 
 #include <nuttx/coresight/coresight_replicator.h>
 
@@ -274,6 +275,24 @@ replicator_register(FAR const struct coresight_desc_s *desc)
 
 void replicator_unregister(FAR struct coresight_replicator_dev_s *repdev)
 {
+  irqstate_t flags;
+
+  flags = enter_critical_section();
+  if (repdev->csdev.refcnt > 0)
+    {
+      int i;
+
+      for (i = 0; i < repdev->csdev.outport_num; i++)
+        {
+          if (repdev->port_refcnt[i] > 0)
+            {
+              replicator_hw_disable(repdev, i);
+            }
+        }
+    }
+
+  leave_critical_section(flags);
   coresight_unregister(&repdev->csdev);
+
   kmm_free(repdev);
 }
diff --git a/drivers/coresight/coresight_tmc_etf.c b/drivers/coresight/coresight_tmc_etf.c
index c494c2f34d..67cb659c58 100644
--- a/drivers/coresight/coresight_tmc_etf.c
+++ b/drivers/coresight/coresight_tmc_etf.c
@@ -319,6 +319,11 @@ static int tmc_etf_link_enable(FAR struct coresight_dev_s *csdev,
     (FAR struct coresight_tmc_dev_s *)csdev;
   int ret;
 
+  if (csdev->refcnt != 1)
+    {
+      return 0;
+    }
+
   ret = coresight_claim_device(tmcdev->csdev.addr);
   if (ret < 0)
     {
@@ -345,6 +350,11 @@ static void tmc_etf_link_disable(FAR struct coresight_dev_s *csdev,
   FAR struct coresight_tmc_dev_s *tmcdev =
     (FAR struct coresight_tmc_dev_s *)csdev;
 
+  if (csdev->refcnt != 1)
+    {
+      return;
+    }
+
   tmc_etf_hw_disable(tmcdev);
   coresight_disclaim_device(tmcdev->csdev.addr);
 }
@@ -545,6 +555,19 @@ void tmc_etf_unregister(FAR struct coresight_tmc_dev_s * tmcdev)
       snprintf(pathname, sizeof(pathname), "/dev/%s", tmcdev->csdev.name);
       unregister_driver(pathname);
     }
+  else if (tmcdev->csdev.refcnt > 0)
+    {
+      irqstate_t flags;
+
+      /* Link device should disable hw here, and sink device will disable
+       * hw in coresight_core.c
+       */
+
+      flags = enter_critical_section();
+      tmc_etf_hw_disable(tmcdev);
+      coresight_disclaim_device(tmcdev->csdev.addr);
+      leave_critical_section(flags);
+    }
 
   coresight_unregister(&tmcdev->csdev);
 }
diff --git a/include/nuttx/coresight/coresight_funnel.h b/include/nuttx/coresight/coresight_funnel.h
index 37d53016a6..43e02fc306 100644
--- a/include/nuttx/coresight/coresight_funnel.h
+++ b/include/nuttx/coresight/coresight_funnel.h
@@ -35,6 +35,7 @@ struct coresight_funnel_dev_s
 {
   struct coresight_dev_s csdev;
   uint32_t priority;                 /* Port selection order. */
+  uint8_t port_num;                  /* Port numbre. */
   uint8_t port_refcnt[0];            /* Port refcnt. */
 };