mirror of
https://github.com/eclipse/upm.git
synced 2025-03-16 05:27:28 +03:00

This commit addresses all warnings emitted from -Wunused-variable in the C++ src. Signed-off-by: Noel Eck <noel.eck@intel.com>
603 lines
13 KiB
C++
603 lines
13 KiB
C++
/*
|
|
* Author: Jon Trulson <jtrulson@ics.com>
|
|
* Copyright (c) 2015 Intel Corporation.
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining
|
|
* a copy of this software and associated documentation files (the
|
|
* "Software"), to deal in the Software without restriction, including
|
|
* without limitation the rights to use, copy, modify, merge, publish,
|
|
* distribute, sublicense, and/or sell copies of the Software, and to
|
|
* permit persons to whom the Software is furnished to do so, subject to
|
|
* the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be
|
|
* included in all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
|
|
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
|
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
|
|
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
|
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
|
|
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
*/
|
|
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <stdexcept>
|
|
|
|
#include "zfm20.hpp"
|
|
|
|
using namespace upm;
|
|
using namespace std;
|
|
|
|
static const int defaultDelay = 100; // max wait time for read
|
|
|
|
ZFM20::ZFM20(int uart)
|
|
{
|
|
m_ttyFd = -1;
|
|
|
|
if ( !(m_uart = mraa_uart_init(uart)) )
|
|
{
|
|
throw std::invalid_argument(std::string(__FUNCTION__) +
|
|
": mraa_uart_init() failed");
|
|
return;
|
|
}
|
|
|
|
// This requires a recent MRAA (1/2015)
|
|
const char *devPath = mraa_uart_get_dev_path(m_uart);
|
|
|
|
if (!devPath)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": mraa_uart_get_dev_path() failed");
|
|
return;
|
|
}
|
|
|
|
// now open the tty
|
|
if ( (m_ttyFd = open(devPath, O_RDWR)) == -1)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": open of " +
|
|
string(devPath) + " failed: " +
|
|
string(strerror(errno)));
|
|
return;
|
|
}
|
|
|
|
// Set the default password and address
|
|
setPassword(ZFM20_DEFAULT_PASSWORD);
|
|
setAddress(ZFM20_DEFAULT_ADDRESS);
|
|
|
|
initClock();
|
|
}
|
|
|
|
ZFM20::~ZFM20()
|
|
{
|
|
if (m_ttyFd != -1)
|
|
close(m_ttyFd);
|
|
|
|
mraa_deinit();
|
|
}
|
|
|
|
bool ZFM20::dataAvailable(unsigned int millis)
|
|
{
|
|
if (m_ttyFd == -1)
|
|
return false;
|
|
|
|
struct timeval timeout;
|
|
|
|
// no waiting
|
|
timeout.tv_sec = 0;
|
|
timeout.tv_usec = millis * 1000;
|
|
|
|
fd_set readfds;
|
|
|
|
FD_ZERO(&readfds);
|
|
|
|
FD_SET(m_ttyFd, &readfds);
|
|
|
|
if (select(m_ttyFd + 1, &readfds, NULL, NULL, &timeout) > 0)
|
|
return true; // data is ready
|
|
else
|
|
return false;
|
|
}
|
|
|
|
int ZFM20::readData(char *buffer, int len)
|
|
{
|
|
if (m_ttyFd == -1)
|
|
return(-1);
|
|
|
|
if (!dataAvailable(defaultDelay))
|
|
return 0; // timed out
|
|
|
|
int rv = read(m_ttyFd, buffer, len);
|
|
|
|
if (rv < 0)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": read() failed: " +
|
|
string(strerror(errno)));
|
|
return rv;
|
|
}
|
|
|
|
return rv;
|
|
}
|
|
|
|
int ZFM20::writeData(char *buffer, int len)
|
|
{
|
|
if (m_ttyFd == -1)
|
|
return(-1);
|
|
|
|
// first, flush any pending but unread input
|
|
tcflush(m_ttyFd, TCIFLUSH);
|
|
|
|
int rv = write(m_ttyFd, buffer, len);
|
|
|
|
if (rv < 0)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": write() failed: " +
|
|
string(strerror(errno)));
|
|
return rv;
|
|
}
|
|
|
|
if (rv == 0)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": write() failed, no bytes written");
|
|
return rv;
|
|
}
|
|
|
|
tcdrain(m_ttyFd);
|
|
|
|
return rv;
|
|
}
|
|
|
|
bool ZFM20::setupTty(speed_t baud)
|
|
{
|
|
if (m_ttyFd == -1)
|
|
return(false);
|
|
|
|
struct termios termio;
|
|
|
|
// get current modes
|
|
tcgetattr(m_ttyFd, &termio);
|
|
|
|
// setup for a 'raw' mode. 81N, no echo or special character
|
|
// handling, such as flow control.
|
|
cfmakeraw(&termio);
|
|
|
|
// set our baud rates
|
|
cfsetispeed(&termio, baud);
|
|
cfsetospeed(&termio, baud);
|
|
|
|
// make it so
|
|
if (tcsetattr(m_ttyFd, TCSAFLUSH, &termio) < 0)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": tcsetattr() failed: " +
|
|
string(strerror(errno)));
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
int ZFM20::writeCmdPacket(uint8_t *pkt, int len)
|
|
{
|
|
uint8_t rPkt[ZFM20_MAX_PKT_LEN];
|
|
|
|
rPkt[0] = ZFM20_START1; // header bytes
|
|
rPkt[1] = ZFM20_START2;
|
|
|
|
rPkt[2] = (m_address >> 24) & 0xff; // address
|
|
rPkt[3] = (m_address >> 16) & 0xff;
|
|
rPkt[4] = (m_address >> 8) & 0xff;
|
|
rPkt[5] = m_address & 0xff;
|
|
|
|
rPkt[6] = PKT_COMMAND;
|
|
|
|
rPkt[7] = ((len + 2) >> 8) & 0xff; // length (+ len bytes)
|
|
rPkt[8] = (len + 2) & 0xff;
|
|
|
|
// compute the starting checksum
|
|
uint16_t cksum = rPkt[7] + rPkt[8] + PKT_COMMAND;
|
|
|
|
int j = 9;
|
|
for (int i=0; i<len; i++)
|
|
{
|
|
rPkt[j] = pkt[i];
|
|
cksum += rPkt[j];
|
|
j++;
|
|
}
|
|
|
|
rPkt[j++] = (cksum >> 8) & 0xff; // store the cksum
|
|
rPkt[j++] = cksum & 0xff;
|
|
|
|
return writeData((char *)rPkt, j);
|
|
}
|
|
|
|
void ZFM20::initClock()
|
|
{
|
|
gettimeofday(&m_startTime, NULL);
|
|
}
|
|
|
|
uint32_t ZFM20::getMillis()
|
|
{
|
|
struct timeval elapsed, now;
|
|
uint32_t elapse;
|
|
|
|
// get current time
|
|
gettimeofday(&now, NULL);
|
|
|
|
// compute the delta since m_startTime
|
|
if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 )
|
|
{
|
|
elapsed.tv_usec += 1000000;
|
|
elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1;
|
|
}
|
|
else
|
|
{
|
|
elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec;
|
|
}
|
|
|
|
elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000));
|
|
|
|
// never return 0
|
|
if (elapse == 0)
|
|
elapse = 1;
|
|
|
|
return elapse;
|
|
}
|
|
|
|
bool ZFM20::verifyPacket(uint8_t *pkt, int len)
|
|
{
|
|
// verify packet header
|
|
if (pkt[0] != ZFM20_START1 || pkt[1] != ZFM20_START2)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": Invalid packet header");
|
|
return false;
|
|
}
|
|
|
|
// check the ack byte
|
|
if (pkt[6] != PKT_ACK)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": Invalid ACK code");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool ZFM20::getResponse(uint8_t *pkt, int len)
|
|
{
|
|
char buf[ZFM20_MAX_PKT_LEN];
|
|
|
|
initClock();
|
|
|
|
int idx = 0;
|
|
int timer = 0;
|
|
int rv;
|
|
|
|
while (idx < len)
|
|
{
|
|
// wait for some data
|
|
if (!dataAvailable(100))
|
|
{
|
|
timer += getMillis();
|
|
if (timer > ZFM20_TIMEOUT)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": Timed out waiting for packet");
|
|
return false;
|
|
}
|
|
|
|
continue;
|
|
}
|
|
|
|
if ((rv = readData(buf, ZFM20_MAX_PKT_LEN)) == 0)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": readData() failed, no data returned");
|
|
return false;
|
|
}
|
|
|
|
// copy it into the user supplied buffer
|
|
for (int i=0; i<rv; i++)
|
|
{
|
|
pkt[idx++] = buf[i];
|
|
if (idx >= len)
|
|
break;
|
|
}
|
|
}
|
|
|
|
// now verify it.
|
|
return verifyPacket(pkt, len);
|
|
}
|
|
|
|
bool ZFM20::verifyPassword()
|
|
{
|
|
const int pktLen = 5;
|
|
uint8_t pkt[pktLen] = {CMD_VERIFY_PASSWORD,
|
|
static_cast<uint8_t>((m_password >> 24) & 0xff),
|
|
static_cast<uint8_t>((m_password >> 16) & 0xff),
|
|
static_cast<uint8_t>((m_password >> 8) & 0xff),
|
|
static_cast<uint8_t>(m_password & 0xff) };
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
int ZFM20::getNumTemplates()
|
|
{
|
|
const int pktLen = 1;
|
|
uint8_t pkt[pktLen] = {CMD_GET_TMPL_COUNT};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 14;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
// check confirmation code
|
|
if (rPkt[9] != 0x00)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": Invalid confirmation code");
|
|
return 0;
|
|
}
|
|
|
|
return ((rPkt[10] << 8) | rPkt[11]);
|
|
}
|
|
|
|
bool ZFM20::setNewPassword(uint32_t pwd)
|
|
{
|
|
const int pktLen = 5;
|
|
uint8_t pkt[pktLen] = {CMD_SET_PASSWORD,
|
|
static_cast<uint8_t>((pwd >> 24) & 0xff),
|
|
static_cast<uint8_t>((pwd >> 16) & 0xff),
|
|
static_cast<uint8_t>((pwd >> 8) & 0xff),
|
|
static_cast<uint8_t>(pwd & 0xff) };
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
// check confirmation code
|
|
if (rPkt[9] != 0x00)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": Invalid confirmation code");
|
|
return false;
|
|
}
|
|
|
|
m_password = pwd;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool ZFM20::setNewAddress(uint32_t addr)
|
|
{
|
|
const int pktLen = 5;
|
|
uint8_t pkt[pktLen] = {CMD_SET_ADDRESS,
|
|
static_cast<uint8_t>((addr >> 24) & 0xff),
|
|
static_cast<uint8_t>((addr >> 16) & 0xff),
|
|
static_cast<uint8_t>((addr >> 8) & 0xff),
|
|
static_cast<uint8_t>(addr & 0xff) };
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
// check confirmation code
|
|
if (rPkt[9] != 0x00)
|
|
{
|
|
throw std::runtime_error(std::string(__FUNCTION__) +
|
|
": Invalid confirmation code");
|
|
return false;
|
|
}
|
|
|
|
m_address = addr;
|
|
|
|
return true;
|
|
}
|
|
|
|
uint8_t ZFM20::generateImage()
|
|
{
|
|
const int pktLen = 1;
|
|
uint8_t pkt[pktLen] = {CMD_GEN_IMAGE};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
return rPkt[9];
|
|
}
|
|
|
|
uint8_t ZFM20::image2Tz(int slot)
|
|
{
|
|
if (slot != 1 && slot != 2)
|
|
{
|
|
throw std::out_of_range(std::string(__FUNCTION__) +
|
|
": slot must be 1 or 2");
|
|
return ERR_INTERNAL_ERR;
|
|
}
|
|
|
|
const int pktLen = 2;
|
|
uint8_t pkt[pktLen] = {CMD_IMG2TZ,
|
|
static_cast<uint8_t>(slot & 0xff)};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
return rPkt[9];
|
|
}
|
|
|
|
uint8_t ZFM20::createModel()
|
|
{
|
|
const int pktLen = 1;
|
|
uint8_t pkt[pktLen] = {CMD_REGMODEL};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
return rPkt[9];
|
|
}
|
|
|
|
uint8_t ZFM20::storeModel(int slot, uint16_t id)
|
|
{
|
|
if (slot != 1 && slot != 2)
|
|
{
|
|
throw std::out_of_range(std::string(__FUNCTION__) +
|
|
": slot must be 1 or 2");
|
|
return ERR_INTERNAL_ERR;
|
|
}
|
|
|
|
const int pktLen = 4;
|
|
uint8_t pkt[pktLen] = {CMD_STORE,
|
|
static_cast<uint8_t>(slot & 0xff),
|
|
static_cast<uint8_t>((id >> 8) & 0xff),
|
|
static_cast<uint8_t>(id & 0xff)};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
return rPkt[9];
|
|
}
|
|
|
|
uint8_t ZFM20::deleteModel(uint16_t id)
|
|
{
|
|
const int pktLen = 5;
|
|
uint8_t pkt[pktLen] = {CMD_DELETE_TMPL,
|
|
static_cast<uint8_t>((id >> 8) & 0xff),
|
|
static_cast<uint8_t>(id & 0xff),
|
|
0x00,
|
|
0x01};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
return rPkt[9];
|
|
}
|
|
|
|
uint8_t ZFM20::deleteDB()
|
|
{
|
|
const int pktLen = 1;
|
|
uint8_t pkt[pktLen] = {CMD_EMPTYDB};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 12;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
return rPkt[9];
|
|
}
|
|
|
|
uint8_t ZFM20::search(int slot, uint16_t *id, uint16_t *score)
|
|
{
|
|
*id = 0;
|
|
*score = 0;
|
|
|
|
if (slot != 1 && slot != 2)
|
|
{
|
|
throw std::out_of_range(std::string(__FUNCTION__) +
|
|
": slot must be 1 or 2");
|
|
return ERR_INTERNAL_ERR;
|
|
}
|
|
|
|
// search from page 0x0000 to page 0x00a3
|
|
const int pktLen = 6;
|
|
uint8_t pkt[pktLen] = {CMD_SEARCH,
|
|
static_cast<uint8_t>(slot & 0xff),
|
|
0x00,
|
|
0x00,
|
|
0x00,
|
|
0xa3};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 16;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
// if it was found, extract the location and the score
|
|
if (rPkt[9] == ERR_OK)
|
|
{
|
|
*id = ((rPkt[10] << 8) & 0xff) | (rPkt[11] & 0xff);
|
|
*score = ((rPkt[12] << 8) & 0xff) | (rPkt[13] & 0xff);
|
|
}
|
|
|
|
return rPkt[9];
|
|
}
|
|
|
|
uint8_t ZFM20::match(uint16_t *score)
|
|
{
|
|
*score = 0;
|
|
|
|
const int pktLen = 1;
|
|
uint8_t pkt[pktLen] = {CMD_MATCH};
|
|
|
|
writeCmdPacket(pkt, pktLen);
|
|
|
|
// now read a response
|
|
const int rPktLen = 14;
|
|
uint8_t rPkt[rPktLen];
|
|
|
|
getResponse(rPkt, rPktLen);
|
|
|
|
*score = ((rPkt[10] << 8) & 0xff) | (rPkt[11] & 0xff);
|
|
|
|
return rPkt[9];
|
|
}
|
|
|
|
|