WebSVN - NaviCtrl - Diff - Rev 171 and 190 - /tags/V2.07b


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

Subversion Repositories NaviCtrl

(root)/tags/V2.07b_Beta/mkprotocol.c @ 702 - Rev 171 → 190