--- tme/host/bsd/bsd-bpf.c 2018/04/24 16:37:52 1.1 +++ tme/host/bsd/bsd-bpf.c 2018/04/24 16:42:22 1.1.1.4 @@ -1,4 +1,4 @@ -/* $Id: bsd-bpf.c,v 1.1 2018/04/24 16:37:52 root Exp $ */ +/* $Id: bsd-bpf.c,v 1.1.1.4 2018/04/24 16:42:22 root Exp $ */ /* host/bsd/bsd-bpf.c - BSD Berkeley Packet Filter Ethernet support: */ @@ -34,11 +34,13 @@ */ #include -_TME_RCSID("$Id: bsd-bpf.c,v 1.1 2018/04/24 16:37:52 root Exp $"); +_TME_RCSID("$Id: bsd-bpf.c,v 1.1.1.4 2018/04/24 16:42:22 root Exp $"); /* includes: */ #include "bsd-impl.h" #include +#include +#include #include #include #include @@ -74,6 +76,12 @@ _TME_RCSID("$Id: bsd-bpf.c,v 1.1 2018/04 /* macros: */ +/* ARP and RARP opcodes: */ +#define TME_NET_ARP_OPCODE_REQUEST (0x0001) +#define TME_NET_ARP_OPCODE_REPLY (0x0002) +#define TME_NET_ARP_OPCODE_REV_REQUEST (0x0003) +#define TME_NET_ARP_OPCODE_REV_REPLY (0x0004) + /* the callout flags: */ #define TME_BSD_BPF_CALLOUT_CHECK (0) #define TME_BSD_BPF_CALLOUT_RUNNING TME_BIT(0) @@ -92,9 +100,8 @@ struct tme_bsd_bpf { /* our mutex: */ tme_mutex_t tme_bsd_bpf_mutex; - /* our reader and writer conditions: */ + /* our reader condition: */ tme_cond_t tme_bsd_bpf_cond_reader; - tme_cond_t tme_bsd_bpf_cond_writer; /* the callout flags: */ unsigned int tme_bsd_bpf_callout_flags; @@ -115,6 +122,34 @@ struct tme_bsd_bpf { in the packet buffer: */ size_t tme_bsd_bpf_buffer_offset; size_t tme_bsd_bpf_buffer_end; + + /* when nonzero, the packet delay time, in microseconds: */ + unsigned long tme_bsd_bpf_delay_time; + + /* all packets received on or before this time can be released: */ + struct timeval tme_bsd_bpf_delay_release; + + /* when nonzero, the packet delay sleep time, in microseconds: */ + unsigned long tme_bsd_bpf_delay_sleep; + + /* when nonzero, the packet delay is sleeping: */ + int tme_bsd_bpf_delay_sleeping; +}; + +/* a crude ARP header: */ +struct tme_net_arp_header { + tme_uint8_t tme_net_arp_header_hardware[2]; + tme_uint8_t tme_net_arp_header_protocol[2]; + tme_uint8_t tme_net_arp_header_hardware_length; + tme_uint8_t tme_net_arp_header_protocol_length; + tme_uint8_t tme_net_arp_header_opcode[2]; +}; + +/* a crude partial IPv4 header: */ +struct tme_net_ipv4_header { + tme_uint8_t tme_net_ipv4_header_v_hl; + tme_uint8_t tme_net_ipv4_header_tos; + tme_uint8_t tme_net_ipv4_header_length[2]; }; /* the accept and reject packet insns: */ @@ -204,7 +239,7 @@ _tme_bsd_bpf_filter(struct tme_ethernet_ void _tme_bsd_bpf_dump_filter(const struct bpf_program *program) { - int pc; + unsigned int pc; FILE *fp; const struct bpf_insn *insn; char ldsize; @@ -212,7 +247,7 @@ _tme_bsd_bpf_dump_filter(const struct bp fp = stderr; for (pc = 0, insn = program->bf_insns; - pc < program->bf_len; + pc < (unsigned int) program->bf_len; pc++, insn++) { /* the PC: */ @@ -281,6 +316,7 @@ _tme_bsd_bpf_callout(struct tme_bsd_bpf int callouts, later_callouts; unsigned int ctrl; int rc; + int status; tme_ethernet_fid_t frame_id; struct tme_ethernet_frame_chunk frame_chunk_buffer; tme_uint8_t frame[TME_ETHERNET_FRAME_MAX]; @@ -367,8 +403,14 @@ _tme_bsd_bpf_callout(struct tme_bsd_bpf /* if the read was successful: */ if (rc > 0) { + /* check the size of the frame: */ + assert(rc <= sizeof(frame)); + /* do the write: */ - tme_thread_write(bpf->tme_bsd_bpf_fd, frame, rc); + status = tme_thread_write(bpf->tme_bsd_bpf_fd, frame, rc); + + /* writes must succeed: */ + assert (status == rc); /* mark that we need to loop to callout to read more frames: */ bpf->tme_bsd_bpf_callout_flags |= TME_BSD_BPF_CALLOUT_READ; @@ -390,6 +432,7 @@ static void _tme_bsd_bpf_th_reader(struct tme_bsd_bpf *bpf) { ssize_t buffer_end; + unsigned long sleep_usec; /* lock the mutex: */ tme_mutex_lock(&bpf->tme_bsd_bpf_mutex); @@ -397,11 +440,45 @@ _tme_bsd_bpf_th_reader(struct tme_bsd_bp /* loop forever: */ for (;;) { - /* if the buffer is not empty, wait until it is: */ + /* if the delay sleeping flag is set: */ + if (bpf->tme_bsd_bpf_delay_sleeping) { + + /* clear the delay sleeping flag: */ + bpf->tme_bsd_bpf_delay_sleeping = FALSE; + + /* call out that we can be read again: */ + _tme_bsd_bpf_callout(bpf, TME_BSD_BPF_CALLOUT_CTRL); + } + + /* if a delay has been requested: */ + sleep_usec = bpf->tme_bsd_bpf_delay_sleep; + if (sleep_usec > 0) { + + /* clear the delay sleep time: */ + bpf->tme_bsd_bpf_delay_sleep = 0; + + /* set the delay sleeping flag: */ + bpf->tme_bsd_bpf_delay_sleeping = TRUE; + + /* unlock our mutex: */ + tme_mutex_unlock(&bpf->tme_bsd_bpf_mutex); + + /* sleep for the delay sleep time: */ + tme_thread_sleep_yield(0, sleep_usec); + + /* lock our mutex: */ + tme_mutex_lock(&bpf->tme_bsd_bpf_mutex); + + continue; + } + + /* if the buffer is not empty, wait until either it is, + or we're asked to do a delay: */ if (bpf->tme_bsd_bpf_buffer_offset < bpf->tme_bsd_bpf_buffer_end) { tme_cond_wait_yield(&bpf->tme_bsd_bpf_cond_reader, &bpf->tme_bsd_bpf_mutex); + continue; } /* unlock the mutex: */ @@ -562,6 +639,11 @@ _tme_bsd_bpf_read(struct tme_ethernet_co struct tme_bsd_bpf *bpf; struct bpf_hdr the_bpf_header; struct tme_ethernet_frame_chunk frame_chunk_buffer; + size_t buffer_offset_next; + const struct tme_ethernet_header *ethernet_header; + const struct tme_net_arp_header *arp_header; + const struct tme_net_ipv4_header *ipv4_header; + tme_uint16_t ethertype; unsigned int count; int rc; @@ -582,10 +664,13 @@ _tme_bsd_bpf_read(struct tme_ethernet_co if ((bpf->tme_bsd_bpf_buffer_offset + sizeof(the_bpf_header)) > bpf->tme_bsd_bpf_buffer_end) { - tme_log(&bpf->tme_bsd_bpf_element->tme_element_log_handle, 1, TME_OK, - (&bpf->tme_bsd_bpf_element->tme_element_log_handle, - _("flushed garbage BPF header bytes"))); - bpf->tme_bsd_bpf_buffer_end = 0; + if (bpf->tme_bsd_bpf_buffer_offset + != bpf->tme_bsd_bpf_buffer_end) { + tme_log(&bpf->tme_bsd_bpf_element->tme_element_log_handle, 1, TME_OK, + (&bpf->tme_bsd_bpf_element->tme_element_log_handle, + _("flushed garbage BPF header bytes"))); + bpf->tme_bsd_bpf_buffer_offset = bpf->tme_bsd_bpf_buffer_end; + } break; } @@ -594,6 +679,15 @@ _tme_bsd_bpf_read(struct tme_ethernet_co bpf->tme_bsd_bpf_buffer + bpf->tme_bsd_bpf_buffer_offset, sizeof(the_bpf_header)); + buffer_offset_next + = (((bpf->tme_bsd_bpf_buffer_offset + + the_bpf_header.bh_hdrlen + + the_bpf_header.bh_datalen) + == bpf->tme_bsd_bpf_buffer_end) + ? bpf->tme_bsd_bpf_buffer_end + : (bpf->tme_bsd_bpf_buffer_offset + + BPF_WORDALIGN(the_bpf_header.bh_hdrlen + + the_bpf_header.bh_datalen))); bpf->tme_bsd_bpf_buffer_offset += the_bpf_header.bh_hdrlen; /* if we're missing some part of the packet: */ @@ -603,7 +697,7 @@ _tme_bsd_bpf_read(struct tme_ethernet_co tme_log(&bpf->tme_bsd_bpf_element->tme_element_log_handle, 1, TME_OK, (&bpf->tme_bsd_bpf_element->tme_element_log_handle, _("flushed truncated BPF packet"))); - bpf->tme_bsd_bpf_buffer_offset += the_bpf_header.bh_datalen; + bpf->tme_bsd_bpf_buffer_offset = buffer_offset_next; continue; } @@ -612,10 +706,61 @@ _tme_bsd_bpf_read(struct tme_ethernet_co tme_log(&bpf->tme_bsd_bpf_element->tme_element_log_handle, 1, TME_OK, (&bpf->tme_bsd_bpf_element->tme_element_log_handle, _("flushed short BPF packet"))); - bpf->tme_bsd_bpf_buffer_offset += the_bpf_header.bh_datalen; + bpf->tme_bsd_bpf_buffer_offset = buffer_offset_next; continue; } + /* if packets need to be delayed: */ + if (bpf->tme_bsd_bpf_delay_time > 0) { + + /* if the current release time is before this packet's time: */ + if ((bpf->tme_bsd_bpf_delay_release.tv_sec + < the_bpf_header.bh_tstamp.tv_sec) + || ((bpf->tme_bsd_bpf_delay_release.tv_sec + == the_bpf_header.bh_tstamp.tv_sec) + && (bpf->tme_bsd_bpf_delay_release.tv_usec + < the_bpf_header.bh_tstamp.tv_usec))) { + + /* update the current release time, by taking the current time + and subtracting the delay time: */ + gettimeofday(&bpf->tme_bsd_bpf_delay_release, NULL); + if (bpf->tme_bsd_bpf_delay_release.tv_usec < bpf->tme_bsd_bpf_delay_time) { + bpf->tme_bsd_bpf_delay_release.tv_usec += 1000000UL; + bpf->tme_bsd_bpf_delay_release.tv_sec--; + } + bpf->tme_bsd_bpf_delay_release.tv_usec -= bpf->tme_bsd_bpf_delay_time; + } + + /* if the current release time is still before this packet's + time: */ + if ((bpf->tme_bsd_bpf_delay_release.tv_sec + < the_bpf_header.bh_tstamp.tv_sec) + || ((bpf->tme_bsd_bpf_delay_release.tv_sec + == the_bpf_header.bh_tstamp.tv_sec) + && (bpf->tme_bsd_bpf_delay_release.tv_usec + < the_bpf_header.bh_tstamp.tv_usec))) { + + /* set the sleep time: */ + assert ((bpf->tme_bsd_bpf_delay_release.tv_sec + == the_bpf_header.bh_tstamp.tv_sec) + || ((bpf->tme_bsd_bpf_delay_release.tv_sec + 1) + == the_bpf_header.bh_tstamp.tv_sec)); + bpf->tme_bsd_bpf_delay_sleep + = (((bpf->tme_bsd_bpf_delay_release.tv_sec + == the_bpf_header.bh_tstamp.tv_sec) + ? 0 + : 1000000UL) + + the_bpf_header.bh_tstamp.tv_usec + - bpf->tme_bsd_bpf_delay_release.tv_usec); + + /* rewind the buffer pointer: */ + bpf->tme_bsd_bpf_buffer_offset -= the_bpf_header.bh_hdrlen; + + /* stop now: */ + break; + } + } + /* form the single frame chunk: */ frame_chunk_buffer.tme_ethernet_frame_chunk_next = NULL; frame_chunk_buffer.tme_ethernet_frame_chunk_bytes @@ -623,14 +768,96 @@ _tme_bsd_bpf_read(struct tme_ethernet_co frame_chunk_buffer.tme_ethernet_frame_chunk_bytes_count = the_bpf_header.bh_datalen; + /* some network interfaces haven't removed the CRC yet when they + pass a packet to BPF. packets in a tme ethernet connection + never have CRCs, so here we attempt to detect them and strip + them off. + + unfortunately there's no general way to do this. there's a + chance that the last four bytes of an actual packet just + happen to be the Ethernet CRC of all of the previous bytes in + the packet, so we can't just strip off what looks like a + valid CRC, plus the CRC calculation itself isn't cheap. + + the only way to do this well seems to be to look at the + protocol. if we can determine what the correct minimum size + of the packet should be based on the protocol, and the size + we got is four bytes more than that, assume that the last four + bytes are a CRC and strip it off: */ + + /* assume that we won't be able to figure out the correct minimum + size of the packet: */ + count = 0; + + /* get the Ethernet header and packet type: */ + ethernet_header = (struct tme_ethernet_header *) (bpf->tme_bsd_bpf_buffer + bpf->tme_bsd_bpf_buffer_offset); + ethertype = ethernet_header->tme_ethernet_header_type[0]; + ethertype = (ethertype << 8) + ethernet_header->tme_ethernet_header_type[1]; + + /* dispatch on the packet type: */ + switch (ethertype) { + + /* an ARP or RARP packet: */ + case TME_ETHERNET_TYPE_ARP: + case TME_ETHERNET_TYPE_RARP: + arp_header = (struct tme_net_arp_header *) (ethernet_header + 1); + switch ((((tme_uint16_t) arp_header->tme_net_arp_header_opcode[0]) << 8) + + arp_header->tme_net_arp_header_opcode[1]) { + case TME_NET_ARP_OPCODE_REQUEST: + case TME_NET_ARP_OPCODE_REPLY: + case TME_NET_ARP_OPCODE_REV_REQUEST: + case TME_NET_ARP_OPCODE_REV_REPLY: + count = (TME_ETHERNET_HEADER_SIZE + + sizeof(struct tme_net_arp_header) + + (2 * arp_header->tme_net_arp_header_hardware_length) + + (2 * arp_header->tme_net_arp_header_protocol_length)); + default: + break; + } + break; + + /* an IPv4 packet: */ + case TME_ETHERNET_TYPE_IPV4: + ipv4_header = (struct tme_net_ipv4_header *) (ethernet_header + 1); + count = ipv4_header->tme_net_ipv4_header_length[0]; + count = (count << 8) + ipv4_header->tme_net_ipv4_header_length[1]; + count += TME_ETHERNET_HEADER_SIZE; + break; + + default: + break; + } + + /* if we were able to figure out the correct minimum size of the + packet, and the packet from BPF is exactly that minimum size + plus the CRC size, set the length of the packet to be the + correct minimum size. NB that we can't let the packet become + smaller than (TME_ETHERNET_FRAME_MIN - TME_ETHERNET_CRC_SIZE): */ + if (count != 0) { + count = TME_MAX(count, + (TME_ETHERNET_FRAME_MIN + - TME_ETHERNET_CRC_SIZE)); + if (frame_chunk_buffer.tme_ethernet_frame_chunk_bytes_count + == (count + TME_ETHERNET_CRC_SIZE)) { + frame_chunk_buffer.tme_ethernet_frame_chunk_bytes_count = count; + } + } + /* copy out the frame: */ count = tme_ethernet_chunks_copy(frame_chunks, &frame_chunk_buffer); - /* if this isn't a peek: */ - if (!(flags & TME_ETHERNET_READ_PEEK)) { + /* if this is a peek: */ + if (flags & TME_ETHERNET_READ_PEEK) { + + /* rewind the buffer pointer: */ + bpf->tme_bsd_bpf_buffer_offset -= the_bpf_header.bh_hdrlen; + } + + /* otherwise, this isn't a peek: */ + else { /* update the buffer pointer: */ - bpf->tme_bsd_bpf_buffer_offset += the_bpf_header.bh_datalen; + bpf->tme_bsd_bpf_buffer_offset = buffer_offset_next; } /* success: */ @@ -638,9 +865,11 @@ _tme_bsd_bpf_read(struct tme_ethernet_co break; } - /* if the buffer is empty, notify the reader that we need more data: */ - if (bpf->tme_bsd_bpf_buffer_offset - >= bpf->tme_bsd_bpf_buffer_end) { + /* if the buffer is empty, or if we failed to read a packet, + wake up the reader: */ + if ((bpf->tme_bsd_bpf_buffer_offset + >= bpf->tme_bsd_bpf_buffer_end) + || rc <= 0) { tme_cond_notify(&bpf->tme_bsd_bpf_cond_reader, TRUE); } @@ -748,6 +977,7 @@ TME_ELEMENT_SUB_NEW_DECL(tme_host_bsd,bp u_int packet_buffer_size; const char *ifr_name_user; struct ifreq *ifr; + unsigned long delay_time; int arg_i; int usage; int rc; @@ -755,6 +985,7 @@ TME_ELEMENT_SUB_NEW_DECL(tme_host_bsd,bp /* check our arguments: */ usage = 0; ifr_name_user = NULL; + delay_time = 0; arg_i = 1; for (;;) { @@ -765,6 +996,12 @@ TME_ELEMENT_SUB_NEW_DECL(tme_host_bsd,bp arg_i += 2; } + /* a delay time in microseconds: */ + else if (TME_ARG_IS(args[arg_i + 0], "delay") + && (delay_time = tme_misc_unumber_parse(args[arg_i + 1], 0)) > 0) { + arg_i += 2; + } + /* if we ran out of arguments: */ else if (args[arg_i + 0] == NULL) { break; @@ -783,10 +1020,11 @@ TME_ELEMENT_SUB_NEW_DECL(tme_host_bsd,bp if (usage) { tme_output_append_error(_output, - "%s %s [ interface %s ]", + "%s %s [ interface %s ] [ delay %s ]", _("usage:"), args[0], - _("INTERFACE")); + _("INTERFACE"), + _("MICROSECONDS")); return (EINVAL); } @@ -921,10 +1159,11 @@ TME_ELEMENT_SUB_NEW_DECL(tme_host_bsd,bp bpf->tme_bsd_bpf_fd = bpf_fd; bpf->tme_bsd_bpf_buffer_size = packet_buffer_size; bpf->tme_bsd_bpf_buffer = tme_new(tme_uint8_t, packet_buffer_size); + bpf->tme_bsd_bpf_delay_time = delay_time; /* start the threads: */ tme_mutex_init(&bpf->tme_bsd_bpf_mutex); - tme_cond_init(&bpf->tme_bsd_bpf_cond_writer); + tme_cond_init(&bpf->tme_bsd_bpf_cond_reader); tme_thread_create((tme_thread_t) _tme_bsd_bpf_th_reader, bpf); /* fill the element: */