bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SDO configuration interface, SDO download state machine.

This commit is contained in:
Florian Pose 2006-08-03 16:48:53 +00:00
parent e15945b26b
commit 32d3bc8d0f
5 changed files with 425 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
examples/mini/mini.c
View File

@ -64,6 +64,7 @@ uint8_t k_stat;
ec_pdo_reg_t domain1_pdos[] = {
{"1", Beckhoff_EL4132_Output1, &r_ana_out},
{"8", Beckhoff_EL5001_Value, NULL},
{}
};
@ -123,6 +124,8 @@ void release_lock(void *data)
int __init init_mini_module(void)
{
ec_slave_t *slave;
printk(KERN_INFO "=== Starting Minimal EtherCAT environment... ===\n");
if ((master = ecrt_request_master(0)) == NULL) {
@ -145,6 +148,12 @@ int __init init_mini_module(void)
goto out_release_master;
}
if (!(slave = ecrt_master_get_slave(master, "8")))
goto out_release_master;
if (ecrt_slave_conf_sdo8(slave, 0x4061, 1, 0))
goto out_release_master;
printk(KERN_INFO "Activating master...\n");
if (ecrt_master_activate(master)) {
printk(KERN_ERR "Failed to activate master!\n");

292
master/fsm.c
View File

@ -41,6 +41,7 @@
#include "globals.h"
#include "fsm.h"
#include "master.h"
#include "mailbox.h"
/*****************************************************************************/
@ -73,6 +74,7 @@ void ec_fsm_slaveconf_init(ec_fsm_t *);
void ec_fsm_slaveconf_sync(ec_fsm_t *);
void ec_fsm_slaveconf_preop(ec_fsm_t *);
void ec_fsm_slaveconf_fmmu(ec_fsm_t *);
void ec_fsm_slaveconf_sdoconf(ec_fsm_t *);
void ec_fsm_slaveconf_saveop(ec_fsm_t *);
void ec_fsm_slaveconf_op(ec_fsm_t *);
@ -90,9 +92,16 @@ void ec_fsm_change_code(ec_fsm_t *);
void ec_fsm_change_ack(ec_fsm_t *);
void ec_fsm_change_check_ack(ec_fsm_t *);
void ec_fsm_coe_down_start(ec_fsm_t *);
void ec_fsm_coe_down_request(ec_fsm_t *);
void ec_fsm_coe_down_check(ec_fsm_t *);
void ec_fsm_coe_down_fetch(ec_fsm_t *);
void ec_fsm_end(ec_fsm_t *);
void ec_fsm_error(ec_fsm_t *);
void ec_canopen_abort_msg(uint32_t);
/*****************************************************************************/
/**
@ -1403,6 +1412,7 @@ void ec_fsm_slaveconf_preop(ec_fsm_t *fsm /**< finite state machine */)
void ec_fsm_slaveconf_fmmu(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
@ -1413,6 +1423,51 @@ void ec_fsm_slaveconf_fmmu(ec_fsm_t *fsm /**< finite state machine */)
return;
}
// No CoE configuration to be applied? Jump to SAVEOP state.
if (list_empty(&slave->sdo_confs)) {
// set state to SAVEOP
fsm->slave_state = ec_fsm_slaveconf_saveop;
fsm->change_new = EC_SLAVE_STATE_SAVEOP;
fsm->change_state = ec_fsm_change_start;
fsm->change_state(fsm); // execute immediately
return;
}
fsm->slave_state = ec_fsm_slaveconf_sdoconf;
fsm->sdodata = list_entry(slave->sdo_confs.next, ec_sdo_data_t, list);
fsm->coe_state = ec_fsm_coe_down_start;
fsm->coe_state(fsm); // execute immediately
}
/*****************************************************************************/
/**
Slave state: SDOCONF.
*/
void ec_fsm_slaveconf_sdoconf(ec_fsm_t *fsm /**< finite state machine */)
{
fsm->coe_state(fsm); // execute CoE state machine
if (fsm->coe_state == ec_fsm_error) {
fsm->slave->error_flag = 1;
fsm->slave_state = ec_fsm_error;
return;
}
if (fsm->coe_state != ec_fsm_end) return;
// Another SDO to configure?
if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) {
fsm->sdodata = list_entry(fsm->sdodata->list.next,
ec_sdo_data_t, list);
fsm->coe_state = ec_fsm_coe_down_start;
fsm->coe_state(fsm); // execute immediately
return;
}
// All SDOs are now configured.
// set state to SAVEOP
fsm->slave_state = ec_fsm_slaveconf_saveop;
fsm->change_new = EC_SLAVE_STATE_SAVEOP;
@ -1934,8 +1989,245 @@ void ec_fsm_change_check_ack(ec_fsm_t *fsm /**< finite state machine */)
ec_master_queue_datagram(fsm->master, datagram);
}
/******************************************************************************
* CoE state machine
*****************************************************************************/
/**
CoE state: DOWN_START.
*/
void ec_fsm_coe_down_start(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_sdo_data_t *sdodata = fsm->sdodata;
uint8_t *data;
EC_INFO("Downloading SDO 0x%04X:%i to slave %i.\n",
sdodata->index, sdodata->subindex, slave->ring_position);
if (slave->sii_rx_mailbox_size < 6 + 10 + sdodata->size) {
EC_ERR("SDO fragmenting not supported yet!\n");
fsm->coe_state = ec_fsm_error;
return;
}
if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,
sdodata->size + 10))) {
fsm->coe_state = ec_fsm_error;
return;
}
EC_WRITE_U16(data, 0x2 << 12); // SDO request
EC_WRITE_U8 (data + 2, (0x1 // size specified
| 0x1 << 5)); // Download request
EC_WRITE_U16(data + 3, sdodata->index);
EC_WRITE_U8 (data + 5, sdodata->subindex);
EC_WRITE_U32(data + 6, sdodata->size);
memcpy(data + 6, sdodata->data, sdodata->size);
ec_master_queue_datagram(fsm->master, datagram);
fsm->coe_state = ec_fsm_coe_down_request;
}
/*****************************************************************************/
/**
CoE state: DOWN_REQUEST.
*/
void ec_fsm_coe_down_request(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
fsm->coe_state = ec_fsm_error;
EC_ERR("Reception of CoE download request failed.\n");
return;
}
fsm->coe_start = get_cycles();
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->master, datagram);
fsm->coe_state = ec_fsm_coe_down_check;
}
/*****************************************************************************/
/**
CoE state: DOWN_CHECK.
*/
void ec_fsm_coe_down_check(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
fsm->coe_state = ec_fsm_error;
EC_ERR("Reception of CoE mailbox check datagram failed.\n");
return;
}
if (!ec_slave_mbox_check(datagram)) {
if (get_cycles() - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {
fsm->coe_state = ec_fsm_error;
EC_ERR("Timeout while checking SDO configuration on slave %i.\n",
slave->ring_position);
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->master, datagram);
return;
}
// Fetch response
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
ec_master_queue_datagram(fsm->master, datagram);
fsm->coe_state = ec_fsm_coe_down_fetch;
}
/*****************************************************************************/
/**
CoE state: DOWN_FETCH.
*/
void ec_fsm_coe_down_fetch(ec_fsm_t *fsm /**< finite state machine */)
{
ec_datagram_t *datagram = &fsm->datagram;
ec_slave_t *slave = fsm->slave;
uint8_t *data;
size_t rec_size;
ec_sdo_data_t *sdodata = fsm->sdodata;
if (datagram->state != EC_DATAGRAM_RECEIVED
|| datagram->working_counter != 1) {
fsm->coe_state = ec_fsm_error;
EC_ERR("Reception of CoE download response failed.\n");
return;
}
if (!(data = ec_slave_mbox_fetch(slave, datagram, 0x03, &rec_size))) {
fsm->coe_state = ec_fsm_error;
return;
}
if (rec_size < 6) {
fsm->coe_state = ec_fsm_error;
EC_ERR("Received data is too small (%i bytes):\n", rec_size);
ec_print_data(data, rec_size);
return;
}
if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request
EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request
fsm->coe_state = ec_fsm_error;
EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n",
sdodata->index, sdodata->subindex, sdodata->size,
slave->ring_position);
if (rec_size < 10) {
EC_ERR("Incomplete Abort command:\n");
ec_print_data(data, rec_size);
}
else
ec_canopen_abort_msg(EC_READ_U32(data + 6));
return;
}
if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response
EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response
EC_READ_U16(data + 3) != sdodata->index || // index
EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex
fsm->coe_state = ec_fsm_error;
EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n",
sdodata->index, sdodata->subindex, sdodata->size);
EC_ERR("Invalid SDO download response at slave %i!\n",
slave->ring_position);
ec_print_data(data, rec_size);
return;
}
fsm->coe_state = ec_fsm_end; // success
}
/*****************************************************************************/
/**
SDO abort messages.
The "abort SDO transfer request" supplies an abort code,
which can be translated to clear text. This table does
the mapping of the codes and messages.
*/
const ec_code_msg_t sdo_abort_messages[] = {
{0x05030000, "Toggle bit not changed"},
{0x05040000, "SDO protocol timeout"},
{0x05040001, "Client/Server command specifier not valid or unknown"},
{0x05040005, "Out of memory"},
{0x06010000, "Unsupported access to an object"},
{0x06010001, "Attempt to read a write-only object"},
{0x06010002, "Attempt to write a read-only object"},
{0x06020000, "This object does not exist in the object directory"},
{0x06040041, "The object cannot be mapped into the PDO"},
{0x06040042, "The number and length of the objects to be mapped would"
" exceed the PDO length"},
{0x06040043, "General parameter incompatibility reason"},
{0x06040047, "Gerneral internal incompatibility in device"},
{0x06060000, "Access failure due to a hardware error"},
{0x06070010, "Data type does not match, length of service parameter does"
" not match"},
{0x06070012, "Data type does not match, length of service parameter too"
" high"},
{0x06070013, "Data type does not match, length of service parameter too"
" low"},
{0x06090011, "Subindex does not exist"},
{0x06090030, "Value range of parameter exceeded"},
{0x06090031, "Value of parameter written too high"},
{0x06090032, "Value of parameter written too low"},
{0x06090036, "Maximum value is less than minimum value"},
{0x08000000, "General error"},
{0x08000020, "Data cannot be transferred or stored to the application"},
{0x08000021, "Data cannot be transferred or stored to the application"
" because of local control"},
{0x08000022, "Data cannot be transferred or stored to the application"
" because of the present device state"},
{0x08000023, "Object dictionary dynamic generation fails or no object"
" dictionary is present"},
{}
};
/*****************************************************************************/
/**
Outputs an SDO abort message.
*/
void ec_canopen_abort_msg(uint32_t abort_code)
{
const ec_code_msg_t *abort_msg;
for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {
if (abort_msg->code == abort_code) {
EC_ERR("SDO abort message 0x%08X: \"%s\".\n",
abort_msg->code, abort_msg->message);
return;
}
}
EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code);
}
/******************************************************************************
* Common state functions
*****************************************************************************/
/**
State: ERROR.
*/

4
master/fsm.h
View File

@ -76,6 +76,10 @@ struct ec_fsm
void (*change_state)(ec_fsm_t *); /**< slave state change state function */
uint8_t change_new; /**< input: new state */
cycles_t change_start; /**< change start */
void (*coe_state)(ec_fsm_t *); /**< CoE state function */
ec_sdo_data_t *sdodata; /**< input/output: SDO data object */
cycles_t coe_start; /**< CoE timestamp */
};
/*****************************************************************************/

107
master/slave.c
View File

@ -155,6 +155,7 @@ int ec_slave_init(ec_slave_t *slave, /**< EtherCAT slave */
INIT_LIST_HEAD(&slave->sii_syncs);
INIT_LIST_HEAD(&slave->sii_pdos);
INIT_LIST_HEAD(&slave->sdo_dictionary);
INIT_LIST_HEAD(&slave->sdo_confs);
INIT_LIST_HEAD(&slave->varsize_fields);
for (i = 0; i < 4; i++) {
@ -182,6 +183,7 @@ void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */)
ec_sii_pdo_entry_t *entry, *next_ent;
ec_sdo_t *sdo, *next_sdo;
ec_sdo_entry_t *en, *next_en;
ec_sdo_data_t *sdodata, *next_sdodata;
ec_varsize_t *var, *next_var;
slave = container_of(kobj, ec_slave_t, kobj);
@ -231,6 +233,13 @@ void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */)
kfree(sdo);
}
// free all SDO configurations
list_for_each_entry_safe(sdodata, next_sdodata, &slave->sdo_confs, list) {
list_del(&sdodata->list);
kfree(sdodata->data);
kfree(sdodata);
}
// free information about variable sized data fields
list_for_each_entry_safe(var, next_var, &slave->varsize_fields, list) {
list_del(&var->list);
@ -913,6 +922,47 @@ int ec_slave_is_coupler(const ec_slave_t *slave)
&& slave->sii_product_code == 0x044C2C52;
}
/*****************************************************************************/
/**
\return 0 in case of success, else < 0
*/
int ec_slave_conf_sdo(ec_slave_t *slave, /**< EtherCAT slave */
uint16_t sdo_index, /**< SDO index */
uint8_t sdo_subindex, /**< SDO subindex */
const uint8_t *data, /**< SDO data */
size_t size /**< SDO size in bytes */
)
{
ec_sdo_data_t *sdodata;
if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) {
EC_ERR("Slave %i does not support CoE!\n", slave->ring_position);
return -1;
}
if (!(sdodata = (ec_sdo_data_t *)
kmalloc(sizeof(ec_sdo_data_t), GFP_KERNEL))) {
EC_ERR("Failed to allocate memory for SDO configuration object!\n");
return -1;
}
if (!(sdodata->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
EC_ERR("Failed to allocate memory for SDO configuration data!\n");
kfree(sdodata);
return -1;
}
sdodata->index = sdo_index;
sdodata->subindex = sdo_subindex;
memcpy(sdodata->data, data, size);
sdodata->size = size;
list_add_tail(&sdodata->list, &slave->sdo_confs);
return 0;
}
/******************************************************************************
* Realtime interface
*****************************************************************************/
@ -922,6 +972,60 @@ int ec_slave_is_coupler(const ec_slave_t *slave)
\ingroup RealtimeInterface
*/
int ecrt_slave_conf_sdo8(ec_slave_t *slave, /**< EtherCAT slave */
uint16_t sdo_index, /**< SDO index */
uint8_t sdo_subindex, /**< SDO subindex */
uint8_t value /**< new SDO value */
)
{
uint8_t data[1];
EC_WRITE_U8(data, value);
return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 1);
}
/*****************************************************************************/
/**
\return 0 in case of success, else < 0
\ingroup RealtimeInterface
*/
int ecrt_slave_conf_sdo16(ec_slave_t *slave, /**< EtherCAT slave */
uint16_t sdo_index, /**< SDO index */
uint8_t sdo_subindex, /**< SDO subindex */
uint16_t value /**< new SDO value */
)
{
uint8_t data[2];
EC_WRITE_U16(data, value);
return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 2);
}
/*****************************************************************************/
/**
\return 0 in case of success, else < 0
\ingroup RealtimeInterface
*/
int ecrt_slave_conf_sdo32(ec_slave_t *slave, /**< EtherCAT slave */
uint16_t sdo_index, /**< SDO index */
uint8_t sdo_subindex, /**< SDO subindex */
uint32_t value /**< new SDO value */
)
{
uint8_t data[4];
EC_WRITE_U32(data, value);
return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 4);
}
/*****************************************************************************/
/**
\return 0 in case of success, else < 0
\ingroup RealtimeInterface
*/
int ecrt_slave_pdo_size(ec_slave_t *slave, /**< EtherCAT slave */
uint16_t pdo_index, /**< PDO index */
uint8_t pdo_subindex, /**< PDO subindex */
@ -989,6 +1093,9 @@ int ecrt_slave_pdo_size(ec_slave_t *slave, /**< EtherCAT slave */
/**< \cond */
EXPORT_SYMBOL(ecrt_slave_conf_sdo8);
EXPORT_SYMBOL(ecrt_slave_conf_sdo16);
EXPORT_SYMBOL(ecrt_slave_conf_sdo32);
EXPORT_SYMBOL(ecrt_slave_pdo_size);
/**< \endcond */

13
master/slave.h
View File

@ -199,6 +199,18 @@ ec_sdo_entry_t;
/*****************************************************************************/
typedef struct
{
struct list_head list;
uint16_t index;
uint8_t subindex;
uint8_t *data;
size_t size;
}
ec_sdo_data_t;
/*****************************************************************************/
/**
FMMU configuration.
*/
@ -291,6 +303,7 @@ struct ec_slave
uint8_t fmmu_count; /**< number of FMMUs used */
struct list_head sdo_dictionary; /**< SDO directory list */
struct list_head sdo_confs; /**< list of SDO configurations */
struct list_head varsize_fields; /**< size information for variable-sized
data fields. */