00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "config/compiler.h"
00038
00039 #include "wmp_config.h"
00040 #include "core/include/manage.h"
00041 #include "core/include/flow_control.h"
00042 #include "core/include/queue_core.h"
00043 #include "core/include/aura.h"
00044 #include "core/include/queue_core.h"
00045 #include "core/include/wmp_utils.h"
00046 #include "core/include/task_timing.h"
00047
00048
00049
00050
00051 char isolated_count[32] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00052 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
00053
00054 static int confirmed(unsigned int ack, unsigned short ack_hash, short ack_part){
00055 if (status.wait_ack_of_hash == ack_hash
00056 && status.wait_ack_of_part == ack_part
00057 && ack == status.wait_ack_from){
00058 return 1;
00059 }else{
00060 return 0;
00061 }
00062 }
00063
00064 int create_new_token(wmpFrame * t) {
00065 int i = 0;
00066 if (status.power_save && t->hdr.waiting <= 1 && status.lastRecvdType != MESSAGE ){
00067 if (t->hdr.sleep > 0){
00068 wmpSendAck(t);
00069 t->hdr.sleep = 0;
00070 return RECEIVE;
00071 }
00072 }
00073 t->hdr.sleep = ms_to_us(500);
00074 t->hdr.serial = status.highestSerial;
00075 t->hdr.to = UNDEF;
00076 t->hdr.type = FRESH_TOKEN;
00077 t->hdr.retries = 0;
00078 t->hdr.from = status.id;
00079 t->hdr.loop_id += 1;
00080 t->tkn.maxPri = UNDEF;
00081 t->tkn.idMaxPri = UNDEF;
00082 t->tkn.age = 0;
00083 t->hdr.burst = 0;
00084 t->hdr.waiting = 0;
00085
00086
00087 for (i = 0; i < status.N_NODES; i++) {
00088 nstat_clearReached(i);
00089 }
00090
00091 nstat_setReached(status.id);
00092 nstat_clearLost(status.id);
00093 status.lr = UNDEF;
00094
00095
00096 for (i = 0; i < status.N_NODES; i++) {
00097 if (isIsolated(lqm_get_ptr(), i)) {
00098 nstat_setLost(i);
00099 }
00100 }
00101
00102 t->tkn.beginner = status.id;
00103 return EVALUATE_TOKEN;
00104 }
00105
00106 int evaluate_token(wmpFrame * t) {
00107 int i, tot, last_one, burst = 0, waiting = 0, delay;
00108 signed char highestPriority = UNDEF;
00109 unsigned int age;
00110 if (t->tkn.idMaxPri >=0){
00111 delay = task_get_next_predicted_push_delay_most_priority_than(t->tkn.maxPri+1);
00112 }else{
00113 delay = task_get_next_predicted_push_delay();
00114 }
00115
00116 if (delay > 1000){
00117 delay = 1000;
00118 }
00119
00120 delay = ms_to_us(delay);
00121
00122 if (delay < t->hdr.sleep){
00123 t->hdr.sleep = delay;
00124 }
00125
00126
00127
00128 aura_clear();
00129
00130 if ((t->hdr.from != status.id) || (t->hdr.type == FRESH_TOKEN)) {
00131
00132 t->hdr.type = TOKEN;
00133
00134
00135 if (t->tkn.idMaxPri != UNDEF) {
00136 t->tkn.age += wmp_calculate_frame_duration_ms(status.rate,
00137 wmp_get_frame_total_lenght(t));
00138 }
00139
00140 if (!nstat_isReached(status.id)) {
00141 status.lr = t->hdr.from;
00142 }
00143 status.emergency_lr = t->hdr.from;
00144
00145
00146 if (status.enable_message_reschedule) {
00147 if (status.wait_ack_from) {
00148 if (confirmed(t->hdr.ack, t->tkn.ack_hash, t->tkn.ack_part)){
00149 wmp_queue_tx_confirm();
00150 }else{
00151 int age, port, period, priority;
00152 wmp_queue_tx_get_last_popped_info(&age, &port, &priority);
00153 period = task_get_priority_period(priority);
00154 if (status.secure || age < period) {
00155 t->hdr.sleep = 0;
00156 wmp_queue_tx_reschedule();
00157 } else{
00158 wmp_queue_tx_confirm();
00159 }
00160 }
00161
00162 t->hdr.ack = 0;
00163 t->hdr.fc = 0;
00164 t->tkn.ack_hash = 0;
00165 t->tkn.ack_part = 0;
00166 status.wait_ack_from = 0;
00167 }
00168 }else{
00169 wmp_queue_tx_confirm();
00170 }
00171
00172 if (status.loop_id != t->hdr.loop_id){
00173 status.loop_id = t->hdr.loop_id;
00174 }
00175
00176
00177 waiting = wmp_queue_tx_get_len();
00178
00179 if (waiting > 0) {
00180 int id = wmp_queue_tx_get_head_id();
00181 highestPriority = wmp_queue_tx_get_elem_priority(id);
00182 age = wmp_queue_tx_get_elem_age(id);
00183 burst = wmp_queue_tx_get_elem_burst(id);
00184
00185 t->hdr.waiting += waiting;
00186
00187 if (highestPriority > t->tkn.maxPri) {
00188
00189 t->tkn.idMaxPri = status.id;
00190 t->tkn.maxPri = highestPriority;
00191 t->tkn.age = age;
00192 t->hdr.burst = burst;
00193 } else if (highestPriority == t->tkn.maxPri) {
00194 if (t->tkn.age < age) {
00195 t->tkn.idMaxPri = status.id;
00196 t->tkn.age = age;
00197 t->hdr.burst = burst;
00198 }
00199 }
00200 }
00201
00202
00203 nstat_setReached(status.id);
00204 nstat_clearLost(status.id);
00205
00206
00207 t->hdr.from = status.id;
00208
00209 }
00210
00211
00212 for (i = 0; i < status.N_NODES; i++) {
00213 if (i != status.id) {
00214 if (nstat_isLost(i) && !nstat_isReached(i)) {
00215 if ((t->hdr.loop_id % wmpGetNumOfNodes()) == status.id) {
00216
00217
00218
00219 lqm_set_val(status.id, i, 100);
00220 lqm_set_val(i, status.id, 100);
00221
00222 t->hdr.to = i;
00223 t->hdr.retries = 0;
00224 t->hdr.type = TOKEN;
00225 t->hdr.sleep = 0;
00226 return SEND_TOKEN;
00227 }
00228 }
00229 }
00230 }
00231
00232
00233 tot = 0;
00234 for (i = 0; i < status.N_NODES; i++) {
00235 if (i == status.id) {
00236 continue;
00237 }
00238 tot += lqm_get_val(status.id, i);
00239 }
00240
00241 if (tot == 0) {
00242 t->hdr.sleep = 0;
00243 status.highestSerial+=status.id*5;
00244
00245 return NEW_TOKEN;
00246 }
00247
00248
00249
00250 last_one = 1;
00251 for (i = 0; i < status.N_NODES; i++) {
00252 last_one = last_one && nstat_isReached(i);
00253 }
00254 if (last_one > 0) {
00255
00256
00257
00258 if (status.id != t->tkn.beginner ) {
00259 nstat_clearReached(t->tkn.beginner);
00260 return EVALUATE_TOKEN;
00261 }
00262
00263
00264 if (t->tkn.idMaxPri >= 0) {
00265
00266
00267 return CREATE_AUTHORIZATION;
00268 } else {
00269
00270 return NEW_TOKEN;
00271 }
00272 } else {
00273
00274 int bestRssi;
00275 signed char selected = UNDEF;
00276
00277 if (status.use_prim) {
00278
00279 int i, j;
00280 char ** prim_lqm;
00281
00282 lqm_backup();
00283
00284 prim_lqm = prim(lqm_get_ptr());
00285 for (i = 0; i < status.N_NODES; i++) {
00286 for (j = 0; j < status.N_NODES; j++) {
00287 if (i != j) {
00288 lqm_set_val(i, j, prim_lqm[i][j]);
00289 }
00290 }
00291 }
00292 } else if (status.use_prune) {
00293 char ** pruned_lqm;
00294
00295 lqm_backup();
00296
00297 pruned_lqm = lqm_prune(lqm_get_ptr());
00298 lqm_copy_to(lqm_get_ptr(), pruned_lqm);
00299
00300 }
00301
00302
00303 if (status.use_prob) {
00304 int best_prob = 0;
00305 lqm_compute_prob(lqm_get_ptr());
00306 for (i = 0; i < status.N_NODES; i++) {
00307 if (i != status.id) {
00308 if (!nstat_isReached(i) && !nstat_isLost(i)) {
00309 if (lqm_prob_get_val(status.id, i) > best_prob) {
00310 best_prob = lqm_prob_get_val(status.id, i);
00311 selected = i;
00312 }
00313 }
00314 }
00315 }
00316 } else {
00317
00318 bestRssi = 0;
00319 for (i = 0; i < status.N_NODES; i++) {
00320 if (i == status.id) {
00321 continue;
00322 }
00323 if (!nstat_isReached(i) && !nstat_isLost(i)) {
00324 if (lqm_get_val(status.id, i) > bestRssi) {
00325 bestRssi = lqm_get_val(status.id, i);
00326 selected = i;
00327 }
00328 }
00329 }
00330 }
00331 if (status.use_prim || status.use_prune) {
00332 lqm_restore();
00333 }
00334
00335 if (selected == UNDEF) {
00336
00337
00338
00339 if (status.lr == UNDEF) {
00340 t->hdr.sleep = 0;
00341 status.highestSerial+=status.id*5;
00342 return NEW_TOKEN;
00343 } else {
00344
00345 t->hdr.to = status.lr;
00346 t->hdr.retries = 0;
00347 t->hdr.type = TOKEN;
00348
00349 status.lr = UNDEF;
00350 return SEND_TOKEN;
00351 }
00352
00353 ASSERT(0);
00354 } else {
00355
00356 t->hdr.to = selected;
00357 t->hdr.retries = 0;
00358 t->hdr.type = TOKEN;
00359 return SEND_TOKEN;
00360 }
00361 }
00362 }
00363
00364 int manage_token_expired_timeout(wmpFrame* t) {
00365
00366
00367
00368
00369
00370
00371
00372 if (status.retries < status.maxPerNodeRetries && t->hdr.retries
00373 < status.maxTotalRetries) {
00374 status.retries++;
00375 t->hdr.retries++;
00376 t->hdr.sleep = 0;
00377 return RETRY;
00378 } else {
00379 nstat_setReached(t->hdr.to);
00380 status.retries = 0;
00381
00382
00383 rssi_reset(t->hdr.to);
00384 lqm_set_val(status.id, t->hdr.to, 0);
00385 t->hdr.sleep = 0;
00386 return EVALUATE_TOKEN;
00387 }
00388 }
00389
00390 int manage_authorization_expired_timeout(wmpFrame * t) {
00391 if (status.retries < status.maxPerNodeRetries && t->hdr.retries
00392 < status.maxTotalRetries) {
00393 status.retries++;
00394 t->hdr.retries++;
00395 t->hdr.sleep = 0;
00396 return RETRY;
00397 } else {
00398 status.retries = 0;
00399
00400
00401 rssi_reset(t->hdr.to);
00402 lqm_set_val(status.id, t->hdr.to, 0);
00403 t->hdr.sleep = 0;
00404 return NEW_TOKEN;
00405 }
00406 }
00407
00408 int manage_message_expired_timeout(wmpFrame * t) {
00409 if (status.retries < status.maxPerNodeRetries && t->hdr.retries
00410 < status.maxTotalRetries) {
00411 status.retries++;
00412 t->hdr.retries++;
00413 t->hdr.sleep = 0;
00414 return RETRY;
00415 } else {
00416 status.retries = 0;
00417
00418
00419 rssi_reset(t->hdr.to);
00420 lqm_set_val(status.id, t->hdr.to, 0);
00421 t->hdr.sleep = 0;
00422 return NEW_TOKEN;
00423 }
00424 }
00425
00426 int evaluate_foreign(wmpFrame * t) {
00427 return RECEIVE;
00428 }
00429
00430 static signed char getNext(wmpFrame * t) {
00431
00432 signed char next = UNDEF;
00433 unsigned int reached = 0;
00434 int i, j, dest = UNDEF, cost;
00435
00436 if (t->hdr.type == MESSAGE) {
00437 dest = t->msg.dest;
00438 reached = t->msg.reached;
00439 } else if (t->hdr.type == AUTHORIZATION) {
00440 dest = t->aut.dest;
00441 reached = t->aut.reached;
00442 }
00443 if (status.use_prim) {
00444 next = nextStepWithCost(prim(lqm_get_ptr()), status.id, dest, &cost);
00445 } else if (status.use_prune){
00446 next = nextStepWithCost(lqm_prune(lqm_get_ptr()), status.id, dest, &cost);
00447 }else if (status.use_prob){
00448 char path[32];
00449 lqm_compute_prob(lqm_get_ptr());
00450 next = lqm_prob_get_path(status.id, dest, path);
00451
00452 }
00453
00454 if (next == UNDEF || mBitsIsSet(reached, next)) {
00455 lqm_backup();
00456 for (i = 0; i < status.N_NODES; i++) {
00457 if (i == status.id) {
00458 continue;
00459 }
00460 if (mBitsIsSet(reached,i)) {
00461 for (j = 0; j < status.N_NODES; j++) {
00462 lqm_set_val(i, j, 0);
00463 lqm_set_val(j, i, 0);
00464 }
00465 }
00466 }
00467 if (status.use_prob){
00468 char path[32];
00469 lqm_compute_prob(lqm_get_ptr());
00470 next = lqm_prob_get_path(status.id, dest, path);
00471 }else{
00472 next = nextStepWithCost(lqm_get_ptr(), status.id, dest, &cost);
00473 }
00474 lqm_restore();
00475 }
00476 return next;
00477 }
00478
00479 int evaluate_message(wmpFrame * t) {
00480 int i, more = 0;
00481
00482 if (mBitsIsSet(t->msg.type, BURST)){
00483 aura_clear();
00484 }
00485 if (t->hdr.from != status.id) {
00486 if (mBitsIsSet(t->msg.type, AURA_MSG)){
00487 aura_add(aura_msg, t->hdr.from);
00488 }else{
00489 aura_add(aura_auth, t->hdr.from);
00490 }
00491 }
00492
00493 if (mBitsIsSet(t->msg.type,AURA_MSG)){
00494 aura_store_msg(&t->msg);
00495 }
00496
00497 aura_discard_unnecessary(t->msg.dest);
00498
00499 mBitsSet(t->msg.reached,status.id);
00500
00501
00502 if (mBitsIsSet(t->msg.type,AURA_MSG)){
00503 if (mBitsIsSet(t->msg.dest,status.id)) {
00504 mBitsUnset(t->msg.dest,status.id);
00505
00506 enqueue_message(t);
00507 mBitsSet(t->hdr.ack, wmpGetNodeId());
00508
00509
00510 if (wmp_queue_rx_get_room() > 5) {
00511 mBitsSet(t->hdr.fc, wmpGetNodeId());
00512 }else{
00513 mBitsUnset(t->hdr.fc, wmpGetNodeId());
00514 }
00515 }
00516 }
00517
00518 for (i = 0; i<status.N_NODES ;i++){
00519 more = more || mBitsIsSet(t->msg.dest,i);
00520 }
00521
00522 if (more){
00523 aura_t type;
00524 int next = aura_get_next(t,&type);
00525
00526 if (type == aura_auth){
00527
00528 mBitsUnset(t->msg.type,AURA_MSG);
00529
00530 } else{
00531 mBitsSet(t->msg.type,AURA_MSG);
00532 aura_restore_msg(&t->msg);
00533 if (next < 0 || mBitsIsSet(t->msg.reached,next)) {
00534 t->msg.reached = 0;
00535 return NEW_TOKEN;
00536 }
00537 }
00538
00539 if (next >= 0) {
00540 t->hdr.from = status.id;
00541 t->hdr.to = next;
00542 t->hdr.retries = 0;
00543 t->hdr.type = MESSAGE;
00544
00545 aura_add(type, t->hdr.to);
00546
00547 t->msg.age += wmp_calculate_frame_duration_ms(status.rate,
00548 wmp_get_frame_total_lenght(t));
00549
00550 wmp_print_append(t);
00551 wmp_print("%d ",wmpGetNodeId());
00552
00553 return SEND_MESSAGE;
00554 }
00555 }
00556 if (status.power_save && t->hdr.burst > 1 && t->hdr.sleep > ms_to_us(5)){
00557 signed char msg_src = t->msg.src;
00558 unsigned short ack_hash = t->msg.msg_hash;
00559 signed char ack_part_id = t->msg.part_id;
00560 t->hdr.from = status.id;
00561 t->hdr.type = AUTHORIZATION;
00562 t->hdr.retries = 0;
00563 t->hdr.burst = t->hdr.burst - 1;
00564 mBitsSet(t->aut.type, BURST);
00565 t->aut.dest = msg_src;
00566 t->aut.ack_hash = ack_hash;
00567 t->aut.ack_part = ack_part_id;
00568 return EVALUATE_AUTHORIZATION;
00569 }else{
00570 unsigned short ack_hash = t->msg.msg_hash;
00571 signed char ack_part_id = t->msg.part_id;
00572 t->msg.reached = 0;
00573 t->tkn.ack_hash = ack_hash;
00574 t->tkn.ack_part = ack_part_id;
00575 return NEW_TOKEN;
00576 }
00577 }
00578
00579 int evaluate_authorization(wmpFrame * t) {
00580 if (t->aut.dest == UNDEF){
00581 return NEW_TOKEN;
00582 }
00583
00584 if (mBitsIsSet(t->aut.type, BURST)){
00585 aura_clear();
00586 }
00587
00588 if (t->hdr.from == status.id) {
00589 t->aut.reached = 0;
00590 } else {
00591 aura_add(aura_auth, t->hdr.from);
00592 }
00593 mBitsSet(t->aut.reached,status.id);
00594
00595 if (t->aut.dest == status.id) {
00596
00597
00598 if (status.enable_message_reschedule) {
00599 if (status.wait_ack_from) {
00600 if (confirmed(t->hdr.ack, t->aut.ack_hash, t->aut.ack_part)){
00601 wmp_queue_tx_confirm();
00602 }else{
00603 int age, port, period, priority;
00604 wmp_queue_tx_get_last_popped_info(&age, &port, &priority);
00605 period = task_get_priority_period(priority);
00606 if (status.secure || age < period) {
00607 t->hdr.sleep = 0;
00608 wmp_queue_tx_reschedule();
00609 } else{
00610 wmp_queue_tx_confirm();
00611 }
00612 }
00613 t->hdr.ack = 0;
00614 t->hdr.fc = 0;
00615 t->tkn.ack_hash = 0;
00616 t->tkn.ack_part = 0;
00617 status.wait_ack_from = 0;
00618 }
00619 }else{
00620 wmp_queue_tx_confirm();
00621 }
00622 if (wmp_queue_tx_get_count() > 0) {
00623 return CREATE_MESSAGE;
00624 } else {
00625 return NEW_TOKEN;
00626 }
00627 } else {
00628 int next = getNext(t);
00629 if (next >= 0) {
00630 if (mBitsIsSet(t->aut.reached,next)) {
00631 return NEW_TOKEN;
00632 } else {
00633 t->hdr.from = status.id;
00634 t->hdr.to = next;
00635 t->hdr.retries = 0;
00636 t->hdr.type = AUTHORIZATION;
00637 aura_add(aura_auth, t->hdr.to);
00638 return SEND_AUTHORIZATION;
00639 }
00640 } else {
00641 return NEW_TOKEN;
00642 }
00643 }
00644 }
00645
00646 int create_authorization(wmpFrame * t) {
00647 t->aut.dest = t->tkn.idMaxPri;
00648 t->aut.src = status.id;
00649 t->hdr.from = status.id;
00650 t->hdr.type = AUTHORIZATION;
00651 t->hdr.retries = 0;
00652 mBitsUnset(t->aut.type,BURST);
00653 return EVALUATE_AUTHORIZATION;
00654 }
00655
00656 int create_message(wmpFrame * t) {
00657 static longMsg_t * m;
00658 int m_id = wmp_queue_tx_pop_part(&m);
00659
00660 t->hdr.burst = wmp_queue_tx_get_elem_burst(m_id);
00661
00662 t->hdr.type = MESSAGE;
00663 t->hdr.from = status.id;
00664 t->hdr.to = status.id;
00665
00666 t->msg.age = us_to_ms(((int)(getRawActualTimeus()-m->ts)));
00667 t->msg.dest = m->dest;
00668
00669 t->msg.src = status.id;
00670 memcpy(wmp_get_message_data_pointer(t), m->pointer, m->this_part_size);
00671 t->msg.len = (unsigned short) m->this_part_size;
00672 t->msg.port = m->port;
00673 t->msg.priority = m->priority;
00674
00675 t->msg.msg_hash = m->hash;
00676 t->msg.part_id = m->part_id;
00677 t->msg.msg_part_size = m->msg_part_size;
00678
00679 t->msg.reached = 0;
00680
00681 mBitsSet(t->msg.type,AURA_MSG);
00682 if (m->rescheduled){
00683 mBitsSet(t->msg.type,RESCHEDULED);
00684 }else{
00685 mBitsUnset(t->msg.type,RESCHEDULED);
00686 }
00687
00688 t->hdr.ack = 0;
00689 t->hdr.fc = 0;
00690 status.wait_ack_from = m->dest;
00691 status.wait_ack_of_hash = m->hash;
00692 status.wait_ack_of_part = m->part_id;
00693
00694 wmp_queue_tx_pop_part_done(m_id);
00695
00696 wmp_print_clean(t);
00697 return EVALUATE_MESSAGE;
00698 }
00699
00700 int enqueue_message(wmpFrame * t) {
00701 static longMsg_t m;
00702 m.src = t->msg.src;
00703 m.priority = t->msg.priority;
00704 m.dest = t->msg.dest;
00705 m.size = t->msg.len;
00706 m.data = wmp_get_message_data_pointer(t);
00707 m.port = t->msg.port;
00708 m.hash = t->msg.msg_hash;
00709 m.part_id = t->msg.part_id;
00710 m.msg_part_size = t->msg.msg_part_size;
00711 wmp_queue_rx_push_part(&m);
00712 return NEW_TOKEN;
00713 }
00714