Subversion Repositories freemyipod

//

//

//    Copyright 2010 TheSeven

//

//

//    This file is part of emBIOS.

//

//    emBIOS is free software: you can redistribute it and/or

//    modify it under the terms of the GNU General Public License as

//    published by the Free Software Foundation, either version 2 of the

//    License, or (at your option) any later version.

//

//    emBIOS is distributed in the hope that it will be useful,

//    but WITHOUT ANY WARRANTY; without even the implied warranty of

//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

//    See the GNU General Public License for more details.

//

//    You should have received a copy of the GNU General Public License along

//    with emBIOS.  If not, see <http://www.gnu.org/licenses/>.

//

//

#include "global.h"

#include "thread.h"

#include "timer.h"

#include "panic.h"

#include "util.h"

struct scheduler_thread scheduler_threads[MAX_THREADS] IBSS_ATTR;

struct scheduler_thread* current_thread IBSS_ATTR;

uint32_t last_tick IBSS_ATTR;

bool scheduler_frozen IBSS_ATTR;

extern struct wakeup dbgwakeup;

void mutex_init(struct mutex* obj)

{

    memset(obj, 0, sizeof(struct mutex));

}

void mutex_add_to_queue(struct mutex* obj, struct scheduler_thread* thread)

{

    struct scheduler_thread* t;

    if (!obj->waiters || obj->waiters->priority <= thread->priority)

    {

        thread->queue_next = obj->waiters;

        obj->waiters = thread;

    }

    else

    {

        t = obj->waiters;

        while (t->queue_next && t->queue_next->priority > thread->priority)

            t = t->queue_next;

        thread->queue_next = t->queue_next;

        t->queue_next = thread;

    }

}

void mutex_remove_from_queue(struct mutex* obj, struct scheduler_thread* thread)

{

    struct scheduler_thread* t;

    if (!obj->waiters) return;

    if (obj->waiters == thread) obj->waiters = thread->queue_next;

    else

    {

        t = obj->waiters;

        while (t->queue_next)

        {

            if (t->queue_next == thread) t->queue_next = thread->queue_next;

            t = t->queue_next;

        }

    }

}

int mutex_lock(struct mutex* obj, int timeout)

{

    int ret = THREAD_OK;

    struct scheduler_thread* thread;

    uint32_t mode = enter_critical_section();

    if (!obj->count)

    {

        obj->count = 1;

        obj->owner = current_thread;

    }

    else if (obj->owner == current_thread) obj->count++;

    else

    {

        if (timeout)

        {

            current_thread->state = THREAD_BLOCKED;

            current_thread->block_type = THREAD_BLOCK_MUTEX;

            current_thread->blocked_by = obj;

            current_thread->timeout = timeout;

            current_thread->blocked_since = USEC_TIMER;

            mutex_add_to_queue(obj, current_thread);

            leave_critical_section(mode);

            context_switch();

            if (obj->owner != current_thread) return THREAD_TIMEOUT;

            return THREAD_OK;

        }

        else ret = THREAD_TIMEOUT;

    }

    leave_critical_section(mode);

    return ret;

}

int mutex_unlock(struct mutex* obj)

{

    int ret = THREAD_OK;

    uint32_t mode = enter_critical_section();

    if (!obj->count)

    {

        leave_critical_section(mode);

        panicf(PANIC_KILLTHREAD, "Trying to unlock non-owned mutex! (%08X)", obj);

    }

    if (obj->owner != current_thread)

    {

        leave_critical_section(mode);

        panicf(PANIC_KILLTHREAD, "Trying to unlock mutex owned by different thread! (%08X)", obj);

    }

    if (--(obj->count)) ret = obj->count;

    else if (obj->waiters)

    {

        obj->count = 1;

        obj->owner = obj->waiters;

        obj->waiters->state = THREAD_READY;

        obj->waiters->block_type = THREAD_NOT_BLOCKED;

        obj->waiters->blocked_by = NULL;

        obj->waiters->timeout = 0;

        obj->waiters = obj->waiters->queue_next;

    }

    leave_critical_section(mode);

    return ret;

}

void wakeup_init(struct wakeup* obj)

{

    memset(obj, 0, sizeof(struct wakeup));

}

int wakeup_wait(struct wakeup* obj, int timeout)

{

    int ret = THREAD_OK;

    uint32_t mode = enter_critical_section();

    if (obj->waiter)

    {

        leave_critical_section(mode);

        panicf(PANIC_KILLTHREAD, "Multiple threads waiting single wakeup! (%08X)", obj);

    }

    if (obj->signalled) obj->signalled = false;

    else

    {

        if (timeout)

        {

            current_thread->state = THREAD_BLOCKED;

            current_thread->block_type = THREAD_BLOCK_WAKEUP;

            current_thread->blocked_by = obj;

            current_thread->timeout = timeout;

            current_thread->blocked_since = USEC_TIMER;

            obj->waiter = current_thread;

            leave_critical_section(mode);

            context_switch();

            obj->waiter = NULL;

            if (!obj->signalled) return THREAD_TIMEOUT;

            obj->signalled = false;

            return THREAD_OK;

        }

        else ret = THREAD_TIMEOUT;

    }

    leave_critical_section(mode);

    return ret;

}

int wakeup_signal(struct wakeup* obj)

{

    int ret = THREAD_OK;

    uint32_t mode = enter_critical_section();

    obj->signalled = true;

    if (obj->waiter)

    {

        obj->waiter->state = THREAD_READY;

        obj->waiter->block_type = THREAD_NOT_BLOCKED;

        obj->waiter->blocked_by = NULL;

        obj->waiter->timeout = 0;

        ret = THREAD_FOUND;

    }

    leave_critical_section(mode);

    return ret;

}

void sleep(int usecs)

{

    if (usecs)

    {

        uint32_t mode = enter_critical_section();

        current_thread->state = THREAD_BLOCKED;

        current_thread->block_type = THREAD_BLOCK_SLEEP;

        current_thread->timeout = usecs;

        current_thread->blocked_since = USEC_TIMER;

        leave_critical_section(mode);

    }

    context_switch();

}

void scheduler_init(void)

{

    memset(scheduler_threads, 0, sizeof(scheduler_threads));

    scheduler_frozen = false;

    last_tick = USEC_TIMER;

    current_thread = scheduler_threads;

    current_thread->state = THREAD_RUNNING;

    current_thread->startusec = last_tick;

    current_thread->name = "idle thread";

    current_thread->stack = (uint32_t*)-1;

    setup_tick();

}

bool scheduler_freeze(bool value)

{

    bool old = scheduler_frozen;

    scheduler_frozen = value;

    return old;

}

void scheduler_switch(int thread)

{

    int i;

    uint32_t score, best;

    uint32_t usec = USEC_TIMER;

    if (current_thread->state == THREAD_RUNNING) current_thread->state = THREAD_READY;

    current_thread->cputime_total += usec - current_thread->startusec;

    current_thread->cputime_current += usec - current_thread->startusec;

    if ((int)current_thread->stack != -1 && *current_thread->stack != 0xaffebeaf)

    {

        for (i = 0; i < MAX_THREADS; i++)

            if (scheduler_threads[i].type == USER_THREAD)

                scheduler_threads[i].state = THREAD_SUSPENDED;

        current_thread->state = THREAD_DEFUNCT;

        current_thread->block_type = THREAD_DEFUNCT_STKOV;

        wakeup_signal(&dbgwakeup);

    }

    if (usec - last_tick > SCHEDULER_TICK)

    {

        last_tick = usec;

        for (i = 0; i < MAX_THREADS; i++)

        {

            scheduler_threads[i].cpuload = scheduler_threads[i].cputime_current / SCHEDULER_TICK;

            scheduler_threads[i].cputime_current = 0;

        }

    }

    if (scheduler_frozen) thread = 0;

    else

    {

        for (i = 0; i < MAX_THREADS; i++)

            if (scheduler_threads[i].state == THREAD_BLOCKED

             && scheduler_threads[i].timeout != -1

             && TIME_AFTER(usec, scheduler_threads[i].blocked_since

                               + scheduler_threads[i].timeout))

            {

                if (scheduler_threads[i].block_type == THREAD_BLOCK_MUTEX)

                    mutex_remove_from_queue((struct mutex*)scheduler_threads[i].blocked_by,

                                            &scheduler_threads[i]);

                scheduler_threads[i].state = THREAD_READY;

                scheduler_threads[i].block_type = THREAD_NOT_BLOCKED;

                scheduler_threads[i].blocked_by = NULL;

                scheduler_threads[i].timeout = 0;

            }

        if (thread >= 0 && thread < MAX_THREADS && scheduler_threads[thread].state == THREAD_READY)

            current_thread = &scheduler_threads[thread];

        else

        {

            thread = 0;

            best = 0xffffffff;

            for (i = 0; i < MAX_THREADS; i++)

                if (scheduler_threads[i].state == THREAD_READY && scheduler_threads[i].priority)

                {

                    score = scheduler_threads[i].cputime_current / scheduler_threads[i].priority;

                    if (score < best)

                    {

                        best = score;

                        thread = i;

                    }

                }

        }

    }

    current_thread = &scheduler_threads[thread];

    current_thread->state = THREAD_RUNNING;

    current_thread->startusec = USEC_TIMER;

}

int thread_create(const char* name, const void* code, void* stack,

                  int stacksize, enum thread_type type, int priority, bool run)

{

    int ret = NO_MORE_THREADS;

    int i;

    for (i = 0; i < stacksize >> 2; i ++) ((uint32_t*)stack)[i] = 0xaffebeaf;

    uint32_t mode = enter_critical_section();

    for (i = 0; i < MAX_THREADS; i++)

        if (scheduler_threads[i].state == THREAD_FREE)

        {

            ret = i;

            memset(&scheduler_threads[i], 0, sizeof(struct scheduler_thread));

            scheduler_threads[i].state = run ? THREAD_READY : THREAD_SUSPENDED;

            scheduler_threads[i].type = type;

            scheduler_threads[i].name = name;

            scheduler_threads[i].priority = priority;

            scheduler_threads[i].cpsr = 0x1f;

            scheduler_threads[i].regs[15] = (uint32_t)code;

            scheduler_threads[i].regs[14] = (uint32_t)thread_exit;

            scheduler_threads[i].regs[13] = (uint32_t)stack + stacksize;

            scheduler_threads[i].stack = stack;

            break;

        }

    leave_critical_section(mode);

    return ret;

}

int thread_suspend(int thread)

{

    int ret = THREAD_OK;

    struct scheduler_thread* t = &scheduler_threads[thread];

    bool needsswitch = false;

    uint32_t mode = enter_critical_section();

    if (thread == -1) t = current_thread;

    else if (thread < 0 || thread >= MAX_THREADS) ret = INVALID_THREAD;

    else if (t->state == THREAD_FREE) ret = INVALID_THREAD;

    else if (t->state == THREAD_SUSPENDED) ret = ALREADY_SUSPENDED;

    if (ret == THREAD_OK)

    {

        if (t->state == THREAD_RUNNING) needsswitch = true;

        else if (t->state == THREAD_BLOCKED)

        {

            if (t->block_type == THREAD_BLOCK_SLEEP)

            {

                if (t->timeout != -1) t->timeout -= USEC_TIMER - t->blocked_since;

            }

            else if (t->block_type == THREAD_BLOCK_MUTEX)

            {

                mutex_remove_from_queue((struct mutex*)t->blocked_by, t);

                if (t->timeout != -1) t->timeout -= USEC_TIMER - t->blocked_since;

            }

            else if (t->block_type == THREAD_BLOCK_WAKEUP)

            {

                if (t->timeout != -1) t->timeout -= USEC_TIMER - t->blocked_since;

            }

        }

        t->state = THREAD_SUSPENDED;

    }

    leave_critical_section(mode);

    if (needsswitch) context_switch();

    return ret;

}

int thread_resume(int thread)

{

    int ret = THREAD_OK;

    struct scheduler_thread* t = &scheduler_threads[thread];

    bool needsswitch = false;

    uint32_t mode = enter_critical_section();

    if (thread == -1) t = current_thread;

    else if (thread < 0 || thread >= MAX_THREADS) ret = INVALID_THREAD;

    else if (t->state == THREAD_FREE) ret = INVALID_THREAD;

    else if (t->state != THREAD_SUSPENDED) ret = ALREADY_RESUMED;

    if (ret == THREAD_OK)

    {

        if (t->block_type == THREAD_BLOCK_SLEEP)

            t->blocked_since = USEC_TIMER;

        else if (t->block_type == THREAD_BLOCK_MUTEX)

        {

            mutex_add_to_queue((struct mutex*)t->blocked_by, t);

            t->blocked_since = USEC_TIMER;

            t->state = THREAD_BLOCKED;

        }

        else if (t->block_type == THREAD_BLOCK_WAKEUP)

        {

            t->blocked_since = USEC_TIMER;

            t->state = THREAD_BLOCKED;

        }

        else t->state = THREAD_READY;

    }

    leave_critical_section(mode);

    return ret;

}

int thread_terminate(int thread)

{

    int ret = THREAD_OK;

    struct scheduler_thread* t = &scheduler_threads[thread];

    bool needsswitch = false;

    uint32_t mode = enter_critical_section();

    if (thread == -1) t = current_thread;

    else if (thread < 0 || thread >= MAX_THREADS) ret = INVALID_THREAD;

    else if (t->state == THREAD_FREE) ret = INVALID_THREAD;

    if (ret == THREAD_OK)

    {

        if (t->state == THREAD_RUNNING) needsswitch = true;

        else if (t->state == THREAD_BLOCKED)

        {

            if (t->block_type == THREAD_BLOCK_MUTEX)

                mutex_remove_from_queue((struct mutex*)t->blocked_by, t);

            else if (t->block_type == THREAD_BLOCK_WAKEUP)

                ((struct wakeup*)t->blocked_by)->waiter = NULL;

        }

        t->state = THREAD_FREE;

    }

    leave_critical_section(mode);

    if (needsswitch) context_switch();

    return ret;

}

void thread_exit()

{

    thread_terminate(-1);

}