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