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 1606 6c5849669900
parent 1563 ddedb71b3a85
child 1791 27030c6b2de3
equal deleted inserted replaced
1605:1a588303083a 1606:6c5849669900 
    43 
    43 
    44 /** Maximum time in ms to wait for responses when reading out the dictionary.
 
    44 /** Maximum time in ms to wait for responses when reading out the dictionary.
 
    45  */
 
    45  */
 
    46 #define EC_FSM_COE_DICT_TIMEOUT 3000
 
    46 #define EC_FSM_COE_DICT_TIMEOUT 3000
 
    47 
    47 
       
    48 #define EC_COE_DOWN_REQ_HEADER_SIZE      10
 
       
    49 #define EC_COE_DOWN_SEG_REQ_HEADER_SIZE  3
 
       
    50 #define EC_COE_DOWN_SEG_MIN_DATA_SIZE    7
 
       
    51 
    48 /*****************************************************************************/
 
    52 /*****************************************************************************/
 
    49 
    53 
    50 void ec_fsm_coe_dict_start(ec_fsm_coe_t *);
 
    54 void ec_fsm_coe_dict_start(ec_fsm_coe_t *);
 
    51 void ec_fsm_coe_dict_request(ec_fsm_coe_t *);
 
    55 void ec_fsm_coe_dict_request(ec_fsm_coe_t *);
 
    52 void ec_fsm_coe_dict_check(ec_fsm_coe_t *);
 
    56 void ec_fsm_coe_dict_check(ec_fsm_coe_t *);
 
    60 
    64 
    61 void ec_fsm_coe_down_start(ec_fsm_coe_t *);
 
    65 void ec_fsm_coe_down_start(ec_fsm_coe_t *);
 
    62 void ec_fsm_coe_down_request(ec_fsm_coe_t *);
 
    66 void ec_fsm_coe_down_request(ec_fsm_coe_t *);
 
    63 void ec_fsm_coe_down_check(ec_fsm_coe_t *);
 
    67 void ec_fsm_coe_down_check(ec_fsm_coe_t *);
 
    64 void ec_fsm_coe_down_response(ec_fsm_coe_t *);
 
    68 void ec_fsm_coe_down_response(ec_fsm_coe_t *);
 
       
    69 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *);
 
       
    70 void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *);
 
    65 
    71 
    66 void ec_fsm_coe_up_start(ec_fsm_coe_t *);
 
    72 void ec_fsm_coe_up_start(ec_fsm_coe_t *);
 
    67 void ec_fsm_coe_up_request(ec_fsm_coe_t *);
 
    73 void ec_fsm_coe_up_request(ec_fsm_coe_t *);
 
    68 void ec_fsm_coe_up_check(ec_fsm_coe_t *);
 
    74 void ec_fsm_coe_up_check(ec_fsm_coe_t *);
 
    69 void ec_fsm_coe_up_response(ec_fsm_coe_t *);
 
    75 void ec_fsm_coe_up_response(ec_fsm_coe_t *);
 
   444         fsm->state = ec_fsm_coe_dict_check;
 
   450         fsm->state = ec_fsm_coe_dict_check;
 
   445         return;
 
   451         return;
 
   446     }
 
   452     }
 
   447 
   453 
   448     if (rec_size < 3) {
 
   454     if (rec_size < 3) {
 
   449         EC_ERR("Received corrupted SDO dictionary response (size %u).\n",
 
   455         EC_ERR("Received corrupted SDO dictionary response (size %zu).\n",
 
   450                 rec_size);
 
   456                 rec_size);
 
   451         fsm->state = ec_fsm_coe_error;
 
   457         fsm->state = ec_fsm_coe_error;
 
   452         return;
 
   458         return;
 
   453     }
 
   459     }
 
   454 
   460 
   478         fsm->state = ec_fsm_coe_dict_check;
 
   484         fsm->state = ec_fsm_coe_dict_check;
 
   479         return;
 
   485         return;
 
   480     }
 
   486     }
 
   481 
   487 
   482     if (rec_size < 8 || rec_size % 2) {
 
   488     if (rec_size < 8 || rec_size % 2) {
 
   483         EC_ERR("Invalid data size %u!\n", rec_size);
 
   489         EC_ERR("Invalid data size %zu!\n", rec_size);
 
   484         ec_print_data(data, rec_size);
 
   490         ec_print_data(data, rec_size);
 
   485         fsm->state = ec_fsm_coe_error;
 
   491         fsm->state = ec_fsm_coe_error;
 
   486         return;
 
   492         return;
 
   487     }
 
   493     }
 
   488 
   494 
   688         fsm->state = ec_fsm_coe_dict_desc_check;
 
   694         fsm->state = ec_fsm_coe_dict_desc_check;
 
   689         return;
 
   695         return;
 
   690     }
 
   696     }
 
   691 
   697 
   692     if (rec_size < 3) {
 
   698     if (rec_size < 3) {
 
   693         EC_ERR("Received corrupted SDO description response (size %u).\n",
 
   699         EC_ERR("Received corrupted SDO description response (size %zu).\n",
 
   694                 rec_size);
 
   700                 rec_size);
 
   695         fsm->state = ec_fsm_coe_error;
 
   701         fsm->state = ec_fsm_coe_error;
 
   696         return;
 
   702         return;
 
   697     }
 
   703     }
 
   698 
   704 
   705         fsm->state = ec_fsm_coe_error;
 
   711         fsm->state = ec_fsm_coe_error;
 
   706         return;
 
   712         return;
 
   707     }
 
   713     }
 
   708 
   714 
   709     if (rec_size < 8) {
 
   715     if (rec_size < 8) {
 
   710         EC_ERR("Received corrupted SDO description response (size %u).\n",
 
   716         EC_ERR("Received corrupted SDO description response (size %zu).\n",
 
   711                 rec_size);
 
   717                 rec_size);
 
   712         fsm->state = ec_fsm_coe_error;
 
   718         fsm->state = ec_fsm_coe_error;
 
   713         return;
 
   719         return;
 
   714     }
 
   720     }
 
   715 
   721 
   927         return;
 
   933         return;
 
   928     }
 
   934     }
 
   929 
   935 
   930     if (rec_size < 3) {
 
   936     if (rec_size < 3) {
 
   931         EC_ERR("Received corrupted SDO entry description response "
 
   937         EC_ERR("Received corrupted SDO entry description response "
 
   932                 "(size %u).\n", rec_size);
 
   938                 "(size %zu).\n", rec_size);
 
   933         fsm->state = ec_fsm_coe_error;
 
   939         fsm->state = ec_fsm_coe_error;
 
   934         return;
 
   940         return;
 
   935     }
 
   941     }
 
   936 
   942 
   937     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   943     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information
 
   944         return;
 
   950         return;
 
   945     }
 
   951     }
 
   946 
   952 
   947     if (rec_size < 9) {
 
   953     if (rec_size < 9) {
 
   948         EC_ERR("Received corrupted SDO entry description response "
 
   954         EC_ERR("Received corrupted SDO entry description response "
 
   949                 "(size %u).\n", rec_size);
 
   955                 "(size %zu).\n", rec_size);
 
   950         fsm->state = ec_fsm_coe_error;
 
   956         fsm->state = ec_fsm_coe_error;
 
   951         return;
 
   957         return;
 
   952     }
 
   958     }
 
   953 
   959 
   954     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
 
   960     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information
 
   967         fsm->state = ec_fsm_coe_dict_entry_check;
 
   973         fsm->state = ec_fsm_coe_dict_entry_check;
 
   968         return;
 
   974         return;
 
   969     }
 
   975     }
 
   970 
   976 
   971     if (rec_size < 16) {
 
   977     if (rec_size < 16) {
 
   972         EC_ERR("Invalid data size %u!\n", rec_size);
 
   978         EC_ERR("Invalid data size %zu!\n", rec_size);
 
   973         ec_print_data(data, rec_size);
 
   979         ec_print_data(data, rec_size);
 
   974         fsm->state = ec_fsm_coe_error;
 
   980         fsm->state = ec_fsm_coe_error;
 
   975         return;
 
   981         return;
 
   976     }
 
   982     }
 
   977 
   983 
  1059 
  1065 
  1060 /******************************************************************************
 
  1066 /******************************************************************************
 
  1061  *  CoE state machine
 
  1067  *  CoE state machine
 
  1062  *****************************************************************************/
 
  1068  *****************************************************************************/
 
  1063 
  1069 
  1064 /**
 
  1070 /** CoE state: DOWN START.
 
  1065    CoE state: DOWN START.
 
  1071  */
 
  1066 */
 
  1072 void ec_fsm_coe_down_start(
 
  1067 
  1073         ec_fsm_coe_t *fsm /**< finite state machine */
 
  1068 void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)
 
  1074         )
 
  1069 {
 
  1075 {
 
  1070     ec_datagram_t *datagram = fsm->datagram;
 
  1076     ec_datagram_t *datagram = fsm->datagram;
 
  1071     ec_slave_t *slave = fsm->slave;
 
  1077     ec_slave_t *slave = fsm->slave;
 
  1072     ec_sdo_request_t *request = fsm->request;
 
  1078     ec_sdo_request_t *request = fsm->request;
 
  1073     uint8_t *data;
 
  1079     uint8_t *data;
 
  1074     uint8_t size;
 
  1080     uint8_t data_set_size;
 
  1075 
  1081 
  1076     if (fsm->slave->master->debug_level) {
 
  1082     if (fsm->slave->master->debug_level) {
 
  1077         char subidxstr[10];
 
  1083         char subidxstr[10];
 
  1078         if (request->complete_access) {
 
  1084         if (request->complete_access) {
 
  1079             subidxstr[0] = 0x00;
 
  1085             subidxstr[0] = 0x00;
 
  1089         EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
 
  1095         EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
 
  1090         fsm->state = ec_fsm_coe_error;
 
  1096         fsm->state = ec_fsm_coe_error;
 
  1091         return;
 
  1097         return;
 
  1092     }
 
  1098     }
 
  1093 
  1099 
       
  1100     if (slave->configured_rx_mailbox_size < 
       
  1101             EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) {
 
       
  1102         EC_ERR("Mailbox too small!\n");
 
       
  1103         fsm->state = ec_fsm_coe_error;
 
       
  1104         return;
 
       
  1105     }
 
       
  1106 
  1094     if (request->data_size <= 4) { // use expedited transfer type
 
  1107     if (request->data_size <= 4) { // use expedited transfer type
 
  1095         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10);
 
  1108         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
 
       
  1109                 EC_COE_DOWN_REQ_HEADER_SIZE);
 
  1096         if (IS_ERR(data)) {
 
  1110         if (IS_ERR(data)) {
 
  1097             fsm->state = ec_fsm_coe_error;
 
  1111             fsm->state = ec_fsm_coe_error;
 
  1098             return;
 
  1112             return;
 
  1099         }
 
  1113         }
 
  1100 
  1114 
  1101         size = 4 - request->data_size;
 
  1115         fsm->remaining = 0;
 
       
  1116 
       
  1117         data_set_size = 4 - request->data_size;
 
  1102 
  1118 
  1103         EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
  1119         EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
  1104         EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited
 
  1120         EC_WRITE_U8 (data + 2, (0x3 // size specified, expedited
 
  1105                     | size << 2
 
  1121                     | data_set_size << 2
 
  1106                     | ((request->complete_access ? 1 : 0) << 4) 
  1122                     | ((request->complete_access ? 1 : 0) << 4) 
  1107                     | 0x1 << 5)); // Download request
 
  1123                     | 0x1 << 5)); // Download request
 
  1108         EC_WRITE_U16(data + 3, request->index);
 
  1124         EC_WRITE_U16(data + 3, request->index);
 
  1109         EC_WRITE_U8 (data + 5,
 
  1125         EC_WRITE_U8 (data + 5,
 
  1110                 request->complete_access ? 0x00 : request->subindex);
 
  1126                 request->complete_access ? 0x00 : request->subindex);
 
  1111         memcpy(data + 6, request->data, request->data_size);
 
  1127         memcpy(data + 6, request->data, request->data_size);
 
  1112         memset(data + 6 + request->data_size, 0x00, 4 - request->data_size);
 
  1128         memset(data + 6 + request->data_size, 0x00, 4 - request->data_size);
 
  1113 
  1129 
  1114         if (slave->master->debug_level) {
 
  1130         if (slave->master->debug_level) {
 
  1115             EC_DBG("Expedited download request:\n");
 
  1131             EC_DBG("Expedited download request:\n");
 
  1116             ec_print_data(data, 10);
 
  1132             ec_print_data(data, EC_COE_DOWN_REQ_HEADER_SIZE);
 
  1117         }
 
  1133         }
 
  1118     }
 
  1134     }
 
  1119     else { // request->data_size > 4, use normal transfer type
 
  1135     else { // request->data_size > 4, use normal transfer type
 
  1120         if (slave->configured_rx_mailbox_size < 6 + 10 + request->data_size) {
 
  1136         size_t data_size,
 
  1121             EC_ERR("SDO fragmenting not supported yet!\n"); // FIXME
 
  1137                max_data_size =
 
  1122             fsm->state = ec_fsm_coe_error;
 
  1138                    slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE,
 
  1123             return;
 
  1139                required_data_size =
 
       
  1140                    EC_COE_DOWN_REQ_HEADER_SIZE + request->data_size;
 
       
  1141 
       
  1142         if (max_data_size < required_data_size) {
 
       
  1143             // segmenting needed
 
       
  1144             data_size = max_data_size;
 
       
  1145         } else {
 
       
  1146             data_size = required_data_size;
 
  1124         }
 
  1147         }
 
  1125 
  1148 
  1126         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
 
  1149         data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
 
  1127                 request->data_size + 10);
 
  1150                 data_size);
 
  1128         if (IS_ERR(data)) {
 
  1151         if (IS_ERR(data)) {
 
  1129             fsm->state = ec_fsm_coe_error;
 
  1152             fsm->state = ec_fsm_coe_error;
 
  1130             return;
 
  1153             return;
 
  1131         }
 
  1154         }
 
  1132 
  1155 
       
  1156         fsm->offset = 0;
 
       
  1157         fsm->remaining = request->data_size;
 
       
  1158 
  1133         EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
  1159         EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
  1134         EC_WRITE_U8 (data + 2, (0x1 // size indicator, normal
 
  1160         EC_WRITE_U8(data + 2,
 
  1135                     | ((request->complete_access ? 1 : 0) << 4) 
  1161                 0x1 // size indicator, normal
 
  1136                     | 0x1 << 5)); // Download request
 
  1162                 | ((request->complete_access ? 1 : 0) << 4) 
       
  1163                 | 0x1 << 5); // Download request
 
  1137         EC_WRITE_U16(data + 3, request->index);
 
  1164         EC_WRITE_U16(data + 3, request->index);
 
  1138         EC_WRITE_U8 (data + 5,
 
  1165         EC_WRITE_U8 (data + 5,
 
  1139                 request->complete_access ? 0x00 : request->subindex);
 
  1166                 request->complete_access ? 0x00 : request->subindex);
 
  1140         EC_WRITE_U32(data + 6, request->data_size);
 
  1167         EC_WRITE_U32(data + 6, request->data_size);
 
  1141         memcpy(data + 10, request->data, request->data_size);
 
  1168 
       
  1169         if (data_size > EC_COE_DOWN_REQ_HEADER_SIZE) {
 
       
  1170             size_t segment_size = data_size - EC_COE_DOWN_REQ_HEADER_SIZE;
 
       
  1171             memcpy(data + EC_COE_DOWN_REQ_HEADER_SIZE,
 
       
  1172                     request->data, segment_size);
 
       
  1173             fsm->offset += segment_size;
 
       
  1174             fsm->remaining -= segment_size;
 
       
  1175         }
 
  1142 
  1176 
  1143         if (slave->master->debug_level) {
 
  1177         if (slave->master->debug_level) {
 
  1144             EC_DBG("Normal download request:\n");
 
  1178             EC_DBG("Normal download request:\n");
 
  1145             ec_print_data(data, 10 + request->data_size);
 
  1179             ec_print_data(data, data_size);
 
  1146         }
 
  1180         }
 
  1147     }
 
  1181     }
 
  1148 
  1182 
  1149     fsm->request->jiffies_sent = jiffies;
 
  1183     fsm->request->jiffies_sent = jiffies;
 
  1150     fsm->retries = EC_FSM_RETRIES;
 
  1184     fsm->retries = EC_FSM_RETRIES;
 
  1253     fsm->state = ec_fsm_coe_down_response;
 
  1287     fsm->state = ec_fsm_coe_down_response;
 
  1254 }
 
  1288 }
 
  1255 
  1289 
  1256 /*****************************************************************************/
 
  1290 /*****************************************************************************/
 
  1257 
  1291 
       
  1292 void ec_fsm_coe_down_prepare_segment_request(
 
       
  1293         ec_fsm_coe_t *fsm /**< finite state machine */
 
       
  1294         )
 
       
  1295 {
 
       
  1296     ec_datagram_t *datagram = fsm->datagram;
 
       
  1297     ec_slave_t *slave = fsm->slave;
 
       
  1298     ec_sdo_request_t *request = fsm->request;
 
       
  1299     size_t max_segment_size =
 
       
  1300         slave->configured_rx_mailbox_size
 
       
  1301         - EC_MBOX_HEADER_SIZE
 
       
  1302         - EC_COE_DOWN_SEG_REQ_HEADER_SIZE;
 
       
  1303     size_t segment_size, data_size;
 
       
  1304     uint8_t last_segment, seg_data_size, *data;
 
       
  1305 
       
  1306     if (fsm->remaining > max_segment_size) {
 
       
  1307         segment_size = max_segment_size;
 
       
  1308         last_segment = 0;
 
       
  1309     } else {
 
       
  1310         segment_size = fsm->remaining;
 
       
  1311         last_segment = 1;
 
       
  1312     }
 
       
  1313 
       
  1314     if (segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
 
       
  1315         seg_data_size = 0x00;
 
       
  1316         data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size;
 
       
  1317     } else {
 
       
  1318         seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size;
 
       
  1319         data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE
 
       
  1320             + EC_COE_DOWN_SEG_MIN_DATA_SIZE;
 
       
  1321     }
 
       
  1322 
       
  1323     data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
 
       
  1324             data_size);
 
       
  1325     if (IS_ERR(data)) {
 
       
  1326         fsm->state = ec_fsm_coe_error;
 
       
  1327         return;
 
       
  1328     }
 
       
  1329 
       
  1330     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
       
  1331     EC_WRITE_U8(data + 2, (last_segment ? 1 : 0)
 
       
  1332             | (seg_data_size << 1) 
       
  1333             | (fsm->toggle << 4)
 
       
  1334             | (0x00 << 5)); // Download segment request
 
       
  1335     memcpy(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE,
 
       
  1336             request->data + fsm->offset, segment_size);
 
       
  1337     if (segment_size < EC_COE_DOWN_SEG_MIN_DATA_SIZE) {
 
       
  1338         memset(data + EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size, 0x00,
 
       
  1339                 EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size);
 
       
  1340     }
 
       
  1341 
       
  1342     fsm->offset += segment_size;
 
       
  1343     fsm->remaining -= segment_size;
 
       
  1344 
       
  1345     if (slave->master->debug_level) {
 
       
  1346         EC_DBG("Download segment request:\n");
 
       
  1347         ec_print_data(data, data_size);
 
       
  1348     }
 
       
  1349 
       
  1350     fsm->state = ec_fsm_coe_down_seg_check;
 
       
  1351 }
 
       
  1352 
       
  1353 /*****************************************************************************/
 
       
  1354 
  1258 /**
 
  1355 /**
 
  1259    CoE state: DOWN RESPONSE.
 
  1356    CoE state: DOWN RESPONSE.
 
  1260    \todo Timeout behavior
 
  1357    \todo Timeout behavior
 
  1261 */
 
  1358 */
 
  1262 
  1359 
  1312         ec_print_data(data, rec_size);
 
  1409         ec_print_data(data, rec_size);
 
  1313     }
 
  1410     }
 
  1314 
  1411 
  1315     if (rec_size < 6) {
 
  1412     if (rec_size < 6) {
 
  1316         fsm->state = ec_fsm_coe_error;
 
  1413         fsm->state = ec_fsm_coe_error;
 
  1317         EC_ERR("Received data are too small (%u bytes):\n", rec_size);
 
  1414         EC_ERR("Received data are too small (%zu bytes):\n", rec_size);
 
  1318         ec_print_data(data, rec_size);
 
  1415         ec_print_data(data, rec_size);
 
  1319         return;
 
  1416         return;
 
  1320     }
 
  1417     }
 
  1321 
  1418 
  1322     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
  1419     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
  1326         if (request->complete_access) {
 
  1423         if (request->complete_access) {
 
  1327             subidxstr[0] = 0x00;
 
  1424             subidxstr[0] = 0x00;
 
  1328         } else {
 
  1425         } else {
 
  1329             sprintf(subidxstr, ":%02X", request->subindex);
 
  1426             sprintf(subidxstr, ":%02X", request->subindex);
 
  1330         }
 
  1427         }
 
  1331         EC_ERR("SDO download 0x%04X%s (%u bytes) aborted on slave %u.\n",
 
  1428         EC_ERR("SDO download 0x%04X%s (%zu bytes) aborted on slave %u.\n",
 
  1332                 request->index, subidxstr, request->data_size,
 
  1429                 request->index, subidxstr, request->data_size,
 
  1333                 slave->ring_position);
 
  1430                 slave->ring_position);
 
  1334         if (rec_size < 10) {
 
  1431         if (rec_size < 10) {
 
  1335             EC_ERR("Incomplete abort command:\n");
 
  1432             EC_ERR("Incomplete abort command:\n");
 
  1336             ec_print_data(data, rec_size);
 
  1433             ec_print_data(data, rec_size);
 
  1355         fsm->retries = EC_FSM_RETRIES;
 
  1452         fsm->retries = EC_FSM_RETRIES;
 
  1356         fsm->state = ec_fsm_coe_down_check;
 
  1453         fsm->state = ec_fsm_coe_down_check;
 
  1357         return;
 
  1454         return;
 
  1358     }
 
  1455     }
 
  1359 
  1456 
  1360     fsm->state = ec_fsm_coe_end; // success
 
  1457     if (fsm->remaining) { // more segments to download
 
       
  1458         fsm->toggle = 0;
 
       
  1459         ec_fsm_coe_down_prepare_segment_request(fsm);
 
       
  1460     } else {
 
       
  1461         fsm->state = ec_fsm_coe_end; // success
 
       
  1462     }
 
       
  1463 }
 
       
  1464 
       
  1465 /*****************************************************************************/
 
       
  1466 
       
  1467 /**
 
       
  1468    CoE state: DOWN SEG CHECK.
 
       
  1469 */
 
       
  1470 
       
  1471 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)
 
       
  1472 {
 
       
  1473     ec_datagram_t *datagram = fsm->datagram;
 
       
  1474     ec_slave_t *slave = fsm->slave;
 
       
  1475 
       
  1476     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
       
  1477         return;
 
       
  1478 
       
  1479     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
       
  1480         fsm->state = ec_fsm_coe_error;
 
       
  1481         EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"
 
       
  1482                 " (datagram state %u).\n",
 
       
  1483                slave->ring_position, datagram->state);
 
       
  1484         return;
 
       
  1485     }
 
       
  1486 
       
  1487     if (datagram->working_counter != 1) {
 
       
  1488         fsm->state = ec_fsm_coe_error;
 
       
  1489         EC_ERR("Reception of CoE mailbox segment check"
 
       
  1490                 " datagram failed on slave %u: ", slave->ring_position);
 
       
  1491         ec_datagram_print_wc_error(datagram);
 
       
  1492         return;
 
       
  1493     }
 
       
  1494 
       
  1495     if (!ec_slave_mbox_check(datagram)) {
 
       
  1496         unsigned long diff_ms =
 
       
  1497             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
 
       
  1498         if (diff_ms >= fsm->request->response_timeout) {
 
       
  1499             fsm->state = ec_fsm_coe_error;
 
       
  1500             EC_ERR("Timeout while waiting for SDO download segment response "
 
       
  1501                     "on slave %u.\n", slave->ring_position);
 
       
  1502             return;
 
       
  1503         }
 
       
  1504 
       
  1505         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  1506         fsm->retries = EC_FSM_RETRIES;
 
       
  1507         return;
 
       
  1508     }
 
       
  1509 
       
  1510     // Fetch response
 
       
  1511     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
 
       
  1512     fsm->retries = EC_FSM_RETRIES;
 
       
  1513     fsm->state = ec_fsm_coe_down_seg_response;
 
       
  1514 }
 
       
  1515 
       
  1516 /*****************************************************************************/
 
       
  1517 
       
  1518 /**
 
       
  1519    CoE state: DOWN SEG RESPONSE.
 
       
  1520    \todo Timeout behavior
 
       
  1521 */
 
       
  1522 
       
  1523 void ec_fsm_coe_down_seg_response(
 
       
  1524         ec_fsm_coe_t *fsm /**< finite state machine */
 
       
  1525         )
 
       
  1526 {
 
       
  1527     ec_datagram_t *datagram = fsm->datagram;
 
       
  1528     ec_slave_t *slave = fsm->slave;
 
       
  1529     uint8_t *data, mbox_prot;
 
       
  1530     size_t rec_size;
 
       
  1531     ec_sdo_request_t *request = fsm->request;
 
       
  1532 
       
  1533     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
       
  1534         return; // FIXME: request again?
 
       
  1535 
       
  1536     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
       
  1537         fsm->state = ec_fsm_coe_error;
 
       
  1538         EC_ERR("Failed to receive CoE download response datagram from"
 
       
  1539                " slave %u (datagram state %u).\n",
 
       
  1540                slave->ring_position, datagram->state);
 
       
  1541         return;
 
       
  1542     }
 
       
  1543 
       
  1544     if (datagram->working_counter != 1) {
 
       
  1545         fsm->state = ec_fsm_coe_error;
 
       
  1546         EC_ERR("Reception of CoE download response failed on slave %u: ",
 
       
  1547                 slave->ring_position);
 
       
  1548         ec_datagram_print_wc_error(datagram);
 
       
  1549         return;
 
       
  1550     }
 
       
  1551 
       
  1552     data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
 
       
  1553     if (IS_ERR(data)) {
 
       
  1554         fsm->state = ec_fsm_coe_error;
 
       
  1555         return;
 
       
  1556     }
 
       
  1557 
       
  1558     if (mbox_prot != 0x03) { // CoE
 
       
  1559         fsm->state = ec_fsm_coe_error;
 
       
  1560         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
       
  1561         return;
 
       
  1562     }
 
       
  1563 
       
  1564     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {
 
       
  1565         // check for CoE response again
 
       
  1566         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  1567         fsm->retries = EC_FSM_RETRIES;
 
       
  1568         fsm->state = ec_fsm_coe_down_check;
 
       
  1569         return;
 
       
  1570     }
 
       
  1571 
       
  1572     if (slave->master->debug_level) {
 
       
  1573         EC_DBG("Download response:\n");
 
       
  1574         ec_print_data(data, rec_size);
 
       
  1575     }
 
       
  1576 
       
  1577     if (rec_size < 6) {
 
       
  1578         fsm->state = ec_fsm_coe_error;
 
       
  1579         EC_ERR("Received data are too small (%zu bytes):\n", rec_size);
 
       
  1580         ec_print_data(data, rec_size);
 
       
  1581         return;
 
       
  1582     }
 
       
  1583 
       
  1584     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
       
  1585         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
 
       
  1586         char subidxstr[10];
 
       
  1587         fsm->state = ec_fsm_coe_error;
 
       
  1588         if (request->complete_access) {
 
       
  1589             subidxstr[0] = 0x00;
 
       
  1590         } else {
 
       
  1591             sprintf(subidxstr, ":%02X", request->subindex);
 
       
  1592         }
 
       
  1593         EC_ERR("SDO download 0x%04X%s (%zu bytes) aborted on slave %u.\n",
 
       
  1594                 request->index, subidxstr, request->data_size,
 
       
  1595                 slave->ring_position);
 
       
  1596         if (rec_size < 10) {
 
       
  1597             EC_ERR("Incomplete abort command:\n");
 
       
  1598             ec_print_data(data, rec_size);
 
       
  1599         } else {
 
       
  1600             fsm->request->abort_code = EC_READ_U32(data + 6);
 
       
  1601             ec_canopen_abort_msg(fsm->request->abort_code);
 
       
  1602         }
 
       
  1603         return;
 
       
  1604     }
 
       
  1605 
       
  1606     if (EC_READ_U16(data) >> 12 != 0x3 ||
 
       
  1607             ((EC_READ_U8(data + 2) >> 5) != 0x01)) { // segment response
 
       
  1608         if (slave->master->debug_level) {
 
       
  1609             EC_DBG("Invalid SDO download response at slave %u! Retrying...\n",
 
       
  1610                     slave->ring_position);
 
       
  1611             ec_print_data(data, rec_size);
 
       
  1612         }
 
       
  1613         // check for CoE response again
 
       
  1614         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
       
  1615         fsm->retries = EC_FSM_RETRIES;
 
       
  1616         fsm->state = ec_fsm_coe_down_seg_check;
 
       
  1617         return;
 
       
  1618     }
 
       
  1619 
       
  1620     if (((EC_READ_U8(data + 2) >> 4) & 0x01) != fsm->toggle) {
 
       
  1621         EC_ERR("Invalid toggle received during segmented download:\n");
 
       
  1622         ec_print_data(data, rec_size);
 
       
  1623         fsm->state = ec_fsm_coe_error;
 
       
  1624         return;
 
       
  1625     }
 
       
  1626 
       
  1627     if (fsm->remaining) { // more segments to download
 
       
  1628         fsm->toggle = !fsm->toggle;
 
       
  1629         ec_fsm_coe_down_prepare_segment_request(fsm);
 
       
  1630     } else {
 
       
  1631         fsm->state = ec_fsm_coe_end; // success
 
       
  1632     }
 
  1361 }
 
  1633 }
 
  1362 
  1634 
  1363 /*****************************************************************************/
 
  1635 /*****************************************************************************/
 
  1364 
  1636 
  1365 /**
 
  1637 /**
 
  1524     }
 
  1796     }
 
  1525 
  1797 
  1526     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
  1798     EC_WRITE_U16(data, 0x2 << 12); // SDO request
 
  1527     EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
 
  1799     EC_WRITE_U8 (data + 2, (fsm->toggle << 4 // toggle
 
  1528                 | 0x3 << 5)); // upload segment request
 
  1800                 | 0x3 << 5)); // upload segment request
 
       
  1801     memset(data + 3, 0x00, 7);
 
  1529 
  1802 
  1530     if (fsm->slave->master->debug_level) {
 
  1803     if (fsm->slave->master->debug_level) {
 
  1531         EC_DBG("Upload segment request:\n");
 
  1804         EC_DBG("Upload segment request:\n");
 
  1532         ec_print_data(data, 10);
 
  1805         ec_print_data(data, 10);
 
  1533     }
 
  1806     }
 
  1595         return;
 
  1868         return;
 
  1596     }
 
  1869     }
 
  1597 
  1870 
  1598     if (rec_size < 6) {
 
  1871     if (rec_size < 6) {
 
  1599         fsm->state = ec_fsm_coe_error;
 
  1872         fsm->state = ec_fsm_coe_error;
 
  1600         EC_ERR("Received currupted SDO upload response (%u bytes)!\n", rec_size);
 
  1873         EC_ERR("Received currupted SDO upload response (%zu bytes)!\n", rec_size);
 
  1601         ec_print_data(data, rec_size);
 
  1874         ec_print_data(data, rec_size);
 
  1602         return;
 
  1875         return;
 
  1603     }
 
  1876     }
 
  1604 
  1877 
  1605     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
  1878     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
 
  1655         }
 
  1928         }
 
  1656 
  1929 
  1657         if (rec_size < 6 + fsm->complete_size) {
 
  1930         if (rec_size < 6 + fsm->complete_size) {
 
  1658             fsm->state = ec_fsm_coe_error;
 
  1931             fsm->state = ec_fsm_coe_error;
 
  1659             EC_ERR("Received currupted SDO expedited upload"
 
  1932             EC_ERR("Received currupted SDO expedited upload"
 
  1660                     " response (only %u bytes)!\n", rec_size);
 
  1933                     " response (only %zu bytes)!\n", rec_size);
 
  1661             ec_print_data(data, rec_size);
 
  1934             ec_print_data(data, rec_size);
 
  1662             return;
 
  1935             return;
 
  1663         }
 
  1936         }
 
  1664 
  1937 
  1665         if (ec_sdo_request_copy_data(request, data + 6, fsm->complete_size)) {
 
  1938         if (ec_sdo_request_copy_data(request, data + 6, fsm->complete_size)) {
 
  1668         }
 
  1941         }
 
  1669     } else { // normal
 
  1942     } else { // normal
 
  1670         if (rec_size < 10) {
 
  1943         if (rec_size < 10) {
 
  1671             fsm->state = ec_fsm_coe_error;
 
  1944             fsm->state = ec_fsm_coe_error;
 
  1672             EC_ERR("Received currupted SDO normal upload"
 
  1945             EC_ERR("Received currupted SDO normal upload"
 
  1673                     " response (only %u bytes)!\n", rec_size);
 
  1946                     " response (only %zu bytes)!\n", rec_size);
 
  1674             ec_print_data(data, rec_size);
 
  1947             ec_print_data(data, rec_size);
 
  1675             return;
 
  1948             return;
 
  1676         }
 
  1949         }
 
  1677 
  1950 
  1678         data_size = rec_size - 10;
 
  1951         data_size = rec_size - 10;
 
  1697 
  1970 
  1698         fsm->toggle = 0;
 
  1971         fsm->toggle = 0;
 
  1699 
  1972 
  1700         if (data_size < fsm->complete_size) {
 
  1973         if (data_size < fsm->complete_size) {
 
  1701             if (master->debug_level)
 
  1974             if (master->debug_level)
 
  1702                 EC_DBG("SDO data incomplete (%u / %u). Segmenting...\n",
 
  1975                 EC_DBG("SDO data incomplete (%zu / %u). Segmenting...\n",
 
  1703                         data_size, fsm->complete_size);
 
  1976                         data_size, fsm->complete_size);
 
  1704 
  1977 
  1705             ec_fsm_coe_up_prepare_segment_request(fsm);
 
  1978             ec_fsm_coe_up_prepare_segment_request(fsm);
 
  1706             fsm->retries = EC_FSM_RETRIES;
 
  1979             fsm->retries = EC_FSM_RETRIES;
 
  1707             fsm->state = ec_fsm_coe_up_seg_request;
 
  1980             fsm->state = ec_fsm_coe_up_seg_request;
 
  1925         return;
 
  2198         return;
 
  1926     }
 
  2199     }
 
  1927 
  2200 
  1928     if (request->data_size != fsm->complete_size) {
 
  2201     if (request->data_size != fsm->complete_size) {
 
  1929         EC_WARN("SDO upload 0x%04X:%02X on slave %u: Assembled data"
 
  2202         EC_WARN("SDO upload 0x%04X:%02X on slave %u: Assembled data"
 
  1930                 " size (%u) does not match complete size (%u)!\n",
 
  2203                 " size (%zu) does not match complete size (%u)!\n",
 
  1931                 request->index, request->subindex, slave->ring_position,
 
  2204                 request->index, request->subindex, slave->ring_position,
 
  1932                 request->data_size, fsm->complete_size);
 
  2205                 request->data_size, fsm->complete_size);
 
  1933     }
 
  2206     }
 
  1934 
  2207 
  1935     if (master->debug_level) {
 
  2208     if (master->debug_level) {
etherlabmaster