/*!
* Block thread (on conditional variable) and release monitor
*/
int sys__monitor_wait ( monitor_t *monitor, monitor_q *queue )
{
kmonitor_t *kmonitor;
kmonitor_q *kqueue;
ASSERT_ERRNO_AND_EXIT ( monitor && monitor->ptr, E_INVALID_HANDLE );
ASSERT_ERRNO_AND_EXIT ( queue && queue->ptr, E_INVALID_HANDLE );
kmonitor = monitor->ptr;
kqueue = queue->ptr;
ASSERT_ERRNO_AND_EXIT ( kmonitor->owner == k_get_active_thread (),
E_NOT_OWNER );
disable_interrupts ();
SET_ERRNO ( SUCCESS );
k_set_thread_qdata ( NULL, kmonitor );
k_enqueue_thread ( NULL, &kqueue->queue );
kmonitor->owner = k_threadq_get ( &kmonitor->queue );
if ( !k_release_thread ( &kmonitor->queue ) )
kmonitor->lock = FALSE;
k_schedule_threads ();
enable_interrupts ();
EXIT ( SUCCESS );
}
/*!
* Unlock monitor (or block trying)
*/
int sys__monitor_unlock ( monitor_t *monitor )
{
kmonitor_t *kmonitor;
ASSERT_ERRNO_AND_EXIT ( monitor && monitor->ptr, E_INVALID_HANDLE );
kmonitor = monitor->ptr;
ASSERT_ERRNO_AND_EXIT ( kmonitor->owner == k_get_active_thread (),
E_NOT_OWNER );
disable_interrupts ();
SET_ERRNO ( SUCCESS );
kmonitor->owner = k_threadq_get ( &kmonitor->queue );
if ( !k_release_thread ( &kmonitor->queue ) )
kmonitor->lock = FALSE;
else
k_schedule_threads ();
enable_interrupts ();
EXIT ( SUCCESS );
}
/*! Unlock device */
int k_device_unlock ( kdevice_t *dev )
{
if ( k_release_thread ( &dev->thrq ) )
k_schedule_threads ();
else
dev->locked = FALSE;
return 0;
}
/*! Release first or all threads from monitor queue (cond.var.) */
static int k_monitor_release ( monitor_q *queue, int broadcast )
{
kmonitor_q *kqueue;
kthread_t *kthr;
kmonitor_t *kmonitor;
int reschedule = 0;
ASSERT_ERRNO_AND_EXIT ( queue && queue->ptr, E_INVALID_HANDLE );
disable_interrupts ();
kqueue = queue->ptr;
do {
kthr = k_threadq_get ( &kqueue->queue ); /* first from queue */
if ( !kthr )
break;
kmonitor = k_get_thread_qdata ( kthr );
if ( !kmonitor->lock ) /* monitor not locked? */
{
/* unblocked thread becomes monitor owner */
kmonitor->lock = TRUE;
kmonitor->owner = kthr;
k_release_thread ( &kqueue->queue );/*to ready threads*/
reschedule++;
}
else {
/* move thread from monitor queue (cond.var.)
to monitor entrance queue */
kthr = k_threadq_remove ( &kqueue->queue, NULL );
k_enqueue_thread ( kthr, &kmonitor->queue );
}
}
while ( kthr && broadcast );
SET_ERRNO ( SUCCESS );
if ( reschedule )
k_schedule_threads ();
enable_interrupts ();
EXIT ( SUCCESS );
}
/*!
* Send message to queue or signal to thread
*/
int sys__msg_post ( void *p )
{
/* parameters on thread stack */
int dest_type; /* MSG_QUEUE, MSG_THREAD or MSG_SIGNAL */
void *dest; /* (msg_q *) or (thread_t *) */
msg_t *msg; /* { type, size, data[0..size-1] } */
uint flags;
/* local variables */
thread_t *thr;
kthread_t *kthr, *new_kthr;
kthrmsg_qs *thrmsg;
kgmsg_q *kgmsgq;
kmsg_q *kmsgq;
msg_q *msgq;
kmsg_t *kmsg;
msg_t *cmsg;
kprocess_t *proc;
dest_type = *( (int *) p ); p += sizeof (int);
dest = *( (void **) p ); p += sizeof (void *);
msg = *( (msg_t **) p ); p += sizeof (msg_t *);
flags = *( (uint *) p );
ASSERT_ERRNO_AND_EXIT ( dest && msg, E_INVALID_HANDLE );
dest = U2K_GET_ADR ( dest, k_get_active_process () );
msg = U2K_GET_ADR ( msg, k_get_active_process () );
if ( dest_type == MSG_THREAD || dest_type == MSG_SIGNAL )
{
thr = dest;
kthr = k_get_kthread ( thr );
ASSERT_ERRNO_AND_EXIT ( kthr, E_DONT_EXIST );
thrmsg = k_get_thrmsg ( kthr );
kmsgq = &thrmsg->msgq;
}
else if ( dest_type == MSG_QUEUE )
{
msgq = dest;
kgmsgq = msgq->handle;
ASSERT_ERRNO_AND_EXIT ( kgmsgq && kgmsgq->id == msgq->id,
E_INVALID_HANDLE );
kmsgq = &kgmsgq->mq;
}
else {
EXIT ( E_INVALID_TYPE );
}
if ( dest_type == MSG_THREAD || dest_type == MSG_QUEUE )
{
/* send message to queue */
if ( kmsgq->min_prio <= msg->type ) /* msg has required prio. */
{
kmsg = kmalloc ( sizeof (kmsg_t) + msg->size );
ASSERT_ERRNO_AND_EXIT ( kmsg, E_NO_MEMORY );
kmsg->msg.type = msg->type;
kmsg->msg.size = msg->size;
memcpy ( kmsg->msg.data, msg->data, msg->size );
list_append ( &kmsgq->msgs, kmsg, &kmsg->list );
/* is thread waiting for message? */
if ( k_release_thread ( &kmsgq->thrq ) )
k_schedule_threads ();
EXIT ( SUCCESS );
}
else { /* ignore message */
EXIT ( E_IGNORED );
}
}
/* must be MSG_SIGNAL */
if ( thrmsg->sig_prio <= msg->type )
{
/* create thread that will service this signal */
cmsg = k_create_thread_private_storage ( kthr,
sizeof (msg_t) + msg->size );
cmsg->type = msg->type;
cmsg->size = msg->size;
memcpy ( cmsg->data, msg->data, msg->size );
proc = k_get_thread_process ( kthr );
new_kthr = k_create_thread (
thrmsg->signal_handler,
K2U_GET_ADR ( cmsg, proc ), proc->pi->exit,
k_get_thread_prio ( kthr ) + 1, NULL, 0, 1, proc
);
ASSERT_ERRNO_AND_EXIT ( new_kthr, k_get_errno() );
k_set_thread_private_storage ( new_kthr, cmsg );
SET_ERRNO ( SUCCESS );
k_schedule_threads ();
RETURN ( SUCCESS );
}
else { /* ignore signal */
EXIT ( E_IGNORED );
}
}
/*! Send message to queue or signal to thread */
int sys__msg_post ( int dest_type, void *dest, msg_t *msg, uint flags )
{
thread_t *thr;
kthread_t *kthr, *new_kthr;
kthrmsg_qs *thrmsg;
kgmsg_q *kgmsgq;
kmsg_q *kmsgq;
msg_q *msgq;
kmsg_t *kmsg;
msg_t *cmsg;
SYS_ENTRY();
ASSERT_ERRNO_AND_SYS_EXIT ( dest && msg, E_INVALID_HANDLE );
if ( dest_type == MSG_THREAD || dest_type == MSG_SIGNAL )
{
thr = dest;
kthr = k_get_kthread ( thr );
ASSERT_ERRNO_AND_SYS_EXIT ( kthr, E_DONT_EXIST );
thrmsg = k_get_thrmsg ( kthr );
kmsgq = &thrmsg->msgq;
}
else if ( dest_type == MSG_QUEUE )
{
msgq = dest;
kgmsgq = msgq->handle;
ASSERT_ERRNO_AND_SYS_EXIT ( kgmsgq && kgmsgq->id == msgq->id,
E_INVALID_HANDLE );
kmsgq = &kgmsgq->mq;
}
else {
SYS_EXIT ( E_INVALID_TYPE );
}
if ( dest_type == MSG_THREAD || dest_type == MSG_QUEUE )
{
/* send message to queue */
if ( kmsgq->min_prio <= msg->type ) /* msg has required prio. */
{
kmsg = kmalloc ( sizeof (kmsg_t) + msg->size );
ASSERT_ERRNO_AND_SYS_EXIT ( kmsg, E_NO_MEMORY );
kmsg->msg.type = msg->type;
kmsg->msg.size = msg->size;
memcpy ( kmsg->msg.data, msg->data, msg->size );
list_append ( &kmsgq->msgs, kmsg, &kmsg->list );
/* is thread waiting for message? */
if ( k_release_thread ( &kmsgq->thrq ) )
k_schedule_threads ();
SYS_EXIT ( SUCCESS );
}
else { /* ignore message */
SYS_EXIT ( E_IGNORED );
}
}
/* must be MSG_SIGNAL */
//promijenjen uvijet
//ako je signal_handler postavljen, tada šalji signal
if ( thrmsg->sig_prio <= msg->type && thrmsg->signal_handler[msg->type] != NULL )
{
/* create thread that will service this signal */
cmsg = k_create_thread_private_storage ( kthr,
sizeof (msg_t) + msg->size );
cmsg->type = msg->type;
cmsg->size = msg->size;
memcpy ( cmsg->data, msg->data, msg->size );
new_kthr = k_create_thread (
//koji handler???
thrmsg->signal_handler[msg->type], cmsg, pi.exit,
k_get_thread_prio ( kthr ) + 1, NULL, 0, 1
);
ASSERT_ERRNO_AND_SYS_EXIT ( new_kthr, k_get_errno() );
k_set_thread_private_storage ( new_kthr, cmsg );
//SET_ERRNO ( SUCCESS );
k_schedule_threads ();
SYS_EXIT ( SUCCESS );
}
else { /* ignore signal */
SYS_EXIT ( E_IGNORED );
}
}
