hschumann2/TempleOS-Source-Code

1 2asm {3        ALIGN   16,OC_NOP4USE165//See TempleOS MultiCore.6 7//This code gets copied to MP_VECT_ADDR.8//See MemCpy(MP_VECT_ADDR.9COREAP_16BIT_INIT::10        JMP     @@0511 12        ALIGN   4,OC_NOP13AP_GDT_PTR:     DU16    sizeof(CGDT)-1;14                DU64    0;15 16@@05:   CLI17        WBINVD18        MOV     AX,MP_VECT_ADDR/1619        MOV     DS,AX20        LGDT    U32 [CAP16BitInit.ap_gdt_ptr]  //See mp->ap_gdt_ptr21 22        MOV     EAX,SYS_START_CR023        MOV_CR0_EAX24        DU8     0x66,0xEA;               //JMP CGDT.cs32:AP_32BIT_INIT25        DU32    AP_32BIT_INIT;26        DU16    CGDT.cs32;27COREAP_16BIT_INIT_END::28 29USE3230AP_32BIT_INIT:31        MOV     AX,CGDT.ds32        MOV     DS,AX33        MOV     ES,AX34        MOV     FS,AX35        MOV     GS,AX36        MOV     SS,AX37 38@@05:   LOCK39        BTS     U32 [SYS_MP_CNT_LOCK],040        JC      @@0541 42        MOV     ESI,U32 [SYS_MP_CNT_INITIAL]43        LOCK44        INC     U32 [SYS_MP_CNT_INITIAL]45        LOCK46        BTR     U32 [SYS_MP_CNT_LOCK],047 48        CMP     ESI,MP_PROCESSORS_NUM49        JAE     I32 _SYS_HLT50 51        IMUL2   ESI,sizeof(CCPU)52        ADD     ESI,U32 [SYS_CPU_STRUCTS]53 54        LEA     ESP,U32 CCPU.start_stk+sizeof(CCPU.start_stk)[ESI]55        PUSH    U32 RFLAGG_START56        POPFD57        PUSH    U32 0   //Return from next call will be 64-bit58        CALL    SYS_ENTER_LONG_MODE59USE6460        FNINIT61        MOV     RAX,RSI62        CALL    SET_GS_BASE63@@10:   MOV     RAX,U64 CCPU.seth_task[RSI]64        TEST    RAX,RAX65        JZ      @@1066        MOV     U64 CTask.gs[RAX],RSI67        CALL    SET_FS_BASE68 69        JMP     I32 _TASK_CONTEXT_RESTORE70}71 72U0 TSSBusy(I64 tr,Bool val=OFF)73{//See ::/Demo/Lectures/Ring3.HC.74  LBEqu((&sys_gdt)(U8 *)+tr+4,9,val);75}76 77CTSS *TSSNew(I64 cpu_num)78{79  U32 *d,*d1;80  CTSS *tss=CAlloc(sizeof(CTSS));81  tss->io_map_offset=offset(CTSS.io_map);82  MemSet(tss->io_map,0xFF,0x10000/8);83 84  tss->st0=MAlloc(MEM_INTERRUPT_STK);85  tss->rsp0=tss->st0(U8 *)+MSize(tss->st0);86  tss->st1=MAlloc(MEM_INTERRUPT_STK);87  tss->rsp1=tss->st1(U8 *)+MSize(tss->st1);88  tss->st2=MAlloc(MEM_INTERRUPT_STK);89  tss->rsp2=tss->st2(U8 *)+MSize(tss->st2);90 91  tss->tr      =offset(CGDT.tr)+cpu_num*16;92  tss->tr_ring3=offset(CGDT.tr_ring3)+cpu_num*16;93 94  d=(&sys_gdt)(U8 *)+tss->tr;95  d1=d(U8 *)+4;96  *d =0x0000FFFF;97  *d1=0x008F8900;98  d(U8 *)+=2;99  *d|=tss & 0x00FFFFFF;100  *d1++|=tss & 0xFF000000;101  *d1++=tss>>32;102  *d1=0;103 104  d=(&sys_gdt)(U8 *)+tss->tr_ring3;105  d1=d(U8 *)+4;106  *d =0x0000FFFF;107  *d1=0x008FE900;108  d(U8 *)+=2;109  *d|=tss & 0x00FFFFFF;110  *d1++|=tss & 0xFF000000;111  *d1++=tss>>32;112  *d1=0;113 114  return tss;115}116 117CCPU *CPUStructInit(I64 num,CCPU *c,CTask *seth_task)118{//Seth is null when called by adam on CSysFixedArea.boot_cpu0119  MemSet(c,0,sizeof(CCPU));120  c->addr=c;121  c->num=num;122  c->idle_factor=0.01;123  QueInit(&c->next_dying);124  if (Bt(&sys_run_level,RLf_16MEG_ADAM_HEAP_CTRL)) {125    c->idle_task=Spawn(0,NULL,"Idle Task",,Fs,,0);126    LBts(&c->idle_task->task_flags,TASKf_IDLE);127    c->tss=TSSNew(num);128  }129  c->seth_task=seth_task;// It waits for this to be filled-in: seth_task130  return c;131}132 133U0 MPInt(U8 num,I64 cpu_num=1)134{//Generate interrupt for specified core.135  if (cpu_num>=mp_cnt) {136    if (!Bt(&sys_run_level,RLf_MP))137      return;138    else139      throw('MultCore');140  }141  PUSHFD142  CLI //Multitasking safe because each core has a local apic and IRQ's are off143  while (*(dev.uncached_alias+LAPIC_ICR_LOW)(U32 *)&0x1000)144    PAUSE145  *(dev.uncached_alias+LAPIC_ICR_HIGH)(U32 *)=dev.mp_apic_ids[cpu_num]<<24;146  *(dev.uncached_alias+LAPIC_ICR_LOW)(U32 *)=0x4000+num;147  POPFD148}149 150U0 MPIntAll(U8 num)151{//Generate interrupt for all but own core.152  PUSHFD153  CLI //Multitasking safe because each core has a local apic and IRQ's are off154  while (*(dev.uncached_alias+LAPIC_ICR_LOW)(U32 *)&0x1000)155    PAUSE156  *(dev.uncached_alias+LAPIC_ICR_LOW)(U32 *)=0xC4800+num;157  POPFD158}159 160U0 MPNMInt()161{//Generate nonmaskable interrupt.162  *(dev.uncached_alias+LAPIC_ICR_LOW)(U32 *)=0xC4400;163}164 165U0 MPHalt()166{//Halt all other cores.167  mp_cnt=1;168  MPNMInt;169  Busy(10000);170}171 172U0 MPAPICInit()173{//Called by adam during start-up174//and other cores during initialization175  //after Core0StartMP().176  *(dev.uncached_alias+LAPIC_SVR)(U32 *)|=LAPICF_APIC_ENABLED;177  dev.mp_apic_ids[Gs->num]=*(dev.uncached_alias+LAPIC_APIC_ID)(U32 *)>>24;178  *(dev.uncached_alias+LAPIC_LDR)(U32 *)=dev.mp_apic_ids[Gs->num]<<24;179  *(dev.uncached_alias+LAPIC_DFR)(U32 *)=0xF0000000;180 181  //  MemSet(dev.uncached_alias+LAPIC_IRR,0,0x20);182  //  MemSet(dev.uncached_alias+LAPIC_ISR,0,0x20);183  //  MemSet(dev.uncached_alias+LAPIC_TMR,0,0x20);184 185  SetRAX(Gs->tss->tr);186  LTR   AX187  if (Gs->num) {188    IntInit1;189    SetRFlags(RFLAGG_NORMAL);190  }191}192 193#assert !offset(CJobCtrl.next_waiting)194 195U0 CoreAPSethTask()196{197  CJobCtrl *ctrl=&Fs->srv_ctrl;198  while (TRUE) {199    STI200    do {201      TaskKillDying;202      do PAUSE203      while (LBts(&ctrl->flags,JOBCf_LOCKED));204    } while (ctrl->next_waiting!=ctrl && JobRunOne(GetRFlags,ctrl));205    CLI206    LBts(&Fs->task_flags,TASKf_AWAITING_MSG);207    LBtr(&ctrl->flags,JOBCf_LOCKED);208    LBts(&Fs->task_flags,TASKf_IDLE);209    Yield;210    LBtr(&Fs->task_flags,TASKf_IDLE);211  }212}213 214CJob *JobQue(I64 (*fp_addr)(U8 *data),U8 *data=NULL,215       I64 target_cpu=1,I64 flags=1<<JOBf_FREE_ON_COMPLETE,216       I64 job_code=JOBT_CALL,U8 *aux_str=NULL,I64 aux1=0,I64 aux2=0)217{//Queue multicore jobs, handled by Seth tasks.218//Set flags to zero if you wish to get the res.219  //See ::/Demo/MultiCore/Lock.HC220  CJobCtrl *ctrl;221  CJob *tmpc;222  CTask *seth;223  if (!(0<=target_cpu<mp_cnt))224    throw('MultCore');225  tmpc=ACAlloc(sizeof(CJob));226  if (aux_str)227    tmpc->aux_str=AStrNew(aux_str);228  tmpc->job_code=job_code;229  tmpc->addr=fp_addr;230  tmpc->fun_arg=data;231  tmpc->flags=flags;232  tmpc->aux1=aux1;233  tmpc->aux2=aux2;234  seth=cpu_structs[target_cpu].seth_task;235  tmpc->ctrl=ctrl=&seth->srv_ctrl;236  PUSHFD237  CLI238  while (LBts(&ctrl->flags,JOBCf_LOCKED))239    Yield;240  if (ctrl->next_waiting==ctrl && LBtr(&seth->task_flags,TASKf_AWAITING_MSG))241    MPInt(I_WAKE,target_cpu);242  QueIns(tmpc,ctrl->last_waiting);243  LBtr(&ctrl->flags,JOBCf_LOCKED);244  POPFD245  return tmpc;246}247 248CTask *SpawnQue(U0 (*fp_addr)(U8 *data),U8 *data=NULL,U8 *task_name=NULL,249        I64 target_cpu, CTask *parent=NULL, //NULL means adam250        I64 stk_size=0,I64 flags=1<<JOBf_ADD_TO_QUE)251{252  CTask *res;253  CJob *tmpc=JobQue(fp_addr,data,target_cpu,254        flags,JOBT_SPAWN_TASK,task_name,parent,stk_size);255  CJobCtrl *ctrl;256 257  while (!Bt(&tmpc->flags,JOBf_DONE)) {258    LBts(&Fs->task_flags,TASKf_IDLE);259    Yield;260  }261  LBtr(&Fs->task_flags,TASKf_IDLE);262 263  res=tmpc->spawned_task;264  ctrl=tmpc->ctrl;265  PUSHFD266  CLI267  while (LBts(&ctrl->flags,JOBCf_LOCKED))268    Yield;269  QueRem(tmpc);270  LBtr(&ctrl->flags,JOBCf_LOCKED);271  POPFD272  JobDel(tmpc);273  return res;274}275 276U0 CoreAPSethInit()277{//Called by multicore's seth task after Core0StartMP()278//as the first thing a CPU does before waiting for jobs.279  MPAPICInit;280  Fs->rip=&CoreAPSethTask;281  TaskContextRestore;282}283 284U0 Core0StartMP()285{//Called by adam during start-up.286  CTask *task;287  U8 buf[STR_LEN];288  CAP16BitInit *mp=MP_VECT_ADDR;289  CCPU *c;290  I64 i,my_mp_cnt;291  CRAXRBCRCXRDX ee;292 293  CPUId(0x1,&ee);294  if (!Bt(&ee.rdx,9))295    return;296 297  PUSHFD298  CLI299  if (mp_cnt>1) {300    my_mp_cnt=mp_cnt;301    MPHalt; //sets mp_cnt to 1302    for (i=1;i<my_mp_cnt;i++) {303      c=&cpu_structs[i];304      JobQueDel(&c->seth_task->srv_ctrl.next_waiting);305      JobQueDel(&c->seth_task->srv_ctrl.next_done);306    }307  }308  MemSet(&cpu_structs[1],0,sizeof(CCPU)*(MP_PROCESSORS_NUM-1));309 310  //When you start-up other cores, they jump to an addr311  //specified by a byte vect number, MPN_VECT which corresponds312  //to a location 4096*vect number, MP_VECT_ADDR.313  MemCpy(mp,COREAP_16BIT_INIT,COREAP_16BIT_INIT_END-COREAP_16BIT_INIT);314  MemCpy(&mp->ap_gdt_ptr,SYS_GDT_PTR,sizeof(CSysLimitBase));315  mp_cnt_initial=mp_cnt=1;316  mp_cnt_lock=0;317 318  *(dev.uncached_alias+LAPIC_LVT_ERR)(U32 *)=319        *(dev.uncached_alias+LAPIC_LVT_ERR)(U32 *)&0xFFFFFF00+MPN_VECT;320  WBINVD //Not sure why this is needed. Might just need delay. MemCpy above?321 322  *(dev.uncached_alias+LAPIC_ICR_LOW)(U32 *)=0xC4500; //assert init IPI323  Busy(10000);324 325  *(dev.uncached_alias+LAPIC_ICR_LOW)(U32 *)=0xC4600+MPN_VECT; //start-up326  Busy(200);327  *(dev.uncached_alias+LAPIC_ICR_LOW)(U32 *)=0xC4600+MPN_VECT;328 329  Busy(100000);330  for (i=0;i<10000;i++)331    LBts(&mp_cnt_lock,0); //Don't let more through332  my_mp_cnt=mp_cnt_initial;333 334  if (my_mp_cnt>MP_PROCESSORS_NUM)335    my_mp_cnt=MP_PROCESSORS_NUM;336 337  for (i=1;i<my_mp_cnt;i++) {338    StrPrint(buf,"Seth Task CPU%02X",i);339    task=Spawn(&CoreAPSethInit,NULL,buf,,,MEM_SETH_STK,0);340    task->rflags=RFLAGG_START;341//CTask alloced off this core's seth_task's heap (Which is Adam)342    CPUStructInit(i,&cpu_structs[i],task);343    WBINVD //Not sure why this is needed.  Might just need delay.344  }345 346  //Make sure they're all up-and-running347  for (i=1;i<my_mp_cnt;i++)348    while (!Bt(&cpu_structs[i].seth_task->task_flags,TASKf_AWAITING_MSG))349      PAUSE;350 351  POPFD352  mp_cnt=my_mp_cnt; //Finalize cnt353}354 355U0 Core0Init()356{//Called by adam during start-up357  CRAXRBCRCXRDX ee;358  CPUId(0x1,&ee);359 360  mp_cnt_initial=mp_cnt=1;361  mp_cnt_lock=0;362 363  dbg.mp_crash=ACAlloc(sizeof(CMPCrash));364 365  //Must be in code heap because init code uses 32 bit addr of cpu_struct366  adam_task->gs=cpu_structs=367        CAlloc(sizeof(CCPU)*MP_PROCESSORS_NUM,Fs->code_heap);368  CPUStructInit(0,cpu_structs,adam_task);369  asm {//RAX has GS370    IMPORT SET_GS_BASE;371    CALL SET_GS_BASE372  }373  if (Bt(&ee.rdx,9)) {374//Unnecessary?375    //    SetMSR(IA32_LAPIC_BASE,dev.uncached_alias+LAPIC_BASE+0x900);376    MPAPICInit;377  }378}379 380interrupt U0 IntMPCrash()381{//Entering the debugger from another core causes an interrupt on Core0382//Which calls this routine.383  *(dev.uncached_alias+LAPIC_EOI)(U32 *)=0;384  mp_cnt=1;385  Raw(ON);386  text.raw_flags|=RWF_SHOW_DOLLAR;387  "MP Crash CPU%02X Task:%08X\n"388  "RIP:%P\n",dbg.mp_crash->cpu_num,dbg.mp_crash->task,dbg.mp_crash->rip;389  Panic(dbg.mp_crash->msg,dbg.mp_crash->msg_num);390}391