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

1026
	arm7_9_clear_halt(target);
1027

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1159
	LOG_DEBUG("-");
1160

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

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

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

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

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

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

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

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

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

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

1244
	LOG_DEBUG("-");
1245

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

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

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

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

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

			if (mode_change)
			{
				u32 tmp_cpsr;
1303

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

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


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

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

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

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

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

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

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

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

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

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

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

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

1426
	LOG_DEBUG("-");
1427

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

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

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

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

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

1454
1455
1456
			target->debug_reason = DBG_REASON_SINGLESTEP;

			arm7_9_restore_context(target);
1457

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

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

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

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

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

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

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

1496
	arm7_9_restore_context(target);
1497

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

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

1519
	arm7_9_restart_core(target);
1520

1521
	target->debug_reason = DBG_REASON_NOTHALTED;
1522

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

1536
	LOG_DEBUG("target resumed");
1537

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

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

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

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

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

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

1599
1600
1601
	target->debug_reason = DBG_REASON_SINGLESTEP;

	arm7_9_restore_context(target);
1602

1603
	arm7_9->enable_single_step(target);
1604

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

1619
1620
	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);

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

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

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

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

1639
	return err;
1640
1641
1642
1643
1644
1645

}

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

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

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

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

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

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

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

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

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

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

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

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

1706
	return ERROR_OK;
1707

1708
1709
1710
1711
1712
1713
1714
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1867
1868
1869
1870
1871
				for (i = 1; i <= thisrun_accesses; i++)
				{
					if (i > last_reg)
						last_reg = i;
					arm7_9->load_byte_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, 1);
1882

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

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

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

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

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

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

		return ERROR_TARGET_DATA_ABORT;
	}
1915

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

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

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

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

1954
1955
1956
	/* 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);
1957

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

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

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

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

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

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

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