Subversion Repositories Projects

Rev

Rev 391 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
391 Brean 1
#include <Parser.h>
2
 
3
// Base64 Decoder
4
// see Parser.h for details about sRxData
393 Brean 5
void Parser::decode64(sRxData &rx)
391 Brean 6
{
393 Brean 7
    int length = rx.str.size();
391 Brean 8
    unsigned char a,b,c,d;
9
    unsigned char ptr = 0;
10
    unsigned char x,y,z;
11
    int ptrOut[150];
12
 
13
    int ptrIn = 3;
14
    int max = length;
15
    int len = length;
16
    int decLen = 0;
17
 
18
    if (rx.Input[ptrIn] == 0) {
19
        throw "Nothing received";
20
    }
21
 
22
    while(len != 0) {
393 Brean 23
        a = rx.input[ptrIn++] - '=';
24
        b = rx.input[ptrIn++] - '=';
25
        c = rx.input[ptrIn++] - '=';
26
        d = rx.input[ptrIn++] - '=';
391 Brean 27
 
28
        if(ptrIn > max - 2) break;
29
 
30
        x = (a << 2) | (b >> 4);
31
        y = ((b & 0x0f) << 4) | (c >> 2);
32
        z = ((c & 0x03) << 6) | d;
33
 
34
        if(len--) ptrOut[ptr++] = x; else break;
35
        if(len--) ptrOut[ptr++] = y; else break;
36
        if(len--) ptrOut[ptr++] = z; else break;
37
    }
38
 
39
    for (int a=0; a<ptr; a++) {
40
        if (Long == false) {
41
            int b1, b2, b3;
42
 
43
            b1 = ptrOut[a++];
44
            b2 = ptrOut[a];
45
 
46
            b3 = (b2 << 8) | b1;
47
 
48
            if (b3 > 32767)
49
                b3 = b3 - 65536;
50
 
51
            RX.decode[DecLen] = b3;
52
            DecLen++;
53
        } else {
54
            RX.decode[DecLen] = ptrOut[a];
55
            DecLen++;
56
        }
57
        RX.DecLen = DecLen;
58
    }
59
}
60
 
61
// base64 encoder
62
string Parser::encode64(char data[150],unsigned int length)
63
{
64
    unsigned int pt = 0;
65
    unsigned char a,b,c;
66
    unsigned char ptr = 0;
67
 
68
    char tx_buff[150];
69
 
70
    while(Length > 0)
71
    {
72
        if(Length) { a = data[ptr++]; length--;} else a = 0;
73
        if(Length) { b = data[ptr++]; length--;} else b = 0;
74
        if(Length) { c = data[ptr++]; length--;} else c = 0;
75
 
76
        tx_buff[pt++] = '=' + (a >> 2);
77
        tx_buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
78
        tx_buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
79
        tx_buff[pt++] = '=' + ( c & 0x3f);
80
    }
81
    tx_buff[pt] = 0;
82
 
83
    return string(tx_buff);
84
}
85
 
86
// Datensatz nach 8bit Integer
87
int Parser::dataToChar(int *data , int start, bool is_signed) {
88
    int out = (data[start]);
89
 
90
    if ((out > 128) && (is_signed))
91
      out = out - 256;
92
 
93
    return out;
94
}
95
 
96
// received char to 8 bit integer
97
int Parser::charToData(int data) {
98
    if (data < 0)
99
        return data + 256;
100
    return data;
101
}
102
 
103
// convert data to 16bit Integer
104
int Parser::dataToInt(int *Data , int Start, bool is_signed)
105
{
106
    int Out = (Data[Start+1]<<8) | (Data[Start+0]);
107
 
108
    if ((Out > 32767) && (is_signed))
109
      Out = Out - 65536;
110
 
111
    return Out;
112
 
113
}
114
 
115
// convert data to 32bit Long
116
long Parser::dataToLong(int *Data , int Start, bool is_signed)
117
{
118
    long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]);
119
 
120
    if ((Out > 32767) && (is_signed))
121
      Out = Out;
122
 
123
    return Out;
124
}
125
 
126
float Parser::getFloat(long value, int count)
127
{
128
    long num = math.pow(10, count);
129
 
130
    float temp = value;
131
 
132
    return value / num;
133
}
134
 
135
string Parser::dataToString(int Data[150], int Start, int End)
136
{
137
    char String[150];
138
    for (int a = Start; a < End; a++)
139
    {
140
        String[a - Start] = Data[a];
141
    }
142
    String[End - Start] = '\0';
143
 
144
    return string(String);
145
}
146
 
147
// check CRC
148
bool Parser::check_CRC(char * RX, int length)
149
{
150
    int CRC = 0;
151
    char *RX;
152
 
153
    if (RX[1] == 127)
154
    {
155
        RX[1] = 0;
156
    }
157
 
158
    for(int i=0; i < length-2; i++)
159
    {
160
            CRC+=RX[i];
161
    }
162
 
163
    CRC = CRC % 4096;
164
 
165
    if(RX[length - 2] != ('=' + (CRC / 64)))
166
    {
167
        return false;
168
    }
169
 
170
    if(RX[length - 1] != ('=' + CRC % 64))
171
    {
172
        return false;
173
    }
174
 
175
    return true;
176
}
177
 
178
// add CRC
179
string Parser::add_CRC(char * TX, int length)
180
{
181
    unsigned int tmpCRC = 0;
182
 
183
    char CRC[2];
184
 
185
    for(int i = 0; i < length; i++)
186
    {
187
        tmpCRC += TX[i];
188
    }
189
 
190
    tmpCRC %= 4096;
191
 
192
    CRC[0] = '=' + tmpCRC / 64;
193
    CRC[1] = '=' + tmpCRC % 64;
194
    CRC[2] = '\0';
195
 
196
    return string(TX) + string(CRC);
197
}
198