ARM: ipipe: add user-visible TSC helpers

arch/arm/Kconfig

@@ -912,6 +912,15 @@ config PLAT_PXA
 config PLAT_VERSATILE
 	bool
 
+if IPIPE
+config IPIPE_ARM_KUSER_TSC
+       bool
+       select HAVE_IPIPE_TRACER_SUPPORT
+       select GENERIC_TIME_VSYSCALL
+       select IPIPE_HAVE_HOSTRT if IPIPE
+       default y if ARM_TIMER_SP804 || ARCH_MXC || ARCH_OMAP
+endif
+
 source "arch/arm/firmware/Kconfig"
 
 source arch/arm/mm/Kconfig

arch/arm/kernel/Makefile

@@ -89,6 +89,7 @@ head-y			:= head$(MMUEXT).o
 obj-$(CONFIG_DEBUG_LL)	+= debug.o
 obj-$(CONFIG_EARLY_PRINTK)	+= early_printk.o
 obj-$(CONFIG_IPIPE)	+= ipipe.o
+obj-$(CONFIG_IPIPE_ARM_KUSER_TSC) += ipipe_tsc.o ipipe_tsc_asm.o
 
 # This is executed very early using a temporary stack when no memory allocator
 # nor global data is available. Everything has to be allocated on the stack.

arch/arm/kernel/entry-armv.S

@@ -907,6 +907,50 @@ ENDPROC(__switch_to)
 #endif
 	.endm
 
+#ifdef CONFIG_IPIPE
+/*
+	I-pipe tsc area, here we store data shared with user-space for
+	tsc-emulation. If CONFIG_IPIPE_ARM_KUSER_TSC is enabled
+	__ipipe_kuser_get_tsc will be overwritten with the real TSC
+	emulation code.
+*/
+	.globl	__ipipe_tsc_area
+	.equ	__ipipe_tsc_area, VECTORS_BASE + 0x1000 + __ipipe_tsc_area_start - __kuser_helper_end
+
+#ifdef CONFIG_IPIPE_ARM_KUSER_TSC
+	.globl  __ipipe_tsc_addr
+	.equ	__ipipe_tsc_addr, VECTORS_BASE + 0x1000 + .LCcntr_addr - __kuser_helper_end
+
+	.globl	__ipipe_tsc_get
+	.equ	__ipipe_tsc_get, VECTORS_BASE + 0x1000 + __ipipe_kuser_get_tsc - __kuser_helper_end
+#endif
+
+	.align 5
+	.globl  __ipipe_tsc_area_start
+__ipipe_tsc_area_start:
+	.rep  3
+	.word 0
+	.endr
+
+#ifdef CONFIG_IPIPE_ARM_KUSER_TSC
+	.rep  4
+	.word 0
+	.endr
+.LCcntr_addr:
+	.word 0
+
+	.align 5
+__ipipe_kuser_get_tsc:
+	nop
+	mov	r0, #0
+	mov	r1, #0
+	usr_ret	lr
+	.rep 20
+	.word 0
+	.endr
+#endif
+#endif
+
 	.macro	kuser_pad, sym, size
 	.if	(. - \sym) & 3
 	.rept	4 - (. - \sym) & 3

arch/arm/kernel/ipipe_tsc.c

+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/timer.h>
+#include <linux/clocksource.h>
+#include <linux/ipipe_tickdev.h>
+#include <linux/cpufreq.h>
+#include <linux/ipipe.h>
+
+#include <asm/cacheflush.h>
+#include <asm/traps.h>
+
+typedef unsigned long long __ipipe_tsc_t(void);
+
+extern __ipipe_tsc_t __ipipe_freerunning_64,
+	__ipipe_freerunning_32,
+	__ipipe_freerunning_countdown_32,
+	__ipipe_freerunning_16,
+	__ipipe_freerunning_countdown_16,
+	__ipipe_decrementer_16,
+	__ipipe_freerunning_twice_16,
+	__ipipe_freerunning_arch;
+extern unsigned long __ipipe_tsc_addr;
+
+static struct __ipipe_tscinfo tsc_info;
+
+static struct clocksource clksrc = {
+	.name = "ipipe_tsc",
+	.rating = 0x7fffffff,
+	.read = (typeof(clksrc.read))__ipipe_tsc_get,
+	.mask = CLOCKSOURCE_MASK(64),
+	.flags = CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+struct ipipe_tsc_value_t {
+	unsigned long long last_tsc;
+	unsigned last_cnt;
+};
+
+unsigned long __ipipe_kuser_tsc_freq;
+
+struct ipipe_tsc_value_t *ipipe_tsc_value;
+
+static struct ipipe_tsc_update_timer {
+	struct timer_list timer;
+	long period;
+} ipipe_tsc_update_timer;
+
+static void __ipipe_tsc_update_fn(struct timer_list *t)
+{
+	struct ipipe_tsc_update_timer *upt;
+
+	upt = container_of(t, struct ipipe_tsc_update_timer, timer);
+	__ipipe_tsc_update();
+	mod_timer(&ipipe_tsc_update_timer.timer, jiffies + upt->period);
+}
+
+void __init __ipipe_tsc_register(struct __ipipe_tscinfo *info)
+{
+	struct ipipe_tsc_value_t *vector_tsc_value;
+	unsigned long long wrap_ms;
+	unsigned long *tsc_addr;
+	__ipipe_tsc_t *implem;
+	unsigned long flags;
+	int registered;
+	char *tsc_area;
+
+#if !defined(CONFIG_CPU_USE_DOMAINS)
+	extern char __ipipe_tsc_area_start[], __kuser_helper_end[];
+
+	tsc_area = (char *)vectors_page + 0x1000
+		+ (__ipipe_tsc_area_start - __kuser_helper_end);
+	tsc_addr = (unsigned long *)
+		(tsc_area + ((char *)&__ipipe_tsc_addr - __ipipe_tsc_area));
+#else
+	tsc_area = __ipipe_tsc_area;
+	tsc_addr = &__ipipe_tsc_addr;
+#endif
+	registered = ipipe_tsc_value != NULL;
+
+	if (WARN_ON(info->freq == 0))
+		return;
+
+	if (registered && info->freq < tsc_info.freq)
+		return;
+
+	ipipe_tsc_value = (struct ipipe_tsc_value_t *)tsc_area;
+	vector_tsc_value = (struct ipipe_tsc_value_t *)__ipipe_tsc_area;
+
+	switch(info->type) {
+	case IPIPE_TSC_TYPE_FREERUNNING:
+		switch(info->u.mask) {
+		case 0xffff:
+			implem = &__ipipe_freerunning_16;
+			break;
+		case 0xffffffff:
+			implem = &__ipipe_freerunning_32;
+			break;
+		case 0xffffffffffffffffULL:
+			implem = &__ipipe_freerunning_64;
+			break;
+		default:
+			goto unimplemented;
+		}
+		break;
+
+	case IPIPE_TSC_TYPE_DECREMENTER:
+		if (info->u.mask != 0xffff)
+			goto unimplemented;
+		implem = &__ipipe_decrementer_16;
+		break;
+
+	case IPIPE_TSC_TYPE_FREERUNNING_COUNTDOWN:
+		switch(info->u.mask) {
+		case 0xffff:
+			implem = &__ipipe_freerunning_countdown_16;
+			break;
+		case 0xffffffff:
+			implem = &__ipipe_freerunning_countdown_32;
+			break;
+		default:
+			goto unimplemented;
+		}
+		break;
+
+	case IPIPE_TSC_TYPE_FREERUNNING_TWICE:
+		if (info->u.mask != 0xffff)
+			goto unimplemented;
+		implem = &__ipipe_freerunning_twice_16;
+		break;
+
+	case IPIPE_TSC_TYPE_FREERUNNING_ARCH:
+		implem = &__ipipe_freerunning_arch;
+		break;
+
+	default:
+	unimplemented:
+		printk("I-pipe: Unimplemented tsc configuration, "
+		       "type: %d, mask: 0x%08Lx\n", info->type, info->u.mask);
+		BUG();
+	}
+
+	tsc_info = *info;
+	*tsc_addr = tsc_info.counter_vaddr;
+	if (tsc_info.type == IPIPE_TSC_TYPE_DECREMENTER) {
+		tsc_info.u.dec.last_cnt = &vector_tsc_value->last_cnt;
+		tsc_info.u.dec.tsc = &vector_tsc_value->last_tsc;
+	} else
+		tsc_info.u.fr.tsc = &vector_tsc_value->last_tsc;
+
+	flags = hard_local_irq_save();
+	ipipe_tsc_value->last_tsc = 0;
+	memcpy(tsc_area + 0x20, implem, 0x60);
+	flush_icache_range((unsigned long)(tsc_area),
+			   (unsigned long)(tsc_area + 0x80));
+	hard_local_irq_restore(flags);
+
+	__ipipe_kuser_tsc_freq = tsc_info.freq;
+
+	wrap_ms = info->u.mask;
+	do_div(wrap_ms, tsc_info.freq / 1000);
+
+	printk(KERN_INFO "I-pipe, %u.%03u MHz clocksource, wrap in %Lu ms\n",
+		tsc_info.freq / 1000000, (tsc_info.freq % 1000000) / 1000,
+		wrap_ms);
+
+	if (!registered) {
+		timer_setup(&ipipe_tsc_update_timer.timer, __ipipe_tsc_update_fn, 0);
+		clocksource_register_hz(&clksrc, tsc_info.freq);
+	} else
+		__clocksource_update_freq_hz(&clksrc, tsc_info.freq);
+
+	wrap_ms *= HZ / 2;
+	do_div(wrap_ms, 1000);
+	if (wrap_ms > 0x7fffffff)
+		wrap_ms = 0x7fffffff;
+	ipipe_tsc_update_timer.period = wrap_ms;
+	mod_timer(&ipipe_tsc_update_timer.timer, jiffies + wrap_ms);
+
+	__ipipe_tracer_hrclock_initialized();
+}
+
+void __ipipe_mach_get_tscinfo(struct __ipipe_tscinfo *info)
+{
+	*info = tsc_info;
+}
+
+void __ipipe_tsc_update(void)
+{
+	if (tsc_info.type == IPIPE_TSC_TYPE_DECREMENTER) {
+		unsigned cnt = *(unsigned *)tsc_info.counter_vaddr;
+		int offset = ipipe_tsc_value->last_cnt - cnt;
+		if (offset < 0)
+			offset += tsc_info.u.dec.mask + 1;
+		ipipe_tsc_value->last_tsc += offset;
+		ipipe_tsc_value->last_cnt = cnt;
+		return;
+	}
+
+	/* Update last_tsc, in order to remain compatible with legacy
+	   user-space 32 bits free-running counter implementation */
+	ipipe_tsc_value->last_tsc = __ipipe_tsc_get() - 1;
+}
+EXPORT_SYMBOL(__ipipe_tsc_get);
+
+void __ipipe_update_vsyscall(struct timekeeper *tk)
+{
+	if (tk->tkr_mono.clock == &clksrc)
+		ipipe_update_hostrt(tk);
+}
+
+#if !IS_ENABLED(CONFIG_VDSO)
+void update_vsyscall(struct timekeeper *tk)
+{
+	__ipipe_update_vsyscall(tk);
+}
+
+void update_vsyscall_tz(void)
+{
+}
+#endif
+
+#ifdef CONFIG_CPU_FREQ
+
+static __init void update_timer_freq(void *data)
+{
+	unsigned int hrclock_freq = *(unsigned int *)data;
+
+	__ipipe_timer_refresh_freq(hrclock_freq);
+}
+
+static __init int cpufreq_transition_handler(struct notifier_block *nb,
+				      unsigned long state, void *data)
+{
+	struct cpufreq_freqs *freqs = data;
+	unsigned int freq;
+
+	if (state == CPUFREQ_POSTCHANGE &&
+	    ipipe_tsc_value && tsc_info.refresh_freq) {
+		freq = tsc_info.refresh_freq();
+		if (freq) {
+			if (freqs->cpu == 0) {
+				int oldrate;
+				tsc_info.freq = freq;
+				__ipipe_tsc_register(&tsc_info);
+				__ipipe_report_clockfreq_update(freq);
+				/* force timekeeper to recalculate the clocksource */
+				oldrate = clksrc.rating;
+				clocksource_change_rating(&clksrc, 0);
+				clocksource_change_rating(&clksrc, oldrate);
+			}
+			smp_call_function_single(freqs->cpu, update_timer_freq,
+						 &freq, 1);
+		}
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block __initdata cpufreq_nb = {
+	.notifier_call = cpufreq_transition_handler,
+};
+
+static __init int register_cpufreq_notifier(void)
+{
+	cpufreq_register_notifier(&cpufreq_nb,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+	return 0;
+}
+core_initcall(register_cpufreq_notifier);
+
+static __init int unregister_cpufreq_notifier(void)
+{
+	cpufreq_unregister_notifier(&cpufreq_nb,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+	return 0;
+}
+late_initcall(unregister_cpufreq_notifier);
+
+#endif /* CONFIG_CPUFREQ */

arch/arm/kernel/ipipe_tsc_asm.S

+#include <asm/assembler.h>
+#include <asm/asm-offsets.h>
+#include <asm/glue.h>
+
+	.macro	usr_ret, reg
+#ifdef CONFIG_ARM_THUMB
+	bx	\reg
+#else
+	mov	pc, \reg
+#endif
+	.endm
+
+	.macro	usr_reteq, reg
+#ifdef CONFIG_ARM_THUMB
+	bxeq	\reg
+#else
+	moveq	 pc, \reg
+#endif
+	.endm
+
+	.macro	myldrd, rd1, rd2, rtmp, label
+#if __LINUX_ARM_ARCH__ < 5
+	adr	\rtmp, \label
+	ldm	\rtmp, { \rd1, \rd2 }
+#else
+	ldrd	\rd1, \label
+#endif
+	.endm
+
+/*
+	We use the same mechanism as Linux user helpers to store
+	variables and functions related to TSC emulation, so that they
+	can also be used in user-space.
+
+	The function ipipe_tsc_register will copy the proper
+	implemntation to the vectors page. We repeat the data area so
+	that the PC relative operations are computed correctly.
+*/
+
+	.section	.init.text, "ax", %progbits
+ THUMB(	.arm	)
+
+	.align 5
+	.rep	7
+	.word	0
+	.endr
+.LCfr64_cntr_addr:
+	.word 	0
+
+	.align 5
+	.globl 	__ipipe_freerunning_64
+__ipipe_freerunning_64:
+	ldr	r0, .LCfr64_cntr_addr
+/* User-space entry-point: r0 is the hardware counter virtual address */
+	mov 	r2, r0
+#ifndef CONFIG_CPU_BIG_ENDIAN
+/* Little endian */
+	ldr 	r1, [r2, #4]
+1:	ldr	r0, [r2]
+	ldr	r3, [r2, #4]
+	cmp	r3, r1
+	usr_reteq lr
+	mov	r1, r3
+	b	1b
+#else /* Big endian */
+	ldr 	r0, [r2]
+1:	ldr	r1, [r2, #4]
+	ldr	r3, [r2]
+	cmp	r3, r0
+	usr_reteq lr
+	mov	r0, r3
+	b	1b
+#endif /* Big endian */
+
+	.align 5
+.LCfr32_last_tsc:
+	.rep	7
+	.word	0
+	.endr
+.LCfr32_cntr_addr:
+	.word 	0
+
+	.align 5
+	.globl 	__ipipe_freerunning_32
+__ipipe_freerunning_32:
+	ldr	r0, .LCfr32_cntr_addr
+/* User-space entry-point: r0 is the hardware counter virtual address */
+	myldrd	r2, r3, r1, .LCfr32_last_tsc
+#ifndef CONFIG_CPU_BIG_ENDIAN
+/* Little endian */
+	ldr	r0, [r0]
+	cmp	r2, r0
+	adc	r1, r3, #0
+#else /* Big endian */
+	ldr	r1, [r0]
+	cmp	r3, r1
+	adc	r0, r2, #0
+#endif /* Big endian */
+	usr_ret lr
+
+	.align 5
+.LCfrcd32_last_tsc:
+	.rep	7
+	.word	0
+	.endr
+.LCfrcd32_cntr_addr:
+	.word 	0
+
+	.align 5
+	.globl __ipipe_freerunning_countdown_32
+__ipipe_freerunning_countdown_32:
+	ldr	r0, .LCfrcd32_cntr_addr
+/* User-space entry-point: r0 is the hardware counter virtual address */
+	myldrd	r2, r3, r1, .LCfrcd32_last_tsc
+#ifndef CONFIG_CPU_BIG_ENDIAN
+/* Little endian */
+	ldr	r0, [r0]
+	mvn	r0, r0
+	cmp	r2, r0
+	adc	r1, r3, #0
+#else /* Big endian */
+	ldr	r1, [r0]
+	mvn	r1, r1
+	cmp	r3, r1
+	adc	r0, r2, #0
+#endif /* Big endian */
+	usr_ret lr
+
+	.align 5
+.LCfr16_last_tsc:
+	.rep	7
+	.word	0
+	.endr
+.LCfr16_cntr_addr:
+	.word 	0
+
+	.align 5
+	.globl __ipipe_freerunning_16
+__ipipe_freerunning_16:
+	ldr	r0, .LCfr16_cntr_addr
+/* User-space entry-point: r0 is the hardware counter virtual address */
+1:	myldrd	r2, r3, r1, .LCfr16_last_tsc
+	ldrh	ip, [r0]
+#ifndef CONFIG_CPU_BIG_ENDIAN
+/* Little endian */
+	ldr	r1, .LCfr16_last_tsc
+	cmp	r1, r2
+	mov	r1, r2, lsr #16
+	bne	1b
+	orr	r0, ip, r1, lsl #16
+	cmp	r2, r0
+	addhis	r0, r0, #0x10000
+	adc	r1, r3, #0
+#else /* Big endian */
+	ldr	r1, .LCfr16_last_tsc + 4
+	cmp	r1, r3
+	mov	r1, r3, lsr #16
+	bne	1b
+	orr	r1, ip, r1, lsl #16
+	cmp	r3, r1
+	addhis	r1, r1, #0x10000
+	adc	r0, r2, #0
+#endif /* Big endian */
+	usr_ret lr
+
+	.align 5
+.LCfrcd16_last_tsc:
+	.rep	7
+	.word	0
+	.endr
+.LCfrcd16_cntr_addr:
+	.word 	0
+
+	.align 5
+	.globl __ipipe_freerunning_countdown_16
+__ipipe_freerunning_countdown_16:
+	ldr	r0, .LCfrcd16_cntr_addr
+/* User-space entry-point: r0 is the hardware counter virtual address */
+1:	myldrd	r2, r3, r1, .LCfrcd16_last_tsc
+	ldrh	ip, [r0]
+#ifndef CONFIG_CPU_BIG_ENDIAN
+/* Little endian */
+	ldr	r1, .LCfrcd16_last_tsc
+	rsb	ip, ip, #0x10000
+	cmp	r1, r2
+	mov	r1, r2, lsr #16
+	bne	1b
+	orr	r0, ip, r1, lsl #16
+	cmp	r2, r0
+	addhis	r0, r0, #0x10000
+	adc	r1, r3, #0
+#else /* Big endian */
+	ldr	r1, .LCfrcd16_last_tsc + 4
+	rsb	ip, ip, #0x10000
+	cmp	r1, r3
+	mov	r1, r3, lsr #16
+	bne	1b
+	orr	r1, ip, r1, lsl #16
+	cmp	r3, r1
+	addhis	r1, r1, #0x10000
+	adc	r0, r2, #0
+#endif /* Big endian */
+	usr_ret lr
+
+	.align 5
+.LCfrt16_last_tsc:
+	.rep	7
+	.word	0
+	.endr
+.LCfrt16_cntr_addr:
+	.word 	0
+
+	.align 5
+	.globl __ipipe_freerunning_twice_16
+__ipipe_freerunning_twice_16:
+	ldr	r0, .LCfrt16_cntr_addr
+/* User-space entry-point: r0 is the hardware counter virtual address */
+1:	myldrd	r2, r3, r1, .LCfrt16_last_tsc
+2:	ldrh	ip, [r0]
+	ldrh	r1, [r0]
+	cmp	r1, ip
+	bne	2b
+#ifndef CONFIG_CPU_BIG_ENDIAN
+/* Little endian */
+	ldr	r1, .LCfrt16_last_tsc
+	cmp	r1, r2
+	mov	r1, r2, lsr #16
+	bne	1b
+	orr	r0, ip, r1, lsl #16
+	cmp	r2, r0
+	addhis	r0, r0, #0x10000
+	adc	r1, r3, #0
+#else /* Big endian */
+	ldr	r1, .LCfrt16_last_tsc + 4
+	cmp	r1, r3
+	mov	r1, r3, lsr #16
+	bne	1b
+	orr	r1, ip, r1, lsl #16
+	cmp	r3, r1
+	addhis	r1, r1, #0x10000
+	adc	r0, r2, #0
+#endif /* Big endian */
+	usr_ret lr
+
+	.align 5
+.LCdec16_last_tsc:
+	.rep	2
+	.word	0
+	.endr
+.LCdec16_last_cnt:
+	.rep	5
+	.word	0
+	.endr
+.LCdec16_cntr_addr:
+	.word 	0
+
+	.align 5
+	.globl __ipipe_decrementer_16
+__ipipe_decrementer_16:
+	ldr	r0, .LCdec16_cntr_addr
+/* User-space entry-point: r0 is the hardware counter virtual address */
+#ifndef CONFIG_CPU_BIG_ENDIAN
+/* Little endian */
+1:	ldr	r1, .LCdec16_last_tsc