/*
 * mutex.c: mutex and priority inheritance support 
 *
 * Copyright (C) 2000 TimeSys Corporation
 *
 * This is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.
 *
 * This software 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this software; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA
 */

#include <linux/config.h>
#include <linux/sched.h>
#include <rk/rk_linux.h>
#include <rk/rk.h>
#include <linux/list.h>

#define RS_NOT_DEPLETED(rs) (rs && rs->rs_cpu && !(rs->rs_cpu->rsv_state & RSV_IS_DEPLETED))
/*
#define RS_NOT_DEPLETED(rs) (rs && rs->rs_cpu && (cpu_reserve_eligible(rs->rs_cpu)))
*/
#define BLOCKING_PRIORITY -1

typedef struct {
    struct list_head lh;
    struct task_struct *ts;
} task_list;

struct mutex;

typedef struct {
    struct list_head lh;
    struct mutex *mtx;
} mutex_list;



typedef struct mutex {
    mutex_list pq_next;
    struct list_head mutexes;	/* For task_struct.mutexes */
    struct mutex *next;		/* Next in hash bucket */
    int id;			/* Mutex ID */
    int uid;			/* Mutex creator */
    int gid;			/* group that can access mutex,
				   -1 for only owner, -2 for all */
    struct task_struct *owner;	/* Mutex owner */
    int proto;			/* PTHREAD_PRIO_INHERIT or PTHREAD_PRIO_NONE */
    struct list_head blocked;	/* Priority queue of blocked processes */
    int islocked;		/* 0 if free, 1 otherwise */
    unsigned long flags;
} mutex;

#ifdef RK_MUTEX_DEBUG_1
void dump_mutex_list(struct list_head *lh)
{
    struct list_head *ptr = lh->next;
    printk("-- mutex list --\n");
    while (ptr != lh) {
	printk("mutex(%d)\n", list_entry(ptr, mutex_list, lh)->mtx->id);
	ptr = ptr->next;
	if (ptr == NULL) {
	    printk("ugg! dump_mutex_list: ptr == NULL\n");
	}
    }
    printk("-- end mutex list --\n");
}

void dump_task_list(struct list_head *lh)
{
    struct list_head *ptr = lh->next;
    printk("-- proc list --\n");
    while (ptr != lh) {
	printk("proc(%d)\n", list_entry(ptr, task_list, lh)->ts->pid);
	ptr = ptr->next;
	if (ptr == NULL) {
	    printk("ugg! dump_task_list: ptr == NULL\n");
	}
    }
    printk("-- end proc list --\n");
}
#endif

#define MUTEX_HASH_SIZE	1024
static mutex *mutex_tbl[MUTEX_HASH_SIZE];
static int cur_mutex_id = 1;

void rt_mutex_init(void)
{
    memset(mutex_tbl, 0, sizeof(mutex *) * MUTEX_HASH_SIZE);
}

static inline mutex *lookup_nolock(int id)
{
    int hash = id % MUTEX_HASH_SIZE;
    mutex *mtx = mutex_tbl[hash];

    while (mtx) {
	if (mtx->id == id)
	    break;
	mtx = mtx->next;
    }

    return mtx;
}

static inline mutex *lookup(int id)
{
    mutex *mtx;
    unsigned long flags;

    rk_spin_lock(flags);
    mtx = lookup_nolock(id);
    rk_spin_unlock(flags);

    return mtx;
}

int sys_rt_mutex_create(int proto, int group)
{
    int hash, id = cur_mutex_id;
    mutex *mtx;
    unsigned long flags;

    if (proto != PTHREAD_PRIO_INHERIT && proto != PTHREAD_PRIO_NONE)
	return -EINVAL;

    rk_spin_lock(flags);
    while (lookup_nolock(id)) {
	id++;
	if (id <= 0)
	    id = 1;
    }
    cur_mutex_id = id + 1;
    hash = id % MUTEX_HASH_SIZE;

    mtx = malloc(sizeof(mutex));
    if (!mtx)
	return -ENOMEM;

    memset(mtx, 0, sizeof(mutex));
    mtx->uid = current->uid;
    mtx->gid = group < 0 ? current->gid : group;
    mtx->id = id;
    mtx->proto = proto;

    INIT_LIST_HEAD(&mtx->pq_next.lh);
    mtx->pq_next.mtx = mtx;

    INIT_LIST_HEAD(&mtx->blocked);

    mtx->next = mutex_tbl[hash];
    mutex_tbl[hash] = mtx;
    rk_spin_unlock(flags);

    return id;
}

int sys_rt_mutex_destroy(int mutex_id)
{
    mutex *mtx = lookup(mutex_id), *prev;
    int hash;
    struct list_head *blocked;
    unsigned long flags;

    if (!mtx)
	return -EINVAL;

    if (current->euid && current->euid != mtx->uid
	&& current->uid != mtx->uid) return -EPERM;

    rk_spin_lock(flags);

    /* Remove this mutex from the hash table */
    hash = mtx->id % MUTEX_HASH_SIZE;
    prev = mutex_tbl[hash];
    if (prev == mtx)
	mutex_tbl[hash] = mtx->next;
    else {
	while (prev->next != mtx)
	    mtx = mtx->next;
	prev->next = mtx->next;
    }

    /* Wake up all blocked processes and free the list's memory */
    blocked = mtx->blocked.next;
    while (blocked != &mtx->blocked) {
	task_list *tl;
	tl = list_entry(blocked, task_list, lh);
	wake_up_process(tl->ts);
	list_del(blocked);
	blocked = blocked->next;
	free(tl);
    }

    rk_spin_unlock(flags);
    free(mtx);

    return 0;
}

/* This is called when the effective priority of a process changes, so that
	it can be moved in the blocked priority queue if necessary. */
static void fix_prio_queue(struct task_struct *proc)
{
    struct list_head *lh;
    task_list *tl = NULL;	/* Filled in once this process is seen */
    unsigned long flags;

#ifdef DEBUG_RK
    printk("fixing prio queue on pid %d (pri %d base %d)\n",
	   proc->pid, proc->rt_priority, proc->rt_base_priority);
#endif				/* DEBUG_RK */

    if (!proc->blocked_on) {
#ifdef DEBUG_RK
	printk("not blocked.\n");
#endif				/* DEBUG_RK */
	return;			/* Process is not blocked */
    }
#ifdef DEBUG_RK
    printk("blocked on %d\n", proc->blocked_on->owner->pid);
#endif				/* DEBUG_RK */

    rk_spin_lock(flags);
    lh = proc->blocked_on->blocked.next;
    while (lh != &proc->blocked_on->blocked) {
	task_list *cur;
	cur = list_entry(lh, task_list, lh);

#ifdef DEBUG_RK
	printk("pid %d blocked, pri %d (base %d)\n", cur->ts->pid,
	       cur->ts->rt_priority, cur->ts->rt_base_priority);
#endif				/* DEBUG_RK */

	if (cur->ts == proc) {
	    /* Found this process */
	    if (tl) {
		/* This process was already reinserted in the list */
#ifdef DEBUG_RK
		printk("Found old entry, removing\n");
#endif				/* DEBUG_RK */
		list_del(lh);
		free(cur);
		goto out;
	    }

	    tl = cur;

	    if (lh->next == &proc->blocked_on->blocked ||
		list_entry(lh->next, task_list, lh)->ts->rt_priority >=
		proc->rt_priority) {
#ifdef DEBUG_RK
		printk("No move necessary.\n");
#endif				/* DEBUG_RK */

		goto out;	/* It doesn't need to be moved. */
	    }

	    /* Process must be moved farther down the list */
#ifdef DEBUG_RK
	    printk("Removing for later insertion.\n");
#endif				/* DEBUG_RK */

	    list_del(lh);
	} else if ((tl && cur->ts->rt_priority == proc->rt_priority) ||
		   cur->ts->rt_priority < proc->rt_priority) {
	    /* Put the process here. */
#ifdef DEBUG_RK
	    printk("Inserting process here.\n");
#endif				/* DEBUG_RK */
	    if (tl) {
		/* It was already removed */
#ifdef DEBUG_RK
		printk("Already removed.\n");
#endif				/* DEBUG_RK */
		list_add(&tl->lh, lh->prev);
		goto out;
	    }

	    tl = (task_list *) malloc(sizeof(task_list));
	    tl->ts = proc;
	    list_add(&tl->lh, lh->prev);
	}

	lh = lh->next;
    }
  out:
    rk_spin_unlock(flags);
}

static inline void do_prio_inherit(mutex * mtx,
				   struct task_struct *blocker)
{
    struct task_struct *owner;
    unsigned long flags;

    rk_spin_lock(flags);
    owner = mtx->owner;
    while (owner && owner->rt_priority < blocker->rt_priority) {
#ifdef DEBUG_RK
	printk("Setting priority of pid %d from %d (base %d) to %d\n",
	       owner->pid, owner->rt_priority, owner->rt_base_priority,
	       blocker->rt_priority);
#endif				/* DEBUG_RK */


	owner->rt_priority = blocker->rt_priority;
	fix_prio_queue(owner);

	if (owner->blocked_on)
	    owner = owner->blocked_on->owner;
	else
	    break;

	if (!owner)
	    panic("blocked_on mutex with NULL owner\n");
    }
    rk_spin_unlock(flags);
}

/* Add the mutex to the owned mutex of the process. head is the pointer
 * from the process to the owned mutexes
 */
void add_owned_mutex_pq(mutex_list * mtxl, struct list_head *head)
{
    struct list_head *ptr;
    /* if the mutex has no process blocked on it its priority is
     * -99 to say that is the lowest of all, even lower that the
     * 'blocking' priority
     */

    int new_mtx_priority = -99;
    int insert_before = 0;

    if (head->next == head) {
#ifdef RK_MUTEX_DEBUG_3
	printk("add_owned_mutex_pq - adding to empty queue mutexID(%d)\n",
	       list_entry(mtxl, mutex_list, lh)->mtx->id);
	printk("&mtxl->lh (%x), head(%x)\n", (unsigned int) &mtxl->lh,
	       (unsigned int) head);
#endif
	list_add(&mtxl->lh, head);
#ifdef RK_MUTEX_DEBUG_3
	printk("add_owned_mutex_pq: after adding to empty queue\n");
#endif
	return;
    }
#ifdef RK_MUTEX_DEBUG_3
    printk("add_owned_mutex_pq: adding to a NOT empty queue\n");
#endif
    /*process blocked waiting for this mutex */
    new_mtx_priority = 0;

    if (list_entry(mtxl, mutex_list, lh)->mtx->blocked.next !=
	&list_entry(mtxl, mutex_list, lh)->mtx->blocked) {
	new_mtx_priority =
	    list_entry(list_entry(mtxl, mutex_list, lh)->mtx->blocked.next,
		       task_list, lh)->ts->rt_priority;
    }
#ifdef RK_MUTEX_DEBUG_3
    printk
	("add_owned_mutex_pq: after getting priority of current mutex\n");
#endif
    for (ptr = head->next; (ptr != head); ptr = ptr->next) {
	if (list_entry(ptr, mutex_list, lh)->mtx->blocked.next !=
	    &list_entry(ptr, mutex_list, lh)->mtx->blocked) {
	    if (new_mtx_priority >
		list_entry(list_entry(ptr, mutex_list, lh)->mtx->blocked.
			   next, task_list, lh)->ts->rt_priority) {
		/* stop the search and add into the previous */
		insert_before = 1;
		break;
	    }
	}
    }
    /* Add the case when the inner if is false */
#ifdef RK_MUTEX_DEBUG_3
    printk("add_owned_mutex_pq: adding\n");
#endif
    if (insert_before)
	list_add(&mtxl->lh, ptr->prev);
    else
	list_add(&mtxl->lh, ptr);
}

void inline remove_owned_mutex_pq(mutex_list * mtxl,
				  struct list_head *head)
{
    list_del(&mtxl->lh);
}

void reposition_owned_mutex_pq(mutex_list * mtxl, struct list_head *head)
{
    remove_owned_mutex_pq(mtxl, head);
    add_owned_mutex_pq(mtxl, head);
}

void add_blocked_process_pq(task_list * tskl, struct list_head *head)
{
    struct list_head *ptr;
    int insert_before = 0;

    if (head->next == head) {
#ifdef RK_MUTEX_DEBUG_3
	printk("add_blocked_process_pq: adding to empty. pid(%d)\n",
	       tskl->ts->pid);
#endif
	list_add(&(tskl->lh), head);
	return;
    }

    for (ptr = head->next; ptr != head; ptr = ptr->next) {
	if (tskl->ts->rt_priority >
	    list_entry(ptr, task_list, lh)->ts->rt_priority) {
	    /* stop the search and add into the previous */
	    insert_before = 1;
	    break;
	}
    }
#ifdef RK_MUTEX_DEBUG_3
    printk("add_blocked_process_pq: adding after pid(%d)\n",
	   list_entry(ptr->prev, task_list, lh)->ts->pid);
#endif
    if (insert_before)
	list_add(&(tskl->lh), ptr->prev);
    else
	list_add(&(tskl->lh), ptr);
}

void inline remove_blocked_process_pq(task_list * tskl,
				      struct list_head *head)
{
    list_del(&tskl->lh);
}

void reposition_blocked_process_pq(task_list * tskl,
				   struct list_head *head)
{
    remove_blocked_process_pq(tskl, head);
    add_blocked_process_pq(tskl, head);
}

/* This function switches from one resource_set to another and changes the
 * rs_proc_list of the rs. Note that the resource set list of the process is
 * not changed though. This is because this switch is considered a temporal
 * assignment 
 */

void cpu_reserve_stop_account_no_enforcement(rk_reserve_t, cpu_tick_t);
void cpu_reserve_start_account_no_enforcement(rk_reserve_t);



void switch_resource_set(struct task_struct *proc,
			 struct rk_resource_set *prev,
			 struct rk_resource_set *next)
{

    struct list_head *ptr;

    /* delete from previous rs proc list if this rs is not part of the
     * resource_set_list of the process
     */

    if ((!prev) || (!next) || (!proc)) {
	// printk("switch_resource_set: PANIC: one or more parameters are NULL");
	// don't know what to do.
    }

    if (prev != NULL) {
	for (ptr = proc->rs_list.resource_set_list.next;
	     (ptr != &proc->rs_list.resource_set_list)
	     &&
	     (list_entry
	      (ptr, struct resource_set_list,
	       resource_set_list)->rk_resource_set != prev);
	     ptr = ptr->next);

	if (list_entry
	    (ptr, struct resource_set_list,
	     resource_set_list)->rk_resource_set != prev) {
	    for (ptr = prev->rs_proc_list.next;
		 (ptr != &prev->rs_proc_list)
		 &&
		 (list_entry
		  (ptr, struct rs_proc_list,
		   rs_proc_list)->rs_proc_task != proc); ptr = ptr->next);

	    if (list_entry
		(ptr, struct rs_proc_list,
		 rs_proc_list)->rs_proc_task == proc) {
#ifdef RK_MUTEX_DEBUG_3
		printk("switch_resource_set pid(%d) deleted from rs(%x)\n",
		       proc->pid, (unsigned int) prev);
#endif
		list_del(ptr);
		/* free the memory */
		free(ptr);
	    }
	}
    }

    /* add to new rs proc list if it is not there */

    if (next != NULL) {
	for (ptr = next->rs_proc_list.next;
	     (ptr != &next->rs_proc_list) &&
	     (list_entry
	      (ptr, struct rs_proc_list,
	       rs_proc_list)->rs_proc_task != proc); ptr = ptr->next);

	if (list_entry
	    (ptr, struct rs_proc_list, rs_proc_list)->rs_proc_task != proc) {
	    struct rs_proc_list *rs_proc;
#ifdef RK_MUTEX_DEBUG_3
	    printk("adding pid(%d) to rs(%x)\n", proc->pid,
		   (unsigned int) next);
#endif
	    rs_proc = malloc(sizeof(struct rs_proc_list));
	    bzero(rs_proc, sizeof(struct rs_proc_list));

	    INIT_LIST_HEAD(&rs_proc->rs_proc_list);

	    rs_proc->rs_proc_task = proc;
	    rs_proc->rs_proc_pid = proc->pid;

	    list_add(&rs_proc->rs_proc_list, &next->rs_proc_list);
	}
    }

    /* if prev->rs_cpu == NULL then it means the resource_set_schedule is
     * calling it and the cpu has already been updated. Therefore we need
     * to leave the responsibility of stopping and starting the accounting
     * to the caller, as well as the assignment to the current_rs and
     * current_cpu_rsv
     */

    if (prev && (rk_current_resource_set == prev) && (prev->rs_cpu)) {
	cpu_tick_data_t now;
#ifdef RK_MUTEX_DEBUG_3
	printk("mutex: switing current pid(%d), from rs(%x) to rs(%x)\n",
	       proc->pid, (unsigned int) prev, (unsigned int) next);

	printk("switching: cpu(%x)\n",
	       (unsigned int) rk_current_resource_set->rs_cpu);
#endif

	if (rk_current_resource_set->rs_cpu) {
	    rk_rdtsc(&now);
	    rk_current_resource_set->rs_cpu->rsv_ops->
		stop_account(rk_current_resource_set->rs_cpu, &now);
	} else {
	    printk("switch_rs:rk_current_rs->rs_cpu == NULL\n");
	}

#ifdef RK_MUTEX_DEBUG_3
	printk("mutex: switing current rs(%x) 1\n", (unsigned int) prev);
#endif
    }
    if (next) {
	proc->rk_resource_set = next;
	if ((proc == current) && (next != rk_current_resource_set)
	    && prev->rs_cpu) {
	    rk_current_resource_set = next;
	    rk_current_cpu_reserve = next->rs_cpu;

	    if (next->rs_cpu) {
#ifdef RK_MUTEX_DEBUG_3
		printk("mutex: switing current rs(%x) 2\n",
		       (unsigned int) prev);
#endif

		/* we are going to try with the regular start accounting */
		next->rs_cpu->rsv_ops->start_account(next->rs_cpu);
#ifdef RK_MUTEX_DEBUG_3
		printk("mutex: switing current rs(%x) 2.5 \n",
		       (unsigned int) prev);
#endif
		if (next->rs_cpu->rsv_state & RSV_IS_DEPLETED) {
#ifdef RK_MUTEX_DEBUG_3
		    printk
			("panic: mutex: switing current rs(%x) 3 - enforcing\n",
			 (unsigned int) prev);
#endif
		}
	    }
	}
    } else {
	proc->rk_resource_set = next;
#ifdef RK_MUTEX_DEBUG_3
	printk("switch_rs: proc->rs = rs(%x)\n", (unsigned int) next);
#endif
	if ((current == proc) && prev->rs_cpu) {
	    rk_current_resource_set = next;
	    rk_current_cpu_reserve = NULL;
	}
    }

    if (proc->rk_resource_set && proc->rk_resource_set->rs_cpu) {
	/* make sure the process is not blocked on its old reserve */
	proc->rk_cannot_schedule = FALSE;
	current->need_resched = 1;
    }
}

int try_reinheritance(struct task_struct *proc)
{
    int candidate_rt_priority = 0;
    int candidate_rt_policy = proc->policy;
    struct rk_resource_set *candidate_rs = NULL;
    mutex *mtx;
    struct list_head *mtxl;
    struct task_struct *tsk;
    struct list_head *tskl;

#ifdef RK_MUTEX_DEBUG_INHERIT
    printk("try_reinherit: pid(%d)\n", proc->pid);
#endif

    if (RS_NOT_DEPLETED(proc->rk_resource_set)) {
	candidate_rs = proc->rk_resource_set;
	candidate_rt_priority = proc->rt_priority;
	candidate_rt_policy = proc->policy;
    }

    if (RS_NOT_DEPLETED(proc->rk_base_resource_set)
	&& (proc->rt_base_priority > proc->rt_priority)) {
#ifdef RK_MUTEX_DEBUG_3
	printk("reinheritance seting candidate to base\n");
#endif
	candidate_rt_priority = proc->rt_base_priority;
	candidate_rt_policy = proc->rt_base_policy;
	candidate_rs = proc->rk_base_resource_set;
    }
#ifdef RK_MUTEX_DEBUG_4
    printk("try_reinheritance:");
    dump_mutex_list(&proc->mutexes);
#endif

    {
	int found = 0;

	for (mtxl = proc->mutexes.next;
	     mtxl && (mtxl != &proc->mutexes) && !found; mtxl = mtxl->next) {
	    mtx = list_entry(mtxl, mutex_list, lh)->mtx;
#ifdef RK_MUTEX_DEBUG_INHERIT
	    printk("try_reinheritance: evaluating mutex(%d)\n", mtx->id);
#endif
	    if ((mtx != NULL) && (mtx->proto == PTHREAD_PRIO_INHERIT)) {
		for (tskl = mtx->blocked.next;
		     tskl && (tskl != &mtx->blocked) && !found;
		     tskl = tskl->next) {
		    tsk = list_entry(tskl, task_list, lh)->ts;
#ifdef RK_MUTEX_DEBUG_INHERIT
		    printk
			("try_reinheritance: evaluating blocked process pid(%d) blocked on mtx(%d)\n",
			 tsk->pid, mtx->id);
#endif
		    if (RS_NOT_DEPLETED(tsk->rk_resource_set) &&
			((tsk->rt_priority > candidate_rt_priority))) {
			if (tsk->rk_resource_set != proc->rk_resource_set) {
			    /* set the candidate to this resource set */
			    candidate_rt_priority = tsk->rt_priority;
			    candidate_rt_policy = tsk->policy;
			    candidate_rs = tsk->rk_resource_set;
#ifdef RK_MUTEX_DEBUG_3
			    printk
				("mutex: pid(%d) reinheriting from pid(%d), rs(%x)\n",
				 proc->pid, tsk->pid,
				 (unsigned int) candidate_rs);

#endif
#ifdef RK_MUTEX_DEBUG_INHERIT
			    printk
				("mutex: pid(%d) reinheriting from pid(%d), rs(%x)\n",
				 proc->pid, tsk->pid,
				 (unsigned int) candidate_rs);

#endif
			    found = 1;
			}
		    }
		}
	    }
	}
    }

    if (candidate_rt_priority == BLOCKING_PRIORITY)
	candidate_rt_priority = 0;


    /* change the resource set */
    if (proc->rk_resource_set != candidate_rs) {
#ifdef RK_MUTEX_DEBUG_3
	printk
	    ("before activating inherited resource pid(%d) prev(%x), next(%x)\n",
	     proc->pid, (unsigned int) proc->rk_resource_set,
	     (unsigned int) candidate_rs);
#endif

#ifdef RK_MUTEX_DEBUG_INHERIT
	printk
	    ("before activating inherited resource pid(%d) prev(%x), next(%x)\n",
	     proc->pid, (unsigned int) proc->rk_resource_set,
	     (unsigned int) candidate_rs);
#endif

	proc->rt_priority = candidate_rt_priority;
	proc->policy = candidate_rt_policy;

#ifdef RK_MUTEX_DEBUG_3
	printk("try_reihn: pid(%d) changing from rs(%p) to rs(%p)\n",
	       proc->pid, proc->rk_resource_set, candidate_rs);
#endif

	switch_resource_set(proc, proc->rk_resource_set, candidate_rs);

	/* only if rs ! = NULL */
	if (RS_NOT_DEPLETED(proc->rk_resource_set))
	    proc->rk_cannot_schedule = FALSE;

#ifdef RK_MUTEX_DEBUG_3
	printk
	    ("after activating inherited resource pid(%d), proc->prior(%ld) proc->rs(%x), next(%x)\n",
	     proc->pid, proc->rt_priority,
	     (unsigned int) proc->rk_resource_set,
	     (unsigned int) candidate_rs);
#endif
	return 1;
    } else
	return 0;
}


int recursive_try_reinheritance(struct task_struct *proc)
{
    int candidate_rt_priority = 0;
    int candidate_rt_policy = proc->policy;
    struct rk_resource_set *candidate_rs = NULL;
    mutex *mtx;
    struct list_head *mtxl;
    struct task_struct *tsk;
    struct list_head *tskl;

#ifdef RK_MUTEX_DEBUG_INHERIT
    printk("try_reinherit: pid(%d)\n", proc->pid);
#endif

    if (RS_NOT_DEPLETED(proc->rk_resource_set)) {
	candidate_rs = proc->rk_resource_set;
	candidate_rt_priority = proc->rt_priority;
	candidate_rt_policy = proc->policy;
    }

    if (RS_NOT_DEPLETED(proc->rk_base_resource_set)
	&& (proc->rt_base_priority > proc->rt_priority)) {
#ifdef RK_MUTEX_DEBUG_3
	printk("reinheritance seting candidate to base\n");
#endif
	candidate_rt_priority = proc->rt_base_priority;
	candidate_rt_policy = proc->rt_base_policy;
	candidate_rs = proc->rk_base_resource_set;
    }
#ifdef RK_MUTEX_DEBUG_4
    printk("try_reinheritance:");
    dump_mutex_list(&proc->mutexes);
#endif

    {
	int found = 0;

	for (mtxl = proc->mutexes.next;
	     mtxl && (mtxl != &proc->mutexes) && !found; mtxl = mtxl->next) {
	    mtx = list_entry(mtxl, mutex_list, lh)->mtx;
#ifdef RK_MUTEX_DEBUG_INHERIT
	    printk("try_reinheritance: evaluating mutex(%d)\n", mtx->id);
#endif
	    if ((mtx != NULL) && (mtx->proto == PTHREAD_PRIO_INHERIT)) {
		for (tskl = mtx->blocked.next;
		     tskl && (tskl != &mtx->blocked) && !found;
		     tskl = tskl->next) {
		    tsk = list_entry(tskl, task_list, lh)->ts;
#ifdef RK_MUTEX_DEBUG_INHERIT
		    printk
			("try_reinheritance: evaluating blocked process pid(%d) blocked on mtx(%d)\n",
			 tsk->pid, mtx->id);
#endif
		    if (RS_NOT_DEPLETED(tsk->rk_resource_set) &&
			((tsk->rt_priority > candidate_rt_priority))) {
			if (tsk->rk_resource_set != proc->rk_resource_set) {
			    /* set the candidate to this resource set */
			    candidate_rt_priority = tsk->rt_priority;
			    candidate_rt_policy = tsk->policy;
			    candidate_rs = tsk->rk_resource_set;
#ifdef RK_MUTEX_DEBUG_3
			    printk
				("mutex: pid(%d) reinheriting from pid(%d), rs(%x)\n",
				 proc->pid, tsk->pid,
				 (unsigned int) candidate_rs);

#endif
#ifdef RK_MUTEX_DEBUG_INHERIT
			    printk
				("mutex: pid(%d) reinheriting from pid(%d), rs(%x)\n",
				 proc->pid, tsk->pid,
				 (unsigned int) candidate_rs);

#endif
			    found = 1;
			}
		    }
		}
	    }
	}
    }

    if (candidate_rt_priority == BLOCKING_PRIORITY)
	candidate_rt_priority = 0;


    /* change the resource set */
    if (proc->rk_resource_set != candidate_rs) {
#ifdef RK_MUTEX_DEBUG_3
	printk
	    ("before activating inherited resource pid(%d) prev(%x), next(%x)\n",
	     proc->pid, (unsigned int) proc->rk_resource_set,
	     (unsigned int) candidate_rs);
#endif

#ifdef RK_MUTEX_DEBUG_INHERIT
	printk
	    ("before activating inherited resource pid(%d) prev(%x), next(%x)\n",
	     proc->pid, (unsigned int) proc->rk_resource_set,
	     (unsigned int) candidate_rs);
#endif

	proc->rt_priority = candidate_rt_priority;
	proc->policy = candidate_rt_policy;

#ifdef RK_MUTEX_DEBUG_3
	printk("try_reihn: pid(%d) changing from rs(%p) to rs(%p)\n",
	       proc->pid, proc->rk_resource_set, candidate_rs);
#endif

	/* we are not going to adjust the accounting of the current resource set
	 * we assume that the caller would do it for this version
	 */
	//switch_resource_set(proc, proc->rk_resource_set, candidate_rs);

	proc->rk_resource_set = candidate_rs;


	/* only if rs ! = NULL */
	if (RS_NOT_DEPLETED(proc->rk_resource_set))
	    proc->rk_cannot_schedule = FALSE;


#ifdef RK_MUTEX_DEBUG_3
	printk
	    ("after activating inherited resource pid(%d), proc->prior(%ld) proc->rs(%x), next(%x)\n",
	     proc->pid, proc->rt_priority,
	     (unsigned int) proc->rk_resource_set,
	     (unsigned int) candidate_rs);
#endif
	return 1;
    } else
	return 0;
}

#ifdef RK_MUTEX_DEBUG_3
#define RK_MUTEX_PROPAGATION_1
#endif


void blocking_chain_priority_propagation(struct task_struct *proc,
					 int recursive)
{
    static int count = 0;
    int need_mutex_reposition = 0;

    if (count > 10) {
	printk("ARGGGGGG ;-P too much recursive on blocking chain\n");
	return;
    }

    count++;

#ifdef RK_MUTEX_PROPAGATION_1
    printk("blocking_chain: entering pid(%d)\n", proc->pid);
#endif


    if (proc->blocked_on
	&& (proc->blocked_on->proto == PTHREAD_PRIO_INHERIT)) {
	task_list *proc_taskl;
	struct list_head *ptr;
	struct task_struct *blocked_on_owner = proc->blocked_on->owner;
#ifdef RK_MUTEX_PROPAGATION_1
	printk("blocking_chain: before searching for pointer chain()\n");
#endif
	/* reposition in the blocked processes pq */
	for (ptr = proc->blocked_on->blocked.next;
	     (ptr != &proc->blocked_on->blocked) &&
	     (list_entry(ptr, task_list, lh)->ts != proc);
	     ptr = ptr->next);
#ifdef RK_MUTEX_PROPAGATION_1
	printk("blocking_chain: after searching for()\n");
#endif

	/* if it is the first in the blocked processes then the mutex
	 * should be reposition in the list as well.
	 */

	/* we will force it because it seems that this condition is
	 * not accurate
	 */
	need_mutex_reposition = 1;

	if ((list_entry(ptr, task_list, lh)->ts)
	    && (list_entry(ptr, task_list, lh)->ts == proc)) {
	    proc_taskl = list_entry(ptr, task_list, lh);
#ifdef RK_MUTEX_PROPAGATION_1
	    printk("before reposition blocked process pq\n");
#endif
	    reposition_blocked_process_pq(proc_taskl,
					  &(proc->blocked_on->blocked));

#ifdef RK_MUTEX_PROPAGATION_1
	    printk
		("blocking_chain: after reposition_blocked_process_pq\n");
#endif

	    if (need_mutex_reposition) {
		/* reposition in the owned mutex pq */

#ifdef RK_MUTEX_PROPAGATION_1
		printk("blocking_chain: before search for mutex\n");
#endif
		for (ptr = &(proc->blocked_on->pq_next.lh);
		     (ptr != ptr->next)
		     && (list_entry(ptr, mutex_list, lh)->mtx !=
			 proc->blocked_on); ptr = ptr->next);
#ifdef RK_MUTEX_PROPAGATION_1
		printk("blocking_chain: after search for mutex\n");
#endif

		if ((list_entry(ptr, mutex_list, lh)->mtx)
		    && (list_entry(ptr, mutex_list, lh)->mtx ==
			proc->blocked_on)) {
#ifdef RK_MUTEX_PROPAGATION_1
		    printk
			("blocking_chain: before reposition owned mutex\n");
#endif
		    reposition_owned_mutex_pq(&list_entry
					      (ptr, mutex_list,
					       lh)->mtx->pq_next,
					      &(proc->blocked_on->owner->
						mutexes));
#ifdef RK_MUTEX_PROPAGATION_1
		    printk
			("blocking_chain: after reposition owned mutex\n");
#endif
		}

		if (blocked_on_owner != NULL) {
		    if (recursive) {
#ifdef RK_MUTEX_PROPAGATION_1
			printk
			    ("blocking_chain: recursively inheriting & propagating to pid(%d)\n",
			     blocked_on_owner->pid);
#endif
			/* See if I can inherit */
			if (recursive_try_reinheritance(blocked_on_owner)) {
			    /* if inherited, then propagate */
#if RSV_PROPAGATION
			    blocking_chain_priority_propagation
				(blocked_on_owner, recursive);
#endif
			}
		    }
		} else {
		    printk
			("blocking_chain: PANIC trying to reinherit on null blocked_on_owner\n");
		}
	    }
	}

    }
#ifdef RK_MUTEX_PROPAGATION_1
    printk("blocking_chain: done\n");
#endif

    count--;
}

/* Adjusts the base priority of a process, ensuring that priority
	queues are adjusted and that any priority inheritance/uninheritance
	needed is performed. */

/* for reserve inheritance the strategy is as follows:
 * adjust priority
 * OnDepletion (newbase < current_base)
 *    if base priority is higher return to base resource set
 *    if OWNER_OF_MUTEX:
 *         check if the highest priority among the blocked
 *         processes is higher than mine. If so inherit make sure
 *         that you do not inherit the same reserve that is being
 *         adjusted (it could happen that the blocked process has 
 *         not being adjusted yet).
 *    if BLOCKED_ON_MUTEX:
 *         reposition itself in the mutex priority queue.
 *         if WAS HIGHEST blocked on this mutex
 *             reposition the mutex in the priority queue of the
 *             owner's owned mutexes.
 *             NOTE THAT THE PROPAGATION OF THE PRIORITY ON THE
 *             DEPLETION CASE WILL HAPPEN WHEN EACH PROCESS ATTACHED TO
 *             THIS RESERVE IS DEPLETED.
 * OnReplenishment (newbase > current_base)
 *    if BLOCKED_ON_MUTEX:
 *       reposition itself in the mutex priority queue.
 *       if HIGHEST blocked on this mutex
 *          reposition the mutex in the priority queue of owner's
 *          owned mutexes.
 *          if mutex is HIGHEST owned mutex
 *             propagate rs to this process.
 *             propagate this rs through the chain of blocked processes on
 *             mutex.
 *             NOTE THAT THE PROPAGATION OF THE REPLENISHMENT OF THIS 
 *             RS NEEDS TO BE DONE AT THIS TIME.
 */

/* Preconditions:
 * The status of the process that is being adjusted is as follows:
 * rk_resource_set has: the eligible resource set with the highest priority
 * from the queue of resource sets. Note that the inherited resource set is not
 * added to this queue given that on every adjustment we need to re-inherit any
 * way.
 * the newbase is the priority that corresponds to the current resource_set
 *
 * We need to represent the blocking of a process as a low priority so we can
 * use this same function to search for a reserve to inherit. After we try to
 * inherit the reserve if the priority is still the 'blocking' priority
 * (we need to use a negative value) we need to return it to a zero value.
 */

/*
 * Postconditions:
 * The process currently being adjusted should be attached to it's own
 * resource_set to receive the replenishment event, and to any inherited
 * resource set to receive the depletion event.
 * It is possible to be attached to a inherited resource set after a
 * depletion if the inherited event has a higher depleted priority that
 * our own priority. This should be taken into account in the new reserve
 * and resource set schedule function for the multi-reserve/resource set
 * scheduler
 *
 * The resulting priority could be negative if the 'blocking' priority
 * is chosen to be so. In that case the calling function is responsible
 * of returning that priority to positive and actually blocking the process.
 */

void rt_adjbaseprio(struct task_struct *proc, int policy, int newbase)
{
    int depletion = (proc->rt_priority > newbase);
#ifdef RK_MUTEX_DEBUG_INHERIT
    printk("rt_adjbaseprio pid(%d), policy(%d), priority(%d) rs(%x)\n",
	   proc->pid, policy, newbase,
	   (unsigned int) proc->rk_resource_set);
#endif

#ifdef RK_MUTEX_DEBUG_1
    printk("rt_adjbaseprio pid(%d), policy(%d), priority(%d) rs(%x)\n",
	   proc->pid, policy, newbase,
	   (unsigned int) proc->rk_resource_set);
#endif

    proc->policy = policy;
    proc->rt_priority = newbase;

    /* if the resource set being adjusted is the current base then 
     * adjust it
     */

    if (proc->rk_base_resource_set == proc->rk_resource_set) {
	proc->rt_base_policy = policy;
	proc->rt_base_priority = newbase;
    }


    if (depletion) {
#ifdef RK_MUTEX_DEBUG_3
	printk("adjbase: before try_reinh\n");
#endif
	try_reinheritance(proc);
    }


    /* check if this process is blocked waiting for a mutex */
#ifdef RK_MUTEX_DEBUG_3
    printk("adjbase: before blocking_chain...\n");
#endif

#if RSV_PROPAGATION
    // blocking_chain_priority_propagation(proc, !depletion);
    blocking_chain_priority_propagation(proc, 1);
#endif

#ifdef RK_MUTEX_DEBUG_1
    printk
	("rt_adjbaseprio pid(%d)i, policy(%d), priority(%d), rs(%x) done\n",
	 proc->pid, (int) proc->policy, (int) proc->rt_priority,
	 (unsigned int) proc->rk_resource_set);
#endif
#ifdef RK_MUTEX_DEBUG_INHERIT
    printk
	("rt_adjbaseprio pid(%d)i, policy(%d), priority(%d), rs(%x) done\n",
	 proc->pid, (int) proc->policy, (int) proc->rt_priority,
	 (unsigned int) proc->rk_resource_set);
#endif
}

/* Called from either sys_rt_mutex_trylock or sys_mutex_lock with 
	mtx->lock held */
inline int rt_mutex_trylock(mutex * mtx)
{
    unsigned long flags;

    if (mtx->gid == -1 && mtx->uid != current->uid)
	return -EACCES;

    if (mtx->gid >= 0 && current->euid && !in_group_p(mtx->gid))
	return -EACCES;

    if (mtx->islocked)
	return -EBUSY;

    mtx->islocked++;

    rk_spin_lock(flags);
    {
	mtx->owner = current;
	INIT_LIST_HEAD(&mtx->pq_next.lh);
#ifdef RK_MUTEX_DEBUG_3
	printk("rt_mutex_trylock:before add_owned_mutex_pq mtxid(%d)\n",
	       mtx->id);
#endif
	add_owned_mutex_pq(&mtx->pq_next, &current->mutexes);
#ifdef RK_MUTEX_DEBUG_3
	printk("rt_mutex_trylock:after add_owned_mutex_pq mtxid(%d)\n",
	       mtx->id);
#endif
    }
    rk_spin_unlock(flags);

    return 0;
}


int sys_rt_mutex_trylock(int mutex_id)
{
    mutex *mtx = lookup(mutex_id);
    int ret;

    if (!mtx)
	return -EINVAL;

    rk_spin_lock(mtx->flags);
    ret = rt_mutex_trylock(mtx);
    rk_spin_unlock(mtx->flags);

    return ret;
}

int sys_rt_mutex_lock(int mutex_id)
{
    mutex *mtx = lookup(mutex_id);
    task_list *tl, *cur_tl;
    struct list_head *cur_lh;
    int insert_prev = 0;

#ifndef RK_MUTEX_DEBUG_1
#ifdef RK_MUTEX_DEBUG_INHERIT
    printk("mutex: entering lock pid(%d) rs(%x) mutex id(%d)\n",
	   current->pid, (unsigned int) current->rk_resource_set,
	   mutex_id);
#endif
#endif

#ifdef RK_MUTEX_DEBUG_1
    printk("mutex: entering lock pid(%d) rs(%x) mutex id(%d)\n",
	   current->pid, (unsigned int) current->rk_resource_set,
	   mutex_id);
    if (current->rs_list.resource_set_list.next == NULL) {
	printk("mutex locking panic: resource_set_list.next == NULL\n");
    }
#endif
    if (!mtx)
	return -EINVAL;

    rk_spin_lock(mtx->flags);
#ifdef RK_MUTEX_DEBUG_3
    printk("mutex: lock: before rt_mutex_trylock mid(%d)\n", mutex_id);
#endif
    if (!rt_mutex_trylock(mtx)) {
#ifdef RK_MUTEX_DEBUG_1
	printk("Granted lock(%d) to pid %d rs(%x)\n", mutex_id,
	       current->pid, (unsigned int) current->rk_resource_set);
	if (current->rs_list.resource_set_list.next == NULL) {
	    printk("mutex locking panic: resource_set_list.next == NULL");
	}
#endif
	rk_spin_unlock(mtx->flags);
	return 0;
    }
#ifdef RK_MUTEX_DEBUG_1
    if (mtx->owner)
	printk("Pid %d blocking on mutex %d held by pid %d\n",
	       current->pid, mutex_id, mtx->owner->pid);
    else
	printk("Blocking on mutex with no owner???\n");
#endif

    tl = (task_list *) malloc(sizeof(task_list));
    if (!tl) {
	rk_spin_unlock(mtx->flags);
	return -ENOMEM;
    }


    INIT_LIST_HEAD(&tl->lh);

    tl->ts = current;

    /* Add to blocked list */
    cur_lh = mtx->blocked.next;
    while (cur_lh != &mtx->blocked) {
	cur_tl = list_entry(cur_lh, task_list, lh);
	if (cur_tl->ts->rt_priority < current->rt_priority) {
	    insert_prev = 1;
	    break;
	}
	cur_lh = cur_lh->next;
    }

    current->blocked_on = mtx;

    if (insert_prev) {
	list_add(&tl->lh, cur_lh->prev);
#ifdef RK_MUTEX_DEBUG_3
	printk("lock: add blocked pid(%d) to blocked list (prev)\n",
	       current->pid);
#endif
    } else {
	list_add(&tl->lh, cur_lh);
#ifdef RK_MUTEX_DEBUG_3
	printk("lock: add blocked pid(%d) to blocked list (next)\n",
	       current->pid);
#endif
    }

#ifdef RK_MUTEX_DEBUG_4
    printk("locking:");
    dump_task_list(&mtx->blocked);
#endif

    if (mtx->proto == PTHREAD_PRIO_INHERIT) {
#ifdef RK_MUTEX_DEBUG_3
	printk("lock: before try_reinheritance\n");
#endif
	try_reinheritance(mtx->owner);

	/* Try to propagate the inherited priority */
	if (mtx->owner->blocked_on) {
#ifdef RK_MUTEX_DEBUG_3
	    printk("lock: before propagating\n");
#endif
#if RSV_PROPAGATION
	    blocking_chain_priority_propagation(mtx->owner, 1);
#endif
	}
    }
#ifdef RK_MUTEX_DEBUG_1
    printk("mutex: leaving lock\n");
#endif

    current->state = TASK_UNINTERRUPTIBLE;
    rk_spin_unlock(mtx->flags);

    schedule();
    return 0;
}

void remove_inherited_reserves(struct task_struct *tsk)
{
    struct list_head *ptr;

    for (ptr = tsk->rs_list.resource_set_list.next;
	 (ptr != &tsk->rs_list.resource_set_list)
	 &&
	 (list_entry
	  (ptr, struct resource_set_list,
	   resource_set_list)->rk_resource_set != tsk->rk_resource_set);
	 ptr = ptr->next);

    if (list_entry
	(ptr, struct resource_set_list,
	 resource_set_list)->rk_resource_set != tsk->rk_resource_set) {
	for (ptr = tsk->rk_resource_set->rs_proc_list.next;
	     (ptr != &tsk->rk_resource_set->rs_proc_list)
	     &&
	     (list_entry
	      (ptr, struct rs_proc_list,
	       rs_proc_list)->rs_proc_task != tsk); ptr = ptr->next);

	if (list_entry
	    (ptr, struct rs_proc_list, rs_proc_list)->rs_proc_task == tsk) {
#ifdef RK_MUTEX_DEBUG_3
	    printk
		("remove_inherited_reserves: removing inherited rs(%x) from list\n",
		 (unsigned int) tsk->rk_resource_set);
#endif
	    list_del(ptr);
	    free(ptr);
	    tsk->rk_resource_set = NULL;
	    tsk->rk_base_resource_set = NULL;
	}
    }
}

int sys_rt_mutex_unlock(int mutex_id)
{
    extern void rk_resource_set_adjust_accounting(void);
    long flags;
    int rt_task = 0;
    mutex *mtx = lookup(mutex_id);
    task_list *tl;
    struct task_struct *ts;
    struct list_head *lh;

#ifndef RK_MUTEX_DEBUG_1
#ifdef RK_MUTEX_DEBUG_INHERIT
    printk
	("mutex: entering unlock pid(%d) proc->rs(%x) prio(%ld) mutexID(%d)\n",
	 current->pid, (unsigned int) current->rk_resource_set,
	 current->rt_priority, mutex_id);
#endif
#endif

#ifdef RK_MUTEX_DEBUG_1
    printk
	("mutex: entering unlock pid(%d) proc->rs(%x) prio(%ld) mutexID(%d)\n",
	 current->pid, (unsigned int) current->rk_resource_set,
	 current->rt_priority, mutex_id);
#endif


#ifdef DEBUG_RK
    printk("rt_mutex_unlock (mutex %d)...\n", mutex_id);
#endif				/* DEBUG_RK */

    if (!mtx) {
#ifdef DEBUG_RK
	printk("mutex id %d does not exist\n", mutex_id);
#endif
	return -EINVAL;
    }

    if (mtx->owner != current) {
#ifdef DEBUG_RK
	printk("current->pid=%d, mtx->owner=%x, mtx->owner->pid=%d\n",
	       current->pid, mtx->owner, mtx->owner ? mtx->owner->pid : 0);
#endif				/* DEBUG_RK */
	return -EPERM;
    }
#ifdef DEBUG_RK
    printk("pid %d:pri is %d real, %d base\n", current->pid,
	   current->rt_priority, current->rt_base_priority);
#endif				/* DEBUG_RK */

    rk_spin_lock(flags);

    list_del(&mtx->pq_next.lh);

    /* Start from base priority and re-inherit from highest priority
       blocker on each remaining mutex that this process holds */
    if (mtx->proto == PTHREAD_PRIO_INHERIT) {

#ifdef RK_MUTEX_DEBUG_1
	printk("unlock: trying to inherit\n");
#endif

	if (current->rk_resource_set != current->rk_base_resource_set) {
	    extern rk_reserve_t
		rk_resource_set_update_cpu(rk_resource_set_t);
	    extern void cpu_reserve_sched(struct rs_proc_list *,
					  cpu_reserve_t cpu);
	    extern void rk_resource_set_schedule(struct rs_proc_list
						 *rs_proc,
						 unsigned int arg);

#ifdef RK_MUTEX_DEBUG_3
	    printk("unlock: back to the base rs(%x) pid(%d)\n",
		   (unsigned int) current->rk_base_resource_set,
		   current->pid);
#endif

	    rt_task = 1;
	    remove_inherited_reserves(current);


	    if (RS_NOT_DEPLETED(current->rk_base_resource_set)) {
		current->rk_resource_set = current->rk_base_resource_set;
		current->rt_priority = current->rt_base_priority;
		current->policy = current->rt_base_policy;
	    } else {
		current->rk_resource_set = NULL;
		current->rt_priority = current->rt_base_priority;
		current->policy = current->rt_base_policy;
	    }


	    if (current->rk_resource_set) {
		/* update the current cpu of the resource set */
		{
		    rk_reserve_t
			rk_resource_set_update_cpu(rk_resource_set_t);
		    rk_resource_set_update_cpu(current->rk_resource_set);
		}
		/* If this resource set is still eligible,
		 * we can just update the scheduling parameters of
		 * all tasks bound to this resource set using the
		 * new cpu reserve
		 */
		if (current->rk_resource_set->rs_cpu) {

		    rs_proc_list_condition_rs_apply
			(&current->rk_resource_set->rs_proc_list,
			 current->rk_resource_set, cpu_reserve_sched,
			 (cpu_reserve_t)
			 (current->rk_resource_set->rs_cpu->rsv_rsv));
		}
		/* This resource set is not eligible, each task
		 * that uses this resource set needs to reschedule
		 * another resource set */
		else {
#ifdef RK_MUTEX_DEBUG_3
		    printk
			("cpu_reserve.c:enforce: calling rk_resource_set_schedule on rs(%x) first pid(%d)\n",
			 (unsigned int) current->rk_resource_set,
			 list_entry(current->rk_resource_set->rs_proc_list.
				    next, struct rs_proc_list,
				    rs_proc_list)->rs_proc_task->pid);
#endif
		    rs_proc_list_condition_rs_apply
			(&current->rk_resource_set->rs_proc_list,
			 current->rk_resource_set,
			 rk_resource_set_schedule, (cpu_reserve_t) 0);
		}

		/* If there is an update of the current reserve of
		 * the current task, adjust the accounting of the
		 * current resource set if there is a change
		 */

		/* we are moving this to after try_reinheritance ... 
		 */
		// rk_resource_set_adjust_accounting();


	    }
	}
#ifdef RK_MUTEX_DEBUG_1
	printk(" **** unlock: trying to reinherit pid(%d), rs(%p)\n",
	       current->pid, current->rk_resource_set);
#endif
	try_reinheritance(current);
	rk_resource_set_adjust_accounting();

#ifdef RK_MUTEX_DEBUG_1
	printk("*** unlock: inherited pid(%d), rs(%p)\n", current->pid,
	       current->rk_resource_set);
#endif
    }


    if (mtx->blocked.next == &mtx->blocked) {
	mtx->islocked--;
#ifdef RK_MUTEX_DEBUG_1
	printk("No new owner! exiting...\n");
#endif				/* RK_MUTEX_DEBUG _1 */
	rk_spin_unlock(flags);

	current->need_resched = 1;
	return 0;
    }


    /* Find the new owner */
    tl = list_entry(mtx->blocked.next, task_list, lh);
    ts = tl->ts;
    lh = mtx->blocked.next;
    list_del(lh);
    free(lh);

#ifdef RK_MUTEX_DEBUG_1
    printk("New owner is pid %d, prio %d (base %d)\n", ts->pid,
	   (unsigned int) ts->rt_priority, ts->rt_base_priority);
#endif				/* RK_MUTEX_DEBUG_1 */


    /* Assign the mutex to the new owner */
    mtx->owner = ts;

    /* just to make sure */
    mtx->pq_next.mtx = mtx;

#ifdef RK_MUTEX_DEBUG_1
    printk("unlock: before calling add_owned_mutex_pq mtx id(%d)\n",
	   mtx->id);
#endif
    add_owned_mutex_pq(&mtx->pq_next, &ts->mutexes);
#ifdef RK_MUTEX_DEBUG_1
    printk("unlock: after calling add_owned_mutex_pq\n");
#endif

#ifdef RK_MUTEX_DEBUG_1
    printk("mutex: leaving unlock\n");
#endif

    ts->blocked_on = NULL;
    wake_up_process(ts);
    if (rt_task && (current->rk_resource_set == NULL)) {
	current->rk_cannot_schedule = TRUE;
    }
    rk_spin_unlock(flags);
    current->need_resched = 1;
    return ts->pid;
}
