static inline int8_t set_num_funcs_in_send_state(sos_shm_t key)
{
fetcher_cam_t *cam;
mod_header_t hdr;
cam = (fetcher_cam_t *) ker_shm_get( KER_FETCHER_PID, key);
if( cam == NULL ) {
// We don't have the module, give up
return -EINVAL;
}
if( cam->fetchtype != FETCHTYPE_MODULE ) {
// not module
return SOS_OK;
}
// if we are currently fetching the same module
if( fst != NULL && (fst->map.key == key) ) {
if( (fst->map.bitmap[0] & 0x01) != 0 ) {
// number func is not available
return -EINVAL;
}
}
// read module header from flash and set it
if( ker_codemem_read(cam->cm, KER_FETCHER_PID,
&hdr, sizeof(mod_header_t), 0) != SOS_OK ) {
return -EINVAL;
}
send_state.num_funcs = hdr.num_sub_func + hdr.num_prov_func;
return SOS_OK;
}
static int8_t handle_loader_ls_on_node( Message *msg )
{
msg_ls_reply_t *reply;
uint8_t i;
reply = (msg_ls_reply_t *) ker_malloc( sizeof( msg_ls_reply_t ),
KER_DFT_LOADER_PID );
if( reply == NULL ) return -ENOMEM;
for( i = 0; i < NUM_LOADER_PARAMS_ENTRIES; i ++ ) {
sos_cam_t key = ker_cam_key( KER_DFT_LOADER_PID, i );
loader_cam_t *cam;
uint8_t buf[2];
cam = ker_cam_lookup( key );
if( cam != NULL && cam->fetcher.status == FETCHING_DONE
&& (ker_codemem_read( cam->fetcher.cm, KER_DFT_LOADER_PID,
buf, 2, 0) == SOS_OK)) {
DEBUG_PID( KER_DFT_LOADER_PID, "Data(%d) = %d %d\n", i, buf[0], buf[1]);
reply->pam_dst_pid[ i ] = buf[0];
reply->pam_dst_subpid[ i ] = buf[1];
} else {
DEBUG_PID( KER_DFT_LOADER_PID, "Data(%d) = NULL\n", i);
/*
if( cam != NULL && cam->fetcher.status == FETCHING_DONE) {
DEBUG_PID( KER_DFT_LOADER_PID, "ker_codemem_read failed...\n");
}
*/
reply->pam_dst_pid[ i ] = NULL_PID;
reply->pam_dst_subpid[ i ] = 0;
}
}
for( i = 0; i < NUM_LOADER_MODULE_ENTRIES; i ++ ) {
sos_cam_t key = ker_cam_key( KER_DFT_LOADER_PID,
NUM_LOADER_PARAMS_ENTRIES + i );
loader_cam_t *cam;
cam = ker_cam_lookup( key );
if( cam != NULL && cam->fetcher.status == FETCHING_DONE ) {
mod_header_ptr p;
sos_code_id_t cid;
// Get the address of the module header
p = ker_codemem_get_header_address( cam->fetcher.cm );
cid = sos_read_header_word( p, offsetof(mod_header_t, code_id));
// warning: already netowrk order...
reply->code_id[ i ] = cid;
} else {
reply->code_id[ i ] = 0;
}
}
post_auto( KER_DFT_LOADER_PID,
KER_DFT_LOADER_PID,
MSG_LOADER_LS_REPLY,
sizeof( msg_ls_reply_t ),
reply,
SOS_MSG_RELEASE,
msg->saddr);
return SOS_OK;
}
static inline void send_fragment()
{
uint16_t frag_id;
uint8_t i, j;
uint8_t mask = 1;
uint8_t ret;
fetcher_fragment_t *out_pkt;
fetcher_cam_t *cam;
if ( send_state.map == NULL ) {
ker_timer_stop(KER_FETCHER_PID, FETCHER_TRANSMIT_TID);
return;
}
cam = (fetcher_cam_t *) ker_shm_get( KER_FETCHER_PID, send_state.map->key);
if ( cam == NULL ) {
// file got deleted. give up!
free_send_state_map();
return;
}
if ( send_state.frag != NULL) {
//! timer fires faster than data reading. Highly unlikely...
//! but we still handle it.
return;
}
//! search map and find one fragment to send
for(i = 0; i < send_state.map->bitmap_size; i++) {
//! for each byte
if(send_state.map->bitmap[i] != 0) {
break;
}
}
if(i == send_state.map->bitmap_size) {
/*
* Did not find any block...
*/
free_send_state_map();
return;
}
//sos_assert(i < send_state.map->bitmap_size);
frag_id = i * 8;
mask = 1;
for(j = 0; j < 8; j++, mask = mask << 1) {
if(mask & (send_state.map->bitmap[i])) {
send_state.map->bitmap[i] &= ~(mask);
break;
}
}
//sos_assert(j < 8);
frag_id += j;
print_bitmap(send_state.map);
out_pkt = (fetcher_fragment_t*)ker_malloc(sizeof(fetcher_fragment_t), KER_FETCHER_PID);
if(out_pkt == NULL){
DEBUG_PID(KER_FETCHER_PID,"malloc fetcher_fragment_t failed\n");
goto send_fragment_postproc;
}
out_pkt->frag_id = ehtons(frag_id);
out_pkt->key = ehtons(send_state.map->key);
ret = ker_codemem_read(cam->cm, KER_FETCHER_PID,
out_pkt->fragment, FETCHER_FRAGMENT_SIZE,
frag_id * (code_addr_t)FETCHER_FRAGMENT_SIZE);
if(ret == SOS_SPLIT) {
send_state.frag = out_pkt;
} else if(ret != SOS_OK){
DEBUG_PID(KER_FETCHER_PID, "codemem_read failed\n");
ker_free(out_pkt);
goto send_fragment_postproc;
}
//DEBUG("out_pkt has addr %x\n", (int)out_pkt);
DEBUG_PID(KER_FETCHER_PID, "send_fragment: frag_id = %d to %d\n", frag_id, send_state.dest);
ret = post_auto(KER_FETCHER_PID,
KER_FETCHER_PID,
MSG_FETCHER_FRAGMENT,
sizeof(fetcher_fragment_t),
out_pkt,
SOS_MSG_RELEASE | SOS_MSG_RELIABLE,
send_state.dest);
if( ret == SOS_OK ) {
send_state.num_msg_in_queue++;
}
send_fragment_postproc:
if(check_map(send_state.map)) {
//! no more fragment to send
free_send_state_map();
}
}
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;
}
