void monitor_deliver_outgoing_msg_to_monitor(Message *m)
{
monitor_cb *curr;
#ifdef MSG_TRACE
#ifdef PC_PLATFORM
msg_trace(m, true);
#endif
#endif
if(cb_list == NULL) return;
curr = cb_list;
while(curr) {
/**
* We only deliver the message if the source is not
* the monitor. It does not make sense to deliver
* the message sent by the monitor.
*/
if((curr->type & MON_NET_OUTGOING) != 0 &&
curr->mod_handle->pid != m->sid) {
msg_handler_t handler =
(msg_handler_t)sos_read_header_ptr(
curr->mod_handle->header,
offsetof(mod_header_t,module_handler));
void *handler_state = curr->mod_handle->handler_state;
handler(handler_state, m);
}
curr = curr->next;
}
}
void monitor_deliver_incoming_msg_to_monitor(Message *m)
{
uint8_t type;
monitor_cb *curr;
#ifdef MSG_TRACE
#ifdef PC_PLATFORM
msg_trace(m, false);
#endif
#endif
if(cb_list == NULL) return;
/**
* in SOS, incoming message can be both local and
* from the network
*/
if(m->saddr == node_address) {
/**
* local message
*/
type = MON_LOCAL;
} else {
/**
* from network
*/
type = MON_NET_INCOMING;
}
curr = cb_list;
while(curr) {
/**
* We only deliver the message if the destination is
* not the monitor. It does not make sense to
* deliver the message twice
*/
if((curr->type & type) != 0 &&
curr->mod_handle->pid != m->did) {
msg_handler_t handler =
(msg_handler_t)sos_read_header_ptr(
curr->mod_handle->header,
offsetof(mod_header_t,module_handler));
void *handler_state = curr->mod_handle->handler_state;
#ifdef SOS_USE_PREEMPTION
// push the old pid and priority
*pid_sp++ = curr_pid;
*pri_sp++ - curr_pri;
// set the new priority
curr_pri = get_module_priority(curr->mod_handle->pid);
#endif
curr_pid = curr->mod_handle->pid;
handler(handler_state, m);
#ifdef SOS_USE_PREEMPTION
// pop the old pid and priority
curr_pid = *(--pid_sp);
curr_pri = *(--pri_sp);
#endif
}
curr = curr->next;
}
}
void monitor_deliver_outgoing_msg_to_monitor(Message *m)
{
monitor_cb *curr;
#ifndef SOS_USE_PREEMPTION
sos_pid_t prev_pid;
#endif
#ifdef MSG_TRACE
#ifdef PC_PLATFORM
msg_trace(m, true);
#endif
#endif
if(cb_list == NULL) return;
curr = cb_list;
while(curr) {
/**
* We only deliver the message if the source is not
* the monitor. It does not make sense to deliver
* the message sent by the monitor.
*/
if((curr->type & MON_NET_OUTGOING) != 0 &&
curr->mod_handle->pid != m->sid) {
msg_handler_t handler =
(msg_handler_t)sos_read_header_ptr(
curr->mod_handle->header,
offsetof(mod_header_t,module_handler));
void *handler_state = curr->mod_handle->handler_state;
#ifdef SOS_USE_PREEMPTION
// push the old pid and priority
*pid_sp++ = curr_pid;
*pri_sp++ = curr_pri;
curr_pri = get_module_priority(curr->mod_handle->pid);
#else
prev_pid = curr_pid;
#endif
curr_pid = curr->mod_handle->pid;
handler(handler_state, m);
#ifdef SOS_USE_PREEMPTION
// pop the old pid and priority
curr_pri = *(--pri_sp);
curr_pid = *(--pid_sp);
#else
curr_pid = prev_pid;
#endif
}
curr = curr->next;
}
}
/**
* @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 );
}
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);
}
}
}
/**
* @brief de-register a task (module)
* @param pid task id to be removed
* Note that this function cannot be used inside interrupt handler
*/
int8_t ker_deregister_module(sos_pid_t pid)
{
HAS_CRITICAL_SECTION;
uint8_t bins = hash_pid(pid);
sos_module_t *handle;
sos_module_t *prev_handle = NULL;
msg_handler_t handler;
/**
* Search the bins while save previous node
* Once found the module, connect next module to previous one
* put module back to freelist
*/
handle = mod_bin[bins];
while(handle != NULL) {
if(handle->pid == pid) {
break;
} else {
prev_handle = handle;
handle = handle->next;
}
}
if(handle == NULL) {
// unable to find the module
return -EINVAL;
}
handler = (msg_handler_t)sos_read_header_ptr(handle->header,
offsetof(mod_header_t,
module_handler));
if(handler != NULL) {
void *handler_state = handle->handler_state;
Message msg;
// sos_pid_t prev_pid = curr_pid;
//curr_pid = handle->pid;
ker_push_current_pid(handle->pid);
msg.did = handle->pid;
msg.sid = KER_SCHED_PID;
msg.type = MSG_FINAL;
msg.len = 0;
msg.data = NULL;
msg.flag = 0;
handler(handler_state, &msg);
ker_pop_current_pid();
// curr_pid = prev_pid;
}
// First remove handler from the list.
// link the bin back
ENTER_CRITICAL_SECTION();
if(prev_handle == NULL) {
mod_bin[bins] = handle->next;
} else {
prev_handle->next = handle->next;
}
LEAVE_CRITICAL_SECTION();
// remove the thread pid allocation
if(handle->pid >= SCHED_MIN_THREAD_PID) {
uint8_t i = handle->pid - SCHED_MIN_THREAD_PID;
pid_pool[i/8] &= ~(1 << (i % 8));
}
// remove system services
timer_remove_all(pid);
// sensor_remove_all(pid);
// ker_timestamp_deregister(pid);
fntable_remove_all(handle);
// free up memory
// NOTE: we can only free up memory at the last step
// because fntable is using the state
if((SOS_KER_STATIC_MODULE & (handle->flag)) == 0) {
ker_free(handle);
}
mem_remove_all(pid);
return 0;
}
static int8_t handle_fetcher_done( Message *msg )
{
fetcher_state_t *f = (fetcher_state_t*) msg->data;
if( is_fetcher_succeed( f ) == true ) {
//mod_header_ptr p;
loader_cam_t *cam;
fetcher_commit(f, true);
st.blocked = 0;
restartInterval( 0 );
cam = ker_cam_lookup( f->map.key );
if( cam->fetcher.fetchtype == FETCHTYPE_DATA) {
uint8_t buf[2];
ker_codemem_read( cam->fetcher.cm, KER_DFT_LOADER_PID, buf, 2, 0);
post_short(buf[0], KER_DFT_LOADER_PID, MSG_LOADER_DATA_AVAILABLE,
buf[1], cam->fetcher.cm, 0);
DEBUG_PID(KER_DFT_LOADER_PID, "Data Ready\n" );
#ifdef LOADER_NET_EXPERIMENT
ker_led(LED_GREEN_TOGGLE);
#endif
} else {
uint8_t mcu_type;
#ifndef SOS_SIM
uint8_t plat_type;
#endif
mod_header_ptr p;
#ifdef MINIELF_LOADER
// Link and load the module here
melf_load_module(cam->fetcher.cm);
#endif//MINIELF_LOADER
// Get the address of the module header
p = ker_codemem_get_header_address( cam->fetcher.cm );
// get processor type and platform type
mcu_type = sos_read_header_byte(p,
offsetof( mod_header_t, processor_type ));
#ifdef SOS_SIM
if( (mcu_type == MCU_TYPE) )
// In simulation, we don't check for platform
#else
plat_type = sos_read_header_byte(p,
offsetof( mod_header_t, platform_type ));
if( (mcu_type == MCU_TYPE) &&
( plat_type == HW_TYPE || plat_type == PLATFORM_ANY) )
#endif
{
/*
* If MCU is matched, this means we are using the same
* instruction set.
* And if this module is for this *specific* platform or
* simply for all platform with the same MCU
*/
// mark module executable
ker_codemem_mark_executable( cam->fetcher.cm );
if (cam->version & 0x80) {
#ifdef SOS_SFI
#ifdef MINIELF_LOADER
sfi_modtable_register(cam->fetcher.cm);
if (SOS_OK == ker_verify_module(cam->fetcher.cm)){
sfi_modtable_flash(p);
ker_register_module(p);
}
else
sfi_exception(KER_VERIFY_FAIL_EXCEPTION);
#else
uint16_t init_offset, code_size, handler_addr;
uint32_t handler_byte_addr, module_start_byte_addr;
uint16_t handler_sfi_addr;
handler_sfi_addr = sos_read_header_ptr(p, offsetof(mod_header_t, module_handler));
handler_addr = sfi_modtable_get_real_addr(handler_sfi_addr);
handler_byte_addr = (handler_addr * 2);
module_start_byte_addr = (p * 2);
init_offset = (uint16_t)(handler_byte_addr - module_start_byte_addr);
code_size = cam->code_size * LOADER_SIZE_MULTIPLIER;
if (SOS_OK == ker_verify_module(cam->fetcher.cm, init_offset, code_size)){
sfi_modtable_flash(p);
ker_register_module(p);
}
else
sfi_exception(KER_VERIFY_FAIL_EXCEPTION);
#endif //MINIELF_LOADER
#else
ker_register_module(p);
#endif //SOS_SFI
}
}
}
process_version_data( st.version_data, st.recent_neighbor);
} else {
DEBUG_PID( KER_DFT_LOADER_PID, "Fetch failed!, request %d\n", st.recent_neighbor);
f = (fetcher_state_t*) ker_msg_take_data(KER_DFT_LOADER_PID, msg);
fetcher_restart( f, st.recent_neighbor );
}
return SOS_OK;
}
