/*
This is DSP firmware load program, use age"./dsploader firmwarename"
It support both elf and coff format dsp firmare.
Due to hardware limited DSP entry point should be align with 0x200
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <fcntl.h>
#include <ctype.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
 #include "dsploader.h"
#define FATAL do { fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", \
  __LINE__, __FILE__, errno, strerror(errno)); exit(1); } while(0)
int fd;

#define MAP_SIZE 4096UL
#define MAP_MASK (MAP_SIZE - 1)


off_t target;

uint32_t coff_load(uint32_t image_source, int type)
{
	unsigned int i,length;
	unsigned int entry_point;
	unsigned int section_size, load_addr, run_addr;
	unsigned int offset,dst;
	void* mmap_addr;
	
	//Structures to store the file information
	TCoffHeader *tCoffHeader;
	TCoffOptionalHeader *tCoffOptionalHeader;
	TSectionHeader *tSectionHeader;
	printf("sizeof(TCoffHeader) %d,sizeof(TCoffOptionalHeader) %d,sizeof(TSectionHeader) %d\n",
		   sizeof(TCoffHeader),sizeof(TCoffOptionalHeader),sizeof(TSectionHeader));
	//memset(&tCoffHeader,0,HEADER_SIZE);
	//memset(&tCoffOptionalHeader,0,OPTIONAL_HEADER_SIZE);
	//memset(&tSectionHeader,0,SECTION_HEADER_SIZE);

	//Parse .out file, get statistics information
	tCoffHeader = (TCoffHeader *)(image_source);//Get file header, length HEADER_SIZE
	if(tCoffHeader->uiOptionalHeaderBytes)   //Have a optional header
		{
			//Get file optional header, length tCoffHeader->uiOptionalHeaderBytes
			tCoffOptionalHeader = (TCoffOptionalHeader *)(image_source + sizeof(TCoffHeader));
			entry_point = tCoffOptionalHeader->uiEntryPoint;
		}
	//printf("coff entry_point 0x%x,sec num %d\n",entry_point,tCoffHeader->usSectionNumber);	
	//Load sections
	for(i=0; i<tCoffHeader->usSectionNumber; i++)
	        {
					//Move the file pointer to the new section headers.
					//fseek(DSPOutFile,HEADER_SIZE+tCoffHeader.uiOptionalHeaderBytes+i*SECTION_HEADER_SIZE,0);

					//Get section header
					//fread((char *)&tSectionHeader,1,SECTION_HEADER_SIZE,DSPOutFile);
			//Get secrion header, length SECTION_HEADER_SIZE
			tSectionHeader = (TSectionHeader *)(image_source+HEADER_SIZE+tCoffHeader->uiOptionalHeaderBytes+i*SECTION_HEADER_SIZE);
	                if(tSectionHeader->uiSectionSize==0)
	                    continue;
	                if((tSectionHeader->uiFlags&(SECTION_TYPE_DSECT|SECTION_TYPE_NOLOAD|SECTION_TYPE_COPY)))
	                    continue;

	                //Get section information
               		section_size = tSectionHeader->uiSectionSize; //Section size
               		load_addr = tSectionHeader->uiVirtalAddress;  //Load address
               		run_addr = tSectionHeader->uiPysicalAddress;  //Run address

               		//Move the file pointer to the raw section data.
               		//fseek(DSPOutFile,tSectionHeader.uiRawDataPointer,0);
               		//Load section data
               		//fread((char *)load_addr,1,section_size,DSPOutFile);

			if (load_addr < 0x40800000){	//dsp1 use 0x800000, dsp2 use 0x800000
				if (type == 2)	// dsp2 core  L3 0x41000000	
					dst += 0x40800000;
				if (type == 1)	// dsp1 core   L3 0x40800000
					dst += 0x40000000;
			}	
			target = dst;
			//memcpy(dst, tSectionHeader->uiRawDataPointer + image_source, section_size );
			length = target%MAP_SIZE+section_size+MAP_SIZE-(target%MAP_SIZE+section_size)%MAP_SIZE;
			//printf("origin loadaddr 0x%x,map dst 0x%x, length 0x%x,mapsize 0x%x\n",
			//load_addr,dst,section_size,length);

			mmap_addr = mmap(0, length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
			if(mmap_addr == (void *) -1) FATAL;
			//printf("Memory mapped at address %p.\n", mmap_addr); 
			//printf("dst 0x%x,src 0x%x,size 0x%x\n",(void *)mmap_addr + (target & MAP_MASK),tSectionHeader->uiRawDataPointer + image_source, section_size);
			memcpy((void *)mmap_addr + (target & MAP_MASK),tSectionHeader->uiRawDataPointer + image_source, section_size);
			//printf("i=%d\n",i);
			munmap(mmap_addr, length);		
			//printf("2\n");
	        }

	return entry_point;
}

uint32_t elf_load(uint32_t image_source, int type)
{
	struct Elf32_Ehdr *ehdr;
	struct Elf32_Phdr *phdr;
	uint32_t dst;
	int i,length;
	void* mmap_addr;
	
	ehdr = (struct Elf32_Ehdr *)image_source;

	if (ehdr->e_ident[0]!=0x7f ||
		ehdr->e_ident[1]!='E' ||
		ehdr->e_ident[2]!='L' ||
		ehdr->e_ident[3]!='F'){
		printf("it's not elf format\n");
		return 0;
	}
#if 1	
	printf("*************Ehdr information start**************\n");
	printf("type(1-relocatable 2-executable 3-shared 4-core)=\t%d\n",ehdr->e_type);
	printf("machine(arm-40,386-3,c6000-140)=\t%d\n",ehdr->e_machine);
	printf("version(0-none,1-current)=\t%d\n",ehdr->e_version);
	printf("entry point=\t0x%x\n",ehdr->e_entry);
	printf("program header offset =\t0x%x\n",ehdr->e_phoff);
	printf("section header offset=\t0x%x\n",ehdr->e_shoff);
	printf("cpu flags=\t%d\n",ehdr->e_flags);
	printf("elf header size=\t%d\n",ehdr->e_ehsize);
	printf("program header entsize=\t%d\n",ehdr->e_phentsize);
	printf("program header num=\t%d\n",ehdr->e_phnum);
	printf("section header entsize=\t%d\n",ehdr->e_shentsize);
	printf("section header num=\t%d\n",ehdr->e_shnum);
	printf("section header str table index=%d\n",ehdr->e_shstrndx);
	printf("*************Ehdr information end**************\n\n");
#endif	
	phdr = (struct Elf32_Phdr*)(malloc(ehdr->e_phnum * ehdr->e_phentsize));
	memcpy((void *)phdr,ehdr->e_phoff + (void *)image_source,ehdr->e_phentsize*ehdr->e_phnum);
#if 1
	printf("************Phdr information start**************\n");
	printf("p_type=\t%d\n",phdr->p_type);
	printf("p_offset=\t%d\n",phdr->p_offset);	
	printf("p_vaddr=\t%d\n",phdr->p_vaddr);
	printf("p_paddr=\t%d\n",phdr->p_paddr);
	printf("p_filesz=\t%d\n",phdr->p_filesz);
	printf("p_memsz=\t%d\n",phdr->p_memsz);
	printf("p_flags=\t%d\n",phdr->p_flags);
	printf("p_align=\t%d\n",phdr->p_align);
	printf("*************Phdr information end**************\n\n");
#endif

	printf("Entry point = 0x%x ph_offset=%x phnum=%d phdr=%x\n",ehdr->e_entry, ehdr->e_phoff, ehdr->e_phnum,phdr);
	for (i=0;i<ehdr->e_phnum;i++){
printf("entering for loop.\n");
		if (phdr->p_filesz != 0){
printf("entering if loop.\n");
			dst = phdr->p_paddr;
			if (phdr->p_paddr < 0x40800000){	//dsp1 use 0x800000, dsp2 use 0x800000
				if (type == 2)	// dsp2 core  L3 0x41000000	
					dst += 0x40800000;
				if (type == 1)	// dsp1 core   L3 0x40800000
					dst += 0x40000000;
			}	
			printf("dstaddr=0x%x srcaddr=0x%x size=%d(0x%x)\n",
				dst,phdr->p_offset + image_source,phdr->p_filesz,phdr->p_filesz);
				
			target = dst;
			length = target%MAP_SIZE+phdr->p_filesz+MAP_SIZE - (target%MAP_SIZE+phdr->p_filesz)%MAP_SIZE;
			mmap_addr = mmap(0, length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
			if(mmap_addr == (void *) -1) FATAL;
			printf("Memory mapped at address %p.\n", mmap_addr); 
			
			memcpy((void *)mmap_addr + (target & MAP_MASK),phdr->p_offset + (void *)image_source, phdr->p_filesz);
			munmap(mmap_addr, length);
		}
		phdr ++;
	}
	printf("Load finished\n");
	return (ehdr->e_entry);
}
#define MAILBOX_IOC_MAGIC  'k'
#define MAILBOX_DSP1_START    _IOWR(MAILBOX_IOC_MAGIC,0,int)
#define MAILBOX_DSP2_START    _IOWR(MAILBOX_IOC_MAGIC, 3,int)
void start_dsp(int entry, int type)
{
	int fd_dsp,ret,argv;
	argv = entry;
	if((fd_dsp = open("/dev/dsp_mailbox", O_RDWR | O_SYNC)) == -1) FATAL;
	if(type == 1)
	{
		ret = ioctl(fd_dsp,MAILBOX_DSP1_START, &argv);
	}else if(type == 2)
	{
		ret = ioctl(fd_dsp,MAILBOX_DSP2_START, &argv);
	}
	if(ret == -1) FATAL;
	close(fd_dsp);
}

void dsp_runelf(unsigned int file_buf, int type)
{
	unsigned int entry = 0;
	entry = elf_load(file_buf, type);
	if (entry != 0){
		start_dsp(entry, type);
		printf("elf load success,entry point 0x%x\n",entry);
	}else{
		entry = coff_load(file_buf,type);
		if (entry != 0){
			start_dsp(entry,type);
			printf("coff load success,entry point 0x%x\n",entry);
		}
	}
}

/*END OF GEMSS PRCM Definition*/ 
int main(int argc, char **argv) {
	unsigned long read_result, writeval;
	unsigned char* file_buf = NULL;
	int devicetype = 0;
	struct stat statbuff; 

	int access_type = 'w';
	int size;
	if(argc < 3) {
		fprintf(stderr, "\nUsage:\t%s filename\n",argv[0]);
		exit(1);
	}
	if(strcmp(argv[2],"dsp1") == 0) 
	{
		devicetype = 1;
	}else if(strcmp(argv[2],"dsp2") == 0)
	{
		devicetype = 2;
	}else{
		exit(1);
	}
    if(stat(argv[1], &statbuff) < 0) FATAL;
	printf("filesize=%d\n",statbuff.st_size);
	file_buf = malloc(statbuff.st_size);
	if(file_buf==NULL)  FATAL;
	
    if((fd = open(argv[1], O_RDONLY)) == -1) FATAL;
	size = read(fd,file_buf,statbuff.st_size);
	if(size < statbuff.st_size)
	{
		printf("read from file %d, filesize %d\n",size,statbuff.st_size);
		exit(1);
	}
	close(fd);
	
    if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1) FATAL;
    printf("/dev/mem opened.\n"); 
    fflush(stdout);
	dsp_runelf(file_buf,devicetype);

	if(file_buf) free(file_buf);
    close(fd);
    return 0;
}

