openamp-loader.c

#include "openamp/remoteproc_loader.h"
#include <openamp/remoteproc.h>
#include <openamp/elf_loader.h>
#include <metal/alloc.h>
#include <metal/utilities.h> /* metal_container_of */

int image_store_open(void *store, const char *path, const void **img_data);
void image_store_close(void *store);
int image_store_load(void *store, size_t offset, size_t size,
        const void **data,
        metal_phys_addr_t pa,
        struct metal_io_region *io, char is_blocking);


struct image_store_ops store_ops = {
	.open = image_store_open,
	.close = image_store_close,
	.load = image_store_load
};


static struct remoteproc *rproc_init(struct remoteproc *rproc,
				 struct remoteproc_ops *ops, void *arg);
static void rproc_remove(struct remoteproc *rproc);
/* Start cpu via writing to gpio pin via devmem ?*/
static int rproc_start(struct remoteproc *rproc);
static int rproc_stop(struct remoteproc *rproc);
static int rproc_shutdown(struct remoteproc *rproc);
static void *rproc_mmap(struct remoteproc *rproc,
		    metal_phys_addr_t *pa, metal_phys_addr_t *da,
		    size_t size, unsigned int attribute,
		    struct metal_io_region **io);

struct rproc_priv {
    struct remoteproc *rproc;
    int cpu_id;
};

struct remoteproc_ops rproc_ops = {
    .init = rproc_init,
    .remove = rproc_remove,
    .start = rproc_start,
    .stop = rproc_stop,
    .shutdown = rproc_shutdown,
    .mmap = rproc_mmap,
};


int load_exectuable_noblock(struct remoteproc *rproc,
			     struct image_store_ops *store_ops, void *store,
			     const char *img_path);

struct remoteproc rproc;

/* TODO How to make this generic */
struct mem_file {
	/* Address to read raw data into, Should be buffer address?*/
	const void *base;
};

static struct mem_file image = {
	.base = (void *)(0x00000000),
};

int main(int argc, char *argv[]){
	/* Setup libmetal */
	struct metal_init_params init_param = METAL_INIT_DEFAULTS;
	
	int ret = metal_init(&init_param);
	if (ret){
		printf("Failed to initialize libmetal\n");
	}

	metal_list_init(&rproc.mems);
	const char * elf_filename = argv[1];
	image.base = 0x0;
	    /* Not sure how much of remote proc is needed */
	rproc.state = RPROC_READY;
	rproc.loader = &elf_ops;
	rproc.ops = &rproc_ops;
	
    load_exectuable_noblock( &rproc, &store_ops, &image, elf_filename);

}

FILE * store_fid = 0x0;
#define STORE_BUFFER_SIZE (1024*2)
char store_buffer[STORE_BUFFER_SIZE];



int image_store_open(void *store, const char *path, const void **img_data){
    /* Probably a dev mem open, an fopen */
	store_fid = fopen(path,"rb");
	if(store_fid == 0)
		return -1;
	int bytes_read = fread(store_buffer, 1,sizeof(store_buffer),store_fid);
	*img_data = store_buffer;
	printf("loader_open %s\n", path);
    return bytes_read;
}

void image_store_close(void *store){
	/* FID is already closed */
	if(store_fid == 0)
		return;

	int status = fclose(store_fid);
	if(status != 0)
		return;
	store_fid = 0x0;

    return;
}

int image_store_load(void *store, size_t offset, size_t size,
        const void **data,
        metal_phys_addr_t pa,
        struct metal_io_region *io, char is_blocking){

	fseek(store_fid,offset,SEEK_SET);
	printf("fread  %d bytes %x\n", size, pa);
	int bytes_read = 0;
	if(pa != -1){
		bytes_read  = fread(pa, 1, size,store_fid);
		printf("case pa=-1: bytes_read = %d\n",bytes_read);
	}else{
		if(size > sizeof(store_buffer))
			size = sizeof(store_buffer);
		bytes_read = fread(store_buffer, 1, size,store_fid);

	}
	printf("read done %d bytes\n", bytes_read);

    /* Probably a dev mem write use printfs to figure out what its doing? */
    return bytes_read ;

}

int cpuid = 1;

int load_exectuable_noblock(struct remoteproc *rproc,
			     struct image_store_ops *store_ops, void *store,
			     const char *img_path)
{
	int ret;
	const void *img_data;
	void *img_info = NULL;
	metal_phys_addr_t pa;
	struct metal_io_region *io;
	size_t offset, noffset;
	size_t len, nlen, nmlen;
	unsigned char padding;

	if (rproc == NULL)
		return -EINVAL;
	/* Configure remoteproc to get ready to load executable */
	remoteproc_config(rproc, NULL);
	remoteproc_init(rproc, &rproc_ops, &cpuid);
	/* Load remoteproc executable */
	printf("Start to load executable with remoteproc_load() \r\n");
	ret = store_ops->open(store, img_path, &img_data);
	if (ret <= 0)
		return -EINVAL;
	offset = 0;
	len = (size_t)ret;
	do {
		nlen = 0;
		pa = METAL_BAD_PHYS;
		io = NULL;
		nmlen = 0;
		printf("%s, loading 0x%lx,0x%lx\r\n",
			 __func__, offset, len);
		ret = remoteproc_load_noblock(rproc, img_data, offset, len,
					      &img_info, &pa, &io, &noffset,
					      &nlen, &nmlen, &padding);
		if (ret) {
			/* TODO CERROR */
			printf("failed to load executable, 0x%lx,0x%lx\r\n",
				offset, len);
			return ret;
		}
		if (nlen == 0)
			break;
		offset = noffset;
		len = nlen;
		ret = store_ops->load(store, noffset, nlen, &img_data, pa,
				      io, 1);
		if (ret != (int)nlen) {
			/* TODO CERROR */
			printf("failed to load data to memory, 0x%lx,0x%lx\r\n",
				noffset, nlen);
			return ret;
		}
		if (nmlen > nlen && io != NULL) {
			/* pad the rest of the memory with 0 */
			size_t tmpoffset;

			tmpoffset = metal_io_phys_to_offset(io, pa + nlen);
			metal_io_block_set(io, tmpoffset, padding,
					   (nmlen - nlen));

		}
	} while(1);

	/* Start the processor */
	printf("Loaded?\n");

	return 0;
}


/* Remote proc calls */
static struct remoteproc *rproc_init(struct remoteproc *rproc,
				 struct remoteproc_ops *ops, void *arg)
{
	struct rproc_priv *priv;
	unsigned int cpu_id = *((unsigned int *)arg);

	if(cpu_id > 1){
		printf("%s: invalid node id: %d\n\r", __func__, cpu_id);
		return NULL;
	}

	printf("%s: node id: %d\n\r", __func__, cpu_id);
	priv = metal_allocate_memory(sizeof(*priv));
	if (!priv)
		return NULL;
	memset(priv, 0, sizeof(*priv));
	priv->rproc = rproc;
	priv->cpu_id = cpu_id;
	priv->rproc->ops = ops;
	metal_list_init(&priv->rproc->mems);
	priv->rproc->priv = priv;
	rproc->state = RPROC_READY;
	return priv->rproc;
}

static void rproc_remove(struct remoteproc *rproc)
{
	struct rproc_priv *priv;

	priv = (struct rproc_priv *)rproc->priv;
	metal_free_memory(priv);
}

/* TODO start processor by writing to gpio pin */
static int rproc_start(struct remoteproc *rproc)
{
	struct rproc_priv *priv;
	int ret;

	priv = rproc->priv;
	if(priv->cpu_id == 1){
		/* TODO Toggle GPIO pin for cpu1 */
	}
	else{
		return -1;
	}
	return 0;
}
/* TODO stop processor by writing to gpio pin */
static int rproc_stop(struct remoteproc *rproc)
{
	/* It is lacking a stop operation in the libPM */
	(void)rproc;
	return 0;
}

static int rproc_shutdown(struct remoteproc *rproc)
{
	struct rproc_priv *priv;
	int ret;
	struct remoteproc_mem *mem;
	struct metal_list *node;

	priv = rproc->priv;
	/* Delete all the registered remoteproc memories */
	metal_list_for_each(&rproc->mems, node) {
		struct metal_list *tmpnode;
		metal_phys_addr_t pa, pa_end;

		mem = metal_container_of(node, struct remoteproc_mem, node);
		tmpnode = node;
		/* Release TCM resource */
		pa = mem->pa;
		pa_end = metal_io_phys(mem->io, metal_io_region_size(mem->io));
		node = tmpnode->prev;
		metal_list_del(tmpnode);
		metal_free_memory(mem->io);
		metal_free_memory(mem);
		/* call xilpm release node for relevant memory */
	}

	return 0;
}
typedef long long int u64;
/*!
* @pa: physical memory
* @da: device memory
*/
static void *rproc_mmap(struct remoteproc *rproc,
		    metal_phys_addr_t *pa, metal_phys_addr_t *da,
		    size_t size, unsigned int attribute,
		    struct metal_io_region **io)
{
	struct remoteproc_mem *mem;
	metal_phys_addr_t lpa, lda, lda_end;
	metal_phys_addr_t page_size, page_boundary, page_offset;



	if ((!da || !pa) || (*da == METAL_BAD_PHYS && *pa == METAL_BAD_PHYS))
		return NULL;

	printf("%s: pa=0x%lx, da=0x%lx, size=0x%lx, atrribute=0x%x\n\r",
		__func__, *pa, *da, size, attribute);
	lda = *da;
	lpa = *pa;
	// if (!attribute)
	// 	attribute = NORM_SHARED_NCACHE | PRIV_RW_USER_RW;

	page_size = sysconf(_SC_PAGE_SIZE);
	page_offset = lda%page_size;
	if(page_offset !=0) {
		printf("Segment not alligned on a page boundary page_offset=%d\n",page_offset );

	}

	lda_end = lda + size + page_offset;
	void *virtual_address;
#if 1 /* Debug on Linux using files for testing. */
	char  fname[512];
	sprintf(fname, "_%08X.bin", lda);
	FILE * fid = fopen(fname,"ab+");
	int fd = fileno(fid);
	int status = ftruncate(fd,  size + page_offset); /* Ensure file size is desired length */
	printf("truncate Status is %d\n", status);
	virtual_address = (uintptr_t)mmap(NULL, size+page_offset, PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0x0);
#else /* Use devmem, works on microblaze */
	int fd = open("/dev/mem", O_RDWR | O_SYNC); /*TOOD USE THIS METHOD */

	if(fd == -1)
	{
		printf("devmem open failed\n");
		return NULL;
	}
	virtual_address = (uintptr_t)mmap(NULL, size+page_offset, PROT_READ|PROT_WRITE, MAP_SHARED, fd, lda-page_offset);
#endif
	virtual_address += page_offset;
	
	printf("Virt address %x\n", virtual_address);
	if(virtual_address == -1){
		printf("Bad virtual address\n");
		return -1;
	}
	lpa = virtual_address;

	mem = metal_allocate_memory(sizeof(*mem));
	if (!mem)
		return NULL;
	mem->pa = lpa;
	mem->da = lda;

	*io = metal_allocate_memory(sizeof(struct metal_io_region));
	if (!*io) {
		metal_free_memory(mem);
		close(fd);
		return NULL;
	}

	/* This code only runs on the R5, which has a flat memory
	 * space. Therefore, we use the same value for the physical
	 * and virtual addresses we pass in to metal_io_init().
	 */
	metal_io_init(*io, (void *)virtual_address, &mem->pa, size,
		      sizeof(metal_phys_addr_t)<<3, attribute, NULL);
	mem->io = *io;
	metal_list_add_tail(&rproc->mems, &mem->node);
	*pa = lpa;
	*da = lda;
	mem->size = size;
	return metal_io_phys_to_virt(*io, mem->pa);
}