arm7_9_common.c 77.2 KB
Newer Older
oharboe's avatar
oharboe committed
1001
		}
1002
1003
1004
1005
1006
1007
1008
	}
	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);
1009
1010
		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);
1011
1012
1013
	}

	target->debug_reason = DBG_REASON_DBGRQ;
1014

1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
	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_
1033
	LOG_DEBUG("-");
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
#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);
1046

1047
	arm7_9_clear_halt(target);
1048

1049
1050
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1051
		return retval;
1052
1053
1054
1055
1056
1057
1058
1059
	}

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


	if (target->state != TARGET_HALTED)
	{
1060
		LOG_WARNING("target not halted");
1061
1062
		return ERROR_TARGET_NOT_HALTED;
	}
1063

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

1080
1081
1082
1083
1084
1085
	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);
1086

1087
1088
	if ((retval = jtag_execute_queue()) != ERROR_OK)
		return retval;
1089

1090
1091
	/* if the core has been executing in Thumb state, set the T bit */
	if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1092
1093
		cpsr |= 0x20;

1094
1095
1096
	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;
1097

1098
	armv4_5->core_mode = cpsr & 0x1f;
1099

1100
1101
1102
	if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
	{
		target->state = TARGET_UNKNOWN;
1103
		LOG_ERROR("cpsr contains invalid mode value - communication failure");
1104
1105
		return ERROR_TARGET_FAILURE;
	}
1106

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

1109
1110
	if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
	{
1111
		LOG_DEBUG("thumb state, applying fixups");
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
		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
	{
1130
		LOG_ERROR("unknown debug reason: %i", target->debug_reason);
1131
1132
	}

1133
1134
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1135

1136
1137
	for (i=0; i<=15; i++)
	{
1138
		LOG_DEBUG("r%i: 0x%8.8x", i, context[i]);
1139
1140
1141
1142
		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;
	}
1143

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

1146
1147
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1148
1149

	/* exceptions other than USR & SYS have a saved program status register */
1150
	if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
	{
		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 */
1161
1162
	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;
1163

1164
	if ((retval = jtag_execute_queue()) != ERROR_OK)
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
		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;

1180
	LOG_DEBUG("-");
1181

1182
1183
	if (target->state != TARGET_HALTED)
	{
1184
		LOG_WARNING("target not halted");
1185
1186
		return ERROR_TARGET_NOT_HALTED;
	}
1187

1188
1189
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199

	/* 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;
1200
1201

		/* check if there are invalid registers in the current mode
1202
1203
1204
1205
1206
1207
		 */
		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;
		}
1208

1209
1210
1211
		if (!valid)
		{
			u32 tmp_cpsr;
1212

1213
			/* change processor mode (and mask T bit) */
1214
1215
			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);
1216
			tmp_cpsr &= ~0x20;
1217
1218
1219
1220
1221
			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)
1222
				{
1223
1224
1225
1226
1227
1228
					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;
				}
			}
1229

1230
1231
1232
			/* if only the PSR is invalid, mask is all zeroes */
			if (mask)
				arm7_9->read_core_regs(target, mask, reg_p);
1233

1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
			/* 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;
			}
		}
	}

1244
1245
	/* 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);
1246

1247
1248
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1249
		return retval;
1250
1251
1252
1253
1254
1255
1256
1257
	}
	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;
1258
	reg_t *reg;
1259
1260
1261
1262
1263
	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;
1264

1265
	LOG_DEBUG("-");
1266

1267
1268
	if (target->state != TARGET_HALTED)
	{
1269
		LOG_WARNING("target not halted");
1270
1271
		return ERROR_TARGET_NOT_HALTED;
	}
1272

1273
1274
	if (arm7_9->pre_restore_context)
		arm7_9->pre_restore_context(target);
1275

1276
1277
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1278

1279
1280
1281
1282
1283
	/* 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++)
	{
1284
		LOG_DEBUG("examining %s mode", armv4_5_mode_strings[i]);
1285
1286
		dirty = 0;
		mode_change = 0;
1287
		/* check if there are dirty registers in the current mode
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
		*/
		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;
1298
					LOG_DEBUG("examining dirty reg: %s", reg->name);
1299
1300
					if ((reg_arch_info->mode != ARMV4_5_MODE_ANY)
						&& (reg_arch_info->mode != current_mode)
1301
						&& !((reg_arch_info->mode == ARMV4_5_MODE_USR) && (armv4_5->core_mode == ARMV4_5_MODE_SYS))
1302
1303
1304
						&& !((reg_arch_info->mode == ARMV4_5_MODE_SYS) && (armv4_5->core_mode == ARMV4_5_MODE_USR)))
					{
						mode_change = 1;
1305
						LOG_DEBUG("require mode change");
1306
1307
1308
1309
					}
				}
				else
				{
1310
					LOG_ERROR("BUG: dirty register '%s', but no valid data", reg->name);
1311
1312
1313
				}
			}
		}
1314

1315
1316
1317
1318
1319
1320
1321
1322
1323
		if (dirty)
		{
			u32 mask = 0x0;
			int num_regs = 0;
			u32 regs[16];

			if (mode_change)
			{
				u32 tmp_cpsr;
1324

1325
				/* change processor mode (mask T bit) */
1326
1327
				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);
1328
				tmp_cpsr &= ~0x20;
1329
1330
1331
				arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
				current_mode = armv4_5_number_to_mode(i);
			}
1332

1333
1334
1335
1336
			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;
1337
1338


1339
1340
1341
1342
1343
1344
1345
				if (reg->dirty == 1)
				{
					regs[j] = buf_get_u32(reg->value, 0, 32);
					mask |= 1 << j;
					num_regs++;
					reg->dirty = 0;
					reg->valid = 1;
1346
					LOG_DEBUG("writing register %i of mode %s with value 0x%8.8x", j, armv4_5_mode_strings[i], regs[j]);
1347
1348
				}
			}
1349

1350
1351
1352
1353
			if (mask)
			{
				arm7_9->write_core_regs(target, mask, regs);
			}
1354

1355
1356
1357
1358
			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))
			{
1359
				LOG_DEBUG("writing SPSR of mode %i with value 0x%8.8x", i, buf_get_u32(reg->value, 0, 32));
1360
1361
1362
1363
				arm7_9->write_xpsr(target, buf_get_u32(reg->value, 0, 32), 1);
			}
		}
	}
1364

1365
1366
	if ((armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 0) && (armv4_5->core_mode != current_mode))
	{
1367
		/* restore processor mode (mask T bit) */
1368
		u32 tmp_cpsr;
1369

1370
1371
		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);
1372
		tmp_cpsr &= ~0x20;
1373
		LOG_DEBUG("writing lower 8 bit of cpsr with value 0x%2.2x", tmp_cpsr);
1374
1375
1376
1377
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
	else if (armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 1)
	{
1378
		/* CPSR has been changed, full restore necessary (mask T bit) */
1379
		LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1380
		arm7_9->write_xpsr(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32) & ~0x20, 0);
1381
1382
1383
		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
	}
1384

1385
	/* restore PC */
1386
	LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1387
1388
	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;
1389

1390
1391
1392
1393
1394
1395
1396
1397
1398
1399
1400
	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;
1401

1402
1403
	/* set RESTART instruction */
	jtag_add_end_state(TAP_RTI);
oharboe's avatar
oharboe committed
1404
1405
1406
1407
	if (arm7_9->need_bypass_before_restart) {
		arm7_9->need_bypass_before_restart = 0;
		arm_jtag_set_instr(jtag_info, 0xf, NULL);
	}
1408
	arm_jtag_set_instr(jtag_info, 0x4, NULL);
1409

1410
	jtag_add_runtest(1, TAP_RTI);
1411
	return jtag_execute_queue();
1412
1413
1414
1415
1416
}

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

1418
1419
1420
1421
1422
1423
1424
1425
1426
1427
1428
	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;
1429

1430
1431
1432
	/* set any pending breakpoints */
	while (breakpoint)
	{
1433
		arm7_9_set_breakpoint(target, breakpoint);
1434
1435
1436
1437
1438
1439
1440
1441
1442
1443
1444
		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];
1445
	int err;
1446

1447
	LOG_DEBUG("-");
1448

1449
1450
	if (target->state != TARGET_HALTED)
	{
1451
		LOG_WARNING("target not halted");
1452
1453
		return ERROR_TARGET_NOT_HALTED;
	}
1454

1455
1456
1457
1458
	if (!debug_execution)
	{
		target_free_all_working_areas(target);
	}
1459

1460
1461
1462
	/* 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);
1463

1464
1465
1466
1467
1468
	/* 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))))
		{
1469
			LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
1470
			arm7_9_unset_breakpoint(target, breakpoint);
1471

1472
			LOG_DEBUG("enable single-step");
1473
			arm7_9->enable_single_step(target);
1474

1475
1476
1477
			target->debug_reason = DBG_REASON_SINGLESTEP;

			arm7_9_restore_context(target);
1478

1479
1480
1481
1482
1483
1484
1485
1486
			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
			{
1487
				LOG_ERROR("unhandled core state");
1488
				return ERROR_FAIL;
1489
			}
1490

1491
1492
			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));
1493
			err = arm7_9_execute_sys_speed(target);
1494

1495
			LOG_DEBUG("disable single-step");
1496
			arm7_9->disable_single_step(target);
1497
1498
1499
1500
1501
1502
1503
1504

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

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

1508
			LOG_DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
1509
1510
1511
			arm7_9_set_breakpoint(target, breakpoint);
		}
	}
1512

1513
1514
1515
	/* enable any pending breakpoints and watchpoints */
	arm7_9_enable_breakpoints(target);
	arm7_9_enable_watchpoints(target);
1516

1517
	arm7_9_restore_context(target);
1518

1519
1520
1521
1522
1523
1524
1525
1526
1527
1528
	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
	{
1529
		LOG_ERROR("unhandled core state");
1530
		return ERROR_FAIL;
1531
	}
1532

1533
1534
1535
1536
1537
1538
	/* 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));
1539

1540
	arm7_9_restart_core(target);
1541

1542
	target->debug_reason = DBG_REASON_NOTHALTED;
1543

1544
1545
1546
1547
1548
1549
1550
1551
1552
1553
1554
1555
	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);
	}
1556

1557
	LOG_DEBUG("target resumed");
1558

1559
1560
1561
1562
1563
1564
1565
	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;
1566

1567
1568
1569
1570
1571
1572
	/* 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);
1573
1574
	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);
1575
1576
1577
1578
	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);
1579
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1580
1581
1582
1583
1584
1585
1586
1587
1588
1589
1590
1591
1592
1593
1594
1595
1596
1597
1598
1599
1600
1601
}

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;
1602
	breakpoint_t *breakpoint = NULL;
1603
	int err;
1604
1605
1606

	if (target->state != TARGET_HALTED)
	{
1607
		LOG_WARNING("target not halted");
1608
1609
		return ERROR_TARGET_NOT_HALTED;
	}
1610

1611
1612
1613
	/* 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);
1614

1615
1616
1617
1618
	/* 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);
1619

1620
1621
1622
	target->debug_reason = DBG_REASON_SINGLESTEP;

	arm7_9_restore_context(target);
1623

1624
	arm7_9->enable_single_step(target);
1625

1626
1627
1628
1629
1630
1631
1632
1633
1634
1635
	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
	{
1636
		LOG_ERROR("unhandled core state");
1637
		return ERROR_FAIL;
1638
	}
1639

1640
1641
	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);

1642
	err = arm7_9_execute_sys_speed(target);
1643
	arm7_9->disable_single_step(target);
1644

1645
1646
	/* registers are now invalid */
	armv4_5_invalidate_core_regs(target);
1647

1648
1649
1650
1651
1652
1653
	if (err != ERROR_OK)
	{
		target->state = TARGET_UNKNOWN;
	} else {
		arm7_9_debug_entry(target);
		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1654
		LOG_DEBUG("target stepped");
1655
	}
1656

1657
1658
	if (breakpoint)
		arm7_9_set_breakpoint(target, breakpoint);
1659

1660
	return err;
1661
1662
1663
1664
1665
1666

}

int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
{
	u32* reg_p[16];
1667
	u32 value;
1668
1669
1670
	int retval;
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1671

1672
1673
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1674

1675
	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;
1676

1677
1678
	if ((num < 0) || (num > 16))
		return ERROR_INVALID_ARGUMENTS;
1679

1680
1681
1682
1683
1684
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))
	{
		u32 tmp_cpsr;
1685

1686
		/* change processor mode (mask T bit) */
1687
1688
		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
		tmp_cpsr |= mode;
1689
		tmp_cpsr &= ~0x20;
1690
1691
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
1692

1693
1694
1695
	if ((num >= 0) && (num <= 15))
	{
		/* read a normal core register */
1696
		reg_p[num] = &value;
1697

1698
1699
1700
1701
1702
1703
1704
1705
1706
		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;
1707

1708
		arm7_9->read_xpsr(target, &value, spsr);
1709
	}
1710

1711
1712
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1713
		return retval;
1714
	}
1715

1716
1717
	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;
1718
	buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value, 0, 32, value);
1719

1720
1721
1722
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
1723
1724
		/* 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);
1725
	}
1726

1727
	return ERROR_OK;
1728

1729
1730
1731
1732
1733
1734
1735
}

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

1737
1738
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
1739

1740
1741
1742
1743
	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;
1744

1745
1746
1747
1748
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
		u32 tmp_cpsr;
1749

1750
		/* change processor mode (mask T bit) */
1751
1752
		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
		tmp_cpsr |= mode;
1753
		tmp_cpsr &= ~0x20;
1754
1755
		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
	}
1756

1757
1758
1759
1760
	if ((num >= 0) && (num <= 15))
	{
		/* write a normal core register */
		reg[num] = value;
1761

1762
1763
1764
1765
1766
1767
1768
1769
1770
		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;
1771

1772
1773
1774
		/* if we're writing the CPSR, mask the T bit */
		if (!spsr)
			value &= ~0x20;
1775

1776
1777
		arm7_9->write_xpsr(target, value, spsr);
	}
1778

1779
1780
	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;
1781

1782
1783
1784
	if ((mode != ARMV4_5_MODE_ANY)
			&& (mode != armv4_5->core_mode)
			&& (reg_mode != ARMV4_5_MODE_ANY))	{
1785
1786
		/* 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);
1787
	}
1788

1789
	return jtag_execute_queue();
1790
1791
1792
1793
1794
1795
}

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

1797
1798
1799
1800
1801
1802
1803
	u32 reg[16];
	int num_accesses = 0;
	int thisrun_accesses;
	int i;
	u32 cpsr;
	int retval;
	int last_reg = 0;
1804

1805
	LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1806
1807
1808

	if (target->state != TARGET_HALTED)
	{
1809
		LOG_WARNING("target not halted");
1810
1811
1812
1813
1814
1815
1816
1817
1818
		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;
1819

1820
1821
1822
	/* load the base register with the address of the first word */
	reg[0] = address;
	arm7_9->write_core_regs(target, 0x1, reg);
1823

oharboe's avatar
oharboe committed
1824
1825
	int j=0;

1826
1827
1828
1829
1830
1831
1832
1833
	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;
1834

1835
1836
				if (last_reg <= thisrun_accesses)
					last_reg = thisrun_accesses;
1837

1838
				arm7_9->load_word_regs(target, reg_list);
1839

1840
1841
1842
1843
1844
1845
1846
				/* 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);
1847

1848
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 4);
1849

1850
1851
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 4;
1852
				num_accesses += thisrun_accesses;
oharboe's avatar
oharboe committed
1853
1854
1855
1856
1857

				if ((j++%1024)==0)
				{
					keep_alive();
				}
1858
			}
1859
1860
1861
1862
1863
1864
1865
			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;
1866

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

1881
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 2);
1882

1883
1884
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 2;
1885
				num_accesses += thisrun_accesses;
oharboe's avatar
oharboe committed
1886
1887
1888
1889
1890

				if ((j++%1024)==0)
				{
					keep_alive();
				}
1891
			}
1892
1893
1894
1895
1896
1897
1898
			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;
1899

1900
1901
1902
1903
1904
				for (i = 1; i <= thisrun_accesses; i++)
				{
					if (i > last_reg)
						last_reg = i;
					arm7_9->load_byte_reg(target, i);
1905
1906
1907
1908
1909
1910
1911
					/* 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);
1912
				}
1913

1914
				arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 1);
1915

1916
1917
				/* advance buffer, count number of accesses */
				buffer += thisrun_accesses * 1;
1918
				num_accesses += thisrun_accesses;
oharboe's avatar
oharboe committed
1919
1920
1921
1922
1923

				if ((j++%1024)==0)
				{
					keep_alive();
				}
1924
			}
1925
1926
			break;
		default:
1927
			LOG_ERROR("BUG: we shouldn't get here");
1928
1929
1930
			exit(-1);
			break;
	}
1931

1932
1933
1934
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;

1935
	for (i=0; i<=last_reg; i++)
1936
		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;
1937
1938
1939
1940

	arm7_9->read_xpsr(target, &cpsr, 0);
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
1941
		LOG_ERROR("JTAG error while reading cpsr");
ntfreak's avatar
ntfreak committed
1942
		return ERROR_TARGET_DATA_ABORT;
1943
1944
1945
1946
	}

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

1949
		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1950
1951
1952

		return ERROR_TARGET_DATA_ABORT;
	}
1953

1954
1955
1956
1957
1958
1959
1960
	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;
1961
	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1962

1963
1964
1965
1966
1967
1968
1969
1970
	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
1971
#ifdef _DEBUG_ARM7_9_
1972
	LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
ntfreak's avatar
ntfreak committed
1973
#endif
1974
1975
1976

	if (target->state != TARGET_HALTED)
	{
1977
		LOG_WARNING("target not halted");
1978
1979
1980
1981
1982
1983
1984
1985
1986
		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;
1987

1988
1989
1990
	/* load the base register with the address of the first word */
	reg[0] = address;
	arm7_9->write_core_regs(target, 0x1, reg);
1991

1992
1993
1994
	/* 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);
1995

1996
1997
1998
1999
2000
	switch (size)
	{
		case 4:
			while (num_accesses < count)
			{
For faster browsing, not all history is shown. View entire blame