diff --git a/CMakeLists.txt b/CMakeLists.txt index fe26ca62..9bfda669 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,6 +66,7 @@ set(NEURON_BASE_SOURCES src/base/group.c src/base/metrics.c src/connection/connection.c + src/connection/connection_eth.c src/connection/mqtt_client.c src/event/event_linux.c src/event/event_unix.c diff --git a/include/neuron/connection/neu_connection_eth.h b/include/neuron/connection/neu_connection_eth.h new file mode 100644 index 00000000..416e423f --- /dev/null +++ b/include/neuron/connection/neu_connection_eth.h @@ -0,0 +1,53 @@ +/** + * NEURON IIoT System for Industry 4.0 + * Copyright (C) 2020-2023 EMQ Technologies Co., Ltd All rights reserved. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + **/ + +#ifndef NEURON_CONNECTION_ETH_H +#define NEURON_CONNECTION_ETH_H + +#include + +#include "define.h" + +// profinet + +typedef struct neu_conn_eth neu_conn_eth_t; + +neu_conn_eth_t *neu_conn_eth_init(const char *interface, void *ctx); +int neu_conn_eth_uninit(neu_conn_eth_t *conn); +int neu_conn_eth_check_interface(const char *interface); + +typedef void (*neu_conn_eth_msg_callback)(neu_conn_eth_t *conn, void *ctx, + uint16_t protocol, uint16_t n_byte, + uint8_t *bytes, uint8_t src_mac[6]); + +int neu_conn_eth_register_lldp(neu_conn_eth_t *conn, const char *device, + neu_conn_eth_msg_callback callback); +int neu_conn_eth_unregister_lldp(neu_conn_eth_t *conn, const char *device); + +typedef struct neu_conn_eth_sub neu_conn_eth_sub_t; + +neu_conn_eth_sub_t *neu_conn_eth_register(neu_conn_eth_t *conn, + uint16_t protocol, uint8_t mac[6], + neu_conn_eth_msg_callback callback); +int neu_conn_eth_unregister(neu_conn_eth_t *conn, neu_conn_eth_sub_t *sub); + +int neu_conn_eth_send(neu_conn_eth_t *conn, uint8_t dst_mac[6], + uint16_t protocol, uint16_t n_byte, uint8_t *bytes); + +#endif \ No newline at end of file diff --git a/include/neuron/neuron.h b/include/neuron/neuron.h index c3ec8b30..31e1e5f0 100644 --- a/include/neuron/neuron.h +++ b/include/neuron/neuron.h @@ -46,6 +46,7 @@ extern "C" { #include "utils/utlist.h" #include "connection/neu_connection.h" +#include "connection/neu_connection_eth.h" #include "event/event.h" #include "json/json.h" diff --git a/src/connection/connection.c b/src/connection/connection.c index 210a989b..876e310d 100644 --- a/src/connection/connection.c +++ b/src/connection/connection.c @@ -389,7 +389,7 @@ ssize_t neu_conn_recv(neu_conn_t *conn, uint8_t *buf, ssize_t len) conn->fd, len, ret, strerror(errno), errno); } - if (errno == EPIPE || ret == -1) { + if (errno == EPIPE || ret == -1 || ret == 0) { conn_disconnect(conn); } diff --git a/src/connection/connection_eth.c b/src/connection/connection_eth.c new file mode 100644 index 00000000..34212291 --- /dev/null +++ b/src/connection/connection_eth.c @@ -0,0 +1,461 @@ +/** + * NEURON IIoT System for Industry 4.0 + * Copyright (C) 2020-2023 EMQ Technologies Co., Ltd All rights reserved. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + **/ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "connection/neu_connection_eth.h" +#include "event/event.h" +#include "utils/log.h" +#include "utils/utextend.h" + +typedef struct neu_conn_eth_sub { + uint16_t protocol; + char mac[ETH_ALEN]; +} callback_elem_key_t; + +typedef struct { + callback_elem_key_t key; + + neu_conn_eth_msg_callback callback; + + UT_hash_handle hh; +} callback_elem_t; + +typedef struct { + char device[NEU_NODE_NAME_LEN]; + + neu_conn_eth_msg_callback callback; + + UT_hash_handle hh; +} lldp_callback_elem_t; + +typedef struct interface_conn { + char * interface; + neu_events_t *events; + uint8_t count; + uint8_t buf[1500]; + + int lldp_fd; + int profinet_fd; + uint8_t mac[ETH_ALEN]; + + neu_event_io_t *profinet_event; + neu_event_io_t *lldp_event; + + lldp_callback_elem_t *lldp_callbacks; + callback_elem_t * callbacks; + pthread_mutex_t mtx; +} interface_conn_t; + +static pthread_mutex_t mtx = { 0 }; +static bool mtx_init = false; +#define interface_max 4 +static interface_conn_t in_conns[interface_max]; + +struct neu_conn_eth { + interface_conn_t *ic; + void * ctx; +}; + +static int get_mac(neu_conn_eth_t *conn, const char *interface); +static int init_socket(const char *interface, uint16_t protocol); +static int uninit_socket(int fd); + +static int profinet_msg_cb(enum neu_event_io_type type, int fd, void *usr_data); +static int lldp_msg_cb(enum neu_event_io_type type, int fd, void *usr_data); +static char *lldp_parse_port_id(uint16_t n_byte, uint8_t *bytes); + +neu_conn_eth_t *neu_conn_eth_init(const char *interface, void *ctx) +{ + if (!mtx_init) { + pthread_mutex_init(&mtx, NULL); + mtx_init = true; + } + + neu_conn_eth_t *conn_eth = calloc(1, sizeof(neu_conn_eth_t)); + int i = 0; + bool find = false; + + pthread_mutex_lock(&mtx); + + for (i = 0; i < interface_max; i++) { + if (in_conns[i].interface != NULL && + strcmp(in_conns[i].interface, interface) == 0) { + in_conns[i].count += 1; + conn_eth->ic = &in_conns[i]; + find = true; + break; + } + } + if (!find) { + for (i = 0; i < interface_max; i++) { + if (in_conns[i].interface == NULL) { + neu_event_io_param_t param = { 0 }; + + in_conns[i].interface = strdup(interface); + in_conns[i].events = neu_event_new(); + in_conns[i].count += 1; + in_conns[i].profinet_fd = init_socket(interface, 0x8892); + in_conns[i].lldp_fd = init_socket(interface, ETH_P_LLDP); + get_mac(conn_eth, interface); + pthread_mutex_init(&in_conns[i].mtx, NULL); + conn_eth->ic = &in_conns[i]; + + param.fd = in_conns[i].profinet_fd; + param.usr_data = (void *) conn_eth; + param.cb = profinet_msg_cb; + + in_conns[i].profinet_event = + neu_event_add_io(in_conns[i].events, param); + + param.fd = in_conns[i].lldp_fd; + param.cb = lldp_msg_cb; + + in_conns[i].lldp_event = + neu_event_add_io(in_conns[i].events, param); + + break; + } + } + } + + pthread_mutex_unlock(&mtx); + + conn_eth->ctx = ctx; + return conn_eth; +} + +int neu_conn_eth_uninit(neu_conn_eth_t *conn) +{ + pthread_mutex_lock(&mtx); + conn->ic->count -= 1; + if (conn->ic->count == 0) { + neu_event_del_io(conn->ic->events, conn->ic->profinet_event); + neu_event_del_io(conn->ic->events, conn->ic->lldp_event); + neu_event_close(conn->ic->events); + + uninit_socket(conn->ic->profinet_fd); + uninit_socket(conn->ic->lldp_fd); + + free(conn->ic->interface); + pthread_mutex_destroy(&conn->ic->mtx); + } + pthread_mutex_unlock(&mtx); + + free(conn); + + return 0; +} + +int neu_conn_eth_check_interface(const char *interface) +{ + struct ifreq ifr = { 0 }; + int fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); + int ret = -1; + + strcpy(ifr.ifr_name, interface); + ret = ioctl(fd, SIOCGIFHWADDR, &ifr); + + close(fd); + return ret; +} + +neu_conn_eth_sub_t *neu_conn_eth_register(neu_conn_eth_t *conn, + uint16_t protocol, uint8_t mac[6], + neu_conn_eth_msg_callback callback) +{ + callback_elem_key_t key = { 0 }; + neu_conn_eth_sub_t *sub = NULL; + callback_elem_t * elem = NULL; + + key.protocol = protocol; + memcpy(key.mac, mac, ETH_ALEN); + + pthread_mutex_lock(&conn->ic->mtx); + + HASH_FIND(hh, conn->ic->callbacks, &key, sizeof(callback_elem_key_t), elem); + if (elem == NULL) { + elem = calloc(1, sizeof(callback_elem_t)); + sub = calloc(1, sizeof(neu_conn_eth_sub_t)); + + elem->key.protocol = protocol; + memcpy(key.mac, mac, ETH_ALEN); + elem->callback = callback; + + memcpy(sub->mac, mac, ETH_ALEN); + sub->protocol = protocol; + + HASH_ADD(hh, conn->ic->callbacks, key, sizeof(callback_elem_key_t), + elem); + } + + pthread_mutex_unlock(&conn->ic->mtx); + + return sub; +} + +int neu_conn_eth_unregister(neu_conn_eth_t *conn, neu_conn_eth_sub_t *sub) +{ + callback_elem_key_t key = { 0 }; + callback_elem_t * elem = NULL; + + key.protocol = sub->protocol; + memcpy(key.mac, sub->mac, ETH_ALEN); + + pthread_mutex_lock(&conn->ic->mtx); + + HASH_FIND(hh, conn->ic->callbacks, &key, sizeof(callback_elem_key_t), elem); + if (elem != NULL) { + HASH_DEL(conn->ic->callbacks, elem); + free(elem); + } + + pthread_mutex_unlock(&conn->ic->mtx); + + free(sub); + + return 0; +} + +int neu_conn_eth_register_lldp(neu_conn_eth_t *conn, const char *device, + neu_conn_eth_msg_callback callback) +{ + lldp_callback_elem_t *elem = NULL; + int ret = 0; + + pthread_mutex_lock(&conn->ic->mtx); + + HASH_FIND_STR(conn->ic->lldp_callbacks, device, elem); + if (elem != NULL) { + ret = -1; + } else { + elem = calloc(1, sizeof(lldp_callback_elem_t)); + + elem->callback = callback; + strcpy(elem->device, device); + + HASH_ADD_STR(conn->ic->lldp_callbacks, device, elem); + } + + pthread_mutex_unlock(&conn->ic->mtx); + + return ret; +} + +int neu_conn_eth_unregister_lldp(neu_conn_eth_t *conn, const char *device) +{ + lldp_callback_elem_t *elem = NULL; + + pthread_mutex_lock(&conn->ic->mtx); + HASH_FIND_STR(conn->ic->lldp_callbacks, device, elem); + if (elem != NULL) { + HASH_DEL(conn->ic->lldp_callbacks, elem); + free(elem); + } + pthread_mutex_unlock(&conn->ic->mtx); + + return 0; +} + +int neu_conn_eth_send(neu_conn_eth_t *conn, uint8_t dst_mac[6], + uint16_t protocol, uint16_t n_byte, uint8_t *bytes) +{ + int fd = 0; + int ret = 0; + struct ethhdr *ehdr = (struct ethhdr *) bytes; + + switch (protocol) { + case 0x8892: + fd = conn->ic->profinet_fd; + break; + case ETH_P_LLDP: + fd = conn->ic->lldp_fd; + break; + default: + assert(1 == 0); + break; + } + + memset(bytes, 0, sizeof(struct ethhdr)); + memcpy(ehdr->h_source, conn->ic->mac, ETH_ALEN); + memcpy(ehdr->h_dest, dst_mac, ETH_ALEN); + ehdr->h_proto = htons(protocol); + + ret = send(fd, bytes, n_byte, 0); + return ret; +} + +static int get_mac(neu_conn_eth_t *conn, const char *interface) +{ + struct ifreq ifr = { 0 }; + int fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); + int ret = -1; + + strcpy(ifr.ifr_name, interface); + if (ioctl(fd, SIOCGIFHWADDR, &ifr) == 0) { + memcpy(conn->ic->mac, ifr.ifr_hwaddr.sa_data, ETH_ALEN); + ret = 0; + } + + close(fd); + return ret; +} + +static int init_socket(const char *interface, uint16_t protocol) +{ + struct ifreq ifr = { 0 }; + struct sockaddr_ll sll = { 0 }; + struct packet_mreq mreq = { 0 }; + const uint8_t pn_mcast_addr[ETH_ALEN] = { + 0x01, 0x0e, 0xcf, 0x00, 0x00, 0x00 + }; + int fd = socket(PF_PACKET, SOCK_RAW, htons(protocol)); + int ret = 0; + + assert(fd > 0); + setsockopt(fd, SOL_SOCKET, SO_DONTROUTE, &ret, sizeof(ret)); + + strcpy(ifr.ifr_name, interface); + if (ioctl(fd, SIOCGIFINDEX, &ifr) != 0) { + close(fd); + return -1; + } + + sll.sll_family = AF_PACKET; + sll.sll_ifindex = ifr.ifr_ifindex; + sll.sll_protocol = htons(protocol); + + mreq.mr_ifindex = ifr.ifr_ifindex; + mreq.mr_type = PACKET_HOST | PACKET_MR_MULTICAST; + mreq.mr_alen = ETH_ALEN; + memcpy(mreq.mr_address, pn_mcast_addr, ETH_ALEN); + + ifr.ifr_flags = 0; + ioctl(fd, SIOCGIFFLAGS, &ifr); + + bind(fd, (struct sockaddr *) &sll, sizeof(sll)); + + if (protocol == 0x8892) { + ret = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, + sizeof(mreq)); + assert(ret == 0); + } + + return fd; +} +static int uninit_socket(int fd) +{ + close(fd); + + return 0; +} + +static int profinet_msg_cb(enum neu_event_io_type type, int fd, void *usr_data) +{ + neu_conn_eth_t *conn = (neu_conn_eth_t *) usr_data; + + switch (type) { + case NEU_EVENT_IO_READ: + break; + case NEU_EVENT_IO_CLOSED: + case NEU_EVENT_IO_HUP: + nlog_warn("eth conn profinet error type: %d, fd: %d(%s)", type, fd, + conn->ic->interface); + break; + } + + return 0; +} + +static int lldp_msg_cb(enum neu_event_io_type type, int fd, void *usr_data) +{ + neu_conn_eth_t *conn = (neu_conn_eth_t *) usr_data; + int ret = 0; + char * port_id = NULL; + + switch (type) { + case NEU_EVENT_IO_READ: + memset(conn->ic->buf, 0, sizeof(conn->ic->buf)); + ret = recv(fd, conn->ic->buf, sizeof(conn->ic->buf), 0); + port_id = lldp_parse_port_id(ret, conn->ic->buf); + + if (port_id != NULL) { + lldp_callback_elem_t *elem = NULL; + struct ethhdr * ehdr = (struct ethhdr *) conn->ic->buf; + + HASH_FIND_STR(conn->ic->callbacks, port_id, elem); + if (elem != NULL) { + elem->callback( + conn, conn->ctx, ETH_P_LLDP, ret - sizeof(struct ethhdr), + conn->ic->buf + sizeof(struct ethhdr), ehdr->h_source); + } + } + + break; + case NEU_EVENT_IO_CLOSED: + case NEU_EVENT_IO_HUP: + nlog_warn("eth conn lldp error type: %d, fd: %d(%s)", type, fd, + conn->ic->interface); + break; + } + + return 0; +} + +struct lldp_tvl { + uint16_t type : 7; + uint16_t length : 9; +} __attribute__((packed)); + +#define END_OF_LLDPDU_TLV_TYPE 0x00 +#define CHASSIS_ID_TLV_TYPE 0x01 +#define PORT_ID_TLV_TYPE 0x02 +#define TIME_TO_LIVE_TLV_TYPE 0x03 + +static char *lldp_parse_port_id(uint16_t n_byte, uint8_t *bytes) +{ + struct lldp_tvl *tvl = (struct lldp_tvl *) bytes; + + if (n_byte < sizeof(struct lldp_tvl) + tvl->length) { + return NULL; + } + + if (tvl->type == PORT_ID_TLV_TYPE) { + static __thread char port_id[NEU_NODE_NAME_LEN] = { 0 }; + uint8_t * data = (uint8_t *) &tvl[1]; + + strncpy(port_id, (char *) data + 1, tvl->length - 1); + return port_id; + } else { + return lldp_parse_port_id( + n_byte - sizeof(struct lldp_tvl) - tvl->length, + bytes + sizeof(struct lldp_tvl) + tvl->length); + } +} \ No newline at end of file diff --git a/src/main.c b/src/main.c index 4ec7ef3d..e908f9f9 100644 --- a/src/main.c +++ b/src/main.c @@ -48,6 +48,7 @@ static void sig_handler(int sig) } zlog_fini(); exit_flag = true; + exit(-1); } static int neuron_run(const neu_cli_args_t *args)