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>

1 files changed, 71 insertions, 54 deletions

diff --git a/arch/mips/kernel/cpu-probe.c b/arch/mips/kernel/cpu-probe.c
index 2911ad5977d7..73ab840f13bd 100644
--- a/arch/mips/kernel/cpu-probe.c
+++ b/arch/mips/kernel/cpu-probe.c
@@ -33,6 +33,41 @@
 #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.
  */
 static inline void cpu_set_fpu_fcsr_mask(struct cpuinfo_mips *c)
@@ -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;