rockbox/uisimulator/sdl/kernel.c

/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 * $Id$
 *
 * Copyright (C) 2002 by Felix Arends
 *
 * All files in this archive are subject to the GNU General Public License.
 * See the file COPYING in the source tree root for full license agreement.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/

#include <stdlib.h>
#include <SDL.h>
#include <SDL_thread.h>
#include "memory.h"
#include "system-sdl.h"
#include "uisdl.h"
#include "kernel.h"
#include "thread-sdl.h"
#include "thread.h"
#include "debug.h"

/* Condition to signal that "interrupts" may proceed */
static SDL_cond *sim_thread_cond;
/* Mutex to serialize changing levels and exclude other threads while
 * inside a handler */
static SDL_mutex *sim_irq_mtx;
static int interrupt_level = HIGHEST_IRQ_LEVEL;
static int handlers_pending = 0;
static int status_reg = 0;

extern struct core_entry cores[NUM_CORES];

/* Nescessary logic:
 * 1) All threads must pass unblocked
 * 2) Current handler must always pass unblocked
 * 3) Threads must be excluded when irq routine is running
 * 4) No more than one handler routine should execute at a time
 */
int set_irq_level(int level)
{
    SDL_LockMutex(sim_irq_mtx);

    int oldlevel = interrupt_level;

    if (status_reg == 0 && level == 0 && oldlevel != 0)
    {
        /* Not in a handler and "interrupts" are being reenabled */
        if (handlers_pending > 0)
            SDL_CondSignal(sim_thread_cond);
    }

    interrupt_level = level; /* save new level */

    SDL_UnlockMutex(sim_irq_mtx);
    return oldlevel;
}

void sim_enter_irq_handler(void)
{
    SDL_LockMutex(sim_irq_mtx);
    handlers_pending++;

    if(interrupt_level != 0)
    {
        /* "Interrupts" are disabled. Wait for reenable */
        SDL_CondWait(sim_thread_cond, sim_irq_mtx);
    }

    status_reg = 1;
}

void sim_exit_irq_handler(void)
{
    if (--handlers_pending > 0)
        SDL_CondSignal(sim_thread_cond);

    status_reg = 0;
    SDL_UnlockMutex(sim_irq_mtx);
}

bool sim_kernel_init(void)
{
    sim_irq_mtx = SDL_CreateMutex();
    if (sim_irq_mtx == NULL)
    {
        fprintf(stderr, "Cannot create sim_handler_mtx\n");
        return false;
    }

    sim_thread_cond = SDL_CreateCond();
    if (sim_thread_cond == NULL)
    {
        fprintf(stderr, "Cannot create sim_thread_cond\n");
        return false;
    }

    return true;
}

void sim_kernel_shutdown(void)
{
    SDL_DestroyMutex(sim_irq_mtx);
    SDL_DestroyCond(sim_thread_cond);
}

volatile long current_tick = 0;
static void (*tick_funcs[MAX_NUM_TICK_TASKS])(void);

/* This array holds all queues that are initiated. It is used for broadcast. */
static struct event_queue *all_queues[MAX_NUM_QUEUES];
static int num_queues = 0;

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
/* Moves waiting thread's descriptor to the current sender when a
   message is dequeued */
static void queue_fetch_sender(struct queue_sender_list *send,
                               unsigned int i)
{
    struct thread_entry **spp = &send->senders[i];

    if(*spp)
    {
        send->curr_sender = *spp;
        *spp = NULL;
    }
}

/* Puts the specified return value in the waiting thread's return value
   and wakes the thread  - a sender should be confirmed to exist first */
static void queue_release_sender(struct thread_entry **sender,
                                 intptr_t retval)
{
    (*sender)->retval = retval;
    wakeup_thread_no_listlock(sender);
    if(*sender != NULL)
    {
        fprintf(stderr, "queue->send slot ovf: %p\n", *sender);
        exit(-1);
    }
}

/* Releases any waiting threads that are queued with queue_send -
   reply with NULL */
static void queue_release_all_senders(struct event_queue *q)
{
    if(q->send)
    {
        unsigned int i;
        for(i = q->read; i != q->write; i++)
        {
            struct thread_entry **spp =
                &q->send->senders[i & QUEUE_LENGTH_MASK];
            if(*spp)
            {
                queue_release_sender(spp, 0);
            }
        }
    }
}

/* Enables queue_send on the specified queue - caller allocates the extra
   data structure */
void queue_enable_queue_send(struct event_queue *q,
                             struct queue_sender_list *send)
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
    q->send = NULL;
    if(send)
    {
        q->send = send;
        memset(send, 0, sizeof(*send));
    }
    set_irq_level(oldlevel);
}
#endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */

void queue_init(struct event_queue *q, bool register_queue)
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);

    q->read   = 0;
    q->write  = 0;
    thread_queue_init(&q->queue);
#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
    q->send   = NULL; /* No message sending by default */
#endif

    if(register_queue)
    {
        if(num_queues >= MAX_NUM_QUEUES)
        {
            fprintf(stderr, "queue_init->out of queues");
            exit(-1);
        }
        /* Add it to the all_queues array */
        all_queues[num_queues++] = q;
    }

    set_irq_level(oldlevel);
}

void queue_delete(struct event_queue *q)
{
    int i;
    bool found = false;

    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);

    /* Find the queue to be deleted */
    for(i = 0;i < num_queues;i++)
    {
        if(all_queues[i] == q)
        {
            found = true;
            break;
        }
    }

    if(found)
    {
        /* Move the following queues up in the list */
        for(;i < num_queues-1;i++)
        {
            all_queues[i] = all_queues[i+1];
        }
        
        num_queues--;
    }

    /* Release threads waiting on queue head */
    thread_queue_wake(&q->queue);

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
    /* Release waiting threads and reply to any dequeued message
       waiting for one. */
    queue_release_all_senders(q);
    queue_reply(q, 0);
#endif

    q->read = 0;
    q->write = 0;

    set_irq_level(oldlevel);
}

void queue_wait(struct event_queue *q, struct queue_event *ev)
{
    unsigned int rd;
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
    if (q->send && q->send->curr_sender)
    {
        /* auto-reply */
        queue_release_sender(&q->send->curr_sender, 0);
    }
#endif

    if (q->read == q->write)
    {
        do
        {
            cores[CURRENT_CORE].irq_level = oldlevel;
            block_thread(&q->queue);
            oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
        }
        while (q->read == q->write);
    }

    rd = q->read++ & QUEUE_LENGTH_MASK;
    *ev = q->events[rd];

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
    if(q->send && q->send->senders[rd])
    {
        /* Get data for a waiting thread if one */
        queue_fetch_sender(q->send, rd);
    }
#endif

    set_irq_level(oldlevel);
}

void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
    if (q->send && q->send->curr_sender)
    {
        /* auto-reply */
        queue_release_sender(&q->send->curr_sender, 0);
    }
#endif

    if (q->read == q->write && ticks > 0)
    {
        cores[CURRENT_CORE].irq_level = oldlevel;
        block_thread_w_tmo(&q->queue, ticks);
        oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
    }

    if(q->read != q->write)
    {
        unsigned int rd = q->read++ & QUEUE_LENGTH_MASK;
        *ev = q->events[rd];

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
        if(q->send && q->send->senders[rd])
        {
            /* Get data for a waiting thread if one */
            queue_fetch_sender(q->send, rd);
        }
#endif
    }
    else
    {
        ev->id = SYS_TIMEOUT;
    }

    set_irq_level(oldlevel);
}

void queue_post(struct event_queue *q, long id, intptr_t data)
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);

    unsigned int wr = q->write++ & QUEUE_LENGTH_MASK;

    q->events[wr].id   = id;
    q->events[wr].data = data;

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
    if(q->send)
    {
        struct thread_entry **spp = &q->send->senders[wr];

        if(*spp)
        {
            /* overflow protect - unblock any thread waiting at this index */
            queue_release_sender(spp, 0);
        }
    }
#endif

    wakeup_thread(&q->queue);

    set_irq_level(oldlevel);
}

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
{
    int oldlevel = set_irq_level(oldlevel);

    unsigned int wr = q->write++ & QUEUE_LENGTH_MASK;

    q->events[wr].id   = id;
    q->events[wr].data = data;

    if(q->send)
    {
        struct thread_entry **spp = &q->send->senders[wr];

        if(*spp)
        {
            /* overflow protect - unblock any thread waiting at this index */
            queue_release_sender(spp, 0);
        }

        wakeup_thread(&q->queue);

        cores[CURRENT_CORE].irq_level = oldlevel;
        block_thread_no_listlock(spp);
        return thread_get_current()->retval;
    }

    /* Function as queue_post if sending is not enabled */
    wakeup_thread(&q->queue);
    set_irq_level(oldlevel);
    return 0;
}

#if 0 /* not used now but probably will be later */
/* Query if the last message dequeued was added by queue_send or not */
bool queue_in_queue_send(struct event_queue *q)
{
    return q->send && q->send->curr_sender;
}
#endif

/* Replies with retval to any dequeued message sent with queue_send */
void queue_reply(struct event_queue *q, intptr_t retval)
{
    if(q->send && q->send->curr_sender)
    {
        queue_release_sender(&q->send->curr_sender, retval);
    }
}
#endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */

bool queue_empty(const struct event_queue* q)
{
    return ( q->read == q->write );
}

bool queue_peek(struct event_queue *q, struct queue_event *ev)
{
    if (q->read == q->write)
         return false;

    bool have_msg = false;

    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);

    if (q->read != q->write)
    {
        *ev = q->events[q->read & QUEUE_LENGTH_MASK];
        have_msg = true;
    }

    set_irq_level(oldlevel);

    return have_msg;
}

void queue_clear(struct event_queue* q)
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);

    /* fixme: This is potentially unsafe in case we do interrupt-like processing */
#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
    /* Release all thread waiting in the queue for a reply -
       dequeued sent message will be handled by owning thread */
    queue_release_all_senders(q);
#endif
    q->read = 0;
    q->write = 0;

    set_irq_level(oldlevel);
}

void queue_remove_from_head(struct event_queue *q, long id)
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);

    while(q->read != q->write)
    {
        unsigned int rd = q->read & QUEUE_LENGTH_MASK;

        if(q->events[rd].id != id)
        {
            break;
        }

#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
        if(q->send)
        {
            struct thread_entry **spp = &q->send->senders[rd];

            if(*spp)
            {
                /* Release any thread waiting on this message */
                queue_release_sender(spp, 0);
            }
        }
#endif
        q->read++;
    }

    set_irq_level(oldlevel);
}

int queue_count(const struct event_queue *q)
{
    return q->write - q->read;
}

int queue_broadcast(long id, intptr_t data)
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
    int i;
    
    for(i = 0;i < num_queues;i++)
    {
        queue_post(all_queues[i], id, data);
    }

    set_irq_level(oldlevel);   
    return num_queues;
}

void yield(void)
{
    switch_thread(NULL);
}

void sleep(int ticks)
{
    sleep_thread(ticks);
}

void sim_tick_tasks(void)
{
    int i;

    /* Run through the list of tick tasks */
    for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
    {
        if(tick_funcs[i])
        {
            tick_funcs[i]();
        }
    }
}

int tick_add_task(void (*f)(void))
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
    int i;

    /* Add a task if there is room */
    for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
    {
        if(tick_funcs[i] == NULL)
        {
            tick_funcs[i] = f;
            set_irq_level(oldlevel);
            return 0;
        }
    }
    fprintf(stderr, "Error! tick_add_task(): out of tasks");
    exit(-1);
    return -1;
}

int tick_remove_task(void (*f)(void))
{
    int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
    int i;

    /* Remove a task if it is there */
    for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
    {
        if(tick_funcs[i] == f)
        {
            tick_funcs[i] = NULL;
            set_irq_level(oldlevel);
            return 0;
        }
    }

    set_irq_level(oldlevel);
    return -1;
}

/* Very simple mutex simulation - won't work with pre-emptive
   multitasking, but is better than nothing at all */
void mutex_init(struct mutex *m)
{
    m->queue = NULL;
    m->thread = NULL;
    m->count = 0;
    m->locked = 0;
}

void mutex_lock(struct mutex *m)
{
    struct thread_entry *const thread = thread_get_current();

    if(thread == m->thread)
    {
        m->count++;
        return;
    }

    if (!test_and_set(&m->locked, 1))
    {
        m->thread = thread;
        return;
    }

    block_thread_no_listlock(&m->queue);
}

void mutex_unlock(struct mutex *m)
{
    /* unlocker not being the owner is an unlocking violation */
    if(m->thread != thread_get_current())
    {
        fprintf(stderr, "spinlock_unlock->wrong thread");
        exit(-1);
    }    

    if (m->count > 0)
    {
        /* this thread still owns lock */
        m->count--;
        return;
    }

    m->thread = wakeup_thread_no_listlock(&m->queue);

    if (m->thread == NULL)
    {
        /* release lock */
        m->locked = 0;
    }
}

void spinlock_init(struct spinlock *l)
{
    l->locked = 0;
    l->thread = NULL;
    l->count = 0;
}

void spinlock_lock(struct spinlock *l)
{
    struct thread_entry *const thread = thread_get_current();

    if (l->thread == thread)
    {
        l->count++;
        return;
    }

    while(test_and_set(&l->locked, 1))
    {
        switch_thread(NULL);
    }

    l->thread = thread;
}

void spinlock_unlock(struct spinlock *l)
{
    /* unlocker not being the owner is an unlocking violation */
    if(l->thread != thread_get_current())
    {
        fprintf(stderr, "spinlock_unlock->wrong thread");
        exit(-1);
    }    

    if (l->count > 0)
    {
        /* this thread still owns lock */
        l->count--;
        return;
    }

    /* clear owner */
    l->thread = NULL;
    l->locked = 0;
}

#ifdef HAVE_SEMAPHORE_OBJECTS
void semaphore_init(struct semaphore *s, int max, int start)
{
    if(max <= 0 || start < 0 || start > max)
    {
        fprintf(stderr, "semaphore_init->inv arg");
        exit(-1);
    }
    s->queue = NULL;
    s->max = max;
    s->count = start;
}

void semaphore_wait(struct semaphore *s)
{
    if(--s->count >= 0)
        return;
    block_thread_no_listlock(&s->queue);
}

void semaphore_release(struct semaphore *s)
{
    if(s->count < s->max)
    {
        if(++s->count <= 0)
        {
            if(s->queue == NULL)
            {
                /* there should be threads in this queue */
                fprintf(stderr, "semaphore->wakeup");
                exit(-1);
            }
            /* a thread was queued - wake it up */
            wakeup_thread_no_listlock(&s->queue);
        }
    }
}
#endif /* HAVE_SEMAPHORE_OBJECTS */

#ifdef HAVE_EVENT_OBJECTS
void event_init(struct event *e, unsigned int flags)
{
    e->queues[STATE_NONSIGNALED] = NULL;
    e->queues[STATE_SIGNALED] = NULL;
    e->state = flags & STATE_SIGNALED;
    e->automatic = (flags & EVENT_AUTOMATIC) ? 1 : 0;
}

void event_wait(struct event *e, unsigned int for_state)
{
    unsigned int last_state = e->state;

    if(e->automatic != 0)
    {
        /* wait for false always satisfied by definition
           or if it just changed to false */
        if(last_state == STATE_SIGNALED || for_state == STATE_NONSIGNALED)
        {
            /* automatic - unsignal */
            e->state = STATE_NONSIGNALED;
            return;
        }
        /* block until state matches */
    }
    else if(for_state == last_state)
    {
        /* the state being waited for is the current state */
        return;
    }

    /* current state does not match wait-for state */
    block_thread_no_listlock(&e->queues[for_state]);
}

void event_set_state(struct event *e, unsigned int state)
{
    unsigned int last_state = e->state;

    if(last_state == state)
    {
        /* no change */
        return;
    }

    if(state == STATE_SIGNALED)
    {
        if(e->automatic != 0)
        {
            struct thread_entry *thread;

            if(e->queues[STATE_NONSIGNALED] != NULL)
            {
                /* no thread should have ever blocked for nonsignaled */
                fprintf(stderr, "set_event_state->queue[NS]:S");
                exit(-1);
            }

            /* pass to next thread and keep unsignaled - "pulse" */
            thread = wakeup_thread_no_listlock(&e->queues[STATE_SIGNALED]);
            e->state = thread != NULL ? STATE_NONSIGNALED : STATE_SIGNALED;
        }
        else
        {
            /* release all threads waiting for signaled */
            thread_queue_wake_no_listlock(&e->queues[STATE_SIGNALED]);
            e->state = STATE_SIGNALED;
        }
    }
    else
    {
        /* release all threads waiting for unsignaled */
        if(e->queues[STATE_NONSIGNALED] != NULL && e->automatic != 0)
        {
            /* no thread should have ever blocked */
            fprintf(stderr, "set_event_state->queue[NS]:NS");
            exit(-1);
        }

        thread_queue_wake_no_listlock(&e->queues[STATE_NONSIGNALED]);
        e->state = STATE_NONSIGNALED;
    }
}
#endif /* HAVE_EVENT_OBJECTS */