1 /* 2 * Author: Jon Trulson <jtrulson (at) ics.com> 3 * Copyright (c) 2015 Intel Corporation. 4 * 5 * Permission is hereby granted, free of charge, to any person obtaining 6 * a copy of this software and associated documentation files (the 7 * "Software"), to deal in the Software without restriction, including 8 * without limitation the rights to use, copy, modify, merge, publish, 9 * distribute, sublicense, and/or sell copies of the Software, and to 10 * permit persons to whom the Software is furnished to do so, subject to 11 * the following conditions: 12 * 13 * The above copyright notice and this permission notice shall be 14 * included in all copies or substantial portions of the Software. 15 * 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 */ 24 25 #include <iostream> 26 #include <string> 27 #include <stdexcept> 28 29 #include "zfm20.h" 30 31 using namespace upm; 32 using namespace std; 33 34 static const int defaultDelay = 100; // max wait time for read 35 36 ZFM20::ZFM20(int uart) 37 { 38 m_ttyFd = -1; 39 40 if ( !(m_uart = mraa_uart_init(uart)) ) 41 { 42 throw std::invalid_argument(std::string(__FUNCTION__) + 43 ": mraa_uart_init() failed"); 44 return; 45 } 46 47 // This requires a recent MRAA (1/2015) 48 const char *devPath = mraa_uart_get_dev_path(m_uart); 49 50 if (!devPath) 51 { 52 throw std::runtime_error(std::string(__FUNCTION__) + 53 ": mraa_uart_get_dev_path() failed"); 54 return; 55 } 56 57 // now open the tty 58 if ( (m_ttyFd = open(devPath, O_RDWR)) == -1) 59 { 60 throw std::runtime_error(std::string(__FUNCTION__) + 61 ": open of " + 62 string(devPath) + " failed: " + 63 string(strerror(errno))); 64 return; 65 } 66 67 // Set the default password and address 68 setPassword(ZFM20_DEFAULT_PASSWORD); 69 setAddress(ZFM20_DEFAULT_ADDRESS); 70 71 initClock(); 72 } 73 74 ZFM20::~ZFM20() 75 { 76 if (m_ttyFd != -1) 77 close(m_ttyFd); 78 79 mraa_deinit(); 80 } 81 82 bool ZFM20::dataAvailable(unsigned int millis) 83 { 84 if (m_ttyFd == -1) 85 return false; 86 87 struct timeval timeout; 88 89 // no waiting 90 timeout.tv_sec = 0; 91 timeout.tv_usec = millis * 1000; 92 93 int nfds; 94 fd_set readfds; 95 96 FD_ZERO(&readfds); 97 98 FD_SET(m_ttyFd, &readfds); 99 100 if (select(m_ttyFd + 1, &readfds, NULL, NULL, &timeout) > 0) 101 return true; // data is ready 102 else 103 return false; 104 } 105 106 int ZFM20::readData(char *buffer, int len) 107 { 108 if (m_ttyFd == -1) 109 return(-1); 110 111 if (!dataAvailable(defaultDelay)) 112 return 0; // timed out 113 114 int rv = read(m_ttyFd, buffer, len); 115 116 if (rv < 0) 117 { 118 throw std::runtime_error(std::string(__FUNCTION__) + 119 ": read() failed: " + 120 string(strerror(errno))); 121 return rv; 122 } 123 124 return rv; 125 } 126 127 int ZFM20::writeData(char *buffer, int len) 128 { 129 if (m_ttyFd == -1) 130 return(-1); 131 132 // first, flush any pending but unread input 133 tcflush(m_ttyFd, TCIFLUSH); 134 135 int rv = write(m_ttyFd, buffer, len); 136 137 if (rv < 0) 138 { 139 throw std::runtime_error(std::string(__FUNCTION__) + 140 ": write() failed: " + 141 string(strerror(errno))); 142 return rv; 143 } 144 145 if (rv == 0) 146 { 147 throw std::runtime_error(std::string(__FUNCTION__) + 148 ": write() failed, no bytes written"); 149 return rv; 150 } 151 152 tcdrain(m_ttyFd); 153 154 return rv; 155 } 156 157 bool ZFM20::setupTty(speed_t baud) 158 { 159 if (m_ttyFd == -1) 160 return(false); 161 162 struct termios termio; 163 164 // get current modes 165 tcgetattr(m_ttyFd, &termio); 166 167 // setup for a 'raw' mode. 81N, no echo or special character 168 // handling, such as flow control. 169 cfmakeraw(&termio); 170 171 // set our baud rates 172 cfsetispeed(&termio, baud); 173 cfsetospeed(&termio, baud); 174 175 // make it so 176 if (tcsetattr(m_ttyFd, TCSAFLUSH, &termio) < 0) 177 { 178 throw std::runtime_error(std::string(__FUNCTION__) + 179 ": tcsetattr() failed: " + 180 string(strerror(errno))); 181 return false; 182 } 183 184 return true; 185 } 186 187 int ZFM20::writeCmdPacket(uint8_t *pkt, int len) 188 { 189 uint8_t rPkt[ZFM20_MAX_PKT_LEN]; 190 191 rPkt[0] = ZFM20_START1; // header bytes 192 rPkt[1] = ZFM20_START2; 193 194 rPkt[2] = (m_address >> 24) & 0xff; // address 195 rPkt[3] = (m_address >> 16) & 0xff; 196 rPkt[4] = (m_address >> 8) & 0xff; 197 rPkt[5] = m_address & 0xff; 198 199 rPkt[6] = PKT_COMMAND; 200 201 rPkt[7] = ((len + 2) >> 8) & 0xff; // length (+ len bytes) 202 rPkt[8] = (len + 2) & 0xff; 203 204 // compute the starting checksum 205 uint16_t cksum = rPkt[7] + rPkt[8] + PKT_COMMAND; 206 207 int j = 9; 208 for (int i=0; i<len; i++) 209 { 210 rPkt[j] = pkt[i]; 211 cksum += rPkt[j]; 212 j++; 213 } 214 215 rPkt[j++] = (cksum >> 8) & 0xff; // store the cksum 216 rPkt[j++] = cksum & 0xff; 217 218 return writeData((char *)rPkt, j); 219 } 220 221 void ZFM20::initClock() 222 { 223 gettimeofday(&m_startTime, NULL); 224 } 225 226 uint32_t ZFM20::getMillis() 227 { 228 struct timeval elapsed, now; 229 uint32_t elapse; 230 231 // get current time 232 gettimeofday(&now, NULL); 233 234 // compute the delta since m_startTime 235 if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 ) 236 { 237 elapsed.tv_usec += 1000000; 238 elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1; 239 } 240 else 241 { 242 elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec; 243 } 244 245 elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000)); 246 247 // never return 0 248 if (elapse == 0) 249 elapse = 1; 250 251 return elapse; 252 } 253 254 bool ZFM20::verifyPacket(uint8_t *pkt, int len) 255 { 256 // verify packet header 257 if (pkt[0] != ZFM20_START1 || pkt[1] != ZFM20_START2) 258 { 259 throw std::runtime_error(std::string(__FUNCTION__) + 260 ": Invalid packet header"); 261 return false; 262 } 263 264 // check the ack byte 265 if (pkt[6] != PKT_ACK) 266 { 267 throw std::runtime_error(std::string(__FUNCTION__) + 268 ": Invalid ACK code"); 269 return false; 270 } 271 272 return true; 273 } 274 275 bool ZFM20::getResponse(uint8_t *pkt, int len) 276 { 277 char buf[ZFM20_MAX_PKT_LEN]; 278 279 initClock(); 280 281 int idx = 0; 282 int timer = 0; 283 int rv; 284 int plen = 0; 285 286 while (idx < len) 287 { 288 // wait for some data 289 if (!dataAvailable(100)) 290 { 291 timer += getMillis(); 292 if (timer > ZFM20_TIMEOUT) 293 { 294 throw std::runtime_error(std::string(__FUNCTION__) + 295 ": Timed out waiting for packet"); 296 return false; 297 } 298 299 continue; 300 } 301 302 if ((rv = readData(buf, ZFM20_MAX_PKT_LEN)) == 0) 303 { 304 throw std::runtime_error(std::string(__FUNCTION__) + 305 ": readData() failed, no data returned"); 306 return false; 307 } 308 309 // copy it into the user supplied buffer 310 for (int i=0; i<rv; i++) 311 { 312 pkt[idx++] = buf[i]; 313 if (idx >= len) 314 break; 315 } 316 } 317 318 // now verify it. 319 return verifyPacket(pkt, len); 320 } 321 322 bool ZFM20::verifyPassword() 323 { 324 const int pktLen = 5; 325 uint8_t pkt[pktLen] = {CMD_VERIFY_PASSWORD, 326 static_cast<uint8_t>((m_password >> 24) & 0xff), 327 static_cast<uint8_t>((m_password >> 16) & 0xff), 328 static_cast<uint8_t>((m_password >> 8) & 0xff), 329 static_cast<uint8_t>(m_password & 0xff) }; 330 331 writeCmdPacket(pkt, pktLen); 332 333 // now read a response 334 const int rPktLen = 12; 335 uint8_t rPkt[rPktLen]; 336 337 getResponse(rPkt, rPktLen); 338 339 340 return true; 341 } 342 343 int ZFM20::getNumTemplates() 344 { 345 const int pktLen = 1; 346 uint8_t pkt[pktLen] = {CMD_GET_TMPL_COUNT}; 347 348 writeCmdPacket(pkt, pktLen); 349 350 // now read a response 351 const int rPktLen = 14; 352 uint8_t rPkt[rPktLen]; 353 354 getResponse(rPkt, rPktLen); 355 356 // check confirmation code 357 if (rPkt[9] != 0x00) 358 { 359 throw std::runtime_error(std::string(__FUNCTION__) + 360 ": Invalid confirmation code"); 361 return 0; 362 } 363 364 return ((rPkt[10] << 8) | rPkt[11]); 365 } 366 367 bool ZFM20::setNewPassword(uint32_t pwd) 368 { 369 const int pktLen = 5; 370 uint8_t pkt[pktLen] = {CMD_SET_PASSWORD, 371 static_cast<uint8_t>((pwd >> 24) & 0xff), 372 static_cast<uint8_t>((pwd >> 16) & 0xff), 373 static_cast<uint8_t>((pwd >> 8) & 0xff), 374 static_cast<uint8_t>(pwd & 0xff) }; 375 376 writeCmdPacket(pkt, pktLen); 377 378 // now read a response 379 const int rPktLen = 12; 380 uint8_t rPkt[rPktLen]; 381 382 getResponse(rPkt, rPktLen); 383 384 // check confirmation code 385 if (rPkt[9] != 0x00) 386 { 387 throw std::runtime_error(std::string(__FUNCTION__) + 388 ": Invalid confirmation code"); 389 return false; 390 } 391 392 m_password = pwd; 393 394 return true; 395 } 396 397 bool ZFM20::setNewAddress(uint32_t addr) 398 { 399 const int pktLen = 5; 400 uint8_t pkt[pktLen] = {CMD_SET_ADDRESS, 401 static_cast<uint8_t>((addr >> 24) & 0xff), 402 static_cast<uint8_t>((addr >> 16) & 0xff), 403 static_cast<uint8_t>((addr >> 8) & 0xff), 404 static_cast<uint8_t>(addr & 0xff) }; 405 406 writeCmdPacket(pkt, pktLen); 407 408 // now read a response 409 const int rPktLen = 12; 410 uint8_t rPkt[rPktLen]; 411 412 getResponse(rPkt, rPktLen); 413 414 // check confirmation code 415 if (rPkt[9] != 0x00) 416 { 417 throw std::runtime_error(std::string(__FUNCTION__) + 418 ": Invalid confirmation code"); 419 return false; 420 } 421 422 m_address = addr; 423 424 return true; 425 } 426 427 uint8_t ZFM20::generateImage() 428 { 429 const int pktLen = 1; 430 uint8_t pkt[pktLen] = {CMD_GEN_IMAGE}; 431 432 writeCmdPacket(pkt, pktLen); 433 434 // now read a response 435 const int rPktLen = 12; 436 uint8_t rPkt[rPktLen]; 437 438 getResponse(rPkt, rPktLen); 439 440 return rPkt[9]; 441 } 442 443 uint8_t ZFM20::image2Tz(int slot) 444 { 445 if (slot != 1 && slot != 2) 446 { 447 throw std::out_of_range(std::string(__FUNCTION__) + 448 ": slot must be 1 or 2"); 449 return ERR_INTERNAL_ERR; 450 } 451 452 const int pktLen = 2; 453 uint8_t pkt[pktLen] = {CMD_IMG2TZ, 454 static_cast<uint8_t>(slot & 0xff)}; 455 456 writeCmdPacket(pkt, pktLen); 457 458 // now read a response 459 const int rPktLen = 12; 460 uint8_t rPkt[rPktLen]; 461 462 getResponse(rPkt, rPktLen); 463 464 return rPkt[9]; 465 } 466 467 uint8_t ZFM20::createModel() 468 { 469 const int pktLen = 1; 470 uint8_t pkt[pktLen] = {CMD_REGMODEL}; 471 472 writeCmdPacket(pkt, pktLen); 473 474 // now read a response 475 const int rPktLen = 12; 476 uint8_t rPkt[rPktLen]; 477 478 getResponse(rPkt, rPktLen); 479 480 return rPkt[9]; 481 } 482 483 uint8_t ZFM20::storeModel(int slot, uint16_t id) 484 { 485 if (slot != 1 && slot != 2) 486 { 487 throw std::out_of_range(std::string(__FUNCTION__) + 488 ": slot must be 1 or 2"); 489 return ERR_INTERNAL_ERR; 490 } 491 492 const int pktLen = 4; 493 uint8_t pkt[pktLen] = {CMD_STORE, 494 static_cast<uint8_t>(slot & 0xff), 495 static_cast<uint8_t>((id >> 8) & 0xff), 496 static_cast<uint8_t>(id & 0xff)}; 497 498 writeCmdPacket(pkt, pktLen); 499 500 // now read a response 501 const int rPktLen = 12; 502 uint8_t rPkt[rPktLen]; 503 504 getResponse(rPkt, rPktLen); 505 506 return rPkt[9]; 507 } 508 509 uint8_t ZFM20::deleteModel(uint16_t id) 510 { 511 const int pktLen = 5; 512 uint8_t pkt[pktLen] = {CMD_DELETE_TMPL, 513 static_cast<uint8_t>((id >> 8) & 0xff), 514 static_cast<uint8_t>(id & 0xff), 515 0x00, 516 0x01}; 517 518 writeCmdPacket(pkt, pktLen); 519 520 // now read a response 521 const int rPktLen = 12; 522 uint8_t rPkt[rPktLen]; 523 524 getResponse(rPkt, rPktLen); 525 526 return rPkt[9]; 527 } 528 529 uint8_t ZFM20::deleteDB() 530 { 531 const int pktLen = 1; 532 uint8_t pkt[pktLen] = {CMD_EMPTYDB}; 533 534 writeCmdPacket(pkt, pktLen); 535 536 // now read a response 537 const int rPktLen = 12; 538 uint8_t rPkt[rPktLen]; 539 540 getResponse(rPkt, rPktLen); 541 542 return rPkt[9]; 543 } 544 545 uint8_t ZFM20::search(int slot, uint16_t *id, uint16_t *score) 546 { 547 *id = 0; 548 *score = 0; 549 550 if (slot != 1 && slot != 2) 551 { 552 throw std::out_of_range(std::string(__FUNCTION__) + 553 ": slot must be 1 or 2"); 554 return ERR_INTERNAL_ERR; 555 } 556 557 // search from page 0x0000 to page 0x00a3 558 const int pktLen = 6; 559 uint8_t pkt[pktLen] = {CMD_SEARCH, 560 static_cast<uint8_t>(slot & 0xff), 561 0x00, 562 0x00, 563 0x00, 564 0xa3}; 565 566 writeCmdPacket(pkt, pktLen); 567 568 // now read a response 569 const int rPktLen = 16; 570 uint8_t rPkt[rPktLen]; 571 572 getResponse(rPkt, rPktLen); 573 574 // if it was found, extract the location and the score 575 if (rPkt[9] == ERR_OK) 576 { 577 *id = ((rPkt[10] << 8) & 0xff) | (rPkt[11] & 0xff); 578 *score = ((rPkt[12] << 8) & 0xff) | (rPkt[13] & 0xff); 579 } 580 581 return rPkt[9]; 582 } 583 584 uint8_t ZFM20::match(uint16_t *score) 585 { 586 *score = 0; 587 588 const int pktLen = 1; 589 uint8_t pkt[pktLen] = {CMD_MATCH}; 590 591 writeCmdPacket(pkt, pktLen); 592 593 // now read a response 594 const int rPktLen = 14; 595 uint8_t rPkt[rPktLen]; 596 597 getResponse(rPkt, rPktLen); 598 599 *score = ((rPkt[10] << 8) & 0xff) | (rPkt[11] & 0xff); 600 601 return rPkt[9]; 602 } 603 604 605
