master/fsm_coe.c
changeset 926 6bb33c6ec770
parent 895 5c2125247dde
child 962 ea83a7aab57a
equal deleted inserted replaced
925:939f3e9bba1e 926:6bb33c6ec770
   878     }
   878     }
   879 
   879 
   880     if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
   880     if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information
   881         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
   881         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response
   882         EC_ERR("Sdo information error response at slave %u while"
   882         EC_ERR("Sdo information error response at slave %u while"
   883                " fetching Sdo entry 0x%04X:%i!\n", slave->ring_position,
   883                " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position,
   884                sdo->index, fsm->subindex);
   884                sdo->index, fsm->subindex);
   885         ec_canopen_abort_msg(EC_READ_U32(data + 6));
   885         ec_canopen_abort_msg(EC_READ_U32(data + 6));
   886         fsm->state = ec_fsm_coe_error;
   886         fsm->state = ec_fsm_coe_error;
   887         return;
   887         return;
   888     }
   888     }
   890     if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information
   890     if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information
   891         (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response
   891         (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response
   892         EC_READ_U16(data + 6) != sdo->index || // Sdo index
   892         EC_READ_U16(data + 6) != sdo->index || // Sdo index
   893         EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex
   893         EC_READ_U8(data + 8) != fsm->subindex) { // Sdo subindex
   894         EC_ERR("Invalid entry description response at slave %u while"
   894         EC_ERR("Invalid entry description response at slave %u while"
   895                " fetching Sdo entry 0x%04X:%u!\n", slave->ring_position,
   895                " fetching Sdo entry 0x%04X:%02X!\n", slave->ring_position,
   896                sdo->index, fsm->subindex);
   896                sdo->index, fsm->subindex);
   897         ec_print_data(data, rec_size);
   897         ec_print_data(data, rec_size);
   898         fsm->state = ec_fsm_coe_error;
   898         fsm->state = ec_fsm_coe_error;
   899         return;
   899         return;
   900     }
   900     }
   996     ec_slave_t *slave = fsm->slave;
   996     ec_slave_t *slave = fsm->slave;
   997     ec_sdo_request_t *request = fsm->request;
   997     ec_sdo_request_t *request = fsm->request;
   998     uint8_t *data;
   998     uint8_t *data;
   999 
   999 
  1000     if (fsm->slave->master->debug_level) {
  1000     if (fsm->slave->master->debug_level) {
  1001         EC_DBG("Downloading Sdo 0x%04X:%u to slave %u.\n",
  1001         EC_DBG("Downloading Sdo 0x%04X:%02X to slave %u.\n",
  1002                request->index, request->subindex, slave->ring_position);
  1002                request->index, request->subindex, slave->ring_position);
  1003         ec_print_data(request->data, request->data_size);
  1003         ec_print_data(request->data, request->data_size);
  1004     }
  1004     }
  1005 
  1005 
  1006     if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
  1006     if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
  1183     }
  1183     }
  1184 
  1184 
  1185     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1185     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1186         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1186         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1187         fsm->state = ec_fsm_coe_error;
  1187         fsm->state = ec_fsm_coe_error;
  1188         EC_ERR("Sdo download 0x%04X:%X (%u bytes) aborted on slave %u.\n",
  1188         EC_ERR("Sdo download 0x%04X:%02X (%u bytes) aborted on slave %u.\n",
  1189                request->index, request->subindex, request->data_size,
  1189                request->index, request->subindex, request->data_size,
  1190                slave->ring_position);
  1190                slave->ring_position);
  1191         if (rec_size < 10) {
  1191         if (rec_size < 10) {
  1192             EC_ERR("Incomplete Abort command:\n");
  1192             EC_ERR("Incomplete Abort command:\n");
  1193             ec_print_data(data, rec_size);
  1193             ec_print_data(data, rec_size);
  1200     if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1200     if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1201         EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
  1201         EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
  1202         EC_READ_U16(data + 3) != request->index || // index
  1202         EC_READ_U16(data + 3) != request->index || // index
  1203         EC_READ_U8 (data + 5) != request->subindex) { // subindex
  1203         EC_READ_U8 (data + 5) != request->subindex) { // subindex
  1204         fsm->state = ec_fsm_coe_error;
  1204         fsm->state = ec_fsm_coe_error;
  1205         EC_ERR("Sdo download 0x%04X:%X (%u bytes) failed:\n",
  1205         EC_ERR("Sdo download 0x%04X:%02X (%u bytes) failed:\n",
  1206                request->index, request->subindex, request->data_size);
  1206                request->index, request->subindex, request->data_size);
  1207         EC_ERR("Invalid Sdo download response at slave %u!\n",
  1207         EC_ERR("Invalid Sdo download response at slave %u!\n",
  1208                slave->ring_position);
  1208                slave->ring_position);
  1209         ec_print_data(data, rec_size);
  1209         ec_print_data(data, rec_size);
  1210         return;
  1210         return;
  1226     ec_master_t *master = slave->master;
  1226     ec_master_t *master = slave->master;
  1227     ec_sdo_request_t *request = fsm->request;
  1227     ec_sdo_request_t *request = fsm->request;
  1228     uint8_t *data;
  1228     uint8_t *data;
  1229 
  1229 
  1230     if (master->debug_level)
  1230     if (master->debug_level)
  1231         EC_DBG("Uploading Sdo 0x%04X:%u from slave %u.\n",
  1231         EC_DBG("Uploading Sdo 0x%04X:%02X from slave %u.\n",
  1232                request->index, request->subindex, slave->ring_position);
  1232                request->index, request->subindex, slave->ring_position);
  1233 
  1233 
  1234     if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
  1234     if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
  1235         EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
  1235         EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
  1236         fsm->state = ec_fsm_coe_error;
  1236         fsm->state = ec_fsm_coe_error;
  1414         return;
  1414         return;
  1415     }
  1415     }
  1416 
  1416 
  1417     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1417     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1418         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1418         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1419         EC_ERR("Sdo upload 0x%04X:%X aborted on slave %u.\n",
  1419         EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n",
  1420                request->index, request->subindex, slave->ring_position);
  1420                request->index, request->subindex, slave->ring_position);
  1421         if (rec_size >= 10)
  1421         if (rec_size >= 10)
  1422             ec_canopen_abort_msg(EC_READ_U32(data + 6));
  1422             ec_canopen_abort_msg(EC_READ_U32(data + 6));
  1423         else
  1423         else
  1424             EC_ERR("No abort message.\n");
  1424             EC_ERR("No abort message.\n");
  1441         if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1441         if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1442                 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
  1442                 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
  1443                 EC_READ_U16(data + 3) != request->index || // index
  1443                 EC_READ_U16(data + 3) != request->index || // index
  1444                 EC_READ_U8 (data + 5) != request->subindex) { // subindex
  1444                 EC_READ_U8 (data + 5) != request->subindex) { // subindex
  1445             fsm->state = ec_fsm_coe_error;
  1445             fsm->state = ec_fsm_coe_error;
  1446             EC_ERR("Sdo upload 0x%04X:%X failed:\n",
  1446             EC_ERR("Sdo upload 0x%04X:%02X failed:\n",
  1447                     request->index, request->subindex);
  1447                     request->index, request->subindex);
  1448             EC_ERR("Invalid Sdo upload expedited response at slave %u!\n",
  1448             EC_ERR("Invalid Sdo upload expedited response at slave %u!\n",
  1449                     slave->ring_position);
  1449                     slave->ring_position);
  1450             ec_print_data(data, rec_size);
  1450             ec_print_data(data, rec_size);
  1451             return;
  1451             return;
  1482         if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1482         if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1483                 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
  1483                 EC_READ_U8 (data + 2) >> 5 != 0x2 || // upload response
  1484                 EC_READ_U16(data + 3) != request->index || // index
  1484                 EC_READ_U16(data + 3) != request->index || // index
  1485                 EC_READ_U8 (data + 5) != request->subindex) { // subindex
  1485                 EC_READ_U8 (data + 5) != request->subindex) { // subindex
  1486             fsm->state = ec_fsm_coe_error;
  1486             fsm->state = ec_fsm_coe_error;
  1487             EC_ERR("Sdo upload 0x%04X:%X failed:\n",
  1487             EC_ERR("Sdo upload 0x%04X:%02X failed:\n",
  1488                     request->index, request->subindex);
  1488                     request->index, request->subindex);
  1489             EC_ERR("Invalid Sdo normal upload response at slave %u!\n",
  1489             EC_ERR("Invalid Sdo normal upload response at slave %u!\n",
  1490                     slave->ring_position);
  1490                     slave->ring_position);
  1491             ec_print_data(data, rec_size);
  1491             ec_print_data(data, rec_size);
  1492             return;
  1492             return;
  1705         return;
  1705         return;
  1706     }
  1706     }
  1707 
  1707 
  1708     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1708     if (EC_READ_U16(data) >> 12 == 0x2 && // Sdo request
  1709         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1709         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort Sdo transfer request
  1710         EC_ERR("Sdo upload 0x%04X:%X aborted on slave %u.\n",
  1710         EC_ERR("Sdo upload 0x%04X:%02X aborted on slave %u.\n",
  1711                request->index, request->subindex, slave->ring_position);
  1711                request->index, request->subindex, slave->ring_position);
  1712         ec_canopen_abort_msg(EC_READ_U32(data + 6));
  1712         ec_canopen_abort_msg(EC_READ_U32(data + 6));
  1713         fsm->state = ec_fsm_coe_error;
  1713         fsm->state = ec_fsm_coe_error;
  1714         return;
  1714         return;
  1715     }
  1715     }
  1716 
  1716 
  1717     if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1717     if (EC_READ_U16(data) >> 12 != 0x3 || // Sdo response
  1718         EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response
  1718         EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response
  1719         EC_ERR("Sdo upload 0x%04X:%X failed:\n", request->index, request->subindex);
  1719         EC_ERR("Sdo upload 0x%04X:%02X failed:\n", request->index, request->subindex);
  1720         EC_ERR("Invalid Sdo upload segment response at slave %u!\n",
  1720         EC_ERR("Invalid Sdo upload segment response at slave %u!\n",
  1721                slave->ring_position);
  1721                slave->ring_position);
  1722         ec_print_data(data, rec_size);
  1722         ec_print_data(data, rec_size);
  1723         fsm->state = ec_fsm_coe_error;
  1723         fsm->state = ec_fsm_coe_error;
  1724         return;
  1724         return;