ref: 8d93ccdbb2a2dee8d2eb1387da1b316fdf84b208
dir: /kern/arm.c/
#include <u.h>
#include <lib.h>
#include "dat.h"
#include "fns.h"
#include "proc.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)
{
fprint(2, "undefined instruction %8ux @ %8ux", instr, up->R[15] - 4);
}
u32int
evenaddr(u32int addr, u32int mask)
{
if((addr & mask) == 0)
return addr;
return addr & ~mask;
}
static u32int
doshift(u32int instr, uint *carry)
{
ulong amount, val;
val = up->R[instr & 15];
if(instr & (1<<4)) {
if(instr & (1<<7))
invalid(instr);
amount = up->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) {
uint carry = 0;
if(instr & (1<<4))
invalid(instr);
offset = doshift(instr, &carry);
} else
offset = instr & ((1<<12) - 1);
if(!(instr & fU))
offset = - offset;
Rn = up->R + ((instr >> 16) & 15);
Rd = up->R + ((instr >> 12) & 15);
if((instr & (fW | fP)) == fW)
invalid(instr);
if(Rn == up->R + 15) {
if(instr & fW)
invalid(instr);
addr = up->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:
*(uint*) targ = *Rd;
break;
case fL:
*Rd = *(u32int*) targ;
break;
case fB | fL:
*Rd = *(uint*) targ;
break;
}
if(Rd == up->R + 15 && !(instr & fL)) {
if(instr & fB)
*(uint*) targ += 8;
else
*(u32int*) targ += 8;
}
segunlock(seg);
if(!(instr & fP))
addr += offset;
if((instr & fW) || !(instr & fP))
*Rn = addr;
}
static void
swap(u32int instr)
{
u32int *Rm, *Rn, *Rd, *targ, addr, old, new;
Segment *seg;
Rm = up->R + (instr & 15);
Rd = up->R + ((instr >> 12) & 15);
Rn = up->R + ((instr >> 16) & 15);
if(Rm == up->R + 15 || Rd == up->R + 15 || Rn == up->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((int*)targ, old, new));
if(instr & fB) {
old >>= 8*(addr&3);
old &= 0xFF;
}
*Rd = old;
segunlock(seg);
}
static u32int
add(u32int a, u32int b, uint type, uint *carry, uint *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;
uint carry, overflow;
Rn = up->R[(instr >> 16) & 15];
Rd = up->R + ((instr >> 12) & 15);
if(((instr >> 16) & 15) == 15) {
Rn += 4;
if(!(instr & fI) && (instr & (1<<4)))
Rn += 4;
}
if(Rd == up->R + 15 && (instr & fS))
invalid(instr);
carry = (up->CPSR & flC) != 0;
overflow = (up->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)){
fprint(2, "no PSR transfers plz");
return;
}
if(op >= 5 && op < 8)
carry = (up->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) {
up->CPSR &= ~FLAGS;
if(result == 0)
up->CPSR |= flZ;
if(result & (1<<31))
up->CPSR |= flN;
if(carry)
up->CPSR |= flC;
if(overflow)
up->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)
up->R[14] = up->R[15];
up->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 = up->R[instr & 15];
}
if(!(instr & fU))
offset = - offset;
if(!(instr & fP) && (instr & fW))
invalid(instr);
Rn = up->R + ((instr >> 16) & 15);
Rd = up->R + ((instr >> 12) & 15);
if(Rn == up->R + 15 || Rd == up->R + 15)
fprint(2, "R15 in halfword");
target = *Rn;
if(instr & fP)
target += offset;
if(instr & fH)
target = evenaddr(target, 1);
switch(instr & (fSg | fH | fL)) {
case fSg: *(uint*) vaddr(target, 1, &seg) = *Rd; break;
case fSg | fL: *Rd = (long) *(char*) vaddr(target, 1, &seg); break;
case fH: case fSg | fH: *(uint16_t*) vaddr(target, 2, &seg) = *Rd; break;
case fH | fL: *Rd = *(uint16_t*) 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;
print("Checking invalid\n");
if(instr & (1<<22))
invalid(instr);
Rn = up->R +((instr >> 16) & 15);
if(Rn == up->R + 15 || instr & (1<<15))
print("R15 in 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)
up->R[i] = *(u32int*) vaddr(targ, 4, &seg);
else
*(u32int*) vaddr(targ, 4, &seg) = up->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)
up->R[i] = *(u32int*) vaddr(targ, 4, &seg);
else
*(u32int*) vaddr(targ, 4, &seg) = up->R[i];
segunlock(seg);
if(!(instr & fP))
targ -= 4;
}
}
if(instr & fW)
*Rn = targ;
print("Farnished\n");
}
static void
multiply(u32int instr)
{
u32int *Rd, *Rn, *Rs, *Rm, res;
Rm = up->R + (instr & 15);
Rs = up->R + ((instr >> 8) & 15);
Rn = up->R + ((instr >> 12) & 15);
Rd = up->R + ((instr >> 16) & 15);
if(Rd == Rm || Rm == up->R + 15 || Rs == up->R + 15 || Rn == up->R + 15 || Rd == up->R + 15)
invalid(instr);
res = *Rm * *Rs;
if(instr & (1<<21))
res += *Rn;
*Rd = res;
if(instr & (1<<20)) {
up->CPSR &= ~(flN | flZ);
if(res & (1<<31))
up->CPSR |= flN;
if(res == 0)
up->CPSR |= flZ;
}
}
static void
multiplylong(u32int instr)
{
u32int *RdH, *RdL, *Rs, *Rm;
u64int res;
Rm = up->R + (instr & 15);
Rs = up->R + ((instr >> 8) & 15);
RdL = up->R + ((instr >> 12) & 15);
RdH = up->R + ((instr >> 16) & 15);
if(RdL == RdH || RdH == Rm || RdL == Rm || Rm == up->R + 15 || Rs == up->R + 15 || RdL == up->R + 15 || RdH == up->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)) {
up->CPSR &= ~FLAGS;
if(res == 0)
up->CPSR |= flN;
if(res & (1LL<<63))
up->CPSR |= flV;
}
}
static void
singleex(u32int instr)
{
u32int *Rn, *Rd, *Rm, *targ, addr;
Segment *seg;
Rd = up->R + ((instr >> 12) & 15);
Rn = up->R + ((instr >> 16) & 15);
if(Rd == up->R + 15 || Rn == up->R + 15)
invalid(instr);
addr = evenaddr(*Rn, 3);
if(instr & fS) {
targ = vaddr(addr, 4, &seg);
*Rd = *targ;
up->lladdr = addr;
up->llval = *Rd;
segunlock(seg);
} else {
Rm = up->R + (instr & 15);
if(Rm == up->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 enough approximation for
* libc's _tas(), _cas() and _xinc()/_xdec().
*/
*Rd = addr != up->lladdr || !cas((int*)targ, up->llval, *Rm);
segunlock(seg);
clrex();
}
}
void
clrex(void)
{
up->lladdr = 0;
up->llval = 0;
}
static void
barrier(void)
{
static Lock l;
lock(&l);
unlock(&l);
}
int
step(void)
{
u32int instr;
Segment *seg;
print("Grabbing instruction\n");
instr = *(u32int*) vaddr(up->R[15], 4, &seg);
print("Grabbed instruction\n");
segunlock(seg);
up->R[15] += 4;
switch(instr >> 28) {
case 0x0: if(!(up->CPSR & flZ)) return 1; break;
case 0x1: if(up->CPSR & flZ) return 1; break;
case 0x2: if(!(up->CPSR & flC)) return 1; break;
case 0x3: if(up->CPSR & flC) return 1; break;
case 0x4: if(!(up->CPSR & flN)) return 1; break;
case 0x5: if(up->CPSR & flN) return 1; break;
case 0x6: if(!(up->CPSR & flV)) return 1; break;
case 0x7: if(up->CPSR & flV) return 1; break;
case 0x8: if(!(up->CPSR & flC) || (up->CPSR & flZ)) return 1; break;
case 0x9: if((up->CPSR & flC) && !(up->CPSR & flZ)) return 1; break;
case 0xA: if(!(up->CPSR & flN) != !(up->CPSR & flV)) return 1; break;
case 0xB: if(!(up->CPSR & flN) == !(up->CPSR & flV)) return 1; break;
case 0xC: if((up->CPSR & flZ) || !(up->CPSR & flN) != !(up->CPSR & flV)) return 1; break;
case 0xD: if(!(up->CPSR & flZ) && !(up->CPSR & flN) == !(up->CPSR & flV)) return 1; break;
case 0xE: break;
case 0xF:
switch(instr & 0xFFF000F0){
case 0xF5700010: /* CLREX */
print("clrex\n");
//clrex();
return 1;
case 0xF5700040: /* DSB */
case 0xF5700050: /* DMB */
case 0xF5700060: /* ISB */
print("barrier\n");
//barrier();
return 1;
}
default:
fprint(2, "condition code %x not implemented (instr %ux, ps %ux)", instr >> 28, instr, up->R[15]);
return 0;
}
if((instr & 0x0FB00FF0) == 0x01000090)
print("Swap\n");
//swap(instr);
else if((instr & 0x0FE000F0) == 0x01800090)
print("Singleex\n");
//singleex(instr);
else if((instr & 0x0FC000F0) == 0x90)
print("Multiply\n");
//multiply(instr);
else if((instr & 0x0F8000F0) == 0x800090)
print("Multiplylong\n");
//multiplylong(instr);
else if((instr & ((1<<26) | (1<<27))) == (1 << 26))
print("Single\n");
//single(instr);
else if((instr & 0x0E000090) == 0x90 && (instr & 0x60))
print("Halfword\n");
//halfword(instr);
else if((instr & ((1<<26) | (1<<27))) == 0)
print("Alu\n");
//alu(instr);
else if((instr & (7<<25)) == (5 << 25))
print("Branch\n");
//branch(instr);
else if((instr & (15<<24)) == (15 << 24))
print("Syscall\n");
//_syscall();
else if((instr & (7<<25)) == (4 << 25))
print("Block\n");
//block(instr);
else if((instr & 0x0E000F00) == 0x0C000100)
print("Fpatransfer\n");
//fpatransfer(instr);
else if((instr & 0x0E000F10) == 0x0E000100)
print("Fpaoperation\n");
//fpaoperation(instr);
else if((instr & 0x0E000F10) == 0x0E000110)
print("Fparegtransfer\n");
//fparegtransfer(instr);
else if((instr & 0x0F000A10) == 0x0E000A00)
print("Vfpoperation\n");
//vfpoperation(instr);
else if((instr & 0x0F000F10) == 0x0E000A10)
print("Vfpregtransfer\n");
//vfpregtransfer(instr);
else if((instr & 0x0F000A00) == 0x0D000A00)
print("Vfprmtransfer\n");
//vfprmtransfer(instr);
else {
print("Invalid\n");
invalid(instr);
return 0;
}
return 1;
}