kernel/canethgw.c

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