master/fsm_coe.c
changeset 1037 59f191c9185b
parent 1035 7fafc1a7451e
child 1038 7ca4103ba170
equal deleted inserted replaced
1036:dc2008bc8113 1037:59f191c9185b
   428         fsm->retries = EC_FSM_RETRIES;
   428         fsm->retries = EC_FSM_RETRIES;
   429         fsm->state = ec_fsm_coe_dict_check;
   429         fsm->state = ec_fsm_coe_dict_check;
   430         return;
   430         return;
   431     }
   431     }
   432 
   432 
       
   433     if (rec_size < 3) {
       
   434         EC_ERR("Received corrupted Sdo dictionary response (size %u).\n",
       
   435                 rec_size);
       
   436         fsm->state = ec_fsm_coe_error;
       
   437         return;
       
   438     }
       
   439 
   433     if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
   440     if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
   434         (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
   441         (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
   435         EC_ERR("Sdo information error response at slave %u!\n",
   442         EC_ERR("Sdo information error response at slave %u!\n",
   436                slave->ring_position);
   443                slave->ring_position);
   437         ec_canopen_abort_msg(EC_READ_U32(data + 6));
   444         if (rec_size < 10) {
       
   445             EC_ERR("Incomplete Sdo information error response:\n");
       
   446             ec_print_data(data, rec_size);
       
   447         } else {
       
   448             ec_canopen_abort_msg(EC_READ_U32(data + 6));
       
   449         }
   438         fsm->state = ec_fsm_coe_error;
   450         fsm->state = ec_fsm_coe_error;
   439         return;
   451         return;
   440     }
   452     }
   441 
   453 
   442     if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information
   454     if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information
   446         ec_print_data(data, rec_size);
   458         ec_print_data(data, rec_size);
   447         fsm->state = ec_fsm_coe_error;
   459         fsm->state = ec_fsm_coe_error;
   448         return;
   460         return;
   449     }
   461     }
   450 
   462 
   451     if (rec_size < 8) {
   463     if (rec_size < 8 || rec_size % 2) {
   452         EC_ERR("Invalid data size!\n");
   464         EC_ERR("Invalid data size %u!\n", rec_size);
   453         ec_print_data(data, rec_size);
   465         ec_print_data(data, rec_size);
   454         fsm->state = ec_fsm_coe_error;
   466         fsm->state = ec_fsm_coe_error;
   455         return;
   467         return;
   456     }
   468     }
   457 
   469 
   654         fsm->retries = EC_FSM_RETRIES;
   666         fsm->retries = EC_FSM_RETRIES;
   655         fsm->state = ec_fsm_coe_dict_desc_check;
   667         fsm->state = ec_fsm_coe_dict_desc_check;
   656         return;
   668         return;
   657     }
   669     }
   658 
   670 
       
   671     if (rec_size < 3) {
       
   672         EC_ERR("Received corrupted Sdo description response (size %u).\n",
       
   673                 rec_size);
       
   674         fsm->state = ec_fsm_coe_error;
       
   675         return;
       
   676     }
       
   677 
   659     if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
   678     if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
   660         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
   679         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
   661         EC_ERR("Sdo information error response at slave %u while"
   680         EC_ERR("Sdo information error response at slave %u while"
   662                " fetching Sdo 0x%04X!\n", slave->ring_position,
   681                " fetching Sdo 0x%04X!\n", slave->ring_position,
   663                sdo->index);
   682                sdo->index);
   664         ec_canopen_abort_msg(EC_READ_U32(data + 6));
   683         ec_canopen_abort_msg(EC_READ_U32(data + 6));
       
   684         fsm->state = ec_fsm_coe_error;
       
   685         return;
       
   686     }
       
   687 
       
   688     if (rec_size < 8) {
       
   689         EC_ERR("Received corrupted Sdo description response (size %u).\n",
       
   690                 rec_size);
   665         fsm->state = ec_fsm_coe_error;
   691         fsm->state = ec_fsm_coe_error;
   666         return;
   692         return;
   667     }
   693     }
   668 
   694 
   669     if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information
   695     if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information
   870         fsm->retries = EC_FSM_RETRIES;
   896         fsm->retries = EC_FSM_RETRIES;
   871         fsm->state = ec_fsm_coe_dict_entry_check;
   897         fsm->state = ec_fsm_coe_dict_entry_check;
   872         return;
   898         return;
   873     }
   899     }
   874 
   900 
       
   901     if (rec_size < 3) {
       
   902         EC_ERR("Received corrupted Sdo entry description response "
       
   903                 "(size %u).\n", rec_size);
       
   904         fsm->state = ec_fsm_coe_error;
       
   905         return;
       
   906     }
       
   907 
   875     if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
   908     if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
   876         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
   909         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
   877         EC_ERR("Sdo information error response at slave %u while"
   910         EC_ERR("Sdo information error response at slave %u while"
   878                " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position,
   911                " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position,
   879                sdo->index, fsm->subindex);
   912                sdo->index, fsm->subindex);
   880         ec_canopen_abort_msg(EC_READ_U32(data + 6));
   913         ec_canopen_abort_msg(EC_READ_U32(data + 6));
       
   914         fsm->state = ec_fsm_coe_error;
       
   915         return;
       
   916     }
       
   917 
       
   918     if (rec_size < 9) {
       
   919         EC_ERR("Received corrupted Sdo entry description response "
       
   920                 "(size %u).\n", rec_size);
   881         fsm->state = ec_fsm_coe_error;
   921         fsm->state = ec_fsm_coe_error;
   882         return;
   922         return;
   883     }
   923     }
   884 
   924 
   885     if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information
   925     if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information
   893         fsm->state = ec_fsm_coe_error;
   933         fsm->state = ec_fsm_coe_error;
   894         return;
   934         return;
   895     }
   935     }
   896 
   936 
   897     if (rec_size < 16) {
   937     if (rec_size < 16) {
   898         EC_ERR("Invalid data size!\n");
   938         EC_ERR("Invalid data size %u!\n", rec_size);
   899         ec_print_data(data, rec_size);
   939         ec_print_data(data, rec_size);
   900         fsm->state = ec_fsm_coe_error;
   940         fsm->state = ec_fsm_coe_error;
   901         return;
   941         return;
   902     }
   942     }
   903 
   943 
  1189         ec_print_data(data, rec_size);
  1229         ec_print_data(data, rec_size);
  1190     }
  1230     }
  1191 
  1231 
  1192     if (rec_size < 6) {
  1232     if (rec_size < 6) {
  1193         fsm->state = ec_fsm_coe_error;
  1233         fsm->state = ec_fsm_coe_error;
  1194         EC_ERR("Received data is too small (%u bytes):\n", rec_size);
  1234         EC_ERR("Received data are too small (%u bytes):\n", rec_size);
  1195         ec_print_data(data, rec_size);
  1235         ec_print_data(data, rec_size);
  1196         return;
  1236         return;
  1197     }
  1237     }
  1198 
  1238 
  1199     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1239     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1203                request->index, request->subindex, request->data_size,
  1243                request->index, request->subindex, request->data_size,
  1204                slave->ring_position);
  1244                slave->ring_position);
  1205         if (rec_size < 10) {
  1245         if (rec_size < 10) {
  1206             EC_ERR("Incomplete Abort command:\n");
  1246             EC_ERR("Incomplete Abort command:\n");
  1207             ec_print_data(data, rec_size);
  1247             ec_print_data(data, rec_size);
  1208         }
  1248         } else {
  1209         else
  1249             fsm->request->abort_code = EC_READ_U32(data + 6);
  1210             ec_canopen_abort_msg(EC_READ_U32(data + 6));
  1250             ec_canopen_abort_msg(fsm->request->abort_code);
       
  1251         }
  1211         return;
  1252         return;
  1212     }
  1253     }
  1213 
  1254 
  1214     if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1255     if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1215         EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
  1256         EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
  1444 
  1485 
  1445     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1486     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1446         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1487         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1447         EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n",
  1488         EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n",
  1448                request->index, request->subindex, slave->ring_position);
  1489                request->index, request->subindex, slave->ring_position);
  1449         if (rec_size >= 10)
  1490         if (rec_size >= 10) {
  1450             ec_canopen_abort_msg(EC_READ_U32(data + 6));
  1491             request->abort_code = EC_READ_U32(data + 6);
  1451         else
  1492             ec_canopen_abort_msg(request->abort_code);
       
  1493         } else {
  1452             EC_ERR("No abort message.\n");
  1494             EC_ERR("No abort message.\n");
       
  1495         }
  1453         fsm->state = ec_fsm_coe_error;
  1496         fsm->state = ec_fsm_coe_error;
  1454         return;
  1497         return;
  1455     }
  1498     }
  1456 
  1499 
  1457     // normal or expedited?
  1500     // normal or expedited?
  1735 
  1778 
  1736     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1779     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1737         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1780         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1738         EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n",
  1781         EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n",
  1739                request->index, request->subindex, slave->ring_position);
  1782                request->index, request->subindex, slave->ring_position);
  1740         ec_canopen_abort_msg(EC_READ_U32(data + 6));
  1783         request->abort_code = EC_READ_U32(data + 6);
       
  1784         ec_canopen_abort_msg(request->abort_code);
  1741         fsm->state = ec_fsm_coe_error;
  1785         fsm->state = ec_fsm_coe_error;
  1742         return;
  1786         return;
  1743     }
  1787     }
  1744 
  1788 
  1745     if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1789     if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response