From 0bdfdda193d97c2dcf8459d0b106bc52b235e250 Mon Sep 17 00:00:00 2001
From: Sascha Wildner <saw@online.de>
Date: Fri, 3 Jun 2011 14:42:25 +0200
Subject: [PATCH] kernel: Add rdmsr_safe() in preparation for the coretemp(4)
 update.

---
 sys/cpu/i386/include/cpufunc.h     |  1 +
 sys/cpu/x86_64/include/cpufunc.h   |  1 +
 sys/platform/pc32/i386/support.s   | 33 ++++++++++++++++++++++++++++++
 sys/platform/pc64/x86_64/support.s | 30 +++++++++++++++++++++++++++
 4 files changed, 65 insertions(+)

diff --git a/sys/cpu/i386/include/cpufunc.h b/sys/cpu/i386/include/cpufunc.h
index ed4ad357be..aa20505413 100644
--- a/sys/cpu/i386/include/cpufunc.h
+++ b/sys/cpu/i386/include/cpufunc.h
@@ -750,6 +750,7 @@ void	ltr		(u_short sel);
 u_int	rcr0		(void);
 u_int	rcr3		(void);
 u_int	rcr4		(void);
+int	rdmsr_safe	(u_int msr, uint64_t *val);
 void    reset_dbregs    (void);
 __END_DECLS
 
diff --git a/sys/cpu/x86_64/include/cpufunc.h b/sys/cpu/x86_64/include/cpufunc.h
index 004fb72f91..94f8d34d95 100644
--- a/sys/cpu/x86_64/include/cpufunc.h
+++ b/sys/cpu/x86_64/include/cpufunc.h
@@ -919,6 +919,7 @@ void	intr_restore(register_t rf);
 
 #endif	/* __GNUC__ */
 
+int	rdmsr_safe(u_int msr, uint64_t *val);
 void	reset_dbregs(void);
 
 __END_DECLS
diff --git a/sys/platform/pc32/i386/support.s b/sys/platform/pc32/i386/support.s
index 581dcf7a5d..e7ce4db0e2 100644
--- a/sys/platform/pc32/i386/support.s
+++ b/sys/platform/pc32/i386/support.s
@@ -732,6 +732,39 @@ ENTRY(longjmp)
 	incl	%eax
 	ret
 
+/*
+ * Support for reading MSRs in the safe manner.
+ */
+ENTRY(rdmsr_safe)
+/* int rdmsr_safe(u_int msr, uint64_t *data) */
+	movl	PCPU(curthread),%ecx
+	movl	TD_PCB(%ecx), %ecx
+	movl	$msr_onfault,PCB_ONFAULT(%ecx)
+
+	movl	4(%esp),%ecx
+	rdmsr
+	movl	8(%esp),%ecx
+	movl	%eax,(%ecx)
+	movl	%edx,4(%ecx)
+	xorl	%eax,%eax
+
+	movl	PCPU(curthread),%ecx
+	movl	TD_PCB(%ecx), %ecx
+	movl	%eax,PCB_ONFAULT(%ecx)
+
+	ret
+
+/*
+ * MSR operations fault handler
+ */
+	ALIGN_TEXT
+msr_onfault:
+	movl	PCPU(curthread),%ecx
+	movl	TD_PCB(%ecx), %ecx
+	movl	$0,PCB_ONFAULT(%ecx)
+	movl	$EFAULT,%eax
+	ret
+
 /*
  * Support for BB-profiling (gcc -a).  The kernbb program will extract
  * the data from the kernel.
diff --git a/sys/platform/pc64/x86_64/support.s b/sys/platform/pc64/x86_64/support.s
index b74c56b260..3c1b2e726b 100644
--- a/sys/platform/pc64/x86_64/support.s
+++ b/sys/platform/pc64/x86_64/support.s
@@ -699,6 +699,36 @@ ENTRY(longjmp)
 	incl	%eax
 	ret
 
+/*
+ * Support for reading MSRs in the safe manner.
+ */
+ENTRY(rdmsr_safe)
+/* int rdmsr_safe(u_int msr, uint64_t *data) */
+	movq	PCPU(curthread),%r8
+	movq	TD_PCB(%r8), %r8
+	movq	$msr_onfault,PCB_ONFAULT(%r8)
+	movl	%edi,%ecx
+	rdmsr			/* Read MSR pointed by %ecx. Returns
+				   hi byte in edx, lo in %eax */
+	salq	$32,%rdx	/* sign-shift %rdx left */
+	movl	%eax,%eax	/* zero-extend %eax -> %rax */
+	orq	%rdx,%rax
+	movq	%rax,(%rsi)
+	xorq	%rax,%rax
+	movq	%rax,PCB_ONFAULT(%r8)
+	ret
+
+/*
+ * MSR operations fault handler
+ */
+	ALIGN_TEXT
+msr_onfault:
+	movq	PCPU(curthread),%r8
+	movq	TD_PCB(%r8), %r8
+	movq	$0,PCB_ONFAULT(%r8)
+	movl	$EFAULT,%eax
+	ret
+
 /*
  * Support for BB-profiling (gcc -a).  The kernbb program will extract
  * the data from the kernel.
-- 
2.41.0