net/can/canethgw.c

   1 /*
   2  * Copyright: (c) 2012 Czech Technical University in Prague
   3  *
   4  * Authors:
   5  *      Radek Matějka <radek.matejka@gmail.com>
   6  *      Michal Sojka  <sojkam1@fel.cvut.cz>
   7  *
   8  * Funded by: Volkswagen Group Research
   9  *
  10  * This program is free software; you can redistribute it and/or
  11  * modify it under the terms of the GNU General Public License
  12  * as published by the Free Software Foundation; either version
  13  * 2 of the License, or (at your option) any later version.
  14  */
  15
  16 #include <linux/module.h>
  17 #include <linux/kernel.h>
  18 #include <linux/kthread.h>
  19 #include <linux/file.h>
  20 #include <net/sock.h>
  21 #include <linux/can.h>
  22 #include <linux/miscdevice.h>
  23 #include <linux/can/canethgw.h>
  24
  25 MODULE_LICENSE("GPL");
  26
  27 struct cegw_job
  28 {
  29         struct kref refcount;
  30         struct socket* can_sock;
  31         struct socket* udp_sock;
  32         __u32  udp_dstcnt;
  33         struct sockaddr_in udp_dst[0];
  34 };
  35
  36 static int cegw_udp2can(void *data);
  37 static int cegw_udp_send(struct socket *udp_sock, struct can_frame *cf,
  38                 struct sockaddr_in* addr);
  39 static int cegw_can2udp(void *data);
  40 static int cegw_can_send(struct socket *can_sock, struct can_frame *cf);
  41 static int cegw_thread_start(void *data);
  42 static int cegw_thread_stop(struct cegw_job *job);
  43 static void cegw_job_release(struct kref *ref);
  44
  45 static int cegw_udp_send(struct socket *udp_sock, struct can_frame *cf, struct sockaddr_in* addr)
  46 {
  47         struct msghdr mh;
  48         struct kvec vec;
  49         int err;
  50
  51         mh.msg_name = addr;
  52         mh.msg_namelen = sizeof(*addr);
  53         mh.msg_control = NULL;
  54         mh.msg_controllen = 0;
  55         mh.msg_flags = 0;
  56
  57         vec.iov_base = cf;
  58         vec.iov_len = sizeof(*cf);
  59
  60         err = kernel_sendmsg(udp_sock, &mh, &vec, 1, sizeof(*cf));
  61
  62         return err;
  63 }
  64
  65 static int cegw_can_send(struct socket* can_sock, struct can_frame* cf)
  66 {
  67         struct msghdr mh;
  68         struct kvec vec;
  69         int err;
  70
  71         mh.msg_name = NULL;
  72         mh.msg_namelen = 0;
  73         mh.msg_control = NULL;
  74         mh.msg_controllen = 0;
  75         mh.msg_flags = 0;
  76
  77         vec.iov_base = cf;
  78         vec.iov_len = sizeof(*cf);
  79
  80         err = kernel_sendmsg(can_sock, &mh, &vec, 1, sizeof(*cf));
  81
  82         return err;
  83 }
  84
  85 /**
  86  * cegw_udp2can - performs udp->can routing
  87  *
  88  * This function is run as a thread.
  89  */
  90 static int cegw_udp2can(void *data)
  91 {
  92         struct can_frame cf;
  93         struct kvec vec;
  94         struct msghdr mh;
  95         struct cegw_job *job = (struct cegw_job *)data;
  96         struct socket *udp_sock = NULL, *can_sock = NULL;
  97         int ret = 0;
  98
  99         memset(&mh, 0, sizeof(mh));
 100         udp_sock = job->udp_sock;
 101         can_sock = job->can_sock;
 102
 103         while (1) {
 104                 vec.iov_base = &cf;
 105                 vec.iov_len = sizeof(cf);
 106                 ret = kernel_recvmsg(udp_sock, &mh, &vec, 1,
 107                                      sizeof(cf), 0);
 108                 if (ret < 1)
 109                         break;
 110
 111                 cf.can_id = be32_to_cpu(cf.can_id);
 112                 cegw_can_send(can_sock, &cf);
 113         }
 114
 115         cegw_thread_stop(job);
 116         kref_put(&job->refcount, cegw_job_release);
 117         do_exit(ret);
 118 }
 119
 120 /**
 121  * cegw_can2udp - performs can->udp routing
 122  *
 123  * This function is run as a thread.
 124  */
 125 static int cegw_can2udp(void* data)
 126 {
 127         struct msghdr mh;
 128         struct kvec vec;
 129         struct can_frame cf;
 130         struct cegw_job* job = (struct cegw_job*)data;
 131         struct socket* udp_sock = job->udp_sock;
 132         struct socket* can_sock = job->can_sock;
 133         int i;
 134         int ret;
 135
 136         memset(&mh, 0, sizeof(mh));
 137
 138         while (1) {
 139                 vec.iov_base = &cf;
 140                 vec.iov_len = sizeof(cf);
 141
 142                 ret = kernel_recvmsg(can_sock, &mh, &vec, 1,
 143                                      sizeof(cf), 0);
 144                 if (ret != sizeof(cf))
 145                         break;
 146
 147                 cf.can_id = cpu_to_be32(cf.can_id);
 148                 for (i=0; i<job->udp_dstcnt; i++) {
 149                         cegw_udp_send(udp_sock, &cf, &job->udp_dst[i]);
 150                 }
 151         }
 152
 153         cegw_thread_stop(job);
 154         kref_put(&job->refcount, cegw_job_release);
 155         do_exit(ret);
 156 }
 157
 158 static void cegw_job_release(struct kref *ref)
 159 {
 160         struct cegw_job *job = container_of(ref, struct cegw_job, refcount);
 161
 162         fput(job->can_sock->file);
 163         fput(job->udp_sock->file);
 164         kfree(job);
 165 }
 166
 167 /**
 168  * cegw_thread_start - start working threads
 169  * @data: (struct cegw_job *) with sockets and udp addresses filled in
 170  *
 171  * Two threads are started. One is serving udp->can routing and the other
 172  * can->udp.
 173  */
 174 static int cegw_thread_start(void *data)
 175 {
 176         struct task_struct *task = NULL;
 177         struct cegw_job *job = (struct cegw_job *)data;
 178
 179         kref_init(&job->refcount);
 180
 181         task = kthread_run(cegw_udp2can, data, "canethgw_udp2can");
 182         if (IS_ERR(task)) {
 183                 kref_put(&job->refcount, cegw_job_release);
 184                 return -ENOMEM;
 185         }
 186
 187         kref_get(&job->refcount);
 188         task = kthread_run(cegw_can2udp, data, "canethgw_can2udp");
 189         if (IS_ERR(task)) {
 190                 cegw_thread_stop(job);
 191                 kref_put(&job->refcount, cegw_job_release);
 192                 return -ENOMEM;
 193         }
 194
 195         return 0;
 196 }
 197
 198 /**
 199  * cegw_thread_stop - stops threads
 200  */
 201 static int cegw_thread_stop(struct cegw_job *job)
 202 {
 203         int how = SHUT_RDWR;
 204         struct sock *sk = NULL;
 205         struct socket *udp_sock = job->udp_sock;
 206         struct socket *can_sock = job->can_sock;
 207
 208         kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 209
 210         /* PF_CAN sockets do not implement shutdown - do it manualy */
 211         sk = can_sock->sk;
 212         how++;
 213         lock_sock(sk);
 214         sk->sk_shutdown |= how;
 215         sk->sk_state_change(sk);
 216         release_sock(sk);
 217
 218         return 0;
 219 }
 220
 221 static int cegw_open(struct inode *inode, struct file *file)
 222 {
 223         file->private_data = NULL;
 224
 225         if (try_module_get(THIS_MODULE) == false)
 226                 return -EAGAIN;
 227
 228         return 0;
 229 }
 230
 231 static int cegw_release(struct inode *inode, struct file *file)
 232 {
 233         struct cegw_job *job = (struct cegw_job *)file->private_data;
 234
 235         if (job) {
 236                 cegw_thread_stop(job);
 237         }
 238
 239         module_put(THIS_MODULE);
 240         return 0;
 241 }
 242
 243 /**
 244  * cegw_ioctl_start - processes ioctl CEGW_IOCTL_START call
 245  *
 246  * The function takes over cegw_ioctl structure from userspace and
 247  * prepares cegw_job structure. The cegw_job is stored in
 248  * file->private_data and used by kernel threads to serve gateway
 249  * functionality.
 250  */
 251 static long cegw_ioctl_start(struct file *file, unsigned long arg)
 252 {
 253         int i;
 254         int err = 0;
 255         __u32 dstcnt = 0;
 256         __u32 addrlen = 0;
 257         struct cegw_ioctl gwctl;
 258         struct cegw_job *job = NULL;
 259
 260         err = copy_from_user(&gwctl, (void __user *)arg, sizeof(gwctl));
 261         if (err != 0)
 262                 return -EFAULT;
 263
 264         dstcnt = gwctl.udp_dstcnt;
 265         addrlen = gwctl.udp_addrlen;
 266
 267         if (addrlen != sizeof(struct sockaddr_in))
 268                 return -EAFNOSUPPORT;
 269
 270         job = kmalloc(GFP_KERNEL, sizeof(*job) + dstcnt*addrlen );
 271         if (job == NULL)
 272                 return -ENOMEM;
 273
 274         err = copy_from_user(&job->udp_dst, (void __user *)(arg + sizeof(struct cegw_ioctl)), dstcnt*addrlen);
 275         if (err != 0) {
 276                 kfree(job);
 277                 return -EFAULT;
 278         }
 279
 280         for (i=0; i<dstcnt; i++) {
 281                 if (job->udp_dst[i].sin_family != AF_INET) {
 282                         kfree(job);
 283                         return -EAFNOSUPPORT;
 284                 }
 285         }
 286
 287         job->udp_sock = sockfd_lookup(gwctl.udp_sock, &err);
 288         if (job->udp_sock == NULL) {
 289                 kfree(job);
 290                 return err;
 291         }
 292
 293         job->can_sock = sockfd_lookup(gwctl.can_sock, &err);
 294         if (job->can_sock == NULL) {
 295                 fput(job->udp_sock->file);
 296                 kfree(job);
 297                 return err;
 298         }
 299
 300         if (job->can_sock->ops->family != AF_CAN ||
 301             job->can_sock->type != CAN_RAW) {
 302                 kfree(job);
 303                 return -EBADF;
 304         }
 305
 306         job->udp_dstcnt = dstcnt;
 307
 308         err = cegw_thread_start(job);
 309         if (err != 0)
 310                 return err;
 311
 312         file->private_data = job;
 313         return 0;
 314 }
 315
 316 static long cegw_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 317 {
 318         long err;
 319
 320         switch (cmd) {
 321                 case CEGW_IOCTL_START:
 322                         err = cegw_ioctl_start(file, arg);
 323                         break;
 324                 default:
 325                         err = -EOPNOTSUPP;
 326                         break;
 327         }
 328
 329         return err;
 330 }
 331
 332 static const struct file_operations cegw_fops = {
 333         .owner = THIS_MODULE,
 334         .open = cegw_open,
 335         .release = cegw_release,
 336         .unlocked_ioctl = cegw_ioctl
 337 };
 338
 339 static struct miscdevice cegw_device = {
 340         .minor = MISC_DYNAMIC_MINOR,
 341         .name = "canethgw",
 342         .fops = &cegw_fops
 343 };
 344
 345 static int __init cegw_init(void)
 346 {
 347         return misc_register(&cegw_device);
 348 }
 349
 350 static void __exit cegw_exit(void)
 351 {
 352         misc_deregister(&cegw_device);
 353
 354         return;
 355 }
 356
 357 module_init(cegw_init);
 358 module_exit(cegw_exit);