MIPS: Factor out FPU feature probing

Factor out FPU feature probing, mainly to remove code duplication from
`fpu_disable'.  No functional change although shuffle some code to avoid
forward references.

Signed-off-by: Maciej W. Rozycki <macro@linux-mips.org>
Cc: linux-mips@linux-mips.org
Patchwork: https://patchwork.linux-mips.org/patch/9712/
Signed-off-by: Ralf Baechle <ralf@linux-mips.org>

diff --git a/arch/mips/kernel/cpu-probe.c b/arch/mips/kernel/cpu-probe.c
index 2911ad5977d79183859e88fbcb26acec7b89e775..73ab840f13bd05a7579b51eb15dc3a927e81a448 100644 (file)
--- a/arch/mips/kernel/cpu-probe.c
+++ b/arch/mips/kernel/cpu-probe.c
@@ -32,6 +32,41 @@
 #include <asm/spram.h>
 #include <asm/uaccess.h>
 
+/*
+ * Get the FPU Implementation/Revision.
+ */
+static inline unsigned long cpu_get_fpu_id(void)
+{
+       unsigned long tmp, fpu_id;
+
+       tmp = read_c0_status();
+       __enable_fpu(FPU_AS_IS);
+       fpu_id = read_32bit_cp1_register(CP1_REVISION);
+       write_c0_status(tmp);
+       return fpu_id;
+}
+
+/*
+ * Check if the CPU has an external FPU.
+ */
+static inline int __cpu_has_fpu(void)
+{
+       return (cpu_get_fpu_id() & FPIR_IMP_MASK) != FPIR_IMP_NONE;
+}
+
+static inline unsigned long cpu_get_msa_id(void)
+{
+       unsigned long status, msa_id;
+
+       status = read_c0_status();
+       __enable_fpu(FPU_64BIT);
+       enable_msa();
+       msa_id = read_msa_ir();
+       disable_msa();
+       write_c0_status(status);
+       return msa_id;
+}
+
 /*
  * Determine the FCSR mask for FPU hardware.
  */
@@ -82,13 +117,42 @@ static void cpu_set_nofpu_id(struct cpuinfo_mips *c)
 /* Determined FPU emulator mask to use for the boot CPU with "nofpu".  */
 static unsigned int mips_nofpu_msk31;
 
+/*
+ * Set options for FPU hardware.
+ */
+static void cpu_set_fpu_opts(struct cpuinfo_mips *c)
+{
+       c->fpu_id = cpu_get_fpu_id();
+       mips_nofpu_msk31 = c->fpu_msk31;
+
+       if (c->isa_level & (MIPS_CPU_ISA_M32R1 | MIPS_CPU_ISA_M64R1 |
+                           MIPS_CPU_ISA_M32R2 | MIPS_CPU_ISA_M64R2 |
+                           MIPS_CPU_ISA_M32R6 | MIPS_CPU_ISA_M64R6)) {
+               if (c->fpu_id & MIPS_FPIR_3D)
+                       c->ases |= MIPS_ASE_MIPS3D;
+               if (c->fpu_id & MIPS_FPIR_FREP)
+                       c->options |= MIPS_CPU_FRE;
+       }
+
+       cpu_set_fpu_fcsr_mask(c);
+}
+
+/*
+ * Set options for the FPU emulator.
+ */
+static void cpu_set_nofpu_opts(struct cpuinfo_mips *c)
+{
+       c->options &= ~MIPS_CPU_FPU;
+       c->fpu_msk31 = mips_nofpu_msk31;
+
+       cpu_set_nofpu_id(c);
+}
+
 static int mips_fpu_disabled;
 
 static int __init fpu_disable(char *s)
 {
-       boot_cpu_data.options &= ~MIPS_CPU_FPU;
-       boot_cpu_data.fpu_msk31 = mips_nofpu_msk31;
-       cpu_set_nofpu_id(&boot_cpu_data);
+       cpu_set_nofpu_opts(&boot_cpu_data);
        mips_fpu_disabled = 1;
 
        return 1;
@@ -231,41 +295,6 @@ static inline void set_elf_platform(int cpu, const char *plat)
                __elf_platform = plat;
 }
 
-/*
- * Get the FPU Implementation/Revision.
- */
-static inline unsigned long cpu_get_fpu_id(void)
-{
-       unsigned long tmp, fpu_id;
-
-       tmp = read_c0_status();
-       __enable_fpu(FPU_AS_IS);
-       fpu_id = read_32bit_cp1_register(CP1_REVISION);
-       write_c0_status(tmp);
-       return fpu_id;
-}
-
-/*
- * Check if the CPU has an external FPU.
- */
-static inline int __cpu_has_fpu(void)
-{
-       return (cpu_get_fpu_id() & FPIR_IMP_MASK) != FPIR_IMP_NONE;
-}
-
-static inline unsigned long cpu_get_msa_id(void)
-{
-       unsigned long status, msa_id;
-
-       status = read_c0_status();
-       __enable_fpu(FPU_64BIT);
-       enable_msa();
-       msa_id = read_msa_ir();
-       disable_msa();
-       write_c0_status(status);
-       return msa_id;
-}
-
 static inline void cpu_probe_vmbits(struct cpuinfo_mips *c)
 {
 #ifdef __NEED_VMBITS_PROBE
@@ -1441,22 +1470,10 @@ void cpu_probe(void)
                               ~(1 << MIPS_PWCTL_PWEN_SHIFT));
        }
 
-       if (c->options & MIPS_CPU_FPU) {
-               c->fpu_id = cpu_get_fpu_id();
-               mips_nofpu_msk31 = c->fpu_msk31;
-
-               if (c->isa_level & (MIPS_CPU_ISA_M32R1 | MIPS_CPU_ISA_M64R1 |
-                                   MIPS_CPU_ISA_M32R2 | MIPS_CPU_ISA_M64R2 |
-                                   MIPS_CPU_ISA_M32R6 | MIPS_CPU_ISA_M64R6)) {
-                       if (c->fpu_id & MIPS_FPIR_3D)
-                               c->ases |= MIPS_ASE_MIPS3D;
-                       if (c->fpu_id & MIPS_FPIR_FREP)
-                               c->options |= MIPS_CPU_FRE;
-               }
-
-               cpu_set_fpu_fcsr_mask(c);
-       } else
-               cpu_set_nofpu_id(c);
+       if (c->options & MIPS_CPU_FPU)
+               cpu_set_fpu_opts(c);
+       else
+               cpu_set_nofpu_opts(c);
 
        if (cpu_has_mips_r2_r6) {
                c->srsets = ((read_c0_srsctl() >> 26) & 0x0f) + 1;