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