diff --git a/Core/Src/main.c b/Core/Src/main.c index cfa93a0..94c53c9 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -149,6 +149,19 @@ static void BootDiag_ReportCh390(void) cfg->net.mask[0], cfg->net.mask[1], cfg->net.mask[2], cfg->net.mask[3], cfg->net.gw[0], cfg->net.gw[1], cfg->net.gw[2], cfg->net.gw[3], cfg->mux_mode); + SEGGER_RTT_printf(0, + "ETH rx ok=%u drop=%u pbuf_fail=%u filt=%u ipv6=%u udp=%u igmp=%u lldp=%u other_eth=%u other_ipv4=%u last=0x%04X\r\n", + (unsigned int)diag.rx_packets_ok, + (unsigned int)diag.rx_packets_drop, + (unsigned int)diag.rx_pbuf_alloc_failed, + (unsigned int)diag.rx_filtered_frames, + (unsigned int)diag.rx_filtered_ipv6, + (unsigned int)diag.rx_filtered_udp, + (unsigned int)diag.rx_filtered_igmp, + (unsigned int)diag.rx_filtered_lldp, + (unsigned int)diag.rx_filtered_other_eth, + (unsigned int)diag.rx_filtered_other_ipv4, + diag.last_eth_type); } static void App_ConfigureLinks(const device_config_t *cfg) diff --git a/Drivers/CH390/ch390_runtime.c b/Drivers/CH390/ch390_runtime.c index 403cb4e..2ce0135 100644 --- a/Drivers/CH390/ch390_runtime.c +++ b/Drivers/CH390/ch390_runtime.c @@ -22,11 +22,18 @@ static uint8_t ch390_runtime_drain_rx(struct netif *netif, uint8_t max_frames) { struct pbuf *p; uint8_t drained = 0u; + uint8_t rx_ready; while (drained < max_frames) { p = ch390_runtime_input_frame(netif); if (p == NULL) { - break; + ch390_read_reg(CH390_MRCMDX); + rx_ready = ch390_read_reg(CH390_MRCMDX); + if ((rx_ready & CH390_PKT_RDY) == 0u) { + break; + } + drained++; + continue; } ch390_runtime_dispatch_frame(netif, p); drained++; @@ -49,7 +56,179 @@ static uint8_t g_link_restart_pending; #define HEALTH_FAIL_SHIFT 4u #define HEALTH_FAIL_MASK 0xF0u +#define CH390_RX_FILTER_ENABLE 1u +#define CH390_RX_PREFIX_LEN 38u +#define CH390_ETH_HEADER_LEN 14u +#define CH390_IPV4_MIN_HEADER_LEN 20u +#define CH390_ETH_TYPE_IPV4 0x0800u +#define CH390_ETH_TYPE_ARP 0x0806u +#define CH390_ETH_TYPE_VLAN 0x8100u +#define CH390_ETH_TYPE_IPV6 0x86DDu +#define CH390_ETH_TYPE_QINQ 0x88A8u +#define CH390_ETH_TYPE_LLDP 0x88CCu +#define CH390_IP_PROTO_ICMP 1u +#define CH390_IP_PROTO_IGMP 2u +#define CH390_IP_PROTO_TCP 6u +#define CH390_IP_PROTO_UDP 17u + +typedef enum { + CH390_RX_ACCEPT = 0, + CH390_RX_DROP_MALFORMED, + CH390_RX_DROP_IPV6, + CH390_RX_DROP_LLDP, + CH390_RX_DROP_UDP, + CH390_RX_DROP_IGMP, + CH390_RX_DROP_OTHER_ETH, + CH390_RX_DROP_OTHER_IPV4 +} ch390_rx_filter_result_t; + static bool ch390_mac_address_valid(const uint8_t *mac); +static ch390_rx_filter_result_t ch390_runtime_filter_frame(const uint8_t *prefix, uint16_t frame_len, uint16_t prefix_len); +static void ch390_runtime_count_filtered_frame(ch390_rx_filter_result_t result); +static void ch390_runtime_copy_prefix_to_pbuf(struct pbuf *p, const uint8_t *prefix, uint16_t prefix_len); +static void ch390_runtime_read_remaining_to_pbuf(struct pbuf *p, uint16_t offset); + +static ch390_rx_filter_result_t ch390_runtime_filter_frame(const uint8_t *prefix, uint16_t frame_len, uint16_t prefix_len) +{ + uint16_t eth_type; + uint16_t l2_header_len = CH390_ETH_HEADER_LEN; + uint16_t ip_offset; + uint8_t ip_version; + uint8_t ip_ihl; + uint8_t ip_proto; + + if ((prefix == NULL) || (frame_len < CH390_ETH_HEADER_LEN) || (prefix_len < CH390_ETH_HEADER_LEN)) { + return CH390_RX_DROP_MALFORMED; + } + + eth_type = (uint16_t)(((uint16_t)prefix[12] << 8) | prefix[13]); + g_diag.last_eth_type = eth_type; + + if ((eth_type == CH390_ETH_TYPE_VLAN) || (eth_type == CH390_ETH_TYPE_QINQ)) { + if ((frame_len < (CH390_ETH_HEADER_LEN + 4u)) || (prefix_len < (CH390_ETH_HEADER_LEN + 4u))) { + return CH390_RX_DROP_MALFORMED; + } + l2_header_len = (uint16_t)(CH390_ETH_HEADER_LEN + 4u); + eth_type = (uint16_t)(((uint16_t)prefix[16] << 8) | prefix[17]); + g_diag.last_eth_type = eth_type; + } + + if (eth_type == CH390_ETH_TYPE_ARP) { + g_diag.rx_arp_frames++; + return CH390_RX_ACCEPT; + } + + if (eth_type == CH390_ETH_TYPE_IPV6) { + return CH390_RX_DROP_IPV6; + } + + if (eth_type == CH390_ETH_TYPE_LLDP) { + return CH390_RX_DROP_LLDP; + } + + if (eth_type != CH390_ETH_TYPE_IPV4) { + return CH390_RX_DROP_OTHER_ETH; + } + + ip_offset = l2_header_len; + if ((frame_len < (uint16_t)(ip_offset + CH390_IPV4_MIN_HEADER_LEN)) || + (prefix_len < (uint16_t)(ip_offset + CH390_IPV4_MIN_HEADER_LEN))) { + return CH390_RX_DROP_MALFORMED; + } + + ip_version = (uint8_t)(prefix[ip_offset] >> 4); + ip_ihl = (uint8_t)(prefix[ip_offset] & 0x0Fu); + if ((ip_version != 4u) || (ip_ihl < 5u)) { + return CH390_RX_DROP_MALFORMED; + } + + ip_proto = prefix[ip_offset + 9u]; + g_diag.rx_ip_frames++; + + if (ip_proto == CH390_IP_PROTO_ICMP) { + g_diag.rx_ipv4_icmp_frames++; + return CH390_RX_ACCEPT; + } + + if (ip_proto == CH390_IP_PROTO_TCP) { + g_diag.rx_ipv4_tcp_frames++; + return CH390_RX_ACCEPT; + } + + if (ip_proto == CH390_IP_PROTO_UDP) { + g_diag.rx_ipv4_udp_frames++; + return CH390_RX_DROP_UDP; + } + + if (ip_proto == CH390_IP_PROTO_IGMP) { + g_diag.rx_filtered_igmp++; + return CH390_RX_DROP_IGMP; + } + + return CH390_RX_DROP_OTHER_IPV4; +} + +static void ch390_runtime_count_filtered_frame(ch390_rx_filter_result_t result) +{ + g_diag.rx_filtered_frames++; + switch (result) { + case CH390_RX_DROP_MALFORMED: + g_diag.rx_filtered_malformed++; + break; + case CH390_RX_DROP_IPV6: + g_diag.rx_filtered_ipv6++; + break; + case CH390_RX_DROP_LLDP: + g_diag.rx_filtered_lldp++; + break; + case CH390_RX_DROP_UDP: + g_diag.rx_filtered_udp++; + break; + case CH390_RX_DROP_IGMP: + break; + case CH390_RX_DROP_OTHER_ETH: + g_diag.rx_filtered_other_eth++; + break; + case CH390_RX_DROP_OTHER_IPV4: + g_diag.rx_filtered_other_ipv4++; + break; + case CH390_RX_ACCEPT: + default: + break; + } +} + +static void ch390_runtime_copy_prefix_to_pbuf(struct pbuf *p, const uint8_t *prefix, uint16_t prefix_len) +{ + struct pbuf *q; + uint16_t copied = 0u; + + for (q = p; (q != NULL) && (copied < prefix_len); q = q->next) { + uint16_t chunk = (uint16_t)(prefix_len - copied); + if (chunk > q->len) { + chunk = q->len; + } + memcpy(q->payload, &prefix[copied], chunk); + copied = (uint16_t)(copied + chunk); + } +} + +static void ch390_runtime_read_remaining_to_pbuf(struct pbuf *p, uint16_t offset) +{ + struct pbuf *q; + uint16_t skipped = 0u; + + for (q = p; q != NULL; q = q->next) { + if (skipped >= offset) { + ch390_read_mem((uint8_t *)q->payload, q->len); + } else if ((uint16_t)(skipped + q->len) > offset) { + uint16_t in_chunk_offset = (uint16_t)(offset - skipped); + uint16_t read_len = (uint16_t)(q->len - in_chunk_offset); + ch390_read_mem(&((uint8_t *)q->payload)[in_chunk_offset], read_len); + } + skipped = (uint16_t)(skipped + q->len); + } +} static uint8_t ch390_runtime_is_restart_pending(void) { @@ -154,12 +333,14 @@ struct pbuf *ch390_runtime_input_frame(struct netif *netif) { struct ethernetif *ethernetif = (struct ethernetif *)netif->state; struct pbuf *p = NULL; - struct pbuf *q; uint16_t len; uint16_t frame_len; + uint16_t prefix_len; uint8_t rcr; uint8_t rx_ready; uint8_t rx_header[4]; + uint8_t frame_prefix[CH390_RX_PREFIX_LEN]; + ch390_rx_filter_result_t filter_result; ch390_read_reg(CH390_MRCMDX); rx_ready = ch390_read_reg(CH390_MRCMDX); @@ -196,6 +377,23 @@ struct pbuf *ch390_runtime_input_frame(struct netif *netif) } ethernetif->rx_len = frame_len; + prefix_len = frame_len; + if (prefix_len > CH390_RX_PREFIX_LEN) { + prefix_len = CH390_RX_PREFIX_LEN; + } + ch390_read_mem(frame_prefix, prefix_len); + +#if CH390_RX_FILTER_ENABLE + filter_result = ch390_runtime_filter_frame(frame_prefix, frame_len, prefix_len); + if (filter_result != CH390_RX_ACCEPT) { + ch390_drop_packet((uint16_t)(frame_len - prefix_len)); + LINK_STATS_INC(link.drop); + g_diag.rx_packets_drop++; + ch390_runtime_count_filtered_frame(filter_result); + return NULL; + } +#endif + len = ethernetif->rx_len; #if ETH_PAD_SIZE len += ETH_PAD_SIZE; @@ -206,9 +404,8 @@ struct pbuf *ch390_runtime_input_frame(struct netif *netif) #if ETH_PAD_SIZE pbuf_remove_header(p, ETH_PAD_SIZE); #endif - for (q = p; q != NULL; q = q->next) { - ch390_read_mem((uint8_t *)q->payload, q->len); - } + ch390_runtime_copy_prefix_to_pbuf(p, frame_prefix, prefix_len); + ch390_runtime_read_remaining_to_pbuf(p, prefix_len); #if ETH_PAD_SIZE pbuf_add_header(p, ETH_PAD_SIZE); #endif @@ -218,10 +415,11 @@ struct pbuf *ch390_runtime_input_frame(struct netif *netif) g_diag.last_frame_len = frame_len; g_diag.last_payload_len = p->tot_len; } else { - ch390_drop_packet(ethernetif->rx_len); + ch390_drop_packet((uint16_t)(frame_len - prefix_len)); LINK_STATS_INC(link.memerr); LINK_STATS_INC(link.drop); g_diag.rx_packets_drop++; + g_diag.rx_pbuf_alloc_failed++; } return p; diff --git a/Drivers/CH390/ch390_runtime.h b/Drivers/CH390/ch390_runtime.h index 9bf6498..727b4e3 100644 --- a/Drivers/CH390/ch390_runtime.h +++ b/Drivers/CH390/ch390_runtime.h @@ -38,8 +38,20 @@ typedef struct { uint32_t rx_packets_drop; uint32_t tx_packets_ok; uint32_t tx_packets_timeout; + uint32_t rx_pbuf_alloc_failed; + uint32_t rx_filtered_frames; + uint32_t rx_filtered_ipv6; + uint32_t rx_filtered_udp; + uint32_t rx_filtered_igmp; + uint32_t rx_filtered_lldp; + uint32_t rx_filtered_other_eth; + uint32_t rx_filtered_other_ipv4; + uint32_t rx_filtered_malformed; uint32_t rx_arp_frames; uint32_t rx_ip_frames; + uint32_t rx_ipv4_icmp_frames; + uint32_t rx_ipv4_tcp_frames; + uint32_t rx_ipv4_udp_frames; uint32_t rx_other_frames; uint32_t rx_unicast_self_frames; uint32_t rx_broadcast_frames;