static int8_t handle_timeout( Message *msg )
{
MsgParam* params = ( MsgParam * )( msg->data );
switch( params->byte ) {
case TOU_TID:
{
if( st.net_state != MAINTAIN ) {
restartInterval( 0 );
} else {
restartInterval( st.tou + 1 );
}
break;
}
case TRAN_TID:
{
/*
* Transmit Version number
*/
if( st.blocked == 0 ) {
version_t *v;
v = ker_malloc( sizeof(version_t), KER_DFT_LOADER_PID );
if( v == NULL ) return -ENOMEM;
*v = version_hton(st.version_data->version);
post_link(KER_DFT_LOADER_PID, KER_DFT_LOADER_PID,
MSG_VERSION_ADV, sizeof(version_t), v,
SOS_MSG_RELEASE | SOS_MSG_ALL_LINK_IO,
BCAST_ADDRESS);
}
break;
}
case DATA_TID:
{
msg_version_data_t *d;
d = (msg_version_data_t *) ker_malloc(
sizeof( msg_version_data_t ),
KER_DFT_LOADER_PID);
if( d != NULL ) {
memcpy(d, st.version_data, sizeof( msg_version_data_t ));
d->version = version_hton( st.version_data->version );
post_link(KER_DFT_LOADER_PID, KER_DFT_LOADER_PID,
MSG_VERSION_DATA,
sizeof( msg_version_data_t ),
d,
SOS_MSG_RELEASE | SOS_MSG_ALL_LINK_IO,
BCAST_ADDRESS);
}
st.net_state &= ~SEND_DATA;
break;
}
}
return SOS_OK;
}
graph_t* __attribute__((sos_claim)) make_graph_return(int size) {
point_t *points;
edge_t *edges;
graph_t *g;
points = ker_malloc(sizeof(struct point) * size, 1);
edges = ker_malloc(sizeof(struct edge) * size, 1);
g = ker_malloc(sizeof(struct graph), 1);
g->points = points;
g->edges = edges;
return g;
}
graph_t* __attribute__((sos_claim)) make_graph_return_bad_a(int size) {
point_t *points;
edge_t *edges;
graph_t *g;
points = ker_malloc(sizeof(struct point) * size, 1);
edges = ker_malloc(sizeof(struct edge) * size, 1);
// Note that memory for graph g is not allocated
g->points = points;
g->edges = edges;
return g;
}
int8_t uart_startTransceiverRx( uint8_t rx_len, uint8_t flags) {
if (state[RX].state != UART_IDLE) {
return -EBUSY;
}
state[RX].flags = UART_SYS_SHARED_FLAGS_MSK & flags; // get shared flags
if (flags & UART_SOS_MSG_FLAG) {
state[RX].msgHdr = msg_create();
if (state[RX].msgHdr == NULL) {
return -ENOMEM;
}
} else {
state[RX].buff = ker_malloc(rx_len, UART_PID);
if (state[RX].buff == NULL) {
return -ENOMEM;
}
}
state[RX].msgLen = rx_len; // expected rx_len
state[RX].idx = 0;
state[RX].crc = 0;
state[RX].state = UART_HDLC_START;
state[RX].hdlc_state = HDLC_START;
uart_enable_rx();
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;
}
int8_t ker_slab_init( sos_pid_t pid, slab_t *slab, uint8_t item_size, uint8_t items_per_pool, uint8_t flag )
{
uint8_t i;
if( items_per_pool > 8 ) {
return -EINVAL;
}
//
// build empty vector
//
slab->head = NULL;
slab->num_items_per_pool = items_per_pool;
slab->empty_vector = 0;
slab->item_size = item_size;
slab->flag = flag;
for( i = 0; i < items_per_pool; i++ ) {
slab->empty_vector <<= 1;
slab->empty_vector |= 0x01;
}
if( slab->flag & SLAB_LONGTERM ) {
slab->head = malloc_longterm( sizeof( slab_item_t ) + items_per_pool * item_size, pid );
} else {
slab->head = ker_malloc( sizeof( slab_item_t ) + items_per_pool * item_size, pid );
}
if( slab->head == NULL ) {
return -ENOMEM;
}
slab->head->next = NULL;
slab->head->alloc = 0;
slab->head->gc_mark = 0;
return SOS_OK;
}
static inline void handle_request_timeout()
{
if(fst == NULL) {
return;
}
//sos_assert(fst != NULL);
//DEBUG("handle request timeout, retx = %d\n", fst->retx);
fst->retx++;
if(fst->retx <= FETCHER_REQUEST_MAX_RETX) {
fetcher_bitmap_t *m;
uint8_t size = sizeof(fetcher_bitmap_t) + fst->map.bitmap_size;
DEBUG_PID(KER_FETCHER_PID,"send request to %d\n", fst->src_addr);
print_bitmap(&fst->map);
m = (fetcher_bitmap_t *) ker_malloc( size, KER_FETCHER_PID);
if( m != NULL ) {
memcpy(m, &(fst->map), size);
m->key = ehtons( m->key );
post_auto(KER_FETCHER_PID,
KER_FETCHER_PID,
MSG_FETCHER_REQUEST,
size,
m,
SOS_MSG_RELEASE,
fst->src_addr);
}
restart_request_timer();
} else {
DEBUG_PID(KER_FETCHER_PID, "request failed!!!\n");
//codemem_close(&fst->cm, false);
send_fetcher_done();
}
}
static int8_t handle_init()
{
ker_permanent_timer_init((&st.tou_timer), KER_DFT_LOADER_PID,
TOU_TID, TIMER_ONE_SHOT);
ker_permanent_timer_init((&st.tran_timer), KER_DFT_LOADER_PID,
TRAN_TID, TIMER_ONE_SHOT);
ker_permanent_timer_init((&st.data_timer), KER_DFT_LOADER_PID,
DATA_TID, TIMER_ONE_SHOT);
st.net_state = MAINTAIN;
st.tran_count = 0;
st.data_count = 0;
st.blocked = 0;
st.recent_neighbor = 0;
st.version_data = ker_malloc(sizeof(msg_version_data_t), KER_DFT_LOADER_PID);
if(st.version_data == NULL) return -ENOMEM;
memset((uint8_t *) st.version_data, 0, sizeof(msg_version_data_t));
/*
* Use default version to be 1 so that loader_pc can probe loaded data
*/
st.version_data->version = 1;
restartInterval( 0 );
return SOS_OK;
}
int8_t fetcher_request(sos_pid_t req_id, sos_shm_t key, uint16_t size, uint16_t src)
{
uint8_t bitmap_size; //! size of the bitmap in bytes
uint16_t num_fragments;
uint8_t i;
fetcher_state_t *f;
fetcher_cam_t *cam;
cam = (fetcher_cam_t *) ker_shm_get( KER_FETCHER_PID, key);
if( cam == NULL ) return -EINVAL;
//if(fst != NULL) return -EBUSY;
DEBUG_PID(KER_FETCHER_PID, "fetcher_request, req_id = %d, size = %d, src = %d\n", req_id, size, src);
num_fragments = ((size + (FETCHER_FRAGMENT_SIZE - 1))/ FETCHER_FRAGMENT_SIZE);
bitmap_size = (uint8_t)((num_fragments + 7)/ 8);
//DEBUG("size = %d\n", sizeof(fetcher_state_t) + bitmap_size);
f = ker_malloc(sizeof(fetcher_state_t) + bitmap_size, KER_FETCHER_PID);
if(f == NULL) {
return -ENOMEM;
}
//DEBUG("num_fragments = %d, bitmap_zie = %d\n", num_fragments, bitmap_size);
f->requester = req_id;
f->map.key = key;
f->map.bitmap_size = bitmap_size;
f->src_addr = src;
f->num_funcs = 0;
f->next = NULL;
f->cm = cam->cm;
for(i = 0; i < bitmap_size; i++) {
f->map.bitmap[i] = 0xff;
}
if((num_fragments) % 8) {
f->map.bitmap[bitmap_size - 1] =
(1 << (num_fragments % 8)) - 1;
}
print_bitmap(&f->map);
//! backoff first!!!
f->retx = 0;
if(fst != NULL) {
fetcher_state_t *tmp = fst;
cam->status = FETCHING_QUEUED;
while(tmp->next != NULL) { tmp = tmp->next; }
tmp->next = f;
return SOS_OK;
}
cam->status = FETCHING_STARTED;
fst = f;
//! setup timer
ker_timer_start(KER_FETCHER_PID,
FETCHER_REQUEST_TID,
FETCHER_REQUEST_BACKOFF_SLOT * ((ker_rand() % FETCHER_REQUEST_MAX_SLOT) + 1));
//DEBUG("request ret = %d\n", ret);
return SOS_OK;
}
/**
* Read data from the spi
*/
int8_t ker_spi_read_data(
uint8_t *sharedBuf,
uint8_t rx_len,
uint8_t rx_cnt,
uint8_t calling_id) {
HAS_CRITICAL_SECTION;
if (s.state == SPI_SYS_IDLE) { // not reserved
return -EINVAL;
}
if ((s.calling_mod_id != calling_id) ||
((s.state != SPI_SYS_WAIT) && (s.state != SPI_SYS_DMA_WAIT) && (s.state != SPI_SYS_RX_WAIT))) {
return -EBUSY;
}
// get a handle to users buffer
if (rx_len >= MAX_SPI_READ_LEN) {
return -EINVAL;
} else {
s.len = rx_len;
}
// need to assert CS pin
if (s.flags & SPI_SYS_CS_HIGH_FLAG) {
spi_cs_high(s.addr);
} else {
spi_cs_low(s.addr);
}
// only get/malloc a buffer if we are not currently in a DMA sequence
if ((s.flags & SPI_SYS_SHARED_MEM_FLAG)) {
if (NULL == sharedBuf) {
return -EINVAL;
} else {
s.cnt = rx_cnt;
s.bufPtr = sharedBuf;
if (!(s.state == SPI_SYS_DMA_WAIT)) {
s.usrBuf = sharedBuf;
}
}
} else {
// ignore value of sharedBuf
if (NULL == (s.bufPtr = s.usrBuf = ker_malloc(s.len, SPI_PID))) {
return -ENOMEM;
} else {
s.cnt = 1;
}
}
ENTER_CRITICAL_SECTION();
s.state = SPI_SYS_RX;
LEAVE_CRITICAL_SECTION();
return spi_masterRxData(s.usrBuf, s.len, s.flags);
}
int main() {
ads7828_state_t *s __attribute__((__sos_store__));
unsigned int __cil_tmp45 ;
unsigned int __cil_tmp46 ;
__cil_tmp45 = (unsigned int )s;
__cil_tmp46 = __cil_tmp45 + 4;
(*((uint8_t **)__cil_tmp46)) = (uint8_t *)ker_malloc(1U, (unsigned char)129);
return 0;
}
int8_t i2c_startTransceiverRx(uint8_t addr,
uint8_t rx_msg_len,
uint8_t flags) {
HAS_CRITICAL_SECTION;
uint8_t alt_state = (i2c.flags & I2C_MASTER_FLAG)?I2C_MASTER_IDLE:I2C_SLAVE_IDLE;
while (!((i2c.state == I2C_IDLE) || (i2c.state == alt_state))) {
return -EBUSY;
}
if (flags & I2C_SOS_MSG_FLAG) {
return -EINVAL;
}
i2c.flags = I2C_SYS_SHARED_FLAGS_MSK & flags;
if (rx_msg_len >= I2C_MAX_MSG_LEN) {
return -ENOMEM;
}
/**
* \bug Um, maybe this check is not such a good idea. i2c.dataBuf needs to
* be null. This should be stronger and take the form of an assert such as:
*
* assert(i2c.dataBuf == NULL);
*
* But no asserts for us :-(
*/
//if (i2c.dataBuf == NULL) {
if ((i2c.dataBuf = ker_malloc(rx_msg_len, I2C_PID)) == NULL) {
return -ENOMEM;
}
//}
i2c.msgLen = rx_msg_len; i2c.idx = 0;
ENTER_CRITICAL_SECTION();
i2c.addr = (addr<<1); // shift destination address once (and only once!)
i2c.msg_state = SOS_MSG_WAIT;
saveState(i2c.state);
if (i2c.flags & I2C_MASTER_FLAG) {
i2c.state = I2C_MASTER_ARB;
i2c_setCtrlReg((1<<TWINT)|(1<<TWSTA)|(1<<TWEN)|(1<<TWIE));
} else { // else sit and wait
i2c.state = I2C_SLAVE_WAIT;
i2c_setCtrlReg((1<<TWEA)|(1<<TWEN)|(1<<TWIE));
}
i2c.msg_state = SOS_MSG_RX_START;
LEAVE_CRITICAL_SECTION();
return SOS_OK;
}
char ker_cam_add(int key, void *value)
{
int *cam;
cam = ker_malloc(sizeof(int) * 42, 69);
if( cam == 0 ) { return -1; }
cam[0] = 1;
cam[2] = key;
cam[3] = value;
cam_bin[key] = cam;
return 0;
}
void* ker_slab_alloc( slab_t *slab, sos_pid_t pid )
{
slab_item_t *itr = slab->head;
slab_item_t *prev = slab->head;
while( itr != NULL ) {
DEBUG(" itr->alloc = %x\n", itr->alloc);
if( itr->alloc != slab->empty_vector ) {
break;
}
prev = itr;
itr = itr->next;
}
if( itr == NULL ) {
//
// The pool is exhausted, create a new one
//
DEBUG("pool exhausted\n");
if( slab->flag & SLAB_LONGTERM ) {
prev->next = malloc_longterm( sizeof( slab_item_t ) + slab->num_items_per_pool * slab->item_size, pid );
} else {
prev->next = ker_malloc( sizeof( slab_item_t ) + slab->num_items_per_pool * slab->item_size, pid );
}
if( prev->next == NULL ) {
DEBUG("alloc NULL\n");
return NULL;
}
itr = prev->next;
itr->next = NULL;
itr->alloc = 0x01;
itr->gc_mark = 0;
return itr->mem;
} else {
uint8_t i;
uint8_t mask = 0x01;
DEBUG("find free slot in pool\n");
for( i = 0; i < slab->num_items_per_pool; i++, mask<<=1 ) {
if( (itr->alloc & mask) == 0 ) {
itr->alloc |= mask;
return itr->mem + (i * slab->item_size);
}
}
}
return NULL;
}
/**
* @brief Server receiver side
*/
static void server_recv_thread(int fd)
{
uint8_t d[1024];
int cnt;
//sched_yield();
while( 1 ) {
cnt = read(fd, d, SOS_MSG_HEADER_SIZE);
if (cnt == 0) { // The socket is closed!
interrupt_remove_read_fd(fd);
close(fd);
break;
}
if(cnt != SOS_MSG_HEADER_SIZE)
break;
Message *buf = (Message*)d;
Message *m = msg_create();
if (m == NULL)
break;
uint8_t *data = ker_malloc(buf->len,UART_PID);
if (data == NULL) {
msg_dispose(m);
break;
}
memcpy(m,buf,SOS_MSG_HEADER_SIZE);
m->daddr = entohs(m->daddr);
m->saddr = entohs(m->saddr);
m->data = data;
m->flag = SOS_MSG_RELEASE;
while ((cnt = read(fd, m->data, m->len)) < 0);
if (cnt != m->len) {
msg_dispose(m);
break;
}
handle_incoming_msg(m, SOS_MSG_UART_IO);
}
}
uint8_t *ker_msg_take_data(sos_pid_t pid, Message *msg_in)
{
uint8_t *ret;
Message *msg; //!< message we will be taking data out
if(msg_in->type == MSG_PKT_SENDDONE) {
msg = (Message*) (msg_in->data);
} else {
msg = msg_in;
}
if(flag_msg_release(msg->flag)) {
ker_change_own((void*)msg->data, pid);
ret = msg->data;
msg->len = 0;
msg->data = NULL;
msg->flag &= ~(SOS_MSG_RELEASE);
return ret;
} else {
ret = (uint8_t*)ker_malloc(msg->len, pid);
if(ret == NULL) return NULL;
memcpy(ret, msg->data, msg->len);
return ret;
}
}
static void start_experiment(uint16_t size)
{
sos_cam_t key;
loader_cam_t *cam;
codemem_t cm;
uint8_t buf[2];
buf[0] = 128;
buf[1] = 1;
DEBUG_PID(KER_DFT_LOADER_PID, "start experiment: size = %d\n", size);
cm = ker_codemem_alloc( size, CODEMEM_TYPE_EXECUTABLE);
ker_codemem_write(cm, KER_DFT_LOADER_PID, buf, 2, 0);
cam = ker_malloc(sizeof(loader_cam_t), KER_DFT_LOADER_PID);
st.version_data->pam_ver[0]++;
st.version_data->pam_size[0] = (size + (LOADER_SIZE_MULTIPLIER - 1)) / LOADER_SIZE_MULTIPLIER;
st.version_data->version ++;
key = ker_cam_key( KER_DFT_LOADER_PID, 0 );
cam->fetcher.fetchtype = FETCHTYPE_DATA;
cam->fetcher.cm = cm;
ker_cam_add(key, cam);
restartInterval(0);
}
//-------------------------------------------------------
// MESSAGE HANDLER
//-------------------------------------------------------
static int8_t
matrix_arithmetic_msg_handler (void *state, Message * msg)
{
// matrix_arithmetic_t *s = (matrix_arithmetic_t*)state;
switch (msg->type)
{
case MSG_INIT:
{
break;
}
case MSG_FINAL:
{
break;
}
// int8_t add (const CYCLOPS_Matrix * A, const CYCLOPS_Matrix * B, CYCLOPS_Matrix * C)
case ADD:
{
matrix_math_t *m = (matrix_math_t *) (msg->data);
CYCLOPS_Matrix *A = (CYCLOPS_Matrix *) (m->ImgPtrA);
CYCLOPS_Matrix *B = (CYCLOPS_Matrix *) (m->ImgPtrB);
CYCLOPS_Matrix *C =
(CYCLOPS_Matrix *) ker_malloc (sizeof (CYCLOPS_Matrix),
MATRIX_ARITHMETICS_PID);
if (NULL == C)
{
return -ENOMEM;
}
if (add (A, B, C) != SOS_OK)
{
return -EINVAL;
}
//PUT_PORT();
//post_long
break;
}
// int8_t Sub (const CYCLOPS_Matrix * A, const CYCLOPS_Matrix * B, CYCLOPS_Matrix * C)
case SUB:
{
matrix_math_t *m = (matrix_math_t *) (msg->data);
CYCLOPS_Matrix *A = (CYCLOPS_Matrix *) (m->ImgPtrA);
CYCLOPS_Matrix *B = (CYCLOPS_Matrix *) (m->ImgPtrB);
CYCLOPS_Matrix *C =
(CYCLOPS_Matrix *) ker_malloc (sizeof (CYCLOPS_Matrix),
MATRIX_ARITHMETICS_PID);
if (NULL == C)
{
return -ENOMEM;
}
if (Sub (A, B, C) != SOS_OK)
{
return -EINVAL;
}
//PUT_PORT();
//post_long
break;
}
// int8_t abssub (const CYCLOPS_Matrix * A, const CYCLOPS_Matrix * B, CYCLOPS_Matrix * C
case ABS_SUB:
{
matrix_math_t *m = (matrix_math_t *) (msg->data);
CYCLOPS_Matrix *A = (CYCLOPS_Matrix *) (m->ImgPtrA);
CYCLOPS_Matrix *B = (CYCLOPS_Matrix *) (m->ImgPtrB);
CYCLOPS_Matrix *C =
(CYCLOPS_Matrix *) ker_malloc (sizeof (CYCLOPS_Matrix),
MATRIX_ARITHMETICS_PID);
if (NULL == C)
{
return -ENOMEM;
}
if (abssub (A, B, C) != SOS_OK)
{
return -EINVAL;
}
// Kevin - resulting Matrix will be put unto updateBackground, and abssub => 2 ports
//PUT_PORT();
//PUT_PORT();
// Kevin - if these are all sos modules, shouldnt is use self state s->M ???
// post_long(MATRIX_ARITHMETICS_PID, MATRIX_IMAGE_PID, ABS_SUB, sizeof(CYCLOPS_Matrix), M, SOS_MSG_RELEASE);
// post_long(MATRIX_ARITHMETICS_PID, MATRIX_IMAGE_PID, ABS_SUB, sizeof(CYCLOPS_Matrix), M, SOS_MSG_RELEASE);
break;
}
// int8_t getRow (const CYCLOPS_Matrix * A, CYCLOPS_Matrix * res, uint16_t row)
case GET_ROW:
{
matrix_location_t *m = (matrix_location_t *) (msg->data);
CYCLOPS_Matrix *A = (CYCLOPS_Matrix *) (m->matPtr);
uint16_t row = (m->row);
CYCLOPS_Matrix *res =
(CYCLOPS_Matrix *) ker_malloc (sizeof (CYCLOPS_Matrix),
MATRIX_ARITHMETICS_PID);
if (NULL == res)
{
return -ENOMEM;
}
if (getRow (A, res, row) != SOS_OK)
{
return -EINVAL;
}
//PUT_PORT();
//post_long
break;
}
// int8_t getCol (const CYCLOPS_Matrix * A, CYCLOPS_Matrix * res, uint16_t col)
case GET_COLUMN:
{
matrix_location_t *m = (matrix_location_t *) (msg->data);
CYCLOPS_Matrix *A = (CYCLOPS_Matrix *) (m->matPtr);
uint16_t col = (m->col);
CYCLOPS_Matrix *res =
(CYCLOPS_Matrix *) ker_malloc (sizeof (CYCLOPS_Matrix),
MATRIX_ARITHMETICS_PID);
if (NULL == res)
{
return -ENOMEM;
}
if (getCol (A, res, col) != SOS_OK)
{
return -EINVAL;
}
//PUT_PORT();
//post_long
break;
}
// int8_t getBit (const CYCLOPS_Matrix * A, uint16_t row, uint16_t col)
case GET_BIT:
{
matrix_location_t *m = (matrix_location_t *) (msg->data);
CYCLOPS_Matrix *A = (CYCLOPS_Matrix *) (m->matPtr);
uint16_t row = (m->row);
uint16_t col = (m->col);
CYCLOPS_Matrix *res =
(CYCLOPS_Matrix *) ker_malloc (sizeof (CYCLOPS_Matrix),
MATRIX_ARITHMETICS_PID);
if (NULL == res)
{
return -ENOMEM;
}
if (getBit (A, row, col) != SOS_OK)
{
return -EINVAL;
}
//PUT_PORT();
//post_long
break;
}
// int8_t scale (const CYCLOPS_Matrix * A, CYCLOPS_Matrix * C, const uint32_t s)
case SCALE:
{
matrix_scale_t *m = (matrix_scale_t *) (msg->data);
CYCLOPS_Matrix *A = (CYCLOPS_Matrix *) (m->matPtrA);
CYCLOPS_Matrix *C = (CYCLOPS_Matrix *) (m->matPtrC);
uint32_t s = (m->scale_value);
if (scale (A, C, s) != SOS_OK)
{
return -EINVAL;
}
//PUT_PORT();
//post_long
break;
}
// int8_t threshold (const CYCLOPS_Matrix * A, CYCLOPS_Matrix * B, uint32_t t)
case THRESHOLD:
{
matrix_thresh_t *m = (matrix_thresh_t *) (msg->data);
CYCLOPS_Matrix *A = (CYCLOPS_Matrix *) (m->matPtr);
uint32_t t = (m->threshhold_value);
CYCLOPS_Matrix *B =
(CYCLOPS_Matrix *) ker_malloc (sizeof (CYCLOPS_Matrix),
MATRIX_ARITHMETICS_PID);
if (NULL == B)
{
return -ENOMEM;
}
if (threshold (A, B, t) != SOS_OK)
{
return -EINVAL;
}
//PUT_PORT();
//post_long
break;
}
default:
return -EINVAL;
}
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_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 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;
}
points = ker_malloc(sizeof(struct point) * size, 1);
edges = ker_malloc(sizeof(struct edge) * size, 1);
g = ker_malloc(sizeof(struct graph), 1);
g->points = points;
g->edges = edges;
return g;
}
void make_graph_formal(int size, graph_t *new_graph __attribute__((sos_claim))) {
point_t *points;
edge_t *edges;
graph_t *g;
points = ker_malloc(sizeof(struct point) * size, 1);
edges = ker_malloc(sizeof(struct edge) * size, 1);
g = ker_malloc(sizeof(struct graph), 1);
g->points = points;
g->edges = edges;
new_graph = g;
return;
}
void free_graph(struct graph * g __attribute__((sos_release))) {
free((void *) (g->points));
free((void *) (g->edges));
free((void *) g);
}
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 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;
}
}
}
/**
* @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
}
/**
* @brief receiver thread
*/
static void recv_thread(int fd)
{
DEBUG("radio: receiver start\n");
while(1)
{
uint8_t read_buf[SEND_BUF_SIZE];
Message *recv_msg = NULL;
int cnt;
//sched_yield();
DEBUG("Radio: Receiver Idle ... \n");
cnt = read(topo_self.sock, read_buf, SEND_BUF_SIZE);
// XXX Using recvfrom will cause problem on Cygwin
//cnt = recvfrom(topo_self.sock, read_buf, SEND_BUF_SIZE, 0, &fromaddr, &fromaddrlen);
DEBUG("Radio: Read %d bytes\n", cnt);
if(cnt < 0 && cnt != -1) {
DEBUG("Radio: unable to read, exiting...\n");
exit(1);
}
if( cnt == 0 || cnt == -1) {
return;
}
if(cnt < SOS_MSG_HEADER_SIZE) {
DEBUG("Radio: get incomplete header\n");
//! something is wrong...
return;
}
if((((Message*)read_buf)->len) != (cnt - SOS_MSG_HEADER_SIZE)) {
DEBUG("Radio: invalid data payload size\n");
return;
}
recv_msg = msg_create();
if(recv_msg == NULL) {
DEBUG("Radio: no message header\n");
return;
}
memcpy(recv_msg, read_buf, SOS_MSG_HEADER_SIZE);
if(recv_msg->len != 0) {
recv_msg->data = ker_malloc(recv_msg->len, RADIO_PID);
if(recv_msg->data != NULL) {
recv_msg->flag = SOS_MSG_RELEASE;
memcpy(recv_msg->data, read_buf + SOS_MSG_HEADER_SIZE, recv_msg->len);
if(recv_msg->type == MSG_TIMESTAMP)
{
uint32_t timestamp = ker_systime32();
memcpy(&recv_msg->data[4], (uint8_t *)(×tamp), sizeof(uint32_t));
}
if(bTsEnable) {
timestamp_incoming(recv_msg, ker_systime32());
}
DEBUG("handle incoming msg \n");
handle_incoming_msg(recv_msg, SOS_MSG_RADIO_IO);
} else {
msg_dispose(recv_msg);
}
} else {
recv_msg->data = NULL;
recv_msg->flag = 0;
if(bTsEnable) {
timestamp_incoming(recv_msg, ker_systime32());
}
handle_incoming_msg(recv_msg, SOS_MSG_RADIO_IO);
}
}
}
