1 /*
2 * Copyright (c) 2015 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_DECLARE(net_zperf, CONFIG_NET_ZPERF_LOG_LEVEL);
9
10 #include <zephyr/kernel.h>
11
12 #include <zephyr/net/socket.h>
13 #include <zephyr/net/zperf.h>
14
15 #include "zperf_internal.h"
16
17 static uint8_t sample_packet[sizeof(struct zperf_udp_datagram) +
18 sizeof(struct zperf_client_hdr_v1) +
19 PACKET_SIZE_MAX];
20
21 static struct zperf_async_upload_context udp_async_upload_ctx;
22
zperf_upload_decode_stat(const uint8_t * data,size_t datalen,struct zperf_results * results)23 static inline void zperf_upload_decode_stat(const uint8_t *data,
24 size_t datalen,
25 struct zperf_results *results)
26 {
27 struct zperf_server_hdr *stat;
28
29 if (datalen < sizeof(struct zperf_udp_datagram) +
30 sizeof(struct zperf_server_hdr)) {
31 NET_WARN("Network packet too short");
32 }
33
34 stat = (struct zperf_server_hdr *)
35 (data + sizeof(struct zperf_udp_datagram));
36
37 results->nb_packets_rcvd = ntohl(UNALIGNED_GET(&stat->datagrams));
38 results->nb_packets_lost = ntohl(UNALIGNED_GET(&stat->error_cnt));
39 results->nb_packets_outorder =
40 ntohl(UNALIGNED_GET(&stat->outorder_cnt));
41 results->total_len = ntohl(UNALIGNED_GET(&stat->total_len2));
42 results->time_in_us = ntohl(UNALIGNED_GET(&stat->stop_usec)) +
43 ntohl(UNALIGNED_GET(&stat->stop_sec)) * USEC_PER_SEC;
44 results->jitter_in_us = ntohl(UNALIGNED_GET(&stat->jitter2)) +
45 ntohl(UNALIGNED_GET(&stat->jitter1)) * USEC_PER_SEC;
46 }
47
zperf_upload_fin(int sock,uint32_t nb_packets,uint64_t end_time,uint32_t packet_size,struct zperf_results * results)48 static inline int zperf_upload_fin(int sock,
49 uint32_t nb_packets,
50 uint64_t end_time,
51 uint32_t packet_size,
52 struct zperf_results *results)
53 {
54 uint8_t stats[sizeof(struct zperf_udp_datagram) +
55 sizeof(struct zperf_server_hdr)] = { 0 };
56 struct zperf_udp_datagram *datagram;
57 struct zperf_client_hdr_v1 *hdr;
58 uint32_t secs = k_ticks_to_ms_ceil32(end_time) / 1000U;
59 uint32_t usecs = k_ticks_to_us_ceil32(end_time) - secs * USEC_PER_SEC;
60 int loop = 2;
61 int ret = 0;
62 struct timeval rcvtimeo = {
63 .tv_sec = 2,
64 .tv_usec = 0,
65 };
66
67 while (ret <= 0 && loop-- > 0) {
68 datagram = (struct zperf_udp_datagram *)sample_packet;
69
70 /* Fill the packet header */
71 datagram->id = htonl(-nb_packets);
72 datagram->tv_sec = htonl(secs);
73 datagram->tv_usec = htonl(usecs);
74
75 hdr = (struct zperf_client_hdr_v1 *)(sample_packet +
76 sizeof(*datagram));
77
78 /* According to iperf documentation (in include/Settings.hpp),
79 * if the flags == 0, then the other values are ignored.
80 * But even if the values in the header are ignored, try
81 * to set there some meaningful values.
82 */
83 hdr->flags = 0;
84 hdr->num_of_threads = htonl(1);
85 hdr->port = 0;
86 hdr->buffer_len = sizeof(sample_packet) -
87 sizeof(*datagram) - sizeof(*hdr);
88 hdr->bandwidth = 0;
89 hdr->num_of_bytes = htonl(packet_size);
90
91 /* Send the packet */
92 ret = zsock_send(sock, sample_packet, packet_size, 0);
93 if (ret < 0) {
94 NET_ERR("Failed to send the packet (%d)", errno);
95 continue;
96 }
97
98 /* Receive statistics */
99 ret = zsock_setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &rcvtimeo,
100 sizeof(rcvtimeo));
101 if (ret < 0) {
102 NET_ERR("setsockopt error (%d)", errno);
103 continue;
104 }
105
106 ret = zsock_recv(sock, stats, sizeof(stats), 0);
107 if (ret == -EAGAIN) {
108 NET_WARN("Stats receive timeout");
109 } else if (ret < 0) {
110 NET_ERR("Failed to receive packet (%d)", errno);
111 }
112 }
113
114 /* Decode statistics */
115 if (ret > 0) {
116 zperf_upload_decode_stat(stats, ret, results);
117 } else {
118 return ret;
119 }
120
121 /* Drain RX */
122 while (true) {
123 ret = zsock_recv(sock, stats, sizeof(stats), ZSOCK_MSG_DONTWAIT);
124 if (ret < 0) {
125 break;
126 }
127
128 NET_WARN("Drain one spurious stat packet!");
129 }
130
131 return 0;
132 }
133
udp_upload(int sock,int port,unsigned int duration_in_ms,unsigned int packet_size,unsigned int rate_in_kbps,struct zperf_results * results)134 static int udp_upload(int sock, int port,
135 unsigned int duration_in_ms,
136 unsigned int packet_size,
137 unsigned int rate_in_kbps,
138 struct zperf_results *results)
139 {
140 uint32_t packet_duration_us = zperf_packet_duration(packet_size, rate_in_kbps);
141 uint32_t packet_duration = k_us_to_ticks_ceil32(packet_duration_us);
142 uint32_t delay = packet_duration;
143 uint32_t nb_packets = 0U;
144 int64_t start_time, end_time;
145 int64_t print_time, last_loop_time;
146 uint32_t print_period;
147 int ret;
148
149 if (packet_size > PACKET_SIZE_MAX) {
150 NET_WARN("Packet size too large! max size: %u",
151 PACKET_SIZE_MAX);
152 packet_size = PACKET_SIZE_MAX;
153 } else if (packet_size < sizeof(struct zperf_udp_datagram)) {
154 NET_WARN("Packet size set to the min size: %zu",
155 sizeof(struct zperf_udp_datagram));
156 packet_size = sizeof(struct zperf_udp_datagram);
157 }
158
159 /* Start the loop */
160 start_time = k_uptime_ticks();
161 last_loop_time = start_time;
162 end_time = start_time + k_ms_to_ticks_ceil64(duration_in_ms);
163
164 /* Print log every seconds */
165 print_period = k_ms_to_ticks_ceil32(MSEC_PER_SEC);
166 print_time = start_time + print_period;
167
168 (void)memset(sample_packet, 'z', sizeof(sample_packet));
169
170 do {
171 struct zperf_udp_datagram *datagram;
172 struct zperf_client_hdr_v1 *hdr;
173 uint64_t usecs64;
174 uint32_t secs, usecs;
175 int64_t loop_time;
176 int32_t adjust;
177
178 /* Timestamp */
179 loop_time = k_uptime_ticks();
180
181 /* Algorithm to maintain a given baud rate */
182 if (last_loop_time != loop_time) {
183 adjust = packet_duration;
184 adjust -= (int32_t)(loop_time - last_loop_time);
185 } else {
186 /* It's the first iteration so no need for adjustment
187 */
188 adjust = 0;
189 }
190
191 if ((adjust >= 0) || (-adjust < delay)) {
192 delay += adjust;
193 } else {
194 delay = 0U; /* delay should never be negative */
195 }
196
197 last_loop_time = loop_time;
198
199 usecs64 = k_ticks_to_us_floor64(loop_time);
200 secs = usecs64 / USEC_PER_SEC;
201 usecs = usecs64 - (uint64_t)secs * USEC_PER_SEC;
202
203 /* Fill the packet header */
204 datagram = (struct zperf_udp_datagram *)sample_packet;
205
206 datagram->id = htonl(nb_packets);
207 datagram->tv_sec = htonl(secs);
208 datagram->tv_usec = htonl(usecs);
209
210 hdr = (struct zperf_client_hdr_v1 *)(sample_packet +
211 sizeof(*datagram));
212 hdr->flags = 0;
213 hdr->num_of_threads = htonl(1);
214 hdr->port = htonl(port);
215 hdr->buffer_len = sizeof(sample_packet) -
216 sizeof(*datagram) - sizeof(*hdr);
217 hdr->bandwidth = htonl(rate_in_kbps);
218 hdr->num_of_bytes = htonl(packet_size);
219
220 /* Send the packet */
221 ret = zsock_send(sock, sample_packet, packet_size, 0);
222 if (ret < 0) {
223 NET_ERR("Failed to send the packet (%d)", errno);
224 return -errno;
225 } else {
226 nb_packets++;
227 }
228
229 if (IS_ENABLED(CONFIG_NET_ZPERF_LOG_LEVEL_DBG)) {
230 if (print_time >= loop_time) {
231 NET_DBG("nb_packets=%u\tdelay=%u\tadjust=%d",
232 nb_packets, (unsigned int)delay,
233 (int)adjust);
234 print_time += print_period;
235 }
236 }
237
238 /* Wait */
239 #if defined(CONFIG_ARCH_POSIX)
240 k_busy_wait(USEC_PER_MSEC);
241 #else
242 if (delay != 0) {
243 k_sleep(K_TICKS(delay));
244 }
245 #endif
246 } while (last_loop_time < end_time);
247
248 end_time = k_uptime_ticks();
249
250 ret = zperf_upload_fin(sock, nb_packets, end_time, packet_size,
251 results);
252 if (ret < 0) {
253 return ret;
254 }
255
256 /* Add result coming from the client */
257 results->nb_packets_sent = nb_packets;
258 results->client_time_in_us =
259 k_ticks_to_us_ceil32(end_time - start_time);
260 results->packet_size = packet_size;
261
262 return 0;
263 }
264
zperf_udp_upload(const struct zperf_upload_params * param,struct zperf_results * result)265 int zperf_udp_upload(const struct zperf_upload_params *param,
266 struct zperf_results *result)
267 {
268 int port = 0;
269 int sock;
270 int ret;
271
272 if (param == NULL || result == NULL) {
273 return -EINVAL;
274 }
275
276 if (param->peer_addr.sa_family == AF_INET) {
277 port = ntohs(net_sin(¶m->peer_addr)->sin_port);
278 } else if (param->peer_addr.sa_family == AF_INET6) {
279 port = ntohs(net_sin6(¶m->peer_addr)->sin6_port);
280 } else {
281 NET_ERR("Invalid address family (%d)",
282 param->peer_addr.sa_family);
283 return -EINVAL;
284 }
285
286 sock = zperf_prepare_upload_sock(¶m->peer_addr, param->options.tos,
287 param->options.priority, IPPROTO_UDP);
288 if (sock < 0) {
289 return sock;
290 }
291
292 ret = udp_upload(sock, port, param->duration_ms, param->packet_size,
293 param->rate_kbps, result);
294
295 zsock_close(sock);
296
297 return ret;
298 }
299
udp_upload_async_work(struct k_work * work)300 static void udp_upload_async_work(struct k_work *work)
301 {
302 struct zperf_async_upload_context *upload_ctx =
303 &udp_async_upload_ctx;
304 struct zperf_results result;
305 int ret;
306
307 upload_ctx->callback(ZPERF_SESSION_STARTED, NULL,
308 upload_ctx->user_data);
309
310 ret = zperf_udp_upload(&upload_ctx->param, &result);
311 if (ret < 0) {
312 upload_ctx->callback(ZPERF_SESSION_ERROR, NULL,
313 upload_ctx->user_data);
314 } else {
315 upload_ctx->callback(ZPERF_SESSION_FINISHED, &result,
316 upload_ctx->user_data);
317 }
318 }
319
zperf_udp_upload_async(const struct zperf_upload_params * param,zperf_callback callback,void * user_data)320 int zperf_udp_upload_async(const struct zperf_upload_params *param,
321 zperf_callback callback, void *user_data)
322 {
323 if (param == NULL || callback == NULL) {
324 return -EINVAL;
325 }
326
327 if (k_work_is_pending(&udp_async_upload_ctx.work)) {
328 return -EBUSY;
329 }
330
331 memcpy(&udp_async_upload_ctx.param, param, sizeof(*param));
332 udp_async_upload_ctx.callback = callback;
333 udp_async_upload_ctx.user_data = user_data;
334
335 zperf_async_work_submit(&udp_async_upload_ctx.work);
336
337 return 0;
338 }
339
zperf_udp_uploader_init(void)340 void zperf_udp_uploader_init(void)
341 {
342 k_work_init(&udp_async_upload_ctx.work, udp_upload_async_work);
343 }
344
