diff --git a/core/cpu.c b/core/cpu.c index 730c1a0..068b68f 100644 --- a/core/cpu.c +++ b/core/cpu.c @@ -18,76 +18,76 @@ #define CPU_ADDF_S 9 #define CPU_ADDI 10 #define CPU_AND 11 -#define CPU_ANDBSU 12 -#define CPU_ANDI 13 -#define CPU_ANDNBSU 14 -#define CPU_BCOND 15 -#define CPU_CAXI 16 -#define CPU_CLI 17 -#define CPU_CMP_IMM 18 -#define CPU_CMP_REG 19 -#define CPU_CMPF_S 20 -#define CPU_CVT_SW 21 -#define CPU_CVT_WS 22 -#define CPU_DIV 23 -#define CPU_DIVF_S 24 -#define CPU_DIVU 25 -#define CPU_HALT 26 -#define CPU_IN_B 27 -#define CPU_IN_H 28 -#define CPU_IN_W 29 -#define CPU_JAL 30 -#define CPU_JMP 31 -#define CPU_JR 32 -#define CPU_LD_B 33 -#define CPU_LD_H 34 -#define CPU_LD_W 35 -#define CPU_LDSR 36 -#define CPU_MOV_IMM 37 -#define CPU_MOV_REG 38 -#define CPU_MOVBSU 39 -#define CPU_MOVEA 40 -#define CPU_MOVHI 41 -#define CPU_MPYHW 42 -#define CPU_MUL 43 -#define CPU_MULF_S 44 -#define CPU_MULU 45 -#define CPU_NOT 46 -#define CPU_NOTBSU 47 -#define CPU_OR 48 -#define CPU_ORBSU 49 -#define CPU_ORI 50 -#define CPU_ORNBSU 51 -#define CPU_OUT_B 52 -#define CPU_OUT_H 53 -#define CPU_OUT_W 54 -#define CPU_RETI 55 -#define CPU_REV 56 -#define CPU_SAR_IMM 57 -#define CPU_SAR_REG 58 -#define CPU_SCH0BSD 59 -#define CPU_SCH0BSU 60 -#define CPU_SCH1BSD 61 -#define CPU_SCH1BSU 62 -#define CPU_SEI 63 -#define CPU_SETF 64 -#define CPU_SHL_IMM 65 -#define CPU_SHL_REG 66 -#define CPU_SHR_IMM 67 -#define CPU_SHR_REG 68 -#define CPU_ST_B 69 -#define CPU_ST_H 70 -#define CPU_ST_W 71 -#define CPU_STSR 72 -#define CPU_SUB 73 -#define CPU_SUBF_S 74 -#define CPU_TRAP 75 -#define CPU_TRNC_SW 76 -#define CPU_XB 77 -#define CPU_XH 78 -#define CPU_XOR 79 -#define CPU_XORBSU 80 -#define CPU_XORI 81 +#define CPU_ANDI 12 +#define CPU_BCOND 13 +#define CPU_CAXI 14 +#define CPU_CLI 15 +#define CPU_CMP_IMM 16 +#define CPU_CMP_REG 17 +#define CPU_CMPF_S 18 +#define CPU_CVT_SW 19 +#define CPU_CVT_WS 20 +#define CPU_DIV 21 +#define CPU_DIVF_S 22 +#define CPU_DIVU 23 +#define CPU_HALT 24 +#define CPU_IN_B 25 +#define CPU_IN_H 26 +#define CPU_IN_W 27 +#define CPU_JAL 28 +#define CPU_JMP 29 +#define CPU_JR 30 +#define CPU_LD_B 31 +#define CPU_LD_H 32 +#define CPU_LD_W 33 +#define CPU_LDSR 34 +#define CPU_MOV_IMM 35 +#define CPU_MOV_REG 36 +#define CPU_MOVEA 37 +#define CPU_MOVHI 38 +#define CPU_MPYHW 39 +#define CPU_MUL 40 +#define CPU_MULF_S 41 +#define CPU_MULU 42 +#define CPU_NOT 43 +#define CPU_OR 44 +#define CPU_ORI 45 +#define CPU_OUT_B 46 +#define CPU_OUT_H 47 +#define CPU_OUT_W 48 +#define CPU_RETI 49 +#define CPU_REV 50 +#define CPU_SAR_IMM 51 +#define CPU_SAR_REG 52 +#define CPU_SCH0BSD 53 +#define CPU_SCH0BSU 54 +#define CPU_SCH1BSD 55 +#define CPU_SCH1BSU 56 +#define CPU_SEI 57 +#define CPU_SETF 58 +#define CPU_SHL_IMM 59 +#define CPU_SHL_REG 60 +#define CPU_SHR_IMM 61 +#define CPU_SHR_REG 62 +#define CPU_ST_B 63 +#define CPU_ST_H 64 +#define CPU_ST_W 65 +#define CPU_STSR 66 +#define CPU_SUB 67 +#define CPU_SUBF_S 68 +#define CPU_TRAP 69 +#define CPU_TRNC_SW 70 +#define CPU_XB 71 +#define CPU_XH 72 +#define CPU_XOR 73 +#define CPU_XORI 74 +#define CPU_ANDBSU 75 /* Keep bit string ALU commands sequential */ +#define CPU_ANDNBSU 76 +#define CPU_MOVBSU 77 +#define CPU_NOTBSU 78 +#define CPU_ORBSU 79 +#define CPU_ORNBSU 80 +#define CPU_XORBSU 81 #define CPU_XORNBSU 82 /* Abstract operand types */ @@ -124,8 +124,15 @@ /* Master clocks per CPU cycles */ #define cpuClocks(x) x -/* Shorthand */ -#define auxData sim->cpu.aux.data +/* Shorthands */ +#define auxBS sim->cpu.aux.bs +#define auxData sim->cpu.aux.data +#define bsDestAddr sim->cpu.program[29] +#define bsDestBit sim->cpu.program[26] +#define bsLength sim->cpu.program[28] +#define bsSkipped sim->cpu.program[29] +#define bsSrcAddr sim->cpu.program[30] +#define bsSrcBit sim->cpu.program[27] @@ -666,6 +673,158 @@ static int32_t cpuBitwise(VB *sim, int32_t c) { return c; } +/* Bit string arithmetic common processing */ +static int cpuBSArithmetic(VB *sim) { + uint32_t dest; + uint32_t src; + switch (sim->cpu.step) { + case 0: + bsDestAddr &= 0xFFFFFFFC; + bsDestBit &= 0x0000001F; + bsSrcAddr &= 0xFFFFFFFC; + bsSrcBit &= 0x0000001F; + sim->cpu.clocks += cpuClocks(35); /* TODO: Research */ + + /* Nothing to do */ + if (bsLength == 0) + break; + + /* Fallthrough */ + case 1: + + /* Read the low source word */ + if (cpuRead(sim, bsSrcAddr, VB_S32, (int32_t *) &src)) { + sim->cpu.step = 1; + return 1; + } + auxBS.src = src; + + /* Fallthrough */ + case 2: + + /* Read the high source word */ + if (cpuRead(sim, bsSrcAddr + 1, VB_S32, (int32_t *) &src)) { + sim->cpu.step = 2; + return 1; + } + auxBS.src = (uint32_t) auxBS.src | (uint64_t) src << 32; + + /* Fallthrough */ + case 3: + + /* Read the destination word */ + if (cpuRead(sim, bsDestAddr, VB_S32, (int32_t *) &dest)) { + sim->cpu.step = 3; + return 1; + } + + /* Perform the ALU operation */ + src = bsSrcBit < bsDestBit ? + auxBS.src << (bsDestBit - bsSrcBit ) : + auxBS.src >> (bsSrcBit - bsDestBit) + ; + switch (sim->cpu.operation) { + case CPU_ANDBSU : dest &= src; break; + case CPU_ANDNBSU: dest &= ~src; break; + case CPU_MOVBSU : dest = src; break; + case CPU_NOTBSU : dest = ~src; break; + case CPU_ORBSU : dest |= src; break; + case CPU_ORNBSU : dest |= ~src; break; + case CPU_XORBSU : dest ^= src; break; + case CPU_XORNBSU: dest ^= ~src; break; + } + + /* Change only the bits that are part of the string */ + src = 0xFFFFFFFF << bsDestBit; + if ((uint32_t) 32 - bsDestBit > (uint32_t) bsLength) + src &= ~(0xFFFFFFFF << (bsDestBit + bsLength)); + auxBS.dest = (auxBS.dest & ~src) | (dest & src); + + /* Fallthrough */ + case 4: + + /* Write the destination word */ + if (cpuWrite(sim, bsDestAddr, VB_S32, auxBS.dest)) { + sim->cpu.step = 4; + return 1; + } + + /* Select number of bits to advance */ + src = 32 - bsDestBit; + if (src > (uint32_t) bsLength) + src = bsLength; + + /* Advance to next output word */ + if (bsSrcBit + src >= 32) + bsSrcAddr += 4; + if (bsDestBit + src >= 32) + bsDestAddr += 4; + bsSrcBit = (bsSrcBit + src) & 31; + bsDestBit = (bsDestBit + src) & 31; + bsLength -= src; + auxBS.src >>= 32; + + /* Update state */ + sim->cpu.clocks += cpuClocks(12); /* TODO: Research */ + sim->cpu.step = 2; /* Read high source word */ + } + + /* Exit condition */ + if (bsLength == 0) + cpuAdvance(sim, 0); + return 0; +} + +/* Bit string search common processing */ +static int cpuBSSearch(VB *sim, int bit, int dir) { + switch (sim->cpu.step) { + case 0: + bsSrcAddr &= 0xFFFFFFFC; + bsSrcBit &= 0x0000001F; + sim->cpu.clocks += cpuClocks(50); /* TODO: Research */ + sim->cpu.psw.z = 1; + sim->cpu.step = 1; + + /* Nothing to do */ + if (bsLength == 0) + break; + + /* Fallthrough */ + case 1: + + /* Read the source word */ + if (cpuRead(sim, bsSrcAddr, VB_S32, &auxData.value)) + return 1; + sim->cpu.clocks += cpuClocks(5); /* TODO: Research */ + + /* Process all remaining bits in source word */ + do { + + /* Match was found */ + if ((auxData.value >> bsSrcBit & 1) == bit) + sim->cpu.psw.z = 0; + + /* Match was not found */ + else bsSkipped++; + + /* Advance to next bit */ + bsSrcBit = (bsSrcBit + dir) & 31; + bsLength--; + if (bsSrcBit != (dir == 1 ? 0 : 31)) + continue; + + /* Advance to next word */ + bsSrcAddr += dir << 2; + break; + } while (sim->cpu.psw.z && bsLength != 0); + } + + /* Exit condition */ + if (!sim->cpu.psw.z || bsLength == 0) + cpuAdvance(sim, 0); + return 0; +} + /* Test a condition */ static int cpuCondition(VB *sim, int id) { switch (id) { @@ -883,12 +1042,22 @@ static void cpuAND(VB *sim) { cpuAdvance(sim, cpuClocks(1)); } +/* ANDBSU */ +static int cpuANDBSU(VB *sim) { + return cpuBSArithmetic(sim); +} + /* ANDI */ static void cpuANDI(VB *sim) { cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg1(sim) & cpuGetImm16U(sim))); cpuAdvance(sim, cpuClocks(1)); } +/* ANDNBSU */ +static int cpuANDNBSU(VB *sim) { + return cpuBSArithmetic(sim); +} + /* BCOND */ static void cpuBCOND(VB *sim) { if (cpuCondition(sim, cpuGetCond(sim))) @@ -1186,6 +1355,11 @@ static void cpuMOVReg(VB *sim) { cpuAdvance(sim, cpuClocks(1)); } +/* MOVBSU */ +static int cpuMOVBSU(VB *sim) { + return cpuBSArithmetic(sim); +} + /* MOVEA */ static void cpuMOVEA(VB *sim) { cpuSetReg2(sim, cpuGetReg1(sim) + cpuGetImm16S(sim)); @@ -1253,18 +1427,33 @@ static void cpuNOT(VB *sim) { cpuAdvance(sim, cpuClocks(1)); } +/* NOTBSU */ +static int cpuNOTBSU(VB *sim) { + return cpuBSArithmetic(sim); +} + /* OR */ static void cpuOR(VB *sim) { cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg2(sim) | cpuGetReg1(sim))); cpuAdvance(sim, cpuClocks(1)); } +/* ORBSU */ +static int cpuORBSU(VB *sim) { + return cpuBSArithmetic(sim); +} + /* ORI */ static void cpuORI(VB *sim) { cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg1(sim) | cpuGetImm16U(sim))); cpuAdvance(sim, cpuClocks(1)); } +/* ORNBSU */ +static int cpuORNBSU(VB *sim) { + return cpuBSArithmetic(sim); +} + /* OUT.B */ static int cpuOUT_B(VB *sim) { return cpuST_OUT(sim, VB_U8); @@ -1317,6 +1506,26 @@ static void cpuSARReg(VB *sim) { cpuAdvance(sim, cpuClocks(1)); } +/* SCH0BSD */ +static int cpuSCH0BSD(VB *sim) { + return cpuBSSearch(sim, 0, -1); +} + +/* SCH0BSU */ +static int cpuSCH0BSU(VB *sim) { + return cpuBSSearch(sim, 0, +1); +} + +/* SCH1BSD */ +static int cpuSCH1BSD(VB *sim) { + return cpuBSSearch(sim, 1, -1); +} + +/* SCH1BSU */ +static int cpuSCH1BSU(VB *sim) { + return cpuBSSearch(sim, 1, +1); +} + /* SEI */ static void cpuSEI(VB *sim) { sim->cpu.psw.id = 1; @@ -1447,12 +1656,22 @@ static void cpuXOR(VB *sim) { cpuAdvance(sim, cpuClocks(1)); } +/* XORBSU */ +static int cpuXORBSU(VB *sim) { + return cpuBSArithmetic(sim); +} + /* XORI */ static void cpuXORI(VB *sim) { cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg1(sim) ^ cpuGetImm16U(sim))); cpuAdvance(sim, cpuClocks(1)); } +/* XORNBSU */ +static int cpuXORNBSU(VB *sim) { + return cpuBSArithmetic(sim); +} + /***************************** Library Functions *****************************/ @@ -1488,7 +1707,9 @@ static int cpuEmulate(VB *sim, uint32_t clocks) { case CPU_ADDF_S : cpuADDF_S (sim); break; case CPU_ADDI : cpuADDI (sim); break; case CPU_AND : cpuAND (sim); break; + case CPU_ANDBSU : cpuANDBSU (sim); break; case CPU_ANDI : cpuANDI (sim); break; + case CPU_ANDNBSU: cpuANDNBSU(sim); break; case CPU_BCOND : cpuBCOND (sim); break; case CPU_CAXI : brk = cpuCAXI (sim); break; case CPU_CLI : cpuCLI (sim); break; @@ -1513,6 +1734,7 @@ static int cpuEmulate(VB *sim, uint32_t clocks) { case CPU_LDSR : cpuLDSR (sim); break; case CPU_MOV_IMM: cpuMOVImm (sim); break; case CPU_MOV_REG: cpuMOVReg (sim); break; + case CPU_MOVBSU : cpuMOVBSU (sim); break; case CPU_MOVEA : cpuMOVEA (sim); break; case CPU_MOVHI : cpuMOVHI (sim); break; case CPU_MPYHW : cpuMPYHW (sim); break; @@ -1520,8 +1742,11 @@ static int cpuEmulate(VB *sim, uint32_t clocks) { case CPU_MULF_S : cpuMULF_S (sim); break; case CPU_MULU : cpuMULU (sim); break; case CPU_NOT : cpuNOT (sim); break; + case CPU_NOTBSU : cpuNOTBSU (sim); break; case CPU_OR : cpuOR (sim); break; + case CPU_ORBSU : cpuORBSU (sim); break; case CPU_ORI : cpuORI (sim); break; + case CPU_ORNBSU : cpuORNBSU (sim); break; case CPU_OUT_B : brk = cpuOUT_B (sim); break; case CPU_OUT_H : brk = cpuOUT_H (sim); break; case CPU_OUT_W : brk = cpuOUT_W (sim); break; @@ -1529,6 +1754,10 @@ static int cpuEmulate(VB *sim, uint32_t clocks) { case CPU_REV : cpuREV (sim); break; case CPU_SAR_IMM: cpuSARImm (sim); break; case CPU_SAR_REG: cpuSARReg (sim); break; + case CPU_SCH0BSD: cpuSCH0BSD(sim); break; + case CPU_SCH0BSU: cpuSCH0BSU(sim); break; + case CPU_SCH1BSD: cpuSCH1BSD(sim); break; + case CPU_SCH1BSU: cpuSCH1BSU(sim); break; case CPU_SEI : cpuSEI (sim); break; case CPU_SETF : cpuSETF (sim); break; case CPU_SHL_IMM: cpuSHLImm (sim); break; @@ -1546,7 +1775,9 @@ static int cpuEmulate(VB *sim, uint32_t clocks) { case CPU_XB : cpuXB (sim); break; case CPU_XH : cpuXH (sim); break; case CPU_XOR : cpuXOR (sim); break; + case CPU_XORBSU : cpuXORBSU (sim); break; case CPU_XORI : cpuXORI (sim); break; + case CPU_XORNBSU: cpuXORNBSU(sim); break; default: return -1; /* TODO: Temporary for debugging */ } diff --git a/core/vb.c b/core/vb.c index 2431fc5..6934313 100644 --- a/core/vb.c +++ b/core/vb.c @@ -65,10 +65,14 @@ struct VB { /* Working data */ union { + struct { + uint32_t dest; + uint64_t src; + } bs; /* Arithmetic bit strings */ struct { uint32_t address; int32_t value; - } data; + } data; /* Data accesses */ } aux; /* Other state */