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 986 a486591ba86b
parent 975 dae04112bcc7
child 991 2548ca639b1f
equal deleted inserted replaced
985:90bcaf9ce3d5 986:a486591ba86b 
   302         return; // FIXME: request again?
 
   302         return; // FIXME: request again?
 
   303 
   303 
   304     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   304     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   305         fsm->state = ec_fsm_coe_error;
 
   305         fsm->state = ec_fsm_coe_error;
 
   306         EC_ERR("Failed to receive CoE dictionary request datagram for"
 
   306         EC_ERR("Failed to receive CoE dictionary request datagram for"
 
   307                " slave %u (datagram state %i).\n",
 
   307                " slave %u (datagram state %u).\n",
 
   308                slave->ring_position, datagram->state);
 
   308                slave->ring_position, datagram->state);
 
   309         return;
 
   309         return;
 
   310     }
 
   310     }
 
   311 
   311 
   312     if (datagram->working_counter != 1) {
 
   312     if (datagram->working_counter != 1) {
 
   339         return;
 
   339         return;
 
   340 
   340 
   341     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   341     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   342         fsm->state = ec_fsm_coe_error;
 
   342         fsm->state = ec_fsm_coe_error;
 
   343         EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
 
   343         EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
 
   344                 " (datagram state %i).\n",
 
   344                 " (datagram state %u).\n",
 
   345                slave->ring_position, datagram->state);
 
   345                slave->ring_position, datagram->state);
 
   346         return;
 
   346         return;
 
   347     }
 
   347     }
 
   348 
   348 
   349     if (datagram->working_counter != 1) {
 
   349     if (datagram->working_counter != 1) {
 
   395         return; // FIXME: request again?
 
   395         return; // FIXME: request again?
 
   396 
   396 
   397     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   397     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   398         fsm->state = ec_fsm_coe_error;
 
   398         fsm->state = ec_fsm_coe_error;
 
   399         EC_ERR("Failed to receive CoE dictionary response datagram for"
 
   399         EC_ERR("Failed to receive CoE dictionary response datagram for"
 
   400                " slave %u (datagram state %i).\n",
 
   400                " slave %u (datagram state %u).\n",
 
   401                slave->ring_position, datagram->state);
 
   401                slave->ring_position, datagram->state);
 
   402         return;
 
   402         return;
 
   403     }
 
   403     }
 
   404 
   404 
   405     if (datagram->working_counter != 1) {
 
   405     if (datagram->working_counter != 1) {
 
   534         return; // FIXME: check for response first?
 
   534         return; // FIXME: check for response first?
 
   535 
   535 
   536     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   536     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   537         fsm->state = ec_fsm_coe_error;
 
   537         fsm->state = ec_fsm_coe_error;
 
   538         EC_ERR("Failed to receive CoE Sdo description request datagram for"
 
   538         EC_ERR("Failed to receive CoE Sdo description request datagram for"
 
   539                " slave %u (datagram state %i).\n",
 
   539                " slave %u (datagram state %u).\n",
 
   540                slave->ring_position, datagram->state);
 
   540                slave->ring_position, datagram->state);
 
   541         return;
 
   541         return;
 
   542     }
 
   542     }
 
   543 
   543 
   544     if (datagram->working_counter != 1) {
 
   544     if (datagram->working_counter != 1) {
 
   571         return;
 
   571         return;
 
   572 
   572 
   573     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   573     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   574         fsm->state = ec_fsm_coe_error;
 
   574         fsm->state = ec_fsm_coe_error;
 
   575         EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
 
   575         EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
 
   576                 " (datagram state %i).\n",
 
   576                 " (datagram state %u).\n",
 
   577                 slave->ring_position, datagram->state);
 
   577                 slave->ring_position, datagram->state);
 
   578         return;
 
   578         return;
 
   579     }
 
   579     }
 
   580 
   580 
   581     if (datagram->working_counter != 1) {
 
   581     if (datagram->working_counter != 1) {
 
   626         return; // FIXME: request again?
 
   626         return; // FIXME: request again?
 
   627 
   627 
   628     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   628     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   629         fsm->state = ec_fsm_coe_error;
 
   629         fsm->state = ec_fsm_coe_error;
 
   630         EC_ERR("Failed to receive CoE Sdo description response datagram from"
 
   630         EC_ERR("Failed to receive CoE Sdo description response datagram from"
 
   631                " slave %u (datagram state %i).\n",
 
   631                " slave %u (datagram state %u).\n",
 
   632                slave->ring_position, datagram->state);
 
   632                slave->ring_position, datagram->state);
 
   633         return;
 
   633         return;
 
   634     }
 
   634     }
 
   635 
   635 
   636     if (datagram->working_counter != 1) {
 
   636     if (datagram->working_counter != 1) {
 
   748         return; // FIXME: check for response first?
 
   748         return; // FIXME: check for response first?
 
   749 
   749 
   750     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   750     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   751         fsm->state = ec_fsm_coe_error;
 
   751         fsm->state = ec_fsm_coe_error;
 
   752         EC_ERR("Failed to receive CoE Sdo entry request datagram for"
 
   752         EC_ERR("Failed to receive CoE Sdo entry request datagram for"
 
   753                " slave %u (datagram state %i).\n",
 
   753                " slave %u (datagram state %u).\n",
 
   754                slave->ring_position, datagram->state);
 
   754                slave->ring_position, datagram->state);
 
   755         return;
 
   755         return;
 
   756     }
 
   756     }
 
   757 
   757 
   758     if (datagram->working_counter != 1) {
 
   758     if (datagram->working_counter != 1) {
 
   786         return;
 
   786         return;
 
   787 
   787 
   788     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   788     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   789         fsm->state = ec_fsm_coe_error;
 
   789         fsm->state = ec_fsm_coe_error;
 
   790         EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
 
   790         EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
 
   791                 " (datagram state %i).\n",
 
   791                 " (datagram state %u).\n",
 
   792                slave->ring_position, datagram->state);
 
   792                slave->ring_position, datagram->state);
 
   793         return;
 
   793         return;
 
   794     }
 
   794     }
 
   795 
   795 
   796     if (datagram->working_counter != 1) {
 
   796     if (datagram->working_counter != 1) {
 
   842         return; // FIXME: request again?
 
   842         return; // FIXME: request again?
 
   843 
   843 
   844     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   844     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   845         fsm->state = ec_fsm_coe_error;
 
   845         fsm->state = ec_fsm_coe_error;
 
   846         EC_ERR("Failed to receive CoE Sdo description response datagram from"
 
   846         EC_ERR("Failed to receive CoE Sdo description response datagram from"
 
   847                " slave %u (datagram state %i).\n",
 
   847                " slave %u (datagram state %u).\n",
 
   848                slave->ring_position, datagram->state);
 
   848                slave->ring_position, datagram->state);
 
   849         return;
 
   849         return;
 
   850     }
 
   850     }
 
   851 
   851 
   852     if (datagram->working_counter != 1) {
 
   852     if (datagram->working_counter != 1) {
 
  1054         return; // FIXME: check for response first?
 
  1054         return; // FIXME: check for response first?
 
  1055 
  1055 
  1056     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1056     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1057         fsm->state = ec_fsm_coe_error;
 
  1057         fsm->state = ec_fsm_coe_error;
 
  1058         EC_ERR("Failed to receive CoE download request datagram for"
 
  1058         EC_ERR("Failed to receive CoE download request datagram for"
 
  1059                " slave %u (datagram state %i).\n",
 
  1059                " slave %u (datagram state %u).\n",
 
  1060                slave->ring_position, datagram->state);
 
  1060                slave->ring_position, datagram->state);
 
  1061         return;
 
  1061         return;
 
  1062     }
 
  1062     }
 
  1063 
  1063 
  1064     if (datagram->working_counter != 1) {
 
  1064     if (datagram->working_counter != 1) {
 
  1091         return;
 
  1091         return;
 
  1092 
  1092 
  1093     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1093     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1094         fsm->state = ec_fsm_coe_error;
 
  1094         fsm->state = ec_fsm_coe_error;
 
  1095         EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
 
  1095         EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
 
  1096                 " (datagram state %i).\n",
 
  1096                 " (datagram state %u).\n",
 
  1097                slave->ring_position, datagram->state);
 
  1097                slave->ring_position, datagram->state);
 
  1098         return;
 
  1098         return;
 
  1099     }
 
  1099     }
 
  1100 
  1100 
  1101     if (datagram->working_counter != 1) {
 
  1101     if (datagram->working_counter != 1) {
 
  1145         return; // FIXME: request again?
 
  1145         return; // FIXME: request again?
 
  1146 
  1146 
  1147     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1147     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1148         fsm->state = ec_fsm_coe_error;
 
  1148         fsm->state = ec_fsm_coe_error;
 
  1149         EC_ERR("Failed to receive CoE download response datagram from"
 
  1149         EC_ERR("Failed to receive CoE download response datagram from"
 
  1150                " slave %u (datagram state %i).\n",
 
  1150                " slave %u (datagram state %u).\n",
 
  1151                slave->ring_position, datagram->state);
 
  1151                slave->ring_position, datagram->state);
 
  1152         return;
 
  1152         return;
 
  1153     }
 
  1153     }
 
  1154 
  1154 
  1155     if (datagram->working_counter != 1) {
 
  1155     if (datagram->working_counter != 1) {
 
  1283         return; // FIXME: check for response first?
 
  1283         return; // FIXME: check for response first?
 
  1284 
  1284 
  1285     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1285     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1286         fsm->state = ec_fsm_coe_error;
 
  1286         fsm->state = ec_fsm_coe_error;
 
  1287         EC_ERR("Failed to receive CoE upload request for slave %u"
 
  1287         EC_ERR("Failed to receive CoE upload request for slave %u"
 
  1288                 " (datagram state %i).\n",
 
  1288                 " (datagram state %u).\n",
 
  1289                slave->ring_position, datagram->state);
 
  1289                slave->ring_position, datagram->state);
 
  1290         return;
 
  1290         return;
 
  1291     }
 
  1291     }
 
  1292 
  1292 
  1293     if (datagram->working_counter != 1) {
 
  1293     if (datagram->working_counter != 1) {
 
  1320         return;
 
  1320         return;
 
  1321 
  1321 
  1322     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1322     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1323         fsm->state = ec_fsm_coe_error;
 
  1323         fsm->state = ec_fsm_coe_error;
 
  1324         EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
 
  1324         EC_ERR("Failed to receive CoE mailbox check datagram from slave %u"
 
  1325                 " (datagram state %i).\n",
 
  1325                 " (datagram state %u).\n",
 
  1326                slave->ring_position, datagram->state);
 
  1326                slave->ring_position, datagram->state);
 
  1327         return;
 
  1327         return;
 
  1328     }
 
  1328     }
 
  1329 
  1329 
  1330     if (datagram->working_counter != 1) {
 
  1330     if (datagram->working_counter != 1) {
 
  1377         return; // FIXME: request again?
 
  1377         return; // FIXME: request again?
 
  1378 
  1378 
  1379     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1379     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1380         fsm->state = ec_fsm_coe_error;
 
  1380         fsm->state = ec_fsm_coe_error;
 
  1381         EC_ERR("Failed to receive CoE upload response datagram for"
 
  1381         EC_ERR("Failed to receive CoE upload response datagram for"
 
  1382                " slave %u (datagram state %i).\n",
 
  1382                " slave %u (datagram state %u).\n",
 
  1383                slave->ring_position, datagram->state);
 
  1383                slave->ring_position, datagram->state);
 
  1384         return;
 
  1384         return;
 
  1385     }
 
  1385     }
 
  1386 
  1386 
  1387     if (datagram->working_counter != 1) {
 
  1387     if (datagram->working_counter != 1) {
 
  1523         }
 
  1523         }
 
  1524 
  1524 
  1525         fsm->toggle = 0;
 
  1525         fsm->toggle = 0;
 
  1526 
  1526 
  1527         if (data_size < complete_size) {
 
  1527         if (data_size < complete_size) {
 
  1528             EC_WARN("Sdo data incomplete (%i / %i).\n",
 
  1528             EC_WARN("Sdo data incomplete (%u / %u).\n",
 
  1529                     data_size, complete_size);
 
  1529                     data_size, complete_size);
 
  1530 
  1530 
  1531             if (!(data = ec_slave_mbox_prepare_send(slave, datagram,
 
  1531             if (!(data = ec_slave_mbox_prepare_send(slave, datagram,
 
  1532                                                     0x03, 3))) {
 
  1532                                                     0x03, 3))) {
 
  1533                 fsm->state = ec_fsm_coe_error;
 
  1533                 fsm->state = ec_fsm_coe_error;
 
  1573         return; // FIXME: check for response first?
 
  1573         return; // FIXME: check for response first?
 
  1574 
  1574 
  1575     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1575     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1576         fsm->state = ec_fsm_coe_error;
 
  1576         fsm->state = ec_fsm_coe_error;
 
  1577         EC_ERR("Failed to receive CoE upload segment request datagram for"
 
  1577         EC_ERR("Failed to receive CoE upload segment request datagram for"
 
  1578                " slave %u (datagram state %i).\n",
 
  1578                " slave %u (datagram state %u).\n",
 
  1579                slave->ring_position, datagram->state);
 
  1579                slave->ring_position, datagram->state);
 
  1580         return;
 
  1580         return;
 
  1581     }
 
  1581     }
 
  1582 
  1582 
  1583     if (datagram->working_counter != 1) {
 
  1583     if (datagram->working_counter != 1) {
 
  1610         return;
 
  1610         return;
 
  1611 
  1611 
  1612     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1612     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1613         fsm->state = ec_fsm_coe_error;
 
  1613         fsm->state = ec_fsm_coe_error;
 
  1614         EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
 
  1614         EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
 
  1615                 " (datagram state %i).\n",
 
  1615                 " (datagram state %u).\n",
 
  1616                slave->ring_position, datagram->state);
 
  1616                slave->ring_position, datagram->state);
 
  1617         return;
 
  1617         return;
 
  1618     }
 
  1618     }
 
  1619 
  1619 
  1620     if (datagram->working_counter != 1) {
 
  1620     if (datagram->working_counter != 1) {
 
  1668         return; // FIXME: request again?
 
  1668         return; // FIXME: request again?
 
  1669 
  1669 
  1670     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1670     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
  1671         fsm->state = ec_fsm_coe_error;
 
  1671         fsm->state = ec_fsm_coe_error;
 
  1672         EC_ERR("Failed to receive CoE upload segment response datagram for"
 
  1672         EC_ERR("Failed to receive CoE upload segment response datagram for"
 
  1673                " slave %u (datagram state %i).\n",
 
  1673                " slave %u (datagram state %u).\n",
 
  1674                slave->ring_position, datagram->state);
 
  1674                slave->ring_position, datagram->state);
 
  1675         return;
 
  1675         return;
 
  1676     }
 
  1676     }
 
  1677 
  1677 
  1678     if (datagram->working_counter != 1) {
 
  1678     if (datagram->working_counter != 1) {
 
  1737     last_segment = EC_READ_U8(data + 2) & 0x01;
 
  1737     last_segment = EC_READ_U8(data + 2) & 0x01;
 
  1738     seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;
 
  1738     seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;
 
  1739     data_size = rec_size - 10;
 
  1739     data_size = rec_size - 10;
 
  1740 
  1740 
  1741     if (data_size != seg_size) {
 
  1741     if (data_size != seg_size) {
 
  1742         EC_WARN("Sdo segment data invalid (%i / %i)"
 
  1742         EC_WARN("Sdo segment data invalid (%u / %u)"
 
  1743                 " - Fragmenting not implemented.\n",
 
  1743                 " - Fragmenting not implemented.\n",
 
  1744                 data_size, seg_size);
 
  1744                 data_size, seg_size);
 
  1745     }
 
  1745     }
 
  1746 
  1746 
  1747     memcpy(request->data + request->data_size, data + 10, data_size);
 
  1747     memcpy(request->data + request->data_size, data + 10, data_size);
etherlabmaster