1 /*
2 * Copyright (c) 2022 René Beckmann
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /** @file mqtt_sn_transport_udp.c
8 *
9 * @brief MQTT-SN UDP transport.
10 */
11
12 #include <errno.h>
13
14 #include <zephyr/net/mqtt_sn.h>
15 #include <zephyr/net/net_ip.h>
16 #include <zephyr/net/socket.h>
17 #include <zephyr/net/igmp.h>
18
19 #include <zephyr/posix/fcntl.h>
20
21 #include <zephyr/logging/log.h>
22 LOG_MODULE_DECLARE(net_mqtt_sn, CONFIG_MQTT_SN_LOG_LEVEL);
23
get_ip_str(const struct sockaddr * sa,char * s,size_t maxlen)24 static char *get_ip_str(const struct sockaddr *sa, char *s, size_t maxlen)
25 {
26 switch (sa->sa_family) {
27 case AF_INET:
28 inet_ntop(AF_INET, &(((struct sockaddr_in *)sa)->sin_addr), s, maxlen);
29 break;
30
31 case AF_INET6:
32 inet_ntop(AF_INET6, &(((struct sockaddr_in6 *)sa)->sin6_addr), s, maxlen);
33 break;
34
35 default:
36 strncpy(s, "Unknown AF", maxlen);
37 break;
38 }
39
40 return s;
41 }
42
tp_udp_init(struct mqtt_sn_transport * transport)43 static int tp_udp_init(struct mqtt_sn_transport *transport)
44 {
45 struct mqtt_sn_transport_udp *udp = UDP_TRANSPORT(transport);
46 int err;
47 struct sockaddr addrm;
48 struct ip_mreqn mreqn;
49 int optval;
50 struct net_if *iface;
51
52 udp->sock = zsock_socket(udp->bcaddr.sa_family, SOCK_DGRAM, 0);
53 if (udp->sock < 0) {
54 return errno;
55 }
56
57 LOG_DBG("Socket %d", udp->sock);
58
59 optval = 1;
60 err = zsock_setsockopt(udp->sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
61 if (err < 0) {
62 return errno;
63 }
64
65 if (IS_ENABLED(CONFIG_MQTT_SN_LOG_LEVEL_DBG)) {
66 char ip[30], *out;
67 uint16_t port;
68
69 out = get_ip_str((struct sockaddr *)&udp->bcaddr, ip, sizeof(ip));
70 switch (udp->bcaddr.sa_family) {
71 case AF_INET:
72 port = ntohs(((struct sockaddr_in *)&udp->bcaddr)->sin_port);
73 break;
74 case AF_INET6:
75 port = ntohs(((struct sockaddr_in6 *)&udp->bcaddr)->sin6_port);
76 break;
77 default:
78 break;
79 }
80
81 if (out != NULL) {
82 LOG_DBG("Binding to Brodcast IP %s:%u", out, port);
83 }
84 }
85
86 switch (udp->bcaddr.sa_family) {
87 case AF_INET:
88 if (IS_ENABLED(CONFIG_NET_IPV4)) {
89 addrm.sa_family = AF_INET;
90 ((struct sockaddr_in *)&addrm)->sin_port =
91 ((struct sockaddr_in *)&udp->bcaddr)->sin_port;
92 ((struct sockaddr_in *)&addrm)->sin_addr.s_addr = INADDR_ANY;
93 }
94 break;
95 case AF_INET6:
96 if (IS_ENABLED(CONFIG_NET_IPV6)) {
97 addrm.sa_family = AF_INET6;
98 ((struct sockaddr_in6 *)&addrm)->sin6_port =
99 ((struct sockaddr_in6 *)&udp->bcaddr)->sin6_port;
100 memcpy(&((struct sockaddr_in6 *)&addrm)->sin6_addr, &in6addr_any,
101 sizeof(struct in6_addr));
102 break;
103 }
104 default:
105 LOG_ERR("Unknown AF");
106 return -EINVAL;
107 }
108
109 err = zsock_bind(udp->sock, &addrm, sizeof(addrm));
110 if (err) {
111 LOG_ERR("Error during bind: %d", errno);
112 return errno;
113 }
114
115 memcpy(&mreqn.imr_multiaddr, &udp->bcaddr.data[2], sizeof(udp->bcaddr.data) - 2);
116 if (udp->bcaddr.sa_family == AF_INET && IS_ENABLED(CONFIG_NET_IPV4)) {
117 iface = net_if_ipv4_select_src_iface(
118 &((struct sockaddr_in *)&udp->bcaddr)->sin_addr);
119 } else if (udp->bcaddr.sa_family == AF_INET6 && IS_ENABLED(CONFIG_NET_IPV6)) {
120 iface = net_if_ipv6_select_src_iface(&((struct sockaddr_in6 *)&addrm)->sin6_addr);
121 } else {
122 LOG_ERR("Unknown AF");
123 return -EINVAL;
124 }
125 mreqn.imr_ifindex = net_if_get_by_iface(iface);
126
127 err = zsock_setsockopt(udp->sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreqn, sizeof(mreqn));
128 if (err < 0) {
129 return errno;
130 }
131
132 optval = CONFIG_MQTT_SN_LIB_BROADCAST_RADIUS;
133 err = zsock_setsockopt(udp->sock, IPPROTO_IP, IP_MULTICAST_TTL, &optval, sizeof(optval));
134 if (err < 0) {
135 return errno;
136 }
137
138 return 0;
139 }
140
tp_udp_deinit(struct mqtt_sn_transport * transport)141 static void tp_udp_deinit(struct mqtt_sn_transport *transport)
142 {
143 struct mqtt_sn_transport_udp *udp = UDP_TRANSPORT(transport);
144
145 zsock_close(udp->sock);
146 }
147
tp_udp_sendto(struct mqtt_sn_client * client,void * buf,size_t sz,const void * dest_addr,size_t addrlen)148 static int tp_udp_sendto(struct mqtt_sn_client *client, void *buf, size_t sz, const void *dest_addr,
149 size_t addrlen)
150 {
151 struct mqtt_sn_transport_udp *udp = UDP_TRANSPORT(client->transport);
152 int rc;
153 int ttl;
154 socklen_t ttl_len;
155
156 if (dest_addr == NULL) {
157 LOG_HEXDUMP_DBG(buf, sz, "Sending Broadcast UDP packet");
158
159 /* Set ttl if requested value does not match existing*/
160 rc = zsock_getsockopt(udp->sock, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, &ttl_len);
161 if (rc < 0) {
162 return -errno;
163 }
164 if (ttl != addrlen) {
165 ttl = addrlen;
166 rc = zsock_setsockopt(udp->sock, IPPROTO_IP, IP_MULTICAST_TTL, &ttl,
167 sizeof(ttl));
168 if (rc < 0) {
169 return -errno;
170 }
171 }
172
173 rc = zsock_sendto(udp->sock, buf, sz, 0, &udp->bcaddr, udp->bcaddrlen);
174 } else {
175 LOG_HEXDUMP_DBG(buf, sz, "Sending Addressed UDP packet");
176 rc = zsock_sendto(udp->sock, buf, sz, 0, dest_addr, addrlen);
177 }
178
179 if (rc < 0) {
180 return -errno;
181 }
182
183 if (rc != sz) {
184 return -EIO;
185 }
186
187 return 0;
188 }
189
tp_udp_recvfrom(struct mqtt_sn_client * client,void * buffer,size_t length,void * src_addr,size_t * addrlen)190 static ssize_t tp_udp_recvfrom(struct mqtt_sn_client *client, void *buffer, size_t length,
191 void *src_addr, size_t *addrlen)
192 {
193 struct mqtt_sn_transport_udp *udp = UDP_TRANSPORT(client->transport);
194 int rc;
195 struct sockaddr *srcaddr = src_addr;
196
197 rc = zsock_recvfrom(udp->sock, buffer, length, 0, src_addr, addrlen);
198 LOG_DBG("recv %d", rc);
199 if (rc < 0) {
200 return -errno;
201 }
202
203 LOG_HEXDUMP_DBG(buffer, rc, "recv");
204
205 if (*addrlen != udp->bcaddrlen) {
206 return rc;
207 }
208
209 if (memcmp(srcaddr->data, udp->bcaddr.data, *addrlen) != 0) {
210 return rc;
211 }
212
213 src_addr = NULL;
214 *addrlen = 1;
215 return rc;
216 }
217
tp_udp_poll(struct mqtt_sn_client * client)218 static int tp_udp_poll(struct mqtt_sn_client *client)
219 {
220 struct mqtt_sn_transport_udp *udp = UDP_TRANSPORT(client->transport);
221 int rc;
222
223 struct zsock_pollfd pollfd = {
224 .fd = udp->sock,
225 .events = ZSOCK_POLLIN,
226 };
227
228 rc = zsock_poll(&pollfd, 1, 0);
229 if (rc < 1) {
230 return rc;
231 }
232
233 LOG_DBG("revents %d", pollfd.revents & ZSOCK_POLLIN);
234
235 return pollfd.revents & ZSOCK_POLLIN;
236 }
237
mqtt_sn_transport_udp_init(struct mqtt_sn_transport_udp * udp,struct sockaddr * bcaddr,socklen_t addrlen)238 int mqtt_sn_transport_udp_init(struct mqtt_sn_transport_udp *udp, struct sockaddr *bcaddr,
239 socklen_t addrlen)
240 {
241 if (!udp || !bcaddr || !addrlen) {
242 return -EINVAL;
243 }
244
245 memset(udp, 0, sizeof(*udp));
246
247 udp->tp = (struct mqtt_sn_transport){.init = tp_udp_init,
248 .deinit = tp_udp_deinit,
249 .sendto = tp_udp_sendto,
250 .poll = tp_udp_poll,
251 .recvfrom = tp_udp_recvfrom};
252
253 udp->sock = 0;
254
255 memcpy(&udp->bcaddr, bcaddr, addrlen);
256 udp->bcaddrlen = addrlen;
257
258 return 0;
259 }
260
