forked from ROMEO/obsw
91 lines
2.2 KiB
C
91 lines
2.2 KiB
C
#include <arpa/inet.h>
|
|
#include <netdb.h>
|
|
#include <netinet/in.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/types.h>
|
|
#include <unistd.h>
|
|
|
|
#include <hardware/interfaces.h>
|
|
|
|
extern const char *sim_ip;
|
|
extern int ai_family;
|
|
|
|
int compare_string_chars(const char *c_string, const char *chars,
|
|
size_t chars_len) {
|
|
for (int i = 0; i < chars_len; i++) {
|
|
if (c_string[i] == 0) {
|
|
return 0;
|
|
}
|
|
if (c_string[i] != chars[i]) {
|
|
;
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
const char *get_port_number(const char *path, size_t path_len) {
|
|
if (compare_string_chars("uart0", path, path_len) == 1) {
|
|
return "8100";
|
|
}
|
|
if (compare_string_chars("uart1", path, path_len) == 1) {
|
|
return "8101";
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
int hw_device_open(const char *path, size_t path_len) {
|
|
const char *port_number = get_port_number(path, path_len);
|
|
if (port_number == NULL) {
|
|
return -1;
|
|
}
|
|
|
|
struct addrinfo addrinfo_hint;
|
|
addrinfo_hint.ai_flags = AI_ADDRCONFIG;
|
|
addrinfo_hint.ai_family = ai_family;
|
|
addrinfo_hint.ai_socktype = SOCK_DGRAM;
|
|
addrinfo_hint.ai_protocol = 0;
|
|
addrinfo_hint.ai_addrlen = 0;
|
|
addrinfo_hint.ai_addr = NULL;
|
|
addrinfo_hint.ai_canonname = NULL;
|
|
addrinfo_hint.ai_next = NULL;
|
|
|
|
int sock;
|
|
struct addrinfo *addr_candidates, *current_candidate;
|
|
|
|
int result =
|
|
getaddrinfo(sim_ip, port_number, &addrinfo_hint, &addr_candidates);
|
|
if (result != 0) {
|
|
fprintf(stderr, "hw_device_open, reading sim address: %s\n",
|
|
gai_strerror(result));
|
|
exit(-1);
|
|
}
|
|
|
|
for (current_candidate = addr_candidates; current_candidate != NULL;
|
|
current_candidate = current_candidate->ai_next) {
|
|
sock = socket(current_candidate->ai_family, current_candidate->ai_socktype,
|
|
current_candidate->ai_protocol);
|
|
if (sock == -1) {
|
|
continue;
|
|
}
|
|
|
|
if (connect(sock, current_candidate->ai_addr,
|
|
current_candidate->ai_addrlen) != -1) {
|
|
break; /* Success */
|
|
}
|
|
|
|
// Connect failed, close socket
|
|
close(sock);
|
|
}
|
|
|
|
if (current_candidate == NULL) { /* No address succeeded */
|
|
fprintf(stderr, "hw_device_open, invalid sim address\n");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
return sock;
|
|
} |