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 if_name=$1
26	local uds_address=$2
27	local extra_args=""
28
29	if ! [ -z "${uds_address}" ]; then
30		extra_args="${extra_args} -z ${uds_address}"
31	fi
32
33	phc2sys_log="$(mktemp)"
34
35	chrt -f 10 phc2sys -m \
36		-c ${if_name} \
37		-s CLOCK_REALTIME \
38		-O ${UTC_TAI_OFFSET} \
39		--step_threshold 0.00002 \
40		--first_step_threshold 0.00002 \
41		${extra_args} \
42		> "${phc2sys_log}" 2>&1 &
43	phc2sys_pid=$!
44
45	echo "phc2sys logs to ${phc2sys_log} and has pid ${phc2sys_pid}"
46
47	sleep 1
48}
49
50phc2sys_stop()
51{
52	{ kill ${phc2sys_pid} && wait ${phc2sys_pid}; } 2> /dev/null
53	rm "${phc2sys_log}" 2> /dev/null
54}
55
56ptp4l_start()
57{
58	local if_name=$1
59	local slave_only=$2
60	local uds_address=$3
61	local log="ptp4l_log_${if_name}"
62	local pid="ptp4l_pid_${if_name}"
63	local extra_args=""
64
65	if [ "${slave_only}" = true ]; then
66		extra_args="${extra_args} -s"
67	fi
68
69	# declare dynamic variables ptp4l_log_${if_name} and ptp4l_pid_${if_name}
70	# as global, so that they can be referenced later
71	declare -g "${log}=$(mktemp)"
72
73	chrt -f 10 ptp4l -m -2 -P \
74		-i ${if_name} \
75		--step_threshold 0.00002 \
76		--first_step_threshold 0.00002 \
77		--tx_timestamp_timeout 100 \
78		--uds_address="${uds_address}" \
79		${extra_args} \
80		> "${!log}" 2>&1 &
81	declare -g "${pid}=$!"
82
83	echo "ptp4l for interface ${if_name} logs to ${!log} and has pid ${!pid}"
84
85	sleep 1
86}
87
88ptp4l_stop()
89{
90	local if_name=$1
91	local log="ptp4l_log_${if_name}"
92	local pid="ptp4l_pid_${if_name}"
93
94	{ kill ${!pid} && wait ${!pid}; } 2> /dev/null
95	rm "${!log}" 2> /dev/null
96}
97
98cpufreq_max()
99{
100	local cpu=$1
101	local freq="cpu${cpu}_freq"
102	local governor="cpu${cpu}_governor"
103
104	# Kernel may be compiled with CONFIG_CPU_FREQ disabled
105	if ! [ -d /sys/bus/cpu/devices/cpu${cpu}/cpufreq ]; then
106		return
107	fi
108
109	# declare dynamic variables cpu${cpu}_freq and cpu${cpu}_governor as
110	# global, so they can be referenced later
111	declare -g "${freq}=$(cat /sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_min_freq)"
112	declare -g "${governor}=$(cat /sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_governor)"
113
114	cat /sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_max_freq > \
115		/sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_min_freq
116	echo -n "performance" > \
117		/sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_governor
118}
119
120cpufreq_restore()
121{
122	local cpu=$1
123	local freq="cpu${cpu}_freq"
124	local governor="cpu${cpu}_governor"
125
126	if ! [ -d /sys/bus/cpu/devices/cpu${cpu}/cpufreq ]; then
127		return
128	fi
129
130	echo "${!freq}" > /sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_min_freq
131	echo -n "${!governor}" > \
132		/sys/bus/cpu/devices/cpu${cpu}/cpufreq/scaling_governor
133}
134
135isochron_recv_start()
136{
137	local if_name=$1
138	local uds=$2
139	local extra_args=$3
140
141	if ! [ -z "${uds}" ]; then
142		extra_args="--unix-domain-socket ${uds}"
143	fi
144
145	isochron rcv \
146		--interface ${if_name} \
147		--sched-priority 98 \
148		--sched-fifo \
149		--utc-tai-offset ${UTC_TAI_OFFSET} \
150		--quiet \
151		${extra_args} & \
152	isochron_pid=$!
153
154	sleep 1
155}
156
157isochron_recv_stop()
158{
159	{ kill ${isochron_pid} && wait ${isochron_pid}; } 2> /dev/null
160}
161
162isochron_do()
163{
164	local sender_if_name=$1; shift
165	local receiver_if_name=$1; shift
166	local sender_uds=$1; shift
167	local receiver_uds=$1; shift
168	local base_time=$1; shift
169	local cycle_time=$1; shift
170	local shift_time=$1; shift
171	local num_pkts=$1; shift
172	local vid=$1; shift
173	local priority=$1; shift
174	local dst_ip=$1; shift
175	local isochron_dat=$1; shift
176	local extra_args=""
177	local receiver_extra_args=""
178	local vrf="$(master_name_get ${sender_if_name})"
179	local use_l2="true"
180
181	if ! [ -z "${dst_ip}" ]; then
182		use_l2="false"
183	fi
184
185	if ! [ -z "${vrf}" ]; then
186		dst_ip="${dst_ip}%${vrf}"
187	fi
188
189	if ! [ -z "${vid}" ]; then
190		vid="--vid=${vid}"
191	fi
192
193	if [ -z "${receiver_uds}" ]; then
194		extra_args="${extra_args} --omit-remote-sync"
195	fi
196
197	if ! [ -z "${shift_time}" ]; then
198		extra_args="${extra_args} --shift-time=${shift_time}"
199	fi
200
201	if [ "${use_l2}" = "true" ]; then
202		extra_args="${extra_args} --l2 --etype=0xdead ${vid}"
203		receiver_extra_args="--l2 --etype=0xdead"
204	else
205		extra_args="${extra_args} --l4 --ip-destination=${dst_ip}"
206		receiver_extra_args="--l4"
207	fi
208
209	cpufreq_max ${ISOCHRON_CPU}
210
211	isochron_recv_start "${h2}" "${receiver_uds}" "${receiver_extra_args}"
212
213	isochron send \
214		--interface ${sender_if_name} \
215		--unix-domain-socket ${sender_uds} \
216		--priority ${priority} \
217		--base-time ${base_time} \
218		--cycle-time ${cycle_time} \
219		--num-frames ${num_pkts} \
220		--frame-size 64 \
221		--txtime \
222		--utc-tai-offset ${UTC_TAI_OFFSET} \
223		--cpu-mask $((1 << ${ISOCHRON_CPU})) \
224		--sched-fifo \
225		--sched-priority 98 \
226		--client 127.0.0.1 \
227		--sync-threshold 5000 \
228		--output-file ${isochron_dat} \
229		${extra_args} \
230		--quiet
231
232	isochron_recv_stop
233
234	cpufreq_restore ${ISOCHRON_CPU}
235}
236