/*
 * Testing multitasking with HI-TECH C and RTM/Z80.
 * Make rotation effects on four Z80 PIO's in four
 * independent simultaneously running tasks.
 *
 * Copyleft Lumir Vanek
 */

#include <dlist.h>
#include <balloc.h>
#include <rtsys.h>
#include <rtclk.h>
#include <stdio.h>
#include <string.h>
#include <io.h>


#define LEDS		00H		/* Digital I/O board */

#define PIO1_A_D	068H		/* Z80 PIO #1 */
#define PIO1_A_C	06AH
#define PIO1_B_D	069H
#define PIO1_B_C	06BH

#define PIO2_A_D	06CH		/* Z80 PIO #2 */
#define PIO2_A_C	06EH
#define PIO2_B_D	06DH
#define PIO2_B_C	06FH

#define PIO3_A_D	0F0H		/* Z80 PIO #3 */
#define PIO3_A_C	0F2H
#define PIO3_B_D	0F1H
#define PIO3_B_C	0F3H

#define PIO4_A_D	0F4H		/* Z80 PIO #4 */
#define PIO4_A_C	0F6H
#define PIO4_B_D	0F5H
#define PIO4_B_C	0F7H

	void (*fp)(void); 		/* Pointer to function */
	
	struct Semaphore* S1;	  	/* Pointers to Semaphores */
	struct Semaphore* S2;
	struct Semaphore* S3;
	struct Semaphore* S4;
	struct Semaphore* S_IO;
	struct Semaphore* Timer_Sem;
	
	struct RTClkCB* Timer;

	/* We have four tasks */
	void* TCB_1;		
	void* TCB_2;
	void* TCB_3;
	void* TCB_4;

	unsigned long n1 = 0;
	unsigned long n2 = 0;
	unsigned long n3 = 0;
	unsigned long n4 = 0;
	unsigned long n5 = 0;

	char buf[50];

void Pio1(void)
{
	int i, j;
	for (i = 0; i < 12000; i++)
	{
#asm
		PUSH AF
		PUSH DE
		
		LD D, 10000000B
		LD E, 00000001B
		
		; Display initial state
		LD A, E
		OUT (PIO1_A_D), A
		LD A, D
		OUT (PIO1_B_D), A
		
		LD A, 10000000B
		OUT (LEDS), A
		
		POP DE
		POP AF
#endasm
		
		Wait(S1);
		
		for (j = 0; j < 7; j++) /* 8 LED's = initial state + 7 rotation cycles */
		{
#asm
			PUSH AF
			PUSH DE
		
			; Read PIO #1 to DE
			IN A, (PIO1_A_D) 
			LD E, A
			IN A, (PIO1_B_D) 
			LD D, A
				
			;
			; 8-bit rotation in D and E
			;

			SCF			; The Carry Flag in the flags register is set
			RL E			; Rotate E left, bit 7 goes to Carry Flag and CF to bit 0  
			
			SCF			; The Carry Flag in the flags register is set
			RR D			; Rotate D right, Carry Flag with E(b7) goes to bit 7, D(b0) goes to CF
				
			; Write DE back to PIO #1
			LD A, E
			OUT (PIO1_A_D), A
			LD A, D
			OUT (PIO1_B_D), A
			
			POP DE
			POP AF
#endasm			
			Wait(S1);
		}

		n1++;
		Wait(S1);
	}
	
	StopTask(TCB_1);
}

/* 16-bit left shift in DE */
void Pio2(void)
{
	int i, j;

#asm

	LD A, 01000000B
	OUT (LEDS), A
	;CALL ASMDELAY

#endasm

	Wait(S2);
	
	for (i = 0; i < 6000; i++)
	{
#asm
		PUSH AF
		PUSH DE
	
		LD DE, 0001H
		
		; Display initial state
		LD A, E
		OUT (PIO2_A_D), A
		LD A, D
		OUT (PIO2_B_D), A
		
		POP DE
		POP AF
#endasm

		Wait(S2);

		for (j = 0; j < 7; j++) /* 8 LED's = 7 shift cycles */
		{
#asm
			PUSH AF
			PUSH DE
		
			; Read PIO #2 to E
			IN A, (PIO2_A_D) 
			LD E, A

			SLA E		; Shift E left, bit 7 goes to Carry Flag bit 0 is reset	
			
			; Write E back to PIO #2
			LD A, E
			OUT (PIO2_A_D), A
			
			POP DE
			POP AF
#endasm			
			Wait(S2);
		}

#asm
		; Do eigth cycle
		PUSH AF
		PUSH DE
		
		; Read PIO #2 to E
		IN A, (PIO2_A_D)
		LD E, A

		SLA E			; Shift E left, bit 7 goes to Carry Flag bit 0 is reset	
		RL D			; Push Carry Flag to D bit 0

		; Write DE back to PIO #2
		LD A, E
		OUT (PIO2_A_D), A
		LD A, D
		OUT (PIO2_B_D), A
			
		SLA D			; Shift D left, bit 7 goes to Carry Flag bit 0 is reset
	
		; Write D back to PIO #2
		LD A, D
		OUT (PIO2_B_D), A
		
		POP DE
		POP AF
#endasm	
	
		Wait(S2);
		
		for (j = 0; j < 7; j++) /* 8 LED's = 7 shift cycles */
		{
#asm
			PUSH AF
			PUSH DE
		
			; Read PIO #3 to D
			IN A, (PIO3_B_D) 
			LD D, A
		
			SLA D		; Shift D left, bit 7 goes to Carry Flag bit 0 is reset
			
			; Write D back to PIO #2
			LD A, D
			OUT (PIO2_B_D), A
			
			POP DE
			POP AF
#endasm			
			Wait(S2);
		}
	
		n2++;
		Wait(S2);
	}
	
	StopTask(TCB_2);
}

/* 16-bit left shift in DE */
void Pio3(void)
{
	int i, j;

#asm

	LD A, 00100000B
	OUT (LEDS), A

#endasm

	Wait(S3);
	
	for (i = 0; i < 6000; i++)
	{
#asm
		PUSH AF
		PUSH DE
			
		LD DE, 0001H
		
		; Display initial state
		LD A, E
		OUT (PIO3_A_D), A
		LD A, D
		OUT (PIO3_B_D), A
		
		POP DE
		POP AF
#endasm

		Wait(S3);

		for (j = 0; j < 7; j++) /* 8 LED's = 7 rotation cycles */
		{
#asm
			PUSH AF
			PUSH DE
		
			; Read PIO #3 to E
			IN A, (PIO3_A_D) 
			LD E, A
				
			SLA E		; Shift E left, bit 7 goes to Carry Flag bit 0 is reset	
			
			; Write E back to PIO #3
			LD A, E
			OUT (PIO3_A_D), A
			
			POP DE
			POP AF
#endasm			
			Wait(S3);
		}

#asm
		; Do eigth cycle
		PUSH AF
		PUSH DE
		
		; Read PIO #3 to E
		IN A, (PIO3_A_D) 
		LD E, A
			
		SLA E			; Shift E left, bit 7 goes to Carry Flag bit 0 is reset	
		RL D			; Push Carry Flag to D bit 0
			
		; Write DE back to PIO #3
		LD A, E
		OUT (PIO3_A_D), A
		LD A, D
		OUT (PIO3_B_D), A
			
		SLA D			; Shift D left, bit 7 goes to Carry Flag bit 0 is reset
	
		; Write D back to PIO #3
		LD A, D
		OUT (PIO3_B_D), A
		
		POP DE
		POP AF
#endasm	
		
		Wait(S3);
		
		for (j = 0; j < 7; j++) /* 8 LED's = 7 shift cycles */
		{
#asm
			PUSH AF
			PUSH DE
			
			; Read PIO #3 to D
			IN A, (PIO3_B_D) 
			LD D, A
		
			SLA D		; Shift D left, bit 7 goes to Carry Flag bit 0 is reset
			
			; Write D back to PIO #3
			LD A, D
			OUT (PIO3_B_D), A
			
			POP DE
			POP AF
#endasm			
			Wait(S3);
		}

		n3++;
		Wait(S3);
	}
	
	StopTask(TCB_3);
}

void Pio4(void)
{
	int i, j;
	
#asm

	LD A, 00010000B
	OUT (LEDS), A

#endasm		
	
	Wait(S4);
	
	for (i = 0; i < 500; i++)
	{
#asm
		PUSH AF
		LD A, 0
		OUT (PIO4_A_D), A
		POP AF
#endasm		

		for (j = 0; j < 255; j++) 
		{
#asm
			PUSH AF

			; Read PIO #4 A to A
			IN A, (PIO4_A_D) 
			INC A
			; Write A back to PIO #4
			OUT (PIO4_A_D), A
		
			POP AF
#endasm	

			Wait(S4);
		}
		
		n4++;
		Wait(S4);
	}
	
	StopTask(TCB_4);
}

void StartTasks(void)
{
	int one = 1;
	S1 = MakeSem();
	S2 = MakeSem();
	S3 = MakeSem();
	S4 = MakeSem();
	S_IO = MakeSem();
	Timer_Sem = MakeSem();
	Timer = MakeTimer();
	
	sprintf(buf, "Hello RC2014, Z80 CPU + four Z80 PIO's !\r\n");
	CON_Write(buf, strlen(buf), S_IO);
	Wait(S_IO);
	
	sprintf(buf, "Running four independent tasks on RTM/Z80\r\n");
	CON_Write(buf, strlen(buf), S_IO);
	Wait(S_IO);

	fp = Pio1;
	TCB_1 = RunTask(0x1E0, (void*) fp, 3);
	
	fp = Pio2;
	TCB_2 = RunTask(0x1E0, (void*) fp, 4);
	
	fp = Pio3;
	TCB_3 = RunTask(0x1E0, (void*) fp, 3);
	
	fp = Pio4;
	TCB_4 = RunTask(0x1E0, (void*) fp, 3);
	
	StartTimer(Timer, Timer_Sem, 15, 1);
	
	do
	{
		Wait(Timer_Sem);
		
		if (GetTaskSts(TCB_1))
			Signal(S1);
		
		if (GetTaskSts(TCB_2))
			Signal(S2);
		
		if (GetTaskSts(TCB_3))
			Signal(S3);
		
		if (GetTaskSts(TCB_4))
			Signal(S4);
		
		if (!GetTaskSts(TCB_1) && !GetTaskSts(TCB_2) && !GetTaskSts(TCB_3) && !GetTaskSts(TCB_4))
		{
			StopTimer(Timer);
			break;
		}
		
		if (CTRL_C())
		{
			StopTimer(Timer);
			
			StopTask(TCB_1);
			StopTask(TCB_2);
			StopTask(TCB_3);
			StopTask(TCB_4);
	
			break;
		}
		
		n5++;
		
	} while (1 == one);
	
	DropSem(S1);
	DropSem(S2);
	DropSem(S3);
	DropSem(S4);
	
	
	sprintf(buf, "\r\n T1 counted %lu", n1);
	CON_Write(buf, strlen(buf), S_IO);
	Wait(S_IO);
	
	sprintf(buf, "\r\n T2 counted %lu", n2);
	CON_Write(buf,strlen(buf), S_IO);
	
	sprintf(buf, "\r\n T3 counted %lu", n3);
	CON_Write(buf,strlen(buf), S_IO);
	Wait(S_IO);
	
	sprintf(buf, "\r\n T4 counted %lu", n4);
	CON_Write(buf, strlen(buf), S_IO);
	Wait(S_IO);
	
	sprintf(buf, "\r\n T5 counted %lu", n5);
	CON_Write(buf, strlen(buf), S_IO);
	Wait(S_IO);
	
	sprintf(buf, "\r\nFinished, returning to SCM\r\n");
	CON_Write(buf, strlen(buf), S_IO);
	Wait(S_IO);
	
	for (one = 1; one < 5000; one++)
	{
		;
	}
	
	DropSem(S_IO);
	
	ShutDown();
}

	
/* Entry point to program */
void main(void)
{
	/* Set SC103 - Z80 PIO's to output mode */
#asm

	PUSH AF

	LD A, 0FH			; Control word for PIO, set output mode
	OUT (PIO1_A_C), A		; PIO #1 port A
	OUT (PIO1_B_C), A		; PIO #1 port B
	OUT (PIO2_A_C), A		; PIO #2 port A
	OUT (PIO2_B_C), A		; PIO #2 port B
	OUT (PIO3_A_C), A		; PIO #3 port A
	OUT (PIO3_B_C), A		; PIO #3 port B
	OUT (PIO4_A_C), A		; PIO #4 port A
	OUT (PIO4_B_C), A		; PIO #4 port B
		
	; Turn all LED's OFF
	XOR A				; LD A, 0 -> XOR A
	OUT (LEDS), A
	OUT (PIO1_A_D), A
	OUT (PIO1_B_D), A
	OUT (PIO2_A_D), A
	OUT (PIO2_B_D), A
	OUT (PIO3_A_D), A
	OUT (PIO3_B_D), A
	OUT (PIO4_A_D), A
	OUT (PIO4_B_D), A
	
	POP AF
	
#endasm

	fp = StartTasks;
	StartUp(0x1E0, (void*) fp, 50);
}