cd95876834c509ac033336a66b1c117e55b55205
[akaros.git] / kern / arch / sparc / fpu.c
1 #include <arch/sparcfpu.h>
2 #include <arch/arch.h>
3 #include <arch/trap.h>
4 #include <pmap.h>
5
6 static inline uint32_t* effective_address(trapframe_t* state, uint32_t insn)
7 {
8         uint32_t rs1 = state->gpr[(insn>>14)&0x1F];
9         uint32_t rs2 = state->gpr[insn&0x1F];
10         struct { signed int x:13; } s;
11         int32_t imm = s.x = insn&0x1FFF;
12
13         return (uint32_t*)((insn & 0x2000) ? rs1+imm : rs1+rs2);
14 }
15
16 void fp_access_exception(trapframe_t* state, void* addr)
17 {
18         state->fault_status = 1;
19         state->fault_addr = (uint32_t)addr;
20         data_access_exception(state);
21 }
22
23 static inline uint32_t fp_load_word(trapframe_t* state, uint32_t* addr)
24 {
25         uint32_t word;
26         if((long)addr % sizeof(word))
27                 address_unaligned(state);
28         if(memcpy_from_user(current,&word,addr,sizeof(word)))
29                 fp_access_exception(state,addr);
30         return word;
31 }
32
33 static inline uint64_t fp_load_dword(trapframe_t* state, uint64_t* addr)
34 {
35         uint64_t word;
36         if((long)addr % sizeof(word))
37                 address_unaligned(state);
38         if(memcpy_from_user(current,&word,addr,sizeof(word)))
39                 fp_access_exception(state,addr);
40         return word;
41 }
42
43 static inline void fp_store_word(trapframe_t* state, uint32_t* addr, uint32_t word)
44 {
45         if((long)addr % sizeof(word))
46                 address_unaligned(state);
47         if(memcpy_to_user(current,addr,&word,sizeof(word)))
48                 fp_access_exception(state,addr);
49 }
50
51 static inline void fp_store_dword(trapframe_t* state, uint64_t* addr, uint64_t word)
52 {
53         if((long)addr % sizeof(word))
54                 address_unaligned(state);
55         if(memcpy_to_user(current,addr,&word,sizeof(word)))
56                 fp_access_exception(state,addr);
57 }
58
59 void emulate_fpu(trapframe_t* state, ancillary_state_t* astate)
60 {
61         sparcfpu_t thefpu;
62         sparcfpu_t* fpu = &thefpu;
63         sparcfpu_init(fpu);
64
65         // pretend there are no FP exceptions right now.
66         // we should catch them again after emulation 
67         sparcfpu_setFSR(fpu,astate->fsr & ~0x1C000);
68         memcpy(fpu->freg,astate->fpr,sizeof(astate->fpr));
69
70         int annul = 0;
71         uint32_t nnpc = state->npc+4;
72
73         uint64_t dword;
74         uint32_t reg,word;
75         int32_t disp;
76         uint32_t* addr;
77         struct { signed int x:22; } disp22;
78
79         uint32_t insn = fp_load_word(state,(uint32_t*)state->pc);
80         fp_insn_t* fpi = (fp_insn_t*)&insn;
81
82         switch(insn >> 30)
83         {
84                 case 0:
85                         switch((insn >> 22) & 0x7)
86                         {
87                                 case 6:
88                                 {
89                                         int cond = (insn>>25)&0xF;
90                                         if(check_fcc[cond][fpu->FSR.fcc])
91                                         {
92                                                 annul = (cond == fccA || cond == fccN) && ((insn>>29)&1);
93                                                 disp = disp22.x = insn & 0x3FFFFF;
94                                                 nnpc = state->pc + 4*disp;
95                                         }
96                                         else annul = (insn>>29)&1;
97                                         break;
98                                 }
99                                 default:
100                                         illegal_instruction(state);
101                                         break;
102                         }
103                         break;
104                 case 1:
105                         illegal_instruction(state);
106                         break;
107                 case 2:
108                         switch((insn >> 19) & 0x3F)
109                         {
110                                 case 0x34:
111                                         sparcfpu_fpop1(fpu,*fpi);
112                                         break;
113                                 case 0x35:
114                                         sparcfpu_fpop2(fpu,*fpi);
115                                         break;
116                                 default:
117                                         illegal_instruction(state);
118                                         break;
119                         }
120                         break;
121                 case 3:
122                         switch((insn >> 19) & 0x3F)
123                         {
124                                 case 0x20:
125                                         sparcfpu_wrregs(fpu,(insn>>25)&0x1F,fp_load_word(state,effective_address(state,insn)));
126                                         break;
127                                 case 0x21: // ldfsr
128                                         addr = effective_address(state,insn);
129                                         word = fp_load_word(state,addr);
130                                         sparcfpu_setFSR(fpu,word);
131                                         break;
132                                 case 0x23:
133                                         addr = effective_address(state,insn);
134                                         reg = (insn>>25)&0x1F;
135                                         dword = fp_load_dword(state,(uint64_t*)addr);
136                                         sparcfpu_wrregs(fpu,reg,(uint32_t)(dword>>32));
137                                         sparcfpu_wrregs(fpu,reg+1,(uint32_t)dword);
138                                         break;
139                                 case 0x24:
140                                         addr = effective_address(state,insn);
141                                         fp_store_word(state,addr,sparcfpu_regs(fpu,(insn>>25)&0x1F));
142                                         break;
143                                 case 0x25: // stfsr
144                                         addr = effective_address(state,insn);
145                                         fp_store_word(state,addr,sparcfpu_getFSR(fpu));
146                                         fpu->FSR.ftt = 0;
147                                         break;
148                                 case 0x27:
149                                         addr = effective_address(state,insn);
150                                         reg = (insn>>25)&0x1F;
151                                         dword = (((uint64_t)sparcfpu_regs(fpu,reg))<<32)|(uint64_t)sparcfpu_regs(fpu,reg+1);
152                                         fp_store_dword(state,(uint64_t*)addr,dword);
153                                         break;
154                                 default:
155                                         illegal_instruction(state);
156                                         break;
157                         }
158                         break;
159         }
160
161         astate->fsr = sparcfpu_getFSR(fpu);
162
163         // got an FP exception after re-execution
164         if(fpu->FSR.ftt)
165                 real_fp_exception(state,astate);
166         else
167         {
168                 if(!annul)
169                 {
170                         state->pc = state->npc;
171                         state->npc = nnpc;
172                 }
173                 else
174                 {
175                         state->pc = nnpc;
176                         state->npc = nnpc+1;
177                 }
178         }
179
180         static_assert(sizeof(astate->fpr) == sizeof(fpu->freg));
181         memcpy(astate->fpr,fpu->freg,sizeof(astate->fpr));
182 }