WebSVN - NaviCtrl - Diff - Rev 151 and 153


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch
// + 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 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 this software (or part of it) to systems (other than 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); 
}
 
/**************************************************************/
/* 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
                {
                        if (c != '\r') // no termination character received
                        {
                                pRxBuff->pData[pRxBuff->Position++] = c; // copy byte to rxd buffer
                                pRxBuff->DataBytes++;
                        }
                        else // termination character received
                        {
                                // calculate checksum from transmitted data
                                u16 crc = 0, i;
                                u8 crc1, crc2;
                                for(i = 0; i < (pRxBuff->Position-2); 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-2]) && (crc2 == pRxBuff->pData[pRxBuff->Position-1]))
                                {   // checksum is valid
                                        pRxBuff->pData[pRxBuff->Position++] = '\r';     // set termination character
                                        pRxBuff->DataBytes = pRxBuff->Position;
                                        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 data                                                */
/**************************************************************/
void MKProtocol_DecodeSerialFrame(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;
        }
        pRxBuff->pData[1] -= 'a'; // substract address offset
        pSerialMsg->pAddress = &(pRxBuff->pData[1]);
        pSerialMsg->pCmdID = &(pRxBuff->pData[2]);
        pSerialMsg->pData = &(pRxBuff->pData[3]);
        pSerialMsg->DataLen = ptrOut - 3;       // return number of data in bytes
        pRxBuff->Position = 0;
        pRxBuff->DataBytes = ptrOut;
}
 

Rev 151	Rev 153
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	6	// + Nur für den privaten Gebrauch
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 der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	28	// + Die PORTIERUNG 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 this software (or part of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed	43	// + * PORTING this software (or part of it) to systems (other than 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	/**************************************************************/	78	/**************************************************************/
79	/* Create serial output frame */	79	/* Create serial output frame */
80	/**************************************************************/	80	/**************************************************************/
81	void MKProtocol_CreateSerialFrame(Buffer_t* pTxBuff, u8 CmdID, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....	81	void MKProtocol_CreateSerialFrame(Buffer_t* pTxBuff, u8 CmdID, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
82	{	82	{
83	va_list ap;	83	va_list ap;
84		84
85	u8 a,b,c;	85	u8 a,b,c;
86	u8 ptr = 0;	86	u8 ptr = 0;
87	u16 tmpCRC = 0, i;	87	u16 tmpCRC = 0, i;
88		88
89	u8* pdata = NULL;	89	u8* pdata = NULL;
90	int len = 0;	90	int len = 0;
91		91
92	if(pTxBuff->Locked == FALSE)	92	if(pTxBuff->Locked == FALSE)
93	{ // tx-buffer is not in use	93	{ // tx-buffer is not in use
94	// lock the buffer	94	// lock the buffer
95	pTxBuff->Locked = TRUE;	95	pTxBuff->Locked = TRUE;
96	pTxBuff->Position = 0;	96	pTxBuff->Position = 0;
97	pTxBuff->pData[pTxBuff->Position++] = '#'; // Start character	97	pTxBuff->pData[pTxBuff->Position++] = '#'; // Start character
98	pTxBuff->pData[pTxBuff->Position++] = 'a' + Address; // Address (a=0; b=1,...)	98	pTxBuff->pData[pTxBuff->Position++] = 'a' + Address; // Address (a=0; b=1,...)
99	pTxBuff->pData[pTxBuff->Position++] = CmdID; // Command	99	pTxBuff->pData[pTxBuff->Position++] = CmdID; // Command
100		100
101	va_start(ap, numofbuffers);	101	va_start(ap, numofbuffers);
102	if(numofbuffers)	102	if(numofbuffers)
103	{	103	{
104	pdata = va_arg(ap, u8*);	104	pdata = va_arg(ap, u8*);
105	len = va_arg(ap, int);	105	len = va_arg(ap, int);
106	ptr = 0;	106	ptr = 0;
107	numofbuffers--;	107	numofbuffers--;
108	}	108	}
109	while(len)	109	while(len)
110	{	110	{
111	if(len)	111	if(len)
112	{	112	{
113	a = pdata[ptr++];	113	a = pdata[ptr++];
114	len--;	114	len--;
115	if((!len) && numofbuffers) // try to jump to next buffer	115	if((!len) && numofbuffers) // try to jump to next buffer
116	{	116	{
117	pdata = va_arg(ap, u8*);	117	pdata = va_arg(ap, u8*);
118	len = va_arg(ap, int);	118	len = va_arg(ap, int);
119	ptr = 0;	119	ptr = 0;
120	numofbuffers--;	120	numofbuffers--;
121	}	121	}
122	}	122	}
123	else a = 0;	123	else a = 0;
124	if(len)	124	if(len)
125	{	125	{
126	b = pdata[ptr++];	126	b = pdata[ptr++];
127	len--;	127	len--;
128	if((!len) && numofbuffers) // try to jump to next buffer	128	if((!len) && numofbuffers) // try to jump to next buffer
129	{	129	{
130	pdata = va_arg(ap, u8*);	130	pdata = va_arg(ap, u8*);
131	len = va_arg(ap, int);	131	len = va_arg(ap, int);
132	ptr = 0;	132	ptr = 0;
133	numofbuffers--;	133	numofbuffers--;
134	}	134	}
135	}	135	}
136	else b = 0;	136	else b = 0;
137	if(len)	137	if(len)
138	{	138	{
139	c = pdata[ptr++];	139	c = pdata[ptr++];
140	len--;	140	len--;
141	if((!len) && numofbuffers) // try to jump to next buffer	141	if((!len) && numofbuffers) // try to jump to next buffer
142	{	142	{
143	pdata = va_arg(ap, u8*);	143	pdata = va_arg(ap, u8*);
144	len = va_arg(ap, int);	144	len = va_arg(ap, int);
145	ptr = 0;	145	ptr = 0;
146	numofbuffers--;	146	numofbuffers--;
147	}	147	}
148	}	148	}
149	else c = 0;	149	else c = 0;
150	pTxBuff->pData[pTxBuff->Position++] = '=' + (a >> 2);	150	pTxBuff->pData[pTxBuff->Position++] = '=' + (a >> 2);
151	pTxBuff->pData[pTxBuff->Position++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	151	pTxBuff->pData[pTxBuff->Position++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
152	pTxBuff->pData[pTxBuff->Position++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	152	pTxBuff->pData[pTxBuff->Position++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
153	pTxBuff->pData[pTxBuff->Position++] = '=' + ( c & 0x3f);	153	pTxBuff->pData[pTxBuff->Position++] = '=' + ( c & 0x3f);
154	}	154	}
155	va_end(ap);	155	va_end(ap);
156	// add crc	156	// add crc
157	for(i = 0; i < pTxBuff->Position; i++)	157	for(i = 0; i < pTxBuff->Position; i++)
158	{	158	{
159	tmpCRC += pTxBuff->pData[i];	159	tmpCRC += pTxBuff->pData[i];
160	}	160	}
161	tmpCRC %= 4096;	161	tmpCRC %= 4096;
162	pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC / 64;	162	pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC / 64;
163	pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC % 64;	163	pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC % 64;
164	pTxBuff->pData[pTxBuff->Position++] = '\r';	164	pTxBuff->pData[pTxBuff->Position++] = '\r';
165	pTxBuff->DataBytes = pTxBuff->Position;	165	pTxBuff->DataBytes = pTxBuff->Position;
166	pTxBuff->Position = 0; // reset buffer position for transmision	166	pTxBuff->Position = 0; // reset buffer position for transmision
167	} // EOF buffer not in use	167	} // EOF buffer not in use
168	}	168	}
169		169
170	// typical called in an UART Rx ISR	170	// typical called in an UART Rx ISR
171	/**************************************************************/	171	/**************************************************************/
172	/* Collect serial frame */	172	/* Collect serial frame */
173	/**************************************************************/	173	/**************************************************************/
174	void MKProtocol_CollectSerialFrame(Buffer_t* pRxBuff, u8 c)	174	u8 MKProtocol_CollectSerialFrame(Buffer_t* pRxBuff, u8 c)
175	{	175	{
176	if(pRxBuff->Locked == FALSE)	176	if(pRxBuff->Locked == FALSE)
177	{ // rx buffer not locked	177	{ // rx buffer not locked
178	if(c == '#') // if syncronisation character is received	178	if(c == '#') // if syncronisation character is received
179	{	179	{
180	pRxBuff->Position = 0; // reset buffer	180	pRxBuff->Position = 0; // reset buffer
181	pRxBuff->pData[pRxBuff->Position++] = c; // copy 1st byte to buffer	181	pRxBuff->pData[pRxBuff->Position++] = c; // copy 1st byte to buffer
182	pRxBuff->DataBytes = 1;	182	pRxBuff->DataBytes = 1;
183	}	183	}
184	else if (pRxBuff->Position < pRxBuff->Size) // rx buffer not full	184	else if (pRxBuff->Position < pRxBuff->Size) // rx buffer not full
185	{	185	{
186	if (c != '\r') // no termination character received	186	if (c != '\r') // no termination character received
187	{	187	{
188	pRxBuff->pData[pRxBuff->Position++] = c; // copy byte to rxd buffer	188	pRxBuff->pData[pRxBuff->Position++] = c; // copy byte to rxd buffer
189	pRxBuff->DataBytes++;	189	pRxBuff->DataBytes++;
190	}	190	}
191	else // termination character received	191	else // termination character received
192	{	192	{
193	// calculate checksum from transmitted data	193	// calculate checksum from transmitted data
194	u16 crc = 0, i;	194	u16 crc = 0, i;
195	u8 crc1, crc2;	195	u8 crc1, crc2;
196	for(i = 0; i < (pRxBuff->Position-2); i++)	196	for(i = 0; i < (pRxBuff->Position-2); i++)
197	{	197	{
198	crc += pRxBuff->pData[i];	198	crc += pRxBuff->pData[i];
199	}	199	}
200	crc %= 4096;	200	crc %= 4096;
201	crc1 = '=' + crc / 64;	201	crc1 = '=' + crc / 64;
202	crc2 = '=' + crc % 64;	202	crc2 = '=' + crc % 64;
203	// compare checksum to transmitted checksum bytes	203	// compare checksum to transmitted checksum bytes
204	if((crc1 == pRxBuff->pData[pRxBuff->Position-2]) && (crc2 == pRxBuff->pData[pRxBuff->Position-1]))	204	if((crc1 == pRxBuff->pData[pRxBuff->Position-2]) && (crc2 == pRxBuff->pData[pRxBuff->Position-1]))
205	{ // checksum is valid	205	{ // checksum is valid
206	pRxBuff->pData[pRxBuff->Position++] = '\r'; // set termination character	206	pRxBuff->pData[pRxBuff->Position++] = '\r'; // set termination character
207	pRxBuff->DataBytes = pRxBuff->Position;	207	pRxBuff->DataBytes = pRxBuff->Position;
208	pRxBuff->Position = 0;	208	pRxBuff->Position = 0;
209	pRxBuff->Locked = TRUE; // lock the rxd buffer	209	pRxBuff->Locked = TRUE; // lock the rxd buffer
210	// if 2nd byte is an 'R' start bootloader	210	// if 2nd byte is an 'R' start bootloader
211	if(pRxBuff->pData[2] == 'R')	211	if(pRxBuff->pData[2] == 'R')
212	{	212	{
213	PowerOff();	213	PowerOff();
214	VIC_DeInit();	214	VIC_DeInit();
215	Execute_Bootloader(); // Reset-Commando - Bootloader starten	215	Execute_Bootloader(); // Reset-Commando - Bootloader starten
216	}	216	}
217	} // eof checksum valid	217	} // eof checksum valid
218	else	218	else
219	{ // checksum is invalid	219	{ // checksum is invalid
220	Buffer_Clear(pRxBuff);	220	Buffer_Clear(pRxBuff);
221	} // eof checksum invalid	221	} // eof checksum invalid
222	} // eof termination character received	222	} // eof termination character received
223	} // rxd buffer not full	223	} // rxd buffer not full
224	else // rxd buffer overrun	224	else // rxd buffer overrun
225	{	225	{
226	Buffer_Clear(pRxBuff);	226	Buffer_Clear(pRxBuff);
227	} // eof rxd buffer overrrun	227	} // eof rxd buffer overrrun
228	}	228	}
-		229	return(pRxBuff->Locked);
229	}	230	}
230		231
231	/**************************************************************/	232	/**************************************************************/
232	/* Decode data */	233	/* Decode data */
233	/**************************************************************/	234	/**************************************************************/
234	void MKProtocol_DecodeSerialFrame(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)	235	void MKProtocol_DecodeSerialFrame(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)
235	{	236	{
236	u8 a,b,c,d;	237	u8 a,b,c,d;
237	u8 x,y,z;	238	u8 x,y,z;
238	u8 ptrIn = 3; // start with first data byte in rx buffer	239	u8 ptrIn = 3; // start with first data byte in rx buffer
239	u8 ptrOut = 3;	240	u8 ptrOut = 3;
240	u8 len = pRxBuff->DataBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )	241	u8 len = pRxBuff->DataBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
241	while(len)	242	while(len)
242	{	243	{
243	a = pRxBuff->pData[ptrIn++] - '=';	244	a = pRxBuff->pData[ptrIn++] - '=';
244	b = pRxBuff->pData[ptrIn++] - '=';	245	b = pRxBuff->pData[ptrIn++] - '=';
245	c = pRxBuff->pData[ptrIn++] - '=';	246	c = pRxBuff->pData[ptrIn++] - '=';
246	d = pRxBuff->pData[ptrIn++] - '=';	247	d = pRxBuff->pData[ptrIn++] - '=';
247	//if(ptrIn > ReceivedBytes - 3) break;	248	//if(ptrIn > ReceivedBytes - 3) break;
248		249
249	x = (a << 2) \| (b >> 4);	250	x = (a << 2) \| (b >> 4);
250	y = ((b & 0x0f) << 4) \| (c >> 2);	251	y = ((b & 0x0f) << 4) \| (c >> 2);
251	z = ((c & 0x03) << 6) \| d;	252	z = ((c & 0x03) << 6) \| d;
252		253
253	if(len--) pRxBuff->pData[ptrOut++] = x; else break;	254	if(len--) pRxBuff->pData[ptrOut++] = x; else break;
254	if(len--) pRxBuff->pData[ptrOut++] = y; else break;	255	if(len--) pRxBuff->pData[ptrOut++] = y; else break;
255	if(len--) pRxBuff->pData[ptrOut++] = z; else break;	256	if(len--) pRxBuff->pData[ptrOut++] = z; else break;
256	}	257	}
-		258	pRxBuff->pData[1] -= 'a'; // substract address offset
257	pSerialMsg->Address = pRxBuff->pData[1] - 'a';	259	pSerialMsg->pAddress = &(pRxBuff->pData[1]);
258	pSerialMsg->CmdID = pRxBuff->pData[2];	260	pSerialMsg->pCmdID = &(pRxBuff->pData[2]);
259	pSerialMsg->pData = &(pRxBuff->pData[3]);	261	pSerialMsg->pData = &(pRxBuff->pData[3]);
260	pSerialMsg->DataLen = ptrOut - 3; // return number of data in bytes	262	pSerialMsg->DataLen = ptrOut - 3; // return number of data in bytes
261	pRxBuff->Position = 0;	263	pRxBuff->Position = 0;
262	pRxBuff->DataBytes = ptrOut;	264	pRxBuff->DataBytes = ptrOut;
263	}	265	}
264		266

Subversion Repositories NaviCtrl

(root)/tags/V2.10c/mkprotocol.c @ 811 - Rev 151 → 153