Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Tammo Jan Dijkema
dt_ctrl
Commits
142d7994
Commit
142d7994
authored
Oct 10, 2013
by
Jeroen Vreeken
Browse files
Basic tracing working again.
Timestamps are still wrong
parent
35017641
Changes
7
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
t
raceval
*
t
rc
)
static
void
timespec_normalize
(
struct
t
imespec
*
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_fun
c
=
fun
c
;
return
0
;
dst
->
tv_se
c
+
=
add
->
tv_se
c
;
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"
,
1
0000
);
trace
=
trace_open
(
"localhost"
,
2
0000
);
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
;