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