From b0eace9ef17fa1e30b3fae8adf3094cf7706f308 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Mon, 13 May 2013 20:21:19 +0200 Subject: [PATCH] Implemented emergency register requests to analyze a broken bus. --- master/fsm_master.c | 40 ++++++++++++++++++++++++++++++++++++++++ master/ioctl.c | 23 +++++++++++++++-------- master/ioctl.h | 3 ++- master/master.c | 1 + master/master.h | 2 ++ master/reg_request.c | 1 + master/reg_request.h | 1 + tool/Command.cpp | 24 +++++++++++++++++++++++- tool/Command.h | 12 ++++++++++++ tool/CommandRegRead.cpp | 1 + tool/CommandRegWrite.cpp | 28 ++++++++++++++++++---------- tool/main.cpp | 9 ++++++++- 12 files changed, 124 insertions(+), 21 deletions(-) diff --git a/master/fsm_master.c b/master/fsm_master.c index cefc373c..0388096c 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -198,7 +198,47 @@ void ec_fsm_master_state_start( ec_fsm_master_t *fsm /**< Master state machine. */ ) { + ec_master_t *master = fsm->master; + fsm->idle = 1; + + // check for emergency requests + if (!list_empty(&master->emerg_reg_requests)) { + ec_reg_request_t *request; + + // get first request + request = list_entry(master->emerg_reg_requests.next, + ec_reg_request_t, list); + list_del_init(&request->list); // dequeue + request->state = EC_INT_REQUEST_BUSY; + + if (request->transfer_size > fsm->datagram->mem_size) { + EC_MASTER_ERR(master, "Emergency request data too large!\n"); + request->state = EC_INT_REQUEST_FAILURE; + wake_up_all(&master->request_queue); + fsm->state(fsm); // continue + return; + } + + if (request->dir != EC_DIR_OUTPUT) { + EC_MASTER_ERR(master, "Emergency requests must be" + " write requests!\n"); + request->state = EC_INT_REQUEST_FAILURE; + wake_up_all(&master->request_queue); + fsm->state(fsm); // continue + return; + } + + EC_MASTER_DBG(master, 1, "Writing emergency register request...\n"); + ec_datagram_apwr(fsm->datagram, request->ring_position, + request->address, request->transfer_size); + memcpy(fsm->datagram->data, request->data, request->transfer_size); + fsm->datagram->device_index = EC_DEVICE_MAIN; + request->state = EC_INT_REQUEST_SUCCESS; + wake_up_all(&master->request_queue); + return; + } + ec_datagram_brd(fsm->datagram, 0x0130, 2); ec_datagram_zero(fsm->datagram); fsm->datagram->device_index = fsm->dev_idx; diff --git a/master/ioctl.c b/master/ioctl.c index 8565efb6..a4708d85 100644 --- a/master/ioctl.c +++ b/master/ioctl.c @@ -1144,16 +1144,23 @@ static ATTRIBUTES int ec_ioctl_slave_reg_write( return -EINTR; } - if (!(slave = ec_master_find_slave(master, 0, io.slave_position))) { - up(&master->master_sem); - ec_reg_request_clear(&request); - EC_MASTER_ERR(master, "Slave %u does not exist!\n", - io.slave_position); - return -EINVAL; + if (io.emergency) { + request.ring_position = io.slave_position; + // schedule request. + list_add_tail(&request.list, &master->emerg_reg_requests); } + else { + if (!(slave = ec_master_find_slave(master, 0, io.slave_position))) { + up(&master->master_sem); + ec_reg_request_clear(&request); + EC_MASTER_ERR(master, "Slave %u does not exist!\n", + io.slave_position); + return -EINVAL; + } - // schedule request. - list_add_tail(&request.list, &slave->reg_requests); + // schedule request. + list_add_tail(&request.list, &slave->reg_requests); + } up(&master->master_sem); diff --git a/master/ioctl.h b/master/ioctl.h index 105e54e9..30969741 100644 --- a/master/ioctl.h +++ b/master/ioctl.h @@ -56,7 +56,7 @@ * * Increment this when changing the ioctl interface! */ -#define EC_IOCTL_VERSION_MAGIC 27 +#define EC_IOCTL_VERSION_MAGIC 28 // Command-line tool #define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t) @@ -419,6 +419,7 @@ typedef struct { typedef struct { // inputs uint16_t slave_position; + uint8_t emergency; uint16_t address; size_t size; uint8_t *data; diff --git a/master/master.c b/master/master.c index f59a1419..81255dee 100644 --- a/master/master.c +++ b/master/master.c @@ -232,6 +232,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ master->app_cb_data = NULL; INIT_LIST_HEAD(&master->sii_requests); + INIT_LIST_HEAD(&master->emerg_reg_requests); init_waitqueue_head(&master->request_queue); diff --git a/master/master.h b/master/master.h index d3ea23b7..9ab7e4ec 100644 --- a/master/master.h +++ b/master/master.h @@ -305,6 +305,8 @@ struct ec_master { void *app_cb_data; /**< Application callback data. */ struct list_head sii_requests; /**< SII write requests. */ + struct list_head emerg_reg_requests; /**< Emergency register access + requests. */ wait_queue_head_t request_queue; /**< Wait queue for external requests from user space. */ diff --git a/master/reg_request.c b/master/reg_request.c index 7df4ec92..c1670d61 100644 --- a/master/reg_request.c +++ b/master/reg_request.c @@ -62,6 +62,7 @@ int ec_reg_request_init( reg->address = 0; reg->transfer_size = 0; reg->state = EC_INT_REQUEST_INIT; + reg->ring_position = 0; return 0; } diff --git a/master/reg_request.h b/master/reg_request.h index 94b2577f..f1221292 100644 --- a/master/reg_request.h +++ b/master/reg_request.h @@ -54,6 +54,7 @@ struct ec_reg_request { uint16_t address; /**< Register address. */ size_t transfer_size; /**< Size of the data to transfer. */ ec_internal_request_state_t state; /**< Request state. */ + uint16_t ring_position; /**< Ring position for emergency requests. */ }; /*****************************************************************************/ diff --git a/tool/Command.cpp b/tool/Command.cpp index e01fdbe7..63a69e6d 100644 --- a/tool/Command.cpp +++ b/tool/Command.cpp @@ -150,7 +150,9 @@ class AliasPositionParser: Command::Command(const string &name, const string &briefDesc): name(name), briefDesc(briefDesc), - verbosity(Normal) + verbosity(Normal), + emergency(false), + force(false) { } @@ -204,6 +206,13 @@ void Command::setDataType(const string &t) /*****************************************************************************/ +void Command::setEmergency(bool e) +{ + emergency = e; +}; + +/*****************************************************************************/ + void Command::setForce(bool f) { force = f; @@ -489,6 +498,19 @@ Command::DomainList Command::selectedDomains(MasterDevice &m, /****************************************************************************/ +int Command::emergencySlave() const +{ + unsigned int ret; + + stringstream str; + str << positions; + str >> ret; + + return ret; +} + +/****************************************************************************/ + string Command::alStateString(uint8_t state) { string ret; diff --git a/tool/Command.h b/tool/Command.h index 8138654e..70b41793 100644 --- a/tool/Command.h +++ b/tool/Command.h @@ -107,6 +107,9 @@ class Command void setDataType(const string &); const string &getDataType() const; + void setEmergency(bool); + bool getEmergency() const; + void setForce(bool); bool getForce() const; @@ -140,6 +143,7 @@ class Command ConfigList selectedConfigs(MasterDevice &); typedef list DomainList; DomainList selectedDomains(MasterDevice &, const ec_ioctl_master_t &); + int emergencySlave() const; static string alStateString(uint8_t); @@ -152,6 +156,7 @@ class Command string positions; string domains; string dataType; + bool emergency; bool force; string outputFile; string skin; @@ -189,6 +194,13 @@ inline const string &Command::getDataType() const /****************************************************************************/ +inline bool Command::getEmergency() const +{ + return emergency; +} + +/****************************************************************************/ + inline bool Command::getForce() const { return force; diff --git a/tool/CommandRegRead.cpp b/tool/CommandRegRead.cpp index d1d83140..a2969d76 100644 --- a/tool/CommandRegRead.cpp +++ b/tool/CommandRegRead.cpp @@ -149,6 +149,7 @@ void CommandRegRead::execute(const StringVector &args) throwSingleSlaveRequired(slaves.size()); } io.slave_position = slaves.front().position; + io.emergency = false; io.data = new uint8_t[io.size]; diff --git a/tool/CommandRegWrite.cpp b/tool/CommandRegWrite.cpp index 9a8a57e9..7a40f919 100644 --- a/tool/CommandRegWrite.cpp +++ b/tool/CommandRegWrite.cpp @@ -68,10 +68,12 @@ string CommandRegWrite::helpString(const string &binaryBaseName) const << typeInfo() << endl << "Command-specific options:" << endl - << " --alias -a " << endl - << " --position -p Slave selection. See the help of" << endl - << " the 'slaves' command." << endl - << " --type -t Data type (see above)." << endl + << " --alias -a " << endl + << " --position -p Slave selection. See the help of" + << endl + << " the 'slaves' command." << endl + << " --type -t Data type (see above)." << endl + << " --emergency -e Send as emergency request." << endl << endl << numericInfo(); @@ -85,7 +87,6 @@ void CommandRegWrite::execute(const StringVector &args) stringstream strOffset, err; ec_ioctl_slave_reg_t io; ifstream file; - SlaveList slaves; if (args.size() != 2) { err << "'" << getName() << "' takes exactly two arguments!"; @@ -158,12 +159,19 @@ void CommandRegWrite::execute(const StringVector &args) throw e; } - slaves = selectedSlaves(m); - if (slaves.size() != 1) { - delete [] io.data; - throwSingleSlaveRequired(slaves.size()); + if (getEmergency()) { + io.slave_position = emergencySlave(); + io.emergency = true; + } + else { + SlaveList slaves = selectedSlaves(m); + if (slaves.size() != 1) { + delete [] io.data; + throwSingleSlaveRequired(slaves.size()); + } + io.slave_position = slaves.front().position; + io.emergency = false; } - io.slave_position = slaves.front().position; // send data to master try { diff --git a/tool/main.cpp b/tool/main.cpp index e9520014..00c0d7f2 100644 --- a/tool/main.cpp +++ b/tool/main.cpp @@ -83,6 +83,7 @@ string domains = "-"; // all domains string dataTypeStr; Command::Verbosity verbosity = Command::Normal; bool force = false; +bool emergency = false; bool helpRequested = false; string outputFile; string skin; @@ -149,6 +150,7 @@ void getOptions(int argc, char **argv) {"type", required_argument, NULL, 't'}, {"output-file", required_argument, NULL, 'o'}, {"skin", required_argument, NULL, 's'}, + {"emergency", no_argument, NULL, 'e'}, {"force", no_argument, NULL, 'f'}, {"quiet", no_argument, NULL, 'q'}, {"verbose", no_argument, NULL, 'v'}, @@ -157,7 +159,7 @@ void getOptions(int argc, char **argv) }; do { - c = getopt_long(argc, argv, "m:a:p:d:t:o:s:fqvh", longOptions, NULL); + c = getopt_long(argc, argv, "m:a:p:d:t:o:s:efqvh", longOptions, NULL); switch (c) { case 'm': @@ -188,6 +190,10 @@ void getOptions(int argc, char **argv) skin = optarg; break; + case 'e': + emergency = true; + break; + case 'f': force = true; break; @@ -315,6 +321,7 @@ int main(int argc, char **argv) cmd->setDataType(dataTypeStr); cmd->setOutputFile(outputFile); cmd->setSkin(skin); + cmd->setEmergency(emergency); cmd->setForce(force); cmd->execute(commandArgs); } catch (InvalidUsageException &e) {