1#!/bin/bash
2# SPDX-License-Identifier: GPL-2.0
3# Copyright 2021-2022 NXP
4
5REQUIRE_ISOCHRON=${REQUIRE_ISOCHRON:=yes}
6REQUIRE_LINUXPTP=${REQUIRE_LINUXPTP:=yes}
7
8# Tunables
9UTC_TAI_OFFSET=37
10ISOCHRON_CPU=1
11
12if [[ "$REQUIRE_ISOCHRON" = "yes" ]]; then
13	# https://github.com/vladimiroltean/tsn-scripts
14	# WARNING: isochron versions pre-1.0 are unstable,
15	# always use the latest version
16	require_command isochron
17fi
18if [[ "$REQUIRE_LINUXPTP" = "yes" ]]; then
19	require_command phc2sys
20	require_command ptp4l
21fi
22
23phc2sys_start()
24{
25	local uds_address=$1
26	local extra_args=""
27
28	if ! [ -z "${uds_address}" ]; then
29		extra_args="${extra_args} -z ${uds_address}"
30	fi
31
32	phc2sys_log="$(mktemp)"
33
34	chrt -f 10 phc2sys -m \
35		-a -rr \
36		--step_threshold 0.00002 \
37		--first_step_threshold 0.00002 \
38		${extra_args} \
39		> "${phc2sys_log}" 2>&1 &
40	phc2sys_pid=$!
41
42	echo "phc2sys logs to ${phc2sys_log} and has pid ${phc2sys_pid}"
43
44	sleep 1
45}
46
47phc2sys_stop()
48{
49	{ kill ${phc2sys_pid} && wait ${phc2sys_pid}; } 2> /dev/null
50	rm "${phc2sys_log}" 2> /dev/null
51}
52
53# Replace space separators from interface list with underscores
54if_names_to_label()
55{
56	local if_name_list="$1"
57
58	echo "${if_name_list/ /_}"
59}
60
61ptp4l_start()
62{
63	local if_names="$1"
64	local slave_only=$2
65	local uds_address=$3
66	local log="ptp4l_log_$(if_names_to_label ${if_names})"
67	local pid="ptp4l_pid_$(if_names_to_label ${if_names})"
68	local extra_args=""
69
70	for if_name in ${if_names}; do
71		extra_args="${extra_args} -i ${if_name}"
72	done
73
74	if [ "${slave_only}" = true ]; then
75		extra_args="${extra_args} -s"
76	fi
77
78	# declare dynamic variables ptp4l_log_${if_name} and ptp4l_pid_${if_name}
79	# as global, so that they can be referenced later
80	declare -g "${log}=$(mktemp)"
81
82	chrt -f 10 ptp4l -m -2 -P \
83		--step_threshold 0.00002 \
84		--first_step_threshold 0.00002 \
85		--tx_timestamp_timeout 100 \
86		--uds_address="${uds_address}" \
87		${extra_args} \
88		> "${!log}" 2>&1 &
89	declare -g "${pid}=$!"
90
91	echo "ptp4l for interfaces ${if_names} logs to ${!log} and has pid ${!pid}"
92
93	sleep 1
94}
95
96ptp4l_stop()
97{
98	local if_names="$1"
99	local log="ptp4l_log_$(if_names_to_label ${if_names})"
100	local pid="ptp4l_pid_$(if_names_to_label ${if_names})"
101
102	{ kill ${!pid} && wait ${!pid}; } 2> /dev/null
103	rm "${!log}" 2> /dev/null
104}
105
106cpufreq_max()
107{
108	local cpu=$1
109	local freq="cpu${cpu}_freq"
110	local governor="cpu${cpu}_governor"
111
112	# Kernel may be compiled with CONFIG_CPU_FREQ disabled
113	if ! [ -d /sys/bus/cpu/devices/cpu${cpu}/cpufreq ]; then
114		return
115	fi
116
117	# declare dynamic variables cpu${cpu}_freq and cpu${cpu}_governor as
118	# global, so they can be referenced later
119	declare -g "${freq}=$(cat /sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_min_freq)"
120	declare -g "${governor}=$(cat /sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_governor)"
121
122	cat /sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_max_freq > \
123		/sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_min_freq
124	echo -n "performance" > \
125		/sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_governor
126}
127
128cpufreq_restore()
129{
130	local cpu=$1
131	local freq="cpu${cpu}_freq"
132	local governor="cpu${cpu}_governor"
133
134	if ! [ -d /sys/bus/cpu/devices/cpu${cpu}/cpufreq ]; then
135		return
136	fi
137
138	echo "${!freq}" > /sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_min_freq
139	echo -n "${!governor}" > \
140		/sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_governor
141}
142
143isochron_recv_start()
144{
145	local if_name=$1
146	local uds=$2
147	local stats_port=$3
148	local extra_args=$4
149	local pid="isochron_pid_${stats_port}"
150
151	if ! [ -z "${uds}" ]; then
152		extra_args="${extra_args} --unix-domain-socket ${uds}"
153	fi
154
155	isochron rcv \
156		--interface ${if_name} \
157		--sched-priority 98 \
158		--sched-fifo \
159		--utc-tai-offset ${UTC_TAI_OFFSET} \
160		--stats-port ${stats_port} \
161		--quiet \
162		${extra_args} & \
163	declare -g "${pid}=$!"
164
165	sleep 1
166}
167
168isochron_recv_stop()
169{
170	local stats_port=$1
171	local pid="isochron_pid_${stats_port}"
172
173	{ kill ${!pid} && wait ${!pid}; } 2> /dev/null
174}
175
176isochron_do()
177{
178	local sender_if_name=$1; shift
179	local receiver_if_name=$1; shift
180	local sender_uds=$1; shift
181	local receiver_uds=$1; shift
182	local base_time=$1; shift
183	local cycle_time=$1; shift
184	local shift_time=$1; shift
185	local num_pkts=$1; shift
186	local vid=$1; shift
187	local priority=$1; shift
188	local dst_ip=$1; shift
189	local isochron_dat=$1; shift
190	local extra_args=""
191	local receiver_extra_args=""
192	local vrf="$(master_name_get ${sender_if_name})"
193	local use_l2="true"
194
195	if ! [ -z "${dst_ip}" ]; then
196		use_l2="false"
197	fi
198
199	if ! [ -z "${vrf}" ]; then
200		dst_ip="${dst_ip}%${vrf}"
201	fi
202
203	if ! [ -z "${vid}" ]; then
204		vid="--vid=${vid}"
205	fi
206
207	if [ -z "${receiver_uds}" ]; then
208		extra_args="${extra_args} --omit-remote-sync"
209	fi
210
211	if ! [ -z "${shift_time}" ]; then
212		extra_args="${extra_args} --shift-time=${shift_time}"
213	fi
214
215	if [ "${use_l2}" = "true" ]; then
216		extra_args="${extra_args} --l2 --etype=0xdead ${vid}"
217		receiver_extra_args="--l2 --etype=0xdead"
218	else
219		extra_args="${extra_args} --l4 --ip-destination=${dst_ip}"
220		receiver_extra_args="--l4"
221	fi
222
223	cpufreq_max ${ISOCHRON_CPU}
224
225	isochron_recv_start "${h2}" "${receiver_uds}" 5000 "${receiver_extra_args}"
226
227	isochron send \
228		--interface ${sender_if_name} \
229		--unix-domain-socket ${sender_uds} \
230		--priority ${priority} \
231		--base-time ${base_time} \
232		--cycle-time ${cycle_time} \
233		--num-frames ${num_pkts} \
234		--frame-size 64 \
235		--txtime \
236		--utc-tai-offset ${UTC_TAI_OFFSET} \
237		--cpu-mask $((1 << ${ISOCHRON_CPU})) \
238		--sched-fifo \
239		--sched-priority 98 \
240		--client 127.0.0.1 \
241		--sync-threshold 5000 \
242		--output-file ${isochron_dat} \
243		${extra_args} \
244		--quiet
245
246	isochron_recv_stop 5000
247
248	cpufreq_restore ${ISOCHRON_CPU}
249}
250