WebSVN - NaviCtrl - Diff - Rev 190 and 244


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + FOR NON COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
//
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <string.h>
#include <stdarg.h>
#include "91x_lib.h"
#include "mkprotocol.h" 
#include "ramfunc.h"
#include "usb.h"
 
void Buffer_Clear(Buffer_t* pBuffer)
{
        pBuffer->DataBytes = 0;
        pBuffer->Position = 0;
        pBuffer->Locked = FALSE;
}
 
void Buffer_Init(Buffer_t* pBuffer, u8* pDataBuffer, u16 DataBufferSize)
{
        pBuffer->pData = pDataBuffer;
        pBuffer->Size = DataBufferSize;
        Buffer_Clear(pBuffer); 
}
 
u8 Buffer_Copy(Buffer_t* pSrcBuffer, Buffer_t* pDstBuffer)
{
        u8 retval = 0;
 
        if( (pSrcBuffer != NULL) && (pDstBuffer != NULL) )
        {
                if(pSrcBuffer->Locked && !(pDstBuffer->Locked) && (pDstBuffer->Size >= pSrcBuffer->DataBytes))
                {
                        memcpy(pDstBuffer->pData, pSrcBuffer->pData, pSrcBuffer->DataBytes);
                        pDstBuffer->DataBytes = pSrcBuffer->DataBytes;
                        pDstBuffer->Position = 0;
                        pDstBuffer->Locked = TRUE;
                        retval = 1;
                }
        }
        return retval;
}
 
/**************************************************************/
/* Create serial output frame                                 */
/**************************************************************/
void MKProtocol_CreateSerialFrame(Buffer_t* pTxBuff, u8 CmdID, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
{
        va_list ap;
 
        u8 a,b,c;
        u8 ptr = 0;
        u16 tmpCRC = 0, i;
 
        u8* pdata = NULL;
        int len = 0;
       
        if(pTxBuff->Locked == FALSE)
        {       // tx-buffer is not in use
                // lock the buffer
                pTxBuff->Locked = TRUE;
                pTxBuff->Position = 0;
                pTxBuff->pData[pTxBuff->Position++] = '#';                          // Start character
                pTxBuff->pData[pTxBuff->Position++] = 'a' + Address;    // Address (a=0; b=1,...)
                pTxBuff->pData[pTxBuff->Position++] = CmdID;                    // Command
       
                va_start(ap, numofbuffers);
                if(numofbuffers)
                {
                        pdata = va_arg(ap, u8*);
                        len = va_arg(ap, int);
                        ptr = 0;
                        numofbuffers--;
                }
                while(len)
                {
                        if(len)
                        {
                                a = pdata[ptr++];
                                len--;
                                if((!len) && numofbuffers) // try to jump to next buffer
                                {
                                        pdata = va_arg(ap, u8*);
                                        len = va_arg(ap, int);
                                        ptr = 0;
                                        numofbuffers--;
                                }
                        }
                        else a = 0;
                        if(len)
                        {
                                b = pdata[ptr++];
                                len--;
                                if((!len) && numofbuffers) // try to jump to next buffer
                                {
                                        pdata = va_arg(ap, u8*);
                                        len = va_arg(ap, int);
                                        ptr = 0;
                                        numofbuffers--;
                                }
                        }
                        else b = 0;
                        if(len)
                        {
                                c = pdata[ptr++];
                                len--;
                                if((!len) && numofbuffers) // try to jump to next buffer
                                {
                                        pdata = va_arg(ap, u8*);
                                        len = va_arg(ap, int);
                                        ptr = 0;
                                        numofbuffers--;
                                }
                        }
                        else c = 0;
                        pTxBuff->pData[pTxBuff->Position++] = '=' + (a >> 2);
                        pTxBuff->pData[pTxBuff->Position++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
                        pTxBuff->pData[pTxBuff->Position++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
                        pTxBuff->pData[pTxBuff->Position++] = '=' + ( c & 0x3f);
                }
                va_end(ap);
                // add crc
                for(i = 0; i < pTxBuff->Position; i++)
                {
                        tmpCRC += pTxBuff->pData[i];
                }
                tmpCRC %= 4096;
                pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC / 64;
                pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC % 64;
                pTxBuff->pData[pTxBuff->Position++] = '\r';
                pTxBuff->DataBytes = pTxBuff->Position;
                pTxBuff->Position = 0;  // reset buffer position for transmision
        } // EOF buffer not in use
}
 
// typical called in an UART Rx ISR
/**************************************************************/
/* Collect serial frame                                       */
/**************************************************************/
u8 MKProtocol_CollectSerialFrame(Buffer_t* pRxBuff, u8 c)
{
        if(pRxBuff->Locked == FALSE)
        { // rx buffer not locked
                if(c == '#') // if syncronisation character is received
                {
                        pRxBuff->Position = 0;                                          // reset buffer
                        pRxBuff->pData[pRxBuff->Position++] = c;        // copy 1st byte to buffer
                        pRxBuff->DataBytes = 1;
                }
                else if (pRxBuff->Position < pRxBuff->Size) // rx buffer not full
                {
                        pRxBuff->pData[pRxBuff->Position++] = c; // copy byte to rxd buffer
                        pRxBuff->DataBytes++;
                        // termination character received and sync has been established
                        if ((c == '\r') && (pRxBuff->pData[0]== '#'))
                        {
                                // calculate checksum from transmitted data
                                u16 crc = 0, i;
                                u8 crc1, crc2;
                                for(i = 0; i < (pRxBuff->Position-3); i++)
                                {
                                        crc +=  pRxBuff->pData[i];  
                                }              
                                crc %= 4096;
                                crc1 = '=' + crc / 64;
                                crc2 = '=' + crc % 64;
                                // compare checksum to transmitted checksum bytes
                                if((crc1 == pRxBuff->pData[pRxBuff->Position-3]) && (crc2 == pRxBuff->pData[pRxBuff->Position-2]))
                                {
                                    // checksum is valid
                                        pRxBuff->Position = 0;                                 
                                        pRxBuff->Locked = TRUE;                                     // lock the rxd buffer 
                                        // if 2nd byte is an 'R' start bootloader
                                        if(pRxBuff->pData[2] == 'R')
                                        {
                                                PowerOff();
                                                VIC_DeInit();
                                                Execute_Bootloader(); // Reset-Commando - Bootloader starten
                                        }
                                } // eof checksum valid
                                else
                                {       // checksum is invalid
                                        Buffer_Clear(pRxBuff);
                                }  // eof checksum invalid
                        } // eof termination character received
                } // rxd buffer not full
                else // rxd buffer overrun
                {
                        Buffer_Clear(pRxBuff);
                } // eof rxd buffer overrrun
        }
        return(pRxBuff->Locked);
}
 
/**************************************************************/
/* Decode detination address                                  */
/**************************************************************/
void MKProtocol_DecodeSerialFrameHeader(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)
{
        if(pRxBuff->Locked)
        {
                pSerialMsg->Address = pRxBuff->pData[1] - 'a';
                pSerialMsg->CmdID = pRxBuff->pData[2];
        }
        else
        {
                pSerialMsg->Address = 0;
                pSerialMsg->CmdID = ' ';
        }      
}
 
/**************************************************************/
/* Decode data                                                */
/**************************************************************/
void MKProtocol_DecodeSerialFrameData(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)
{
        u8 a,b,c,d;
        u8 x,y,z;
        u8 ptrIn = 3; // start with first data byte in rx buffer
        u8 ptrOut = 3;
        u8 len = pRxBuff->DataBytes - 6;         // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
        while(len)
        {
                a = pRxBuff->pData[ptrIn++] - '=';
                b = pRxBuff->pData[ptrIn++] - '=';
                c = pRxBuff->pData[ptrIn++] - '=';
                d = pRxBuff->pData[ptrIn++] - '=';
                //if(ptrIn > ReceivedBytes - 3) break;
 
                x = (a << 2) | (b >> 4);
                y = ((b & 0x0f) << 4) | (c >> 2);
                z = ((c & 0x03) << 6) | d;
 
                if(len--) pRxBuff->pData[ptrOut++] = x; else break;
                if(len--) pRxBuff->pData[ptrOut++] = y; else break;
                if(len--) pRxBuff->pData[ptrOut++] = z; else break;
        }
        pSerialMsg->pData = &(pRxBuff->pData[3]);
        pSerialMsg->DataLen = ptrOut - 3;       // return number of data in bytes
        pRxBuff->Position = 0;
        pRxBuff->DataBytes = ptrOut;
}
 

Rev 190	Rev 244
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + Copyright (c) 2008 Ingo Busker, Holger Buss	5	// + Copyright (c) 2008 Ingo Busker, Holger Buss
6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY	6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
7	// + FOR NON COMMERCIAL USE ONLY	7	// + FOR NON COMMERCIAL USE ONLY
8	// + www.MikroKopter.com	8	// + www.MikroKopter.com
9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13	// + bzgl. der Nutzungsbedingungen aufzunehmen.	13	// + bzgl. der Nutzungsbedingungen aufzunehmen.
14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15	// + Verkauf von Luftbildaufnahmen, usw.	15	// + Verkauf von Luftbildaufnahmen, usw.
16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22	// + eindeutig als Ursprung verlinkt werden	22	// + eindeutig als Ursprung verlinkt werden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25	// + Benutzung auf eigene Gefahr	25	// + Benutzung auf eigene Gefahr
26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29	// + mit unserer Zustimmung zulässig	29	// + mit unserer Zustimmung zulässig
30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34	// + this list of conditions and the following disclaimer.	34	// + this list of conditions and the following disclaimer.
35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36	// + from this software without specific prior written permission.	36	// + from this software without specific prior written permission.
37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted	37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38	// + for non-commercial use (directly or indirectly)	38	// + for non-commercial use (directly or indirectly)
39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40	// + with our written permission	40	// + with our written permission
41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42	// + clearly linked as origin	42	// + clearly linked as origin
43	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed	43	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
44	//	44	//
45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55	// + POSSIBILITY OF SUCH DAMAGE.	55	// + POSSIBILITY OF SUCH DAMAGE.
56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57	#include <string.h>	57	#include <string.h>
58	#include <stdarg.h>	58	#include <stdarg.h>
59	#include "91x_lib.h"	59	#include "91x_lib.h"
60	#include "mkprotocol.h"	60	#include "mkprotocol.h"
61	#include "ramfunc.h"	61	#include "ramfunc.h"
62	#include "usb.h"	62	#include "usb.h"
63		63
64	void Buffer_Clear(Buffer_t* pBuffer)	64	void Buffer_Clear(Buffer_t* pBuffer)
65	{	65	{
66	pBuffer->DataBytes = 0;	66	pBuffer->DataBytes = 0;
67	pBuffer->Position = 0;	67	pBuffer->Position = 0;
68	pBuffer->Locked = FALSE;	68	pBuffer->Locked = FALSE;
69	}	69	}
70		70
71	void Buffer_Init(Buffer_t* pBuffer, u8* pDataBuffer, u16 DataBufferSize)	71	void Buffer_Init(Buffer_t* pBuffer, u8* pDataBuffer, u16 DataBufferSize)
72	{	72	{
73	pBuffer->pData = pDataBuffer;	73	pBuffer->pData = pDataBuffer;
74	pBuffer->Size = DataBufferSize;	74	pBuffer->Size = DataBufferSize;
75	Buffer_Clear(pBuffer);	75	Buffer_Clear(pBuffer);
76	}	76	}
77		77
78	u8 Buffer_Copy(Buffer_t* pSrcBuffer, Buffer_t* pDstBuffer)	78	u8 Buffer_Copy(Buffer_t* pSrcBuffer, Buffer_t* pDstBuffer)
79	{	79	{
80	u8 retval = 0;	80	u8 retval = 0;
81		81
82	if( (pSrcBuffer != NULL) && (pDstBuffer != NULL) )	82	if( (pSrcBuffer != NULL) && (pDstBuffer != NULL) )
83	{	83	{
84	if(pSrcBuffer->Locked && !(pDstBuffer->Locked) && (pDstBuffer->Size >= pSrcBuffer->DataBytes))	84	if(pSrcBuffer->Locked && !(pDstBuffer->Locked) && (pDstBuffer->Size >= pSrcBuffer->DataBytes))
85	{	85	{
86	memcpy(pDstBuffer->pData, pSrcBuffer->pData, pSrcBuffer->DataBytes);	86	memcpy(pDstBuffer->pData, pSrcBuffer->pData, pSrcBuffer->DataBytes);
87	pDstBuffer->DataBytes = pSrcBuffer->DataBytes;	87	pDstBuffer->DataBytes = pSrcBuffer->DataBytes;
88	pDstBuffer->Position = 0;	88	pDstBuffer->Position = 0;
89	pDstBuffer->Locked = TRUE;	89	pDstBuffer->Locked = TRUE;
90	retval = 1;	90	retval = 1;
91	}	91	}
92	}	92	}
93	return retval;	93	return retval;
94	}	94	}
95		95
96	/**************************************************************/	96	/**************************************************************/
97	/* Create serial output frame */	97	/* Create serial output frame */
98	/**************************************************************/	98	/**************************************************************/
99	void MKProtocol_CreateSerialFrame(Buffer_t* pTxBuff, u8 CmdID, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....	99	void MKProtocol_CreateSerialFrame(Buffer_t* pTxBuff, u8 CmdID, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
100	{	100	{
101	va_list ap;	101	va_list ap;
102		102
103	u8 a,b,c;	103	u8 a,b,c;
104	u8 ptr = 0;	104	u8 ptr = 0;
105	u16 tmpCRC = 0, i;	105	u16 tmpCRC = 0, i;
106		106
107	u8* pdata = NULL;	107	u8* pdata = NULL;
108	int len = 0;	108	int len = 0;
109		109
110	if(pTxBuff->Locked == FALSE)	110	if(pTxBuff->Locked == FALSE)
111	{ // tx-buffer is not in use	111	{ // tx-buffer is not in use
112	// lock the buffer	112	// lock the buffer
113	pTxBuff->Locked = TRUE;	113	pTxBuff->Locked = TRUE;
114	pTxBuff->Position = 0;	114	pTxBuff->Position = 0;
115	pTxBuff->pData[pTxBuff->Position++] = '#'; // Start character	115	pTxBuff->pData[pTxBuff->Position++] = '#'; // Start character
116	pTxBuff->pData[pTxBuff->Position++] = 'a' + Address; // Address (a=0; b=1,...)	116	pTxBuff->pData[pTxBuff->Position++] = 'a' + Address; // Address (a=0; b=1,...)
117	pTxBuff->pData[pTxBuff->Position++] = CmdID; // Command	117	pTxBuff->pData[pTxBuff->Position++] = CmdID; // Command
118		118
119	va_start(ap, numofbuffers);	119	va_start(ap, numofbuffers);
120	if(numofbuffers)	120	if(numofbuffers)
121	{	121	{
122	pdata = va_arg(ap, u8*);	122	pdata = va_arg(ap, u8*);
123	len = va_arg(ap, int);	123	len = va_arg(ap, int);
124	ptr = 0;	124	ptr = 0;
125	numofbuffers--;	125	numofbuffers--;
126	}	126	}
127	while(len)	127	while(len)
128	{	128	{
129	if(len)	129	if(len)
130	{	130	{
131	a = pdata[ptr++];	131	a = pdata[ptr++];
132	len--;	132	len--;
133	if((!len) && numofbuffers) // try to jump to next buffer	133	if((!len) && numofbuffers) // try to jump to next buffer
134	{	134	{
135	pdata = va_arg(ap, u8*);	135	pdata = va_arg(ap, u8*);
136	len = va_arg(ap, int);	136	len = va_arg(ap, int);
137	ptr = 0;	137	ptr = 0;
138	numofbuffers--;	138	numofbuffers--;
139	}	139	}
140	}	140	}
141	else a = 0;	141	else a = 0;
142	if(len)	142	if(len)
143	{	143	{
144	b = pdata[ptr++];	144	b = pdata[ptr++];
145	len--;	145	len--;
146	if((!len) && numofbuffers) // try to jump to next buffer	146	if((!len) && numofbuffers) // try to jump to next buffer
147	{	147	{
148	pdata = va_arg(ap, u8*);	148	pdata = va_arg(ap, u8*);
149	len = va_arg(ap, int);	149	len = va_arg(ap, int);
150	ptr = 0;	150	ptr = 0;
151	numofbuffers--;	151	numofbuffers--;
152	}	152	}
153	}	153	}
154	else b = 0;	154	else b = 0;
155	if(len)	155	if(len)
156	{	156	{
157	c = pdata[ptr++];	157	c = pdata[ptr++];
158	len--;	158	len--;
159	if((!len) && numofbuffers) // try to jump to next buffer	159	if((!len) && numofbuffers) // try to jump to next buffer
160	{	160	{
161	pdata = va_arg(ap, u8*);	161	pdata = va_arg(ap, u8*);
162	len = va_arg(ap, int);	162	len = va_arg(ap, int);
163	ptr = 0;	163	ptr = 0;
164	numofbuffers--;	164	numofbuffers--;
165	}	165	}
166	}	166	}
167	else c = 0;	167	else c = 0;
168	pTxBuff->pData[pTxBuff->Position++] = '=' + (a >> 2);	168	pTxBuff->pData[pTxBuff->Position++] = '=' + (a >> 2);
169	pTxBuff->pData[pTxBuff->Position++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	169	pTxBuff->pData[pTxBuff->Position++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
170	pTxBuff->pData[pTxBuff->Position++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	170	pTxBuff->pData[pTxBuff->Position++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
171	pTxBuff->pData[pTxBuff->Position++] = '=' + ( c & 0x3f);	171	pTxBuff->pData[pTxBuff->Position++] = '=' + ( c & 0x3f);
172	}	172	}
173	va_end(ap);	173	va_end(ap);
174	// add crc	174	// add crc
175	for(i = 0; i < pTxBuff->Position; i++)	175	for(i = 0; i < pTxBuff->Position; i++)
176	{	176	{
177	tmpCRC += pTxBuff->pData[i];	177	tmpCRC += pTxBuff->pData[i];
178	}	178	}
179	tmpCRC %= 4096;	179	tmpCRC %= 4096;
180	pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC / 64;	180	pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC / 64;
181	pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC % 64;	181	pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC % 64;
182	pTxBuff->pData[pTxBuff->Position++] = '\r';	182	pTxBuff->pData[pTxBuff->Position++] = '\r';
183	pTxBuff->DataBytes = pTxBuff->Position;	183	pTxBuff->DataBytes = pTxBuff->Position;
184	pTxBuff->Position = 0; // reset buffer position for transmision	184	pTxBuff->Position = 0; // reset buffer position for transmision
185	} // EOF buffer not in use	185	} // EOF buffer not in use
186	}	186	}
187		187
188	// typical called in an UART Rx ISR	188	// typical called in an UART Rx ISR
189	/**************************************************************/	189	/**************************************************************/
190	/* Collect serial frame */	190	/* Collect serial frame */
191	/**************************************************************/	191	/**************************************************************/
192	u8 MKProtocol_CollectSerialFrame(Buffer_t* pRxBuff, u8 c)	192	u8 MKProtocol_CollectSerialFrame(Buffer_t* pRxBuff, u8 c)
193	{	193	{
194	if(pRxBuff->Locked == FALSE)	194	if(pRxBuff->Locked == FALSE)
195	{ // rx buffer not locked	195	{ // rx buffer not locked
196	if(c == '#') // if syncronisation character is received	196	if(c == '#') // if syncronisation character is received
197	{	197	{
198	pRxBuff->Position = 0; // reset buffer	198	pRxBuff->Position = 0; // reset buffer
199	pRxBuff->pData[pRxBuff->Position++] = c; // copy 1st byte to buffer	199	pRxBuff->pData[pRxBuff->Position++] = c; // copy 1st byte to buffer
200	pRxBuff->DataBytes = 1;	200	pRxBuff->DataBytes = 1;
201	}	201	}
202	else if (pRxBuff->Position < pRxBuff->Size) // rx buffer not full	202	else if (pRxBuff->Position < pRxBuff->Size) // rx buffer not full
203	{	203	{
204	pRxBuff->pData[pRxBuff->Position++] = c; // copy byte to rxd buffer	204	pRxBuff->pData[pRxBuff->Position++] = c; // copy byte to rxd buffer
205	pRxBuff->DataBytes++;	205	pRxBuff->DataBytes++;
-		206	// termination character received and sync has been established
206	if (c == '\r') // termination character received	207	if ((c == '\r') && (pRxBuff->pData[0]== '#'))
207	{	208	{
208	// calculate checksum from transmitted data	209	// calculate checksum from transmitted data
209	u16 crc = 0, i;	210	u16 crc = 0, i;
210	u8 crc1, crc2;	211	u8 crc1, crc2;
211	for(i = 0; i < (pRxBuff->Position-3); i++)	212	for(i = 0; i < (pRxBuff->Position-3); i++)
212	{	213	{
213	crc += pRxBuff->pData[i];	214	crc += pRxBuff->pData[i];
214	}	215	}
215	crc %= 4096;	216	crc %= 4096;
216	crc1 = '=' + crc / 64;	217	crc1 = '=' + crc / 64;
217	crc2 = '=' + crc % 64;	218	crc2 = '=' + crc % 64;
218	// compare checksum to transmitted checksum bytes	219	// compare checksum to transmitted checksum bytes
219	if((crc1 == pRxBuff->pData[pRxBuff->Position-3]) && (crc2 == pRxBuff->pData[pRxBuff->Position-2]))	220	if((crc1 == pRxBuff->pData[pRxBuff->Position-3]) && (crc2 == pRxBuff->pData[pRxBuff->Position-2]))
-		221	{
220	{ // checksum is valid	222	// checksum is valid
221	pRxBuff->Position = 0;	223	pRxBuff->Position = 0;
222	pRxBuff->Locked = TRUE; // lock the rxd buffer	224	pRxBuff->Locked = TRUE; // lock the rxd buffer
223	// if 2nd byte is an 'R' start bootloader	225	// if 2nd byte is an 'R' start bootloader
224	if(pRxBuff->pData[2] == 'R')	226	if(pRxBuff->pData[2] == 'R')
225	{	227	{
226	PowerOff();	228	PowerOff();
227	VIC_DeInit();	229	VIC_DeInit();
228	Execute_Bootloader(); // Reset-Commando - Bootloader starten	230	Execute_Bootloader(); // Reset-Commando - Bootloader starten
229	}	231	}
230	} // eof checksum valid	232	} // eof checksum valid
231	else	233	else
232	{ // checksum is invalid	234	{ // checksum is invalid
233	Buffer_Clear(pRxBuff);	235	Buffer_Clear(pRxBuff);
234	} // eof checksum invalid	236	} // eof checksum invalid
235	} // eof termination character received	237	} // eof termination character received
236	} // rxd buffer not full	238	} // rxd buffer not full
237	else // rxd buffer overrun	239	else // rxd buffer overrun
238	{	240	{
239	Buffer_Clear(pRxBuff);	241	Buffer_Clear(pRxBuff);
240	} // eof rxd buffer overrrun	242	} // eof rxd buffer overrrun
241	}	243	}
242	return(pRxBuff->Locked);	244	return(pRxBuff->Locked);
243	}	245	}
244		246
245	/**************************************************************/	247	/**************************************************************/
246	/* Decode detination address */	248	/* Decode detination address */
247	/**************************************************************/	249	/**************************************************************/
248	void MKProtocol_DecodeSerialFrameHeader(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)	250	void MKProtocol_DecodeSerialFrameHeader(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)
249	{	251	{
250	if(pRxBuff->Locked)	252	if(pRxBuff->Locked)
251	{	253	{
252	pSerialMsg->Address = pRxBuff->pData[1] - 'a';	254	pSerialMsg->Address = pRxBuff->pData[1] - 'a';
253	pSerialMsg->CmdID = pRxBuff->pData[2];	255	pSerialMsg->CmdID = pRxBuff->pData[2];
254	}	256	}
255	else	257	else
256	{	258	{
257	pSerialMsg->Address = 0;	259	pSerialMsg->Address = 0;
258	pSerialMsg->CmdID = ' ';	260	pSerialMsg->CmdID = ' ';
259	}	261	}
260	}	262	}
261		263
262	/**************************************************************/	264	/**************************************************************/
263	/* Decode data */	265	/* Decode data */
264	/**************************************************************/	266	/**************************************************************/
265	void MKProtocol_DecodeSerialFrameData(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)	267	void MKProtocol_DecodeSerialFrameData(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)
266	{	268	{
267	u8 a,b,c,d;	269	u8 a,b,c,d;
268	u8 x,y,z;	270	u8 x,y,z;
269	u8 ptrIn = 3; // start with first data byte in rx buffer	271	u8 ptrIn = 3; // start with first data byte in rx buffer
270	u8 ptrOut = 3;	272	u8 ptrOut = 3;
271	u8 len = pRxBuff->DataBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )	273	u8 len = pRxBuff->DataBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
272	while(len)	274	while(len)
273	{	275	{
274	a = pRxBuff->pData[ptrIn++] - '=';	276	a = pRxBuff->pData[ptrIn++] - '=';
275	b = pRxBuff->pData[ptrIn++] - '=';	277	b = pRxBuff->pData[ptrIn++] - '=';
276	c = pRxBuff->pData[ptrIn++] - '=';	278	c = pRxBuff->pData[ptrIn++] - '=';
277	d = pRxBuff->pData[ptrIn++] - '=';	279	d = pRxBuff->pData[ptrIn++] - '=';
278	//if(ptrIn > ReceivedBytes - 3) break;	280	//if(ptrIn > ReceivedBytes - 3) break;
279		281
280	x = (a << 2) \| (b >> 4);	282	x = (a << 2) \| (b >> 4);
281	y = ((b & 0x0f) << 4) \| (c >> 2);	283	y = ((b & 0x0f) << 4) \| (c >> 2);
282	z = ((c & 0x03) << 6) \| d;	284	z = ((c & 0x03) << 6) \| d;
283		285
284	if(len--) pRxBuff->pData[ptrOut++] = x; else break;	286	if(len--) pRxBuff->pData[ptrOut++] = x; else break;
285	if(len--) pRxBuff->pData[ptrOut++] = y; else break;	287	if(len--) pRxBuff->pData[ptrOut++] = y; else break;
286	if(len--) pRxBuff->pData[ptrOut++] = z; else break;	288	if(len--) pRxBuff->pData[ptrOut++] = z; else break;
287	}	289	}
288	pSerialMsg->pData = &(pRxBuff->pData[3]);	290	pSerialMsg->pData = &(pRxBuff->pData[3]);
289	pSerialMsg->DataLen = ptrOut - 3; // return number of data in bytes	291	pSerialMsg->DataLen = ptrOut - 3; // return number of data in bytes
290	pRxBuff->Position = 0;	292	pRxBuff->Position = 0;
291	pRxBuff->DataBytes = ptrOut;	293	pRxBuff->DataBytes = ptrOut;
292	}	294	}
293		295

Subversion Repositories NaviCtrl

(root)/tags/V0.24c/mkprotocol.c @ 415 - Rev 190 → 244