arch: cxd56xx: Introduce cxd56_testset.c

Summary:
- I noticed that ldrex/strex on cxd56xx have an issue
- The issue is still under investigation
- This commit introduces a custom testset to avoid the issue

Impact:
- Affects cxd56xx in SMP mode if it is enabled

Testing:
- Tested with spresense:wifi_smp

Signed-off-by: Masayuki Ishikawa <Masayuki.Ishikawa@jp.sony.com>

arch/arm/src/cxd56xx/Kconfig

@@ -1284,4 +1284,11 @@ config CXD56_GEOFENCE
 
 endif
 
+config CXD56_TESTSET
+	bool "Use custom testset for spinlock"
+	default n
+	depends on SPINLOCK
+	---help---
+		Use custom testset based on hardware semaphore
+
 endmenu

arch/arm/src/cxd56xx/Make.defs

@@ -36,7 +36,11 @@
 ############################################################################
 
 CMN_ASRCS  = arm_saveusercontext.S arm_fullcontextrestore.S arm_switchcontext.S
-CMN_ASRCS += arm_testset.S vfork.S
+CMN_ASRCS += vfork.S
+
+ifneq ($(CONFIG_CXD56_TESTSET),y)
+CMN_ASRCS += arm_testset.S
+endif
 
 ifeq ($(CONFIG_ARCH_SETJMP_H),y)
 ifeq ($(CONFIG_ARCH_TOOLCHAIN_GNU),y)
@@ -110,6 +114,9 @@ CHIP_CSRCS += cxd56_cpuidlestack.c
 CHIP_CSRCS += cxd56_cpuindex.c
 CHIP_CSRCS += cxd56_cpupause.c
 CHIP_CSRCS += cxd56_cpustart.c
+ifeq ($(CONFIG_CXD56_TESTSET),y)
+CHIP_CSRCS += cxd56_testset.c
+endif
 endif
 
 ifeq ($(CONFIG_CXD56_UART0),y)

arch/arm/src/cxd56xx/cxd56_sph.c

@@ -300,9 +300,13 @@ int cxd56_sphinitialize(FAR const char *devname)
   int ret;
   int i;
 
-  /* No. 0-2 and 15 semaphores are reserved by other system. */
+  /* No. 0-2 and (14)-15 semaphores are reserved by other system. */
 
+#ifdef CONFIG_CXD56_TESTSET
+  for (i = 3; i < 14; i++)
+#else
   for (i = 3; i < 15; i++)
+#endif
     {
       ret = cxd56_sphdevinit(devname, i);
       if (ret != OK)

arch/arm/src/cxd56xx/cxd56_testset.c

@@ -0,0 +1,90 @@
+/****************************************************************************
+ * arch/arm/src/cxd56xx/cxd56_testset.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 <nuttx/config.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spinlock.h>
+
+#include "hardware/cxd56_sph.h"
+#include "cxd56_sph.h"
+
+#include "arm_arch.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define SPH_SMP 14  /* Use hardware semaphore #14 */
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: up_testset
+ *
+ * Description:
+ *   Perform and atomic test and set operation on the provided spinlock.
+ *   This function must be provided via the architecture-specific logic.
+ *
+ * Input Parameters:
+ *   lock - The address of spinlock object.
+ *
+ * Returned Value:
+ *   The spinlock is always locked upon return.  The value of previous value
+ *   of the spinlock variable is returned, either SP_LOCKED if the spinlock
+ *   as previously locked (meaning that the test-and-set operation failed to
+ *   obtain the lock) or SP_UNLOCKED if the spinlock was previously unlocked
+ *   (meaning that we successfully obtained the lock)
+ *
+ ****************************************************************************/
+
+spinlock_t up_testset(volatile FAR spinlock_t *lock)
+{
+  spinlock_t ret;
+  uint32_t sphlocked = ((up_cpu_index() + 2) << 16) | 0x1;
+
+  /* Lock hardware semaphore */
+
+  do
+    {
+      putreg32(REQ_LOCK, CXD56_SPH_REQ(SPH_SMP));
+    }
+  while (getreg32(CXD56_SPH_STS(SPH_SMP)) != sphlocked);
+
+  ret = *lock;
+
+  if (ret == SP_UNLOCKED)
+    {
+      *lock = SP_LOCKED;
+      SP_DMB();
+    }
+
+  /* Unlock hardware semaphore */
+
+  putreg32(REQ_UNLOCK, CXD56_SPH_REQ(SPH_SMP));
+
+  return ret;
+}