ref: fe54357c8428219c5a27c4cbbebc8d49cc29bdf7
dir: /kern/arm.c/
#include <u.h>
#include <lib.h>
#include "dat.h"
#include "fns.h"
#include "proc.h"
enum {
fI = 1<<25, // Immediate bit
fP = 1<<24, // Pre/post indexing bit
fLi = 1<<24, // Link bit for branching
fU = 1<<23, // Up/down bit
fB = 1<<22, // Byte/word bit
fW = 1<<21, // Write back bit
fL = 1<<20, // Load/store bit
fS = 1<<20, // Set condition bit in multiply
fA = 1<<21, // Accumulate bit in multiply
fSg = 1<<6,
fH = 1<<5,
};
void
invalid(u32int instr)
{
panic("undefined instruction %.8ux @ %.8ux", instr, up->R[15] - 4);
}
u32int
evenaddr(u32int addr, u32int mask)
{
if((addr & mask) == 0)
return addr;
panic("unaligned access %8ux @ %8ux", addr, up->R[15] - 4);
return addr & ~mask;
}
static u32int
doshift(u32int instr, u8int *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);
}
}
/* Single Data Transfer instruction */
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 = up->R + ((instr >> 16) & 15);
Rd = up->R + ((instr >> 12) & 15);
if(debug > 1)
print("single: Rn=%d Rd=%d offset=%.8ux pid=%d\n", ((instr >> 16) & 15), ((instr >> 12) & 15), offset, up->pid);
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:
*(u8int*) targ = *Rd;
break;
case fL:
*Rd = *(u32int*) targ;
break;
case fB | fL:
*Rd = *(u8int*) targ;
break;
}
if(Rd == up->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;
}
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(debug > 1)
print("swap: Rm=%d Rd=%d Rn=%d pid=%d\n", (instr & 15), ((instr >> 12) & 15), ((instr >> 16) & 15), up->pid);
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, 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 = 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))
panic("no PSR transfers plz");
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(debug > 1)
print("alu: Rn=%d Rd=%d op=%d operand=%.8ux pid=%d\n", ((instr >> 16) & 15), ((instr >> 12) & 15), ((instr >> 21) & 15), operand, up->pid);
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(debug > 1)
print("branch: offset=%.8ux pid=%d\n", offset, up->pid);
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(debug > 1)
print("halfword: Rn=%d, Rd=%d pid=%d\n", ((instr >> 16) & 15), ((instr >> 12) & 15), up->pid);
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: *(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;
if(instr & (1<<22))
invalid(instr);
Rn = up->R +((instr >> 16) & 15);
if(debug > 1)
print("block: Rn=%d pid=%d\n", ((instr >> 16) & 15), up->pid);
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;
}
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(debug > 1)
print("multiply: Rm=%d Rs=%d Rn=%d Rd=%d pid=%d\n", (instr & 15), ((instr >> 8) & 15), ((instr >> 12) & 15), ((instr >> 16) & 15), up->pid);
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;
if(debug > 1)
print("multiplylong\n");
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;
if(debug > 1)
print("singleex: Rd=%d Rn=%d pid=%d\n", ((instr >> 12) & 15), ((instr >> 16) & 15), up->pid);
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);
*Rd = addr != up->lladdr || !cas((int*)targ, up->llval, *Rm);
segunlock(seg);
clrex();
}
}
void
clrex(void)
{
if(debug > 1)
print("clrex\n");
up->lladdr = 0;
up->llval = 0;
}
static void
barrier(void)
{
if(debug > 1)
print("barrier\n");
static Lock l;
lock(&l);
unlock(&l);
}
void
step(void)
{
u32int instr;
//char *state;
Segment *seg;
switch(up->procctl) {
case Proc_traceme:
//if(up->nnote == 0)
// return;
/* No break */
case Proc_stopme:
up->procctl = 0;
//state = up->psstate;
//up->psstate = statename[Stopped];
/* free a waiting debugger */
//spllo();
//qlock(&up->debug);
//if(up->pdbg != nil) {
// wakeup(&up->pdbg->sleep);
// up->pdbg = nil;
//}
//qunlock(&up->debug);
//splhi();
//up->state = Stopped;
//sched();
//up->psstate = state;
return;
case Proc_exitme:
pexit("Killed", 1);
case Proc_exitbig:
pprint("Killed: Insufficient physical memory\n");
pexit("Killed: Insufficient physical memory", 1);
}
instr = *(u32int*) vaddr(up->R[15], 4, &seg);
segunlock(seg);
if(debug > 2) {
print("%.8ux %.8ux %c%c%c%c\n", up->R[15], instr,
(up->CPSR & flZ) ? 'Z' : ' ',
(up->CPSR & flC) ? 'C' : ' ',
(up->CPSR & flN) ? 'N' : ' ',
(up->CPSR & flV) ? 'V' : ' '
);
}
up->R[15] += 4;
// CPSR testing for conditional execution
switch(instr >> 28) {
case 0x0: if(!(up->CPSR & flZ)) return; break;
case 0x1: if(up->CPSR & flZ) return; break;
case 0x2: if(!(up->CPSR & flC)) return; break;
case 0x3: if(up->CPSR & flC) return; break;
case 0x4: if(!(up->CPSR & flN)) return; break;
case 0x5: if(up->CPSR & flN) return; break;
case 0x6: if(!(up->CPSR & flV)) return; break;
case 0x7: if(up->CPSR & flV) return; break;
case 0x8: if(!(up->CPSR & flC) || (up->CPSR & flZ)) return; break;
case 0x9: if((up->CPSR & flC) && !(up->CPSR & flZ)) return; break;
case 0xA: if(!(up->CPSR & flN) != !(up->CPSR & flV)) return; break;
case 0xB: if(!(up->CPSR & flN) == !(up->CPSR & flV)) return; break;
case 0xC: if((up->CPSR & flZ) || !(up->CPSR & flN) != !(up->CPSR & flV)) return; break;
case 0xD: if(!(up->CPSR & flZ) && !(up->CPSR & flN) == !(up->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:
panic("condition code %x not implemented (instr %ux, ps %ux)", instr >> 28, instr, up->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 ((instr & 0x0F000A10) == 0x0E000A00)
vfpoperation(instr);
else if ((instr & 0x0F000F10) == 0x0E000A10)
vfpregtransfer(instr);
else if ((instr & 0x0F000A00) == 0x0D000A00)
vfprmtransfer(instr);
else {
invalid(instr);
}
}