bus.c

skyeye_1_2_2_Rel.rar 最新skyeye源码

	int sl_index; 
	sl_map = iohandlerFlvlMap[fl_index]; 
	if(!sl_map) {
		sl_map = iohandlerFlvlMap[fl_index] = malloc(sizeof(uint8_t*)*IOH_SLVL_SZ);
		if(!sl_map) {
			fprintf(stderr,"Out of memory in IOH_Map2LvlBlock\n");
			exit(4724);
		} else {
			memset(sl_map,0,sizeof(uint8_t*)*IOH_SLVL_SZ);
			ioh_flvl_use_count[fl_index]++;
		}
	} 
	sl_index = (addr & IOH_SLVL_MASK)>>IOH_SLVL_SHIFT;
	if(!sl_map[sl_index]) {
		sl_map[sl_index] = h;
	} else {
		fprintf(stderr,"There is already a handler for IO-Region 0x%08x\n",addr);
		exit(3246);
	}
}

void
IOH_NewRegion(uint32_t addr,uint32_t length,IOReadProc *readproc,IOWriteProc *writeproc,int flags,void *clientData) 
{
	IOHandler *h;
	int swap_endian;
	int byteorder;
	if(flags & IOH_FLG_BIG_ENDIAN) {
		byteorder = en_BIG_ENDIAN;
	} else if(flags & IOH_FLG_HOST_ENDIAN) {
		byteorder = HOST_BYTEORDER;
	} else {
		byteorder = en_LITTLE_ENDIAN;
	}
	if(byteorder != HOST_BYTEORDER) {
		swap_endian=1;
	} else {
		swap_endian=0;
	}
	while(length) {
		if(!(addr&IOH_MAP_BLOCKMASK) && (length >= IOH_MAP_BLOCKSIZE)) {
			h=allocate_iohandler(addr,readproc,writeproc,swap_endian,clientData);
			h->flags = 0;
			h->len = 4;
			IOH_MapBlock(addr,h); 
			length-=IOH_MAP_BLOCKSIZE;
			addr+=IOH_MAP_BLOCKSIZE;
		} else if(!(addr&IOH_SLVL_BLOCKMASK) && (length >= IOH_SLVL_BLOCKSIZE)) {
			h=allocate_iohandler(addr,readproc,writeproc,swap_endian,clientData);
			h->flags = 0;
			h->len = 4;
			IOH_Map2LvlBlock(addr,h); 
			length-=IOH_SLVL_BLOCKSIZE;
			addr+=IOH_SLVL_BLOCKSIZE;
		} else {
			fprintf(stderr,"io region not correctly aligned addr 0x%08x, len 0x%08x\n",addr,length);
			exit(34245);
		}
	}
}

static inline void
IOH_UnMapBlock(uint32_t addr) 
{
	int index;
	IOHandler *handler;
	index=addr>>IOH_MAP_SHIFT;
	handler=iohandlerMap[index];
	if(handler) {
		iohandlerMap[index]=NULL;
		free(handler);
	}
}

static inline void
IOH_UnMap2LvlBlock(uint32_t addr) {
	int fl_index;
	int sl_index;
	IOHandler *handler;
	IOHandler **sl_map;
	fl_index=addr>>IOH_FLVL_SHIFT;
	sl_map=iohandlerFlvlMap[fl_index];
	if(!sl_map) {
		return;
	}
	sl_index = (addr & IOH_SLVL_MASK) >> IOH_SLVL_SHIFT;
	handler = sl_map[sl_index];
	if(handler) {
		sl_map[sl_index]=NULL;
		free(handler);
		ioh_flvl_use_count[fl_index]--;
		if(ioh_flvl_use_count[fl_index]==0) {
			free(iohandlerFlvlMap[fl_index]);
			iohandlerFlvlMap[fl_index]=0;
		}
	}
}

void
IOH_DeleteRegion(uint32_t addr,uint32_t length) 
{
	//fprintf(stderr,"Delete Region %08x\n",addr);
	while(length) {
		if(!(addr&IOH_MAP_BLOCKMASK) && (length >= IOH_MAP_BLOCKSIZE)) {
			IOH_UnMapBlock(addr);
			length-=IOH_MAP_BLOCKSIZE;
			addr+=IOH_MAP_BLOCKSIZE;
		} else if(!(addr&IOH_SLVL_BLOCKMASK) && (length >= IOH_SLVL_BLOCKSIZE)) {
			IOH_UnMap2LvlBlock(addr);
			length-=IOH_SLVL_BLOCKSIZE;
			addr+=IOH_SLVL_BLOCKSIZE;

		} else {
			fprintf(stderr,"IOH_DeleteRegion: only IO regions with alignment to 0x%08x allowed\n",IOH_SLVL_BLOCKSIZE);
			return;
		}
	}

}

uint32_t
IOH_Delete(uint32_t address) 
{
	IOHandler *next,*cursor,*prev=NULL;
	uint32_t hash=IOH_HASH(address);
	uint32_t flags; 
	for(cursor=iohandlerHash[hash];cursor;prev=cursor,cursor=next) {
		next=cursor->next;
		if(cursor->cpu_addr==address) {
			if(prev) {
				prev->next=cursor->next;
			} else {
				iohandlerHash[hash]=cursor->next;
			}
			flags = cursor->flags;
			free(cursor);
			return flags;
		}
	}
	return 0;
}
void
IOH_Delete8(uint32_t address) 
{
	IOH_Delete(address);
}
void
IOH_Delete16(uint32_t address) 
{
	int i;
	uint32_t flags;
	for(i=0;i<2;i++) {
		flags = IOH_Delete(address); 
		if(!(flags & IOH_FLG_PA_CBSE)) {
			break;
		}
	}
}
void
IOH_Delete32(uint32_t address) 
{
	int i;
	uint32_t flags;
	for(i=0;i<4;i++) {
		flags = IOH_Delete(address); 
		if(!(flags & IOH_FLG_PA_CBSE)) {
			break;
		}
	}
}

/*
 * ---------------------------------------
 * Warning: 64 Bit write does not work
 * because writeproc is still 32Bit
 * ---------------------------------------
 */
void
IO_Write64(uint64_t value,uint32_t addr) 
{
	IOHandler *h=IOH_Find(addr);
	if(!h || !h->writeproc) {
		fprintf(stderr,"Write: No Handler for %08x\n",addr);
		return;
	}
	h->writeproc(h->clientData,value,addr,4);
	return;
}
void
IO_Write32(uint32_t value,uint32_t addr) { 
	IOHandler *h=IOH_Find(addr);
	if(!h || !h->writeproc) {
		fprintf(stderr,"Write: No Handler for %08x\n",addr);
		return;
	}
	if(!h->swap_endian) {
		h->writeproc(h->clientData,value,addr,4);
	} else {
		h->writeproc(h->clientData,swap32(value),addr,4);
	}
	return;
}
void
IO_Write16(uint16_t value,uint32_t addr) 
{ 
	IOHandler *h=IOH_Find(addr);
	if(!h || !h->writeproc) {
		//fprintf(stderr,"No handler for %08x\n",addr);
		return;
	}
	if(!h->swap_endian) {
		h->writeproc(h->clientData,value,addr,2);
	} else {
		h->writeproc(h->clientData,swap16(value),addr,2);
	}
	return;
}
void
IO_Write8(uint8_t value,uint32_t addr) 
{
	IOHandler *h=IOH_Find(addr);
	if(!h || !h->writeproc) {
		fprintf(stderr,"No iohandler for %08x\n",addr);
		return;
	}
	h->writeproc(h->clientData,value,addr,1);
	return;
}

/*
 * ---------------------------------------------------
 * Warning: 64 Bit read does not work, because
 * readproc returns only 32 Bit
 * ---------------------------------------------------
 */
uint64_t
IO_Read64(uint32_t addr) 
{ 
	IOHandler *h=IOH_Find(addr);
	if(!h || !h->readproc) {
		return 0;
	}
	return h->readproc(h->clientData,addr,8);
}

uint32_t
IO_Read32(uint32_t addr) 
{ 
	IOHandler *h;
	uint32_t value;
	h=IOH_Find(addr);
	if(!h || !h->readproc) {
		return 0;
	}
	if(likely(h->len == 4)) {
		if(!h->swap_endian) {
			return h->readproc(h->clientData,addr,4);
		} else {
			return swap32(h->readproc(h->clientData,addr,4));
		}
	} else if(h->len == 2) {
		value = h->readproc(h->clientData,addr,4);	
		if(h->flags & IOH_FLG_OSZR_NEXT)  {
			h=IOH_Find(addr+2);
			if(h && h->readproc) {
				value |= h->readproc(h->clientData,addr+2,4) << 16;	
			}
		}
		if(h->swap_endian) {
			value = swap32(value); 
		}
	} else if(h->len == 1) {
		value = h->readproc(h->clientData,addr,4);	
		if(h->flags & IOH_FLG_OSZR_NEXT)  {
			int i;
			for(i=1;i<4;i++) {
				h=IOH_Find(addr+i);
				if(h && h->readproc) {
					value |= h->readproc(h->clientData,addr+i,4) << (i<<3);	
				}
			}
		}
		if(h->swap_endian) {
			value = swap32(value); 
		}
	} else {
		fprintf(stderr,"Illegal handler len %d for addr 0x%08x\n",h->len,addr);
		return 0;
	}
	return value;
}

uint16_t
IO_Read16(uint32_t addr)
{ 
	IOHandler *h=IOH_Find(addr);
	uint32_t value;
	if(!h || !h->readproc) {
		return 0;
	}
	value =  h->readproc(h->clientData,addr,2);
	if(likely(h->len == 2)) {
		if(h->swap_endian) {
			value = swap16(value);
		}
	} else if(h->len == 1) {
		if(h->flags & IOH_FLG_OSZR_NEXT) 
		{
			h=IOH_Find(addr+1);
			if(h && h->readproc) {
				value = value | h->readproc(h->clientData,addr+1,2)<<8;
			}
			if(h->swap_endian) {
				value = swap16(value);
			}
		}
		return value;
 	} else if(h->len == 4) {
		if(h->swap_endian) {
			value = swap32(value);
		}
		if(h->flags & IOH_FLG_PRD_RARP) {
			value = value >> ((addr & 3) << 3);
			return value;
		}	
	} else {
		fprintf(stderr,"Illegal handler len %d for addr 0x%08x\n",h->len,addr);
	}
	return value;
}

uint8_t
IO_Read8(uint32_t addr) 
{
	IOHandler *h=IOH_Find(addr);
	uint32_t value;
	if(!h || !h->readproc) {
		return 0;
	}
	value = h->readproc(h->clientData,addr,1);
	if(likely(h->len == 1)) {
		return value;	
	} else if(h->len == 2) {
		if(h->swap_endian) {
			value = swap16(value);
		}
		if(h->flags & IOH_FLG_PRD_RARP) {
			value = value >> ((addr & 3) << 3);
			return value;
		}	
 	} else if(h->len == 4) {
		if(h->swap_endian) {
			value = swap32(value);
		}
		if(h->flags & IOH_FLG_PRD_RARP) {
			value = value >> ((addr & 3) << 3);
			return value;
		}
	}
	return h->readproc(h->clientData,addr,1);
}

static void *
ckalloc(size_t size) 
{
	void *mem=malloc(size);
	if(!mem) {
		fprintf(stderr,"Out of memory\n");
		exit(423);
	}
	memset(mem,0,size);
	return mem;
}

static struct Bus mainBus = {
	.read32 = Bus_Read32,
        .read16 = Bus_Read16,
       	.read8 = Bus_Read8,
        .readblock = Bus_Read,
	.write32 = Bus_Write32,
	.write16 = Bus_Write16,
	.write8 = Bus_Write8,
	.writeblock = Bus_Write
};
/*
 * --------------------------------------------------------------
 * Create and Initialize memory and IO-Handler maps and Hashes
 * --------------------------------------------------------------
 */

void
Bus_Init(InvalidateCallback *invalidate,uint32_t min_memblocksize) 
{
	int i;
	InvalidateProc = invalidate;

	twoLevelMMap.frst_lvl_sz = (4096);
	twoLevelMMap.frst_lvl_mask =  (0xfff00000);
	twoLevelMMap.frst_lvl_shift =  (20);
	for(i=0;i<twoLevelMMap.frst_lvl_shift;i++) {
		if((min_memblocksize>>i)==1) {
			break;
		}
	}
	twoLevelMMap.scnd_lvl_shift =  i;
	twoLevelMMap.scnd_lvl_blocksize = min_memblocksize;
	twoLevelMMap.scnd_lvl_blockmask = min_memblocksize-1;
	twoLevelMMap.scnd_lvl_mask =  (~0 << i) & ~twoLevelMMap.frst_lvl_mask; 
	twoLevelMMap.scnd_lvl_sz = (1<<(twoLevelMMap.frst_lvl_shift-twoLevelMMap.scnd_lvl_shift));

	iohandlerHash = ckalloc(sizeof(IOHandler*)*IOH_HASH_SIZE);
	mem_map_read = ckalloc(sizeof(uint8_t*)*MEM_MAP_ENTRIES);
	mem_map_write = ckalloc(sizeof(uint8_t*)*MEM_MAP_ENTRIES);

	twoLevelMMap.flvlmap_read = ckalloc(sizeof(uint8_t*)*twoLevelMMap.frst_lvl_sz);
	twoLevelMMap.flvlmap_write = ckalloc(sizeof(uint8_t*)*twoLevelMMap.frst_lvl_sz);
	twoLevelMMap.flvl_map_read_use_count = ckalloc(sizeof(*twoLevelMMap.flvl_map_read_use_count)*twoLevelMMap.frst_lvl_sz);
	twoLevelMMap.flvl_map_write_use_count = ckalloc(sizeof(*twoLevelMMap.flvl_map_read_use_count)*twoLevelMMap.frst_lvl_sz);

	iohandlerMap=ckalloc(sizeof(IOHandler *)*IOH_MAP_ENTRIES);
	iohandlerFlvlMap=ckalloc(sizeof(IOHandler **)*IOH_FLVL_SZ);
	MainBus = &mainBus; 
	fprintf(stderr,"MemMap and IO-Handler Hash initialized\n");
}