1#include "arm.h"
  2
  3#include "isa-inlines.h"
  4
  5static inline enum RegisterBank _ARMSelectBank(enum PrivilegeMode);
  6
  7void ARMSetPrivilegeMode(struct ARMCore* cpu, enum PrivilegeMode mode) {
  8	if (mode == cpu->privilegeMode) {
  9		// Not switching modes after all
 10		return;
 11	}
 12
 13	enum RegisterBank newBank = _ARMSelectBank(mode);
 14	enum RegisterBank oldBank = _ARMSelectBank(cpu->privilegeMode);
 15	if (newBank != oldBank) {
 16		// Switch banked registers
 17		if (mode == MODE_FIQ || cpu->privilegeMode == MODE_FIQ) {
 18			int oldFIQBank = oldBank == BANK_FIQ;
 19			int newFIQBank = newBank == BANK_FIQ;
 20			cpu->bankedRegisters[oldFIQBank][2] = cpu->gprs[8];
 21			cpu->bankedRegisters[oldFIQBank][3] = cpu->gprs[9];
 22			cpu->bankedRegisters[oldFIQBank][4] = cpu->gprs[10];
 23			cpu->bankedRegisters[oldFIQBank][5] = cpu->gprs[11];
 24			cpu->bankedRegisters[oldFIQBank][6] = cpu->gprs[12];
 25			cpu->gprs[8] = cpu->bankedRegisters[newFIQBank][2];
 26			cpu->gprs[9] = cpu->bankedRegisters[newFIQBank][3];
 27			cpu->gprs[10] = cpu->bankedRegisters[newFIQBank][4];
 28			cpu->gprs[11] = cpu->bankedRegisters[newFIQBank][5];
 29			cpu->gprs[12] = cpu->bankedRegisters[newFIQBank][6];
 30		}
 31		cpu->bankedRegisters[oldBank][0] = cpu->gprs[ARM_SP];
 32		cpu->bankedRegisters[oldBank][1] = cpu->gprs[ARM_LR];
 33		cpu->gprs[ARM_SP] = cpu->bankedRegisters[newBank][0];
 34		cpu->gprs[ARM_LR] = cpu->bankedRegisters[newBank][1];
 35
 36		cpu->bankedSPSRs[oldBank] = cpu->spsr.packed;
 37		cpu->spsr.packed = cpu->bankedSPSRs[newBank];
 38
 39	}
 40	cpu->privilegeMode = mode;
 41}
 42
 43static inline enum RegisterBank _ARMSelectBank(enum PrivilegeMode mode) {
 44	switch (mode) {
 45		case MODE_USER:
 46		case MODE_SYSTEM:
 47			// No banked registers
 48			return BANK_NONE;
 49		case MODE_FIQ:
 50			return BANK_FIQ;
 51		case MODE_IRQ:
 52			return BANK_IRQ;
 53		case MODE_SUPERVISOR:
 54			return BANK_SUPERVISOR;
 55		case MODE_ABORT:
 56			return BANK_ABORT;
 57		case MODE_UNDEFINED:
 58			return BANK_UNDEFINED;
 59		default:
 60			// This should be unreached
 61			return BANK_NONE;
 62	}
 63}
 64
 65void ARMInit(struct ARMCore* cpu) {
 66	cpu->memory = 0;
 67	cpu->board = 0;
 68}
 69
 70void ARMAssociateMemory(struct ARMCore* cpu, struct ARMMemory* memory) {
 71	cpu->memory = memory;
 72}
 73
 74void ARMAssociateBoard(struct ARMCore* cpu, struct ARMBoard* board) {
 75	cpu->board = board;
 76	board->cpu = cpu;
 77}
 78
 79void ARMReset(struct ARMCore* cpu) {
 80	int i;
 81	for (i = 0; i < 16; ++i) {
 82		cpu->gprs[i] = 0;
 83	}
 84	for (i = 0; i < 6; ++i) {
 85		cpu->bankedRegisters[i][0] = 0;
 86		cpu->bankedRegisters[i][1] = 0;
 87		cpu->bankedRegisters[i][2] = 0;
 88		cpu->bankedRegisters[i][3] = 0;
 89		cpu->bankedRegisters[i][4] = 0;
 90		cpu->bankedRegisters[i][5] = 0;
 91		cpu->bankedRegisters[i][6] = 0;
 92		cpu->bankedSPSRs[i] = 0;
 93	}
 94
 95	cpu->cpsr.packed = MODE_SYSTEM;
 96	cpu->spsr.packed = 0;
 97
 98	cpu->cyclesToEvent = 0;
 99
100	cpu->shifterOperand = 0;
101	cpu->shifterCarryOut = 0;
102
103	cpu->executionMode = MODE_THUMB;
104	_ARMSetMode(cpu, MODE_ARM);
105
106	cpu->board->reset(cpu->board);
107}
108