arm7_9_common.c 79.3 KB
Newer Older
1001
1002
		/* program EmbeddedICE Debug Control Register to assert DBGRQ
		 */
oharboe's avatar
oharboe committed
1003
1004
1005
		if (arm7_9->set_special_dbgrq) {
			arm7_9->set_special_dbgrq(target);
		} else {
1006
			buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 1);
oharboe's avatar
oharboe committed
1007
1008
			embeddedice_store_reg(dbg_ctrl);
		}
1009
1010
1011
1012
1013
1014
1015
	}
	else
	{
		/* program watchpoint unit to match on any address
		 */
		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1016
1017
		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1018
1019
1020
	}

	target->debug_reason = DBG_REASON_DBGRQ;
1021

1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
	return ERROR_OK;
}

int arm7_9_debug_entry(target_t *target)
{
	int i;
	u32 context[16];
	u32* context_p[16];
	u32 r0_thumb, pc_thumb;
	u32 cpsr;
	int retval;
	/* get pointers to arch-specific information */
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];

#ifdef _DEBUG_ARM7_9_
1040
	LOG_DEBUG("-");
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
#endif

	if (arm7_9->pre_debug_entry)
		arm7_9->pre_debug_entry(target);

	/* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
	 * ensure that DBGRQ is cleared
	 */
	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
	embeddedice_store_reg(dbg_ctrl);
1053

1054
	arm7_9_clear_halt(target);
1055

1056
1057
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1058
		return retval;
1059
1060
1061
1062
1063
1064
1065
1066
	}

	if ((retval = arm7_9->examine_debug_reason(target)) != ERROR_OK)
		return retval;


	if (target->state != TARGET_HALTED)
	{
1067
		LOG_WARNING("target not halted");
1068
1069
		return ERROR_TARGET_NOT_HALTED;
	}
1070

1071
1072
1073
	/* if the target is in Thumb state, change to ARM state */
	if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
	{
1074
		LOG_DEBUG("target entered debug from Thumb state");
1075
1076
1077
		/* Entered debug from Thumb mode */
		armv4_5->core_state = ARMV4_5_STATE_THUMB;
		arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
1078
		LOG_DEBUG("r0_thumb: 0x%8.8x, pc_thumb: 0x%8.8x", r0_thumb, pc_thumb);
1079
1080
1081
	}
	else
	{
1082
		LOG_DEBUG("target entered debug from ARM state");
1083
1084
1085
		/* Entered debug from ARM mode */
		armv4_5->core_state = ARMV4_5_STATE_ARM;
	}
1086

1087
1088
1089
1090
1091
1092
	for (i = 0; i < 16; i++)
		context_p[i] = &context[i];
	/* save core registers (r0 - r15 of current core mode) */
	arm7_9->read_core_regs(target, 0xffff, context_p);

	arm7_9->read_xpsr(target, &cpsr, 0);
1093

1094
1095
	if ((retval = jtag_execute_queue()) != ERROR_OK)
		return retval;
1096

1097
1098
	/* if the core has been executing in Thumb state, set the T bit */
	if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1099
1100
		cpsr |= 0x20;

1101
1102
1103
	buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, cpsr);
	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1104

1105
	armv4_5->core_mode = cpsr & 0x1f;
1106

1107
1108
1109
	if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
	{
		target->state = TARGET_UNKNOWN;
1110
		LOG_ERROR("cpsr contains invalid mode value - communication failure");
1111
1112
		return ERROR_TARGET_FAILURE;
	}
1113

1114
	LOG_DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);
1115

1116
1117
	if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
	{
1118
		LOG_DEBUG("thumb state, applying fixups");
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
		context[0] = r0_thumb;
		context[15] = pc_thumb;
	} else if (armv4_5->core_state == ARMV4_5_STATE_ARM)
	{
		/* adjust value stored by STM */
		context[15] -= 3 * 4;
	}

	if ((target->debug_reason == DBG_REASON_BREAKPOINT)
			|| (target->debug_reason == DBG_REASON_SINGLESTEP)
			|| (target->debug_reason == DBG_REASON_WATCHPOINT)
			|| (target->debug_reason == DBG_REASON_WPTANDBKPT)
			|| ((target->debug_reason == DBG_REASON_DBGRQ) && (arm7_9->use_dbgrq == 0)))
		context[15] -= 3 * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
	else if (target->debug_reason == DBG_REASON_DBGRQ)
		context[15] -= arm7_9->dbgreq_adjust_pc * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
	else
	{
1137
		LOG_ERROR("unknown debug reason: %i", target->debug_reason);
1138
1139
	}

1140
1141
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1142

1143
1144
	for (i=0; i<=15; i++)
	{
1145
		LOG_DEBUG("r%i: 0x%8.8x", i, context[i]);
1146
1147
1148
1149
		buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, context[i]);
		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;
		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
	}
1150

1151
	LOG_DEBUG("entered debug state at PC 0x%x", context[15]);
1152

1153
1154
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1155
1156

	/* exceptions other than USR & SYS have a saved program status register */
1157
	if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
	{
		u32 spsr;
		arm7_9->read_xpsr(target, &spsr, 1);
		jtag_execute_queue();
		buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, spsr);
		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;
		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;
	}

	/* r0 and r15 (pc) have to be restored later */
1168
1169
	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).valid;
	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).valid;
1170

1171
	if ((retval = jtag_execute_queue()) != ERROR_OK)
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
		return retval;

	if (arm7_9->post_debug_entry)
		arm7_9->post_debug_entry(target);

	return ERROR_OK;
}

int arm7_9_full_context(target_t *target)
{
	int i;
	int retval;
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;

1187
	LOG_DEBUG("-");
1188

1189
1190
	if (target->state != TARGET_HALTED)
	{
1191
		LOG_WARNING("target not halted");
1192
1193
		return ERROR_TARGET_NOT_HALTED;
	}
1194

1195
1196
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206

	/* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
	 * SYS shares registers with User, so we don't touch SYS
	 */
	for(i = 0; i < 6; i++)
	{
		u32 mask = 0;
		u32* reg_p[16];
		int j;
		int valid = 1;
1207
1208

		/* check if there are invalid registers in the current mode
1209
1210
1211
1212
1213
1214
		 */
		for (j = 0; j <= 16; j++)
		{
			if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
				valid = 0;
		}
1215

1216
1217
1218
		if (!valid)
		{
			u32 tmp_cpsr;
1219

1220
			/* change processor mode (and mask T bit) */
1221
1222
			tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
			tmp_cpsr |= armv4_5_number_to_mode(i);
1223
			tmp_cpsr &= ~0x20;
1224
1225
1226
1227
1228
			arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);

			for (j = 0; j < 15; j++)
			{
				if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1229
				{
1230
1231
1232
1233
1234
1235
					reg_p[j] = (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value;
					mask |= 1 << j;
					ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;
					ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
				}
			}
1236

1237
1238
1239
			/* if only the PSR is invalid, mask is all zeroes */
			if (mask)
				arm7_9->read_core_regs(target, mask, reg_p);
1240

1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
			/* check if the PSR has to be read */
			if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid == 0)
			{
				arm7_9->read_xpsr(target, (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).value, 1);
				ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;
				ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
			}
		}
	}

1251
1252
	/* restore processor mode (mask T bit) */
	arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1253

1254
1255
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1256
		return retval;
1257
1258
1259
1260
1261
1262
1263
1264
	}
	return ERROR_OK;
}

int arm7_9_restore_context(target_t *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1265
	reg_t *reg;
1266
1267
1268
1269
1270
	armv4_5_core_reg_t *reg_arch_info;
	enum armv4_5_mode current_mode = armv4_5->core_mode;
	int i, j;
	int dirty;
	int mode_change;
1271

1272
	LOG_DEBUG("-");
1273

1274
1275
	if (target->state != TARGET_HALTED)
	{
1276
		LOG_WARNING("target not halted");
1277
1278
		return ERROR_TARGET_NOT_HALTED;
	}
1279

1280
1281
	if (arm7_9->pre_restore_context)
		arm7_9->pre_restore_context(target);
1282

1283
1284
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1285

1286
1287
1288
1289
1290
	/* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
	 * SYS shares registers with User, so we don't touch SYS
	 */
	for (i = 0; i < 6; i++)
	{
1291
		LOG_DEBUG("examining %s mode", armv4_5_mode_strings[i]);
1292
1293
		dirty = 0;
		mode_change = 0;
1294
		/* check if there are dirty registers in the current mode
1295
1296
1297
1298
1299
1300
1301
1302
1303
1304
		*/
		for (j = 0; j <= 16; j++)
		{
			reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
			reg_arch_info = reg->arch_info;
			if (reg->dirty == 1)
			{
				if (reg->valid == 1)
				{
					dirty = 1;
1305
					LOG_DEBUG("examining dirty reg: %s", reg->name);
1306
1307
					if ((reg_arch_info->mode != ARMV4_5_MODE_ANY)
						&& (reg_arch_info->mode != current_mode)
1308
						&& !((reg_arch_info->mode == ARMV4_5_MODE_USR) && (armv4_5->core_mode == ARMV4_5_MODE_SYS))
1309
1310
1311
						&& !((reg_arch_info->mode == ARMV4_5_MODE_SYS) && (armv4_5->core_mode == ARMV4_5_MODE_USR)))
					{
						mode_change = 1;
1312
						LOG_DEBUG("require mode change");
1313
1314
1315
1316
					}
				}
				else
				{
1317
					LOG_ERROR("BUG: dirty register '%s', but no valid data", reg->name);
1318
1319
1320
				}
			}
		}
1321

1322
1323
1324
1325
1326
1327
1328
1329
1330
		if (dirty)
		{
			u32 mask = 0x0;
			int num_regs = 0;
			u32 regs[16];

			if (mode_change)
			{
				u32 tmp_cpsr;
1331

1332
				/* change processor mode (mask T bit) */
1333
1334
				tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
				tmp_cpsr |= armv4_5_number_to_mode(i);
1335
				tmp_cpsr &= ~0x20;
1336
1337
1338
				arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
				current_mode = armv4_5_number_to_mode(i);
			}
1339

1340
1341
1342
1343
			for (j = 0; j <= 14; j++)
			{
				reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
				reg_arch_info = reg->arch_info;
1344
1345


1346
1347
1348
1349
1350
1351
1352
				if (reg->dirty == 1)
				{
					regs[j] = buf_get_u32(reg->value, 0, 32);
					mask |= 1 << j;
					num_regs++;
					reg->dirty = 0;
					reg->valid = 1;
1353
					LOG_DEBUG("writing register %i of mode %s with value 0x%8.8x", j, armv4_5_mode_strings[i], regs[j]);
1354
1355
				}
			}
1356

1357
1358
1359
1360
			if (mask)
			{
				arm7_9->write_core_regs(target, mask, regs);
			}
1361

1362
1363
1364
1365
			reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16);
			reg_arch_info = reg->arch_info;
			if ((reg->dirty) && (reg_arch_info->mode != ARMV4_5_MODE_ANY))
			{
1366
				LOG_DEBUG("writing SPSR of mode %i with value 0x%8.8x", i, buf_get_u32(reg->value, 0, 32));
1367
1368
1369
1370
				arm7_9->write_xpsr(target, buf_get_u32(reg->value, 0, 32), 1);
			}
		}
	}
1371

1372
1373
	if ((armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 0) && (armv4_5->core_mode != current_mode))
	{
1374
		/* restore processor mode (mask T bit) */
1375
		u32 tmp_cpsr;
1376

1377
1378
		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
		tmp_cpsr |= armv4_5_number_to_mode(i);
1379
		tmp_cpsr &= ~0x20;
1380
		LOG_DEBUG("writing lower 8 bit of cpsr with value 0x%2.2x", tmp_cpsr);
1381
1382
1383
1384
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
	else if (armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 1)
	{
1385
		/* CPSR has been changed, full restore necessary (mask T bit) */
1386
		LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1387
		arm7_9->write_xpsr(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32) & ~0x20, 0);
1388
1389
1390
		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
	}
1391

1392
	/* restore PC */
1393
	LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1394
1395
	arm7_9->write_pc(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
	armv4_5->core_cache->reg_list[15].dirty = 0;
1396

1397
1398
1399
1400
1401
1402
1403
1404
1405
1406
1407
	if (arm7_9->post_restore_context)
		arm7_9->post_restore_context(target);

	return ERROR_OK;
}

int arm7_9_restart_core(struct target_s *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
1408

1409
1410
	/* set RESTART instruction */
	jtag_add_end_state(TAP_RTI);
oharboe's avatar
oharboe committed
1411
1412
1413
1414
	if (arm7_9->need_bypass_before_restart) {
		arm7_9->need_bypass_before_restart = 0;
		arm_jtag_set_instr(jtag_info, 0xf, NULL);
	}
1415
	arm_jtag_set_instr(jtag_info, 0x4, NULL);
1416

1417
	jtag_add_runtest(1, TAP_RTI);
1418
	return jtag_execute_queue();
1419
1420
1421
1422
1423
}

void arm7_9_enable_watchpoints(struct target_s *target)
{
	watchpoint_t *watchpoint = target->watchpoints;
1424

1425
1426
1427
1428
1429
1430
1431
1432
1433
1434
1435
	while (watchpoint)
	{
		if (watchpoint->set == 0)
			arm7_9_set_watchpoint(target, watchpoint);
		watchpoint = watchpoint->next;
	}
}

void arm7_9_enable_breakpoints(struct target_s *target)
{
	breakpoint_t *breakpoint = target->breakpoints;
1436

1437
1438
1439
1440
1441
1442
1443
1444
1445
1446
1447
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
	/* set any pending breakpoints */
	while (breakpoint)
	{
		if (breakpoint->set == 0)
			arm7_9_set_breakpoint(target, breakpoint);
		breakpoint = breakpoint->next;
	}
}

void arm7_9_disable_bkpts_and_wpts(struct target_s *target)
{
	breakpoint_t *breakpoint = target->breakpoints;
	watchpoint_t *watchpoint = target->watchpoints;

	/* set any pending breakpoints */
	while (breakpoint)
	{
		if (breakpoint->set != 0)
			arm7_9_unset_breakpoint(target, breakpoint);
		breakpoint = breakpoint->next;
	}
1458

1459
1460
1461
1462
1463
1464
1465
1466
1467
1468
1469
1470
1471
1472
	while (watchpoint)
	{
		if (watchpoint->set != 0)
			arm7_9_unset_watchpoint(target, watchpoint);
		watchpoint = watchpoint->next;
	}
}

int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	breakpoint_t *breakpoint = target->breakpoints;
	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1473
	int err;
1474

1475
	LOG_DEBUG("-");
1476

1477
1478
	if (target->state != TARGET_HALTED)
	{
1479
		LOG_WARNING("target not halted");
1480
1481
		return ERROR_TARGET_NOT_HALTED;
	}
1482

1483
1484
1485
1486
	if (!debug_execution)
	{
		target_free_all_working_areas(target);
	}
1487

1488
1489
1490
	/* current = 1: continue on current pc, otherwise continue at <address> */
	if (!current)
		buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1491

1492
1493
1494
1495
1496
	/* the front-end may request us not to handle breakpoints */
	if (handle_breakpoints)
	{
		if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
		{
1497
			LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
1498
			arm7_9_unset_breakpoint(target, breakpoint);
1499

1500
			LOG_DEBUG("enable single-step");
1501
			arm7_9->enable_single_step(target);
1502

1503
1504
1505
			target->debug_reason = DBG_REASON_SINGLESTEP;

			arm7_9_restore_context(target);
1506

1507
1508
1509
1510
1511
1512
1513
1514
			if (armv4_5->core_state == ARMV4_5_STATE_ARM)
				arm7_9->branch_resume(target);
			else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
			{
				arm7_9->branch_resume_thumb(target);
			}
			else
			{
1515
				LOG_ERROR("unhandled core state");
1516
				return ERROR_FAIL;
1517
			}
1518

1519
1520
			buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
			embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1521
			err = arm7_9_execute_sys_speed(target);
1522

1523
			LOG_DEBUG("disable single-step");
1524
			arm7_9->disable_single_step(target);
1525
1526
1527
1528
1529
1530
1531
1532

			if (err != ERROR_OK)
			{
				arm7_9_set_breakpoint(target, breakpoint);
				target->state = TARGET_UNKNOWN;
				return err;
			}

1533
			arm7_9_debug_entry(target);
1534
			LOG_DEBUG("new PC after step: 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1535

1536
			LOG_DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
1537
1538
1539
			arm7_9_set_breakpoint(target, breakpoint);
		}
	}
1540

1541
1542
1543
	/* enable any pending breakpoints and watchpoints */
	arm7_9_enable_breakpoints(target);
	arm7_9_enable_watchpoints(target);
1544

1545
	arm7_9_restore_context(target);
1546

1547
1548
1549
1550
1551
1552
1553
1554
1555
1556
	if (armv4_5->core_state == ARMV4_5_STATE_ARM)
	{
		arm7_9->branch_resume(target);
	}
	else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
	{
		arm7_9->branch_resume_thumb(target);
	}
	else
	{
1557
		LOG_ERROR("unhandled core state");
1558
		return ERROR_FAIL;
1559
	}
1560

1561
1562
1563
1564
1565
1566
	/* deassert DBGACK and INTDIS */
	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
	/* INTDIS only when we really resume, not during debug execution */
	if (!debug_execution)
		buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 0);
	embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1567

1568
	arm7_9_restart_core(target);
1569

1570
	target->debug_reason = DBG_REASON_NOTHALTED;
1571

1572
1573
1574
1575
1576
1577
1578
1579
1580
1581
1582
1583
	if (!debug_execution)
	{
		/* registers are now invalid */
		armv4_5_invalidate_core_regs(target);
		target->state = TARGET_RUNNING;
		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
	}
	else
	{
		target->state = TARGET_DEBUG_RUNNING;
		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
	}
1584

1585
	LOG_DEBUG("target resumed");
1586

1587
1588
1589
1590
1591
1592
1593
	return ERROR_OK;
}

void arm7_9_enable_eice_step(target_t *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1594

1595
1596
1597
1598
1599
1600
	/* setup an inverse breakpoint on the current PC
	* - comparator 1 matches the current address
	* - rangeout from comparator 1 is connected to comparator 0 rangein
	* - comparator 0 matches any address, as long as rangein is low */
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1601
1602
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~(EICE_W_CTRL_RANGE|EICE_W_CTRL_nOPC) & 0xff);
1603
1604
1605
1606
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0);
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffff);
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
1607
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1608
1609
1610
1611
1612
1613
1614
1615
1616
1617
1618
1619
1620
1621
1622
1623
1624
1625
1626
1627
1628
1629
}

void arm7_9_disable_eice_step(target_t *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;

	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE]);
	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK]);
	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK]);
	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK]);
	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE]);
}

int arm7_9_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1630
	breakpoint_t *breakpoint = NULL;
1631
	int err;
1632
1633
1634

	if (target->state != TARGET_HALTED)
	{
1635
		LOG_WARNING("target not halted");
1636
1637
		return ERROR_TARGET_NOT_HALTED;
	}
1638

1639
1640
1641
	/* current = 1: continue on current pc, otherwise continue at <address> */
	if (!current)
		buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1642

1643
1644
1645
1646
	/* the front-end may request us not to handle breakpoints */
	if (handle_breakpoints)
		if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
			arm7_9_unset_breakpoint(target, breakpoint);
1647

1648
1649
1650
	target->debug_reason = DBG_REASON_SINGLESTEP;

	arm7_9_restore_context(target);
1651

1652
	arm7_9->enable_single_step(target);
1653

1654
1655
1656
1657
1658
1659
1660
1661
1662
1663
	if (armv4_5->core_state == ARMV4_5_STATE_ARM)
	{
		arm7_9->branch_resume(target);
	}
	else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
	{
		arm7_9->branch_resume_thumb(target);
	}
	else
	{
1664
		LOG_ERROR("unhandled core state");
1665
		return ERROR_FAIL;
1666
	}
1667

1668
1669
	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);

1670
	err = arm7_9_execute_sys_speed(target);
1671
	arm7_9->disable_single_step(target);
1672

1673
1674
	/* registers are now invalid */
	armv4_5_invalidate_core_regs(target);
1675

1676
1677
1678
1679
1680
1681
	if (err != ERROR_OK)
	{
		target->state = TARGET_UNKNOWN;
	} else {
		arm7_9_debug_entry(target);
		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1682
		LOG_DEBUG("target stepped");
1683
	}
1684

1685
1686
	if (breakpoint)
		arm7_9_set_breakpoint(target, breakpoint);
1687

1688
	return err;
1689
1690
1691
1692
1693
1694

}

int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
{
	u32* reg_p[16];
1695
	u32 value;
1696
1697
1698
	int retval;
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1699

1700
1701
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1702

1703
	enum armv4_5_mode reg_mode = ((armv4_5_core_reg_t*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info)->mode;
1704

1705
1706
	if ((num < 0) || (num > 16))
		return ERROR_INVALID_ARGUMENTS;
1707

1708
1709
1710
1711
1712
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))
	{
		u32 tmp_cpsr;
1713

1714
		/* change processor mode (mask T bit) */
1715
1716
		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
		tmp_cpsr |= mode;
1717
		tmp_cpsr &= ~0x20;
1718
1719
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
1720

1721
1722
1723
	if ((num >= 0) && (num <= 15))
	{
		/* read a normal core register */
1724
		reg_p[num] = &value;
1725

1726
1727
1728
1729
1730
1731
1732
1733
1734
		arm7_9->read_core_regs(target, 1 << num, reg_p);
	}
	else
	{
		/* read a program status register
		 * if the register mode is MODE_ANY, we read the cpsr, otherwise a spsr
		 */
		armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
		int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1735

1736
		arm7_9->read_xpsr(target, &value, spsr);
1737
	}
1738

1739
1740
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1741
		return retval;
1742
	}
1743

1744
1745
	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1746
	buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value, 0, 32, value);
1747

1748
1749
1750
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
1751
1752
		/* restore processor mode (mask T bit) */
		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1753
	}
1754

1755
	return ERROR_OK;
1756

1757
1758
1759
1760
1761
1762
1763
}

int arm7_9_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)
{
	u32 reg[16];
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1764

1765
1766
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1767

1768
1769
1770
1771
	enum armv4_5_mode reg_mode = ((armv4_5_core_reg_t*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info)->mode;

	if ((num < 0) || (num > 16))
		return ERROR_INVALID_ARGUMENTS;
1772

1773
1774
1775
1776
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
		u32 tmp_cpsr;
1777

1778
		/* change processor mode (mask T bit) */
1779
1780
		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
		tmp_cpsr |= mode;
1781
		tmp_cpsr &= ~0x20;
1782
1783
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
1784

1785
1786
1787
1788
	if ((num >= 0) && (num <= 15))
	{
		/* write a normal core register */
		reg[num] = value;
1789

1790
1791
1792
1793
1794
1795
1796
1797
1798
		arm7_9->write_core_regs(target, 1 << num, reg);
	}
	else
	{
		/* write a program status register
		* if the register mode is MODE_ANY, we write the cpsr, otherwise a spsr
		*/
		armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
		int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1799

1800
1801
1802
		/* if we're writing the CPSR, mask the T bit */
		if (!spsr)
			value &= ~0x20;
1803

1804
1805
		arm7_9->write_xpsr(target, value, spsr);
	}
1806

1807
1808
	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1809

1810
1811
1812
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
1813
1814
		/* restore processor mode (mask T bit) */
		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1815
	}
1816

1817
	return jtag_execute_queue();
1818
1819
1820
1821
1822
1823
}

int arm7_9_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1824

1825
1826
1827
1828
1829
1830
1831
	u32 reg[16];
	int num_accesses = 0;
	int thisrun_accesses;
	int i;
	u32 cpsr;
	int retval;
	int last_reg = 0;
1832

1833
	LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1834
1835
1836

	if (target->state != TARGET_HALTED)
	{
1837
		LOG_WARNING("target not halted");
1838
1839
1840
1841
1842
1843
1844
1845
1846
		return ERROR_TARGET_NOT_HALTED;
	}

	/* sanitize arguments */
	if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
		return ERROR_INVALID_ARGUMENTS;

	if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
		return ERROR_TARGET_UNALIGNED_ACCESS;
1847

1848
1849
1850
	/* load the base register with the address of the first word */
	reg[0] = address;
	arm7_9->write_core_regs(target, 0x1, reg);
1851

1852
1853
1854
1855
1856
1857
1858
1859
	switch (size)
	{
		case 4:
			while (num_accesses < count)
			{
				u32 reg_list;
				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1860

1861
1862
				if (last_reg <= thisrun_accesses)
					last_reg = thisrun_accesses;
1863

1864
				arm7_9->load_word_regs(target, reg_list);
1865

1866
1867
1868
1869
1870
1871
1872
				/* fast memory reads are only safe when the target is running
				 * from a sufficiently high clock (32 kHz is usually too slow)
				 */
				if (arm7_9->fast_memory_access)
					arm7_9_execute_fast_sys_speed(target);
				else
					arm7_9_execute_sys_speed(target);
1873

1874
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 4);
1875

1876
1877
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 4;
1878
				num_accesses += thisrun_accesses;
1879
			}
1880
1881
1882
1883
1884
1885
1886
			break;
		case 2:
			while (num_accesses < count)
			{
				u32 reg_list;
				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1887

1888
1889
1890
1891
1892
				for (i = 1; i <= thisrun_accesses; i++)
				{
					if (i > last_reg)
						last_reg = i;
					arm7_9->load_hword_reg(target, i);
1893
1894
1895
1896
1897
1898
1899
					/* fast memory reads are only safe when the target is running
					 * from a sufficiently high clock (32 kHz is usually too slow)
					 */
					if (arm7_9->fast_memory_access)
						arm7_9_execute_fast_sys_speed(target);
					else
						arm7_9_execute_sys_speed(target);
1900
				}
1901

1902
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 2);
1903

1904
1905
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 2;
1906
				num_accesses += thisrun_accesses;
1907
			}
1908
1909
1910
1911
1912
1913
1914
			break;
		case 1:
			while (num_accesses < count)
			{
				u32 reg_list;
				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1915

1916
1917
1918
1919
1920
				for (i = 1; i <= thisrun_accesses; i++)
				{
					if (i > last_reg)
						last_reg = i;
					arm7_9->load_byte_reg(target, i);
1921
1922
1923
1924
1925
1926
1927
					/* fast memory reads are only safe when the target is running
					 * from a sufficiently high clock (32 kHz is usually too slow)
					 */
					if (arm7_9->fast_memory_access)
						arm7_9_execute_fast_sys_speed(target);
					else
						arm7_9_execute_sys_speed(target);
1928
				}
1929

1930
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 1);
1931

1932
1933
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 1;
1934
				num_accesses += thisrun_accesses;
1935
			}
1936
1937
			break;
		default:
1938
			LOG_ERROR("BUG: we shouldn't get here");
1939
1940
1941
			exit(-1);
			break;
	}
1942

1943
1944
1945
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;

1946
	for (i=0; i<=last_reg; i++)
1947
		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid;
1948
1949
1950
1951

	arm7_9->read_xpsr(target, &cpsr, 0);
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1952
		LOG_ERROR("JTAG error while reading cpsr");
ntfreak's avatar
ntfreak committed
1953
		return ERROR_TARGET_DATA_ABORT;
1954
1955
1956
1957
	}

	if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
	{
1958
		LOG_WARNING("memory read caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
1959

1960
		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1961
1962
1963

		return ERROR_TARGET_DATA_ABORT;
	}
1964

1965
1966
1967
1968
1969
1970
1971
	return ERROR_OK;
}

int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1972
	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1973

1974
1975
1976
1977
1978
1979
1980
1981
	u32 reg[16];
	int num_accesses = 0;
	int thisrun_accesses;
	int i;
	u32 cpsr;
	int retval;
	int last_reg = 0;

ntfreak's avatar
ntfreak committed
1982
#ifdef _DEBUG_ARM7_9_
1983
	LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
ntfreak's avatar
ntfreak committed
1984
#endif
1985
1986
1987

	if (target->state != TARGET_HALTED)
	{
1988
		LOG_WARNING("target not halted");
1989
1990
1991
1992
1993
1994
1995
1996
1997
		return ERROR_TARGET_NOT_HALTED;
	}

	/* sanitize arguments */
	if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
		return ERROR_INVALID_ARGUMENTS;

	if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
		return ERROR_TARGET_UNALIGNED_ACCESS;
1998

1999
2000
	/* load the base register with the address of the first word */
	reg[0] = address;
For faster browsing, not all history is shown. View entire blame