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);	}}voidIOH_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 voidIOH_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 voidIOH_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;		}	}}voidIOH_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_tIOH_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;}voidIOH_Delete8(uint32_t address) {	IOH_Delete(address);}voidIOH_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;		}	}}voidIOH_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 * --------------------------------------- */voidIO_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;}voidIO_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;}voidIO_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;}voidIO_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_tIO_Read64(uint32_t addr) { 	IOHandler *h=IOH_Find(addr);	if(!h || !h->readproc) {		return 0;	}	return h->readproc(h->clientData,addr,8);}uint32_tIO_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_tIO_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_tIO_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 * -------------------------------------------------------------- */voidBus_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");}