

/*
 * libspe - A wrapper library to adapt the JSRE SPU usage model to SPUFS
 * Copyright (C) 2005 IBM Corp.
 *
 * This library is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License,
 * or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful, but
 *  WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 *  or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
 *  License for more details.
 *
 *   You should have received a copy of the GNU Lesser General Public License
 *   along with this library; if not, write to the Free Software Foundation,
 *   Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <elf.h>
#include <errno.h>
#include <fcntl.h>
#include <pthread.h>
#define _GNU_SOURCE
#include <sched.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <wait.h>

#include <linux/unistd.h>

#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/poll.h>
#include <sys/select.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/syscall.h>

#include <sys/spe.h>

#include "libspe.h"
#include "spe.h"
#include "elf_loader.h"
#include "spe_exec.h"
#include "default_c99_handler.h"
#include "default_posix1_handler.h"


/*
 * Helpers
 *
 * */


int __env_spu_debug_start = 0;
int __env_spu_info = 0;
int check_env = 1;

extern int default_priority;
extern int default_policy;
extern int default_eventmask;


/*Default SPE library call handlers for 21xx stop-and-signal.
*/
static void *handlers[] = {
	default_c99_handler, default_posix1_handler,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,
	NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL };


/*
 * Context update notification function for debuggers.
 */

void  __spe_context_update_event(void) __attribute__((noinline));
void  __spe_context_update_event(void)
{
	/* Prevent function from being marked "pure".  */
	asm volatile ("" : : : "memory"); 
}

/*
 * int spe_ldr[]:
 * SPE code that performs the actual parameter setting:
 */
static int spe_ldr[] = {
	0x30fff203,
	0x30fff404,
	0x30fff605,
	0x30fff806,
	0x30fff000,
	0x35000000,
	0x00000000,
	0x00000000
};

void
env_check(void)
{
	if (getenv("SPU_DEBUG_START"))
		__env_spu_debug_start = strtol ((const char *)getenv("SPU_DEBUG_START"),(char **)NULL, 0);
	if (getenv("SPU_INFO"))
		__env_spu_info = strtol ((const char *)getenv("SPU_INFO"),(char **)NULL, 0);
	check_env = 0;
}


static int
spe_open_files(struct thread_store *spe, const char *pathname)
{
	char filename[256];
	int ret;
	
		 /* this one mail fail */
		 sprintf(filename, "%s/psmap", pathname);
		 spe->fd_psmap = open(filename, O_RDWR);
		 spe->ps_mmap_base=MAP_FAILED;

	sprintf(filename, "%s/mbox", pathname);
	spe->fd_mbox = open(filename, O_RDONLY);

	sprintf(filename, "%s/mbox_stat", pathname);
	spe->fd_mbox_stat = open(filename, O_RDONLY);

	sprintf(filename, "%s/ibox", pathname);
	spe->fd_ibox = open(filename, O_RDONLY);

	sprintf(filename, "%s/ibox_stat", pathname);
	spe->fd_ibox_stat = open(filename, O_RDONLY);

	sprintf(filename, "%s/wbox", pathname);
	spe->fd_wbox = open(filename, O_WRONLY);

	sprintf(filename, "%s/wbox_stat", pathname);
	spe->fd_wbox_stat = open(filename, O_RDONLY);

	sprintf(filename, "%s/npc", pathname);
	spe->fd_npc = open(filename, O_RDWR);
		
	sprintf(filename, "%s/signal1", pathname);
	spe->fd_sig1 = open(filename, O_RDWR);
	spe->signal1_mmap_base=MAP_FAILED;
	
	sprintf(filename, "%s/signal2", pathname);
	spe->fd_sig2 = open(filename, O_RDWR);
	spe->signal2_mmap_base=MAP_FAILED;
	
	/* this one may fail */
	sprintf(filename, "%s/mfc", pathname);
	spe->fd_mfc = open(filename, O_RDWR);
	spe->mfc_mmap_base=MAP_FAILED;

	/* this one may fail */
	sprintf(filename, "%s/cntl", pathname);
	spe->fd_cntl = open(filename, O_RDWR);
	spe->cntl_mmap_base=MAP_FAILED;

	/* this one may fail */
	sprintf(filename, "%s/mss", pathname);
	spe->fd_mss = open(filename, O_RDWR);
	spe->mss_mmap_base=MAP_FAILED;

	ret=pipe(spe->ev_pipe);
	if (ret)
			spe->ev_pipe[0] = spe->ev_pipe[1] = -1;
		
	if (spe->fd_mbox      < 0 || spe->fd_ibox      < 0 || spe->fd_wbox      < 0 ||
	    spe->fd_mbox_stat < 0 || spe->fd_ibox_stat < 0 || spe->fd_wbox_stat < 0 ||
	    spe->fd_npc       < 0 || spe->fd_sig1      < 0 || spe->fd_sig2      < 0 || 
		     ret ) {
		DEBUG_PRINTF("Could not open all SPE files.\n");
		 		 close(spe->fd_psmap);
		close(spe->fd_mbox);
		close(spe->fd_mbox_stat);
		close(spe->fd_ibox);
		close(spe->fd_ibox_stat);
		close(spe->fd_wbox);
		close(spe->fd_wbox_stat);
		close(spe->fd_npc);
		close(spe->fd_sig1);
		close(spe->fd_sig2);
		close(spe->fd_mfc);
		close(spe->fd_cntl);
		close(spe->fd_mss);
		close(spe->ev_pipe[0]);
		close(spe->ev_pipe[1]);
		return -1;
	}
	return 0;
}

static int spe_setup_mem(struct thread_store *thread_store,
			 void *argp, void *envp) 
{
	struct spe_exec_params spe_params __attribute__ ((aligned (4096)));
	void *spe_ld_buf;
	struct spe_ld_info ld_info;
	int rc;
	addr64 argp64, envp64, tid64;

#ifdef SPE_NO_MMAP
	ssize_t count = 0, num = 0;
	spe_ld_buf = malloc(LS_SIZE);
#else
 	spe_ld_buf = thread_store->mem_mmap_base;
#endif
	memset(spe_ld_buf, 0, LS_SIZE);

	rc = load_spe_elf (&thread_store->handle, spe_ld_buf, &ld_info);
	if (rc != 0) {
		DEBUG_PRINTF ("Load SPE ELF failed..\n");
		return rc;
	}

	/* Add SPE exec program */
	DEBUG_PRINTF ("Add exec prog dst:0x%04x size:0x%04x\n",
		      SPE_LDR_START, sizeof (spe_ldr));
	memcpy (spe_ld_buf + SPE_LDR_START, &spe_ldr, sizeof (spe_ldr));

	/* Add SPE exec parameters */
	spe_params.gpr0[0] = ld_info.entry;
	spe_params.gpr0[1] = 0;
	spe_params.gpr0[2] = 0;
	spe_params.gpr0[3] = 0;

	argp64.ull = (unsigned long long) (unsigned long) argp;
	envp64.ull = (unsigned long long) (unsigned long) envp;
	tid64.ull = (unsigned long long) (unsigned long) thread_store;
	
	
	if (thread_store->flags & SPE_USER_REGS) {
		/* When flags & SPE_USER_REGS is set, argp points
		 * to an array of 3x128b registers to be passed
		 * directly to the SPE program.
		 */
		memcpy(&spe_params.gpr3[0], argp, sizeof(unsigned int) * 12);
	} else {
		/* Regular parameter convention for spe_create_thread. */
		spe_params.gpr3[0] = tid64.ui[0];
		spe_params.gpr3[1] = tid64.ui[1];
		spe_params.gpr3[2] = 0;
		spe_params.gpr3[3] = 0;
	
		spe_params.gpr4[0] = argp64.ui[0];
		spe_params.gpr4[1] = argp64.ui[1];
		spe_params.gpr4[2] = 0;
		spe_params.gpr4[3] = 0;
	
		spe_params.gpr5[0] = envp64.ui[0];
		spe_params.gpr5[1] = envp64.ui[1];
		spe_params.gpr5[2] = 0;
		spe_params.gpr5[3] = 0;
	}

	spe_params.gpr6[0] = 0;
	spe_params.gpr6[1] = 0;
	spe_params.gpr6[2] = 0;
	spe_params.gpr6[3] = 0;

	DEBUG_PRINTF ("Add exec param dst:0x%04x size:0x%04x\n",
		      SPE_PARAM_START, sizeof (spe_params));
	memcpy (spe_ld_buf + SPE_PARAM_START, &spe_params,
		sizeof (spe_params));

#ifdef SPE_NO_MMAP
	/* Copy SPE image to SPUfs */
	do {
		num = write (memfd, spe_ld_buf + count, LS_SIZE - count);
		if (num == -1) {
			DEBUG_PRINTF ("Transfer SPE ELF failed..\n");
			return -errno;
		}
		count += num;
	} while (count < LS_SIZE && num);
	close (memfd);

	/* Free the SPE Buffer */
	free (spe_ld_buf);
#endif
	return rc;
}

/**
 * Low-level API
 * 
 */

void * spe_setup (spe_gid_t gid, spe_program_handle_t *handle, void *argp, void *envp, int flags)
{
	struct thread_store *thread_store;
	int rc, memfd, sigfd;
	char signame[256], memname[256], pathname[256];
	int spe_create_flags;

	if(!gid) {
		gid=spe_gid_setup(default_policy,default_priority,default_eventmask);
	} else {
		struct group_store *grp = gid;
		if (grp->numListSize == MAX_THREADS_PER_GROUP) {
		  	DEBUG_PRINTF ("Thread group has reached maximum number of members.\n");
		  	errno = ENOMEM;
		  	return NULL;
		}
	}

	if (flags & SPE_ISOLATE)
		flags |= SPE_MAP_PS;

	/*Allocate thread structure */
	thread_store = calloc (1, sizeof *thread_store);
	if (!thread_store) {
		DEBUG_PRINTF ("Could not allocate thread store\n");
		errno = ENOMEM;
		return NULL;
	}

	thread_store->group_id = gid;
	thread_store->flags = flags;

	/* Make the SPU Directory */

	sprintf (pathname, "/spu/spethread-%i-%lu",
		 getpid (), (unsigned long)thread_store);

	DEBUG_PRINTF ("create %s\n", pathname);

	spe_create_flags = 0;
	if (flags & SPE_ISOLATE)
		spe_create_flags = SPU_CREATE_ISOLATE | SPU_CREATE_NOSCHED;

	if (((struct group_store*)thread_store->group_id)->use_events)
		spe_create_flags |= SPU_CREATE_EVENTS_ENABLED;

	rc = spe_create(pathname, spe_create_flags, S_IRUSR | S_IWUSR | S_IXUSR);
	thread_store->fd_run = rc;
	if (rc < 0) {
		DEBUG_PRINTF ("Could not create SPE %s: %s\n",
				pathname, strerror(errno));
		return NULL;
	}

	sprintf (memname, "%s/mem", pathname);

	/* Check SPE */

	memfd = open (memname, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
	if (memfd < 0) {
		DEBUG_PRINTF ("Could not open SPE mem file.\n");
		return NULL;
	}

	/* Prepare Loader */
#ifndef SPE_NO_MMAP	
	thread_store->mem_mmap_base = mmap(0, LS_SIZE,
			PROT_READ | PROT_WRITE, MAP_SHARED, memfd, 0);
	thread_store->fd_mem = memfd;

	if (!thread_store->mem_mmap_base) {
		DEBUG_PRINTF ("Could not allocate SPE memory. \n");
		errno = ENOMEM;
		return NULL;
	}
#endif

	memcpy(&thread_store->handle, handle, sizeof *handle);

	if (!(flags & SPE_ISOLATE)) {
		rc = spe_setup_mem(thread_store, argp, envp);
		if (rc) {
			perror("setup mem");
			return NULL;
		}
	}

	rc = spe_open_files(thread_store, pathname);
	if (rc) {
		perror("open files\n");
		return NULL;
	}

	rc = add_thread_to_group(gid, thread_store);
        
	if ( flags & SPE_CFG_SIGNOTIFY1_OR ) {
		char one[] = "1";
		int ignore;

		sprintf (signame, "%s/signal1_type", pathname);
		sigfd = open (signame, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
		if (sigfd < 0) {
			DEBUG_PRINTF ("Could not open SPE signal1_type file.\n");
			return NULL;
		}
		ignore=write (sigfd, one, 1);
		close(sigfd);
	}
        if ( flags & SPE_CFG_SIGNOTIFY2_OR ) {
		char one[] = "1";
		int ignore;
   
		sprintf (signame, "%s/signal2_type", pathname);
		sigfd = open (signame, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
		if (sigfd < 0) {
			DEBUG_PRINTF ("Could not open SPE signal2_type file.\n");
			return NULL;
		} 
		ignore=write (sigfd, one, 1);
		close(sigfd); 
	}


	/* Register SPE image start address as "object-id" for oprofile.  */
	sprintf (signame, "%s/object-id", pathname);
	sigfd = open (signame, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
	if (sigfd >= 0) {
		char buf[100];
		sprintf (buf, "%p", handle->elf_image);
		write (sigfd, buf, strlen (buf) + 1);
		close (sigfd);
	}
	
	__spe_context_update_event();
	
	thread_store->stop = 0;
	return thread_store;
}

void spe_cleanup (void *ptr)
{
	struct thread_store *thread_store = ptr;
	char pathname[256];

	sprintf (pathname, "/spu/spethread-%i-%lu",
		 getpid (), (unsigned long)thread_store);

	close (thread_store->fd_run);
	close (thread_store->fd_mbox);
	close (thread_store->fd_mbox_stat);
	close (thread_store->fd_ibox);
	close (thread_store->fd_ibox_stat);
	close (thread_store->fd_wbox);
	close (thread_store->fd_wbox_stat);
	close (thread_store->fd_npc);
	
		 /* If psmap exists, unmap just that, else unmap individual bits */
		 if (thread_store->fd_psmap < 0) {
		 		 if (thread_store->signal1_mmap_base != MAP_FAILED)
		 		 		 munmap(thread_store->signal1_mmap_base,0x1000);
		 		 if (thread_store->signal2_mmap_base != MAP_FAILED)
		 		 		 munmap(thread_store->signal2_mmap_base,0x1000);
		 		 if (thread_store->mfc_mmap_base != MAP_FAILED)
		 		 		 munmap(thread_store->mfc_mmap_base,0x1000);
		 		 if (thread_store->cntl_mmap_base != MAP_FAILED)
		 		 		 munmap(thread_store->cntl_mmap_base,0x1000);
		 		 if (thread_store->mss_mmap_base != MAP_FAILED)
		 		 		 munmap(thread_store->mss_mmap_base,0x1000);
		 } else {
		 		 if (thread_store->ps_mmap_base != MAP_FAILED)
		 		 		 munmap(thread_store->ps_mmap_base, 0x20000);
		 		 close (thread_store->fd_psmap);
		 }
		 close (thread_store->fd_mss);
		 close (thread_store->fd_cntl);
		 close (thread_store->fd_mfc);
	close (thread_store->fd_sig1);
	close (thread_store->fd_sig2);

#ifndef SPE_NO_MMAP
	munmap(thread_store->mem_mmap_base,LS_SIZE);
	close (thread_store->fd_mem);
#endif
	close (thread_store->ev_pipe[0]);
        close (thread_store->ev_pipe[1]);

	remove_thread_from_group(thread_store->group_id, thread_store);

	free (thread_store);
	
	__spe_context_update_event();
}

unsigned int set_npc (void *ptr, unsigned int npc)
{
	struct thread_store *thread_store = ptr;
	int ret;
	char npcText[9];

	sprintf(npcText,"0x%06x\n",npc);
	ret = write(thread_store->fd_npc, npcText, 9);
	if (ret < 0)
		return -EINVAL;
	return npc;
}

static void spe_exit_isolated(struct thread_store *thread)
{
	spe_spu_control_area_t *control_area;

	control_area = spe_get_ps_area(thread, SPE_CONTROL_AREA);
	if (!control_area) {
		DEBUG_PRINTF("%s: could not access SPE control area: %s\n",
				__FUNCTION__, strerror(errno));
		return;
	}

	/* 0b10 is an isolated exit request */
	control_area->SPU_RunCntl = 0x2;
}

int do_spe_run (void *ptr)
{
	struct thread_store *thread_store = ptr;
	int runfd;

	unsigned int npc;
	unsigned int code;
	unsigned int status;
	unsigned int isolated_ret = 0;

	npc = SPE_LDR_PROG_start;

	int ret, rc;

        DEBUG_PRINTF ("do_spe_run\n");
	runfd = thread_store->fd_run;

	thread_store->thread_status = 1; 
	
	/* Note: in order to be able to kill a running SPE thread the 
	 *       spe thread must be cancelable.
	 */
	
	pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
	
	do {
		thread_store->thread_status = 2; 
		
		ret = spe_run(runfd, &npc, &status);
		code = ret >> 16;
                DEBUG_PRINTF ("SPE (errno=%d) (npc=0x%06x) (code=0x%08x) (ret=0x%08x) (status=0x%08x).\n",
			      errno, npc, code, ret & 0xffff, status);

		thread_store->thread_status = 10; //SPE _not_ running.
		
		/* If a signal was sent to STOP execution we will pause and wait 
		 * for a continue signal before going on.
		 */

		if ( thread_store->stop ) {
			pthread_mutex_lock(&thread_store->event_lock);
			pthread_cond_init(&thread_store->event_deliver, NULL);
			
			thread_store->thread_status = 20; //SPE _pausing_.
			/* Now wait for the continue signal */
			
			pthread_cond_wait(&thread_store->event_deliver, &thread_store->event_lock);
			pthread_mutex_unlock(&thread_store->event_lock);
			thread_store->stop = 0;
			if (thread_store->killed) // no events when we got killed 
				return -1;
			thread_store->thread_status = 10; //SPE _not_ running.
		}
                
		if (ret < 0) {
			if (((struct group_store*)thread_store->group_id)->use_events && errno == EIO) {
				/* Events are turned on, and this is an event.
				 */
				
				thread_store->event = status;
				thread_store->ev_data = npc;
				//
				//Todo Error handling
				//
				write(thread_store->ev_pipe[1], &thread_store->event, 4);
				 
			} else {
				DEBUG_PRINTF ("Could not run on SPE: %s.\n",
				strerror(errno));
				return -errno;
			}
		} else if (ret & 0x20) {
			if (((struct group_store*)thread_store->group_id)->use_events ) {
				/* Events are turned on, and this is an event.
				*/
				thread_store->event = SPE_EVENT_SPE_ERROR;
 				thread_store->ev_data = npc;
				write(thread_store->ev_pipe[1], &thread_store->event, 4);
				errno = EFAULT;
				DEBUG_PRINTF ("Returned invalid instruction: %s.\n",
                                strerror(errno));
                                return -errno;
			} else {
				DEBUG_PRINTF ("Could not run on SPE: %s.\n",
				strerror(errno));
				return -errno;
			}
		} else if (ret & 0x4) {
			if (((struct group_store*)thread_store->group_id)->use_events ) {
				/* Events are turned on, and this is an event.
				*/
				thread_store->event = SPE_EVENT_SPE_TRAPPED;
 				thread_store->ev_data = npc;
				write(thread_store->ev_pipe[1], &thread_store->event, 4);
				errno = EFAULT;
				DEBUG_PRINTF ("Returned invalid execution: %s.\n",
                                strerror(errno));
                                return -errno;
			} else {
				DEBUG_PRINTF ("Could not run on SPE: %s.\n",
				strerror(errno));
				return -errno;
			}
		} else if (ret & 0x1) {
			if (((struct group_store*)thread_store->group_id)->use_events ) {
				/* Events are turned on, and this is an event.
				*/
				thread_store->event = status;
				thread_store->ev_data = npc;
				write(thread_store->ev_pipe[1], &thread_store->event, 4);
				errno = EFAULT;
				DEBUG_PRINTF ("Returned invalid data: %s.\n",
                                strerror(errno));
                                return -errno;
                        } else {
                                DEBUG_PRINTF ("Could not run on SPE: %s.\n",
                                strerror(errno));
                                return -errno;
                        }
                } else if ((ret & 0x6) == 0) {
                        if (((struct group_store*)thread_store->group_id)->use_events ) {
                                /* Events are turned on, and this is an event.
                                */
                                thread_store->event = status;
                                thread_store->ev_data = npc;
                                write(thread_store->ev_pipe[1], &thread_store->event, 4);
                                errno = EFAULT;
                                DEBUG_PRINTF ("Returned invalid data: %s.\n",
                                strerror(errno));
                                return -errno;
                        } else {
                                DEBUG_PRINTF ("Could not run on SPE: %s.\n",
                                strerror(errno));
                                return -errno;
                        }
		} else if ((code & 0xff00) == 0x2100) {
			int callnum = code&0xff;
			
			DEBUG_PRINTF ("SPE library call: ");
			
			if (!handlers[callnum]) {
				DEBUG_PRINTF ("Unsupported SPE library call signal: 21%i.",callnum);
				break;
			} else {
				int (*handler)(void *, unsigned int);
				handler=handlers[callnum];
				
				rc = handler(thread_store->mem_mmap_base, npc);
				if (rc) {
			        	DEBUG_PRINTF ("SPE library call unsupported.\n");
					thread_store->thread_status = 99; 
				        return -ENOSYS;
			        } else {
					npc += 4;
				}
			}
		} else if (thread_store->flags & SPE_ISOLATE) {
			if (code == 0) {
				/* isolated app completed sucessfully, SPE is
				 * back in non-isolated state */
				break;
			} else {
				/* still in isolated mode (either the load
				 * failed, or the app has exited). Remember the
				 * status code and issue an exit */
				isolated_ret = ret;
				spe_exit_isolated(thread_store);
				code = 0;
			}

		} else if (code < 0x2000) {
			pthread_mutex_lock(&thread_store->event_lock);
			pthread_cond_init(&thread_store->event_deliver, NULL);
			
			if (((struct group_store*)thread_store->group_id)->use_events) {
				int callnum = code;
				//
				// Event - driven 21xx hanlers
				//
				
				thread_store->event = SPE_EVENT_STOP;
				thread_store->ev_data = callnum;
				//
				//Todo Error handling
				//
				write(thread_store->ev_pipe[1], &thread_store->event, 4);
				
				//Show that we're waiting
				thread_store->event_pending = 1;
				
				// Wait for a signal.
				pthread_cond_wait(&thread_store->event_deliver, &thread_store->event_lock);

				thread_store->event_pending = 0;
				
				pthread_mutex_unlock(&thread_store->event_lock);
			} else {
				pthread_mutex_unlock(&thread_store->event_lock);
				
				DEBUG_PRINTF ("SPE user events not enabled.\n");
				thread_store->thread_status = 99; 
			        return -ENOSYS;
			}
		}
        } while (((code & 0xff00) == 0x2100 || (code < 0x2000)) && !thread_store->killed );

	DEBUG_PRINTF ("SPE thread result: %08x:%04x\n", npc, code);
	
	/* Save status & npc */	
	if (thread_store->flags & SPE_ISOLATE) {
		code = isolated_ret >> 16;
		thread_store->ret_status = isolated_ret;
		thread_store->npc = 0;
	} else {
		thread_store->ret_status = ret;
		thread_store->npc = npc;
	}

	//
	//Event code
	//
	if (((struct group_store*)thread_store->group_id)->use_events) {
		thread_store->event = SPE_EVENT_THREAD_EXIT;
		thread_store->ev_data = code & 0xff;
		//
		//Todo Error handling
		//
		write(thread_store->ev_pipe[1], &thread_store->event, 4);
	}

	thread_store->thread_status = 99; 
	return code;
}

static int
toe_check_syms(Elf32_Ehdr *ehdr, Elf32_Shdr *sh)
{
	Elf32_Sym  *sym, *sym_hdr, *sym_end;
	Elf32_Shdr *shdr;
	char *str_table;
	char *sym_name;
	int ret;

	shdr = (Elf32_Shdr*) ((char*) ehdr + ehdr->e_shoff);
	sym_hdr = (Elf32_Sym*) ((char*) ehdr + sh->sh_offset);
	sym_end = (Elf32_Sym*) ((char*) ehdr + sh->sh_offset + sh->sh_size);
	str_table = (char*)ehdr + shdr[sh->sh_link].sh_offset;

	ret = 0;
	for (sym = sym_hdr; sym < sym_end; sym++)
		if (sym->st_name) {
			sym_name = str_table + sym->st_name;
			if ((strncmp(sym_name, "_EAR_", 5) == 0) &&
			    (strcmp(sym_name, "_EAR_") != 0)) {
				/*
				 * We have a prefix of _EAR_ followed by
				 * something else. This is not currently
				 * (and might not ever be) supported: for
				 * a _EAR_foo, it requires a lookup of foo
				 * in the ppu ELF file.
				 */
				fprintf(stderr, "Invalid _EAR_ symbol '%s'\n",
					sym_name);
				errno = EINVAL;
				ret = 1;
			}
		}
	return ret;
}

static int
toe_ear (spe_program_handle_t *speh)
{
	Elf32_Ehdr *ehdr;
	Elf32_Shdr *shdr, *sh;
	char *str_table;
	char **ch;
	int ret;
	long toe_size;

	ehdr = (Elf32_Ehdr*) (speh->elf_image);
	shdr = (Elf32_Shdr*) ((char*) ehdr + ehdr->e_shoff);
	str_table = (char*)ehdr + shdr[ehdr->e_shstrndx].sh_offset;

	toe_size = 0;
	for (sh = shdr; sh < &shdr[ehdr->e_shnum]; ++sh)
		if (strcmp(".toe", str_table + sh->sh_name) == 0)
			toe_size += sh->sh_size;

	ret = 0;
	if (toe_size > 0) {
		for (sh = shdr; sh < &shdr[ehdr->e_shnum]; ++sh)
			if (sh->sh_type == SHT_SYMTAB || sh->sh_type ==
			    SHT_DYNSYM)
				ret = toe_check_syms(ehdr, sh);
		if (!ret && toe_size != 16) {
			/* Paranoia */
			fprintf(stderr, "Unexpected toe size of %ld\n",
				toe_size);
			errno = EINVAL;
			ret = 1;
		}
	}
	if (!ret && toe_size) {
		/*
		 * Allocate toe_shadow, and fill it with elf_image.
		 */
		speh->toe_shadow = malloc(toe_size);
		if (speh->toe_shadow) {
			ch = (char**) speh->toe_shadow;
			if (sizeof(char*) == 8) {
				ch[0] = (char*) speh->elf_image;
				ch[1] = 0;
			} else {
				ch[0] = 0;
				ch[1] = (char*) speh->elf_image;
				ch[2] = 0;
				ch[3] = 0;
			}
		} else {
			errno = ENOMEM;
			ret = 1;
		}
	}
	return ret;
}

struct image_handle {
	spe_program_handle_t speh;
	unsigned int map_size;
};

spe_program_handle_t *spe_open_image(const char *filename)
{
	/* allocate an extra integer in the spe handle to keep the mapped size information */
	struct image_handle *ret;
	int binfd = -1, f_stat;
	struct stat statbuf;
	size_t ps = getpagesize ();

	ret = malloc(sizeof(struct image_handle));
	if (!ret)
		return NULL;

	ret->speh.handle_size = sizeof(spe_program_handle_t);
	ret->speh.toe_shadow = NULL;

	binfd = open(filename, O_RDONLY);
	if (binfd < 0)
		goto ret_err;

	f_stat = fstat(binfd, &statbuf);
	if (f_stat < 0)
		goto ret_err;

	/* Sanity: is it executable ?
	 */
	if(!(statbuf.st_mode & (S_IXUSR | S_IXGRP | S_IXOTH)))
	{
		errno=EACCES;
		goto ret_err;
	}
	
	/* now store the size at the extra allocated space */
	ret->map_size = (statbuf.st_size + ps - 1) & ~(ps - 1);

	ret->speh.elf_image = mmap(NULL, ret->map_size,
			           PROT_WRITE | PROT_READ,
				   MAP_PRIVATE, binfd, 0);
	if (ret->speh.elf_image == MAP_FAILED)
		goto ret_err;

	/*Verify that this is a valid SPE ELF object*/

	if((verify_spe_elf_image((spe_program_handle_t *)ret)))
		goto ret_err;

	if (toe_ear(&ret->speh))
		goto ret_err;

	/* ok */
	close(binfd);
	return (spe_program_handle_t *)ret;

	/* err & cleanup */
ret_err:
	if (binfd >= 0)
		close(binfd);

	free(ret);
	return NULL;
}

int spe_close_image(spe_program_handle_t *handle)
{
	int ret = 0;
	struct image_handle *ih;

	if (!handle) {
		errno = EINVAL;
		return -1;
	}
	
	ih = (struct image_handle *)handle;

	if (!ih->speh.elf_image || !ih->map_size) {
		errno = EINVAL;
		return -1;
	}

	if (ih->speh.toe_shadow)
		free(ih->speh.toe_shadow);

	ret = munmap(ih->speh.elf_image, ih->map_size );
	free(handle);

	return ret;
}

void register_handler(void * handler, unsigned int callnum ) 
{
	handlers[callnum]=handler;
}

/**
 * Library API
 * 
 */


void * spe_thread (void *ptr)
{
	/* If the SPU_INFO (or SPU_DEBUG_START) environment variable is set,
	   output a message to stderr telling the user how to attach a debugger
	   to the new SPE thread.  */
	if (__env_spu_debug_start || __env_spu_info) {
		int tid = syscall (__NR_gettid);
		fprintf (stderr, "Starting SPE thread %p, to attach debugger use: spu-gdb -p %d\n",
			 ptr, tid);

		/* In the case of SPU_DEBUG_START, actually wait until the user *has*
		   attached a debugger to this thread.  This is done here by doing an
		   sigwait on the empty set, which will return with EINTR after the
		   debugger has attached.  */
		if (__env_spu_debug_start) {
			sigset_t set;
			sigemptyset (&set);
			/* Use syscall to avoid glibc looping on EINTR.  */
			syscall (__NR_rt_sigtimedwait, &set, (void *) 0,
				 (void *) 0, _NSIG / 8);
		}
	}
	return (void *) (unsigned long) do_spe_run (ptr);
}

void *spe_get_ls (speid_t speid)
{
	struct thread_store *thread_store = speid;
	
	if (!srch_thread(speid))
		return NULL;
	
#ifdef SPE_NO_MMAP
	errno = EFAULT;
	return -1;
#else
	return thread_store->mem_mmap_base;
#endif
}

int spe_get_context(speid_t speid, struct spe_ucontext *uc)
{
	printf("spe_get_context: not implemented in this release.\n");

	errno=ENOSYS;
	return -1;
}

int spe_set_context(speid_t speid, struct spe_ucontext *uc)
{
	printf("spe_set_context: not implemented in this release.\n");

	errno=ENOSYS;
	return -1;
}

int __spe_get_context_fd(speid_t speid)
{
	struct thread_store *thread_store = speid;
	
	if (!srch_thread(speid))
		return -1;

	return thread_store->fd_run;
}
		

/*
 * mfc.h direct call-ins
 *
 * */

int spe_write_in_mbox (speid_t speid ,unsigned int data)
{
	struct thread_store *thread_store = speid;
	int rc;
	
	if (!srch_thread(speid))
		return -1;
	
	if (thread_store->thread_status == 99)
		return -1;

	rc = write(thread_store->fd_wbox, &data, 4);
	if (rc == 4)
		rc = 0;

	return rc;
}

int spe_stat_in_mbox (speid_t speid)
{
	struct thread_store *thread_store = speid;
	int rc, ret;

	if (!srch_thread(speid))
		return -1;
	
	rc = read(thread_store->fd_wbox_stat, &ret, 4);
	if (rc != 4)
		ret = -1;

	return ret;
}

unsigned int spe_read_out_mbox (speid_t speid)
{
        struct thread_store *thread_store = speid;
        int rc;
	unsigned int ret;

	if (!srch_thread(speid))
		return -1;
	
        rc = read(thread_store->fd_mbox, &ret, 4);
        if (rc != 4)
	        ret = -1;

        return ret;
}

int spe_stat_out_mbox (speid_t speid)
{
	struct thread_store *thread_store = speid;
        int rc, ret;

	if (!srch_thread(speid))
		return -1;
	
        rc = read(thread_store->fd_mbox_stat, &ret, 4);
        if (rc != 4)
                ret = -1;

        return ret;
}

int spe_stat_out_intr_mbox (speid_t speid)
{
        struct thread_store *thread_store = speid;
        int rc, ret;

	if (!srch_thread(speid))
		return -1;
	
        rc = read(thread_store->fd_ibox_stat, &ret, 4);
        if (rc != 4)
                ret = -1;

        return ret;
}

unsigned int spe_read_out_intr_mbox(speid_t speid)
{
	struct thread_store *thread_store = speid;
	int rc;
	unsigned int ret;

	if (!srch_thread(speid))
		return -1;
	
        rc = read(thread_store->fd_ibox, &ret, 4);
        if (rc != 4)
	        ret = -1;

        return ret;
}


int spe_write_signal (speid_t speid, unsigned int signal_reg, unsigned int data)
{
	struct thread_store *thread_store = speid;
	int rc;
	
	if (!srch_thread(speid))
		return -1;
	
	if (thread_store->thread_status == 99)
		return -1;

	if (signal_reg == SPE_SIG_NOTIFY_REG_1)
		rc = write(thread_store->fd_sig1, &data, 4);
	else if (signal_reg == SPE_SIG_NOTIFY_REG_2)
		rc = write(thread_store->fd_sig2, &data, 4);
	else
		return -1;
	
	if (rc == 4)
		rc = 0;

	return rc;
}

/**
 * do_spe_map_ps_area(spe, offset, fd, size)
 *
 * This is an internal function for mmapping part of the SPE problem state.
 * it will use either the 128k single problem state mapping on recent kernels
 * or the individual per-file mmap using the passed-in fd if the former is
 * not available.
 */
void *do_spe_map_ps_area(struct thread_store *spe, off_t offset, int fd,
		 		 		  size_t size)
{
		 /* Check if 128k mapping is possible and not mapped yet */
		 if (spe->fd_psmap >= 0 && spe->ps_mmap_base == MAP_FAILED) {
		 		 spe->ps_mmap_base = mmap(0, 0x20000, PROT_READ | PROT_WRITE,
		 		 		 		 		  MAP_SHARED, spe->fd_psmap, 0);
		 		 if (spe->ps_mmap_base == MAP_FAILED)
		 		 		 return MAP_FAILED;
		 }

		 /* If we have 128k mapping, return an offset within */
		 if (spe->ps_mmap_base != MAP_FAILED) {
		 		 if (offset > 0x20000 || (offset + size) > 0x20000)
		 		 		 return MAP_FAILED;
		 		 return spe->ps_mmap_base + offset;
		 }

		 /* Use the individual file mapping */
		 return mmap(0, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
}



/*
 * 
 */
 
