upm/src/zfm20/zfm20.cxx
Noel Eck a3a1fdc81b Wunused-variable: Fixed all unused variables in src
This commit addresses all warnings emitted from -Wunused-variable
in the C++ src.

Signed-off-by: Noel Eck <noel.eck@intel.com>
2016-11-02 21:44:27 -07:00

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];
}