[IA64] support FPSWA emulation

Signed-off-by: Masaki Kanno <kanno.masaki@jp.fujitsu.com>

xen/arch/ia64/xen/process.c

@@ -33,6 +33,7 @@
 #include "hpsim_ssc.h"
 #include <xen/multicall.h>
 #include <asm/debugger.h>
+#include <asm/fpswa.h>
 
 extern void die_if_kernel(char *str, struct pt_regs *regs, long err);
 /* FIXME: where these declarations shold be there ? */
@@ -394,6 +395,99 @@ void ia64_do_page_fault (unsigned long address, unsigned long isr, struct pt_reg
 	reflect_interruption(isr, regs, fault);
 }
 
+fpswa_interface_t *fpswa_interface = 0;
+
+void trap_init (void)
+{
+	if (ia64_boot_param->fpswa)
+		/* FPSWA fixup: make the interface pointer a virtual address: */
+		fpswa_interface = __va(ia64_boot_param->fpswa);
+	else
+		printk("No FPSWA supported.\n");
+}
+
+static fpswa_ret_t
+fp_emulate (int fp_fault, void *bundle, long *ipsr, long *fpsr, long *isr, long *pr, long *ifs,
+            struct pt_regs *regs)
+{
+	fp_state_t fp_state;
+	fpswa_ret_t ret;
+
+	if (!fpswa_interface)
+		return ((fpswa_ret_t) {-1, 0, 0, 0});
+
+	memset(&fp_state, 0, sizeof(fp_state_t));
+
+	/*
+	 * compute fp_state.  only FP registers f6 - f11 are used by the
+	 * kernel, so set those bits in the mask and set the low volatile
+	 * pointer to point to these registers.
+	 */
+	fp_state.bitmask_low64 = 0xfc0;  /* bit6..bit11 */
+
+	fp_state.fp_state_low_volatile = (fp_state_low_volatile_t *) &regs->f6;
+	/*
+	 * unsigned long (*EFI_FPSWA) (
+	 *      unsigned long    trap_type,
+	 *      void             *Bundle,
+	 *      unsigned long    *pipsr,
+	 *      unsigned long    *pfsr,
+	 *      unsigned long    *pisr,
+	 *      unsigned long    *ppreds,
+	 *      unsigned long    *pifs,
+	 *      void             *fp_state);
+	 */
+	ret = (*fpswa_interface->fpswa)((unsigned long) fp_fault, bundle,
+					(unsigned long *) ipsr, (unsigned long *) fpsr,
+					(unsigned long *) isr, (unsigned long *) pr,
+					(unsigned long *) ifs, &fp_state);
+
+	return ret;
+}
+
+/*
+ * Handle floating-point assist faults and traps for domain.
+ */
+static unsigned long
+handle_fpu_swa (int fp_fault, struct pt_regs *regs, unsigned long isr)
+{
+	struct vcpu *v = current;
+	IA64_BUNDLE bundle;
+	IA64_BUNDLE __get_domain_bundle(UINT64);
+	unsigned long fault_ip;
+	fpswa_ret_t ret;
+
+	fault_ip = regs->cr_iip;
+	/*
+	 * When the FP trap occurs, the trapping instruction is completed.
+	 * If ipsr.ri == 0, there is the trapping instruction in previous bundle.
+	 */
+	if (!fp_fault && (ia64_psr(regs)->ri == 0))
+		fault_ip -= 16;
+	bundle = __get_domain_bundle(fault_ip);
+	if (!bundle.i64[0] && !bundle.i64[1]) {
+		printk("%s: floating-point bundle at 0x%lx not mapped\n",
+		       __FUNCTION__, fault_ip);
+		return -1;
+	}
+
+	ret = fp_emulate(fp_fault, &bundle, &regs->cr_ipsr, &regs->ar_fpsr,
+	                 &isr, &regs->pr, &regs->cr_ifs, regs);
+
+	if (ret.status) {
+		PSCBX(v, fpswa_ret) = ret;
+		printk("%s(%s): fp_emulate() returned %ld\n",
+		       __FUNCTION__, fp_fault?"fault":"trap", ret.status);
+	} else {
+		if (fp_fault) {
+			/* emulation was successful */
+			vcpu_increment_iip(v);
+		}
+	}
+
+	return ret.status;
+}
+
 void
 ia64_fault (unsigned long vector, unsigned long isr, unsigned long ifa,
 	    unsigned long iim, unsigned long itir, unsigned long arg5,
@@ -758,29 +852,31 @@ ia64_handle_reflection (unsigned long ifa, struct pt_regs *regs, unsigned long i
 			vector = IA64_NAT_CONSUMPTION_VECTOR; break;
 		}
 #endif
-printf("*** NaT fault... attempting to handle as privop\n");
-printf("isr=%016lx, ifa=%016lx, iip=%016lx, ipsr=%016lx\n",
-       isr, ifa, regs->cr_iip, psr);
+		printf("*** NaT fault... attempting to handle as privop\n");
+		printf("isr=%016lx, ifa=%016lx, iip=%016lx, ipsr=%016lx\n",
+		       isr, ifa, regs->cr_iip, psr);
 		//regs->eml_unat = 0;  FIXME: DO WE NEED THIS???
 		// certain NaT faults are higher priority than privop faults
 		vector = priv_emulate(v,regs,isr);
 		if (vector == IA64_NO_FAULT) {
-printf("*** Handled privop masquerading as NaT fault\n");
+			printf("*** Handled privop masquerading as NaT fault\n");
 			return;
 		}
 		vector = IA64_NAT_CONSUMPTION_VECTOR; break;
 	    case 27:
-//printf("*** Handled speculation vector, itc=%lx!\n",ia64_get_itc());
+		//printf("*** Handled speculation vector, itc=%lx!\n",ia64_get_itc());
 		PSCB(current,iim) = iim;
 		vector = IA64_SPECULATION_VECTOR; break;
 	    case 30:
 		// FIXME: Should we handle unaligned refs in Xen??
 		vector = IA64_UNALIGNED_REF_VECTOR; break;
 	    case 32:
-		printf("ia64_handle_reflection: handling FP fault");
+		if (!(handle_fpu_swa(1, regs, isr))) return;
+		printf("ia64_handle_reflection: handling FP fault\n");
 		vector = IA64_FP_FAULT_VECTOR; break;
 	    case 33:
-		printf("ia64_handle_reflection: handling FP trap");
+		if (!(handle_fpu_swa(0, regs, isr))) return;
+		printf("ia64_handle_reflection: handling FP trap\n");
 		vector = IA64_FP_TRAP_VECTOR; break;
 	    case 34:
 		printf("ia64_handle_reflection: handling lowerpriv trap");

xen/arch/ia64/xen/xensetup.c

@@ -53,6 +53,7 @@ extern void alloc_dom0(void);
 extern void setup_per_cpu_areas(void);
 extern void mem_init(void);
 extern void init_IRQ(void);
+extern void trap_init(void);
 
 /* opt_nosmp: If true, secondary processors are ignored. */
 static int opt_nosmp = 0;
@@ -321,6 +322,8 @@ void start_kernel(void)
 
     init_frametable();
 
+    trap_init();
+
     alloc_dom0();
 
     end_boot_allocator();

xen/include/asm-ia64/linux/asm/README.origin

@@ -16,6 +16,7 @@ current.h		-> linux/include/asm-ia64/current.h
 delay.h			-> linux/include/asm-ia64/delay.h
 div64.h			-> linux/include/asm-ia64/div64.h
 dma.h			-> linux/include/asm-ia64/dma.h
+fpswa.h			-> linux/include/asm-ia64/fpswa.h
 fpu.h			-> linux/include/asm-ia64/fpu.h
 hardirq.h		-> linux/include/asm-ia64/hardirq.h
 hdreg.h			-> linux/include/asm-ia64/hdreg.h

xen/include/asm-ia64/linux/asm/fpswa.h

@@ -0,0 +1,73 @@
+#ifndef _ASM_IA64_FPSWA_H
+#define _ASM_IA64_FPSWA_H
+
+/*
+ * Floating-point Software Assist
+ *
+ * Copyright (C) 1999 Intel Corporation.
+ * Copyright (C) 1999 Asit Mallick <asit.k.mallick@intel.com>
+ * Copyright (C) 1999 Goutham Rao <goutham.rao@intel.com>
+ */
+
+typedef struct {
+	/* 4 * 128 bits */
+	unsigned long fp_lp[4*2];
+} fp_state_low_preserved_t;
+
+typedef struct {
+	/* 10 * 128 bits */
+	unsigned long fp_lv[10 * 2];
+} fp_state_low_volatile_t;
+
+typedef	struct {
+	/* 16 * 128 bits */
+	unsigned long fp_hp[16 * 2];
+} fp_state_high_preserved_t;
+
+typedef struct {
+	/* 96 * 128 bits */
+	unsigned long fp_hv[96 * 2];
+} fp_state_high_volatile_t;
+
+/**
+ * floating point state to be passed to the FP emulation library by
+ * the trap/fault handler
+ */
+typedef struct {
+	unsigned long			bitmask_low64;
+	unsigned long			bitmask_high64;
+	fp_state_low_preserved_t	*fp_state_low_preserved;
+	fp_state_low_volatile_t		*fp_state_low_volatile;
+	fp_state_high_preserved_t	*fp_state_high_preserved;
+	fp_state_high_volatile_t	*fp_state_high_volatile;
+} fp_state_t;
+
+typedef struct {
+	unsigned long status;
+	unsigned long err0;
+	unsigned long err1;
+	unsigned long err2;
+} fpswa_ret_t;
+
+/**
+ * function header for the Floating Point software assist
+ * library. This function is invoked by the Floating point software
+ * assist trap/fault handler.
+ */
+typedef fpswa_ret_t (*efi_fpswa_t) (unsigned long trap_type, void *bundle, unsigned long *ipsr,
+				    unsigned long *fsr, unsigned long *isr, unsigned long *preds,
+				    unsigned long *ifs, fp_state_t *fp_state);
+
+/**
+ * This is the FPSWA library interface as defined by EFI.  We need to pass a 
+ * pointer to the interface itself on a call to the assist library
+ */
+typedef struct {
+	unsigned int	 revision;
+	unsigned int	 reserved;
+	efi_fpswa_t	 fpswa;
+} fpswa_interface_t;
+
+extern fpswa_interface_t *fpswa_interface;
+
+#endif /* _ASM_IA64_FPSWA_H */