static int memc_min_pwr_update(struct kona_memc *kmemc,
		struct kona_memc_node *memc_node, int action)
{
	u32 new_val;
	int ret = 0;
	spin_lock(&kmemc->memc_lock);
	switch (action) {
	case MEMC_NODE_ADD:
		plist_node_init(&memc_node->node, memc_node->min_pwr);
		plist_add(&memc_node->node, &kmemc->min_pwr_list);
		break;
	case MEMC_NODE_DEL:
		plist_del(&memc_node->node, &kmemc->min_pwr_list);
		break;
	case MEMC_NODE_UPDATE:
		plist_del(&memc_node->node, &kmemc->min_pwr_list);
		plist_node_init(&memc_node->node, memc_node->min_pwr);
		plist_add(&memc_node->node, &kmemc->min_pwr_list);
		break;
	default:
		BUG();
		return -EINVAL;
	}
	new_val = plist_last(&kmemc->min_pwr_list)->prio;
	if (new_val != kmemc->active_min_pwr) {
		ret = memc_set_min_pwr(kmemc, new_val, MEMC_AP_MIN_PWR);
		if (!ret)
			kmemc->active_min_pwr = new_val;
	}
	spin_unlock(&kmemc->memc_lock);
	return ret;
}

/*
 * Task blocks on lock.
 *
 * Prepare waiter and propagate pi chain
 *
 * This must be called with lock->wait_lock held.
 */
static int task_blocks_on_rt_mutex(struct rt_mutex *lock,
                                   struct rt_mutex_waiter *waiter,
                                   int detect_deadlock)
{
    struct task_struct *owner = rt_mutex_owner(lock);
    struct rt_mutex_waiter *top_waiter = waiter;
    unsigned long flags;
    int boost = 0, res;

    spin_lock_irqsave(&current->pi_lock, flags);
    __rt_mutex_adjust_prio(current);
    waiter->task = current;
    waiter->lock = lock;
    plist_node_init(&waiter->list_entry, current->prio);
    plist_node_init(&waiter->pi_list_entry, current->prio);

    /* Get the top priority waiter on the lock */
    if (rt_mutex_has_waiters(lock))
        top_waiter = rt_mutex_top_waiter(lock);
    plist_add(&waiter->list_entry, &lock->wait_list);

    current->pi_blocked_on = waiter;

    spin_unlock_irqrestore(&current->pi_lock, flags);

    if (waiter == rt_mutex_top_waiter(lock)) {
        spin_lock_irqsave(&owner->pi_lock, flags);
        plist_del(&top_waiter->pi_list_entry, &owner->pi_waiters);
        plist_add(&waiter->pi_list_entry, &owner->pi_waiters);

        __rt_mutex_adjust_prio(owner);
        if (owner->pi_blocked_on) {
            boost = 1;
            /* gets dropped in rt_mutex_adjust_prio_chain()! */
            get_task_struct(owner);
        }
        spin_unlock_irqrestore(&owner->pi_lock, flags);
    }
    else if (debug_rt_mutex_detect_deadlock(waiter, detect_deadlock)) {
        spin_lock_irqsave(&owner->pi_lock, flags);
        if (owner->pi_blocked_on) {
            boost = 1;
            /* gets dropped in rt_mutex_adjust_prio_chain()! */
            get_task_struct(owner);
        }
        spin_unlock_irqrestore(&owner->pi_lock, flags);
    }
    if (!boost)
        return 0;

    spin_unlock(&lock->wait_lock);

    res = rt_mutex_adjust_prio_chain(owner, detect_deadlock, lock, waiter,
                                     current);

    spin_lock(&lock->wait_lock);

    return res;
}

/*
 * Task blocks on lock.
 *
 * Prepare waiter and propagate pi chain
 *
 * This must be called with lock->wait_lock held.
 */
static int task_blocks_on_rt_mutex(struct rt_mutex *lock,
				   struct rt_mutex_waiter *waiter,
				   struct task_struct *task,
				   int detect_deadlock)
{
	struct task_struct *owner = rt_mutex_owner(lock);
	struct rt_mutex_waiter *top_waiter = waiter;
	unsigned long flags;
	int chain_walk = 0, res;

	raw_spin_lock_irqsave(&task->pi_lock, flags);
	__rt_mutex_adjust_prio(task);
	waiter->task = task;
	waiter->lock = lock;
	plist_node_init(&waiter->list_entry, task->prio);
	plist_node_init(&waiter->pi_list_entry, task->prio);

	/* Get the top priority waiter on the lock */
	if (rt_mutex_has_waiters(lock))
		top_waiter = rt_mutex_top_waiter(lock);
	plist_add(&waiter->list_entry, &lock->wait_list);

	task->pi_blocked_on = waiter;

	raw_spin_unlock_irqrestore(&task->pi_lock, flags);

	if (waiter == rt_mutex_top_waiter(lock)) {
		raw_spin_lock_irqsave(&owner->pi_lock, flags);
		plist_del(&top_waiter->pi_list_entry, &owner->pi_waiters);
		plist_add(&waiter->pi_list_entry, &owner->pi_waiters);

		__rt_mutex_adjust_prio(owner);
		if (owner->pi_blocked_on)
			chain_walk = 1;
		raw_spin_unlock_irqrestore(&owner->pi_lock, flags);
	}
	else if (debug_rt_mutex_detect_deadlock(waiter, detect_deadlock))
		chain_walk = 1;

	if (!chain_walk)
		return 0;

	/*
	 * The owner can't disappear while holding a lock,
	 * so the owner struct is protected by wait_lock.
	 * Gets dropped in rt_mutex_adjust_prio_chain()!
	 */
	get_task_struct(owner);

	raw_spin_unlock(&lock->wait_lock);

	res = rt_mutex_adjust_prio_chain(owner, detect_deadlock, lock, waiter,
					 task);

	raw_spin_lock(&lock->wait_lock);

	return res;
}

int
pdict_add_persistent_change_listener(pdict_t *pd, const char *kpat,
    pdl_notify_func_t notify, void *arg)
{
	pdict_persistent_listener_t *pl;
	static int lid = 1;

	if (!(pl = malloc(sizeof (*pl))))
		return 0;
	memset(pl, 0, sizeof (*pl));
	pl->pdpl_l.pdl_notify = notify;
	pl->pdpl_l.pdl_arg = arg;
	if (regcomp(&pl->pdpl_regex, kpat, REG_EXTENDED | REG_NOSUB) != 0) {
		// XXX todo: communicate error context is not libc
		free(pl); pl = NULL;
		pu_log(PUL_WARN, 0, "Failed regcomp in pdict_add_persistent_change_listener.");
		return 0;
	}

	plist_add((void *)(size_t)lid, pl, &pd->pd_persistent_listeners);

	pl->pdpl_new = 1;
	if (!_pdict_walk_int(pd,
	    pdict_ent_add_persistent_change_listener_dcb, pl)) {
		_pdict_walk_int(pd,
		    pdict_ent_remove_persistent_change_listener_dcb, pl);
		plist_remove((void *)(size_t)lid, &pd->pd_persistent_listeners, NULL);
		regfree(&pl->pdpl_regex);
		free(pl); pl = NULL;
		pu_log(PUL_WARN, 0, "Failed _pdict_walk_int in pdict_add_persistent_change_listener.");
		return 0;
	}
	pl->pdpl_new = 0;
	return lid++;
}

void traillist_update(traillist tlist, vector2 p, float dt){
	tlist->col_timer += dt;
	if(tlist->trailtoggle == 1 && tlist->firsttrail == NULL){
		tlist->firsttrail = add_trail();
		plist_add(&tlist->firsttrail->t.draw_trail, p);
		plist_add(&tlist->firsttrail->t.col_trail, p);
	}


	trailnode* trail = tlist->firsttrail;
	trailnode* prev = NULL;

	if(tlist->trailtoggle){
		plist_add(&trail->t.draw_trail, p);
		if(tlist->col_timer > 0.1){
			plist_add(&trail->t.col_trail, p);
			tlist->col_timer = 0;
		}
		plist_head(&trail->t.col_trail, p);
	}	

	/*TODO once you find out that a trail is done,
	 * free the trailnode and reoranize the list.
	 * */
	while(trail != NULL){
		if(plist_update(trail->t.col_trail, dt)){
			trail->t.col_trail = NULL;
		}
		if(plist_update(trail->t.draw_trail, dt)){
			if(prev == NULL){
				tlist->firsttrail = NULL;
				return;
			}
			trail->t.draw_trail = NULL;
			trailnode* tmp = trail;
			prev->next = trail->next;
			trail = trail->next;
			free(tmp);
		}
		else{
			prev = trail;
			trail = trail->next;
		}
	}
}

/*
 * Optimization: check if we can steal the lock from the
 * assigned pending owner [which might not have taken the
 * lock yet]:
 */
static inline int try_to_steal_lock(struct rt_mutex *lock,
				    struct task_struct *task)
{
	struct task_struct *pendowner = rt_mutex_owner(lock);
	struct rt_mutex_waiter *next;
	unsigned long flags;

	if (!rt_mutex_owner_pending(lock))
		return 0;

	if (pendowner == task)
		return 1;

	raw_spin_lock_irqsave(&pendowner->pi_lock, flags);
	if (task->prio >= pendowner->prio) {
		raw_spin_unlock_irqrestore(&pendowner->pi_lock, flags);
		return 0;
	}

	/*
	 * Check if a waiter is enqueued on the pending owners
	 * pi_waiters list. Remove it and readjust pending owners
	 * priority.
	 */
	if (likely(!rt_mutex_has_waiters(lock))) {
		raw_spin_unlock_irqrestore(&pendowner->pi_lock, flags);
		return 1;
	}

	/* No chain handling, pending owner is not blocked on anything: */
	next = rt_mutex_top_waiter(lock);
	plist_del(&next->pi_list_entry, &pendowner->pi_waiters);
	__rt_mutex_adjust_prio(pendowner);
	raw_spin_unlock_irqrestore(&pendowner->pi_lock, flags);

	/*
	 * We are going to steal the lock and a waiter was
	 * enqueued on the pending owners pi_waiters queue. So
	 * we have to enqueue this waiter into
	 * task->pi_waiters list. This covers the case,
	 * where task is boosted because it holds another
	 * lock and gets unboosted because the booster is
	 * interrupted, so we would delay a waiter with higher
	 * priority as task->normal_prio.
	 *
	 * Note: in the rare case of a SCHED_OTHER task changing
	 * its priority and thus stealing the lock, next->task
	 * might be task:
	 */
	if (likely(next->task != task)) {
		raw_spin_lock_irqsave(&task->pi_lock, flags);
		plist_add(&next->pi_list_entry, &task->pi_waiters);
		__rt_mutex_adjust_prio(task);
		raw_spin_unlock_irqrestore(&task->pi_lock, flags);
	}
	return 1;
}

static int
add_to_wa_list(const char *k, const char *v, void *arg)
{
	wa_t *wa = arg;
	
	if (regexec(&wa->wa_regex, k, 0, NULL, 0) == 0)
		return plist_add((void *)k, (void *)v, &wa->wa_l);
	return 1;
}

/* Read the content of the directory, make an array of absolute paths for
 * all recognized files. Put directories, playlists and sound files
 * in proper structures. Return 0 on error.*/
int read_directory (const char *directory, struct file_list *dirs,
		struct file_list *playlists, struct plist *plist)
{
	DIR *dir;
	struct dirent *entry;
	int show_hidden = options_get_int ("ShowHiddenFiles");
	int dir_is_root;
	
	assert (directory != NULL);
	assert (*directory == '/');
	assert (dirs != NULL);
	assert (playlists != NULL);
	assert (plist != NULL);

	if (!(dir = opendir(directory))) {
		error ("Can't read directory: %s", strerror(errno));
		return 0;
	}

	if (!strcmp(directory, "/"))
		dir_is_root = 1;

	else
		dir_is_root = 0;

	while ((entry = readdir(dir))) {
		char file[PATH_MAX];
		enum file_type type;

		if (user_wants_interrupt()) {
			error ("Interrupted! Not all files read!");
			break;
		}
		
		if (!strcmp(entry->d_name, ".") || !strcmp(entry->d_name, ".."))
			continue;
		if (!show_hidden && entry->d_name[0] == '.')
			continue;
		if (snprintf(file, sizeof(file), "%s/%s", dir_is_root ?
					"" : directory,	entry->d_name)
				>= (int)sizeof(file)) {
			error ("Path too long!");
			return 0;
		}
		type = file_type (file);
		if (type == F_SOUND)
			plist_add (plist, file);
		else if (type == F_DIR)
			file_list_add (dirs, file);
		else if (type == F_PLAYLIST)
			file_list_add (playlists, file);
	}

	closedir (dir);

	return 1;
}

static void enqueue_pushable_task(struct rq *rq, struct task_struct *p)
{
	plist_del(&p->pushable_tasks, &rq->rt.pushable_tasks);
	plist_node_init(&p->pushable_tasks, p->prio);
	plist_add(&p->pushable_tasks, &rq->rt.pushable_tasks);

	/* Update the highest prio pushable task */
	if (p->prio < rq->rt.highest_prio.next)
		rq->rt.highest_prio.next = p->prio;
}

/*
 * Wake up the next waiter on the lock.
 *
 * Remove the top waiter from the current tasks waiter list and from
 * the lock waiter list. Set it as pending owner. Then wake it up.
 *
 * Called with lock->wait_lock held.
 */
static void wakeup_next_waiter(struct rt_mutex *lock, int savestate)
{
	struct rt_mutex_waiter *waiter;
	struct task_struct *pendowner;

	spin_lock(&current->pi_lock);

	waiter = rt_mutex_top_waiter(lock);
	plist_del(&waiter->list_entry, &lock->wait_list);

	/*
	 * Remove it from current->pi_waiters. We do not adjust a
	 * possible priority boost right now. We execute wakeup in the
	 * boosted mode and go back to normal after releasing
	 * lock->wait_lock.
	 */
	plist_del(&waiter->pi_list_entry, &current->pi_waiters);
	pendowner = waiter->task;
	waiter->task = NULL;

	rt_mutex_set_owner(lock, pendowner, RT_MUTEX_OWNER_PENDING);

	spin_unlock(&current->pi_lock);

	/*
	 * Clear the pi_blocked_on variable and enqueue a possible
	 * waiter into the pi_waiters list of the pending owner. This
	 * prevents that in case the pending owner gets unboosted a
	 * waiter with higher priority than pending-owner->normal_prio
	 * is blocked on the unboosted (pending) owner.
	 */
	spin_lock(&pendowner->pi_lock);

	WARN_ON(!pendowner->pi_blocked_on);
	WARN_ON(pendowner->pi_blocked_on != waiter);
	WARN_ON(pendowner->pi_blocked_on->lock != lock);

	pendowner->pi_blocked_on = NULL;

	if (rt_mutex_has_waiters(lock)) {
		struct rt_mutex_waiter *next;

		next = rt_mutex_top_waiter(lock);
		plist_add(&next->pi_list_entry, &pendowner->pi_waiters);
	}
	spin_unlock(&pendowner->pi_lock);

	if (savestate)
		wake_up_process_mutex(pendowner);
	else
		wake_up_process(pendowner);
}

/* Copy the item to the playlist. Return the index of the added item. */
int plist_add_from_item (struct plist *plist, const struct plist_item *item)
{
	int pos = plist_add (plist, item->file);

	plist_item_copy (&plist->items[pos], item);

	if (item->tags && item->tags->time != -1) {
		plist->total_time += item->tags->time;
		plist->items_with_time++;
	}

	return pos;
}

void emit_frame(emit_desc* frame) {
    for (int i = 0; i < frame->n; ++i) {
        particle* p = NULL;
        float r = randfloat(frame->r - frame->ur, frame->r + frame->ur);
        float g = randfloat(frame->g - frame->ug, frame->g + frame->ug);
        float b = randfloat(frame->b - frame->ub, frame->b + frame->ub);
        pextra* pe = new_pextra(r, g, b, frame->blender);
        p = particle_new_full(frame->x, frame->y, frame->ux, frame->uy,
                              frame->rad, frame->urad, frame->ds, frame->uds,
                              frame->theta, frame->utheta,
                              frame->life, frame->ulife,
                              frame->force, frame->limit, pe);
        plist_add(emitter.particles, p);
    }
}

/**
 * pm_qos_update_target - manages the constraints list and calls the notifiers
 *  if needed
 * @c: constraints data struct
 * @node: request to add to the list, to update or to remove
 * @action: action to take on the constraints list
 * @value: value of the request to add or update
 *
 * This function returns 1 if the aggregated constraint value has changed, 0
 *  otherwise.
 */
int pm_qos_update_target(struct pm_qos_constraints *c, struct plist_node *node,
			 enum pm_qos_req_action action, int value)
{
	unsigned long flags;
	int prev_value, curr_value, new_value;

	spin_lock_irqsave(&pm_qos_lock, flags);
	prev_value = pm_qos_get_value(c);
	if (value == PM_QOS_DEFAULT_VALUE)
		new_value = c->default_value;
	else
		new_value = value;

	switch (action) {
	case PM_QOS_REMOVE_REQ:
		plist_del(node, &c->list);
		break;
	case PM_QOS_UPDATE_REQ:
		/*
		 * to change the list, we atomically remove, reinit
		 * with new value and add, then see if the extremal
		 * changed
		 */
		plist_del(node, &c->list);
	case PM_QOS_ADD_REQ:
		plist_node_init(node, new_value);
		plist_add(node, &c->list);
		break;
	default:
		/* no action */
		;
	}

	curr_value = pm_qos_get_value(c);
	pm_qos_set_value(c, curr_value);

	spin_unlock_irqrestore(&pm_qos_lock, flags);

	if (prev_value != curr_value) {
		blocking_notifier_call_chain(c->notifiers,
					     (unsigned long)curr_value,
					     NULL);
		return 1;
	} else {
		return 0;
	}
}

/*
 * Remove a waiter from a lock
 *
 * Must be called with lock->wait_lock held
 */
static void remove_waiter(struct rt_mutex *lock,
                          struct rt_mutex_waiter *waiter)
{
    int first = (waiter == rt_mutex_top_waiter(lock));
    struct task_struct *owner = rt_mutex_owner(lock);
    unsigned long flags;
    int boost = 0;

    spin_lock_irqsave(&current->pi_lock, flags);
    plist_del(&waiter->list_entry, &lock->wait_list);
    waiter->task = NULL;
    current->pi_blocked_on = NULL;
    spin_unlock_irqrestore(&current->pi_lock, flags);

    if (first && owner != current) {

        spin_lock_irqsave(&owner->pi_lock, flags);

        plist_del(&waiter->pi_list_entry, &owner->pi_waiters);

        if (rt_mutex_has_waiters(lock)) {
            struct rt_mutex_waiter *next;

            next = rt_mutex_top_waiter(lock);
            plist_add(&next->pi_list_entry, &owner->pi_waiters);
        }
        __rt_mutex_adjust_prio(owner);

        if (owner->pi_blocked_on) {
            boost = 1;
            /* gets dropped in rt_mutex_adjust_prio_chain()! */
            get_task_struct(owner);
        }
        spin_unlock_irqrestore(&owner->pi_lock, flags);
    }

    WARN_ON(!plist_node_empty(&waiter->pi_list_entry));

    if (!boost)
        return;

    spin_unlock(&lock->wait_lock);

    rt_mutex_adjust_prio_chain(owner, 0, lock, NULL, current);

    spin_lock(&lock->wait_lock);
}

static int
_pdict_ent_add_change_listener(pdict_ent_t *pde, pdl_notify_func_t notify,
    void *arg)
{
	pdict_listener_t *l;

	if (!(l = malloc(sizeof (*l))))
		return 0;
	memset(l, 0, sizeof (*l));
	l->pdl_notify = notify;
	l->pdl_arg = arg;
	if (!plist_add(l, 0, &pde->pde_listeners)) {
		free(l); l = NULL;
		pu_log(PUL_WARN, 0, "Failed plist_add in _pdict_ent_add_change_listener.");
		return 0;
	}

	return 1;
}

HNode* hlist_add(HList* hlist, const char* name, const void* obj) {
  if (!hlist) {
    return 0;
  }

  HNode* n = hlist_lookup(hlist, name, HEADER_TYPE_NONE, 0, 0);
  if (!n) {
    Header* h = header_lookup_standard(HEADER_TYPE_NONE, name);
    if (!h) {
      h = header_create(name);
    }
    hlist_grow(hlist);
    n = &hlist->data[hlist->ulen++];
    n->header = h;
    n->values = plist_create();
    HLIST_FLAG_CLR(hlist, HLIST_FLAGS_SORTED);
  }

  plist_add(n->values, obj);
  GLOG(("=C= Added [%s] => %p (%d)", name, obj, n->header->order));
  return n;
}

/*
 * Task blocks on lock.
 *
 * Prepare waiter and propagate pi chain
 *
 * This must be called with lock->wait_lock held.
 */
static int task_blocks_on_rt_mutex(struct rt_mutex *lock,
				   struct rt_mutex_waiter *waiter,
				   struct task_struct *task,
				   int detect_deadlock)
{
	struct task_struct *owner = rt_mutex_owner(lock);
	struct rt_mutex_waiter *top_waiter = waiter;
	unsigned long flags;
	int chain_walk = 0, res;

	/*
	 * Early deadlock detection. We really don't want the task to
	 * enqueue on itself just to untangle the mess later. It's not
	 * only an optimization. We drop the locks, so another waiter
	 * can come in before the chain walk detects the deadlock. So
	 * the other will detect the deadlock and return -EDEADLOCK,
	 * which is wrong, as the other waiter is not in a deadlock
	 * situation.
	 */
	if (detect_deadlock && owner == task)
		return -EDEADLK;

	raw_spin_lock_irqsave(&task->pi_lock, flags);
	__rt_mutex_adjust_prio(task);
	waiter->task = task;
	waiter->lock = lock;
	plist_node_init(&waiter->list_entry, task->prio);
	plist_node_init(&waiter->pi_list_entry, task->prio);

	/* Get the top priority waiter on the lock */
	if (rt_mutex_has_waiters(lock))
		top_waiter = rt_mutex_top_waiter(lock);
	plist_add(&waiter->list_entry, &lock->wait_list);

	task->pi_blocked_on = waiter;

	raw_spin_unlock_irqrestore(&task->pi_lock, flags);

	if (!owner)
		return 0;

	if (waiter == rt_mutex_top_waiter(lock)) {
		raw_spin_lock_irqsave(&owner->pi_lock, flags);
		plist_del(&top_waiter->pi_list_entry, &owner->pi_waiters);
		plist_add(&waiter->pi_list_entry, &owner->pi_waiters);

		__rt_mutex_adjust_prio(owner);
		if (owner->pi_blocked_on)
			chain_walk = 1;
		raw_spin_unlock_irqrestore(&owner->pi_lock, flags);
	}
	else if (debug_rt_mutex_detect_deadlock(waiter, detect_deadlock))
		chain_walk = 1;

	if (!chain_walk)
		return 0;

	/*
	 * The owner can't disappear while holding a lock,
	 * so the owner struct is protected by wait_lock.
	 * Gets dropped in rt_mutex_adjust_prio_chain()!
	 */
	get_task_struct(owner);

	raw_spin_unlock(&lock->wait_lock);

	res = rt_mutex_adjust_prio_chain(owner, detect_deadlock, lock, waiter,
					 task);

	raw_spin_lock(&lock->wait_lock);

	return res;
}

/*
 * Try to take an rt-mutex
 *
 * Must be called with lock->wait_lock held.
 *
 * @lock:   the lock to be acquired.
 * @task:   the task which wants to acquire the lock
 * @waiter: the waiter that is queued to the lock's wait list. (could be NULL)
 */
static int try_to_take_rt_mutex(struct rt_mutex *lock, struct task_struct *task,
		struct rt_mutex_waiter *waiter)
{
	/*
	 * We have to be careful here if the atomic speedups are
	 * enabled, such that, when
	 *  - no other waiter is on the lock
	 *  - the lock has been released since we did the cmpxchg
	 * the lock can be released or taken while we are doing the
	 * checks and marking the lock with RT_MUTEX_HAS_WAITERS.
	 *
	 * The atomic acquire/release aware variant of
	 * mark_rt_mutex_waiters uses a cmpxchg loop. After setting
	 * the WAITERS bit, the atomic release / acquire can not
	 * happen anymore and lock->wait_lock protects us from the
	 * non-atomic case.
	 *
	 * Note, that this might set lock->owner =
	 * RT_MUTEX_HAS_WAITERS in the case the lock is not contended
	 * any more. This is fixed up when we take the ownership.
	 * This is the transitional state explained at the top of this file.
	 */
	mark_rt_mutex_waiters(lock);

	if (rt_mutex_owner(lock))
		return 0;

	/*
	 * It will get the lock because of one of these conditions:
	 * 1) there is no waiter
	 * 2) higher priority than waiters
	 * 3) it is top waiter
	 */
	if (rt_mutex_has_waiters(lock)) {
		if (task->prio >= rt_mutex_top_waiter(lock)->list_entry.prio) {
			if (!waiter || waiter != rt_mutex_top_waiter(lock))
				return 0;
		}
	}

	if (waiter || rt_mutex_has_waiters(lock)) {
		unsigned long flags;
		struct rt_mutex_waiter *top;

		raw_spin_lock_irqsave(&task->pi_lock, flags);

		/* remove the queued waiter. */
		if (waiter) {
			plist_del(&waiter->list_entry, &lock->wait_list);
			task->pi_blocked_on = NULL;
		}

		/*
		 * We have to enqueue the top waiter(if it exists) into
		 * task->pi_waiters list.
		 */
		if (rt_mutex_has_waiters(lock)) {
			top = rt_mutex_top_waiter(lock);
			top->pi_list_entry.prio = top->list_entry.prio;
			plist_add(&top->pi_list_entry, &task->pi_waiters);
		}
		raw_spin_unlock_irqrestore(&task->pi_lock, flags);
	}

	/* We got the lock. */
	debug_rt_mutex_lock(lock);

	rt_mutex_set_owner(lock, task);

	rt_mutex_deadlock_account_lock(lock, task);

	return 1;
}

//.........这里部分代码省略.........
		goto out_unlock_pi;

	/*
	 * Check the orig_waiter state. After we dropped the locks,
	 * the previous owner of the lock might have released the lock.
	 */
	if (orig_waiter && !rt_mutex_owner(orig_lock))
		goto out_unlock_pi;

	/*
	 * Drop out, when the task has no waiters. Note,
	 * top_waiter can be NULL, when we are in the deboosting
	 * mode!
	 */
	if (top_waiter && (!task_has_pi_waiters(task) ||
			   top_waiter != task_top_pi_waiter(task)))
		goto out_unlock_pi;

	/*
	 * When deadlock detection is off then we check, if further
	 * priority adjustment is necessary.
	 */
	if (!detect_deadlock && waiter->list_entry.prio == task->prio)
		goto out_unlock_pi;

	lock = waiter->lock;
	if (!raw_spin_trylock(&lock->wait_lock)) {
		raw_spin_unlock_irqrestore(&task->pi_lock, flags);
		cpu_relax();
		goto retry;
	}

	/* Deadlock detection */
	if (lock == orig_lock || rt_mutex_owner(lock) == top_task) {
		debug_rt_mutex_deadlock(deadlock_detect, orig_waiter, lock);
		raw_spin_unlock(&lock->wait_lock);
		ret = deadlock_detect ? -EDEADLK : 0;
		goto out_unlock_pi;
	}

	top_waiter = rt_mutex_top_waiter(lock);

	/* Requeue the waiter */
	plist_del(&waiter->list_entry, &lock->wait_list);
	waiter->list_entry.prio = task->prio;
	plist_add(&waiter->list_entry, &lock->wait_list);

	/* Release the task */
	raw_spin_unlock_irqrestore(&task->pi_lock, flags);
	if (!rt_mutex_owner(lock)) {
		/*
		 * If the requeue above changed the top waiter, then we need
		 * to wake the new top waiter up to try to get the lock.
		 */

		if (top_waiter != rt_mutex_top_waiter(lock))
			wake_up_process(rt_mutex_top_waiter(lock)->task);
		raw_spin_unlock(&lock->wait_lock);
		goto out_put_task;
	}
	put_task_struct(task);

	/* Grab the next task */
	task = rt_mutex_owner(lock);
	get_task_struct(task);
	raw_spin_lock_irqsave(&task->pi_lock, flags);

	if (waiter == rt_mutex_top_waiter(lock)) {
		/* Boost the owner */
		plist_del(&top_waiter->pi_list_entry, &task->pi_waiters);
		waiter->pi_list_entry.prio = waiter->list_entry.prio;
		plist_add(&waiter->pi_list_entry, &task->pi_waiters);
		__rt_mutex_adjust_prio(task);

	} else if (top_waiter == waiter) {
		/* Deboost the owner */
		plist_del(&waiter->pi_list_entry, &task->pi_waiters);
		waiter = rt_mutex_top_waiter(lock);
		waiter->pi_list_entry.prio = waiter->list_entry.prio;
		plist_add(&waiter->pi_list_entry, &task->pi_waiters);
		__rt_mutex_adjust_prio(task);
	}

	raw_spin_unlock_irqrestore(&task->pi_lock, flags);

	top_waiter = rt_mutex_top_waiter(lock);
	raw_spin_unlock(&lock->wait_lock);

	if (!detect_deadlock && waiter != top_waiter)
		goto out_put_task;

	goto again;

 out_unlock_pi:
	raw_spin_unlock_irqrestore(&task->pi_lock, flags);
 out_put_task:
	put_task_struct(task);

	return ret;
}

void init(char *str)
{
    transceiver_command_t tcmd;
    msg_t m;
    uint8_t chan = RADIO_CHANNEL;

    char command;

    int res = sscanf(str, "init %c", &command);

    if (res < 1) {
        printf("Usage: init (r|n)\n");
        printf("\tr\tinitialize as root\n");
        printf("\tn\tinitialize as node router\n");
    }

    uint8_t state;

    if ((command == 'n') || (command == 'r')) {
        printf("INFO: Initialize as %s on address %d\n", ((command == 'n') ? "node" : "root"), id);
        if (!id || (id > 255)) {
            printf("ERROR: address not a valid 8 bit integer\n");
            return;
        }

        state = rpl_init(TRANSCEIVER, id);

        if (state != SIXLOWERROR_SUCCESS) {
            printf("Error initializing RPL\n");
        }
        else {
            puts("6LoWPAN and RPL initialized.");
        }

        if (command == 'r') {
            rpl_init_root();
            is_root = 1;
        }
        else {
            ipv6_iface_set_routing_provider(rpl_get_next_hop);
        }
        int monitor_pid = thread_create(monitor_stack_buffer, MONITOR_STACK_SIZE, PRIORITY_MAIN-2, CREATE_STACKTEST, monitor, "monitor");
        transceiver_register(TRANSCEIVER, monitor_pid);
        ipv6_register_packet_handler(monitor_pid);
        //sixlowpan_lowpan_register(monitor_pid);
    }
    else {
        printf("ERROR: Unknown command '%c'\n", command);
        return;
    }

    /* TODO: check if this works as intended */
    ipv6_addr_t prefix, tmp;
    ipv6_addr_init(&std_addr, 0xABCD, 0xEF12, 0, 0, 0x1034, 0x00FF, 0xFE00, id);
    ipv6_addr_init_prefix(&prefix, &std_addr, 64);
    plist_add(&prefix, 64, NDP_OPT_PI_VLIFETIME_INFINITE, 0, 1, ICMPV6_NDP_OPT_PI_FLAG_AUTONOM);
    ipv6_init_iface_as_router();
    /* add global address */
    ipv6_addr_set_by_eui64(&tmp, &std_addr);
    ipv6_iface_add_addr(&tmp, IPV6_ADDR_TYPE_GLOBAL, NDP_ADDR_STATE_PREFERRED, 0, 0);
            
    /* set channel to 10 */
    tcmd.transceivers = TRANSCEIVER;
    tcmd.data = &chan;
    m.type = SET_CHANNEL;
    m.content.ptr = (void *) &tcmd;

    msg_send_receive(&m, &m, transceiver_pid);
    printf("Channel set to %u\n", RADIO_CHANNEL);

    destiny_init_transport_layer();
    puts("Destiny initialized");
    /* start transceiver watchdog */
}