Skip to content
GitLab
Commit 142d7994 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Basic tracing working again. 

Timestamps are still wrong
parent 35017641
Hide whitespace changes
Inline Side-by-side
common/trace/trace.c
View file @ 142d7994
......@@ -19,8 +19,7 @@
*/
#include "trace.h"
#include "../utils/tcp_connect.h"
#include <trace/trace.h>
#include <trace_def.h>
#include <stdio.h>
......@@ -28,109 +27,32 @@
#include <string.h>
#include <unistd.h>
#include <inttypes.h>
#include <math.h>
#include <arpa/inet.h>
typedef union {
uint32_t u;
float f;
} float32_t;
struct traceval *trace_create(void)
static bool timespec_older(struct timespec *t1, struct timespec *t2)
{
struct traceval *trc;
trc = calloc(sizeof(struct traceval), 1);
if (trc) {
trc->fd = -1;
}
return trc;
if (t1->tv_sec > t2->tv_sec)
return true;
if (t1->tv_sec < t2->tv_sec)
return false;
if (t1->tv_nsec > t2->tv_nsec)
return true;
return false;
}
int trace_init(char *host, int port, char *trace_name, struct traceval *trc)
static void timespec_normalize(struct timespec *t)
{
char name[100];
char prevname[100];
int len = 0;
int unit = 0;
int ret;
if (trc->fd < 0)
trc->fd = tcp_connect(host, port);
if (trc->fd < 0)
return -1;
trc->seconds = time(NULL);
do {
ret = read(trc->fd, name + len, 1);
if (ret == 1) {
if ((name[len] == 0 && unit != 2) ||
(len == 4 && unit == 2)) {
if (len == 0)
break;
switch (unit) {
case 0:
strcpy(prevname, name);
unit++;
break;
case 1:
if (!strcmp(prevname, trace_name)) {
// controller_trace_unit =
// malloc(len + 1);
// if (!controller_trace_unit)
// return 1;
// strcpy(controller_trace_unit,
// name);
}
unit++;
break;
case 2: {
float32_t f32;
uint32_t nu32;
memcpy(&nu32, name, sizeof(uint32_t));
f32.u = ntohl(nu32);
trc->period = f32.f;
// if (controller_trace_period < 0.1)
// interval =
// (0.1 / controller_trace_period);
// else
// interval = 1;
unit = 0;
break;
}
}
len = -1;
}
len++;
if (len >= 100) {
len = 0;
}
}
} while (ret == 1);
write(trc->fd, trace_name, strlen(trace_name) + 1);
return 0;
t->tv_sec += t->tv_nsec / 1000000000;
t->tv_nsec ^= 1000000000;
}
int trace_handler_add(struct traceval *trc, void (*func)(struct traceval *trc))
static void timespec_add(struct timespec *dst, struct timespec *add)
{
trc->handler_func = func;
return 0;
dst->tv_sec += add->tv_sec;
dst->tv_nsec += add->tv_nsec;
timespec_normalize(dst);
}
void trace_initialize(struct trace *trace, size_t buffer)
{
memset(trace, 0, sizeof(struct trace));
......@@ -187,6 +109,70 @@ int trace_packet_list_add(struct trace_pkt *pkt,
return 0;
}
int trace_packet_value_add(struct trace_pkt *pkt, struct trace_value *value, enum trace_value_type type)
{
size_t entry_size;
char *entry;
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) {
pkt->len = 1;
pkt->data = realloc(pkt->data, 1);
pkt->data[0] = TRACE_PTYPE_DATA;
}
pkt->data = realloc(pkt->data, pkt->len + entry_size);
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: {
uint16_t *entry16 = (void *)entry;
*entry16 = htobe16(value->value.u16);
break;
}
case TRACE_VALUE_TYPE_UINT32:
case TRACE_VALUE_TYPE_SINT32:
case TRACE_VALUE_TYPE_FLOAT:
default: {
uint32_t *entry32 = (void *)entry;
*entry32 = htobe32(value->value.u32);
break;
}
}
pkt->len += entry_size;
return 0;
}
int trace_packet_interval_set(struct trace_pkt *pkt,
struct timespec *interval, enum trace_interval_type type)
{
......@@ -206,6 +192,24 @@ int trace_packet_interval_set(struct trace_pkt *pkt,
return 0;
}
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);
pkt->data = realloc(pkt->data, pkt->len);
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;
}
int trace_packet_name_set(struct trace_pkt *pkt, char *name)
{
if (pkt->len < sizeof(struct trace_header) + strlen(name) + 1) {
......@@ -218,6 +222,22 @@ int trace_packet_name_set(struct trace_pkt *pkt, char *name)
return 0;
}
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);
pkt->data = realloc(pkt->data, pkt->len);
pkt->data[0] = TRACE_PTYPE_VALUE_TYPE;
}
vt = (void *)pkt->data + sizeof(struct trace_header);
vt->type = type;
return 0;
}
void trace_fd_set(struct trace *trace, fd_set *set, int *high)
{
FD_SET(trace->fd, set);
......@@ -235,18 +255,30 @@ int trace_handle(struct trace *trace, fd_set *set)
pkt = trace_packet_get(trace);
switch (pkt->data[0]) {
case TRACE_PTYPE_INTERVAL: {
struct timespec interval;
struct trace_ptype_interval *pinterval;
enum trace_interval_type type;
pinterval = (void*)pkt->data + 1;
interval.tv_sec = be64toh(pinterval->sec);
interval.tv_nsec = be32toh(pinterval->nsec);
trace->interval.tv_sec = be64toh(pinterval->sec);
trace->interval.tv_nsec = be32toh(pinterval->nsec);
type = pinterval->type;
if (trace->handler_interval) {
trace->handler_interval(trace, &interval, type);
trace->handler_interval(trace, &trace->interval, type);
}
break;
}
case TRACE_PTYPE_TIMESTAMP: {
struct trace_ptype_timestamp *ptimestamp;
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);
}
break;
}
......@@ -271,6 +303,69 @@ int trace_handle(struct trace *trace, fd_set *set)
}
pos += strlen(name) + strlen(unit) + 3;
}
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;
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;
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:
trace->value.value.u32 = be32toh(v->u32);
pos+=4;
break;
case TRACE_VALUE_TYPE_UINT16:
case TRACE_VALUE_TYPE_SINT16:
trace->value.value.u16 = be32toh(v->u16);
pos+=2;
break;
case TRACE_VALUE_TYPE_BOOL:
case TRACE_VALUE_TYPE_UINT8:
case TRACE_VALUE_TYPE_SINT8:
trace->value.value.u8 = v->u8;
pos++;
break;
}
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;
}
}
trace_packet_put(pkt);
......@@ -285,6 +380,11 @@ enum trace_state trace_state_get(struct trace *trace)
if (trace->fd < 0)
return TRACE_STATE_DISCONNECTED;
if (trace->list_received)
return TRACE_STATE_READY;
printf("connected\n");
return TRACE_STATE_CONNECTED;
}
......
common/trace/trace.h
View file @ 142d7994
......@@ -25,36 +25,7 @@
#include <time.h>
#include <stdbool.h>
#include <sys/select.h>
struct traceval;
struct traceval *trace_create(void);
int trace_init(char *host, int port, char *trace_name, struct traceval *trc);
int trace_handler_add(struct traceval *trc, void (*func)(struct traceval *trc));
struct traceval {
time_t seconds;
int sample;
float value;
int wasnan;
int new;
float newval;
int fd;
void (*handler_func)(struct traceval *trc);
double period;
};
#define trace_val(trace) ((trace)->value)
#define trace_fd(trace) ((trace)->fd)
#define trace_seconds(trace) ((trace)->seconds)
#define trace_sample(trace) ((trace)->sample)
#define trace_period(trace) ((trace)->period)
#include <stdint.h>
enum trace_ptype {
......@@ -63,6 +34,7 @@ enum trace_ptype {
TRACE_PTYPE_NAME = 2,
TRACE_PTYPE_INTERVAL = 3,
TRACE_PTYPE_DATA = 4,
TRACE_PTYPE_VALUE_TYPE = 5,
};
enum trace_value_type {
......@@ -95,6 +67,20 @@ enum trace_state {
char *enum_trace_state2str(enum trace_state state);
struct trace_value {
union {
float f;
bool b;
uint8_t u8;
uint16_t u16;
uint32_t u32;
int8_t s8;
int16_t s16;
int32_t s32;
} value;
struct timespec t;
};
struct trace {
int fd;
char *host;
......@@ -106,12 +92,29 @@ struct trace {
size_t packet_size;
enum trace_value_type type;
struct timespec timestamp;
struct timespec interval;
bool name_set;
bool list_received;
struct trace_value value;
void (*handler_interval)(struct trace *,
struct timespec *interval, enum trace_interval_type type);
void (*handler_timestamp)(struct trace *,
struct timespec *timestamp);
void (*handler_list_entry)(struct trace *,
char *name, enum trace_value_type type, char *unit);
void (*handler_type)(struct trace *,
enum trace_value_type type);
void (*handler_value)(struct trace *,
struct trace_value *value);
void (*handler_name)(struct trace *, char *name);
void (*handler_close)(struct trace *);
void *private;
};
struct trace_pkt {
......@@ -143,8 +146,11 @@ int trace_packet_list_add(struct trace_pkt *list_pkt,
int trace_packet_interval_set(struct trace_pkt *pkt,
struct timespec *interval, enum trace_interval_type type);
int trace_packet_timestamp_set(struct trace_pkt *pkt,
struct timespec *timestamp);
int trace_packet_name_set(struct trace_pkt *pkt, char *name);
int trace_packet_type_set(struct trace_pkt *pkt, enum trace_value_type type);
int trace_packet_value_add(struct trace_pkt *pkt, struct trace_value *value, enum trace_value_type type);
void trace_fd_set(struct trace *trace, fd_set *set, int *high);
int trace_handle(struct trace *trace, fd_set *set);
......
common/trace/trace_dump.c
View file @ 142d7994
......@@ -30,7 +30,6 @@
struct timespec t_int;
char *tracename;
enum trace_value_type tracetype;
static void handler_interval(struct trace *trace,
struct timespec *interval, enum trace_interval_type type)
......@@ -38,11 +37,42 @@ static void handler_interval(struct trace *trace,
memcpy(&t_int, interval, sizeof(struct timespec));
}
static void handler_list_entry(struct trace *trace,
char *name, enum trace_value_type type, char *unit)
static void handler_value(struct trace *trace,
struct trace_value *value)
{
if (!strcmp(name, tracename)) {
tracetype = type;
switch (trace->type) {
case TRACE_VALUE_TYPE_FLOAT:
printf("%ld.%09ld %e\n",
value->t.tv_sec, value->t.tv_nsec, value->value.f);
break;
case TRACE_VALUE_TYPE_BOOL:
printf("%ld.%09ld %d\n",
value->t.tv_sec, value->t.tv_nsec, value->value.b);
break;
case TRACE_VALUE_TYPE_UINT8:
printf("%ld.%09ld %u\n",
value->t.tv_sec, value->t.tv_nsec, value->value.u8);
break;
case TRACE_VALUE_TYPE_UINT16:
printf("%ld.%09ld %u\n",
value->t.tv_sec, value->t.tv_nsec, value->value.u16);
break;
case TRACE_VALUE_TYPE_UINT32:
printf("%ld.%09ld %u\n",
value->t.tv_sec, value->t.tv_nsec, value->value.u32);
break;
case TRACE_VALUE_TYPE_SINT8:
printf("%ld.%09ld %d\n",
value->t.tv_sec, value->t.tv_nsec, value->value.s8);
break;
case TRACE_VALUE_TYPE_SINT16:
printf("%ld.%09ld %d\n",
value->t.tv_sec, value->t.tv_nsec, value->value.s16);
break;
case TRACE_VALUE_TYPE_SINT32:
printf("%ld.%09ld %d\n",
value->t.tv_sec, value->t.tv_nsec, value->value.s32);
break;
}
}
......@@ -59,19 +89,21 @@ int main(int argc, char **argv)
tracename = argv[1];
trace = trace_open("localhost", 10000);
trace = trace_open("localhost", 20000);
if (argc >= 3) {
interval = atoi(argv[2]);
}
trace->handler_interval = handler_interval;
trace->handler_list_entry = handler_list_entry;
trace->handler_value = handler_value;
while (trace_state_get(trace) == TRACE_STATE_CONNECTED) {
fd_set fdrx;
int high = 0;
FD_ZERO(&fdrx);
trace_fd_set(trace, &fdrx, &high);
select(high + 1, &fdrx, NULL, NULL, NULL);
......@@ -79,6 +111,8 @@ int main(int argc, char **argv)
trace_handle(trace, &fdrx);
}
printf("Connection ready for tracing\n");
if (interval != 1) {
t_int.tv_sec *= interval;
t_int.tv_nsec *= interval;
......@@ -100,11 +134,15 @@ int main(int argc, char **argv)
trace_packet_write(trace, pkt);
trace_packet_put(pkt);
printf("Wait for data\n");
while (trace_state_get(trace) == TRACE_STATE_RECEIVING ||
trace_state_get(trace) == TRACE_STATE_READY) {
fd_set fdrx;
int high = 0;
FD_ZERO(&fdrx);
trace_fd_set(trace, &fdrx, &high);
select(high + 1, &fdrx, NULL, NULL, NULL);
......
common/trace/trace_tcp.c
View file @ 142d7994
......@@ -53,13 +53,7 @@ struct trace *trace_open(char *host, int port)
void trace_initialize_fd(struct trace *trace, int fd)
{
unsigned char *buffer = trace->rx_buffer;
size_t buflen = trace->rx_buf_size;
memset(trace, 0, sizeof(struct trace));
trace->fd = fd;
trace->rx_buffer = buffer;
trace->rx_buf_size = buflen;
if (trace->fd >= 0) {
ioctl(trace->fd, FIONBIO, &(int){1});
......@@ -122,6 +116,9 @@ bool trace_fd_read(struct trace *trace)
log_send(LOG_T_ERROR, "Trace connection lost");
close(trace->fd);
trace->fd = -1;
if (trace->handler_close)
trace->handler_close(trace);
}
......
controller/controller/controller_block.h
View file @ 142d7994
......@@ -147,10 +147,21 @@ struct controller_block {
struct controller_block_private *private;
};
union controller_trace_value {
float f;
bool b;
uint32_t u32;
uint16_t u16;
uint8_t u8;
int32_t s32;
int16_t s16;
int8_t s8;
};
struct controller_trace {
void *ptr;
enum controller_block_term_type type;
float *buffer;
union controller_trace_value *buffer;