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