first release

14 files changed, 2815 insertions, 0 deletions

diff --git a/README.md b/README.md
index 84aaa13..88b0006 100644
--- a/README.md
+++ b/README.md
@@ -2,3 +2,6 @@ SimpleMotionV2
 ==============
 
 SimpleMotion V2 library
+
+For more information, see:
+http://granitedevices.com/wiki/SimpleMotion_V2
diff --git a/SimpleMotionV2.pri b/SimpleMotionV2.pri
new file mode 100644
index 0000000..0885991
--- /dev/null
+++ b/SimpleMotionV2.pri
@@ -0,0 +1,12 @@
+

+INCLUDEPATH += $$PWD

+DEPENDPATH += $$PWD

+

+

+DEFINES += SIMPLEMOTIONV2_LIBRARY

+

+SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/rs232.c

+

+HEADERS += $$PWD/simplemotion_private.h\

+    $$PWD/rs232.h $$PWD/busdevice.h  $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h

+

diff --git a/SimpleMotionV2lib.pro b/SimpleMotionV2lib.pro
new file mode 100644
index 0000000..c908e95
--- /dev/null
+++ b/SimpleMotionV2lib.pro
@@ -0,0 +1,13 @@
+#-------------------------------------------------

+#

+# Project created by QtCreator 2013-11-14T13:11:57

+#

+#-------------------------------------------------

+

+include(SimpleMotionV2.pri)

+

+QT       -= gui

+

+TARGET = simplemotionv2

+TEMPLATE = lib

+

diff --git a/busdevice.c b/busdevice.c
new file mode 100644
index 0000000..9bf82c8
--- /dev/null
+++ b/busdevice.c
@@ -0,0 +1,173 @@
+#include "busdevice.h"
+
+#include "rs232.h"
+#include <string.h>
+
+#define BD_NONE 0
+#define BD_RS 1
+#define BD_FTDI 2
+
+//how much bytes available in transmit buffer
+#define TANSMIT_BUFFER_LENGTH 128
+
+typedef struct _SMBusDevice
+{
+	//common
+    smint8 bdType;//bus device type (such as rs232 or ftdi lib or mcu UART etc). 1=rs232 lib
+	smbool opened;
+
+    SM_STATUS cumulativeSmStatus;
+
+	//used for rs232 lib only
+	int comPort;
+
+    smuint8 txBuffer[TANSMIT_BUFFER_LENGTH];
+    smint32 txBufferUsed;//how many bytes in buffer currently
+
+	//used for FTDI lib only
+} SMBusDevice;
+
+//init on first open
+smbool bdInitialized=smfalse;
+SMBusDevice BusDevice[SM_MAX_BUSES];
+
+//init device struct table
+void smBDinit()
+{
+	int i;
+	for(i=0;i<SM_MAX_BUSES;i++)
+	{
+		BusDevice[i].bdType=BD_NONE;
+		BusDevice[i].opened=smfalse;
+        BusDevice[i].txBufferUsed=0;
+	}
+	bdInitialized=smtrue;
+}
+
+//ie "COM1" "VSD2USB"
+//return -1 if fails, otherwise handle number
+smbusdevicehandle smBDOpen( const char *devicename )
+{
+	int handle;
+
+	//true on first call
+	if(bdInitialized==smfalse)
+		smBDinit();
+
+	//find free handle
+	for(handle=0;handle<SM_MAX_BUSES;handle++)
+	{
+		if(BusDevice[handle].opened==smfalse) break;//choose this
+	}
+
+	//all handles in use
+	if(handle>=SM_MAX_BUSES) return -1;
+
+        if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0) //use rs232 lib
+	{
+            BusDevice[handle].comPort=OpenComport( devicename, SM_BAUDRATE );
+                if( BusDevice[handle].comPort == -1 )
+		{
+			return -1; //failed to open
+		}
+		BusDevice[handle].bdType=BD_RS;
+        BusDevice[handle].txBufferUsed=0;
+	}
+	else//no other bus types supproted yet
+	{
+		return -1;
+	}
+
+	//success
+    BusDevice[handle].cumulativeSmStatus=0;
+	BusDevice[handle].opened=smtrue;
+	return handle;
+}
+
+smbool smIsBDHandleOpen( const smbusdevicehandle handle )
+{
+	if(handle<0) return smfalse;
+	if(handle>=SM_MAX_BUSES) return smfalse;
+	return BusDevice[handle].opened;
+}
+
+//return true if ok
+smbool smBDClose( const smbusdevicehandle handle )
+{
+	//check if handle valid & open
+	if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
+
+	if( BusDevice[handle].bdType==BD_RS )
+	{
+		CloseComport( BusDevice[handle].comPort );
+		BusDevice[handle].opened=smfalse;
+		return smtrue;
+	}
+
+	return smfalse;
+}
+
+
+
+
+//write one byte to buffer and send later with smBDTransmit()
+//returns true on success
+smbool smBDWrite(const smbusdevicehandle handle, const smuint8 byte )
+{
+	//check if handle valid & open
+	if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
+
+	if( BusDevice[handle].bdType==BD_RS )
+	{
+        if(BusDevice[handle].txBufferUsed<TANSMIT_BUFFER_LENGTH)
+        {
+            //append to buffer
+            BusDevice[handle].txBuffer[BusDevice[handle].txBufferUsed]=byte;
+            BusDevice[handle].txBufferUsed++;
+            return smtrue;
+        }
+        else
+            return smfalse;
+	}
+
+	return smfalse;
+}
+
+smbool smBDTransmit(const smbusdevicehandle handle)
+{
+    //check if handle valid & open
+    if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
+
+    if( BusDevice[handle].bdType==BD_RS )
+    {
+        if(SendBuf(BusDevice[handle].comPort,BusDevice[handle].txBuffer, BusDevice[handle].txBufferUsed)==BusDevice[handle].txBufferUsed)
+        {
+            BusDevice[handle].txBufferUsed=0;
+            return smtrue;
+        }
+        else
+        {
+            BusDevice[handle].txBufferUsed=0;
+            return smfalse;
+        }
+    }
+    return smfalse;
+}
+
+//read one byte from bus. if byte not immediately available, block return up to SM_READ_TIMEOUT millisecs to wait data
+//returns true if byte read sucessfully
+smbool smBDRead( const smbusdevicehandle handle, smuint8 *byte )
+{
+	//check if handle valid & open
+	if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
+
+	if( BusDevice[handle].bdType==BD_RS )
+	{
+		int n;
+		n=PollComport(BusDevice[handle].comPort, byte, 1);
+		if( n!=1 ) return smfalse;
+		else return smtrue;
+	}
+
+	return smfalse;
+}
diff --git a/busdevice.h b/busdevice.h
new file mode 100644
index 0000000..580cda8
--- /dev/null
+++ b/busdevice.h
@@ -0,0 +1,35 @@
+
+//HW interface to phyiscal bus device
+
+#ifndef SM_BUSDEVICE
+#define SM_BUSDEVICE
+
+#include "simplemotion.h"
+#include "simplemotion_private.h"
+
+typedef smint8 smbusdevicehandle;
+
+#define SM_BAUDRATE 460800
+
+//ie "COM1" "VSD2USB"
+//return 0-1 if fails, otherwise handle number
+smbusdevicehandle smBDOpen( const char *devicename );
+
+//return true if ok
+smbool smBDClose( const smbusdevicehandle handle );
+
+//write one byte to trasmit buffer. send data with smBDTransmit()
+//returns smtrue on success. smfalse could mean buffer full error if forgot to call smBDTransmit
+smbool smBDWrite( const smbusdevicehandle handle , const smuint8 byte );
+
+//write transmit buffer to physical device
+//returns true on success
+smbool smBDTransmit(const smbusdevicehandle handle);
+
+//read one byte from bus. if byte not immediately available, block return up to SM_READ_TIMEOUT millisecs to wait data
+//returns true if byte read sucessfully
+smbool smBDRead( const smbusdevicehandle handle , smuint8 *byte );
+
+
+
+#endif
diff --git a/rs232.c b/rs232.c
new file mode 100644
index 0000000..1773767
--- /dev/null
+++ b/rs232.c
@@ -0,0 +1,366 @@
+/*

+***************************************************************************

+*

+* Author: Teunis van Beelen

+*

+* Copyright (C) 2005, 2006, 2007, 2008, 2009 Teunis van Beelen

+*

+* teuniz@gmail.com

+*

+***************************************************************************

+*

+* 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 version 2 of the License.

+*

+* 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.,

+* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

+*

+***************************************************************************

+*

+* This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt

+*

+***************************************************************************

+*/

+

+

+

+#include "rs232.h"

+#include "simplemotion_private.h"

+

+

+#ifdef __linux__   /* Linux */

+

+

+int OpenComport(const char * comport_name, int baudrate)

+{

+    int error;

+    int handle;

+	int baudr;

+        struct termios new_port_settings;

+

+	switch(baudrate)

+	{

+		case      50 : baudr = B50;

+		break;

+		case      75 : baudr = B75;

+		break;

+		case     110 : baudr = B110;

+		break;

+		case     134 : baudr = B134;

+		break;

+		case     150 : baudr = B150;

+		break;

+		case     200 : baudr = B200;

+		break;

+		case     300 : baudr = B300;

+		break;

+		case     600 : baudr = B600;

+		break;

+		case    1200 : baudr = B1200;

+		break;

+		case    1800 : baudr = B1800;

+		break;

+		case    2400 : baudr = B2400;

+		break;

+		case    4800 : baudr = B4800;

+		break;

+		case    9600 : baudr = B9600;

+		break;

+		case   19200 : baudr = B19200;

+		break;

+		case   38400 : baudr = B38400;

+		break;

+		case   57600 : baudr = B57600;

+		break;

+		case  115200 : baudr = B115200;

+		break;

+		case  230400 : baudr = B230400;

+		break;

+		case  460800 : baudr = B460800;

+		break;

+		case  500000 : baudr = B500000;

+		break;

+		case  576000 : baudr = B576000;

+		break;

+		case  921600 : baudr = B921600;

+		break;

+		case 1000000 : baudr = B1000000;

+		break;

+		default      : printf("invalid baudrate\n");

+		return(1);

+		break;

+	}

+

+	//Cport[comport_number] = open(comports[comport_number], O_RDWR | O_NOCTTY | O_NDELAY);

+        handle = open(comport_name, O_RDWR | O_NOCTTY );

+        if(handle==-1)

+	{

+		perror("unable to open comport ");

+                return(handle);

+	}

+

+    memset(&new_port_settings, 0, sizeof(new_port_settings));  /* clear the new struct */

+

+	new_port_settings.c_cflag = baudr | CS8 | CLOCAL | CREAD;

+	new_port_settings.c_iflag = IGNPAR;

+	new_port_settings.c_oflag = 0;

+	new_port_settings.c_lflag = 0;

+	new_port_settings.c_cc[VMIN] = 0;      /* block untill n bytes are received */

+	new_port_settings.c_cc[VTIME] = readTimeoutMs/100;     /* block untill a timer expires (n * 100 mSec.) */

+        error = tcsetattr(handle, TCSANOW, &new_port_settings);

+	if(error==-1)

+	{

+                close(handle);

+                perror("unable to adjust portsettings ");

+                return(-1);

+	}

+

+        return(handle);

+}

+

+

+int PollComport(int comport_number, unsigned char *buf, int size)

+{

+	int n;

+

+	#ifndef __STRICT_ANSI__                       /* __STRICT_ANSI__ is defined when the -ansi option is used for gcc */

+	if(size>SSIZE_MAX)  size = (int)SSIZE_MAX;  /* SSIZE_MAX is defined in limits.h */

+		#else

+		if(size>4096)  size = 4096;

+		#endif

+

+                n = read(comport_number, buf, size);

+

+	return(n);

+}

+

+

+int SendByte(int comport_number, unsigned char byte)

+{

+	int n;

+

+        n = write(comport_number, &byte, 1);

+	if(n<0)  return(1);

+

+	return(0);

+}

+

+

+int SendBuf(int comport_number, unsigned char *buf, int size)

+{

+        return(write(comport_number, buf, size));

+}

+

+

+void CloseComport(int comport_number)

+{

+        close(comport_number);

+        //feature removed, not restorint old settings :

+        //tcsetattr(comport_number, TCSANOW, old_port_settings + comport_number);

+}

+

+/*

+Constant  Description

+TIOCM_LE  DSR (data set ready/line enable)

+TIOCM_DTR DTR (data terminal ready)

+TIOCM_RTS RTS (request to send)

+TIOCM_ST  Secondary TXD (transmit)

+TIOCM_SR  Secondary RXD (receive)

+TIOCM_CTS CTS (clear to send)

+TIOCM_CAR DCD (data carrier detect)

+TIOCM_CD  Synonym for TIOCM_CAR

+TIOCM_RNG RNG (ring)

+TIOCM_RI  Synonym for TIOCM_RNG

+TIOCM_DSR DSR (data set ready)

+*/

+

+int IsCTSEnabled(int comport_number)

+{

+	int status;

+

+        status = ioctl(comport_number, TIOCMGET, &status);

+

+	if(status&TIOCM_CTS) return(1);

+	else return(0);

+}

+

+

+#else         /* windows */

+

+#ifdef DEBUG_COMPORT

+//for finding problem with windows calls

+void print_win32_system_error(char *name) {

+    // Retrieve, format, and print out a message from the last error.  The

+    // `name' that's passed should be in the form of a present tense noun

+    // (phrase) such as "opening file".

+    //

+    char *ptr = NULL;

+    FormatMessage(

+                FORMAT_MESSAGE_ALLOCATE_BUFFER |

+                FORMAT_MESSAGE_FROM_SYSTEM,

+                0,

+                GetLastError(),

+                0,

+                (char *)&ptr,

+                1024,

+                NULL);

+

+    fprintf(stderr, "\nError %s: %ls\n", name, ptr);

+    LocalFree(ptr);

+}

+#endif

+

+int OpenComport(const char * comport_name, int baudrate)

+{

+

+    char baudr[64], portname[64];

+    HANDLE handle;

+

+

+    switch(baudrate)

+    {

+    case     110 : strcpy(baudr, "baud=110 data=8 parity=N stop=1");

+        break;

+    case     300 : strcpy(baudr, "baud=300 data=8 parity=N stop=1");

+        break;

+    case     600 : strcpy(baudr, "baud=600 data=8 parity=N stop=1");

+        break;

+    case    1200 : strcpy(baudr, "baud=1200 data=8 parity=N stop=1");

+        break;

+    case    2400 : strcpy(baudr, "baud=2400 data=8 parity=N stop=1");

+        break;

+    case    4800 : strcpy(baudr, "baud=4800 data=8 parity=N stop=1");

+        break;

+    case    9600 : strcpy(baudr, "baud=9600 data=8 parity=N stop=1");

+        break;

+    case   19200 : strcpy(baudr, "baud=19200 data=8 parity=N stop=1");

+        break;

+    case   38400 : strcpy(baudr, "baud=38400 data=8 parity=N stop=1");

+        break;

+    case   57600 : strcpy(baudr, "baud=57600 data=8 parity=N stop=1");

+        break;

+    case  115200 : strcpy(baudr, "baud=115200 data=8 parity=N stop=1");

+        break;

+    case  128000 : strcpy(baudr, "baud=128000 data=8 parity=N stop=1");

+        break;

+    case  256000 : strcpy(baudr, "baud=256000 data=8 parity=N stop=1");

+        break;

+    case  460800 : strcpy(baudr, "baud=460800 data=8 parity=N stop=1");

+        break;

+    default      : printf("invalid baudrate\n");

+        return(-1);

+        break;

+    }

+

+    strcpy(portname,"\\\\.\\");

+    strcat(portname,comport_name);

+

+    handle = CreateFileA(portname,

+                         GENERIC_READ|GENERIC_WRITE,

+                         0,                          /* no share  */

+                         NULL,                       /* no security */

+                         OPEN_EXISTING,

+                         0,                          /* no threads */

+                         NULL);                      /* no templates */

+

+    if(handle==INVALID_HANDLE_VALUE)

+    {

+        printf("unable to open comport\n");

+        return(-1);

+    }

+

+    DCB port_settings;

+    memset(&port_settings, 0, sizeof(port_settings));  /* clear the new struct  */

+    port_settings.DCBlength = sizeof(port_settings);

+

+    if(!BuildCommDCBA(baudr, &port_settings))

+    {

+        printf("unable to set comport dcb settings\n");

+        CloseHandle(handle);

+        return(-1);

+    }

+

+    if(!SetCommState(handle, &port_settings))

+    {

+        printf("unable to set comport cfg settings\n");

+        CloseHandle(handle);

+        return(-1);

+    }

+

+    COMMTIMEOUTS Cptimeouts;

+

+    Cptimeouts.ReadIntervalTimeout         = 0;

+    Cptimeouts.ReadTotalTimeoutMultiplier  = 0;

+    Cptimeouts.ReadTotalTimeoutConstant    = readTimeoutMs;

+    Cptimeouts.WriteTotalTimeoutMultiplier = 50;

+    Cptimeouts.WriteTotalTimeoutConstant   = 50;

+

+

+    if(!SetCommTimeouts(handle, &Cptimeouts))

+    {

+        printf("unable to set comport time-out settings\n");

+        CloseHandle(handle);

+        return(-1);

+    }

+

+    return( (int)handle);

+}

+

+

+int PollComport(int comport_number, unsigned char *buf, int size)

+{

+    int n;

+

+    if(size>4096)  size = 4096;

+

+    /* added the void pointer cast, otherwise gcc will complain about */

+    /* "warning: dereferencing type-punned pointer will break strict aliasing rules" */

+

+    ReadFile((HANDLE)comport_number, buf, size, (LPDWORD)((void *)&n), NULL);

+

+    return(n);

+}

+

+

+int SendByte(int comport_number, unsigned char byte)

+{

+    int n;

+

+    WriteFile((HANDLE)comport_number, &byte, 1, (LPDWORD)((void *)&n), NULL);

+

+    if(n<0)  return(1);

+

+    return(0);

+}

+

+

+

+int SendBuf(int comport_number, unsigned char *buf, int size)

+{

+    int n;

+

+    if(WriteFile((HANDLE)comport_number, buf, size, (LPDWORD)((void *)&n), NULL))

+    {

+        return(n);

+    }

+

+    return(-1);

+}

+

+

+void CloseComport(int comport_number)

+{

+    CloseHandle((HANDLE)comport_number);

+}

+

+

+

+#endif

+

diff --git a/rs232.h b/rs232.h
new file mode 100644
index 0000000..7c90fea
--- /dev/null
+++ b/rs232.h
@@ -0,0 +1,81 @@
+/*

+***************************************************************************

+*

+* Author: Teunis van Beelen

+*

+* Copyright (C) 2005, 2006, 2007, 2008, 2009 Teunis van Beelen

+*

+* teuniz@gmail.com

+*

+***************************************************************************

+*

+* 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 version 2 of the License.

+*

+* 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.,

+* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

+*

+***************************************************************************

+*

+* This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt

+*

+***************************************************************************

+*/

+

+/* libraray modified for Simplemotion, major changes:

+  -OpenComport now returns actual file handle that must be passed to rx/tx functions, or -1 if fails

+

+  Todo:

+  -Restore port settings at CloseComport

+  */

+

+#ifndef rs232_INCLUDED

+#define rs232_INCLUDED

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+#include <stdio.h>

+#include <string.h>

+

+

+

+#ifdef __linux__

+

+#include <termios.h>

+#include <sys/ioctl.h>

+#include <unistd.h>

+#include <fcntl.h>

+#include <sys/types.h>

+#include <sys/stat.h>

+#include <limits.h>

+

+#else

+

+#include <windows.h>

+

+#endif

+

+//return port handle or -1 if fails

+int OpenComport(const char * comport_name, int baudrate);

+int PollComport(int, unsigned char *, int);

+int SendByte(int, unsigned char);

+int SendBuf(int, unsigned char *, int);

+void CloseComport(int);

+

+

+#ifdef __cplusplus

+} /* extern "C" */

+#endif

+

+#endif

+

+

diff --git a/simplemotion.c b/simplemotion.c
new file mode 100644
index 0000000..5b09860
--- /dev/null
+++ b/simplemotion.c
@@ -0,0 +1,877 @@
+//Copyright (c) Granite Devices Oy

+

+/*

+     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; version 2 of the License.

+

+     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.

+*/

+

+#include <stdio.h>

+#include <string.h>

+

+#include "busdevice.h"

+#include "sm485.h"

+#include <stdarg.h>

+

+

+#include "simplemotion_private.h"

+

+SM_STATUS smParseReturnData( smbus handle, smuint8 data );

+

+#define HANDLE_STAT(stat) if(stat!=SM_OK)return (stat);

+#define HANDLE_STAT_AND_RET(stat,returndata) { if(returndata==RET_INVALID_CMD||returndata==RET_INVALID_PARAM) return SM_ERR_PARAMETER; if(stat!=SM_OK) return (stat); }

+

+enum RecvState {WaitCmdId,WaitAddr,WaitPayloadSize,WaitPayload,WaitCrcHi,WaitCrcLo};

+FILE *smDebugOut=NULL;

+

+//useful macros from extracting/storing multibyte values from/to byte buffer

+#define bufput32bit(buf, pos, val) *((smuint32*)(smuint8*)((buf)+(pos)))=((smuint32)(val))

+#define bufput16bit(buf, pos, val) *((smuint16*)(smuint8*)((buf)+(pos)))=((smuint16)(val))

+#define bufput8bit(buf, pos, val) *((smuint8*)(smuint8*)((buf)+(pos)))=((smuint8)(val))

+#define bufget32bit(buf, pos) (*((smuint32*)(smuint8*)((buf)+(pos))))

+#define bufget16bit(buf, pos) (*((smuint16*)(smuint8*)((buf)+(pos))))

+#define bufget8bit(buf, pos) (*((smuint8*)(smuint8*)((buf)+(pos))))

+

+

+smbool smIsHandleOpen( const smbus handle );

+

+SM_STATUS smReceiveReturnPacket( smbus bushandle );

+

+typedef struct SM_BUS_

+{

+    smint8 bdHandle;

+    smbool opened;

+

+    enum RecvState recv_state,recv_state_next;

+

+    smint16 recv_payloadsize;

+    smint16 recv_storepos;

+    smuint8 recv_rsbuf[SM485_RSBUFSIZE];

+    smuint8 recv_cmdid;

+    smuint8 recv_addr;

+    smuint16 recv_crc;

+    smuint16 recv_read_crc_hi;

+    smbool receiveComplete;

+    smbool transmitBufFull;//set true if user uploads too much commands in one SM transaction. if true, on execute commands, nothing will be sent to bus to prevent unvanted clipped commands and buffer will be cleared

+    char busDeviceName[SM_BUSDEVICENAME_LEN];

+

+    smint16 cmd_send_queue_bytes;//for queued device commands

+    smint16 cmd_recv_queue_bytes;//recv_queue_bytes counted upwards at every smGetQueued.. and compared to payload size

+

+

+    SM_STATUS cumulativeSmStatus;

+} SM_BUS;

+

+

+SM_BUS smBus[SM_MAX_BUSES];

+smuint16 readTimeoutMs=SM_READ_TIMEOUT;

+

+//init on first smOpenBus call

+smbool smInitialized=smfalse;

+

+//if debug message has priority this or above will be printed to debug stream

+smVerbosityLevel smDebugThreshold=Trace;

+

+#ifdef ENABLE_DEBUG_PRINTS

+void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...)

+{

+    va_list fmtargs;

+    char buffer[1024];

+

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return;

+

+    if(smDebugOut!=NULL && verbositylevel <= smDebugThreshold )

+    {

+        va_start(fmtargs,format);

+        vsnprintf(buffer,sizeof(buffer)-1,format,fmtargs);

+        va_end(fmtargs);

+        fprintf(smDebugOut,"%s: %s",smBus[handle].busDeviceName, buffer);

+    }

+

+}

+#else

+#define smDebug(...)

+#endif

+

+void smResetSM485variables(smbus handle)

+{

+    smBus[handle].recv_state=WaitCmdId;

+    smBus[handle].recv_state_next=WaitCmdId;

+    smBus[handle].recv_payloadsize=-1;

+    smBus[handle].recv_storepos=0;//number of bytes to expect data in cmd, -1=wait cmd header

+    smBus[handle].recv_cmdid=0;// cmdid=0 kun ed komento suoritettu

+    smBus[handle].recv_addr=255;

+    smBus[handle].recv_crc=SM485_CRCINIT;

+    smBus[handle].recv_read_crc_hi=0xffff;//bottom bits will be contains only 1 byte when read

+    smBus[handle].receiveComplete=smfalse;

+    smBus[handle].transmitBufFull=smfalse;

+    smBus[handle].cmd_send_queue_bytes=0;

+    smBus[handle].cmd_recv_queue_bytes=0;

+}

+

+

+smuint16 calcCRC16(smuint8 data, smuint16 crc)

+{

+    unsigned int i; /* will index into CRC lookup */

+

+    i = (crc>>8) ^ data; /* calculate the CRC  */

+    crc = (((crc&0xff) ^ table_crc16_hi[i])<<8) | table_crc16_lo[i];

+

+    return crc;

+}

+

+smuint16 calcCRC16Buf(const char *buffer, smuint16 buffer_length)

+{

+    smuint8 crc_hi = 0xFF; /* high CRC byte initialized */

+    smuint8 crc_lo = 0xFF; /* low CRC byte initialized */

+    unsigned int i; /* will index into CRC lookup */

+

+    /* pass through message buffer */

+    while (buffer_length--) {

+        i = crc_hi ^ *buffer++; /* calculate the CRC  */

+        crc_hi = crc_lo ^ table_crc16_hi[i];

+        crc_lo = table_crc16_lo[i];

+    }

+

+    return (crc_hi << 8 | crc_lo);

+}

+

+SM_STATUS smSetTimeout( smuint16 millsecs )

+{

+    if(millsecs<=5000)

+    {

+        readTimeoutMs=millsecs;

+        return SM_OK;

+    }

+    return SM_ERR_PARAMETER;

+}

+

+unsigned long smGetVersion()

+{

+    return SM_VERSION;

+}

+

+

+//init bus struct table

+void smBusesInit()

+{

+    int i;

+    for(i=0;i<SM_MAX_BUSES;i++)

+    {

+        smBus[i].opened=smfalse;;

+        smResetSM485variables(i);

+    }

+    smInitialized=smtrue;

+}

+

+smbool smIsHandleOpen( const smbus handle )

+{

+    if(handle<0) return smfalse;

+    if(handle>=SM_MAX_BUSES) return smfalse;

+    return smBus[handle].opened;

+}

+

+

+/** Open SM RS485 communication bus. Parameters:

+-devicename: on Windows COM port as "COMx" or on unix /dev/ttySx or /dev/ttyUSBx where x=port number

+-return value: integer handle to be used with all other commands, -1 if fails

+*/

+smbus smOpenBus( const char * devicename )

+{

+    int handle;

+

+    //true on first call

+    if(smInitialized==smfalse)

+        smBusesInit();

+

+    //find free handle

+    for(handle=0;handle<SM_MAX_BUSES;handle++)

+    {

+        if(smBus[handle].opened==smfalse) break;//choose this

+    }

+    //all handles in use

+    if(handle>=SM_MAX_BUSES) return -1;

+

+    //open bus device

+    smBus[handle].bdHandle=smBDOpen(devicename);

+    if(smBus[handle].bdHandle==-1) return -1;

+

+    //success

+    strncpy( smBus[handle].busDeviceName, devicename, SM_BUSDEVICENAME_LEN );

+    smBus[handle].busDeviceName[SM_BUSDEVICENAME_LEN-1]=0;//null terminate string

+    smBus[handle].opened=smtrue;

+    return handle;

+}

+

+/** Close connection to given bus handle number. This frees communication link therefore makes it available for other apps for opening.

+-return value: a SM_STATUS value, i.e. SM_OK if command succeed

+*/

+LIB SM_STATUS smCloseBus( const smbus bushandle )

+{

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);

+

+    smBus[bushandle].opened=smfalse;

+

+    if( smBDClose(smBus[bushandle].bdHandle) == smfalse ) return recordStatus(bushandle,SM_ERR_BUS);

+

+    return recordStatus(bushandle,SM_OK);

+}

+

+char *cmdidToStr(smuint8 cmdid )

+{

+    char *str;

+    switch(cmdid)

+    {

+    case SMCMD_INSTANT_CMD : str="SMCMD_INSTANT_CMD";break;

+    case SMCMD_INSTANT_CMD_RET :str="SMCMD_INSTANT_CMD_RET";break;

+    case SMCMD_BUFFERED_CMD :str="SMCMD_BUFFERED_CMD";break;

+    case SMCMD_BUFFERED_RETURN_DATA :str="SMCMD_BUFFERED_RETURN_DATA";break;

+    case SMCMD_BUFFERED_RETURN_DATA_RET :str="SMCMD_BUFFERED_RETURN_DATA_RET";break;

+    case SMCMD_BUFFERED_CMD_RET:str="SMCMD_BUFFERED_CMD_RET";break;

+#ifdef PROCESS_IMAGE_SUPPORT

+    case SMCMD_PROCESS_IMAGE:str="SMCMD_PROCESS_IMAGE";break;

+    case SMCMD_PROCESS_IMAGE_RET: str="SMCMD_PROCESS_IMAGE_RET";break;

+#endif

+    case SMCMD_GET_CLOCK: str="SMCMD_GET_CLOCK";break;

+    case SMCMD_GET_CLOCK_RET :str="SMCMD_GET_CLOCK_RET";break;

+    default: str="unknown cmdid";break;

+    }

+    //puts(str);

+    return str;

+}

+

+//write one byte to tx buffer

+//returns true on success

+smbool smWriteByte( const smbus handle, const smuint8 byte, smuint16 *crc )

+{

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    smbool success=smBDWrite(smBus[handle].bdHandle,byte);

+    if(crc!=NULL)

+        *crc = calcCRC16(byte,*crc);

+

+    if(success==smtrue)

+        smDebug(handle, High, "  sent byte %02x\n",byte);

+    else

+        smDebug(handle, High, "  sending byte %02x failed\n",byte);

+

+    return success;

+}

+

+//write tx buffer to bus

+//returns true on success

+smbool smTransmitBuffer( const smbus handle )

+{

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    smbool success=smBDTransmit(smBus[handle].bdHandle);

+    return success;

+}

+

+SM_STATUS smSendSMCMD( smbus handle, smuint8 cmdid, smuint8 addr, smuint8 datalen, smuint8 *cmddata )

+{

+    int i;

+    smuint8 data;

+    smuint16 sendcrc;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    sendcrc=SM485_CRCINIT;

+

+    smDebug(handle, Mid, "> %s (id=%d, addr=%d, payload=%d)\n",cmdidToStr(cmdid),cmdid,

+            addr,

+            datalen);

+

+

+    smDebug(handle,High,"ID ");

+    if( smWriteByte(handle,cmdid, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

+

+    if(cmdid&SMCMD_MASK_N_PARAMS)

+    {

+        smDebug(handle,High,"Nparams ");

+        if( smWriteByte(handle,datalen, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

+    }

+

+    smDebug(handle,High,"ADDR ");

+    if( smWriteByte(handle,addr, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

+

+    for(i=0;i<datalen;i++)

+    {

+        smDebug(handle,High,"DATA ");

+        if( smWriteByte(handle,cmddata[i], &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

+    }

+    smDebug(handle,High,"CRC ");

+    if( smWriteByte(handle,sendcrc>>8, NULL)  != smtrue ) return recordStatus(handle,SM_ERR_BUS);

+    smDebug(handle,High,"CRC ");

+    if( smWriteByte(handle,sendcrc&0xff,NULL) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

+

+    //transmit bytes to bus that were written in buffer by smWriteByte calls

+    if( smTransmitBuffer(handle) != smtrue ) return recordStatus(handle,SM_ERR_BUS);

+

+    return recordStatus(handle,SM_OK);

+}

+

+

+

+

+SM_STATUS smReceiveErrorHandler( smbus handle, smbool flushrx )

+{

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+

+    //empty pending rx buffer to avoid further parse errors

+    if(flushrx==smtrue)

+    {

+        smbool success;

+        do{

+            smuint8 rx;

+            success=smBDRead(handle,&rx);

+        }while(success==smtrue);

+    }

+    smResetSM485variables(handle);

+    smBus[handle].receiveComplete=smtrue;

+    return recordStatus(handle,SM_ERR_COMMUNICATION);

+}

+

+

+SM_STATUS smAppendSMCommandToQueue( smbus handle, int smpCmdType,smint32 paramvalue  )

+{

+    smuint8 txbyte;

+    int cmdlength;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    switch(smpCmdType)

+    {

+    case SMPCMD_SETPARAMADDR:

+        cmdlength=2;

+        break;

+    case SMPCMD_24B:

+        cmdlength=3;

+        break;

+    case SMPCMD_32B:

+        cmdlength=4;

+        break;

+    default:

+        return recordStatus(handle,SM_ERR_PARAMETER);

+        break;

+    }

+

+    //check if space if buffer

+    if(smBus[handle].cmd_send_queue_bytes>(SM485_MAX_PAYLOAD_BYTES-cmdlength) )

+    {

+        smBus[handle].transmitBufFull=smtrue; //when set true, smExecute will do nothing but clear transmit buffer. so this prevents any of overflowed commands getting thru

+        return recordStatus(handle,SM_ERR_LENGTH); //overflow, too many commands in buffer

+    }

+

+    if(smpCmdType==SMPCMD_SETPARAMADDR)

+    {

+        SMPayloadCommand16 newcmd;

+        newcmd.ID=SMPCMD_SETPARAMADDR;

+        newcmd.param=paramvalue;

+

+        //          bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes, 5);

+        //            bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes, 6);

+        /*

+FIXME

+            ei toimi, menee vaa nollaa*/

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);

+    }

+    if(smpCmdType==SMPCMD_24B)

+    {

+        SMPayloadCommand24 newcmd;

+        newcmd.ID=SMPCMD_24B;

+        newcmd.param=paramvalue;

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[2]);

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);

+    }

+    if(smpCmdType==SMPCMD_32B)

+    {

+        SMPayloadCommand32 newcmd;

+        newcmd.ID=SMPCMD_32B;

+        newcmd.param=paramvalue;

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[3]);

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[2]);

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);

+        bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);

+    }

+

+

+    return recordStatus(handle,SM_OK);

+}

+

+

+SMPayloadCommandRet32 smConvertToPayloadRet32_16(SMPayloadCommandRet16 in)

+{

+    SMPayloadCommandRet32 out;

+    out.ID=in.ID;

+    out.retData=(long)in.retData; //negative value, extend ones

+    return out;

+}

+

+//for library internal use only

+SM_STATUS smTransmitReceiveCommandQueue( const smbus bushandle, const smaddr targetaddress, smuint8 cmdid )

+{

+    SM_STATUS stat;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);

+

+    if(smBus[bushandle].transmitBufFull!=smtrue) //dont send/receive commands if queue was overflowed by user error

+    {

+        stat=smSendSMCMD(bushandle,cmdid,targetaddress, smBus[bushandle].cmd_send_queue_bytes, smBus[bushandle].recv_rsbuf ); //send commands to bus

+        if(stat!=SM_OK) return recordStatus(bushandle,stat);

+    }

+

+    smBus[bushandle].cmd_send_queue_bytes=0;

+    smBus[bushandle].cmd_recv_queue_bytes=0;//counted upwards at every smGetQueued.. and compared to payload size

+

+    if(smBus[bushandle].transmitBufFull!=smtrue)//dont send/receive commands if queue was overflowed by user error

+    {

+        stat=smReceiveReturnPacket(bushandle);//blocking wait & receive return values from bus

+        if(stat!=SM_OK) return recordStatus(bushandle,stat); //maybe timeouted

+    }

+

+    smBus[bushandle].transmitBufFull=smfalse;//reset overflow status

+    return recordStatus(bushandle,SM_OK);

+}

+

+

+SM_STATUS smExecuteCommandQueue( const smbus bushandle, const smaddr targetaddress )

+{

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

+

+    return recordStatus(bushandle,smTransmitReceiveCommandQueue(bushandle,targetaddress,SMCMD_INSTANT_CMD));

+}

+

+SM_STATUS smUploadCommandQueueToDeviceBuffer( const smbus bushandle, const smaddr targetaddress )

+{

+    return recordStatus(bushandle,smTransmitReceiveCommandQueue(bushandle,targetaddress,SMCMD_BUFFERED_CMD));

+}

+

+//return number of how many bytes waiting to be read with smGetQueuedSMCommandReturnValue

+SM_STATUS smBytesReceived( const smbus bushandle, smint32 *bytesinbuffer )

+{

+    if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);

+

+    smint32 bytes=smBus[bushandle].recv_payloadsize - smBus[bushandle].cmd_recv_queue_bytes;//how many bytes waiting to be read with smGetQueuedSMCommandReturnValue

+    *bytesinbuffer=bytes;

+

+    return recordStatus(bushandle,SM_OK);

+}

+

+SM_STATUS smGetQueuedSMCommandReturnValue(  const smbus bushandle, smint32 *retValue )

+{

+    smuint8 rxbyte, rettype;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

+

+

+    //if get called so many times that receive queue buffer is already empty, return error

+    if(smBus[bushandle].cmd_recv_queue_bytes>=smBus[bushandle].recv_payloadsize)

+    {

+

+        smDebug(Trace,bushandle,"Packet receive error, return data coudn't be parsed\n");

+

+        //return 0

+        if(retValue!=NULL) *retValue=0;//check every time if retValue is set NULL by caller -> don't store anything to it if its NULL

+

+        return recordStatus(bushandle,SM_ERR_LENGTH);//not a single byte left

+    }

+

+    //get first byte to deterime packet length

+    rxbyte=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);

+    rettype=rxbyte>>6; //extract ret packet header 2 bits

+

+    //read rest of data based on packet header:

+

+    if(rettype == SMPRET_16B)

+    {

+        //extract return packet and convert to 32 bit and return

+        SMPayloadCommandRet16 read;

+        smuint8 *readBuf=(smuint8*)&read;

+        readBuf[1]=rxbyte;

+        readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);

+        smDebug(Trace,bushandle,"RET16B: %d\n",read.retData);

+

+        if(retValue!=NULL) *retValue=read.retData;

+        return recordStatus(bushandle,SM_OK);

+    }

+    if(rettype == SMPRET_24B)

+    {

+        //extract return packet and convert to 32 bit and return

+        SMPayloadCommandRet24 read;

+        smuint8 *readBuf=(smuint8*)&read;

+        readBuf[2]=rxbyte;

+        readBuf[1]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);

+        readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);

+        smDebug(Trace,bushandle,"RET24B: %d\n",read.retData);

+

+        if(retValue!=NULL) *retValue=read.retData;

+        return recordStatus(bushandle,SM_OK);

+    }

+    if(rettype == SMPRET_32B)

+    {

+        //extract return packet and convert to 32 bit and return

+        SMPayloadCommandRet32 read;

+        smuint8 *readBuf=(smuint8*)&read;

+        readBuf[3]=rxbyte;

+        readBuf[2]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);

+        readBuf[1]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);

+        readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);

+        smDebug(Trace,bushandle,"RET32B: %d\n",read.retData);

+

+        if(retValue!=NULL) *retValue=read.retData;

+        return recordStatus(bushandle,SM_OK);

+    }

+    if(rettype == SMPRET_OTHER) //8bit

+    {

+        //extract return packet and convert to 32 bit and return

+        SMPayloadCommandRet8 read;

+        smuint8 *readBuf=(smuint8*)&read;

+        readBuf[0]=rxbyte;

+        smDebug(Trace,bushandle,"RET_OTHER: %d\n",read.retData);

+

+        if(retValue!=NULL) *retValue=read.retData;

+        return recordStatus(bushandle,SM_OK);

+    }

+

+    return recordStatus(bushandle,SM_ERR_PARAMETER); //something went wrong, rettype not known

+}

+

+

+SM_STATUS smReceiveReturnPacket( smbus bushandle )

+{

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

+

+    smDebug(bushandle, High, "  Reading reply packet\n");

+    do

+    {

+        smuint8 ret;

+        SM_STATUS stat;

+

+        smbool succ=smBDRead(bushandle,&ret);

+

+        if(succ==smfalse)

+        {

+            smReceiveErrorHandler(bushandle,smfalse);

+            smDebug(bushandle, High, "  smRawCommand RX read byte failed\n");

+            return recordStatus(bushandle,SM_ERR_COMMUNICATION);

+        }

+        else

+        {

+            smDebug(bushandle, High, "  got byte %02x \n",ret);

+        }

+

+        stat=smParseReturnData( bushandle, ret );

+        if(stat!=SM_OK) return recordStatus(bushandle,stat);

+    } while(smBus[bushandle].receiveComplete==smfalse); //loop until complete packaget has been read

+

+    //return data read complete

+    smDebug(bushandle,Mid, "< %s (id=%d, addr=%d, payload=%d)\n",

+            cmdidToStr( smBus[bushandle].recv_cmdid ),

+            smBus[bushandle].recv_cmdid,

+            smBus[bushandle].recv_addr,

+            smBus[bushandle].recv_payloadsize);

+

+    return recordStatus(bushandle,SM_OK);

+}

+

+

+/** Set stream where debug output is written. By default nothing is written. */

+LIB void smSetDebugOutput( smVerbosityLevel level, FILE *stream )

+{

+    smDebugThreshold=level;

+    smDebugOut=stream;

+}

+

+

+//short returndata16=0, payload=0;

+//can be called at any frequency

+SM_STATUS smParseReturnData( smbus handle, smuint8 data )

+{

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    //buffered variable allows placing if's in any order (because recv_state may changes in this function)

+    smBus[handle].recv_state=smBus[handle].recv_state_next;

+    smBus[handle].receiveComplete=smfalse;//overwritten to true later if complete

+

+    if(smBus[handle].recv_state==WaitPayload)

+    {

+        smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);

+

+        //normal handling for all payload data

+        if(smBus[handle].recv_storepos<SM485_MAX_PAYLOAD_BYTES)

+            smBus[handle].recv_rsbuf[smBus[handle].recv_storepos++]=data;

+        else//rx payload buffer overflow

+        {

+            return recordStatus(handle,(smReceiveErrorHandler(handle,smtrue)));

+        }

+

+        //all received

+        if(smBus[handle].recv_payloadsize<=smBus[handle].recv_storepos)

+            smBus[handle].recv_state_next=WaitCrcHi;

+

+        return recordStatus(handle,SM_OK);

+    }

+

+

+    if(smBus[handle].recv_state==WaitCmdId)

+    {

+        smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);

+        smBus[handle].recv_cmdid=data;

+        switch(data&SMCMD_MASK_PARAMS_BITS)//commands with fixed payload size

+        {

+        case SMCMD_MASK_2_PARAMS: smBus[handle].recv_payloadsize=2; smBus[handle].recv_state_next=WaitAddr; break;

+        case SMCMD_MASK_0_PARAMS: smBus[handle].recv_payloadsize=0; smBus[handle].recv_state_next=WaitAddr; break;

+        case SMCMD_MASK_N_PARAMS: smBus[handle].recv_payloadsize=-1; smBus[handle].recv_state_next=WaitPayloadSize;break;//-1 = N databytes

+        default:

+            return recordStatus(handle,(smReceiveErrorHandler(handle, smtrue)));

+            break; //error, unsupported command id

+        }

+

+        return recordStatus(handle,SM_OK);

+    }

+

+    //no data payload size known yet

+    if(smBus[handle].recv_state==WaitPayloadSize)

+    {

+        smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);

+        smBus[handle].recv_payloadsize=data;

+        smBus[handle].recv_state_next=WaitAddr;

+        return recordStatus(handle,SM_OK);

+    }

+

+    if(smBus[handle].recv_state==WaitAddr)

+    {

+        smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);

+        smBus[handle].recv_addr=data;//can be receiver or sender addr depending on cmd

+        if(smBus[handle].recv_payloadsize>smBus[handle].recv_storepos)

+            smBus[handle].recv_state_next=WaitPayload;

+        else

+            smBus[handle].recv_state_next=WaitCrcHi;

+        return recordStatus(handle,SM_OK);

+    }

+

+    if(smBus[handle].recv_state==WaitCrcHi)

+    {

+        smBus[handle].recv_read_crc_hi=data;//crc_msb

+        smBus[handle].recv_state_next=WaitCrcLo;

+        return recordStatus(handle,SM_OK);

+    }

+

+    //get crc_lsb, check crc and execute

+    if(smBus[handle].recv_state==WaitCrcLo)

+    {

+        if(((smBus[handle].recv_read_crc_hi<<8)|data)!=smBus[handle].recv_crc)

+        {

+            //CRC error

+            return recordStatus(handle,(smReceiveErrorHandler(handle,smtrue)));

+        }

+        else

+        {

+            //CRC ok

+            //if(smBus[handle].recv_addr==config.deviceAddress || smBus[handle].recv_cmdid==SMCMD_GET_CLOCK_RET || smBus[handle].recv_cmdid==SMCMD_PROCESS_IMAGE ) executeSMcmd();

+            smBus[handle].receiveComplete=smtrue;

+        }

+

+        //smResetSM485variables(handle);

+        smBus[handle].recv_storepos=0;

+        smBus[handle].recv_crc=SM485_CRCINIT;

+        smBus[handle].recv_state_next=WaitCmdId;

+        return recordStatus(handle,SM_OK);

+    }

+

+    return recordStatus(handle,SM_OK);

+}

+

+

+//reply consumes 16 bytes in payload buf, so max calls per cycle is 7

+SM_STATUS smAppendGetParamCommandToQueue( smbus handle, smint16 paramAddress )

+{

+    SM_STATUS stat=SM_NONE;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    //possible errors will set bits to stat

+    stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN ); //2b

+    stat|=smAppendSMCommandToQueue( handle, SMPCMD_24B, SMPRET_32B );//3b

+    stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR );//2b

+    stat|=smAppendSMCommandToQueue( handle, SMPCMD_24B, paramAddress );//3b

+    //=10 bytes

+

+    return recordStatus(handle,stat);

+}

+

+SM_STATUS smGetQueuedGetParamReturnValue(  const smbus bushandle, smint32 *retValue )

+{

+    smint32 retVal=0;

+    SM_STATUS stat=SM_NONE;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

+

+    //must get all inserted commands from buffer

+    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );//4x4b

+    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );

+    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );

+    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );  //the real return value is here

+    if(retValue!=NULL) *retValue=retVal;

+    return recordStatus(bushandle,stat);

+}

+

+//consumes 6 bytes in payload buf, so max calls per cycle is 20

+SM_STATUS smAppendSetParamCommandToQueue( smbus handle, smint16 paramAddress, smint32 paramValue )

+{

+    SM_STATUS stat=SM_NONE;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, paramAddress );//2b

+    stat|=smAppendSMCommandToQueue( handle, SMPCMD_32B, paramValue );//4b

+    return recordStatus(handle,stat);

+}

+

+

+SM_STATUS smGetQueuedSetParamReturnValue(  const smbus bushandle, smint32 *retValue )

+{

+    smint32 retVal=0;

+    SM_STATUS stat=SM_NONE;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;

+

+    //must get all inserted commands from buffer

+    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );

+    stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );  //the real return value is here

+    if(retValue!=NULL) *retValue=retVal;

+

+    return recordStatus(bushandle,stat);

+}

+

+

+SM_STATUS smGetBufferClock( const smbus handle, const smaddr targetaddr, smuint16 *clock )

+{

+    SM_STATUS stat;

+

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return recordStatus(handle,SM_ERR_NODEVICE);

+

+    stat=smSendSMCMD(handle, SMCMD_GET_CLOCK ,targetaddr, 0, NULL ); //send get clock commands to bus

+    if(stat!=SM_OK) return recordStatus(handle,stat);

+

+    stat=smReceiveReturnPacket(handle);//blocking wait & receive return values from bus

+    if(stat!=SM_OK) return recordStatus(handle,stat); //maybe timeouted

+

+    if(clock!=NULL)

+        *clock=bufget16bit(smBus[handle].recv_rsbuf,0);

+

+    smBus[handle].recv_storepos=0;

+

+    return recordStatus(handle,SM_OK);

+}

+

+/** Simple read & write of parameters with internal queueing, so only one call needed.

+Use these for non-time critical operations. */

+SM_STATUS smRead1Parameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1 )

+{

+    SM_STATUS smStat=0;

+

+    smStat|=smAppendGetParamCommandToQueue(handle,paramId1);

+    smStat|=smExecuteCommandQueue(handle,nodeAddress);

+    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);

+

+    return recordStatus(handle,smStat);

+}

+

+SM_STATUS smRead2Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 )

+{

+    SM_STATUS smStat=0;

+

+    smStat|=smAppendGetParamCommandToQueue(handle,paramId1);

+    smStat|=smAppendGetParamCommandToQueue(handle,paramId2);

+    smStat|=smExecuteCommandQueue(handle,nodeAddress);

+    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);

+    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal2);

+

+    return recordStatus(handle,smStat);

+}

+

+SM_STATUS smRead3Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 ,const smint16 paramId3, smint32 *paramVal3 )

+{

+    SM_STATUS smStat=0;

+

+    smStat|=smAppendGetParamCommandToQueue(handle,paramId1);

+    smStat|=smAppendGetParamCommandToQueue(handle,paramId2);

+    smStat|=smAppendGetParamCommandToQueue(handle,paramId3);

+    smStat|=smExecuteCommandQueue(handle,nodeAddress);

+    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);

+    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal2);

+    smStat|=smGetQueuedGetParamReturnValue(handle,paramVal3);

+

+    return recordStatus(handle,smStat);

+}

+

+SM_STATUS smSetParameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId, smint32 paramVal )

+{

+    smint32 nul;

+    SM_STATUS smStat=0;

+

+    smStat|=smAppendSetParamCommandToQueue( handle, paramId, paramVal );

+    smStat|=smExecuteCommandQueue(handle,nodeAddress);

+    smStat|=smGetQueuedSetParamReturnValue(  handle, &nul );

+

+    return recordStatus(handle,smStat);

+}

+

+

+

+

+//accumulates status to internal variable by ORing the bits. returns same value that is fed as paramter

+SM_STATUS recordStatus( const smbus handle, const SM_STATUS stat )

+{

+    //check if bus handle is valid & opened

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    smBus[handle].cumulativeSmStatus|=stat;

+    return stat;

+}

+

+

+/** This function returns all occurred SM_STATUS bits after smOpenBus or resetCumulativeStatus call*/

+SM_STATUS getCumulativeStatus( const smbus handle )

+{

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    return smBus[handle].cumulativeSmStatus;

+}

+

+/** Reset cululative status so getCumultiveStatus returns 0 after calling this until one of the other functions are called*/

+void resetCumulativeStatus( const smbus handle )

+{

+    if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;

+

+    smBus[handle].cumulativeSmStatus=0;

+}

+

diff --git a/simplemotion.h b/simplemotion.h
new file mode 100644
index 0000000..28743c4
--- /dev/null
+++ b/simplemotion.h
@@ -0,0 +1,140 @@
+//Global SimpleMotion functions & definitions

+//Copyright (c) Granite Devices Oy

+

+/*

+     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; version 2 of the License.

+

+     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.

+*/

+

+#ifndef SIMPLEMOTION_H

+#define SIMPLEMOTION_H

+

+#ifdef WIN32

+//dll specs

+#ifdef BUILD_DLL

+    #define LIB __declspec(dllexport)

+#else

+//    #define LIB __declspec(dllimport)

+#define LIB

+#endif

+#else

+#define LIB

+#endif

+

+#include <stdio.h>

+#include "simplemotion_defs.h"

+

+

+#ifdef __cplusplus

+extern "C"{

+#endif

+

+//possible return values (SM_STATUS type)

+#define SM_NONE 0

+#define SM_OK 1

+#define SM_ERR_NODEVICE 2

+#define SM_ERR_BUS 4

+#define SM_ERR_COMMUNICATION 8

+#define SM_ERR_PARAMETER 16

+#define SM_ERR_LENGTH 32

+

+///////////////////////////////////////////////////////////////////////////////////////

+//TYPES & VALUES //////////////////////////////////////////////////////////////////////

+///////////////////////////////////////////////////////////////////////////////////////

+//declare SM lib types

+typedef long smbus;

+typedef unsigned long smuint32;

+typedef unsigned short smuint16;

+typedef unsigned char smuint8;

+typedef long smint32;

+typedef short smint16;

+typedef char smint8;

+typedef char smbool;

+#define smtrue 1

+#define smfalse 0

+typedef int SM_STATUS;

+typedef smuint8 smaddr;

+

+//comment out to disable, gives smaller & faster code

+#define ENABLE_DEBUG_PRINTS

+

+typedef enum _smVerbosityLevel {Off,Low,Mid,High,Trace} smVerbosityLevel;

+

+

+

+//max number of simultaneously opened buses. change this and recompiple SMlib if

+//necessary (to increase channels or reduce to save memory)

+//#define SM_MAX_BUSES 5

+///////////////////////////////////////////////////////////////////////////////////////

+//FUNCTIONS////////////////////////////////////////////////////////////////////////////

+///////////////////////////////////////////////////////////////////////////////////////

+

+/** Open SM RS485 communication bus. Parameters:

+	-devicename: "USB2VSD" or com port as "COMx" where x=1-16

+	-return value: handle to be used with all other commands, -1 if fails

+	*/

+LIB smbus smOpenBus( const char * devicename );

+

+/** Set timeout of how long to wait reply packet from bus. Must be set before smOpenBus and cannot be changed afterwards

+ * max value 5000ms. In unix this is rounded to 100ms (rounding downwards), so 99 or less gives 0ms timeout.

+ *

+ *This is the only function that returns SM_STATUS which doesn't accumulate status bits to be read with getCumulativeStatus because it has no bus handle

+ */

+LIB SM_STATUS smSetTimeout( smuint16 millsecs );

+

+/** Close connection to given bus handle number. This frees communication link therefore makes it available for other apps for opening.

+  -return value: a SM_STATUS value, i.e. SM_OK if command succeed

+*/

+LIB SM_STATUS smCloseBus( const smbus bushandle );

+

+

+/** Return SM lib version number in hexadecimal format.

+Ie V 2.5.1 would be 0x020501 and 1.2.33 0x010233 */

+LIB smuint32 smGetVersion();

+

+

+/** Set stream where debug output is written. By default nothing is written. */

+LIB void smSetDebugOutput( smVerbosityLevel level, FILE *stream );

+

+/** This function returns all occurred SM_STATUS bits after smOpenBus or resetCumulativeStatus call*/

+LIB SM_STATUS getCumulativeStatus( const smbus handle );

+/** Reset cululative status so getCumultiveStatus returns 0 after calling this until one of the other functions are called*/

+LIB void resetCumulativeStatus( const smbus handle );

+

+

+/** SMV2 Device communication functionss */

+LIB SM_STATUS smAppendCommandToQueue( smbus handle, smuint8 cmdid, smuint16 param  );

+LIB SM_STATUS smExecuteCommandQueue( const smbus bushandle, const smaddr targetaddress );

+LIB smuint16  smGetQueuedCommandReturnValue(  const smbus bushandle, smuint16 cmdnumber );

+

+LIB SM_STATUS smUploadCommandQueueToDeviceBuffer( const smbus bushandle, const smaddr targetaddress );

+LIB SM_STATUS smBytesReceived( const smbus bushandle, smint32 *bytesinbuffer );

+

+LIB SM_STATUS smAppendSMCommandToQueue( smbus handle, int smpCmdType, smint32 paramvalue  );

+LIB SM_STATUS smGetQueuedSMCommandReturnValue(  const smbus bushandle, smint32 *retValue );

+

+LIB SM_STATUS smAppendGetParamCommandToQueue( smbus handle, smint16 paramAddress );

+LIB SM_STATUS smGetQueuedGetParamReturnValue(  const smbus bushandle, smint32 *retValue  );

+LIB SM_STATUS smAppendSetParamCommandToQueue( smbus handle, smint16 paramAddress, smint32 paramValue );

+LIB SM_STATUS smGetQueuedSetParamReturnValue(  const smbus bushandle, smint32 *retValue  );

+

+/** Simple read & write of parameters with internal queueing, so only one call needed.

+Use these for non-time critical operations. */

+LIB SM_STATUS smRead1Parameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1 );

+LIB SM_STATUS smRead2Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 );

+LIB SM_STATUS smRead3Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 ,const smint16 paramId3, smint32 *paramVal3 );

+LIB SM_STATUS smSetParameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId, smint32 paramVal );

+

+

+LIB SM_STATUS smGetBufferClock( const smbus handle, const smaddr targetaddr, smuint16 *clock );

+

+#ifdef __cplusplus

+}

+#endif

+#endif // SIMPLEMOTION_H

diff --git a/simplemotion_defs.h b/simplemotion_defs.h
new file mode 100644
index 0000000..cc00b95
--- /dev/null
+++ b/simplemotion_defs.h
@@ -0,0 +1,502 @@
+/*

+ * simplemotion_defs.h

+ * This file contains addresses for simple motion parameters (SMP's) and if parameter is a bit field

+ * or enum (low number of choices), then also bits or choices in the register are defined here.

+ *

+ *  Created on: 25.2.2012

+ *      Author: Tero

+ */

+

+

+/* SMV2 address space areas:

+ * 0 NULL, can read or write but has no effect

+ * 1-63 SM bus specific parameters

+ * 128-167 digital I/O registers for GPIO

+ * 168-199 analog I/O reigisters for GPIO

+ * 201-8190 application specific parameter addresses. in this case motor drive registers

+ * 8191 NOP command. if SM command SMPCMD_SET_PARAM_ADDR with SMP_ADDR_NOP as parameter is sent, no actions are taken in nodes. it's NOP (no operation) command.

+ */

+

+

+#ifndef SIMPLEMOTION_DEFS_H_

+#define SIMPLEMOTION_DEFS_H_

+

+/*

+ * bit shift macros

+ */

+

+#ifndef BV

+//bitvise shifts

+#define BV(bit) (1<<(bit))

+#define BVL(bit) (1L<<(bit))

+#endif

+

+

+//SMP command types

+#define SMPCMD_SETPARAMADDR 2

+#define SMPCMD_24B 1

+#define SMPCMD_32B 0

+#define SMPRET_OTHER 3

+#define SMPRET_16B 2

+#define SMPRET_24B 1

+#define SMPRET_32B 0

+

+

+/*

+ * SimpleMotion V2 constants

+ */

+

+/* broadcast address, if packet is sent to SM_BROADCAST_ADDR then all nodes in the network

+ * will read it but no-one will send a reply because it would cause packet collision*/

+ #define SM_BROADCAST_ADDR 0

+

+//these bits define attributes over parameter address to retreive different types of values as return data

+#define SMP_VALUE_MASK  0x0000

+#define SMP_MIN_VALUE_MASK  0x4000 //so requesting i.e. param (SMP_TIMEOUT|SMP_MIN_VALUE_MASK) will give the minimum settable value for param SMP_TIMEOUT

+#define SMP_MAX_VALUE_MASK  0x8000

+//mask to filter attributes

+#define SMP_ATTRIBUTE_BITS_MASK  0xC000//C=1100

+//mask for addresses

+#define SMP_ADDRESS_BITS_MASK  0x1FFF //E=1110

+

+

+//SMP packet header bits (2 bits). These determine content length and type

+#define SMPCMD_SET_PARAM_ADDR 2

+#define SMPCMD_24B 1

+#define SMPCMD_32B 0

+#define SMPRET_CMD_STATUS 3

+#define SMPRET_16B 2

+#define SMPRET_24B 1

+#define SMPRET_32B 0

+

+//if command is to set param addr to this -> NOP

+#define SMP_ADDR_NOP 0x3fff

+

+//SMPCommand values that are returned with every SMPCommand when SMPRET_OTHER set as SMP_RETURN_PARAM_LEN:

+#define SMP_CMD_STATUS_ACK 0

+#define SMP_CMD_STATUS_NACK 1

+#define SMP_CMD_STATUS_INVALID_ADDR 2

+#define SMP_CMD_STATUS_INVALID_VALUE 4

+#define SMP_CMD_STATUS_VALUE_TOO_HIGH 8

+#define SMP_CMD_STATUS_VALUE_TOO_LOW 16

+#define SMP_CMD_STATUS_OTHER_MASK32 (3L<<30) //1byte packet. payload codes:

+

+//params 0-63 are reserved for SM specific params, application specifics are >64-8090

+#define SMP_SM_RESERVED_ADDRESS_SPACE 63

+

+//SMP=Simple motion parameter

+#define SMP_NULL 0

+#define SMP_NODE_ADDRSS 1

+#define SMP_BUS_MODE 2

+#define SMP_SM_VERSION 3

+#define SMP_SM_VERSION_COMPAT 4

+#define SMP_BUS_SPEED 5

+#define SMP_BUFFER_FREE_BYTES 6

+#define SMP_BUFFERED_CMD_STATUS 7

+#define SMP_BUFFERED_CMD_PERIOD 8

+#define SMP_RETURN_PARAM_ADDR 9

+#define SMP_RETURN_PARAM_LEN 10

+#define SMP_TIMEOUT 12

+#define SMP_CUMULATIVE_STATUS 13 //error bits are set here if any, (SMP_CMD_STATUS_... bits). clear by writing 0

+

+//bit mask

+#define SM_BUFCMD_STAT_IDLE 1

+#define SM_BUFCMD_STAT_RUN 2

+#define SM_BUFCMD_STAT_UNDERRUN 4

+#define SM_BUFCMD_STAT_OVERRUN 8

+

+//each DIGITAL variable is 16 bits thus holds 16 i/o's

+#define SMP_DIGITAL_IN_VALUES_1 128

+#define SMP_DIGITAL_IN_VALUES_2 129

+#define SMP_DIGITAL_IN_VALUES_3 130

+#define SMP_DIGITAL_IN_VALUES_4 131

+

+#define SMP_DIGITAL_IN_CHANGED_1 132

+#define SMP_DIGITAL_IN_CHANGED_2 133

+#define SMP_DIGITAL_IN_CHANGED_3 134

+#define SMP_DIGITAL_IN_CHANGED_4 135

+

+#define SMP_DIGITAL_OUT_VALUE_1 136

+#define SMP_DIGITAL_OUT_VALUE_2 137

+#define SMP_DIGITAL_OUT_VALUE_3 138

+#define SMP_DIGITAL_OUT_VALUE_4 139

+

+//set IO pin direction: 0=input, 1=output. May not be supported by HW.

+#define SMP_DIGITAL_IO_DIRECTION_1 140

+#define SMP_DIGITAL_IO_DIRECTION_2 141

+#define SMP_DIGITAL_IO_DIRECTION_3 142

+#define SMP_DIGITAL_IO_DIRECTION_4 143

+

+#define SMP_ANALOG_IN_VALUE_1 168

+#define SMP_ANALOG_IN_VALUE_2 169

+#define SMP_ANALOG_IN_VALUE_3 170

+#define SMP_ANALOG_IN_VALUE_4 171

+#define SMP_ANALOG_IN_VALUE_5 172

+// continue to .. 183

+

+#define SMP_ANALOG_OUT_VALUE_1 184

+#define SMP_ANALOG_OUT_VALUE_2 185

+#define SMP_ANALOG_OUT_VALUE_3 186

+#define SMP_ANALOG_OUT_VALUE_4 187

+#define SMP_ANALOG_OUT_VALUE_5 188

+// continue to .. 200

+

+

+

+/*

+ * List of motor control parameters.

+ *

+ * WARNING: Don't assume anything about these variables! Refer to SimpleMotion V2 documentation

+ * for details. If unknown, ask Granite Devices.

+ */

+

+#define SMP_INCREMENTAL_POS_TARGET 550

+#define SMP_ABSOLUTE_POS_TARGET 551

+#define SMP_FAULTS 552

+	//bitfield bits:

+	#define FLT_FOLLOWERROR	BV(1)

+	#define FLT_OVERCURRENT BV(2)

+	#define FLT_COMMUNICATION BV(3)

+	#define FLT_ENCODER	BV(4)

+	#define FLT_OVERTEMP BV(5)

+	#define FLT_UNDERVOLTAGE BV(6)

+	#define FLT_OVERVOLTAGE BV(7)

+	#define FLT_PROGRAM_OR_MEM BV(8)

+	#define FLT_HARDWARE BV(9)

+	#define FLT_OVERVELOCITY BV(10)

+	#define FLT_INIT BV(11)

+	#define FLT_MOTION BV(12)

+	#define FLT_RANGE BV(13)

+	#define FLT_PSTAGE_FORCED_OFF BV(14)

+	#define FLT_HOST_COMM_ERROR BV(15)

+	//IO side macros

+	#define FLT_GC_COMM BV(15)

+	#define FLT_QUEUE_FULL FLT_PROGRAM_OR_MEM

+	#define FLT_SM485_ERROR FLT_COMMUNICATION

+	#define FLT_FIRMWARE FLT_PROGRAM_OR_MEM //non-recoverable program error

+	#define FLT_ALLOC FLT_PROGRAM_OR_MEM //memory etc allocation failed

+

+#define SMP_FIRST_FAULT 8115

+#define SMP_STATUS 553

+	//bitfield bits:

+	#define STAT_POWER_ON BV(0)

+	#define STAT_TARGET_REACHED BV(1)//this is 1 when trajectory planner target reached

+	#define STAT_FERROR_RECOVERY BV(2)

+	#define STAT_RUN BV(3)//run is true only if motor is being actually driven. run=0 clears integrators etc

+	#define STAT_ENABLED BV(4)

+	#define STAT_FAULTSTOP BV(5)

+	#define STAT_FERROR_WARNING BV(6)//follow error warning, recovering or disabled

+	#define STAT_STO_ACTIVE BV(7)

+	#define STAT_SERVO_READY BV(8)//ready for user command: initialized, running (no fault), not recovering, not homing & no homing aborted, not running sequence

+	#define STAT_BRAKING BV(10)

+	#define STAT_HOMING BV(11)

+	#define STAT_INITIALIZED BV(12)

+	#define STAT_VOLTAGES_OK BV(13)

+	#define STAT_PERMANENT_STOP BV(15)//outputs disabled until reset

+

+#define SMP_SYSTEM_CONTROL 554 //writing 1 initiates settings save to flash, writing 2=device restart, 4=abort buffered motion

+	//choices:

+	#define SMP_SYSTEM_CONTROL_SAVECFG 1

+	#define SMP_SYSTEM_CONTROL_RESTART 2

+	#define SMP_SYSTEM_CONTROL_ABORTBUFFERED 4

+    #define SMP_SYSTEM_SAMPLE_TEST_VARIABLES 8 //production testing function

+    #define SMP_SYSTEM_START_PRODUCTION_TEST 16 //production testing function

+    #define SMP_SYSTEM_STOP_PRODUCTION_TEST 32 //production testing function

+#define SMP_POS_FERROR_TRIP 555

+#define SMP_VEL_FERROR_TRIP 556

+#define SMP_POS_ERROR_RECOVERY_SPEED 557

+#define SMP_MOTOR_MODE 558

+	//choices:

+	#define MOTOR_NONE 0

+	//ac servo

+	#define MOTOR_AC_VECTOR 1

+	//trapezoidal ac

+	#define MOTOR_BLDC 2

+	//with regen resistor

+	#define MOTOR_DC 3

+	//without regen, parallel out

+	#define MOTOR_PARALLEL_DC 4

+	//open loop AC=stepper, no feedback

+	#define MOTOR_STEPPER 5

+	#define MOTOR_STEPPER_W_ENCODER 6

+	#define MOTOR_STEPPER_SERVO 7

+	#define _MOTOR_LAST 7

+

+

+

+//position follow error trip point

+//velocity follow error trip point. units: velocity command (counts per PIDcycle * divider)

+

+//#define CFG_INPUT_FILTER_LEN 30

+#define SMP_CONTROL_MODE 559

+	//control mode choices:

+	#define CM_TORQUE 2

+	#define CM_VELOCITY 1

+	#define CM_POSITION 0

+#define SMP_INPUT_MULTIPLIER 560

+#define SMP_INPUT_DIVIDER 561

+#define SMP_INPUT_REFERENCE_MODE 562

+	//choices:

+	#define SMP_INPUT_REFERENCE_MODE_SERIALONLY 0

+	#define SMP_INPUT_REFERENCE_MODE_PULSETRAIN 1

+	#define SMP_INPUT_REFERENCE_MODE_QUADRATURE 2

+	#define SMP_INPUT_REFERENCE_MODE_PWM 3

+	#define SMP_INPUT_REFERENCE_MODE_ANALOG 4

+	#define SMP_INPUT_REFERENCE_MODE_RESERVED1 5

+	#define SMP_INPUT_REFERENCE_MODE_RESERVED2 6

+	#define SMP_INPUT_REFERENCE_MODE_LAST_ 6

+

+#define SMP_ABS_IN_OFFSET 563

+#define SMP_ABS_IN_SCALER 564

+#define SMP_ENCODER_PPR 565

+#define SMP_MOTOR_POLEPAIRS 566

+

+

+//flag bits & general

+#define SMP_DRIVE_FLAGS 567

+	//bitfield bits:

+	#define FLAG_DISABLED_AT_STARTUP BV(0)

+	#define FLAG_NO_DCBUS_FAULT BV(1)

+	#define FLAG_INVERT_ENCODER BV(3)

+	#define FLAG_INVERT_MOTOR_DIRECTION BV(4) /*invert positive direction*/

+	#define FLAG_DISABLE_DEADTIMECORR_LO_SPEED BV(5)

+	#define FLAG_USE_PULSE_IN_ACCEL_LIMIT BV(7)

+	#define FLAG_2PHASE_AC_MOTOR BV(9)

+	#define FLAG_ALLOW_VOLTAGE_CLIPPING BV(10)

+	#define FLAG_USE_INPUT_LP_FILTER BV(11)

+	#define FLAG_USE_PID_CONTROLLER BV(12)//PIV is the default if bit is 0

+	#define FLAG_INVERTED_HALLS BV(13)

+	#define FLAG_USE_HALLS BV(14)

+    #define FLAG_MECH_BRAKE_DURING_PHASING BV(15)

+#define SMP_MOTION_FAULT_THRESHOLD 568

+#define SMP_HV_VOLTAGE_HI_LIMIT 569

+#define SMP_HV_VOLTAGE_LOW_LIMIT 570

+#define SMP_MOTOR_MAX_SPEED 572

+

+

+//primary feedback loop 200-299

+#define SMP_VEL_I 200

+#define SMP_POS_P 201

+#define SMP_VEL_P 202

+#define SMP_VEL_FF 220

+#define SMP_ACC_FF 221

+

+//anti dither limits

+#define SMP_ANTIDITHER_MODE 230

+

+//secondary feedback loop 300-399

+//NOT IMPLEMENTED YET

+

+//current loop related 400-499

+#define SMP_TORQ_P 401

+#define SMP_TORQ_I 402

+#define SMP_MOTOR_RES 405

+#define SMP_MOTOR_IND 406

+

+#define SMP_TORQUELIMIT_CONT 410

+#define SMP_TORQUELIMIT_PEAK 411

+#define SMP_TORQUELIMIT_HOMING 415

+#define SMP_TORQUEFAULT_MARGIN 420

+#define SMP_TORQUEFAULT_OC_TOLERANCE 421

+

+#define SMP_MOTOR_THERMAL_TIMECONSTANT 430

+#define SMP_PHASESEARCH_VOLTAGE_SLOPE 480

+#define SMP_PHASESEARCH_CURRENT 481

+#define SMP_TORQUE_LPF_BANDWIDTH 490

+

+//motor rev to rotary/linear unit scale. like 5mm/rev or 0.1rev/rev. 30bit parameter & scale 100000=1.0

+#define SMP_AXIS_SCALE 491

+//output unit 0=mm 1=um 2=inch 3=revolutions 4=degrees

+#define SMP_AXIS_UNITS 492

+//0=none 1=qei1 2=qei2 3=resolver 4=ssi 5=biss

+#define SMP_FB1_DEVICE_SELECTION 493

+#define SMP_FB2_DEVICE_SELECTION 494

+//in 1/2500 seconds.

+#define SMP_GOAL_FAULT_FILTER_TIME 495

+//in speed scale

+#define SMP_OVERSPEED_FAULT_LIMIT 496

+//0=min 6=max

+#define SMP_OVERCURRENT_FAULT_SENSITIVITY 497

+//when hit limt switch, do: 0=nothing, 1=disable torque, 2=fault stop, 3=servoed stop

+#define SMP_LIMIT_SW_FUNCTION 498

+		//choices:

+        #define SMP_LIMIT_SW_DISABLED 0

+        #define SMP_LIMIT_SW_NOTORQUE 1

+        #define SMP_LIMIT_SW_FAULTSTOP 2

+        #define SMP_LIMIT_SW_SERVOSTOP 3

+        #define _SMP_LIMIT_SW_LAST 3

+

+#define SMP_CONTROL_BITS1 2533

+	//bitfiled values:

+	//CB1 & CB2 enables must be 1 to have drive enabled

+	//Control bits 1 are controlled by host software

+	#define SMP_CB1_ENABLE BV(0)

+	#define SMP_CB1_CLEARFAULTS BV(1)

+	#define SMP_CB1_QUICKSTOP BV(2)

+	#define SMP_CB1_USE_TRAJPLANNER BV(3)

+	#define SMP_STATIC_CBS1 (SMP_CB1_ENABLE|SMP_CB1_USE_TRAJPLANNER)

+

+#define SMP_CONTROL_BITS2 2534

+	//bitfiled values:

+	//both enables must be 1 to have drive enabled

+	//Control bits 2 are controlled by physical inputs

+	#define SMP_CB2_ENABLE BV(0)

+	#define SMP_CB2_ENA_POS_FEED BV(1)

+	#define SMP_CB2_ENA_NEG_FEED BV(2)

+	#define SMP_CB2_HOMESW_ON BV(3)

+    #define SMP_CB2_CLEARFAULTS BV(4)

+

+//trajectory planner

+#define SMP_TRAJ_PLANNER_ACCEL 800

+#define SMP_TRAJ_PLANNER_STOP_DECEL 801

+#define SMP_TRAJ_PLANNER_VEL 802

+//accel limit for homing & ferror recovery

+//if set to 0, use CFG_TRAJ_PLANNER_ACCEL instead (for backwards compatibility)

+#define SMP_TRAJ_PLANNER_HOMING_ACCEL 803

+#define SMP_TRAJ_PLANNER_HOMING_VEL 804

+#define SMP_TRAJ_PLANNER_HOMING_BITS 806

+	//homing config bit field. illegal to set Not to use INDEX or HOME

+	#define HOMING_POS_INDEX_POLARITY BV(0)

+	#define HOMING_POS_INDEX_SEARCH_DIRECTION BV(1)

+	#define HOMING_POS_HOME_SWITCH_POLARITY BV(2)

+	#define HOMING_USE_HOME_SWITCH BV(3)

+	#define HOMING_USE_INDEX_PULSE BV(4)

+	#define HOMING_HOME_AT_POWER_ON BV(5)

+	#define HOMING_FULL_SPEED_OFFSET_MOVE BV(6)

+	#define HOMING_ENABLED BV(7) /*if 0, homing cant be started */

+	#define _HOMING_CFG_MAX_VALUE 0x00ff

+

+#define SMP_HOMING_CONTROL 7532

+

+

+//hard stop triggered by follow error in encoder counts

+#define SMP_TRAJ_PLANNER_HOMING_HARD_STOP_THRESHOLD 808

+//32bit position offset in encoder counts, value must not overflow on divider*offset

+#define SMP_TRAJ_PLANNER_HOMING_OFFSET 823

+//signed 32bit absolute position limits, active after homing. disabled when both=0

+#define SMP_ABSPOSITION_HI_LIMIT 835

+#define SMP_ABSPOSITION_LO_LIMIT 836

+

+//readouts

+#define SMP_ACTUAL_BUS_VOLTAGE 900

+#define SMP_ACTUAL_TORQUE 901

+#define SMP_ACTUAL_VELOCITY_FB 902

+#define SMP_ACTUAL_POSITION_FB 903

+#define SMP_SCOPE_CHANNEL_VALUE 904

+#define SMP_SCOPE_CHANNEL_SELECT 905

+

+#define SMP_BRAKE_POWERON_RELEASE_DELAY 910

+#define SMP_BRAKE_STOP_ENGAGE_DELAY 911

+#define SMP_DYNAMIC_BRAKING_SPEED 912

+

+

+//////////////////////////////////////////////////////////////////////////////////RUNTIME PARAMS 5000-9999

+

+#define SMP_CAPTURE_SOURCE 5020

+	//bitfield values (shift these with BV())

+	#define CAPTURE_TORQUE_TARGET 1

+	#define CAPTURE_TORQUE_ACTUAL 2

+	#define CAPTURE_VELOCITY_TARGET 3

+	#define CAPTURE_VELOCITY_ACTUAL 4

+	#define CAPTURE_POSITION_TARGET 5

+	#define CAPTURE_POSITION_ACTUAL 6

+	#define CAPTURE_FOLLOW_ERROR 7

+	#define CAPTURE_OUTPUT_VOLTAGE 8

+	#define CAPTURE_BUS_VOLTAGE 9

+	#define CAPTURE_STATUSBITS 10

+	#define CAPTURE_FAULTBITS 11

+	#define CAPTURE_P_OUT 12

+	#define CAPTURE_I_OUT 13

+	#define CAPTURE_D_OUT 14

+	#define CAPTURE_FF_OUT 15

+	#define CAPTURE_RAW_POS 25

+	//8 bit signed values combined, only for return data, not for scope

+	#define CAPTURE_TORQ_AND_FERROR 26

+

+	//rest are availalbe in debug/development firmware only:

+	#define CAPTURE_PWM1 16

+	#define CAPTURE_PWM2 17

+	#define CAPTURE_PWM3 18

+	#define CAPTURE_DEBUG1 19

+	#define CAPTURE_DEBUG2 20

+	#define CAPTURE_CURRENT1 21

+	#define CAPTURE_CURRENT2 22

+	#define CAPTURE_ACTUAL_FLUX 23

+	#define CAPTURE_OUTPUT_FLUX 24

+

+#define SMP_CAPTURE_TRIGGER 5011

+	//choices:

+	#define TRIG_NONE 0

+	#define TRIG_INSTANT 1

+	#define TRIG_TARGETCHANGE 2

+	#define TRIG_TARGETCHANGE_POS 3

+	#define TRIG_SERIALCMD 4

+	#define TRIG_FAULT 6

+

+#define SMP_CAPTURE_SAMPLERATE 5012

+//rdonly

+#define SMP_CAPTURE_BUF_LENGHT 5013

+//this is looped 0-n to make samples 0-n readable from SMP_CAPTURE_BUFFER_GET_VALUE

+#define SMP_CAPTURE_BUFFER_GET_ADDR 5333

+#define SMP_CAPTURE_BUFFER_GET_VALUE 5334

+

+//#define RUNTIME_FEATURES1 6000

+#define SMP_SERIAL_NR 6002

+#define SMP_DRIVE_CAPABILITIES 6006

+#define SMP_FIRMWARE_VERSION 6010

+#define SMP_FIRMWARE_BACKWARDS_COMP_VERSION 6011

+#define SMP_GC_FIRMWARE_VERSION 6014

+#define SMP_GC_FIRMWARE_BACKWARDS_COMP_VERSION 6015

+#define SMP_DEVICE_TYPE 6020

+#define SMP_PID_FREQUENCY 6055

+

+//affects only in MC_VECTOR & MC_BLDC mode

+#define SMP_OVERRIDE_COMMUTATION_ANGLE 8000

+#define SMP_TORQUE_CMD_OVERRIDE 8001

+#define SMP_OVERRIDE_COMMUTATION_FREQ 8003

+#define SMP_OVERRIDE_REGENRES_DUTY 8004

+

+/*IO side CPU sends encoder counter at index every time index is encountered. homing uses this info */

+#define SMP_INDEX_PULSE_LOCATION 8005

+

+

+//GC side debug params

+#define SMP_DEBUGPARAM1 8100

+#define SMP_DEBUGPARAM2 8101

+#define SMP_DEBUGPARAM3 8102

+//for STM32 side override

+#define SMP_DEBUGPARAM4 8103

+#define SMP_DEBUGPARAM5 8104

+#define SMP_DEBUGPARAM6 8105

+#define SMP_FAULT_LOCATION1  8110 //this is for GC side

+#define SMP_FAULT_LOCATION2  8111 //this is for STM side

+#define SMP_OVERRIDE_PARAM_ADDR_1 8120

+#define SMP_OVERRIDE_PARAM_ADDR_2 8121

+#define SMP_OVERRIDE_PARAM_ADDR_3 8122

+#define SMP_OVERRIDE_PARAM_VAL_1 8125

+#define SMP_OVERRIDE_PARAM_VAL_2 8126

+#define SMP_OVERRIDE_PARAM_VAL_3 8127

+

+

+/*DFU Bootloader parameters begin:*/

+//SMP_FAULTS bitfield bits:

+#define FLT_INVALID_FW BV(5)

+#define FLT_FLASHING_FAIL BV(6)

+#define FLT_FLASHING_COMMSIDE_FAIL BV(7)

+//SMP_STATUS bitfield bits:

+#define STAT_GC_FLASHED_ONCE BV(1)//to prevent flashing it twice in same GCBL session which causes error

+

+//write a value here to execute a bootloader function

+#define SMP_BOOTLOADER_FUNCTION 191

+//upload 16bits of data to buffer. max buffer length 4096 bytes before it must be written with "write block" function

+//total uploaded amount must be multiple of 4 bytes before issuing write function

+#define SMP_BOOTLOADER_UPLOAD 192

+//bootloaded status

+#define SMP_BOOTLOADER_STAT 193

+        #define BOOTLOADER_STAT_FLASH_VERIFIED_OK BV(0)

+

+/*DFU Bootloader parameters end:*/

+

+

+

+#endif /* SIMPLEMOTION_DEFS_H_ */

diff --git a/simplemotion_private.h b/simplemotion_private.h
new file mode 100644
index 0000000..e78ac42
--- /dev/null
+++ b/simplemotion_private.h
@@ -0,0 +1,120 @@
+//Internal functions & definitions, not for library user

+//Copyright (c) Granite Devices Oy

+

+/*

+     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; version 2 of the License.

+

+     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.

+*/

+

+#ifndef SIMPLEMOTION_PRIVATE_H

+#define SIMPLEMOTION_PRIVATE_H

+

+#include "simplemotion.h"

+#include "busdevice.h"

+#include <stdio.h>

+

+#define SM_VERSION 0x020000

+//max number of simultaneously opened buses. change this and recompiple SMlib if

+//necessary (to increase channels or reduce to save memory)

+#define SM_MAX_BUSES 10

+

+//bus device types

+#define BUSDEV_NONE 0

+#define BUSDEV_RS 1 /* rs232 like com port support */

+#define BUSDEV_FTDI 2 /*not implemented yet: direct FTDI lib support*/

+

+#define SM_BUSDEVICENAME_LEN 64

+

+//default timeout in ms

+//Argon drive's worst case response time should be ~20ms with max length packets

+#define SM_READ_TIMEOUT 500

+

+

+extern const smuint8 table_crc16_hi[];

+extern const smuint8 table_crc16_lo[];

+extern FILE *smDebugOut; //such as stderr or file handle. if NULL, debug info disbled

+extern smuint16 readTimeoutMs;

+

+void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...);

+

+SM_STATUS smRawCmd( const char *axisname, smuint8 cmd, smuint16 val, smuint32 *retdata );

+

+

+typedef struct {

+        /* ID=0 param size 30 bits (cmd total 4 bytes)

+         * ID=1 param size 22 bits (cmd total 3 bytes)

+         * ID=2 set parameter store address, param: 14 bits=register address  (cmd total 2 bytes)

+         * ID=3 reserved

+         */

+        long param :30; //LSB 30 bits

+        long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID

+} __attribute__ ((packed)) SMPayloadCommand32;

+

+typedef struct {

+        /* ID=0 param size 30 bits (cmd total 4 bytes)

+         * ID=1 param size 22 bits (cmd total 3 bytes)

+         * ID=2 set parameter store address, param: 14 bits=register address  (cmd total 2 bytes)

+         * ID=3 reserved

+         */

+        long param :14; //LSB 30 bits

+        long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID

+} __attribute__ ((packed)) SMPayloadCommand16;

+

+typedef struct {

+        /* ID=0 param size 30 bits (cmd total 4 bytes)

+         * ID=1 param size 22 bits (cmd total 3 bytes)

+         * ID=2 set parameter store address, param: 14 bits=register address  (cmd total 2 bytes)

+         * ID=3 reserved

+         */

+        long param :22; //MSB 30 bits

+        long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID

+} __attribute__ ((packed)) SMPayloadCommand24;

+

+//SM payload command return data structure

+typedef struct {

+        /* ID=0 ret data 30 bits (tot 4 bytes)

+         * ID=1 ret data 22 bits (tot 3 bytes)

+         * ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK.

+         * ID=3 reserved

+         */

+        long retData: 30; //LSB 30 bits

+        long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID

+} __attribute__ ((packed)) SMPayloadCommandRet32;

+//SM payload command return data structure

+typedef struct {

+        /* ID=0 ret data 30 bits (tot 4 bytes)

+         * ID=1 ret data 22 bits (tot 3 bytes)

+         * ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK.

+         * ID=3 reserved

+         */

+        long retData: 22; //LSB 30 bits

+        long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID

+} __attribute__ ((packed)) SMPayloadCommandRet24;

+//SM payload command return data structure

+typedef struct {

+        /* ID=0 ret data 30 bits (tot 4 bytes)

+         * ID=1 ret data 22 bits (tot 3 bytes)

+         * ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK.

+         * ID=3 reserved

+         */

+        long retData: 14; //LSB 30 bits

+        long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID

+} __attribute__ ((packed)) SMPayloadCommandRet16;

+//SM payload command return data structure

+typedef struct {

+        /* ID=0 ret data 30 bits (tot 4 bytes)

+         * ID=1 ret data 22 bits (tot 3 bytes)

+         * ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK.

+         * ID=3 reserved

+         */

+        long retData: 6; //LSB 30 bits

+        long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID

+} __attribute__ ((packed)) SMPayloadCommandRet8;

+

+#endif // SIMPLEMOTION_PRIVATE_H

diff --git a/sm485.h b/sm485.h
new file mode 100644
index 0000000..40d9fdb
--- /dev/null
+++ b/sm485.h
@@ -0,0 +1,71 @@
+//VSD SM485 bus definitions
+
+#ifndef SM485_H
+#define SM485_H
+
+//SERIAL COMMANDS
+#define SM485_CRCINIT 0x0
+#define SM485_BUFSIZE 128
+#define SM485_RSBUFSIZE 128
+#define SM485_CMDBUFSIZE 4096
+
+//cmd number must be 0-31
+
+#define SM485_VERSION 90
+#define SM485_VERSION_COMPAT 85
+
+#define SM485_MAX_PAYLOAD_BYTES 120
+
+//00xb
+#define SMCMD_MASK_0_PARAMS 0
+//01xb, 2 bytes as payload
+#define SMCMD_MASK_2_PARAMS 2
+//10xb
+#define SMCMD_MASK_N_PARAMS 4
+//11xb
+#define SMCMD_MASK_RESERVED 6
+//xx1b
+#define SMCMD_MASK_RETURN 1
+//110b
+#define SMCMD_MASK_PARAMS_BITS 6
+
+
+#define SMCMD_ID(cmdnumber, flags) (((cmdnumber)<<3)|flags)
+
+//format 5, u8 bytesfollowing,u8 toaddr,cmddata,u16 crc
+//return, 6, u8 bytesfollowing,u8 fromaddr, returndata,u16 crc
+#define SMCMD_INSTANT_CMD SMCMD_ID(4,SMCMD_MASK_N_PARAMS)
+#define SMCMD_INSTANT_CMD_RET SMCMD_ID(4,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
+
+//format  ID, u8 bytesfollowing,u8 toaddr,cmddata,u16 crc
+//return, ID, u8 bytesfollowing,u8 fromaddr,returndata,u16 crc
+#define SMCMD_BUFFERED_CMD SMCMD_ID(6,SMCMD_MASK_N_PARAMS)
+#define SMCMD_BUFFERED_CMD_RET SMCMD_ID(6,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
+
+//read return data from buffered cmds. repeat this command before SMCMD_BUFFERED_CMD until N!=120 from download
+#define SMCMD_BUFFERED_RETURN_DATA SMCMD_ID(7,SMCMD_MASK_0_PARAMS)
+#define SMCMD_BUFFERED_RETURN_DATA_RET SMCMD_ID(7,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
+
+//cmd 20,u8 toaddr, u8 crc
+//ret 21, u8=2, u8 fromaddr, u16 clock,u16 crc, muut clientit lukee t?n
+//oldret 21, u8 fromaddr, u16 clock,u16 crc, muut clientit lukee t?n
+#define SMCMD_GET_CLOCK SMCMD_ID(10,SMCMD_MASK_0_PARAMS)
+#define SMCMD_GET_CLOCK_RET SMCMD_ID(10,SMCMD_MASK_2_PARAMS|SMCMD_MASK_RETURN)
+
+#ifdef PROCESS_IMAGE_SUPPORT
+
+//PROCESS_IMAGE communication not supported by SM V2.0.0. placeholders here
+
+//upload process data to bus devices. each device knows it's data offset in payload
+//(set by SMREGISTER_PDATA_IN_OFFSET, which is 0xffff if not accepting anything)
+#define SMCMD_PROCESS_IMAGE SMCMD_ID(5,SMCMD_MASK_N_PARAMS)
+
+//retrun of feedback process data is the tricky part
+//the device that has process data address of 0 answers first including SMCMD header (3bytes) and then its own return process data
+//SMREGISTER_PDATA_OUT_OFFSET is location of device's return processdata
+#define SMCMD_PROCESS_IMAGE_RET SMCMD_ID(5,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
+
+#endif
+
+
+#endif
diff --git a/sm_consts.c b/sm_consts.c
new file mode 100644
index 0000000..3ac98bb
--- /dev/null
+++ b/sm_consts.c
@@ -0,0 +1,61 @@
+#include "simplemotion_private.h"

+

+

+/* Table of CRC16 values for high-order byte */

+const smuint8 table_crc16_hi[] = {

+	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,

+	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,

+	0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,

+	0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,

+	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,

+	0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,

+	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,

+	0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,

+	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,

+	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,

+	0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,

+	0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,

+	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,

+	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,

+	0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,

+	0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,

+	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,

+	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,

+	0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,

+	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,

+	0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,

+	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,

+	0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,

+	0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,

+	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,

+	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40

+};

+/* Table of CRC16 values for low-order byte */

+const smuint8 table_crc16_lo[] = {

+	0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,

+	0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,

+	0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,

+	0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,

+	0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,

+	0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,

+	0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,

+	0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,

+	0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,

+	0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,

+	0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,

+	0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,

+	0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,

+	0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,

+	0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,

+	0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,

+	0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,

+	0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,

+	0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,

+	0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,

+	0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,

+	0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,

+	0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,

+	0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,

+	0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,

+	0x43, 0x83, 0x41, 0x81, 0x80, 0x40

+};

diff --git a/smtest.c b/smtest.c
new file mode 100644
index 0000000..e021da6
--- /dev/null
+++ b/smtest.c
@@ -0,0 +1,361 @@
+#include "simplemotion.h"
+#include <stdio.h>
+#include <stdlib.h>
+
+#define ADDR 255
+#include "vsd_cmd.h"
+
+
+//gcc -o testi *.c
+
+//#include "D:\gd\vsdr\vsdr-stm32\src\simplemotion_defs.h"
+#include "/home/tero/shr/svn/granited/firmware_design/eclipseworkspace/vsdr-stm32/src/simplemotion_defs.h"
+
+smbus h;
+       smint32 ret;
+       SM_STATUS stat=0;
+
+
+
+void test()
+{
+		//printf( "stat %d\n", smRawCommand( h, ADDR, 2, 0, &ret ) );
+	//printf("ret=%08x\n",ret);
+
+	//printf( "stat %d\n", smRawCommand( h, 2, 2, 0, &ret ) );
+	//printf("ret=%08x\n",ret);
+
+
+	
+        //stat|=smAppendSetParamCommandToQueue(h,SMP_DIGITAL_OUT_VALUE_1,151);
+//        stat|=smAppendSetParamCommandToQueue(h,SMP_INPUT_MULTIPLIER,11);
+        stat|=smExecuteCommandQueue( h, ADDR );
+        stat|=smGetQueuedSetParamReturnValue(h,&ret);
+        printf("setstat=%d val=%d\n",stat,ret);
+
+        stat=0;
+        stat|=smAppendGetParamCommandToQueue(h,SMP_BUS_SPEED);
+        stat|=smAppendGetParamCommandToQueue(h,SMP_SM_VERSION_COMPAT);
+        stat|=smAppendGetParamCommandToQueue(h,SMP_DIGITAL_OUT_VALUE_1);
+        stat|=smExecuteCommandQueue( h, ADDR );
+        stat|=smGetQueuedGetParamReturnValue(h,&ret);
+        printf("stat=%d val=%d\n",stat,ret);
+        stat|=smGetQueuedGetParamReturnValue(h,&ret);
+        printf("stat=%d val=%d\n",stat,ret);
+        stat|=smGetQueuedGetParamReturnValue(h,&ret);
+        printf("stat=%d val=%d\n",stat,ret);
+
+
+/*        smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN );
+        smAppendSMCommandToQueue( h, SMPCMD_24B, SMPRET_32B );
+        smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR );
+        smAppendSMCommandToQueue( h, SMPCMD_32B, SMP_BUS_SPEED );
+
+		smAppendSMCommandToQueue( h, SMPCMD_32B,SMP_INPUT_MULTIPLIER );
+		smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_INPUT_MULTIPLIER );
+		smAppendSMCommandToQueue( h, SMPCMD_24B, 100 );
+//		SMP_INPUT_MULTIPLIER
+
+
+        smExecuteCommandQueue( h, ADDR );
+        printf("",smGetQueuedSMCommandReturnValue( h ));
+        printf("",smGetQueuedSMCommandReturnValue( h ));
+        printf("",smGetQueuedSMCommandReturnValue( h ));
+        printf("",smGetQueuedSMCommandReturnValue( h ));
+        printf("",smGetQueuedSMCommandReturnValue( h ));
+        printf("",smGetQueuedSMCommandReturnValue( h ));
+        printf("",smGetQueuedSMCommandReturnValue( h ));
+ //       printf("%d\n",smGetQueuedSMCommandReturnValue( h ));*/
+
+        /*	smAppendCommandToQueue( h, CMD_GET_PARAM, RUNTIME_FAULTBITS );
+        smAppendCommandToQueue( h, CMD_GET_PARAM, RUNTIME_STATUSBITS );
+        smAppendCommandToQueue( h, CMD_GET_PARAM, RUNTIME_FIRMWARE_VERSION );
+        smExecuteCommandQueue( h, ADDR );
+        printf("%d\n",smGetQueuedCommandReturnValue( h, 0 ));
+        printf("%d\n",smGetQueuedCommandReturnValue( h, 1 ));
+        printf("%d\n",smGetQueuedCommandReturnValue( h, 2 ));
+        */
+}
+
+void tests();
+
+/*BL functions started by writing a value to this param:
+ * 1:mass erase and reset write pos counter to 0
+ * 2:write block
+ * 3:verify flash (update BOOTLOADER_STAT_FLASH_VERIFIED_OK status)
+ * 4:start main app
+ */
+#define SMP_BOOTLOADER_FUNCTION 191
+//upload 16bits of data to buffer. max buffer length 4096 bytes before it must be written with "write block" function
+//total uploaded amount must be multiple of 4 bytes before issuing write function
+#define SMP_BOOTLOADER_UPLOAD 192
+//bootloaded status
+#define SMP_BOOTLOADER_STAT 193
+	#define BOOTLOADER_STAT_FLASH_VERIFIED_OK BV(0)
+#define SMP_FIRMWARE_VERSION 6010
+#define SMP_BACKWARDS_COMP_VERSION 6012
+
+#define SUPPORTED_BL_VERSION_MIN 10000
+#define SUPPORTED_BL_VERSION_MAX 10010
+
+
+//load app rom from file. args:
+//pass pointer to "data". this func will allocate the memory
+//return number of 16bit words or -1 if load failed
+int loadBLfile( const char *filename, smuint16 **data )
+{
+	int bytes;
+	int i;
+	FILE *f;
+	smuint16 *buf;
+	
+	//open file
+	f=fopen(filename,"rb");
+	if(f==NULL)
+		return -1;
+	
+	//find file size
+	fseek(f, 0L, SEEK_END);
+	bytes = ftell(f);
+	fseek(f, 0L, SEEK_SET);
+	printf("size=%d\n",bytes);
+	//allocate
+	buf=(smuint16*)malloc((bytes+1)/2);//round upwards
+	if(buf==NULL)
+		return -1;
+	
+	//load
+	for(i=0;i<bytes;i++)
+	{
+		int get=fgetc(f);
+		if(get==EOF) 
+			return -1;
+		
+		//treat data as byte array
+		((smuint8*)buf)[i]=get;
+	}
+		printf(".\n");
+	//fclose(f);
+		printf(".\n");
+	
+	*data=buf;
+	
+	return ((bytes+1)/2);
+}
+
+#define BL_CHUNK_LEN 32
+
+int uploadGCfile(const char *fname)
+{
+	smint32 blversion,blcompversion;
+	stat=0;	
+	smuint16 *bin;
+	int i,c;
+	
+	int size=loadBLfile(fname,&bin);
+	if(size<0)
+	{
+		printf("load failed\n");
+		return;
+	}
+	printf("loaded %d words\n",size);
+	
+	
+	
+	//get possible node detected faults in communications
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, SMP_CUMULATIVE_STATUS );
+	//set return param type and command type
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_32B, SMPRET_CMD_STATUS );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_UPLOAD );
+	stat|=smExecuteCommandQueue( h, ADDR );
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	int smnodestats=ret;
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);	
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);	
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);	
+	
+	if(stat!=SM_OK || smnodestats!=SMP_CMD_STATUS_ACK)
+	{
+		printf("SM errors occurred:%d\n",smnodestats);
+		exit(5);
+	}
+	
+	//upload data in 32=BL_CHUNK_LEN word chunks
+	for(i=0;i<size;)
+	{
+		printf("pos %d\n",i);
+		//getchar();
+		
+		stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_UPLOAD );
+		for(c=0;c<BL_CHUNK_LEN;c++)
+		{
+			smuint16 upword;
+			//pad end of file with constant
+			if(i>=size)
+				upword=0xeeee;
+			else
+				upword=bin[i];
+			printf("i=%d\n",i);
+			
+			stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, upword );
+			i++;
+		}
+//		stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION );
+//		stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, 2);//do write
+		
+		stat|=smExecuteCommandQueue( h, ADDR );
+		
+		for(c=0;c<BL_CHUNK_LEN+1;c++)
+		{
+			stat|=smGetQueuedSMCommandReturnValue( h,&ret );
+			if(stat!=SM_OK)
+			{
+				printf("error stat=%d\n",stat);
+				exit(6);
+			}
+		}
+	}
+	
+	printf("done. enter to flash\n");
+	getchar();
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, 5);//flash gc
+	stat|=smExecuteCommandQueue( h, ADDR );
+}
+
+void blcommtest(const char *fname)
+{
+	smint32 blversion,blcompversion;
+	stat=0;	
+	smuint16 *bin;
+	int i,c;
+	
+	int size=loadBLfile(fname,&bin);
+	if(size<0)
+	{
+		printf("load failed\n");
+		return;
+	}
+	printf("loaded %d words\n",size);
+		
+	
+	//read bl variables
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_32B, SMPRET_32B );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_32B, SMP_FIRMWARE_VERSION );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_32B, SMP_BACKWARDS_COMP_VERSION );
+	stat|=smExecuteCommandQueue( h, ADDR );
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	blversion=ret;
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	blcompversion=ret;
+	
+	//if version ok
+	//if(blversion
+
+	//mass erase flash
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_32B, 1 );//do mass erase
+	stat|=smExecuteCommandQueue( h, ADDR );
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+
+	usleep(1000000);
+	
+	//flash
+	
+	//get possible node detected faults in communications
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, SMP_CUMULATIVE_STATUS );
+	//set return param type and command type
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_32B, SMPRET_CMD_STATUS );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_UPLOAD );
+	stat|=smExecuteCommandQueue( h, ADDR );
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);
+	int smnodestats=ret;
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);	
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);	
+	stat|=smGetQueuedSMCommandReturnValue( h,&ret );printf("stat=%d val=%d\n",stat,ret);	
+	
+	if(stat!=SM_OK || smnodestats!=SMP_CMD_STATUS_ACK)
+	{
+		printf("SM errors occurred:%d\n",smnodestats);
+		exit(5);
+	}
+	
+	//upload data in 32=BL_CHUNK_LEN word chunks
+	for(i=0;i<size;)
+	{
+		printf("pos %d\n",i);
+		//getchar();
+		
+		stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_UPLOAD );
+		for(c=0;c<BL_CHUNK_LEN;c++)
+		{
+			smuint16 upword;
+			//pad end of file with constant
+			if(i>=size)
+				upword=0xeeee;
+			else
+				upword=bin[i];
+			printf("i=%d\n",i);
+			
+			stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, upword );
+			i++;
+		}
+		stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION );
+		stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, 2);//do write
+		
+		stat|=smExecuteCommandQueue( h, ADDR );
+		
+		for(c=0;c<BL_CHUNK_LEN+3;c++)
+		{
+			stat|=smGetQueuedSMCommandReturnValue( h,&ret );
+			if(stat!=SM_OK)
+			{
+				printf("error stat=%d\n",stat);
+				exit(6);
+			}
+		}
+	}
+	
+	printf("done. enter to run\n");
+	getchar();
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION );
+	stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, 4);//launch
+	stat|=smExecuteCommandQueue( h, ADDR );
+
+	
+}
+
+int main(int args, const char *argv[])
+{
+	if(args<2)
+	{
+		printf("args: rom bin file name\n");
+		return(0);
+	}
+
+	smSetDebugOutput(High,stderr);
+
+	smSetTimeout(1500);
+        h=smOpenBus("COM17"); //17=USB1 16=0
+
+	printf("handle=%d\n",h);
+	if(h==-1) return 3;
+
+	
+	uploadGCfile(argv[1]);
+	//blcommtest(argv[1]);
+
+	smCloseBus(h);
+}