#include "mmu.h"

// internal function to get segment at offset
// @returns 0 on success, nonzero on error
int getSegment(mmu_t *mmu, int offset, memory_map_t *segment) {
	int i;
	for(i=0; i<mmu->length; i++) {
		*segment=mmu->segments[i];
		if(segment->pos<=offset&&segment->pos+segment->len>offset) return 0;
	}
	return 1;
}

int Mmu_checkAccess(mmu_t *mmu, int offset) {
	memory_map_t segment;
	if(getSegment(mmu, offset, &segment)) {
		return 0;
	}
	return segment.access;
}

void Mmu_readByte(mmu_t *mmu, char *byte, int offset) {
	memory_map_t segment;
	if(getSegment(mmu, offset, &segment)) {
		if(mmu->onError==MMU_ONERROR_ZERO) *byte=0;
		return;
	}
	if(segment.access&MMU_ACCESS_r) {
		if(segment.access&&MMU_ACCESS_absent) {
			segment.getter(offset-segment.pos, byte, segment.opaque);
		} else {
			*byte=segment.pointer[offset-segment.pos];
		}
	} else {
		if(mmu->onError==MMU_ONERROR_ZERO) *byte=0;
	}
}
void Mmu_writeByte(mmu_t *mmu, char byte, int offset) {
	memory_map_t segment;
	if(getSegment(mmu, offset, &segment)) {
		return;
	}
	if(segment.access&MMU_ACCESS_w) {
		if(segment.access&&MMU_ACCESS_absent) {
			segment.setter(offset-segment.pos, byte, segment.opaque);
		} else {
			segment.pointer[offset-segment.pos]=byte;
		}
	}
}

void Mmu_readHalfWord(mmu_t *mmu, half_word_t *half, int offset) {
	for(int i=0; i<2; i++) {
		Mmu_readByte(mmu, &half->bytes[i], offset+i);
	}
}
void Mmu_writeHalfWord(mmu_t *mmu, half_word_t half, int offset) {
	for(int i=0; i<2; i++) {
		Mmu_writeByte(mmu, half.bytes[i], offset+i);
	}
}

void Mmu_readWord(mmu_t *mmu, word_t *half, int offset) {
	for(int i=0; i<4; i++) {
		Mmu_readByte(mmu, &half->bytes[i], offset+i);
	}
}
void Mmu_writeWord(mmu_t *mmu, word_t half, int offset) {
	for(int i=0; i<4; i++) {
		Mmu_writeByte(mmu, half.bytes[i], offset+i);
	}
}

void Mmu_readBlock(mmu_t *mmu, void* data, int offset, int length) {
	//get a char pointer on the data
	char* ptr=(char*) data;
	
	//get the segment
	memory_map_t segment;
	if(getSegment(mmu, offset, &segment)) {
		//clear the buffer
		for(int i=0; i<length; i++) *(ptr++)=0;
	}
	
	//get the segment region
	int len=length;
	int off=offset-segment.pos;
	if(segment.len-off<length) len=segment.len-off;
	
	//check if we can optimize
	if((segment.access&MMU_ACCESS_r)&&!(segment.access&MMU_ACCESS_absent)) {
		//copy the bytes faster
		for(int i=0; i<len; i++) ptr[i]=segment.pointer[off+i];
		
		//check if we did the whole buffer
		if(len==length) return;
		
		//read the rest with this function recursively
		Mmu_readBlock(mmu, ptr+len, offset+len, length-len);
		return;
	} else if(segment.access&MMU_ACCESS_r) {
		//read byte per byte
		for(int i=0; i<len; i++) segment.getter(i+off, ptr+i, segment.opaque);
		
		//check if we did the whole buffer
		if(len==length) return;
		
		//read the rest with this function recursively
		Mmu_readBlock(mmu, ptr+len, offset+len, length-len);
		return;
	}
	
	//we have an error somewhere
	if(mmu->onError==MMU_ONERROR_ZERO) {
		//clear the buffer
		for(int i=0; i<length; i++) *(ptr++)=0;
	}
}

void Mmu_writeBlock(mmu_t *mmu, void* data, int offset, int length) {
	//get a char pointer on the data
	char* ptr=(char*) data;
	
	//get the segment
	memory_map_t segment;
	if(getSegment(mmu, offset, &segment)) return;
	
	//get the segment region
	int len=length;
	int off=offset-segment.pos;
	if(segment.len-off<length) len=segment.len-off;
	
	//check if we can optimize
	if((segment.access&MMU_ACCESS_w)&&!(segment.access&MMU_ACCESS_absent)) {
		//copy the bytes faster
		for(int i=0; i<len; i++) segment.pointer[off+i]=ptr[i];
		
		//check if we did the whole buffer
		if(len==length) return;
		
		//read the rest with this function recursively
		Mmu_writeBlock(mmu, ptr+len, offset+len, length-len);
		return;
	} else if(segment.access&MMU_ACCESS_w) {
		//read byte per byte
		for(int i=0; i<len; i++) segment.setter(i+off, ptr[i], segment.opaque);
		
		//check if we did the whole buffer
		if(len==length) return;
		
		//read the rest with this function recursively
		Mmu_writeBlock(mmu, ptr+len, offset+len, length-len);
		return;
	}
}

void initMMU(mmu_t *mmu) {
	mmu->length=0;
	mmu->onError=0;
}

int Mmu_addSegment(mmu_t *mmu, int pos, int len, char* pointer, char access, char info) {
	if(mmu->length==32||(access&MMU_ACCESS_absent)) return 0;
	if(!pointer) return 1;
	mmu->segments[mmu->length].pos=pos;
	mmu->segments[mmu->length].len=len;
	mmu->segments[mmu->length].pointer=pointer;
	mmu->segments[mmu->length].access=access;
	mmu->segments[mmu->length].info=info;
	mmu->length++;
	return 1;
}
int Mmu_addAbsentSegment(mmu_t *mmu, int pos, int len, mmu_getter_t getter, mmu_setter_t setter, void* opaque, char access, char info) {
	if(mmu->length==32) return 0;
	if((!getter)||(!setter)) return 0;
	mmu->segments[mmu->length].pos=pos;
	mmu->segments[mmu->length].len=len;
	mmu->segments[mmu->length].opaque=opaque;
	mmu->segments[mmu->length].getter=getter;
	mmu->segments[mmu->length].setter=setter;
	mmu->segments[mmu->length].access=access|MMU_ACCESS_absent;
	mmu->segments[mmu->length].info=info;
	mmu->length++;
	return 1;
}

int Mmu_removeSegment(mmu_t *mmu, int pos) {
	for(int i=0; i<mmu->length; i++) {
		if(mmu->segments[i].pos==pos) {
			if(i!=mmu->length-1) {
				mmu->segments[i]=mmu->segments[mmu->length-1];
			}
			mmu->length--;
			return 1;
		}
	}
	return 0;
}