Here's my multiprocessing code.
Code: Select all
asm {
////**************************PROCEDURE*************************
ALIGN 16,OC_NOP
USE16
MP_INIT_START::
JMP MP2_START2
ALIGN 4,OC_NOP
MP2_SYS_TEMP_PTR: DU4 0,0;
MP2_START2:
CLI
MOV_EAX_CR0
OR EAX,0x60000000
MOV_CR0_EAX
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 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
FLDCW U2 [SYS_INIT_FLOAT_CTRL_WORD]
@@1: LOCK
BTS U4 [SYS_MP_CNT_LOCK],0
JC @@1
WBINVD //This might be paranoid
MOV ESI,U4 [SYS_MP_CNT]
LOCK
INC U4 [SYS_MP_CNT]
LOCK
BTR U4 [SYS_MP_CNT_LOCK],0
MOV_EAX_CR0
AND EAX,~0x60000000 //enable cache
MOV_CR0_EAX
IMUL2 ESI,CPU_CACHED_SIZE
ADD ESI,U4 [SYS_CPU_CACHED]
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_SET_GS_BASE
POP RSI
@@2: MOV RBX,U8 CPU_SETH_TSS[RSI]
OR RBX,RBX
JZ @@2
MOV U8 TSS_GS[RBX],RSI
MOV RAX,RBX
CALL SET_FS_BASE
JMP I4 RESTORE_CONTEXT
};
void MPInt(U8 num,U8 cpu_num=1)
{
U4 *dl=MP_ICR_LOW,*dh=MP_ICR_HIGH;
while (LBts(&mp_ctrl->flags,MPCCf_APIC_LOCKED))
SwapInNextTask;
PushFD;
Cli;
while (*dl&0x1000);
*dh=mp_apic_ids[cpu_num]<<24;
*dl=0x4800+num;
PopFD;
LBtr(&mp_ctrl->flags,MPCCf_APIC_LOCKED);
}
void MPEOI()
{
U4 *dl=MP_EOI;
while (LBts(&mp_ctrl->flags,MPCCf_APIC_LOCKED))
SwapInNextTask;
*dl=0;
LBtr(&mp_ctrl->flags,MPCCf_APIC_LOCKED);
}
void MPIntAll(U8 num)
{ //(All but self)
U4 *dl=MP_ICR_LOW;
while (LBts(&mp_ctrl->flags,MPCCf_APIC_LOCKED))
SwapInNextTask;
PushFD;
Cli;
while (*dl&0x1000);
*dl=0xC4800+num;
PopFD;
LBtr(&mp_ctrl->flags,MPCCf_APIC_LOCKED);
}
void MPNMInt()
{
U4 *dl=MP_ICR_LOW;
*dl=0xC4400;
}
void MPHalt()
{
mp_cnt=1;
MPNMInt; //Hlt All other processors
}
void MPWbInvdAll()
{
MPIntAll(I_WBINVD);
WbInvd;
}
void MPInitAPIC()
{
RaxRbxRcxRdx cpu_id;
I8 i;
U4 *d;
d=MP_SVR;
*d|=MP_APIC_ENABLED;
CpuId(1,&cpu_id);
i=cpu_id.rbx>>24&0xFF;
//This is the only way I could get it to work
mp_apic_ids[Gs->num]=1<<i;
//We're not supposed to change this
d=MP_LDR;
*d=mp_apic_ids[Gs->num]<<24;
d=MP_DFR;
*d=0xF0000000;
MemSet(MP_IRR,0,0x20);
MemSet(MP_ISR,0,0x20);
MemSet(MP_TMR,0,0x20);
SetRAX(Gs->tr);
asm {
LTR U4 RAX
}
if (Gs->num)
InitIDT;
}
void MPWaitForTask()
{
U8 timeout=0;
MPCmdStruct *tempm;
Preempt(OFF);
Sti;
Bts(&Fs->task_flags,TSSf_IDLE);
while (TRUE) {
while (TRUE) {
if (GetTimeStamp>timeout) {
WbInvd;
FinishOffDyingTsses;
timeout=GetTimeStamp+time_stamp_freq>>9;
} else if (mp_ctrl->next_waiting==mp_ctrl)
SwapInNextTask;
else
break;
}
while (LBts(&mp_ctrl->flags,MPCCf_LOCKED))
SwapInNextTask;
tempm=mp_ctrl->next_waiting;
while (tempm!=mp_ctrl && !Bt(&tempm->target_cpu_mask,Gs->num))
tempm=tempm->next;
if (tempm!=mp_ctrl) {
RemQue(tempm);
LBts(&tempm->flags,MPCf_DISPATCHED);
tempm->handler_cpu=Gs->num;
LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
Btr(&Fs->task_flags,TSSf_IDLE);
switch (tempm->cmd_code) {
case MPCT_CALL:
tempm->result=CallInd(tempm->add,tempm->data);
Preempt(OFF);
Sti;
break;
case MPCT_SPAWN_TASK:
if (tempm->desc)
tempm->tss=Spawn(tempm->add,tempm->data,tempm->desc);
else
tempm->tss=Spawn(tempm->add,tempm->data,"MP Job");
break;
}
if (Bt(&tempm->flags,MPCf_FREE_ON_COMPLETE)) {
Free(tempm->desc);
Free(tempm);
} else {
while (LBts(&mp_ctrl->flags,MPCCf_LOCKED))
SwapInNextTask;
InsQue(tempm,mp_ctrl->last_done);
LBts(&tempm->flags,MPCf_DONE);
LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
}
LBtr(&Gs->uncached_address->uncached_flags,CPUUf_NOT_READY);
Bts(&Fs->task_flags,TSSf_IDLE);
} else
LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
}
}
MPCmdStruct *MPQueueJob(void *add,void *data=NULL,
I1 *desc=NULL,
U8 flags=1<<MPCf_FREE_ON_COMPLETE,
BoolI1 spawn=FALSE,I8 target_cpu_mask=ALL_MASK)
{
MPCmdStruct *tempm=MAllocHCZ(sizeof(MPCmdStruct),mp_heap);
if (desc)
tempm->desc=StrNewHC(desc,mp_heap);
if (spawn)
tempm->cmd_code=MPCT_SPAWN_TASK;
else
tempm->cmd_code=MPCT_CALL;
tempm->add=add;
tempm->data=data;
tempm->target_cpu_mask=target_cpu_mask;
tempm->flags=flags;
while (LBts(&mp_ctrl->flags,MPCCf_LOCKED))
SwapInNextTask;
InsQue(tempm,mp_ctrl->last_waiting);
LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
return tempm;
}
MPCmdStruct *MPJob(void *add,void *data=NULL,
U8 flags=1<<MPCf_FREE_ON_COMPLETE,
I8 target_cpu_mask=ALL_MASK)
//Set flags to zero if you wish to
//get the result.
{
return MPQueueJob(add,data,NULL,flags,FALSE,target_cpu_mask);
}
I8 MPJobResult(MPCmdStruct *tempm)
{
I8 result;
while (!Bt(&tempm->flags,MPCf_DONE))
SwapInNextTask;
while (LBts(&mp_ctrl->flags,MPCCf_LOCKED))
SwapInNextTask;
RemQue(tempm);
LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
result=tempm->result;
Free(tempm->desc);
Free(tempm);
return result;
}
TssStruct *MPSpawn(void *add,I8 data=0,I1 *desc=NULL,I8 target_cpu=ALL_MASK)
{
TssStruct *result;
MPCmdStruct *tempm=MPQueueJob(add,data,desc,0,TRUE,target_cpu);
while (!tempm->tss)
SwapInNextTask;
result=tempm->tss;
while (LBts(&mp_ctrl->flags,MPCCf_LOCKED))
SwapInNextTask;
RemQue(tempm);
LBtr(&mp_ctrl->flags,MPCCf_LOCKED);
Free(tempm->desc);
Free(tempm);
return result;
}
void MPInitCPUTask()
{
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;
CPUCachedStruct *c;
CPUUncachedStruct *uc;
BlkPool *bp,*saved_bp;
I8 shared_blks;
MPCmdStruct *tempm,*tempm1;
PushFD;
Cli;
if (mp_cnt>1) {
MPHalt;
BusyWait(10000);
tempm=mp_ctrl->next_waiting;
while (tempm!=mp_ctrl) {
tempm1=tempm->next;
RemQue(tempm);
Free(tempm->desc);
Free(tempm);
tempm=tempm1;
}
tempm=mp_ctrl->next_done;
while (tempm!=&mp_ctrl->next_done) {
tempm1=tempm->next;
RemQue(tempm);
Free(tempm->desc);
Free(tempm);
tempm=tempm1;
}
mp_cnt=1;
}
MemSet(&cpu_cached[1],0,sizeof(CPUCachedStruct)*(MP_MAX_PROCESSORS-1));
MemSet(&cpu_uncached[1],0,sizeof(CPUUncachedStruct)*(MP_MAX_PROCESSORS-1));
MemCpy(MP_VECTOR_ADDRESS,MP_INIT_START,MP_INIT_END-MP_INIT_START);
mp->sys_temp_ptr=MAXGDT*16-1+gdttab><(U1 *)<<16;
mp_cnt=1;
mp_cnt_lock=0;
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);
for (i=1;i<mp_cnt;i++) {
c =&cpu_cached[i];
uc=&cpu_uncached[i];
SPrintF(buf,"Seth Task CPU#%d",i);
shared_blks=1; //One 2Meg blk
bp=Alloc2MegMemBlks(&shared_blks,sys_code_bp);
BlkPoolInit(bp,shared_blks<<12);
saved_bp=Gs->code_bp;
Gs->code_bp=bp;
tss=Spawn(&MPInitCPUTask,0,buf,NULL,NULL,DEFAULT_STACK,FALSE);
tss->in_queue_signature=TSSS_IN_QUEUE_SIGNATURE;
InitCPUCachedStruct(i,c);
c->uncached_address=InitCPUUncachedStruct(uc);
c->code_bp=bp;
Gs->code_bp=saved_bp;
c->tr=InitRealTssStruct;
c->seth_tss=tss;
WbInvd;
}
PopFD;
}
void MPInit()
{
I8 shared_blks=1; //One 2Meg blk
TssStruct *tss;
RaxRbxRcxRdx ee;
CPUCachedStruct *c;
BlkPool *bp;
CpuId(0x1,&ee);
mp_cnt=1;
mp_cnt_lock=0;
WbInvd;
bp=AllocUncachedMemBlks(&shared_blks);
mp_heap=IndependentHeapCtrlInit(bp,shared_blks<<12);
mp_ctrl=MAllocHCZ(sizeof(MPCmdCtrl),mp_heap);
mp_ctrl->next_waiting=mp_ctrl->last_waiting=mp_ctrl;
mp_ctrl->next_done=mp_ctrl->last_done=&mp_ctrl->next_done;
mp_crash=MAllocHCZ(sizeof(MPCrashStruct),mp_heap);
cpu_cached =MAllocHCZ(sizeof(CPUCachedStruct)*MP_MAX_PROCESSORS,adam_tss->code_heap);
cpu_uncached=MAllocHCZ(sizeof(CPUUncachedStruct)*MP_MAX_PROCESSORS,mp_heap);
c=cpu_cached;
MemCpy(c,&sys_temp_cpu0_struct,sizeof(CPUCachedStruct));
c->cached_address =cpu_cached;
c->uncached_address=InitCPUUncachedStruct(cpu_uncached);
SetGs(c);
tss=adam_tss;
tss->time_slice_start=GetTimeStamp;
tss->in_queue_signature=TSSS_IN_QUEUE_SIGNATURE;
c->seth_tss=tss;
c->code_bp=sys_code_bp;
c->data_bp=sys_data_bp;
c->tr=InitRealTssStruct;
c->idle_tss=Spawn(0,NULL,"Idle",Fs,NULL,DEFAULT_STACK,FALSE);
if (Bt(&ee.rdx,9))
MPInitAPIC;
}
asm {MP_CRASH::}
void MPCrash()
{
MPEOI;
mp_cnt=1;
Raw(ON);
dc_flags|=DCF_SHOW_DOLLAR;
coutln "MP Crash CPU#",mp_crash->cpu_num," Tss:",mp_crash->tss;
WbInvd;
Debugger(mp_crash->msg,mp_crash->msg_num);
}