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

Added phy commands.

This commit is contained in:
Florian Pose 2008-08-13 13:23:52 +00:00
parent 568505a4df
commit d975aafa18
14 changed files with 656 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
master/cdev.c
View File

@ -986,6 +986,162 @@ int ec_cdev_ioctl_slave_sii_write(
/*****************************************************************************/
/** Read a slave's physical memory.
*/
int ec_cdev_ioctl_slave_phy_read(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg /**< ioctl() argument. */
)
{
ec_ioctl_slave_phy_t data;
ec_slave_t *slave;
uint8_t *contents;
ec_phy_request_t request;
if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
return -EFAULT;
}
if (!data.length)
return 0;
if (!(contents = kmalloc(data.length, GFP_KERNEL))) {
EC_ERR("Failed to allocate %u bytes for phy data.\n", data.length);
return -ENOMEM;
}
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(slave = ec_master_find_slave(
master, 0, data.slave_position))) {
up(&master->master_sem);
EC_ERR("Slave %u does not exist!\n", data.slave_position);
return -EINVAL;
}
// init phy request
INIT_LIST_HEAD(&request.list);
request.slave = slave;
request.dir = EC_DIR_INPUT;
request.data = contents;
request.offset = data.offset;
request.length = data.length;
request.state = EC_REQUEST_QUEUED;
// schedule request.
list_add_tail(&request.list, &master->phy_requests);
up(&master->master_sem);
// wait for processing through FSM
if (wait_event_interruptible(master->phy_queue,
request.state != EC_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
if (request.state == EC_REQUEST_QUEUED) {
// abort request
list_del(&request.list);
up(&master->master_sem);
kfree(contents);
return -EINTR;
}
up(&master->master_sem);
}
// wait until master FSM has finished processing
wait_event(master->phy_queue, request.state != EC_REQUEST_BUSY);
if (request.state == EC_REQUEST_SUCCESS) {
if (copy_to_user((void __user *) data.data, contents, data.length))
return -EFAULT;
}
kfree(contents);
return request.state == EC_REQUEST_SUCCESS ? 0 : -EIO;
}
/*****************************************************************************/
/** Write a slave's physical memory.
*/
int ec_cdev_ioctl_slave_phy_write(
ec_master_t *master, /**< EtherCAT master. */
unsigned long arg /**< ioctl() argument. */
)
{
ec_ioctl_slave_phy_t data;
ec_slave_t *slave;
uint8_t *contents;
ec_phy_request_t request;
if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
return -EFAULT;
}
if (!data.length)
return 0;
if (!(contents = kmalloc(data.length, GFP_KERNEL))) {
EC_ERR("Failed to allocate %u bytes for phy data.\n", data.length);
return -ENOMEM;
}
if (copy_from_user(contents, (void __user *) data.data, data.length)) {
kfree(contents);
return -EFAULT;
}
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(slave = ec_master_find_slave(
master, 0, data.slave_position))) {
up(&master->master_sem);
EC_ERR("Slave %u does not exist!\n", data.slave_position);
kfree(contents);
return -EINVAL;
}
// init phy request
INIT_LIST_HEAD(&request.list);
request.slave = slave;
request.dir = EC_DIR_OUTPUT;
request.data = contents;
request.offset = data.offset;
request.length = data.length;
request.state = EC_REQUEST_QUEUED;
// schedule request.
list_add_tail(&request.list, &master->phy_requests);
up(&master->master_sem);
// wait for processing through FSM
if (wait_event_interruptible(master->phy_queue,
request.state != EC_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
if (request.state == EC_REQUEST_QUEUED) {
// abort request
list_del(&request.list);
up(&master->master_sem);
kfree(contents);
return -EINTR;
}
up(&master->master_sem);
}
// wait until master FSM has finished processing
wait_event(master->phy_queue, request.state != EC_REQUEST_BUSY);
kfree(contents);
return request.state == EC_REQUEST_SUCCESS ? 0 : -EIO;
}
/*****************************************************************************/
/** Get slave configuration information.
*/
int ec_cdev_ioctl_config(
@ -1282,6 +1438,12 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_slave_sii_write(master, arg);
case EC_IOCTL_SLAVE_PHY_READ:
return ec_cdev_ioctl_slave_phy_read(master, arg);
case EC_IOCTL_SLAVE_PHY_WRITE:
if (!(filp->f_mode & FMODE_WRITE))
return -EPERM;
return ec_cdev_ioctl_slave_phy_write(master, arg);
case EC_IOCTL_CONFIG:
return ec_cdev_ioctl_config(master, arg);
case EC_IOCTL_CONFIG_PDO:

104
master/fsm_master.c
View File

@ -59,6 +59,7 @@ void ec_fsm_master_state_scan_slave(ec_fsm_master_t *);
void ec_fsm_master_state_write_sii(ec_fsm_master_t *);
void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *);
void ec_fsm_master_state_sdo_request(ec_fsm_master_t *);
void ec_fsm_master_state_phy_request(ec_fsm_master_t *);
/*****************************************************************************/
@ -324,6 +325,59 @@ int ec_fsm_master_action_process_sii(
/*****************************************************************************/
/** Check for pending phy requests and process one.
*
* \return non-zero, if a phy request is processed.
*/
int ec_fsm_master_action_process_phy(
ec_fsm_master_t *fsm /**< Master state machine. */
)
{
ec_master_t *master = fsm->master;
ec_phy_request_t *request;
// search the first request to be processed
while (1) {
if (list_empty(&master->phy_requests))
break;
// get first request
request = list_entry(master->phy_requests.next,
ec_phy_request_t, list);
list_del_init(&request->list); // dequeue
request->state = EC_REQUEST_BUSY;
// found pending request; process it!
if (master->debug_level)
EC_DBG("Processing phy request for slave %u...\n",
request->slave->ring_position);
fsm->phy_request = request;
if (request->dir == EC_DIR_INPUT) {
ec_datagram_fprd(fsm->datagram, request->slave->station_address,
request->offset, request->length);
} else {
if (request->length > fsm->datagram->mem_size) {
EC_ERR("Request length (%u) exceeds maximum datagram size (%u)!\n",
request->length, fsm->datagram->mem_size);
request->state = EC_REQUEST_FAILURE;
wake_up(&master->phy_queue);
continue;
}
ec_datagram_fpwr(fsm->datagram, request->slave->station_address,
request->offset, request->length);
memcpy(fsm->datagram->data, request->data, request->length);
}
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_phy_request;
return 1;
}
return 0;
}
/*****************************************************************************/
/** Check for pending Sdo requests and process one.
*
* \return non-zero, if an Sdo request is processed.
@ -460,6 +514,10 @@ void ec_fsm_master_action_idle(
if (ec_fsm_master_action_process_sii(fsm))
return; // SII write request found
// check for pending phy requests.
if (ec_fsm_master_action_process_phy(fsm))
return; // phy request processing
ec_fsm_master_restart(fsm);
}
@ -862,3 +920,49 @@ void ec_fsm_master_state_sdo_request(
}
/*****************************************************************************/
/** Master state: PHY.
*/
void ec_fsm_master_state_phy_request(
ec_fsm_master_t *fsm /**< Master state machine. */
)
{
ec_master_t *master = fsm->master;
ec_datagram_t *datagram = fsm->datagram;
ec_phy_request_t *request = fsm->phy_request;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_ERR("Failed to receive phy request datagram (state %u).\n",
datagram->state);
request->state = EC_REQUEST_FAILURE;
wake_up(&master->phy_queue);
ec_fsm_master_restart(fsm);
return;
}
if (request->dir == EC_DIR_INPUT) { // read request
if (request->data)
kfree(request->data);
request->data = kmalloc(request->length, GFP_KERNEL);
if (!request->data) {
EC_ERR("Failed to allocate %u bytes of memory for phy request.\n",
request->length);
request->state = EC_REQUEST_FAILURE;
wake_up(&master->phy_queue);
ec_fsm_master_restart(fsm);
return;
}
memcpy(request->data, datagram->data, request->length);
}
request->state = EC_REQUEST_SUCCESS;
wake_up(&master->phy_queue);
// check for another PHY request
if (ec_fsm_master_action_process_phy(fsm))
return; // processing another request
ec_fsm_master_restart(fsm);
}
/*****************************************************************************/

15
master/fsm_master.h
View File

@ -65,6 +65,20 @@ typedef struct {
/*****************************************************************************/
/** Physical memory request.
*/
typedef struct {
struct list_head list; /**< List head. */
ec_slave_t *slave; /**< EtherCAT slave. */
ec_direction_t dir; /**< Direction. */
uint16_t offset; /**< Physical memory offset. */
size_t length; /**< Number of bytes. */
uint8_t *data;
ec_request_state_t state; /**< State of the request. */
} ec_phy_request_t;
/*****************************************************************************/
/** Slave/Sdo request record for master's Sdo request list.
*/
typedef struct {
@ -94,6 +108,7 @@ struct ec_fsm_master {
ec_sii_write_request_t *sii_request; /**< SII write request */
off_t sii_index; /**< index to SII write request data */
ec_sdo_request_t *sdo_request; /**< Sdo request to process. */
ec_phy_request_t *phy_request; /**< Physical memory request to process. */
ec_fsm_coe_t fsm_coe; /**< CoE state machine */
ec_fsm_pdo_t fsm_pdo; /**< Pdo configuration state machine. */

20
master/ioctl.h
View File

@ -72,10 +72,12 @@
#define EC_IOCTL_SLAVE_SDO_DOWNLOAD EC_IOWR(0x0d, ec_ioctl_slave_sdo_download_t)
#define EC_IOCTL_SLAVE_SII_READ EC_IOWR(0x0e, ec_ioctl_slave_sii_t)
#define EC_IOCTL_SLAVE_SII_WRITE EC_IOW(0x0f, ec_ioctl_slave_sii_t)
#define EC_IOCTL_CONFIG EC_IOWR(0x10, ec_ioctl_config_t)
#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x11, ec_ioctl_config_pdo_t)
#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x12, ec_ioctl_config_pdo_entry_t)
#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x13, ec_ioctl_config_sdo_t)
#define EC_IOCTL_SLAVE_PHY_READ EC_IOWR(0x10, ec_ioctl_slave_phy_t)
#define EC_IOCTL_SLAVE_PHY_WRITE EC_IOW(0x11, ec_ioctl_slave_phy_t)
#define EC_IOCTL_CONFIG EC_IOWR(0x12, ec_ioctl_config_t)
#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x13, ec_ioctl_config_pdo_t)
#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x14, ec_ioctl_config_pdo_entry_t)
#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x15, ec_ioctl_config_sdo_t)
#define EC_IOCTL_STRING_SIZE 64
@ -286,6 +288,16 @@ typedef struct {
/*****************************************************************************/
typedef struct {
// inputs
uint16_t slave_position;
uint16_t offset;
uint16_t length;
uint8_t *data;
} ec_ioctl_slave_phy_t;
/*****************************************************************************/
typedef struct {
// inputs
uint32_t config_index;

3
master/master.c
View File

@ -141,6 +141,9 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
INIT_LIST_HEAD(&master->slave_sdo_requests);
init_waitqueue_head(&master->sdo_queue);
INIT_LIST_HEAD(&master->phy_requests);
init_waitqueue_head(&master->phy_queue);
// init devices
if (ec_device_init(&master->main_device, master))
goto out_return;

3
master/master.h
View File

@ -165,6 +165,9 @@ struct ec_master {
struct list_head slave_sdo_requests; /**< Sdo access requests. */
wait_queue_head_t sdo_queue; /**< Wait queue for Sdo access requests
from user space. */
struct list_head phy_requests; /**< Physical memory requests. */
wait_queue_head_t phy_queue; /**< Wait queue for phy requests. */
};
/*****************************************************************************/

114
tool/CommandPhyRead.cpp Normal file
View File

@ -0,0 +1,114 @@
/*****************************************************************************
*
* $Id$
*
****************************************************************************/
#include <iostream>
#include <iomanip>
using namespace std;
#include "CommandPhyRead.h"
/*****************************************************************************/
CommandPhyRead::CommandPhyRead():
Command("phy_read", "Output a slave's physical memory contents.")
{
}
/*****************************************************************************/
string CommandPhyRead::helpString() const
{
stringstream str;
str << getName() << " [OPTIONS] <OFFSET> <LENGTH>" << endl
<< endl
<< getBriefDescription() << endl
<< endl
<< "This command requires a single slave to be selected." << endl
<< endl
<< "Arguments:" << endl
<< " OFFSET is the physical memory address. Must" << endl
<< " be an unsigned 16 bit number." << endl
<< " LENGTH is the number of bytes to read and must also be" << endl
<< " an unsigned 16 bit number. OFFSET plus LENGTH" << endl
<< " may not exceed 64k." << endl
<< endl
<< "Command-specific options:" << endl
<< " --alias -a <alias>" << endl
<< " --position -p <pos> Slave selection. See the help of" << endl
<< " the 'slaves' command." << endl
<< endl
<< numericInfo();
return str.str();
}
/****************************************************************************/
void CommandPhyRead::execute(MasterDevice &m, const StringVector &args)
{
SlaveList slaves;
ec_ioctl_slave_phy_t data;
stringstream strOffset, strLength, err;
uint16_t i;
if (args.size() != 2) {
err << "'" << getName() << "' takes two arguments!";
throwInvalidUsageException(err);
}
strOffset << args[0];
strOffset
>> resetiosflags(ios::basefield) // guess base from prefix
>> data.offset;
if (strOffset.fail()) {
err << "Invalid offset '" << args[0] << "'!";
throwInvalidUsageException(err);
}
strLength << args[1];
strLength
>> resetiosflags(ios::basefield) // guess base from prefix
>> data.length;
if (strLength.fail()) {
err << "Invalid length '" << args[1] << "'!";
throwInvalidUsageException(err);
}
if (!data.length) {
return;
}
if ((uint32_t) data.offset + data.length > 0xffff) {
err << "Offset and length exceeding 64k!";
throwInvalidUsageException(err);
}
m.open(MasterDevice::Read);
slaves = selectedSlaves(m);
if (slaves.size() != 1) {
throwSingleSlaveRequired(slaves.size());
}
data.slave_position = slaves.front().position;
data.data = new uint8_t[data.length];
try {
m.readPhy(&data);
} catch (MasterDeviceException &e) {
delete [] data.data;
throw e;
}
for (i = 0; i < data.length; i++) {
cout << data.data[i];
}
delete [] data.data;
}
/*****************************************************************************/

26
tool/CommandPhyRead.h Normal file
View File

@ -0,0 +1,26 @@
/*****************************************************************************
*
* $Id$
*
****************************************************************************/
#ifndef __COMMANDPHYREAD_H__
#define __COMMANDPHYREAD_H__
#include "Command.h"
/****************************************************************************/
class CommandPhyRead:
public Command
{
public:
CommandPhyRead();
string helpString() const;
void execute(MasterDevice &, const StringVector &);
};
/****************************************************************************/
#endif

148
tool/CommandPhyWrite.cpp Normal file
View File

@ -0,0 +1,148 @@
/*****************************************************************************
*
* $Id$
*
****************************************************************************/
#include <iostream>
#include <iomanip>
#include <fstream>
using namespace std;
#include "CommandPhyWrite.h"
#include "sii_crc.h"
#include "byteorder.h"
/*****************************************************************************/
CommandPhyWrite::CommandPhyWrite():
Command("phy_write", "Write data to a slave's physical memory.")
{
}
/*****************************************************************************/
string CommandPhyWrite::helpString() const
{
stringstream str;
str << getName() << " [OPTIONS] <OFFSET> <FILENAME>" << endl
<< endl
<< getBriefDescription() << endl
<< endl
<< "This command requires a single slave to be selected." << endl
<< endl
<< "Arguments:" << endl
<< " OFFSET must be the physical memory offset to start." << endl
<< " FILENAME must be a path to a file with data to write." << endl
<< " If it is '-', data are read from stdin." << endl
<< endl
<< "Command-specific options:" << endl
<< " --alias -a <alias>" << endl
<< " --position -p <pos> Slave selection. See the help of" << endl
<< " the 'slaves' command." << endl
<< endl
<< numericInfo();
return str.str();
}
/****************************************************************************/
void CommandPhyWrite::execute(MasterDevice &m, const StringVector &args)
{
stringstream strOffset, err;
ec_ioctl_slave_phy_t data;
ifstream file;
SlaveList slaves;
if (args.size() != 2) {
err << "'" << getName() << "' takes exactly one argument!";
throwInvalidUsageException(err);
}
strOffset << args[0];
strOffset
>> resetiosflags(ios::basefield) // guess base from prefix
>> data.offset;
if (strOffset.fail()) {
err << "Invalid offset '" << args[0] << "'!";
throwInvalidUsageException(err);
}
if (args[1] == "-") {
loadPhyData(&data, cin);
} else {
file.open(args[1].c_str(), ifstream::in | ifstream::binary);
if (file.fail()) {
err << "Failed to open '" << args[0] << "'!";
throwCommandException(err);
}
loadPhyData(&data, file);
file.close();
}
if ((uint32_t) data.offset + data.length > 0xffff) {
err << "Offset and length exceeding 64k!";
delete [] data.data;
throwInvalidUsageException(err);
}
try {
m.open(MasterDevice::ReadWrite);
} catch (MasterDeviceException &e) {
delete [] data.data;
throw e;
}
slaves = selectedSlaves(m);
if (slaves.size() != 1) {
delete [] data.data;
throwSingleSlaveRequired(slaves.size());
}
data.slave_position = slaves.front().position;
// send data to master
try {
m.writePhy(&data);
} catch (MasterDeviceException &e) {
delete [] data.data;
throw e;
}
if (getVerbosity() == Verbose) {
cerr << "Physical memory writing finished." << endl;
}
delete [] data.data;
}
/*****************************************************************************/
void CommandPhyWrite::loadPhyData(
ec_ioctl_slave_phy_t *data,
const istream &in
)
{
stringstream err;
ostringstream tmp;
tmp << in.rdbuf();
string const &contents = tmp.str();
if (getVerbosity() == Verbose) {
cerr << "Read " << contents.size() << " bytes of data." << endl;
}
if (contents.size() > 0xffff) {
err << "Invalid data size " << contents.size() << "!";
throwInvalidUsageException(err);
}
data->length = contents.size();
// allocate buffer and read file into buffer
data->data = new uint8_t[data->length];
contents.copy((char *) data->data, contents.size());
}
/*****************************************************************************/

29
tool/CommandPhyWrite.h Normal file
View File

@ -0,0 +1,29 @@
/*****************************************************************************
*
* $Id$
*
****************************************************************************/
#ifndef __COMMANDPHYWRITE_H__
#define __COMMANDPHYWRITE_H__
#include "Command.h"
/****************************************************************************/
class CommandPhyWrite:
public Command
{
public:
CommandPhyWrite();
string helpString() const;
void execute(MasterDevice &, const StringVector &);
private:
void loadPhyData(ec_ioctl_slave_phy_t *, const istream &);
};
/****************************************************************************/
#endif

4
tool/Makefile.am
View File

@ -45,6 +45,8 @@ ethercat_SOURCES = \
CommandDownload.cpp \
CommandMaster.cpp \
CommandPdos.cpp \
CommandPhyRead.cpp \
CommandPhyWrite.cpp \
CommandSdos.cpp \
CommandSiiRead.cpp \
CommandSiiWrite.cpp \
@ -68,6 +70,8 @@ noinst_HEADERS = \
CommandDownload.h \
CommandMaster.h \
CommandPdos.h \
CommandPhyRead.h \
CommandPhyWrite.h \
CommandSdos.h \
CommandSiiRead.h \
CommandSiiWrite.h \

26
tool/MasterDevice.cpp
View File

@ -344,6 +344,32 @@ void MasterDevice::writeSii(
/****************************************************************************/
void MasterDevice::readPhy(
ec_ioctl_slave_phy_t *data
)
{
if (ioctl(fd, EC_IOCTL_SLAVE_PHY_READ, data) < 0) {
stringstream err;
err << "Failed to read physical memory: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::writePhy(
ec_ioctl_slave_phy_t *data
)
{
if (ioctl(fd, EC_IOCTL_SLAVE_PHY_WRITE, data) < 0) {
stringstream err;
err << "Failed to write physical memory: " << strerror(errno);
throw MasterDeviceException(err);
}
}
/****************************************************************************/
void MasterDevice::setDebug(unsigned int debugLevel)
{
if (ioctl(fd, EC_IOCTL_MASTER_DEBUG, debugLevel) < 0) {

2
tool/MasterDevice.h
View File

@ -86,6 +86,8 @@ class MasterDevice
void getSdoEntry(ec_ioctl_slave_sdo_entry_t *, uint16_t, int, uint8_t);
void readSii(ec_ioctl_slave_sii_t *);
void writeSii(ec_ioctl_slave_sii_t *);
void readPhy(ec_ioctl_slave_phy_t *);
void writePhy(ec_ioctl_slave_phy_t *);
void setDebug(unsigned int);
void sdoDownload(ec_ioctl_slave_sdo_download_t *);
void sdoUpload(ec_ioctl_slave_sdo_upload_t *);

4
tool/main.cpp
View File

@ -19,6 +19,8 @@ using namespace std;
#include "CommandDownload.h"
#include "CommandMaster.h"
#include "CommandPdos.h"
#include "CommandPhyRead.h"
#include "CommandPhyWrite.h"
#include "CommandSdos.h"
#include "CommandSiiRead.h"
#include "CommandSiiWrite.h"
@ -263,6 +265,8 @@ int main(int argc, char **argv)
commandList.push_back(new CommandDownload());
commandList.push_back(new CommandMaster());
commandList.push_back(new CommandPdos());
commandList.push_back(new CommandPhyRead());
commandList.push_back(new CommandPhyWrite());
commandList.push_back(new CommandSdos());
commandList.push_back(new CommandSiiRead());
commandList.push_back(new CommandSiiWrite());