Subversion Repositories thor

// ============================================================================
//        __
//   \\__/ o\    (C) 2012-2016  Robert Finch, Stratford
//    \  __ /    All rights reserved.
//     \/_//     robfinch<remove>@finitron.ca
//       ||
//
//
// This source file 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 3 of the License, or     
// (at your option) any later version.                                      
//                                                                          
// This source file 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, see <http://www.gnu.org/licenses/>.    
//                                                                          
// ============================================================================
//
#include "config.h"
#include "const.h"
#include "types.h"
#include "proto.h"
#include "glo.h"
 
extern int shell();
extern void KeybdIRQ();
extern pascal int putstr(char *, int);
extern int printf(char *, ...);
void panic(char *msg);
extern int FMTK_FreeMbx(hMBX);
 
int irq_stack[512];
int sp_tmp;
int FMTK_Inited;
JCB jcbs[NR_JCB];
extern TCB tcbs[NR_TCB];
extern hTCB readyQ[8];
int sysstack[1024];
int stacks[NR_TCB][1024];
int sys_stacks[NR_TCB][512];
int bios_stacks[NR_TCB][512];
int fmtk_irq_stack[512];
int fmtk_sys_stack[512];
MBX mailbox[NR_MBX];
MSG message[NR_MSG];
int nMsgBlk;
int nMailbox;
hJCB freeJCB;
hMSG freeMSG;
hMBX freeMBX;
JCB *IOFocusNdx;
int IOFocusTbl[4];
int iof_switch;
int BIOS1_sema;
int iof_sema;
int sys_sema;
int BIOS_RespMbx;
char hasUltraHighPriorityTasks;
int missed_ticks;
 
short int video_bufs[NR_JCB][4096];
extern hTCB TimeoutList;
 
extern void gfx_demo();
 
// This table needed in case we want to call the OS routines directly.
// It is also used by the system call interrupt as a vector table.
 
naked void FMTK_BrTbl()
{
      asm {
		  align 4
dh	FMTKInitialize_
dh	FMTK_StartTask_
dh	FMTK_ExitTask_
dh	FMTK_KillTask_
dh	FMTK_SetTaskPriority_
dh	FMTK_Sleep_
dh	FMTK_AllocMbx_
dh	FMTK_FreeMbx_
dh	FMTK_PostMsg_
dh	FMTK_SendMsg_
dh	FMTK_WaitMsg_
dh	FMTK_CheckMsg_
      }
}
 
naked int GetVecno()
{
    asm {
        mfspr  r1,12
        rts
    }
}
 
naked void DisplayIRQLive()
{
     asm {
         lh       r1,hs:328
         addui    r1,r1,#1
         sh       r1,hs:328
     }
}
 
JCB *GetJCBPtr()
{
    return &jcbs[tcbs[GetRunningTCB()].hJob];
}
 
 
// ----------------------------------------------------------------------------
// Select a task to run.
// ----------------------------------------------------------------------------
 
private const __int8 startQ[32] = { 0, 0, 0, 1, 0, 0, 0, 2, 0, 0, 0, 3, 0, 1, 0, 4, 0, 0, 0, 5, 0, 0, 0, 6, 0, 1, 0, 7, 0, 0, 0, 0 };
private __int8 startQNdx;
 
private hTCB SelectTaskToRun()
{
	int nn,kk;
	TCB *p, *q;
	int qToCheck;
    hTCB h;
 
	startQNdx++;
	startQNdx &= 31;
	qToCheck = startQ[startQNdx];
	qToCheck &= 7;
	for (nn = 0; nn < 8; nn++) {
		h = readyQ[qToCheck];
		if (h >= 0 && h < NR_TCB) {
    		p = &tcbs[h];
            kk = 0;
            // Can run the head of a lower Q level if it's not the running
            // task, otherwise look to the next task.
            if (h != GetRunningTCB())
           		q = p;
    		else
           		q = &tcbs[p->next];
            do {  
                if (!(q->status & TS_RUNNING)) {
                    if (q->affinity == getCPU()) {
        			   readyQ[qToCheck] = q - tcbs;
        			   return q - tcbs;
                    }
                }
                q = &tcbs[q->next];
                kk = kk + 1;
            } while (q != p && kk < NR_TCB);
        }
		qToCheck++;
		qToCheck &= 7;
	}
	return GetRunningTCB();
	panic("No entries in ready queue.");
}
 
// ----------------------------------------------------------------------------
// There isn't any 'C' code in the SystemCall() function. If there were it
// would have to be arranged like the TimerIRQ() or RescheduleIRQ() functions.
// ----------------------------------------------------------------------------
 
naked FMTK_SystemCall()
{
	asm {
		sync
		sw		r2,TCB_r2[tr]
		sw		r3,TCB_r3[tr]
		sw		r4,TCB_r4[tr]
		sw		r5,TCB_r5[tr]
		sw		r6,TCB_r6[tr]
		sw		r7,TCB_r7[tr]
		sw		r8,TCB_r8[tr]
		sw		r9,TCB_r9[tr]
		sw		r10,TCB_r10[tr]
		sw		r11,TCB_r11[tr]
		sw		r12,TCB_r12[tr]
		sw		r13,TCB_r13[tr]
		sw		r14,TCB_r14[tr]
		sw		r15,TCB_r15[tr]
		sw		r16,TCB_r16[tr]
		sw		r17,TCB_r17[tr]
		sw		r18,TCB_r18[tr]
		sw		r19,TCB_r19[tr]
		sw		r20,TCB_r20[tr]
		sw		r21,TCB_r21[tr]
		sw		r22,TCB_r22[tr]
		sw		r23,TCB_r23[tr]
		sw		r24,TCB_r24[tr]
		sw		r25,TCB_r25[tr]
		sw		r26,TCB_r26[tr]
		sw		r27,TCB_r27[tr]
		sws		ds,TCB_ds[tr]
		sws		es,TCB_es[tr]
		sws		fs,TCB_fs[tr]
		sws		gs,TCB_gs[tr]
		sws		hs,TCB_hs[tr]
		sws		ss,TCB_ss[tr]
		sws		cs,TCB_cs[tr]
		sws		ds.lmt,TCB_dslmt[tr]
		sws		es.lmt,TCB_eslmt[tr]
		sws		fs.lmt,TCB_fslmt[tr]
		sws		gs.lmt,TCB_gslmt[tr]
		sws		hs.lmt,TCB_hslmt[tr]
		sws		ss.lmt,TCB_sslmt[tr]
		sws		cs.lmt,TCB_cslmt[tr]
		sws		c1,TCB_c1[tr]
		sws		c2,TCB_c2[tr]
		sws		c3,TCB_c3[tr]
		sws		c4,TCB_c4[tr]
		sws		c5,TCB_c5[tr]
		sws		c6,TCB_c6[tr]
		sws		c7,TCB_c7[tr]
		sws		c8,TCB_c8[tr]
		sws		c9,TCB_c9[tr]
		sws		c10,TCB_c10[tr]
		sws		c11,TCB_c11[tr]
		sws		c13,TCB_c13[tr]
		sws		c14,TCB_c14[tr]
		sws		pregs,TCB_pregs[tr]
	}
    asm {
		ldi		sp,#sysstack_+1023*8	; switch to system stack
		mtspr	ss,r0
		ldi		ss.lmt,#-1
		mtspr	ds,r0			; and data segment
		ldi		ds.lmt,#-1
		mfspr	r6,c13			; r6 = return address
    	lbu	    r7,[r6]			; r7 = static call number parameter
		addui	r6,r6,#1
		mtspr	c13,r6
		sws		c13,TCB_c13[tr]	; save c13 in task image
    	cmpi	p0,r7,#20
p0.gtu  ldi     r1,#E_BadFuncno
p0.gtu	br		.0001
		addui	sp,sp,#-40
		sw		r1,[sp]
		sw		r2,8[sp]
		sw		r3,16[sp]
		sw		r4,24[sp]
		sw		r5,32[sp]
		jhi		c1,cs:FMTK_BrTbl_[r7]	; do the system function
.0001:
		; leave r1 as system call result
		lw		r2,TCB_r2[tr]
		lw		r3,TCB_r3[tr]
		lw		r4,TCB_r4[tr]
		lw		r5,TCB_r5[tr]
		lw		r6,TCB_r6[tr]
		lw		r7,TCB_r7[tr]
		lw		r8,TCB_r8[tr]
		lw		r9,TCB_r9[tr]
		lw		r10,TCB_r10[tr]
		lw		r11,TCB_r11[tr]
		lw		r12,TCB_r12[tr]
		lw		r13,TCB_r13[tr]
		lw		r14,TCB_r14[tr]
		lw		r15,TCB_r15[tr]
		lw		r16,TCB_r16[tr]
		lw		r17,TCB_r17[tr]
		lw		r18,TCB_r18[tr]
		lw		r19,TCB_r19[tr]
		lw		r20,TCB_r20[tr]
		lw		r21,TCB_r21[tr]
		lw		r22,TCB_r22[tr]
		lw		r23,TCB_r23[tr]
		lw		r24,TCB_r24[tr]
		lw		r25,TCB_r25[tr]
		lw		r26,TCB_r26[tr]
		lw		r27,TCB_r27[tr]
		lws		ds,TCB_ds[tr]
		lws		es,TCB_es[tr]
		lws		fs,TCB_fs[tr]
		lws		gs,TCB_gs[tr]
		lws		hs,TCB_hs[tr]
		lws		ss,TCB_ss[tr]
		lws		cs,TCB_cs[tr]
		lws		ds.lmt,TCB_dslmt[tr]
		lws		es.lmt,TCB_eslmt[tr]
		lws		fs.lmt,TCB_fslmt[tr]
		lws		gs.lmt,TCB_gslmt[tr]
		lws		hs.lmt,TCB_hslmt[tr]
		lws		ss.lmt,TCB_sslmt[tr]
		lws		cs.lmt,TCB_cslmt[tr]
		lws		c1,TCB_c1[tr]
		lws		c2,TCB_c2[tr]
		lws		c3,TCB_c3[tr]
		lws		c4,TCB_c4[tr]
		lws		c5,TCB_c5[tr]
		lws		c6,TCB_c6[tr]
		lws		c7,TCB_c7[tr]
		lws		c8,TCB_c8[tr]
		lws		c9,TCB_c9[tr]
		lws		c10,TCB_c10[tr]
		lws		c11,TCB_c11[tr]
		lws		c13,TCB_c13[tr]
		lws		c14,TCB_c14[tr]
		lws		pregs,TCB_pregs[tr]
		sync
		rte
    }
}
 
// ----------------------------------------------------------------------------
// If timer interrupts are enabled during a priority #0 task, this routine
// only updates the missed ticks and remains in the same task. No timeouts
// are updated and no task switches will occur. The timer tick routine
// basically has a fixed latency when priority #0 is present.
// ----------------------------------------------------------------------------
 
void FMTK_SchedulerIRQ()
{
	TCB *t;
 
	prolog asm {
		sync
		sw		r1,TCB_ir1[tr]
		sw		r2,TCB_ir2[tr]
		sw		r3,TCB_ir3[tr]
		sw		r4,TCB_ir4[tr]
		sw		r5,TCB_ir5[tr]
		sw		r6,TCB_ir6[tr]
		sw		r7,TCB_ir7[tr]
		sw		r8,TCB_ir8[tr]
		sw		r9,TCB_ir9[tr]
		sw		r10,TCB_ir10[tr]
		sw		r11,TCB_ir11[tr]
		sw		r12,TCB_ir12[tr]
		sw		r13,TCB_ir13[tr]
		sw		r14,TCB_ir14[tr]
		sw		r15,TCB_ir15[tr]
		sw		r16,TCB_ir16[tr]
		sw		r17,TCB_ir17[tr]
		sw		r18,TCB_ir18[tr]
		sw		r19,TCB_ir19[tr]
		sw		r20,TCB_ir20[tr]
		sw		r21,TCB_ir21[tr]
		sw		r22,TCB_ir22[tr]
		sw		r23,TCB_ir23[tr]
		sw		r24,TCB_ir24[tr]
		sw		r25,TCB_ir25[tr]
		sw		r26,TCB_ir26[tr]
		sw		r27,TCB_ir27[tr]
		sws		ds,TCB_ids[tr]
		sws		es,TCB_ies[tr]
		sws		fs,TCB_ifs[tr]
		sws		gs,TCB_igs[tr]
		sws		hs,TCB_ihs[tr]
		sws		ss,TCB_iss[tr]
		sws		cs,TCB_ics[tr]
		sws		ds.lmt,TCB_idslmt[tr]
		sws		es.lmt,TCB_ieslmt[tr]
		sws		fs.lmt,TCB_ifslmt[tr]
		sws		gs.lmt,TCB_igslmt[tr]
		sws		hs.lmt,TCB_ihslmt[tr]
		sws		ss.lmt,TCB_isslmt[tr]
		sws		cs.lmt,TCB_icslmt[tr]
		sws		c1,TCB_ic1[tr]
		sws		c2,TCB_ic2[tr]
		sws		c3,TCB_ic3[tr]
		sws		c4,TCB_ic4[tr]
		sws		c5,TCB_ic5[tr]
		sws		c6,TCB_ic6[tr]
		sws		c7,TCB_ic7[tr]
		sws		c8,TCB_ic8[tr]
		sws		c9,TCB_ic9[tr]
		sws		c10,TCB_ic10[tr]
		sws		c11,TCB_ic11[tr]
		sws		c13,TCB_ic13[tr]
		sws		c14,TCB_ic14[tr]
		sws		pregs,TCB_ipregs[tr]
 
		ldi		sp,#irq_stack_+511*8	; setup system stack and data segments
		mtspr	ss,r0
		ldis	ss.lmt,#-1
		mtspr	ds,r0
		ldis	ds.lmt,#-1
		ldi		hs,#$FFD00000
		ldi		hs.lmt,#$100000
		ldi		r1,#2					; reset edge sense circuit
		sh		r1,hs:PIC_ESR
	}
	DisplayIRQLive();
	if (LockSemaphore(&sys_sema,10)) {
		t = GetRunningTCBPtr();
		t->ticks = t->ticks + (t->endTick - t->startTick);
		if (t->priority != 000) {
			t->status = TS_PREEMPT;
			while (TimeoutList >= 0 && TimeoutList < NR_TCB) {
				if (tcbs[TimeoutList].timeout<=0)
					InsertIntoReadyList(PopTimeoutList());
				else {
					tcbs[TimeoutList].timeout = tcbs[TimeoutList].timeout - missed_ticks - 1;
					missed_ticks = 0;
					break;
				}
			}
			if (t->priority > 002)
			SetRunningTCB(SelectTaskToRun());
			GetRunningTCBPtr()->status = TS_RUNNING;
		}
		else {
			missed_ticks++;
		}
		UnlockSemaphore(&sys_sema);
	}
	else {
		missed_ticks++;
	}
	// If an exception was flagged (eg CTRL-C) return to the catch handler
	// not the interrupted code.
	t = GetRunningTCBPtr();
	if (t->exception) {
		t->icregs[1] = t->icregs[11];    // set link register to catch handler
		t->icregs[13] = t->icregs[11];   // and the PC register
		t->iregs[1] = t->exception;    // r1 = exception value
		t->exception = 0;
		t->iregs[2] = 24;              // r2 = exception type
	}
	epilog asm {
		lw		r1,TCB_ir1[tr]
		lw		r2,TCB_ir2[tr]
		lw		r3,TCB_ir3[tr]
		lw		r4,TCB_ir4[tr]
		lw		r5,TCB_ir5[tr]
		lw		r6,TCB_ir6[tr]
		lw		r7,TCB_ir7[tr]
		lw		r8,TCB_ir8[tr]
		lw		r9,TCB_ir9[tr]
		lw		r10,TCB_ir10[tr]
		lw		r11,TCB_ir11[tr]
		lw		r12,TCB_ir12[tr]
		lw		r13,TCB_ir13[tr]
		lw		r14,TCB_ir14[tr]
		lw		r15,TCB_ir15[tr]
		lw		r16,TCB_ir16[tr]
		lw		r17,TCB_ir17[tr]
		lw		r18,TCB_ir18[tr]
		lw		r19,TCB_ir19[tr]
		lw		r20,TCB_ir20[tr]
		lw		r21,TCB_ir21[tr]
		lw		r22,TCB_ir22[tr]
		lw		r23,TCB_ir23[tr]
		lw		r24,TCB_ir24[tr]
		lw		r25,TCB_ir25[tr]
		lw		r26,TCB_ir26[tr]
		lw		r27,TCB_ir27[tr]
		lws		ds,TCB_ids[tr]
		lws		es,TCB_ies[tr]
		lws		fs,TCB_ifs[tr]
		lws		gs,TCB_igs[tr]
		lws		hs,TCB_ihs[tr]
		lws		ss,TCB_iss[tr]
		lws		cs,TCB_ics[tr]
		lws		ds.lmt,TCB_idslmt[tr]
		lws		es.lmt,TCB_ieslmt[tr]
		lws		fs.lmt,TCB_ifslmt[tr]
		lws		gs.lmt,TCB_igslmt[tr]
		lws		hs.lmt,TCB_ihslmt[tr]
		lws		ss.lmt,TCB_isslmt[tr]
		lws		cs.lmt,TCB_icslmt[tr]
		lws		c1,TCB_ic1[tr]
		lws		c2,TCB_ic2[tr]
		lws		c3,TCB_ic3[tr]
		lws		c4,TCB_ic4[tr]
		lws		c5,TCB_ic5[tr]
		lws		c6,TCB_ic6[tr]
		lws		c7,TCB_ic7[tr]
		lws		c8,TCB_ic8[tr]
		lws		c9,TCB_ic9[tr]
		lws		c10,TCB_ic10[tr]
		lws		c11,TCB_ic11[tr]
		lws		c13,TCB_ic13[tr]
		lws		c14,TCB_ic14[tr]
		lws		pregs,TCB_ipregs[tr]
		sync
		rti
	}
}
 
void FMTK_SchedulerIRQ2()
{
     TCB *t;
 
	prolog asm {
		sync
		sw		r1,TCB_ir1[tr]
		sw		r2,TCB_ir2[tr]
		sw		r3,TCB_ir3[tr]
		sw		r4,TCB_ir4[tr]
		sw		r5,TCB_ir5[tr]
		sw		r6,TCB_ir6[tr]
		sw		r7,TCB_ir7[tr]
		sw		r8,TCB_ir8[tr]
		sw		r9,TCB_ir9[tr]
		sw		r10,TCB_ir10[tr]
		sw		r11,TCB_ir11[tr]
		sw		r12,TCB_ir12[tr]
		sw		r13,TCB_ir13[tr]
		sw		r14,TCB_ir14[tr]
		sw		r15,TCB_ir15[tr]
		sw		r16,TCB_ir16[tr]
		sw		r17,TCB_ir17[tr]
		sw		r18,TCB_ir18[tr]
		sw		r19,TCB_ir19[tr]
		sw		r20,TCB_ir20[tr]
		sw		r21,TCB_ir21[tr]
		sw		r22,TCB_ir22[tr]
		sw		r23,TCB_ir23[tr]
		sw		r24,TCB_ir24[tr]
		sw		r25,TCB_ir25[tr]
		sw		r26,TCB_ir26[tr]
		sw		r27,TCB_ir27[tr]
		sws		ds,TCB_ids[tr]
		sws		es,TCB_ies[tr]
		sws		fs,TCB_ifs[tr]
		sws		gs,TCB_igs[tr]
		sws		hs,TCB_ihs[tr]
		sws		ss,TCB_iss[tr]
		sws		cs,TCB_ics[tr]
		sws		ds.lmt,TCB_idslmt[tr]
		sws		es.lmt,TCB_ieslmt[tr]
		sws		fs.lmt,TCB_ifslmt[tr]
		sws		gs.lmt,TCB_igslmt[tr]
		sws		hs.lmt,TCB_ihslmt[tr]
		sws		ss.lmt,TCB_isslmt[tr]
		sws		cs.lmt,TCB_icslmt[tr]
		sws		c1,TCB_ic1[tr]
		sws		c2,TCB_ic2[tr]
		sws		c3,TCB_ic3[tr]
		sws		c4,TCB_ic4[tr]
		sws		c5,TCB_ic5[tr]
		sws		c6,TCB_ic6[tr]
		sws		c7,TCB_ic7[tr]
		sws		c8,TCB_ic8[tr]
		sws		c9,TCB_ic9[tr]
		sws		c10,TCB_ic10[tr]
		sws		c11,TCB_ic11[tr]
		sws		c13,TCB_ic13[tr]
		sws		c14,TCB_ic14[tr]
		sws		pregs,TCB_ipregs[tr]
 
		ldi		sp,#irq_stack_+511*8	; setup system stack and data segments
		mtspr	ss,r0
		ldis	ss.lmt,#-1
		mtspr	ds,r0
		ldis	ds.lmt,#-1
		ldi		hs,#$FFD00000
		ldi		hs.lmt,#$100000
		ldi		r1,#2					; reset edge sense circuit
		sh		r1,hs:PIC_ESR
	}
	 t->icregs[14] += 4;		// update return address
     // Explicit rescheduling request.
     t = GetRunningTCBPtr();
     t->ticks = t->ticks + (t->endTick - t->startTick);
     t->status = TS_GIVEAWAY;
     //t->iipc = t->iipc + 4;  // advance the return address
     SetRunningTCB(SelectTaskToRun());
     t = GetRunningTCBPtr();
     t->status = TS_RUNNING;
     // If an exception was flagged (eg CTRL-C) return to the catch handler
     // not the interrupted code.
     if (t->exception) {
         t->cregs[1] = t->cregs[11];   // set link register to catch handler
         t->cregs[13] = t->cregs[11];  // and the PC register
         t->regs[1] = t->exception;    // r1 = exception value
         t->exception = 0;
         t->regs[2] = 24;              // r2 = exception type
     }
     // Restore the processor registers and return using an RTE.
	epilog asm {
		lw		r1,TCB_ir1[tr]
		lw		r2,TCB_ir2[tr]
		lw		r3,TCB_ir3[tr]
		lw		r4,TCB_ir4[tr]
		lw		r5,TCB_ir5[tr]
		lw		r6,TCB_ir6[tr]
		lw		r7,TCB_ir7[tr]
		lw		r8,TCB_ir8[tr]
		lw		r9,TCB_ir9[tr]
		lw		r10,TCB_ir10[tr]
		lw		r11,TCB_ir11[tr]
		lw		r12,TCB_ir12[tr]
		lw		r13,TCB_ir13[tr]
		lw		r14,TCB_ir14[tr]
		lw		r15,TCB_ir15[tr]
		lw		r16,TCB_ir16[tr]
		lw		r17,TCB_ir17[tr]
		lw		r18,TCB_ir18[tr]
		lw		r19,TCB_ir19[tr]
		lw		r20,TCB_ir20[tr]
		lw		r21,TCB_ir21[tr]
		lw		r22,TCB_ir22[tr]
		lw		r23,TCB_ir23[tr]
		lw		r24,TCB_ir24[tr]
		lw		r25,TCB_ir25[tr]
		lw		r26,TCB_ir26[tr]
		lw		r27,TCB_ir27[tr]
		lws		ds,TCB_ids[tr]
		lws		es,TCB_ies[tr]
		lws		fs,TCB_ifs[tr]
		lws		gs,TCB_igs[tr]
		lws		hs,TCB_ihs[tr]
		lws		ss,TCB_iss[tr]
		lws		cs,TCB_ics[tr]
		lws		ds.lmt,TCB_idslmt[tr]
		lws		es.lmt,TCB_ieslmt[tr]
		lws		fs.lmt,TCB_ifslmt[tr]
		lws		gs.lmt,TCB_igslmt[tr]
		lws		hs.lmt,TCB_ihslmt[tr]
		lws		ss.lmt,TCB_isslmt[tr]
		lws		cs.lmt,TCB_icslmt[tr]
		lws		c1,TCB_ic1[tr]
		lws		c2,TCB_ic2[tr]
		lws		c3,TCB_ic3[tr]
		lws		c4,TCB_ic4[tr]
		lws		c5,TCB_ic5[tr]
		lws		c6,TCB_ic6[tr]
		lws		c7,TCB_ic7[tr]
		lws		c8,TCB_ic8[tr]
		lws		c9,TCB_ic9[tr]
		lws		c10,TCB_ic10[tr]
		lws		c11,TCB_ic11[tr]
		lws		c13,TCB_ic13[tr]
		lws		c14,TCB_ic14[tr]
		lws		pregs,TCB_ipregs[tr]
		sync
		rti
	}
}
 
void panic(char *msg)
{
     putstr(msg,84);
j1:  goto j1;
}
 
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
 
void IdleTask()
{
     int ii;
     __int32 *screen = (__int32 *)0xFFD00000;
 
//     try {
j1:  ;
         forever {
             try {
                 ii++;
                 if (getCPU()==0)
					 outh(&screen[81],ii);
             }
             catch(static __exception ex=0) {
                 if (ex&0xFFFFFFFFL==515) {
                     printf("IdleTask: CTRL-C pressed.\r\n");
                 }
                 else
                     throw ex;
             }
         }
/*
     }
     catch (static __exception ex1=0) {
         printf("IdleTask: exception %d.\r\n", ex1);
         goto j1;
     }
*/
}
 
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
 
int FMTK_KillTask(int taskno)
{
    hTCB ht;
    int nn;
    JCB *j;
 
    check_privilege();
    ht = taskno;
    if (LockSemaphore(&sys_sema,-1)) {
        RemoveFromReadyList(ht);
        RemoveFromTimeoutList(ht);
        for (nn = 0; nn < 4; nn++)
            if (tcbs[ht].hMailboxes[nn] >= 0 && tcbs[ht].hMailboxes[nn] < NR_MBX) {
                FMTK_FreeMbx(tcbs[ht].hMailboxes[nn]);
                tcbs[ht].hMailboxes[nn] = -1;
            }
        // remove task from job's task list
        j = &jcbs[tcbs[ht].hJob];
        for (nn = 0; nn < 8; nn++) {
            if (j->tasks[nn]==ht)
                j->tasks[nn] = -1;
        }
        // If the job no longer has any tasks associated with it, it is 
        // finished.
        for (nn = 0; nn < 8; nn++)
            if (j->tasks[nn]!=-1)
                break;
        if (nn == 8) {
            j->next = freeJCB;
            freeJCB = j - jcbs;
        }
        UnlockSemaphore(&sys_sema);
    }
}
 
 
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
 
int FMTK_ExitTask()
{
    check_privilege();
    KillTask(GetRunningTCB());
    asm { 
		db	$01
		db	$A6		; INT
		db	$CE
		db	$02
	}     // reschedule
j1: goto j1;
}
 
 
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
 
int FMTK_StartTask(int priority, int affinity, int adr, int parm, hJCB job)
{
    hTCB ht;
    TCB *t;
    int nn;
 
	try {
        asm {
            ldi   r1,#66
            sc    r1,hs:LEDS
        }
    check_privilege();
        asm {
            ldi   r1,#60
            sc    r1,hs:LEDS
        }
    if (LockSemaphore(&sys_sema,-1)) {
        ht = freeTCB;
        freeTCB = tcbs[ht].next;
        UnlockSemaphore(&sys_sema);
    }
        asm {
            ldi   r1,#61
            sc    r1,hs:LEDS
        }
    t = &tcbs[ht];
    t->affinity = affinity;
    t->priority = priority;
    t->hJob = job;
    // Insert into the job's list of tasks.
    for (nn = 0; nn < 8; nn++) {
        if (jcbs[job].tasks[nn]<0) {
            jcbs[job].tasks[nn] = ht;
            break;
        }
    }
        asm {
            ldi   r1,#62
            sc    r1,hs:LEDS
        }
    if (nn == 8) {
        if (LockSemaphore(&sys_sema,-1)) {
            tcbs[ht].next = freeTCB;
            freeTCB = ht;
            UnlockSemaphore(&sys_sema);
        }
        return E_TooManyTasks;
    }
        asm {
            ldi   r1,#63
            sc    r1,hs:LEDS
        }
    t->iregs[1] = parm;
    t->icregs[1] = FMTK_ExitTask;
    t->iregs[27] = (unsigned int)t->stack + 1023*8;
    t->icregs[13] = adr;
    t->icregs[14] = adr;
//    t->icr0 = 0x140000000L;
    t->startTick = 0;
    t->endTick = 0;
    t->ticks = 0;
    t->exception = 0;
        asm {
            ldi   r1,#64
            sc    r1,hs:LEDS
        }
    if (LockSemaphore(&sys_sema,-1)) {
        InsertIntoReadyList(ht);
        UnlockSemaphore(&sys_sema);
    }
        asm {
            ldi   r1,#65
            sc    r1,hs:LEDS
        }
	}
	catch(static int ex)
		printf("StartTask error: %d\r\n", ex);
    return E_Ok;
}
 
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
 
int FMTK_Sleep(int timeout)
{
    hTCB ht;
 
    check_privilege();
    if (LockSemaphore(&sys_sema,-1)) {
        ht = GetRunningTCB();
        RemoveFromReadyList(ht);
        InsertIntoTimeoutList(ht, timeout);
        UnlockSemaphore(&sys_sema);
    }
    asm { 
		db	$01
		db	$A6		; INT
		db	$CE
		db	$02
	}     // reschedule
    return E_Ok;
}
 
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
 
int FMTK_SetTaskPriority(hTCB ht, int priority)
{
    TCB *t;
 
    check_privilege();
    if (priority > 077 || priority < 000)
       return E_Arg;
    if (LockSemaphore(&sys_sema,-1)) {
        t = &tcbs[ht];
        if (t->status & (TS_RUNNING | TS_READY)) {
            RemoveFromReadyList(ht);
            t->priority = priority;
            InsertIntoReadyList(ht);
        }
        else
            t->priority = priority;
        UnlockSemaphore(&sys_sema);
    }
    return E_Ok;
}
 
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
 
void FMTKInitialize()
{
	int nn,jj;
 
    check_privilege();
//    firstcall
    {
        asm {
            ldi   r1,#20
            sc    r1,hs:LEDS
        }
        hasUltraHighPriorityTasks = 0;
        missed_ticks = 0;
 
        IOFocusTbl[0] = 0;
        IOFocusNdx = null;
        iof_switch = 0;
 
    	SetRunningTCB(0);
		tcbs[0].hJob = 0;
        UnlockSemaphore(&sys_sema);
        UnlockSemaphore(&iof_sema);
        UnlockSemaphore(&kbd_sema);
 
        for (nn = 0; nn < NR_MSG; nn++) {
            message[nn].link = nn+1;
        }
        message[NR_MSG-1].link = -1;
        freeMSG = 0;
 
        asm {
            ldi   r1,#30
            sc    r1,hs:LEDS
        }
 
        for (nn = 0; nn < NR_JCB; nn++) {
            jcbs[nn].number = nn;
            for (jj = 0; jj < 8; jj++)
                jcbs[nn].tasks[jj] = -1;
            if (nn == 0 ) {
                jcbs[nn].pVidMem = 0xFFD00000;
                jcbs[nn].pVirtVidMem = &video_bufs[nn];
                jcbs[nn].NormAttr = 0x0026B800;
				asm {
					ldi   r1,#31
					sc    r1,hs:LEDS
				}
                RequestIOFocus(&jcbs[0]);
				asm {
					ldi   r1,#34
					sc    r1,hs:LEDS
				}
           }
            else {
                 jcbs[nn].pVidMem = &video_bufs[nn];
                 jcbs[nn].pVirtVidMem = &video_bufs[nn];
                 jcbs[nn].NormAttr = 0x0026B800;
            }
				asm {
					ldi   r1,#35
					sc    r1,hs:LEDS
				}
            jcbs[nn].VideoRows = 31;
            jcbs[nn].VideoCols = 84;
            jcbs[nn].CursorRow = 0;
            jcbs[nn].CursorCol = 0;
            jcbs[nn].KeybdHead = 0;
            jcbs[nn].KeybdTail = 0;
            jcbs[nn].KeyState1 = 0;
            jcbs[nn].KeyState2 = 0;
			jcbs[nn].KeybdBufSz = 64;
        }
 
		ClearScreen();
		HomeCursor();
		printf("FMTK Initializing");
 
        asm {
            ldi   r1,#40
            sc    r1,hs:LEDS
        }
 
    	for (nn = 0; nn < 8; nn++)
    		readyQ[nn] = -1;
    	for (nn = 0; nn < NR_TCB; nn++) {
            tcbs[nn].number = nn;
    		tcbs[nn].next = nn+1;
    		tcbs[nn].prev = -1;
        asm {
            ldi   r1,#41
            sc    r1,hs:LEDS
        }
 
    		tcbs[nn].status = 0;
    		tcbs[nn].priority = 070;
    		tcbs[nn].affinity = 0;
        asm {
            ldi   r1,#46
            sc    r1,hs:LEDS
        }
 
    		tcbs[nn].sys_stack = &sys_stacks[nn] + 511;
    		tcbs[nn].bios_stack = &bios_stacks[nn] + 511;
        asm {
            ldi   r1,#42
            sc    r1,hs:LEDS
        }
 
    		tcbs[nn].stack = &stacks[nn] + 1023;
    		tcbs[nn].hJob = 0;
    		tcbs[nn].timeout = 0;
        asm {
            ldi   r1,#43
            sc    r1,hs:LEDS
        }
 
    		tcbs[nn].hMailboxes[0] = -1;
    		tcbs[nn].hMailboxes[1] = -1;
    		tcbs[nn].hMailboxes[2] = -1;
    		tcbs[nn].hMailboxes[3] = -1;
        asm {
            ldi   r1,#44
            sc    r1,hs:LEDS
        }
 
    		if (nn<2) {
                tcbs[nn].affinity = nn;
                tcbs[nn].priority = 030;
            }
            tcbs[nn].exception = 0;
        asm {
            ldi   r1,#47
            sc    r1,hs:LEDS
        }
 
    	}
    	tcbs[NR_TCB-1].next = -1;
    	freeTCB = 2;
        asm {
            ldi   r1,#48
            sc    r1,hs:LEDS
        }
    	InsertIntoReadyList(0);
    	InsertIntoReadyList(1);
    	tcbs[0].status = TS_RUNNING;
    	tcbs[1].status = TS_RUNNING;
        asm {
            ldi   r1,#49
            sc    r1,hs:LEDS
        }
    	SetRunningTCB(0);
    	TimeoutList = -1;
    	set_vector(4,FMTK_SystemCall);
    	set_vector(2,FMTK_SchedulerIRQ2);
    	set_vector(194,FMTK_SchedulerIRQ);
		set_vector(195,KeybdIRQ);
        asm {
            ldi   r1,#50
            sc    r1,hs:LEDS
        }
    	FMTK_StartTask(030, 0, FocusSwitcher, 0, 0);
        asm {
            ldi   r1,#55
            sc    r1,hs:LEDS
        }
    	FMTK_StartTask(033, 0, shell, 0, 1);
        asm {
            ldi   r1,#52
            sc    r1,hs:LEDS
        }
//    	FMTK_StartTask(055, 0, gfx_demo, 0, 1);
        FMTK_StartTask(077, 0, IdleTask, 0, 0);
        asm {
            ldi   r1,#53
            sc    r1,hs:LEDS
        }
//      FMTK_StartTask(077, 1, IdleTask, 0, 0);
    	FMTK_Inited = 0x12345678;
        asm {
            ldi   r1,#56
            sc    r1,hs:LEDS
        }
    }
}