coresight:add link coresight device support

Signed-off-by: liaoao <liaoao@xiaomi.com>

drivers/coresight/CMakeLists.txt

@@ -21,5 +21,13 @@
 if(CONFIG_CORESIGHT)
   set(SRCS coresight_core.c coresight_common.c)
 
+  if(CONFIG_CORESIGHT_FUNNEL)
+    list(APPEND SRCS coresight_funnel.c)
+  endif()
+
+  if(CONFIG_CORESIGHT_REPLICATOR)
+    list(APPEND SRCS coresight_replicator.c)
+  endif()
+
   target_sources(drivers PRIVATE ${SRCS})
 endif()

drivers/coresight/Kconfig

@@ -19,4 +19,12 @@ config CORESIGHT_TIMEOUT
 	int "Timeout us for waiting register state change"
 	default 100
 
+config CORESIGHT_FUNNEL
+	bool "Funnel coresight device support"
+	default n
+
+config CORESIGHT_REPLICATOR
+	bool "Replicator coresight device support"
+	default n
+
 endif # CORESIGHT

drivers/coresight/Make.defs

@@ -24,6 +24,14 @@ ifeq ($(CONFIG_CORESIGHT),y)
 
 CSRCS += coresight_core.c coresight_common.c
 
+ifeq ($(CONFIG_CORESIGHT_FUNNEL),y)
+CSRCS += coresight_funnel.c
+endif
+
+ifeq ($(CONFIG_CORESIGHT_REPLICATOR),y)
+CSRCS += coresight_replicator.c
+endif
+
 DEPPATH += --dep-path coresight
 VPATH += :coresight
 CFLAGS += ${INCDIR_PREFIX}$(TOPDIR)$(DELIM)drivers$(DELIM)coresight

drivers/coresight/coresight_funnel.c

@@ -0,0 +1,254 @@
+/****************************************************************************
+ * drivers/coresight/coresight_funnel.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <errno.h>
+#include <debug.h>
+#include <nuttx/kmalloc.h>
+
+#include <nuttx/coresight/coresight_funnel.h>
+
+#include "coresight_common.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Funnel registers */
+
+#define FUNNEL_FUNCTL           0x000
+#define FUNNEL_PRICTL           0x004
+
+#define FUNNEL_HOLDTIME_MASK    0xf00
+#define FUNNEL_HOLDTIME_SHFT    0x8
+#define FUNNEL_HOLDTIME         (0x7 << FUNNEL_HOLDTIME_SHFT)
+#define FUNNEL_ENS_MASK         0xff
+
+/****************************************************************************
+ * Private Functions Prototypes
+ ****************************************************************************/
+
+static int funnel_enable(FAR struct coresight_dev_s *csdev,
+                         int iport, int oport);
+static void funnel_disable(FAR struct coresight_dev_s *csdev,
+                           int iport, int oport);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static const struct coresight_link_ops_s g_funnel_link_ops =
+{
+  .enable  = funnel_enable,
+  .disable = funnel_disable,
+};
+
+static const struct coresight_ops_s g_funnel_ops =
+{
+  .link_ops = &g_funnel_link_ops,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: funnel_hw_enable
+ ****************************************************************************/
+
+static int funnel_hw_enable(FAR struct coresight_funnel_dev_s *fundev,
+                            int port)
+{
+  uint32_t functl;
+  int ret = 0;
+
+  coresight_unlock(fundev->csdev.addr);
+  functl = coresight_get32(fundev->csdev.addr + FUNNEL_FUNCTL);
+  coresight_lock(fundev->csdev.addr);
+
+  /* Only claim the device when the first slave port is enabled */
+
+  if (!(functl & FUNNEL_ENS_MASK))
+    {
+      ret = coresight_claim_device(fundev->csdev.addr);
+      if (ret < 0)
+        {
+          cserr("%s claim failed\n", fundev->csdev.name);
+          return ret;
+        }
+    }
+
+  coresight_unlock(fundev->csdev.addr);
+  functl &= ~FUNNEL_HOLDTIME_MASK;
+  functl |= FUNNEL_HOLDTIME;
+  functl |= 1 << port;
+  coresight_put32(functl, fundev->csdev.addr + FUNNEL_FUNCTL);
+  coresight_put32(fundev->priority, fundev->csdev.addr + FUNNEL_PRICTL);
+  coresight_lock(fundev->csdev.addr);
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: funnel_hw_disable
+ ****************************************************************************/
+
+static void funnel_hw_disable(FAR struct coresight_funnel_dev_s *fundev,
+                              int port)
+{
+  uint32_t functl;
+
+  coresight_unlock(fundev->csdev.addr);
+  functl = coresight_get32(fundev->csdev.addr + FUNNEL_FUNCTL);
+  functl &= ~(1 << port);
+  coresight_put32(functl, fundev->csdev.addr + FUNNEL_FUNCTL);
+  coresight_lock(fundev->csdev.addr);
+
+  if (!(functl & FUNNEL_ENS_MASK))
+    {
+      coresight_disclaim_device(fundev->csdev.addr);
+    }
+}
+
+/****************************************************************************
+ * Name: funnel_enable
+ ****************************************************************************/
+
+static int funnel_enable(FAR struct coresight_dev_s *csdev,
+                         int iport, int oport)
+{
+  FAR struct coresight_funnel_dev_s *fundev =
+    (FAR struct coresight_funnel_dev_s *)csdev;
+  int ret = 0;
+
+  if (fundev->port_refcnt[iport]++ == 0)
+    {
+      ret = funnel_hw_enable(fundev, iport);
+      if (ret < 0)
+        {
+          fundev->port_refcnt[iport]--;
+          cserr("%s inport %d enabled failed\n", csdev->name, iport);
+        }
+    }
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: funnel_disable
+ ****************************************************************************/
+
+static void funnel_disable(FAR struct coresight_dev_s *csdev,
+                           int iport, int oport)
+{
+  FAR struct coresight_funnel_dev_s *fundev =
+    (FAR struct coresight_funnel_dev_s *)csdev;
+
+  if (--fundev->port_refcnt[iport] == 0)
+    {
+      funnel_hw_disable(fundev, iport);
+      csinfo("%s inport %d disabled\n", csdev->name, iport);
+    }
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: set_funnel_priority
+ *
+ * Description:
+ *   Set funnel ports priority. It should to be called when port has not been
+ *   enabled.
+ *
+ * Input Parameters:
+ *   fundev  - Pointer to the funnel coresight device.
+ *   priority- Priority to set.
+ *
+ ****************************************************************************/
+
+void set_funnel_priority(FAR struct coresight_funnel_dev_s *fundev,
+                         uint32_t priority)
+{
+  fundev->priority = priority;
+}
+
+/****************************************************************************
+ * Name: funnel_register
+ *
+ * Description:
+ *   Register a funnel devices.
+ *
+ * Input Parameters:
+ *   desc  - A description of this coresight device.
+ *
+ * Returned Value:
+ *   Pointer to a funnel device on success; NULL on failure.
+ *
+ ****************************************************************************/
+
+FAR struct coresight_funnel_dev_s *
+funnel_register(FAR const struct coresight_desc_s *desc)
+{
+  FAR struct coresight_funnel_dev_s *fundev;
+  FAR struct coresight_dev_s *csdev;
+  int ret;
+
+  fundev = kmm_zalloc(sizeof(struct coresight_funnel_dev_s) +
+                      sizeof(uint8_t) * desc->inport_num);
+  if (fundev == NULL)
+    {
+      cserr("%s:malloc failed!\n", desc->name);
+      return NULL;
+    }
+
+  csdev = &fundev->csdev;
+  csdev->ops = &g_funnel_ops;
+  ret = coresight_register(csdev, desc);
+  if (ret < 0)
+    {
+      kmm_free(fundev);
+      cserr("%s: register failed\n", desc->name);
+      return NULL;
+    }
+
+  return fundev;
+}
+
+/****************************************************************************
+ * Name: funnel_unregister
+ *
+ * Description:
+ *   Unregister a funnel devices.
+ *
+ * Input Parameters:
+ *   fundev  - Pointer to the funnel device.
+ *
+ ****************************************************************************/
+
+void funnel_unregister(FAR struct coresight_funnel_dev_s *fundev)
+{
+  coresight_unregister(&fundev->csdev);
+  kmm_free(fundev);
+}

drivers/coresight/coresight_replicator.c

@@ -0,0 +1,279 @@
+/****************************************************************************
+ * drivers/coresight/coresight_replicator.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <errno.h>
+#include <debug.h>
+#include <nuttx/kmalloc.h>
+
+#include <nuttx/coresight/coresight_replicator.h>
+
+#include "coresight_common.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Replicator registers */
+
+#define REPLICATOR_IDFILTER0      0x000
+#define REPLICATOR_IDFILTER1      0x004
+
+/****************************************************************************
+ * Private Functions Prototypes
+ ****************************************************************************/
+
+static int replicator_enable(FAR struct coresight_dev_s *csdev,
+                             int iport, int oport);
+static void replicator_disable(FAR struct coresight_dev_s *csdev,
+                               int iport, int oport);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static const struct coresight_link_ops_s g_replicator_link_ops =
+{
+  .enable  = replicator_enable,
+  .disable = replicator_disable,
+};
+
+static const struct coresight_ops_s g_replicator_ops =
+{
+  .link_ops = &g_replicator_link_ops,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: replicator_hw_enable
+ ****************************************************************************/
+
+static int
+replicator_hw_enable(FAR struct coresight_replicator_dev_s *repdev,
+                     int port)
+{
+  uint32_t id0val;
+  uint32_t id1val;
+  int ret;
+
+  if (port != 0 && port != 1)
+    {
+      return -EINVAL;
+    }
+
+  coresight_unlock(repdev->csdev.addr);
+  id0val = coresight_get32(repdev->csdev.addr + REPLICATOR_IDFILTER0);
+  id1val = coresight_get32(repdev->csdev.addr + REPLICATOR_IDFILTER1);
+
+  /* Only claim the device when the first slave port is enabled */
+
+  if (id0val == 0xff && id1val == 0xff)
+    {
+      ret = coresight_claim_device(repdev->csdev.addr);
+      if (ret < 0)
+        {
+          cserr("%s claim failed\n", repdev->csdev.name);
+          coresight_lock(repdev->csdev.addr);
+          return ret;
+        }
+    }
+
+  switch (port)
+    {
+      case 0:
+        coresight_put32(0x00, repdev->csdev.addr + REPLICATOR_IDFILTER0);
+        break;
+
+      case 1:
+        coresight_put32(0x00, repdev->csdev.addr + REPLICATOR_IDFILTER1);
+        break;
+
+      default:
+        break;
+    }
+
+  coresight_lock(repdev->csdev.addr);
+  return ret;
+}
+
+/****************************************************************************
+ * Name: replicator_hw_disable
+ ****************************************************************************/
+
+static void
+replicator_hw_disable(FAR struct coresight_replicator_dev_s *repdev,
+                      int port)
+{
+  uint32_t id0val;
+  uint32_t id1val;
+  uint32_t off;
+
+  switch (port)
+    {
+      case 0:
+        off = REPLICATOR_IDFILTER0;
+        break;
+
+      case 1:
+        off = REPLICATOR_IDFILTER1;
+        break;
+
+      default:
+        return;
+    }
+
+  coresight_unlock(repdev->csdev.addr);
+  coresight_put32(0xff, repdev->csdev.addr + off);
+  id0val = coresight_get32(repdev->csdev.addr + REPLICATOR_IDFILTER0);
+  id1val = coresight_get32(repdev->csdev.addr + REPLICATOR_IDFILTER1);
+  coresight_lock(repdev->csdev.addr);
+
+  if (id0val == 0xff && id1val == 0xff)
+    {
+      coresight_disclaim_device(repdev->csdev.addr);
+    }
+}
+
+/****************************************************************************
+ * Name: replicator_enable
+ ****************************************************************************/
+
+static int replicator_enable(FAR struct coresight_dev_s *csdev,
+                             int iport, int oport)
+{
+  FAR struct coresight_replicator_dev_s *repdev =
+    (FAR struct coresight_replicator_dev_s *)csdev;
+  int ret = 0;
+
+  if (repdev->port_refcnt[oport]++ == 0)
+    {
+      ret = replicator_hw_enable(repdev, oport);
+      if (ret < 0)
+        {
+          repdev->port_refcnt[oport]--;
+          cserr("%s inport %d enabled failed\n", csdev->name, oport);
+        }
+    }
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: replicator_disable
+ ****************************************************************************/
+
+static void replicator_disable(FAR struct coresight_dev_s *csdev,
+                               int iport, int oport)
+{
+  FAR struct coresight_replicator_dev_s *repdev =
+    (FAR struct coresight_replicator_dev_s *)csdev;
+
+  if (--repdev->port_refcnt[oport] == 0)
+    {
+      replicator_hw_disable(repdev, oport);
+      csinfo("%s inport %d disabled\n", csdev->name, oport);
+    }
+}
+
+/****************************************************************************
+ * Name: replicator_reset
+ ****************************************************************************/
+
+static void replicator_reset(FAR struct coresight_replicator_dev_s *repdev)
+{
+  if (coresight_claim_device(repdev->csdev.addr) == 0)
+    {
+      coresight_unlock(repdev->csdev.addr);
+      coresight_put32(0xff, repdev->csdev.addr + REPLICATOR_IDFILTER0);
+      coresight_put32(0xff, repdev->csdev.addr + REPLICATOR_IDFILTER1);
+      coresight_lock(repdev->csdev.addr);
+      coresight_disclaim_device(repdev->csdev.addr);
+    }
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: replicator_register
+ *
+ * Description:
+ *   Register a replicator devices.
+ *
+ * Input Parameters:
+ *   desc  - A description of this coresight device.
+ *
+ * Returned Value:
+ *   Pointer to a replicator device on success; NULL on failure.
+ *
+ ****************************************************************************/
+
+FAR struct coresight_replicator_dev_s *
+replicator_register(FAR const struct coresight_desc_s *desc)
+{
+  FAR struct coresight_replicator_dev_s *repdev;
+  FAR struct coresight_dev_s *csdev;
+  int ret;
+
+  repdev = kmm_zalloc(sizeof(struct coresight_replicator_dev_s) +
+                      sizeof(uint8_t) * desc->outport_num);
+  if (repdev == NULL)
+    {
+      cserr("%s:malloc failed!\n", desc->name);
+      return NULL;
+    }
+
+  csdev = &repdev->csdev;
+  csdev->ops = &g_replicator_ops;
+  ret = coresight_register(csdev, desc);
+  if (ret < 0)
+    {
+      kmm_free(repdev);
+      cserr("%s: register failed\n", desc->name);
+      return NULL;
+    }
+
+  replicator_reset(repdev);
+  return repdev;
+}
+
+/****************************************************************************
+ * Name: replicator_unregister
+ *
+ * Description:
+ *   Unregister a replicator devices.
+ *
+ * Input Parameters:
+ *   fundev  - Pointer to the replicator device.
+ *
+ ****************************************************************************/
+
+void replicator_unregister(FAR struct coresight_replicator_dev_s *repdev)
+{
+  coresight_unregister(&repdev->csdev);
+  kmm_free(repdev);
+}

include/nuttx/coresight/coresight.h

@@ -122,6 +122,10 @@ struct coresight_desc_s
   enum coresight_dev_type_e type;
   union coresight_dev_subtype_u subtype;
 
+  /* Used in funnel devices. */
+
+  int inport_num;
+
   /* Description of outports of current device. */
 
   int outport_num;

include/nuttx/coresight/coresight_funnel.h

@@ -0,0 +1,91 @@
+/****************************************************************************
+ * include/nuttx/coresight/coresight_funnel.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __INCLUDE_NUTTX_CORESIGHT_CORESIGHT_FUNNEL_H
+#define __INCLUDE_NUTTX_CORESIGHT_CORESIGHT_FUNNEL_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/coresight/coresight.h>
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+struct coresight_funnel_dev_s
+{
+  struct coresight_dev_s csdev;
+  uint32_t priority;                 /* Port selection order. */
+  uint8_t port_refcnt[0];            /* Port refcnt. */
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: set_funnel_priority
+ *
+ * Description:
+ *   Set funnel ports priority. It should to be called when port has not been
+ *   enabled.
+ *
+ * Input Parameters:
+ *   fundev  - Pointer to the funnel coresight device.
+ *   priority- Priority to set.
+ *
+ ****************************************************************************/
+
+void set_funnel_priority(FAR struct coresight_funnel_dev_s *fundev,
+                         uint32_t priority);
+
+/****************************************************************************
+ * Name: funnel_register
+ *
+ * Description:
+ *   Register an funnel devices.
+ *
+ * Input Parameters:
+ *   desc  - A description of this coresight device.
+ *
+ * Returned Value:
+ *   Pointer to a funnel device on success; NULL on failure.
+ *
+ ****************************************************************************/
+
+FAR struct coresight_funnel_dev_s *
+funnel_register(FAR const struct coresight_desc_s *desc);
+
+/****************************************************************************
+ * Name: funnel_unregister
+ *
+ * Description:
+ *   Unregister a funnel devices.
+ *
+ * Input Parameters:
+ *   fundev  - Pointer to the funnel device.
+ *
+ ****************************************************************************/
+
+void funnel_unregister(FAR struct coresight_funnel_dev_s *fundev);
+
+#endif  //__INCLUDE_NUTTX_CORESIGHT_CORESIGHT_FUNNEL_H

include/nuttx/coresight/coresight_replicator.h

@@ -0,0 +1,74 @@
+/****************************************************************************
+ * include/nuttx/coresight/coresight_replicator.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __INCLUDE_NUTTX_CORESIGHT_CORESIGHT_REPLICATOR_H
+#define __INCLUDE_NUTTX_CORESIGHT_CORESIGHT_REPLICATOR_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/coresight/coresight.h>
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+struct coresight_replicator_dev_s
+{
+  struct coresight_dev_s csdev;
+  uint8_t port_refcnt[0];            /* Port refcnt. */
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: replicator_register
+ *
+ * Description:
+ *   Register an replicator devices.
+ *
+ * Input Parameters:
+ *   desc  - A description of this coresight device.
+ *
+ * Returned Value:
+ *   Pointer to a replicator device on success; NULL on failure.
+ *
+ ****************************************************************************/
+
+FAR struct coresight_replicator_dev_s *
+replicator_register(FAR const struct coresight_desc_s *desc);
+
+/****************************************************************************
+ * Name: replicator_unregister
+ *
+ * Description:
+ *   Unregister a replicator devices.
+ *
+ * Input Parameters:
+ *   fundev  - Pointer to the replicator device.
+ *
+ ****************************************************************************/
+
+void replicator_unregister(FAR struct coresight_replicator_dev_s *repdev);
+
+#endif  //__INCLUDE_NUTTX_CORESIGHT_CORESIGHT_REPLICATOR_H