ref: 39bf5b987e8ab53a74adec5a15b30f4eda85b357
dir: /sys/src/cmd/5e/arm.c/
#include <u.h>
#include <libc.h>
#include <thread.h>
#include <bio.h>
#include <mach.h>
#include "dat.h"
#include "fns.h"
enum {
fI = 1<<25,
fP = 1<<24,
fLi = 1<<24,
fU = 1<<23,
fB = 1<<22,
fW = 1<<21,
fL = 1<<20,
fS = 1<<20,
fSg = 1<<6,
fH = 1<<5,
};
void
invalid(u32int instr)
{
suicide("undefined instruction %8ux @ %8ux", instr, P->R[15] - 4);
}
u32int
evenaddr(u32int addr, u32int mask)
{
if((addr & mask) == 0)
return addr;
suicide("unaligned access %8ux @ %8ux", addr, P->R[15] - 4);
return addr & ~mask;
}
static u32int
doshift(u32int instr, u8int *carry)
{
ulong amount, val;
val = P->R[instr & 15];
if(instr & (1<<4)) {
if(instr & (1<<7))
invalid(instr);
amount = P->R[(instr >> 8) & 15] & 0xFF;
if(amount == 0)
return val;
} else {
amount = (instr >> 7) & 31;
if(amount == 0 && (instr & (3<<5)) != 0)
amount = 32;
}
switch((instr >> 5) & 3) {
default:
if(amount == 0)
return val;
if(amount < 32) {
*carry = (val >> (32 - amount)) & 1;
return val << amount;
}
*carry = val & 1;
return 0;
case 1:
if(amount < 32){
*carry = (val >> (amount - 1)) & 1;
return val >> amount;
}
*carry = val >> 31;
return 0;
case 2:
if(amount < 32){
*carry = (val >> (amount - 1)) & 1;
return ((long) val) >> amount;
}
if((long)val < 0){
*carry = 1;
return -1;
}
*carry = 0;
return 0;
case 3:
amount &= 31;
if(amount){
*carry = (val >> (amount - 1)) & 1;
return (val >> amount) | (val << (32 - amount));
}
amount = *carry & 1;
*carry = val & 1;
return (val>>1) | (amount<<31);
}
}
static void
single(u32int instr)
{
long offset;
u32int addr;
u32int *Rn, *Rd;
void *targ;
Segment *seg;
if(instr & fI) {
u8int carry = 0;
if(instr & (1<<4))
invalid(instr);
offset = doshift(instr, &carry);
} else
offset = instr & ((1<<12) - 1);
if(!(instr & fU))
offset = - offset;
Rn = P->R + ((instr >> 16) & 15);
Rd = P->R + ((instr >> 12) & 15);
if((instr & (fW | fP)) == fW)
invalid(instr);
if(Rn == P->R + 15) {
if(instr & fW)
invalid(instr);
addr = P->R[15] + 4;
}
else
addr = *Rn;
if(instr & fP)
addr += offset;
if((instr & fB) == 0)
addr = evenaddr(addr, 3);
targ = vaddr(addr, (instr & fB) == 0 ? 4 : 1, &seg);
switch(instr & (fB | fL)) {
case 0:
*(u32int*) targ = *Rd;
break;
case fB:
*(u8int*) targ = *Rd;
break;
case fL:
*Rd = *(u32int*) targ;
break;
case fB | fL:
*Rd = *(u8int*) targ;
break;
}
if(Rd == P->R + 15 && !(instr & fL)) {
if(instr & fB)
*(u8int*) targ += 8;
else
*(u32int*) targ += 8;
}
segunlock(seg);
if(!(instr & fP))
addr += offset;
if((instr & fW) || !(instr & fP))
*Rn = addr;
}
/* atomic compare and swap from libc */
extern int cas(u32int *p, u32int old, u32int new);
static void
swap(u32int instr)
{
u32int *Rm, *Rn, *Rd, *targ, addr, old, new;
Segment *seg;
Rm = P->R + (instr & 15);
Rd = P->R + ((instr >> 12) & 15);
Rn = P->R + ((instr >> 16) & 15);
if(Rm == P->R + 15 || Rd == P->R + 15 || Rn == P->R + 15)
invalid(instr);
addr = *Rn;
if((instr & fB) == 0)
addr = evenaddr(addr, 3);
targ = (u32int *) vaddr(addr & ~3, 4, &seg);
do {
old = *targ;
new = *Rm;
if(instr & fB){
new &= 0xFF;
new <<= 8*(addr&3);
new |= old & ~(0xFF << 8*(addr&3));
}
} while(!cas(targ, old, new));
if(instr & fB) {
old >>= 8*(addr&3);
old &= 0xFF;
}
*Rd = old;
segunlock(seg);
}
static u32int
add(u32int a, u32int b, u8int type, u8int *carry, u8int *overflow)
{
u32int res1;
u64int res2;
if(type) {
res2 = (u64int)a - b + *carry - 1;
res1 = res2;
if(((a ^ b) & (1<<31)) && !((b ^ res1) & (1<<31))) *overflow = 1;
else *overflow = 0;
if(res2 & 0x100000000LL) *carry = 0;
else *carry = 1;
} else {
res2 = (u64int)a + b + *carry;
res1 = res2;
if(!((a ^ b) & (1<<31)) && ((b ^ res1) & (1<<31))) *overflow = 1;
else *overflow = 0;
if(res2 & 0x100000000LL) *carry = 1;
else *carry = 0;
}
return res1;
}
static void
alu(u32int instr)
{
u32int Rn, *Rd, operand, shift, result, op;
u8int carry, overflow;
Rn = P->R[(instr >> 16) & 15];
Rd = P->R + ((instr >> 12) & 15);
if(((instr >> 16) & 15) == 15) {
Rn += 4;
if(!(instr & fI) && (instr & (1<<4)))
Rn += 4;
}
if(Rd == P->R + 15 && (instr & fS))
invalid(instr);
carry = (P->CPSR & flC) != 0;
overflow = (P->CPSR & flV) != 0;
if(instr & fI) {
operand = instr & 0xFF;
shift = ((instr >> 8) & 15) << 1;
if(shift){
operand = (operand >> shift) | (operand << (32 - shift));
carry = operand >> 31;
}
} else
operand = doshift(instr, &carry);
op = (instr >> 21) & 15;
if(op >= 8 && op <= 11 && !(instr & fS))
sysfatal("no PSR transfers plz");
if(op >= 5 && op < 8)
carry = (P->CPSR & flC) != 0;
switch(op) {
case 0: case 8: result = Rn & operand; break;
case 1: case 9: result = Rn ^ operand; break;
case 2: case 10: carry = 1; case 6: result = add(Rn, operand, 1, &carry, &overflow); break;
case 3: carry = 1; case 7: result = add(operand, Rn, 1, &carry, &overflow); break;
case 4: case 11: carry = 0; case 5: result = add(operand, Rn, 0, &carry, &overflow); break;
case 12: result = Rn | operand; break;
case 13: result = operand; break;
case 14: result = Rn & ~operand; break;
case 15: result = ~operand; break;
default: result = 0; /* never happens */
}
if(instr & fS) {
P->CPSR &= ~FLAGS;
if(result == 0)
P->CPSR |= flZ;
if(result & (1<<31))
P->CPSR |= flN;
if(carry)
P->CPSR |= flC;
if(overflow)
P->CPSR |= flV;
}
if(op < 8 || op >= 12)
*Rd = result;
}
static void
branch(u32int instr)
{
long offset;
offset = instr & ((1<<24) - 1);
if(offset & (1<<23))
offset |= ~((1 << 24) - 1);
offset *= 4;
if(instr & fLi)
P->R[14] = P->R[15];
P->R[15] += offset + 4;
}
static void
halfword(u32int instr)
{
u32int offset, target, *Rn, *Rd;
Segment *seg;
if(instr & (1<<22)) {
offset = (instr & 15) | ((instr >> 4) & 0xF0);
} else {
if((instr & 15) == 15)
invalid(instr);
offset = P->R[instr & 15];
}
if(!(instr & fU))
offset = - offset;
if(!(instr & fP) && (instr & fW))
invalid(instr);
Rn = P->R + ((instr >> 16) & 15);
Rd = P->R + ((instr >> 12) & 15);
if(Rn == P->R + 15 || Rd == P->R + 15)
sysfatal("R15 in halfword");
target = *Rn;
if(instr & fP)
target += offset;
if(instr & fH)
target = evenaddr(target, 1);
switch(instr & (fSg | fH | fL)) {
case fSg: *(u8int*) vaddr(target, 1, &seg) = *Rd; break;
case fSg | fL: *Rd = (long) *(char*) vaddr(target, 1, &seg); break;
case fH: case fSg | fH: *(u16int*) vaddr(target, 2, &seg) = *Rd; break;
case fH | fL: *Rd = *(u16int*) vaddr(target, 2, &seg); break;
case fH | fL | fSg: *Rd = (long) *(short*) vaddr(target, 2, &seg); break;
}
segunlock(seg);
if(!(instr & fP))
target += offset;
if(!(instr & fP) || (instr & fW))
*Rn = target;
}
static void
block(u32int instr)
{
int i;
u32int targ, *Rn;
Segment *seg;
if(instr & (1<<22))
invalid(instr);
Rn = P->R + ((instr >> 16) & 15);
if(Rn == P->R + 15 || instr & (1<<15))
sysfatal("R15 block");
targ = evenaddr(*Rn, 3);
if(instr & fU) {
for(i = 0; i < 16; i++) {
if(!(instr & (1<<i)))
continue;
if(instr & fP)
targ += 4;
if(instr & fL)
P->R[i] = *(u32int*) vaddr(targ, 4, &seg);
else
*(u32int*) vaddr(targ, 4, &seg) = P->R[i];
segunlock(seg);
if(!(instr & fP))
targ += 4;
}
} else {
for(i = 15; i >= 0; i--) {
if(!(instr & (1<<i)))
continue;
if(instr & fP)
targ -= 4;
if(instr & fL)
P->R[i] = *(u32int*) vaddr(targ, 4, &seg);
else
*(u32int*) vaddr(targ, 4, &seg) = P->R[i];
segunlock(seg);
if(!(instr & fP))
targ -= 4;
}
}
if(instr & fW)
*Rn = targ;
}
static void
multiply(u32int instr)
{
u32int *Rd, *Rn, *Rs, *Rm, res;
Rm = P->R + (instr & 15);
Rs = P->R + ((instr >> 8) & 15);
Rn = P->R + ((instr >> 12) & 15);
Rd = P->R + ((instr >> 16) & 15);
if(Rd == Rm || Rm == P->R + 15 || Rs == P->R + 15 || Rn == P->R + 15 || Rd == P->R + 15)
invalid(instr);
res = *Rm * *Rs;
if(instr & (1<<21))
res += *Rn;
*Rd = res;
if(instr & (1<<20)) {
P->CPSR &= ~(flN | flZ);
if(res & (1<<31))
P->CPSR |= flN;
if(res == 0)
P->CPSR |= flZ;
}
}
static void
multiplylong(u32int instr)
{
u32int *RdH, *RdL, *Rs, *Rm;
u64int res;
Rm = P->R + (instr & 15);
Rs = P->R + ((instr >> 8) & 15);
RdL = P->R + ((instr >> 12) & 15);
RdH = P->R + ((instr >> 16) & 15);
if(RdL == RdH || RdH == Rm || RdL == Rm || Rm == P->R + 15 || Rs == P->R + 15 || RdL == P->R + 15 || RdH == P->R + 15)
invalid(instr);
if(instr & (1<<22))
res = ((vlong)*(int*)Rs) * *(int*)Rm;
else {
res = *Rs;
res *= *Rm;
}
if(instr & (1<<21)) {
res += *RdL;
res += ((uvlong)*RdH) << 32;
}
*RdL = res;
*RdH = res >> 32;
if(instr & (1<<20)) {
P->CPSR &= ~FLAGS;
if(res == 0)
P->CPSR |= flN;
if(res & (1LL<<63))
P->CPSR |= flV;
}
}
static void
singleex(u32int instr)
{
u32int *Rn, *Rd, *Rm, *targ, addr;
Segment *seg;
Rd = P->R + ((instr >> 12) & 15);
Rn = P->R + ((instr >> 16) & 15);
if(Rd == P->R + 15 || Rn == P->R + 15)
invalid(instr);
addr = evenaddr(*Rn, 3);
if(instr & fS) {
targ = vaddr(addr, 4, &seg);
*Rd = *targ;
P->lladdr = addr;
P->llval = *Rd;
segunlock(seg);
} else {
Rm = P->R + (instr & 15);
if(Rm == P->R + 15)
invalid(instr);
targ = vaddr(addr, 4, &seg);
/*
* this is not quite correct as we will succeed even
* if the value was modified and then restored to its
* original value but good enougth approximation for
* libc's _tas(), _cas() and _xinc()/_xdec().
*/
*Rd = addr != P->lladdr || !cas(targ, P->llval, *Rm);
segunlock(seg);
clrex();
}
}
void
clrex(void)
{
P->lladdr = 0;
P->llval = 0;
}
static void
barrier(void)
{
static Lock l;
lock(&l);
unlock(&l);
}
void
step(void)
{
u32int instr;
Segment *seg;
instr = *(u32int*) vaddr(P->R[15], 4, &seg);
segunlock(seg);
if(fulltrace) {
print("%d ", P->pid);
if(havesymbols) {
Symbol s;
char buf[512];
if(findsym(P->R[15], CTEXT, &s) >= 0)
print("%s ", s.name);
if(fileline(buf, 512, P->R[15]) >= 0)
print("%s ", buf);
}
print("%.8ux %.8ux %c%c%c%c\n", P->R[15], instr,
(P->CPSR & flZ) ? 'Z' : ' ',
(P->CPSR & flC) ? 'C' : ' ',
(P->CPSR & flN) ? 'N' : ' ',
(P->CPSR & flV) ? 'V' : ' '
);
}
P->R[15] += 4;
switch(instr >> 28) {
case 0x0: if(!(P->CPSR & flZ)) return; break;
case 0x1: if(P->CPSR & flZ) return; break;
case 0x2: if(!(P->CPSR & flC)) return; break;
case 0x3: if(P->CPSR & flC) return; break;
case 0x4: if(!(P->CPSR & flN)) return; break;
case 0x5: if(P->CPSR & flN) return; break;
case 0x6: if(!(P->CPSR & flV)) return; break;
case 0x7: if(P->CPSR & flV) return; break;
case 0x8: if(!(P->CPSR & flC) || (P->CPSR & flZ)) return; break;
case 0x9: if((P->CPSR & flC) && !(P->CPSR & flZ)) return; break;
case 0xA: if(!(P->CPSR & flN) != !(P->CPSR & flV)) return; break;
case 0xB: if(!(P->CPSR & flN) == !(P->CPSR & flV)) return; break;
case 0xC: if((P->CPSR & flZ) || !(P->CPSR & flN) != !(P->CPSR & flV)) return; break;
case 0xD: if(!(P->CPSR & flZ) && !(P->CPSR & flN) == !(P->CPSR & flV)) return; break;
case 0xE: break;
case 0xF:
switch(instr & 0xFFF000F0){
case 0xF5700010: /* CLREX */
clrex();
return;
case 0xF5700040: /* DSB */
case 0xF5700050: /* DMB */
case 0xF5700060: /* ISB */
barrier();
return;
}
default: sysfatal("condition code %x not implemented (instr %ux, ps %ux)", instr >> 28, instr, P->R[15]);
}
if((instr & 0x0FB00FF0) == 0x01000090)
swap(instr);
else if((instr & 0x0FE000F0) == 0x01800090)
singleex(instr);
else if((instr & 0x0FC000F0) == 0x90)
multiply(instr);
else if((instr & 0x0F8000F0) == 0x800090)
multiplylong(instr);
else if((instr & ((1<<26) | (1<<27))) == (1 << 26))
single(instr);
else if((instr & 0x0E000090) == 0x90 && (instr & 0x60))
halfword(instr);
else if((instr & ((1<<26) | (1<<27))) == 0)
alu(instr);
else if((instr & (7<<25)) == (5 << 25))
branch(instr);
else if((instr & (15<<24)) == (15 << 24))
syscall();
else if((instr & (7<<25)) == (4 << 25))
block(instr);
else if((instr & 0x0E000F00) == 0x0C000100)
fpatransfer(instr);
else if((instr & 0x0E000F10) == 0x0E000100)
fpaoperation(instr);
else if((instr & 0x0E000F10) == 0x0E000110)
fparegtransfer(instr);
else if(vfp && ((instr & 0x0F000A10) == 0x0E000A00))
vfpoperation(instr);
else if(vfp && ((instr & 0x0F000F10) == 0x0E000A10))
vfpregtransfer(instr);
else if(vfp && ((instr & 0x0F000A00) == 0x0D000A00))
vfprmtransfer(instr);
else
invalid(instr);
}