asm.c

motorola自己开发的针对coldfire 5272的Dbug bootloader程序

	write_data (16,opword);
}	

/****************************************************************/
void
afun2 (int ea_mask, uint16 opword)
{
	(void) ea_mask;
	/* First opword is known, Second word is immediate data */
	/* STOP #0xDATA                                         */ 

	ext_word = (uint16)get_imm_data(instruct[1],0xFFFF0000);
	if (!error)
	{
		ext_word_flag = 1;
		write_data (16,opword);
		handle_ext_words();
	}
}
/****************************************************************/
void 
afun3 (int ea_mask, uint16 opword)
{
	/* ORI, ANDI, SUBI, ADDI, EORI,*/
	/* ORI #<data>,Dn		     */

	if (size_is_long())
	{
		ext_long = get_imm_data(instruct[1],0x0);
		if (!error)
		{
			ext_long_flag = 1;
			afun5(ea_mask,opword);
		}
	}
}	
/****************************************************************/
void
afun4 (int ea_mask, uint16 opword)
{
	/* NEG,NEGX,NOT,SWAP,EXTB */
	/* Dn - source operand	     */
	/* opword: XXXXXXXXXXXXX|REG#	*/
 
	int Dn;
	(void)ea_mask;
	
	if (size_is_long())
	{
		Dn = get_data_register(instruct[1]);
		if (!reg_error)
		{
			opword = (uint16)(opword | Dn);
			write_data(16,opword);
		}
		else
			printf(ERR_DATA_REG,instruct[1]);
	}
}
/****************************************************************/
void
afun5 (int ea_mask, uint16 opword)
{
	/* Dn - destination operand     */
	/* opword: XXXXXXXXXXXXX|REG#   */

	int Dn;
	(void) ea_mask;

	Dn = get_data_register(instruct[2]);
	if (!reg_error)
	{
		opword = (uint16)(opword | Dn);
		write_data(16,opword);
		handle_ext_words();
	}
	else
		printf(ERR_DATA_REG,instruct[2]);
}
/****************************************************************/
void
afun5b (int ea_mask, uint16 opword)
{
	/* Rn - destination register    */
	/* opword: XXXXXXXXXXXXX|REG#   */

	int Rn;
	(void) ea_mask;

	if ((Rn=get_data_register(instruct[2]))==8)				/* Check is Rn IS NOT a Data register */
	{	
		if ((Rn=get_addr_register(instruct[2])) != 8)		/* Check is Rn IS an Address register */
		{
			opword = (uint16)(opword | Rn | 8 );					/* Rn is a Address register */
			write_data(16,opword);
			handle_ext_words();
		}
		else
			printf(ERR_REG,instruct[2]);					/* Rn is NOT a data or address register */
	}
	else
	{
			opword = (uint16)(opword | Rn);					/* Rn is a Data register */
			write_data(16,opword);
			handle_ext_words();
	}

}
/****************************************************************/
void
afun6 (int ea_mask, uint16 opword)
{
	/* LINK An,#<displacement>      */
	/* opword: XXXXXXXXXXXXX|REG#   */	
	/*         |  displacement  |   */

	int32 disp;

	disp = get_imm_data(instruct[2],0x0);
	if (!error)
	{
                /* 16-bit, -32768 < disp < 32767 */
		if (disp <= 32767 && disp >= -32768)
		{	
			ext_word = (uint16)disp;
			ext_word_flag = 1;
			afun7(ea_mask,opword);
		}
		else
                        printf("Error: displacement larger that 16-bits\n");
	}
}	
/****************************************************************/
void
afun7 (int ea_mask, uint16 opword)
{
	/* UNLK An 			*/
	/* An - source operand		*/
	/* opword: XXXXXXXXXXXXX|REG#	*/

	int An;
	(void) ea_mask;

	An = get_addr_register(instruct[1]);
	if (!reg_error)
	{
		opword = (uint16)(opword | An);
		write_data(16,opword);
		handle_ext_words();
	}
	else
		printf(ERR_ADDR_REG,instruct[1]);
}
/****************************************************************/
void
afun8 (int ea_mask, uint16 opword)
{
	/* PEA, JMP, JSR, Scc's */
	/* <ea> - source operand*/
	/* XXXXXXXXXX|mod|reg|  */

	uint16 ea;
	ea = get_ea(instruct[1],ea_mask,0);
	if (!error)
	{
		opword = (uint16)(opword | ea);
		write_data(16,opword);
		handle_ext_words();
	}
}
/****************************************************************/
static void
afun8b (int ea_mask, uint16 opword, int force_long)
{
	/* <ea> - source operand*/
	/* XXXXXXXXXX|mod|reg|  */

	uint16 ea;
	ea = get_ea(instruct[2],ea_mask,force_long);
	if (!error)
	{
		opword = (uint16)(opword | ea);
		write_data(16,opword);
		handle_ext_words();
	}
}
/****************************************************************/
void 
afun9 (int ea_mask, uint16 opword)
{
	/* BTST, BSET, BCHG, BCLR */

	if (instruct[1][0] == '#') 	/* BXXX #<data>,<ea> */ 
	{
		ea_mask = EA_DATA1;
		if (strcasecmp(instruct[0],"BTST") == 0)
			opword = 0x0800;
		else if (strcasecmp(instruct[0],"BCHG") == 0)
			opword = 0x0840;
		else if (strcasecmp(instruct[0],"BSET") == 0)
			opword = 0x08C0;
		else  			 /* BCLR */
			opword = 0x0880;
		
		opword2 = (uint16)get_imm_data(instruct[1],0xffffff00);	
		if (!error)		
		{
			opword2_flag = 1;	/* tell handle_ext_words() that there */
			afun8b(ea_mask,opword,0);		/*    are two opwords */
		}
	}
	else				/* BXXX Dn,<ea> */ 
	{
		if (strcasecmp(instruct[0],"BTST") == 0)
			opword = 0x0100;
		else if (strcasecmp(instruct[0],"BCHG") == 0)
			opword = 0x0140;
		else if (strcasecmp(instruct[0],"BSET") == 0)
			opword = 0x01C0;
		else  			 /* BCLR */
			opword = 0x0180;

		if (strcasecmp(instruct[0],"BTST") == 0)
			ea_mask = EA_DATA;
		else 
			ea_mask = EA_DATALT1;

		afun17(ea_mask,opword);
	}
}
/****************************************************************/
void
afun10 (int ea_mask,  uint16 opword)
{
	/* CLR, TST, WDDATA    */
	/* XXXXXXXX|SZ|MOD|REG */

	opword = (uint16)(opword & 0xff00); /* clear size bits */
	opword = (uint16)(opword | (size << 6));
	afun8(ea_mask,opword);
}
/****************************************************************/
void
afun11 (int ea_mask, uint16 opword) 
{
	/* EXT.s Dn  */
	/* XXXXXXX|OPM|XXX|REG	*/

	switch (size)  /* write OPMODE for each instruction */
	{
		case 0:
			error = TRUE;
			printf("Invalid size specified:  .W or .L only\n");
			break;
		case 1:
			opword = (uint16)(opword | 0x80);
			break;
		case 2:
			opword = (uint16)(opword | 0xC0);
			break;
	}
	if (!error)
		afun4(ea_mask,opword);
}				
/****************************************************************/
void 
afun12 (int ea_mask, uint16 opword)
{
	/* MULU, MULS   */
	/* MULx.s <ea>,Dn */
	/* .W: XXXX|REG|XXX|MOD|REG	*/
	/* .L: XXXXXXXXXX|MOD|REG	*/
	/*     X|RDI|XXXXXXXXXXXX	*/

	int Dn;
		
	switch (size)
	{
		case 0:
			error = TRUE;
			printf("Invalid size specified:  .W or .L only\n");
			break;
		case 1:
			if (strcasecmp(instruct[0],"MULS") == 0)
				opword = 0xC1C0;
			else 	/* MULU */
				opword = 0xC0C0;
			ea_mask = EA_DATA;
			afun15b(ea_mask,opword,1);
			break;
		case 2:
			opword = 0x4C00;
			ea_mask = (DRD | ARI | ARIPO | ARIPR | ARID);
			if (strcasecmp(instruct[0],"MULS") == 0)
				opword2 = 0x0800;
			Dn = get_data_register(instruct[2]);
			if (!reg_error)
			{
				opword2 = (uint16)(opword2 | (Dn << 12));
				opword2_flag = 1;
				afun8(ea_mask,opword);
			}	
	}
}
/****************************************************************/
void
afun13 (int ea_mask, uint16 opword)
{
	/* TRAP #<vector> */
	/* XXXXXXXXXXXXX|VEC|	*/
	uint16 vector;
	(void) ea_mask;

	vector = (uint16)get_imm_data(instruct[1],0xfffffff0);
	if (!error)
	{
		opword = (uint16)(opword | vector);
		write_data(16,opword);
	}
}
/****************************************************************/
void
afun14 (int ea_mask, uint16 opword)
{
	/* LEA, ADDA, SUBA,*/
	/* LEA <ea>,An		 	*/
	/* XXXX|REG|XXXXXX|MOD|REG	*/

	int An;
	
	if (size_is_long())
	{
		An = get_addr_register(instruct[2]);
		if (!reg_error)
		{
			ext_long_flag=1;
			opword = (uint16)(opword | (An << 9));
			afun8(ea_mask,opword);
		}
		else
			printf(ERR_ADDR_REG,instruct[2]);
	}
}
/****************************************************************/
void
afun15 (int ea_mask, uint16 opword)
{
	/* CMP <ea>,Dn			*/
	/* XXXX|REG|XXXXXX|MOD|REG	*/
	int Dn;
#if (defined(CPU_MCF5407))

	opword= 0xB000;
	switch (size)
	{
		case 0:
			/* byte */
			break;
		case 1:
			opword = (uint16)(opword | (1 << 6));
			break;
		case 2:
			opword = (uint16)(opword | (1 << 7));
			ext_long_flag=1;
			break;
		default:
			opword = (uint16)(opword | (1 << 7));
			ext_long_flag=1;
			break;
	}
	
	Dn = get_data_register(instruct[2]);
	if (!reg_error)
	{
		opword = (uint16)(opword | (Dn << 9));
		afun8(ea_mask,opword);
	}
	else
		printf(ERR_DATA_REG,instruct[2]);
#else
	if (size_is_long())
	{
		Dn = get_data_register(instruct[2]);
		if (!reg_error)
		{
			ext_long_flag=1;
			opword = (uint16)(opword | (Dn << 9));
			afun8(ea_mask,opword);
		}
		else
			printf(ERR_DATA_REG,instruct[2]);
	}	
#endif
}
/****************************************************************/
void
afun15b (int ea_mask, uint16 opword, int force_long)
{
	int Dn, ea;

	Dn = get_data_register(instruct[2]);
	if (!reg_error)
	{
		opword = (uint16)(opword | (Dn << 9));
		ea = get_ea(instruct[1],ea_mask,force_long);
		if (!error)
		{
			opword = (uint16)(opword | ea);
			write_data(16,opword);
			handle_ext_words();
		}
	}
	else
		printf(ERR_DATA_REG,instruct[2]);
}
/****************************************************************/
void
afun16 (int ea_mask, uint16 opword)
{
	/* ADDQ, SUBQ, #<data>,<ea> 	*/
	/* XXXX|DATa|XXX|MOD|REG    	*/
	uint16 data;
	
	if (size_is_long())
	{
		data = (uint16)get_imm_data(instruct[1],0xfffffff0);
		if (!error)
		{
			if (data == 8)
				data = 0;
			opword = (uint16)(opword | (data << 9));
			afun8b(ea_mask,opword,0);
		}
	}
}
/****************************************************************/
void
afun17 (int ea_mask, uint16 opword)
{
	/* EOR Dn,<ea> */
	/* XXXX|REG|XXXXXX|MOD|REG	*/
	int Dn;

	if (size_is_long())
	{
		Dn = get_data_register(instruct[1]);
		if (!reg_error)
		{
			opword = (uint16)(opword | (Dn << 9));
			afun8b(ea_mask,opword,0);
		}
		else
			printf(ERR_DATA_REG,instruct[2]);
	}
}
/****************************************************************/
void
afun18 (int ea_mask, uint16 opword)
{
	/* BRA, BSR, Bcc's */
	/* Bxx <label> */

	int32 disp;
	uint32 label_addr;
	(void) ea_mask;

	if (symtab_convert_string(instruct[1],(ADDRESS *)&label_addr))
	{
		disp = label_addr - (asm_pc + 2);  /* +2 from def. branch statements */
		/* 32-bit,-2147483648 < disp < 2147483647 */
		/* 16-bit,     -32768 < disp < 32767      */
		/* 8-bit,        -128 < disp < 127	      */
		if (disp <= 127 && disp >= -128)			
		{
			disp = disp & 0x00ff;
			opword = (uint16)(opword | disp);
			write_data(16,opword);
		}
		else if (((disp > 127) && (disp <= 32767)) || ((disp < -128) && (disp >= -32768)))
		{
			disp = disp & 0xffff;
			ext_word = (int16)disp;
			ext_word_flag = 1;
			write_data(16,opword);
			handle_ext_words();
		}
		#if (defined(CPU_MCF5407))
			else if(((disp > 32767) && (disp <= 2147483647)) 
				 || ((disp < -32768) && (disp >= (int32)-2147483648)))
			{
				ext_long = (int32)disp;
				ext_long_flag = 1;
				opword = (uint16)(opword | 0xFF);
				write_data(16,opword);
				handle_ext_words();
			}
			else
			{
				printf("Error: displacement larger that 32-bits\n");
				return;
			}
		#else
			else
			{
				printf("Error: displacement larger that 16-bits\n");
				return;
			}
		#endif
	
	}
	else
		printf("Not a valid label: %s\n",instruct[1]);
} 
/****************************************************************/
void
afun19 (int ea_mask, uint16 opword)
{
	/* MOVEQ */
	/* MOVEQ #<data>,Dn */

	uint16 data;
	int Dn;
	(void) ea_mask;

	if (size_is_long())
	{
		data = (uint16)get_imm_data(instruct[1],0xFFFFFF00);
		if (!error)
		{
			Dn = get_data_register(instruct[2]);
			if (!reg_error)
			{
				opword = (uint16)(opword | (Dn << 9) | data);
				write_data(16,opword); 
			}
			else
       		         	printf(ERR_DATA_REG,instruct[2]);
		}
	}
}
/****************************************************************/
void 
afun20 (int ea_mask, uint16 opword)
{
	/* AND, OR, ADD, SUB 	*/
	/* AND Dn,<ea>  	*/
	/* AND <ea>,Dn 	 	*/

	int Dn;
	/* ColdFire restricts these instructions to be size uint32 (ie: AND.L) 
	 * But, if immediate addressing is used, get_ea() may only detect
	 * an extension word.  To correct this, a variable 'force_long' is 
	 * set when calling afun#(). 'afun8b(ea_mask,opword,force_long)' 
	 */

	if (size_is_long())
	{
		if ((get_data_register(instruct[2])) != 8)	
		{
			if ((strcasecmp(instruct[0],"AND") == 0) ||
			    (strcasecmp(instruct[0], "OR") == 0))
				ea_mask = EA_DATA;
			else 
				ea_mask = EA_ALL;
			opword = (uint16)(opword | 0x80);  /* <ea>,Dn */
			afun15b(ea_mask, opword,1);
		}
		else 
		{
			if ((Dn = get_data_register(instruct[1])) != 8)
			{
				opword = (uint16)(opword | 0x180 | (Dn << 9));
				ea_mask = EA_MEMALT1;
				afun8b(ea_mask,opword,0);
			}
			else
				printf("Invalid operands: %s,%s\n",instruct[1],instruct[2]);
		}
	}
}	
/****************************************************************/
void
afun21 (int ea_mask, uint16 opword)
{
	/* WDEBUG <ea> */

	opword2 = 0x0003;
	opword2_flag = 1;
	afun8(ea_mask,opword);
} 
/****************************************************************/
void
afun22 (int ea_mask, uint16 opword)
{
	/* ASL, ASR, LSL, LSR             */
	/* ASd Dx,Dy       LSd Dx,Dy      */
	/* ASd #<data>,Dy  LSd #<data>,Dy */ 

	int count, Dy;
	(void)ea_mask;
	
	if (size_is_long())
	{
		if ((Dy = get_data_register(instruct[2])) != 8)
		{
			if ((count = get_data_register(instruct[1])) != 8)
			{	/* Dx,Dy */
				opword = (uint16)(opword | (count << 9) | 0x20 | Dy);
				write_data(16,opword); 
			}
			else 
			{ 	/* #<data>,Dy */ 
				count = (uint16)get_imm_data(instruct[1],0xffffff0);
				if (!error)
				{
					if (count == 0x8) 
						count = 0;
					opword = (uint16)(opword | (count << 9) | Dy);
					write_data(16,opword);
				} 	 
			}	
		}
		else