Skip to content

T
tsn-measures
Commit 645e9fb8 authored by Joanne Hugé's avatar Joanne Hugé
Browse files
Options

Add motor-control

parent ce9576b4
Hide whitespace changes
Inline Side-by-side
Showing with 2714 additions and 0 deletions
src/motor-control/build/Makefile 0 → 100644
View file @ 645e9fb8
SERVER_PROG = server
CLIENT_PROG = client
SRCDIR = ../src
SERVER_SRCS = server.c
SERVER_SRCS += recv_packet.c
SERVER_SRCS += send_packet.c
SERVER_SRCS += common.c
SERVER_SRCS += gpio.c
CLIENT_SRCS = client.c
CLIENT_SRCS += recv_packet.c
CLIENT_SRCS += send_packet.c
CLIENT_SRCS += common.c
SERVER_OBJS = $(SERVER_SRCS:%.c=%.o)
CLIENT_OBJS = $(CLIENT_SRCS:%.c=%.o)
ifeq ($(DEBUG),)
CFLAGS = -O2
else
CFLAGS = -Og -g -Wall -Wextra
endif
CFLAGS += -MD -MP
CFLAGS += -I $(SRCDIR)
CFLAGS += -std=gnu99
LLIBS = -pthread
ifneq ($(WITH_XDP),)
CFLAGS += -D WITH_XDP
LDFLAGS += -L/usr/lib -lbpf
else ifneq ($(WITH_GIT_XDP),)
IFLAGS += -I ${HOME}/libbpf/include
CFLAGS += -D WITH_XDP $(IFLAGS)
LDIRS += -L${HOME}/libbpf/src
LLIBS += -lelf -lz -l:libbpf.a
endif
vpath %.c $(SRCDIR)
all: links
links: bin/$(SERVER_PROG) bin/$(CLIENT_PROG)
bin/$(SERVER_PROG): $(SERVER_PROG)
mkdir -p bin
ln -fs $(realpath $(SERVER_PROG)) $@
bin/$(CLIENT_PROG): $(CLIENT_PROG)
mkdir -p bin
ln -fs $(realpath $(CLIENT_PROG)) $@
xdp_kern.o: xdp_kern.c
clang $(IFLAGS) -isystem /usr/include/arm-linux-gnueabihf -S -target bpf -D __BPF_TRACING__ -Wall -O2 -emit-llvm -c -g -o xdp_kern.ll $^
llc -march=bpf -filetype=obj -o $@ xdp_kern.ll
ifneq ($(WITH_GIT_XDP),)
$(SERVER_PROG): $(SERVER_OBJS) xdp_kern.o
$(CC) $(LDFLAGS) $(LDIRS) $(SERVER_OBJS) $(LLIBS) -o $@
else
$(SERVER_PROG): $(SERVER_OBJS)
$(CC) $(LDFLAGS) $(LDIRS) $^ $(LLIBS) -o $@
endif
$(CLIENT_PROG): $(CLIENT_OBJS)
$(CC) $(LDFLAGS) $(LDIRS) $^ $(LLIBS) -o $@
-include $(subst .c,.d,$(SERVER_SRCS))
-include $(subst .c,.d,$(CLIENT_SRCS))
clean:
$(RM) -rf bin
$(RM) $(SERVER_OBJS) $(SERVER_PROG) $(subst .c,.d,$(SERVER_SRCS))
$(RM) $(CLIENT_OBJS) $(CLIENT_PROG) $(subst .c,.d,$(CLIENT_SRCS))
.PHONY: clean all links
src/motor-control/src/bpf_endian.h 0 → 100644
View file @ 645e9fb8
/* SPDX-License-Identifier: GPL-2.0 */
/* Copied from $(LINUX)/tools/testing/selftests/bpf/bpf_endian.h */
#ifndef __BPF_ENDIAN__
#define __BPF_ENDIAN__
#include <linux/swab.h>
/* LLVM's BPF target selects the endianness of the CPU
* it compiles on, or the user specifies (bpfel/bpfeb),
* respectively. The used __BYTE_ORDER__ is defined by
* the compiler, we cannot rely on __BYTE_ORDER from
* libc headers, since it doesn't reflect the actual
* requested byte order.
*
* Note, LLVM's BPF target has different __builtin_bswapX()
* semantics. It does map to BPF_ALU | BPF_END | BPF_TO_BE
* in bpfel and bpfeb case, which means below, that we map
* to cpu_to_be16(). We could use it unconditionally in BPF
* case, but better not rely on it, so that this header here
* can be used from application and BPF program side, which
* use different targets.
*/
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
# define __bpf_ntohs(x)__builtin_bswap16(x)
# define __bpf_htons(x)__builtin_bswap16(x)
# define __bpf_constant_ntohs(x)___constant_swab16(x)
# define __bpf_constant_htons(x)___constant_swab16(x)
# define __bpf_ntohl(x)__builtin_bswap32(x)
# define __bpf_htonl(x)__builtin_bswap32(x)
# define __bpf_constant_ntohl(x)___constant_swab32(x)
# define __bpf_constant_htonl(x)___constant_swab32(x)
#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
# define __bpf_ntohs(x)(x)
# define __bpf_htons(x)(x)
# define __bpf_constant_ntohs(x)(x)
# define __bpf_constant_htons(x)(x)
# define __bpf_ntohl(x)(x)
# define __bpf_htonl(x)(x)
# define __bpf_constant_ntohl(x)(x)
# define __bpf_constant_htonl(x)(x)
#else
# error "Fix your compiler's __BYTE_ORDER__?!"
#endif
#define bpf_htons(x)\
(__builtin_constant_p(x) ?\
__bpf_constant_htons(x) : __bpf_htons(x))
#define bpf_ntohs(x)\
(__builtin_constant_p(x) ?\
__bpf_constant_ntohs(x) : __bpf_ntohs(x))
#define bpf_htonl(x)\
(__builtin_constant_p(x) ?\
__bpf_constant_htonl(x) : __bpf_htonl(x))
#define bpf_ntohl(x)\
(__builtin_constant_p(x) ?\
__bpf_constant_ntohl(x) : __bpf_ntohl(x))
#endif /* __BPF_ENDIAN__ */
src/motor-control/src/bpf_helpers.h 0 → 100644
View file @ 645e9fb8
/* SPDX-License-Identifier: GPL-2.0 */
/* Copied from $(LINUX)/tools/testing/selftests/bpf/bpf_helpers.h */
/* Added to fix compilation on old Ubuntu systems - please preserve when
updating file! */
#ifndef __always_inline
# define __always_inline inline __attribute__((always_inline))
#endif
#ifndef __BPF_HELPERS_H
#define __BPF_HELPERS_H
/* helper macro to place programs, maps, license in
* different sections in elf_bpf file. Section names
* are interpreted by elf_bpf loader
*/
#define SEC(NAME) __attribute__((section(NAME), used))
/* helper functions called from eBPF programs written in C */
static void *(*bpf_map_lookup_elem)(void *map, void *key) =
(void *) BPF_FUNC_map_lookup_elem;
static int (*bpf_map_update_elem)(void *map, void *key, void *value,
unsigned long long flags) =
(void *) BPF_FUNC_map_update_elem;
static int (*bpf_map_delete_elem)(void *map, void *key) =
(void *) BPF_FUNC_map_delete_elem;
static int (*bpf_map_push_elem)(void *map, void *value,
unsigned long long flags) =
(void *) BPF_FUNC_map_push_elem;
static int (*bpf_map_pop_elem)(void *map, void *value) =
(void *) BPF_FUNC_map_pop_elem;
static int (*bpf_map_peek_elem)(void *map, void *value) =
(void *) BPF_FUNC_map_peek_elem;
static int (*bpf_probe_read)(void *dst, int size, void *unsafe_ptr) =
(void *) BPF_FUNC_probe_read;
static unsigned long long (*bpf_ktime_get_ns)(void) =
(void *) BPF_FUNC_ktime_get_ns;
static int (*bpf_trace_printk)(const char *fmt, int fmt_size, ...) =
(void *) BPF_FUNC_trace_printk;
static void (*bpf_tail_call)(void *ctx, void *map, int index) =
(void *) BPF_FUNC_tail_call;
static unsigned long long (*bpf_get_smp_processor_id)(void) =
(void *) BPF_FUNC_get_smp_processor_id;
static unsigned long long (*bpf_get_current_pid_tgid)(void) =
(void *) BPF_FUNC_get_current_pid_tgid;
static unsigned long long (*bpf_get_current_uid_gid)(void) =
(void *) BPF_FUNC_get_current_uid_gid;
static int (*bpf_get_current_comm)(void *buf, int buf_size) =
(void *) BPF_FUNC_get_current_comm;
static unsigned long long (*bpf_perf_event_read)(void *map,
unsigned long long flags) =
(void *) BPF_FUNC_perf_event_read;
static int (*bpf_clone_redirect)(void *ctx, int ifindex, int flags) =
(void *) BPF_FUNC_clone_redirect;
static int (*bpf_redirect)(int ifindex, int flags) =
(void *) BPF_FUNC_redirect;
static int (*bpf_redirect_map)(void *map, int key, int flags) =
(void *) BPF_FUNC_redirect_map;
static int (*bpf_perf_event_output)(void *ctx, void *map,
unsigned long long flags, void *data,
int size) =
(void *) BPF_FUNC_perf_event_output;
static int (*bpf_get_stackid)(void *ctx, void *map, int flags) =
(void *) BPF_FUNC_get_stackid;
static int (*bpf_probe_write_user)(void *dst, void *src, int size) =
(void *) BPF_FUNC_probe_write_user;
static int (*bpf_current_task_under_cgroup)(void *map, int index) =
(void *) BPF_FUNC_current_task_under_cgroup;
static int (*bpf_skb_get_tunnel_key)(void *ctx, void *key, int size, int flags) =
(void *) BPF_FUNC_skb_get_tunnel_key;
static int (*bpf_skb_set_tunnel_key)(void *ctx, void *key, int size, int flags) =
(void *) BPF_FUNC_skb_set_tunnel_key;
static int (*bpf_skb_get_tunnel_opt)(void *ctx, void *md, int size) =
(void *) BPF_FUNC_skb_get_tunnel_opt;
static int (*bpf_skb_set_tunnel_opt)(void *ctx, void *md, int size) =
(void *) BPF_FUNC_skb_set_tunnel_opt;
static unsigned long long (*bpf_get_prandom_u32)(void) =
(void *) BPF_FUNC_get_prandom_u32;
static int (*bpf_xdp_adjust_head)(void *ctx, int offset) =
(void *) BPF_FUNC_xdp_adjust_head;
static int (*bpf_xdp_adjust_meta)(void *ctx, int offset) =
(void *) BPF_FUNC_xdp_adjust_meta;
static int (*bpf_get_socket_cookie)(void *ctx) =
(void *) BPF_FUNC_get_socket_cookie;
static int (*bpf_setsockopt)(void *ctx, int level, int optname, void *optval,
int optlen) =
(void *) BPF_FUNC_setsockopt;
static int (*bpf_getsockopt)(void *ctx, int level, int optname, void *optval,
int optlen) =
(void *) BPF_FUNC_getsockopt;
static int (*bpf_sock_ops_cb_flags_set)(void *ctx, int flags) =
(void *) BPF_FUNC_sock_ops_cb_flags_set;
static int (*bpf_sk_redirect_map)(void *ctx, void *map, int key, int flags) =
(void *) BPF_FUNC_sk_redirect_map;
static int (*bpf_sk_redirect_hash)(void *ctx, void *map, void *key, int flags) =
(void *) BPF_FUNC_sk_redirect_hash;
static int (*bpf_sock_map_update)(void *map, void *key, void *value,
unsigned long long flags) =
(void *) BPF_FUNC_sock_map_update;
static int (*bpf_sock_hash_update)(void *map, void *key, void *value,
unsigned long long flags) =
(void *) BPF_FUNC_sock_hash_update;
static int (*bpf_perf_event_read_value)(void *map, unsigned long long flags,
void *buf, unsigned int buf_size) =
(void *) BPF_FUNC_perf_event_read_value;
static int (*bpf_perf_prog_read_value)(void *ctx, void *buf,
unsigned int buf_size) =
(void *) BPF_FUNC_perf_prog_read_value;
static int (*bpf_override_return)(void *ctx, unsigned long rc) =
(void *) BPF_FUNC_override_return;
static int (*bpf_msg_redirect_map)(void *ctx, void *map, int key, int flags) =
(void *) BPF_FUNC_msg_redirect_map;
static int (*bpf_msg_redirect_hash)(void *ctx,
void *map, void *key, int flags) =
(void *) BPF_FUNC_msg_redirect_hash;
static int (*bpf_msg_apply_bytes)(void *ctx, int len) =
(void *) BPF_FUNC_msg_apply_bytes;
static int (*bpf_msg_cork_bytes)(void *ctx, int len) =
(void *) BPF_FUNC_msg_cork_bytes;
static int (*bpf_msg_pull_data)(void *ctx, int start, int end, int flags) =
(void *) BPF_FUNC_msg_pull_data;
static int (*bpf_msg_push_data)(void *ctx, int start, int end, int flags) =
(void *) BPF_FUNC_msg_push_data;
static int (*bpf_msg_pop_data)(void *ctx, int start, int cut, int flags) =
(void *) BPF_FUNC_msg_pop_data;
static int (*bpf_bind)(void *ctx, void *addr, int addr_len) =
(void *) BPF_FUNC_bind;
static int (*bpf_xdp_adjust_tail)(void *ctx, int offset) =
(void *) BPF_FUNC_xdp_adjust_tail;
static int (*bpf_skb_get_xfrm_state)(void *ctx, int index, void *state,
int size, int flags) =
(void *) BPF_FUNC_skb_get_xfrm_state;
static int (*bpf_sk_select_reuseport)(void *ctx, void *map, void *key, __u32 flags) =
(void *) BPF_FUNC_sk_select_reuseport;
static int (*bpf_get_stack)(void *ctx, void *buf, int size, int flags) =
(void *) BPF_FUNC_get_stack;
static int (*bpf_fib_lookup)(void *ctx, struct bpf_fib_lookup *params,
int plen, __u32 flags) =
(void *) BPF_FUNC_fib_lookup;
static int (*bpf_lwt_push_encap)(void *ctx, unsigned int type, void *hdr,
unsigned int len) =
(void *) BPF_FUNC_lwt_push_encap;
static int (*bpf_lwt_seg6_store_bytes)(void *ctx, unsigned int offset,
void *from, unsigned int len) =
(void *) BPF_FUNC_lwt_seg6_store_bytes;
static int (*bpf_lwt_seg6_action)(void *ctx, unsigned int action, void *param,
unsigned int param_len) =
(void *) BPF_FUNC_lwt_seg6_action;
static int (*bpf_lwt_seg6_adjust_srh)(void *ctx, unsigned int offset,
unsigned int len) =
(void *) BPF_FUNC_lwt_seg6_adjust_srh;
static int (*bpf_rc_repeat)(void *ctx) =
(void *) BPF_FUNC_rc_repeat;
static int (*bpf_rc_keydown)(void *ctx, unsigned int protocol,
unsigned long long scancode, unsigned int toggle) =
(void *) BPF_FUNC_rc_keydown;
static unsigned long long (*bpf_get_current_cgroup_id)(void) =
(void *) BPF_FUNC_get_current_cgroup_id;
static void *(*bpf_get_local_storage)(void *map, unsigned long long flags) =
(void *) BPF_FUNC_get_local_storage;
static unsigned long long (*bpf_skb_cgroup_id)(void *ctx) =
(void *) BPF_FUNC_skb_cgroup_id;
static unsigned long long (*bpf_skb_ancestor_cgroup_id)(void *ctx, int level) =
(void *) BPF_FUNC_skb_ancestor_cgroup_id;
static struct bpf_sock *(*bpf_sk_lookup_tcp)(void *ctx,
struct bpf_sock_tuple *tuple,
int size, unsigned long long netns_id,
unsigned long long flags) =
(void *) BPF_FUNC_sk_lookup_tcp;
static struct bpf_sock *(*bpf_sk_lookup_udp)(void *ctx,
struct bpf_sock_tuple *tuple,
int size, unsigned long long netns_id,
unsigned long long flags) =
(void *) BPF_FUNC_sk_lookup_udp;
static int (*bpf_sk_release)(struct bpf_sock *sk) =
(void *) BPF_FUNC_sk_release;
static int (*bpf_skb_vlan_push)(void *ctx, __be16 vlan_proto, __u16 vlan_tci) =
(void *) BPF_FUNC_skb_vlan_push;
static int (*bpf_skb_vlan_pop)(void *ctx) =
(void *) BPF_FUNC_skb_vlan_pop;
static int (*bpf_rc_pointer_rel)(void *ctx, int rel_x, int rel_y) =
(void *) BPF_FUNC_rc_pointer_rel;
static void (*bpf_spin_lock)(struct bpf_spin_lock *lock) =
(void *) BPF_FUNC_spin_lock;
static void (*bpf_spin_unlock)(struct bpf_spin_lock *lock) =
(void *) BPF_FUNC_spin_unlock;
static struct bpf_sock *(*bpf_sk_fullsock)(struct bpf_sock *sk) =
(void *) BPF_FUNC_sk_fullsock;
static struct bpf_tcp_sock *(*bpf_tcp_sock)(struct bpf_sock *sk) =
(void *) BPF_FUNC_tcp_sock;
static struct bpf_sock *(*bpf_get_listener_sock)(struct bpf_sock *sk) =
(void *) BPF_FUNC_get_listener_sock;
static int (*bpf_skb_ecn_set_ce)(void *ctx) =
(void *) BPF_FUNC_skb_ecn_set_ce;
/* llvm builtin functions that eBPF C program may use to
* emit BPF_LD_ABS and BPF_LD_IND instructions
*/
struct sk_buff;
unsigned long long load_byte(void *skb,
unsigned long long off) asm("llvm.bpf.load.byte");
unsigned long long load_half(void *skb,
unsigned long long off) asm("llvm.bpf.load.half");
unsigned long long load_word(void *skb,
unsigned long long off) asm("llvm.bpf.load.word");
/* a helper structure used by eBPF C program
* to describe map attributes to elf_bpf loader
*/
struct bpf_map_def {
unsigned int type;
unsigned int key_size;
unsigned int value_size;
unsigned int max_entries;
unsigned int map_flags;
unsigned int inner_map_idx;
unsigned int numa_node;
};
#define BPF_ANNOTATE_KV_PAIR(name, type_key, type_val) \
struct ____btf_map_##name { \
type_key key; \
type_val value; \
}; \
struct ____btf_map_##name \
__attribute__ ((section(".maps." #name), used)) \
____btf_map_##name = { }
static int (*bpf_skb_load_bytes)(void *ctx, int off, void *to, int len) =
(void *) BPF_FUNC_skb_load_bytes;
static int (*bpf_skb_load_bytes_relative)(void *ctx, int off, void *to, int len, __u32 start_header) =
(void *) BPF_FUNC_skb_load_bytes_relative;
static int (*bpf_skb_store_bytes)(void *ctx, int off, void *from, int len, int flags) =
(void *) BPF_FUNC_skb_store_bytes;
static int (*bpf_l3_csum_replace)(void *ctx, int off, int from, int to, int flags) =
(void *) BPF_FUNC_l3_csum_replace;
static int (*bpf_l4_csum_replace)(void *ctx, int off, int from, int to, int flags) =
(void *) BPF_FUNC_l4_csum_replace;
static int (*bpf_csum_diff)(void *from, int from_size, void *to, int to_size, int seed) =
(void *) BPF_FUNC_csum_diff;
static int (*bpf_skb_under_cgroup)(void *ctx, void *map, int index) =
(void *) BPF_FUNC_skb_under_cgroup;
static int (*bpf_skb_change_head)(void *, int len, int flags) =
(void *) BPF_FUNC_skb_change_head;
static int (*bpf_skb_pull_data)(void *, int len) =
(void *) BPF_FUNC_skb_pull_data;
static unsigned int (*bpf_get_cgroup_classid)(void *ctx) =
(void *) BPF_FUNC_get_cgroup_classid;
static unsigned int (*bpf_get_route_realm)(void *ctx) =
(void *) BPF_FUNC_get_route_realm;
static int (*bpf_skb_change_proto)(void *ctx, __be16 proto, __u64 flags) =
(void *) BPF_FUNC_skb_change_proto;
static int (*bpf_skb_change_type)(void *ctx, __u32 type) =
(void *) BPF_FUNC_skb_change_type;
static unsigned int (*bpf_get_hash_recalc)(void *ctx) =
(void *) BPF_FUNC_get_hash_recalc;
static unsigned long long (*bpf_get_current_task)(void *ctx) =
(void *) BPF_FUNC_get_current_task;
static int (*bpf_skb_change_tail)(void *ctx, __u32 len, __u64 flags) =
(void *) BPF_FUNC_skb_change_tail;
static long long (*bpf_csum_update)(void *ctx, __u32 csum) =
(void *) BPF_FUNC_csum_update;
static void (*bpf_set_hash_invalid)(void *ctx) =
(void *) BPF_FUNC_set_hash_invalid;
static int (*bpf_get_numa_node_id)(void) =
(void *) BPF_FUNC_get_numa_node_id;
static int (*bpf_probe_read_str)(void *ctx, __u32 size,
const void *unsafe_ptr) =
(void *) BPF_FUNC_probe_read_str;
static unsigned int (*bpf_get_socket_uid)(void *ctx) =
(void *) BPF_FUNC_get_socket_uid;
static unsigned int (*bpf_set_hash)(void *ctx, __u32 hash) =
(void *) BPF_FUNC_set_hash;
static int (*bpf_skb_adjust_room)(void *ctx, __s32 len_diff, __u32 mode,
unsigned long long flags) =
(void *) BPF_FUNC_skb_adjust_room;
/* Scan the ARCH passed in from ARCH env variable (see Makefile) */
#if defined(__TARGET_ARCH_x86)
#define bpf_target_x86
#define bpf_target_defined
#elif defined(__TARGET_ARCH_s930x)
#define bpf_target_s930x
#define bpf_target_defined
#elif defined(__TARGET_ARCH_arm64)
#define bpf_target_arm64
#define bpf_target_defined
#elif defined(__TARGET_ARCH_mips)
#define bpf_target_mips
#define bpf_target_defined
#elif defined(__TARGET_ARCH_powerpc)
#define bpf_target_powerpc
#define bpf_target_defined
#elif defined(__TARGET_ARCH_sparc)
#define bpf_target_sparc
#define bpf_target_defined
#else
#undef bpf_target_defined
#endif
/* Fall back to what the compiler says */
#ifndef bpf_target_defined
#if defined(__x86_64__)
#define bpf_target_x86
#elif defined(__s390x__)
#define bpf_target_s930x
#elif defined(__aarch64__)
#define bpf_target_arm64
#elif defined(__mips__)
#define bpf_target_mips
#elif defined(__powerpc__)
#define bpf_target_powerpc
#elif defined(__sparc__)
#define bpf_target_sparc
#endif
#endif
#if defined(bpf_target_x86)
#define PT_REGS_PARM1(x) ((x)->di)
#define PT_REGS_PARM2(x) ((x)->si)
#define PT_REGS_PARM3(x) ((x)->dx)
#define PT_REGS_PARM4(x) ((x)->cx)
#define PT_REGS_PARM5(x) ((x)->r8)
#define PT_REGS_RET(x) ((x)->sp)
#define PT_REGS_FP(x) ((x)->bp)
#define PT_REGS_RC(x) ((x)->ax)
#define PT_REGS_SP(x) ((x)->sp)
#define PT_REGS_IP(x) ((x)->ip)
#elif defined(bpf_target_s390x)
#define PT_REGS_PARM1(x) ((x)->gprs[2])
#define PT_REGS_PARM2(x) ((x)->gprs[3])
#define PT_REGS_PARM3(x) ((x)->gprs[4])
#define PT_REGS_PARM4(x) ((x)->gprs[5])
#define PT_REGS_PARM5(x) ((x)->gprs[6])
#define PT_REGS_RET(x) ((x)->gprs[14])
#define PT_REGS_FP(x) ((x)->gprs[11]) /* Works only with CONFIG_FRAME_POINTER */
#define PT_REGS_RC(x) ((x)->gprs[2])
#define PT_REGS_SP(x) ((x)->gprs[15])
#define PT_REGS_IP(x) ((x)->psw.addr)
#elif defined(bpf_target_arm64)
#define PT_REGS_PARM1(x) ((x)->regs[0])
#define PT_REGS_PARM2(x) ((x)->regs[1])
#define PT_REGS_PARM3(x) ((x)->regs[2])
#define PT_REGS_PARM4(x) ((x)->regs[3])
#define PT_REGS_PARM5(x) ((x)->regs[4])
#define PT_REGS_RET(x) ((x)->regs[30])
#define PT_REGS_FP(x) ((x)->regs[29]) /* Works only with CONFIG_FRAME_POINTER */
#define PT_REGS_RC(x) ((x)->regs[0])
#define PT_REGS_SP(x) ((x)->sp)
#define PT_REGS_IP(x) ((x)->pc)
#elif defined(bpf_target_mips)
#define PT_REGS_PARM1(x) ((x)->regs[4])
#define PT_REGS_PARM2(x) ((x)->regs[5])
#define PT_REGS_PARM3(x) ((x)->regs[6])
#define PT_REGS_PARM4(x) ((x)->regs[7])
#define PT_REGS_PARM5(x) ((x)->regs[8])
#define PT_REGS_RET(x) ((x)->regs[31])
#define PT_REGS_FP(x) ((x)->regs[30]) /* Works only with CONFIG_FRAME_POINTER */
#define PT_REGS_RC(x) ((x)->regs[1])
#define PT_REGS_SP(x) ((x)->regs[29])
#define PT_REGS_IP(x) ((x)->cp0_epc)
#elif defined(bpf_target_powerpc)
#define PT_REGS_PARM1(x) ((x)->gpr[3])
#define PT_REGS_PARM2(x) ((x)->gpr[4])
#define PT_REGS_PARM3(x) ((x)->gpr[5])
#define PT_REGS_PARM4(x) ((x)->gpr[6])
#define PT_REGS_PARM5(x) ((x)->gpr[7])
#define PT_REGS_RC(x) ((x)->gpr[3])
#define PT_REGS_SP(x) ((x)->sp)
#define PT_REGS_IP(x) ((x)->nip)
#elif defined(bpf_target_sparc)
#define PT_REGS_PARM1(x) ((x)->u_regs[UREG_I0])
#define PT_REGS_PARM2(x) ((x)->u_regs[UREG_I1])
#define PT_REGS_PARM3(x) ((x)->u_regs[UREG_I2])
#define PT_REGS_PARM4(x) ((x)->u_regs[UREG_I3])
#define PT_REGS_PARM5(x) ((x)->u_regs[UREG_I4])
#define PT_REGS_RET(x) ((x)->u_regs[UREG_I7])
#define PT_REGS_RC(x) ((x)->u_regs[UREG_I0])
#define PT_REGS_SP(x) ((x)->u_regs[UREG_FP])
/* Should this also be a bpf_target check for the sparc case? */
#if defined(__arch64__)
#define PT_REGS_IP(x) ((x)->tpc)
#else
#define PT_REGS_IP(x) ((x)->pc)
#endif
#endif
#ifdef bpf_target_powerpc
#define BPF_KPROBE_READ_RET_IP(ip, ctx) ({ (ip) = (ctx)->link; })
#define BPF_KRETPROBE_READ_RET_IP BPF_KPROBE_READ_RET_IP
#elif bpf_target_sparc
#define BPF_KPROBE_READ_RET_IP(ip, ctx) ({ (ip) = PT_REGS_RET(ctx); })
#define BPF_KRETPROBE_READ_RET_IP BPF_KPROBE_READ_RET_IP
#else
#define BPF_KPROBE_READ_RET_IP(ip, ctx) ({ \
bpf_probe_read(&(ip), sizeof(ip), (void *)PT_REGS_RET(ctx)); })
#define BPF_KRETPROBE_READ_RET_IP(ip, ctx) ({ \
bpf_probe_read(&(ip), sizeof(ip), \
(void *)(PT_REGS_FP(ctx) + sizeof(ip))); })
#endif
#endif
src/motor-control/src/client.c 0 → 100644
View file @ 645e9fb8
/*
* Real time packet sending client
*
* Large portions taken from cyclictest
*
*/
#define _GNU_SOURCE
#include <errno.h>
#include <error.h>
#include <fcntl.h>
#include <limits.h>
#include <pthread.h>
#include <sched.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
#include "common.h"
#include "send_packet.h"
// Structs
typedef struct thread_param {
uint64_t interval;
unsigned int max_cycles;
int priority;
int etf_offset;
int64_t send_ts_delay;
int affinity_cpu;
uint64_t start_ts;
} thread_param_t;
typedef struct main_param {
int refresh_rate;
int verbose;
int transition_time;
uint64_t target_interval;
} main_param_t;
// Static functions
static void process_options(int argc, char *argv[]);
static void do_tsn_task(char *data, uint64_t next_txtime);
static void sighand(int sig_num);
// Static variables
static uint64_t nb_cycles;
static main_param_t main_params;
static thread_param_t thread_params;
static egress_param_t egress_params;
static egress_param_t egress_params2;
static egress_stat_t egress_stats = {.packets_sent = 0,
.min_interval = INT_MAX};
static egress_stat_t egress_stats2 = {.packets_sent = 0,
.min_interval = INT_MAX};
static egress_info_t egress_info;
static egress_info_t egress_info2;
static int enable_etf;
static int two_servers = 0;
static struct timespec measures_start;
static struct timespec measures_end;
static char send_data[MAX_BUFFER_SIZE];
static int reverse_motors = 0;
static void help(char *argv[]) {
printf(
"Usage: %s [-a CPU -p PRIO -i USEC -r USEC -l N] [-d BUF_LEN | -c "
"DELAY -s NS] [-b -e OFFSET -q PK_PRIO -gtvT] IF IP IF2 IP2\n\n"
" -h Show help\n"
" -a CPU Pin the real time thread to CPU\n"
" -p PRIO RT thread priority\n"
" -c USEC Send timestamp with the specified delay\n"
" -s NS Common start time reference\n"
" -e USEC txtime userspace offset (for ETF qdisc)\n"
" -q PK_PRIO Packet priority \n"
" -t T Transition time\n"
"\n",
argv[0]);
}
static double i_to_rps(int i) {
return ((double)USEC_PER_SEC) / (MOTOR_STEPS * i);
}
static int rps_to_i(double rps) {
return USEC_PER_SEC / (MOTOR_STEPS * rps);
}
static void *print_thread(void *p) {
(void)p;
for (;;) {
if (thread_params.max_cycles &&
nb_cycles >= ((unsigned int)thread_params.max_cycles))
break;
printf("RPS: %5F RPS Target: %5F s/100 RPS %5d\n", i_to_rps(thread_params.interval / 1000),
i_to_rps(main_params.target_interval),
main_params.transition_time);
printf("\033[%dA", 1);
usleep(100000);
}
pthread_exit(NULL);
}
/*
* Real-time thread:
* - Sends packets at a regular intervall
*/
static void *packet_sending_thread(void *p) {
(void)p;
struct timespec next, current, previous;
uint64_t next_txtime = 0, tx_data = 0;
int ret;
cpu_set_t mask;
uint64_t next_increment = 0;
uint64_t end_t = 0;
uint64_t cur_t = 0;
uint64_t i_t = 0;
uint64_t i_s = thread_params.interval / 1000;
uint64_t i_c = thread_params.interval / 1000;
// Set thread CPU affinity
if (thread_params.affinity_cpu != -1) {
CPU_ZERO(&mask);
CPU_SET(thread_params.affinity_cpu, &mask);
if (sched_setaffinity(0, sizeof(mask), &mask))
error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n",
thread_params.affinity_cpu);
}
clock_gettime(CLOCK_MONOTONIC, &measures_start);
clock_gettime(CLOCK_REALTIME, &next);
if (thread_params.start_ts) {
if (thread_params.start_ts < ts_to_uint(next)) {
fprintf(stderr, "start timestamp is in the past, aborting...\n");
fflush(stdout);
exit(EXIT_FAILURE);
}
if (thread_params.start_ts > (ts_to_uint(next) + UINT64_C(3600000000000))) {
fprintf(stderr, "start timestamp is too high, aborting...\n");
fflush(stdout);
exit(EXIT_FAILURE);
}
next = uint_to_ts(thread_params.start_ts);
}
add_ns(&next, thread_params.interval);
// Set txtime and TX data timestamp if required
if (enable_etf) {
next_txtime = next.tv_sec * NSEC_PER_SEC + next.tv_nsec;
next_txtime += thread_params.etf_offset;
}
tx_data = next.tv_sec * NSEC_PER_SEC + next.tv_nsec;
tx_data += thread_params.send_ts_delay;
printf("Will start at %" PRIu64 "\n", ts_to_uint(next));
// Packet sending loop
for (nb_cycles = 0;; nb_cycles++) {
if (thread_params.max_cycles &&
nb_cycles >= ((unsigned int)thread_params.max_cycles))
break;
clock_gettime(CLOCK_REALTIME, &current);
// Sanity check
if(current.tv_sec > next.tv_sec)
goto invalid_ts;
if(current.tv_sec == next.tv_sec && current.tv_nsec > next.tv_nsec)
goto invalid_ts;
ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next, NULL);
if (ret) {
fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
exit(EXIT_FAILURE);
}
// TX data
encode(tx_data, send_data);
// Get timestamp before TSN task for stats
clock_gettime(CLOCK_REALTIME, &current);
do_tsn_task(send_data, next_txtime);
add_ns(&next, thread_params.interval);
// Update txtime and TX data timestamp if required
if (enable_etf) next_txtime += thread_params.interval;
tx_data = next.tv_sec * NSEC_PER_SEC + next.tv_nsec;
tx_data += thread_params.send_ts_delay;
if(reverse_motors) {
reverse_motors = 0;
tx_data = 17;
}
// Update statistics
if (nb_cycles) {
int interval_us = calcdiff_ns(current, previous) / 1000;
egress_stats.min_interval = _min_(interval_us, egress_stats.min_interval);
egress_stats.max_interval = _max_(interval_us, egress_stats.max_interval);
egress_stats.avg_interval =
(egress_stats.avg_interval * nb_cycles + interval_us) /
(nb_cycles + 1);
}
if(thread_params.interval != (main_params.target_interval * 1000)) {
i_c = thread_params.interval / 1000;
// If target interval changed
if(i_t != main_params.target_interval) {
i_t = main_params.target_interval;
i_s = i_c;
cur_t = 0;
next_increment = 0;
if(i_t < i_s)
end_t = (main_params.transition_time * USEC_PER_SEC * USEC_PER_SEC * (i_c - i_t)) / (100 * MOTOR_STEPS * i_t * i_c);
else
end_t = (main_params.transition_time * USEC_PER_SEC * USEC_PER_SEC * (i_t - i_c)) / (100 * MOTOR_STEPS * i_t * i_c);
}
if(next_increment) {
if(cur_t > next_increment) {
i_c += i_t < i_s ? -1 : 1;
next_increment = 0;
}
} else {
uint64_t next_i = i_t < i_s ? i_c - 1 : i_c + 1;
// Compute time at which we will need to increment / decrement the interval
if(i_t < i_s)
next_increment = (end_t * i_t * (i_s - next_i)) / ((i_s - i_t) * next_i);
else
next_increment = (end_t * i_t * (next_i - i_s)) / ((i_t - i_s) * next_i);
// If next increment time is before next interval
if(next_increment < cur_t + i_c) {
if(i_t < i_s) {
i_c = (i_t * i_s * end_t) / ((i_s - i_t) * (cur_t + i_c) + end_t * i_t);
i_c = i_c < i_t ? i_t : i_c;
}
else {
i_c = (i_t * i_s * end_t) / (end_t * i_t - (i_t - i_s) * (cur_t + i_c));
i_c = i_c > i_t ? i_t : i_c;
}
next_increment = 0;
}
}
cur_t += i_c;
if(i_c < 50) {
fprintf(stderr, "Interval too small, exiting..\n");
exit(EXIT_FAILURE);
}
if(i_c > USEC_PER_SEC) {
fprintf(stderr, "Interval too big, exiting..\n");
exit(EXIT_FAILURE);
}
thread_params.interval = i_c * 1000;
}
previous = current;
}
pthread_exit(NULL);
invalid_ts:
fprintf(stderr, "Invalid timestamp : %" PRIu64 "\n", ts_to_uint(next));
fprintf(stderr, "Current time is : %" PRIu64 "\n", ts_to_uint(current));
fprintf(stderr, "Difference with timestamp : %" PRIi64 "\n\n\n", ((int64_t)ts_to_uint(next)) - ((int64_t)ts_to_uint(current)));
exit(EXIT_FAILURE);
}
static void motor_input(void) {
char user_input[255];
int v;
scanf("%s", user_input);
switch(user_input[0]) {
case 'a':
v = atoi(user_input + 1);
if(v)
main_params.transition_time = v;
break;
case 'r':
reverse_motors = 1;
break;
default:
v = rps_to_i(strtod(user_input, NULL));
if(v)
main_params.target_interval = v;
break;
}
}
/*
* Main thread:
* - Has non-real time priority
* - Handles the IO and creates the real time thread
*/
int main(int argc, char *argv[]) {
pthread_t thread, print_pthread;
struct sched_param param;
pthread_attr_t attr;
// Default configuration values
thread_params.interval = 100000 * 1000;
thread_params.priority = 98;
thread_params.send_ts_delay = 0;
thread_params.affinity_cpu = -1;
thread_params.start_ts = 0;
main_params.refresh_rate = 50000;
main_params.verbose = 0;
egress_params.packet_priority = 3;
egress_params.tx_buffer_len = 1024;
egress_params2.packet_priority = 3;
egress_params2.tx_buffer_len = 1024;
enable_etf = 0;
// Lock all current and future pages from preventing of being paged to
// swap
if (mlockall(MCL_CURRENT | MCL_FUTURE)) {
fprintf(stderr, "mlockall failed");
exit(EXIT_FAILURE);
}
// Process bash options
process_options(argc, argv);
main_params.target_interval = thread_params.interval / 1000;
set_latency_target();
egress_params.use_etf = enable_etf;
egress_params2.use_etf = enable_etf;
egress_info.params = &egress_params;
egress_info.stats = &egress_stats;
egress_info2.params = &egress_params2;
egress_info2.stats = &egress_stats2;
// Catch breaks with sighand
init_signals(sighand);
// Initialize the UDP packet sending socket
init_udp_send(&egress_info);
if(two_servers)
init_udp_send(&egress_info2);
// Initialize pthread attributes (default values)
if (pthread_attr_init(&attr)) {
fprintf(stderr, "init pthread attributes failed\n");
exit(EXIT_FAILURE);
}
// Set a specific stack size
if (pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN)) {
fprintf(stderr, "pthread setstacksize failed\n");
exit(EXIT_FAILURE);
}
// Set scheduler policy and priority of pthread
if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO)) {
fprintf(stderr, "pthread setschedpolicy failed\n");
exit(EXIT_FAILURE);
}
param.sched_priority = thread_params.priority;
if (pthread_attr_setschedparam(&attr, &param)) {
fprintf(stderr, "pthread setschedparam failed\n");
exit(EXIT_FAILURE);
}
/* Use scheduling parameters of attr */
if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) {
fprintf(stderr, "pthread setinheritsched failed\n");
exit(EXIT_FAILURE);
}
// Create the real time thread
if (pthread_create(&thread, &attr, packet_sending_thread, NULL))
error(EXIT_FAILURE, errno, "Couldn't create packet sending thread");
// Create the print thread
if (pthread_create(&print_pthread, NULL, print_thread, NULL))
error(EXIT_FAILURE, errno, "Couldn't create print thread");
// Verbose loop
for (;;) {
usleep(main_params.refresh_rate);
motor_input();
if (thread_params.max_cycles)
if (thread_params.max_cycles == nb_cycles) break;
}
exit(EXIT_SUCCESS);
}
/* Critical TSN task
*/
static void do_tsn_task(char *data, uint64_t next_txtime) {
// One way packet sending
send_udp_packet(data, next_txtime, &egress_info);
if(two_servers)
send_udp_packet(data, next_txtime, &egress_info2);
}
static void sighand(int sig_num) {
(void)sig_num;
exit(EXIT_SUCCESS);
}
/* Process bash options
*/
static void process_options(int argc, char *argv[]) {
for (;;) {
int c = getopt(argc, argv, "a:c:e:hp:q:s:t:");
if (c == -1) break;
switch (c) {
case 'a':
thread_params.affinity_cpu = atoi(optarg);
break;
case 'c':
thread_params.send_ts_delay = strtoll(optarg, NULL, 10) * 1000;
break;
case 'e':
enable_etf = 1;
thread_params.etf_offset = atoi(optarg) * 1000;
break;
case 'h':
help(argv);
exit(EXIT_SUCCESS);
break;
case 'p':
thread_params.priority = atoi(optarg);
break;
case 'q':
egress_params.packet_priority = atoi(optarg);
break;
case 's':
thread_params.start_ts = strtoull(optarg, NULL, 10);
break;
case 't':
main_params.transition_time = atoi(optarg);
break;
}
}
if (argc != optind + 2 && argc != optind + 4) {
if (argc < optind + 2)
fprintf(stderr, "You need to specifiy an interface and IP address\n");
else
fprintf(stderr, "Wrong number of arguments");
help(argv);
exit(EXIT_FAILURE);
}
strcpy(egress_params.network_if, argv[optind]);
strcpy(egress_params.server_ip, argv[optind + 1]);
if (argc == optind + 4) {
two_servers = 1;
strcpy(egress_params2.network_if, argv[optind + 2]);
strcpy(egress_params2.server_ip, argv[optind + 3]);
}
}
src/motor-control/src/common.c 0 → 100644
View file @ 645e9fb8
#define _GNU_SOURCE
#include "common.h"
#include <errno.h>
#include <error.h>
#include <fcntl.h>
#include <inttypes.h>
#include <limits.h>
#include <pthread.h>
#include <sched.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
static int latency_target_fd = -1;
static int32_t latency_target_value = 0;
void (*previous_handlers[NSIG])(int);
static void (*sighand)(int);
uint64_t ts_to_uint(struct timespec t) {
return t.tv_sec * NSEC_PER_SEC + t.tv_nsec;
}
struct timespec uint_to_ts(uint64_t t) {
struct timespec ts;
ts.tv_sec = t / NSEC_PER_SEC;
ts.tv_nsec = t - (ts.tv_sec * NSEC_PER_SEC);
return ts;
}
/* Latency trick
* if the file /dev/cpu_dma_latency exists,
* open it and write a zero into it. This will tell
* the power management system not to transition to
* a high cstate (in fact, the system acts like idle=poll)
* When the fd to /dev/cpu_dma_latency is closed, the behavior
* goes back to the system default.
*
* Documentation/power/pm_qos_interface.txt
*/
void set_latency_target(void) {
struct stat s;
int err;
errno = 0;
err = stat("/dev/cpu_dma_latency", &s);
if (err == -1) {
error(EXIT_FAILURE, errno, "WARN: stat /dev/cpu_dma_latency failed");
return;
}
errno = 0;
latency_target_fd = open("/dev/cpu_dma_latency", O_RDWR);
if (latency_target_fd == -1) {
error(EXIT_FAILURE, errno, "WARN: open /dev/cpu_dma_latency");
return;
}
errno = 0;
err = write(latency_target_fd, &latency_target_value, 4);
if (err < 1) {
error(EXIT_FAILURE, errno, "# error setting cpu_dma_latency to %d!",
latency_target_value);
close(latency_target_fd);
return;
}
printf("# /dev/cpu_dma_latency set to %dus\n", latency_target_value);
}
void add_ns(struct timespec *t, uint64_t ns) {
t->tv_nsec += ns;
while (t->tv_nsec >= ((int64_t)NSEC_PER_SEC)) {
t->tv_sec += 1;
t->tv_nsec -= NSEC_PER_SEC;
}
}
void substract_ns(struct timespec *t, uint64_t ns) {
t->tv_nsec -= ns;
while (t->tv_nsec < 0) {
t->tv_sec -= 1;
t->tv_nsec += NSEC_PER_SEC;
}
}
uint64_t calcdiff_ns(struct timespec t1, struct timespec t2) {
uint64_t diff;
diff = NSEC_PER_SEC * (uint64_t)((int)t1.tv_sec - (int)t2.tv_sec);
diff += ((int)t1.tv_nsec - (int)t2.tv_nsec);
return diff;
}
int64_t calcdiff_ns_signed(struct timespec t1, struct timespec t2) {
int64_t diff;
diff = NSEC_PER_SEC * ((int)t1.tv_sec - (int)t2.tv_sec);
diff += ((int)t1.tv_nsec - (int)t2.tv_nsec);
return diff;
}
int _max_(int a, int b) { return a > b ? a : b; }
int _min_(int a, int b) { return a < b ? a : b; }
static void sighand_wrapper(int sig) {
// If we get un unexpected signal, report it, if not print the histogram
if (sig == SIGINT || sig == SIGTERM)
(*sighand)(sig); // Will print the histogram
else
printf("Uknown signal interrupt: %s (%d)\n", strsignal(sig), sig);
// Execute the default handler
if (previous_handlers[sig] == SIG_DFL) {
signal(sig, SIG_DFL);
raise(sig);
} else if (previous_handlers[sig] == SIG_IGN) {
return;
} else {
(*previous_handlers[sig])(sig);
}
}
void init_signals(void (*_sighand)(int)) {
sighand = _sighand;
for (int i = 0; i < NSIG; i++) signal(i, sighand_wrapper);
}
static int get_bit(uint64_t t, int i) {
return (t & (((uint64_t) 1) << i)) > 0;
}
void encode(uint64_t t, char * b) {
for(int i = 0; i < 64; i++)
b[i] = get_bit(t, i) * 0xff;
}
uint64_t decode(char * b) {
uint64_t ret = 0;
for(int i = 0; i < 64; i++) {
int sum_bits = 0;
for(int j = 0; j < 8; j++)
sum_bits += (b[i] & (1 << j)) > 0;
ret |= (sum_bits >= 4) * (((uint64_t) 1) << i);
}
return ret;
}
src/motor-control/src/common.h 0 → 100644
View file @ 645e9fb8
#ifndef UTILITIES_H
#define UTILITIES_H
#define _GNU_SOURCE
#include <inttypes.h>
#include <signal.h>
#include <stdint.h>
#include <time.h>
#include <unistd.h>
#ifdef WITH_XDP
#include <bpf/bpf.h>
#include <bpf/libbpf.h>
#include <bpf/xsk.h>
#include <linux/if_ether.h>
#include <linux/if_link.h>
#include <linux/if_xdp.h>
#include <linux/ip.h>
#include <linux/udp.h>
#endif
#define MOTOR_STEPS 800
#define NSEC_PER_SEC UINT64_C(1000000000)
#define USEC_PER_SEC UINT64_C(1000000)
#define SERVER_PORT "50000"
#define SERVER_PORT_INT 50000
#define MAX_BUFFER_SIZE 1024
#define err(...) \
do { \
fprintf(stderr, __VA_ARGS__); \
exit(EXIT_FAILURE); \
} while (0)
#define err_errno(...) error(EXIT_FAILURE, errno, __VA_ARGS__);
uint64_t ts_to_uint(struct timespec t);
struct timespec uint_to_ts(uint64_t t);
void add_ns(struct timespec *t, uint64_t ns);
void substract_ns(struct timespec *t, uint64_t ns);
uint64_t calcdiff_ns(struct timespec t1, struct timespec t2);
int64_t calcdiff_ns_signed(struct timespec t1, struct timespec t2);
void set_latency_target(void);
void init_signals(void (*_sighand)(int));
int _min_(int a, int b);
int _max_(int a, int b);
extern void (*previous_handlers[NSIG])(int);
void encode(uint64_t t, char * b);
uint64_t decode(char * b);
#endif
src/motor-control/src/gpio.c 0 → 100644
View file @ 645e9fb8
#include "gpio.h"
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>
static char path[64];
static char cmd[128];
static char * one = "1";
static char * zero = "0";
int enable_gpio(int * fd, int gpio_index) {
FILE *fp;
char gpio_state_str[16];
int gpio_state = 0;
sprintf(path, "/sys/class/gpio/gpio%d/value", gpio_index);
sprintf(cmd, "cat %s", path);
fp = popen(cmd, "r");
if (fp == NULL) {
fprintf(stderr, "Error when reading gpio\n");
exit(EXIT_FAILURE);
}
while (fgets(gpio_state_str, sizeof(gpio_state_str), fp) != NULL) {
gpio_state = atoi(gpio_state_str);
}
pclose(fp);
*fd = open(path, O_WRONLY);
return gpio_state;
}
int toggle_gpio(int fd, int gpio_state) {
if(gpio_state)
write(fd, zero, 1);
else
write(fd, one, 1);
gpio_state = !gpio_state;
return gpio_state;
}
src/motor-control/src/gpio.h 0 → 100644
View file @ 645e9fb8
#ifndef GPIO_H
#define GPIO_H
int enable_gpio(int * fd, int gpio_index);
int toggle_gpio(int fd, int gpio_state);
#endif
src/motor-control/src/parsing_helpers.h 0 → 100644
View file @ 645e9fb8
/* SPDX-License-Identifier: GPL-2.0 */
/*
* This file contains parsing functions that are used in the packetXX XDP
* programs. The functions are marked as __always_inline, and fully defined in
* this header file to be included in the BPF program.
*
* Each helper parses a packet header, including doing bounds checking, and
* returns the type of its contents if successful, and -1 otherwise.
*
* For Ethernet and IP headers, the content type is the type of the payload
* (h_proto for Ethernet, nexthdr for IPv6), for ICMP it is the ICMP type field.
* All return values are in host byte order.
*
* The versions of the functions included here are slightly expanded versions of
* the functions in the packet01 lesson. For instance, the Ethernet header
* parsing has support for parsing VLAN tags.
*/
#ifndef __PARSING_HELPERS_H
#define __PARSING_HELPERS_H
#include <stddef.h>
#include <linux/if_ether.h>
#include <linux/if_packet.h>
#include <linux/ip.h>
#include <linux/ipv6.h>
#include <linux/icmp.h>
#include <linux/icmpv6.h>
#include <linux/udp.h>
#include <linux/tcp.h>
/* Header cursor to keep track of current parsing position */
struct hdr_cursor {
void *pos;
};
/*
* struct vlan_hdr - vlan header
* @h_vlan_TCI: priority and VLAN ID
* @h_vlan_encapsulated_proto: packet type ID or len
*/
struct vlan_hdr {
__be16 h_vlan_TCI;
__be16 h_vlan_encapsulated_proto;
};
/*
* Struct icmphdr_common represents the common part of the icmphdr and icmp6hdr
* structures.
*/
struct icmphdr_common {
__u8 type;
__u8 code;
__sum16 cksum;
};
/* Allow users of header file to redefine VLAN max depth */
#ifndef VLAN_MAX_DEPTH
#define VLAN_MAX_DEPTH 4
#endif
static __always_inline int proto_is_vlan(__u16 h_proto)
{
return !!(h_proto == bpf_htons(ETH_P_8021Q) ||
h_proto == bpf_htons(ETH_P_8021AD));
}
/* Notice, parse_ethhdr() will skip VLAN tags, by advancing nh->pos and returns
* next header EtherType, BUT the ethhdr pointer supplied still points to the
* Ethernet header. Thus, caller can look at eth->h_proto to see if this was a
* VLAN tagged packet.
*/
static __always_inline int parse_ethhdr(struct hdr_cursor *nh, void *data_end,
struct ethhdr **ethhdr)
{
struct ethhdr *eth = nh->pos;
int hdrsize = sizeof(*eth);
struct vlan_hdr *vlh;
__u16 h_proto;
int i;
/* Byte-count bounds check; check if current pointer + size of header
* is after data_end.
*/
if (nh->pos + hdrsize > data_end)
return -1;
nh->pos += hdrsize;
*ethhdr = eth;
vlh = nh->pos;
h_proto = eth->h_proto;
/* Use loop unrolling to avoid the verifier restriction on loops;
* support up to VLAN_MAX_DEPTH layers of VLAN encapsulation.
*/
#pragma unroll
for (i = 0; i < VLAN_MAX_DEPTH; i++) {
if (!proto_is_vlan(h_proto))
break;
if (vlh + 1 > data_end)
break;
h_proto = vlh->h_vlan_encapsulated_proto;
vlh++;
}
nh->pos = vlh;
return h_proto; /* network-byte-order */
}
static __always_inline int parse_ip6hdr(struct hdr_cursor *nh,
void *data_end,
struct ipv6hdr **ip6hdr)
{
struct ipv6hdr *ip6h = nh->pos;
/* Pointer-arithmetic bounds check; pointer +1 points to after end of
* thing being pointed to. We will be using this style in the remainder
* of the tutorial.
*/
if (ip6h + 1 > data_end)
return -1;
nh->pos = ip6h + 1;
*ip6hdr = ip6h;
return ip6h->nexthdr;
}
static __always_inline int parse_iphdr(struct hdr_cursor *nh,
void *data_end,
struct iphdr **iphdr)
{
struct iphdr *iph = nh->pos;
int hdrsize;
if (iph + 1 > data_end)
return -1;
hdrsize = iph->ihl * 4;
/* Variable-length IPv4 header, need to use byte-based arithmetic */
if (nh->pos + hdrsize > data_end)
return -1;
nh->pos += hdrsize;
*iphdr = iph;
return iph->protocol;
}
static __always_inline int parse_icmp6hdr(struct hdr_cursor *nh,
void *data_end,
struct icmp6hdr **icmp6hdr)
{
struct icmp6hdr *icmp6h = nh->pos;
if (icmp6h + 1 > data_end)
return -1;
nh->pos = icmp6h + 1;
*icmp6hdr = icmp6h;
return icmp6h->icmp6_type;
}
static __always_inline int parse_icmphdr(struct hdr_cursor *nh,
void *data_end,
struct icmphdr **icmphdr)
{
struct icmphdr *icmph = nh->pos;
if (icmph + 1 > data_end)
return -1;
nh->pos = icmph + 1;
*icmphdr = icmph;
return icmph->type;
}
static __always_inline int parse_icmphdr_common(struct hdr_cursor *nh,
void *data_end,
struct icmphdr_common **icmphdr)
{
struct icmphdr_common *h = nh->pos;
if (h + 1 > data_end)
return -1;
nh->pos = h + 1;
*icmphdr = h;
return h->type;
}
/*
* parse_tcphdr: parse the udp header and return the length of the udp payload
*/
static __always_inline int parse_udphdr(struct hdr_cursor *nh,
void *data_end,
struct udphdr **udphdr)
{
int len;
struct udphdr *h = nh->pos;
if (h + 1 > data_end)
return -1;
nh->pos = h + 1;
*udphdr = h;
len = bpf_ntohs(h->len) - sizeof(struct udphdr);
if (len < 0)
return -1;
return len;
}
/*
* parse_tcphdr: parse and return the length of the tcp header
*/
static __always_inline int parse_tcphdr(struct hdr_cursor *nh,
void *data_end,
struct tcphdr **tcphdr)
{
int len;
struct tcphdr *h = nh->pos;
if (h + 1 > data_end)
return -1;
len = h->doff * 4;
if ((void *) h + len > data_end)
return -1;
nh->pos = h + 1;
*tcphdr = h;
return len;
}
#endif /* __PARSING_HELPERS_H */
src/motor-control/src/recv_packet.c 0 → 100644
View file @ 645e9fb8
#define _GNU_SOURCE
#include "recv_packet.h"
#include <arpa/inet.h>
#include <errno.h>
#include <error.h>
#include <fcntl.h>
#include <getopt.h>
#include <ifaddrs.h>
#include <inttypes.h>
#include <limits.h>
#include <linux/errqueue.h>
#include <linux/ethtool.h>
#include <linux/net_tstamp.h>
#include <linux/sockios.h>
#include <net/if.h>
#include <netdb.h>
#include <netinet/in.h>
#include <poll.h>
#include <pthread.h>
#include <sched.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
#include "common.h"
#ifdef WITH_XDP
#include <bpf/bpf.h>
#include <bpf/libbpf.h>
#include <bpf/xsk.h>
#include <linux/if_ether.h>
#include <linux/if_link.h>
#include <linux/if_xdp.h>
#include <linux/ip.h>
#include <linux/udp.h>
#endif
static char rx_buffer[MAX_BUFFER_SIZE];
static int sock_fd;
static ingress_param_t *params;
static ingress_stat_t *stats;
// Sets the interface
static int set_if(void) {
struct ifreq ifreq;
memset(&ifreq, 0, sizeof(ifreq));
strncpy(ifreq.ifr_name, params->network_if, sizeof(ifreq.ifr_name) - 1);
if (ioctl(sock_fd, SIOCGIFINDEX, &ifreq))
error(EXIT_FAILURE, errno, "ioctl SIOCGIFINDEX failed\n");
return ifreq.ifr_ifindex;
}
void init_udp_recv(ingress_param_t *_params, ingress_stat_t *_stats) {
int getaddrinfo_err;
int set_if_err;
struct addrinfo hints, *servinfo, *servinfo_it;
params = _params;
stats = _stats;
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_DGRAM;
hints.ai_flags = AI_PASSIVE;
getaddrinfo_err = getaddrinfo(NULL, SERVER_PORT, &hints, &servinfo);
if (getaddrinfo_err != 0) {
fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(getaddrinfo_err));
exit(EXIT_FAILURE);
}
for (servinfo_it = servinfo; servinfo_it;
servinfo_it = servinfo_it->ai_next) {
sock_fd = socket(servinfo->ai_family, servinfo->ai_socktype,
servinfo->ai_protocol);
if (bind(sock_fd, servinfo_it->ai_addr, servinfo_it->ai_addrlen) == -1) {
close(sock_fd);
continue;
}
break;
}
freeaddrinfo(servinfo);
if (sock_fd == -1)
error(EXIT_FAILURE, errno, "Couldn't create receive socket");
set_if_err = set_if();
if (set_if_err < 0) error(EXIT_FAILURE, errno, "Couldn't set interface\n");
if (setsockopt(sock_fd, SOL_SOCKET, SO_BINDTODEVICE, params->network_if,
strlen(params->network_if)))
error(EXIT_FAILURE, errno, "setsockopt SO_BINDTODEVICE failed\n");
}
/*
* Receive UDP packet
*/
void recv_udp_packet() {
struct cmsghdr *cmsg;
struct msghdr msg; // Message hardware, sent to the socket
struct iovec iov; // The iovec structures stores the RX buffer
struct sockaddr_in sin;
struct {
struct cmsghdr cm;
char control[512];
} control;
int recvmsgerr;
iov.iov_base = &rx_buffer;
iov.iov_len = MAX_BUFFER_SIZE - 1;
memset(&msg, 0, sizeof(msg));
msg.msg_name = &sin;
msg.msg_namelen = sizeof(sin);
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = &control;
msg.msg_controllen = sizeof(control);
recvmsgerr = recvmsg(sock_fd, &msg, 0);
if (recvmsgerr < 0)
error(EXIT_FAILURE, errno, "recvmsg failed, ret value: %d\n", recvmsgerr);
for (int i = 0; i < MAX_BUFFER_SIZE; i++) stats->data[i] = rx_buffer[i];
}
#ifdef WITH_XDP
static int xdp_flags = XDP_FLAGS_DRV_MODE;
static struct pollfd fds[1] = {0};
static unsigned int ifindex;
static struct xdpsock xdp_socket;
static void open_xdp_socket(char *network_if) {
struct xsk_socket_config xsk_cfg;
uint32_t idx;
int ret, i;
/* Create XDP socket */
xsk_cfg.rx_size = XSK_RING_CONS__DEFAULT_NUM_DESCS;
xsk_cfg.tx_size = XSK_RING_PROD__DEFAULT_NUM_DESCS;
xsk_cfg.libbpf_flags = 0;
xsk_cfg.xdp_flags = xdp_flags;
xsk_cfg.bind_flags = 0;
ret = xsk_socket__create(&xdp_socket.xsk, network_if, 0, xdp_socket.umem.umem,
&xdp_socket.rx, &xdp_socket.tx, &xsk_cfg);
if (ret) err("xsk_socket__create() failed");
/* Add some buffers */
ret = xsk_ring_prod__reserve(&xdp_socket.umem.fq,
XSK_RING_PROD__DEFAULT_NUM_DESCS, &idx);
if (ret != XSK_RING_PROD__DEFAULT_NUM_DESCS)
err("xsk_ring_prod__reserve() failed");
for (i = 0; i < XSK_RING_PROD__DEFAULT_NUM_DESCS; i++)
*xsk_ring_prod__fill_addr(&xdp_socket.umem.fq, idx++) = i * FRAME_SIZE;
xsk_ring_prod__submit(&xdp_socket.umem.fq, XSK_RING_PROD__DEFAULT_NUM_DESCS);
}
/*
* Init XDP socket
*/
void init_xdp_recv(ingress_param_t *_params, ingress_stat_t *_stats) {
int ret, prog_fd, xsks_map = 0;
struct bpf_prog_load_attr prog_load_attr = {
.prog_type = BPF_PROG_TYPE_XDP,
.file = "/home/oli/tsn-measures/packet-exchange/build/xdp_kern.o",
};
struct xsk_umem_config cfg = {
.fill_size = XSK_RING_PROD__DEFAULT_NUM_DESCS,
.comp_size = XSK_RING_CONS__DEFAULT_NUM_DESCS,
.frame_size = FRAME_SIZE,
.frame_headroom = XSK_UMEM__DEFAULT_FRAME_HEADROOM,
.flags = 0,
};
struct bpf_object *obj;
struct bpf_map *map;
void *buffer = NULL;
params = _params;
stats = _stats;
ret = bpf_prog_load_xattr(&prog_load_attr, &obj, &prog_fd);
if (ret || prog_fd < 0) err("bpf_prog_load_xattr() failed");
map = bpf_object__find_map_by_name(obj, "xsks_map");
xsks_map = bpf_map__fd(map);
if (xsks_map < 0) err("No xsks_map found!");
ifindex = if_nametoindex(params->network_if);
if (!ifindex) err_errno("if_nametoindex() failed");
/* Use XDP _only_ in conjuction with driver assisted mode */
ret = bpf_set_link_xdp_fd(ifindex, prog_fd, xdp_flags);
if (ret) err("bpf_set_link_xdp_fd() failed");
/* Allocate user space memory for xdp frames */
ret =
posix_memalign(&buffer, sysconf(_SC_PAGE_SIZE), NUM_FRAMES * FRAME_SIZE);
if (ret) err_errno("posix_memalign() failed");
ret = xsk_umem__create(&xdp_socket.umem.umem, buffer, NUM_FRAMES * FRAME_SIZE,
&xdp_socket.umem.fq, &xdp_socket.umem.cq, &cfg);
if (ret) err("xsk_umem__create() failed");
xdp_socket.umem.buffer = buffer;
/* Open and bind socket */
open_xdp_socket(params->network_if);
}
void setup_poll_fd(void) {
fds[0].fd = xsk_socket__fd(xdp_socket.xsk);
fds[0].events = POLLIN;
}
static int received;
static uint32_t idx_rx = 0, idx;
static void parse_raw_packet(uint64_t addr, size_t len) {
char *packet;
struct ethhdr *eth;
struct iphdr *ip;
struct udphdr *udp;
size_t min_len = sizeof(*eth) + sizeof(*ip) + sizeof(*udp);
if (len <= min_len) {
stats->xdp_data = NULL;
return;
}
packet = xsk_umem__get_data(xdp_socket.umem.buffer, addr);
eth = (struct ethhdr *)packet;
ip = (struct iphdr *)(packet + sizeof(*eth));
udp = (struct udphdr *)(packet + sizeof(*eth) + sizeof(*ip));
stats->xdp_data = packet + sizeof(*eth) + sizeof(*ip) + sizeof(*udp);
}
/*
* Receive XDP socket
*/
int recv_xdp_packet(struct timespec next) {
int ret;
uint64_t addr;
uint32_t len;
struct timespec next_pre, current;
int k = 0;
ret = poll(fds, 1, -1);
if (ret != 1)
error(EXIT_FAILURE, errno, "poll failed");
received = xsk_ring_cons__peek(&xdp_socket.rx, 1, &idx_rx);
if (received != 1)
error(EXIT_FAILURE, errno, "Received %d packets", received);
/* Get the packet */
addr = xsk_ring_cons__rx_desc(&xdp_socket.rx, idx_rx)->addr;
len = xsk_ring_cons__rx_desc(&xdp_socket.rx, idx_rx)->len;
/* Parse it */
parse_raw_packet(xsk_umem__add_offset_to_addr(addr), len);
return 0;
}
void recv_xdp_cleanup(void) {
uint64_t addr;
int ret;
/* Cleanup */
xsk_ring_cons__release(&xdp_socket.rx, received);
/* Add that particular buffer back to the fill queue */
if (xsk_prod_nb_free(&xdp_socket.umem.fq, received)) {
ret = xsk_ring_prod__reserve(&xdp_socket.umem.fq, received, &idx);
if (ret != received) err("xsk_ring_prod__reserve() failed");
*xsk_ring_prod__fill_addr(&xdp_socket.umem.fq, idx) =
xsk_umem__extract_addr(addr);
xsk_ring_prod__submit(&xdp_socket.umem.fq, received);
}
}
void close_xdp_socket(void) {
xsk_socket__delete(xdp_socket.xsk);
xsk_umem__delete(xdp_socket.umem.umem);
bpf_set_link_xdp_fd(ifindex, -1, xdp_flags);
}
#else
void init_xdp_recv(ingress_param_t *_params, ingress_stat_t *_stats) {
(void)_params;
(void)_stats;
}
void setup_poll_fd(void) {}
void close_xdp_socket(void) {}
int recv_xdp_packet(struct timespec next) {
(void)next;
return 0;
}
void recv_xdp_cleanup(void) {}
#endif
src/motor-control/src/recv_packet.h 0 → 100644
View file @ 645e9fb8
#ifndef RECV_PACKET
#define RECV_PACKET
#include "common.h"
typedef struct ingress_param {
char network_if[16];
size_t tx_buffer_len;
} ingress_param_t;
typedef struct ingress_stat {
int min_interval;
int avg_interval;
int max_interval;
uint64_t packets_received;
uint64_t high_kernel_latency;
uint64_t high_jitter;
char data[MAX_BUFFER_SIZE];
char * xdp_data;
} ingress_stat_t;
void init_udp_recv(ingress_param_t *_params, ingress_stat_t *stats);
void recv_udp_packet(void);
#ifdef WITH_XDP
#define NUM_FRAMES 4096
#define FRAME_SIZE XSK_UMEM__DEFAULT_FRAME_SIZE
struct xsk_umem_info {
struct xsk_ring_prod fq;
struct xsk_ring_cons cq;
struct xsk_umem *umem;
void *buffer;
};
struct xdpsock {
struct xsk_ring_cons rx;
struct xsk_ring_prod tx;
struct xsk_umem_info umem;
struct xsk_socket *xsk;
int fd;
};
#endif
void init_xdp_recv(ingress_param_t * _params, ingress_stat_t *_stats);
int recv_xdp_packet();
void recv_xdp_cleanup(void);
void setup_poll_fd(void);
void close_xdp_socket(void);
#endif
src/motor-control/src/send_packet.c 0 → 100644
View file @ 645e9fb8
/*
*
* UDP packet sending functions
*
* Large portions taken from scheduled tx tools gist
*
*/
#define _GNU_SOURCE
#include "send_packet.h"
#include <arpa/inet.h>
#include <errno.h>
#include <error.h>
#include <fcntl.h>
#include <ifaddrs.h>
#include <inttypes.h>
#include <linux/errqueue.h>
#include <linux/ethtool.h>
#include <linux/net_tstamp.h>
#include <linux/sockios.h>
#include <net/if.h>
#include <netdb.h>
#include <netinet/in.h>
#include <poll.h>
#include <pthread.h>
#include <sched.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include "common.h"
static void *poll_thread(void *p);
static void process_error_queue(egress_info_t * info);
static void init_tx_buffer(egress_info_t * info);
static int set_if(egress_info_t * info);
static int so_timestamping_flags =
SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_SOFTWARE;
/*
* Init UDP socket
*/
void init_udp_send(egress_info_t *info) {
int set_if_err;
pthread_t thread;
egress_param_t *params = info->params;
egress_stat_t *stats = info->stats;
stats->packets_sent = 0;
init_tx_buffer(info);
info->sock_fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (info->sock_fd < 0) error(EXIT_FAILURE, errno, "Socket creation failed\n");
set_if_err = set_if(info);
if (set_if_err < 0) error(EXIT_FAILURE, errno, "Couldn't set interface\n");
if (setsockopt(info->sock_fd, SOL_SOCKET, SO_PRIORITY, &params->packet_priority,
sizeof(params->packet_priority)))
error(EXIT_FAILURE, errno, "Couldn't set socket priority\n");
if (setsockopt(info->sock_fd, SOL_SOCKET, SO_BINDTODEVICE, params->network_if,
strlen(params->network_if)))
error(EXIT_FAILURE, errno, "setsockopt SO_BINDTODEVICE failed\n");
if (params->use_etf) {
info->sk_txtime.clockid = CLOCK_REALTIME;
info->sk_txtime.flags = SOF_TXTIME_REPORT_ERRORS;
if (setsockopt(info->sock_fd, SOL_SOCKET, SO_TXTIME, &info->sk_txtime,
sizeof(info->sk_txtime)))
error(EXIT_FAILURE, errno, "setsockopt SO_TXTIME failed\n");
}
// Create poll thread
if (pthread_create(&thread, NULL, poll_thread, info))
error(EXIT_FAILURE, errno, "Couldn't create poll thread");
}
/*
* Sends udp packets
*/
void send_udp_packet(char *data, uint64_t txtime, egress_info_t * info) {
struct msghdr msg; // Message hardware, sent to the socket
struct cmsghdr *cmsg; // Control message hardware, for txtime
char control[CMSG_SPACE(sizeof(txtime))] = {}; // Stores txtime
struct iovec iov; // The iovec structures stores the TX buffer
int sendmsgerr;
struct sockaddr_in sin; // Server address
egress_param_t *params = info->params;
egress_stat_t *stats = info->stats;
stats->packets_sent++;
for(int i = 0; i < (int)params->tx_buffer_len; i++)
info->tx_buffer[i] = data[i];
memset(&sin, 0, sizeof(sin));
sin.sin_family = AF_INET;
sin.sin_addr.s_addr = inet_addr(params->server_ip);
sin.sin_port = htons(SERVER_PORT_INT);
iov.iov_base = info->tx_buffer;
iov.iov_len = params->tx_buffer_len;
memset(&msg, 0, sizeof(msg));
msg.msg_name = &sin;
msg.msg_namelen = sizeof(sin);
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
if (params->use_etf) {
// We specify the transmission time in the CMSG.
msg.msg_control = control;
msg.msg_controllen = sizeof(control);
cmsg = CMSG_FIRSTHDR(&msg);
cmsg->cmsg_level = SOL_SOCKET;
cmsg->cmsg_type = SCM_TXTIME;
cmsg->cmsg_len = CMSG_LEN(sizeof(uint64_t));
*((uint64_t *)CMSG_DATA(cmsg)) = txtime;
msg.msg_controllen = cmsg->cmsg_len;
}
sendmsgerr = sendmsg(info->sock_fd, &msg, 0);
if (sendmsgerr < 0)
error(EXIT_FAILURE, errno, "sendmsg failed, ret value: %d\n", sendmsgerr);
}
static void *poll_thread(void *p) {
egress_info_t * info = (egress_info_t *) p;
// Poll file descriptor
struct pollfd poll_fd = {.fd = info->sock_fd};
while (1) {
int ret;
ret = poll(&poll_fd, 1, -1);
if (ret == 1 && poll_fd.revents & POLLERR) {
process_error_queue(info);
}
}
return NULL;
}
static void process_error_queue(egress_info_t * info) {
int recv_ret;
// IO vector
unsigned char data_buffer[256]; // Buffer in io vector
struct iovec iov = {.iov_base = data_buffer, .iov_len = sizeof(data_buffer)};
// Control data, will store error or timestamps
unsigned char msg_control[CMSG_SPACE(sizeof(struct sock_extended_err)) +
CMSG_SPACE(sizeof(struct timespec))];
// Message hardware structure, containts IO vector and control message
// hardware
struct msghdr msg = {.msg_iov = &iov,
.msg_iovlen = 1,
.msg_control = msg_control,
.msg_controllen = sizeof(msg_control)};
struct cmsghdr *cmsg;
egress_stat_t *stats = info->stats;
// Timestamps and errors are received in the error queue
recv_ret = recvmsg(info->sock_fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT);
if (recv_ret == -1) {
fprintf(stderr, "recvmsg() failed\n");
return;
}
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING)
continue;
// If an error was received
else {
struct sock_extended_err *serr = (void *)CMSG_DATA(cmsg);
if (serr->ee_origin != SO_EE_ORIGIN_TXTIME) continue;
switch (serr->ee_code) {
case SO_EE_CODE_TXTIME_INVALID_PARAM:
stats->invalid_parameter++;
break;
case SO_EE_CODE_TXTIME_MISSED:
stats->missed_deadline++;
break;
default:
fprintf(stderr, "Uknown TxTime error\n");
}
}
}
}
// Sets the interface
static int set_if(egress_info_t * info) {
struct ifreq ifreq;
egress_param_t * params = info->params;
memset(&ifreq, 0, sizeof(ifreq));
strncpy(ifreq.ifr_name, params->network_if, sizeof(ifreq.ifr_name) - 1);
if (ioctl(info->sock_fd, SIOCGIFINDEX, &ifreq))
error(EXIT_FAILURE, errno, "ioctl SIOCGIFINDEX failed\n");
return ifreq.ifr_ifindex;
}
static void init_tx_buffer(egress_info_t * info) {
egress_param_t * params = info->params;
if (params->tx_buffer_len < 1) {
fprintf(stderr, "tx buffer length should be greater than 1\n");
exit(EXIT_FAILURE);
}
info->tx_buffer = malloc(params->tx_buffer_len);
}
src/motor-control/src/send_packet.h 0 → 100644
View file @ 645e9fb8
#ifndef SEND_PACKET_H
#define SEND_PACKET_H
#include <arpa/inet.h>
#include <errno.h>
#include <error.h>
#include <fcntl.h>
#include <ifaddrs.h>
#include <inttypes.h>
#include <linux/errqueue.h>
#include <linux/ethtool.h>
#include <linux/net_tstamp.h>
#include <linux/sockios.h>
#include <net/if.h>
#include <netdb.h>
#include <netinet/in.h>
#include <poll.h>
#include <pthread.h>
#include <sched.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include "common.h"
typedef struct egress_param {
int packet_priority;
size_t tx_buffer_len;
char server_ip[45];
char network_if[16];
int use_etf;
} egress_param_t;
typedef struct egress_stat {
uint64_t packets_sent;
uint64_t high_kernel_latency;
uint64_t invalid_parameter;
uint64_t missed_deadline;
int min_interval;
int avg_interval;
int max_interval;
} egress_stat_t;
typedef struct egress_info {
int sock_fd;
struct sock_txtime sk_txtime;
char *tx_buffer;
egress_param_t *params;
egress_stat_t *stats;
} egress_info_t;
void init_udp_send(egress_info_t *info);
void send_udp_packet(char *data, uint64_t txtime, egress_info_t * info);
#endif
src/motor-control/src/server.c 0 → 100644
View file @ 645e9fb8
/*
* Real time packet receiving server
*
* Large portions taken from cyclictest
*
*/
#define _GNU_SOURCE
#include <arpa/inet.h>
#include <errno.h>
#include <error.h>
#include <inttypes.h>
#include <limits.h>
#include <netdb.h>
#include <netinet/in.h>
#include <pthread.h>
#include <sched.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <unistd.h>
#include "common.h"
#include "gpio.h"
#include "recv_packet.h"
#define ERROR_MARGIN_NS 400000
// Structs
enum TSNTask { RECV_PACKET_TASK, RTT_TASK, XDP_TASK };
typedef struct thread_param {
int priority;
int affinity_cpu;
uint64_t latency_threshold;
uint64_t start_ts;
} thread_param_t;
typedef struct main_params {
int refresh_rate;
int verbose;
} main_param_t;
static void process_options(int argc, char *argv[]);
static void sighand(int sig_num);
// Static variables
static main_param_t main_params;
static thread_param_t thread_params;
static ingress_param_t ingress_params;
static ingress_stat_t ingress_stats = {.min_interval = INT_MAX};
static enum TSNTask tsn_task;
static struct timespec measures_start;
static struct timespec measures_end;
static struct timespec emit_signal_next;
static pthread_mutex_t emit_signal_mutex;
static pthread_cond_t emit_signal_ts_received;
static int interval_us;
static int gpio86_fd;
static int gpio84_fd;
static int gpio86_state;
static int gpio84_state;
static int received_reverse_motor = 0;
static void help(char *argv[]) {
printf(
"Usage: %s [-f IF -a CPU -p PRIO -r USEC] [-v] [-x]\n\n"
" -h Show help\n"
" -f IF Set the network interface to be used\n"
" -a CPU Pin the real time thread to CPU\n"
" -p PRIO RT thread priority\n"
" -r USEC non-RT main thread refresh interval\n"
" -s NS Common start time reference\n"
" -x Use AF_XDP sockets\n"
" -v Verbose\n"
"\n",
argv[0]);
}
static void *emit_signal_thread(void *p) {
(void)p;
cpu_set_t mask;
struct timespec current;
struct timespec previous_emit, previous_ts;
int ret;
int reverse_motor = 0;
// Set thread CPU affinity
if (thread_params.affinity_cpu) {
CPU_ZERO(&mask);
CPU_SET(thread_params.affinity_cpu, &mask);
if (sched_setaffinity(0, sizeof(mask), &mask))
error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n",
thread_params.affinity_cpu);
}
pthread_mutex_lock(&emit_signal_mutex);
for (int i = 0;;i++) {
pthread_cond_wait(&emit_signal_ts_received, &emit_signal_mutex);
if(received_reverse_motor) {
received_reverse_motor = 0;
reverse_motor = 1;
continue;
}
ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &emit_signal_next, NULL);
if (ret) {
fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
exit(EXIT_FAILURE);
}
if(reverse_motor) {
gpio84_state = toggle_gpio(gpio84_fd, gpio84_state);
reverse_motor = 0;
}
else {
gpio86_state = toggle_gpio(gpio86_fd, gpio86_state);
usleep(3);
gpio86_state = toggle_gpio(gpio86_fd, gpio86_state);
}
clock_gettime(CLOCK_REALTIME, &current);
previous_emit = current;
previous_ts = emit_signal_next;
}
pthread_mutex_unlock(&emit_signal_mutex);
pthread_exit(NULL);
}
/* Real-time thread:
* - Measures intervals between packet receptions
*/
static void *tsn_thread(void *p) {
(void)p;
struct timespec current, previous, next;
cpu_set_t mask;
// Set thread CPU affinity
if (thread_params.affinity_cpu) {
CPU_ZERO(&mask);
CPU_SET(thread_params.affinity_cpu, &mask);
if (sched_setaffinity(0, sizeof(mask), &mask))
error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n",
thread_params.affinity_cpu);
}
clock_gettime(CLOCK_MONOTONIC, &measures_start);
if (tsn_task == XDP_TASK) setup_poll_fd();
if (thread_params.start_ts) {
clock_gettime(CLOCK_REALTIME, &next);
if (thread_params.start_ts < ts_to_uint(next)) {
fprintf(stderr, "start timestamp is in the past, aborting...\n");
exit(EXIT_FAILURE);
}
if (thread_params.start_ts > (ts_to_uint(next) + UINT64_C(3600000000000))) {
fprintf(stderr, "start timestamp is too high, aborting...\n");
exit(EXIT_FAILURE);
}
next = uint_to_ts(thread_params.start_ts);
}
// Packet receiving loop
for (ingress_stats.packets_received = 0;; ingress_stats.packets_received++) {
// Receive UDP or XDP packet
if (tsn_task == RECV_PACKET_TASK)
recv_udp_packet();
else
recv_xdp_packet();
// Get time for statistics
clock_gettime(CLOCK_REALTIME, &current);
recv_xdp_cleanup();
interval_us = calcdiff_ns(current, previous) / 1000;
uint64_t emit_signal_t;
if(tsn_task == XDP_TASK)
emit_signal_t = decode(ingress_stats.xdp_data);
else
emit_signal_t = decode(ingress_stats.data);
if(emit_signal_t < UINT64_C(1576800000000000000) && emit_signal_t != 17)
continue;
pthread_mutex_lock(&emit_signal_mutex);
if(emit_signal_t == 17)
received_reverse_motor = 1;
else
emit_signal_next = uint_to_ts(emit_signal_t);
pthread_cond_signal(&emit_signal_ts_received);
pthread_mutex_unlock(&emit_signal_mutex);
// Update stats
if (ingress_stats.packets_received) {
ingress_stats.min_interval =
_min_(interval_us, ingress_stats.min_interval);
ingress_stats.max_interval =
_max_(interval_us, ingress_stats.max_interval);
ingress_stats.avg_interval =
(ingress_stats.avg_interval * ingress_stats.packets_received +
interval_us) /
(ingress_stats.packets_received + 1);
}
previous = current;
}
return NULL;
}
static void create_thread(void *(*thread_function)(void *)) {
pthread_t thread;
pthread_attr_t attr;
struct sched_param param;
// Initialize pthread attributes (default values)
if (pthread_attr_init(&attr)) {
fprintf(stderr, "init pthread attributes failed\n");
exit(EXIT_FAILURE);
}
// Set a specific stack size
if (pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN)) {
fprintf(stderr, "pthread setstacksize failed\n");
exit(EXIT_FAILURE);
}
// Set scheduler policy and priority of pthread
if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO)) {
fprintf(stderr, "pthread setschedpolicy failed\n");
exit(EXIT_FAILURE);
}
param.sched_priority = thread_params.priority;
if (pthread_attr_setschedparam(&attr, &param)) {
fprintf(stderr, "pthread setschedparam failed\n");
exit(EXIT_FAILURE);
}
// Use scheduling parameters of attr
if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) {
fprintf(stderr, "pthread setinheritsched failed\n");
exit(EXIT_FAILURE);
}
// Create the real time thread
if (pthread_create(&thread, &attr, thread_function, NULL))
error(EXIT_FAILURE, errno, "Couldn't create packet receiving thread");
}
/* Main thread:
* - Has non-real time priority
* - Handles the IO and creates real time threads
*/
int main(int argc, char *argv[]) {
// Initial values
ingress_stats.min_interval = INT_MAX;
ingress_stats.avg_interval = 0;
ingress_stats.max_interval = 0;
ingress_stats.packets_received = 0;
// Default configuration values
thread_params.priority = 98;
thread_params.affinity_cpu = 0;
thread_params.start_ts = 0;
main_params.refresh_rate = 50000;
main_params.verbose = 0;
tsn_task = RECV_PACKET_TASK;
ingress_params.tx_buffer_len = 1024;
// Process bash options
process_options(argc, argv);
set_latency_target();
// Catch breaks with sighand to exit XDP
init_signals(sighand);
// Initialize the XDP or UDP packet receiving socket
if (tsn_task == XDP_TASK)
init_xdp_recv(&ingress_params, &ingress_stats);
else
init_udp_recv(&ingress_params, &ingress_stats);
pthread_mutex_init(&emit_signal_mutex, NULL);
pthread_cond_init(&emit_signal_ts_received, NULL);
gpio86_state = enable_gpio(&gpio86_fd, 86);
gpio84_state = enable_gpio(&gpio84_fd, 84);
create_thread(tsn_thread);
create_thread(emit_signal_thread);
// Verbose loop
for (;;) {
usleep(main_params.refresh_rate);
// Stats printing
if (main_params.verbose && ingress_stats.packets_received > 1) {
int jitter = ingress_stats.max_interval - ingress_stats.min_interval;
printf("%9" PRIu64 ": J: %5d, I (10us): %3d %3d %3d [%3d]",
ingress_stats.packets_received, jitter,
ingress_stats.min_interval / 10, ingress_stats.avg_interval / 10,
ingress_stats.max_interval / 10, (int)ingress_stats.high_jitter);
printf("\n");
printf("\033[%dA", 1);
}
}
pthread_mutex_destroy(&emit_signal_mutex);
pthread_cond_destroy(&emit_signal_ts_received);
close_xdp_socket();
}
static void sighand(int sig_num) {
(void)sig_num;
if (tsn_task == XDP_TASK) {
close_xdp_socket();
}
if (ingress_stats.high_jitter)
fprintf(stderr, "%d packets were lost\n", ((int)ingress_stats.high_jitter));
exit(EXIT_SUCCESS);
}
/* Process bash options
*/
static void process_options(int argc, char *argv[]) {
int network_if_specified = 0;
for (;;) {
int c = getopt(argc, argv, "a:s:f:hp:r:vx");
if (c == -1) break;
switch (c) {
case 'a':
thread_params.affinity_cpu = atoi(optarg);
break;
case 's':
thread_params.start_ts = strtoull(optarg, NULL, 10);
break;
case 'f':
network_if_specified = 1;
strcpy(ingress_params.network_if, optarg);
break;
case 'h':
help(argv);
exit(EXIT_SUCCESS);
break;
case 'p':
thread_params.priority = atoi(optarg);
break;
case 'r':
main_params.refresh_rate = atoi(optarg);
break;
case 'v':
main_params.verbose = 1;
break;
case 'x':
tsn_task = XDP_TASK;
break;
}
}
if (!network_if_specified) {
fprintf(stderr, "You need to specifiy an network interface\n");
help(argv);
exit(EXIT_FAILURE);
}
if (argc != optind) {
fprintf(stderr, "Too many arguments\n");
help(argv);
exit(EXIT_FAILURE);
}
}
src/motor-control/src/xdp_kern.c 0 → 100644
View file @ 645e9fb8
#define KBUILD_MODNAME "blub"
#include <linux/bpf.h>
#include <linux/if_ether.h>
#include <linux/if_link.h>
#include <linux/if_xdp.h>
#include <linux/in.h>
#include <linux/ip.h>
#include <linux/udp.h>
#include "bpf_helpers.h"
#include "bpf_endian.h"
#include "parsing_helpers.h"
#define UDP_PORT 50000
#define bpf_printk(fmt, ...) \
({ \
char ____fmt[] = fmt; \
bpf_trace_printk(____fmt, sizeof(____fmt), ##__VA_ARGS__); \
})
struct bpf_map_def SEC("maps") xsks_map = {
.type = BPF_MAP_TYPE_XSKMAP,
.key_size = sizeof(int),
.value_size = sizeof(int),
.max_entries = 64,
};
SEC("xdp_sock")
int xdp_sock_prog(struct xdp_md *ctx)
{
int eth_type, ip_type, index;
struct ethhdr *eth;
struct iphdr *iphdr;
struct ipv6hdr *ipv6hdr;
void *data_end = (void *)(long)ctx->data_end;
void *data = (void *)(long)ctx->data;
struct hdr_cursor nh = { .pos = data };
index = ctx->rx_queue_index;
eth_type = parse_ethhdr(&nh, data_end, &eth);
if (eth_type < 0)
return XDP_PASS;
if (eth_type == bpf_htons(ETH_P_IP)) {
ip_type = parse_iphdr(&nh, data_end, &iphdr);
} else if (eth_type == bpf_htons(ETH_P_IPV6)) {
ip_type = parse_ip6hdr(&nh, data_end, &ipv6hdr);
} else {
return XDP_PASS;
}
// only support UDP for now
if (ip_type != IPPROTO_UDP)
return XDP_PASS;
struct udphdr *udphdr;
// don't mess with ports outside our purview, if specified
if (parse_udphdr(&nh, data_end, &udphdr) < 0)
return XDP_PASS;
if (bpf_ntohs(udphdr->dest) != UDP_PORT)
return XDP_PASS;
/* If socket bound to rx_queue then redirect to user space */
if (bpf_map_lookup_elem(&xsks_map, &index))
return bpf_redirect_map(&xsks_map, index, 0);
/* Else pass to Linux' network stack */
return XDP_PASS;
}
char _license[] SEC("license") = "GPL";
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7