/*
[ GPL ]
Code by JaeHyuk Cho <mailto:minzkn@infoeq.com> KOREA

MZ Local Thread library v0.0.1b

- Simple is best !
*/

#if !defined(DEF_SOURCE_thread_c)
#define DEF_SOURCE_thread_c "thread.c"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define t_inline_asm __asm__ __volatile__
#define ML_Alloc(m_size) malloc(m_size)
#define ML_Free(m_ptr) free(m_ptr)
#define ML_PeekPtr(m_cast,m_base,m_offset) ((m_cast)(((unsigned char *)(m_base)) + (m_offset)))
#define ML_PeekDoubleWord(m_ptr,m_offset) *ML_PeekPtr(unsigned long *,m_ptr,m_offset)
#define ML_PokeDoubleWord(m_ptr,m_offset,m_value) *ML_PeekPtr(unsigned long *,m_ptr,m_offset) = m_value

typedef struct ts_STACK
{
void *Stack;
int StackSize, StackPointer;
}t_STACK;

typedef struct ts_THREAD_TASK
{
struct ts_THREAD_TASK *Next;
t_STACK *Stack;
unsigned int TaskID, ESP, Tick, Active;
void * (*Entry)(void *, void *);
void *Argument;
}t_THREAD_TASK;

typedef struct ts_THREAD
{
t_THREAD_TASK *Task;
t_THREAD_TASK *CurrentTask;
unsigned int TaskCount, MakeID;
}t_THREAD;

t_THREAD *ML_CreateTHREAD(void);
t_THREAD *ML_DestroyTHREAD(t_THREAD *s_THREAD);
t_THREAD *ML_AddTHREAD(t_THREAD *s_THREAD, void * (*s_ThreadFunction)(void *, void *), void *s_Argument, int s_StackSize);
t_THREAD *ML_RunTHREAD(t_THREAD *s_THREAD);
int ML_SleepTHREAD(t_THREAD *s_THREAD);

t_STACK *ML_CreateSTACK(int s_StackSize);
t_STACK *ML_DestroySTACK(t_STACK *s_STACK);
int ML_PushSTACK(t_STACK *s_STACK, int s_Value);
int ML_PopSTACK(t_STACK *s_STACK, int *s_Value);
int ML_SetSTACK(t_STACK *s_STACK, int s_StackPointer);

void __ML_ReturnTHREAD__(void);




#if 1 /* TEST ------------------------------- */
void * (test_0)(void *s_thread_handle, void *s_argument)
{
 int s_count = 0;
 while((s_count++) < 100)
 {
  (void)printf("test_0 : %3d (\"%s\")\n", s_count, (char *)s_argument);
  (void)ML_SleepTHREAD((t_THREAD *)s_thread_handle);  /* context switch */
 }
 return(s_argument);
}

void * (test_1)(void *s_thread_handle, void *s_argument)
{
 int s_count = 0;
 while((s_count++) < 100)
 {
  (void)printf("test_1 : %3d (\"%s\")\n", s_count, (char *)s_argument);
  (void)ML_SleepTHREAD((t_THREAD *)s_thread_handle);  /* context switch */
 }
 return(s_argument);
}

void * (test_2)(void *s_thread_handle, void *s_argument)
{
 int s_count = 0;
 while((s_count++) < 100)
 {
  (void)printf("test_2 : %3d (\"%s\")\n", s_count, (char *)s_argument);
  (void)ML_SleepTHREAD((t_THREAD *)s_thread_handle);  /* context switch */
 }
 return(s_argument);
}

void * (test_3)(void *s_thread_handle, void *s_argument)
{
 int s_count = 0;
 while((s_count++) < 100)
 {
  (void)printf("test_3 : %3d (\"%s\")\n", s_count, (char *)s_argument);
  (void)ML_SleepTHREAD((t_THREAD *)s_thread_handle);  /* context switch */
 }
 return(s_argument);
}

int main(void)
{
 t_THREAD *s_thread;
 s_thread = ML_CreateTHREAD();

 ML_AddTHREAD(s_thread, test_0, "USER ARGUMENT - 0", (8 << 10));
 ML_AddTHREAD(s_thread, test_1, "USER ARGUMENT - 1", (8 << 10));
 ML_AddTHREAD(s_thread, test_2, "USER ARGUMENT - 2", (8 << 10));
 ML_AddTHREAD(s_thread, test_3, "USER ARGUMENT - 3", (8 << 10));

 (void)printf("Run.\n");
 ML_RunTHREAD(s_thread);
 (void)printf("End.\n");
 
 s_thread = ML_DestroyTHREAD(s_thread);
 return(0);
}
#endif /* TEST ------------------------------- */




#if 0 /* TEST ------------------------------- */
void * (test_0)(void *s_thread_handle, void *s_argument)
{
 int s_context = *((int *)s_argument);
 int s_count;
 int s_color;
 s_color = (s_context % 6) + 31;
 (void)printf("[color=%d] \x1b[1;%dmbegin thread (id=%d)\x1b[0m\n", s_color, s_color, s_context);
 for(s_count = 0;s_count < 100;s_count++)
 {
  (void)printf("[color=%d] \x1b[1;%dmtest thread (id=%d, count=%d)\x1b[0m\n", s_color, s_color, s_context, s_count);
  (void)ML_SleepTHREAD((t_THREAD *)s_thread_handle);  /* context switch */
 }
 (void)printf("[color=%d] \x1b[1;%dmend thread (id=%d)\x1b[0m\n", s_color, s_color, s_context);
 return(s_argument);
}

int main(void)
{
 t_THREAD *s_thread;
 int s_count;
 int s_context[ 10 ];
 s_thread = ML_CreateTHREAD();

 for(s_count = 0;s_count < (sizeof(s_context) / sizeof(int));s_count++)
 {
  s_context[s_count] = s_count;
  ML_AddTHREAD(s_thread, test_0, &s_context[s_count], (8 << 10));
 }

 (void)printf("Run.\n");
 ML_RunTHREAD(s_thread);
 (void)printf("End.\n");
 
 s_thread = ML_DestroyTHREAD(s_thread);
 return(0);
}
#endif /* TEST ------------------------------- */





static void *__ML_ManagerTHREAD__(void *s_ThreadHandle, void *s_Argument)
{
static t_THREAD *sg_THREAD = (t_THREAD *)0;
if(sg_THREAD != (t_THREAD *)s_ThreadHandle)sg_THREAD = (t_THREAD *)s_ThreadHandle;
ML_SleepTHREAD((t_THREAD *)s_ThreadHandle);
if(((t_THREAD *)s_ThreadHandle)->Task->Active == 0)return(s_Argument);
t_inline_asm(
  "__ML_ReturnTHREAD__:\n\t"
  "pushl $__ML_ReturnTHREAD__\n\t" /* Retry push return address */
);
t_inline_asm(
  "\n\t"
  : "=a"(((t_THREAD *)s_ThreadHandle)->CurrentTask->Argument)
);
((t_THREAD *)s_ThreadHandle)->CurrentTask->Active = 0;
ML_SleepTHREAD((t_THREAD *)s_ThreadHandle);
return(s_Argument);
}

t_THREAD *ML_CreateTHREAD(void)
{
t_THREAD *s_Return;
s_Return = (t_THREAD *)ML_Alloc(sizeof(t_THREAD));
if(s_Return)
{
  s_Return->Task = s_Return->CurrentTask = (t_THREAD_TASK *)0;
  s_Return->TaskCount = s_Return->MakeID = 0u;
  s_Return = ML_AddTHREAD(s_Return, __ML_ManagerTHREAD__, (void *)0, (4 << 10));
}
return(s_Return);
}

t_THREAD *ML_DestroyTHREAD(t_THREAD *s_THREAD)
{
t_THREAD_TASK *s_THREAD_TASK;
if(s_THREAD)
{
  while(s_THREAD->Task && s_THREAD->TaskCount--)
  {
   s_THREAD_TASK = s_THREAD->Task;
   s_THREAD->Task = s_THREAD->Task->Next;
   if(s_THREAD_TASK->Stack)(void)ML_DestroySTACK(s_THREAD_TASK->Stack);
   (void)ML_Free(s_THREAD_TASK);
  }
  (void)ML_Free(s_THREAD);
  s_THREAD = (t_THREAD *)0;
}
return(s_THREAD);
}

t_THREAD *ML_AddTHREAD(t_THREAD *s_THREAD, void * (*s_ThreadFunction)(void *, void *), void *s_Argument, int s_StackSize)
{
t_THREAD_TASK *s_THREAD_TASK;
if(s_THREAD == (t_THREAD *)0)s_THREAD = ML_CreateTHREAD();
if(s_THREAD)
{
  if(s_THREAD->Task)
  {
   s_THREAD_TASK = s_THREAD->Task;
   while(s_THREAD_TASK->Next && s_THREAD_TASK->Next != s_THREAD->Task)s_THREAD_TASK = s_THREAD_TASK->Next;
   s_THREAD_TASK->Next = (t_THREAD_TASK *)ML_Alloc(sizeof(t_THREAD_TASK));
   s_THREAD_TASK = s_THREAD_TASK->Next;
   if(s_THREAD->CurrentTask == (t_THREAD_TASK *)0)s_THREAD->CurrentTask = s_THREAD->Task;
  }
  else s_THREAD->Task = s_THREAD->CurrentTask = s_THREAD_TASK = (t_THREAD_TASK *)ML_Alloc(sizeof(t_THREAD_TASK));
  if(s_THREAD_TASK)
  {
   if(s_StackSize < ( 4 << 10 ))s_StackSize = ( 4 << 10 );
   s_THREAD_TASK->Next = s_THREAD->Task;
   s_THREAD_TASK->Stack = ML_CreateSTACK(s_StackSize);
   s_THREAD_TASK->TaskID = (s_THREAD->MakeID++);
   s_THREAD_TASK->Tick = 0;
   s_THREAD_TASK->Active = 1;
   s_THREAD_TASK->Entry = s_ThreadFunction;
   s_THREAD_TASK->Argument = s_Argument;
   s_THREAD_TASK->ESP = (unsigned int)s_THREAD_TASK->Stack->Stack + s_THREAD_TASK->Stack->StackPointer;
   s_THREAD->TaskCount++;
  }
}
return(s_THREAD);
}

t_THREAD *ML_RunTHREAD(t_THREAD *s_THREAD)
{
struct { unsigned int eax, ebx, ecx, edx, esi, edi, ebp, esp, flags; }s_Register;
t_THREAD_TASK *s_THREAD_TASK;
unsigned int s_RegisterAddress, s_TempEBX;
if(s_THREAD)
{
  if(s_THREAD->Task)
  {
   s_RegisterAddress = (unsigned int)(&s_Register);
   t_inline_asm(
    "\n\t"
    "movl %%ebx, %1\n\t"
    "movl %0, %%ebx\n\t"
    "movl %%eax, 0(%%ebx)\n\t"
    "movl %1, %%eax\n\t"
    "movl %%eax, 4(%%ebx)\n\t"
    "movl 0(%%ebx), %%eax\n\t"
    "movl %%ecx, 8(%%ebx)\n\t"
    "movl %%edx, 12(%%ebx)\n\t"
    "movl %%esi, 16(%%ebx)\n\t"
    "movl %%edi, 20(%%ebx)\n\t"
    "movl %%ebp, 24(%%ebx)\n\t"
    "movl %%esp, 28(%%ebx)\n\t"
    "pushfl\n\t"
    "popl 32(%%ebx)\n\t"
    "movl 4(%%ebx), %%ebx\n\t"
    "\n\t"
    :
    : "m"(s_RegisterAddress), "m"(s_TempEBX)
   );
   s_THREAD_TASK = s_THREAD->Task;
   ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.flags);
   ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.esp);
   ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.ebp);
   ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.edi);
   ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.esi);
   ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.edx);
   ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.ecx);
   ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.ebx);
   ML_PushSTACK(s_THREAD_TASK->Stack, (int)s_THREAD_TASK->Argument);
   ML_PushSTACK(s_THREAD_TASK->Stack, (int)s_THREAD);
   s_THREAD_TASK->ESP = (unsigned int)s_THREAD_TASK->Stack->Stack + s_THREAD_TASK->Stack->StackPointer;
   s_THREAD_TASK = s_THREAD_TASK->Next;
   while(s_THREAD_TASK && s_THREAD_TASK != s_THREAD->Task)
   {
    ML_PushSTACK(s_THREAD_TASK->Stack, (int)s_THREAD_TASK->Argument);
    ML_PushSTACK(s_THREAD_TASK->Stack, (int)s_THREAD);
    ML_PushSTACK(s_THREAD_TASK->Stack, (int)__ML_ReturnTHREAD__);

    ML_PushSTACK(s_THREAD_TASK->Stack, (int)s_THREAD_TASK->Argument);
    ML_PushSTACK(s_THREAD_TASK->Stack, (int)s_THREAD);
    ML_PushSTACK(s_THREAD_TASK->Stack, (int)__ML_ReturnTHREAD__);

    ML_PushSTACK(s_THREAD_TASK->Stack, (int)s_THREAD_TASK->Entry);  /* First swich entry */

    ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.flags);
    ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.ebp);
    ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.edi);
    ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.esi);
    ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.edx);
    ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.ecx);
    ML_PushSTACK(s_THREAD_TASK->Stack, s_Register.ebx);
    s_THREAD_TASK->ESP = (unsigned int)s_THREAD_TASK->Stack->Stack + s_THREAD_TASK->Stack->StackPointer;
    s_THREAD_TASK = s_THREAD_TASK->Next;
   }
   t_inline_asm(
    "\n\t"
    "movl %1, %%ecx\n\t"
    "movl %0, %%ebp\n\t"
    "movl %%ebp, %%esp\n\t"
    "call *%%ecx\n\t"
    "addl $4 + 4, %%esp\n\t"
    "popl %%ebx\n\t"
    "popl %%ecx\n\t"
    "popl %%edx\n\t"
    "popl %%esi\n\t"
    "popl %%edi\n\t"
    "popl %%ebp\n\t"
    "popl %%eax\n\t" /* Change stack (x86) */
    "popfl\n\t"
    "movl %%eax, %%esp\n\t"
    "\n\t"
    :
    : "m"(s_THREAD->Task->ESP), "m"(s_THREAD->Task->Entry)
   );
  }
}
return(s_THREAD);
}

int ML_SleepTHREAD(t_THREAD *s_THREAD)
{
s_THREAD->CurrentTask->Tick++;
t_inline_asm(
  "\n\t"
  "movl %%esp, %%eax\n\t"
  "subl $28, %%eax\n\t"
  "\n\t"
  : "=a"(s_THREAD->CurrentTask->ESP)
);
do
{
  s_THREAD->CurrentTask = s_THREAD->CurrentTask->Next;
  if(s_THREAD->CurrentTask == s_THREAD->Task)
  {
   if(s_THREAD->Task->Active == 1)
   {
    s_THREAD->CurrentTask->Active = 0;
    continue;
   }
   else break;
  }
}while(s_THREAD->CurrentTask->Active == 0);
if(s_THREAD->CurrentTask != s_THREAD->Task)s_THREAD->Task->Active = 1;
t_inline_asm(
  "\n\t"
  "pushfl\n\t"
  "pushl %%ebp\n\t"
  "pushl %%edi\n\t"
  "pushl %%esi\n\t"
  "pushl %%edx\n\t"
  "pushl %%ecx\n\t"
  "pushl %%ebx\n\t"
  "movl %0, %%esp\n\t"
  "popl %%ebx\n\t"
  "popl %%ecx\n\t"
  "popl %%edx\n\t"
  "popl %%esi\n\t"
  "popl %%edi\n\t"
  "popl %%ebp\n\t"
  "popfl\n\t"
  "\n\t"
  :
  : "a"(s_THREAD->CurrentTask->ESP)
);
return(1);
}

t_STACK *ML_CreateSTACK(int s_StackSize)
{
t_STACK *s_Return;
if(s_StackSize < (4 << 10))s_StackSize = (4 << 10);
s_Return = (t_STACK *)ML_Alloc(sizeof(t_STACK));
if(s_Return)
{
  s_Return->Stack = (void *)ML_Alloc(s_StackSize);
  s_Return->StackSize = s_Return->StackPointer = s_StackSize;
}
return(s_Return);
}

t_STACK *ML_DestroySTACK(t_STACK *s_STACK)
{
if(s_STACK)
{
  if(s_STACK->Stack && s_STACK->StackSize > 0)(void)ML_Free(s_STACK->Stack);
  (void)ML_Free(s_STACK);
  s_STACK = (t_STACK *)0;
}
return(s_STACK);
}

int ML_PushSTACK(t_STACK *s_STACK, int s_Value)
{
if(s_STACK)
{
  if(s_STACK->Stack && s_STACK->StackSize >= sizeof(s_Value) && s_STACK->StackPointer >= sizeof(s_Value))
  {
   s_STACK->StackPointer -= sizeof(s_Value);
   ML_PokeDoubleWord(s_STACK->Stack, s_STACK->StackPointer, s_Value);
   return(s_STACK->StackPointer);
  }
}
return(0);
}

int ML_PopSTACK(t_STACK *s_STACK, int *s_Value)
{
int s_Return = (-1);
if(s_STACK)
{
  if(s_STACK->Stack && s_STACK->StackSize >= sizeof(int) && s_STACK->StackPointer <= (s_STACK->StackSize - sizeof(int)))
  {
   s_Return = ML_PeekDoubleWord(s_STACK->Stack, s_STACK->StackPointer);
   s_STACK->StackPointer += sizeof(int);
  }
}
if(s_Value)*(s_Value) = s_Return;
return(s_Return);
}

int ML_SetSTACK(t_STACK *s_STACK, int s_StackPointer)
{
if(s_STACK)
{
  s_STACK->StackPointer = s_StackPointer;
  return(s_STACK->StackPointer);
}
return(0);
}

#endif

/* End of source */