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