int8_t fetcher_cancel(sos_pid_t req_id, sos_shm_t key)
{
fetcher_state_t *tmp;
fetcher_state_t *prev;
if( fst == NULL ) return -EINVAL;
if( fst->map.key == key ) {
tmp = fst;
start_new_fetch();
ker_free( tmp );
/*
* Cancel sender as well
*/
if( (send_state.map != NULL) && (send_state.map->key == key)) {
free_send_state_map();
}
return SOS_OK;
}
prev = fst;
tmp = fst->next;
while(tmp != NULL) {
if( tmp->map.key == key ) {
prev->next = tmp->next;
ker_free( tmp );
return SOS_OK;
}
prev = tmp;
tmp = tmp->next;
}
return -EINVAL;
}
static int tr_send_data(Data *s, Message *msg)
{
void *hdr = ker_msg_take_data(42, msg);
int dup;
int my_id = s->fieldB;
if(hdr == 0) return -1;
if(s->fieldA == 0) {
if(my_id == 42) {
post_net(42,
42,
69,
msg->len,
msg->data,
0x04,
255);
// MEMORY LEAK HERE. The above call to post should be realeasing
// hdr rather than msg->data.
return 0;
} else {
ker_free(hdr);
return -1;
}
}
if(s->fieldA >= ((int*)hdr)[0] ) {
ker_free(hdr);
return -1;
}
if(my_id != 18) {
my_id += 4;
} else {
dup = 0;
}
if(dup == 0) {
post_net(42,
42,
69,
msg->len,
msg->data,
0x04,
s->fieldA);
return 0;
} else {
ker_free(hdr);
return -1;
}
}
/**
* @brief handle the process of creating senddone message
* @param msg_sent the Message just sent or delivered
* @param succ is the delivery successful?
* @param msg_owner the owner of the message
* NOTE the implementation will need to improve
*/
void msg_send_senddone(Message *msg_sent, bool succ, sos_pid_t msg_owner)
{
uint8_t flag;
if(flag_msg_reliable(msg_sent->flag) == 0) {
msg_dispose(msg_sent);
return;
}
/*
* Release the memory
*/
if(flag_msg_release(msg_sent->flag)){
ker_free(msg_sent->data);
msg_sent->flag &= ~(SOS_MSG_RELEASE);
msg_sent->data = NULL;
}
if(succ == false) {
flag = SOS_MSG_SEND_FAIL;
} else {
flag = 0;
}
if(post_long(msg_sent->sid, msg_owner, MSG_PKT_SENDDONE,
sizeof(Message), msg_sent, flag) < 0) {
msg_dispose(msg_sent);
}
}
/**
* @brief Pre-allocate timers for a module at load time
*/
int8_t timer_preallocate(sos_pid_t pid, uint8_t num_timers)
{
// We have already checked if num_timer > 0 and pid is not NULL_PID
uint8_t i, j;
sos_timer_t* tt[MAX_PRE_ALLOCATED_TIMERS];
//! We cannot allow a single module to pre allocate a lot of timers
if (num_timers > MAX_PRE_ALLOCATED_TIMERS)
return -EINVAL;
//! First try to safely allocate memory blocks for all the pre-allocated timers
for (i = 0; i < num_timers; i++){
tt[i] = (sos_timer_t*)malloc_longterm(sizeof(sos_timer_t), TIMER_PID);
if (tt[i] == NULL){
for (j = 0; j < i; j++){
ker_free(tt[j]);
}
return -ENOMEM;
}
}
//! If we get here then we have all the memory allocated
//! Now initialize all the data structures and just add them to the timer pool
for (i = 0; i < num_timers; i++){
timer_pre_alloc_block_init(tt[i], pid);
}
return SOS_OK;
}
void msg_send_senddone(Message *msg_sent, int succ, int msg_owner)
{
int flag;
if(msg_sent->flag == 0) {
ker_free(msg_sent);
return;
}
if(msg_sent->flag) {
ker_free(msg_sent->data);
msg_sent->flag = 0;
}
return;
}
void slab_gc( slab_t *slab, sos_pid_t pid )
{
slab_item_t *itr = slab->head;
slab_item_t *prev = NULL;
//
// Detect memory leak while marking the memory to malloc
//
while( itr != NULL ) {
if( itr->alloc != itr->gc_mark ) {
// leak!
DEBUG_GC("leak in slab %d %d\n", itr->alloc, itr->gc_mark);
led_red_toggle();
itr->alloc = itr->gc_mark;
if( itr->alloc == 0 && itr != slab->head ) {
prev->next = itr->next;
ker_free( itr );
itr = prev;
} else {
// mark it used to malloc
itr->gc_mark = 0;
ker_gc_mark( pid, itr );
}
} else {
itr->gc_mark = 0;
ker_gc_mark( pid, itr );
}
prev = itr;
itr = itr->next;
}
}
// 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;
}
static int8_t handle_version_data( Message *msg )
{
msg_version_data_t *pkt = (msg_version_data_t *) msg->data;
if( st.net_state & SEND_DATA ) {
/*
* We have data to send
*/
if( pkt->version == st.version_data->version) {
st.data_count ++;
if( st.data_count >= OVERHEARD_K ) {
st.net_state &= ~SEND_DATA;
ker_timer_stop( KER_DFT_LOADER_PID, DATA_TID );
}
return SOS_OK;
}
}
if( pkt->version > st.version_data->version ) {
restartInterval( 0 );
ker_free(st.version_data);
st.version_data = (msg_version_data_t*) ker_msg_take_data(KER_DFT_LOADER_PID, msg);
process_version_data( st.version_data, msg->saddr );
}
return SOS_OK;
}
/*************************************************************************
* the callback fnction for reading data from cc2420 *
*************************************************************************/
void _MacRecvCallBack(int16_t timestamp)
{
VMAC_PPDU ppdu;
vhal_data vd;
mac_to_vhal(&ppdu, &vd);
Radio_Disable_Interrupt(); //disable interrupt while reading data
if( !Radio_Recv_Pack(&vd) ) {
Radio_Enable_Interrupt(); //enable interrupt
return;
}
Radio_Enable_Interrupt(); //enable interrupt
vhal_to_mac(&vd, &ppdu);
// Andreas - filter node ID here, even before allocating any new memory
// if you're using sos/config/base you must comment this block out!
if (net_to_host(ppdu.mpdu.daddr) != NODE_ADDR && net_to_host(ppdu.mpdu.daddr) != BCAST_ADDRESS)
{
ker_free(vd.payload);
return;
}
Message *msg = msg_create();
if( msg == NULL ) {
ker_free(vd.payload);
return;
}
mac_to_sosmsg(&ppdu, msg);
// Andreas - start debug
#ifdef ENA_VMAC_UART_DEBUG
uint8_t *payload;
uint8_t msg_len;
msg_len=msg->len;
payload = msg->data;
//post_uart(msg->sid, msg->did, msg->type, msg_len, payload, SOS_MSG_RELEASE, msg->daddr);
// Swap daddr with saddr, because daddr is useless when debugging.
// Of course, if sossrv says "dest addr: 15" that actually means the message SENDER was node 15
post_uart(msg->sid, msg->did, msg->type, msg_len, payload, SOS_MSG_RELEASE, msg->saddr);
#endif
//if (msg->daddr == NODE_ADDR || msg->daddr == BCAST_ADDRESS)
handle_incoming_msg(msg, SOS_MSG_RADIO_IO);
}
/**
* @brief remove timers for a particular pid
*/
int8_t timer_remove_all(sos_pid_t pid){
list_link_t *link;
for(link = deltaq.l_next;
link != (&deltaq); link = link->l_next) {
sos_timer_t *h = (sos_timer_t*)link;
if(h->pid == pid) {
link = link->l_prev;
timer_remove_timer(h);
ker_free(h);
// break; Ram - Why are we breaking from this loop ?
}
}
for (link = timer_pool.l_next; link != (&timer_pool); link = link->l_next){
sos_timer_t *h = (sos_timer_t*)link;
if (h->pid == pid){
link = link->l_prev;
list_remove((list_link_t*)h);
ker_free(h);
}
}
for (link = prealloc_timer_pool.l_next; link != (&prealloc_timer_pool); link = link->l_next){
sos_timer_t *h = (sos_timer_t*)link;
if (h->pid == pid){
link = link->l_prev;
list_remove((list_link_t*)h);
ker_free(h);
}
}
for (link = periodic_pool.l_next; link != (&periodic_pool); link = link->l_next){
sos_timer_t *h = (sos_timer_t*)link;
if (h->pid == pid){
link = link->l_prev;
list_remove((list_link_t*)h);
ker_free(h);
}
}
return SOS_OK;
}
int post_net(int sid,
int mid,
int did,
int data_size,
void* data,
int flag,
int address) {
ker_free(data);
return flag;
}
/**
* @brief dispose message
* return message header back to message repostitary
*/
void msg_dispose(Message *m)
{
HAS_CRITICAL_SECTION;
if(flag_msg_release(m->flag)) {
ker_free(m->data);
}
ENTER_CRITICAL_SECTION();
ker_slab_free( &msg_slab, m );
LEAVE_CRITICAL_SECTION();
}
static int8_t do_register_module(mod_header_ptr h, sos_module_t *handle,
void *init, uint8_t init_size, uint8_t flag)
{
sos_pid_t pid;
uint8_t st_size;
int8_t ret;
// Disallow usage of NULL_PID
if (flag == SOS_CREATE_THREAD) {
pid = sched_get_pid_from_pool();
if (pid == NULL_PID) return -ENOMEM;
} else {
pid = sos_read_header_byte(h, offsetof(mod_header_t, mod_id));
/*
* Disallow the usage of thread ID
*/
if (pid > APP_MOD_MAX_PID) return -EINVAL;
}
// Read the state size and allocate a separate memory block for it
st_size = sos_read_header_byte(h, offsetof(mod_header_t, state_size));
//DEBUG("registering module pid %d with size %d\n", pid, st_size);
if (st_size){
//handle->handler_state = (uint8_t*)ker_malloc(st_size, pid);
handle->handler_state = (uint8_t*)malloc_longterm(st_size, KER_SCHED_PID);
// If there is no memory to store the state of the module
if (handle->handler_state == NULL){
return -ENOMEM;
}
} else {
handle->handler_state = NULL;
}
// Initialize the data structure
handle->header = h;
handle->pid = pid;
handle->flag = 0;
handle->next = NULL;
// add to the bin
ret = sched_register_module(handle, h, init, init_size);
if(ret != SOS_OK) {
ker_free(handle->handler_state); //! Free the memory block to hold module state
return ret;
}
return SOS_OK;
}
sos_pid_t ker_spawn_module(mod_header_ptr h, void *init, uint8_t init_size, uint8_t flag)
{
sos_module_t *handle;
if(h == 0) return NULL_PID;
// Allocate a memory block to hold the module list entry
//handle = (sos_module_t*)ker_malloc(sizeof(sos_module_t), KER_SCHED_PID);
handle = (sos_module_t*)malloc_longterm(sizeof(sos_module_t), KER_SCHED_PID);
if (handle == NULL) {
return NULL_PID;
}
if( do_register_module(h, handle, init, init_size, flag) != SOS_OK) {
ker_free(handle);
return NULL_PID;
}
return handle->pid;
}
/**
* @brief register new module
* NOTE: this function cannot be called in the interrupt handler
* That is, the function is not thread safe
* NOTE: h is stored in program memory, which can be different from RAM
* special access function is needed.
*/
int8_t ker_register_module(mod_header_ptr h)
{
sos_module_t *handle;
int8_t ret;
if(h == 0) return -EINVAL;
handle = (sos_module_t*)malloc_longterm(sizeof(sos_module_t), KER_SCHED_PID);
if (handle == NULL) {
return -ENOMEM;
}
ret = do_register_module(h, handle, NULL, 0, 0);
if(ret != SOS_OK) {
ker_free(handle);
}
ker_change_own(handle->handler_state, handle->pid);
return ret;
}
//! Free the first timer block beloning to pid in the timer_pool
int8_t ker_timer_release(sos_pid_t pid, uint8_t tid)
{
sos_timer_t* tt;
//! First stop the timer if it is running
ker_timer_stop(pid, tid);
//! Get the timer block from the pool
tt = alloc_from_timer_pool(pid, tid);
if (tt == NULL)
return -EINVAL;
//! Deep free of the timer
ker_free(tt);
return SOS_OK;
}
void ker_slab_free( slab_t *slab, void* mem )
{
slab_item_t *itr = slab->head;
slab_item_t *prev = NULL;
while( itr != NULL ) {
if( ((uint8_t*)mem) >= itr->mem && ((uint8_t*)mem) < (itr->mem + slab->num_items_per_pool * slab->item_size) ) {
uint8_t mask = 1 << ( ( ((uint8_t*)mem) - (itr->mem) ) / slab->item_size );
itr->alloc &= ~mask;
if( itr->alloc == 0 && itr != slab->head ) {
prev->next = itr->next;
ker_free( itr );
}
return;
}
prev = itr;
itr = itr->next;
}
ker_panic();
}
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 fetcher_handler(void *state, Message *msg)
{
switch (msg->type) {
case MSG_FETCHER_FRAGMENT:
{
fetcher_fragment_t *f;
f = (fetcher_fragment_t*)msg->data;
f->key = entohs( f->key );
f->frag_id = entohs(f->frag_id);
DEBUG_PID(KER_FETCHER_PID,"MSG_FETCHER_FRAGMENT:\n");
handle_overheard_fragment(msg);
if(fst == NULL) {
DEBUG_PID(KER_FETCHER_PID, "NO Request!!!\n");
return SOS_OK; //!< no request
}
//DEBUG_PID(KER_FETCHER_PID,"calling restart_request_timer()\n");
restart_request_timer();
fst->retx = 0;
//DEBUG_PID(KER_FETCHER_PID,"calling handle_data()\n");
return handle_data(msg);
}
case MSG_FETCHER_REQUEST:
{
fetcher_bitmap_t *bmap =
(fetcher_bitmap_t *) msg->data;
bmap->key = entohs( bmap->key );
//! received request from neighbors
DEBUG("handling request to %d from %d\n", msg->daddr, msg->saddr);
if(msg->daddr == ker_id()) {
return handle_request(msg);
}
if(fst == NULL) return SOS_OK; //!< no request
restart_request_timer();
fst->retx = 0;
return SOS_OK;
}
case MSG_TIMER_TIMEOUT:
{
MsgParam *params = (MsgParam*)(msg->data);
if(params->byte == FETCHER_REQUEST_TID) {
//DEBUG("request timeout\n");
if( no_mem_retry ) {
send_fetcher_done();
return SOS_OK;
}
handle_request_timeout();
} else if(params->byte == FETCHER_TRANSMIT_TID) {
//DEBUG("send fragment timeout\n");
if( send_state.num_msg_in_queue < FETCHER_MAX_MSG_IN_QUEUE ) {
send_fragment();
}
}
return SOS_OK;
}
case MSG_PKT_SENDDONE:
{
if( send_state.num_msg_in_queue > 0 ) {
send_state.num_msg_in_queue--;
}
return SOS_OK;
}
#ifdef SOS_HAS_EXFLASH
case MSG_EXFLASH_WRITEDONE:
{
ker_free(send_state.fragr);
send_state.fragr = NULL;
check_map_and_post();
return SOS_OK;
}
case MSG_EXFLASH_READDONE:
{
post_auto(KER_FETCHER_PID,
KER_FETCHER_PID,
MSG_FETCHER_FRAGMENT,
sizeof(fetcher_fragment_t),
send_state.frag,
SOS_MSG_RELEASE,
send_state.dest);
send_state.frag = NULL;
return SOS_OK;
}
#endif
case MSG_INIT:
{
send_state.map = NULL;
send_state.frag = NULL;
send_state.fragr = NULL;
send_state.num_msg_in_queue = 0;
ker_msg_change_rules(KER_FETCHER_PID, SOS_MSG_RULES_PROMISCUOUS);
ker_permanent_timer_init(&(send_state.timer), KER_FETCHER_PID, FETCHER_TRANSMIT_TID, TIMER_REPEAT);
ker_timer_init(KER_FETCHER_PID, FETCHER_REQUEST_TID, TIMER_ONE_SHOT);
return SOS_OK;
}
}
return -EINVAL;
}
static void free_send_state_map()
{
ker_free(send_state.map);
send_state.map = NULL;
ker_timer_stop(KER_FETCHER_PID, FETCHER_TRANSMIT_TID);
}
static void process_version_data( msg_version_data_t *v, uint16_t saddr )
{
uint8_t i;
for( i = 0; i < NUM_LOADER_PARAMS_ENTRIES + NUM_LOADER_MODULE_ENTRIES; i++ ) {
sos_cam_t key = ker_cam_key( KER_DFT_LOADER_PID, i);
loader_cam_t *cam;
uint8_t type;
uint8_t size;
uint8_t ver;
if( i < NUM_LOADER_PARAMS_ENTRIES) {
size = (v->pam_size[i]);
ver = (v->pam_ver[i]);
type = FETCHTYPE_DATA;
} else {
size = (v->mod_size[i - NUM_LOADER_PARAMS_ENTRIES]);
ver = (v->mod_ver[i - NUM_LOADER_PARAMS_ENTRIES]);
type = FETCHTYPE_MODULE;
}
cam = ker_cam_lookup( key );
if( cam == NULL && size == 0 ) {
// We cannot find entry and the size is zero
// skip this slot
continue;
}
if( cam == NULL ) {
// we need to add a new module
cam = (loader_cam_t*) ker_malloc(sizeof(loader_cam_t), KER_DFT_LOADER_PID);
if( cam == NULL) {
return;
}
if( ker_cam_add( key, cam ) != SOS_OK ) {
ker_free( cam );
return;
}
} else {
// we need to replace a module
if( cam->version == ver ) {
continue;
}
//! new version of module found...
if( cam->fetcher.status != FETCHING_DONE ) {
if( cam->fetcher.status == FETCHING_STARTED ) {
st.blocked = 0;
restartInterval( 0 );
}
fetcher_cancel( KER_DFT_LOADER_PID, key );
ker_codemem_free(cam->fetcher.cm);
} else /* if( cam->fetcher.status == FETCHING_DONE ) */ {
ker_codemem_free(cam->fetcher.cm);
}
if( size == 0 ) {
//! an rmmod case
ker_cam_remove( key );
ker_free( cam );
continue;
}
//! an insmod case with cam
}
if( request_new_module( key, cam, size, saddr, type) != SOS_OK ) {
ker_cam_remove( key );
ker_free( cam );
} else {
// another insmod case
cam->version = ver;
// only do one fetching
return;
}
}
}
/**
* Send MSG_I2C_READ_DONE
*/
void i2c_read_done(uint8_t *buff, uint8_t len, uint8_t status) {
uint8_t *bufPtr = NULL;
Message *msgPtr = NULL;
// this is a problem that should be handled
if (buff == NULL) {
return;
}
if ((i2c_sys.calling_mod_id != NULL_PID) && (status & I2C_SYS_ERROR_FLAG)) {
goto post_error_msg;
}
// the bus was reserved the read was of the length we requested
if ((i2c_sys.calling_mod_id != NULL_PID) && (len == i2c_sys.rxLen) &&
// the data was recieved in the correct mode
(((i2c_sys.state == I2C_SYS_MASTER_RX) && (I2C_SYS_MASTER_FLAG & status)) ||
((i2c_sys.state == I2C_SYS_SLAVE_RX) && !(I2C_SYS_MASTER_FLAG & status)))) {
// reserved read done will only be raw reads, wrap in message and send
post_long(
i2c_sys.calling_mod_id,
I2C_PID,
MSG_I2C_READ_DONE,
len,
buff,
SOS_MSG_RELEASE|SOS_MSG_HIGH_PRIORITY);
i2c_sys.state = (i2c_sys.flags & I2C_SYS_MASTER_FLAG)?I2C_SYS_MASTER_WAIT:I2C_SYS_SLAVE_WAIT;
return;
}
// there appers to be a bug in avr-gcc this a work around to make sure
// the cast to message works correctly
// the following code seems to cat the value 0x800008 independent of what
// buff actually ii2c_sys. this has no correlation to the data in buff or the
// value of the pointer the cast (along with many others that should) works
// in gdb but fail to execute correctly when compilied
/* bufPtr = &buff[HDLC_PROTOCOL_SIZE]; */
// DO NOT CHANGE THIS SECTION OF CODE
// start of section
bufPtr = buff+1;
// end of DO NOT CHANGE SECTION
// if it passes sanity checks give it to the scheduler
if (!(I2C_SYS_MASTER_FLAG & status) && (len >= SOS_MSG_HEADER_SIZE) && (buff[0] == HDLC_SOS_MSG)) {
if ((len >= (HDLC_PROTOCOL_SIZE + SOS_MSG_HEADER_SIZE + ((Message*)bufPtr)->len + SOS_MSG_CRC_SIZE)) &&
(len <= I2C_MAX_MSG_LEN)) {
// please do not edit the next line
bufPtr = buff;
// we have enough bytes for it to be a message, lets start the copy out
// XXX msgPtr = (Message*)ker_malloc(sizeof(Message), I2C_PID);
msgPtr = msg_create();
if (msgPtr !=NULL) {
uint8_t i=0;
uint16_t runningCRC=0, crc_in=0;
// extract the protocol field
for (i=0; i<HDLC_PROTOCOL_SIZE; i++) {
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
// extract the header
bufPtr = &buff[HDLC_PROTOCOL_SIZE];
for (i=0; i<SOS_MSG_HEADER_SIZE; i++) {
((uint8_t*)msgPtr)[i] = bufPtr[i];
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
// extract the data if it exists
if (msgPtr->len != 0) {
uint8_t *dataPtr;
dataPtr = ker_malloc(((Message*)msgPtr)->len, I2C_PID);
if (dataPtr != NULL) {
msgPtr->data = dataPtr;
bufPtr = &buff[HDLC_PROTOCOL_SIZE+SOS_MSG_HEADER_SIZE];
for (i=0; i<msgPtr->len; i++) {
msgPtr->data[i] = bufPtr[i];
runningCRC = crcByte(runningCRC, bufPtr[i]);
}
} else { // -ENOMEM
ker_free(msgPtr);
goto post_error_msg;
}
} else {
msgPtr->data = NULL;
}
// get the CRC and check it
bufPtr = &buff[HDLC_PROTOCOL_SIZE+SOS_MSG_HEADER_SIZE+msgPtr->len];
crc_in = bufPtr[0] | (bufPtr[1]<<8);
if (crc_in == runningCRC) {
// message passed all sanity checks including crc
LED_DBG(LED_YELLOW_TOGGLE);
if(msgPtr->data != NULL ) {
msgPtr->flag = SOS_MSG_RELEASE;
}
handle_incoming_msg(msgPtr, SOS_MSG_I2C_IO);
return;
} else { // clean up
ker_free(msgPtr->data);
ker_free(msgPtr);
}
}
}
}
// if we make it to here return error message
post_error_msg:
post_short(
i2c_sys.calling_mod_id,
I2C_PID,
MSG_ERROR,
READ_ERROR,
0,
SOS_MSG_HIGH_PRIORITY);
}
int8_t timer_micro_reboot(sos_module_t *handle){
sos_pid_t pid;
list_link_t *link;
mod_header_ptr hdr;
uint8_t num_timers_left;
pid = handle->pid;
hdr = handle->header;
num_timers_left = sos_read_header_byte(hdr, offsetof(mod_header_t, num_timers));
DEBUG("Timer: Pre-alloc timers requested %d \n", num_timers_left);
if (num_timers_left > 0){
for (link = prealloc_timer_pool.l_next;
link != (&prealloc_timer_pool);
link = link->l_next){
sos_timer_t *h = (sos_timer_t*)link;
if (h->pid == pid)
num_timers_left--; // Assert - This value should NEVER become negative
}
}
DEBUG("Timer: Timers left to pre-allocate %d \n", num_timers_left);
//! Stop all timers of pid
//! Move timer blocks to the pre-allocated pool
//! or free them
for (link = deltaq.l_next;
link != (&deltaq);
link = link->l_next){
sos_timer_t *h = (sos_timer_t*)link;
if (h->pid == pid){
link = link->l_prev;
timer_remove_timer(h);
if (num_timers_left > 0){
num_timers_left--;
timer_pre_alloc_block_init(h, pid);
DEBUG("Timer: Allocated active timer \n");
}
else
ker_free(h);
}
}
//! Remove all initialized timers
//! Move timer blocks to the pre-allocated pool
//! or free them
for (link = timer_pool.l_next;
link != (&timer_pool);
link = link->l_next){
sos_timer_t *h = (sos_timer_t*)link;
if (h->pid == pid){
link = link->l_prev;
list_remove((list_link_t*)h);
if (num_timers_left > 0){
num_timers_left--;
timer_pre_alloc_block_init(h, pid);
DEBUG("Timer: Allocated an initialzed but non-running timer\n");
}
else
ker_free(h);
}
}
//! If necessary, allocate memory for the pre-allocated timers
if (num_timers_left > 0){
DEBUG("Timer: Alloc more memory for timers\n");
if (timer_preallocate(pid, num_timers_left) != SOS_OK)
return -ENOMEM;
}
return SOS_OK;
}
static int8_t simple_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t*)state;
/** Switch to the correct message handler
*
* This module handles three types of messages:
*
* \li MSG_INIT to start a timer and allocated a buffer
*
* \li MSG_TIMER_TIMEOUT to periodicly write a value to the buffer. Note
* that we are expecting that the timer will have timer with ID equal to
* SIMPLE_TID
*
* \li MSG_FINAL to stop the timer and deallocate the buffer
*
*/
switch (msg->type){
case MSG_INIT:
{
// I'm alive!
LED_DBG(LED_GREEN_TOGGLE);
s->pid = msg->did;
s->state = (uint8_t *) ker_malloc(sizeof(uint8_t), s->pid);
*(s->state) = 0;
ker_timer_init(s->pid, SIMPLE_TID, TIMER_REPEAT);
ker_timer_start(s->pid, SIMPLE_TID, 1000);
break;
}
case MSG_FINAL:
{
// Bye bye!
ker_free(s->state);
ker_timer_stop(s->pid, SIMPLE_TID);
break;
}
case MSG_TIMER_TIMEOUT:
{
MsgParam* params = (MsgParam*)(msg->data);
if (params->byte == SIMPLE_TID)
{
// Blink the LED
if (*(s->state) == 1) {
LED_DBG(LED_YELLOW_OFF);
} else {
LED_DBG(LED_YELLOW_ON);
}
// Update the state
*(s->state) += 1;
if (*(s->state) > 1) {
*(s->state) = 0;
}
}
break;
}
default:
return -EINVAL;
}
return SOS_OK;
}
/**
* Send MSG_SPI_READ_DONE
*/
void spi_read_done(uint8_t *buff, uint8_t length, uint8_t status) {
if (s.flags & SPI_SYS_CS_HIGH_FLAG) {
spi_cs_release_high(s.addr);
} else {
spi_cs_release_low(s.addr);
}
if((NULL == buff) && (NULL_PID != s.calling_mod_id)) {
post_short(
s.calling_mod_id,
SPI_PID,
MSG_ERROR,
SPI_READ_ERROR,
0,
SOS_MSG_HIGH_PRIORITY);
return;
}
if (NULL == buff) {
return;
}
if (s.len == length) {
s.cnt--;
if (s.cnt > 0) {
s.bufPtr += s.len;
s.state = SPI_SYS_DMA_WAIT;
} else {
// read done send response
if (s.flags & SPI_SYS_READ_DONE_CB_FLAG) {
s.read_done_cb(s.usrBuf, length, s.cnt, status);
} else {
if (s.flags & SPI_SYS_SHARED_MEM_FLAG) {
post_long(
s.calling_mod_id,
SPI_PID,
MSG_SPI_READ_DONE,
length, // read length
s.usrBuf, // return the buffer pointer
SOS_MSG_HIGH_PRIORITY);
} else {
// dont trust the user to free the memory to message only if not shared
post_long(
s.calling_mod_id,
SPI_PID,
MSG_SPI_READ_DONE,
length, // byte length
s.usrBuf,
SOS_MSG_RELEASE|SOS_MSG_HIGH_PRIORITY);
}
}
/*if (s.flags & SPI_SYS_SHARED_MEM_FLAG) {
ker_free(s.usrBuf);
}*/
s.usrBuf = NULL;
s.bufPtr = NULL;
}
s.state = SPI_SYS_WAIT;
} else { // short message
post_short(
s.calling_mod_id,
SPI_PID,
MSG_ERROR,
SPI_READ_ERROR,
0,
SOS_MSG_HIGH_PRIORITY);
if (!(s.flags & SPI_SYS_SHARED_MEM_FLAG)) {
ker_free(s.usrBuf);
}
spi_system_init();
}
if (!(s.flags & SPI_SYS_LOCK_BUS_FLAG)) {
ker_spi_release_bus(s.calling_mod_id);
}
}
/**
* @brief ISR for reception
* This is the writer of rx_queue.
*/
uart_recv_interrupt() {
#ifdef SOS_USE_PREEMPTION
HAS_PREEMPTION_SECTION;
DISABLE_PREEMPTION();
#endif
uint8_t err;
uint8_t byte_in;
static uint16_t crc_in;
static uint8_t saved_state;
SOS_MEASUREMENT_IDLE_END()
LED_DBG(LED_YELLOW_TOGGLE);
//! NOTE that the order has to be this in AVR
err = uart_checkError();
byte_in = uart_getByte();
//DEBUG("uart_recv_interrupt... %d %d %d %d %d\n", byte_in, err,
// state[RX].state, state[RX].msg_state, state[RX].hdlc_state);
switch (state[RX].state) {
case UART_IDLE:
if ((err != 0) || (byte_in != HDLC_FLAG)) {
break;
}
state[RX].state = UART_HDLC_START;
break;
case UART_HDLC_START:
case UART_PROTOCOL:
if (err != 0) {
uart_reset_recv();
break;
}
switch (byte_in) {
//! ignore repeated start symbols
case HDLC_FLAG:
state[RX].state = UART_HDLC_START;
break;
case HDLC_SOS_MSG:
if(state[RX].msgHdr == NULL) {
state[RX].msgHdr = msg_create();
} else {
if((state[RX].msgHdr->data != NULL) &&
(flag_msg_release(state[RX].msgHdr->flag))){
ker_free(state[RX].msgHdr->data);
state[RX].msgHdr->flag &= ~SOS_MSG_RELEASE;
}
}
if(state[RX].msgHdr != NULL) {
state[RX].msg_state = SOS_MSG_RX_HDR;
state[RX].crc = crcByte(0, byte_in);
state[RX].flags |= UART_SOS_MSG_FLAG;
state[RX].idx = 0;
state[RX].state = UART_DATA;
state[RX].hdlc_state = HDLC_DATA;
} else {
// need to generate no mem error
uart_reset_recv();
}
break;
case HDLC_RAW:
if ((state[RX].buff = ker_malloc(UART_MAX_MSG_LEN, UART_PID)) != NULL) {
state[RX].msg_state = SOS_MSG_RX_RAW;
if (state[RX].flags & UART_CRC_FLAG) {
state[RX].crc = crcByte(0, byte_in);
}
state[RX].state = UART_DATA;
state[RX].hdlc_state = HDLC_DATA;
} else {
uart_reset_recv();
}
state[RX].idx = 0;
break;
default:
uart_reset_recv();
break;
}
break;
case UART_DATA:
if (err != 0) {
uart_reset_recv();
break;
}
// recieve an escape byte, wait for next byte
if (byte_in == HDLC_CTR_ESC) {
saved_state = state[RX].hdlc_state;
state[RX].hdlc_state = HDLC_ESCAPE;
break;
}
if (byte_in == HDLC_FLAG) { // got an end of message symbol
/*
if (state[RX].msg_state == SOS_MSG_RX_RAW) {
// end of raw recieve
// should bundle and send off
// trash for now
state[RX].hdlc_state = HDLC_IDLE;
state[RX].state = UART_IDLE;
state[RX].flags |= UART_DATA_RDY_FLAG;
uart_read_done(state[RX].idx, 0);
} else {
// got an end of message symbol early
*/
uart_reset_recv();
//}
break;
}
if (state[RX].hdlc_state == HDLC_ESCAPE) {
byte_in ^= 0x20;
state[RX].hdlc_state = saved_state;
}
switch (state[RX].msg_state) {
case SOS_MSG_RX_HDR:
if (byte_in == HDLC_FLAG) { // got an end of message symbol
uart_reset_recv();
break;
}
uint8_t *tmpPtr = (uint8_t*)(state[RX].msgHdr);
tmpPtr[state[RX].idx++] = byte_in;
state[RX].crc = crcByte(state[RX].crc, byte_in);
if (state[RX].idx == SOS_MSG_HEADER_SIZE) {
// if (state[RX].msgLen != state[RX].msgHdr->len) ????????
state[RX].msgLen = state[RX].msgHdr->len;
if (state[RX].msgLen < UART_MAX_MSG_LEN) {
if (state[RX].msgLen != 0) {
state[RX].buff = (uint8_t*)ker_malloc(state[RX].msgLen, UART_PID);
if (state[RX].buff != NULL) {
state[RX].msgHdr->data = state[RX].buff;
state[RX].msgHdr->flag = SOS_MSG_RELEASE;
state[RX].msg_state = SOS_MSG_RX_DATA;
state[RX].idx = 0;
} else {
uart_reset_recv();
}
} else { // 0 length packet go straight to crc
state[RX].msgHdr->flag &= ~SOS_MSG_RELEASE;
state[RX].msgHdr->data = NULL;
state[RX].msg_state = SOS_MSG_RX_CRC_LOW;
}
} else { // invalid msg length
uart_reset_recv();
}
}
break;
case SOS_MSG_RX_RAW:
case SOS_MSG_RX_DATA:
if (err != 0) {
uart_reset_recv();
return;
}
state[RX].buff[state[RX].idx++] = byte_in;
if (state[RX].flags & UART_CRC_FLAG) {
state[RX].crc = crcByte(state[RX].crc, byte_in);
}
if (state[RX].idx == state[RX].msgLen) {
if (state[RX].flags & UART_SOS_MSG_FLAG) {
state[RX].hdlc_state = HDLC_CRC;
state[RX].msg_state = SOS_MSG_RX_CRC_LOW;
} else {
// rx buffer overflow
uart_reset_recv();
}
}
break;
case SOS_MSG_RX_CRC_LOW:
crc_in = byte_in;
state[RX].msg_state = SOS_MSG_RX_CRC_HIGH;
break;
case SOS_MSG_RX_CRC_HIGH:
crc_in |= ((uint16_t)(byte_in) << 8);
state[RX].hdlc_state = HDLC_PADDING;
state[RX].msg_state = SOS_MSG_RX_END;
state[RX].state = UART_HDLC_STOP;
break;
case SOS_MSG_RX_END: // should never get here
default:
uart_reset_recv();
break;
}
break;
case UART_HDLC_STOP:
if (byte_in != HDLC_FLAG) {
// silently drop until hdlc stop symbol
break;
} else { // sos msg rx done
state[RX].hdlc_state = HDLC_IDLE;
if(crc_in == state[RX].crc) {
#ifndef NO_SOS_UART_MGR
set_uart_address(entohs(state[RX].msgHdr->saddr));
#endif
if(state[RX].msgHdr->type == MSG_TIMESTAMP){
uint32_t timestp = ker_systime32();
memcpy(((uint8_t*)(state[RX].msgHdr->data) + sizeof(uint32_t)),(uint8_t*)(×tp),sizeof(uint32_t));
}
handle_incoming_msg(state[RX].msgHdr, SOS_MSG_UART_IO);
state[RX].msgHdr = NULL;
} else {
msg_dispose(state[RX].msgHdr);
state[RX].msgHdr = NULL;
}
state[RX].state = UART_IDLE;
state[RX].msg_state = SOS_MSG_NO_STATE;
state[RX].hdlc_state = HDLC_IDLE;
//uart_reset_recv();
//state[RX].msg_state = SOS_MSG_NO_STATE;
//state[RX].state = UART_HDLC_START;
}
break;
// XXX fall through
default:
uart_reset_recv();
break;
} // state[RX].state
#ifdef SOS_USE_PREEMPTION
// enable interrupts because
// enabling preemption can cause one to occur
ENABLE_GLOBAL_INTERRUPTS();
// enable preemption
ENABLE_PREEMPTION(NULL);
#endif
}
int8_t i2c_initTransceiver(uint8_t ownAddress, uint8_t flags) {
HAS_CRITICAL_SECTION;
// Set own TWI slave address. Accept TWI General Calls.
i2c.ownAddr = ((ownAddress<<1)&0xFE);
if (ownAddress != 0x7f) {
// 1111xxx is reserved addres space so 0x7f is an
// invalid address and it is safe to use it as a flag
// we will also enable the general call recognition bit
TWAR = i2c.ownAddr|(1<<TWGCE);
} else {
if (!(flags&I2C_MASTER_FLAG)){
// can not give a slave an invalid address
return -EINVAL;
}
}
// get flag settings from the upper layer
i2c.flags = I2C_SYS_SHARED_FLAGS_MSK & flags;
// do some clean up
i2c.msgLen = 0;
i2c.txPending = 0;
i2c.idx = 0;
// free all allocated buffers
if (i2c.msgBuf != NULL) {
i2c.dataBuf = NULL;
i2c.msgBuf = NULL;
}
i2c.msg_state = SOS_MSG_NO_STATE;
/**
* \bug I2C system may want to NULL the i2c.dataBuf after i2c_send_done,
* i2c_read_done, and any error. We could then verify that i2c.dataBuf is
* null in i2c_initTranceiver and call ker_panic if it is not null. Same
* goes for the rxDataBuf
*/
// Roy: This results in a hard to track bug. If the user frees I2C data
// after the I2C sends a MSG_I2C_SEND_DONE, this will free it a second time.
// Kernel messaging avoids this by explitily setting i2c.dataBuff to null.
// Well, maybe. Maybe not... Regardless, this is bad!
//
// if (i2c.dataBuf != NULL) {
// ker_free(i2c.dataBuf);
//}
// The problem described abave is NOT a problem with the i2c.rxDataBuf. A
// call to i2c_read_done creates a deep copy of the rxDataBuf that is
// SOS_MSG_RELEASE'ed. Thus, the rxDataBuff can hang around. A down side
// to the current implementation is that the rxDataBuf is not released with
// when the buffer is released. We do not leak this data, but it is not
// made availible to the system.
//
// pre allocate recieve buffer
if (i2c.rxDataBuf != NULL) {
ker_free(i2c.rxDataBuf);
}
i2c.rxDataBuf = ker_malloc(I2C_MAX_MSG_LEN, I2C_PID);
if (i2c.rxDataBuf == NULL) {
return -ENOMEM;
}
ENTER_CRITICAL_SECTION();
if (i2c.flags & I2C_MASTER_FLAG) {
i2c.state = I2C_MASTER_IDLE;
} else {
i2c.state = I2C_SLAVE_IDLE;
}
// enable TWI interrupt and ack
i2c_setCtrlReg((1<<TWINT)|(1<<TWEA)|(1<<TWEN)|(1<<TWIE));
LEAVE_CRITICAL_SECTION();
return SOS_OK;
}
/**
* @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;
}
