1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
|
#include <xen/sched.h>
#include <xen/init.h>
#include <asm/processor.h>
#include <asm/vfp.h>
void vfp_save_state(struct vcpu *v)
{
v->arch.vfp.fpexc = READ_CP32(FPEXC);
WRITE_CP32(v->arch.vfp.fpexc | FPEXC_EN, FPEXC);
v->arch.vfp.fpscr = READ_CP32(FPSCR);
if ( v->arch.vfp.fpexc & FPEXC_EX ) /* Check for sub-architecture */
{
v->arch.vfp.fpinst = READ_CP32(FPINST);
if ( v->arch.vfp.fpexc & FPEXC_FP2V )
v->arch.vfp.fpinst2 = READ_CP32(FPINST2);
/* Disable FPEXC_EX */
WRITE_CP32((v->arch.vfp.fpexc | FPEXC_EN) & ~FPEXC_EX, FPEXC);
}
/* Save {d0-d15} */
asm volatile("stc p11, cr0, [%1], #32*4"
: "=Q" (*v->arch.vfp.fpregs1) : "r" (v->arch.vfp.fpregs1));
/* 32 x 64 bits registers? */
if ( (READ_CP32(MVFR0) & MVFR0_A_SIMD_MASK) == 2 )
{
/* Save {d16-d31} */
asm volatile("stcl p11, cr0, [%1], #32*4"
: "=Q" (*v->arch.vfp.fpregs2) : "r" (v->arch.vfp.fpregs2));
}
WRITE_CP32(v->arch.vfp.fpexc & ~(FPEXC_EN), FPEXC);
}
void vfp_restore_state(struct vcpu *v)
{
//uint64_t test[16];
WRITE_CP32(READ_CP32(FPEXC) | FPEXC_EN, FPEXC);
/* Restore {d0-d15} */
asm volatile("ldc p11, cr0, [%1], #32*4"
: : "Q" (*v->arch.vfp.fpregs1), "r" (v->arch.vfp.fpregs1));
/* 32 x 64 bits registers? */
if ( (READ_CP32(MVFR0) & MVFR0_A_SIMD_MASK) == 2 ) /* 32 x 64 bits registers */
/* Restore {d16-d31} */
asm volatile("ldcl p11, cr0, [%1], #32*4"
: : "Q" (*v->arch.vfp.fpregs2), "r" (v->arch.vfp.fpregs2));
if ( v->arch.vfp.fpexc & FPEXC_EX )
{
WRITE_CP32(v->arch.vfp.fpinst, FPINST);
if ( v->arch.vfp.fpexc & FPEXC_FP2V )
WRITE_CP32(v->arch.vfp.fpinst2, FPINST2);
}
WRITE_CP32(v->arch.vfp.fpscr, FPSCR);
WRITE_CP32(v->arch.vfp.fpexc, FPEXC);
}
static __init int vfp_init(void)
{
unsigned int vfpsid;
unsigned int vfparch;
vfpsid = READ_CP32(FPSID);
printk("VFP implementer 0x%02x architecture %d part 0x%02x variant 0x%x "
"rev 0x%x\n",
(vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
(vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT,
(vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
(vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
(vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);
vfparch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;
if ( vfparch < 2 )
panic("Xen only support VFP 3\n");
return 0;
}
presmp_initcall(vfp_init);
/*
* Local variables:
* mode: C
* c-file-style: "BSD"
* c-basic-offset: 4
* indent-tabs-mode: nil
* End:
*/
|
