trace.c 12.2 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/*
	Trace support

	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2011
	Copyright Stichting C.A. Muller Radioastronomiestation, 2011

	This program 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.

	This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.

*/

Jeroen Vreeken's avatar
Jeroen Vreeken committed
22
#include <trace/trace.h>
23
#include <trace_def.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
24

Jeroen Vreeken's avatar
Jeroen Vreeken committed
25
26
27
#include <tcp_connect.h>
#include <log/log.h>

Jeroen Vreeken's avatar
Jeroen Vreeken committed
28
#include <stdio.h>
29
#include <stdlib.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
30
31
32
#include <string.h>
#include <unistd.h>
#include <inttypes.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
33
#include <sys/ioctl.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
34
35


Jeroen Vreeken's avatar
Jeroen Vreeken committed
36
static bool timespec_older(struct timespec *t1, struct timespec *t2)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
37
{
Jeroen Vreeken's avatar
Jeroen Vreeken committed
38
	if (t1->tv_sec < t2->tv_sec)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
39
40
		return true;
	if (t1->tv_sec > t2->tv_sec)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
41
		return false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
42
	if (t1->tv_nsec < t2->tv_nsec)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
43
44
		return true;
	return false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
45
46
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
47
static void timespec_normalize(struct timespec *t)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
48
{
Jeroen Vreeken's avatar
Jeroen Vreeken committed
49
50
51
52
	long newsec = t->tv_nsec / 1000000000;
	
	t->tv_sec += newsec;
	t->tv_nsec -= newsec * 1000000000;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
53
54
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
55
static void timespec_add(struct timespec *dst, struct timespec *add)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
56
{
Jeroen Vreeken's avatar
Jeroen Vreeken committed
57
58
59
	dst->tv_sec += add->tv_sec;
	dst->tv_nsec += add->tv_nsec;
	timespec_normalize(dst);
60
61
62
63
64
65
66
67
68
69
}

void trace_initialize(struct trace *trace, size_t buffer)
{
	memset(trace, 0, sizeof(struct trace));
	trace->rx_buffer = malloc(buffer);
	trace->rx_buf_size = buffer;
	trace->fd = -1;
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
70
int trace_name_set(struct trace *trace, char *name)
71
{
Jeroen Vreeken's avatar
Jeroen Vreeken committed
72
73
74
75
76
77
78
79
80
81
82
83
	struct trace_pkt *pkt;

	if (trace->name)
		free(trace->name);
	trace->name = strdup(name);
	
	pkt = trace_packet_new();
	trace_packet_name_set(pkt, name);
	trace_packet_write(trace, pkt);
	trace_packet_put(pkt);
	
	return 0;
84
85
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
86
87
88
89
void trace_autorecover(struct trace *trace, bool value)
{
	trace->recover = value;
}
90

Jeroen Vreeken's avatar
Jeroen Vreeken committed
91
92

void trace_packet_initialize(struct trace_pkt *pkt)
93
{
Jeroen Vreeken's avatar
Jeroen Vreeken committed
94
	memset(pkt, 0, sizeof(struct trace_pkt));
95
96
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
97
98
99
100
101
102
103
104
105


#define PACKET_BUFLEN 256
#define PACKET_POOL_CHUNK 16

static struct trace_pkt **packet_pool;
static int packet_pool_size = 0;
static int packet_pool_cur = 0;

106
107
108
109
struct trace_pkt *trace_packet_new(void)
{
	struct trace_pkt *pkt;
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
110
111
112
113
114
115
	if (packet_pool_cur) {
		pkt = packet_pool[packet_pool_cur-1];
		packet_pool_cur--;
		return pkt;
	}
	
116
	pkt = calloc(1, sizeof(struct trace_pkt));
Jeroen Vreeken's avatar
Jeroen Vreeken committed
117
118
119
	pkt->data = malloc(PACKET_BUFLEN);
	pkt->buflen = PACKET_BUFLEN;
	
120
121
122
	return pkt;
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
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
int trace_packet_resize(struct trace_pkt *pkt, size_t len)
{
	if (len <= pkt->buflen)
		return 0;
	
	pkt->data = realloc(pkt->data, len);
	pkt->buflen = len;

	return 0;
}

void trace_packet_put(struct trace_pkt *pkt)
{
	if (pkt->buflen > PACKET_BUFLEN) {
		free(pkt->data);
		free(pkt);
		return;
	}
	
	if (packet_pool_cur >= packet_pool_size) {
		packet_pool_size += PACKET_POOL_CHUNK;
		packet_pool = realloc(packet_pool,
		    sizeof(struct trace_pkt *) * packet_pool_size);
	}
	packet_pool[packet_pool_cur] = pkt;
	pkt->len = 0;
	packet_pool_cur++;
}

152
153
154
155
int trace_packet_list_add(struct trace_pkt *pkt, 
    char *name, enum trace_value_type type, char *unit)
{
	size_t entry_size = strlen(name)+1 + 1 + strlen(unit)+1;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
156
	unsigned char *entry;
157
158

	if (!pkt->len) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
159
		trace_packet_resize(pkt, 1);
160
161
		pkt->len = 1;
		pkt->data[0] = TRACE_PTYPE_LIST;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
162
	}
163
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
164
	trace_packet_resize(pkt, pkt->len + entry_size);
165
166
	entry = pkt->data + pkt->len;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
167
	strcpy((char *)entry, name);
168
169
170
171
172
	entry += strlen(name) + 1;

	*entry = type;
	entry++;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
173
	strcpy((char *)entry, unit);
174
175
176
177
178
179
	
	pkt->len += entry_size;
		
	return 0;
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
180
181
182
int trace_packet_value_add(struct trace_pkt *pkt, struct trace_value *value, enum trace_value_type type)
{
	size_t entry_size;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
183
184
185
186
187
188
189
	struct {
		uint16_t u16;
	} __packed *entry16;
	struct {
		uint32_t u32;
	} __packed *entry32;
	unsigned char *entry;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209

	switch (type) {
		case TRACE_VALUE_TYPE_BOOL:
		case TRACE_VALUE_TYPE_UINT8:
		case TRACE_VALUE_TYPE_SINT8:
			entry_size = 1;
			break;
		case TRACE_VALUE_TYPE_SINT16:
		case TRACE_VALUE_TYPE_UINT16:
			entry_size = 2;
			break;
		case TRACE_VALUE_TYPE_UINT32:
		case TRACE_VALUE_TYPE_SINT32:
		case TRACE_VALUE_TYPE_FLOAT:
		default:
			entry_size = 4;
			break;
	}

	if (!pkt->len) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
210
		trace_packet_resize(pkt, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
211
212
213
214
		pkt->len = 1;
		pkt->data[0] = TRACE_PTYPE_DATA;
	}
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
215
	trace_packet_resize(pkt, pkt->len + entry_size);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
216
217
218
219
220
221
222
223
224
225
	entry = pkt->data + pkt->len;

	switch (type) {
		case TRACE_VALUE_TYPE_BOOL:
		case TRACE_VALUE_TYPE_UINT8:
		case TRACE_VALUE_TYPE_SINT8:
			*entry = value->value.u8;
			break;
		case TRACE_VALUE_TYPE_SINT16:
		case TRACE_VALUE_TYPE_UINT16: {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
226
			entry16 = (void *)entry;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
227
			
Jeroen Vreeken's avatar
Jeroen Vreeken committed
228
			entry16->u16 = htobe16(value->value.u16);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
229
230
231
232
233
234
235
			
			break;
		}
		case TRACE_VALUE_TYPE_UINT32:
		case TRACE_VALUE_TYPE_SINT32:
		case TRACE_VALUE_TYPE_FLOAT:
		default: {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
236
			entry32 = (void *)entry;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
237
			
Jeroen Vreeken's avatar
Jeroen Vreeken committed
238
			entry32->u32 = htobe32(value->value.u32);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
239
240
241
242
243
244
245
246
247
248
249
			
			break;
		}
	}
	
	
	pkt->len += entry_size;
		
	return 0;
}

250
251
252
253
254
255
256
int trace_packet_interval_set(struct trace_pkt *pkt, 
    struct timespec *interval, enum trace_interval_type type)
{
	struct trace_ptype_interval *pinterval;
	
	if (pkt->len < sizeof(struct trace_ptype_interval) + sizeof(struct trace_header)) {
		pkt->len = sizeof(struct trace_ptype_interval) + sizeof(struct trace_header);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
257
		trace_packet_resize(pkt, pkt->len);
258
		pkt->data[0] = TRACE_PTYPE_INTERVAL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
259
	}
260
261
262
263
264
265
	pinterval = (struct trace_ptype_interval *)&pkt->data[1];
	
	pinterval->sec = htobe64(interval->tv_sec);
	pinterval->nsec = htobe32(interval->tv_nsec);
	pinterval->type = type;
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
266
267
268
	return 0;
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
269
270
271
272
273
274
275
int trace_packet_timestamp_set(struct trace_pkt *pkt, 
    struct timespec *timestamp)
{
	struct trace_ptype_timestamp *ptimestamp;
	
	if (pkt->len < sizeof(struct trace_ptype_timestamp) + sizeof(struct trace_header)) {
		pkt->len = sizeof(struct trace_ptype_timestamp) + sizeof(struct trace_header);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
276
		trace_packet_resize(pkt, pkt->len);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
277
278
279
280
281
282
283
284
285
286
		pkt->data[0] = TRACE_PTYPE_TIMESTAMP;
	}
	ptimestamp = (struct trace_ptype_timestamp *)&pkt->data[1];
	
	ptimestamp->sec = htobe64(timestamp->tv_sec);
	ptimestamp->nsec = htobe32(timestamp->tv_nsec);
	
	return 0;
}

287
int trace_packet_name_set(struct trace_pkt *pkt, char *name)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
288
{
289
290
	if (pkt->len < sizeof(struct trace_header) + strlen(name) + 1) {
		pkt->len = strlen(name) + 1 + sizeof(struct trace_header);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
291
		trace_packet_resize(pkt, pkt->len);
292
293
		pkt->data[0] = TRACE_PTYPE_NAME;
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
294
	strcpy((char *)pkt->data + 1, name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
295
296
297
298
	
	return 0;
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
299
300
301
302
303
304
int trace_packet_type_set(struct trace_pkt *pkt, enum trace_value_type type)
{
	struct trace_ptype_value_type *vt;

	if (pkt->len < sizeof(struct trace_header) + sizeof(struct trace_ptype_value_type)) {
		pkt->len = sizeof(struct trace_header) + sizeof(struct trace_ptype_value_type);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
305
		trace_packet_resize(pkt, pkt->len);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
306
307
308
309
310
311
312
313
314
		pkt->data[0] = TRACE_PTYPE_VALUE_TYPE;
	}
	vt = (void *)pkt->data + sizeof(struct trace_header);
	
	vt->type = type;
		
	return 0;
}

315
316
void trace_fd_set(struct trace *trace, fd_set *set, int *high)
{
Jeroen Vreeken's avatar
Jeroen Vreeken committed
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
	if (trace->fd < 0) {
		if (!trace->recover)
			return;

		if (trace->recovertime >= time(NULL))
			return;
		trace->recovertime = time(NULL);

		log_send(LOG_T_DEBUG, "Attempt to recover trace");
		trace->fd = tcp_connect(trace->host, trace->port);
		if (trace->fd >= 0) {
			struct trace_pkt *pkt;
		
			ioctl(trace->fd, FIONBIO, &(int){1});

			if (trace->name) {
				pkt = trace_packet_new();
				trace_packet_name_set(pkt, trace->name);
				trace_packet_write(trace, pkt);
				trace_packet_put(pkt);
			}
		}
		if (trace->fd < 0) {
			log_send(LOG_T_DEBUG, "Failed to recover");
			return;
		}
	}

345
346
347
348
349
350
351
352
353
354
355
356
357
	FD_SET(trace->fd, set);

	if (trace->fd > *high)
		*high = trace->fd;
}

int trace_handle(struct trace *trace, fd_set *set)
{
	if (FD_ISSET(trace->fd, set)) {
		while (trace_fd_read(trace)) {
			struct trace_pkt *pkt;
			
			pkt = trace_packet_get(trace);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
358
359
			if (!pkt)
				break;
360
361
362
363
364
365
366
			switch (pkt->data[0]) {
				case TRACE_PTYPE_INTERVAL: {
					struct trace_ptype_interval *pinterval;
					enum trace_interval_type type;
					
					pinterval = (void*)pkt->data + 1;
					
Jeroen Vreeken's avatar
Jeroen Vreeken committed
367
368
					trace->interval.tv_sec = be64toh(pinterval->sec);
					trace->interval.tv_nsec = be32toh(pinterval->nsec);
369
370
371
					type = pinterval->type;
					
					if (trace->handler_interval) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
372
373
374
375
376
377
						trace->handler_interval(trace, &trace->interval, type);
					}
					break;
				}
				case TRACE_PTYPE_TIMESTAMP: {
					struct trace_ptype_timestamp *ptimestamp;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
378
										
Jeroen Vreeken's avatar
Jeroen Vreeken committed
379
380
381
382
383
384
385
					ptimestamp = (void*)pkt->data + 1;
					
					trace->timestamp.tv_sec = be64toh(ptimestamp->sec);
					trace->timestamp.tv_nsec = be32toh(ptimestamp->nsec);
					
					if (trace->handler_timestamp) {
						trace->handler_timestamp(trace, &trace->timestamp);
386
387
388
389
					}
					break;
				}
				case TRACE_PTYPE_LIST: {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
390
					unsigned char *listentry;
391
392
393
394
395
396
397
398
399
					size_t pos = 1;
					
					while (pos < pkt->len) {
						char *name;
						enum trace_value_type type;
						char *unit;
						
						listentry = pkt->data + pos;
						
Jeroen Vreeken's avatar
Jeroen Vreeken committed
400
						name = (char *)listentry;
401
						type = listentry[strlen(name) + 1];
Jeroen Vreeken's avatar
Jeroen Vreeken committed
402
						unit = (char *)listentry+strlen(name) + 2;
403
404
405
406
407
408
409
						
						if (trace->handler_list_entry) {
							trace->handler_list_entry(trace,
							    name, type, unit);
						}
						pos += strlen(name) + strlen(unit) + 3;
					}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
410
411
412
413
414
415
416
417
418
419
420
					trace->list_received = true;
					break;
				}
				case TRACE_PTYPE_VALUE_TYPE: {
					struct trace_ptype_value_type *vt;
					enum trace_value_type type;
					
					vt = (void*)pkt->data + 1;
					type = vt->type;
					
					trace->type = type;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
421
					trace->type_set = type;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
422
423
424
425
426
427
428
429
430
431
432
					if (trace->handler_type) {
						trace->handler_type(trace, type);
					}
					break;
				}
				case TRACE_PTYPE_NAME: {
					char *name;
					
					name = (void*)pkt->data + 1;
					
					trace->name_set = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
433
					trace->name = strdup(name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449

					if (trace->handler_name) {
						trace->handler_name(trace, name);
					}
					break;
				}
				case TRACE_PTYPE_DATA: {
					size_t pos = 1;
					struct trace_ptype_value *v;
					
					while (pos < pkt->len) {
						v = (void *)pkt->data + pos;
						switch (trace->type) {
							case TRACE_VALUE_TYPE_FLOAT:
							case TRACE_VALUE_TYPE_UINT32:
							case TRACE_VALUE_TYPE_SINT32:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
450
								trace->value.value.u32 = be32toh(v->u.u32);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
451
452
453
454
								pos+=4;
								break;
							case TRACE_VALUE_TYPE_UINT16:
							case TRACE_VALUE_TYPE_SINT16:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
455
								trace->value.value.u16 = be32toh(v->u.u16);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
456
457
458
459
460
								pos+=2;
								break;
							case TRACE_VALUE_TYPE_BOOL:
							case TRACE_VALUE_TYPE_UINT8:
							case TRACE_VALUE_TYPE_SINT8:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
461
								trace->value.value.u8 = v->u.u8;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
462
463
464
								pos++;
								break;
						}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
465

Jeroen Vreeken's avatar
Jeroen Vreeken committed
466
467
468
469
470
471
472
473
474
475
						if (timespec_older(&trace->value.t, &trace->timestamp)) {
							trace->value.t = trace->timestamp;
						} else {
							timespec_add(&trace->value.t, &trace->interval);
						}
						if (trace->handler_value) {
							trace->handler_value(trace, &trace->value);
						}
					}
					break;
476
477
478
479
				}
			}
			trace_packet_put(pkt);
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
480
		return 0;
481
482
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
483
	return 1;
484
485
486
487
488
489
490
}

enum trace_state trace_state_get(struct trace *trace)
{
	if (trace->fd < 0)
		return TRACE_STATE_DISCONNECTED;
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
491
492
493
	if (trace->list_received)
		return TRACE_STATE_READY;
	
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
	return TRACE_STATE_CONNECTED;
}


char *enum_trace_value_type2str(enum trace_value_type type)
{
	switch(type) {
		case TRACE_VALUE_TYPE_FLOAT:
			return "FLOAT";
		case TRACE_VALUE_TYPE_BOOL:
			return "BOOL";
		case TRACE_VALUE_TYPE_UINT8:
			return "UINT8";
		case TRACE_VALUE_TYPE_UINT16:
			return "UINT16";
		case TRACE_VALUE_TYPE_UINT32:
			return "UINT32";
		case TRACE_VALUE_TYPE_SINT8:
			return "SINT8";
		case TRACE_VALUE_TYPE_SINT16:
			return "SINT16";
		case TRACE_VALUE_TYPE_SINT32:
			return "SINT32";
		default:
			return "unknown";
	}
}


char *enum_trace_interval_type2str(enum trace_interval_type type)
{
	switch(type) {
		case TRACE_INTERVAL_TYPE_INTERVAL:
			return "INTERVAL";
		case TRACE_INTERVAL_TYPE_CHANGED:
			return "CHANGED";
		case TRACE_INTERVAL_TYPE_CHANGED_INTERVAL:
			return "CHANGED_INTERVAL";
		default:
			return "unknown";
	}
}


char *enum_trace_state2str(enum trace_state state)
{
	switch(state) {
		case TRACE_STATE_DISCONNECTED:
			return "DISCONECTED";
		case TRACE_STATE_CONNECTED:
			return "CONNECTED";
		case TRACE_STATE_READY:
			return "READY";
		case TRACE_STATE_RECEIVING:
			return "RECEIVING";
		default:
			return "unknown";
	}
}