align.c

32cpu的代码

#include <stdio.h>

long load_align(unsigned long virtual_addr, long data_in, unsigned data_width){
	long data_load_align_out;
        	        		switch(data_width){
        			case 1 : if((virtual_addr&(0x00000003))==0){
        				data_load_align_out=(((data_in>>7)&(0x00000001)) == 1)?  (data_in|(0xffffff00)):(data_in&(0x000000ff));
        				}else if((virtual_addr&(0x00000003))==1){
        				data_load_align_out=(((data_in>>15)&(0x00000001)) == 1)?  ((data_in>>8)|(0xffffff00)):((data_in>>8)&(0x000000ff));
        				}else if((virtual_addr&(0x00000003))==2){
        				data_load_align_out=(((data_in>>23)&(0x00000001)) == 1)?  ((data_in>>16)|(0xffffff00)):((data_in>>16)&(0x000000ff));
        				}else if((virtual_addr&(0x00000003))==3){
        				data_load_align_out=(((data_in>>31)&(0x00000001)) == 1)?  ((data_in>>24)|(0xffffff00)):((data_in>>24)&(0x000000ff));	
        				}else{
        				  printf("byte select error when loading data!width=1\n");
        				}
        			       break;
        			case 2 : if((virtual_addr&(0x00000003))==0){
        				data_load_align_out=(((data_in>>15)&(0x00000001)) == 1)?  ((data_in)|(0xffff0000)):((data_in)&(0x0000ffff));
        				}else if((virtual_addr&(0x00000003))==2){
        				data_load_align_out=(((data_in>>31)&(0x00000001)) == 1)?  ((data_in>>16)|(0xffff0000)):((data_in>>16)&(0x0000ffff));
        				}else{
        				  printf("byte select error when loading data!width=2\n");
        				}
        			       break;
        			case 4 :data_load_align_out=data_in;
        			       break;
        			default:printf("data width error when loading data!\n");
        		}
        		return(data_load_align_out);	
}


long store_align(unsigned long virtual_addr,long data_store,long data_old,unsigned data_width){
	long data_store_align_out;
	
        		switch(data_width){
        			case 1 : if((virtual_addr&(0x00000003))==0){
        				data_store_align_out=(data_old&(0xffffff00))|(data_store&(0x000000ff));
        				}else if((virtual_addr&(0x00000003))==1){
        				data_store_align_out=(data_old&(0xffff00ff))|((data_store&(0x000000ff))<<8);
        				}else if((virtual_addr&(0x00000003))==2){
        				data_store_align_out=(data_old&(0xff00ffff))|((data_store&(0x000000ff))<<16);
        				}else if((virtual_addr&(0x00000003))==3){
        				data_store_align_out=(data_old&(0x00ffffff))|((data_store&(0x000000ff))<<24);	
        				}else{
        				  printf("byte select error when storing data!width=1\n");
        				}
        			       break;
        			case 2 : if((virtual_addr&(0x00000003))==0){
        				data_store_align_out=(data_old&(0xffff0000))|(data_store&(0x0000ffff));
        				}else if((virtual_addr&(0x00000003))==2){
        				data_store_align_out=(data_old&(0x0000ffff))|((data_store&(0xffff0000))<<16);
        				}else{
        				  printf("byte select error when storing data!width=2\n");
        				}
        			       break;
        			case 4 :data_store_align_out=data_store;
        			       break;
        			default:printf("data width errorwhen storing data!\n");
                        }
	     return(data_store_align_out);
	}