diff --git a/samples/net/dsa/CMakeLists.txt b/samples/net/dsa/CMakeLists.txt index 6f80b96f5e1..f22d38467c7 100644 --- a/samples/net/dsa/CMakeLists.txt +++ b/samples/net/dsa/CMakeLists.txt @@ -5,4 +5,5 @@ cmake_minimum_required(VERSION 3.20.0) find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(dsa) -target_sources(app PRIVATE src/main.c src/dsa_lldp.c) +target_sources(app PRIVATE src/main.c) +target_sources_ifdef(CONFIG_NET_SAMPLE_DSA_LLDP app PRIVATE src/dsa_lldp.c) diff --git a/samples/net/dsa/Kconfig b/samples/net/dsa/Kconfig new file mode 100644 index 00000000000..de09e85206f --- /dev/null +++ b/samples/net/dsa/Kconfig @@ -0,0 +1,25 @@ +# Private config options for DSA + +# Copyright 2024 NXP +# SPDX-License-Identifier: Apache-2.0 + +mainmenu "DSA sample application" + +if NET_DSA + +config NET_SAMPLE_DSA_MAX_SLAVE_PORTS + int "DSA slave ports maximum number" + range 2 10 + default 3 + help + DSA slave ports maximum number. + +config NET_SAMPLE_DSA_LLDP + bool "DSA LLDP example" + default y + help + Enable DSA LLDP example. + +endif + +source "Kconfig.zephyr" diff --git a/samples/net/dsa/src/dsa.h b/samples/net/dsa/src/dsa.h new file mode 100644 index 00000000000..a279293e74f --- /dev/null +++ b/samples/net/dsa/src/dsa.h @@ -0,0 +1,22 @@ +/* + * Copyright (c) 2020 DENX Software Engineering GmbH + * Copyright 2024 NXP + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __DSA_SAMPLE__ +#define __DSA_SAMPLE__ + +#include +#include + +extern struct ud user_data; + +/* User data for the interface callback */ +struct ud { + struct net_if *lan[CONFIG_NET_SAMPLE_DSA_MAX_SLAVE_PORTS]; + struct net_if *master; +}; + +#endif diff --git a/samples/net/dsa/src/dsa_lldp.c b/samples/net/dsa/src/dsa_lldp.c index 775cb02295f..3a25f683fa9 100644 --- a/samples/net/dsa/src/dsa_lldp.c +++ b/samples/net/dsa/src/dsa_lldp.c @@ -18,17 +18,83 @@ #include #include -#include "main.h" /* Loglevel of dsa_lldp function */ -LOG_MODULE_DECLARE(net_dsa_lldp_sample, CONFIG_NET_DSA_LOG_LEVEL); +LOG_MODULE_DECLARE(net_dsa_sample, CONFIG_NET_DSA_LOG_LEVEL); + +#include "dsa_lldp.h" #define LLDP_SYSTEM_NAME_SIZE 24 #define LLDP_ETHER_TYPE 0x88CC #define LLDP_INPUT_DATA_BUF_SIZE 512 #define DSA_BUF_SIZ 128 -int dsa_lldp_send(struct net_if *iface, struct instance_data *pd, uint16_t lan, int src_port, - int origin_port, int cmd, struct eth_addr *origin_addr) + +static const uint8_t eth_filter_l2_addr_base[][6] = { + /* MAC address of other device - for filtering testing */ + {0x01, 0x80, 0xc2, 0x00, 0x00, 0x03}}; + +enum net_verdict dsa_ll_addr_switch_cb(struct net_if *iface, struct net_pkt *pkt) +{ + struct net_eth_hdr *hdr = NET_ETH_HDR(pkt); + struct net_linkaddr lladst; + + net_pkt_cursor_init(pkt); + lladst.len = sizeof(hdr->dst.addr); + lladst.addr = &hdr->dst.addr[0]; + + /* + * Pass packet to lan1..3 when matching one from + * check_ll_ether_addr table + */ + if (check_ll_ether_addr(lladst.addr, ð_filter_l2_addr_base[0][0])) { + return NET_CONTINUE; + } + + return NET_OK; +} + +int start_slave_port_packet_socket(struct net_if *iface, struct instance_data *pd) +{ + struct sockaddr_ll dst; + int ret; + + pd->sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); + if (pd->sock < 0) { + LOG_ERR("Failed to create RAW socket : %d", errno); + return -errno; + } + + dst.sll_ifindex = net_if_get_by_iface(iface); + dst.sll_family = AF_PACKET; + + ret = bind(pd->sock, (const struct sockaddr *)&dst, sizeof(struct sockaddr_ll)); + if (ret < 0) { + LOG_ERR("Failed to bind packet socket : %d", errno); + return -errno; + } + + return 0; +} + +void dsa_lldp(struct ud *user_data) +{ + uint8_t tbl_buf[8]; + + /* + * Set static table to forward LLDP protocol packets + * to master port. + */ + dsa_switch_set_mac_table_entry(user_data->lan[0], ð_filter_l2_addr_base[0][0], BIT(4), 0, + 0); + dsa_switch_get_mac_table_entry(user_data->lan[0], tbl_buf, 0); + + LOG_INF("DSA static MAC address table entry [%d]:", 0); + LOG_INF("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x", tbl_buf[7], tbl_buf[6], tbl_buf[5], + tbl_buf[4], tbl_buf[3], tbl_buf[2], tbl_buf[1], tbl_buf[0]); +} + +static int dsa_lldp_send(struct net_if *iface, struct instance_data *pd, uint16_t lan, int src_port, + int origin_port, int cmd, struct eth_addr *origin_addr) { int ret, len; char buffer[DSA_BUF_SIZ], sys_name[LLDP_SYSTEM_NAME_SIZE]; @@ -91,7 +157,7 @@ int dsa_lldp_send(struct net_if *iface, struct instance_data *pd, uint16_t lan, return 0; } -void dsa_lldp_print_info(uint8_t *lldp_p, uint8_t lanid) +static void dsa_lldp_print_info(uint8_t *lldp_p, uint8_t lanid) { uint16_t tl, length; uint8_t type, subtype; @@ -145,8 +211,8 @@ void dsa_lldp_print_info(uint8_t *lldp_p, uint8_t lanid) } while (1); } -int dsa_lldp_recv(struct net_if *iface, struct instance_data *pd, uint16_t *lan, int *origin_port, - struct eth_addr *origin_addr) +static int dsa_lldp_recv(struct net_if *iface, struct instance_data *pd, uint16_t *lan, + int *origin_port, struct eth_addr *origin_addr) { struct ethernet_context *ctx = net_if_l2_data(iface); struct net_eth_hdr *eth_hdr = (struct net_eth_hdr *)pd->recv_buffer; diff --git a/samples/net/dsa/src/dsa_lldp.h b/samples/net/dsa/src/dsa_lldp.h new file mode 100644 index 00000000000..c66202b7e40 --- /dev/null +++ b/samples/net/dsa/src/dsa_lldp.h @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2020 DENX Software Engineering GmbH + * Copyright 2024 NXP + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __DSA_SAMPLE_LLDP__ +#define __DSA_SAMPLE_LLDP__ + +#include "dsa.h" + +#define MCAST_DEST_MAC0 0x01 +#define MCAST_DEST_MAC1 0x80 +#define MCAST_DEST_MAC2 0xc2 +#define MCAST_DEST_MAC3 0x00 +#define MCAST_DEST_MAC4 0x00 +#define MCAST_DEST_MAC5 0x03 + +#define RECV_BUFFER_SIZE 1280 +#define ETH_ALEN 6 +#define PACKET_LEN 128 + +struct eth_addr { + uint8_t addr[ETH_ALEN]; /* origin hardware address */ +}; + +struct instance_data { + char *if_name; + int sock; + char recv_buffer[RECV_BUFFER_SIZE]; +}; + +static inline bool check_ll_ether_addr(const uint8_t *a, const uint8_t *b) +{ + return ((a[0] ^ b[0]) | (a[1] ^ b[1]) | (a[2] ^ b[2]) | (a[3] ^ b[3]) | (a[4] ^ b[4]) | + (a[5] ^ b[5])) == 0; +} + +static inline void dsa_buf_write_be16(uint16_t tl, uint8_t **p) +{ + uint8_t *v = (uint8_t *)&tl; + **p = v[1]; + (*p)++; + **p = v[0]; + (*p)++; +} + +int start_slave_port_packet_socket(struct net_if *iface, struct instance_data *pd); + +enum net_verdict dsa_ll_addr_switch_cb(struct net_if *iface, struct net_pkt *pkt); + +void dsa_lldp(struct ud *user_data); + +#define CMD_DISCOVER 0 +#define CMD_ACK 1 +#define DSA_STACK_SIZE 4096 +#define DSA_PRIORITY 5 +#define DSA_THREAD_START_DELAY 4000 + +#define DSA_THREAD(ID, FN_RECV, FN_SEND) \ + static void dsa_thread_##ID(void *t1, void *t2, void *t3); \ + K_THREAD_DEFINE(dsa_tid_##ID, DSA_STACK_SIZE, dsa_thread_##ID, NULL, NULL, NULL, \ + DSA_PRIORITY, 0, DSA_THREAD_START_DELAY); \ + \ + void dsa_thread_##ID(void *t1, void *t2, void *t3) \ + { \ + int origin_port, ret; \ + uint16_t seq; \ + struct eth_addr origin_addr; \ + struct instance_data data; \ + struct net_if *iface; \ + \ + iface = user_data.lan[ID - 1]; \ + \ + data.if_name = "lan" #ID; \ + ret = start_slave_port_packet_socket(iface, &data); \ + if (ret < 0) { \ + LOG_ERR("start_slave_port_packet_socket failed %d", ret); \ + return; \ + } \ + dsa_register_recv_callback(iface, dsa_ll_addr_switch_cb); \ + \ + LOG_INF("DSA -> eth/lan" #ID " idx: %d sock: %d", net_if_get_by_iface(iface), \ + data.sock); \ + do { \ + ret = FN_RECV(iface, &data, &seq, &origin_port, &origin_addr); \ + if (ret) { \ + break; \ + } \ + ret = FN_SEND(iface, &data, seq, 0, origin_port, CMD_ACK, &origin_addr); \ + if (ret) { \ + break; \ + } \ + } while (true); \ + } + +#endif /* __DSA_SAMPLE_LLDP__ */ diff --git a/samples/net/dsa/src/main.c b/samples/net/dsa/src/main.c index 9d466288250..81cf5bb03da 100644 --- a/samples/net/dsa/src/main.c +++ b/samples/net/dsa/src/main.c @@ -5,12 +5,17 @@ */ #include -LOG_MODULE_REGISTER(net_dsa_lldp_sample, CONFIG_NET_DSA_LOG_LEVEL); +LOG_MODULE_REGISTER(net_dsa_sample, CONFIG_NET_DSA_LOG_LEVEL); -#include -#include "main.h" +#include "dsa.h" -static void iface_cb(struct net_if *iface, void *user_data) +#if defined(CONFIG_NET_SAMPLE_DSA_LLDP) +#include "dsa_lldp.h" +#endif + +struct ud user_data; + +static void dsa_iface_find_cb(struct net_if *iface, void *user_data) { struct ud *ifaces = user_data; @@ -39,87 +44,15 @@ static void iface_cb(struct net_if *iface, void *user_data) } } -static const uint8_t eth_filter_l2_addr_base[][6] = { - /* MAC address of other device - for filtering testing */ - { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x03 } -}; - -enum net_verdict dsa_ll_addr_switch_cb(struct net_if *iface, - struct net_pkt *pkt) -{ - struct net_eth_hdr *hdr = NET_ETH_HDR(pkt); - struct net_linkaddr lladst; - - net_pkt_cursor_init(pkt); - lladst.len = sizeof(hdr->dst.addr); - lladst.addr = &hdr->dst.addr[0]; - - /* - * Pass packet to lan1..3 when matching one from - * check_ll_ether_addr table - */ - if (check_ll_ether_addr(lladst.addr, ð_filter_l2_addr_base[0][0])) { - return 1; - } - - return 0; -} - -int start_slave_port_packet_socket(struct net_if *iface, - struct instance_data *pd) -{ - struct sockaddr_ll dst; - int ret; - - pd->sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); - if (pd->sock < 0) { - LOG_ERR("Failed to create RAW socket : %d", errno); - return -errno; - } - - dst.sll_ifindex = net_if_get_by_iface(iface); - dst.sll_family = AF_PACKET; - - ret = bind(pd->sock, (const struct sockaddr *)&dst, - sizeof(struct sockaddr_ll)); - if (ret < 0) { - LOG_ERR("Failed to bind packet socket : %d", errno); - return -errno; - } - - return 0; -} - -struct ud user_data_ifaces; -static int init_dsa_ports(void) -{ - uint8_t tbl_buf[8]; - - /* Initialize interfaces - read them to user_data_ifaces */ - (void)memset(&user_data_ifaces, 0, sizeof(user_data_ifaces)); - net_if_foreach(iface_cb, &user_data_ifaces); - - /* - * Set static table to forward LLDP protocol packets - * to master port. - */ - dsa_switch_set_mac_table_entry(user_data_ifaces.lan[0], - ð_filter_l2_addr_base[0][0], - BIT(4), 0, 0); - dsa_switch_get_mac_table_entry(user_data_ifaces.lan[0], tbl_buf, 0); - - LOG_INF("DSA static MAC address table entry [%d]:", 0); - LOG_INF("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x", - tbl_buf[7], tbl_buf[6], tbl_buf[5], tbl_buf[4], - tbl_buf[3], tbl_buf[2], tbl_buf[1], tbl_buf[0]); - - return 0; -} - int main(void) { - init_dsa_ports(); + /* Initialize interfaces - read them to user_data */ + (void)memset(&user_data, 0, sizeof(user_data)); + net_if_foreach(dsa_iface_find_cb, &user_data); +#if defined(CONFIG_NET_SAMPLE_DSA_LLDP) + dsa_lldp(&user_data); +#endif LOG_INF("DSA ports init - OK"); return 0; } diff --git a/samples/net/dsa/src/main.h b/samples/net/dsa/src/main.h deleted file mode 100644 index 5d8cd70e6ad..00000000000 --- a/samples/net/dsa/src/main.h +++ /dev/null @@ -1,118 +0,0 @@ -/* - * Copyright (c) 2020 DENX Software Engineering GmbH - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#ifndef __DSA_SAMPLE__ -#define __DSA_SAMPLE__ - -#include -#include - -#include -#include -#include -#include -#include - -#define MCAST_DEST_MAC0 0x01 -#define MCAST_DEST_MAC1 0x80 -#define MCAST_DEST_MAC2 0xc2 -#define MCAST_DEST_MAC3 0x00 -#define MCAST_DEST_MAC4 0x00 -#define MCAST_DEST_MAC5 0x03 - -#define RECV_BUFFER_SIZE 1280 -#define ETH_ALEN 6 -#define PACKET_LEN 128 - -extern struct ud user_data_ifaces; - -struct eth_addr { - uint8_t addr[ETH_ALEN]; /* origin hardware address */ -}; - -struct instance_data { - char *if_name; - int sock; - char recv_buffer[RECV_BUFFER_SIZE]; -}; - -/* User data for the interface callback */ -struct ud { - struct net_if *lan[3]; - struct net_if *master; -}; - -static inline bool check_ll_ether_addr(const uint8_t *a, const uint8_t *b) -{ - return ((a[0] ^ b[0]) | (a[1] ^ b[1]) | (a[2] ^ b[2]) | - (a[3] ^ b[3]) | (a[4] ^ b[4]) | (a[5] ^ b[5])) == 0; -} - -static inline void dsa_buf_write_be16(uint16_t tl, uint8_t **p) -{ - uint8_t *v = (uint8_t *) &tl; - **p = v[1]; - (*p)++; - **p = v[0]; - (*p)++; -} - -int start_slave_port_packet_socket(struct net_if *iface, - struct instance_data *pd); - -enum net_verdict dsa_ll_addr_switch_cb(struct net_if *iface, - struct net_pkt *pkt); - -#define CMD_DISCOVER 0 -#define CMD_ACK 1 -#define DSA_STACK_SIZE 4096 -#define DSA_PRIORITY 5 -#define DSA_THREAD_START_DELAY 4000 - -#define DSA_THREAD(ID, FN_RECV, FN_SEND) \ - static void dsa_thread_##ID(void *t1, void *t2, void *t3); \ - K_THREAD_DEFINE(dsa_tid_##ID, DSA_STACK_SIZE, \ - dsa_thread_##ID, NULL, NULL, NULL, \ - DSA_PRIORITY, 0, DSA_THREAD_START_DELAY); \ - \ - void dsa_thread_##ID(void *t1, void *t2, void *t3) \ - { \ - int origin_port, ret; \ - uint16_t seq; \ - struct eth_addr origin_addr; \ - struct instance_data data; \ - struct net_if *iface; \ - \ - iface = user_data_ifaces.lan[ID-1]; \ - \ - data.if_name = "lan"#ID; \ - ret = start_slave_port_packet_socket(iface, &data); \ - if (ret < 0) { \ - LOG_ERR("start_slave_port_packet_socket failed %d", \ - ret); \ - return; \ - } \ - dsa_register_recv_callback(iface, \ - dsa_ll_addr_switch_cb); \ - \ - LOG_INF("DSA -> eth/lan"#ID" idx: %d sock: %d", \ - net_if_get_by_iface(iface), data.sock); \ - do { \ - ret = FN_RECV(iface, &data, &seq, \ - &origin_port, &origin_addr); \ - if (ret) { \ - break; \ - } \ - ret = FN_SEND(iface, &data, \ - seq, 0, origin_port, CMD_ACK, \ - &origin_addr); \ - if (ret) { \ - break; \ - } \ - } while (true); \ - } - -#endif /* __DSA_SAMPLE__ */