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/misc/lorem_ipsum.h>
15 #include <zephyr/net/socket.h>
16 #include <zephyr/net/ethernet.h>
17 #include <zephyr/net/net_mgmt.h>
18
19 #include <zephyr/posix/sys/socket.h>
20 #include <zephyr/posix/unistd.h>
21
22 #define STACK_SIZE 1024
23 #if defined(CONFIG_NET_TC_THREAD_COOPERATIVE)
24 #define THREAD_PRIORITY K_PRIO_COOP(CONFIG_NUM_COOP_PRIORITIES - 1)
25 #else
26 #define THREAD_PRIORITY K_PRIO_PREEMPT(8)
27 #endif
28 #define RECV_BUFFER_SIZE 1280
29 #define WAIT_TIME CONFIG_NET_SAMPLE_SEND_WAIT_TIME
30
31 #define FLOOD (CONFIG_NET_SAMPLE_SEND_WAIT_TIME ? 0 : 1)
32
33 static struct k_sem quit_lock;
34
35 struct packet_data {
36 int send_sock;
37 int recv_sock;
38 char recv_buffer[RECV_BUFFER_SIZE];
39 };
40
41 static struct packet_data sock_packet;
42 static bool finish;
43 static K_SEM_DEFINE(iface_up, 0, 1);
44
45 static void recv_packet(void);
46 static void send_packet(void);
47
48 K_THREAD_DEFINE(receiver_thread_id, STACK_SIZE,
49 recv_packet, NULL, NULL, NULL,
50 THREAD_PRIORITY, 0, -1);
51 K_THREAD_DEFINE(sender_thread_id, STACK_SIZE,
52 send_packet, NULL, NULL, NULL,
53 THREAD_PRIORITY, 0, -1);
54
55 const char lorem_ipsum[] = LOREM_IPSUM;
56
quit(void)57 static void quit(void)
58 {
59 k_sem_give(&quit_lock);
60 }
61
start_socket(int * sock)62 static int start_socket(int *sock)
63 {
64 struct sockaddr_ll dst = { 0 };
65 int ret;
66
67 *sock = socket(AF_PACKET,
68 IS_ENABLED(CONFIG_NET_SAMPLE_ENABLE_PACKET_DGRAM) ?
69 SOCK_DGRAM : SOCK_RAW,
70 htons(ETH_P_ALL));
71 if (*sock < 0) {
72 LOG_ERR("Failed to create %s socket : %d",
73 IS_ENABLED(CONFIG_NET_SAMPLE_ENABLE_PACKET_DGRAM) ?
74 "DGRAM" : "RAW",
75 errno);
76 return -errno;
77 }
78
79 dst.sll_ifindex = net_if_get_by_iface(net_if_get_default());
80 dst.sll_family = AF_PACKET;
81
82 ret = bind(*sock, (const struct sockaddr *)&dst,
83 sizeof(struct sockaddr_ll));
84 if (ret < 0) {
85 LOG_ERR("Failed to bind packet socket : %d", errno);
86 return -errno;
87 }
88
89 return 0;
90 }
91
recv_packet_socket(struct packet_data * packet)92 static int recv_packet_socket(struct packet_data *packet)
93 {
94 int ret = 0;
95 int received;
96
97 LOG_INF("Waiting for packets ...");
98
99 do {
100 if (finish) {
101 ret = -1;
102 break;
103 }
104
105 received = recv(packet->recv_sock, packet->recv_buffer,
106 sizeof(packet->recv_buffer), 0);
107
108 if (received < 0) {
109 if (errno == EAGAIN) {
110 continue;
111 }
112
113 LOG_ERR("RAW : recv error %d", errno);
114 ret = -errno;
115 break;
116 }
117
118 LOG_DBG("Received %d bytes", received);
119 } while (true);
120
121 return ret;
122 }
123
recv_packet(void)124 static void recv_packet(void)
125 {
126 int ret;
127 struct timeval timeo_optval = {
128 .tv_sec = 1,
129 .tv_usec = 0,
130 };
131
132 ret = start_socket(&sock_packet.recv_sock);
133 if (ret < 0) {
134 quit();
135 return;
136 }
137
138 ret = setsockopt(sock_packet.recv_sock, SOL_SOCKET, SO_RCVTIMEO,
139 &timeo_optval, sizeof(timeo_optval));
140 if (ret < 0) {
141 quit();
142 return;
143 }
144
145 while (ret == 0) {
146 ret = recv_packet_socket(&sock_packet);
147 if (ret < 0) {
148 quit();
149 return;
150 }
151 }
152 }
153
send_packet_socket(struct packet_data * packet)154 static int send_packet_socket(struct packet_data *packet)
155 {
156 struct sockaddr_ll dst = { 0 };
157 size_t send = 100U;
158 int ret;
159
160 dst.sll_ifindex = net_if_get_by_iface(net_if_get_default());
161
162 if (IS_ENABLED(CONFIG_NET_SAMPLE_ENABLE_PACKET_DGRAM)) {
163 dst.sll_halen = sizeof(struct net_eth_addr);
164
165 /* FIXME: assume IP data atm */
166 dst.sll_protocol = htons(ETH_P_IP);
167
168 ret = net_bytes_from_str(
169 dst.sll_addr,
170 dst.sll_halen,
171 CONFIG_NET_SAMPLE_DESTINATION_ADDR);
172 if (ret < 0) {
173 LOG_ERR("Invalid MAC address '%s'",
174 CONFIG_NET_SAMPLE_DESTINATION_ADDR);
175 }
176 }
177
178 do {
179 if (finish) {
180 ret = -1;
181 break;
182 }
183
184 /* Sending dummy data */
185 ret = sendto(packet->send_sock, lorem_ipsum, send, 0,
186 (const struct sockaddr *)&dst,
187 sizeof(struct sockaddr_ll));
188 if (ret < 0) {
189 LOG_ERR("Failed to send, errno %d", errno);
190 break;
191 } else {
192 if (!FLOOD) {
193 LOG_DBG("Sent %zd bytes", send);
194 }
195 }
196
197 /* If we have received any data, flush it here in order to
198 * not to leak memory in IP stack.
199 */
200 do {
201 static char recv_buffer[RECV_BUFFER_SIZE];
202
203 ret = recv(packet->send_sock, recv_buffer,
204 sizeof(recv_buffer), MSG_DONTWAIT);
205 } while (ret > 0);
206
207 if (!FLOOD) {
208 k_msleep(WAIT_TIME);
209 }
210 } while (true);
211
212 return ret;
213 }
214
send_packet(void)215 static void send_packet(void)
216 {
217 int ret;
218
219 ret = start_socket(&sock_packet.send_sock);
220 if (ret < 0) {
221 quit();
222 return;
223 }
224
225 while (ret == 0) {
226 ret = send_packet_socket(&sock_packet);
227 if (ret < 0) {
228 quit();
229 return;
230 }
231 }
232 }
233
iface_up_handler(struct net_mgmt_event_callback * cb,uint64_t mgmt_event,struct net_if * iface)234 static void iface_up_handler(struct net_mgmt_event_callback *cb,
235 uint64_t mgmt_event, struct net_if *iface)
236 {
237 if (mgmt_event == NET_EVENT_IF_UP) {
238 k_sem_give(&iface_up);
239 }
240 }
241
wait_for_interface(void)242 static void wait_for_interface(void)
243 {
244 struct net_if *iface = net_if_get_default();
245 struct net_mgmt_event_callback iface_up_cb;
246
247 if (net_if_is_up(iface)) {
248 return;
249 }
250
251 net_mgmt_init_event_callback(&iface_up_cb, iface_up_handler,
252 NET_EVENT_IF_UP);
253 net_mgmt_add_event_callback(&iface_up_cb);
254
255 /* Wait for the interface to come up. */
256 k_sem_take(&iface_up, K_FOREVER);
257
258 net_mgmt_del_event_callback(&iface_up_cb);
259 }
260
main(void)261 int main(void)
262 {
263 k_sem_init(&quit_lock, 0, K_SEM_MAX_LIMIT);
264
265 wait_for_interface();
266
267 LOG_INF("Packet socket sample is running");
268
269 k_thread_start(receiver_thread_id);
270 k_thread_start(sender_thread_id);
271
272 k_sem_take(&quit_lock, K_FOREVER);
273
274 LOG_INF("Stopping...");
275
276 finish = true;
277
278 k_thread_join(receiver_thread_id, K_FOREVER);
279 k_thread_join(sender_thread_id, K_FOREVER);
280
281 if (sock_packet.recv_sock >= 0) {
282 (void)close(sock_packet.recv_sock);
283 }
284
285 if (sock_packet.send_sock >= 0) {
286 (void)close(sock_packet.send_sock);
287 }
288 return 0;
289 }
290
