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

Implemented sdo_upload command.

This commit is contained in:
Florian Pose 2008-06-06 15:13:25 +00:00
parent a62466b7fe
commit a85f8fc5d5
6 changed files with 345 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
TODO
View File

@ -9,8 +9,8 @@ $Id$
Version 1.4.0:
* Slaves as array.
* ioctl() rights.
* Replace all Sysfs files via the new ethercat tool.
- Sdo entry value read
- Sdo entry value write
- Slave alias write
- Slave SII write

101
master/cdev.c
View File

@ -137,7 +137,7 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
long retval = 0;
if (master->debug_level)
EC_DBG("ioctl(filp = %x, cmd = %u, arg = %u)\n",
EC_DBG("ioctl(filp = %x, cmd = %u, arg = %x)\n",
(u32) filp, (u32) cmd, (u32) arg);
// FIXME lock
@ -562,19 +562,29 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
break;
}
if (!(sdo = ec_slave_get_sdo_by_pos_const(
slave, data.sdo_position))) {
EC_ERR("Sdo %u does not exist in slave %u!\n",
data.sdo_position, data.slave_position);
retval = -EINVAL;
break;
if (data.sdo_spec <= 0) {
if (!(sdo = ec_slave_get_sdo_by_pos_const(
slave, -data.sdo_spec))) {
EC_ERR("Sdo %u does not exist in slave %u!\n",
-data.sdo_spec, data.slave_position);
retval = -EINVAL;
break;
}
} else {
if (!(sdo = ec_slave_get_sdo_const(
slave, data.sdo_spec))) {
EC_ERR("Sdo 0x%04X does not exist in slave %u!\n",
data.sdo_spec, data.slave_position);
retval = -EINVAL;
break;
}
}
if (!(entry = ec_sdo_get_entry_const(
sdo, data.sdo_entry_subindex))) {
EC_ERR("Sdo entry %u does not exist in Sdo %u "
"in slave %u!\n", data.sdo_entry_subindex,
data.sdo_position, data.slave_position);
EC_ERR("Sdo entry 0x%04X:%02X does not exist "
"in slave %u!\n", sdo->index,
data.sdo_entry_subindex, data.slave_position);
retval = -EINVAL;
break;
}
@ -598,6 +608,77 @@ long eccdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
break;
}
case EC_IOCTL_SDO_UPLOAD:
{
ec_ioctl_sdo_upload_t data;
ec_master_sdo_request_t request;
if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
retval = -EFAULT;
break;
}
if (!(request.slave = ec_master_find_slave(
master, 0, data.slave_position))) {
EC_ERR("Slave %u does not exist!\n", data.slave_position);
retval = -EINVAL;
break;
}
ec_sdo_request_init(&request.req);
ec_sdo_request_address(&request.req,
data.sdo_index, data.sdo_entry_subindex);
ecrt_sdo_request_read(&request.req);
// schedule request.
down(&master->sdo_sem);
list_add_tail(&request.list, &master->slave_sdo_requests);
up(&master->sdo_sem);
// wait for processing through FSM
if (wait_event_interruptible(master->sdo_queue,
request.req.state != EC_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->sdo_sem);
if (request.req.state == EC_REQUEST_QUEUED) {
list_del(&request.req.list);
up(&master->sdo_sem);
retval = -EINTR;
break;
}
// request already processing: interrupt not possible.
up(&master->sdo_sem);
}
// wait until master FSM has finished processing
wait_event(master->sdo_queue, request.req.state != EC_REQUEST_BUSY);
if (request.req.state != EC_REQUEST_SUCCESS) {
retval = -EIO;
break;
}
if (request.req.data_size > data.target_size) {
EC_ERR("Buffer too small.\n");
retval = -EOVERFLOW;
break;
}
data.data_size = request.req.data_size;
if (copy_to_user((void __user *) data.target,
request.req.data, data.data_size)) {
retval = -EFAULT;
break;
}
if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
retval = -EFAULT;
break;
}
ec_sdo_request_clear(&request.req);
break;
}
default:
retval = -ENOIOCTLCMD;
}

17
master/ioctl.h
View File

@ -57,6 +57,7 @@ enum {
EC_IOCTL_SLAVE_STATE,
EC_IOCTL_SDO,
EC_IOCTL_SDO_ENTRY,
EC_IOCTL_SDO_UPLOAD,
};
/*****************************************************************************/
@ -211,7 +212,7 @@ typedef struct {
typedef struct {
// inputs
uint16_t slave_position;
uint16_t sdo_position;
int sdo_spec; // positive: index, negative: list position
uint8_t sdo_entry_subindex;
// outputs
@ -222,4 +223,18 @@ typedef struct {
/*****************************************************************************/
typedef struct {
// inputs
uint16_t slave_position;
uint16_t sdo_index;
uint8_t sdo_entry_subindex;
unsigned int target_size;
uint8_t *target;
// outputs
unsigned int data_size;
} ec_ioctl_sdo_upload_t;
/*****************************************************************************/
#endif

229
tools/Master.cpp
View File

@ -18,6 +18,72 @@ using namespace std;
#include "Master.h"
#if __BYTE_ORDER == __LITTLE_ENDIAN
#define le16tocpu(x) x
#define le32tocpu(x) x
#elif __BYTE_ORDER == __BIG_ENDIAN
#define le16tocpu(x) \
((uint16_t)( \
(((uint16_t)(x) & 0x00ffU) << 8) | \
(((uint16_t)(x) & 0xff00U) >> 8) ))
#define le32tocpu(x) \
((uint32_t)( \
(((uint32_t)(x) & 0x000000ffUL) << 24) | \
(((uint32_t)(x) & 0x0000ff00UL) << 8) | \
(((uint32_t)(x) & 0x00ff0000UL) >> 8) | \
(((uint32_t)(x) & 0xff000000UL) >> 24) ))
#endif
/****************************************************************************/
struct CoEDataType {
const char *name;
uint16_t coeCode;
unsigned int byteSize;
};
static const CoEDataType dataTypes[] = {
{"int8", 0x0002, 1},
{"int16", 0x0003, 2},
{"int32", 0x0004, 4},
{"uint8", 0x0005, 1},
{"uint16", 0x0006, 2},
{"uint32", 0x0007, 4},
{"string", 0x0009, 0},
{"raw", 0xffff, 0},
{}
};
/****************************************************************************/
const CoEDataType *findDataType(const string &str)
{
const CoEDataType *d;
for (d = dataTypes; d->name; d++)
if (str == d->name)
return d;
return NULL;
}
/****************************************************************************/
const CoEDataType *findDataType(uint16_t code)
{
const CoEDataType *d;
for (d = dataTypes; d->name; d++)
if (code == d->coeCode)
return d;
return NULL;
}
/****************************************************************************/
Master::Master()
@ -239,6 +305,139 @@ void Master::listSdos(int slavePosition, bool quiet)
/****************************************************************************/
void Master::sdoUpload(
int slavePosition,
const string &dataTypeStr,
const vector<string> &commandArgs
)
{
stringstream strIndex, strSubIndex;
int number, sval;
ec_ioctl_sdo_upload_t data;
unsigned int i, uval;
const CoEDataType *dataType = NULL;
if (slavePosition < 0) {
stringstream err;
err << "'sdo_upload' requires a slave! Please specify --slave.";
throw MasterException(err.str());
}
data.slave_position = slavePosition;
if (commandArgs.size() != 2) {
stringstream err;
err << "'sdo_upload' takes two arguments!";
throw MasterException(err.str());
}
strIndex << commandArgs[0];
strIndex >> hex >> number;
if (strIndex.fail() || number < 0x0000 || number > 0xffff) {
stringstream err;
err << "Invalid Sdo index '" << commandArgs[0] << "'!";
throw MasterException(err.str());
}
data.sdo_index = number;
strSubIndex << commandArgs[1];
strSubIndex >> hex >> number;
if (strSubIndex.fail() || number < 0x00 || number > 0xff) {
stringstream err;
err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";
throw MasterException(err.str());
}
data.sdo_entry_subindex = number;
if (dataTypeStr != "") { // data type specified
if (!(dataType = findDataType(dataTypeStr))) {
stringstream err;
err << "Invalid data type '" << dataTypeStr << "'!";
throw MasterException(err.str());
}
} else { // no data type specified: fetch from dictionary
ec_ioctl_sdo_entry_t entry;
unsigned int entryByteSize;
try {
getSdoEntry(&entry, slavePosition,
data.sdo_index, data.sdo_entry_subindex);
} catch (MasterException &e) {
stringstream err;
err << "Failed to determine Sdo entry data type. "
<< "Please specify --type.";
throw MasterException(err.str());
}
if (!(dataType = findDataType(entry.data_type))) {
stringstream err;
err << "Pdo entry has unknown data type 0x"
<< hex << setfill('0') << setw(4) << entry.data_type << "!"
<< " Please specify --type.";
throw MasterException(err.str());
}
}
if (dataType->byteSize) {
data.target_size = dataType->byteSize;
} else {
data.target_size = DefaultTargetSize;
}
data.target = new uint8_t[data.target_size + 1];
if (ioctl(fd, EC_IOCTL_SDO_UPLOAD, &data) < 0) {
stringstream err;
err << "Failed to upload Sdo: " << strerror(errno);
delete [] data.target;
throw MasterException(err.str());
}
if (dataType->byteSize && data.data_size != dataType->byteSize) {
stringstream err;
err << "Data type mismatch. Expected " << dataType->name
<< " with " << dataType->byteSize << " byte(s), but got "
<< data.data_size << " byte(s).";
throw MasterException(err.str());
}
cout << setfill('0');
switch (dataType->coeCode) {
case 0x0002: // int8
sval = *(int8_t *) data.target;
cout << sval << " 0x" << hex << setw(2) << sval << endl;
break;
case 0x0003: // int16
sval = le16tocpu(*(int16_t *) data.target);
cout << sval << " 0x" << hex << setw(4) << sval << endl;
break;
case 0x0004: // int32
sval = le32tocpu(*(int32_t *) data.target);
cout << sval << " 0x" << hex << setw(8) << sval << endl;
break;
case 0x0005: // uint8
uval = (unsigned int) *(uint8_t *) data.target;
cout << uval << " 0x" << hex << setw(2) << uval << endl;
break;
case 0x0006: // uint16
uval = le16tocpu(*(uint16_t *) data.target);
cout << uval << " 0x" << hex << setw(4) << uval << endl;
break;
case 0x0007: // uint32
uval = le32tocpu(*(uint32_t *) data.target);
cout << uval << " 0x" << hex << setw(8) << uval << endl;
break;
case 0x0009: // string
cout << string((const char *) data.target, data.data_size)
<< endl;
break;
default:
printRawData(data.target, data.data_size);
break;
}
delete [] data.target;
}
/****************************************************************************/
void Master::requestStates(
int slavePosition,
const vector<string> &commandArgs
@ -474,7 +673,7 @@ void Master::listSlaveSdos(
continue;
for (j = 0; j <= sdo.max_subindex; j++) {
getSdoEntry(&entry, slavePosition, i, j);
getSdoEntry(&entry, slavePosition, -i, j);
cout << " Entry 0x"
<< hex << setfill('0') << setw(2)
@ -807,25 +1006,18 @@ void Master::getSdo(
void Master::getSdoEntry(
ec_ioctl_sdo_entry_t *entry,
uint16_t slaveIndex,
uint16_t sdoPosition,
int sdoSpec,
uint8_t entrySubindex
)
{
entry->slave_position = slaveIndex;
entry->sdo_position = sdoPosition;
entry->sdo_spec = sdoSpec;
entry->sdo_entry_subindex = entrySubindex;
if (ioctl(fd, EC_IOCTL_SDO_ENTRY, entry)) {
stringstream err;
err << "Failed to get Sdo entry: ";
if (errno == EINVAL)
err << "Either slave " << slaveIndex << " does not exist, "
<< "or it contains less than " << sdoPosition + 1
<< " Sdos, or the Sdo at position " << sdoPosition
<< " contains less than " << (unsigned int) entrySubindex + 1
<< " entries!" << endl;
else
err << strerror(errno);
err << strerror(errno);
throw MasterException(err.str());
}
}
@ -867,3 +1059,18 @@ string Master::slaveState(uint8_t state)
}
/****************************************************************************/
void Master::printRawData(
const uint8_t *data,
unsigned int size)
{
cout << hex << setfill('0');
while (size--) {
cout << "0x" << setw(2) << (unsigned int) *data++;
if (size)
cout << " ";
}
cout << endl;
}
/****************************************************************************/

6
tools/Master.h
View File

@ -49,6 +49,7 @@ class Master
void showMaster();
void listPdos(int, bool = false);
void listSdos(int, bool = false);
void sdoUpload(int, const string &, const vector<string> &);
void requestStates(int, const vector<string> &);
void generateXml(int);
@ -72,12 +73,15 @@ class Master
void getPdoEntry(ec_ioctl_pdo_entry_t *, uint16_t, uint8_t, uint8_t,
uint8_t);
void getSdo(ec_ioctl_sdo_t *, uint16_t, uint16_t);
void getSdoEntry(ec_ioctl_sdo_entry_t *, uint16_t, uint16_t, uint8_t);
void getSdoEntry(ec_ioctl_sdo_entry_t *, uint16_t, int, uint8_t);
void requestState(uint16_t, uint8_t);
static string slaveState(uint8_t);
static void printRawData(const uint8_t *, unsigned int);
private:
enum {DefaultTargetSize = 1024};
unsigned int index;
int fd;
};

21
tools/main.cpp
View File

@ -16,16 +16,14 @@ using namespace std;
/*****************************************************************************/
#define DEFAULT_MASTER 0
#define DEFAULT_COMMAND "slaves"
#define DEFAULT_SLAVEPOSITION -1
#define DEFAULT_DOMAININDEX -1
static unsigned int masterIndex = DEFAULT_MASTER;
static int slavePosition = DEFAULT_SLAVEPOSITION;
static int domainIndex = DEFAULT_DOMAININDEX;
static string command = DEFAULT_COMMAND;
static int slavePosition = -1;
static int domainIndex = -1;
static string command;
vector<string> commandArgs;
static bool quiet = false;
string dataTypeStr;
/*****************************************************************************/
@ -41,6 +39,7 @@ void printUsage()
<< " master Show master information." << endl
<< " pdos List Pdo mapping." << endl
<< " sdos List Sdo dictionaries." << endl
<< " sdo_upload (su) Read Sdo entries." << endl
<< " state Request slave states." << endl
<< " xml Generate slave information xmls." << endl
<< "Global options:" << endl
@ -54,6 +53,7 @@ void printUsage()
<< endl
<< " or 'all' for all domains (default)."
<< endl
<< " --type -t <type> Forced Sdo data type." << endl
<< " --quiet -q Show less output." << endl
<< " --help -h Show this help." << endl;
}
@ -70,13 +70,14 @@ void getOptions(int argc, char **argv)
{"master", required_argument, NULL, 'm'},
{"slave", required_argument, NULL, 's'},
{"domain", required_argument, NULL, 'd'},
{"type", required_argument, NULL, 't'},
{"quiet", no_argument, NULL, 'q'},
{"help", no_argument, NULL, 'h'},
{}
};
do {
c = getopt_long(argc, argv, "m:s:d:qh", longOptions, &optionIndex);
c = getopt_long(argc, argv, "m:s:d:t:qh", longOptions, &optionIndex);
switch (c) {
case 'm':
@ -120,6 +121,10 @@ void getOptions(int argc, char **argv)
}
break;
case 't':
dataTypeStr = optarg;
break;
case 'q':
quiet = true;
break;
@ -173,6 +178,8 @@ int main(int argc, char **argv)
master.listPdos(slavePosition, quiet);
} else if (command == "sdos") {
master.listSdos(slavePosition, quiet);
} else if (command == "sdo_upload" || command == "su") {
master.sdoUpload(slavePosition, dataTypeStr, commandArgs);
} else if (command == "state") {
master.requestStates(slavePosition, commandArgs);
} else if (command == "xml") {