1/*
2    NetWinder Floating Point Emulator
3    (c) Rebel.com, 1998-1999
4    (c) Philip Blundell, 1998
5
6    Direct questions, comments to Scott Bambrough <scottb@netwinder.org>
7
8    This program is free software; you can redistribute it and/or modify
9    it under the terms of the GNU General Public License as published by
10    the Free Software Foundation; either version 2 of the License, or
11    (at your option) any later version.
12
13    This program is distributed in the hope that it will be useful,
14    but WITHOUT ANY WARRANTY; without even the implied warranty of
15    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16    GNU General Public License for more details.
17
18    You should have received a copy of the GNU General Public License
19    along with this program; if not, write to the Free Software
20    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
21*/
22
23#include "fpa11.h"
24#include "softfloat.h"
25#include "fpopcode.h"
26#include "fpmodule.h"
27#include "fpmodule.inl"
28
29#include <asm/uaccess.h>
30
31static inline
32void loadSingle(const unsigned int Fn,const unsigned int *pMem)
33{
34   FPA11 *fpa11 = GET_FPA11();
35   fpa11->fType[Fn] = typeSingle;
36   get_user(fpa11->fpreg[Fn].fSingle, pMem);
37}
38
39static inline
40void loadDouble(const unsigned int Fn,const unsigned int *pMem)
41{
42   FPA11 *fpa11 = GET_FPA11();
43   unsigned int *p;
44   p = (unsigned int*)&fpa11->fpreg[Fn].fDouble;
45   fpa11->fType[Fn] = typeDouble;
46   get_user(p[0], &pMem[1]);
47   get_user(p[1], &pMem[0]); /* sign & exponent */
48}
49
50static inline
51void loadExtended(const unsigned int Fn,const unsigned int *pMem)
52{
53   FPA11 *fpa11 = GET_FPA11();
54   unsigned int *p;
55   p = (unsigned int*)&fpa11->fpreg[Fn].fExtended;
56   fpa11->fType[Fn] = typeExtended;
57   get_user(p[0], &pMem[0]);  /* sign & exponent */
58   get_user(p[1], &pMem[2]);  /* ls bits */
59   get_user(p[2], &pMem[1]);  /* ms bits */
60}
61
62static inline
63void loadMultiple(const unsigned int Fn,const unsigned int *pMem)
64{
65   FPA11 *fpa11 = GET_FPA11();
66   register unsigned int *p;
67   unsigned long x;
68
69   p = (unsigned int*)&(fpa11->fpreg[Fn]);
70   get_user(x, &pMem[0]);
71   fpa11->fType[Fn] = (x >> 14) & 0x00000003;
72
73   switch (fpa11->fType[Fn])
74   {
75      case typeSingle:
76      case typeDouble:
77      {
78         get_user(p[0], &pMem[2]);  /* Single */
79         get_user(p[1], &pMem[1]);  /* double msw */
80         p[2] = 0;        /* empty */
81      }
82      break;
83
84      case typeExtended:
85      {
86         get_user(p[1], &pMem[2]);
87         get_user(p[2], &pMem[1]);  /* msw */
88         p[0] = (x & 0x80003fff);
89      }
90      break;
91   }
92}
93
94static inline
95void storeSingle(const unsigned int Fn,unsigned int *pMem)
96{
97   FPA11 *fpa11 = GET_FPA11();
98   union
99   {
100     float32 f;
101     unsigned int i[1];
102   } val;
103
104   switch (fpa11->fType[Fn])
105   {
106      case typeDouble:
107         val.f = float64_to_float32(fpa11->fpreg[Fn].fDouble);
108      break;
109
110      case typeExtended:
111         val.f = floatx80_to_float32(fpa11->fpreg[Fn].fExtended);
112      break;
113
114      default: val.f = fpa11->fpreg[Fn].fSingle;
115   }
116
117   put_user(val.i[0], pMem);
118}
119
120static inline
121void storeDouble(const unsigned int Fn,unsigned int *pMem)
122{
123   FPA11 *fpa11 = GET_FPA11();
124   union
125   {
126     float64 f;
127     unsigned int i[2];
128   } val;
129
130   switch (fpa11->fType[Fn])
131   {
132      case typeSingle:
133         val.f = float32_to_float64(fpa11->fpreg[Fn].fSingle);
134      break;
135
136      case typeExtended:
137         val.f = floatx80_to_float64(fpa11->fpreg[Fn].fExtended);
138      break;
139
140      default: val.f = fpa11->fpreg[Fn].fDouble;
141   }
142
143   put_user(val.i[1], &pMem[0]);	/* msw */
144   put_user(val.i[0], &pMem[1]);	/* lsw */
145}
146
147static inline
148void storeExtended(const unsigned int Fn,unsigned int *pMem)
149{
150   FPA11 *fpa11 = GET_FPA11();
151   union
152   {
153     floatx80 f;
154     unsigned int i[3];
155   } val;
156
157   switch (fpa11->fType[Fn])
158   {
159      case typeSingle:
160         val.f = float32_to_floatx80(fpa11->fpreg[Fn].fSingle);
161      break;
162
163      case typeDouble:
164         val.f = float64_to_floatx80(fpa11->fpreg[Fn].fDouble);
165      break;
166
167      default: val.f = fpa11->fpreg[Fn].fExtended;
168   }
169
170   put_user(val.i[0], &pMem[0]); /* sign & exp */
171   put_user(val.i[1], &pMem[2]);
172   put_user(val.i[2], &pMem[1]); /* msw */
173}
174
175static inline
176void storeMultiple(const unsigned int Fn,unsigned int *pMem)
177{
178   FPA11 *fpa11 = GET_FPA11();
179   register unsigned int nType, *p;
180
181   p = (unsigned int*)&(fpa11->fpreg[Fn]);
182   nType = fpa11->fType[Fn];
183
184   switch (nType)
185   {
186      case typeSingle:
187      case typeDouble:
188      {
189	 put_user(p[0], &pMem[2]); /* single */
190	 put_user(p[1], &pMem[1]); /* double msw */
191	 put_user(nType << 14, &pMem[0]);
192      }
193      break;
194
195      case typeExtended:
196      {
197	 put_user(p[2], &pMem[1]); /* msw */
198	 put_user(p[1], &pMem[2]);
199	 put_user((p[0] & 0x80003fff) | (nType << 14), &pMem[0]);
200      }
201      break;
202   }
203}
204
205unsigned int PerformLDF(const unsigned int opcode)
206{
207   unsigned int *pBase, *pAddress, *pFinal, nRc = 1,
208     write_back = WRITE_BACK(opcode);
209
210   //printk("PerformLDF(0x%08x), Fd = 0x%08x\n",opcode,getFd(opcode));
211
212   pBase = (unsigned int*)readRegister(getRn(opcode));
213   if (REG_PC == getRn(opcode))
214   {
215     pBase += 2;
216     write_back = 0;
217   }
218
219   pFinal = pBase;
220   if (BIT_UP_SET(opcode))
221     pFinal += getOffset(opcode);
222   else
223     pFinal -= getOffset(opcode);
224
225   if (PREINDEXED(opcode)) pAddress = pFinal; else pAddress = pBase;
226
227   switch (opcode & MASK_TRANSFER_LENGTH)
228   {
229      case TRANSFER_SINGLE  : loadSingle(getFd(opcode),pAddress);   break;
230      case TRANSFER_DOUBLE  : loadDouble(getFd(opcode),pAddress);   break;
231      case TRANSFER_EXTENDED: loadExtended(getFd(opcode),pAddress); break;
232      default: nRc = 0;
233   }
234
235   if (write_back) writeRegister(getRn(opcode),(unsigned int)pFinal);
236   return nRc;
237}
238
239unsigned int PerformSTF(const unsigned int opcode)
240{
241   unsigned int *pBase, *pAddress, *pFinal, nRc = 1,
242     write_back = WRITE_BACK(opcode);
243
244   //printk("PerformSTF(0x%08x), Fd = 0x%08x\n",opcode,getFd(opcode));
245   SetRoundingMode(ROUND_TO_NEAREST);
246
247   pBase = (unsigned int*)readRegister(getRn(opcode));
248   if (REG_PC == getRn(opcode))
249   {
250     pBase += 2;
251     write_back = 0;
252   }
253
254   pFinal = pBase;
255   if (BIT_UP_SET(opcode))
256     pFinal += getOffset(opcode);
257   else
258     pFinal -= getOffset(opcode);
259
260   if (PREINDEXED(opcode)) pAddress = pFinal; else pAddress = pBase;
261
262   switch (opcode & MASK_TRANSFER_LENGTH)
263   {
264      case TRANSFER_SINGLE  : storeSingle(getFd(opcode),pAddress);   break;
265      case TRANSFER_DOUBLE  : storeDouble(getFd(opcode),pAddress);   break;
266      case TRANSFER_EXTENDED: storeExtended(getFd(opcode),pAddress); break;
267      default: nRc = 0;
268   }
269
270   if (write_back) writeRegister(getRn(opcode),(unsigned int)pFinal);
271   return nRc;
272}
273
274unsigned int PerformLFM(const unsigned int opcode)
275{
276   unsigned int i, Fd, *pBase, *pAddress, *pFinal,
277     write_back = WRITE_BACK(opcode);
278
279   pBase = (unsigned int*)readRegister(getRn(opcode));
280   if (REG_PC == getRn(opcode))
281   {
282     pBase += 2;
283     write_back = 0;
284   }
285
286   pFinal = pBase;
287   if (BIT_UP_SET(opcode))
288     pFinal += getOffset(opcode);
289   else
290     pFinal -= getOffset(opcode);
291
292   if (PREINDEXED(opcode)) pAddress = pFinal; else pAddress = pBase;
293
294   Fd = getFd(opcode);
295   for (i=getRegisterCount(opcode);i>0;i--)
296   {
297     loadMultiple(Fd,pAddress);
298     pAddress += 3; Fd++;
299     if (Fd == 8) Fd = 0;
300   }
301
302   if (write_back) writeRegister(getRn(opcode),(unsigned int)pFinal);
303   return 1;
304}
305
306unsigned int PerformSFM(const unsigned int opcode)
307{
308   unsigned int i, Fd, *pBase, *pAddress, *pFinal,
309     write_back = WRITE_BACK(opcode);
310
311   pBase = (unsigned int*)readRegister(getRn(opcode));
312   if (REG_PC == getRn(opcode))
313   {
314     pBase += 2;
315     write_back = 0;
316   }
317
318   pFinal = pBase;
319   if (BIT_UP_SET(opcode))
320     pFinal += getOffset(opcode);
321   else
322     pFinal -= getOffset(opcode);
323
324   if (PREINDEXED(opcode)) pAddress = pFinal; else pAddress = pBase;
325
326   Fd = getFd(opcode);
327   for (i=getRegisterCount(opcode);i>0;i--)
328   {
329     storeMultiple(Fd,pAddress);
330     pAddress += 3; Fd++;
331     if (Fd == 8) Fd = 0;
332   }
333
334   if (write_back) writeRegister(getRn(opcode),(unsigned int)pFinal);
335   return 1;
336}
337
338unsigned int EmulateCPDT(const unsigned int opcode)
339{
340  unsigned int nRc = 0;
341
342  //printk("EmulateCPDT(0x%08x)\n",opcode);
343
344  if (LDF_OP(opcode))
345  {
346    nRc = PerformLDF(opcode);
347  }
348  else if (LFM_OP(opcode))
349  {
350    nRc = PerformLFM(opcode);
351  }
352  else if (STF_OP(opcode))
353  {
354    nRc = PerformSTF(opcode);
355  }
356  else if (SFM_OP(opcode))
357  {
358    nRc = PerformSFM(opcode);
359  }
360  else
361  {
362    nRc = 0;
363  }
364
365  return nRc;
366}
367