1 /****************************************************************************** |
1 /****************************************************************************** |
2 * |
2 * |
3 * $Id$ |
3 * $Id$ |
4 * |
4 * |
5 * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH |
5 * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH |
6 * |
6 * |
7 * This file is part of the IgH EtherCAT Master. |
7 * This file is part of the IgH EtherCAT Master. |
8 * |
8 * |
9 * The IgH EtherCAT Master is free software; you can redistribute it and/or |
9 * The IgH EtherCAT Master is free software; you can redistribute it and/or |
10 * modify it under the terms of the GNU General Public License version 2, as |
10 * modify it under the terms of the GNU General Public License version 2, as |
63 void ec_fsm_master_state_dc_read_offset(ec_fsm_master_t *); |
63 void ec_fsm_master_state_dc_read_offset(ec_fsm_master_t *); |
64 void ec_fsm_master_state_dc_write_offset(ec_fsm_master_t *); |
64 void ec_fsm_master_state_dc_write_offset(ec_fsm_master_t *); |
65 void ec_fsm_master_state_write_sii(ec_fsm_master_t *); |
65 void ec_fsm_master_state_write_sii(ec_fsm_master_t *); |
66 void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); |
66 void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); |
67 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); |
67 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); |
68 void ec_fsm_master_state_reg_request(ec_fsm_master_t *); |
|
69 |
68 |
70 void ec_fsm_master_enter_clear_addresses(ec_fsm_master_t *); |
69 void ec_fsm_master_enter_clear_addresses(ec_fsm_master_t *); |
71 void ec_fsm_master_enter_write_system_times(ec_fsm_master_t *); |
70 void ec_fsm_master_enter_write_system_times(ec_fsm_master_t *); |
72 |
71 |
73 /*****************************************************************************/ |
72 /*****************************************************************************/ |
402 return 0; |
401 return 0; |
403 } |
402 } |
404 |
403 |
405 /*****************************************************************************/ |
404 /*****************************************************************************/ |
406 |
405 |
407 /** Check for pending register requests and process one. |
|
408 * |
|
409 * \return non-zero, if a register request is processed. |
|
410 */ |
|
411 int ec_fsm_master_action_process_register( |
|
412 ec_fsm_master_t *fsm /**< Master state machine. */ |
|
413 ) |
|
414 { |
|
415 ec_master_t *master = fsm->master; |
|
416 ec_reg_request_t *request; |
|
417 |
|
418 // search the first request to be processed |
|
419 while (!list_empty(&master->reg_requests)) { |
|
420 |
|
421 // get first request |
|
422 request = list_entry(master->reg_requests.next, |
|
423 ec_reg_request_t, list); |
|
424 list_del_init(&request->list); // dequeue |
|
425 request->state = EC_INT_REQUEST_BUSY; |
|
426 |
|
427 // found pending request; process it! |
|
428 EC_SLAVE_DBG(request->slave, 1, "Processing register request, " |
|
429 "offset 0x%04x, length %zu...\n", |
|
430 request->offset, request->length); |
|
431 |
|
432 if (request->length > fsm->datagram->mem_size) { |
|
433 EC_MASTER_ERR(master, "Request length (%zu) exceeds maximum " |
|
434 "datagram size (%zu)!\n", request->length, |
|
435 fsm->datagram->mem_size); |
|
436 request->state = EC_INT_REQUEST_FAILURE; |
|
437 wake_up(&master->reg_queue); |
|
438 continue; |
|
439 } |
|
440 |
|
441 fsm->reg_request = request; |
|
442 |
|
443 if (request->dir == EC_DIR_INPUT) { |
|
444 ec_datagram_fprd(fsm->datagram, request->slave->station_address, |
|
445 request->offset, request->length); |
|
446 ec_datagram_zero(fsm->datagram); |
|
447 } else { |
|
448 ec_datagram_fpwr(fsm->datagram, request->slave->station_address, |
|
449 request->offset, request->length); |
|
450 memcpy(fsm->datagram->data, request->data, request->length); |
|
451 } |
|
452 fsm->datagram->device_index = request->slave->device_index; |
|
453 fsm->retries = EC_FSM_RETRIES; |
|
454 fsm->state = ec_fsm_master_state_reg_request; |
|
455 return 1; |
|
456 } |
|
457 |
|
458 return 0; |
|
459 } |
|
460 |
|
461 /*****************************************************************************/ |
|
462 |
|
463 /** Check for pending SDO requests and process one. |
406 /** Check for pending SDO requests and process one. |
464 * |
407 * |
465 * \return non-zero, if an SDO request is processed. |
408 * \return non-zero, if an SDO request is processed. |
466 */ |
409 */ |
467 int ec_fsm_master_action_process_sdo( |
410 int ec_fsm_master_action_process_sdo( |
561 fsm->datagram->device_index = fsm->slave->device_index; |
503 fsm->datagram->device_index = fsm->slave->device_index; |
562 return; |
504 return; |
563 } |
505 } |
564 |
506 |
565 // check for pending SII write operations. |
507 // check for pending SII write operations. |
566 if (ec_fsm_master_action_process_sii(fsm)) |
508 if (ec_fsm_master_action_process_sii(fsm)) { |
567 return; // SII write request found |
509 return; // SII write request found |
568 |
510 } |
569 // check for pending register requests. |
|
570 if (ec_fsm_master_action_process_register(fsm)) |
|
571 return; // register request processing |
|
572 |
511 |
573 ec_fsm_master_restart(fsm); |
512 ec_fsm_master_restart(fsm); |
574 } |
513 } |
575 |
514 |
576 /*****************************************************************************/ |
515 /*****************************************************************************/ |
1274 wake_up(&fsm->slave->sdo_queue); |
1213 wake_up(&fsm->slave->sdo_queue); |
1275 |
1214 |
1276 EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n"); |
1215 EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n"); |
1277 |
1216 |
1278 // check for another SDO request |
1217 // check for another SDO request |
1279 if (ec_fsm_master_action_process_sdo(fsm)) |
1218 if (ec_fsm_master_action_process_sdo(fsm)) { |
1280 return; // processing another request |
1219 return; // processing another request |
|
1220 } |
1281 |
1221 |
1282 ec_fsm_master_restart(fsm); |
1222 ec_fsm_master_restart(fsm); |
1283 } |
1223 } |
1284 |
1224 |
1285 /*****************************************************************************/ |
1225 /*****************************************************************************/ |
1286 |
|
1287 /** Master state: REG REQUEST. |
|
1288 */ |
|
1289 void ec_fsm_master_state_reg_request( |
|
1290 ec_fsm_master_t *fsm /**< Master state machine. */ |
|
1291 ) |
|
1292 { |
|
1293 ec_master_t *master = fsm->master; |
|
1294 ec_datagram_t *datagram = fsm->datagram; |
|
1295 ec_reg_request_t *request = fsm->reg_request; |
|
1296 |
|
1297 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1298 EC_MASTER_ERR(master, "Failed to receive register" |
|
1299 " request datagram: "); |
|
1300 ec_datagram_print_state(datagram); |
|
1301 request->state = EC_INT_REQUEST_FAILURE; |
|
1302 wake_up(&master->reg_queue); |
|
1303 ec_fsm_master_restart(fsm); |
|
1304 return; |
|
1305 } |
|
1306 |
|
1307 if (datagram->working_counter == 1) { |
|
1308 if (request->dir == EC_DIR_INPUT) { // read request |
|
1309 if (request->data) |
|
1310 kfree(request->data); |
|
1311 request->data = kmalloc(request->length, GFP_KERNEL); |
|
1312 if (!request->data) { |
|
1313 EC_MASTER_ERR(master, "Failed to allocate %zu bytes" |
|
1314 " of memory for register data.\n", request->length); |
|
1315 request->state = EC_INT_REQUEST_FAILURE; |
|
1316 wake_up(&master->reg_queue); |
|
1317 ec_fsm_master_restart(fsm); |
|
1318 return; |
|
1319 } |
|
1320 memcpy(request->data, datagram->data, request->length); |
|
1321 } |
|
1322 |
|
1323 request->state = EC_INT_REQUEST_SUCCESS; |
|
1324 EC_SLAVE_DBG(request->slave, 1, "Register request successful.\n"); |
|
1325 } else { |
|
1326 request->state = EC_INT_REQUEST_FAILURE; |
|
1327 EC_MASTER_ERR(master, "Register request failed.\n"); |
|
1328 } |
|
1329 |
|
1330 wake_up(&master->reg_queue); |
|
1331 |
|
1332 // check for another register request |
|
1333 if (ec_fsm_master_action_process_register(fsm)) |
|
1334 return; // processing another request |
|
1335 |
|
1336 ec_fsm_master_restart(fsm); |
|
1337 } |
|
1338 |
|
1339 /*****************************************************************************/ |
|