[people/ms/u-boot.git] / examples / standalone / sched.c

/*
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include <common.h>
#include <exports.h>

/*
 * Author: Arun Dharankar <ADharankar@ATTBI.Com>
 *
 * A very simple thread/schedular model:
 *   - only one master thread, and no parent child relation maintained
 *   - parent thread cannot be stopped or deleted
 *   - no permissions or credentials
 *   - no elaborate safety checks
 *   - cooperative multi threading
 *   - Simple round-robin scheduleing with no priorities
 *   - no metering/statistics collection
 *
 * Basic idea of implementing this is to allow more than one tests to
 * execute "simultaneously".
 *
 * This may be modified such thread_yield may be called in syscalls, and
 * timer interrupts.
 */


#define MAX_THREADS 8

#define CTX_SIZE 512
#define STK_SIZE 8*1024

#define STATE_EMPTY 0
#define STATE_RUNNABLE 1
#define STATE_STOPPED 2
#define STATE_TERMINATED 2

#define MASTER_THREAD 0

#define RC_FAILURE	(-1)
#define	RC_SUCCESS	(0)

typedef	vu_char *jmp_ctx;
unsigned long setctxsp (vu_char *sp);
int ppc_setjmp(jmp_ctx env);
void ppc_longjmp(jmp_ctx env, int val);
#define setjmp	ppc_setjmp
#define longjmp	ppc_longjmp

struct lthread {
	int state;
	int retval;
	char stack[STK_SIZE];
	uchar context[CTX_SIZE];
	int (*func) (void *);
	void *arg;
};
static volatile struct lthread lthreads[MAX_THREADS];
static volatile int current_tid = MASTER_THREAD;


static uchar dbg = 0;

#define PDEBUG(fmt, args...)	 {					\
	if(dbg != 0) {							\
		printf("[%s %d %s]: ",__FILE__,__LINE__,__FUNCTION__);\
		printf(fmt, ##args);				\
		printf("\n");					\
	}								\
}

static int testthread (void *);
static void sched_init (void);
static int thread_create (int (*func) (void *), void *arg);
static int thread_start (int id);
static void thread_yield (void);
static int thread_delete (int id);
static int thread_join (int *ret);

#if 0							/* not used yet */
static int thread_stop (int id);
#endif							/* not used yet */

/* An example of schedular test */

#define NUMTHREADS 7
int sched (int ac, char *av[])
{
	int i, j;
	int tid[NUMTHREADS];
	int names[NUMTHREADS];

	app_startup(av);

	sched_init ();

	for (i = 0; i < NUMTHREADS; i++) {
		names[i] = i;
		j = thread_create (testthread, (void *) &names[i]);
		if (j == RC_FAILURE)
			printf ("schedtest: Failed to create thread %d\n", i);
		if (j > 0) {
			printf ("schedtest: Created thread with id %d, name %d\n",
						j, i);
			tid[i] = j;
		}
	}
	printf ("schedtest: Threads created\n");

	printf ("sched_test: function=0x%08x\n", (unsigned)testthread);
	for (i = 0; i < NUMTHREADS; i++) {
		printf ("schedtest: Setting thread %d runnable\n", tid[i]);
		thread_start (tid[i]);
		thread_yield ();
	}
	printf ("schedtest: Started %d threads\n", NUMTHREADS);

	while (1) {
		printf ("schedtest: Waiting for threads to complete\n");
		if (tstc () && getc () == 0x3) {
			printf ("schedtest: Aborting threads...\n");
			for (i = 0; i < NUMTHREADS; i++) {
				printf ("schedtest: Deleting thread %d\n", tid[i]);
				thread_delete (tid[i]);
			}
			return RC_SUCCESS;
		}
		j = -1;
		i = thread_join (&j);
		if (i == RC_FAILURE) {
			printf ("schedtest: No threads pending, "
						"exiting schedular test\n");
			return RC_SUCCESS;
		}
		printf ("schedtest: thread is %d returned %d\n", i, j);
		thread_yield ();
	}

	return RC_SUCCESS;
}

static int testthread (void *name)
{
	int i;

	printf ("testthread: Begin executing thread, myname %d, &i=0x%08x\n",
		*(int *) name, (unsigned)&i);

	printf ("Thread %02d, i=%d\n", *(int *) name, i);

	for (i = 0; i < 0xffff * (*(int *) name + 1); i++) {
		if (tstc () && getc () == 0x3) {
			printf ("testthread: myname %d terminating.\n",
						*(int *) name);
			return *(int *) name + 1;
		}

		if (i % 100 == 0)
			thread_yield ();
	}

	printf ("testthread: returning %d, i=0x%x\n",
				*(int *) name + 1, i);

	return *(int *) name + 1;
}


static void sched_init (void)
{
	int i;

	for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++)
		lthreads[i].state = STATE_EMPTY;

	current_tid = MASTER_THREAD;
	lthreads[current_tid].state = STATE_RUNNABLE;
	PDEBUG ("sched_init: master context = 0x%08x",
		(unsigned)lthreads[current_tid].context);
	return;
}

static void thread_yield (void)
{
	static int i;

	PDEBUG ("thread_yield: current tid=%d", current_tid);

#define SWITCH(new)							\
	if(lthreads[new].state == STATE_RUNNABLE) {			\
		PDEBUG("thread_yield: %d match, ctx=0x%08x",		\
			new,						\
			(unsigned)lthreads[current_tid].context);	\
		if(setjmp(lthreads[current_tid].context) == 0) {	\
			current_tid = new;				\
			PDEBUG("thread_yield: tid %d returns 0",	\
				new);					\
			longjmp(lthreads[new].context, 1);		\
		} else {						\
			PDEBUG("thread_yield: tid %d returns 1",	\
				new);					\
			return;						\
		}							\
	}

	for (i = current_tid + 1; i < MAX_THREADS; i++) {
		SWITCH (i);
	}

	if (current_tid != 0) {
		for (i = 0; i <= current_tid; i++) {
			SWITCH (i);
		}
	}

	PDEBUG ("thread_yield: returning from thread_yield");
	return;
}

static int thread_create (int (*func) (void *), void *arg)
{
	int i;

	for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++) {
		if (lthreads[i].state == STATE_EMPTY) {
			lthreads[i].state = STATE_STOPPED;
			lthreads[i].func = func;
			lthreads[i].arg = arg;
			PDEBUG ("thread_create: returns new tid %d", i);
			return i;
		}
	}

	PDEBUG ("thread_create: returns failure");
	return RC_FAILURE;
}

static int thread_delete (int id)
{
	if (id <= MASTER_THREAD || id > MAX_THREADS)
		return RC_FAILURE;

	if (current_tid == id)
		return RC_FAILURE;

	lthreads[id].state = STATE_EMPTY;
	return RC_SUCCESS;
}

static void thread_launcher (void)
{
	PDEBUG ("thread_launcher: invoking func=0x%08x",
		   (unsigned)lthreads[current_tid].func);

	lthreads[current_tid].retval =
			lthreads[current_tid].func (lthreads[current_tid].arg);

	PDEBUG ("thread_launcher: tid %d terminated", current_tid);

	lthreads[current_tid].state = STATE_TERMINATED;
	thread_yield ();
	printf ("thread_launcher: should NEVER get here!\n");

	return;
}

static int thread_start (int id)
{
	PDEBUG ("thread_start: id=%d", id);
	if (id <= MASTER_THREAD || id > MAX_THREADS) {
		return RC_FAILURE;
	}

	if (lthreads[id].state != STATE_STOPPED)
		return RC_FAILURE;

	if (setjmp (lthreads[current_tid].context) == 0) {
		lthreads[id].state = STATE_RUNNABLE;
		current_tid = id;
		PDEBUG ("thread_start: to be stack=0%08x",
			(unsigned)lthreads[id].stack);
		setctxsp ((vu_char *)&lthreads[id].stack[STK_SIZE]);
		thread_launcher ();
	}

	PDEBUG ("thread_start: Thread id=%d started, parent returns", id);

	return RC_SUCCESS;
}

#if 0	/* not used so far */
static int thread_stop (int id)
{
	if (id <= MASTER_THREAD || id >= MAX_THREADS)
		return RC_FAILURE;

	if (current_tid == id)
		return RC_FAILURE;

	lthreads[id].state = STATE_STOPPED;
	return RC_SUCCESS;
}
#endif	/* not used so far */

static int thread_join (int *ret)
{
	int i, j = 0;

	PDEBUG ("thread_join: *ret = %d", *ret);

	if (!(*ret == -1 || *ret > MASTER_THREAD || *ret < MAX_THREADS)) {
		PDEBUG ("thread_join: invalid tid %d", *ret);
		return RC_FAILURE;
	}

	if (*ret == -1) {
		PDEBUG ("Checking for tid = -1");
		while (1) {
			/* PDEBUG("thread_join: start while-loopn"); */
			j = 0;
			for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++) {
				if (lthreads[i].state == STATE_TERMINATED) {
					*ret = lthreads[i].retval;
					lthreads[i].state = STATE_EMPTY;
					/* PDEBUG("thread_join: returning retval %d of tid %d",
					   ret, i); */
					return RC_SUCCESS;
				}

				if (lthreads[i].state != STATE_EMPTY) {
					PDEBUG ("thread_join: %d used slots tid %d state=%d",
						   j, i, lthreads[i].state);
					j++;
				}
			}
			if (j == 0) {
				PDEBUG ("thread_join: all slots empty!");
				return RC_FAILURE;
			}
			/*  PDEBUG("thread_join: yielding"); */
			thread_yield ();
			/*  PDEBUG("thread_join: back from yield"); */
		}
	}

	if (lthreads[*ret].state == STATE_TERMINATED) {
		i = *ret;
		*ret = lthreads[*ret].retval;
		lthreads[*ret].state = STATE_EMPTY;
		PDEBUG ("thread_join: returing %d for tid %d", *ret, i);
		return RC_SUCCESS;
	}

	PDEBUG ("thread_join: thread %d is not terminated!", *ret);
	return RC_FAILURE;
}
Commit	Line	Data
85ec0bcc	1	/*
1a459660	2	* SPDX-License-Identifier: GPL-2.0+
85ec0bcc WD	3	*/
	4
	5	#include <common.h>
27b207fd	6	#include <exports.h>
85ec0bcc WD	7
	8	/*
	9	* Author: Arun Dharankar <ADharankar@ATTBI.Com>
	10	*
	11	* A very simple thread/schedular model:
	12	* - only one master thread, and no parent child relation maintained
	13	* - parent thread cannot be stopped or deleted
	14	* - no permissions or credentials
	15	* - no elaborate safety checks
	16	* - cooperative multi threading
	17	* - Simple round-robin scheduleing with no priorities
	18	* - no metering/statistics collection
	19	*
	20	* Basic idea of implementing this is to allow more than one tests to
	21	* execute "simultaneously".
	22	*
	23	* This may be modified such thread_yield may be called in syscalls, and
	24	* timer interrupts.
	25	*/
	26
	27
	28	#define MAX_THREADS 8
	29
	30	#define CTX_SIZE 512
	31	#define STK_SIZE 8*1024
	32
	33	#define STATE_EMPTY 0
	34	#define STATE_RUNNABLE 1
	35	#define STATE_STOPPED 2
	36	#define STATE_TERMINATED 2
	37
	38	#define MASTER_THREAD 0
	39
	40	#define RC_FAILURE (-1)
	41	#define RC_SUCCESS (0)
	42
3e38691e WD	43	typedef vu_char *jmp_ctx;
	44	unsigned long setctxsp (vu_char *sp);
	45	int ppc_setjmp(jmp_ctx env);
	46	void ppc_longjmp(jmp_ctx env, int val);
	47	#define setjmp ppc_setjmp
	48	#define longjmp ppc_longjmp
	49
85ec0bcc WD	50	struct lthread {
	51	int state;
	52	int retval;
	53	char stack[STK_SIZE];
	54	uchar context[CTX_SIZE];
	55	int (func) (void );
	56	void *arg;
	57	};
	58	static volatile struct lthread lthreads[MAX_THREADS];
	59	static volatile int current_tid = MASTER_THREAD;
	60
	61
	62	static uchar dbg = 0;
	63
3e38691e WD	64	#define PDEBUG(fmt, args...) { \
3e38691e WD	65	if(dbg != 0) { \
27b207fd WD	66	printf("[%s %d %s]: ",__FILE__,__LINE__,__FUNCTION__);\
	67	printf(fmt, ##args); \
	68	printf("\n"); \
3e38691e WD	69	} \
3e38691e WD	70	}
85ec0bcc WD	71
	72	static int testthread (void *);
	73	static void sched_init (void);
	74	static int thread_create (int (func) (void ), void *arg);
	75	static int thread_start (int id);
	76	static void thread_yield (void);
	77	static int thread_delete (int id);
	78	static int thread_join (int *ret);
3e38691e WD	79
3e38691e WD	80	#if 0 /* not used yet */
85ec0bcc	81	static int thread_stop (int id);
3e38691e	82	#endif /* not used yet */
85ec0bcc WD	83
	84	/* An example of schedular test */
	85
	86	#define NUMTHREADS 7
27b207fd	87	int sched (int ac, char *av[])
85ec0bcc WD	88	{
	89	int i, j;
	90	int tid[NUMTHREADS];
	91	int names[NUMTHREADS];
	92
27b207fd WD	93	app_startup(av);
27b207fd WD	94
85ec0bcc WD	95	sched_init ();
	96
	97	for (i = 0; i < NUMTHREADS; i++) {
	98	names[i] = i;
	99	j = thread_create (testthread, (void *) &names[i]);
	100	if (j == RC_FAILURE)
27b207fd	101	printf ("schedtest: Failed to create thread %d\n", i);
85ec0bcc	102	if (j > 0) {
27b207fd	103	printf ("schedtest: Created thread with id %d, name %d\n",
3e38691e	104	j, i);
85ec0bcc WD	105	tid[i] = j;
	106	}
	107	}
27b207fd	108	printf ("schedtest: Threads created\n");
85ec0bcc	109
27b207fd	110	printf ("sched_test: function=0x%08x\n", (unsigned)testthread);
85ec0bcc	111	for (i = 0; i < NUMTHREADS; i++) {
27b207fd	112	printf ("schedtest: Setting thread %d runnable\n", tid[i]);
85ec0bcc WD	113	thread_start (tid[i]);
	114	thread_yield ();
	115	}
27b207fd	116	printf ("schedtest: Started %d threads\n", NUMTHREADS);
85ec0bcc WD	117
85ec0bcc WD	118	while (1) {
27b207fd WD	119	printf ("schedtest: Waiting for threads to complete\n");
	120	if (tstc () && getc () == 0x3) {
	121	printf ("schedtest: Aborting threads...\n");
85ec0bcc	122	for (i = 0; i < NUMTHREADS; i++) {
27b207fd	123	printf ("schedtest: Deleting thread %d\n", tid[i]);
85ec0bcc WD	124	thread_delete (tid[i]);
	125	}
	126	return RC_SUCCESS;
	127	}
	128	j = -1;
	129	i = thread_join (&j);
	130	if (i == RC_FAILURE) {
27b207fd	131	printf ("schedtest: No threads pending, "
3e38691e	132	"exiting schedular test\n");
85ec0bcc WD	133	return RC_SUCCESS;
85ec0bcc WD	134	}
27b207fd	135	printf ("schedtest: thread is %d returned %d\n", i, j);
85ec0bcc WD	136	thread_yield ();
	137	}
	138
	139	return RC_SUCCESS;
	140	}
	141
	142	static int testthread (void *name)
	143	{
	144	int i;
	145
27b207fd WD	146	printf ("testthread: Begin executing thread, myname %d, &i=0x%08x\n",
27b207fd WD	147	(int ) name, (unsigned)&i);
85ec0bcc	148
27b207fd	149	printf ("Thread %02d, i=%d\n", (int ) name, i);
85ec0bcc WD	150
85ec0bcc WD	151	for (i = 0; i < 0xffff * ((int ) name + 1); i++) {
27b207fd WD	152	if (tstc () && getc () == 0x3) {
27b207fd WD	153	printf ("testthread: myname %d terminating.\n",
3e38691e	154	(int ) name);
85ec0bcc WD	155	return (int ) name + 1;
	156	}
	157
	158	if (i % 100 == 0)
	159	thread_yield ();
	160	}
	161
27b207fd	162	printf ("testthread: returning %d, i=0x%x\n",
3e38691e	163	(int ) name + 1, i);
85ec0bcc WD	164
	165	return (int ) name + 1;
	166	}
	167
	168
	169	static void sched_init (void)
	170	{
	171	int i;
	172
	173	for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++)
	174	lthreads[i].state = STATE_EMPTY;
	175
	176	current_tid = MASTER_THREAD;
	177	lthreads[current_tid].state = STATE_RUNNABLE;
3e38691e	178	PDEBUG ("sched_init: master context = 0x%08x",
27b207fd	179	(unsigned)lthreads[current_tid].context);
85ec0bcc WD	180	return;
	181	}
	182
	183	static void thread_yield (void)
	184	{
	185	static int i;
	186
3e38691e	187	PDEBUG ("thread_yield: current tid=%d", current_tid);
85ec0bcc	188
53677ef1	189	#define SWITCH(new) \
85ec0bcc	190	if(lthreads[new].state == STATE_RUNNABLE) { \
3e38691e	191	PDEBUG("thread_yield: %d match, ctx=0x%08x", \
27b207fd WD	192	new, \
27b207fd WD	193	(unsigned)lthreads[current_tid].context); \
85ec0bcc WD	194	if(setjmp(lthreads[current_tid].context) == 0) { \
85ec0bcc WD	195	current_tid = new; \
3e38691e	196	PDEBUG("thread_yield: tid %d returns 0", \
53677ef1	197	new); \
85ec0bcc WD	198	longjmp(lthreads[new].context, 1); \
85ec0bcc WD	199	} else { \
3e38691e	200	PDEBUG("thread_yield: tid %d returns 1", \
53677ef1	201	new); \
85ec0bcc WD	202	return; \
	203	} \
	204	}
	205
	206	for (i = current_tid + 1; i < MAX_THREADS; i++) {
	207	SWITCH (i);
	208	}
	209
	210	if (current_tid != 0) {
	211	for (i = 0; i <= current_tid; i++) {
	212	SWITCH (i);
	213	}
	214	}
	215
3e38691e	216	PDEBUG ("thread_yield: returning from thread_yield");
85ec0bcc WD	217	return;
	218	}
	219
	220	static int thread_create (int (func) (void ), void *arg)
	221	{
	222	int i;
	223
	224	for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++) {
	225	if (lthreads[i].state == STATE_EMPTY) {
	226	lthreads[i].state = STATE_STOPPED;
	227	lthreads[i].func = func;
	228	lthreads[i].arg = arg;
3e38691e	229	PDEBUG ("thread_create: returns new tid %d", i);
85ec0bcc WD	230	return i;
	231	}
	232	}
	233
3e38691e	234	PDEBUG ("thread_create: returns failure");
85ec0bcc WD	235	return RC_FAILURE;
	236	}
	237
	238	static int thread_delete (int id)
	239	{
	240	if (id <= MASTER_THREAD \|\| id > MAX_THREADS)
	241	return RC_FAILURE;
	242
	243	if (current_tid == id)
	244	return RC_FAILURE;
	245
	246	lthreads[id].state = STATE_EMPTY;
	247	return RC_SUCCESS;
	248	}
	249
	250	static void thread_launcher (void)
	251	{
3e38691e	252	PDEBUG ("thread_launcher: invoking func=0x%08x",
27b207fd	253	(unsigned)lthreads[current_tid].func);
85ec0bcc WD	254
85ec0bcc WD	255	lthreads[current_tid].retval =
3e38691e	256	lthreads[current_tid].func (lthreads[current_tid].arg);
85ec0bcc	257
3e38691e	258	PDEBUG ("thread_launcher: tid %d terminated", current_tid);
85ec0bcc WD	259
	260	lthreads[current_tid].state = STATE_TERMINATED;
	261	thread_yield ();
27b207fd	262	printf ("thread_launcher: should NEVER get here!\n");
85ec0bcc WD	263
	264	return;
	265	}
	266
	267	static int thread_start (int id)
	268	{
3e38691e	269	PDEBUG ("thread_start: id=%d", id);
85ec0bcc WD	270	if (id <= MASTER_THREAD \|\| id > MAX_THREADS) {
	271	return RC_FAILURE;
	272	}
	273
	274	if (lthreads[id].state != STATE_STOPPED)
	275	return RC_FAILURE;
	276
	277	if (setjmp (lthreads[current_tid].context) == 0) {
	278	lthreads[id].state = STATE_RUNNABLE;
	279	current_tid = id;
27b207fd WD	280	PDEBUG ("thread_start: to be stack=0%08x",
27b207fd WD	281	(unsigned)lthreads[id].stack);
77ddac94	282	setctxsp ((vu_char *)&lthreads[id].stack[STK_SIZE]);
85ec0bcc WD	283	thread_launcher ();
	284	}
	285
3e38691e	286	PDEBUG ("thread_start: Thread id=%d started, parent returns", id);
85ec0bcc WD	287
	288	return RC_SUCCESS;
	289	}
	290
27b207fd	291	#if 0 /* not used so far */
85ec0bcc WD	292	static int thread_stop (int id)
	293	{
	294	if (id <= MASTER_THREAD \|\| id >= MAX_THREADS)
	295	return RC_FAILURE;
	296
	297	if (current_tid == id)
	298	return RC_FAILURE;
	299
	300	lthreads[id].state = STATE_STOPPED;
	301	return RC_SUCCESS;
	302	}
27b207fd	303	#endif /* not used so far */
85ec0bcc WD	304
	305	static int thread_join (int *ret)
	306	{
	307	int i, j = 0;
	308
3e38691e	309	PDEBUG ("thread_join: ret = %d", ret);
85ec0bcc WD	310
85ec0bcc WD	311	if (!(ret == -1 \|\| ret > MASTER_THREAD \|\| *ret < MAX_THREADS)) {
3e38691e	312	PDEBUG ("thread_join: invalid tid %d", *ret);
85ec0bcc WD	313	return RC_FAILURE;
	314	}
	315
	316	if (*ret == -1) {
3e38691e	317	PDEBUG ("Checking for tid = -1");
85ec0bcc	318	while (1) {
3e38691e	319	/* PDEBUG("thread_join: start while-loopn"); */
85ec0bcc WD	320	j = 0;
	321	for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++) {
	322	if (lthreads[i].state == STATE_TERMINATED) {
	323	*ret = lthreads[i].retval;
	324	lthreads[i].state = STATE_EMPTY;
3e38691e WD	325	/* PDEBUG("thread_join: returning retval %d of tid %d",
3e38691e WD	326	ret, i); */
85ec0bcc WD	327	return RC_SUCCESS;
	328	}
	329
	330	if (lthreads[i].state != STATE_EMPTY) {
3e38691e WD	331	PDEBUG ("thread_join: %d used slots tid %d state=%d",
3e38691e WD	332	j, i, lthreads[i].state);
85ec0bcc WD	333	j++;
	334	}
	335	}
	336	if (j == 0) {
3e38691e	337	PDEBUG ("thread_join: all slots empty!");
85ec0bcc WD	338	return RC_FAILURE;
85ec0bcc WD	339	}
3e38691e	340	/* PDEBUG("thread_join: yielding"); */
85ec0bcc	341	thread_yield ();
3e38691e	342	/* PDEBUG("thread_join: back from yield"); */
85ec0bcc WD	343	}
	344	}
	345
	346	if (lthreads[*ret].state == STATE_TERMINATED) {
	347	i = *ret;
	348	ret = lthreads[ret].retval;
	349	lthreads[*ret].state = STATE_EMPTY;
3e38691e	350	PDEBUG ("thread_join: returing %d for tid %d", *ret, i);
85ec0bcc WD	351	return RC_SUCCESS;
	352	}
	353
3e38691e	354	PDEBUG ("thread_join: thread %d is not terminated!", *ret);
85ec0bcc WD	355	return RC_FAILURE;
85ec0bcc WD	356	}