1.1 --- a/src/aica/armcore.c	Mon Jan 02 23:07:17 2006 +0000
     1.2 +++ b/src/aica/armcore.c	Tue Jan 10 13:59:19 2006 +0000
     1.3 @@ -1,5 +1,5 @@
     1.4  /**
     1.5 - * $Id: armcore.c,v 1.15 2006-01-02 23:07:17 nkeynes Exp $
     1.6 + * $Id: armcore.c,v 1.16 2006-01-10 13:56:54 nkeynes Exp $
     1.7   * 
     1.8   * ARM7TDMI CPU emulation core.
     1.9   *
    1.10 @@ -155,6 +155,25 @@
    1.11  }
    1.12  
    1.13  /**
    1.14 + * Return a pointer to the specified register in the user bank,
    1.15 + * regardless of the active bank
    1.16 + */
    1.17 +static uint32_t *arm_user_reg( int reg )
    1.18 +{
    1.19 +    if( IS_EXCEPTION_MODE() ) {
    1.20 +	if( reg == 13 || reg == 14 )
    1.21 +	    return &armr.user_r[reg-8];
    1.22 +	if( IS_FIQ_MODE() ) {
    1.23 +	    if( reg >= 8 || reg <= 12 )
    1.24 +		return &armr.user_r[reg-8];
    1.25 +	}
    1.26 +    }
    1.27 +    return &armr.r[reg];
    1.28 +}
    1.29 +
    1.30 +#define USER_R(n) *arm_user_reg(n)
    1.31 +
    1.32 +/**
    1.33   * Set the CPSR to the specified value.
    1.34   *
    1.35   * @param value values to set in CPSR
    1.36 @@ -661,6 +680,7 @@
    1.37      uint32_t pc;
    1.38      uint32_t ir;
    1.39      uint32_t operand, operand2, tmp, tmp2, cond;
    1.40 +    int i;
    1.41  
    1.42      tmp = armr.int_pending & (~armr.cpsr);
    1.43      if( tmp ) {
    1.44 @@ -1121,95 +1141,181 @@
    1.45  	    }
    1.46  	    armr.r[15] = pc + 4 + operand;
    1.47  	} else { /* Load/store multiple */
    1.48 -	    int prestep, poststep;
    1.49 -	    if( PFLAG(ir) ) {
    1.50 -		prestep = UFLAG(ir) ? 4 : -4;
    1.51 -		poststep = 0 ;
    1.52 -	    } else {
    1.53 -		prestep = 0;
    1.54 -		poststep = UFLAG(ir) ? 4 : -4;
    1.55 -	    }
    1.56  	    operand = RN(ir);
    1.57 -	    if( BFLAG(ir) ) { 
    1.58 -		/* Actually S - bit 22. Means "make massively complicated" */
    1.59 -		if( LFLAG(ir) && (ir&0x00008000) ) {
    1.60 -		    /* LDM (3). Much like normal LDM but also copies SPSR
    1.61 -		     * back to CPSR */
    1.62 -		    for( tmp=0; tmp < 16; tmp++ ) {
    1.63 -			if( (ir & (1<<tmp)) ) {
    1.64 -			    operand += prestep;
    1.65 -			    armr.r[tmp] = arm_read_long(operand);
    1.66 -			    operand += poststep;
    1.67 +	    
    1.68 +	    switch( (ir & 0x01D00000) >> 20 ) {
    1.69 +	    case 0: /* STMDA */
    1.70 +		for( i=15; i>= 0; i-- ) {
    1.71 +		    if( (ir & (1<<i)) ) {
    1.72 +			arm_write_long( operand, armr.r[i] );
    1.73 +			operand -= 4;
    1.74 +		    }
    1.75 +		}
    1.76 +		break;
    1.77 +	    case 1: /* LDMDA */
    1.78 +		for( i=15; i>= 0; i-- ) {
    1.79 +		    if( (ir & (1<<i)) ) {
    1.80 +			armr.r[i] = arm_read_long( operand );
    1.81 +			operand -= 4;
    1.82 +		    }
    1.83 +		}
    1.84 +		break;
    1.85 +	    case 4: /* STMDA (S) */
    1.86 +		for( i=15; i>= 0; i-- ) {
    1.87 +		    if( (ir & (1<<i)) ) {
    1.88 +			arm_write_long( operand, USER_R(i) );
    1.89 +			operand -= 4;
    1.90 +		    }
    1.91 +		}
    1.92 +		break;
    1.93 +	    case 5: /* LDMDA (S) */
    1.94 +		if( (ir&0x00008000) ) { /* Load PC */
    1.95 +		    for( i=15; i>= 0; i-- ) {
    1.96 +			if( (ir & (1<<i)) ) {
    1.97 +			    armr.r[i] = arm_read_long( operand );
    1.98 +			    operand -= 4;
    1.99  			}
   1.100  		    }
   1.101 -		    if( WFLAG(ir) ) 
   1.102 -			LRN(ir) = operand;
   1.103  		    arm_restore_cpsr();
   1.104 -		    if( armr.t ) PC &= 0xFFFFFFFE;
   1.105 -		    else PC &= 0xFFFFFFFC;
   1.106  		} else {
   1.107 -		    /* LDM/STM (2). As normal LDM but accesses the User banks
   1.108 -		     * instead of the active ones. Aka the truly evil case
   1.109 -		     */
   1.110 -		    int bank_start;
   1.111 -		    if( IS_FIQ_MODE() )
   1.112 -			bank_start = 8;
   1.113 -		    else if( IS_EXCEPTION_MODE() )
   1.114 -			bank_start = 13;
   1.115 -		    else bank_start = 15;
   1.116 -		    for( tmp=0; tmp<bank_start; tmp++ ) {
   1.117 -			if( (ir & (1<<tmp)) ) {
   1.118 -			    operand += prestep;
   1.119 -			    if( LFLAG(ir) ) {
   1.120 -				armr.r[tmp] = arm_read_long(operand);
   1.121 -			    } else {
   1.122 -				arm_write_long( operand, armr.r[tmp] );
   1.123 -			    }
   1.124 -			    operand += poststep;
   1.125 +		    for( i=15; i>= 0; i-- ) {
   1.126 +			if( (ir & (1<<i)) ) {
   1.127 +			    USER_R(i) = arm_read_long( operand );
   1.128 +			    operand -= 4;
   1.129  			}
   1.130  		    }
   1.131 -		    for( ; tmp < 15; tmp ++ ) {
   1.132 -			if( (ir & (1<<tmp)) ) {
   1.133 -			    operand += prestep;
   1.134 -			    if( LFLAG(ir) ) {
   1.135 -				armr.user_r[tmp-8] = arm_read_long(operand);
   1.136 -			    } else {
   1.137 -				arm_write_long( operand, armr.user_r[tmp-8] );
   1.138 -			    }
   1.139 -			    operand += poststep;
   1.140 +		}
   1.141 +		break;
   1.142 +	    case 8: /* STMIA */
   1.143 +		for( i=0; i< 16; i++ ) {
   1.144 +		    if( (ir & (1<<i)) ) {
   1.145 +			arm_write_long( operand, armr.r[i] );
   1.146 +			operand += 4;
   1.147 +		    }
   1.148 +		}
   1.149 +		break;
   1.150 +	    case 9: /* LDMIA */
   1.151 +		for( i=0; i< 16; i++ ) {
   1.152 +		    if( (ir & (1<<i)) ) {
   1.153 +			armr.r[i] = arm_read_long( operand );
   1.154 +			operand += 4;
   1.155 +		    }
   1.156 +		}
   1.157 +		break;
   1.158 +	    case 12: /* STMIA (S) */
   1.159 +		for( i=0; i< 16; i++ ) {
   1.160 +		    if( (ir & (1<<i)) ) {
   1.161 +			arm_write_long( operand, USER_R(i) );
   1.162 +			operand += 4;
   1.163 +		    }
   1.164 +		}
   1.165 +		break;
   1.166 +	    case 13: /* LDMIA (S) */
   1.167 +		if( (ir&0x00008000) ) { /* Load PC */
   1.168 +		    for( i=0; i < 16; i++ ) {
   1.169 +			if( (ir & (1<<i)) ) {
   1.170 +			    armr.r[i] = arm_read_long( operand );
   1.171 +			    operand += 4;
   1.172  			}
   1.173  		    }
   1.174 -		    if( ir & 0x8000 ) {
   1.175 -			operand += prestep;
   1.176 -			if( LFLAG(ir) ) {
   1.177 -			    /* Actually can't happen, but anyway... */
   1.178 -			    armr.r[15] = arm_read_long(operand);
   1.179 -			} else {
   1.180 -			    arm_write_long( operand, armr.r[15]+ STM_R15_OFFSET - 4 );
   1.181 +		    arm_restore_cpsr();
   1.182 +		} else {
   1.183 +		    for( i=0; i < 16; i++ ) {
   1.184 +			if( (ir & (1<<i)) ) {
   1.185 +			    USER_R(i) = arm_read_long( operand );
   1.186 +			    operand += 4;
   1.187  			}
   1.188 -			operand += poststep;
   1.189  		    }
   1.190  		}
   1.191 -	    } else {
   1.192 -		/* Normal LDM/STM */
   1.193 -		for( tmp=0; tmp < 16; tmp++ ) {
   1.194 -		    if( (ir & (1<<tmp)) ) {
   1.195 -			operand += prestep;
   1.196 -			if( LFLAG(ir) ) {
   1.197 -			    armr.r[tmp] = arm_read_long(operand);
   1.198 -			} else {
   1.199 -			    if( tmp == 15 )
   1.200 -				arm_write_long( operand, 
   1.201 -						armr.r[15] + STM_R15_OFFSET - 4 );
   1.202 -			    else
   1.203 -				arm_write_long( operand, armr.r[tmp] );
   1.204 -			}
   1.205 -			operand += poststep;
   1.206 +		break;
   1.207 +	    case 16: /* STMDB */
   1.208 +		for( i=15; i>= 0; i-- ) {
   1.209 +		    if( (ir & (1<<i)) ) {
   1.210 +			operand -= 4;
   1.211 +			arm_write_long( operand, armr.r[i] );
   1.212  		    }
   1.213  		}
   1.214 -		if( WFLAG(ir) ) 
   1.215 -		    LRN(ir) = operand;
   1.216 +		break;
   1.217 +	    case 17: /* LDMDB */
   1.218 +		for( i=15; i>= 0; i-- ) {
   1.219 +		    if( (ir & (1<<i)) ) {
   1.220 +			operand -= 4;
   1.221 +			armr.r[i] = arm_read_long( operand );
   1.222 +		    }
   1.223 +		}
   1.224 +		break;
   1.225 +	    case 20: /* STMDB (S) */
   1.226 +		for( i=15; i>= 0; i-- ) {
   1.227 +		    if( (ir & (1<<i)) ) {
   1.228 +			operand -= 4;
   1.229 +			arm_write_long( operand, USER_R(i) );
   1.230 +		    }
   1.231 +		}
   1.232 +		break;
   1.233 +	    case 21: /* LDMDB (S) */
   1.234 +		if( (ir&0x00008000) ) { /* Load PC */
   1.235 +		    for( i=15; i>= 0; i-- ) {
   1.236 +			if( (ir & (1<<i)) ) {
   1.237 +			    operand -= 4;
   1.238 +			    armr.r[i] = arm_read_long( operand );
   1.239 +			}
   1.240 +		    }
   1.241 +		    arm_restore_cpsr();
   1.242 +		} else {
   1.243 +		    for( i=15; i>= 0; i-- ) {
   1.244 +			if( (ir & (1<<i)) ) {
   1.245 +			    operand -= 4;
   1.246 +			    USER_R(i) = arm_read_long( operand );
   1.247 +			}
   1.248 +		    }
   1.249 +		}
   1.250 +		break;
   1.251 +	    case 24: /* STMIB */
   1.252 +		for( i=0; i< 16; i++ ) {
   1.253 +		    if( (ir & (1<<i)) ) {
   1.254 +			operand += 4;
   1.255 +			arm_write_long( operand, armr.r[i] );
   1.256 +		    }
   1.257 +		}
   1.258 +		break;
   1.259 +	    case 25: /* LDMIB */
   1.260 +		for( i=0; i< 16; i++ ) {
   1.261 +		    if( (ir & (1<<i)) ) {
   1.262 +			operand += 4;
   1.263 +			armr.r[i] = arm_read_long( operand );
   1.264 +		    }
   1.265 +		}
   1.266 +		break;
   1.267 +	    case 28: /* STMIB (S) */
   1.268 +		for( i=0; i< 16; i++ ) {
   1.269 +		    if( (ir & (1<<i)) ) {
   1.270 +			operand += 4;
   1.271 +			arm_write_long( operand, USER_R(i) );
   1.272 +		    }
   1.273 +		}
   1.274 +		break;
   1.275 +	    case 29: /* LDMIB (S) */
   1.276 +		if( (ir&0x00008000) ) { /* Load PC */
   1.277 +		    for( i=0; i < 16; i++ ) {
   1.278 +			if( (ir & (1<<i)) ) {
   1.279 +			    operand += 4;
   1.280 +			    armr.r[i] = arm_read_long( operand );
   1.281 +			}
   1.282 +		    }
   1.283 +		    arm_restore_cpsr();
   1.284 +		} else {
   1.285 +		    for( i=0; i < 16; i++ ) {
   1.286 +			if( (ir & (1<<i)) ) {
   1.287 +			    operand += 4;
   1.288 +			    USER_R(i) = arm_read_long( operand );
   1.289 +			}
   1.290 +		    }
   1.291 +		}
   1.292 +		break;
   1.293  	    }
   1.294 +	    
   1.295 +	    if( WFLAG(ir) ) 
   1.296 +		LRN(ir) = operand;
   1.297  	}
   1.298  	break;
   1.299      case 3: /* Copro */