303 |
303 |
304 // get first request |
304 // get first request |
305 request = list_entry(master->sii_requests.next, |
305 request = list_entry(master->sii_requests.next, |
306 ec_sii_write_request_t, list); |
306 ec_sii_write_request_t, list); |
307 list_del_init(&request->list); // dequeue |
307 list_del_init(&request->list); // dequeue |
308 request->state = EC_REQUEST_BUSY; |
308 request->state = EC_INT_REQUEST_BUSY; |
309 |
309 |
310 // found pending SII write operation. execute it! |
310 // found pending SII write operation. execute it! |
311 if (master->debug_level) |
311 if (master->debug_level) |
312 EC_DBG("Writing SII data to slave %u...\n", |
312 EC_DBG("Writing SII data to slave %u...\n", |
313 request->slave->ring_position); |
313 request->slave->ring_position); |
343 |
343 |
344 // get first request |
344 // get first request |
345 request = list_entry(master->phy_requests.next, |
345 request = list_entry(master->phy_requests.next, |
346 ec_phy_request_t, list); |
346 ec_phy_request_t, list); |
347 list_del_init(&request->list); // dequeue |
347 list_del_init(&request->list); // dequeue |
348 request->state = EC_REQUEST_BUSY; |
348 request->state = EC_INT_REQUEST_BUSY; |
349 |
349 |
350 // found pending request; process it! |
350 // found pending request; process it! |
351 if (master->debug_level) |
351 if (master->debug_level) |
352 EC_DBG("Processing phy request for slave %u...\n", |
352 EC_DBG("Processing phy request for slave %u...\n", |
353 request->slave->ring_position); |
353 request->slave->ring_position); |
358 request->offset, request->length); |
358 request->offset, request->length); |
359 } else { |
359 } else { |
360 if (request->length > fsm->datagram->mem_size) { |
360 if (request->length > fsm->datagram->mem_size) { |
361 EC_ERR("Request length (%u) exceeds maximum datagram size (%u)!\n", |
361 EC_ERR("Request length (%u) exceeds maximum datagram size (%u)!\n", |
362 request->length, fsm->datagram->mem_size); |
362 request->length, fsm->datagram->mem_size); |
363 request->state = EC_REQUEST_FAILURE; |
363 request->state = EC_INT_REQUEST_FAILURE; |
364 wake_up(&master->phy_queue); |
364 wake_up(&master->phy_queue); |
365 continue; |
365 continue; |
366 } |
366 } |
367 ec_datagram_fpwr(fsm->datagram, request->slave->station_address, |
367 ec_datagram_fpwr(fsm->datagram, request->slave->station_address, |
368 request->offset, request->length); |
368 request->offset, request->length); |
396 slave < master->slaves + master->slave_count; |
396 slave < master->slaves + master->slave_count; |
397 slave++) { |
397 slave++) { |
398 if (!slave->config) |
398 if (!slave->config) |
399 continue; |
399 continue; |
400 list_for_each_entry(req, &slave->config->sdo_requests, list) { |
400 list_for_each_entry(req, &slave->config->sdo_requests, list) { |
401 if (req->state == EC_REQUEST_QUEUED) { |
401 if (req->state == EC_INT_REQUEST_QUEUED) { |
402 |
402 |
403 if (ec_sdo_request_timed_out(req)) { |
403 if (ec_sdo_request_timed_out(req)) { |
404 req->state = EC_REQUEST_FAILURE; |
404 req->state = EC_INT_REQUEST_FAILURE; |
405 if (master->debug_level) |
405 if (master->debug_level) |
406 EC_DBG("Sdo request for slave %u timed out...\n", |
406 EC_DBG("Sdo request for slave %u timed out...\n", |
407 slave->ring_position); |
407 slave->ring_position); |
408 continue; |
408 continue; |
409 } |
409 } |
410 |
410 |
411 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
411 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
412 req->state = EC_REQUEST_FAILURE; |
412 req->state = EC_INT_REQUEST_FAILURE; |
413 continue; |
413 continue; |
414 } |
414 } |
415 |
415 |
416 req->state = EC_REQUEST_BUSY; |
416 req->state = EC_INT_REQUEST_BUSY; |
417 if (master->debug_level) |
417 if (master->debug_level) |
418 EC_DBG("Processing Sdo request for slave %u...\n", |
418 EC_DBG("Processing Sdo request for slave %u...\n", |
419 slave->ring_position); |
419 slave->ring_position); |
420 |
420 |
421 fsm->idle = 0; |
421 fsm->idle = 0; |
436 |
436 |
437 // get first request |
437 // get first request |
438 request = list_entry(master->slave_sdo_requests.next, |
438 request = list_entry(master->slave_sdo_requests.next, |
439 ec_master_sdo_request_t, list); |
439 ec_master_sdo_request_t, list); |
440 list_del_init(&request->list); // dequeue |
440 list_del_init(&request->list); // dequeue |
441 request->req.state = EC_REQUEST_BUSY; |
441 request->req.state = EC_INT_REQUEST_BUSY; |
442 |
442 |
443 slave = request->slave; |
443 slave = request->slave; |
444 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
444 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
445 EC_ERR("Discarding Sdo request, slave %u is in INIT.\n", |
445 EC_ERR("Discarding Sdo request, slave %u is in INIT.\n", |
446 slave->ring_position); |
446 slave->ring_position); |
447 request->req.state = EC_REQUEST_FAILURE; |
447 request->req.state = EC_INT_REQUEST_FAILURE; |
448 wake_up(&master->sdo_queue); |
448 wake_up(&master->sdo_queue); |
449 continue; |
449 continue; |
450 } |
450 } |
451 |
451 |
452 // Found pending Sdo request. Execute it! |
452 // Found pending Sdo request. Execute it! |
810 if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; |
810 if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; |
811 |
811 |
812 if (!ec_fsm_sii_success(&fsm->fsm_sii)) { |
812 if (!ec_fsm_sii_success(&fsm->fsm_sii)) { |
813 EC_ERR("Failed to write SII data to slave %u.\n", |
813 EC_ERR("Failed to write SII data to slave %u.\n", |
814 slave->ring_position); |
814 slave->ring_position); |
815 request->state = EC_REQUEST_FAILURE; |
815 request->state = EC_INT_REQUEST_FAILURE; |
816 wake_up(&master->sii_queue); |
816 wake_up(&master->sii_queue); |
817 ec_fsm_master_restart(fsm); |
817 ec_fsm_master_restart(fsm); |
818 return; |
818 return; |
819 } |
819 } |
820 |
820 |
837 // alias was written |
837 // alias was written |
838 slave->sii.alias = EC_READ_U16(request->words + 4); |
838 slave->sii.alias = EC_READ_U16(request->words + 4); |
839 } |
839 } |
840 // TODO: Evaluate other SII contents! |
840 // TODO: Evaluate other SII contents! |
841 |
841 |
842 request->state = EC_REQUEST_SUCCESS; |
842 request->state = EC_INT_REQUEST_SUCCESS; |
843 wake_up(&master->sii_queue); |
843 wake_up(&master->sii_queue); |
844 |
844 |
845 // check for another SII write request |
845 // check for another SII write request |
846 if (ec_fsm_master_action_process_sii(fsm)) |
846 if (ec_fsm_master_action_process_sii(fsm)) |
847 return; // processing another request |
847 return; // processing another request |
896 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
896 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
897 |
897 |
898 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
898 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
899 EC_DBG("Failed to process Sdo request for slave %u.\n", |
899 EC_DBG("Failed to process Sdo request for slave %u.\n", |
900 fsm->slave->ring_position); |
900 fsm->slave->ring_position); |
901 request->state = EC_REQUEST_FAILURE; |
901 request->state = EC_INT_REQUEST_FAILURE; |
902 wake_up(&master->sdo_queue); |
902 wake_up(&master->sdo_queue); |
903 ec_fsm_master_restart(fsm); |
903 ec_fsm_master_restart(fsm); |
904 return; |
904 return; |
905 } |
905 } |
906 |
906 |
907 // Sdo request finished |
907 // Sdo request finished |
908 request->state = EC_REQUEST_SUCCESS; |
908 request->state = EC_INT_REQUEST_SUCCESS; |
909 wake_up(&master->sdo_queue); |
909 wake_up(&master->sdo_queue); |
910 |
910 |
911 if (master->debug_level) |
911 if (master->debug_level) |
912 EC_DBG("Finished Sdo request for slave %u.\n", |
912 EC_DBG("Finished Sdo request for slave %u.\n", |
913 fsm->slave->ring_position); |
913 fsm->slave->ring_position); |
932 ec_phy_request_t *request = fsm->phy_request; |
932 ec_phy_request_t *request = fsm->phy_request; |
933 |
933 |
934 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
934 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
935 EC_ERR("Failed to receive phy request datagram (state %u).\n", |
935 EC_ERR("Failed to receive phy request datagram (state %u).\n", |
936 datagram->state); |
936 datagram->state); |
937 request->state = EC_REQUEST_FAILURE; |
937 request->state = EC_INT_REQUEST_FAILURE; |
938 wake_up(&master->phy_queue); |
938 wake_up(&master->phy_queue); |
939 ec_fsm_master_restart(fsm); |
939 ec_fsm_master_restart(fsm); |
940 return; |
940 return; |
941 } |
941 } |
942 |
942 |
945 kfree(request->data); |
945 kfree(request->data); |
946 request->data = kmalloc(request->length, GFP_KERNEL); |
946 request->data = kmalloc(request->length, GFP_KERNEL); |
947 if (!request->data) { |
947 if (!request->data) { |
948 EC_ERR("Failed to allocate %u bytes of memory for phy request.\n", |
948 EC_ERR("Failed to allocate %u bytes of memory for phy request.\n", |
949 request->length); |
949 request->length); |
950 request->state = EC_REQUEST_FAILURE; |
950 request->state = EC_INT_REQUEST_FAILURE; |
951 wake_up(&master->phy_queue); |
951 wake_up(&master->phy_queue); |
952 ec_fsm_master_restart(fsm); |
952 ec_fsm_master_restart(fsm); |
953 return; |
953 return; |
954 } |
954 } |
955 memcpy(request->data, datagram->data, request->length); |
955 memcpy(request->data, datagram->data, request->length); |
956 } |
956 } |
957 |
957 |
958 request->state = EC_REQUEST_SUCCESS; |
958 request->state = EC_INT_REQUEST_SUCCESS; |
959 wake_up(&master->phy_queue); |
959 wake_up(&master->phy_queue); |
960 |
960 |
961 // check for another PHY request |
961 // check for another PHY request |
962 if (ec_fsm_master_action_process_phy(fsm)) |
962 if (ec_fsm_master_action_process_phy(fsm)) |
963 return; // processing another request |
963 return; // processing another request |