static void
read_stdin( int fd, void *data ) {
UNUSED( data );
char buf[ 1024 ];
memset( buf, '\0', sizeof( buf ) );
ssize_t ret = read( fd, buf, sizeof( buf ) );
if ( ret < 0 ) {
if ( errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK ) {
return;
}
set_readable( fd, false );
error( "Read error ( errno = %s [%d] ).", strerror( errno ), errno );
return;
}
else if ( ret == 0 ) {
return;
}
if ( stdin_read_buffer == NULL ) {
set_readable( fd, false );
error( "Read buffer is not allocated yet" );
return;
}
char *p = append_back_buffer( stdin_read_buffer, ( size_t ) ret );
memcpy( p, buf, ( size_t ) ret );
char *lf = find_character( stdin_read_buffer->data, stdin_read_buffer->length, '\n' );
while ( lf != NULL ) {
size_t length = ( size_t ) ( lf - ( char * ) stdin_read_buffer->data );
if ( length > 0 ) {
buffer *string = alloc_buffer_with_length( length );
p = append_back_buffer( string, length );
memcpy( p, stdin_read_buffer->data, length );
relay_string( string );
free_buffer( string );
remove_front_buffer( stdin_read_buffer, length + 1 );
}
else {
if ( stdin_read_buffer->length > 0 ) {
remove_front_buffer( stdin_read_buffer, 1 );
}
}
if ( stdin_read_buffer->length == 0 ) {
break;
}
lf = find_character( stdin_read_buffer->data, stdin_read_buffer->length, '\n' );
}
// truncate head room manually
buffer *truncated = duplicate_buffer( stdin_read_buffer );
free_buffer( stdin_read_buffer );
stdin_read_buffer = truncated;
}
static void
flush_send_queue( int fd, void *user_data ) {
UNUSED( fd );
UNUSED( user_data );
assert( send_queue != NULL );
assert( connection.fd >= 0 );
debug( "Flushing send queue ( length = %u ).", send_queue->length );
set_writable( connection.fd, false );
buffer *buf = NULL;
while ( ( buf = peek_message( send_queue ) ) != NULL ) {
ssize_t write_length = write( connection.fd, buf->data, buf->length );
if ( write_length < 0 ) {
if ( errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK ) {
set_writable( connection.fd, true );
return;
}
error( "Failed to send a message to secure channel ( errno = %s [%d] ).",
strerror( errno ), errno );
return;
}
if ( ( size_t ) write_length < buf->length ) {
remove_front_buffer( buf, ( size_t ) write_length );
set_writable( connection.fd, true );
return;
}
buf = dequeue_message( send_queue );
free_buffer( buf );
}
}
static size_t
read_from_buffer( void *ptr, size_t size, size_t nmemb, void *user_data ) {
debug( "Reading contents ( ptr = %p, size = %zu, nmemb = %zu, user_data = %p ).",
ptr, size, nmemb, user_data );
assert( user_data != NULL );
buffer *buf = user_data;
if ( buf == NULL || ( buf != NULL && buf->length == 0 ) ) {
debug( "buffer is empty ( buf = %p, length = %zu ).", buf, buf != NULL ? buf->length : 0 );
return 0;
}
size_t real_size = size * nmemb;
if ( real_size == 0 ) {
debug( "Nothing to read." );
return 0;
}
if ( buf->length < real_size ) {
real_size = buf->length;
}
memcpy( ptr, buf->data, real_size );
remove_front_buffer( buf, real_size );
debug( "%zu bytes data is read from buffer ( buf = %p, length = %zu ).", real_size, buf, buf->length );
return real_size;
}
int
flush_secure_channel( struct switch_info *sw_info ) {
assert( sw_info != NULL );
assert( sw_info->send_queue != NULL );
assert( sw_info->secure_channel_fd >= 0 );
buffer *buf;
ssize_t write_length;
while ( ( buf = peek_message( sw_info->send_queue ) ) != NULL ) {
write_length = write( sw_info->secure_channel_fd, buf->data, buf->length );
if ( write_length < 0 ) {
if ( errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK ) {
return 0;
}
error( "Failed to send a message to secure channel ( errno = %s [%d] ).",
strerror( errno ), errno );
return -1;
}
if ( ( size_t ) write_length < buf->length ) {
remove_front_buffer( buf, ( size_t ) write_length );
return 0;
}
buf = dequeue_message( sw_info->send_queue );
free_buffer( buf );
}
return 0;
}
static buffer *
parse_etherip( const buffer *data ) {
packet_info *packet_info = data->user_data;
if ( packet_info->etherip_version != ETHERIP_VERSION ) {
error( "invalid etherip version 0x%04x.", packet_info->etherip_version );
return NULL;
}
if ( packet_info->etherip_offset == 0 ) {
debug( "too short etherip message" );
return NULL;
}
buffer *copy = duplicate_buffer( data );
if ( copy == NULL ) {
error( "duplicate_buffer failed." );
return NULL;
}
copy->user_data = NULL;
remove_front_buffer( copy, packet_info->etherip_offset );
if ( !parse_packet( copy ) ) {
error( "parse_packet failed." );
free_buffer( copy );
return NULL;
}
debug( "Receive EtherIP packet." );
return copy;
}
void
service_recv_from_application( uint16_t message_type, buffer *buf ) {
openflow_service_header_t *message;
uint64_t datapath_id;
uint16_t service_name_length;
char *service_name;
if ( buf->length < sizeof( openflow_service_header_t ) + sizeof( struct ofp_header ) ) {
error( "Too short openflow application message(%zu).", buf->length );
free_buffer( buf );
return;
}
message = buf->data;
datapath_id = ntohll( message->datapath_id );
service_name_length = ntohs( message->service_name_length );
service_name = remove_front_buffer( buf, sizeof( openflow_service_header_t ) );
if ( service_name_length < 1 ) {
error( "Invalid service name length %u.", service_name_length );
free_buffer( buf );
return;
}
if ( service_name[ service_name_length - 1 ] != '\0' ) {
error( "Service name is not null terminated." );
free_buffer( buf );
return;
}
remove_front_buffer( buf, service_name_length );
switch ( message_type ) {
case MESSENGER_OPENFLOW_MESSAGE:
handle_openflow_message( &datapath_id, service_name, buf );
break;
case MESSENGER_OPENFLOW_DISCONNECT_REQUEST:
free_buffer( buf );
handle_openflow_disconnect_request( &datapath_id );
break;
default:
error( "Unknown message type %d.", message_type );
free_buffer( buf );
break;
}
}
static void
recv_from_secure_channel( int fd, void *user_data ) {
UNUSED( fd );
UNUSED( user_data );
// all queued messages should be processed before receiving new messages from remote
if ( recv_queue->length > 0 ) {
return;
}
if ( fragment_buf == NULL ) {
fragment_buf = alloc_buffer_with_length( RECEIVE_BUFFFER_SIZE );
}
size_t remaining_length = RECEIVE_BUFFFER_SIZE - fragment_buf->length;
char *recv_buf = ( char * ) fragment_buf->data + fragment_buf->length;
ssize_t recv_length = read( connection.fd, recv_buf, remaining_length );
if ( recv_length < 0 ) {
if ( errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK ) {
return;
}
error( "Receive error ( errno = %s [%d] ).", strerror( errno ), errno );
return;
}
if ( recv_length == 0 ) {
debug( "Connection closed by peer." );
disconnected();
reconnect( NULL );
return;
}
fragment_buf->length += ( size_t ) recv_length;
size_t read_total = 0;
while ( fragment_buf->length >= sizeof( struct ofp_header ) ) {
struct ofp_header *header = fragment_buf->data;
uint16_t message_length = ntohs( header->length );
if ( message_length > fragment_buf->length ) {
break;
}
buffer *message = alloc_buffer_with_length( message_length );
char *p = append_back_buffer( message, message_length );
memcpy( p, fragment_buf->data, message_length );
remove_front_buffer( fragment_buf, message_length );
enqueue_message( recv_queue, message );
read_total += message_length;
}
// remove headroom manually for next call
if ( read_total > 0 ) {
memmove( ( char * ) fragment_buf->data - read_total, fragment_buf->data, fragment_buf->length );
fragment_buf->data = ( char * ) fragment_buf->data - read_total;
}
while ( recv_message_from_secure_channel() == true );
}
int
flush_secure_channel( struct switch_info *sw_info ) {
assert( sw_info != NULL );
assert( sw_info->send_queue != NULL );
assert( sw_info->secure_channel_fd >= 0 );
buffer *buf;
ssize_t write_length;
if ( sw_info->send_queue->length == 0 ) {
return 0;
}
set_writable( sw_info->secure_channel_fd, false );
writev_args args;
args.iov = xmalloc( sizeof( struct iovec ) * ( size_t ) sw_info->send_queue->length );
args.iovcnt = 0;
foreach_message_queue( sw_info->send_queue, append_to_writev_args, &args );
if ( args.iovcnt == 0 ) {
xfree( args.iov );
return 0;
}
write_length = writev( sw_info->secure_channel_fd, args.iov, args.iovcnt );
xfree( args.iov );
if ( write_length < 0 ) {
if ( errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK ) {
set_writable( sw_info->secure_channel_fd, true );
return 0;
}
error( "Failed to send a message to secure channel ( errno = %s [%d] ).",
strerror( errno ), errno );
return -1;
}
if ( write_length == 0 ) {
return 0;
}
while ( ( buf = peek_message( sw_info->send_queue ) ) != NULL ) {
if ( write_length == 0 ) {
set_writable( sw_info->secure_channel_fd, true );
return 0;
}
if ( ( size_t ) write_length < buf->length ) {
remove_front_buffer( buf, ( size_t ) write_length );
set_writable( sw_info->secure_channel_fd, true );
return 0;
}
write_length -= ( ssize_t ) buf->length;
buf = dequeue_message( sw_info->send_queue );
free_buffer( buf );
}
return 0;
}
static void
flush_send_queue( int fd, void *user_data ) {
UNUSED( fd );
ether_device *device = user_data;
assert( device != NULL );
debug( "Flushing send queue ( device = %s, queue length = %u ).", device->name, get_packet_buffers_length( device->send_queue ) );
set_writable_safe( device->fd, false );
struct sockaddr_ll sll;
memset( &sll, 0, sizeof( sll ) );
sll.sll_ifindex = device->ifindex;
int count = 0;
buffer *buf = NULL;
while ( ( buf = peek_packet_buffer( device->send_queue ) ) != NULL && count < 256 ) {
#if WITH_PCAP
if( pcap_sendpacket( device->pcap, buf->data, ( int ) buf->length ) < 0 ) {
error( "Failed to send a message to ethernet device ( device = %s, pcap_err = %s ).",
device->name, pcap_geterr( device->pcap ) );
}
size_t length = buf->length;
#else
ssize_t length = sendto( device->fd, buf->data, buf->length, MSG_DONTWAIT, ( struct sockaddr * ) &sll, sizeof( sll ) );
if ( length < 0 ) {
if ( ( errno == EINTR ) || ( errno == EAGAIN ) || ( errno == EWOULDBLOCK ) ) {
break;
}
char error_string[ ERROR_STRING_SIZE ];
error( "Failed to send a message to ethernet device ( device = %s, errno = %s [%d] ).",
device->name, safe_strerror_r( errno, error_string, sizeof( error_string ) ), errno );
return;
}
#endif
if ( ( size_t ) length < buf->length ) {
remove_front_buffer( buf, ( size_t ) length );
break;
}
buf = dequeue_packet_buffer( device->send_queue );
mark_packet_buffer_as_used( device->send_queue, buf );
count++;
}
if ( get_packet_buffers_length( device->send_queue ) > 0 ) {
set_writable_safe( device->fd, true );
}
}
static buffer *
setup_dummy_ether_arp_packet() {
buffer *arp_buffer = setup_dummy_ether_packet( sizeof( ether_header_t ) + sizeof( arp_header_t ) + arp_padding_size, ETH_ETHTYPE_ARP );
arp_header_t *arp = packet_info( arp_buffer )->l3_data.arp;
arp->ar_hrd = htons( ARPHRD_ETHER );
arp->ar_pro = htons( ETH_ETHTYPE_IPV4 );
arp->ar_hln = ETH_ADDRLEN;
arp->ar_pln = IPV4_ADDRLEN;
arp->ar_op = htons( ARPOP_REPLY );
free( arp_buffer->user_data );
arp_buffer->user_data = NULL;
remove_front_buffer( arp_buffer, ETH_PREPADLEN );
return arp_buffer;
}
static void
handle_ovs_vendor_error( uint64_t datapath_id, uint32_t transaction_id, const buffer *data ) {
assert( data != NULL );
assert( data->length > 0 );
buffer *ovs_data = duplicate_buffer( data );
remove_front_buffer( ovs_data, sizeof( ovs_vendor_error ) );
ovs_vendor_error *err = data->data;
debug( "An Open vSwitch vendor error message received ( datapath_id = %#" PRIx64 ", transaction_id = %#x, "
"type = %#x, code = %#x, ovs_data = %p ).",
datapath_id, transaction_id, ntohs( err->type ), ntohs( err->code ), ovs_data );
if ( event_handlers.ovs_error_callback != NULL ) {
event_handlers.ovs_error_callback( datapath_id, transaction_id, ntohs( err->type ), ntohs( err->code ),
ovs_data, event_handlers.ovs_error_user_data );
}
free_buffer( ovs_data );
}
static int
flush_secure_channel_tls( struct switch_info *sw_info ) {
assert( sw_info != NULL );
assert( sw_info->tls );
assert( sw_info->ssl != NULL );
assert( sw_info->send_queue->length > 0 );
buffer *buf = NULL;
while ( ( buf = peek_message( sw_info->send_queue ) ) != NULL ) {
int write_length = SSL_write( sw_info->ssl, buf->data, ( int ) buf->length );
if ( write_length < 0 ) {
int error_no = SSL_get_error( sw_info->ssl, write_length );
switch ( error_no ) {
case SSL_ERROR_WANT_READ:
set_readable( sw_info->secure_channel_fd, true );
case SSL_ERROR_WANT_WRITE:
set_writable( sw_info->secure_channel_fd, true );
return 0;
default:
error( "Failed to send a message to secure channel ( error = %d ).", error_no );
return -1;
}
}
if ( write_length == 0 ) {
set_writable( sw_info->secure_channel_fd, true );
return 0;
}
if ( ( size_t ) write_length < buf->length ) {
remove_front_buffer( buf, ( size_t ) write_length );
set_writable( sw_info->secure_channel_fd, true );
return 0;
}
buf = dequeue_message( sw_info->send_queue );
free_buffer( buf );
}
return 0;
}
static buffer *
setup_dummy_ether_ipv4_packet() {
buffer *ipv4_buffer = setup_dummy_ether_packet( sizeof( ether_header_t ) + sizeof( ipv4_header_t ) + ipv4_padding_size, ETH_ETHTYPE_IPV4 );
ipv4_header_t *ipv4 = packet_info( ipv4_buffer )->l3_data.ipv4;
ipv4->version = IPVERSION;
ipv4->ihl = sizeof( ipv4_header_t ) / 4;
ipv4->tot_len = htons( sizeof( ipv4_header_t ) );
ipv4->ttl = 0;
ipv4->check = 0;
ipv4->protocol = IPPROTO_UDP;
ipv4->saddr = htonl( 0xC0A80067 );
ipv4->daddr = htonl( 0xC0A80036 );
ipv4->frag_off = htons( 0 );
ipv4->check = get_checksum( ( uint16_t * ) packet_info( ipv4_buffer )->l3_data.ipv4, sizeof( ipv4_header_t ) );
free( ipv4_buffer->user_data );
ipv4_buffer->user_data = NULL;
remove_front_buffer( ipv4_buffer, ETH_PREPADLEN );
return ipv4_buffer;
}
static void
test_parse_packet_lldp_over_ip_succeeds() {
const char filename[] = "./unittests/lib/test_packets/lldp_over_ip.cap";
buffer *copy;
buffer *buffer = store_packet_to_buffer( filename );
assert_true( parse_packet( buffer ) );
packet_info *packet_info = buffer->user_data;
assert_int_equal( packet_info->format, ETH_IPV4_ETHERIP );
u_char macda[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
u_char macsa[] = { 0xfe, 0xab, 0x7e, 0x15, 0x3f, 0xc6 };
assert_memory_equal( packet_info->eth_macda, macda, ETH_ADDRLEN );
assert_memory_equal( packet_info->eth_macsa, macsa, ETH_ADDRLEN );
assert_int_equal( packet_info->ipv4_version, 4 );
assert_int_equal( packet_info->ipv4_ihl, 5 );
assert_int_equal( packet_info->ipv4_tos, 0 );
assert_int_equal( packet_info->ipv4_tot_len, 51 );
assert_int_equal( packet_info->ipv4_id, 0 );
assert_int_equal( packet_info->ipv4_frag_off, 0 );
assert_int_equal( packet_info->ipv4_ttl, 1 );
assert_int_equal( packet_info->ipv4_protocol, 97 );
assert_int_equal( packet_info->ipv4_checksum, 0 );
assert_int_equal( packet_info->ipv4_saddr, 0x0a2a7aca );
assert_int_equal( packet_info->ipv4_daddr, 0x0a2a7ad4 );
uint16_t sample = ntohs( *( uint16_t * ) packet_info->l4_payload );
assert_int_equal( sample, 0x0180 );
assert_int_equal( packet_info->l4_payload_length, 31 );
assert_int_equal( packet_info->etherip_version, ETHERIP_VERSION );
assert_int_equal( packet_info->etherip_offset, 36 );
copy = duplicate_buffer( buffer );
assert_true ( copy != NULL );
assert_true( copy->length == buffer->length );
copy->user_data = NULL;
remove_front_buffer( copy, packet_info->etherip_offset );
assert_true( parse_packet( copy ) );
packet_info = copy->user_data;
assert_int_equal( packet_info->format, ETH_LLDP );
u_char lldp_macda[] = { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e };
u_char lldp_macsa[] = { 0xfe, 0xab, 0x7e, 0x15, 0x3f, 0xc6 };
assert_memory_equal( packet_info->eth_macda, lldp_macda, ETH_ADDRLEN );
assert_memory_equal( packet_info->eth_macsa, lldp_macsa, ETH_ADDRLEN );
sample = ntohs( *( uint16_t * ) packet_info->l3_payload );
assert_int_equal( sample, 0x0205 );
assert_int_equal( packet_info->l3_payload_length, 17 );
free_buffer( copy );
free_buffer( buffer );
}
int
recv_from_secure_channel( struct switch_info *sw_info ) {
assert( sw_info != NULL );
assert( sw_info->recv_queue != NULL );
// all queued messages should be processed before receiving new messages from remote
if ( sw_info->recv_queue->length > 0 ) {
return 0;
}
if ( sw_info->fragment_buf == NULL ) {
sw_info->fragment_buf = alloc_buffer_with_length( RECEIVE_BUFFFER_SIZE );
}
size_t remaining_length = RECEIVE_BUFFFER_SIZE - sw_info->fragment_buf->length;
char *recv_buf = ( char * ) sw_info->fragment_buf->data + sw_info->fragment_buf->length;
ssize_t recv_length = read( sw_info->secure_channel_fd, recv_buf, remaining_length );
if ( recv_length < 0 ) {
if ( errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK ) {
return 0;
}
error( "Receive error:%s(%d)", strerror( errno ), errno );
return -1;
}
if ( recv_length == 0 ) {
debug( "Connection closed by peer." );
return -1;
}
sw_info->fragment_buf->length += ( size_t ) recv_length;
size_t read_total = 0;
while ( sw_info->fragment_buf->length >= sizeof( struct ofp_header ) ) {
struct ofp_header *header = sw_info->fragment_buf->data;
if ( header->version != OFP_VERSION ) {
error( "Receive error: invalid version (version %d)", header->version );
ofpmsg_send_error_msg( sw_info,
OFPET_BAD_REQUEST, OFPBRC_BAD_VERSION, sw_info->fragment_buf );
return -1;
}
uint16_t message_length = ntohs( header->length );
if ( message_length > sw_info->fragment_buf->length ) {
break;
}
buffer *message = alloc_buffer_with_length( message_length );
char *p = append_back_buffer( message, message_length );
memcpy( p, sw_info->fragment_buf->data, message_length );
remove_front_buffer( sw_info->fragment_buf, message_length );
enqueue_message( sw_info->recv_queue, message );
read_total += message_length;
}
// remove headroom manually for next call
if ( read_total > 0 ) {
memmove( ( char * ) sw_info->fragment_buf->data - read_total,
sw_info->fragment_buf->data,
sw_info->fragment_buf->length );
sw_info->fragment_buf->data = ( char * ) sw_info->fragment_buf->data - read_total;
}
return 0;
}
