[MIPS] Oprofile: Support VSMP on 34K.

Signed-off-by: Ralf Baechle <ralf@linux-mips.org>

arch/mips/oprofile/op_model_mipsxx.c

@@ -12,16 +12,70 @@
 
 #include "op_impl.h"
 
-#define M_PERFCTL_EXL			(1UL    <<  0)
-#define M_PERFCTL_KERNEL		(1UL    <<  1)
-#define M_PERFCTL_SUPERVISOR		(1UL    <<  2)
-#define M_PERFCTL_USER			(1UL    <<  3)
-#define M_PERFCTL_INTERRUPT_ENABLE	(1UL    <<  4)
-#define M_PERFCTL_EVENT(event)		((event) << 5)
-#define M_PERFCTL_WIDE			(1UL    << 30)
-#define M_PERFCTL_MORE			(1UL    << 31)
+#define M_PERFCTL_EXL			(1UL      <<  0)
+#define M_PERFCTL_KERNEL		(1UL      <<  1)
+#define M_PERFCTL_SUPERVISOR		(1UL      <<  2)
+#define M_PERFCTL_USER			(1UL      <<  3)
+#define M_PERFCTL_INTERRUPT_ENABLE	(1UL      <<  4)
+#define M_PERFCTL_EVENT(event)		((event)  << 5)
+#define M_PERFCTL_VPEID(vpe)		((vpe)    << 16)
+#define M_PERFCTL_MT_EN(filter)		((filter) << 20)
+#define    M_TC_EN_ALL			M_PERFCTL_MT_EN(0)
+#define    M_TC_EN_VPE			M_PERFCTL_MT_EN(1)
+#define    M_TC_EN_TC			M_PERFCTL_MT_EN(2)
+#define M_PERFCTL_TCID(tcid)		((tcid)   << 22)
+#define M_PERFCTL_WIDE			(1UL      << 30)
+#define M_PERFCTL_MORE			(1UL      << 31)
+
+#define M_COUNTER_OVERFLOW		(1UL      << 31)
+
+#ifdef CONFIG_MIPS_MT_SMP
+#define WHAT	(M_TC_EN_VPE | M_PERFCTL_VPEID(smp_processor_id()))
+#else
+#define WHAT	0
+#endif
 
-#define M_COUNTER_OVERFLOW		(1UL    << 31)
+#define __define_perf_accessors(r, n, np)				\
+									\
+static inline unsigned int r_c0_ ## r ## n(void)			\
+{									\
+	unsigned int cpu = smp_processor_id();				\
+									\
+	switch (cpu) {							\
+	case 0:								\
+		return read_c0_ ## r ## n();				\
+	case 1:								\
+		return read_c0_ ## r ## np();				\
+	default:							\
+		BUG();							\
+	}								\
+}									\
+									\
+static inline void w_c0_ ## r ## n(unsigned int value)			\
+{									\
+	unsigned int cpu = smp_processor_id();				\
+									\
+	switch (cpu) {							\
+	case 0:								\
+		write_c0_ ## r ## n(value);				\
+		return;							\
+	case 1:								\
+		write_c0_ ## r ## np(value);				\
+		return;							\
+	default:							\
+		BUG();							\
+	}								\
+}									\
+
+__define_perf_accessors(perfcntr, 0, 2)
+__define_perf_accessors(perfcntr, 1, 3)
+__define_perf_accessors(perfcntr, 2, 2)
+__define_perf_accessors(perfcntr, 3, 2)
+
+__define_perf_accessors(perfctrl, 0, 2)
+__define_perf_accessors(perfctrl, 1, 3)
+__define_perf_accessors(perfctrl, 2, 2)
+__define_perf_accessors(perfctrl, 3, 2)
 
 struct op_mips_model op_model_mipsxx_ops;
 
@@ -66,17 +120,17 @@ static void mipsxx_cpu_setup (void *args)
 
 	switch (counters) {
 	case 4:
-		write_c0_perfctrl3(0);
-		write_c0_perfcntr3(reg.counter[3]);
+		w_c0_perfctrl3(0);
+		w_c0_perfcntr3(reg.counter[3]);
 	case 3:
-		write_c0_perfctrl2(0);
-		write_c0_perfcntr2(reg.counter[2]);
+		w_c0_perfctrl2(0);
+		w_c0_perfcntr2(reg.counter[2]);
 	case 2:
-		write_c0_perfctrl1(0);
-		write_c0_perfcntr1(reg.counter[1]);
+		w_c0_perfctrl1(0);
+		w_c0_perfcntr1(reg.counter[1]);
 	case 1:
-		write_c0_perfctrl0(0);
-		write_c0_perfcntr0(reg.counter[0]);
+		w_c0_perfctrl0(0);
+		w_c0_perfcntr0(reg.counter[0]);
 	}
 }
 
@@ -87,13 +141,13 @@ static void mipsxx_cpu_start(void *args)
 
 	switch (counters) {
 	case 4:
-		write_c0_perfctrl3(reg.control[3]);
+		w_c0_perfctrl3(WHAT | reg.control[3]);
 	case 3:
-		write_c0_perfctrl2(reg.control[2]);
+		w_c0_perfctrl2(WHAT | reg.control[2]);
 	case 2:
-		write_c0_perfctrl1(reg.control[1]);
+		w_c0_perfctrl1(WHAT | reg.control[1]);
 	case 1:
-		write_c0_perfctrl0(reg.control[0]);
+		w_c0_perfctrl0(WHAT | reg.control[0]);
 	}
 }
 
@@ -104,13 +158,13 @@ static void mipsxx_cpu_stop(void *args)
 
 	switch (counters) {
 	case 4:
-		write_c0_perfctrl3(0);
+		w_c0_perfctrl3(0);
 	case 3:
-		write_c0_perfctrl2(0);
+		w_c0_perfctrl2(0);
 	case 2:
-		write_c0_perfctrl1(0);
+		w_c0_perfctrl1(0);
 	case 1:
-		write_c0_perfctrl0(0);
+		w_c0_perfctrl0(0);
 	}
 }
 
@@ -124,12 +178,12 @@ static int mipsxx_perfcount_handler(struct pt_regs *regs)
 	switch (counters) {
 #define HANDLE_COUNTER(n)						\
 	case n + 1:							\
-		control = read_c0_perfctrl ## n();			\
-		counter = read_c0_perfcntr ## n();			\
+		control = r_c0_perfctrl ## n();				\
+		counter = r_c0_perfcntr ## n();				\
 		if ((control & M_PERFCTL_INTERRUPT_ENABLE) &&		\
 		    (counter & M_COUNTER_OVERFLOW)) {			\
 			oprofile_add_sample(regs, n);			\
-			write_c0_perfcntr ## n(reg.counter[n]);		\
+			w_c0_perfcntr ## n(reg.counter[n]);		\
 			handled = 1;					\
 		}
 	HANDLE_COUNTER(3)
@@ -143,35 +197,47 @@ static int mipsxx_perfcount_handler(struct pt_regs *regs)
 
 #define M_CONFIG1_PC	(1 << 4)
 
-static inline int n_counters(void)
+static inline int __n_counters(void)
 {
 	if (!(read_c0_config1() & M_CONFIG1_PC))
 		return 0;
-	if (!(read_c0_perfctrl0() & M_PERFCTL_MORE))
+	if (!(r_c0_perfctrl0() & M_PERFCTL_MORE))
 		return 1;
-	if (!(read_c0_perfctrl1() & M_PERFCTL_MORE))
+	if (!(r_c0_perfctrl1() & M_PERFCTL_MORE))
 		return 2;
-	if (!(read_c0_perfctrl2() & M_PERFCTL_MORE))
+	if (!(r_c0_perfctrl2() & M_PERFCTL_MORE))
 		return 3;
 
 	return 4;
 }
 
+static inline int n_counters(void)
+{
+	int counters = __n_counters();
+
+#ifndef CONFIG_SMP
+	if (current_cpu_data.cputype == CPU_34K)
+		return counters >> 1;
+#endif
+
+	return counters;
+}
+
 static inline void reset_counters(int counters)
 {
 	switch (counters) {
 	case 4:
-		write_c0_perfctrl3(0);
-		write_c0_perfcntr3(0);
+		w_c0_perfctrl3(0);
+		w_c0_perfcntr3(0);
 	case 3:
-		write_c0_perfctrl2(0);
-		write_c0_perfcntr2(0);
+		w_c0_perfctrl2(0);
+		w_c0_perfcntr2(0);
 	case 2:
-		write_c0_perfctrl1(0);
-		write_c0_perfcntr1(0);
+		w_c0_perfctrl1(0);
+		w_c0_perfcntr1(0);
 	case 1:
-		write_c0_perfctrl0(0);
-		write_c0_perfcntr0(0);
+		w_c0_perfctrl0(0);
+		w_c0_perfcntr0(0);
 	}
 }
 
@@ -201,7 +267,6 @@ static int __init mipsxx_init(void)
 		op_model_mipsxx_ops.cpu_type = "mips/25K";
 		break;
 
-#ifndef CONFIG_SMP
 	case CPU_34K:
 		op_model_mipsxx_ops.cpu_type = "mips/34K";
 		break;
@@ -209,7 +274,6 @@ static int __init mipsxx_init(void)
 	case CPU_74K:
 		op_model_mipsxx_ops.cpu_type = "mips/74K";
 		break;
-#endif
 
 	case CPU_5KC:
 		op_model_mipsxx_ops.cpu_type = "mips/5K";