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