ER
chg_pri(ID tskid, PRI tskpri)
{
	TCB		*p_tcb;
	uint_t	newbpri;
	ER		ercd;

	LOG_CHG_PRI_ENTER(tskid, tskpri);
	CHECK_TSKCTX_UNL();
	CHECK_TSKID_SELF(tskid);
	CHECK_TPRI_INI(tskpri);
	p_tcb = get_tcb_self(tskid);
	CHECK_ACPTN(p_tcb->p_tinib->acvct.acptn2);
	newbpri = (tskpri == TPRI_INI) ? p_tcb->p_tinib->ipriority
										: INT_PRIORITY(tskpri);
	if (rundom != TACP_KERNEL) {
		CHECK_ILUSE(newbpri >= p_runtsk->p_tinib->p_dominib->minpriority);
	}

	t_lock_cpu();
	if (TSTAT_DORMANT(p_tcb->tstat)) {
		ercd = E_OBJ;
	}
	else if ((!queue_empty(&(p_tcb->mutex_queue))
										|| TSTAT_WAIT_MTX(p_tcb->tstat))
						&& !((*mtxhook_check_ceilpri)(p_tcb, newbpri))) {
		ercd = E_ILUSE;
	}
	else {
		p_tcb->bpriority = newbpri;
		if (queue_empty(&(p_tcb->mutex_queue))
								|| !((*mtxhook_scan_ceilmtx)(p_tcb))) {
			if (change_priority(p_tcb, newbpri, false)) {
				dispatch();
			}
		}
		ercd = E_OK;
	}
	t_unlock_cpu();

  error_exit:
	LOG_CHG_PRI_LEAVE(ercd);
	return(ercd);
}

errno_t hal_irq_alloc( int irq, void (*func)(void *arg), void *_arg, int is_shareable )
{
    if( irq < 0 && irq >= MAX_IRQ_COUNT )
        panic("IRQ %d > max %d", irq, MAX_IRQ_COUNT-1 );

    if( (!is_shareable) && !queue_empty( &heads[irq] ) )
    {
        printf("IRQ %d asked exculsive, but other user exist", irq);
        return EMLINK;
    }

    if( is_shareable && !queue_empty( &heads[irq] ) && (!((struct handler_q *)queue_first( &heads[irq] ))->is_shareable) )
    {
        printf("IRQ %d asked shared, but already exclusive", irq);
        return EMLINK;
    }


    struct handler_q *out = malloc(sizeof(struct handler_q));
    if(out == 0)
        return ENOMEM;

    out->ihandler = func;
    out->arg = _arg;
    out->is_shareable = is_shareable;

    // mask off IRQ when modifying it's handlers list
    board_interrupt_disable(irq);

    int ie = hal_save_cli();
    hal_spin_lock(&irq_list_lock);
    // Queue insert is not atomic, will break
    // if interrupt is executed on other CPU.
    // That is why we disable irq on controller before doing this.
    queue_enter(&heads[irq], out, struct handler_q *, chain);

    // Do it in spinlock to avoid races with disable_irq
    board_interrupt_enable(irq);

    hal_spin_unlock(&irq_list_lock);
    if(ie) hal_sti();

    return 0;
}

static ssize_t do_read(struct file *fp, char *buf, size_t count, loff_t *ppos)
{
	struct apm_user *	as;
	int			i;
	apm_event_t		event;
	DECLARE_WAITQUEUE(wait, current);

	as = fp->private_data;
	if (check_apm_user(as, "read"))
		return -EIO;
	if (count < sizeof(apm_event_t))
		return -EINVAL;
	if (queue_empty(as)) {
		if (fp->f_flags & O_NONBLOCK)
			return -EAGAIN;
		add_wait_queue(&apm_waitqueue, &wait);

repeat:
		set_current_state(TASK_INTERRUPTIBLE);
		if (queue_empty(as) && !signal_pending(current)) {
			schedule();
			goto repeat;
		}
		set_current_state(TASK_RUNNING);
		remove_wait_queue(&apm_waitqueue, &wait);
	}
	i = count;
	while ((i >= sizeof(event)) && !queue_empty(as)) {
		event = get_queued_event(as);

		if (copy_to_user(buf, &event, sizeof(event))) {
			if (i < count)
				break;
			return -EFAULT;
		}
		buf += sizeof(event);
		i -= sizeof(event);
	}
	if (i < count)
		return count - i;
	if (signal_pending(current))
		return -ERESTARTSYS;
	return 0;
}

ER
wai_flg(ID flgid, FLGPTN waiptn, MODE wfmode, FLGPTN *p_flgptn)
{
	FLGCB	*p_flgcb;
	WINFO_FLG *p_winfo_flg;
	ER		ercd;
	PCB		*my_p_pcb;
	TCB		*p_runtsk;

	LOG_WAI_FLG_ENTER(flgid, waiptn, wfmode, p_flgptn);
	CHECK_TSKCTX_UNL();
	CHECK_FLGID(flgid);
	CHECK_PAR(waiptn != 0U);
	CHECK_PAR(wfmode == TWF_ORW || wfmode == TWF_ANDW);
	p_flgcb = get_flgcb(flgid);

	t_lock_cpu();
	my_p_pcb = get_my_p_pcb();
	T_CHECK_DISPATCH(my_p_pcb);

  retry:
	t_acquire_obj_lock(&GET_OBJLOCK(p_flgcb));
	if ((p_flgcb->p_flginib->flgatr & TA_WMUL) == 0U
					&& !queue_empty(&(p_flgcb->wait_queue))) {
		release_obj_lock(&GET_OBJLOCK(p_flgcb));
		ercd = E_ILUSE;
	}
	else if (check_flg_cond(p_flgcb, waiptn, wfmode, p_flgptn)) {
		release_obj_lock(&GET_OBJLOCK(p_flgcb));
		ercd = E_OK;
	}
	else {
		if ((my_p_pcb = t_acquire_nested_tsk_lock_self(&GET_OBJLOCK(p_flgcb))) == NULL){
			goto retry;
		}
		p_runtsk = my_p_pcb->p_runtsk;
		p_winfo_flg = (WINFO_FLG *)(&(p_runtsk->winfo_obj));
		p_winfo_flg->waiptn = waiptn;
		p_winfo_flg->wfmode = wfmode;
		p_runtsk->tstat = (TS_WAITING | TS_WAIT_FLG);
		wobj_make_wait((WOBJCB *) p_flgcb, p_runtsk);
		release_nested_tsk_lock(my_p_pcb);
		release_obj_lock(&GET_OBJLOCK(p_flgcb));
		dispatch();
		/* 値の参照だけなのでロックは必要ない */
		ercd = p_runtsk->wercd;
		if (ercd == E_OK) {
			*p_flgptn = p_winfo_flg->flgptn;
		}
	}
	t_unlock_cpu();

  error_exit:
	LOG_WAI_FLG_LEAVE(ercd, *p_flgptn);
	return(ercd);
}

ER
ter_tsk(ID tskid)
{
	TCB		*p_tcb;
	bool_t	dspreq = false;
	ER		ercd;

	LOG_TER_TSK_ENTER(tskid);
	CHECK_TSKCTX_UNL();
	CHECK_TSKID(tskid);
	p_tcb = get_tcb(tskid);
	CHECK_NONSELF(p_tcb);
	CHECK_ACPTN(p_tcb->p_tinib->acvct.acptn2);

	t_lock_cpu();
	if (TSTAT_DORMANT(p_tcb->tstat)) {
		ercd = E_OBJ;
	}
	else {
		if (TSTAT_RUNNABLE(p_tcb->tstat)) {
			/*
			 *  p_tcbは自タスクでないため，（シングルプロセッサでは）実
			 *  行状態でなく，make_non_runnable(p_tcb)でタスクディスパッ
			 *  チが必要になることはない．
			 */
			(void) make_non_runnable(p_tcb);
		}
		else if (TSTAT_WAITING(p_tcb->tstat)) {
			if (wait_dequeue_wobj(p_tcb)) {
				dspreq = true;
			}
			wait_dequeue_tmevtb(p_tcb);
		}
		if (!queue_empty(&(p_tcb->mutex_queue))) {
			if ((*mtxhook_release_all)(p_tcb)) {
				dspreq = true;
			}
		}
		make_dormant(p_tcb);
		if (p_tcb->actque) {
			p_tcb->actque = false;
			if (make_active(p_tcb)) {
				dspreq = true;
			}
		}
		if (dspreq) {
			dispatch();
		}
		ercd = E_OK;
	}
	t_unlock_cpu();

  error_exit:
	LOG_TER_TSK_LEAVE(ercd);
	return(ercd);
}

static ER
bit_semaphore(ID semid)
{
	SEMCB			*p_semcb;
	const SEMINIB	*p_seminib;
	uint_t			semcnt;
	QUEUE			*p_queue;
	TCB				*p_tcb;

	if (!(TMIN_SEMID <= (semid) && (semid) <= tmax_semid)) {
		return(E_ID);
	}
	p_semcb = get_semcb(semid);
	p_seminib = p_semcb->p_seminib;
	semcnt = p_semcb->semcnt;

	/*
	 *  初期化ブロックへのポインタの検査
	 */
	if (p_seminib != &(seminib_table[INDEX_SEM(semid)])) {
		return(E_SYS_LINENO);
	}

	/*
	 *  semcntの検査
	 */
	if (semcnt > p_seminib->maxsem) {
		return(E_SYS_LINENO);
	}

	/*
	 *  wait_queueの検査
	 */
	if (semcnt == 0) {
		p_queue = p_semcb->wait_queue.p_next;
		while (p_queue != &(p_semcb->wait_queue)) {
			p_tcb = (TCB *) p_queue;
			p_queue = p_queue->p_next;
			if (!VALID_TCB(p_tcb)) {
				return(E_SYS_LINENO);
			}
			if (p_tcb->tstat != (TS_WAITING | TS_WAIT_SEM)) {
				return(E_SYS_LINENO);
			}
			if (p_semcb != ((WINFO_SEM *)(p_tcb->p_winfo))->p_semcb) {
				return(E_SYS_LINENO);
			}
		}
	}
	else {
		if (!queue_empty(&(p_semcb->wait_queue))) {
			return(E_SYS_LINENO);
		}
	}
	return(E_OK);
}

int dequeue (queue_t *buf, int *data) {
  //int data = 0;
  if(queue_empty(buf))
    return 0;
  else {
    *data = buf->buffer[buf->head];
    buf->head = ((buf->head +1) == QUEUE_SIZE) ? 0 : buf->head + 1;
  }
  return 1;
}

int
solvable_lookup_deparray(Solvable *s, Id keyname, Queue *q, Id marker)
{
  if (!s->repo)
    {
      queue_empty(q);
      return 0;
    }
  return repo_lookup_deparray(s->repo, s - s->repo->pool->solvables, keyname, q, marker);
}

int dequeue_single(struct queue_stub *stub,struct queue_element*ele)
{
    if(queue_empty(stub))
        return -1;
    ele->rte_pkt_offset=stub->records[stub->front_ptr].rte_pkt_offset;
    ele->rte_data_offset=stub->records[stub->front_ptr].rte_data_offset;
    WRITE_MEM_WB();
    stub->front_ptr=(stub->front_ptr+1)%(stub->ele_num+1);
    return 0;
}

/**
* \fn void display_queue_int(queue q);
* \brief Used to display the content of a queue as if they were integers.
* \details This function is non-destructive.
* \param q The queue to display.
*/
void display_queue_int(queue q){
    direction cur_elt;

    printf("< ");
    while(!queue_empty(&q)){
        cur_elt = dequeue(&q);
        printf("%i ", cur_elt);
    }
    printf("<\n");
}

unsigned int poll_qp(struct file *file, poll_table * wait)
{
  int temp_1;

  poll_wait(file, &queue->proc_list, wait);  
  temp_1 = queue_empty();
  if (!temp_1)
    return (0x0001  | 0x0040) ;
  return 0;
}

int dequeue (queue_t *buf) {
  int result;
  if(queue_empty(buf)){
    return 0;
  } else {
    result = buf->buffer[buf->tail];
    buf->tail = ((buf->tail + 1) == QUEUE_SIZE) ? 0 : buf->tail + 1;
  }
  return result;
}

void handle_events (queue_t q)
{
  queue_entry_t event;
  while (!queue_empty (q))
    {
      event = queue_dequeue (q);
      handle_event (event);
      free (event);
    }
}

void queue_push(QUEUE q, VALUE v) {
    if (queue_empty(q)) {
        q->head = q->tail = make_queue_node(v, NULL, NULL);
    }
    else {
        QUEUE_NODE n = make_queue_node(v, tail, NULL);
        q->tail->next = n;
        q->tail = n;
    }
}

static void async_cb(uv_async_t *handle)
{
  Loop *l = handle->loop->data;
  uv_mutex_lock(&l->mutex);
  while (!queue_empty(l->thread_events)) {
    Event ev = queue_get(l->thread_events);
    queue_put_event(l->fast_events, ev);
  }
  uv_mutex_unlock(&l->mutex);
}

void sendGetSW(int sock, struct sockaddr_in from)
{
    packet *p;

    // check if the target chunkHash has been transferred by other peers
    // allow concurrent download
    while (queue_contains(peers[nodeInMap]->chunkHash) == EXIT_FAILURE)
    {
        if (peers[nodeInMap]->next == NULL )
        {
            if (queue_empty() == EXIT_SUCCESS)
            {
                dprintf(STDOUT_FILENO, "JOB is done\n");
                numDataMisses[nodeInMap] = -1;
            }
            return;
        }

        linkNode *temp = peers[nodeInMap];
        peers[nodeInMap] = peers[nodeInMap]->next;
        free(temp);
    }

    if (peers[nodeInMap]->chunkHash == NULL )
    {
        dprintf(STDOUT_FILENO, "sending chunk equals zero\n");
        return;
    }

    p = packet_factory(GET);
    p->packet_length = 20 + BT_CHUNKS_SIZE;
    p->ack_num = 0;

    p->payload[0] = 1;
    p->payload[1] = '\0';
    p->payload[2] = '\0';
    p->payload[3] = '\0';

    strncpy(p->payload + 4, peers[nodeInMap]->chunkHash, BT_CHUNKS_SIZE);
    spiffy_sendto(sock, p, p->packet_length, 0, (struct sockaddr *) &from,
            sizeof(from));

    jobs[nodeInMap] = getChunkId(peers[nodeInMap]->chunkHash, chunkf);

    dprintf(STDOUT_FILENO, "Requesting chunk ID: %d from %d\n", jobs[nodeInMap],
            nodeInMap);

    nextExpected[nodeInMap] = 1;
    GETTTL[nodeInMap] = 0;
    free(p);

    // chunk is transferring, remove it from work queue so that
    // other peers won't transfer this again
    queue_remove(getChunkHash(jobs[nodeInMap], chunkf));
}

ER
twai_flg(ID flgid, FLGPTN waiptn, MODE wfmode, FLGPTN *p_flgptn, TMO tmout)
{
    FLGCB	*p_flgcb;
    WINFO_FLG winfo_flg;
    TMEVTB	tmevtb;
    ER		ercd;

    LOG_TWAI_FLG_ENTER(flgid, waiptn, wfmode, p_flgptn, tmout);
    CHECK_DISPATCH();
    CHECK_FLGID(flgid);
    CHECK_PAR(waiptn != 0U);
    CHECK_PAR(wfmode == TWF_ORW || wfmode == TWF_ANDW);
    CHECK_MACV_WRITE(p_flgptn, FLGPTN);
    CHECK_TMOUT(tmout);
    p_flgcb = get_flgcb(flgid);

    t_lock_cpu();
    if (p_flgcb->p_flginib->flgatr == TA_NOEXS) {
        ercd = E_NOEXS;
    }
    else if (VIOLATE_ACPTN(p_flgcb->p_flginib->acvct.acptn2)) {
        ercd = E_OACV;
    }
    else if ((p_flgcb->p_flginib->flgatr & TA_WMUL) == 0U
             && !queue_empty(&(p_flgcb->wait_queue))) {
        ercd = E_ILUSE;
    }
    else if (check_flg_cond(p_flgcb, waiptn, wfmode, p_flgptn)) {
        ercd = E_OK;
    }
    else if (tmout == TMO_POL) {
        ercd = E_TMOUT;
    }
    else if (p_runtsk->waifbd) {
        ercd = E_RLWAI;
    }
    else {
        winfo_flg.waiptn = waiptn;
        winfo_flg.wfmode = wfmode;
        p_runtsk->tstat = (TS_WAITING | TS_WAIT_FLG);
        wobj_make_wait_tmout((WOBJCB *) p_flgcb, (WINFO_WOBJ *) &winfo_flg,
                             &tmevtb, tmout);
        dispatch();
        ercd = winfo_flg.winfo.wercd;
        if (ercd == E_OK) {
            *p_flgptn = winfo_flg.flgptn;
        }
    }
    t_unlock_cpu();

error_exit:
    LOG_TWAI_FLG_LEAVE(ercd, *p_flgptn);
    return(ercd);
}

ER
ext_tsk(void)
{
	ER		ercd;

	LOG_EXT_TSK_ENTER();
	CHECK_TSKCTX();

	if (t_sense_lock()) {
		/*
		 *  CPUロック状態でext_tskが呼ばれた場合は，CPUロックを解除し
		 *  てからタスクを終了する．実装上は，サービスコール内でのCPU
		 *  ロックを省略すればよいだけ．
		 */
	}
	else {
		t_lock_cpu();
	}
	if (disdsp) {
		/*
		 *  ディスパッチ禁止状態でext_tskが呼ばれた場合は，ディスパッ
		 *  チ許可状態にしてからタスクを終了する．
		 */
		disdsp = false;
	}
	if (!ipmflg) {
		/*
		 *  割込み優先度マスク（IPM）がTIPM_ENAALL以外の状態でext_tsk
		 *  が呼ばれた場合は，IPMをTIPM_ENAALLにしてからタスクを終了す
		 *  る．
		 */
		t_set_ipm(TIPM_ENAALL);
		ipmflg = true;
	}
	dspflg = true;

	(void) make_non_runnable(p_runtsk);
	if (!queue_empty(&(p_runtsk->mutex_queue))) {
		(void) (*mtxhook_release_all)(p_runtsk);
	}
#ifdef TOPPERS_SUPPORT_OVRHDR
	ovrtimer_stop();
#endif /* TOPPERS_SUPPORT_OVRHDR */
	make_dormant(p_runtsk);
	if (p_runtsk->actque) {
		p_runtsk->actque = false;
		(void) make_active(p_runtsk);
	}
	exit_and_dispatch();
	ercd = E_SYS;

  error_exit:
	LOG_EXT_TSK_LEAVE(ercd);
	return(ercd);
}

/*
 * The scheduler is called with the context of the current thread,
 * or NULL when the scheduler is first started.
 *
 * The general operation of this function is:
 *   1.  Save the context argument into the current thread.
 *   2.  Either queue up or deallocate the current thread,
 *       based on its state.
 *   3.  Select a new "ready" thread to run, and set the "current"
 *       variable to that thread.
 *        - If no "ready" thread is available, examine the system
 *          state to handle this situation properly.
 *   4.  Return the context of the thread to run, so that a context-
 *       switch will be performed to start running the next thread.
 *
 * This function is global because it needs to be called from the assembly.
 */
ThreadContext *__sthread_scheduler(ThreadContext *context) {
    if (context != NULL) {
        /* Save the context argument into the current thread. */
        current->context = context;
        /* Either queue up or deallocate current thread, based on its state. */
        if (current->state == ThreadFinished) {
            /* Deallocatate because finished. */
            __sthread_delete(current);
        }
        else if (current->state == ThreadBlocked) {
            /* Queue up because blocked. */
            queue_add(current);
        }
        else if (current->state == ThreadRunning) {
            /* Queue up because yielding and make it ready. */
            current->state = ThreadReady;
            queue_add(current);
        }
    }

    if (queue_empty(&ready_queue)) {
        if (queue_empty(&blocked_queue)) {
            /* Nothing ready and nothing blocked, so done! */
            printf("Done.\n");
            exit(0);
        }
        else {
            /* Nothing ready but still blocked threads, so deadlock. */
            printf("Deadlock.\n");
            /* Exit with error. */
            exit(1);
        }
    }
    else {
        /* Select a new "ready" thread to run, and set the "current" variable to
         * that thread. */
        current = queue_take(&ready_queue);
        current->state = ThreadRunning;
    }

    return current->context;
}

kern_return_t
wait_queue_unlink_all(
	wait_queue_t wq)
{
	wait_queue_element_t wq_element;
	wait_queue_element_t wq_next_element;
	wait_queue_set_t wq_set;
	wait_queue_link_t wql;
	queue_head_t links_queue_head;
	queue_t links = &links_queue_head;
	queue_t q;
	spl_t s;

	if (!wait_queue_is_valid(wq)) {
		return KERN_INVALID_ARGUMENT;
	}

	queue_init(links);

	s = splsched();
	wait_queue_lock(wq);

	q = &wq->wq_queue;

	wq_element = (wait_queue_element_t) queue_first(q);
	while (!queue_end(q, (queue_entry_t)wq_element)) {
		boolean_t alloced;

		WAIT_QUEUE_ELEMENT_CHECK(wq, wq_element);
		wq_next_element = (wait_queue_element_t)
			     queue_next((queue_t) wq_element);

		alloced = (wq_element->wqe_type == WAIT_QUEUE_LINK);
		if (alloced || wq_element->wqe_type == WAIT_QUEUE_LINK_NOALLOC) {
			wql = (wait_queue_link_t)wq_element;
			wq_set = wql->wql_setqueue;
			wqs_lock(wq_set);
			wait_queue_unlink_locked(wq, wq_set, wql);
			wqs_unlock(wq_set);
			if (alloced)
				enqueue(links, &wql->wql_links);
		}
		wq_element = wq_next_element;
	}
	wait_queue_unlock(wq);
	splx(s);

	while(!queue_empty(links)) {
		wql = (wait_queue_link_t) dequeue(links);
		zfree(_wait_queue_link_zone, wql);
	}

	return(KERN_SUCCESS);
}