// Post a message with payload and source address
int8_t post_longer(sos_pid_t did, sos_pid_t sid, uint8_t type, uint8_t len,
void *data, uint16_t flag, uint16_t saddr)
{
Message *m = msg_create();
if(m == NULL){
if(flag_msg_release(flag)){
ker_free(data);
}
return -ENOMEM;
}
m->daddr = node_address;
m->did = did;
m->type = type;
m->saddr = saddr;
m->sid = sid;
m->len = len;
m->data = (uint8_t*)data;
#ifdef SOS_USE_PREEMPTION
// assign priority based on priority of id
m->priority = get_module_priority(did);
#endif
m->flag = flag;
sched_msg_alloc(m);
ker_log( SOS_LOG_POST_LONG, sid, did );
return SOS_OK;
}
//----------------------------------------------------------------------------
// Funcation declarations
//----------------------------------------------------------------------------
// Post a message with no payload
int8_t post_short(sos_pid_t did, sos_pid_t sid, uint8_t type, uint8_t byte,
uint16_t word, uint16_t flag)
{
Message *m = msg_create();
MsgParam *p;
if(m == NULL){
return -ENOMEM;
}
m->daddr = node_address;
m->did = did;
m->type = type;
m->saddr = node_address;
m->sid = sid;
m->len = 3;
#ifdef SOS_USE_PREEMPTION
// assign priority based on did
m->priority = get_module_priority(did);
#endif
p = (MsgParam*)(m->payload);
p->byte = byte;
p->word = word;
m->flag = flag & ((sos_ker_flag_t)(~SOS_MSG_RELEASE));
sched_msg_alloc(m);
ker_log( SOS_LOG_POST_SHORT, sid, did );
return SOS_OK;
}
int8_t ker_timer_start(sos_pid_t pid, uint8_t tid, int32_t interval)
{
sos_timer_t* tt;
//! Start the timer from the timer pool
tt = alloc_from_timer_pool(pid, tid);
//! If the timer does not exist, then it is already in use or not initialized
if (tt == NULL) {
DEBUG_PID(TIMER_PID, "ker_timer_start: tt == NULL\n");
return -EINVAL;
}
// tt->ticks = PROCESSOR_TICKS(interval);
tt->ticks = interval;
tt->delta = interval;
//DEBUG("timer_start(%d) %d %d %d\n", tt->pid, tt->tid, tt->type, tt->ticks);
//! insert into delta queue
print_all_timers("timer_start_start");
timer_delta_q_insert(tt, true);
print_all_timers("timer_start_end");
ker_log( SOS_LOG_TIMER_START, pid, tid );
return SOS_OK;
}
/**
* @brief dispatch short message
* This is used by the callback that was register by interrupt handler
*/
void sched_dispatch_short_message(sos_pid_t dst, sos_pid_t src,
uint8_t type, uint8_t byte,
uint16_t word, uint16_t flag)
{
sos_module_t *handle;
msg_handler_t handler;
void *handler_state;
MsgParam *p;
handle = ker_get_module(dst);
if( handle == NULL ) { return; }
handler = (msg_handler_t)sos_read_header_ptr(handle->header,
offsetof(mod_header_t,
module_handler));
handler_state = handle->handler_state;
p = (MsgParam*)(short_msg.data);
short_msg.did = dst;
short_msg.sid = src;
short_msg.type = type;
p->byte = byte;
p->word = word;
short_msg.flag = flag;
/*
* Update current pid
*/
// curr_pid = dst;
ker_log( SOS_LOG_HANDLE_MSG, curr_pid, type );
ker_push_current_pid(dst);
handler(handler_state, &short_msg);
ker_pop_current_pid();
ker_log( SOS_LOG_HANDLE_MSG_END, curr_pid, type );
}
//! The implementation of this call can be optimized to include the find with
//! the remove. We will do it later
int8_t ker_timer_stop(sos_pid_t pid, uint8_t tid)
{
sos_timer_t* tt;
tt = find_timer_in_periodic_pool(pid, tid);
if( tt == NULL ) {
//! Find the timer block
tt = find_timer_block(pid, tid);
if (tt == NULL) {
return -EINVAL;
} else {
//! Remove the timer from the deltaq and any pending messages in the queue
timer_remove_timer(tt);
}
}
//timer_remove_timeout_from_scheduler(tt);
//! Re-insert timer into the pool
list_insert_tail(&timer_pool, (list_link_t*)tt);
ker_log( SOS_LOG_TIMER_STOP, pid, tid );
return SOS_OK;
}
int8_t ker_timer_restart(sos_pid_t pid, uint8_t tid, int32_t interval)
{
sos_timer_t* tt;
tt = find_timer_in_periodic_pool(pid, tid);
if (tt == NULL) {
//! Locate a running timer or from the timer pool
tt = find_timer_block(pid, tid);
if (tt != NULL){
timer_remove_timer(tt);
//timer_remove_timeout_from_scheduler(tt);
}
else {
tt = alloc_from_timer_pool(pid, tid);
}
}
//! The timer is neither running nor initialized
if (tt == NULL)
return -EINVAL;
/* Special Case restart with existing ticks field */
if( interval <= 0 )
interval = tt->ticks;
if(interval < TIMER_MIN_INTERVAL){
/* Need to put the timer back in to the pool
as an initialized timer that was never started */
list_insert_tail(&timer_pool, (list_link_t*)tt);
return -EPERM;
}
//! Initialize the data structure
tt->ticks = interval;
// tt->ticks = PROCESSOR_TICKS(interval);
tt->delta = interval;
//! Insert into the delta queue
timer_delta_q_insert(tt, true);
ker_log( SOS_LOG_TIMER_RESTART, pid, tid );
return SOS_OK;
}
static void do_dispatch()
{
Message *e; // Current message being dispatched
sos_module_t *handle; // Pointer to the control block of the destination module
Message *inner_msg = NULL; // Message sent as a payload in MSG_PKT_SENDDONE
sos_pid_t senddone_dst_pid = NULL_PID; // Destination module ID for the MSG_PKT_SENDDONE
uint8_t senddone_flag = SOS_MSG_SEND_FAIL; // Status information for the MSG_PKT_SENDDONE
SOS_MEASUREMENT_DEQUEUE_START();
e = mq_dequeue(&schedpq);
SOS_MEASUREMENT_DEQUEUE_END();
handle = ker_get_module(e->did);
// Destination module might muck around with the
// type field. So we check type before dispatch
if(e->type == MSG_PKT_SENDDONE) {
inner_msg = (Message*)(e->data);
}
// Check for reliable message delivery
if(flag_msg_reliable(e->flag)) {
senddone_dst_pid = e->sid;
}
// Deliver message to the monitor
// Ram - Modules might access kernel domain here
monitor_deliver_incoming_msg_to_monitor(e);
if(handle != NULL) {
if(sched_message_filtered(handle, e) == false) {
int8_t ret;
msg_handler_t handler;
void *handler_state;
DEBUG("###################################################################\n");
DEBUG("MESSAGE FROM %d TO %d OF TYPE %d\n", e->sid, e->did, e->type);
DEBUG("###################################################################\n");
// Get the function pointer to the message handler
handler = (msg_handler_t)sos_read_header_ptr(handle->header,
offsetof(mod_header_t,
module_handler));
// Get the pointer to the module state
handler_state = handle->handler_state;
// Change ownership if the release flag is set
// Ram - How to deal with memory blocks that are not released ?
if(flag_msg_release(e->flag)){
ker_change_own(e->data, e->did);
}
DEBUG("RUNNING HANDLER OF MODULE %d \n", handle->pid);
// curr_pid = handle->pid;
ker_log( SOS_LOG_HANDLE_MSG, curr_pid, e->type );
ker_push_current_pid(handle->pid);
ret = handler(handler_state, e);
ker_pop_current_pid();
ker_log( SOS_LOG_HANDLE_MSG_END, curr_pid, e->type );
DEBUG("FINISHED HANDLER OF MODULE %d \n", handle->pid);
if (ret == SOS_OK) senddone_flag = 0;
}
} else {
//XXX no error notification for now.
DEBUG("Scheduler: Unable to find module\n");
}
if(inner_msg != NULL) {
//! this is SENDDONE message
msg_dispose(inner_msg);
msg_dispose(e);
} else {
if(senddone_dst_pid != NULL_PID) {
if(post_long(senddone_dst_pid,
KER_SCHED_PID,
MSG_PKT_SENDDONE,
sizeof(Message), e,
senddone_flag) < 0) {
msg_dispose(e);
}
} else {
//! return message back to the pool
msg_dispose(e);
}
}
}
