UBOOT命令怎么自己添加 嵌入式汇编控制cpu CACH

#include <common.h>
#include <command.h>

/*
static inline int __raw_readsb(unsigned int addr, void *data, int bytelen)
{
	__asm__ __volatile__ ("1:ld.dir8, [r0]\n"
			"sub.fr2, r2, 1\n"
			"bnz.d1b\n"
			"stb.abr8, [r1, 1]\n"
			:
			: "r" (addr), "r" (data), "r" (bytelen)
			: "r8");
	return bytelen;
}


static int disable_interrupts (void)
{
	    unsigned long old,temp;
	        __asm__ __volatile__("mrs %0, cpsr\n"     "orr %1, %0, #0x80\n"
				    "msr cpsr_c, %1" 
				        : "=r" (old), "=r" (temp)
					    :
					        : "memory");
		    return (old & 0x80) == 0;
}

*/

static void show_cache_state(void)
{
	unsigned long state;
	__asm__ volatile ("MRC p15, 0,%0, c1, c0, 0\n\r"
			: "=r" (state)
			:
			: "memory" );
	
	printf(" register SCTLR value: %lx \n", state );
	if(state & (0x01<<12)) {
		printf("Icache : ON\n");
	} else {	
		printf("Icache : OFF\n");
	}
	if(state & (0x01<<2)) {
		printf("Dcache : ON\n");
	} else {	
		printf("Icache : OFF\n");
	}
}

U_BOOT_CMD(nu_cache, 1, 0, show_cache_state,
		"show Icache and Dcache state",
		"nu_cache [args...]\n");




static void do_set_cache(unsigned int icase)
{
	unsigned long value,state;
	__asm__ volatile ("MRC p15, 0,%0, c1, c0, 0\n\r"
			: "=r" (state)
			:
			: "memory" );
	printf(" register SCTLR old value: %lx \n", state );

	switch (icase) {
		case 1:
			value = state & ~(0x01<<2);
			break;
		case 2:
			value = state | (0x01<<2);
			break;
		case 3:
			value = state & ~(0x01<<12);
			break;
		case 4:
			value = state | (0x01<<12);
			break;
		default:
			value = state;
			break;


	}
	printf(" the value will write to is: %#lx \n", value );

	__asm__ volatile ("MCR p15, 0,%0, c1, c0, 0\n\r"
			: 
			: "r" (value)
			: "memory" );
	
	__asm__ volatile ("MRC p15, 0,%0, c1, c0, 0\n\r"
			: "=r" (state)
			:
			: "memory" );
	printf(" register SCTLR new value: %lx \n", state );
}



static int set_cache(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	if(argc < 3) {
		printf("args eeror\n");
		return -1 ;
	}
	if( strcmp(argv[1],"-D")==0 ) {
		printf("set Dcache:");
		if( strcmp(argv[2],"0")==0 ) {
			do_set_cache(1);
		} else if( strcmp(argv[2],"1")==0 ) {
			do_set_cache(2);
		} else {
			printf("argv[2] must is 0 or 1 \n");
			return -1 ;
		}
		printf("\n");
	
	}
	else if( strcmp(argv[1],"-I")==0 ) {
		printf("set Icache:");
		if( strcmp(argv[2],"0")==0 ) {
			do_set_cache(3);
		} else if( strcmp(argv[2],"1")==0 ) {
			do_set_cache(4);
		} else {
			printf("argv[2] must is 0 or 1 \n");
			return -1 ;
		}
		printf("\n");
	}
	else {
		printf("argv[1] must be -D or -I\n");
		return -1;
	}
	
	return 0;
}

U_BOOT_CMD(nu_cache_set, 3, 2, set_cache,
		"set Icache and Dcache ",
		"nu_cache_set [options]\n"
		);