X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Farm946e.c;h=278a70ca24f0447b82801491fd75fbd9de701efd;hp=b9b9cef768d712d8b1fd9d646722462bd29639a1;hb=c734202dc89bb3ee05a204140b3c890451e79686;hpb=9e3d43cfe75df7c4f6797d630576f1a02428b218

diff --git a/src/target/arm946e.c b/src/target/arm946e.c
index b9b9cef768..278a70ca24 100644
--- a/src/target/arm946e.c
+++ b/src/target/arm946e.c
@@ -19,10 +19,9 @@
  *   GNU General Public License for more details.                          *
  *                                                                         *
  *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  ***************************************************************************/
+
 #ifdef HAVE_CONFIG_H
 #include "config.h"
 #endif
@@ -39,8 +38,9 @@
 
 #define NB_CACHE_WAYS 4
 
-static uint32_t dc = 0x0;
-static uint32_t ic = 0x0;
+#define CP15_CTL		0x02
+#define CP15_CTL_DCACHE (1<<2)
+#define CP15_CTL_ICACHE (1<<12)
 
 /**
  * flag to give info about cache manipulation during debug :
@@ -55,8 +55,9 @@ int arm946e_post_debug_entry(struct target *target);
 void arm946e_pre_restore_context(struct target *target);
 static int arm946e_read_cp15(struct target *target, int reg_addr, uint32_t *value);
 
-
-int arm946e_init_arch_info(struct target *target, struct arm946e_common *arm946e, struct jtag_tap *tap)
+int arm946e_init_arch_info(struct target *target,
+	struct arm946e_common *arm946e,
+	struct jtag_tap *tap)
 {
 	struct arm7_9_common *arm7_9 = &arm946e->arm7_9_common;
 
@@ -84,30 +85,40 @@ int arm946e_init_arch_info(struct target *target, struct arm946e_common *arm946e
 	arm946e_preserve_cache = 0;
 
 	/* override hw single-step capability from ARM9TDMI */
-	//arm7_9->has_single_step = 1;
+	/* arm7_9->has_single_step = 1; */
 
 	return ERROR_OK;
 }
 
 static int arm946e_target_create(struct target *target, Jim_Interp *interp)
 {
-	struct arm946e_common *arm946e = calloc(1,sizeof(struct arm946e_common));
+	struct arm946e_common *arm946e = calloc(1, sizeof(struct arm946e_common));
 
 	arm946e_init_arch_info(target, arm946e, target->tap);
 
 	return ERROR_OK;
 }
 
-static int arm946e_verify_pointer(struct command_context *cmd_ctx,
-		struct arm946e_common *arm946e)
+static int arm946e_verify_pointer(struct command_invocation *cmd,
+	struct arm946e_common *arm946e)
 {
 	if (arm946e->common_magic != ARM946E_COMMON_MAGIC) {
-		command_print(cmd_ctx, "target is not an ARM946");
+		command_print(cmd->ctx, "target is not an ARM946");
 		return ERROR_TARGET_INVALID;
 	}
 	return ERROR_OK;
 }
 
+/*
+ * Update cp15_control_reg, saved on debug_entry.
+ */
+static void arm946e_update_cp15_caches(struct target *target, uint32_t value)
+{
+	struct arm946e_common *arm946e = target_to_arm946(target);
+	arm946e->cp15_control_reg = (arm946e->cp15_control_reg & ~(CP15_CTL_DCACHE|CP15_CTL_ICACHE))
+		| (value & (CP15_CTL_DCACHE|CP15_CTL_ICACHE));
+}
+
 /*
  * REVISIT:  The "read_cp15" and "write_cp15" commands could hook up
  * to eventual mrc() and mcr() routines ... the reg_addr values being
@@ -123,11 +134,10 @@ static int arm946e_read_cp15(struct target *target, int reg_addr, uint32_t *valu
 	uint8_t reg_addr_buf = reg_addr & 0x3f;
 	uint8_t nr_w_buf = 0;
 
-	if ((retval = arm_jtag_scann(jtag_info, 0xf, TAP_IDLE)) != ERROR_OK)
-	{
+	retval = arm_jtag_scann(jtag_info, 0xf, TAP_IDLE);
+	if (retval != ERROR_OK)
 		return retval;
-	}
-	retval = arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL, TAP_IDLE);
+	retval = arm_jtag_set_instr(jtag_info->tap, jtag_info->intest_instr, NULL, TAP_IDLE);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -157,10 +167,9 @@ static int arm946e_read_cp15(struct target *target, int reg_addr, uint32_t *valu
 	LOG_DEBUG("addr: 0x%x value: %8.8x", reg_addr, *value);
 #endif
 
-	if ((retval = jtag_execute_queue()) != ERROR_OK)
-	{
+	retval = jtag_execute_queue();
+	if (retval != ERROR_OK)
 		return retval;
-	}
 
 	return ERROR_OK;
 }
@@ -177,11 +186,10 @@ int arm946e_write_cp15(struct target *target, int reg_addr, uint32_t value)
 
 	buf_set_u32(value_buf, 0, 32, value);
 
-	if ((retval = arm_jtag_scann(jtag_info, 0xf, TAP_IDLE)) != ERROR_OK)
-	{
+	retval = arm_jtag_scann(jtag_info, 0xf, TAP_IDLE);
+	if (retval != ERROR_OK)
 		return retval;
-	}
-	retval = arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL, TAP_IDLE);
+	retval = arm_jtag_set_instr(jtag_info->tap, jtag_info->intest_instr, NULL, TAP_IDLE);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -203,73 +211,79 @@ int arm946e_write_cp15(struct target *target, int reg_addr, uint32_t value)
 	LOG_DEBUG("addr: 0x%x value: %8.8x", reg_addr, value);
 #endif
 
-	if ((retval = jtag_execute_queue()) != ERROR_OK)
-	{
+	retval = jtag_execute_queue();
+	if (retval != ERROR_OK)
 		return retval;
-	}
 
 	return ERROR_OK;
 }
 
-uint32_t arm946e_invalidate_whole_dcache(struct target *target)
-{
-
-   uint32_t csize = 0;
-   uint32_t shift = 0;
-   uint32_t cp15_idx, seg, dtag;
-   int nb_idx, idx = 0;
-   int retval;
-
-   /* Get cache type */
-   arm946e_read_cp15(target, 0x01, (uint32_t *) &csize);
+#define GET_ICACHE_SIZE  6
+#define GET_DCACHE_SIZE 18
 
-   csize = (csize >> 18) & 0x0F;
+/*
+ * \param target struct target pointer
+ * \param idsel  select GET_ICACHE_SIZE or GET_DCACHE_SIZE
+ * \returns      cache size, given in bytes
+ */
+static uint32_t arm946e_cp15_get_csize(struct target *target, int idsel)
+{
+	struct arm946e_common *arm946e = target_to_arm946(target);
+	uint32_t csize = arm946e->cp15_cache_info;
+	if (csize == 0) {
+		if (arm946e_read_cp15(target, 0x01, &csize) == ERROR_OK)
+			arm946e->cp15_cache_info = csize;
+	}
+	if (csize & (1<<(idsel-4)))	/* cache absent */
+		return 0;
+	csize = (csize >> idsel) & 0x0F;
+	return csize ? 1 << (12 + (csize-3)) : 0;
+}
 
+uint32_t arm946e_invalidate_whole_dcache(struct target *target)
+{
+	uint32_t csize = arm946e_cp15_get_csize(target, GET_DCACHE_SIZE);
 	if (csize == 0)
-		shift = 0;
-	else
-		shift = csize - 0x3; /* Now 0 = 4KB, 1 = 8KB, ... */
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
-	/* Cache size, given in bytes */
-	csize = 1 << (12 + shift);
-	/* One line (index) is 32 bytes (8 words) long */
-	nb_idx = (csize / 32);	/* gives nb of lines (indexes) in the cache */
+	/* One line (index) is 32 bytes (8 words) long, 4-way assoc
+	 * ARM DDI 0201D, Section 3.3.5
+	 */
+	int nb_idx = (csize / (4*8*NB_CACHE_WAYS));	/* gives nb of lines (indexes) in the cache */
 
 	/* Loop for all segmentde (i.e. ways) */
-	for( seg=0; seg < NB_CACHE_WAYS; seg++)
-	{
+	uint32_t seg;
+	for (seg = 0; seg < NB_CACHE_WAYS; seg++) {
 		/* Loop for all indexes */
-		for(idx=0; idx < nb_idx; idx++)
-		{
+		int idx;
+		for (idx = 0; idx < nb_idx; idx++) {
 			/* Form and write cp15 index (segment + line idx) */
-			cp15_idx = seg << 30 | idx << 5;
-			retval = arm946e_write_cp15(target, 0x3a, cp15_idx);
-			if (retval != ERROR_OK)
-			{
-				LOG_DEBUG("ERROR writing index\n");
+			uint32_t cp15_idx = seg << 30 | idx << 5;
+			int retval = arm946e_write_cp15(target, 0x3a, cp15_idx);
+			if (retval != ERROR_OK) {
+				LOG_DEBUG("ERROR writing index");
 				return retval;
 			}
 
 			/* Read dtag */
+			uint32_t dtag;
 			arm946e_read_cp15(target, 0x16, (uint32_t *) &dtag);
 
 			/* Check cache line VALID bit */
-			if ( !(dtag >> 4 & 0x1) )
+			if (!(dtag >> 4 & 0x1))
 				continue;
 
 			/* Clean data cache line */
 			retval = arm946e_write_cp15(target, 0x35, 0x1);
-			if (retval != ERROR_OK)
-			{
-				LOG_DEBUG("ERROR cleaning cache line\n");
+			if (retval != ERROR_OK) {
+				LOG_DEBUG("ERROR cleaning cache line");
 				return retval;
 			}
 
 			/* Flush data cache line */
 			retval = arm946e_write_cp15(target, 0x1a, 0x1);
-			if (retval != ERROR_OK)
-			{
-				LOG_DEBUG("ERROR flushing cache line\n");
+			if (retval != ERROR_OK) {
+				LOG_DEBUG("ERROR flushing cache line");
 				return retval;
 			}
 		}
@@ -280,18 +294,19 @@ uint32_t arm946e_invalidate_whole_dcache(struct target *target)
 
 uint32_t arm946e_invalidate_whole_icache(struct target *target)
 {
-	int retval;
+	/* Check cache presence before flushing - avoid undefined behavior */
+	uint32_t csize = arm946e_cp15_get_csize(target, GET_ICACHE_SIZE);
+	if (csize == 0)
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
 	LOG_DEBUG("FLUSHING I$");
-
 	/**
 	 *  Invalidate (flush) I$
 	 *  mcr	15, 0, r0, cr7, cr5, {0}
 	 */
-	retval = arm946e_write_cp15(target, 0x0f, 0x1);
-	if (retval != ERROR_OK)
-	{
-		LOG_DEBUG("ERROR flushing I$\n");
+	int retval = arm946e_write_cp15(target, 0x0f, 0x1);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("ERROR flushing I$");
 		return retval;
 	}
 
@@ -302,41 +317,39 @@ int arm946e_post_debug_entry(struct target *target)
 {
 	uint32_t ctr_reg = 0x0;
 	uint32_t retval = ERROR_OK;
+	struct arm946e_common *arm946e = target_to_arm946(target);
 
 	/* See if CACHES are enabled, and save that info
-	 * in the global vars, so that arm946e_pre_restore_context() can use them */
-	arm946e_read_cp15(target, 0x02, (uint32_t *) &ctr_reg);
-	dc = (ctr_reg >> 2) & 0x01;
-	ic = (ctr_reg >> 12) & 0x01;
+	 * in the context bits, so that arm946e_pre_restore_context() can use them */
+	arm946e_read_cp15(target, CP15_CTL, (uint32_t *) &ctr_reg);
 
-	if (arm946e_preserve_cache)
-	{
-		if (dc == 1)
-		{
+	/* Save control reg in the context */
+	arm946e->cp15_control_reg = ctr_reg;
+
+	if (arm946e_preserve_cache) {
+		if (ctr_reg & CP15_CTL_DCACHE) {
 			/* Clean and flush D$ */
 			arm946e_invalidate_whole_dcache(target);
 
 			/* Disable D$ */
-			ctr_reg &= ~(1 << 2);
+			ctr_reg &= ~CP15_CTL_DCACHE;
 		}
 
-		if (ic == 1)
-		{
+		if (ctr_reg & CP15_CTL_ICACHE) {
 			/* Flush I$ */
 			arm946e_invalidate_whole_icache(target);
 
 			/* Disable I$ */
-			ctr_reg &= ~(1 << 12);
+			ctr_reg &= ~CP15_CTL_ICACHE;
 		}
 
 		/* Write the new configuration */
-		retval = arm946e_write_cp15(target, 0x02, ctr_reg);
-		if (retval != ERROR_OK)
-		{
+		retval = arm946e_write_cp15(target, CP15_CTL, ctr_reg);
+		if (retval != ERROR_OK) {
 			LOG_DEBUG("ERROR disabling cache");
 			return retval;
 		}
-	} /* if preserve_cache */
+	}	/* if preserve_cache */
 
 	return ERROR_OK;
 }
@@ -346,69 +359,41 @@ void arm946e_pre_restore_context(struct target *target)
 	uint32_t ctr_reg = 0x0;
 	uint32_t retval;
 
-	if (arm946e_preserve_cache)
-	{
+	if (arm946e_preserve_cache) {
+		struct arm946e_common *arm946e = target_to_arm946(target);
 		/* Get the contents of the CTR reg */
-		arm946e_read_cp15(target, 0x02, (uint32_t *) &ctr_reg);
+		arm946e_read_cp15(target, CP15_CTL, (uint32_t *) &ctr_reg);
 
 		/**
-		 * Read-modify-write CP15 test state register
-		 * to reenable I/D-cache linefills
+		 * Read-modify-write CP15 control
+		 * to reenable I/D-cache operation
+		 * NOTE: It is not possible to disable cache by CP15.
+		 * if arm946e_preserve_cache debugging flag enabled.
 		 */
-		if (dc == 1)
-		{
-			/* Enable D$ */
-			ctr_reg |= 1 << 2;
-		}
-
-		if (ic == 1)
-		{
-			/* Enable I$ */
-			ctr_reg |= 1 << 12;
-		}
+		ctr_reg |= arm946e->cp15_control_reg & (CP15_CTL_DCACHE|CP15_CTL_ICACHE);
 
 		/* Write the new configuration */
-		retval = arm946e_write_cp15(target, 0x02, ctr_reg);
+		retval = arm946e_write_cp15(target, CP15_CTL, ctr_reg);
 		if (retval != ERROR_OK)
-		{
-			LOG_DEBUG("ERROR enabling cache\n");
-		}
-	} /* if preserve_cache */
+			LOG_DEBUG("ERROR enabling cache");
+	}	/* if preserve_cache */
 }
 
 uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
-		uint32_t size, uint32_t count)
+	uint32_t size, uint32_t count)
 {
-	uint32_t csize = 0x0;
-	uint32_t shift = 0;
 	uint32_t cur_addr = 0x0;
 	uint32_t cp15_idx, set, way, dtag;
-	int nb_idx;
 	uint32_t i = 0;
 	int retval;
 
-	for(i = 0; i < count*size; i++)
-	{
+	for (i = 0; i < count*size; i++) {
 		cur_addr = address + i;
 
-		/* Get cache type */
-		arm946e_read_cp15(target, 0x01, (uint32_t *) &csize);
-
-		/* Conclude cache size to find number of lines */
-		csize = (csize >> 18) & 0x0F;
-
-		if (csize == 0)
-			shift = 0;
-		else
-			shift = csize - 0x3; /* Now 0 = 4KB, 1 = 8KB, ... */
-
-		csize = 1 << (12 + shift);
-		nb_idx = (csize / 32);
 
 		set = (cur_addr >> 5) & 0xff;	/* set field is 8 bits long */
 
-		for (way = 0; way < NB_CACHE_WAYS; way++)
-		{
+		for (way = 0; way < NB_CACHE_WAYS; way++) {
 			/**
 			 * Find if the affected address is kept in the cache.
 			 * Because JTAG Scan Chain 15 offers limited approach,
@@ -419,9 +404,8 @@ uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
 			/* Form and write cp15 index (segment + line idx) */
 			cp15_idx = way << 30 | set << 5;
 			retval = arm946e_write_cp15(target, 0x3a, cp15_idx);
-			if (retval != ERROR_OK)
-			{
-				LOG_DEBUG("ERROR writing index\n");
+			if (retval != ERROR_OK) {
+				LOG_DEBUG("ERROR writing index");
 				return retval;
 			}
 
@@ -429,58 +413,52 @@ uint32_t arm946e_invalidate_dcache(struct target *target, uint32_t address,
 			arm946e_read_cp15(target, 0x16, (uint32_t *) &dtag);
 
 			/* Check cache line VALID bit */
-			if ( !(dtag >> 4 & 0x1) )
+			if (!(dtag >> 4 & 0x1))
 				continue;
 
 			/* If line is valid and corresponds to affected address - invalidate it */
-			if (dtag >> 5 == cur_addr >> 5)
-			{
+			if (dtag >> 5 == cur_addr >> 5) {
 				/* Clean data cache line */
 				retval = arm946e_write_cp15(target, 0x35, 0x1);
-				if (retval != ERROR_OK)
-				{
-					LOG_DEBUG("ERROR cleaning cache line\n");
+				if (retval != ERROR_OK) {
+					LOG_DEBUG("ERROR cleaning cache line");
 					return retval;
 				}
 
 				/* Flush data cache line */
 				retval = arm946e_write_cp15(target, 0x1c, 0x1);
-				if (retval != ERROR_OK)
-				{
-					LOG_DEBUG("ERROR flushing cache line\n");
+				if (retval != ERROR_OK) {
+					LOG_DEBUG("ERROR flushing cache line");
 					return retval;
 				}
 
 				break;
 			}
-		} /* loop through all 4 ways */
-	} /* loop through all addresses */
+		}	/* loop through all 4 ways */
+	}	/* loop through all addresses */
 
 	return ERROR_OK;
 }
 
 uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
-		uint32_t size, uint32_t count)
+	uint32_t size, uint32_t count)
 {
 	uint32_t cur_addr = 0x0;
 	uint32_t cp15_idx, set, way, itag;
 	uint32_t i = 0;
 	int retval;
 
-	for(i = 0; i < count*size; i++)
-	{
+	for (i = 0; i < count*size; i++) {
 		cur_addr = address + i;
 
 		set = (cur_addr >> 5) & 0xff;	/* set field is 8 bits long */
 
-		for (way = 0; way < NB_CACHE_WAYS; way++)
-		{
+		for (way = 0; way < NB_CACHE_WAYS; way++) {
 			/* Form and write cp15 index (segment + line idx) */
 			cp15_idx = way << 30 | set << 5;
 			retval = arm946e_write_cp15(target, 0x3a, cp15_idx);
-			if (retval != ERROR_OK)
-			{
-				LOG_DEBUG("ERROR writing index\n");
+			if (retval != ERROR_OK) {
+				LOG_DEBUG("ERROR writing index");
 				return retval;
 			}
 
@@ -488,50 +466,45 @@ uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
 			arm946e_read_cp15(target, 0x17, (uint32_t *) &itag);
 
 			/* Check cache line VALID bit */
-			if ( !(itag >> 4 & 0x1) )
+			if (!(itag >> 4 & 0x1))
 				continue;
 
 			/* If line is valid and corresponds to affected address - invalidate it */
-			if (itag >> 5 == cur_addr >> 5)
-			{
+			if (itag >> 5 == cur_addr >> 5) {
 				/* Flush I$ line */
 				retval = arm946e_write_cp15(target, 0x1d, 0x0);
-				if (retval != ERROR_OK)
-				{
-					LOG_DEBUG("ERROR flushing cache line\n");
+				if (retval != ERROR_OK) {
+					LOG_DEBUG("ERROR flushing cache line");
 					return retval;
 				}
 
 				break;
 			}
-		} /* way loop */
-	} /* addr loop */
+		}	/* way loop */
+	}	/* addr loop */
 
 	return ERROR_OK;
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm946e_write_memory(struct target *target, uint32_t address,
-		uint32_t size, uint32_t count, uint8_t *buffer)
+int arm946e_write_memory(struct target *target, target_addr_t address,
+	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
 
 	LOG_DEBUG("-");
 
+	struct arm946e_common *arm946e = target_to_arm946(target);
 	/* Invalidate D$ if it is ON */
-	if (!arm946e_preserve_cache && dc == 1)
-	{
+	if (!arm946e_preserve_cache && (arm946e->cp15_control_reg & CP15_CTL_DCACHE))
 		arm946e_invalidate_dcache(target, address, size, count);
-	}
 
 	/**
 	 * Write memory
 	 */
-	if ( ( retval = arm7_9_write_memory(target, address,
-			size, count, buffer) ) != ERROR_OK )
-	{
+	retval = arm7_9_write_memory_opt(target, address, size, count, buffer);
+	if (retval != ERROR_OK)
 		return retval;
-	}
 
 	/* *
 	 * Invalidate I$ if it is ON.
@@ -555,98 +528,183 @@ int arm946e_write_memory(struct target *target, uint32_t address,
 	 * the cache write policy is write-through.
 	 * If the data is not in the cache, the controller writes to main memory only.
 	 */
-	if (!arm946e_preserve_cache && ic == 1)
-	{
+	if (!arm946e_preserve_cache && (arm946e->cp15_control_reg & CP15_CTL_ICACHE))
 		arm946e_invalidate_icache(target, address, size, count);
-	}
 
 	return ERROR_OK;
 
 }
 
-int arm946e_read_memory(struct target *target, uint32_t address,
-		uint32_t size, uint32_t count, uint8_t *buffer)
+int arm946e_read_memory(struct target *target, target_addr_t address,
+	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
 
 	LOG_DEBUG("-");
 
-	if ( ( retval = arm7_9_read_memory(target, address,
-			size, count, buffer) ) != ERROR_OK )
-	{
+	retval = arm7_9_read_memory(target, address, size, count, buffer);
+	if (retval != ERROR_OK)
 		return retval;
-	}
 
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(arm946e_handle_cp15)
+{
+	/* one or two arguments, access a single register (write if second argument is given) */
+	if (CMD_ARGC < 1 || CMD_ARGC > 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct target *target = get_current_target(CMD_CTX);
+
+	struct arm946e_common *arm946e = target_to_arm946(target);
+	int retval = arm946e_verify_pointer(CMD, arm946e);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target->state != TARGET_HALTED) {
+		command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	uint32_t address;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+
+	if (CMD_ARGC == 1) {
+		uint32_t value;
+		retval = arm946e_read_cp15(target, address, &value);
+		if (retval != ERROR_OK) {
+			command_print(CMD_CTX, "%s cp15 reg %" PRIi32 " access failed", target_name(target), address);
+			return retval;
+		}
+		retval = jtag_execute_queue();
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Return value in hex format */
+		command_print(CMD_CTX, "0x%08" PRIx32, value);
+	} else if (CMD_ARGC == 2) {
+		uint32_t value;
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
+
+		retval = arm946e_write_cp15(target, address, value);
+		if (retval != ERROR_OK) {
+			command_print(CMD_CTX, "%s cp15 reg %" PRIi32 " access failed", target_name(target), address);
+			return retval;
+		}
+		if (address == CP15_CTL)
+			arm946e_update_cp15_caches(target, value);
+	}
+
+	return ERROR_OK;
+}
 
-COMMAND_HANDLER(arm946e_handle_cp15_command)
+COMMAND_HANDLER(arm946e_handle_idcache)
 {
+	if (CMD_ARGC > 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
 	int retval;
 	struct target *target = get_current_target(CMD_CTX);
 	struct arm946e_common *arm946e = target_to_arm946(target);
 
-	retval = arm946e_verify_pointer(CMD_CTX, arm946e);
+	retval = arm946e_verify_pointer(CMD, arm946e);
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (target->state != TARGET_HALTED)
-	{
+	if (target->state != TARGET_HALTED) {
 		command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	bool icache = (strcmp(CMD_NAME, "icache") == 0);
+	uint32_t csize = arm946e_cp15_get_csize(target, icache ? GET_ICACHE_SIZE : GET_DCACHE_SIZE) / 1024;
+	if (CMD_ARGC == 0) {
+		bool  bena = ((arm946e->cp15_control_reg & (icache ? CP15_CTL_ICACHE : CP15_CTL_DCACHE)) != 0)
+			  && (arm946e->cp15_control_reg & 0x1);
+		if (csize == 0)
+			command_print(CMD_CTX, "%s-cache absent", icache ? "I" : "D");
+		else
+			command_print(CMD_CTX, "%s-cache size: %" PRIu32 "K, %s",
+				      icache ? "I" : "D", csize, bena ? "enabled" : "disabled");
 		return ERROR_OK;
 	}
 
-	/* one or more argument, access a single register (write if second argument is given */
-	if (CMD_ARGC >= 1)
-	{
-		uint32_t address;
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
-
-		if (CMD_ARGC == 1)
-		{
-			uint32_t value;
-			if ((retval = arm946e_read_cp15(target, address, &value)) != ERROR_OK)
-			{
-				command_print(CMD_CTX,
-						"couldn't access reg %" PRIi32,
-						address);
-				return ERROR_OK;
-			}
-			if ((retval = jtag_execute_queue()) != ERROR_OK)
-			{
-				return retval;
-			}
+	bool flush = false;
+	bool enable = false;
+	retval = command_parse_bool_arg(CMD_ARGV[0], &enable);
+	if (retval == ERROR_COMMAND_SYNTAX_ERROR) {
+		if (strcmp(CMD_ARGV[0], "flush") == 0) {
+			flush = true;
+			retval = ERROR_OK;
+		} else
+			return retval;
+	}
 
-			command_print(CMD_CTX, "%" PRIi32 ": %8.8" PRIx32,
-					address, value);
-		}
-		else if (CMD_ARGC == 2)
-		{
-			uint32_t value;
-			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
-			if ((retval = arm946e_write_cp15(target, address, value)) != ERROR_OK)
-			{
-				command_print(CMD_CTX,
-						"couldn't access reg %" PRIi32,
-						address);
-				return ERROR_OK;
-			}
-			command_print(CMD_CTX, "%" PRIi32 ": %8.8" PRIx32,
-					address, value);
-		}
+	/* Do not invalidate or change state, if cache is absent */
+	if (csize == 0) {
+		command_print(CMD_CTX, "%s-cache absent, '%s' operation undefined", icache ? "I" : "D", CMD_ARGV[0]);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	/* NOTE: flushing entire cache will not preserve lock-down cache regions */
+	if (icache) {
+		if ((arm946e->cp15_control_reg & CP15_CTL_ICACHE) && !enable)
+			retval = arm946e_invalidate_whole_icache(target);
+	} else {
+		if ((arm946e->cp15_control_reg & CP15_CTL_DCACHE) && !enable)
+			retval = arm946e_invalidate_whole_dcache(target);
 	}
 
+	if (retval != ERROR_OK || flush)
+		return retval;
+
+	uint32_t value;
+	retval = arm946e_read_cp15(target, CP15_CTL, &value);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t vnew = value;
+	uint32_t cmask = icache ? CP15_CTL_ICACHE : CP15_CTL_DCACHE;
+	if (enable) {
+		if ((value & 0x1) == 0)
+			LOG_WARNING("arm946e: MPU must be enabled for cache to operate");
+		vnew |= cmask;
+	} else
+		vnew &= ~cmask;
+
+	if (vnew == value)
+		return ERROR_OK;
+
+	retval = arm946e_write_cp15(target, CP15_CTL, vnew);
+	if (retval != ERROR_OK)
+		return retval;
+
+	arm946e_update_cp15_caches(target, vnew);
 	return ERROR_OK;
 }
 
 static const struct command_registration arm946e_exec_command_handlers[] = {
 	{
 		.name = "cp15",
-		.handler = arm946e_handle_cp15_command,
+		.handler = arm946e_handle_cp15,
 		.mode = COMMAND_EXEC,
 		.usage = "regnum [value]",
-		.help = "display/modify cp15 register",
+		.help = "read/modify cp15 register",
+	},
+	{
+		.name = "icache",
+		.handler = arm946e_handle_idcache,
+		.mode = COMMAND_EXEC,
+		.usage = "['enable'|'disable'|'flush']",
+		.help = "I-cache info and operations",
+	},
+	{
+		.name = "dcache",
+		.handler = arm946e_handle_idcache,
+		.mode = COMMAND_EXEC,
+		.usage = "['enable'|'disable'|'flush']",
+		.help = "D-cache info and operations",
 	},
 	COMMAND_REGISTRATION_DONE
 };
@@ -659,14 +717,14 @@ const struct command_registration arm946e_command_handlers[] = {
 		.name = "arm946e",
 		.mode = COMMAND_ANY,
 		.help = "arm946e command group",
+		.usage = "",
 		.chain = arm946e_exec_command_handlers,
 	},
 	COMMAND_REGISTRATION_DONE
 };
 
 /** Holds methods for ARM946 targets. */
-struct target_type arm946e_target =
-{
+struct target_type arm946e_target = {
 	.name = "arm946e",
 
 	.poll = arm7_9_poll,
@@ -682,15 +740,14 @@ struct target_type arm946e_target =
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
-	//.read_memory = arm7_9_read_memory,
-	//.write_memory = arm7_9_write_memory,
+	/* .read_memory = arm7_9_read_memory, */
+	/* .write_memory = arm7_9_write_memory, */
 	.read_memory = arm946e_read_memory,
 	.write_memory = arm946e_write_memory,
 
-	.bulk_write_memory = arm7_9_bulk_write_memory,
-
 	.checksum_memory = arm_checksum_memory,
 	.blank_check_memory = arm_blank_check_memory,
 
@@ -698,8 +755,8 @@ struct target_type arm946e_target =
 
 	.add_breakpoint = arm7_9_add_breakpoint,
 	.remove_breakpoint = arm7_9_remove_breakpoint,
-	//.add_breakpoint = arm946e_add_breakpoint,
-	//.remove_breakpoint = arm946e_remove_breakpoint,
+	/* .add_breakpoint = arm946e_add_breakpoint, */
+	/* .remove_breakpoint = arm946e_remove_breakpoint, */
 
 	.add_watchpoint = arm7_9_add_watchpoint,
 	.remove_watchpoint = arm7_9_remove_watchpoint,