TempleOS-Rijn/LT/OSMain/MultiProc.CPZ

asm {
////**************************PROCEDURE*************************
	ALIGN	16,0x90
	USE16
MP_INIT_START::
	JMP	MP2_START2
	ALIGN	4,OC_NOP
MP2_SYS_TEMP_PTR:	DU4	0,0;

MP2_START2:
	CLI

//	  MOV	  EAX,CR0
	DU1	0x0F,0x20,0xC0;
	OR	EAX,0x60000000
//	  MOV	  CR0,EAX
	DU1	0x0F,0x22,0xC0;

	INVD

	MOV	AX,MP_VECTOR_ADDRESS/16
	MOV	DS,AX

	LGDT	U4 [MP_SYS_TEMP_PTR]

	MOV	EAX,SYS_START_CR0|0x60000000
//	  MOV	  CR0,EAX
	DU1	0x0F,0x22,0xC0;

	DU1	0x66,0xEA;		 //JMP SYS_CS_SEL:MP_INIT_OS
	DU4	MP_INIT_OS;
	DU2	SYS_CS_SEL;

MP_INIT_END::
	USE32
MP_INIT_OS:
	MOV	AX,ZERO_DS_SEL
	MOV	DS,AX
	MOV	ES,AX
	MOV	FS,AX
	MOV	GS,AX
	MOV	SS,AX

	MOV	EDI,U4 [SYS_UNCACHED_MP_CNT]
	MOV	ESI,U4 [EDI] //TODO: not perfect
	LOCK
	INC	U4 [EDI]

//	  MOV	  EAX,CR0
	DU1	0x0F,0x20,0xC0;
	AND	EAX,~0x60000000 //enable cache
//	  MOV	  CR0,EAX
	DU1	0x0F,0x22,0xC0;

	IMUL2	ESI,CPU_STRUCT_SIZE
	ADD	ESI,U4 [SYS_MP_CPU_STRUCTS]

	LEA	EAX,U4 CPU_START_STACK_TOP[ESI]
	MOV	ESP,EAX
	PUSH	U4 SYS_START_RFLAGS
	POPFD
	PUSH	U4 0
	CALL	INIT_EM64T
USE64
	PUSH	RSI
	CALL	CP_PUT_GS_BASE
	POP	RSI
@@2:	MOV	RBX,U8 CPU_CAIN_TSS[RSI]
	OR	RBX,RBX
	JZ	@@2
	MOV	RAX,RBX
	CALL	PUT_FS_BASE

	MOV	RAX,U4 SYS_TEMP_PTR
	MOV	U2 [RAX],MAXIDT*16-1
	MOV	U4 2[RAX],SYS_IDTTAB
	MOV	U2 6[RAX],0
	MOV	U4 8[RAX],0
	REX
	LIDT	U4 [RAX]

	JMP	U4 RESTORE_CONTEXT
}

void MPInt(U8 num,U8 cpu_num=1)
{
  U4 *dl=MP_ICR_LOW,*dh=MP_ICR_HIGH;
  U8 old_flags=GetFlags;
  Cli;
  while (*dl&0x1000); //TODO semaphore
  *dh=cpu_num<<24;
  *dl=0x4800+num;
  SetFlags(old_flags);
}

void MPIntAll(U8 num)
{
  U4 *dl=MP_ICR_LOW;
  U8 old_flags=GetFlags;
  Cli;
  while (*dl&0x1000); //TODO semaphore
  *dl=0xC4800+num;
  SetFlags(old_flags);
}

void MPNMInt()
{
  U4 *dl=MP_ICR_LOW;
  *dl=0xC4400;
}

void MPHalt()
{
  MPNMInt; //Hlt All other processors
}

void MPInitAPIC()
{
  U4 *d;
  d=MP_SVR;
  *d=*d|MP_APIC_ENABLED;
  d=MP_LDR;
  *d=Gs->num<<24;
  d=MP_DFR;
  *d=0xF0000000;
  MemSet(MP_IRR,0,0x20);
  MemSet(MP_ISR,0,0x20);
  MemSet(MP_TMR,0,0x20);
  SetTR(Gs->tr);
}

void MPWaitForTask()
{
  MPCmdStruct *tempm;
  Bts(&Fs->task_flags,TSSf_IDLE);
  while (TRUE) {
    WbInvd;
    FinishOffDyingTsses;
    while (mp_ctrl->next_waiting==mp_ctrl)
      SwapInNext;
    while (LBts(&mp_ctrl->flags,MPCCf_LOCKED))
      SwapInNext;
    tempm=mp_ctrl->next_waiting;
    RemQue(tempm);
    LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
    if (tempm!=mp_ctrl) {
      Btr(&Fs->task_flags,TSSf_IDLE);
      switch (tempm->cmd_code) {
	case MPCT_CALL:
	  CallInd(tempm->add,tempm->data);
	  break;
	case MPCT_SPAWN_TASK:
	  Spawn(tempm->add,"MP Job");
	  break;
      }
      if (Bt(&tempm->flags,MPCf_FREE_ON_COMPLETE))
	FreeHC(tempm,mp_heap);
      else {
	while (LBts(&mp_ctrl->flags,MPCCf_LOCKED))
	  SwapInNext;
	InsQue(tempm,mp_ctrl->last_done);
	LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
      }
      LBtr(&Gs->flags,CPUf_NOT_READY);
      Bts(&Fs->task_flags,TSSf_IDLE);
    }
  }
}

void MPQueueJob(void *add,void *data=NULL,
       U8 flags=1<<MPCf_FREE_ON_COMPLETE,BoolU4 spawn=FALSE)
{
  MPCmdStruct *tempm=MAllocHCZ(sizeof(MPCmdStruct),mp_heap);
  if (spawn)
    tempm->cmd_code=MPCT_SPAWN_TASK;
  else
    tempm->cmd_code=MPCT_CALL;
  tempm->add=add;
  tempm->data=data;
  tempm->flags=flags;
  while (LBts(&mp_ctrl->flags,MPCCf_LOCKED))
    SwapInNext;
  InsQue(tempm,mp_ctrl->last_waiting);
  LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
}

void MPInitCPUTsk()
{
  MPInitAPIC;
  Fs->rip=&MPWaitForTask;
  Fs->time_slice_start=GetTimeStamp;
  RestoreContext;
}

void MPStart()
{
  TssStruct *tss;
  I1 buf[128];
  U4 *d;
  MPMainStruct *mp=MP_VECTOR_ADDRESS;
  I8 i=0;
  CPUStruct *c;
  BoolU4 old_preempt=Preempt(OFF);
  I8 shared_blks;

  if (mp_cnt>1) {
    Preempt(old_preempt);
    return;
  }

  MemCpy(MP_VECTOR_ADDRESS,MP_INIT_START,MP_INIT_END-MP_INIT_START);
  mp->sys_temp_ptr=MAXGDT*16-1+gdttab<<16;
  mp_cnt=1;
  *uncached_mp_cnt=1;

  d=MP_LVT3;
  *d=*d&0xFFFFFF00+MP_VECTOR;
  WbInvd;

  d=MP_ICR_LOW;
  *d=0xCC500; //assert init IPI
  BusyWait(10000);

  *d=0xC4600+MP_VECTOR; //start-up
  BusyWait(200);
  *d=0xC4600+MP_VECTOR;$FG$

  BusyWait(10000);

  mp_cnt=*uncached_mp_cnt;
  for (i=1;i<mp_cnt;i++) {
    c=mp_cpu_structs+sizeof(CPUStruct)*i;
    SPrintF(buf,"Cain Task CPU#%d",i);
    tss=Spawn(&MPInitCPUTsk,buf,NULL,NULL,DEFAULT_STACK,FALSE);
    tss->in_queue_signature=TSSS_IN_QUEUE_SIGNATURE;
    tss->gs=InitCPUStruct(i,c);
    shared_blks=1; //One 2Meg blk
    c->cpu_heap=Alloc2MegMemBlks(&shared_blks);
    InitHeapCtrl(c->cpu_heap,shared_blks<<12);
    c->tr=InitTssStruct2;
    c->cain_tss=tss;
    WbInvd;
  }
  Preempt(old_preempt);
}

void MPInit()
{
  I8 shared_blks=1; //One 2Meg blk
  TssStruct *tss;
  RaxRbxRcxRdx e;
  CPUStruct *c;

  CpuId(0x1,&e);

  mp_cnt=1;
  mp_heap=AllocUncachedMemBlks(&shared_blks);
  InitHeapCtrl(mp_heap,shared_blks<<12);
  uncached_mp_cnt=MAllocHC(sizeof(U8),mp_heap);
  *uncached_mp_cnt=1;

  mp_ctrl=MAllocHCZ(sizeof(MPCmdCtrl),mp_heap);
  mp_ctrl->next_waiting=mp_ctrl;
  mp_ctrl->last_waiting=mp_ctrl;
  mp_ctrl->next_done=&mp_ctrl->next_done;
  mp_ctrl->last_done=&mp_ctrl->next_done;

  mp_cpu_structs=MAllocHCZ(sizeof(CPUStruct)*MP_MAX_PROCESSORS,mp_heap);
  c=mp_cpu_structs;
  MemCpy(c,&sys_temp_cpu0_struct,sizeof(CPUStruct));
  c->absolute_address=c;
  SetGs(c);
  tss=adam_tss;
  tss->time_slice_start=GetTimeStamp;
  tss->in_queue_signature=TSSS_IN_QUEUE_SIGNATURE;
  c->cain_tss=tss;
  c->tr=InitTssStruct2;

  SwapInNext;  //Store FPU for spawn
  if (Bt(&e.rdx,9))
    MPInitAPIC;
}