#include <debug.h>
#include <kern/thread_act.h>
#include <mach/thread_status.h>
#include <mach/boolean.h>
#include <kern/misc_protos.h>
#include <kern/simple_lock.h>
#include <ppc/proc_reg.h>
#include <ppc/fpu_protos.h>
#include <ppc/misc_protos.h>
#include <ppc/exception.h>
#if DEBUG
int alignment_exception_count_user;
int alignment_exception_count_kernel;
#endif
#define _AINST(x) boolean_t align_##x##(unsigned long dsisr,\
struct ppc_saved_state *ssp, \
struct ppc_float_state *fsp, \
unsigned long *align_buffer, \
unsigned long dar)
#define _AFENTRY(name, r, b) { #name, align_##name##, r, b, TRUE }
#define _AENTRY(name, r, b) { #name, align_##name##, r, b, FALSE }
#define _ANIL { (void *) 0, (void *) 0, 0, 0 }
_AINST(lwz);
_AINST(stw);
_AINST(lhz);
_AINST(lha);
_AINST(sth);
_AINST(lmw);
_AINST(lfs);
_AINST(lfd);
_AINST(stfs);
_AINST(stfd);
_AINST(lwzu);
_AINST(stwu);
_AINST(lhzu);
_AINST(lhau);
_AINST(sthu);
_AINST(lfsu);
_AINST(lfdu);
_AINST(stfsu);
_AINST(stfdu);
_AINST(lswx);
_AINST(lswi);
_AINST(lwbrx);
_AINST(stwbrx);
_AINST(lhbrx);
_AINST(sthbrx);
_AINST(dcbz);
_AINST(lwzx);
_AINST(stwx);
_AINST(lhzx);
_AINST(lhax);
_AINST(sthx);
_AINST(lfsx);
_AINST(lfdx);
_AINST(stfsx);
_AINST(stfdx);
_AINST(lwzux);
_AINST(stwux);
_AINST(lhzux);
_AINST(lhaux);
_AINST(sthux);
_AINST(stmw);
_AINST(lfsux);
_AINST(lfdux);
_AINST(stfsux);
_AINST(stfdux);
void GET_FPU_REG(struct ppc_float_state *fsp,
unsigned long reg,
unsigned long *value);
void SET_FPU_REG(struct ppc_float_state *fsp,
unsigned long reg,
unsigned long *value);
__inline__ void GET_FPU_REG(struct ppc_float_state *fsp,
unsigned long reg,
unsigned long *value)
{
value[0] = ((unsigned long *) &fsp->fpregs[reg])[0];
value[1] = ((unsigned long *) &fsp->fpregs[reg])[1];
}
__inline__ void SET_FPU_REG(struct ppc_float_state *fsp,
unsigned long reg, unsigned long *value)
{
((unsigned long *) &fsp->fpregs[reg])[0] = value[0];
((unsigned long *) &fsp->fpregs[reg])[1] = value[1];
}
#define GET_REG(p, reg, value, cast) \
{ *((cast *) value) = *((cast *) (&p->r0+reg)); }
#define SET_REG(p, reg, value, cast) \
{ *((cast *) (&p->r0+reg)) = *((cast *) value); }
#define DSISR_BITS_15_16(bits) ((bits>>15) & 0x3)
#define DSISR_BITS_17_21(bits) ((bits>>10) & 0x1f)
#define DSISR_BITS_REG(bits) ((bits>>5) & 0x1f)
#define DSISR_BITS_RA(bits) (bits & 0x1f)
struct ppc_align_instruction {
char *name;
boolean_t (*a_instruct)(unsigned long,
struct ppc_saved_state *,
struct ppc_float_state *,
unsigned long *,
unsigned long );
int a_readbytes;
int a_writebytes;
boolean_t a_is_float;
} align_table00[] = {
_AENTRY(lwz, 4, 0),
_ANIL,
_AENTRY(stw, 0, 4),
_ANIL,
_AENTRY(lhz, 2, 0),
_AENTRY(lha, 2, 0),
_AENTRY(sth, 0, 2),
_AENTRY(lmw, 32*4,0),
_AFENTRY(lfs, 4, 0),
_AFENTRY(lfd, 8, 0),
_AFENTRY(stfs, 0, 4),
_AFENTRY(stfd, 0, 8),
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_AENTRY(lwzu, 4, 0),
_ANIL,
_AENTRY(stwu, 0, 4),
_ANIL,
_AENTRY(lhzu, 2, 0),
_AENTRY(lhau, 2, 0),
_AENTRY(sthu, 0, 2),
_AENTRY(stmw, 0, 0),
_AFENTRY(lfsu, 4, 0),
_AFENTRY(lfdu, 8, 0),
_AFENTRY(stfsu, 0, 4),
_AFENTRY(stfdu, 0, 8),
};
struct ppc_align_instruction align_table01[] = {
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_AENTRY(lswx,32, 0),
_AENTRY(lswi,32, 0),
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
};
struct ppc_align_instruction align_table10[] = {
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_AENTRY(lwbrx, 4, 0),
_ANIL,
_AENTRY(stwbrx, 0, 4),
_ANIL,
_AENTRY(lhbrx, 2, 0),
_ANIL,
_AENTRY(sthbrx, 0, 2),
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_AENTRY(dcbz, 0, 0),
};
struct ppc_align_instruction align_table11[] = {
_AENTRY(lwzx, 4, 0),
_ANIL,
_AENTRY(stwx, 0, 4),
_ANIL,
_AENTRY(lhzx, 2, 0),
_AENTRY(lhax, 2, 0),
_AENTRY(sthx, 0, 2),
_ANIL,
_AFENTRY(lfsx, 4, 0),
_AFENTRY(lfdx, 8, 0),
_AFENTRY(stfsx, 0, 4),
_AFENTRY(stfdx, 0, 8),
_ANIL,
_ANIL,
_ANIL,
_ANIL,
_AENTRY(lwzux, 4, 0),
_ANIL,
_AENTRY(stwux, 0, 4),
_ANIL,
_AENTRY(lhzux, 4, 0),
_AENTRY(lhaux, 4, 0),
_AENTRY(sthux, 0, 4),
_ANIL,
_AFENTRY(lfsux, 4, 0),
_AFENTRY(lfdux, 8, 0),
_AFENTRY(stfsux, 0, 4),
_AFENTRY(stfdux, 0, 8),
};
struct ppc_align_instruction_table {
struct ppc_align_instruction *table;
int size;
} align_tables[4] = {
align_table00, sizeof(align_table00)/
sizeof(struct ppc_align_instruction),
align_table01, sizeof(align_table01)/
sizeof(struct ppc_align_instruction),
align_table10, sizeof(align_table10)/
sizeof(struct ppc_align_instruction),
align_table11, sizeof(align_table11)/
sizeof(struct ppc_align_instruction)
};
extern int real_ncpus;
boolean_t
alignment(unsigned long dsisr, unsigned long dar,
struct ppc_saved_state *ssp)
{
struct ppc_align_instruction_table *table;
struct ppc_align_instruction *entry;
struct ppc_float_state *fpc;
unsigned long align_buffer[32];
boolean_t success = FALSE;
thread_act_t act;
spl_t s;
int i;
#if DEBUG
if (USER_MODE(ssp->srr1)) (void)hw_atomic_add(&alignment_exception_count_user, 1);
else (void)hw_atomic_add(&alignment_exception_count_kernel, 1);
#endif
act = current_act();
table = &align_tables[DSISR_BITS_15_16(dsisr)];
if (table == (void *) 0
|| table->size < DSISR_BITS_17_21(dsisr)) {
#if DEBUG
printf("EXCEPTION NOT HANDLED: Out of range.\n");
printf("dsisr=%X, dar=%X\n",dsisr, dar);
printf("table=%X\n",DSISR_BITS_15_16(dsisr));
printf("table->size=%X\n", table->size);
printf("entry=%X\n",DSISR_BITS_17_21(dsisr));
#endif
goto out;
}
entry = &table->table[DSISR_BITS_17_21(dsisr)];
if (entry->a_instruct == (void *) 0) {
#if DEBUG
printf("EXCEPTION NOT HANDLED: Inst out of table range.\n");
printf("table=%X\n",DSISR_BITS_15_16(dsisr));
printf("entry=%X\n",DSISR_BITS_17_21(dsisr));
#endif
goto out;
}
if (entry->a_is_float)
fpu_save(act);
if (entry->a_readbytes) {
if (USER_MODE(ssp->srr1)) {
if (copyin((char *) dar,
(char *) align_buffer,
entry->a_readbytes)) {
return TRUE;
}
} else {
bcopy((char *) dar,
(char *) align_buffer,
entry->a_readbytes);
}
}
#if 0 && DEBUG
printf("Alignment exception: %s %d,0x%x (r%d/w%d) (tmp %x/%x)\n",
entry->name, DSISR_BITS_REG(dsisr),
dar, entry->a_readbytes, entry->a_writebytes,
align_buffer[0], align_buffer[1]);
printf(" pc=(0x%08X), msr=(0x%X)",ssp->srr0, ssp->srr1);
#endif
success = entry->a_instruct(dsisr,
ssp,
(entry->a_is_float ? find_user_fpu(act) : 0),
align_buffer,
dar);
if (success) {
if (entry->a_writebytes) {
if (USER_MODE(ssp->srr1)) {
if (copyout((char *) align_buffer,
(char *) dar,
entry->a_writebytes)) {
return TRUE;
}
} else {
bcopy((char *) align_buffer,
(char *) dar,
entry->a_writebytes);
}
}
else {
if(entry->a_is_float) {
for(i=0; i < real_ncpus; i++) {
(void)hw_compare_and_store((unsigned int)act, 0, &per_proc_info[i].FPU_thread);
}
}
if (USER_MODE(ssp->srr1)) {
if (copyout((char *) align_buffer,
(char *) dar,
entry->a_writebytes)) {
return TRUE;
}
} else {
bcopy((char *) align_buffer,
(char *) dar,
entry->a_writebytes);
}
}
ssp->srr0 += 4;
}
return !success;
out:
#if 0 && DEBUG
printf("ALIGNMENT EXCEPTION: (dsisr 0x%x) table %d 0x%x\n",
dsisr, DSISR_BITS_15_16(dsisr), DSISR_BITS_17_21(dsisr));
#endif
return TRUE;
}
_AINST(lwz)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned long);
return TRUE;
}
_AINST(stw)
{
GET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned long);
return TRUE;
}
_AINST(lhz)
{
unsigned long value = *((unsigned short *) align_buffer);
SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
return TRUE;
}
_AINST(lha)
{
long value = *((short *) align_buffer);
SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
return TRUE;
}
_AINST(sth)
{
GET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned short);
return TRUE;
}
_AINST(lmw)
{
int i;
for (i = 0; i < (32-DSISR_BITS_REG(dsisr)); i++)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr)+i, &align_buffer[i], unsigned long);
}
return TRUE;
}
struct fpsp {
unsigned long s :1;
unsigned long exp :8;
unsigned long fraction:23;
};
typedef struct fpsp fpsp_t, *fpspPtr;
struct fpdp {
unsigned long s :1;
unsigned long exp :11;
unsigned long fraction:20;
unsigned long fraction1;
};
typedef struct fpdp fpdp_t, *fpdpPtr;
_AINST(lfs)
{
unsigned long lalign_buf[2];
lfs (align_buffer, lalign_buf);
SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
return TRUE;
}
_AINST(lfd)
{
SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
return TRUE;
}
_AINST(stfs)
{
unsigned long lalign_buf[2];
GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
stfs(lalign_buf, align_buffer);
return TRUE;
}
_AINST(stfd)
{
GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
return TRUE;
}
_AINST(lwzu)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned long)
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(stwu)
{
GET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned long)
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(lhzu)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned short)
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(lhau)
{
unsigned long value = *((short *) align_buffer);
SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(sthu)
{
GET_REG(ssp, DSISR_BITS_REG(dsisr), align_buffer, unsigned short)
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(stmw)
{
int i, rS = DSISR_BITS_REG(dsisr);
int numRegs = 32 - rS;
int numBytes = numRegs * 4;
int retval;
for (i = 0; i < numRegs; i++)
{
#if 0
printf(" align_buffer[%d] == 0x%x\n",i,align_buffer[i]);
#endif
GET_REG(ssp, rS+i, &align_buffer[i], unsigned long);
#if 0
printf(" now align_buffer[%d] == 0x%x\n",i,align_buffer[i]);
#endif
}
if (USER_MODE(ssp->srr1)) {
if ((retval=copyout((char *)align_buffer,(char *)dar,numBytes)) != 0) {
return FALSE;
}
#if 0
printf(" copyout(%X, %X, %X) succeeded\n",align_buffer,dar,numBytes);
#endif
}
else {
bcopy((char *) align_buffer, (char *) dar, numBytes);
}
return TRUE;
}
_AINST(lfsu)
{
unsigned long lalign_buf[2];
lfs (align_buffer, lalign_buf);
SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(lfdu)
{
SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(stfsu)
{
unsigned long lalign_buf[2];
GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
stfs(lalign_buf, align_buffer);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(stfdu)
{
GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(lswx)
{
int i, nb, nr, inst, zero = 0;
if (DSISR_BITS_RA(dsisr) >= DSISR_BITS_REG(dsisr) )
return FALSE;
if (USER_MODE(ssp->srr1)) {
if (copyin((char *) ssp->srr0, (char *) &inst, 4 )) {
return FALSE;
}
} else {
bcopy((char *) ssp->srr0, (char *) &inst, 4 );
}
nb = (inst >> 11) & 0x1F;
nr = (nb + sizeof(long)-1) / sizeof(long);
if ((nr + DSISR_BITS_REG(dsisr)) > 31)
return FALSE;
for (i = 0; i < nr; i++)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr)+i, &zero, unsigned long);
}
bcopy((char *) align_buffer, (char *) ssp->r0+DSISR_BITS_REG(dsisr), nb );
return TRUE;
}
_AINST(lswi)
{
int i, nb, nr, inst, zero = 0;
if (DSISR_BITS_RA(dsisr) >= DSISR_BITS_REG(dsisr) )
return FALSE;
if (USER_MODE(ssp->srr1)) {
if (copyin((char *) ssp->srr0, (char *) &inst, 4 )) {
return FALSE;
}
} else {
bcopy((char *) ssp->srr0, (char *) &inst, 4 );
}
nb = (inst >> 11) & 0x1F;
nr = (nb + sizeof(long)-1) / sizeof(long);
if ((nr + DSISR_BITS_REG(dsisr)) > 31)
return FALSE;
for (i = 0; i < nr; i++)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr)+i, &zero, unsigned long);
}
bcopy((char *) align_buffer, (char *) ssp->r0+DSISR_BITS_REG(dsisr), nb );
return TRUE;
}
_AINST(stswx)
{
return FALSE;
}
_AINST(stswi)
{
return FALSE;
}
_AINST(stwcx)
{
return FALSE;
}
_AINST(stdcx)
{
return FALSE;
}
_AINST(lwbrx)
{
unsigned long new_value;
__asm__ volatile("lwbrx %0,0,%1" : : "b" (new_value),
"b" (&align_buffer[0]));
SET_REG(ssp, DSISR_BITS_REG(dsisr), &new_value, unsigned long);
return TRUE;
}
_AINST(stwbrx)
{
unsigned long value;
GET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
__asm__ volatile("stwbrx %0,0,%1" : : "b" (value), "b" (&align_buffer[0]));
return TRUE;
}
_AINST(lhbrx)
{
unsigned short value;
__asm__ volatile("lhbrx %0,0,%1" : : "b" (value), "b" (&align_buffer[0]));
SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned short);
return TRUE;
}
_AINST(sthbrx)
{
unsigned short value;
GET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned short);
__asm__ volatile("sthbrx %0,0,%1" : : "b" (value), "b" (&align_buffer[0]));
return TRUE;
}
_AINST(eciwx)
{
return FALSE;
}
_AINST(ecowx)
{
return FALSE;
}
_AINST(dcbz)
{
long *alignedDAR = (long *)((long)dar & ~(CACHE_LINE_SIZE-1));
if (USER_MODE(ssp->srr1)) {
align_buffer[0] = 0;
align_buffer[1] = 0;
align_buffer[2] = 0;
align_buffer[3] = 0;
align_buffer[4] = 0;
align_buffer[5] = 0;
align_buffer[6] = 0;
align_buffer[7] = 0;
if (copyout((char *)align_buffer,(char *)alignedDAR,CACHE_LINE_SIZE) != 0)
return FALSE;
} else {
alignedDAR[0] = 0;
alignedDAR[1] = 0;
alignedDAR[2] = 0;
alignedDAR[3] = 0;
alignedDAR[4] = 0;
alignedDAR[5] = 0;
alignedDAR[6] = 0;
alignedDAR[7] = 0;
}
return TRUE;
}
_AINST(lwzx)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned long);
return TRUE;
}
_AINST(stwx)
{
GET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned long);
return TRUE;
}
_AINST(lhzx)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned short);
return TRUE;
}
_AINST(lhax)
{
unsigned long value = *((short *) &align_buffer[0]);
SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
return TRUE;
}
_AINST(sthx)
{
GET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned short);
return TRUE;
}
_AINST(lfsx)
{
long lalign_buf[2];
lfs (align_buffer, lalign_buf);
SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
return TRUE;
}
_AINST(lfdx)
{
SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
return TRUE;
}
_AINST(stfsx)
{
long lalign_buf[2];
GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
stfs(lalign_buf, align_buffer);
return TRUE;
}
_AINST(stfdx)
{
GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), align_buffer);
return TRUE;
}
_AINST(lwzux)
{
SET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned long);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(stwux)
{
GET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned long);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(lhzux)
{
unsigned long value = *((unsigned short *)&align_buffer[0]);
SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(lhaux)
{
long value = *((short *) &align_buffer[0]);
SET_REG(ssp, DSISR_BITS_REG(dsisr), &value, unsigned long);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(sthux)
{
GET_REG(ssp, DSISR_BITS_REG(dsisr), &align_buffer[0], unsigned short);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(lfsux)
{
long lalign_buf[2];
lfs (align_buffer, lalign_buf);
SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(lfdux)
{
SET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), &align_buffer[0]);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(stfsux)
{
long lalign_buf[2];
GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), lalign_buf);
stfs(lalign_buf, align_buffer);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}
_AINST(stfdux)
{
GET_FPU_REG(fsp, DSISR_BITS_REG(dsisr), &align_buffer[0]);
SET_REG(ssp, DSISR_BITS_RA(dsisr), &dar, unsigned long);
return TRUE;
}