xtensa: Refactor up_fpucmp to only consider enabled coprocessors
#include <stdint.h>
#include <string.h>
+#include <strings.h>
+
#include <nuttx/irq.h>
#include "xtensa.h"
+#ifdef CONFIG_ARCH_FPU
+
/****************************************************************************
- * Pre-processor Definitions
+ * Private Data
****************************************************************************/
-#ifdef CONFIG_ARCH_FPU
+static uint32_t g_coproc_sa_offsets[] =
+{
+ XTENSA_CP0_SA, XTENSA_CP1_SA, XTENSA_CP2_SA, XTENSA_CP3_SA,
+ XTENSA_CP4_SA, XTENSA_CP5_SA, XTENSA_CP6_SA, XTENSA_CP7_SA
+};
+
+static uint32_t g_coproc_sa_sizes[] =
+{
+ XCHAL_CP0_SA_SIZE, XCHAL_CP1_SA_SIZE, XCHAL_CP2_SA_SIZE, XCHAL_CP3_SA_SIZE,
+ XCHAL_CP4_SA_SIZE, XCHAL_CP5_SA_SIZE, XCHAL_CP6_SA_SIZE, XCHAL_CP7_SA_SIZE
+};
/****************************************************************************
* Public Functions
*
* Description:
* Compare FPU areas from thread context.
+ * This comparison will skip disabled coprocessors.
*
* Input Parameters:
* saveregs1 - Pointer to the saved FPU registers.
@@ -59,8 +74,23 @@ bool up_fpucmp(const void *saveregs1, const void *saveregs2)
{
const uint32_t *regs1 = saveregs1;
const uint32_t *regs2 = saveregs2;
+ uint32_t cpenable = xtensa_get_cpenable();
+ int ndx = 0;
+ bool ret = true;
+ int i;
+
+ while (ret && (i = ffs(cpenable)))
+ {
+ uint32_t reg_offset;
+
+ ndx += i;
+ reg_offset = g_coproc_sa_offsets[ndx - 1] / 4;
+ ret = memcmp(&regs1[COMMON_CTX_REGS + reg_offset],
+ &regs2[COMMON_CTX_REGS + reg_offset],
+ g_coproc_sa_sizes[ndx - 1]) == 0;
+ cpenable >>= i;
+ }
- return memcmp(&regs1[COMMON_CTX_REGS], &regs2[COMMON_CTX_REGS],
- XTENSA_CP_SA_SIZE) == 0;
+ return ret;
}
#endif /* CONFIG_ARCH_FPU */
