Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm_coe.c
changeset 769 2195c2ea37b4
parent 739 4a02162a6301
child 814 a51f857b1b2d
equal deleted inserted replaced
768:b709149d5bba 769:2195c2ea37b4 
   364     ec_datagram_t *datagram = fsm->datagram;
 
   364     ec_datagram_t *datagram = fsm->datagram;
 
   365     ec_slave_t *slave = fsm->slave;
 
   365     ec_slave_t *slave = fsm->slave;
 
   366     uint8_t *data, mbox_prot;
 
   366     uint8_t *data, mbox_prot;
 
   367     size_t rec_size;
 
   367     size_t rec_size;
 
   368     unsigned int sdo_count, i;
 
   368     unsigned int sdo_count, i;
 
   369     uint16_t sdo_index;
 
   369     uint16_t sdo_index, fragments_left;
 
   370     ec_sdo_t *sdo;
 
   370     ec_sdo_t *sdo;
 
   371 
   371 
   372     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   372     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   373         return; // FIXME: request again?
 
   373         return; // FIXME: request again?
 
   374 
   374 
   395     }
 
   395     }
 
   396 
   396 
   397     if (mbox_prot != 0x03) { // CoE
 
   397     if (mbox_prot != 0x03) { // CoE
 
   398         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
   398         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
   399         fsm->state = ec_fsm_coe_error;
 
   399         fsm->state = ec_fsm_coe_error;
 
   400 	return;
 
   400         return;
 
   401     }
 
   401     }
 
   402 
   402 
   403     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   403     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   404         (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
 
   404         (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response
 
   405         EC_ERR("SDO information error response at slave %i!\n",
 
   405         EC_ERR("SDO information error response at slave %i!\n",
 
   406                slave->ring_position);
 
   406                slave->ring_position);
 
   407         ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
   407         ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
   408         fsm->state = ec_fsm_coe_error;
 
   408         fsm->state = ec_fsm_coe_error;
 
   409 	return;
 
   409         return;
 
   410     }
 
   410     }
 
   411 
   411 
   412     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
 
   412     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
 
   413         (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response
 
   413         (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response
 
   414         EC_ERR("Invalid SDO list response at slave %i!\n",
 
   414         EC_ERR("Invalid SDO list response at slave %i!\n",
 
   415                slave->ring_position);
 
   415                slave->ring_position);
 
   416         ec_print_data(data, rec_size);
 
   416         ec_print_data(data, rec_size);
 
   417         fsm->state = ec_fsm_coe_error;
 
   417         fsm->state = ec_fsm_coe_error;
 
   418 	return;
 
   418         return;
 
   419     }
 
   419     }
 
   420 
   420 
   421     if (rec_size < 8) {
 
   421     if (rec_size < 8) {
 
   422         EC_ERR("Invalid data size!\n");
 
   422         EC_ERR("Invalid data size!\n");
 
   423         ec_print_data(data, rec_size);
 
   423         ec_print_data(data, rec_size);
 
   424         fsm->state = ec_fsm_coe_error;
 
   424         fsm->state = ec_fsm_coe_error;
 
   425 	return;
 
   425         return;
 
   426     }
 
   426     }
 
   427 
   427 
   428     sdo_count = (rec_size - 8) / 2;
 
   428     sdo_count = (rec_size - 8) / 2;
 
   429 
   429 
   430     for (i = 0; i < sdo_count; i++) {
 
   430     for (i = 0; i < sdo_count; i++) {
 
   449         }
 
   449         }
 
   450 
   450 
   451         list_add_tail(&sdo->list, &slave->sdo_dictionary);
 
   451         list_add_tail(&sdo->list, &slave->sdo_dictionary);
 
   452     }
 
   452     }
 
   453 
   453 
   454     if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again.
 
   454     fragments_left = EC_READ_U16(data + 4);
 
       
   455     if (slave->master->debug_level && fragments_left) {
 
       
   456         EC_DBG("SDO list fragments left: %u\n", fragments_left);
 
       
   457     }
 
       
   458 
       
   459     if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again.
 
   455         fsm->cycles_start = datagram->cycles_sent;
 
   460         fsm->cycles_start = datagram->cycles_sent;
 
   456         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
   461         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
   457         fsm->retries = EC_FSM_RETRIES;
 
   462         fsm->retries = EC_FSM_RETRIES;
 
   458         fsm->state = ec_fsm_coe_dict_check;
 
   463         fsm->state = ec_fsm_coe_dict_check;
 
   459         return;
 
   464         return;
 
  1323     }
 
  1328     }
 
  1324 
  1329 
  1325     if (mbox_prot != 0x03) { // CoE
 
  1330     if (mbox_prot != 0x03) { // CoE
 
  1326         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
  1331         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
  1327         fsm->state = ec_fsm_coe_error;
 
  1332         fsm->state = ec_fsm_coe_error;
 
  1328 	return;
 
  1333         return;
 
  1329     }
 
  1334     }
 
  1330 
  1335 
  1331     if (rec_size < 10) {
 
  1336     if (rec_size < 3) {
 
  1332         EC_ERR("Received currupted SDO upload response!\n");
 
  1337         EC_ERR("Received currupted SDO upload response (%u bytes)!\n", rec_size);
 
  1333         ec_print_data(data, rec_size);
 
  1338         ec_print_data(data, rec_size);
 
  1334         fsm->state = ec_fsm_coe_error;
 
  1339         fsm->state = ec_fsm_coe_error;
 
  1335 	return;
 
  1340         return;
 
  1336     }
 
  1341     }
 
  1337 
  1342 
  1338     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
  1343     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
  1339         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
  1344         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
  1340         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
 
  1345         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",
 
  1341                entry->sdo->index, entry->subindex, slave->ring_position);
 
  1346                entry->sdo->index, entry->subindex, slave->ring_position);
 
  1342         ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
  1347         if (rec_size >= 10)
 
  1343         fsm->state = ec_fsm_coe_error;
 
  1348             ec_canopen_abort_msg(EC_READ_U32(data + 6));
 
  1344 	return;
 
  1349         else
 
  1345     }
 
  1350             EC_ERR("No abort message.\n");
 
  1346 
  1351         fsm->state = ec_fsm_coe_error;
 
  1347     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
 
  1352         return;
 
  1348         EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
 
  1353     }
 
  1349         EC_READ_U16(data + 3) != entry->sdo->index || // index
 
  1354 
  1350         EC_READ_U8 (data + 5) != entry->subindex) { // subindex
 
  1355     if (request->data) {
 
  1351         EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex);
 
  1356         kfree(request->data);
 
  1352         EC_ERR("Invalid SDO upload response at slave %i!\n",
 
  1357         request->data = NULL;
 
  1353                slave->ring_position);
 
  1358     }
 
  1354         ec_print_data(data, rec_size);
 
  1359     request->size = 0;
 
  1355         fsm->state = ec_fsm_coe_error;
 
  1360 
  1356 	return;
 
  1361     // normal or expedited?
 
  1357     }
 
       
  1358 
       
  1359     data_size = rec_size - 10;
 
       
  1360     expedited = EC_READ_U8(data + 2) & 0x02;
 
  1362     expedited = EC_READ_U8(data + 2) & 0x02;
 
  1361 
  1363 
  1362     if (expedited) {
 
  1364     if (expedited) {
 
       
  1365         if (rec_size < 7) {
 
       
  1366             EC_ERR("Received currupted SDO expedited upload"
 
       
  1367                     " response (only %u bytes)!\n", rec_size);
 
       
  1368             ec_print_data(data, rec_size);
 
       
  1369             fsm->state = ec_fsm_coe_error;
 
       
  1370             return;
 
       
  1371         }
 
       
  1372 
       
  1373         if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
 
       
  1374                 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
 
       
  1375                 EC_READ_U16(data + 3) != entry->sdo->index || // index
 
       
  1376                 EC_READ_U8 (data + 5) != entry->subindex) { // subindex
 
       
  1377             EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex);
 
       
  1378             EC_ERR("Invalid SDO upload response at slave %i!\n",
 
       
  1379                     slave->ring_position);
 
       
  1380             ec_print_data(data, rec_size);
 
       
  1381             fsm->state = ec_fsm_coe_error;
 
       
  1382             return;
 
       
  1383         }
 
       
  1384 
  1363         size_specified = EC_READ_U8(data + 2) & 0x01;
 
  1385         size_specified = EC_READ_U8(data + 2) & 0x01;
 
  1364         if (size_specified) {
 
  1386         if (size_specified) {
 
  1365             complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
 
  1387             complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);
 
  1366         }
 
  1388         } else {
 
  1367         else {
 
       
  1368             complete_size = 4;
 
  1389             complete_size = 4;
 
  1369         }
 
  1390         }
 
  1370     }
 
  1391 
  1371     else {
 
  1392         if (rec_size < 6 + complete_size) {
 
  1372         complete_size = EC_READ_U32(data + 6);
 
  1393             EC_ERR("Received currupted SDO expedited upload"
 
  1373     }
 
  1394                     " response (only %u bytes)!\n", rec_size);
 
  1374 
  1395             ec_print_data(data, rec_size);
 
  1375     if (request->data) {
 
  1396             fsm->state = ec_fsm_coe_error;
 
  1376         kfree(request->data);
 
  1397             return;
 
  1377         request->data = NULL;
 
  1398         }
 
  1378     }
 
  1399 
  1379     request->size = 0;
 
       
  1380 
       
  1381     if (complete_size) {
 
       
  1382         if (!(request->data = (uint8_t *)
 
  1400         if (!(request->data = (uint8_t *)
 
  1383               kmalloc(complete_size + 1, GFP_ATOMIC))) {
 
  1401                     kmalloc(complete_size + 1, GFP_ATOMIC))) {
 
  1384             EC_ERR("Failed to allocate %i bytes of SDO data!\n",
 
  1402             EC_ERR("Failed to allocate %i bytes of SDO data!\n",
 
  1385                    complete_size);
 
  1403                     complete_size);
 
  1386             fsm->state = ec_fsm_coe_error;
 
  1404             fsm->state = ec_fsm_coe_error;
 
  1387             return;
 
  1405             return;
 
  1388         }
 
  1406         }
 
  1389         request->data[complete_size] = 0x00; // just to be sure...
 
  1407         request->data[complete_size] = 0x00; // just to be sure...
 
  1390     }
 
  1408 
  1391 
       
  1392     if (expedited) {
 
       
  1393         memcpy(request->data, data + 6, complete_size);
 
  1409         memcpy(request->data, data + 6, complete_size);
 
  1394         request->size = complete_size;
 
  1410         request->size = complete_size;
 
  1395     }
 
  1411 
  1396     else {
 
  1412     } else { // normal
 
       
  1413         if (rec_size < 10) {
 
       
  1414             EC_ERR("Received currupted SDO normal upload"
 
       
  1415                     " response (only %u bytes)!\n", rec_size);
 
       
  1416             ec_print_data(data, rec_size);
 
       
  1417             fsm->state = ec_fsm_coe_error;
 
       
  1418             return;
 
       
  1419         }
 
       
  1420 
       
  1421         if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
 
       
  1422                 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
 
       
  1423                 EC_READ_U16(data + 3) != entry->sdo->index || // index
 
       
  1424                 EC_READ_U8 (data + 5) != entry->subindex) { // subindex
 
       
  1425             EC_ERR("SDO upload 0x%04X:%X failed:\n", entry->sdo->index, entry->subindex);
 
       
  1426             EC_ERR("Invalid SDO upload response at slave %i!\n",
 
       
  1427                     slave->ring_position);
 
       
  1428             ec_print_data(data, rec_size);
 
       
  1429             fsm->state = ec_fsm_coe_error;
 
       
  1430             return;
 
       
  1431         }
 
       
  1432 
       
  1433         data_size = rec_size - 10;
 
       
  1434         complete_size = EC_READ_U32(data + 6);
 
       
  1435 
       
  1436         if (!complete_size) {
 
       
  1437             EC_ERR("No complete size supplied!\n");
 
       
  1438             ec_print_data(data, rec_size);
 
       
  1439             fsm->state = ec_fsm_coe_error;
 
       
  1440             return;
 
       
  1441         }
 
       
  1442 
       
  1443         if (!(request->data = (uint8_t *)
 
       
  1444                     kmalloc(complete_size + 1, GFP_ATOMIC))) {
 
       
  1445             EC_ERR("Failed to allocate %i bytes of SDO data!\n",
 
       
  1446                     complete_size);
 
       
  1447             fsm->state = ec_fsm_coe_error;
 
       
  1448             return;
 
       
  1449         }
 
       
  1450         request->data[complete_size] = 0x00; // just to be sure...
 
       
  1451 
  1397         memcpy(request->data, data + 10, data_size);
 
  1452         memcpy(request->data, data + 10, data_size);
 
  1398         request->size = data_size;
 
  1453         request->size = complete_size;
 
  1399         fsm->toggle = 0;
 
  1454         fsm->toggle = 0;
 
  1400 
  1455 
  1401         if (data_size < complete_size) {
 
  1456         if (data_size < complete_size) {
 
  1402             EC_WARN("SDO data incomplete (%i / %i).\n",
 
  1457             EC_WARN("SDO data incomplete (%i / %i).\n",
 
  1403                     data_size, complete_size);
 
  1458                     data_size, complete_size);
etherlabmaster