From 0b4443ef7dc2e1dc0711b8ddd55b0c33c76c4ecd Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Thu, 20 Dec 2012 14:17:00 +0100 Subject: [PATCH] Add can-eth-gw from Matejka's repo --- include/linux/can/canethgw.h | 30 +++ net/can/canethgw.c | 347 +++++++++++++++++++++++++++++++++++ 2 files changed, 377 insertions(+) create mode 100644 include/linux/can/canethgw.h create mode 100644 net/can/canethgw.c diff --git a/include/linux/can/canethgw.h b/include/linux/can/canethgw.h new file mode 100644 index 000000000000..65d9cc51b8e9 --- /dev/null +++ b/include/linux/can/canethgw.h @@ -0,0 +1,30 @@ +#ifndef CANETHGW_H +#define CANETHGW_H + +#include + +struct cegw_ioctl +{ + __u32 can_sock; + __u32 udp_sock; + __u32 udp_dstcnt; + __u32 udp_addrlen; + struct sockaddr_in udp_dst[0]; +}; + +#ifdef __KERNEL__ +struct cegw_job +{ + struct kref refcount; + struct socket* can_sock; + struct socket* udp_sock; + __u32 udp_dstcnt; + struct sockaddr_in udp_dst[0]; +}; +#endif + +#define CEGW_IOCTL_BASE 'c' +#define CEGW_IOCTL_START _IOW(CEGW_IOCTL_BASE, 0, struct cegw_ioctl) + +#endif /* CANETHGW_H */ + diff --git a/net/can/canethgw.c b/net/can/canethgw.c new file mode 100644 index 000000000000..d5b030215614 --- /dev/null +++ b/net/can/canethgw.c @@ -0,0 +1,347 @@ +/* + * Copyright: (c) 2012 Czech Technical University in Prague + * + * Authors: + * Radek Matějka + * Michal Sojka + * + * Funded by: Volkswagen Group Research + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version + * 2 of the License, or (at your option) any later version. + */ + +#include +#include +#include +#include +#include +#include +#include +#include "canethgw.h" + +MODULE_LICENSE("GPL"); + +static int cegw_udp2can(void *data); +static int cegw_udp_send(struct socket *udp_sock, struct can_frame *cf, + struct sockaddr_in* addr); +static int cegw_can2udp(void *data); +static int cegw_can_send(struct socket *can_sock, struct can_frame *cf); +static int cegw_thread_start(void *data); +static int cegw_thread_stop(struct cegw_job *job); +static void cegw_job_release(struct kref *ref); + +static int cegw_udp_send(struct socket *udp_sock, struct can_frame *cf, struct sockaddr_in* addr) +{ + struct msghdr mh; + struct kvec vec; + int err; + + 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); + + err = kernel_sendmsg(udp_sock, &mh, &vec, 1, sizeof(*cf)); + + return err; +} + +static int cegw_can_send(struct socket* can_sock, struct can_frame* cf) +{ + struct msghdr mh; + struct kvec vec; + int err; + + 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); + + err = kernel_sendmsg(can_sock, &mh, &vec, 1, sizeof(*cf)); + + return err; +} + +/** + * cegw_udp2can - performs udp->can routing + * + * This function is run as a thread. + */ +static int cegw_udp2can(void *data) +{ + struct can_frame cf; + struct kvec vec; + struct msghdr mh; + struct cegw_job *job = (struct cegw_job *)data; + struct socket *udp_sock = NULL, *can_sock = NULL; + int ret = 0; + + memset(&mh, 0, sizeof(mh)); + udp_sock = job->udp_sock; + can_sock = job->can_sock; + + while (1) { + vec.iov_base = &cf; + vec.iov_len = sizeof(cf); + ret = kernel_recvmsg(udp_sock, &mh, &vec, 1, + sizeof(cf), 0); + if (ret < 1) + break; + + cf.can_id = be32_to_cpu(cf.can_id); + cegw_can_send(can_sock, &cf); + } + + cegw_thread_stop(job); + kref_put(&job->refcount, cegw_job_release); + do_exit(ret); +} + +/** + * cegw_can2udp - performs can->udp routing + * + * This function is run as a thread. + */ +static int cegw_can2udp(void* data) +{ + struct msghdr mh; + struct kvec vec; + struct can_frame cf; + struct cegw_job* job = (struct cegw_job*)data; + struct socket* udp_sock = job->udp_sock; + struct socket* can_sock = job->can_sock; + int i; + int ret; + + memset(&mh, 0, sizeof(mh)); + + while (1) { + vec.iov_base = &cf; + vec.iov_len = sizeof(cf); + + ret = kernel_recvmsg(can_sock, &mh, &vec, 1, + sizeof(cf), 0); + if (ret < 1) + break; + + cf.can_id = cpu_to_be32(cf.can_id); + for (i=0; iudp_dstcnt; i++) { + cegw_udp_send(udp_sock, &cf, &job->udp_dst[i]); + } + } + + cegw_thread_stop(job); + kref_put(&job->refcount, cegw_job_release); + do_exit(ret); +} + +static void cegw_job_release(struct kref *ref) +{ + struct cegw_job *job = container_of(ref, struct cegw_job, refcount); + + fput(job->can_sock->file); + fput(job->udp_sock->file); + kfree(job); +} + +/** + * cegw_thread_start - start working threads + * @data: (struct cegw_job *) with sockets and udp addresses filled in + * + * Two threads are started. One is serving udp->can routing and the other + * can->udp. + */ +static int cegw_thread_start(void *data) +{ + struct task_struct *task = NULL; + struct cegw_job *job = (struct cegw_job *)data; + + kref_init(&job->refcount); + kref_get(&job->refcount); + + task = kthread_run(cegw_udp2can, data, "canethgw_udp2can"); + if (IS_ERR(task)) { + kref_sub(&job->refcount, 2, cegw_job_release); + return -ENOMEM; + } + + task = kthread_run(cegw_can2udp, data, "canethgw_can2udp"); + if (IS_ERR(task)) { + cegw_thread_stop(job); + kref_put(&job->refcount, cegw_job_release); + return -ENOMEM; + } + + return 0; +} + +/** + * cegw_thread_stop - stops threads + */ +static int cegw_thread_stop(struct cegw_job *job) +{ + int how = SHUT_RDWR; + struct sock *sk = NULL; + struct socket *udp_sock = job->udp_sock; + struct socket *can_sock = job->can_sock; + + kernel_sock_shutdown(udp_sock, SHUT_RDWR); + + /* PF_CAN sockets do not implement shutdown - do it manualy */ + sk = can_sock->sk; + how++; + lock_sock(sk); + sk->sk_shutdown |= how; + sk->sk_state_change(sk); + release_sock(sk); + + return 0; +} + +static int cegw_open(struct inode *inode, struct file *file) +{ + file->private_data = NULL; + + if (try_module_get(THIS_MODULE) == false) + return -EAGAIN; + + return 0; +} + +static int cegw_release(struct inode *inode, struct file *file) +{ + struct cegw_job *job = (struct cegw_job *)file->private_data; + + if (job) { + cegw_thread_stop(job); + } + + module_put(THIS_MODULE); + return 0; +} + +/** + * cegw_ioctl_start - processes ioctl CEGW_IOCTL_START call + * + * The function takes over cegw_ioctl structure from userspace and + * prepares cegw_job structure. The cegw_job is stored in + * file->private_data and used by kernel threads to serve gateway + * functionality. + */ +static long cegw_ioctl_start(struct file *file, unsigned long arg) +{ + int i; + int err = 0; + __u32 dstcnt = 0; + __u32 addrlen = 0; + struct cegw_ioctl gwctl; + struct cegw_job *job = NULL; + + err = copy_from_user(&gwctl, (void __user *)arg, sizeof(gwctl)); + if (err != 0) + return -EFAULT; + + dstcnt = gwctl.udp_dstcnt; + addrlen = gwctl.udp_addrlen; + + if (addrlen != sizeof(struct sockaddr_in)) + return -EAFNOSUPPORT; + + /* */ + job = kmalloc(GFP_KERNEL, sizeof(*job) + dstcnt*addrlen ); + if (job == NULL) + return -ENOMEM; + + err = copy_from_user(&job->udp_dst, (void __user *)(arg + sizeof(struct cegw_ioctl)), dstcnt*addrlen); + if (err != 0) { + kfree(job); + return -EFAULT; + } + + for (i=0; iudp_dst[i].sin_family != AF_INET) { + kfree(job); + return -EAFNOSUPPORT; + } + } + + job->udp_sock = sockfd_lookup(gwctl.udp_sock, &err); + if (job->udp_sock == NULL) { + kfree(job); + return err; + } + + job->can_sock = sockfd_lookup(gwctl.can_sock, &err); + if (job->can_sock == NULL) { + fput(job->udp_sock->file); + kfree(job); + return err; + } + + job->udp_dstcnt = dstcnt; + + err = cegw_thread_start(job); + if (err != 0) + return err; + + file->private_data = job; + return 0; +} + +static long cegw_ioctl(struct file *file, unsigned int cmd, unsigned long arg) +{ + int err; + + switch (cmd) { + case CEGW_IOCTL_START: + err = cegw_ioctl_start(file, arg); + break; + default: + err = -EOPNOTSUPP; + break; + } + + return err; +} + +static const struct file_operations cegw_fops = { + .owner = THIS_MODULE, + .open = cegw_open, + .release = cegw_release, + .unlocked_ioctl = cegw_ioctl +}; + +static struct miscdevice cegw_device = { + .minor = MISC_DYNAMIC_MINOR, + .name = "canethgw", + .fops = &cegw_fops +}; + +static int __init cegw_init(void) +{ + misc_register(&cegw_device); + + return 0; +} + +static void __exit cegw_exit(void) +{ + misc_deregister(&cegw_device); + + return; +} + +module_init(cegw_init); +module_exit(cegw_exit); + -- 2.39.2