00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "ethercat_hardware/wg_eeprom.h"
00036 #include "ros/ros.h"
00037
00038 #include <boost/thread.hpp>
00039
00040 namespace ethercat_hardware
00041 {
00042
00043
00044 struct WG0XSpiEepromCmd
00045 {
00046 uint16_t page_;
00047 union
00048 {
00049 uint8_t command_;
00050 struct
00051 {
00052 uint8_t operation_ :4;
00053 uint8_t start_ :1;
00054 uint8_t busy_ :1;
00055 uint8_t unused2_ :2;
00056 }__attribute__ ((__packed__));
00057 };
00058
00059 void build_read(unsigned page)
00060 {
00061 this->page_ = page & 0xffff;
00062 this->operation_ = SPI_READ_OP;
00063 this->start_ = 1;
00064 }
00065 void build_write(unsigned page)
00066 {
00067 this->page_ = page & 0xffff;
00068 this->operation_ = SPI_WRITE_OP;
00069 this->start_ = 1;
00070 }
00071 void build_arbitrary(unsigned length)
00072 {
00073 this->page_ = (length-1) & 0xffff;
00074 this->operation_ = SPI_ARBITRARY_OP;
00075 this->start_ = 1;
00076 }
00077
00078 static const unsigned SPI_READ_OP = 0;
00079 static const unsigned SPI_WRITE_OP = 1;
00080 static const unsigned SPI_ARBITRARY_OP = 3;
00081
00082 static const unsigned SPI_COMMAND_ADDR = 0x0230;
00083 static const unsigned SPI_BUFFER_ADDR = 0xF400;
00084 }__attribute__ ((__packed__));
00085
00086
00087
00088 struct EepromStatusReg
00089 {
00090 union {
00091 uint8_t raw_;
00092 struct {
00093 uint8_t page_size_ : 1;
00094 uint8_t write_protect_ : 1;
00095 uint8_t eeprom_size_ : 4;
00096 uint8_t compare_ : 1;
00097 uint8_t ready_ : 1;
00098 } __attribute__ ((__packed__));
00099 } __attribute__ ((__packed__));
00100 } __attribute__ ((__packed__));
00101
00102
00103
00104
00105 WGEeprom::WGEeprom()
00106 {
00107
00108 }
00109
00110
00119 bool WGEeprom::waitForSpiEepromReady(EthercatCom *com, WGMailbox *mbx)
00120 {
00121 WG0XSpiEepromCmd cmd;
00122
00123
00124 unsigned tries = 0;
00125 do {
00126
00127 ++tries;
00128 if (!readSpiEepromCmd(com, mbx, cmd))
00129 {
00130 ROS_ERROR("Error reading SPI Eeprom Cmd busy bit");
00131 return false;
00132 }
00133
00134 if (!cmd.busy_)
00135 {
00136 return true;
00137 }
00138
00139 usleep(100);
00140 } while (tries <= 10);
00141
00142 ROS_ERROR("Timed out waiting for SPI state machine to be idle (%d)", tries);
00143 return false;
00144 }
00145
00146
00156 bool WGEeprom::sendSpiEepromCmd(EthercatCom *com, WGMailbox *mbx, const WG0XSpiEepromCmd &cmd)
00157 {
00158 if (!waitForSpiEepromReady(com, mbx))
00159 {
00160 return false;
00161 }
00162
00163
00164 if (mbx->writeMailbox(com, WG0XSpiEepromCmd::SPI_COMMAND_ADDR, &cmd, sizeof(cmd)))
00165 {
00166 ROS_ERROR("Error writing SPI EEPROM command");
00167 return false;
00168 }
00169
00170
00171
00172
00173 WG0XSpiEepromCmd stat;
00174 unsigned tries = 0;
00175 do
00176 {
00177 if (!readSpiEepromCmd(com, mbx, stat))
00178 {
00179 return false;
00180 }
00181
00182 if (stat.operation_ != cmd.operation_)
00183 {
00184 ROS_ERROR("Invalid readback of SPI EEPROM operation : got 0x%X, expected 0x%X\n", stat.operation_, cmd.operation_);
00185 return false;
00186 }
00187
00188
00189 if (!stat.busy_)
00190 {
00191 if (tries > 0)
00192 {
00193 ROS_WARN("Eeprom state machine took %d cycles", tries);
00194 }
00195 return true;;
00196 }
00197
00198 fprintf(stderr, "eeprom busy reading again, waiting...\n");
00199 usleep(100);
00200 } while (++tries < 10);
00201
00202 ROS_ERROR("Eeprom SPI state machine busy after %d cycles", tries);
00203 return false;
00204 }
00205
00206
00221 bool WGEeprom::readEepromPage(EthercatCom *com, WGMailbox *mbx, unsigned page, void* data, unsigned length)
00222 {
00223 boost::lock_guard<boost::mutex> lock(mutex_);
00224
00225 if (length > MAX_EEPROM_PAGE_SIZE)
00226 {
00227 ROS_ERROR("Eeprom read length %d > %d", length, MAX_EEPROM_PAGE_SIZE);
00228 return false;
00229 }
00230
00231 if (page >= NUM_EEPROM_PAGES)
00232 {
00233 ROS_ERROR("Eeprom read page %d > %d", page, NUM_EEPROM_PAGES-1);
00234 return false;
00235 }
00236
00237
00238
00239
00240 memset(data,0,length);
00241 if (mbx->writeMailbox(com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, data, length))
00242 {
00243 ROS_ERROR("Error zeroing eeprom data buffer");
00244 return false;
00245 }
00246
00247
00248
00249
00250 WG0XSpiEepromCmd cmd;
00251 memset(&cmd,0,sizeof(cmd));
00252 cmd.build_read(page);
00253 if (!sendSpiEepromCmd(com, mbx, cmd))
00254 {
00255 ROS_ERROR("Error sending SPI read command");
00256 return false;
00257 }
00258
00259
00260
00261
00262
00263 if (mbx->readMailbox(com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, data, length))
00264 {
00265 ROS_ERROR("Error reading eeprom data from buffer");
00266 return false;
00267 }
00268
00269 return true;
00270 }
00271
00272
00287 bool WGEeprom::writeEepromPage(EthercatCom *com, WGMailbox *mbx, unsigned page, const void* data, unsigned length)
00288 {
00289 boost::lock_guard<boost::mutex> lock(mutex_);
00290
00291 if (length > 264)
00292 {
00293 ROS_ERROR("Eeprom write length %d > %d", length, MAX_EEPROM_PAGE_SIZE);
00294 return false;
00295 }
00296
00297 if (page >= NUM_EEPROM_PAGES)
00298 {
00299 ROS_ERROR("Eeprom write page %d > %d", page, NUM_EEPROM_PAGES-1);
00300 return false;
00301 }
00302
00303
00304 if (!waitForSpiEepromReady(com, mbx))
00305 {
00306 return false;
00307 }
00308
00309 const void *write_buf = data;
00310
00311
00312 uint8_t buf[MAX_EEPROM_PAGE_SIZE];
00313 if (length < MAX_EEPROM_PAGE_SIZE)
00314 {
00315 memcpy(buf, data, length);
00316 memset(buf+length, 0xFF, MAX_EEPROM_PAGE_SIZE-length);
00317 write_buf = buf;
00318 }
00319
00320
00321 if (mbx->writeMailbox(com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, write_buf, MAX_EEPROM_PAGE_SIZE))
00322 {
00323 ROS_ERROR("Write of SPI EEPROM buffer failed");
00324 return false;
00325 }
00326
00327
00328 WG0XSpiEepromCmd cmd;
00329 cmd.build_write(page);
00330 if (!sendSpiEepromCmd(com, mbx, cmd))
00331 {
00332 ROS_ERROR("Error giving SPI EEPROM write command");
00333 return false;
00334 }
00335
00336
00337 if (!waitForEepromReady(com, mbx))
00338 {
00339 return false;
00340 }
00341
00342 return true;
00343 }
00344
00345
00357 bool WGEeprom::waitForEepromReady(EthercatCom *com, WGMailbox *mbx)
00358 {
00359
00360 unsigned tries = 0;
00361 EepromStatusReg status_reg;
00362 do {
00363 if (!readEepromStatusReg(com, mbx, status_reg))
00364 {
00365 return false;
00366 }
00367 if (status_reg.ready_)
00368 {
00369 break;
00370 }
00371 usleep(100);
00372 } while (++tries < 20);
00373
00374 if (!status_reg.ready_)
00375 {
00376 ROS_ERROR("Eeprom still busy after %d cycles", tries);
00377 return false;
00378 }
00379
00380 if (tries > 10)
00381 {
00382 ROS_WARN("EEPROM took %d cycles to be ready", tries);
00383 }
00384 return true;
00385 }
00386
00387
00388
00399 bool WGEeprom::readEepromStatusReg(EthercatCom *com, WGMailbox *mbx, EepromStatusReg ®)
00400 {
00401
00402
00403
00404
00405
00406
00407
00408 char data[2] = {0xD7, 0x00};
00409 BOOST_STATIC_ASSERT(sizeof(data) == 2);
00410 if (mbx->writeMailbox(com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, data, sizeof(data)))
00411 {
00412 ROS_ERROR("Writing SPI buffer");
00413 return false;
00414 }
00415
00416 {
00417 WG0XSpiEepromCmd cmd;
00418 cmd.build_arbitrary(sizeof(data));
00419 if (!sendSpiEepromCmd(com, mbx, cmd))
00420 {
00421 ROS_ERROR("Sending SPI abitrary command");
00422 return false;
00423 }
00424 }
00425
00426
00427 if (mbx->readMailbox(com, WG0XSpiEepromCmd::SPI_BUFFER_ADDR, data, sizeof(data)))
00428 {
00429 ROS_ERROR("Reading status register data from SPI buffer");
00430 return false;
00431 }
00432
00433
00434 reg.raw_ = data[1];
00435 return true;
00436 }
00437
00438
00454 bool WGEeprom::readSpiEepromCmd(EthercatCom *com, WGMailbox *mbx, WG0XSpiEepromCmd &cmd)
00455 {
00456 BOOST_STATIC_ASSERT(sizeof(WG0XSpiEepromCmd) == 3);
00457 if (mbx->readMailbox(com, WG0XSpiEepromCmd::SPI_COMMAND_ADDR, &cmd, sizeof(cmd)))
00458 {
00459 ROS_ERROR("Reading SPI command register with mailbox");
00460 return false;
00461 }
00462
00463 return true;
00464 }
00465
00466
00467 };