1.1 --- a/xen/arch/ia64/xen/process.c	Thu May 25 15:38:16 2006 -0600
     1.2 +++ b/xen/arch/ia64/xen/process.c	Thu May 25 15:47:33 2006 -0600
     1.3 @@ -33,6 +33,7 @@
     1.4  #include "hpsim_ssc.h"
     1.5  #include <xen/multicall.h>
     1.6  #include <asm/debugger.h>
     1.7 +#include <asm/fpswa.h>
     1.8  
     1.9  extern void die_if_kernel(char *str, struct pt_regs *regs, long err);
    1.10  /* FIXME: where these declarations shold be there ? */
    1.11 @@ -394,6 +395,99 @@ void ia64_do_page_fault (unsigned long a
    1.12  	reflect_interruption(isr, regs, fault);
    1.13  }
    1.14  
    1.15 +fpswa_interface_t *fpswa_interface = 0;
    1.16 +
    1.17 +void trap_init (void)
    1.18 +{
    1.19 +	if (ia64_boot_param->fpswa)
    1.20 +		/* FPSWA fixup: make the interface pointer a virtual address: */
    1.21 +		fpswa_interface = __va(ia64_boot_param->fpswa);
    1.22 +	else
    1.23 +		printk("No FPSWA supported.\n");
    1.24 +}
    1.25 +
    1.26 +static fpswa_ret_t
    1.27 +fp_emulate (int fp_fault, void *bundle, long *ipsr, long *fpsr, long *isr, long *pr, long *ifs,
    1.28 +            struct pt_regs *regs)
    1.29 +{
    1.30 +	fp_state_t fp_state;
    1.31 +	fpswa_ret_t ret;
    1.32 +
    1.33 +	if (!fpswa_interface)
    1.34 +		return ((fpswa_ret_t) {-1, 0, 0, 0});
    1.35 +
    1.36 +	memset(&fp_state, 0, sizeof(fp_state_t));
    1.37 +
    1.38 +	/*
    1.39 +	 * compute fp_state.  only FP registers f6 - f11 are used by the
    1.40 +	 * kernel, so set those bits in the mask and set the low volatile
    1.41 +	 * pointer to point to these registers.
    1.42 +	 */
    1.43 +	fp_state.bitmask_low64 = 0xfc0;  /* bit6..bit11 */
    1.44 +
    1.45 +	fp_state.fp_state_low_volatile = (fp_state_low_volatile_t *) &regs->f6;
    1.46 +	/*
    1.47 +	 * unsigned long (*EFI_FPSWA) (
    1.48 +	 *      unsigned long    trap_type,
    1.49 +	 *      void             *Bundle,
    1.50 +	 *      unsigned long    *pipsr,
    1.51 +	 *      unsigned long    *pfsr,
    1.52 +	 *      unsigned long    *pisr,
    1.53 +	 *      unsigned long    *ppreds,
    1.54 +	 *      unsigned long    *pifs,
    1.55 +	 *      void             *fp_state);
    1.56 +	 */
    1.57 +	ret = (*fpswa_interface->fpswa)((unsigned long) fp_fault, bundle,
    1.58 +					(unsigned long *) ipsr, (unsigned long *) fpsr,
    1.59 +					(unsigned long *) isr, (unsigned long *) pr,
    1.60 +					(unsigned long *) ifs, &fp_state);
    1.61 +
    1.62 +	return ret;
    1.63 +}
    1.64 +
    1.65 +/*
    1.66 + * Handle floating-point assist faults and traps for domain.
    1.67 + */
    1.68 +static unsigned long
    1.69 +handle_fpu_swa (int fp_fault, struct pt_regs *regs, unsigned long isr)
    1.70 +{
    1.71 +	struct vcpu *v = current;
    1.72 +	IA64_BUNDLE bundle;
    1.73 +	IA64_BUNDLE __get_domain_bundle(UINT64);
    1.74 +	unsigned long fault_ip;
    1.75 +	fpswa_ret_t ret;
    1.76 +
    1.77 +	fault_ip = regs->cr_iip;
    1.78 +	/*
    1.79 +	 * When the FP trap occurs, the trapping instruction is completed.
    1.80 +	 * If ipsr.ri == 0, there is the trapping instruction in previous bundle.
    1.81 +	 */
    1.82 +	if (!fp_fault && (ia64_psr(regs)->ri == 0))
    1.83 +		fault_ip -= 16;
    1.84 +	bundle = __get_domain_bundle(fault_ip);
    1.85 +	if (!bundle.i64[0] && !bundle.i64[1]) {
    1.86 +		printk("%s: floating-point bundle at 0x%lx not mapped\n",
    1.87 +		       __FUNCTION__, fault_ip);
    1.88 +		return -1;
    1.89 +	}
    1.90 +
    1.91 +	ret = fp_emulate(fp_fault, &bundle, &regs->cr_ipsr, &regs->ar_fpsr,
    1.92 +	                 &isr, &regs->pr, &regs->cr_ifs, regs);
    1.93 +
    1.94 +	if (ret.status) {
    1.95 +		PSCBX(v, fpswa_ret) = ret;
    1.96 +		printk("%s(%s): fp_emulate() returned %ld\n",
    1.97 +		       __FUNCTION__, fp_fault?"fault":"trap", ret.status);
    1.98 +	} else {
    1.99 +		if (fp_fault) {
   1.100 +			/* emulation was successful */
   1.101 +			vcpu_increment_iip(v);
   1.102 +		}
   1.103 +	}
   1.104 +
   1.105 +	return ret.status;
   1.106 +}
   1.107 +
   1.108  void
   1.109  ia64_fault (unsigned long vector, unsigned long isr, unsigned long ifa,
   1.110  	    unsigned long iim, unsigned long itir, unsigned long arg5,
   1.111 @@ -758,29 +852,31 @@ ia64_handle_reflection (unsigned long if
   1.112  			vector = IA64_NAT_CONSUMPTION_VECTOR; break;
   1.113  		}
   1.114  #endif
   1.115 -printf("*** NaT fault... attempting to handle as privop\n");
   1.116 -printf("isr=%016lx, ifa=%016lx, iip=%016lx, ipsr=%016lx\n",
   1.117 -       isr, ifa, regs->cr_iip, psr);
   1.118 +		printf("*** NaT fault... attempting to handle as privop\n");
   1.119 +		printf("isr=%016lx, ifa=%016lx, iip=%016lx, ipsr=%016lx\n",
   1.120 +		       isr, ifa, regs->cr_iip, psr);
   1.121  		//regs->eml_unat = 0;  FIXME: DO WE NEED THIS???
   1.122  		// certain NaT faults are higher priority than privop faults
   1.123  		vector = priv_emulate(v,regs,isr);
   1.124  		if (vector == IA64_NO_FAULT) {
   1.125 -printf("*** Handled privop masquerading as NaT fault\n");
   1.126 +			printf("*** Handled privop masquerading as NaT fault\n");
   1.127  			return;
   1.128  		}
   1.129  		vector = IA64_NAT_CONSUMPTION_VECTOR; break;
   1.130  	    case 27:
   1.131 -//printf("*** Handled speculation vector, itc=%lx!\n",ia64_get_itc());
   1.132 +		//printf("*** Handled speculation vector, itc=%lx!\n",ia64_get_itc());
   1.133  		PSCB(current,iim) = iim;
   1.134  		vector = IA64_SPECULATION_VECTOR; break;
   1.135  	    case 30:
   1.136  		// FIXME: Should we handle unaligned refs in Xen??
   1.137  		vector = IA64_UNALIGNED_REF_VECTOR; break;
   1.138  	    case 32:
   1.139 -		printf("ia64_handle_reflection: handling FP fault");
   1.140 +		if (!(handle_fpu_swa(1, regs, isr))) return;
   1.141 +		printf("ia64_handle_reflection: handling FP fault\n");
   1.142  		vector = IA64_FP_FAULT_VECTOR; break;
   1.143  	    case 33:
   1.144 -		printf("ia64_handle_reflection: handling FP trap");
   1.145 +		if (!(handle_fpu_swa(0, regs, isr))) return;
   1.146 +		printf("ia64_handle_reflection: handling FP trap\n");
   1.147  		vector = IA64_FP_TRAP_VECTOR; break;
   1.148  	    case 34:
   1.149  		printf("ia64_handle_reflection: handling lowerpriv trap");

     2.1 --- a/xen/arch/ia64/xen/xensetup.c	Thu May 25 15:38:16 2006 -0600
     2.2 +++ b/xen/arch/ia64/xen/xensetup.c	Thu May 25 15:47:33 2006 -0600
     2.3 @@ -53,6 +53,7 @@ extern void alloc_dom0(void);
     2.4  extern void setup_per_cpu_areas(void);
     2.5  extern void mem_init(void);
     2.6  extern void init_IRQ(void);
     2.7 +extern void trap_init(void);
     2.8  
     2.9  /* opt_nosmp: If true, secondary processors are ignored. */
    2.10  static int opt_nosmp = 0;
    2.11 @@ -321,6 +322,8 @@ void start_kernel(void)
    2.12  
    2.13      init_frametable();
    2.14  
    2.15 +    trap_init();
    2.16 +
    2.17      alloc_dom0();
    2.18  
    2.19      end_boot_allocator();

     3.1 --- a/xen/include/asm-ia64/linux/asm/README.origin	Thu May 25 15:38:16 2006 -0600
     3.2 +++ b/xen/include/asm-ia64/linux/asm/README.origin	Thu May 25 15:47:33 2006 -0600
     3.3 @@ -16,6 +16,7 @@ current.h		-> linux/include/asm-ia64/cur
     3.4  delay.h			-> linux/include/asm-ia64/delay.h
     3.5  div64.h			-> linux/include/asm-ia64/div64.h
     3.6  dma.h			-> linux/include/asm-ia64/dma.h
     3.7 +fpswa.h			-> linux/include/asm-ia64/fpswa.h
     3.8  fpu.h			-> linux/include/asm-ia64/fpu.h
     3.9  hardirq.h		-> linux/include/asm-ia64/hardirq.h
    3.10  hdreg.h			-> linux/include/asm-ia64/hdreg.h

     4.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     4.2 +++ b/xen/include/asm-ia64/linux/asm/fpswa.h	Thu May 25 15:47:33 2006 -0600
     4.3 @@ -0,0 +1,73 @@
     4.4 +#ifndef _ASM_IA64_FPSWA_H
     4.5 +#define _ASM_IA64_FPSWA_H
     4.6 +
     4.7 +/*
     4.8 + * Floating-point Software Assist
     4.9 + *
    4.10 + * Copyright (C) 1999 Intel Corporation.
    4.11 + * Copyright (C) 1999 Asit Mallick <asit.k.mallick@intel.com>
    4.12 + * Copyright (C) 1999 Goutham Rao <goutham.rao@intel.com>
    4.13 + */
    4.14 +
    4.15 +typedef struct {
    4.16 +	/* 4 * 128 bits */
    4.17 +	unsigned long fp_lp[4*2];
    4.18 +} fp_state_low_preserved_t;
    4.19 +
    4.20 +typedef struct {
    4.21 +	/* 10 * 128 bits */
    4.22 +	unsigned long fp_lv[10 * 2];
    4.23 +} fp_state_low_volatile_t;
    4.24 +
    4.25 +typedef	struct {
    4.26 +	/* 16 * 128 bits */
    4.27 +	unsigned long fp_hp[16 * 2];
    4.28 +} fp_state_high_preserved_t;
    4.29 +
    4.30 +typedef struct {
    4.31 +	/* 96 * 128 bits */
    4.32 +	unsigned long fp_hv[96 * 2];
    4.33 +} fp_state_high_volatile_t;
    4.34 +
    4.35 +/**
    4.36 + * floating point state to be passed to the FP emulation library by
    4.37 + * the trap/fault handler
    4.38 + */
    4.39 +typedef struct {
    4.40 +	unsigned long			bitmask_low64;
    4.41 +	unsigned long			bitmask_high64;
    4.42 +	fp_state_low_preserved_t	*fp_state_low_preserved;
    4.43 +	fp_state_low_volatile_t		*fp_state_low_volatile;
    4.44 +	fp_state_high_preserved_t	*fp_state_high_preserved;
    4.45 +	fp_state_high_volatile_t	*fp_state_high_volatile;
    4.46 +} fp_state_t;
    4.47 +
    4.48 +typedef struct {
    4.49 +	unsigned long status;
    4.50 +	unsigned long err0;
    4.51 +	unsigned long err1;
    4.52 +	unsigned long err2;
    4.53 +} fpswa_ret_t;
    4.54 +
    4.55 +/**
    4.56 + * function header for the Floating Point software assist
    4.57 + * library. This function is invoked by the Floating point software
    4.58 + * assist trap/fault handler.
    4.59 + */
    4.60 +typedef fpswa_ret_t (*efi_fpswa_t) (unsigned long trap_type, void *bundle, unsigned long *ipsr,
    4.61 +				    unsigned long *fsr, unsigned long *isr, unsigned long *preds,
    4.62 +				    unsigned long *ifs, fp_state_t *fp_state);
    4.63 +
    4.64 +/**
    4.65 + * This is the FPSWA library interface as defined by EFI.  We need to pass a 
    4.66 + * pointer to the interface itself on a call to the assist library
    4.67 + */
    4.68 +typedef struct {
    4.69 +	unsigned int	 revision;
    4.70 +	unsigned int	 reserved;
    4.71 +	efi_fpswa_t	 fpswa;
    4.72 +} fpswa_interface_t;
    4.73 +
    4.74 +extern fpswa_interface_t *fpswa_interface;
    4.75 +
    4.76 +#endif /* _ASM_IA64_FPSWA_H */