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