--- hatari/src/falcon/dsp_cpu.c 2019/04/09 08:47:23 1.1.1.4 +++ hatari/src/falcon/dsp_cpu.c 2019/04/09 08:48:48 1.1.1.5 @@ -83,6 +83,10 @@ static Uint32 tmp_parmove_len[2]; /* Ho static Uint32 tmp_parmove_type[2]; /* 0=register, 1=memory */ static Uint32 tmp_parmove_space[2]; /* Memory space to write to */ +#if defined(DSP_DISASM) && (DSP_DISASM_MEM==1) +static char str_disasm_memory[2][50]; /* Buffer for memory change text in disasm mode */ +static Uint16 disasm_memory_ptr; /* Pointer for memory change in disasm mode */ +#endif /********************************** * Functions @@ -93,6 +97,8 @@ typedef void (*dsp_emul_t)(void); static void dsp_postexecute_update_pc(void); static void dsp_postexecute_interrupts(void); +static void dsp_setInterruptIPL(Uint32 value); + static void dsp_ccr_update_e_u_n_z(Uint32 *reg0, Uint32 *reg1, Uint32 *reg2); static inline Uint32 read_memory_p(Uint16 address); @@ -217,7 +223,7 @@ static Uint16 dsp_asl56(Uint32 *dest); static Uint16 dsp_asr56(Uint32 *dest); static Uint16 dsp_add56(Uint32 *source, Uint32 *dest); static Uint16 dsp_sub56(Uint32 *source, Uint32 *dest); -static void dsp_mul56(Uint32 source1, Uint32 source2, Uint32 *dest); +static void dsp_mul56(Uint32 source1, Uint32 source2, Uint32 *dest, Uint8 signe); static void dsp_rnd56(Uint32 *dest); /* Instructions with parallel moves */ @@ -453,19 +459,21 @@ static int registers_mask[64]={ 16, 16, 16, 16 }; -static int interrupt_adress[13]={ - 0x0, 0x3e, 0x2, 0x4, - 0x6, 0xff, 0x20, 0x22, 0x0, - 0xe, 0xc, 0x12, 0x10 +static dsp_interrupt_t dsp_interrupt[12] = { + {DSP_INTER_RESET , 0x00, 0, "Reset"}, + {DSP_INTER_ILLEGAL , 0x3e, 0, "Illegal"}, + {DSP_INTER_STACK_ERROR , 0x02, 0, "Stack Error"}, + {DSP_INTER_TRACE , 0x04, 0, "Trace"}, + {DSP_INTER_SWI , 0x06, 0, "Swi"}, + {DSP_INTER_HOST_COMMAND , 0xff, 1, "Host Command"}, + {DSP_INTER_HOST_RCV_DATA, 0x20, 1, "Host receive"}, + {DSP_INTER_HOST_TRX_DATA, 0x22, 1, "Host transmit"}, + {DSP_INTER_SSI_RCV_DATA_E, 0x0e, 2, "SSI receive with exception"}, + {DSP_INTER_SSI_RCV_DATA , 0x0c, 2, "SSI receive"}, + {DSP_INTER_SSI_TRX_DATA_E, 0x12, 2, "SSI transmit with exception"}, + {DSP_INTER_SSI_TRX_DATA , 0x10, 2, "SSI tramsmit"} }; -#if DSP_DISASM_INTER -static const char *interrupt_label[13]={ - "Reset", "Illegal", "Stack Error", "Trace", - "Swi", "Host Command", "Host receive", "Host transmit", "", - "SSI receive with exception", "SSI receive", "SSI transmit with exception", "SSI tramsmit" -}; -#endif /********************************** * Emulator kernel @@ -490,12 +498,16 @@ void dsp56k_execute_instruction(void) #ifdef DSP_DISASM #if DSP_DISASM_REG - dsp56k_disasm_reg_read(); + dsp56k_disasm_reg_save(); #endif #if DSP_DISASM_INST dsp56k_disasm(); #endif +#if DSP_DISASM_MEM + disasm_memory_ptr = 0; +#endif #endif + /* Decode and execute current instruction */ cur_inst = read_memory_p(dsp_core->pc); cur_inst_len = 1; @@ -520,10 +532,6 @@ void dsp56k_execute_instruction(void) /* Process Interrupts */ dsp_postexecute_interrupts(); - /* process peripherals On Chip components */ - dsp_core_process_host_interface(dsp_core); - dsp_core_process_ssi_interface(dsp_core); - #if DSP_COUNT_IPS ++num_inst; if ((num_inst & 63) == 0) { @@ -538,9 +546,22 @@ void dsp56k_execute_instruction(void) #endif #ifdef DSP_DISASM +#if DSP_DISASM_INST + fprintf(stderr, "%s", dsp56k_getInstructionText()); +#endif #if DSP_DISASM_REG dsp56k_disasm_reg_compare(); #endif +#if DSP_DISASM_MEM + /* 1 memory change to display ? */ + if (disasm_memory_ptr == 1) + fprintf(stderr, "\t%s\n", str_disasm_memory[0]); + /* 2 memory changes to display ? */ + else if (disasm_memory_ptr == 2) { + fprintf(stderr, "\t%s\n", str_disasm_memory[0]); + fprintf(stderr, "\t%s\n", str_disasm_memory[1]); + } +#endif #endif } @@ -563,25 +584,17 @@ static void dsp_postexecute_update_pc(vo dsp_core->loop_rep = 0; dsp_core->registers[DSP_REG_LC] = dsp_core->registers[DSP_REG_LCSAVE]; } -#ifdef DSP_DISASM - dsp56k_disasm_force_reg_changed(DSP_REG_LC); -#endif } else { /* Init LC at right value */ if (dsp_core->registers[DSP_REG_LC] == 0) { dsp_core->registers[DSP_REG_LC] = 0x010000; -#ifdef DSP_DISASM - dsp56k_disasm_force_reg_changed(DSP_REG_LC); -#endif } dsp_core->pc_on_rep = 0; } } /* Normal execution, go to next instruction */ - if (cur_inst_len>0) { - dsp_core->pc += cur_inst_len; - } + dsp_core->pc += cur_inst_len; /* When running a DO loop, we test the end of loop with the */ /* updated PC, pointing to last instruction of the loop */ @@ -604,9 +617,6 @@ static void dsp_postexecute_update_pc(vo /* Loop one more time */ dsp_core->pc = dsp_core->registers[DSP_REG_SSH]; } -#ifdef DSP_DISASM - dsp56k_disasm_force_reg_changed(DSP_REG_LC); -#endif } } } @@ -615,9 +625,42 @@ static void dsp_postexecute_update_pc(vo * Interrupts **********************************/ +/* Post a new interrupt to the interrupt table */ +void dsp_add_interrupt(Uint16 inter) +{ + /* detect if this interrupt is used or not */ + if (dsp_core->interrupt_ipl[inter] == -1) + return; + + /* add this interrupt to the pending interrupts table */ + if (dsp_core->interrupt_isPending[inter] == 0) { + dsp_core->interrupt_isPending[inter] = 1; + dsp_core->interrupt_counter ++; + } +} + +static void dsp_setInterruptIPL(Uint32 value) +{ + Uint32 ipl_ssi, ipl_hi, i; + + ipl_ssi = ((value >> 12) & 3) - 1; + ipl_hi = ((value >> 10) & 3) - 1; + + /* set IPL_HI */ + for (i=5;i<8;i++) { + dsp_core->interrupt_ipl[i] = ipl_hi; + } + + /* set IPL_SSI */ + for (i=8;i<12;i++) { + dsp_core->interrupt_ipl[i] = ipl_ssi; + } +} + static void dsp_postexecute_interrupts(void) { - Uint32 ipl, ipl_to_raise, ipl_hi, ipl_ssi, instr1, instr2, i, count1, count2, ipl1, ipl2; + Uint32 index, instr, i; + Sint32 ipl_to_raise, ipl_sr; /* REP is not interruptible */ if (dsp_core->loop_rep) { @@ -625,22 +668,70 @@ static void dsp_postexecute_interrupts(v } /* A fast interrupt can not be interrupted. */ - if (dsp_core->interrupt_state == DSP_INTERRUPT_FAST) { - /* Did we execute the 2-inst interrupt of the vector ? */ - if (dsp_core->pc >= dsp_core->interrupt_instr_fetch+2) { - if (dsp_core->pc == dsp_core->interrupt_instr_fetch+2) { - dsp_core->pc = dsp_core->interrupt_save_pc; - } - dsp_core->interrupt_save_pc = -1; - dsp_core->interrupt_instr_fetch = -1; - dsp_core->interrupt_state = DSP_INTERRUPT_NONE; + if (dsp_core->interrupt_state == DSP_INTERRUPT_DISABLED) { + + switch (dsp_core->interrupt_pipeline_count) { + case 5: + dsp_core->interrupt_pipeline_count --; + return; + case 4: + /* Prefetch interrupt instruction 1 */ + dsp_core->interrupt_save_pc = dsp_core->pc; + dsp_core->pc = dsp_core->interrupt_instr_fetch; + + /* is it a LONG interrupt ? */ + instr = read_memory_p(dsp_core->interrupt_instr_fetch); + if ( ((instr & 0xfff000) == 0x0d0000) || ((instr & 0xffc0ff) == 0x0bc080) ) { + dsp_core->interrupt_state = DSP_INTERRUPT_LONG; + dsp_stack_push(dsp_core->interrupt_save_pc, dsp_core->registers[DSP_REG_SR], 0); + dsp_core->registers[DSP_REG_SR] &= BITMASK(16)-((1<registers[DSP_REG_SR] |= dsp_core->interrupt_IplToRaise<interrupt_pipeline_count --; + return; + case 3: + /* Prefetch interrupt instruction 2 */ + if (dsp_core->pc == dsp_core->interrupt_instr_fetch+1) { + instr = read_memory_p(dsp_core->pc); + if ( ((instr & 0xfff000) == 0x0d0000) || ((instr & 0xffc0ff) == 0x0bc080) ) { + dsp_core->interrupt_state = DSP_INTERRUPT_LONG; + dsp_stack_push(dsp_core->interrupt_save_pc, dsp_core->registers[DSP_REG_SR], 0); + dsp_core->registers[DSP_REG_SR] &= BITMASK(16)-((1<registers[DSP_REG_SR] |= dsp_core->interrupt_IplToRaise<interrupt_pipeline_count --; + return; + case 2: + /* 1 instruction executed after interrupt */ + /* before re enable interrupts */ + /* Was it a FAST interrupt ? */ + if (dsp_core->pc == dsp_core->interrupt_instr_fetch+2) { + dsp_core->pc = dsp_core->interrupt_save_pc; + } + dsp_core->interrupt_pipeline_count --; + return; + case 1: + /* Last instruction executed after interrupt */ + /* before re enable interrupts */ + dsp_core->interrupt_pipeline_count --; + return; + case 0: + /* Re enable interrupts */ + dsp_core->interrupt_save_pc = -1; + dsp_core->interrupt_instr_fetch = -1; + dsp_core->interrupt_state = DSP_INTERRUPT_NONE; + return; } - return; } /* Trace Interrupt ? */ if (dsp_core->registers[DSP_REG_SR] & (1<registers[DSP_REG_SR]>>DSP_SR_I0) & BITMASK(2); + index = 0xffff; + ipl_to_raise = -1; + + /* Arbitrate between all pending interrupts */ + for (i=0; i<12; i++) { + if (dsp_core->interrupt_isPending[i] == 1) { + + /* level 3 interrupt ? */ + if (dsp_core->interrupt_ipl[i] == 3) { + index = i; + break; + } - /* level 3 interrupt ? */ - for (i=0; i<5; i++) { - if (dsp_core->interrupt_table[i] != 0) { - ipl_to_raise = 3; - dsp_core->interrupt_instr_fetch = interrupt_adress[i]; - dsp_core->interrupt_table[i] = 0; - dsp_core->interrupt_counter --; -#if DSP_DISASM_INTER - fprintf(stderr, "Dsp: Interrupt: %s\n", interrupt_label[i]); -#endif - break; - } - } - - /* Level 2 and above interrupt ? */ - ipl = (dsp_core->registers[DSP_REG_SR]>>DSP_SR_I0) & BITMASK(2); - if ((ipl_to_raise==99) && (ipl<3)) { - ipl_hi = (dsp_core->periph[DSP_SPACE_X][DSP_IPR]>>10) & BITMASK(2); - ipl_ssi = (dsp_core->periph[DSP_SPACE_X][DSP_IPR]>>12) & BITMASK(2); - - /* Determine the order of the peripheral interrupt to test . - If 2 or more peripharal have the same interrupt level, - the order is (high priority) : HI, SSI, SCI (low priority) */ - if (ipl_hi >= ipl_ssi) { - ipl1 = ipl_hi; - ipl2 = ipl_ssi; - count1 = 5; - count2 = 9; - } else { - ipl1 = ipl_ssi; - ipl2 = ipl_hi; - count1 = 9; - count2 = 5; - } + /* level 0, 1 ,2 interrupt ? */ + /* if interrupt is masked in SR, don't process it */ + if (dsp_core->interrupt_ipl[i] < ipl_sr) + continue; - if (ipl1 >= ipl) { - for (i=count1; iinterrupt_table[i] != 0) { - ipl_to_raise = ipl1; - dsp_core->interrupt_instr_fetch = interrupt_adress[i]; - dsp_core->interrupt_table[i] = 0; - dsp_core->interrupt_counter --; -#if DSP_DISASM_INTER - fprintf(stderr, "Dsp: Interrupt: %s\n", interrupt_label[i]); -#endif - break; - } - } - } - if (ipl_to_raise == 99) { - if (ipl2>=ipl) { - for (i=count2; iinterrupt_table[i] != 0) { - ipl_to_raise = ipl2; - dsp_core->interrupt_instr_fetch = interrupt_adress[i]; - dsp_core->interrupt_table[i] = 0; - dsp_core->interrupt_counter --; -#if DSP_DISASM_INTER - fprintf(stderr, "Dsp: Interrupt: %s\n", interrupt_label[i]); -#endif - break; - } - } - } + /* if interrupt is lower or equal than current arbitrated interrupt */ + if (dsp_core->interrupt_ipl[i] <= ipl_to_raise) + continue; + + /* save current arbitrated interrupt */ + index = i; + ipl_to_raise = dsp_core->interrupt_ipl[i]; } } - if (ipl_to_raise == 99) { + /* If there's no interrupt to process, return */ + if (index == 0xffff) { return; } - /* SSI receive data with exception */ + /* remove this interrupt from the pending interrupts table */ + dsp_core->interrupt_isPending[index] = 0; + dsp_core->interrupt_counter --; + + /* process arbritrated interrupt */ + ipl_to_raise = dsp_core->interrupt_ipl[index] + 1; + if (ipl_to_raise > 3) { + ipl_to_raise = 3; + } + + dsp_core->interrupt_instr_fetch = dsp_interrupt[index].vectorAddr; + dsp_core->interrupt_pipeline_count = 5; + dsp_core->interrupt_state = DSP_INTERRUPT_DISABLED; + dsp_core->interrupt_IplToRaise = ipl_to_raise; + +#if DSP_DISASM_INTER + fprintf(stderr, "Dsp: Interrupt: %s\n", dsp_interrupt[index].name); +#endif + + /* SSI receive data with exception ? */ if (dsp_core->interrupt_instr_fetch == 0xe) { dsp_core->periph[DSP_SPACE_X][DSP_SSI_SR] &= 0xff-(1<interrupt_instr_fetch == 0x12) { dsp_core->periph[DSP_SPACE_X][DSP_SSI_SR] &= 0xff-(1<interrupt_instr_fetch = dsp_core->hostport[CPU_HOST_CVR] & BITMASK(5); dsp_core->interrupt_instr_fetch *= 2; } - - /* Read the 2 vectored instructions to search a "JSR" (long interrupt) or not (fast interrupt) */ - instr1 = read_memory_p(dsp_core->interrupt_instr_fetch); - instr2 = read_memory_p(dsp_core->interrupt_instr_fetch+1); - dsp_core->interrupt_state = DSP_INTERRUPT_FAST; - - if ( ((instr1 & 0xfff000) == 0x0d0000) || ((instr1 & 0xffc0ff) == 0x0bc080) ){ - dsp_core->interrupt_state = DSP_INTERRUPT_LONG; - } - - else if ( ((instr2 & 0xfff000) == 0x0d0000) || ((instr2 & 0xffc0ff) == 0x0bc080) ){ - dsp_core->interrupt_state = DSP_INTERRUPT_LONG; - } - - if (dsp_core->interrupt_state == DSP_INTERRUPT_FAST){ - /* Execute a fast interrupt */ - dsp_core->interrupt_save_pc = dsp_core->pc; - dsp_core->pc = dsp_core->interrupt_instr_fetch; -#if DSP_DISASM_INTER - fprintf(stderr, "Dsp: Fast Interrupt: %06x\n", dsp_core->pc); -#endif - } else { - /* Execute a long interrupt */ - dsp_stack_push(dsp_core->pc, dsp_core->registers[DSP_REG_SR], 0); - dsp_core->registers[DSP_REG_SR] &= BITMASK(16)-((1<registers[DSP_REG_SR] |= ipl_to_raise<pc = dsp_core->interrupt_instr_fetch; - dsp_core->interrupt_instr_fetch = -1; - dsp_core->interrupt_save_pc = -1; -#if DSP_DISASM_INTER - fprintf(stderr, "Dsp: Long Interrupt: %06x\n", dsp_core->pc); -#endif - } } /********************************** @@ -975,6 +1012,7 @@ static void write_memory_raw(int space, break; case DSP_SSI_CRA: case DSP_SSI_CRB: + dsp_core->periph[DSP_SPACE_X][address-0xffc0] = value; dsp_core_ssi_configure(dsp_core, address-0xffc0, value); break; case DSP_SSI_TSR: @@ -983,6 +1021,14 @@ static void write_memory_raw(int space, case DSP_SSI_TX: dsp_core_ssi_writeTX(dsp_core, value); break; + case DSP_IPR: + dsp_core->periph[DSP_SPACE_X][DSP_IPR] = value; + dsp_setInterruptIPL(value); + break; + case DSP_PCD: + dsp_core->periph[DSP_SPACE_X][DSP_PCD] = value; + dsp_core_setPortCDataRegister(dsp_core, value); + break; default: dsp_core->periph[DSP_SPACE_X][address-0xffc0] = value; break; @@ -1066,25 +1112,25 @@ static void write_memory_disasm(int spac } else { curvalue = read_memory_disasm(space, address); } - - fprintf(stderr,"Dsp: Mem: %c:0x%04x:0x%06x -> 0x%06x\n", space_c, address, oldvalue, curvalue); + sprintf(str_disasm_memory[disasm_memory_ptr],"Mem: %c:0x%04x 0x%06x -> 0x%06x", space_c, address, oldvalue, curvalue); + disasm_memory_ptr ++; } #endif static void dsp_write_reg(Uint32 numreg, Uint32 value) { - Uint32 reg = numreg; + Uint32 stack_error; - if ((reg==DSP_REG_A) || (reg==DSP_REG_B)) { - reg &= 1; - dsp_core->registers[DSP_REG_A0+reg]= 0; - dsp_core->registers[DSP_REG_A1+reg]= value; - dsp_core->registers[DSP_REG_A2+reg]= 0; + if ((numreg==DSP_REG_A) || (numreg==DSP_REG_B)) { + numreg &= 1; + dsp_core->registers[DSP_REG_A0+numreg]= 0; + dsp_core->registers[DSP_REG_A1+numreg]= value; + dsp_core->registers[DSP_REG_A2+numreg]= 0; if (value & (1<<23)) { - dsp_core->registers[DSP_REG_A2+reg]= 0xff; + dsp_core->registers[DSP_REG_A2+numreg]= 0xff; } } else { - switch (reg) { + switch (numreg) { case DSP_REG_OMR: dsp_core->registers[DSP_REG_OMR] = value & 0xc7; break; @@ -1092,6 +1138,12 @@ static void dsp_write_reg(Uint32 numreg, dsp_core->registers[DSP_REG_SR] = value & 0xaf7f; break; case DSP_REG_SP: + stack_error = dsp_core->registers[DSP_REG_SP] & (1<registers[DSP_REG_SP] = value & BITMASK(6); dsp_compute_ssh_ssl(); break; @@ -1099,16 +1151,16 @@ static void dsp_write_reg(Uint32 numreg, dsp_stack_push(value, 0, 1); break; case DSP_REG_SSL: - reg = dsp_core->registers[DSP_REG_SP] & BITMASK(4); - if (reg == 0) { + numreg = dsp_core->registers[DSP_REG_SP] & BITMASK(4); + if (numreg == 0) { value = 0; } - dsp_core->stack[1][reg] = value & BITMASK(16); + dsp_core->stack[1][numreg] = value & BITMASK(16); dsp_core->registers[DSP_REG_SSL] = value & BITMASK(16); break; default: - dsp_core->registers[reg] = value; - dsp_core->registers[reg] &= BITMASK(registers_mask[reg]); + dsp_core->registers[numreg] = value; + dsp_core->registers[numreg] &= BITMASK(registers_mask[numreg]); break; } } @@ -1129,7 +1181,7 @@ static void dsp_stack_push(Uint32 curpc, if ((stack_error==0) && (stack & (1<pc+cur_inst_len, dsp_core->registers[DSP_REG_SR], 0); } else { - dsp_core->interrupt_state = DSP_INTERRUPT_NONE; + dsp_core->interrupt_state = DSP_INTERRUPT_DISABLED; } dsp_core->pc = newpc; @@ -2476,7 +2528,7 @@ static void dsp_jsr_ea(void) dsp_stack_push(dsp_core->pc+cur_inst_len, dsp_core->registers[DSP_REG_SR], 0); } else { - dsp_core->interrupt_state = DSP_INTERRUPT_NONE; + dsp_core->interrupt_state = DSP_INTERRUPT_DISABLED; } dsp_core->pc = newpc; @@ -3087,8 +3139,6 @@ static void dsp_stop(void) static void dsp_swi(void) { /* Raise interrupt p:0x0006 */ - dsp_core_add_interrupt(dsp_core, DSP_INTER_SWI); - dsp_core->instr_cycle += 6; } @@ -3945,20 +3995,17 @@ static Uint16 dsp_asr56(Uint32 *dest) static Uint16 dsp_add56(Uint32 *source, Uint32 *dest) { Uint16 overflow, carry, flg_s, flg_d, flg_r; - Uint32 not_dest[3]; flg_s = (source[0]>>7) & 1; flg_d = (dest[0]>>7) & 1; - not_dest[2] = ((Uint32)(~dest[2])) & BITMASK(24); - not_dest[1] = ((Uint32)(~dest[1])) & BITMASK(24); - not_dest[0] = ((Uint32)(~dest[0])) & BITMASK(8); - /* Add source to dest: D = D+S */ dest[2] += source[2]; dest[1] += source[1]+((dest[2]>>24) & 1); dest[0] += source[0]+((dest[1]>>24) & 1); + carry = (dest[0]>>8) & 1; + dest[2] &= BITMASK(24); dest[1] &= BITMASK(24); dest[0] &= BITMASK(8); @@ -3968,80 +4015,47 @@ static Uint16 dsp_add56(Uint32 *source, /*set overflow*/ overflow = (flg_s ^ flg_r) & (flg_d ^ flg_r); - /* set carry : carry (for a add) = ~dest < source */ - carry = 0; - - if (not_dest[0] < source[0]) { - carry = 1; - } else if (not_dest[0] == source[0]) { - if (not_dest[1] < source[1]) { - carry = 1; - } else if (not_dest[1] == source[1]) { - if (not_dest[2] < source[2]) { - carry = 1; - } - } - } - return (overflow<>24) & 1); dest[0] -= source[0]+((dest[1]>>24) & 1); + carry = (dest[0]>>8) & 1; + dest[2] &= BITMASK(24); dest[1] &= BITMASK(24); dest[0] &= BITMASK(8); flg_s = (source[0]>>7) & 1; - flg_d = (dest_save[0]>>7) & 1; + flg_d = (dest_save>>7) & 1; flg_r = (dest[0]>>7) & 1; /* set overflow */ overflow = (flg_s ^ flg_d) & (flg_r ^ flg_d); - /* set carry : carry (for a sub) = dest < source */ - carry = 0; - - if (dest_save[0] < source[0]) { - carry = 1; - } else if (dest_save[0] == source[0]) { - if (dest_save[1] < source[1]) { - carry = 1; - } else if (dest_save[1] == source[1]) { - if (dest_save[2] < source[2]) { - carry = 1; - } - } - } - return (overflow<registers[srcreg1], dsp_core->registers[srcreg2], source); - - if (cur_inst & (1<<2)) { - dest[0] = dest[1] = dest[2] = 0; - - dsp_sub56(source, dest); - - source[0] = dest[0]; - source[1] = dest[1]; - source[2] = dest[2]; - } + dsp_mul56(dsp_core->registers[srcreg1], dsp_core->registers[srcreg2], source, (cur_inst >> 2) & 1); destreg = (cur_inst>>3) & 1; dest[0] = dsp_core->registers[DSP_REG_A2+destreg]; @@ -4670,17 +4674,7 @@ static void dsp_macr(void) srcreg1 = registers_mpy[value][0]; srcreg2 = registers_mpy[value][1]; - dsp_mul56(dsp_core->registers[srcreg1], dsp_core->registers[srcreg2], source); - - if (cur_inst & (1<<2)) { - dest[0] = dest[1] = dest[2] = 0; - - dsp_sub56(source, dest); - - source[0] = dest[0]; - source[1] = dest[1]; - source[2] = dest[2]; - } + dsp_mul56(dsp_core->registers[srcreg1], dsp_core->registers[srcreg2], source, (cur_inst >> 2) & 1); destreg = (cur_inst>>3) & 1; dest[0] = dsp_core->registers[DSP_REG_A2+destreg]; @@ -4708,28 +4702,19 @@ static void dsp_move(void) static void dsp_mpy(void) { - Uint32 srcreg1, srcreg2, destreg, value, dest[3], source[3]; + Uint32 srcreg1, srcreg2, destreg, value, source[3]; value = (cur_inst>>4) & BITMASK(3); srcreg1 = registers_mpy[value][0]; srcreg2 = registers_mpy[value][1]; - dsp_mul56(dsp_core->registers[srcreg1], dsp_core->registers[srcreg2], source); + dsp_mul56(dsp_core->registers[srcreg1], dsp_core->registers[srcreg2], source, (cur_inst >> 2) & 1); destreg = (cur_inst>>3) & 1; - if (cur_inst & (1<<2)) { - dest[0] = dest[1] = dest[2] = 0; - - dsp_sub56(source, dest); - dsp_core->registers[DSP_REG_A2+destreg] = dest[0]; - dsp_core->registers[DSP_REG_A1+destreg] = dest[1]; - dsp_core->registers[DSP_REG_A0+destreg] = dest[2]; - } else { - dsp_core->registers[DSP_REG_A2+destreg] = source[0]; - dsp_core->registers[DSP_REG_A1+destreg] = source[1]; - dsp_core->registers[DSP_REG_A0+destreg] = source[2]; - } + dsp_core->registers[DSP_REG_A2+destreg] = source[0]; + dsp_core->registers[DSP_REG_A1+destreg] = source[1]; + dsp_core->registers[DSP_REG_A0+destreg] = source[2]; dsp_ccr_update_e_u_n_z( &dsp_core->registers[DSP_REG_A2+destreg], &dsp_core->registers[DSP_REG_A1+destreg], @@ -4746,18 +4731,13 @@ static void dsp_mpyr(void) srcreg1 = registers_mpy[value][0]; srcreg2 = registers_mpy[value][1]; - dsp_mul56(dsp_core->registers[srcreg1], dsp_core->registers[srcreg2], source); + dsp_mul56(dsp_core->registers[srcreg1], dsp_core->registers[srcreg2], source, (cur_inst >> 2) & 1); destreg = (cur_inst>>3) & 1; - if (cur_inst & (1<<2)) { - dest[0] = dest[1] = dest[2] = 0; - dsp_sub56(source, dest); - } else { - dest[0] = source[0]; - dest[1] = source[1]; - dest[2] = source[2]; - } + dest[0] = source[0]; + dest[1] = source[1]; + dest[2] = source[2]; dsp_rnd56(dest);