dld_server.c

Omap5910 上实现双核通信 DSP GateWay

/*
 * dsp_dld/arm/dld_server.c
 *
 * DSP Dynamic Loader Daemon: dld_server.c
 *
 * Copyright (C) 2003-2005 Nokia Corporation
 *
 * Written by Toshihiro Kobayashi <[e-mail]>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * 2005/07/06:  DSP Gateway version 3.3
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <malloc.h>
#include <fcntl.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <sys/ioctl.h>
#include <sys/wait.h>
#include <asm/arch/dsp.h>
#include "dsp_dld.h"
#include "dld_daemon.h"
#include "dld_coff.h"
#include "dld_cmd.h"
#include "dld_taskent.h"
#include "dld_memmgr.h"
#include "dld_symbol.h"
#include "dld_section.h"

#ifdef USE_FORK
#define MAX_CHILDREN	10
#endif

struct server_fds {
	fd_set set;
	int fd_twch;
	int fd_sock;
	int fd_acc;
	int fd_err;
#ifdef USE_FORK
	int fd_pipe[MAX_CHILDREN];
#endif
};
static struct server_fds server_fds;

#ifndef DSP_EMULATION
extern int dev_mklink(int n);
extern int dev_unlink(int n);
#endif
extern u32 task_load(struct taskent *te);
extern int task_clear(struct taskent *te);

static int signal_received = 0;
static server_return_t signal_action;
#ifndef DSP_EMULATION
static int suspend = 0;
#endif

#ifndef DSP_EMULATION
#define TID_MAX	254
static long tstat_prev[TID_MAX];
#endif

extern unsigned long binary_version;

/*
 * event packet operation
 */
static int read_event(int fd, struct server_event *e, size_t maxsz)
{
	if (read(fd, e, SERVER_EVENT_HDRSZ) < 0) {
		prmsg("failed to read event header\n");
		return -1;
	}
	if (e->len > SERVER_EVENT_HDRSZ) {
		if (e->len > maxsz) {
			prmsg("event packet size too large (%d)\n", e->len);
			return -1;
		}
		if (read(fd, &e->data, e->len - SERVER_EVENT_HDRSZ) < 0) {
			prmsg("failed to read event body\n");
			return -1;
		}
	}

	return 0;
}

static void sendback_event(int fd, u32 event)
{
	struct server_event e_sb;

	e_sb.len = sizeof(struct server_event);
	e_sb.event = event;
	write(fd, &e_sb, e_sb.len);
}

#ifdef USE_FORK
/*
 * parent-child communication
 */
static int pipe_alloc(void)
{
	int i;
	int pp[2];

	for (i = 0; i < MAX_CHILDREN; i++) {
		if (server_fds.fd_pipe[i] < 0) {
			pipe(pp);
			server_fds.fd_pipe[i] = pp[0];
			FD_SET(pp[0], &server_fds.set);
			return pp[1];
		}
	}
	/* failed */
	return -1;
}

static int read_pipe(int fd)
{
	char buf[256];
	struct server_event *e = (struct server_event *)buf;
	struct taskent *te;
	u8 minor;
	int ret;

	if (read_event(fd, e, 256) < 0) {
		prmsg("failed to read event packet from pipe\n");
		return -1;
	}

	switch (e->event) {
		case DLD_EVENT_TADD_DONE:
#ifndef DSP_EMULATION
			minor = e->data.task.minor;
			te = taskent_find_by_minor(minor);
			if (--te->cobj->request_lock > 0) {
				prmsg("there're some waiting request.\n");
				te->cobj->request_lock = 0;
				return taskent_process_request_all();
			}
#endif
			return 0;

		case DLD_EVENT_TDEL_DONE:
		case DLD_EVENT_TKILL_DONE:
			minor = e->data.task.minor;
			te = taskent_find_by_minor(minor);
			ret = task_clear(te);
#ifndef DSP_EMULATION
			if (--te->cobj->request_lock > 0) {
				prmsg("there're some waiting request.\n");
				te->cobj->request_lock = 0;
				return taskent_process_request_all();
			}
#endif
			return ret;

		default:
			prmsg("unknown event from pipe: %d\n", e->event);
			return -1;
	}
}
#endif /* USE_FORK */

/*
 * fork for tadd, tdel, tkill
 */
static pid_t fork_task_add(u8 minor, u32 taskadr)
{
#ifndef DSP_EMULATION
	struct omap_dsp_taddinfo taddinfo;
#endif
#ifdef USE_FORK
	struct server_event e;
	int fd_pipe;
	pid_t pid;

	if ((fd_pipe = pipe_alloc()) < 0) {
		prmsg("pipe alloc failed\n");
		return -1;
	}
	if ((pid = fork()) < 0) {
		prmsg("fork failed\n");
		return -1;
	}
	if (pid > 0) {	/* parent */
		close(fd_pipe);
		return pid;
	}
#endif

	/*
	 * child:
	 * theoretically we should close pp[0] and other server fds.
	 * (but not done :p)
	 */
#ifndef DSP_EMULATION
	taddinfo.minor = minor;
	taddinfo.taskadr = taskadr;
	ioctl(server_fds.fd_twch, OMAP_DSP_TWCH_IOCTL_TADD, &taddinfo);
#endif

#ifdef USE_FORK
	e.len = sizeof(struct server_event);
	e.event = DLD_EVENT_TADD_DONE;
	e.data.task.minor = minor;
	write(fd_pipe, &e, e.len);
	close(fd_pipe);
#endif
	return 0;
}

static pid_t fork_task_del(u8 minor)
{
#ifdef USE_FORK
	struct server_event e;
	int fd_pipe;
	pid_t pid;

	if ((fd_pipe = pipe_alloc()) < 0) {
		prmsg("pipe alloc failed\n");
		return -1;
	}
	if ((pid = fork()) < 0) {
		prmsg("fork failed\n");
		return -1;
	}
	if (pid > 0) {	/* parent */
		close(fd_pipe);
		return pid;
	}
#endif

	/*
	 * child:
	 * theoretically we should close pp[0] and other server fds.
	 * (but not done :p)
	 */
#ifndef DSP_EMULATION
	ioctl(server_fds.fd_twch, OMAP_DSP_TWCH_IOCTL_TDEL, minor);
#endif
#ifdef USE_FORK
	e.len = sizeof(struct server_event);
	e.event = DLD_EVENT_TDEL_DONE;
	e.data.task.minor = minor;
	write(fd_pipe, &e, e.len);
	close(fd_pipe);
#endif
	return 0;
}

static pid_t fork_task_kill(u8 minor)
{
#ifdef USE_FORK
	struct server_event e;
	int fd_pipe;
	pid_t pid;

	if ((fd_pipe = pipe_alloc()) < 0) {
		prmsg("pipe alloc failed\n");
		return -1;
	}
	if ((pid = fork()) < 0) {
		prmsg("fork failed\n");
		return -1;
	}
	if (pid > 0) {	/* parent */
		close(fd_pipe);
		return pid;
	}
#endif

	/*
	 * child:
	 * theoretically we should close pp[0] and other server fds.
	 * (but not done :p)
	 */
#ifndef DSP_EMULATION
	ioctl(server_fds.fd_twch, OMAP_DSP_TWCH_IOCTL_TKILL, minor);
#endif
#ifdef USE_FORK
	e.len = sizeof(struct server_event);
	e.event = DLD_EVENT_TKILL_DONE;
	e.data.task.minor = minor;
	write(fd_pipe, &e, e.len);
	close(fd_pipe);
#endif
	return 0;
}

/*
 *
 */
static int server_tadd(struct server_event *e)
{
	u8 minor = e->data.task.minor;
	struct taskent *te = taskent_find_by_minor(minor);
	int fd = server_fds.fd_acc;
	u32 taskadr;
	pid_t pid;

	if (te == NULL)
		goto fail;
	taskadr = task_load(te);
	if (taskadr == OMAP_DSP_TADD_ABORTADR)
		/*
		 * in this case we don't need to process
		 * fork_task_add() with ABORTADR.
		 */
		goto fail;

#ifdef USE_FORK
	if (te->cobj->request_lock) {
		prmsg("device %s is locked. rejecting TADD request.\n",
		      te->devname);
		goto fail;
	}
	te->cobj->request_lock++;
#endif

	pid = fork_task_add(minor, taskadr);
	if (pid < 0) {
		sendback_event(fd, DLD_EVENT_ERROR);
		exit(1);
	}
#ifdef USE_FORK
	if (pid == 0) {	/* child */
		sendback_event(fd, DLD_EVENT_DONE);
		exit(0);
	}
#else
	sendback_event(fd, DLD_EVENT_DONE);
#endif
	/* parent */
	return 0;

fail:
	sendback_event(fd, DLD_EVENT_ERROR);
	return 1;	/* not fatail */
}

static int server_tdel(struct server_event *e)
{
	u8 minor = e->data.task.minor;
	struct taskent *te = taskent_find_by_minor(minor);
	int fd = server_fds.fd_acc;
	pid_t pid;

	if (te == NULL)
		goto fail;

#ifdef USE_FORK
	if (te->cobj->request_lock) {
		prmsg("device %s is locked. rejecting TDEL request.\n",
		      te->devname);
		goto fail;
	}
	te->cobj->request_lock++;
#endif

	pid = fork_task_del(minor);
	if (pid < 0) {
		sendback_event(fd, DLD_EVENT_ERROR);
		exit(1);
	}
#ifdef USE_FORK
	if (pid == 0) {	/* child */
		/*
		 * task_clear() will be done when
		 * the server received TDEL_DONE event.
		 */
		sendback_event(fd, DLD_EVENT_DONE);
		exit(0);
	}
	/* parent */
	return 0;
#else
	sendback_event(fd, DLD_EVENT_DONE);
	return task_clear(te);
#endif

fail:
	sendback_event(fd, DLD_EVENT_ERROR);
	return 1;	/* not fatal */
}

static int server_tkill(struct server_event *e)
{
	u8 minor = e->data.task.minor;
	struct taskent *te = taskent_find_by_minor(minor);
	int fd = server_fds.fd_acc;
	pid_t pid;

	if ((te == NULL) || (te->cobj->usecount == 0))
		goto fail;

#ifdef USE_FORK
	if (te->cobj->request_lock) {
		prmsg("device %s is locked. rejecting TKILL request.\n",
		      te->devname);
		goto fail;
	}
	te->cobj->request_lock++;
#endif

	pid = fork_task_kill(minor);
	if (pid < 0) {
		sendback_event(fd, DLD_EVENT_ERROR);
		exit(1);
	}
#ifdef USE_FORK
	if (pid == 0) {	/* child */
		/*
		 * task_clear() will be done when
		 * the server received TKILL_DONE event.
		 */
		sendback_event(fd, DLD_EVENT_DONE);
		exit(0);
	}
	/* parent */
	return 0;
#else
	sendback_event(fd, DLD_EVENT_DONE);
	return task_clear(te);
#endif

fail:
	sendback_event(fd, DLD_EVENT_ERROR);
	return 1;	/* not fatal */
}

static int server_sendstat_memmgr(int fd)
{
	memmgr_sendstat(NULL, fd);
	taskent_mem_sendstat(fd);
	sendback_event(fd, DLD_EVENT_DONE);
	return 0;
}

extern struct dld_conf dld_conf;

static int server_sendstat_symbol(int fd)
{
#ifdef STICKY_LIST
	symbol_sendstat(NULL, fd);
#else
	struct coffobj *knl_cobj = coff_new(dld_conf.knlfn);
	if (coff_read_kernel(knl_cobj) < 0) {
		sendback_event(fd, DLD_EVENT_ERROR);
		return -1;
	}
	symbol_sendstat(&knl_cobj->symlist, fd);
	coff_free(knl_cobj);
#endif
	taskent_sym_sendstat(fd);
	sendback_event(fd, DLD_EVENT_DONE);
	return 0;
}

static int server_sendstat_section(int fd)
{
#ifdef STICKY_LIST
	section_sendstat(NULL, fd);
#else
	struct coffobj *knl_cobj = coff_new(dld_conf.knlfn);
	if (coff_read_kernel(knl_cobj) < 0) {
		sendback_event(fd, DLD_EVENT_ERROR);
		return -1;
	}
	section_sendstat(&knl_cobj->scnlist, fd);
	coff_free(knl_cobj);
#endif
	taskent_scn_sendstat(fd);
	sendback_event(fd, DLD_EVENT_DONE);
	return 0;
}

static int server_memdump(int fd, struct server_event *e)
{
	u32 addr = e->data.memdump.addr;
	u32 size = e->data.memdump.size;
	char buf[256];
	struct server_event *e1 = (struct server_event *)buf;
	size_t strsz;
	int cnt;
	u32 i;
	u8 *p;

#ifdef DSP_EMULATION

	struct memmgr *mem;

	mem = memmgr_find_by_addr(NULL, addr, size);
	if (mem == NULL) {
		sendback_event(fd, DLD_EVENT_ERROR);
		return -1;
	}

	p = &mem->img[addr - mem->base];

#else

	int space;
	int devfd;
	char *dbuf;

	space = space_find_by_addr(addr, size);
	if (space == SPACE_CROSSING) {
		prmsg("section crossing memory boundary!\n");
		return -1;
	}

	if ((devfd = open(DEVNAME_DSPMEM, O_RDWR)) < 0) {
		prmsg("Can't open %s\n", DEVNAME_DSPMEM);
		return -1;
	}
	lseek(devfd, addr, SEEK_SET);
	if ((dbuf = malloc(size)) == NULL) {
		prmsg("Can't alloc memory at %s line %d\n", __FILE__, __LINE__);
		close(devfd);
		return -1;
	}
	read(devfd, dbuf, size);
	close(devfd);

	p = dbuf;

#endif

	e1->event = DLD_EVENT_STRING;

	cnt = 0;
	for (i = 0; i < size; i++, p++) {
		strsz = 256 - SERVER_EVENT_HDRSZ - cnt;
		if (i % 16 == 15) {
			cnt += snprintf(&e1->data.s[cnt], strsz, " %02x\n", *p);
			e1->len = SERVER_EVENT_HDRSZ + cnt;
			write(fd, e1, e1->len);
			cnt = 0;
		} else {
			cnt += snprintf(&e1->data.s[cnt], strsz, " %02x", *p);
		}
	}
	if (cnt > 0) {
		cnt += snprintf(&e1->data.s[cnt], strsz, " \n");
		e1->len = SERVER_EVENT_HDRSZ + cnt;
		write(fd, e1, e1->len);
	}

	e->event = DLD_EVENT_DONE;
	e->len = SERVER_EVENT_HDRSZ;
	write(fd, e, e->len);

#ifndef DSP_EMULATION
	free(dbuf);
#endif
	return 0;
}

#ifndef DSP_EMULATION
static int server_dsp_run(int fd)
{
	int cfd;

	if ((cfd = open(DEVNAME_CONTROL, O_RDWR)) < 0) {
		prmsg("%s open failed at %s line %d\n",
		      DEVNAME_CONTROL, __FILE__, __LINE__);
		sendback_event(fd, DLD_EVENT_ERROR);
		return 1;
	}
	prmsg("releasing DSP reset\n");
	ioctl(cfd, OMAP_DSP_IOCTL_RUN);
	close(cfd);

	sendback_event(fd, DLD_EVENT_DONE);
	return 0;
}

static int server_dsp_reset(int fd)
{
	int cfd;

	if ((cfd = open(DEVNAME_CONTROL, O_RDWR)) < 0) {
		prmsg("%s open failed at %s line %d\n",
		      DEVNAME_CONTROL, __FILE__, __LINE__);
		sendback_event(fd, DLD_EVENT_ERROR);
		return 1;
	}
	prmsg("DSP reset\n");
	ioctl(cfd, OMAP_DSP_IOCTL_RESET);
	close(cfd);

	sendback_event(fd, DLD_EVENT_DONE);
	return 0;
}

static int server_setrstvect(int fd, struct server_event *e)
{
	u32 addr = e->data.rstvect.addr;
	int cfd;

	if ((cfd = open(DEVNAME_CONTROL, O_RDWR)) < 0) {
		prmsg("%s open failed at %s line %d\n",
		      DEVNAME_CONTROL, __FILE__, __LINE__);
		sendback_event(fd, DLD_EVENT_ERROR);
		return 1;
	}
	prmsg("setting DSP reset vector to 0x%06lx\n", addr);
	ioctl(cfd, OMAP_DSP_IOCTL_SETRSTVECT, addr);
	close(cfd);

	sendback_event(fd, DLD_EVENT_DONE);
	return 0;
}

static int server_cpu_idle(int fd)
{
	int cfd;

	if ((cfd = open(DEVNAME_CONTROL, O_RDWR)) < 0) {
		prmsg("%s open failed at %s line %d\n",
		      DEVNAME_CONTROL, __FILE__, __LINE__);
		sendback_event(fd, DLD_EVENT_ERROR);
		return 1;
	}
	prmsg("setting DSP idle\n");
	ioctl(cfd, OMAP_DSP_IOCTL_CPU_IDLE);
	close(cfd);

	sendback_event(fd, DLD_EVENT_DONE);
	return 0;
}

static int server_dspconfig(int fd)
{
	int cfd;
	int n_task;

	if ((cfd = open(DEVNAME_CONTROL, O_RDWR)) < 0) {
		prmsg("%s open failed at %s line %d\n",
		      DEVNAME_CONTROL, __FILE__, __LINE__);
		sendback_event(fd, DLD_EVENT_ERROR);
		return 1;
	}
	prmsg("DSP configuration ...\n");
	if (ioctl(cfd, OMAP_DSP_IOCTL_DSPCFG) < 0) {
		prmsg("  failed");
		sendback_event(fd, DLD_EVENT_ERROR);
		close(cfd);
		return 1;
	}
	prmsg("  succeeded.");

	if ((n_task = ioctl(cfd, OMAP_DSP_IOCTL_TASKCNT)) < 0) {
		prmsg("TASKCNT failed at %s line %d\n",
		      DEVNAME_CONTROL, __FILE__, __LINE__);
		sendback_event(fd, DLD_EVENT_ERROR);
		close(cfd);
		return 1;
	}
	close(cfd);

	if (dev_mklink(n_task) < 0) {
		sendback_event(fd, DLD_EVENT_ERROR);
		return 1;
	}

	sendback_event(fd, DLD_EVENT_DONE);