arm7_9_common.c 75.9 KB
Newer Older
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
	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_
1013
	LOG_DEBUG("-");
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
#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);
1026

1027
	arm7_9_clear_halt(target);
1028

1029
1030
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1031
		return retval;
1032
1033
1034
1035
1036
1037
1038
1039
	}

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


	if (target->state != TARGET_HALTED)
	{
1040
		LOG_WARNING("target not halted");
1041
1042
		return ERROR_TARGET_NOT_HALTED;
	}
1043

1044
1045
1046
	/* if the target is in Thumb state, change to ARM state */
	if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
	{
1047
		LOG_DEBUG("target entered debug from Thumb state");
1048
1049
1050
		/* Entered debug from Thumb mode */
		armv4_5->core_state = ARMV4_5_STATE_THUMB;
		arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
1051
		LOG_DEBUG("r0_thumb: 0x%8.8x, pc_thumb: 0x%8.8x", r0_thumb, pc_thumb);
1052
1053
1054
	}
	else
	{
1055
		LOG_DEBUG("target entered debug from ARM state");
1056
1057
1058
		/* Entered debug from ARM mode */
		armv4_5->core_state = ARMV4_5_STATE_ARM;
	}
1059

1060
1061
1062
1063
1064
1065
	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);
1066

1067
1068
	if ((retval = jtag_execute_queue()) != ERROR_OK)
		return retval;
1069

1070
1071
	/* if the core has been executing in Thumb state, set the T bit */
	if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1072
1073
		cpsr |= 0x20;

1074
1075
1076
	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;
1077

1078
	armv4_5->core_mode = cpsr & 0x1f;
1079

1080
1081
1082
	if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
	{
		target->state = TARGET_UNKNOWN;
1083
		LOG_ERROR("cpsr contains invalid mode value - communication failure");
1084
1085
		return ERROR_TARGET_FAILURE;
	}
1086

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

1089
1090
	if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
	{
1091
		LOG_DEBUG("thumb state, applying fixups");
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
		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
	{
1110
		LOG_ERROR("unknown debug reason: %i", target->debug_reason);
1111
1112
	}

1113
1114
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1115

1116
1117
	for (i=0; i<=15; i++)
	{
1118
		LOG_DEBUG("r%i: 0x%8.8x", i, context[i]);
1119
1120
1121
1122
		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;
	}
1123

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

1126
1127
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1128
1129

	/* exceptions other than USR & SYS have a saved program status register */
1130
	if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
	{
		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 */
1141
1142
	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;
1143

1144
	if ((retval = jtag_execute_queue()) != ERROR_OK)
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
		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;

1160
	LOG_DEBUG("-");
1161

1162
1163
	if (target->state != TARGET_HALTED)
	{
1164
		LOG_WARNING("target not halted");
1165
1166
		return ERROR_TARGET_NOT_HALTED;
	}
1167

1168
1169
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179

	/* 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;
1180
1181

		/* check if there are invalid registers in the current mode
1182
1183
1184
1185
1186
1187
		 */
		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;
		}
1188

1189
1190
1191
		if (!valid)
		{
			u32 tmp_cpsr;
1192

1193
			/* change processor mode (and mask T bit) */
1194
1195
			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);
1196
			tmp_cpsr &= ~0x20;
1197
1198
1199
1200
1201
			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)
1202
				{
1203
1204
1205
1206
1207
1208
					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;
				}
			}
1209

1210
1211
1212
			/* if only the PSR is invalid, mask is all zeroes */
			if (mask)
				arm7_9->read_core_regs(target, mask, reg_p);
1213

1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
			/* 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;
			}
		}
	}

1224
1225
	/* 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);
1226

1227
1228
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1229
		return retval;
1230
1231
1232
1233
1234
1235
1236
1237
	}
	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;
1238
	reg_t *reg;
1239
1240
1241
1242
1243
	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;
1244

1245
	LOG_DEBUG("-");
1246

1247
1248
	if (target->state != TARGET_HALTED)
	{
1249
		LOG_WARNING("target not halted");
1250
1251
		return ERROR_TARGET_NOT_HALTED;
	}
1252

1253
1254
	if (arm7_9->pre_restore_context)
		arm7_9->pre_restore_context(target);
1255

1256
1257
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1258

1259
1260
1261
1262
1263
	/* 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++)
	{
1264
		LOG_DEBUG("examining %s mode", armv4_5_mode_strings[i]);
1265
1266
		dirty = 0;
		mode_change = 0;
1267
		/* check if there are dirty registers in the current mode
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
		*/
		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;
1278
					LOG_DEBUG("examining dirty reg: %s", reg->name);
1279
1280
					if ((reg_arch_info->mode != ARMV4_5_MODE_ANY)
						&& (reg_arch_info->mode != current_mode)
1281
						&& !((reg_arch_info->mode == ARMV4_5_MODE_USR) && (armv4_5->core_mode == ARMV4_5_MODE_SYS))
1282
1283
1284
						&& !((reg_arch_info->mode == ARMV4_5_MODE_SYS) && (armv4_5->core_mode == ARMV4_5_MODE_USR)))
					{
						mode_change = 1;
1285
						LOG_DEBUG("require mode change");
1286
1287
1288
1289
					}
				}
				else
				{
1290
					LOG_ERROR("BUG: dirty register '%s', but no valid data", reg->name);
1291
1292
1293
				}
			}
		}
1294

1295
1296
1297
1298
1299
1300
1301
1302
1303
		if (dirty)
		{
			u32 mask = 0x0;
			int num_regs = 0;
			u32 regs[16];

			if (mode_change)
			{
				u32 tmp_cpsr;
1304

1305
				/* change processor mode (mask T bit) */
1306
1307
				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);
1308
				tmp_cpsr &= ~0x20;
1309
1310
1311
				arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
				current_mode = armv4_5_number_to_mode(i);
			}
1312

1313
1314
1315
1316
			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;
1317
1318


1319
1320
1321
1322
1323
1324
1325
				if (reg->dirty == 1)
				{
					regs[j] = buf_get_u32(reg->value, 0, 32);
					mask |= 1 << j;
					num_regs++;
					reg->dirty = 0;
					reg->valid = 1;
1326
					LOG_DEBUG("writing register %i of mode %s with value 0x%8.8x", j, armv4_5_mode_strings[i], regs[j]);
1327
1328
				}
			}
1329

1330
1331
1332
1333
			if (mask)
			{
				arm7_9->write_core_regs(target, mask, regs);
			}
1334

1335
1336
1337
1338
			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))
			{
1339
				LOG_DEBUG("writing SPSR of mode %i with value 0x%8.8x", i, buf_get_u32(reg->value, 0, 32));
1340
1341
1342
1343
				arm7_9->write_xpsr(target, buf_get_u32(reg->value, 0, 32), 1);
			}
		}
	}
1344

1345
1346
	if ((armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 0) && (armv4_5->core_mode != current_mode))
	{
1347
		/* restore processor mode (mask T bit) */
1348
		u32 tmp_cpsr;
1349

1350
1351
		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);
1352
		tmp_cpsr &= ~0x20;
1353
		LOG_DEBUG("writing lower 8 bit of cpsr with value 0x%2.2x", tmp_cpsr);
1354
1355
1356
1357
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
	else if (armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 1)
	{
1358
		/* CPSR has been changed, full restore necessary (mask T bit) */
1359
		LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1360
		arm7_9->write_xpsr(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32) & ~0x20, 0);
1361
1362
1363
		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
	}
1364

1365
	/* restore PC */
1366
	LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1367
1368
	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;
1369

1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
	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;
1381

1382
1383
	/* set RESTART instruction */
	jtag_add_end_state(TAP_RTI);
oharboe's avatar
oharboe committed
1384
1385
1386
1387
	if (arm7_9->need_bypass_before_restart) {
		arm7_9->need_bypass_before_restart = 0;
		arm_jtag_set_instr(jtag_info, 0xf, NULL);
	}
1388
	arm_jtag_set_instr(jtag_info, 0x4, NULL);
1389

1390
	jtag_add_runtest(1, TAP_RTI);
1391
	return jtag_execute_queue();
1392
1393
1394
1395
1396
}

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

1398
1399
1400
1401
1402
1403
1404
1405
1406
1407
1408
	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;
1409

1410
1411
1412
	/* set any pending breakpoints */
	while (breakpoint)
	{
1413
		arm7_9_set_breakpoint(target, breakpoint);
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
		breakpoint = breakpoint->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];
1425
	int err;
1426

1427
	LOG_DEBUG("-");
1428

1429
1430
	if (target->state != TARGET_HALTED)
	{
1431
		LOG_WARNING("target not halted");
1432
1433
		return ERROR_TARGET_NOT_HALTED;
	}
1434

1435
1436
1437
1438
	if (!debug_execution)
	{
		target_free_all_working_areas(target);
	}
1439

1440
1441
1442
	/* 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);
1443

1444
1445
1446
1447
1448
	/* 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))))
		{
1449
			LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
1450
			arm7_9_unset_breakpoint(target, breakpoint);
1451

1452
			LOG_DEBUG("enable single-step");
1453
			arm7_9->enable_single_step(target);
1454

1455
1456
1457
			target->debug_reason = DBG_REASON_SINGLESTEP;

			arm7_9_restore_context(target);
1458

1459
1460
1461
1462
1463
1464
1465
1466
			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
			{
1467
				LOG_ERROR("unhandled core state");
1468
				return ERROR_FAIL;
1469
			}
1470

1471
1472
			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));
1473
			err = arm7_9_execute_sys_speed(target);
1474

1475
			LOG_DEBUG("disable single-step");
1476
			arm7_9->disable_single_step(target);
1477
1478
1479
1480
1481
1482
1483
1484

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

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

1488
			LOG_DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
1489
1490
1491
			arm7_9_set_breakpoint(target, breakpoint);
		}
	}
1492

1493
1494
1495
	/* enable any pending breakpoints and watchpoints */
	arm7_9_enable_breakpoints(target);
	arm7_9_enable_watchpoints(target);
1496

1497
	arm7_9_restore_context(target);
1498

1499
1500
1501
1502
1503
1504
1505
1506
1507
1508
	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
	{
1509
		LOG_ERROR("unhandled core state");
1510
		return ERROR_FAIL;
1511
	}
1512

1513
1514
1515
1516
1517
1518
	/* 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));
1519

1520
	arm7_9_restart_core(target);
1521

1522
	target->debug_reason = DBG_REASON_NOTHALTED;
1523

1524
1525
1526
1527
1528
1529
1530
1531
1532
1533
1534
1535
	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);
	}
1536

1537
	LOG_DEBUG("target resumed");
1538

1539
1540
1541
1542
1543
1544
1545
	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;
1546

1547
1548
1549
1550
1551
1552
	/* 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);
1553
1554
	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);
1555
1556
1557
1558
	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);
1559
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1560
1561
1562
1563
1564
1565
1566
1567
1568
1569
1570
1571
1572
1573
1574
1575
1576
1577
1578
1579
1580
1581
}

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;
1582
	breakpoint_t *breakpoint = NULL;
1583
	int err;
1584
1585
1586

	if (target->state != TARGET_HALTED)
	{
1587
		LOG_WARNING("target not halted");
1588
1589
		return ERROR_TARGET_NOT_HALTED;
	}
1590

1591
1592
1593
	/* 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);
1594

1595
1596
1597
1598
	/* 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);
1599

1600
1601
1602
	target->debug_reason = DBG_REASON_SINGLESTEP;

	arm7_9_restore_context(target);
1603

1604
	arm7_9->enable_single_step(target);
1605

1606
1607
1608
1609
1610
1611
1612
1613
1614
1615
	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
	{
1616
		LOG_ERROR("unhandled core state");
1617
		return ERROR_FAIL;
1618
	}
1619

1620
1621
	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);

1622
	err = arm7_9_execute_sys_speed(target);
1623
	arm7_9->disable_single_step(target);
1624

1625
1626
	/* registers are now invalid */
	armv4_5_invalidate_core_regs(target);
1627

1628
1629
1630
1631
1632
1633
	if (err != ERROR_OK)
	{
		target->state = TARGET_UNKNOWN;
	} else {
		arm7_9_debug_entry(target);
		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1634
		LOG_DEBUG("target stepped");
1635
	}
1636

1637
1638
	if (breakpoint)
		arm7_9_set_breakpoint(target, breakpoint);
1639

1640
	return err;
1641
1642
1643
1644
1645
1646

}

int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
{
	u32* reg_p[16];
1647
	u32 value;
1648
1649
1650
	int retval;
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1651

1652
1653
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1654

1655
	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;
1656

1657
1658
	if ((num < 0) || (num > 16))
		return ERROR_INVALID_ARGUMENTS;
1659

1660
1661
1662
1663
1664
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))
	{
		u32 tmp_cpsr;
1665

1666
		/* change processor mode (mask T bit) */
1667
1668
		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
		tmp_cpsr |= mode;
1669
		tmp_cpsr &= ~0x20;
1670
1671
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
1672

1673
1674
1675
	if ((num >= 0) && (num <= 15))
	{
		/* read a normal core register */
1676
		reg_p[num] = &value;
1677

1678
1679
1680
1681
1682
1683
1684
1685
1686
		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;
1687

1688
		arm7_9->read_xpsr(target, &value, spsr);
1689
	}
1690

1691
1692
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1693
		return retval;
1694
	}
1695

1696
1697
	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;
1698
	buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value, 0, 32, value);
1699

1700
1701
1702
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
1703
1704
		/* 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);
1705
	}
1706

1707
	return ERROR_OK;
1708

1709
1710
1711
1712
1713
1714
1715
}

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;
1716

1717
1718
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1719

1720
1721
1722
1723
	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;
1724

1725
1726
1727
1728
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
		u32 tmp_cpsr;
1729

1730
		/* change processor mode (mask T bit) */
1731
1732
		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
		tmp_cpsr |= mode;
1733
		tmp_cpsr &= ~0x20;
1734
1735
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
1736

1737
1738
1739
1740
	if ((num >= 0) && (num <= 15))
	{
		/* write a normal core register */
		reg[num] = value;
1741

1742
1743
1744
1745
1746
1747
1748
1749
1750
		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;
1751

1752
1753
1754
		/* if we're writing the CPSR, mask the T bit */
		if (!spsr)
			value &= ~0x20;
1755

1756
1757
		arm7_9->write_xpsr(target, value, spsr);
	}
1758

1759
1760
	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;
1761

1762
1763
1764
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
1765
1766
		/* 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);
1767
	}
1768

1769
	return jtag_execute_queue();
1770
1771
1772
1773
1774
1775
}

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;
1776

1777
1778
1779
1780
1781
1782
1783
	u32 reg[16];
	int num_accesses = 0;
	int thisrun_accesses;
	int i;
	u32 cpsr;
	int retval;
	int last_reg = 0;
1784

1785
	LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1786
1787
1788

	if (target->state != TARGET_HALTED)
	{
1789
		LOG_WARNING("target not halted");
1790
1791
1792
1793
1794
1795
1796
1797
1798
		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;
1799

1800
1801
1802
	/* load the base register with the address of the first word */
	reg[0] = address;
	arm7_9->write_core_regs(target, 0x1, reg);
1803

1804
1805
1806
1807
1808
1809
1810
1811
	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;
1812

1813
1814
				if (last_reg <= thisrun_accesses)
					last_reg = thisrun_accesses;
1815

1816
				arm7_9->load_word_regs(target, reg_list);
1817

1818
1819
1820
1821
1822
1823
1824
				/* 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);
1825

1826
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 4);
1827

1828
1829
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 4;
1830
				num_accesses += thisrun_accesses;
1831
			}
1832
1833
1834
1835
1836
1837
1838
			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;
1839

1840
1841
1842
1843
1844
				for (i = 1; i <= thisrun_accesses; i++)
				{
					if (i > last_reg)
						last_reg = i;
					arm7_9->load_hword_reg(target, i);
1845
1846
1847
1848
1849
1850
1851
					/* 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);
1852
				}
1853

1854
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 2);
1855

1856
1857
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 2;
1858
				num_accesses += thisrun_accesses;
1859
			}
1860
1861
1862
1863
1864
1865
1866
			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;
1867

1868
1869
1870
1871
1872
				for (i = 1; i <= thisrun_accesses; i++)
				{
					if (i > last_reg)
						last_reg = i;
					arm7_9->load_byte_reg(target, i);
1873
1874
1875
1876
1877
1878
1879
					/* 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);
1880
				}
1881

1882
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 1);
1883

1884
1885
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 1;
1886
				num_accesses += thisrun_accesses;
1887
			}
1888
1889
			break;
		default:
1890
			LOG_ERROR("BUG: we shouldn't get here");
1891
1892
1893
			exit(-1);
			break;
	}
1894

1895
1896
1897
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;

1898
	for (i=0; i<=last_reg; i++)
1899
		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;
1900
1901
1902
1903

	arm7_9->read_xpsr(target, &cpsr, 0);
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1904
		LOG_ERROR("JTAG error while reading cpsr");
ntfreak's avatar
ntfreak committed
1905
		return ERROR_TARGET_DATA_ABORT;
1906
1907
1908
1909
	}

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

1912
		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1913
1914
1915

		return ERROR_TARGET_DATA_ABORT;
	}
1916

1917
1918
1919
1920
1921
1922
1923
	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;
1924
	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1925

1926
1927
1928
1929
1930
1931
1932
1933
	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
1934
#ifdef _DEBUG_ARM7_9_
1935
	LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
ntfreak's avatar
ntfreak committed
1936
#endif
1937
1938
1939

	if (target->state != TARGET_HALTED)
	{
1940
		LOG_WARNING("target not halted");
1941
1942
1943
1944
1945
1946
1947
1948
1949
		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;
1950

1951
1952
1953
	/* load the base register with the address of the first word */
	reg[0] = address;
	arm7_9->write_core_regs(target, 0x1, reg);
1954

1955
1956
1957
	/* Clear DBGACK, to make sure memory fetches work as expected */
	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
	embeddedice_store_reg(dbg_ctrl);
1958

1959
1960
1961
1962
1963
1964
1965
1966
	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;
1967

1968
1969
1970
1971
				for (i = 1; i <= thisrun_accesses; i++)
				{
					if (i > last_reg)
						last_reg = i;
1972
1973
					reg[i] = target_buffer_get_u32(target, buffer);
					buffer += 4;
1974
				}
1975

1976
				arm7_9->write_core_regs(target, reg_list, reg);
1977

1978
				arm7_9->store_word_regs(target, reg_list);
1979

1980
1981
1982
				/* fast memory writes are only safe when the target is running
				 * from a sufficiently high clock (32 kHz is usually too slow)
				 */
1983
				if (arm7_9->fast_memory_access)
1984
1985
1986
					arm7_9_execute_fast_sys_speed(target);
				else
					arm7_9_execute_sys_speed(target);
1987

1988
				num_accesses += thisrun_accesses;
1989
			}
1990
1991
1992
1993
1994
1995
1996
			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;
1997

1998
1999
2000
				for (i = 1; i <= thisrun_accesses; i++)
				{
					if (i > last_reg)
For faster browsing, not all history is shown. View entire blame