1106 fsm->state = ec_fsm_coe_error; |
1106 fsm->state = ec_fsm_coe_error; |
1107 return; |
1107 return; |
1108 } |
1108 } |
1109 |
1109 |
1110 if (mbox_prot != 0x03) { // CoE |
1110 if (mbox_prot != 0x03) { // CoE |
|
1111 fsm->state = ec_fsm_coe_error; |
1111 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1112 EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1112 fsm->state = ec_fsm_coe_error; |
1113 return; |
1113 return; |
|
1114 } |
1114 } |
1115 |
1115 |
1116 if (rec_size < 6) { |
1116 if (rec_size < 6) { |
1117 fsm->state = ec_fsm_coe_error; |
1117 fsm->state = ec_fsm_coe_error; |
1118 EC_ERR("Received data is too small (%i bytes):\n", rec_size); |
1118 EC_ERR("Received data is too small (%u bytes):\n", rec_size); |
1119 ec_print_data(data, rec_size); |
1119 ec_print_data(data, rec_size); |
1120 return; |
1120 return; |
1121 } |
1121 } |
1122 |
1122 |
1123 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1123 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1330 EC_DBG("Upload response:\n"); |
1330 EC_DBG("Upload response:\n"); |
1331 ec_print_data(data, rec_size); |
1331 ec_print_data(data, rec_size); |
1332 } |
1332 } |
1333 |
1333 |
1334 if (mbox_prot != 0x03) { // CoE |
1334 if (mbox_prot != 0x03) { // CoE |
|
1335 fsm->state = ec_fsm_coe_error; |
1335 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1336 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
1336 fsm->state = ec_fsm_coe_error; |
|
1337 return; |
1337 return; |
1338 } |
1338 } |
1339 |
1339 |
1340 if (rec_size < 3) { |
1340 if (rec_size < 3) { |
|
1341 fsm->state = ec_fsm_coe_error; |
1341 EC_ERR("Received currupted Sdo upload response (%u bytes)!\n", rec_size); |
1342 EC_ERR("Received currupted Sdo upload response (%u bytes)!\n", rec_size); |
1342 ec_print_data(data, rec_size); |
1343 ec_print_data(data, rec_size); |
1343 fsm->state = ec_fsm_coe_error; |
|
1344 return; |
1344 return; |
1345 } |
1345 } |
1346 |
1346 |
1347 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1347 if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request |
1348 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1348 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request |
1359 // normal or expedited? |
1359 // normal or expedited? |
1360 expedited = EC_READ_U8(data + 2) & 0x02; |
1360 expedited = EC_READ_U8(data + 2) & 0x02; |
1361 |
1361 |
1362 if (expedited) { |
1362 if (expedited) { |
1363 if (rec_size < 7) { |
1363 if (rec_size < 7) { |
|
1364 fsm->state = ec_fsm_coe_error; |
1364 EC_ERR("Received currupted Sdo expedited upload" |
1365 EC_ERR("Received currupted Sdo expedited upload" |
1365 " response (only %u bytes)!\n", rec_size); |
1366 " response (only %u bytes)!\n", rec_size); |
1366 ec_print_data(data, rec_size); |
1367 ec_print_data(data, rec_size); |
1367 fsm->state = ec_fsm_coe_error; |
|
1368 return; |
1368 return; |
1369 } |
1369 } |
1370 |
1370 |
1371 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1371 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1372 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1372 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1373 EC_READ_U16(data + 3) != request->index || // index |
1373 EC_READ_U16(data + 3) != request->index || // index |
1374 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1374 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
|
1375 fsm->state = ec_fsm_coe_error; |
1375 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1376 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1376 request->index, request->subindex); |
1377 request->index, request->subindex); |
1377 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1378 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1378 slave->ring_position); |
1379 slave->ring_position); |
1379 ec_print_data(data, rec_size); |
1380 ec_print_data(data, rec_size); |
1380 fsm->state = ec_fsm_coe_error; |
|
1381 return; |
1381 return; |
1382 } |
1382 } |
1383 |
1383 |
1384 size_specified = EC_READ_U8(data + 2) & 0x01; |
1384 size_specified = EC_READ_U8(data + 2) & 0x01; |
1385 if (size_specified) { |
1385 if (size_specified) { |
1387 } else { |
1387 } else { |
1388 complete_size = 4; |
1388 complete_size = 4; |
1389 } |
1389 } |
1390 |
1390 |
1391 if (rec_size < 6 + complete_size) { |
1391 if (rec_size < 6 + complete_size) { |
|
1392 fsm->state = ec_fsm_coe_error; |
1392 EC_ERR("Received currupted Sdo expedited upload" |
1393 EC_ERR("Received currupted Sdo expedited upload" |
1393 " response (only %u bytes)!\n", rec_size); |
1394 " response (only %u bytes)!\n", rec_size); |
1394 ec_print_data(data, rec_size); |
1395 ec_print_data(data, rec_size); |
1395 fsm->state = ec_fsm_coe_error; |
|
1396 return; |
1396 return; |
1397 } |
1397 } |
1398 |
1398 |
1399 if (ec_sdo_request_copy_data(request, data + 6, complete_size)) { |
1399 if (ec_sdo_request_copy_data(request, data + 6, complete_size)) { |
1400 fsm->state = ec_fsm_coe_error; |
1400 fsm->state = ec_fsm_coe_error; |
1401 return; |
1401 return; |
1402 } |
1402 } |
1403 } else { // normal |
1403 } else { // normal |
1404 if (rec_size < 10) { |
1404 if (rec_size < 10) { |
|
1405 fsm->state = ec_fsm_coe_error; |
1405 EC_ERR("Received currupted Sdo normal upload" |
1406 EC_ERR("Received currupted Sdo normal upload" |
1406 " response (only %u bytes)!\n", rec_size); |
1407 " response (only %u bytes)!\n", rec_size); |
1407 ec_print_data(data, rec_size); |
1408 ec_print_data(data, rec_size); |
1408 fsm->state = ec_fsm_coe_error; |
|
1409 return; |
1409 return; |
1410 } |
1410 } |
1411 |
1411 |
1412 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1412 if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response |
1413 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1413 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response |
1414 EC_READ_U16(data + 3) != request->index || // index |
1414 EC_READ_U16(data + 3) != request->index || // index |
1415 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
1415 EC_READ_U8 (data + 5) != request->subindex) { // subindex |
|
1416 fsm->state = ec_fsm_coe_error; |
1416 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1417 EC_ERR("Sdo upload 0x%04X:%X failed:\n", |
1417 request->index, request->subindex); |
1418 request->index, request->subindex); |
1418 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1419 EC_ERR("Invalid Sdo upload response at slave %i!\n", |
1419 slave->ring_position); |
1420 slave->ring_position); |
1420 ec_print_data(data, rec_size); |
1421 ec_print_data(data, rec_size); |
1421 fsm->state = ec_fsm_coe_error; |
|
1422 return; |
1422 return; |
1423 } |
1423 } |
1424 |
1424 |
1425 data_size = rec_size - 10; |
1425 data_size = rec_size - 10; |
1426 complete_size = EC_READ_U32(data + 6); |
1426 complete_size = EC_READ_U32(data + 6); |
1427 |
1427 |
1428 if (!complete_size) { |
1428 if (!complete_size) { |
|
1429 fsm->state = ec_fsm_coe_error; |
1429 EC_ERR("No complete size supplied!\n"); |
1430 EC_ERR("No complete size supplied!\n"); |
1430 ec_print_data(data, rec_size); |
1431 ec_print_data(data, rec_size); |
1431 fsm->state = ec_fsm_coe_error; |
|
1432 return; |
1432 return; |
1433 } |
1433 } |
1434 |
1434 |
1435 if (ec_sdo_request_alloc(request, complete_size)) { |
1435 if (ec_sdo_request_alloc(request, complete_size)) { |
1436 fsm->state = ec_fsm_coe_error; |
1436 fsm->state = ec_fsm_coe_error; |