OpenOCD
discovery_tcp.c
Go to the documentation of this file.
1 /*
2  * This file is part of the libjaylink project.
3  *
4  * Copyright (C) 2015-2017 Marc Schink <jaylink-dev@marcschink.de>
5  *
6  * This program is free software: you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation, either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program. If not, see <http://www.gnu.org/licenses/>.
18  */
19 
20 #include <stdlib.h>
21 #include <stdint.h>
22 #include <string.h>
23 #include <ctype.h>
24 #ifdef _WIN32
25 #include <winsock2.h>
26 #else
27 #include <sys/types.h>
28 #include <sys/socket.h>
29 #include <netinet/in.h>
30 #include <arpa/inet.h>
31 #endif
32 
33 #include "libjaylink.h"
34 #include "libjaylink-internal.h"
35 
44 #define ADV_MESSAGE_SIZE 128
45 
47 #define DISC_PORT 19020
48 
50 #define DISC_MESSAGE_SIZE 64
51 
53 #define DISC_TIMEOUT 20
54 
56 static bool compare_devices(const void *a, const void *b)
57 {
58  const struct jaylink_device *dev;
59  const struct jaylink_device *new_dev;
60 
61  dev = a;
62  new_dev = b;
63 
64  if (dev->iface != JAYLINK_HIF_TCP)
65  return false;
66 
67  if (memcmp(dev->ipv4_address, new_dev->ipv4_address,
68  sizeof(dev->ipv4_address)) != 0)
69  return false;
70 
71  if (dev->serial_number != new_dev->serial_number)
72  return false;
73 
74  if (memcmp(dev->mac_address, new_dev->mac_address,
75  sizeof(dev->mac_address)) != 0)
76  return false;
77 
78  if (strcmp(dev->product_name, new_dev->product_name) != 0)
79  return false;
80 
81  if (strcmp(dev->nickname, new_dev->nickname) != 0)
82  return false;
83 
84  if (dev->hw_version.type != new_dev->hw_version.type)
85  return false;
86 
87  if (dev->hw_version.major != new_dev->hw_version.major)
88  return false;
89 
90  if (dev->hw_version.minor != new_dev->hw_version.minor)
91  return false;
92 
93  if (dev->hw_version.revision != new_dev->hw_version.revision)
94  return false;
95 
96  return true;
97 }
98 
99 static struct jaylink_device *find_device(struct list *list,
100  const struct jaylink_device *dev)
101 {
102  struct list *item;
103 
104  item = list_find_custom(list, &compare_devices, dev);
105 
106  if (item)
107  return item->data;
108 
109  return NULL;
110 }
111 
112 static bool parse_adv_message(struct jaylink_device *dev,
113  const uint8_t *buffer)
114 {
115  struct in_addr in;
116  uint32_t tmp;
117 
118  if (memcmp(buffer, "Found", 5) != 0)
119  return false;
120 
121  /*
122  * Use inet_ntoa() instead of inet_ntop() because the latter requires
123  * at least Windows Vista.
124  */
125  memcpy(&in, buffer + 16, 4);
126  memcpy(dev->ipv4_address, inet_ntoa(in), sizeof(dev->ipv4_address));
127 
128  memcpy(dev->mac_address, buffer + 32, sizeof(dev->mac_address));
129  dev->has_mac_address = true;
130 
131  dev->serial_number = buffer_get_u32(buffer, 48);
132  dev->valid_serial_number = true;
133 
134  tmp = buffer_get_u32(buffer, 52);
135  dev->hw_version.type = (tmp / 1000000) % 100;
136  dev->hw_version.major = (tmp / 10000) % 100;
137  dev->hw_version.minor = (tmp / 100) % 100;
138  dev->hw_version.revision = tmp % 100;
139  dev->has_hw_version = true;
140 
141  memcpy(dev->product_name, buffer + 64, sizeof(dev->product_name));
143  dev->has_product_name = isprint((unsigned char)dev->product_name[0]);
144 
145  memcpy(dev->nickname, buffer + 96, sizeof(dev->nickname));
146  dev->nickname[JAYLINK_NICKNAME_MAX_LENGTH - 1] = '\0';
147  dev->has_nickname = isprint((unsigned char)dev->nickname[0]);
148 
149  return true;
150 }
151 
153  struct sockaddr_in *addr, const uint8_t *buffer)
154 {
155  struct jaylink_device tmp;
156  struct jaylink_device *dev;
157 
158  /*
159  * Use inet_ntoa() instead of inet_ntop() because the latter requires
160  * at least Windows Vista.
161  */
162  log_dbg(ctx, "Received advertisement message (IPv4 address = %s).",
163  inet_ntoa(addr->sin_addr));
164 
165  if (!parse_adv_message(&tmp, buffer)) {
166  log_dbg(ctx, "Received invalid advertisement message.");
167  return NULL;
168  }
169 
170  log_dbg(ctx, "Found device (IPv4 address = %s).", tmp.ipv4_address);
171  log_dbg(ctx, "Device: MAC address = %02x:%02x:%02x:%02x:%02x:%02x.",
172  tmp.mac_address[0], tmp.mac_address[1], tmp.mac_address[2],
173  tmp.mac_address[3], tmp.mac_address[4], tmp.mac_address[5]);
174  log_dbg(ctx, "Device: Serial number = %u.", tmp.serial_number);
175 
176  if (tmp.has_product_name)
177  log_dbg(ctx, "Device: Product = %s.", tmp.product_name);
178 
179  if (tmp.has_nickname)
180  log_dbg(ctx, "Device: Nickname = %s.", tmp.nickname);
181 
182  dev = find_device(ctx->discovered_devs, &tmp);
183 
184  if (dev) {
185  log_dbg(ctx, "Ignoring already discovered device.");
186  return NULL;
187  }
188 
189  dev = find_device(ctx->devs, &tmp);
190 
191  if (dev) {
192  log_dbg(ctx, "Using existing device instance.");
193  return jaylink_ref_device(dev);
194  }
195 
196  log_dbg(ctx, "Allocating new device instance.");
197 
198  dev = device_allocate(ctx);
199 
200  if (!dev) {
201  log_warn(ctx, "Device instance malloc failed.");
202  return NULL;
203  }
204 
205  dev->iface = JAYLINK_HIF_TCP;
206 
207  dev->serial_number = tmp.serial_number;
209 
210  memcpy(dev->ipv4_address, tmp.ipv4_address, sizeof(dev->ipv4_address));
211 
212  memcpy(dev->mac_address, tmp.mac_address, sizeof(dev->mac_address));
213  dev->has_mac_address = tmp.has_mac_address;
214 
215  memcpy(dev->product_name, tmp.product_name, sizeof(dev->product_name));
217 
218  memcpy(dev->nickname, tmp.nickname, sizeof(dev->nickname));
219  dev->has_nickname = tmp.has_nickname;
220 
221  dev->hw_version = tmp.hw_version;
222  dev->has_hw_version = tmp.has_hw_version;
223 
224  return dev;
225 }
226 
229 {
230  int ret;
231  int sock;
232  int opt_value;
233  fd_set rfds;
234  struct sockaddr_in addr;
235  size_t addr_length;
236  struct timeval timeout;
237  uint8_t buf[ADV_MESSAGE_SIZE];
238  struct jaylink_device *dev;
239  size_t length;
240  size_t num_devs;
241 
242  sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
243 
244  if (sock < 0) {
245  log_err(ctx, "Failed to create discovery socket.");
246  return JAYLINK_ERR;
247  }
248 
249  opt_value = true;
250 
251  if (!socket_set_option(sock, SOL_SOCKET, SO_BROADCAST, &opt_value,
252  sizeof(opt_value))) {
253  log_err(ctx, "Failed to enable broadcast option for discovery "
254  "socket.");
255  socket_close(sock);
256  return JAYLINK_ERR;
257  }
258 
259  memset(&addr, 0, sizeof(struct sockaddr_in));
260  addr.sin_family = AF_INET;
261  addr.sin_port = htons(DISC_PORT);
262  addr.sin_addr.s_addr = INADDR_ANY;
263 
264  if (!socket_bind(sock, (struct sockaddr *)&addr,
265  sizeof(struct sockaddr_in))) {
266  log_err(ctx, "Failed to bind discovery socket.");
267  socket_close(sock);
268  return JAYLINK_ERR;
269  }
270 
271  addr.sin_family = AF_INET;
272  addr.sin_port = htons(DISC_PORT);
273  addr.sin_addr.s_addr = INADDR_BROADCAST;
274 
275  memset(buf, 0, DISC_MESSAGE_SIZE);
276  memcpy(buf, "Discover", 8);
277 
278  log_dbg(ctx, "Sending discovery message.");
279 
280  length = DISC_MESSAGE_SIZE;
281 
282  if (!socket_sendto(sock, (char *)buf, &length, 0,
283  (const struct sockaddr *)&addr, sizeof(addr))) {
284  log_err(ctx, "Failed to send discovery message.");
285  socket_close(sock);
286  return JAYLINK_ERR_IO;
287  }
288 
289  if (length < DISC_MESSAGE_SIZE) {
290  log_err(ctx, "Only sent %zu bytes of discovery message.",
291  length);
292  socket_close(sock);
293  return JAYLINK_ERR_IO;
294  }
295 
296  timeout.tv_sec = DISC_TIMEOUT / 1000;
297  timeout.tv_usec = (DISC_TIMEOUT % 1000) * 1000;
298 
299  num_devs = 0;
300 
301  while (true) {
302  FD_ZERO(&rfds);
303  FD_SET(sock, &rfds);
304 
305  ret = select(sock + 1, &rfds, NULL, NULL, &timeout);
306 
307  if (ret <= 0)
308  break;
309 
310  if (!FD_ISSET(sock, &rfds))
311  continue;
312 
313  length = ADV_MESSAGE_SIZE;
314  addr_length = sizeof(struct sockaddr_in);
315 
316  if (!socket_recvfrom(sock, buf, &length, 0,
317  (struct sockaddr *)&addr, &addr_length)) {
318  log_warn(ctx, "Failed to receive advertisement "
319  "message.");
320  continue;
321  }
322 
323  /*
324  * Filter out messages with an invalid size. This includes the
325  * broadcast message we sent before.
326  */
327  if (length != ADV_MESSAGE_SIZE)
328  continue;
329 
330  dev = probe_device(ctx, &addr, buf);
331 
332  if (dev) {
334  ctx->discovered_devs, dev);
335  num_devs++;
336  }
337  }
338 
339  socket_close(sock);
340 
341  if (ret < 0) {
342  log_err(ctx, "select() failed.");
343  return JAYLINK_ERR;
344  }
345 
346  log_dbg(ctx, "Found %zu TCP/IP device(s).", num_devs);
347 
348  return JAYLINK_OK;
349 }
static struct jaylink_device * find_device(struct list *list, const struct jaylink_device *dev)
Definition: discovery_tcp.c:99
unsigned short addr
Definition: embeddedice.c:56
long tv_sec
Definition: replacements.h:54
static struct jaylink_device * probe_device(struct jaylink_context *ctx, struct sockaddr_in *addr, const uint8_t *buffer)
JAYLINK_PRIV uint32_t buffer_get_u32(const uint8_t *buffer, size_t offset)
Read a 32-bit unsigned integer value from a buffer.
Definition: buffer.c:122
JAYLINK_API struct jaylink_device * jaylink_ref_device(struct jaylink_device *dev)
Increment the reference count of a device.
Definition: device.c:460
long tv_usec
Definition: replacements.h:55
static bool parse_adv_message(struct jaylink_device *dev, const uint8_t *buffer)
static bool compare_devices(const void *a, const void *b)
Definition: discovery_tcp.c:56
#define NULL
Definition: usb.h:27