1 /*
2 * Copyright (c) 2019 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_pkt_sock_sample, LOG_LEVEL_DBG);
9
10 #include <zephyr/kernel.h>
11 #include <errno.h>
12 #include <stdio.h>
13
14 #include <zephyr/net/socket.h>
15 #include <zephyr/net/ethernet.h>
16 #include <zephyr/net/net_mgmt.h>
17
18 #define STACK_SIZE 1024
19 #if defined(CONFIG_NET_TC_THREAD_COOPERATIVE)
20 #define THREAD_PRIORITY K_PRIO_COOP(CONFIG_NUM_COOP_PRIORITIES - 1)
21 #else
22 #define THREAD_PRIORITY K_PRIO_PREEMPT(8)
23 #endif
24 #define RECV_BUFFER_SIZE 1280
25 #define WAIT_TIME CONFIG_NET_SAMPLE_SEND_WAIT_TIME
26
27 #define FLOOD (CONFIG_NET_SAMPLE_SEND_WAIT_TIME ? 0 : 1)
28
29 static struct k_sem quit_lock;
30
31 struct packet_data {
32 int send_sock;
33 int recv_sock;
34 char recv_buffer[RECV_BUFFER_SIZE];
35 };
36
37 static struct packet_data sock_packet;
38 static bool finish;
39 static K_SEM_DEFINE(iface_up, 0, 1);
40
41 static void recv_packet(void);
42 static void send_packet(void);
43
44 K_THREAD_DEFINE(receiver_thread_id, STACK_SIZE,
45 recv_packet, NULL, NULL, NULL,
46 THREAD_PRIORITY, 0, -1);
47 K_THREAD_DEFINE(sender_thread_id, STACK_SIZE,
48 send_packet, NULL, NULL, NULL,
49 THREAD_PRIORITY, 0, -1);
50
51 /* Generated by http://www.lipsum.com/
52 * 2 paragraphs, 179 words, 1160 bytes of Lorem Ipsum
53 */
54 const char lorem_ipsum[] =
55 "Lorem ipsum dolor sit amet, consectetur adipiscing elit. Quisque "
56 "sodales lorem lorem, sed congue enim vehicula a. Sed finibus diam sed "
57 "odio ultrices pharetra. Nullam dictum arcu ultricies turpis congue, "
58 "vel venenatis turpis venenatis. Nam tempus arcu eros, ac congue libero "
59 "tristique congue. Proin velit lectus, euismod sit amet quam in, "
60 "maximus condimentum urna. Cras vel erat luctus, mattis orci ut, varius "
61 "urna. Nam eu lobortis velit."
62 "\n"
63 "Nullam sit amet diam vel odio sodales cursus vehicula eu arcu. Proin "
64 "fringilla, enim nec consectetur mollis, lorem orci interdum nisi, "
65 "vitae suscipit nisi mauris eu mi. Proin diam enim, mollis ac rhoncus "
66 "vitae, placerat et eros. Suspendisse convallis, ipsum nec rhoncus "
67 "aliquam, ex augue ultrices nisl, id aliquet mi diam quis ante. "
68 "Pellentesque venenatis ornare ultrices. Quisque et porttitor lectus. "
69 "Ut venenatis nunc et urna imperdiet porttitor non laoreet massa. Donec "
70 "eleifend eros in mi sagittis egestas. Sed et mi nunc. Nunc vulputate, "
71 "mauris non ullamcorper viverra, lorem nulla vulputate diam, et congue "
72 "dui velit non erat. Duis interdum leo et ipsum tempor consequat. In "
73 "faucibus enim quis purus vulputate nullam."
74 "\n";
75
quit(void)76 static void quit(void)
77 {
78 k_sem_give(&quit_lock);
79 }
80
start_socket(int * sock)81 static int start_socket(int *sock)
82 {
83 struct sockaddr_ll dst = { 0 };
84 int ret;
85
86 *sock = socket(AF_PACKET,
87 IS_ENABLED(CONFIG_NET_SAMPLE_ENABLE_PACKET_DGRAM) ?
88 SOCK_DGRAM : SOCK_RAW,
89 ETH_P_ALL);
90 if (*sock < 0) {
91 LOG_ERR("Failed to create %s socket : %d",
92 IS_ENABLED(CONFIG_NET_SAMPLE_ENABLE_PACKET_DGRAM) ?
93 "DGRAM" : "RAW",
94 errno);
95 return -errno;
96 }
97
98 dst.sll_ifindex = net_if_get_by_iface(net_if_get_default());
99 dst.sll_family = AF_PACKET;
100
101 ret = bind(*sock, (const struct sockaddr *)&dst,
102 sizeof(struct sockaddr_ll));
103 if (ret < 0) {
104 LOG_ERR("Failed to bind packet socket : %d", errno);
105 return -errno;
106 }
107
108 return 0;
109 }
110
recv_packet_socket(struct packet_data * packet)111 static int recv_packet_socket(struct packet_data *packet)
112 {
113 int ret = 0;
114 int received;
115
116 LOG_INF("Waiting for packets ...");
117
118 do {
119 if (finish) {
120 ret = -1;
121 break;
122 }
123
124 received = recv(packet->recv_sock, packet->recv_buffer,
125 sizeof(packet->recv_buffer), 0);
126
127 if (received < 0) {
128 if (errno == EAGAIN) {
129 continue;
130 }
131
132 LOG_ERR("RAW : recv error %d", errno);
133 ret = -errno;
134 break;
135 }
136
137 LOG_DBG("Received %d bytes", received);
138 } while (true);
139
140 return ret;
141 }
142
recv_packet(void)143 static void recv_packet(void)
144 {
145 int ret;
146 struct timeval timeo_optval = {
147 .tv_sec = 1,
148 .tv_usec = 0,
149 };
150
151 ret = start_socket(&sock_packet.recv_sock);
152 if (ret < 0) {
153 quit();
154 return;
155 }
156
157 ret = setsockopt(sock_packet.recv_sock, SOL_SOCKET, SO_RCVTIMEO,
158 &timeo_optval, sizeof(timeo_optval));
159 if (ret < 0) {
160 quit();
161 return;
162 }
163
164 while (ret == 0) {
165 ret = recv_packet_socket(&sock_packet);
166 if (ret < 0) {
167 quit();
168 return;
169 }
170 }
171 }
172
send_packet_socket(struct packet_data * packet)173 static int send_packet_socket(struct packet_data *packet)
174 {
175 struct sockaddr_ll dst = { 0 };
176 size_t send = 100U;
177 int ret;
178
179 dst.sll_ifindex = net_if_get_by_iface(net_if_get_default());
180
181 if (IS_ENABLED(CONFIG_NET_SAMPLE_ENABLE_PACKET_DGRAM)) {
182 dst.sll_halen = sizeof(struct net_eth_addr);
183
184 /* FIXME: assume IP data atm */
185 dst.sll_protocol = htons(ETH_P_IP);
186
187 ret = net_bytes_from_str(
188 dst.sll_addr,
189 dst.sll_halen,
190 CONFIG_NET_SAMPLE_DESTINATION_ADDR);
191 if (ret < 0) {
192 LOG_ERR("Invalid MAC address '%s'",
193 CONFIG_NET_SAMPLE_DESTINATION_ADDR);
194 }
195 }
196
197 do {
198 if (finish) {
199 ret = -1;
200 break;
201 }
202
203 /* Sending dummy data */
204 ret = sendto(packet->send_sock, lorem_ipsum, send, 0,
205 (const struct sockaddr *)&dst,
206 sizeof(struct sockaddr_ll));
207 if (ret < 0) {
208 LOG_ERR("Failed to send, errno %d", errno);
209 break;
210 } else {
211 if (!FLOOD) {
212 LOG_DBG("Sent %zd bytes", send);
213 }
214 }
215
216 /* If we have received any data, flush it here in order to
217 * not to leak memory in IP stack.
218 */
219 do {
220 static char recv_buffer[RECV_BUFFER_SIZE];
221
222 ret = recv(packet->send_sock, recv_buffer,
223 sizeof(recv_buffer), MSG_DONTWAIT);
224 } while (ret > 0);
225
226 if (!FLOOD) {
227 k_msleep(WAIT_TIME);
228 }
229 } while (true);
230
231 return ret;
232 }
233
send_packet(void)234 static void send_packet(void)
235 {
236 int ret;
237
238 ret = start_socket(&sock_packet.send_sock);
239 if (ret < 0) {
240 quit();
241 return;
242 }
243
244 while (ret == 0) {
245 ret = send_packet_socket(&sock_packet);
246 if (ret < 0) {
247 quit();
248 return;
249 }
250 }
251 }
252
iface_up_handler(struct net_mgmt_event_callback * cb,uint32_t mgmt_event,struct net_if * iface)253 static void iface_up_handler(struct net_mgmt_event_callback *cb,
254 uint32_t mgmt_event, struct net_if *iface)
255 {
256 if (mgmt_event == NET_EVENT_IF_UP) {
257 k_sem_give(&iface_up);
258 }
259 }
260
wait_for_interface(void)261 static void wait_for_interface(void)
262 {
263 struct net_if *iface = net_if_get_default();
264 struct net_mgmt_event_callback iface_up_cb;
265
266 if (net_if_is_up(iface)) {
267 return;
268 }
269
270 net_mgmt_init_event_callback(&iface_up_cb, iface_up_handler,
271 NET_EVENT_IF_UP);
272 net_mgmt_add_event_callback(&iface_up_cb);
273
274 /* Wait for the interface to come up. */
275 k_sem_take(&iface_up, K_FOREVER);
276
277 net_mgmt_del_event_callback(&iface_up_cb);
278 }
279
main(void)280 int main(void)
281 {
282 k_sem_init(&quit_lock, 0, K_SEM_MAX_LIMIT);
283
284 wait_for_interface();
285
286 LOG_INF("Packet socket sample is running");
287
288 k_thread_start(receiver_thread_id);
289 k_thread_start(sender_thread_id);
290
291 k_sem_take(&quit_lock, K_FOREVER);
292
293 LOG_INF("Stopping...");
294
295 finish = true;
296
297 k_thread_join(receiver_thread_id, K_FOREVER);
298 k_thread_join(sender_thread_id, K_FOREVER);
299
300 if (sock_packet.recv_sock >= 0) {
301 (void)close(sock_packet.recv_sock);
302 }
303
304 if (sock_packet.send_sock >= 0) {
305 (void)close(sock_packet.send_sock);
306 }
307 return 0;
308 }
309
