/************************************************************************
*									*
*       Maxon Digital EC Servoamplifier RS232 Protocol Interface        *
*									*
*           Copyright (C) 2009, Centre for Autonomous Systems           *
*           Copyright (C) 2009, University of New South Wales           *
*                   Copyright (C) 2009, John Zaitseff                   *
*									*
************************************************************************/

/*
  Author:  John Zaitseff <J.Zaitseff@unsw.edu.au>
  Date:    13th October, 2009
  Version: 1.1

  $Id$

  This file, "desapi.c", provides the implementation of an interface to
  the Maxon Digital EC Servoamplifier using the RS232 protocol.  This
  allows your program to communicate with a DES module (such as Maxon
  4QEC motor controllers) using a standard serial port.  The interface
  (API) has been based on the one provided by Maxon in their Windows
  DESCMD.DLL library, but is not identical.

  Please note that each library function is documented in the associated
  header file, "desapi.h".  Technically, each function should appear in
  its own file, then linked into a static or dynamic library.  However,
  each function is sufficiently small that the programmer overhead of
  doing so is not worth bothering about.  This source file uses the C99
  (ISO/IEC 9899:1999) dialect of the C programming language.


  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License along
  with this program; if not, write to the Free Software Foundation, Inc.,
  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/


#include <maxon/desapi.h>
#include <unistd.h>
#include <stdbool.h>
#include <inttypes.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <stdarg.h>
#include <alloca.h>


/************************************************************************
*                Internal constants and type definitions                *
************************************************************************/

// File version
static const char const *fileId __attribute__((unused))
    = "$Id$";


// DES Opcode constants
typedef enum des_opcodes {
    DES_OPCODE_ANSWER			= 0x00,
    DES_OPCODE_READSYSSTATUS		= 0x01,
    DES_OPCODE_READERROR		= 0x02,
    DES_OPCODE_CLEARERROR		= 0x03,
    DES_OPCODE_RESET			= 0x04,
    DES_OPCODE_ENABLE			= 0x05,
    DES_OPCODE_READTEMPPARAM		= 0x14,
    DES_OPCODE_SETTEMPPARAM		= 0x15,
    DES_OPCODE_RESETTEMPPARAM		= 0x16,
    DES_OPCODE_SAVETEMPPARAM		= 0x17,
    DES_OPCODE_READALLTEMPPARAM		= 0x18,
    DES_OPCODE_SETALLTEMPPARAM		= 0x19,
    DES_OPCODE_READVERSION		= 0x1A,
    DES_OPCODE_SYSPARSETDEFAULT		= 0x1B,
    DES_OPCODE_SETVELOCITY		= 0x21,
    DES_OPCODE_SETCURRENT		= 0x22,
    DES_OPCODE_STOPMOTION		= 0x23,
    DES_OPCODE_READVELOCITYISMUST	= 0x28,
    DES_OPCODE_READCURRENTISMUST	= 0x29,
    DES_OPCODE_SETUPRECORDER		= 0x30,
    DES_OPCODE_RECORDDATA		= 0x31,
    DES_OPCODE_READNVARIABLES		= 0x32
} des_opcode_t;

// DES Acknowledge constants
enum des_acknowledges {
    DES_ACK_OK		= 0x4F,
    DES_ACK_NOT_OK	= 0x46
};

// Internal delay constants
enum des_delays_int {
    DES_VTIME		= 75	// Serial port time-out, in deciseconds
};


/* The DES serial protocol uses CRC-CCITT (polynomial 0x1021, not
   reflected, initial 0x0000, final XOR 0x0000) as its CRC algorithm */
static const uint16_t des_crc_table[256] = {
    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
    0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
    0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
    0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
    0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
    0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
    0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
    0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
    0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
    0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
    0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
    0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
    0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
    0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
    0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
    0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
    0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
    0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
    0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
    0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
    0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
    0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
    0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
    0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
    0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
    0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
    0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
    0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
    0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
    0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
    0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
    0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};


/************************************************************************
*                     Internal function prototypes                      *
************************************************************************/

/*
  Function:   des_writepacket - Send a packet to the DES
  Parameters: fd          - file descriptor as returned by des_open()
              opcode      - DES_OPCODE_xxx constant to be sent
              isarray     - Is param1 an array?
              num         - number of parameters
              param1, ... - list of parameters
  Returns:    int         - 0 on success, -1 on error (with errno set)

  This function sends a complete data frame to the DES, as documented in
  the Communications Guide.  If isarray is true, the first parameter
  after num is taken to be an array of uint16_t, with num elements.
  Otherwise (if isarray is false), num parameters must be present
  (param1, param2, ..., param_num) in the argument list, each of type
  uint16_t or int16_t.

  If the packet was sent correctly (and acknowledged by the DES), zero is
  returned.  If an error occurs, -1 is returned and errno is set
  appropriately.  Possible errors (in errno) include, amongst others,
  EINVAL (invalid parameter) if num is outside the range 1..256, ENOMEM
  (out of memory) if space could not be allocated on the stack, ETIMEDOUT
  (connection timed out) if the DES did not respond in time and EPROTO
  (protocol error) if the DES sent a "NOT OK" response to a packet.

  Note that if the DES sends a "NOT OK" response (and hence EPROTO is
  returned in errno), the packet is NOT retransmitted.  It is up to the
  caller to retry transmission, with a suggested usleep(DES_EPROTO_DELAY)
  between retries.
*/
static int des_writepacket (int fd, des_opcode_t opcode, bool isarray,
			    int num, ...);


/*
  Function:   des_readpacket - Read a packet from the DES
  Parameters: fd          - file descriptor as returned by des_open()
              isarray     - Is param1 an array?
              num         - number of parameters
              param1, ... - list of parameters (pointers)
  Returns:    int         - 0 on success, -1 on error (with errno set)

  This function receives a complete data frame from the DES, as
  documented in the Communications Guide.  If isarray is true, the first
  parameter after num is taken to be an array of uint16_t, with num
  elements.  Otherwise (if isarray is false), num parameters must be
  present (param1, param2, ..., param_num) in the argument list, each of
  type uint16_t* or int16_t* (ie, pointer to uint16_t or int16_t).

  If the packet was received correctly, zero is returned.  If an error
  occurs, -1 is returned and errno is set appropriately.  Possible errors
  (in errno) include, amongst others, EINVAL (invalid parameter) if num
  is outside the range 1..256 or is more than the number of words sent by
  the DES, ENOMEM (out of memory) if space could not be allocated on the
  stack, ETIMEDOUT (connection timed out) if the DES did not respond in
  time and EPROTO (protocol error) if the CRC of the packet was invalid.

  Note that if the CRC of the packet was incorrect (ie, if the packet was
  corrupted in transmission), and hence EPROTO is returned in errno, the
  function does NOT wait for the DES to retransmit the packet.  It is up
  to the caller to retry receiving the packet, and/or retransmitting the
  original request packet after a suggested delay generated using either
  usleep(DES_EPROTO_DELAY) (short delay) or usleep(DES_TIMEOUT_DELAY)
  (long delay).
*/
static int des_readpacket (int fd, bool isarray, int num, ...);


/*
  Function:   des_crc_init - Initialise CRC algorithm
  Parameters: *crc - Pointer to 16-bit CRC value
  Returns:    void

  This function initialises the CRC algorithm used in the DES serial
  communications protocol.  It must be called before starting to transmit
  or receive a DES packet.
*/
static inline void des_crc_init (uint16_t *crc);


/*
  Function:   des_crc_addbyte - Add a byte to the CRC
  Parameters: *crc - Pointer to current CRC value
              b    - Byte to be added
  Returns:    void

  This function adds one byte to the current CRC value.
*/
static inline void des_crc_addbyte (uint16_t *crc, uint8_t b);


/*
  Function:   des_crc_addword - Add two bytes to the CRC
  Parameters: *crc - Pointer to current CRC value
              w    - 16-bit word to be added
  Returns:    void

  This function adds two bytes (one word) to the current CRC value.  As
  per the DES serial protocol specification, the high order byte is added
  first, then the low order byte.
*/
static inline void des_crc_addword (uint16_t *crc, uint16_t w);


/*
  Function:   des_crc_finish - Finalise the CRC value
  Parameters: *crc - Pointer to the current CRC value
  Returns:    void

  This function calculates the final CRC value.  It must be called after
  all bytes/words have been fed into the CRC (via des_crc_addbyte or
  des_crc_addword).
*/
static inline void des_crc_finish (uint16_t *crc);


/************************************************************************
*                           Library functions                           *
************************************************************************/

// These functions are documented in the file "desapi.h"


/***********************************************************************/
int des_open (const char *path, speed_t speed)
{
    int fd;
    struct termios term;

    fd = open(path, O_RDWR | O_SYNC | O_NOCTTY, S_IRUSR | S_IWUSR);
    if (fd < 0) {
	return fd;
    }

    if (tcgetattr(fd, &term) < 0) {
	close(fd);
	return -1;
    }

    // Set the desired serial port mode (similar to raw mode, with additions)
    term.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP |
		      INLCR | IGNCR | ICRNL | IXON | IXOFF);
    term.c_oflag &= ~OPOST;
    term.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
    term.c_cflag &= ~(CSIZE | PARENB | CSTOPB | CRTSCTS);
    term.c_cflag |= (CS8 | CREAD | CLOCAL);
    term.c_cc[VMIN]  = 0;		// Minimum number of chars to read
    term.c_cc[VTIME] = DES_VTIME;	// Timeout after DES_VTIME deciseconds

    cfsetispeed(&term, speed);
    cfsetospeed(&term, speed);

    if (tcsetattr(fd, TCSAFLUSH, &term) < 0) {
	close(fd);
	return -1;
    }

    if (tcflush(fd, TCIOFLUSH) < 0) {
	close(fd);
	return -1;
    }

    return fd;
}


/***********************************************************************/
int des_close (int fd)
{
    return close(fd);
}


/***********************************************************************/
int des_readsysstatus (int fd, uint16_t *sysstatus)
{
    if (des_writepacket(fd, DES_OPCODE_READSYSSTATUS, false, 1, 0) < 0) {
	return -1;
    }

    return des_readpacket(fd, false, 1, sysstatus);
}


/***********************************************************************/
int des_readerror (int fd, uint16_t *error)
{
    if (des_writepacket(fd, DES_OPCODE_READERROR, false, 1, 0) < 0) {
	return -1;
    }

    return des_readpacket(fd, false, 1, error);
}


/***********************************************************************/
int des_clearerror (int fd)
{
    return des_writepacket(fd, DES_OPCODE_CLEARERROR, false, 1, 0);
}


/***********************************************************************/
int des_reset (int fd)
{
    return des_writepacket(fd, DES_OPCODE_RESET, false, 1, 0);
}


/***********************************************************************/
int des_enable (int fd, int enable)
{
    return des_writepacket(fd, DES_OPCODE_ENABLE, false, 1,
			   enable ? DES_ENABLE : DES_DISABLE);
}


/***********************************************************************/
int des_readtempparam (int fd, des_parameter_t param, void *var)
{
    int dword = (param == DES_SV_POSDETVALUE);	// 32-bit parameter?
    uint16_t varHI, varLO;


    if (des_writepacket(fd, DES_OPCODE_READTEMPPARAM, false, 2, param,
			dword ? 1 : 0) < 0) {
	return -1;
    }

    if (dword) {
	// 32-bit parameter value expected
	if (des_readpacket(fd, false, 2, &varHI, &varLO) < 0) {
	    return -1;
	} else {
	    if (var) {
		*((uint32_t *) var) = ((uint32_t) varHI << 16) | varLO;
	    }
	    return 0;
	}
    } else {
	// 16-bit parameter value expected
	return des_readpacket(fd, false, 1, var);
    }
}


/***********************************************************************/
int des_settempparam (int fd, des_parameter_t param, int val)
{
    int dword = (param == DES_SV_POSDETVALUE);	// 32-bit parameter?
    uint16_t valHI, valLO;


    if (dword) {
	// 32-bit parameter value expected
	valHI = (uint16_t) ((unsigned int) val) >> 16;
	valLO = (uint16_t) val & 0xFFFF;

	return des_writepacket(fd, DES_OPCODE_SETTEMPPARAM, false, 4,
			       param, 1, valHI, valLO);
    } else {
	// 16-bit parameter value expected
	return des_writepacket(fd, DES_OPCODE_SETTEMPPARAM, false, 3,
			       param, 0, val);
    }
}


/***********************************************************************/
int des_resettempparam (int fd)
{
    return des_writepacket(fd, DES_OPCODE_RESETTEMPPARAM, false, 1, 0);
}


/***********************************************************************/
int des_savetempparam (int fd)
{
    return des_writepacket(fd, DES_OPCODE_SAVETEMPPARAM, false, 1, 0);
}


/***********************************************************************/
int des_readalltempparam (int fd, des_sysparam_t *param)
{
    if (! param) {
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    if (des_writepacket(fd, DES_OPCODE_READALLTEMPPARAM, false, 1, 0) < 0) {
	return -1;
    }

    /* It is awkward to list every element in the param struct, but this
       somewhat minimises the problem of packing between elements */
    return des_readpacket(fd, false, DES_SYSPARAM_T_MEMBERS,
			  &(param->baudrate),
			  &(param->sysconfig),
			  &(param->curreggainp),
			  &(param->curreggaini),
			  &(param->maxcuroutput),
			  &(param->speedreggainp),
			  &(param->speedreggaini),
			  &(param->posreggainp),
			  &(param->posreggaini),
			  &(param->posreggaind),
			  &(param->maxspeederror),
			  &(param->settingunitgain),
			  &(param->settingunitoffset),
			  &(param->settingunitdelay),
			  &(param->peakcurrent),
			  &(param->maxcontcurrent),
			  &(param->thermconst),
			  &(param->maxspeed),
			  &(param->acceleration),
			  &(param->speedconstant),
			  &(param->encresolution),
			  &(param->polepairnumber),
			  &(param->qep2rpmfactor),
			  &(param->rpm2qepfactor),
			  &(param->indexoffset),
			  &(param->pwmperiod),
			  &(param->maxdutycycle),
			  &(param->curdetphuoffset),
			  &(param->curdetphvoffset),
			  &(param->adconvoffset),
			  &(param->can_moduleid),
			  &(param->can_serviceid),
			  &(param->can_rpdoid),
			  &(param->can_tpdoid),
			  &(param->can_bcr1),
			  &(param->can_bcr2),
			  &(param->can_opmode),
			  &(param->can_rsdoid),
			  &(param->can_tsdoid),
			  &(param->can_rtr0id),
			  &(param->can_rtr1id),
			  &(param->can_config),
			  &(param->can_lifeguardtime),
			  &(param->errorproc),
			  &(param->maxspeedcur),
			  &(param->hallangleoffs),
			  &(param->maxanglempy1),
			  &(param->maxanglempyn));
}


/***********************************************************************/
int des_setalltempparam (int fd, const des_sysparam_t *param)
{
    if (! param) {
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    /* It is awkward to list every element in the param struct, but this
       somewhat minimises the problem of packing between elements */
    return des_writepacket(fd, DES_OPCODE_SETALLTEMPPARAM, false,
			   DES_SYSPARAM_T_MEMBERS,
			   param->baudrate,
			   param->sysconfig,
			   param->curreggainp,
			   param->curreggaini,
			   param->maxcuroutput,
			   param->speedreggainp,
			   param->speedreggaini,
			   param->posreggainp,
			   param->posreggaini,
			   param->posreggaind,
			   param->maxspeederror,
			   param->settingunitgain,
			   param->settingunitoffset,
			   param->settingunitdelay,
			   param->peakcurrent,
			   param->maxcontcurrent,
			   param->thermconst,
			   param->maxspeed,
			   param->acceleration,
			   param->speedconstant,
			   param->encresolution,
			   param->polepairnumber,
			   param->qep2rpmfactor,
			   param->rpm2qepfactor,
			   param->indexoffset,
			   param->pwmperiod,
			   param->maxdutycycle,
			   param->curdetphuoffset,
			   param->curdetphvoffset,
			   param->adconvoffset,
			   param->can_moduleid,
			   param->can_serviceid,
			   param->can_rpdoid,
			   param->can_tpdoid,
			   param->can_bcr1,
			   param->can_bcr2,
			   param->can_opmode,
			   param->can_rsdoid,
			   param->can_tsdoid,
			   param->can_rtr0id,
			   param->can_rtr1id,
			   param->can_config,
			   param->can_lifeguardtime,
			   param->errorproc,
			   param->maxspeedcur,
			   param->hallangleoffs,
			   param->maxanglempy1,
			   param->maxanglempyn);
}


/***********************************************************************/
int des_readversion (int fd, uint16_t *softversion, uint16_t *hardversion,
		     uint16_t *appnum, uint16_t *appversion)
{
    if (des_writepacket(fd, DES_OPCODE_READVERSION, false, 1, 0) < 0) {
	return -1;
    }

    return des_readpacket(fd, false, 4, softversion, hardversion,
			  appnum, appversion);
}


/***********************************************************************/
int des_setdefaultsysparam (int fd)
{
    return des_writepacket(fd, DES_OPCODE_SYSPARSETDEFAULT, false, 1, 0);
}


/***********************************************************************/
int des_setvelocity (int fd, int16_t velocity)
{
    return des_writepacket(fd, DES_OPCODE_SETVELOCITY, false, 1, velocity);
}


/***********************************************************************/
int des_setcurrent (int fd, int16_t current)
{
    return des_writepacket(fd, DES_OPCODE_SETCURRENT, false, 1, current);
}


/***********************************************************************/
int des_stopmotion (int fd)
{
    return des_writepacket(fd, DES_OPCODE_STOPMOTION, false, 1, 0);
}


/***********************************************************************/
int des_readvelocityismust (int fd, des_ismust_t type, int16_t *isvelocity,
			    int16_t *mustvelocity)
{
    if (des_writepacket(fd, DES_OPCODE_READVELOCITYISMUST, false, 1, type) < 0) {
	return -1;
    }

    return des_readpacket(fd, false, 2, isvelocity, mustvelocity);
}


/***********************************************************************/
int des_readcurrentismust (int fd, des_ismust_t type, int16_t *isqcurrent,
			   int16_t *isdcurrent, int16_t *mustcuramp)
{
    if (des_writepacket(fd, DES_OPCODE_READCURRENTISMUST, false, 1, type) < 0) {
	return -1;
    }

    return des_readpacket(fd, false, 3, isqcurrent, isdcurrent, mustcuramp);
}


/***********************************************************************/
int des_setuprecorder (int fd, uint16_t sampleperiod, des_parameter_t param)
{
    if (sampleperiod >= (DES_VTIME * 1000 / DES_RECORDEDDATA_T_SIZE)) {
	/* This is actually a restriction of the implementation: if we
           could wait longer between the received opcode byte and the
           rest of the packet in des_readpacket() (as called in
           des_recorddata()), we would not need this test. */
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    return des_writepacket(fd, DES_OPCODE_SETUPRECORDER, false, 2,
			   sampleperiod, param);
}


/***********************************************************************/
int des_recorddata (int fd, uint16_t jumpamp, des_recordeddata_t data)
{
    if (! data) {
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    if (des_writepacket(fd, DES_OPCODE_RECORDDATA, false, 1, jumpamp) < 0) {
	return -1;
    }

    return des_readpacket(fd, true, DES_RECORDEDDATA_T_SIZE, data);
}


/***********************************************************************/
int des_readnvariables (int fd, int num, const des_parameter_t params[],
			uint16_t data[])
{
    uint16_t *buf;
    int i;


    if ((! params) || (! data)) {
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    if ((num < 1) || (num > 256)) {
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    // Allocate buffer on stack
    buf = alloca((num + 1) * sizeof(uint16_t));
    if (! buf) {
	errno = ENOMEM;				// Out of memory
	return -1;
    }

    // Copy across params array, making room for num-1 as the first element
    buf[0] = num - 1;
    for (i = 0; i < num; i++) {
	buf[i + 1] = params[i];		/* Cannot use memcpy() as params are of
					   type int[], not of type uint16_t[] */
    }

    if (des_writepacket(fd, DES_OPCODE_READNVARIABLES, true, num + 1, buf) < 0) {
	return -1;
    }

    return des_readpacket(fd, true, num, data);
}


/************************************************************************
*                          Internal functions                           *
************************************************************************/

// These functions are documented at the start of this file


/***********************************************************************/
int des_writepacket (int fd, des_opcode_t opcode, bool isarray, int num, ...)
{
    va_list ap;
    uint8_t *buf;
    uint16_t crc;
    uint16_t val;
    uint16_t *args = NULL;
    int i, n, total;


    if ((num < 1) || (num > 256)) {
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    // Allocate buffer on stack
    buf = alloca(num * sizeof(uint16_t) + 4 * sizeof(uint8_t));
    if (! buf) {
	errno = ENOMEM;				// Out of memory
	return -1;
    }

    // Start processing the argument list
    va_start(ap, num);
    des_crc_init(&crc);

    // Flush the input buffer, just in case
    if (tcflush(fd, TCIFLUSH) < 0) {
	va_end(ap);
	return -1;
    }

    // First byte of packet: opcode
    buf[0] = (uint8_t) opcode;
    des_crc_addbyte(&crc, buf[0]);

    // Send the opcode byte to the serial port
    if (write(fd, buf, 1) < 1) {
	va_end(ap);
	return -1;
    }
    if (tcdrain(fd) < 0) {
	va_end(ap);
	return -1;
    }

    // Wait for the DES to respond
    n = read(fd, buf, 1);
    if (n < 0) {
	va_end(ap);
	return -1;
    } else if (n == 0) {
	va_end(ap);
	errno = ETIMEDOUT;			// Connection timed out
	return -1;
    }

    if (buf[0] != DES_ACK_OK) {
	va_end(ap);
	errno = EPROTO;				// Protocol error
	return -1;
    }

    // Assemble the rest of the packet

    buf[0] = (uint8_t) (num - 1);
    des_crc_addbyte(&crc, buf[0]);

    if (isarray) {
	args = va_arg(ap, uint16_t *);
    }

    for (i = 0; i < num; i++) {
	// Process each argument: add to buffer and calculate CRC
	if (isarray) {
	    val = args ? *args++ : 0;
	} else {
	    val = (uint16_t) va_arg(ap, unsigned int);
	}
	buf[i * sizeof(uint16_t) + 1] = (uint8_t) (val & 0x00FF);
	buf[i * sizeof(uint16_t) + 2] = (uint8_t) (val >> 8);
	des_crc_addword(&crc, val);
    }
    des_crc_finish(&crc);

    buf[num * sizeof(uint16_t) + 1] = (uint8_t) (crc & 0x00FF);
    buf[num * sizeof(uint16_t) + 2] = (uint8_t) (crc >> 8);

    // Send the rest of the packet
    total = num * sizeof(uint16_t) + 3;
    i = 0;
    while (i != total) {
	n = write(fd, buf + i, total - i);
	if (n < 0) {
	    va_end(ap);
	    return -1;
	} else {
	    i += n;
	}
    }
    if (tcdrain(fd) < 0) {
	va_end(ap);
	return -1;
    }

    // Wait for the DES to respond again, but not for DES_OPCODE_RESET
    if (opcode != DES_OPCODE_RESET) {
	n = read(fd, buf, 1);
	if (n < 0) {
	    va_end(ap);
	    return -1;
	} else if (n == 0) {
	    va_end(ap);
	    errno = ETIMEDOUT;			// Connection timed out
	    return -1;
	}

	if (buf[0] != DES_ACK_OK) {
	    va_end(ap);
	    errno = EPROTO;			// Protocol error
	    return -1;
	}
    }

    va_end(ap);
    return 0;
}


/***********************************************************************/
int des_readpacket (int fd, bool isarray, int num, ...)
{
    va_list ap;
    uint8_t *buf;
    uint16_t crc;
    uint16_t val;
    uint16_t *result = NULL;
    int i, n, actualnum;


    if ((num < 1) || (num > 256)) {
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    // Allocate buffer on stack
    buf = alloca(num * sizeof(uint16_t) + 4 * sizeof(uint8_t));
    if (! buf) {
	errno = ENOMEM;				// Out of memory
	return -1;
    }

    // Start processing the argument list
    va_start(ap, num);
    des_crc_init(&crc);

    // Read the opcode byte from the DES
    n = read(fd, buf, 1);
    if (n < 0) {
	va_end(ap);
	return -1;
    } else if (n == 0) {
	va_end(ap);
	errno = ETIMEDOUT;			// Connection timed out
	return -1;
    }

    if (buf[0] != DES_OPCODE_ANSWER) {
	/* Technically, if the first response byte is not DES_OPCODE_ANSWER,
	   the code should respond with DES_ACK_NOT_OK.  However, this might
	   be construed by the DES to be the start of another command (ie, an
	   opcode byte). Therefore, do not send anything. */

	va_end(ap);
	errno = EPROTO;				// Protocol error
	return -1;
    }

    des_crc_addbyte(&crc, buf[0]);

    // Send a response byte
    buf[0] = DES_ACK_OK;
    if (write(fd, buf, 1) < 1) {
	va_end(ap);
	return -1;
    }
    if (tcdrain(fd) < 0) {
	va_end(ap);
	return -1;
    }

    // Read the num-1 byte from the DES
    n = read(fd, buf, 1);
    if (n < 0) {
	va_end(ap);
	return -1;
    } else if (n == 0) {
	va_end(ap);
	errno = ETIMEDOUT;			// Connection timed out
	return -1;
    }

    actualnum = (int) buf[0] + 1;
    if (num > actualnum) {
	/* Technically, we should expect exactly num words of data as a
	   response.  However, various versions of the firmware sometimes
	   return more data than expected, particularly as a response to
	   DES_OPCODE_READALLTEMPPARAM */
	va_end(ap);
	errno = EINVAL;				// Invalid parameter
	return -1;
    }

    des_crc_addbyte(&crc, buf[0]);

    // Read the rest of the packet from the DES

    if (isarray) {
	result = va_arg(ap, uint16_t *);
    }

    for (i = 0; i < actualnum; i++) {

	// Read low byte
	n = read(fd, buf, 1);
	if (n < 0) {
	    va_end(ap);
	    return -1;
	} else if (n == 0) {
	    va_end(ap);
	    errno = ETIMEDOUT;			// Connection timed out
	    return -1;
	}

	val = buf[0];

	// Read high byte

	n = read(fd, buf, 1);
	if (n < 0) {
	    va_end(ap);
	    return -1;
	} else if (n == 0) {
	    va_end(ap);
	    errno = ETIMEDOUT;			// Connection timed out
	    return -1;
	}

	val |= ((uint16_t) buf[0]) << 8;

	des_crc_addword(&crc, val);

	if (i < num) {
	    if (isarray) {
		if (result) {
		    *result++ = val;
		}
	    } else {
		result = va_arg(ap, uint16_t *);
		if (result) {
		    *result = val;
		}
	    }
	}
    }

    // Read in the two-byte CRC from the DES

    n = read(fd, buf, 1);			// Low byte
    if (n < 0) {
	va_end(ap);
	return -1;
    } else if (n == 0) {
	va_end(ap);
	errno = ETIMEDOUT;			// Connection timed out
	return -1;
    }

    val = buf[0];

    n = read(fd, buf, 1);			// High byte
    if (n < 0) {
	va_end(ap);
	return -1;
    } else if (n == 0) {
	va_end(ap);
	errno = ETIMEDOUT;			// Connection timed out
	return -1;
    }

    val |= ((uint16_t) buf[0]) << 8;

    // Send a response byte
    buf[0] = (val == crc) ? DES_ACK_OK : DES_ACK_NOT_OK;
    if (write(fd, buf, 1) < 1) {
	va_end(ap);
	return -1;
    }
    if (tcdrain(fd) < 0) {
	va_end(ap);
	return -1;
    }

    if (val != crc) {
	va_end(ap);
	errno = EPROTO;				// Protocol error
	return -1;
    }

    va_end(ap);
    return 0;
}


/***********************************************************************/
void des_crc_init (uint16_t *crc)
{
    /* CCITT-CRC normally has an initial value of 0xFFFF; not so the
       version used for the DES serial protocol! */
    *crc = 0x0000;
}


/***********************************************************************/
void des_crc_addbyte (uint16_t *crc, uint8_t b)
{
    *crc = des_crc_table[((*crc >> 8) ^ b) & 0xFF] ^ (*crc << 8);
}


/***********************************************************************/
void des_crc_addword (uint16_t *crc, uint16_t w)
{
    /* DES protocol requires CRC to be calculated on high byte, low byte,
       even though transmission is done low byte, high byte */
    des_crc_addbyte(crc, (uint8_t) (w >> 8));
    des_crc_addbyte(crc, (uint8_t) (w & 0xFF));
}


/***********************************************************************/
void des_crc_finish (uint16_t *crc __attribute__((unused)))
{
    /* CCITT-CRC normally has a final XOR of 0xFFFF; not so the version
       used for the DES serial protocol! */
    ;
}


/***********************************************************************/
// End of file
