1 /*
2 * Copyright (c) 2022 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/init.h>
8 #include <zephyr/logging/log.h>
9 #include <zephyr/net/socket.h>
10
11 #include "zperf_internal.h"
12 #include "zperf_session.h"
13
14 LOG_MODULE_REGISTER(net_zperf, CONFIG_NET_ZPERF_LOG_LEVEL);
15
16 /* Get some useful debug routings from net_private.h, requires
17 * that NET_LOG_ENABLED is set.
18 */
19 #define NET_LOG_ENABLED 1
20 #include "net_private.h"
21
22 #include "ipv6.h" /* to get infinite lifetime */
23
24 static struct sockaddr_in6 in6_addr_my = {
25 .sin6_family = AF_INET6,
26 .sin6_port = htons(MY_SRC_PORT),
27 };
28
29 static struct sockaddr_in in4_addr_my = {
30 .sin_family = AF_INET,
31 .sin_port = htons(MY_SRC_PORT),
32 };
33
zperf_get_sin6(void)34 struct sockaddr_in6 *zperf_get_sin6(void)
35 {
36 return &in6_addr_my;
37 }
38
zperf_get_sin(void)39 struct sockaddr_in *zperf_get_sin(void)
40 {
41 return &in4_addr_my;
42 }
43
44 #define ZPERF_WORK_Q_THREAD_PRIORITY \
45 CLAMP(CONFIG_ZPERF_WORK_Q_THREAD_PRIORITY, \
46 K_HIGHEST_APPLICATION_THREAD_PRIO, \
47 K_LOWEST_APPLICATION_THREAD_PRIO)
48
49 #if defined(CONFIG_ZPERF_SESSION_PER_THREAD)
50 static K_EVENT_DEFINE(start_event);
51
52 #define CREATE_WORK_Q(i, _) \
53 static struct k_work_q zperf_work_q_##i; \
54 static K_KERNEL_STACK_DEFINE(zperf_work_q_stack_##i, \
55 CONFIG_ZPERF_WORK_Q_STACK_SIZE)
56
57 /* Both UDP and TCP can have separate sessions so multiply by 2 */
58 #if defined(CONFIG_NET_UDP) && defined(CONFIG_NET_TCP)
59 #define MAX_SESSION_COUNT UTIL_X2(CONFIG_NET_ZPERF_MAX_SESSIONS)
60 #define SESSION_INDEX CONFIG_NET_ZPERF_MAX_SESSIONS
61 #else
62 #define MAX_SESSION_COUNT CONFIG_NET_ZPERF_MAX_SESSIONS
63 #define SESSION_INDEX 0
64 #endif
65
66 LISTIFY(MAX_SESSION_COUNT, CREATE_WORK_Q, (;), _);
67
68 #define SET_WORK_Q(i, _) \
69 [i] = { \
70 .queue = &zperf_work_q_##i, \
71 .stack = zperf_work_q_stack_##i, \
72 .stack_size = K_THREAD_STACK_SIZEOF(zperf_work_q_stack_##i), \
73 }
74
75 static struct zperf_work zperf_work_q[] = {
76 LISTIFY(MAX_SESSION_COUNT, SET_WORK_Q, (,), _)
77 };
78
get_queue(enum session_proto proto,int session_id)79 struct zperf_work *get_queue(enum session_proto proto, int session_id)
80 {
81 if (session_id < 0 || session_id >= CONFIG_NET_ZPERF_MAX_SESSIONS) {
82 return NULL;
83 }
84
85 if (proto < 0 || proto >= SESSION_PROTO_END) {
86 return NULL;
87 }
88
89 NET_DBG("%s using queue %d for session %d\n",
90 proto == SESSION_UDP ? "UDP" : "TCP",
91 proto * SESSION_INDEX + session_id,
92 session_id);
93
94
95 return &zperf_work_q[proto * SESSION_INDEX + session_id];
96 }
97
start_jobs(void)98 void start_jobs(void)
99 {
100 k_event_set(&start_event, START_EVENT);
101 }
102 #else /* CONFIG_ZPERF_SESSION_PER_THREAD */
103
104 K_THREAD_STACK_DEFINE(zperf_work_q_stack, CONFIG_ZPERF_WORK_Q_STACK_SIZE);
105 static struct k_work_q zperf_work_q;
106
107 #endif /* CONFIG_ZPERF_SESSION_PER_THREAD */
108
zperf_get_ipv6_addr(char * host,char * prefix_str,struct in6_addr * addr)109 int zperf_get_ipv6_addr(char *host, char *prefix_str, struct in6_addr *addr)
110 {
111 struct net_if_ipv6_prefix *prefix;
112 struct net_if_addr *ifaddr;
113 int prefix_len;
114 int ret;
115
116 if (!host) {
117 return -EINVAL;
118 }
119
120 ret = net_addr_pton(AF_INET6, host, addr);
121 if (ret < 0) {
122 return -EINVAL;
123 }
124
125 prefix_len = strtoul(prefix_str, NULL, 10);
126
127 ifaddr = net_if_ipv6_addr_add(net_if_get_default(),
128 addr, NET_ADDR_MANUAL, 0);
129 if (!ifaddr) {
130 NET_ERR("Cannot set IPv6 address %s", host);
131 return -EINVAL;
132 }
133
134 prefix = net_if_ipv6_prefix_add(net_if_get_default(),
135 addr, prefix_len,
136 NET_IPV6_ND_INFINITE_LIFETIME);
137 if (!prefix) {
138 NET_ERR("Cannot set IPv6 prefix %s", prefix_str);
139 return -EINVAL;
140 }
141
142 return 0;
143 }
144
145
zperf_get_ipv4_addr(char * host,struct in_addr * addr)146 int zperf_get_ipv4_addr(char *host, struct in_addr *addr)
147 {
148 struct net_if_addr *ifaddr;
149 int ret;
150
151 if (!host) {
152 return -EINVAL;
153 }
154
155 ret = net_addr_pton(AF_INET, host, addr);
156 if (ret < 0) {
157 return -EINVAL;
158 }
159
160 ifaddr = net_if_ipv4_addr_add(net_if_get_default(),
161 addr, NET_ADDR_MANUAL, 0);
162 if (!ifaddr) {
163 NET_ERR("Cannot set IPv4 address %s", host);
164 return -EINVAL;
165 }
166
167 return 0;
168 }
169
zperf_prepare_upload_sock(const struct sockaddr * peer_addr,uint8_t tos,int priority,int tcp_nodelay,int proto)170 int zperf_prepare_upload_sock(const struct sockaddr *peer_addr, uint8_t tos,
171 int priority, int tcp_nodelay, int proto)
172 {
173 socklen_t addrlen = peer_addr->sa_family == AF_INET6 ?
174 sizeof(struct sockaddr_in6) :
175 sizeof(struct sockaddr_in);
176 int type = (proto == IPPROTO_UDP) ? SOCK_DGRAM : SOCK_STREAM;
177 int sock = -1;
178 int ret;
179
180 switch (peer_addr->sa_family) {
181 case AF_INET:
182 if (!IS_ENABLED(CONFIG_NET_IPV4)) {
183 NET_ERR("IPv4 not available.");
184 return -EINVAL;
185 }
186
187 sock = zsock_socket(AF_INET, type, proto);
188 if (sock < 0) {
189 NET_ERR("Cannot create IPv4 network socket (%d)",
190 errno);
191 return -errno;
192 }
193
194 if (tos > 0) {
195 if (zsock_setsockopt(sock, IPPROTO_IP, IP_TOS,
196 &tos, sizeof(tos)) != 0) {
197 NET_WARN("Failed to set IP_TOS socket option. "
198 "Please enable CONFIG_NET_CONTEXT_DSCP_ECN.");
199 }
200 }
201
202 break;
203
204 case AF_INET6:
205 if (!IS_ENABLED(CONFIG_NET_IPV6)) {
206 NET_ERR("IPv6 not available.");
207 return -EINVAL;
208 }
209
210 sock = zsock_socket(AF_INET6, type, proto);
211 if (sock < 0) {
212 NET_ERR("Cannot create IPv6 network socket (%d)",
213 errno);
214 return -errno;
215 }
216
217 if (tos >= 0) {
218 if (zsock_setsockopt(sock, IPPROTO_IPV6, IPV6_TCLASS,
219 &tos, sizeof(tos)) != 0) {
220 NET_WARN("Failed to set IPV6_TCLASS socket option. "
221 "Please enable CONFIG_NET_CONTEXT_DSCP_ECN.");
222 }
223 }
224
225 break;
226
227 default:
228 LOG_ERR("Invalid address family (%d)", peer_addr->sa_family);
229 return -EINVAL;
230 }
231
232 if (IS_ENABLED(CONFIG_NET_CONTEXT_PRIORITY) && priority >= 0) {
233 uint8_t prio = priority;
234
235 if (!IS_ENABLED(CONFIG_NET_ALLOW_ANY_PRIORITY) &&
236 (prio >= NET_MAX_PRIORITIES)) {
237 NET_ERR("Priority %d is too large, maximum allowed is %d",
238 prio, NET_MAX_PRIORITIES - 1);
239 ret = -EINVAL;
240 goto error;
241 }
242
243 if (zsock_setsockopt(sock, SOL_SOCKET, SO_PRIORITY,
244 &prio,
245 sizeof(prio)) != 0) {
246 NET_WARN("Failed to set SOL_SOCKET - SO_PRIORITY socket option.");
247 ret = -errno;
248 goto error;
249 }
250 }
251
252 if (proto == IPPROTO_TCP && tcp_nodelay &&
253 zsock_setsockopt(sock, IPPROTO_TCP, TCP_NODELAY,
254 &tcp_nodelay,
255 sizeof(tcp_nodelay)) != 0) {
256 NET_WARN("Failed to set IPPROTO_TCP - TCP_NODELAY socket option.");
257 ret = -errno;
258 goto error;
259 }
260
261 ret = zsock_connect(sock, peer_addr, addrlen);
262 if (ret < 0) {
263 NET_ERR("Connect failed (%d)", errno);
264 ret = -errno;
265 goto error;
266 }
267
268 return sock;
269
270 error:
271 zsock_close(sock);
272 return ret;
273 }
274
zperf_packet_duration(uint32_t packet_size,uint32_t rate_in_kbps)275 uint32_t zperf_packet_duration(uint32_t packet_size, uint32_t rate_in_kbps)
276 {
277 return (uint32_t)(((uint64_t)packet_size * 8U * USEC_PER_SEC) /
278 (rate_in_kbps * 1024U));
279 }
280
zperf_async_work_submit(enum session_proto proto,int session_id,struct k_work * work)281 void zperf_async_work_submit(enum session_proto proto, int session_id, struct k_work *work)
282 {
283 #if defined(CONFIG_ZPERF_SESSION_PER_THREAD)
284 k_work_submit_to_queue(zperf_work_q[proto * SESSION_INDEX + session_id].queue, work);
285 #else
286 ARG_UNUSED(proto);
287 ARG_UNUSED(session_id);
288
289 k_work_submit_to_queue(&zperf_work_q, work);
290 #endif
291 }
292
zperf_init(void)293 static int zperf_init(void)
294 {
295 #if defined(CONFIG_ZPERF_SESSION_PER_THREAD)
296
297 ARRAY_FOR_EACH(zperf_work_q, i) {
298 struct k_work_queue_config cfg = {
299 .no_yield = false,
300 };
301
302 zperf_work_q[i].start_event = &start_event;
303
304 #define MAX_NAME_LEN sizeof("zperf_work_q[xxx]")
305 char name[MAX_NAME_LEN];
306
307 snprintk(name, sizeof(name), "zperf_work_q[%d]", i);
308 cfg.name = name;
309
310 k_work_queue_init(zperf_work_q[i].queue);
311
312 k_work_queue_start(zperf_work_q[i].queue,
313 zperf_work_q[i].stack,
314 zperf_work_q[i].stack_size,
315 ZPERF_WORK_Q_THREAD_PRIORITY,
316 &cfg);
317 }
318
319 #else /* CONFIG_ZPERF_SESSION_PER_THREAD */
320
321 k_work_queue_init(&zperf_work_q);
322 k_work_queue_start(&zperf_work_q, zperf_work_q_stack,
323 K_THREAD_STACK_SIZEOF(zperf_work_q_stack),
324 ZPERF_WORK_Q_THREAD_PRIORITY,
325 NULL);
326 k_thread_name_set(&zperf_work_q.thread, "zperf_work_q");
327
328 #endif /* CONFIG_ZPERF_SESSION_PER_THREAD */
329
330 if (IS_ENABLED(CONFIG_NET_UDP)) {
331 zperf_udp_uploader_init();
332 }
333 if (IS_ENABLED(CONFIG_NET_TCP)) {
334 zperf_tcp_uploader_init();
335 }
336
337 if (IS_ENABLED(CONFIG_NET_ZPERF_SERVER) ||
338 IS_ENABLED(CONFIG_ZPERF_SESSION_PER_THREAD)) {
339 zperf_session_init();
340 }
341
342 if (IS_ENABLED(CONFIG_NET_SHELL)) {
343 zperf_shell_init();
344 }
345
346 return 0;
347 }
348
349 SYS_INIT(zperf_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);
350