ASM Code:
extern _oldesp, _curresp, _ktss

[global _task_switch]
_task_switch:
        pushf                   ; Save registers
        pusha

        mov eax, [_oldesp]      ; Save old esp
        mov [eax], esp

        mov esp, [_curresp]     ; Switch to the new task

        popa                    ; Restore registers
        popf
        ret                     ; Not doing an iret here because we return to the ISR handler which iret's for us


[global _load_tss]
_load_tss:
        ltr     [_ktss]
        ret

C code - init tasks system:
void tasks_init() {
        int ret;

        /* If this is called twice it's a programming error. Make sure they
         * know about it */
        if(enabled)
                panic("Attempted to called tasks_init twice!");

        /* Initialize the tasks list */
        procs = list_new();

        /* Initialize the TSS */
        memset(&ktss, 0, sizeof(ktss));

        if(!kprocsdone) {
                /* Create the kernel process */
                printf("tasks: creating kernel process...\n");
                debug("tasks", "creating kernel process...");

                ret = exec("kernel", (entry_t)kernel, PRIORITIES);
                if(ret != 0) {
                        if(ret == -1) {
                                panic("Failed to initialize kernel process!");
                        } else {
                                panic("Kernel process did not get PID 0!");
                        }
                }

                /* Set the current process */
                LIST_FOREACH (procs, iter) {
                        process_t *myproc = (process_t *)iter;
                        currproc = myproc;
                }
        }

        /* Set up the currproc and the TSS for the new process */
        curresp = currproc->esp;
        oldesp = &kstack;

        ktss.ss0 = 0x10;
        ktss.esp0 = currproc->esp;

        /* Switch into multitasking mode. The only chance we have of getting
         * past here is if tasks_deinit is called */
        kprocsdone = 1;
        enabled = 1;
        load_tss();
        task_switch();
}

C code - switch tasks:
void schedule() {
        process_t *newproc;

        if(enabled) {
                /* Decrease the time to run */
                currproc->timetorun--;

                /* Check if the process has used up it's timeslice */
                if(currproc->timetorun == 0) {
                        /* Basically this gives a process a timeslice the size
                         * of it's priority. So if the priority is 8, the
                         * process will get 8 ticks each time it runs */
                        currproc->timetorun = currproc->priority;

                        /* Find a new process to run. If there is only one
                         * process, don't switch */
                        LIST_FOREACH (procs, iter) {
                                newproc = (process_t *)iter;

                                if(newproc->pid > currproc->pid) {
                                        if (newproc->running)
                                                break;
                                }
                                newproc = NULL;
                        }

                        /* Grab the first entry in the list (should be the kernel process) */
                        if(newproc == NULL)
                                newproc = (process_t *)procs->next;

                        if(newproc->pid == currproc->pid)
                                return;

                        /* Save the address to the old process's ESP */
                        oldesp = &currproc->esp;

                        /* Change the TSS */
                        ktss.esp0 = newproc->esp;
                        load_tss();

                        /* Set currproc to the new process, and switch */
                        currproc = newproc;
                        curresp = newproc->esp;
                        task_switch();
                }
        }
}

TSS struct:
typedef struct {
        uint16_t backlink, __blh;
        uint32_t esp0;
        uint16_t ss0, __ss0h;
        uint32_t esp1;
        uint16_t ss1, __ss1h;
        uint32_t esp2;
        uint16_t ss2, __ss2h;
        uint32_t cr3;
        uint32_t eip;
        uint32_t eflags;
        uint32_t eax, ecx, edx, ebx;
        uint32_t esp, ebp, esi, edi;
        uint16_t es, __esh;
        uint16_t cs, __csh;
        uint16_t ss, __ssh;
        uint16_t ds, __dsh;
        uint16_t fs, __fsh;
        uint16_t gs, __gsh;
        uint16_t ldt, __ldth;
        uint16_t trace, bitmap;
} tss_t;