osfmk/ppc/alignment.c

   1 /*
   2  * Copyright (c) 2000 Apple Computer, Inc. All rights reserved.
   3  *
   4  * @APPLE_LICENSE_HEADER_START@
   5  *
   6  * The contents of this file constitute Original Code as defined in and
   7  * are subject to the Apple Public Source License Version 1.1 (the
   8  * "License").  You may not use this file except in compliance with the
   9  * License.  Please obtain a copy of the License at
  10  * http://www.apple.com/publicsource and read it before using this file.
  11  *
  12  * This Original Code and all software distributed under the License are
  13  * distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, EITHER
  14  * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
  15  * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
  16  * FITNESS FOR A PARTICULAR PURPOSE OR NON-INFRINGEMENT.  Please see the
  17  * License for the specific language governing rights and limitations
  18  * under the License.
  19  *
  20  * @APPLE_LICENSE_HEADER_END@
  21  */
  22 /*
  23  * Copyright 1996 1995 by Open Software Foundation, Inc. 1997 1996 1995 1994 1993 1992 1991
  24  *              All Rights Reserved
  25  *
  26  * Permission to use, copy, modify, and distribute this software and
  27  * its documentation for any purpose and without fee is hereby granted,
  28  * provided that the above copyright notice appears in all copies and
  29  * that both the copyright notice and this permission notice appear in
  30  * supporting documentation.
  31  *
  32  * OSF DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE
  33  * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  34  * FOR A PARTICULAR PURPOSE.
  35  *
  36  * IN NO EVENT SHALL OSF BE LIABLE FOR ANY SPECIAL, INDIRECT, OR
  37  * CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
  38  * LOSS OF USE, DATA OR PROFITS, WHETHER IN ACTION OF CONTRACT,
  39  * NEGLIGENCE, OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION
  40  * WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  41  *
  42  */
  43 /*
  44  * Copyright 1996 1995 by Apple Computer, Inc. 1997 1996 1995 1994 1993 1992 1991
  45  *              All Rights Reserved
  46  *
  47  * Permission to use, copy, modify, and distribute this software and
  48  * its documentation for any purpose and without fee is hereby granted,
  49  * provided that the above copyright notice appears in all copies and
  50  * that both the copyright notice and this permission notice appear in
  51  * supporting documentation.
  52  *
  53  * APPLE COMPUTER DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE
  54  * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  55  * FOR A PARTICULAR PURPOSE.
  56  *
  57  * IN NO EVENT SHALL APPLE COMPUTER BE LIABLE FOR ANY SPECIAL, INDIRECT, OR
  58  * CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
  59  * LOSS OF USE, DATA OR PROFITS, WHETHER IN ACTION OF CONTRACT,
  60  * NEGLIGENCE, OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION
  61  * WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  62  */
  63
  64 #include <debug.h>
  65 #include <kern/thread_act.h>
  66 #include <mach/thread_status.h>
  67 #include <mach/boolean.h>
  68 #include <kern/misc_protos.h>
  69 #include <kern/simple_lock.h>
  70 #include <ppc/proc_reg.h>
  71 #include <ppc/fpu_protos.h>
  72 #include <ppc/misc_protos.h>
  73 #include <ppc/exception.h>
  74
  75 #if DEBUG
  76 /* These variable may be used to keep track of alignment exceptions */
  77 int alignment_exception_count_user;
  78 int alignment_exception_count_kernel;
  79 #endif
  80
  81 #define _AINST(x)       boolean_t  align_##x##(unsigned long dsisr,\
  82                                                struct ppc_saved_state *ssp, \
  83                                                struct ppc_float_state *fsp, \
  84                                                unsigned long *align_buffer, \
  85                                                unsigned long dar)
  86
  87
  88 #define _AFENTRY(name, r, b)    { #name, align_##name##, r, b, TRUE }
  89 #define _AENTRY(name, r, b)     { #name, align_##name##, r, b, FALSE }
  90 #define _ANIL                   { (void *) 0, (void *) 0, 0, 0 }
  91
  92 _AINST(lwz);
  93 _AINST(stw);
  94 _AINST(lhz);
  95 _AINST(lha);
  96 _AINST(sth);
  97 _AINST(lmw);
  98 _AINST(lfs);
  99 _AINST(lfd);
 100 _AINST(stfs);
 101 _AINST(stfd);
 102 _AINST(lwzu);
 103 _AINST(stwu);
 104 _AINST(lhzu);
 105 _AINST(lhau);
 106 _AINST(sthu);
 107 _AINST(lfsu);
 108 _AINST(lfdu);
 109 _AINST(stfsu);
 110 _AINST(stfdu);
 111 _AINST(lswx);
 112 _AINST(lswi);
 113 _AINST(lwbrx);
 114 _AINST(stwbrx);
 115 _AINST(lhbrx);
 116 _AINST(sthbrx);
 117 _AINST(dcbz);
 118 _AINST(lwzx);
 119 _AINST(stwx);
 120 _AINST(lhzx);
 121 _AINST(lhax);
 122 _AINST(sthx);
 123 _AINST(lfsx);
 124 _AINST(lfdx);
 125 _AINST(stfsx);
 126 _AINST(stfdx);
 127 _AINST(lwzux);
 128 _AINST(stwux);
 129 _AINST(lhzux);
 130 _AINST(lhaux);
 131 _AINST(sthux);
 132 _AINST(stmw);
 133 _AINST(lfsux);
 134 _AINST(lfdux);
 135 _AINST(stfsux);
 136 _AINST(stfdux);
 137
 138 /*
 139  * Routines to set and get FPU registers.
 140  */
 141
 142 void GET_FPU_REG(struct ppc_float_state *fsp,
 143                  unsigned long reg,
 144                  unsigned long *value);
 145 void SET_FPU_REG(struct ppc_float_state *fsp,
 146                  unsigned long reg,
 147                  unsigned long *value);
 148
 149 __inline__ void GET_FPU_REG(struct ppc_float_state *fsp,
 150                             unsigned long reg,
 151                             unsigned long *value)
 152 {
 153         value[0] = ((unsigned long *) &fsp->fpregs[reg])[0];
 154         value[1] = ((unsigned long *) &fsp->fpregs[reg])[1];
 155 }
 156
 157 __inline__ void SET_FPU_REG(struct ppc_float_state *fsp,
 158                             unsigned long reg, unsigned long *value)
 159 {
 160         ((unsigned long *) &fsp->fpregs[reg])[0] = value[0];
 161         ((unsigned long *) &fsp->fpregs[reg])[1] = value[1];
 162 }
 163
 164
 165 /*
 166  * Macros to load and set registers according to
 167  * a given cast type.
 168  */
 169
 170 #define GET_REG(p, reg, value, cast) \
 171         { *((cast *) value) = *((cast *) (&p->r0+reg)); }
 172 #define SET_REG(p, reg, value, cast) \
 173         { *((cast *) (&p->r0+reg)) = *((cast *) value); }
 174
 175 /*
 176  * Macros to help decode the DSISR.
 177  */
 178
 179 #define DSISR_BITS_15_16(bits)  ((bits>>15) & 0x3)
 180 #define DSISR_BITS_17_21(bits)  ((bits>>10) & 0x1f)
 181 #define DSISR_BITS_REG(bits)    ((bits>>5) & 0x1f)
 182 #define DSISR_BITS_RA(bits)     (bits & 0x1f)
 183
 184
 185 struct ppc_align_instruction {
 186         char            *name;
 187         boolean_t       (*a_instruct)(unsigned long,
 188                                       struct ppc_saved_state *,
 189                                       struct ppc_float_state *,
 190                                       unsigned long *,
 191                                       unsigned long );
 192         int             a_readbytes;
 193         int             a_writebytes;
 194         boolean_t       a_is_float;
 195 } align_table00[] = {
 196 _AENTRY(lwz, 4, 0),     /* 00 0 0000 */
 197 _ANIL,                  /* 00 0 0001 */
 198 _AENTRY(stw, 0, 4),     /* 00 0 0010 */
 199 _ANIL,                  /* 00 0 0011 */
 200 _AENTRY(lhz, 2, 0),     /* 00 0 0100 */
 201 _AENTRY(lha, 2, 0),     /* 00 0 0101 */
 202 _AENTRY(sth, 0, 2),     /* 00 0 0110 */
 203 _AENTRY(lmw, 32*4,0),   /* 00 0 0111 */
 204 _AFENTRY(lfs, 4, 0),    /* 00 0 1000 */
 205 _AFENTRY(lfd, 8, 0),    /* 00 0 1001 */
 206 _AFENTRY(stfs, 0, 4),   /* 00 0 1010 */
 207 _AFENTRY(stfd, 0, 8),   /* 00 0 1011 */
 208 _ANIL,                  /* 00 0 1100 ?*/
 209 _ANIL,                  /* 00 0 1101 - lwa */
 210 _ANIL,                  /* 00 0 1110 ?*/
 211 _ANIL,                  /* 00 0 1111 - std */
 212 _AENTRY(lwzu, 4, 0),    /* 00 1 0000 */
 213 _ANIL,                  /* 00 1 0001 ?*/
 214 _AENTRY(stwu, 0, 4),    /* 00 1 0010 */
 215 _ANIL,                  /* 00 1 0011 */
 216 _AENTRY(lhzu, 2, 0),    /* 00 1 0100 */
 217 _AENTRY(lhau, 2, 0),    /* 00 1 0101 */
 218 _AENTRY(sthu, 0, 2),    /* 00 1 0110 */
 219 _AENTRY(stmw, 0, 0),    /* 00 1 0111 */
 220 _AFENTRY(lfsu, 4, 0),   /* 00 1 1000 */
 221 _AFENTRY(lfdu, 8, 0),   /* 00 1 1001 - lfdu */
 222 _AFENTRY(stfsu, 0, 4),  /* 00 1 1010 */
 223 _AFENTRY(stfdu, 0, 8),  /* 00 1 1011 - stfdu */
 224 };
 225
 226 struct ppc_align_instruction align_table01[] = {
 227 _ANIL,                  /* 01 0 0000 - ldx */
 228 _ANIL,                  /* 01 0 0001 ?*/
 229 _ANIL,                  /* 01 0 0010 - stdx */
 230 _ANIL,                  /* 01 0 0011 ?*/
 231 _ANIL,                  /* 01 0 0100 ?*/
 232 _ANIL,                  /* 01 0 0101 - lwax */
 233 _ANIL,                  /* 01 0 0110 ?*/
 234 _ANIL,                  /* 01 0 0111 ?*/
 235 _AENTRY(lswx,32, 0),    /* 01 0 1000 - lswx */
 236 _AENTRY(lswi,32, 0),    /* 01 0 1001 - lswi */
 237 _ANIL,                  /* 01 0 1010 - stswx */
 238 _ANIL,                  /* 01 0 1011 - stswi */
 239 _ANIL,                  /* 01 0 1100 ?*/
 240 _ANIL,                  /* 01 0 1101 ?*/
 241 _ANIL,                  /* 01 0 1110 ?*/
 242 _ANIL,                  /* 01 0 1111 ?*/
 243 _ANIL,                  /* 01 1 0000 - ldux */
 244 _ANIL,                  /* 01 1 0001 ?*/
 245 _ANIL,                  /* 01 1 0010 - stdux */
 246 _ANIL,                  /* 01 1 0011 ?*/
 247 _ANIL,                  /* 01 1 0100 ?*/
 248 _ANIL,                  /* 01 1 0101 - lwaux */
 249 };
 250
 251 struct ppc_align_instruction align_table10[] = {
 252 _ANIL,                  /* 10 0 0000 ?*/
 253 _ANIL,                  /* 10 0 0001 ?*/
 254 _ANIL,                  /* 10 0 0010 - stwcx. */
 255 _ANIL,                  /* 10 0 0011 - stdcx.*/
 256 _ANIL,                  /* 10 0 0100 ?*/
 257 _ANIL,                  /* 10 0 0101 ?*/
 258 _ANIL,                  /* 10 0 0110 ?*/
 259 _ANIL,                  /* 10 0 0111 ?*/
 260 _AENTRY(lwbrx, 4, 0),   /* 10 0 1000 */
 261 _ANIL,                  /* 10 0 1001 ?*/
 262 _AENTRY(stwbrx, 0, 4),  /* 10 0 1010 */
 263 _ANIL,                  /* 10 0 1011 */
 264 _AENTRY(lhbrx, 2, 0),   /* 10 0 1110 */
 265 _ANIL,                  /* 10 0 1101 ?*/
 266 _AENTRY(sthbrx, 0, 2),  /* 10 0 1110 */
 267 _ANIL,                  /* 10 0 1111 ?*/
 268 _ANIL,                  /* 10 1 0000 ?*/
 269 _ANIL,                  /* 10 1 0001 ?*/
 270 _ANIL,                  /* 10 1 0010 ?*/
 271 _ANIL,                  /* 10 1 0011 ?*/
 272 _ANIL,                  /* 10 1 0100 - eciwx */
 273 _ANIL,                  /* 10 1 0101 ?*/
 274 _ANIL,                  /* 10 1 0110 - ecowx */
 275 _ANIL,                  /* 10 1 0111 ?*/
 276 _ANIL,                  /* 10 1 1000 ?*/
 277 _ANIL,                  /* 10 1 1001 ?*/
 278 _ANIL,                  /* 10 1 1010 ?*/
 279 _ANIL,                  /* 10 1 1011 ?*/
 280 _ANIL,                  /* 10 1 1100 ?*/
 281 _ANIL,                  /* 10 1 1101 ?*/
 282 _ANIL,                  /* 10 1 1110 ?*/
 283 _AENTRY(dcbz, 0, 0),    /* 10 1 1111 */
 284 };
 285
 286 struct ppc_align_instruction align_table11[] = {
 287 _AENTRY(lwzx, 4, 0),    /* 11 0 0000 */
 288 _ANIL,                  /* 11 0 0001 ?*/
 289 _AENTRY(stwx, 0, 4),    /* 11 0 0010 */
 290 _ANIL,                  /* 11 0 0011 */
 291 _AENTRY(lhzx, 2, 0),    /* 11 0 0100 */
 292 _AENTRY(lhax, 2, 0),    /* 11 0 0101 */
 293 _AENTRY(sthx, 0, 2),    /* 11 0 0110 */
 294 _ANIL,                  /* 11 0 0111?*/
 295 _AFENTRY(lfsx, 4, 0),   /* 11 0 1000 */
 296 _AFENTRY(lfdx, 8, 0),   /* 11 0 1001 */
 297 _AFENTRY(stfsx, 0, 4),  /* 11 0 1010 */
 298 _AFENTRY(stfdx, 0, 8),  /* 11 0 1011 */
 299 _ANIL,                  /* 11 0 1100 ?*/
 300 _ANIL,                  /* 11 0 1101 ?*/
 301 _ANIL,                  /* 11 0 1110 ?*/
 302 _ANIL,                  /* 11 0 1111 - stfiwx */
 303 _AENTRY(lwzux, 4, 0),   /* 11 1 0000 */
 304 _ANIL,                  /* 11 1 0001 ?*/
 305 _AENTRY(stwux, 0, 4),   /* 11 1 0010 */
 306 _ANIL,                  /* 11 1 0011 */
 307 _AENTRY(lhzux, 4, 0),   /* 11 1 0100 */
 308 _AENTRY(lhaux, 4, 0),   /* 11 1 0101 */
 309 _AENTRY(sthux, 0, 4),   /* 11 1 0110 */
 310 _ANIL,                  /* 11 1 0111 ?*/
 311 _AFENTRY(lfsux, 4, 0),  /* 11 1 1000 */
 312 _AFENTRY(lfdux, 8, 0),  /* 11 1 1001 */
 313 _AFENTRY(stfsux, 0, 4), /* 11 1 1010 */
 314 _AFENTRY(stfdux, 0, 8), /* 11 1 1011 */
 315 };
 316
 317
 318 struct ppc_align_instruction_table {
 319         struct ppc_align_instruction    *table;
 320         int                             size;
 321 } align_tables[4] = {
 322         align_table00,  sizeof(align_table00)/
 323                         sizeof(struct ppc_align_instruction),
 324
 325         align_table01,  sizeof(align_table01)/
 326                         sizeof(struct ppc_align_instruction),
 327
 328         align_table10,  sizeof(align_table10)/
 329                         sizeof(struct ppc_align_instruction),
 330
 331         align_table11,  sizeof(align_table11)/
 332                         sizeof(struct ppc_align_instruction)
 333 };
 334
 335 extern int              real_ncpus;                                             /* Number of actual CPUs */
 336
 337 /*
 338  * Alignment Exception Handler
 339  *
 340  *
 341  * This handler is called when the chip attempts
 342  * to execute an instruction which causes page
 343  * boundaries to be crossed. Typically, this will
 344  * happen on stfd* and lfd* instructions.
 345  * (A request has been made for GNU C compiler
 346  * NOT to make use of these instructions to
 347  * load and store 8 bytes at a time.)
 348  *
 349  * This is a *SLOW* handler. There is room for vast
 350  * improvement. However, it is expected that alignment
 351  * exceptions will be very infrequent.
 352  *
 353  * Not all of the 64 instructions (as listed in
 354  * PowerPC Microprocessor Family book under the Alignment
 355  * Exception section) are handled yet.
 356  * Only the most common ones which are expected to
 357  * happen.
 358  *
 359  * -- Michael Burg, Apple Computer, Inc. 1996
 360  *
 361  * TODO NMGS finish handler
 362  */
 363
 364 boolean_t
 365 alignment(unsigned long dsisr, unsigned long dar,
 366                struct ppc_saved_state *ssp)
 367 {
 368         struct ppc_align_instruction_table      *table;
 369         struct ppc_align_instruction            *entry;
 370         struct ppc_float_state                          *fpc;
 371         unsigned long           align_buffer[32];
 372         boolean_t               success = FALSE;
 373         thread_act_t act;
 374         spl_t                   s;
 375         int i;
 376
 377 #if     DEBUG
 378         if (USER_MODE(ssp->srr1))       (void)hw_atomic_add(&alignment_exception_count_user, 1);
 379         else                                            (void)hw_atomic_add(&alignment_exception_count_kernel, 1);
 380 #endif
 381
 382         act = current_act();                                            /* Get the current activation */
 383
 384         table = &align_tables[DSISR_BITS_15_16(dsisr)];
 385
 386         if (table == (void *) 0
 387         || table->size < DSISR_BITS_17_21(dsisr)) {
 388 #if     DEBUG
 389                 printf("EXCEPTION NOT HANDLED: Out of range.\n");
 390                 printf("dsisr=%X, dar=%X\n",dsisr, dar);
 391                 printf("table=%X\n",DSISR_BITS_15_16(dsisr));
 392                 printf("table->size=%X\n", table->size);
 393                 printf("entry=%X\n",DSISR_BITS_17_21(dsisr));
 394 #endif
 395                 goto out;
 396         }
 397
 398         entry = &table->table[DSISR_BITS_17_21(dsisr)];
 399
 400         if (entry->a_instruct == (void *) 0) {
 401 #if     DEBUG
 402                 printf("EXCEPTION NOT HANDLED: Inst out of table range.\n");
 403                 printf("table=%X\n",DSISR_BITS_15_16(dsisr));
 404                 printf("entry=%X\n",DSISR_BITS_17_21(dsisr));
 405 #endif
 406                 goto out;
 407         }
 408
 409         /*
 410          * Check to see if the instruction is a
 411          * floating point operation. Save off
 412          * the FPU register set ...
 413          */
 414
 415         if (entry->a_is_float)
 416                 fpu_save(act);
 417
 418         /*
 419          * Pull in any bytes which are going to be
 420          * read.
 421          */
 422
 423         if (entry->a_readbytes) {
 424                 if (USER_MODE(ssp->srr1)) {
 425                         if (copyin((char *) dar,
 426                                    (char *) align_buffer,
 427                                    entry->a_readbytes)) {
 428                                 return  TRUE;
 429                         }
 430                 } else {
 431                         bcopy((char *) dar,
 432                               (char *) align_buffer,
 433                               entry->a_readbytes);
 434                 }
 435         }
 436
 437 #if     0 && DEBUG
 438         printf("Alignment exception: %s %d,0x%x (r%d/w%d) (tmp %x/%x)\n",
 439                entry->name, DSISR_BITS_REG(dsisr),
 440                dar, entry->a_readbytes, entry->a_writebytes,
 441                align_buffer[0], align_buffer[1]);
 442         printf("    pc=(0x%08X), msr=(0x%X)",ssp->srr0, ssp->srr1);
 443 #endif
 444
 445
 446         success = entry->a_instruct(dsisr,
 447                                     ssp,
 448                                         (entry->a_is_float ? find_user_fpu(act) : 0),   /* Find this user's FPU state if FP op */
 449                                     align_buffer,
 450                                     dar);
 451
 452         if (success) {
 453                 if (entry->a_writebytes) {
 454                         if (USER_MODE(ssp->srr1)) {
 455                                 if (copyout((char *) align_buffer,
 456                                             (char *) dar,
 457                                             entry->a_writebytes)) {
 458                                         return  TRUE;
 459                                 }
 460                         } else {
 461                                 bcopy((char *) align_buffer,
 462                                       (char *) dar,
 463                                       entry->a_writebytes);
 464                         }
 465                 }
 466                 else {
 467                         if(entry->a_is_float) {                         /* If we are an FP op, blow away live context */
 468                                 for(i=0; i < real_ncpus; i++) { /* Cycle through processors */
 469                                         (void)hw_compare_and_store((unsigned int)act, 0, &per_proc_info[i].FPU_thread); /* Clear if ours */
 470                                 }
 471                         }
 472
 473                         if (USER_MODE(ssp->srr1)) {
 474                                 if (copyout((char *) align_buffer,
 475                                             (char *) dar,
 476                                             entry->a_writebytes)) {
 477                                         return  TRUE;
 478                                 }
 479                         } else {
 480                                 bcopy((char *) align_buffer,
 481                                       (char *) dar,
 482                                       entry->a_writebytes);
 483                         }
 484                 }
 485
 486                 ssp->srr0 += 4; /* Skip the instruction .. */
 487         }
 488
 489         return  !success;
 490
 491 out:
 492 #if     0 && DEBUG
 493         printf("ALIGNMENT EXCEPTION: (dsisr 0x%x) table %d 0x%x\n",
 494                 dsisr, DSISR_BITS_15_16(dsisr), DSISR_BITS_17_21(dsisr));
 495 #endif
 496
 497         return  TRUE;
 498 }
 499
 500 _AINST(lwz)
 501 {
 502         SET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned long);
 503
 504         return  TRUE;
 505 }
 506
 507 _AINST(stw)
 508 {
 509         GET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned long);
 510
 511         return  TRUE;
 512 }
 513
 514 _AINST(lhz)
 515 {
 516         unsigned long value = *((unsigned short *) align_buffer);
 517
 518         SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
 519
 520         return  TRUE;
 521 }
 522
 523 _AINST(lha)
 524 {
 525         long value = *((short *) align_buffer);
 526
 527         SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
 528
 529         return  TRUE;
 530 }
 531
 532 _AINST(sth)
 533 {
 534         GET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned short);
 535
 536         return  TRUE;
 537 }
 538
 539 _AINST(lmw)
 540 {
 541     int i;
 542
 543     for (i = 0; i < (32-DSISR_BITS_REG(dsisr)); i++)
 544     {
 545         SET_REG(ssp, DSISR_BITS_REG(dsisr)+i, &align_buffer[i], unsigned long);
 546     }
 547     return TRUE;
 548 }
 549
 550 struct fpsp {
 551         unsigned long   s       :1;     /* Sign bit */
 552         unsigned long   exp     :8;     /* exponent + bias */
 553         unsigned long   fraction:23;    /* fraction */
 554 };
 555 typedef struct fpsp fpsp_t, *fpspPtr;
 556
 557 struct fpdp {
 558         unsigned long   s       :1;     /* Sign bit */
 559         unsigned long   exp     :11;    /* exponent + bias */
 560         unsigned long   fraction:20;    /* fraction */
 561         unsigned long   fraction1;      /* fraction */
 562 };
 563 typedef struct fpdp fpdp_t, *fpdpPtr;
 564
 565
 566 _AINST(lfs)
 567 {
 568         unsigned long   lalign_buf[2];
 569
 570
 571         lfs (align_buffer, lalign_buf);
 572         SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
 573         return  TRUE;
 574 }
 575
 576 _AINST(lfd)
 577 {
 578         SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
 579         return  TRUE;
 580 }
 581
 582 _AINST(stfs)
 583 {
 584         unsigned long   lalign_buf[2];
 585
 586
 587         GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
 588         stfs(lalign_buf, align_buffer);
 589         return TRUE;
 590 }
 591
 592 _AINST(stfd)
 593 {
 594         GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
 595         return TRUE;
 596 }
 597
 598 _AINST(lwzu)
 599 {
 600         SET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned long)
 601         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 602         return TRUE;
 603 }
 604
 605 _AINST(stwu)
 606 {
 607         GET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned long)
 608         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 609         return TRUE;
 610 }
 611
 612
 613 _AINST(lhzu)
 614 {
 615         SET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned short)
 616         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 617         return TRUE;
 618 }
 619
 620 _AINST(lhau)
 621 {
 622         unsigned long   value = *((short *) align_buffer);
 623
 624         SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
 625         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 626
 627         return  TRUE;
 628 }
 629
 630 _AINST(sthu)
 631 {
 632         GET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned short)
 633         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 634         return  TRUE;
 635 }
 636
 637 _AINST(stmw)
 638 {
 639     int i, rS = DSISR_BITS_REG(dsisr);
 640     int numRegs = 32 - rS;
 641     int numBytes = numRegs * 4;
 642     int retval;
 643
 644
 645     for (i = 0; i < numRegs; i++)
 646     {
 647 #if 0
 648         printf("    align_buffer[%d] == 0x%x\n",i,align_buffer[i]);
 649 #endif
 650         GET_REG(ssp, rS+i, &align_buffer[i], unsigned long);
 651 #if 0
 652         printf("    now align_buffer[%d] == 0x%x\n",i,align_buffer[i]);
 653 #endif
 654     }
 655     if (USER_MODE(ssp->srr1)) {
 656         if ((retval=copyout((char *)align_buffer,(char *)dar,numBytes)) != 0) {
 657             return FALSE;
 658         }
 659 #if 0
 660         printf("    copyout(%X, %X, %X) succeeded\n",align_buffer,dar,numBytes);
 661 #endif
 662     }
 663     else {
 664         bcopy((char *) align_buffer, (char *) dar, numBytes);
 665     }
 666     return TRUE;
 667 }
 668
 669 _AINST(lfsu)
 670 {
 671         unsigned long   lalign_buf[2];
 672
 673
 674         lfs (align_buffer, lalign_buf);
 675         SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
 676         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 677         return  TRUE;
 678 }
 679
 680 _AINST(lfdu)
 681 {
 682         SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
 683         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 684
 685         return  TRUE;
 686 }
 687
 688 _AINST(stfsu)
 689 {
 690         unsigned long   lalign_buf[2];
 691
 692
 693         GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
 694         stfs(lalign_buf, align_buffer);
 695         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 696         return  TRUE;
 697 }
 698
 699
 700 _AINST(stfdu)
 701 {
 702         GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
 703         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 704
 705         return  TRUE;
 706 }
 707
 708 _AINST(lswx)
 709 {
 710     int i, nb, nr, inst, zero = 0;
 711
 712
 713     /* check for invalid form of instruction */
 714     if (DSISR_BITS_RA(dsisr) >= DSISR_BITS_REG(dsisr) )
 715         return FALSE;
 716
 717     if (USER_MODE(ssp->srr1)) {
 718         if (copyin((char *) ssp->srr0, (char *) &inst, 4 )) {
 719         return  FALSE;
 720         }
 721     } else {
 722         bcopy((char *) ssp->srr0, (char *) &inst, 4 );
 723     }
 724
 725     nb = (inst >> 11) & 0x1F;   /* get the number of bytes in the instr */
 726     nr = (nb + sizeof(long)-1) / sizeof(long);/* get the number of regs to copy */
 727
 728     if ((nr + DSISR_BITS_REG(dsisr)) > 31)
 729         return FALSE;           /* not supported yet */
 730
 731     for (i = 0; i < nr; i++)
 732     {
 733         SET_REG(ssp, DSISR_BITS_REG(dsisr)+i, &zero, unsigned long);
 734     }
 735     /* copy the string into the save state */
 736     bcopy((char *) align_buffer, (char *) ssp->r0+DSISR_BITS_REG(dsisr), nb );
 737     return TRUE;
 738 }
 739
 740 _AINST(lswi)
 741 {
 742     int i, nb, nr, inst, zero = 0;
 743
 744
 745     /* check for invalid form of instruction */
 746     if (DSISR_BITS_RA(dsisr) >= DSISR_BITS_REG(dsisr) )
 747         return FALSE;
 748
 749     if (USER_MODE(ssp->srr1)) {
 750         if (copyin((char *) ssp->srr0, (char *) &inst, 4 )) {
 751         return  FALSE;
 752         }
 753     } else {
 754         bcopy((char *) ssp->srr0, (char *) &inst, 4 );
 755     }
 756
 757     nb = (inst >> 11) & 0x1F;   /* get the number of bytes in the instr */
 758     nr = (nb + sizeof(long)-1) / sizeof(long);/* get the number of regs to copy */
 759
 760     if ((nr + DSISR_BITS_REG(dsisr)) > 31)
 761         return FALSE;           /* not supported yet */
 762
 763     for (i = 0; i < nr; i++)
 764     {
 765         SET_REG(ssp, DSISR_BITS_REG(dsisr)+i, &zero, unsigned long);
 766     }
 767     /* copy the string into the save state */
 768     bcopy((char *) align_buffer, (char *) ssp->r0+DSISR_BITS_REG(dsisr), nb );
 769     return TRUE;
 770 }
 771
 772 _AINST(stswx)
 773 {
 774         return  FALSE;
 775 }
 776
 777 _AINST(stswi)
 778 {
 779         return  FALSE;
 780 }
 781
 782
 783
 784
 785
 786
 787
 788 _AINST(stwcx)
 789 {
 790         return  FALSE;
 791 }
 792
 793 _AINST(stdcx)
 794 {
 795         return  FALSE;
 796 }
 797
 798 _AINST(lwbrx)
 799 {
 800         unsigned long   new_value;
 801
 802         __asm__ volatile("lwbrx %0,0,%1" : : "b" (new_value),
 803                         "b" (&align_buffer[0]));
 804
 805         SET_REG(ssp, DSISR_BITS_REG(dsisr), &new_value, unsigned long);
 806
 807         return  TRUE;
 808 }
 809
 810 _AINST(stwbrx)
 811 {
 812         unsigned long   value;
 813
 814         GET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
 815         __asm__ volatile("stwbrx        %0,0,%1" : : "b" (value), "b" (&align_buffer[0]));
 816
 817         return  TRUE;
 818 }
 819
 820 _AINST(lhbrx)
 821 {
 822         unsigned short  value;
 823
 824         __asm__ volatile("lhbrx %0,0,%1" : : "b" (value), "b" (&align_buffer[0]));
 825
 826         SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned short);
 827
 828         return  TRUE;
 829 }
 830
 831 _AINST(sthbrx)
 832 {
 833         unsigned short value;
 834
 835         GET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned short);
 836         __asm__ volatile("sthbrx %0,0,%1" : : "b" (value), "b" (&align_buffer[0]));
 837
 838         return  TRUE;
 839 }
 840
 841 _AINST(eciwx)
 842 {
 843         return  FALSE;
 844 }
 845
 846 _AINST(ecowx)
 847 {
 848         return  FALSE;
 849 }
 850
 851 _AINST(dcbz)
 852 {
 853     long *alignedDAR = (long *)((long)dar & ~(CACHE_LINE_SIZE-1));
 854
 855
 856     if (USER_MODE(ssp->srr1)) {
 857
 858             align_buffer[0] = 0;
 859             align_buffer[1] = 0;
 860             align_buffer[2] = 0;
 861             align_buffer[3] = 0;
 862             align_buffer[4] = 0;
 863             align_buffer[5] = 0;
 864             align_buffer[6] = 0;
 865             align_buffer[7] = 0;
 866
 867         if (copyout((char *)align_buffer,(char *)alignedDAR,CACHE_LINE_SIZE) != 0)
 868             return FALSE;
 869     } else {
 870             /* Cannot use bcopy here just in case it caused the exception */
 871             alignedDAR[0] = 0;
 872             alignedDAR[1] = 0;
 873             alignedDAR[2] = 0;
 874             alignedDAR[3] = 0;
 875             alignedDAR[4] = 0;
 876             alignedDAR[5] = 0;
 877             alignedDAR[6] = 0;
 878             alignedDAR[7] = 0;
 879     }
 880     return      TRUE;
 881 }
 882
 883
 884
 885
 886
 887
 888
 889 _AINST(lwzx)
 890 {
 891         SET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned long);
 892
 893         return  TRUE;
 894 }
 895
 896 _AINST(stwx)
 897 {
 898         GET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned long);
 899
 900         return  TRUE;
 901 }
 902
 903 _AINST(lhzx)
 904 {
 905         SET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned short);
 906
 907         return  TRUE;
 908 }
 909
 910 _AINST(lhax)
 911 {
 912         unsigned long   value   = *((short *) &align_buffer[0]);
 913
 914         SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
 915
 916         return  TRUE;
 917 }
 918
 919 _AINST(sthx)
 920 {
 921         GET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned short);
 922
 923         return  TRUE;
 924 }
 925
 926 _AINST(lfsx)
 927 {
 928         long    lalign_buf[2];
 929
 930
 931         lfs (align_buffer, lalign_buf);
 932         SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
 933         return  TRUE;
 934 }
 935
 936 _AINST(lfdx)
 937 {
 938         SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
 939
 940         return  TRUE;
 941 }
 942
 943 _AINST(stfsx)
 944 {
 945         long    lalign_buf[2];
 946
 947
 948         GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
 949         stfs(lalign_buf, align_buffer);
 950         return  TRUE;
 951 }
 952
 953 _AINST(stfdx)
 954 {
 955         GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
 956
 957         return  TRUE;
 958 }
 959
 960 _AINST(lwzux)
 961 {
 962         SET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned long);
 963         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 964
 965         return  TRUE;
 966 }
 967
 968 _AINST(stwux)
 969 {
 970         GET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned long);
 971         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 972
 973         return  TRUE;
 974 }
 975
 976 _AINST(lhzux)
 977 {
 978         unsigned long value = *((unsigned short *)&align_buffer[0]);
 979
 980         SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
 981         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 982
 983         return  TRUE;
 984 }
 985
 986 _AINST(lhaux)
 987 {
 988         long value = *((short *) &align_buffer[0]);
 989
 990         SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
 991         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
 992
 993         return  TRUE;
 994 }
 995
 996 _AINST(sthux)
 997 {
 998         GET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned short);
 999         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
1000
1001         return  TRUE;
1002 }
1003
1004 _AINST(lfsux)
1005 {
1006         long    lalign_buf[2];
1007
1008
1009         lfs (align_buffer, lalign_buf);
1010         SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
1011         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
1012         return  TRUE;
1013 }
1014
1015 _AINST(lfdux)
1016 {
1017         SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), &align_buffer[0]);
1018         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
1019
1020         return  TRUE;
1021 }
1022
1023
1024 _AINST(stfsux)
1025 {
1026         long    lalign_buf[2];
1027
1028
1029         GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
1030         stfs(lalign_buf, align_buffer);
1031         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
1032         return  TRUE;
1033 }
1034
1035 _AINST(stfdux)
1036 {
1037         GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), &align_buffer[0]);
1038         SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
1039
1040         return  TRUE;
1041 }