OSDev

static inline void longJump(unsigned int selector, unsigned int offset) __attribute__((noreturn));

static inline void longJump(unsigned int selector, unsigned int offset) {
  asm volatile("pushl %r\npushl %r\nlret"::"r"(selector), "r"(offset));
}

short main_tss_d = create_task(0,0,0,0,0,0,0);
asm("ltr %w0"::"a"(main_tss_d*8));
set_busy(main_tss_d,0);
asm("mov %w0, _taskaddr"::"a"(main_tss_d*8));
asm(".byte 0xEA\n.word 0x0000\n_taskaddr:\n.word 0x0000");
set_busy(main_tss_d,1);
printstr("Multitasking initiaized\n");

asm("ltr %w0"::"a"(main_tss_d*8));

typedef struct{
   unsigned short   limit_low, address_low;
   unsigned char   address_3, access_rights, limit_hi_and_flags, address_4;
} GD, *PGD;
PGD gdt;
void set_busy(short seg, char state){
   if((state & 1) == 1)
      gdt[seg].access_rights |= 2;
   else
      gdt[seg].access_rights &= 0xFD;
}
void set_descriptor(unsigned short id, unsigned int limit, unsigned int address, unsigned char access_rights, unsigned char flags){
   gdt[id].limit_low = limit & 0xFFFF;
   gdt[id].limit_hi_and_flags = ((limit >> 16) & 0xF) | ((flags & 0xF) << 4);
   gdt[id].address_low = address & 0xFFFF;
   gdt[id].address_3 = (address >> 16) & 0xFF;
   gdt[id].address_4 = (address >> 24) & 0xFF;
   gdt[id].access_rights = access_rights;
}
short create_task(int cr3, int eip, int eflags, int esp, short cs, short ss, short ds){
   PTSS tss = malloc(sizeof(TSS));
   short gdti = 6; // Just for debug
   set_descriptor(gdti,sizeof(TSS)-1,(unsigned int)tss,0x85,0);
   mfill(tss, sizeof(TSS), 0);
   tss->cr3 = cr3;
   tss->eip = eip;
   tss->eflags = eflags;
   tss->esp = esp;
   tss->cs = cs;
   tss->ss = ss;
   tss->ds = ds;
   return gdti;
}

.type interrupt_common, @function
.func interrupt_common
interrupt_common:
/*
 * CPU pushes:
 *  ss
 *  rsp
 *  rflags
 *  cs
 *  rip
 *  error code (if present)
 * Entry also pushes:
 *  interrupt number
 */

    pushq   %rax
    pushq   %rbx
    pushq   %rcx
    pushq   %rdx
    pushq   %rbp
    pushq   %rsi
    pushq   %rdi
    pushq   %r8
    pushq   %r9
    pushq   %r10
    pushq   %r11
    pushq   %r12
    pushq   %r13
    pushq   %r14
    pushq   %r15

// rdi - context pointer
    movq    %rsp, %rdi
    callq   InterruptDispatcher
// if nonzero, pop error code
    testq   %rax, %rax

    popq    %r15
    popq    %r14
    popq    %r13
    popq    %r12
    popq    %r11
    popq    %r10
    popq    %r9
    popq    %r8
    popq    %rdi
    popq    %rsi
    popq    %rbp
    popq    %rdx
    popq    %rcx
    popq    %rbx
    popq    %rax
    jz      1f
    addq    $8, %rsp
1:
    addq    $8, %rsp
    iretq
.endfunc
.size interrupt_common, . - interrupt_common