1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
|
/**
* This file is part of BCM4751 gpsd
*
* Copyright (C) 2011 Paul Kocialkowski <contact@oaulk.fr>
*
* BCM4751 gpsd is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* BCM4751 gpsd is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with BCM4751 gpsd. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <unistd.h>
#include <termios.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include "meif.h"
void meif_read_loop(int fd)
{
void *data = NULL;
int length = 0x100;
uint16_t *meif_data_waiting = NULL;
int meif_data_waiting_length = 0;
struct meif_message *meif_message = NULL;
struct meif_header *meif_header = NULL;
uint16_t *meif_data = NULL;
int meif_data_length = 0;
uint16_t marker = 0;
int run = 1;
int rc = -1;
int i;
data = malloc(length);
while(run) {
meif_header = NULL;
marker = 0;
if(meif_data == NULL) {
rc = bcm4751_serial_read(fd, data, length, NULL);
if(rc <= 0) {
free(data);
return;
}
} else if(meif_data_length > 0) {
if(meif_data_length > length)
meif_data_length = length;
memcpy(data, (void *) meif_data, meif_data_length);
rc = meif_data_length;
free(meif_data);
meif_data = NULL;
meif_data_length = 0;
}
if(meif_data_waiting == NULL) {
// Read the start marker
for(i=0 ; i < rc ; i++) {
marker = *((uint16_t *) (data + i));
if(marker == MEIF_START) {
// Raw MEIF data and header
meif_data_length = rc - i;
meif_data = (uint16_t *) malloc(meif_data_length);
memcpy((void *) meif_data, (void *) (data + i), meif_data_length);
meif_header = (struct meif_header *) meif_data;
break;
}
}
} else if(meif_data_waiting_length > 0) {
// There is an uncomplete message waiting
// Raw data and header from the waiting and the new data
meif_data_length = meif_data_waiting_length + rc;
meif_data = malloc(meif_data_length);
memcpy((void *) meif_data, (void *) meif_data_waiting, meif_data_waiting_length);
memcpy((void *) meif_data + meif_data_waiting_length, (void *) data, rc);
meif_header = (struct meif_header *) meif_data;
free(meif_data_waiting);
meif_data_waiting = NULL;
meif_data_waiting_length = 0;
}
if(meif_data == NULL || meif_header == NULL) {
printf("No usable data found, skipping!\n");
continue;
}
// The message wasn't fully retrieved
if(meif_data_length < sizeof(struct meif_header) || (meif_data_length - i - sizeof(struct meif_header)) < meif_header->length) {
meif_data_waiting = meif_data;
meif_data_waiting_length = meif_data_length;
meif_data = NULL;
meif_data_length = 0;
continue;
} else {
// Read the end marker
for(i=meif_header->length ; i < meif_data_length ; i++) {
// We go with a 6 left offset from the announced end of the message
marker = *((uint16_t *) ((void *) meif_data + i));
if(marker == MEIF_END) {
// TODO: convert to meif_message
printf("MEIF message (%d bytes):\n", meif_data_length);
hex_dump(meif_data, i + sizeof(marker));
break;
}
}
// No end marker was found
if(i == meif_data_length) {
// meif_data and meif_data_length are still set, so they will be used
continue;
}
// There is still data after the end of the message
if(meif_data_length - i - 1 >= sizeof(marker)) {
meif_data_waiting_length = meif_data_length - i - 2;
meif_data_waiting = (uint16_t *) malloc(meif_data_waiting_length);
memcpy((void *) meif_data_waiting, (void *) meif_data + i + 2, meif_data_waiting_length);
free(meif_data);
meif_data = meif_data_waiting;
meif_data_length = meif_data_waiting_length;
} else {
free(meif_data);
meif_data = NULL;
meif_data_length = 0;
}
meif_data_waiting = NULL;
meif_data_waiting_length = 0;
}
// TODO: if there is a meif message, dispatch and free and set to NULL
}
}
|
