X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-eth-gw.git/blobdiff_plain/08d097af8e481957b9f2e4751e9d29553c18e995..418ae6fe5f5a5f27bd6cd6a2c886c434db571839:/kernel/canethgw.c diff --git a/kernel/canethgw.c b/kernel/canethgw.c index a3307bf..3a9b966 100644 --- a/kernel/canethgw.c +++ b/kernel/canethgw.c @@ -1,3 +1,5 @@ +#define DEBUG 1 + #include #include #include @@ -9,57 +11,81 @@ #include #include #include +#include +#include #include "canethgw.h" +#include +#include +#include + +/** + * ToDo: + * [ ] encapsule module - check inputs + * [ ] refactor - chc .h + * [ ] dump callback + * [ ] rtnl vs nl functions + * [ ] stop threads + * [ ] change listening + * [ ] clean exit - threads, jobs + */ MODULE_LICENSE( "GPL" ); +static int cegw_udp_can( void* data ); +inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port ); +static int cegw_can_udp( void* data ); +inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex ); +static void cegw_thread_start( void ); +static void cegw_thread_stop( void ); +static int cegw_thread_restart( void* arg ); + +#define CEGW_STOPPED 0 +#define CEGW_RUNNING 1 + +static struct socket* can_sock, * udp_sock; static struct task_struct* eth_to_can, * can_to_eth; -static struct socket* udp_sock; -static struct socket* can_sock; -static struct net_device* can_dev; +/* ToDo: protect with mutex */ +static int cegw_state = CEGW_STOPPED; -/*********************** - * UDP - ***********************/ +struct can_eth_gw +{ + int src_ifindex; + struct in_addr dst_addr; + unsigned short dst_port; + struct hlist_node list; +}; -int gw_udp_recv( void* data ) +struct eth_can_gw { - struct can_frame cf; - struct kvec vec; - struct msghdr mh; + int dst_if_idx; + struct hlist_node list; +}; - vec.iov_base = &cf; - vec.iov_len = sizeof(cf); +HLIST_HEAD( can_eth_job ); +HLIST_HEAD( eth_can_job ); - mh.msg_name = NULL; - mh.msg_namelen = 0; - mh.msg_iov = NULL; - mh.msg_iovlen = 0; - mh.msg_control = NULL; - mh.msg_controllen = 0; - mh.msg_flags = 0; - - while( 1 ) - { - if( kthread_should_stop() ) /* up() ?, recv is blocking */ - break; - kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* todo: handle error */ - printk( "received udp msg_id:%d\n", cf.can_id ); - gw_can_send( &cf ); - } +struct +{ + struct can_filter filter; + int can_idx; + /* bind on if */ + struct in_addr eth_addr; + unsigned short eth_port; +} cegw_setting; - return 0; -} +DECLARE_COMPLETION( cegw_compl_init ); +DECLARE_COMPLETION( cegw_compl_exit ); +DEFINE_MUTEX( cegw_setting_mutex ); -void gw_udp_send( struct can_frame* cf ) +inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port ) { struct msghdr mh; struct sockaddr_in addr; struct kvec vec; addr.sin_family = AF_INET; - addr.sin_port = htons( 10502 ); - addr.sin_addr.s_addr = 0x0100007f; + addr.sin_port = htons( port ); + addr.sin_addr = ipaddr; mh.msg_name = &addr; mh.msg_namelen = sizeof( addr ); @@ -73,131 +99,473 @@ void gw_udp_send( struct can_frame* cf ) kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) ); } -/*********************** - * CAN - ***********************/ - -int gw_can_recv( void* data ) +inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex ) { struct msghdr mh; struct kvec vec; + struct sockaddr_can addr; + + addr.can_family = AF_CAN; + addr.can_ifindex = ifindex; + + mh.msg_name = &addr; + mh.msg_namelen = sizeof( addr ); + mh.msg_control = NULL; + mh.msg_controllen = 0; + mh.msg_flags = 0; + + vec.iov_base = cf; + vec.iov_len = sizeof( *cf ); + + kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) ); /* ToDo: handle error */ +} + +/** + * cegw_udp_can - performs udp->can routing + * This function is run as a thread. + */ +static int cegw_udp_can( void* data ) +{ struct can_frame cf; + struct kvec vec; + struct msghdr mh; + struct eth_can_gw* job; + struct hlist_node* pos; + int can_ifidx; + int recv_size; + struct sockaddr_in udp_addr; + struct sockaddr_can can_addr; + + can_addr.can_family = AF_CAN; + can_addr.can_ifindex = 0; + udp_addr.sin_family = AF_INET; + mutex_lock( &cegw_setting_mutex ); + udp_addr.sin_port = htons( cegw_setting.eth_port ); + udp_addr.sin_addr = cegw_setting.eth_addr; + mutex_unlock( &cegw_setting_mutex ); + mh.msg_name = NULL; mh.msg_namelen = 0; + mh.msg_iov = NULL; + mh.msg_iovlen = 0; mh.msg_control = NULL; mh.msg_controllen = 0; mh.msg_flags = 0; - - vec.iov_base = &cf; - vec.iov_len = sizeof( cf ); - + + if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock) != 0 ) + { + printk( KERN_ERR "canethgw: udp socket creation failed\n" ); + return -1; + } + + if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 ) + { + printk( KERN_ERR "canethgw: can socket creation failed\n" ); + return -1; + } + + if( kernel_bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) + { + printk( KERN_ERR "canethgw: udp socket binding failed\n" ); + sock_release( udp_sock ); + sock_release( can_sock ); + return -1; + } + + if( kernel_bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 ) + { + printk( KERN_ERR "canethgw: can socket binding failed\n" ); + sock_release( udp_sock ); + sock_release( can_sock ); + return -1; + } + + pr_devel( "canethgw: cegw_udp_can init complete\n" ); + complete_all( &cegw_compl_init ); /* ToDo: why _all? */ + pr_devel( "canethgw: cegw_udp_can is entering working cycle\n" ); + while( 1 ) { - if( kthread_should_stop() ) /**/ + if( cegw_state == CEGW_STOPPED ) break; - kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 ); - printk( "received can msg_id:%d\n", cf.can_id ); - gw_udp_send( &cf ); + vec.iov_base = &cf; + vec.iov_len = sizeof(cf); + recv_size = kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* ToDo: handle error, size check */ + if( recv_size == 0 ) + { + continue; + } + hlist_for_each_entry_rcu( job, pos, ð_can_job, list ) + { + rcu_read_lock(); /**/ + can_ifidx = job->dst_if_idx; + rcu_read_unlock(); + /* ToDo: from filter */ + cegw_can_send( can_sock, &cf, can_ifidx ); + } } - + + pr_devel( "canethgw: cegw_udp_can is ready for termination\n" ); + wait_for_completion( &cegw_compl_exit ); + sock_release( udp_sock ); + sock_release( can_sock ); + cegw_state = CEGW_STOPPED; /* ToDo */ + pr_devel( "canethgw: cegw_udp_can terminates\n" ); return 0; } -void gw_can_send( struct can_frame* cf ) +/** + * cegw_can_udp - performs can->udp routing + */ +static int cegw_can_udp( void* data ) { struct msghdr mh; - struct kvec vec; - - mh.msg_name = NULL; - mh.msg_namelen = 0; + struct kvec vec; + struct can_frame cf; + struct sockaddr_can ca; + struct can_eth_gw* job; + struct hlist_node* pos; + struct in_addr eth_addr; + u16 eth_port; + int recv_size; + + mh.msg_name = &ca; + mh.msg_namelen = sizeof( ca ); mh.msg_control = NULL; mh.msg_controllen = 0; mh.msg_flags = 0; + + wait_for_completion( &cegw_compl_init ); + pr_devel( "canethgw: cegw_can_udp is entering working cycle\n" ); + + while( 1 ) + { + if( cegw_state == CEGW_STOPPED ) /**/ + break; + vec.iov_base = &cf; + vec.iov_len = sizeof( cf ); + + recv_size = kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 ); + if( recv_size == 0 ) + { + continue; + } + hlist_for_each_entry_rcu( job, pos, &can_eth_job, list ) + { + rcu_read_lock(); + eth_addr = job->dst_addr; + eth_port = job->dst_port; + rcu_read_unlock(); + if( job->src_ifindex == ca.can_ifindex ) + cegw_udp_send( udp_sock, &cf, eth_addr, eth_port ); + } + } - vec.iov_base = cf; - vec.iov_len = sizeof( *cf ); - - kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) ); + complete_all( &cegw_compl_exit ); + pr_devel( "canethgw: cegw_can_udp terminates\n" ); + return 0; } -/*********************** - * module init/exit - ***********************/ +/* NetLink */ -static int __init cangw_init( void ) -{ - struct sockaddr_in udp_addr; - struct sockaddr_can can_addr; - int ifidx = 0; +static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg ) +{ + struct nlattr* tb[ CEGW_MAX+1 ]; + struct can_eth_gw* cethgw = NULL; + struct eth_can_gw* ecangw = NULL; + int ifindex; + u32 ip; + u16 port; + int err = 0; - /* 1. create can socket and bind to it */ - can_dev = dev_get_by_name( &init_net, "vcan0" ); /* net ns?, release counter! */ - if( can_dev == NULL ) + /* ToDo: size check + if (nlmsg_len(nlh) < sizeof(*r)) + return -EINVAL; + */ + + err = nlmsg_parse( nlh, sizeof( struct rtmsg ), tb, CEGW_MAX, NULL ); + if( err < 0 ) { - printk( KERN_ERR "error: vcan0 not found\n" ); - return -1; + printk( KERN_ERR "canethgw: nlmsg_parse error\n" ); + return err; } - ifidx = can_dev->ifindex; - dev_put( can_dev ); - - if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 ) + + if( tb[CEGW_CMD_INFO] == NULL ) { - printk( KERN_ERR "error: can_sock creation failed\n" ); - return -1; + printk( KERN_ERR "canethgw: bad cmd error\n" ); + return -EINVAL; } + + switch( *(int*)nla_data( tb[CEGW_CMD_INFO] ) ) + { + case CEGW_LISTEN: + if( cegw_state == CEGW_RUNNING ) + { + mutex_lock( &cegw_setting_mutex ); + cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] ); + cegw_setting.eth_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] ); + mutex_unlock( &cegw_setting_mutex ); + kthread_run( cegw_thread_restart, NULL, "canethgw" ); + } else + { + mutex_lock( &cegw_setting_mutex ); + cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] ); + cegw_setting.eth_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] ); + mutex_unlock( &cegw_setting_mutex ); + cegw_thread_start(); + } + break; + case CEGW_RULE_CAN_ETH: /* new can->eth rule */ + { + ifindex = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] ); + ip = *(u32*)nla_data( tb[CEGW_ETH_IP] ); + port = *(u16*)nla_data( tb[CEGW_ETH_PORT] ); + pr_devel( "canethgw: new can->eth rule - (%d)->(%x:%hu)\n", ifindex, ip, port ); + + cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL ); + if( cethgw == NULL ) + { + break; + } + cethgw->src_ifindex = ifindex; + cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] ); + cethgw->dst_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] ); + + hlist_add_head_rcu( &cethgw->list, &can_eth_job ); + } + break; + case CEGW_RULE_ETH_CAN: /* new eth->can rule */ + printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CEGW_CAN_IFINDEX] ) ); + ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL ); + if( ecangw == NULL ) + { + break; + } + ecangw->dst_if_idx = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] ); + hlist_add_head_rcu( &ecangw->list, ð_can_job ); + break; + default: + pr_devel( "canethgw: unknown CEGW_CMD_INFO\n" ); + /* ToDo undef operation */ + break; + } + + return 0; +} + +static int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg ) +{ + struct rtmsg* r; + struct nlattr* tb[ CEGW_MAX+1 ]; + struct hlist_node* pos,* n; + struct can_eth_gw* ceth; + struct eth_can_gw* ecan; + + int err = 0; + + if( nlmsg_len(nlh) < sizeof(*r) ) + return -EINVAL; - can_addr.can_family = AF_CAN; - can_addr.can_ifindex = ifidx; + r = nlmsg_data( nlh ); + + if( r->rtm_family != AF_CAN ) + return -EPFNOSUPPORT; + + /* + if( r->gwtype != CGW_TYPE_CAN_ETH_UDP ) + return -EINVAL; + */ + printk( "attrsize=%d\n", nlmsg_attrlen(nlh, sizeof(struct rtmsg)) ); + + err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL ); + if( err != 0 ) + return -EINVAL; + + if( tb[CEGW_CMD_INFO] == NULL ) + return -EINVAL; - if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 ) + if( *(int*)nla_data( tb[CEGW_CMD_INFO] ) == CEGW_FLUSH ) { - printk( KERN_ERR "can_sock bind failed\n" ); - return -1; + hlist_for_each_entry_safe( ceth, pos, n, &can_eth_job, list ) + { + hlist_del( &ceth->list ); + kfree( ceth ); + } + hlist_for_each_entry_safe( ecan, pos, n, ð_can_job, list ) + { + hlist_del( &ecan->list ); + kfree( ecan ); + } } - - /* 2. create udp socket and bind to it */ - if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 ) + // tb[] + return 0; +} + +static int cegw_dump_job( struct sk_buff* skb, struct netlink_callback* cb ) +{ + struct can_eth_gw* ceth; + struct eth_can_gw* ecan; + struct hlist_node* pos; + struct nlmsghdr* nlh; + int idx = 0; + int s_idx = cb->args[0]; + int ifidx, type; + struct in_addr dst_ip; + unsigned short dst_port; + + rcu_read_lock(); + hlist_for_each_entry_rcu( ecan, pos, ð_can_job, list ) { - printk( "error: udp_sock creation failed\n" ); - sock_release( can_sock ); - return -1; + + // if( idx < s_idx ) + // goto cont1; + + nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 ); + + ifidx = ecan->dst_if_idx; + type = CEGW_RULE_ETH_CAN; + nla_put( skb, CEGW_TYPE, sizeof(type), &type ); + nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) ); + + nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifidx), &ifidx ); /* ToDo return */ + nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) ); +cont1: + idx++; } + rcu_read_unlock(); + + rcu_read_lock(); + hlist_for_each_entry_rcu( ceth, pos, &can_eth_job, list ) + { + // if( idx < s_idx ) + // goto cont2; - udp_addr.sin_family = AF_INET; - udp_addr.sin_port = htons( 10501 ); - udp_addr.sin_addr.s_addr = INADDR_ANY; + nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 ); + + ifidx = ceth->src_ifindex; + type = CEGW_RULE_CAN_ETH; + dst_ip = ceth->dst_addr; + dst_port = ceth->dst_port; - if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */ + nla_put( skb, CEGW_TYPE, sizeof(type), &type ); + nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) ); + + nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifidx), &ifidx ); /* ToDo return */ + nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) ); + + nla_put( skb, CEGW_ETH_IP, sizeof(dst_ip), &dst_ip ); /* ToDo return */ + nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_ip) ); + + nla_put( skb, CEGW_ETH_PORT, sizeof(dst_port), &dst_port ); /* ToDo return */ + nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_port) ); + + //nla_put( skb, CGW_ETH_IP, sizeof() IP_ADDR ) +cont2: + idx++; + } + rcu_read_unlock(); + + /* ToDo nlmsg_cancel */ + cb->args[0] = idx; + + return skb->len; +} + +static void cegw_thread_start( void ) +{ + cegw_state = CEGW_RUNNING; + + INIT_COMPLETION( cegw_compl_init ); + INIT_COMPLETION( cegw_compl_exit ); + + eth_to_can = kthread_create( cegw_udp_can, NULL, "canethgw" ); + if( !IS_ERR( eth_to_can ) ) + { + get_task_struct( eth_to_can ); + wake_up_process( eth_to_can ); + } + can_to_eth = kthread_run( cegw_can_udp, NULL, "canethgw" ); + if( !IS_ERR( can_to_eth ) ) { - printk( "error: binding failed\n" ); - sock_release( udp_sock ); - sock_release( can_sock ); - return -1; + get_task_struct( can_to_eth ); + wake_up_process( can_to_eth ); } - /* 3. run bridging threads */ - eth_to_can = kthread_run( gw_udp_recv, NULL, "cangw" ); - can_to_eth = kthread_run( gw_can_recv, NULL, "cangw" ); + /* ToDo: kthread creation fail */ + printk( KERN_INFO "threads are running\n" ); +} - /* - if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 ) - {s - printk( "error: can_sock creation failed\n" ); +/* ToDo: stop when no threads started */ +static void cegw_thread_stop( void ) +{ + int how = SHUT_RDWR; + struct sock* sk = NULL; + + /* be sure sockets exist */ + wait_for_completion( &cegw_compl_init ); + cegw_state = CEGW_STOPPED; + + /* shut down socket */ + sk = can_sock->sk; + how++; + lock_sock( sk ); + sk->sk_shutdown |= how; + sk->sk_state_change( sk ); + release_sock( sk ); + + kernel_sock_shutdown( udp_sock, SHUT_RDWR ); + + /* wait for return to reuse port if restart */ + kthread_stop( eth_to_can ); + kthread_stop( can_to_eth ); +} + +/** + * cegw_thread_restart + */ +static int cegw_thread_restart( void* data ) +{ + printk( "restart\n" ); + + cegw_thread_stop(); + cegw_thread_start(); + + return 0; +} + +/*********************** + * module init/exit + ***********************/ + +static int __init cangw_init( void ) +{ + /* subscribe to netlink */ + if( __rtnl_register( PF_CAN, RTM_GETROUTE, NULL, cegw_dump_job, NULL ) != 0 ) + { + printk( KERN_ERR "error: rtnl_register fail\n" ); + return -1; } - */ - + __rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL ); + __rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL ); + return 0; } static void __exit cangw_exit( void ) { - sock_release( udp_sock ); - sock_release( can_sock ); - + if( cegw_state == CEGW_RUNNING ) + { + cegw_thread_stop(); + /* ToDo: frees mem_cache? */ + /* udp must not exists */ + } + + /* ToDo: unregister netlink + * free jobs */ printk( "cangw: exit\n" ); - //kthread_stop( ts ); } module_init( cangw_init ); module_exit( cangw_exit ); +