+#define DEBUG
+
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/kthread.h>
#include <linux/wait.h>
#include <linux/netdevice.h>
#include <linux/socket.h>
+#include <linux/if_arp.h>
#include <linux/net.h>
+#include <linux/netdevice.h>
#include <linux/can/core.h>
#include <linux/can.h>
#include <net/rtnetlink.h>
+#include <net/sock.h>
#include "canethgw.h"
+#include <linux/completion.h>
+#include <linux/mutex.h>
+#include <linux/miscdevice.h>
+#include <net/inet_common.h>
+
+MODULE_LICENSE("GPL");
+
+static int cegw_udp2can(void *data);
+static void cegw_udp_send(struct socket *udp_sock, struct can_frame *cf,
+ struct sockaddr_in* addr);
+static int cegw_can2udp(void *data);
+static void cegw_can_send(struct socket *can_sock, struct can_frame *cf);
+static int cegw_thread_start(void *data);
+static int cegw_thread_stop(void);
+
+enum __cegw_state {
+ CEGW_RUN,
+ CEGW_STOP,
+ CEGW_EXIT
+};
-/**
- * ToDo
- * [ ] check every input
- * [ ] refactor
- */
-
-MODULE_LICENSE( "GPL" );
-
-static int gw_udp_recv( void* data );
-static void gw_udp_send( struct can_frame* cf, struct in_addr ipaddr, u16 port );
-static int gw_can_recv( void* data );
-static void gw_can_send( struct can_frame* cf, int ifidx );
-static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port );
-
-#define CEGW_STOPPED 0
-#define CEGW_RUNNING 1
-
-static struct task_struct* eth_to_can, * can_to_eth;
-static struct socket* udp_sock = NULL;
-static struct socket* can_sock = NULL;
-static int gw_state = CEGW_STOPPED;
-
-struct can_eth_gw
-{
- int src_if_idx;
- struct in_addr dst_addr;
- unsigned short dst_port;
+struct cegw_rule {
+ int can_ifindex;
+ struct in_addr eth_ip;
+ unsigned short eth_port;
struct hlist_node list;
};
-struct eth_can_gw
-{
- int dst_if_idx;
- struct hlist_node list;
+struct cegw_setting {
+ struct in_addr eth_ip;
+ unsigned short eth_port;
};
-HLIST_HEAD( can_eth_job );
-HLIST_HEAD( eth_can_job );
-struct cegw_setting
-{
- struct can_filter filter;
- int src_idx;
- /* bind on if */
- struct in_addr dst_addr;
- unsigned short dst_port;
-};
+static int cegw_state = CEGW_STOP;
+static struct socket *can_sock = NULL, *udp_sock = NULL;
+static struct task_struct *eth_to_can = NULL, *can_to_eth = NULL;
+static struct notifier_block notifier;
-/***********************
- * UDP
- ***********************/
+static HLIST_HEAD(rule_eth_can);
+static HLIST_HEAD(rule_can_eth);
+static DEFINE_MUTEX(rule_eth_can_mutex);
+static DEFINE_MUTEX(rule_can_eth_mutex);
+static DEFINE_MUTEX(cegw_mutex);
-static int gw_udp_recv( void* data )
+static void cegw_udp_send(struct socket *udp_sock, struct can_frame *cf, struct sockaddr_in* addr)
{
- struct can_frame cf;
- struct kvec vec;
struct msghdr mh;
- struct eth_can_gw* job;
- struct hlist_node* pos;
- int can_ifidx;
-
- vec.iov_base = &cf;
- vec.iov_len = sizeof(cf);
+ struct kvec vec;
- mh.msg_name = NULL;
- mh.msg_namelen = 0;
- mh.msg_iov = NULL;
- mh.msg_iovlen = 0;
+ mh.msg_name = addr;
+ mh.msg_namelen = sizeof(*addr);
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 );
- 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 */
- gw_can_send( &cf, can_ifidx );
- }
- }
+ vec.iov_base = cf;
+ vec.iov_len = sizeof(*cf);
- return 0;
+ /* FIXME: Convert endianing of cf->can_id */
+ kernel_sendmsg(udp_sock, &mh, &vec, 1, sizeof(*cf));
}
-inline static void gw_udp_send( struct can_frame* cf, struct in_addr ipaddr, u16 port )
+static void cegw_can_send(struct socket* can_sock, struct can_frame* cf)
{
struct msghdr mh;
- struct sockaddr_in addr;
struct kvec vec;
-
- addr.sin_family = AF_INET;
- addr.sin_port = htons( port );
- addr.sin_addr = ipaddr;
-
- mh.msg_name = &addr;
- mh.msg_namelen = sizeof( addr );
+
+ mh.msg_name = NULL;
+ mh.msg_namelen = 0;
mh.msg_control = NULL;
mh.msg_controllen = 0;
mh.msg_flags = 0;
-
+
vec.iov_base = cf;
- vec.iov_len = sizeof( *cf );
-
- kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
-}
+ vec.iov_len = sizeof(*cf);
-/***********************
- * CAN
- ***********************/
+ kernel_sendmsg(can_sock, &mh, &vec, 1, sizeof(*cf));
+}
-static int gw_can_recv( void* data )
+/**
+ * cegw_udp2can - performs udp->can routing
+ *
+ * This function is run as a thread.
+ */
+static int cegw_udp2can(void *data)
{
- struct msghdr mh;
- 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;
-
- mh.msg_name = &ca;
- mh.msg_namelen = sizeof( ca );
- mh.msg_control = NULL;
- mh.msg_controllen = 0;
- mh.msg_flags = 0;
-
- vec.iov_base = &cf;
- vec.iov_len = sizeof( cf );
-
- while( 1 )
- {
- if( kthread_should_stop() ) /**/
- break;
- kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
- printk( "received can msg_id:%d, from:%d\n", cf.can_id, ca.can_ifindex );
- hlist_for_each_entry_rcu( job, pos, &can_eth_job, list )
+ struct kvec vec;
+ struct msghdr mh;
+ struct cegw_job *job = (struct cegw_job *)data;
+ struct socket *udp_sock = NULL, *can_sock = NULL;
+ int recv_size;
+
+ memset(&mh, 0, sizeof(mh));
+ udp_sock = job->udp_sock;
+ can_sock = job->can_sock;
+
+ while (1) {
+ printk( "recv\n" );
+ vec.iov_base = &cf;
+ vec.iov_len = sizeof(cf);
+ recv_size = kernel_recvmsg(udp_sock, &mh, &vec, 1,
+ sizeof(cf), 0);
+ /* if(recv_size != sizeof(cf) */
+ if (recv_size < 1) /* ToDo: split 0 and else */
{
- rcu_read_lock();
- eth_addr = job->dst_addr;
- eth_port = job->dst_port;
- rcu_read_unlock();
- printk( KERN_INFO "%x\n", eth_addr );
- if( job->src_if_idx == ca.can_ifindex )
- gw_udp_send( &cf, eth_addr, eth_port );
+ printk("udp2can error\n");
+ break;
}
+
+ /* FIXME: Convert endianing of cf.can_id */
+ printk( "sendit to can\n" );
+ cegw_can_send(can_sock, &cf);
}
-
+
return 0;
}
-inline static void gw_can_send( struct can_frame* cf, int ifidx )
+/**
+ * cegw_can2udp - performs can->udp routing
+ *
+ * Runs as a thread.
+ */
+static int cegw_can2udp(void* data)
{
struct msghdr mh;
struct kvec vec;
- struct sockaddr_can ca =
- {
- .can_family = AF_CAN,
- .can_ifindex = ifidx
- };
-
- mh.msg_name = &ca;
- mh.msg_namelen = sizeof( ca );
- 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 ) );
-}
+ struct can_frame cf;
+ struct cegw_job* job = (struct cegw_job*)data;
+ int recv_size;
+ struct socket* udp_sock = job->udp_sock;
+ struct socket* can_sock = job->can_sock;
+ int i;
-/* NetLink */
+ memset(&mh, 0, sizeof(mh));
-static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
-{
- struct nlattr* tb[ CGW_MAX+1 ];
- struct rtmsg *r;
- struct can_eth_gw* cethgw = NULL;
- struct eth_can_gw* ecangw = NULL;
- int err = 0;
- int type = 0;
-
- /* ToDo: size check
- if (nlmsg_len(nlh) < sizeof(*r))
- return -EINVAL;
- */
-
- err = nlmsg_parse( nlh, sizeof( struct rtmsg ), tb, CGW_MAX, NULL );
- if( err < 0 )
- {
- printk( KERN_ERR "error: nlmsg_parse\n" );
- return err;
- }
+ while (1) {
+ vec.iov_base = &cf;
+ vec.iov_len = sizeof(cf);
- if( tb[CGW_CMD_INFO] == NULL )
- {
- printk( "error: bad cmd\n" );
- return -EINVAL;
- }
+ recv_size = kernel_recvmsg(can_sock, &mh, &vec, 1,
+ sizeof(cf), 0);
- switch( *(int*)nla_data( tb[CGW_CMD_INFO] ) )
- {
- case CEGW_LISTEN:
- listen( 0, *(struct in_addr*)nla_data( tb[CGW_LISTEN_IP] ),
- *(u16*)nla_data( tb[CGW_LISTEN_PORT] ) );
+ if (recv_size < 1)
break;
- case CGW_TYPE_CAN_ETH_UDP:
- printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
- printk( KERN_INFO "eth addr:%x\n", *(u32*)nla_data( tb[CGW_ETH_IP] ) );
- printk( KERN_INFO "eth port:%hu\n", *(u16*)nla_data( tb[CGW_ETH_PORT] ) );
- cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
- if( cethgw == NULL )
- {
- printk( KERN_ERR "error: kmalloc\n" );
- break;
+
+ for( i=0; i<job->udp_dstcnt; i++ )
+ {
+ cegw_udp_send(udp_sock, &cf, &job->udp_dst[i]);
+ }
+ }
+
+ return 0;
+}
+
+static int cegw_notifier(struct notifier_block *nb, unsigned long msg, void *data)
+{
+ struct net_device *dev = (struct net_device *)data;
+ struct cegw_rule *rule;
+ struct hlist_node *pos, *n;
+
+ if (!net_eq(dev_net(dev), &init_net))
+ return NOTIFY_DONE;
+ if (dev->type != ARPHRD_CAN)
+ return NOTIFY_DONE;
+
+ if (msg == NETDEV_UNREGISTER) {
+ hlist_for_each_entry_safe(rule, pos, n, &rule_eth_can, list) {
+ if (rule->can_ifindex == dev->ifindex) {
+ hlist_del(&rule->list);
+ kfree(rule);
}
- cethgw->src_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
- cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CGW_ETH_IP] );
- cethgw->dst_port = *(u16*)nla_data( tb[CGW_ETH_PORT] );
-
- hlist_add_head_rcu( &cethgw->list, &can_eth_job );
- break;
- case CGW_TYPE_ETH_CAN_UDP:
- printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
- ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
- if( ecangw == NULL )
- {
- printk( KERN_ERR "error: kmalloc\n" );
- break;
+ }
+
+ hlist_for_each_entry_safe(rule, pos, n, &rule_can_eth, list) {
+ if (rule->can_ifindex == dev->ifindex) {
+ hlist_del(&rule->list);
+ kfree(rule);
}
- ecangw->dst_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
- hlist_add_head_rcu( &ecangw->list, ð_can_job );
- break;
- default:
- printk( "default" );
- /* ToDo undef operation */
- break;
+ }
}
- return 0;
+ return NOTIFY_DONE;
}
-static int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
+/**
+ * cegw_thread_start - start working threads
+ * @data: (struct cegw_setting *) with new listening address
+ *
+ * Two threads are started. One is serving udp->can routing and the other
+ * can->udp.
+ */
+static int cegw_thread_start(void *data)
{
- struct rtmsg* r;
- struct nlattr* tb[ CGW_MAX+1 ];
- struct hlist_node* pos,* n;
- struct can_eth_gw* ceth;
- struct eth_can_gw* ecan;
+ struct task_struct *task = NULL;
- int err = 0;
+ task = kthread_run(cegw_udp2can, data, "canethgw_udp2can");
+ if (IS_ERR(task)) {
+ goto out_err;
+ }
- if( nlmsg_len(nlh) < sizeof(*r) )
- return -EINVAL;
-
- r = nlmsg_data( nlh );
+ task = kthread_run(cegw_can2udp, data, "canethgw_can2udp");
+ if (IS_ERR(task)) {
+ goto out_err;
+ }
- if( r->rtm_family != AF_CAN )
- return -EPFNOSUPPORT;
+ return 0;
+out_err:
+ return -ENOMEM;
+}
- /*
- if( r->gwtype != CGW_TYPE_CAN_ETH_UDP )
- return -EINVAL;
- */
- printk( "attrsize=%d\n", nlmsg_attrlen(nlh, sizeof(struct rtmsg)) );
+/**
+ * cegw_thread_stop - stops threads and wait for exit
+ *
+ * Waits for threads to stop. Does nothing if cegw_state == CEGW_STOP.
+ */
+static int cegw_thread_stop(void)
+{
+ int how = SHUT_RDWR;
+ struct sock *sk = NULL;
+
+ if (cegw_state == CEGW_STOP)
+ return 0;
+
+ cegw_state = CEGW_STOP;
+ /* 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);
+ sock_release(udp_sock);
+ sock_release(can_sock);
+ can_to_eth = NULL;
+ eth_to_can = NULL;
- err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CGW_MAX, NULL );
- if( err != 0 )
- return -EINVAL;
+ return 0;
+}
- if( tb[CGW_CMD_INFO] == NULL )
- return -EINVAL;
-
- if( *(int*)nla_data( tb[CGW_CMD_INFO] ) == CEGW_FLUSH )
- {
- 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 );
- }
- }
- // tb[]
+static int cegw_open(struct inode *inode, struct file *file)
+{
+ file->private_data = "greetings";
+
+ printk("cegw device opened\n");
return 0;
}
-static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port )
+static int cegw_release(struct inode *inode, struct file *file)
{
- struct sockaddr_in udp_addr;
- struct sockaddr_can can_addr;
- struct socket* tmp;
+ struct cegw_job *job = (struct cegw_job *)file->private_data;
- printk( KERN_INFO "listen called\n" );
+ sock_release( job->udp_sock );
+ sock_release( job->can_sock );
+ printk("cegw device released, data=%s\n", (char *)file->private_data);
+ return 0;
+}
- if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &tmp) != 0 )
+static long cegw_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+ int err = 0;
+ int memsz = 0;
+ __u32 dstcnt = 0;
+ __u32 addrlen = 0;
+ struct cegw_ioctl *gwctl = NULL;
+ struct cegw_job *job = NULL;
+
+ /* ToDo: verify access and authorization */
+ if( get_user(dstcnt, &((struct cegw_ioctl __user *)arg)->udp_dstcnt) != 0 )
+ return -EFAULT;
+ if( get_user(addrlen, &((struct cegw_ioctl __user *)arg)->udp_addrlen) != 0 )
+ return -EFAULT;
+
+ memsz = sizeof(*gwctl) + dstcnt * addrlen;
+ /* ToDo: memory limit */
+ gwctl = kmalloc(GFP_KERNEL, memsz);
+ if (gwctl==NULL)
+ return -ENOMEM;
+
+ err = copy_from_user(gwctl, (void __user *)arg, memsz);
+ if (err != 0)
{
- printk( KERN_ERR "error: can_sock creation failed\n" );
- return -1;
+ return -EFAULT;
}
+ /**/
+ job = kmalloc(GFP_KERNEL, sizeof(*job));
+ if (job == NULL)
+ return -ENOMEM;
- can_addr.can_family = AF_CAN;
- can_addr.can_ifindex = can_ifidx;
-
- if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
- {
- printk( KERN_ERR "can_sock bind failed\n" );
- return -1;
- }
-
- printk( KERN_INFO "can socket success\n" );
+ job->udp_dst = kmalloc(GFP_KERNEL, dstcnt * addrlen); /* ToDo: limit */
+ if (job->udp_dst == NULL)
+ return -ENOMEM;
- udp_addr.sin_family = AF_INET;
- udp_addr.sin_port = htons( eth_port );
- udp_addr.sin_addr = eth_addr;
+ err = copy_from_user(job->udp_dst, (void __user *)(arg + sizeof(struct cegw_ioctl)), dstcnt*addrlen);
+ if (err != 0)
+ return -EFAULT;
- printk( KERN_INFO "trying to bind\n" );
- if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
- {
- printk( "error: binding failed\n" );
- sock_release( udp_sock );
- sock_release( can_sock );
- return -1;
- }
+ job->udp_sock = sockfd_lookup( gwctl->udp_sock, &err );
+ if (job->udp_sock == NULL)
+ return err;
+
+ job->can_sock = sockfd_lookup( gwctl->can_sock, &err );
+ if (job->can_sock == NULL)
+ return err;
+
+ job->udp_dstcnt = dstcnt;
- printk( KERN_INFO "socket established\n" );
-
- /* run threads */
- eth_to_can = kthread_run( gw_udp_recv, NULL, "ethcangw" );
- can_to_eth = kthread_run( gw_can_recv, NULL, "canethgw" );
+ /* ToDo: sin_family? */
- printk( KERN_INFO "threads are running\n" );
+ file->private_data = job;
+ cegw_thread_start(job);
- gw_state = CEGW_RUNNING;
+ /* process udp destinations */
+ switch (cmd) {
+ case CEGW_IOCTL_START:
+ printk("udp_dstcnt=%i\n", gwctl->udp_dstcnt);
+ printk("udp_dst[0] family=%i\n", job->udp_dst[0].sin_family);
+ printk("udp_dst[1] family=%i\n", job->udp_dst[1].sin_family);
+ break;
+ default:
+ printk("undefined ioctl command\n");
+ break;
+ }
return 0;
}
-/***********************
- * module init/exit
- ***********************/
+static const struct file_operations cegw_fops = {
+ .owner = THIS_MODULE,
+ .open = cegw_open,
+ .release = cegw_release,
+ .unlocked_ioctl = cegw_ioctl
+};
-static int __init cangw_init( void )
-{
- if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
- {
- printk( KERN_ERR "error: can_sock creation failed\n" );
- return -1;
- }
+static struct miscdevice cegw_device = {
+ .minor = MISC_DYNAMIC_MINOR,
+ .name = "cegw",
+ .fops = &cegw_fops
+};
+
+static int __init cegw_init(void)
+{
+ misc_register(&cegw_device);
+
+ return 0;
+ notifier.notifier_call = cegw_notifier;
+ register_netdevice_notifier(¬ifier);
- if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
- {
- printk( KERN_ERR "error: udp_sock creation failed\n" );
- sock_release( can_sock );
- return -1;
- }
-
- /* subscribe to netlink */
- //if( __rtnl_register( PF_CAN, RTM_GETROUTE, ) != 0 )
- __rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL );
- __rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL );
- //__rtnl_register( PF_CAN, RTM_DELROUTE, )
-
- /*
- if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 )
- {s
- printk( "error: can_sock creation failed\n" );
- }
- */
-
return 0;
}
-static void __exit cangw_exit( void )
+static void __exit cegw_exit(void)
{
- if( gw_state == CEGW_RUNNING )
- {
- sock_release( udp_sock );
- sock_release( can_sock );
- /* ToDo: stop threads */
- }
+ misc_deregister(&cegw_device);
+
+ return;
+ /* ToDo: effect on cangw? */
+ rtnl_unregister_all(PF_CAN);
+
+ /* wait for rtnl callbacks */
+ rtnl_lock();
+ rtnl_unlock();
+
+ mutex_lock(&cegw_mutex);
+ cegw_thread_stop();
+ cegw_state = CEGW_EXIT;
+ mutex_unlock(&cegw_mutex);
- /* ToDo: unregister netlink
- * free jobs */
- printk( "cangw: exit\n" );
- //kthread_stop( ts );
+ unregister_netdevice_notifier(¬ifier);
+ //cegw_flush();
}
-module_init( cangw_init );
-module_exit( cangw_exit );
+module_init(cegw_init);
+module_exit(cegw_exit);
projects / can-eth-gw.git / blobdiff