WebSVN - NaviCtrl - Diff - Rev 113 and 151


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

Subversion Repositories NaviCtrl

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