kernel/canethgw.c

   1 #define DEBUG
   2
   3 #include <linux/module.h>
   4 #include <linux/kernel.h>
   5 #include <linux/kthread.h>
   6 #include <linux/sched.h>
   7 #include <linux/delay.h>
   8 #include <linux/wait.h>
   9 #include <linux/netdevice.h>
  10 #include <linux/socket.h>
  11 #include <linux/if_arp.h>
  12 #include <linux/net.h>
  13 #include <linux/netdevice.h>
  14 #include <linux/can/core.h>
  15 #include <linux/can.h>
  16 #include <net/rtnetlink.h>
  17 #include <net/sock.h>
  18 #include "canethgw.h"
  19 #include <linux/completion.h>
  20 #include <linux/mutex.h>
  21 #include <linux/miscdevice.h>
  22 #include <net/inet_common.h>
  23
  24 MODULE_LICENSE("GPL");
  25
  26 static int  cegw_udp2can(void *data);
  27 static void cegw_udp_send(struct socket *udp_sock, struct can_frame *cf,
  28                 struct sockaddr_in* addr);
  29 static int  cegw_can2udp(void *data);
  30 static void cegw_can_send(struct socket *can_sock, struct can_frame *cf);
  31 static int cegw_thread_start(void *data);
  32 static int cegw_thread_stop(void);
  33
  34 enum __cegw_state {
  35         CEGW_RUN,
  36         CEGW_STOP,
  37         CEGW_EXIT
  38 };
  39
  40 struct cegw_rule {
  41         int can_ifindex;
  42         struct in_addr eth_ip;
  43         unsigned short eth_port;
  44         struct hlist_node list;
  45 };
  46
  47 struct cegw_setting {
  48         struct in_addr eth_ip;
  49         unsigned short eth_port;
  50 };
  51
  52
  53 static int cegw_state = CEGW_STOP;
  54 static struct socket *can_sock = NULL, *udp_sock = NULL;
  55 static struct task_struct *eth_to_can = NULL, *can_to_eth = NULL;
  56 static struct notifier_block notifier;
  57
  58 static HLIST_HEAD(rule_eth_can);
  59 static HLIST_HEAD(rule_can_eth);
  60 static DEFINE_MUTEX(rule_eth_can_mutex);
  61 static DEFINE_MUTEX(rule_can_eth_mutex);
  62 static DEFINE_MUTEX(cegw_mutex);
  63
  64 static void cegw_udp_send(struct socket *udp_sock, struct can_frame *cf, struct sockaddr_in* addr)
  65 {
  66         struct msghdr mh;
  67         struct kvec vec;
  68
  69         mh.msg_name = addr;
  70         mh.msg_namelen = sizeof(*addr);
  71         mh.msg_control = NULL;
  72         mh.msg_controllen = 0;
  73         mh.msg_flags = 0;
  74
  75         vec.iov_base = cf;
  76         vec.iov_len = sizeof(*cf);
  77
  78         /* FIXME: Convert endianing of cf->can_id */
  79         kernel_sendmsg(udp_sock, &mh, &vec, 1, sizeof(*cf));
  80 }
  81
  82 static void cegw_can_send(struct socket* can_sock, struct can_frame* cf)
  83 {
  84         struct msghdr mh;
  85         struct kvec vec;
  86
  87         mh.msg_name = NULL;
  88         mh.msg_namelen = 0;
  89         mh.msg_control = NULL;
  90         mh.msg_controllen = 0;
  91         mh.msg_flags = 0;
  92
  93         vec.iov_base = cf;
  94         vec.iov_len = sizeof(*cf);
  95
  96         kernel_sendmsg(can_sock, &mh, &vec, 1, sizeof(*cf));
  97 }
  98
  99 /**
 100  * cegw_udp2can - performs udp->can routing
 101  *
 102  * This function is run as a thread.
 103  */
 104 static int cegw_udp2can(void *data)
 105 {
 106         struct can_frame cf;
 107         struct kvec vec;
 108         struct msghdr mh;
 109         struct cegw_job *job = (struct cegw_job *)data;
 110         struct socket *udp_sock = NULL, *can_sock = NULL;
 111         int recv_size;
 112
 113         memset(&mh, 0, sizeof(mh));
 114         udp_sock = job->udp_sock;
 115         can_sock = job->can_sock;
 116
 117         while (1) {
 118                 printk( "recv\n" );
 119                 vec.iov_base = &cf;
 120                 vec.iov_len = sizeof(cf);
 121                 recv_size = kernel_recvmsg(udp_sock, &mh, &vec, 1,
 122                                 sizeof(cf), 0);
 123                 /* if(recv_size != sizeof(cf) */
 124                 if (recv_size < 1) /* ToDo: split 0 and else */
 125                 {
 126                         printk("udp2can error\n");
 127                         break;
 128                 }
 129
 130                 /* FIXME: Convert endianing of cf.can_id */
 131                 printk( "sendit to can\n" );
 132                 cegw_can_send(can_sock, &cf);
 133         }
 134
 135         return 0;
 136 }
 137
 138 /**
 139  * cegw_can2udp - performs can->udp routing
 140  *
 141  * Runs as a thread.
 142  */
 143 static int cegw_can2udp(void* data)
 144 {
 145         struct msghdr mh;
 146         struct kvec vec;
 147         struct can_frame cf;
 148         struct cegw_job* job = (struct cegw_job*)data;
 149         int recv_size;
 150         struct socket* udp_sock = job->udp_sock;
 151         struct socket* can_sock = job->can_sock;
 152         int i;
 153
 154         memset(&mh, 0, sizeof(mh));
 155
 156         while (1) {
 157                 vec.iov_base = &cf;
 158                 vec.iov_len = sizeof(cf);
 159
 160                 recv_size = kernel_recvmsg(can_sock, &mh, &vec, 1,
 161                                            sizeof(cf), 0);
 162
 163                 if (recv_size < 1)
 164                         break;
 165
 166                 for( i=0; i<job->udp_dstcnt; i++ )
 167                 {
 168                         cegw_udp_send(udp_sock, &cf, &job->udp_dst[i]);
 169                 }
 170         }
 171
 172         return 0;
 173 }
 174
 175 static int cegw_notifier(struct notifier_block *nb, unsigned long msg, void *data)
 176 {
 177         struct net_device *dev = (struct net_device *)data;
 178         struct cegw_rule *rule;
 179         struct hlist_node *pos, *n;
 180
 181         if (!net_eq(dev_net(dev), &init_net))
 182                 return NOTIFY_DONE;
 183         if (dev->type != ARPHRD_CAN)
 184                 return NOTIFY_DONE;
 185
 186         if (msg == NETDEV_UNREGISTER) {
 187                 hlist_for_each_entry_safe(rule, pos, n, &rule_eth_can, list) {
 188                         if (rule->can_ifindex == dev->ifindex) {
 189                                 hlist_del(&rule->list);
 190                                 kfree(rule);
 191                         }
 192                 }
 193
 194                 hlist_for_each_entry_safe(rule, pos, n, &rule_can_eth, list) {
 195                         if (rule->can_ifindex == dev->ifindex) {
 196                                 hlist_del(&rule->list);
 197                                 kfree(rule);
 198                         }
 199                 }
 200         }
 201
 202         return NOTIFY_DONE;
 203 }
 204
 205 /**
 206  * cegw_thread_start - start working threads
 207  * @data: (struct cegw_setting *) with new listening address
 208  *
 209  * Two threads are started. One is serving udp->can routing and the other
 210  * can->udp.
 211  */
 212 static int cegw_thread_start(void *data)
 213 {
 214         struct task_struct *task = NULL;
 215
 216         task = kthread_run(cegw_udp2can, data, "canethgw_udp2can");
 217         if (IS_ERR(task)) {
 218                 goto out_err;
 219         }
 220
 221         task = kthread_run(cegw_can2udp, data, "canethgw_can2udp");
 222         if (IS_ERR(task)) {
 223                 goto out_err;
 224         }
 225
 226         return 0;
 227 out_err:
 228         return -ENOMEM;
 229 }
 230
 231 /**
 232  * cegw_thread_stop - stops threads and wait for exit
 233  *
 234  * Waits for threads to stop. Does nothing if cegw_state == CEGW_STOP.
 235  */
 236 static int cegw_thread_stop(void)
 237 {
 238         int how = SHUT_RDWR;
 239         struct sock *sk = NULL;
 240
 241         if (cegw_state == CEGW_STOP)
 242                 return 0;
 243
 244         cegw_state = CEGW_STOP;
 245         /* shut down socket */
 246         sk = can_sock->sk;
 247         how++;
 248         lock_sock(sk);
 249         sk->sk_shutdown |= how;
 250         sk->sk_state_change(sk);
 251         release_sock(sk);
 252
 253         kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 254
 255         /* wait for return to reuse port if restart */
 256         kthread_stop(eth_to_can);
 257         kthread_stop(can_to_eth);
 258         sock_release(udp_sock);
 259         sock_release(can_sock);
 260         can_to_eth = NULL;
 261         eth_to_can = NULL;
 262
 263         return 0;
 264 }
 265
 266 static int cegw_open(struct inode *inode, struct file *file)
 267 {
 268         file->private_data = "greetings";
 269
 270         printk("cegw device opened\n");
 271         return 0;
 272 }
 273
 274 static int cegw_release(struct inode *inode, struct file *file)
 275 {
 276         struct cegw_job *job = (struct cegw_job *)file->private_data;
 277
 278         sock_release( job->udp_sock );
 279         sock_release( job->can_sock );
 280         printk("cegw device released, data=%s\n", (char *)file->private_data);
 281         return 0;
 282 }
 283
 284 static long cegw_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 285 {
 286         int err = 0;
 287         int memsz = 0;
 288         __u32 dstcnt = 0;
 289         __u32 addrlen = 0;
 290         struct cegw_ioctl *gwctl = NULL;
 291         struct cegw_job *job = NULL;
 292
 293         /* ToDo: verify access and authorization */
 294         if( get_user(dstcnt, &((struct cegw_ioctl __user *)arg)->udp_dstcnt) != 0 )
 295                 return -EFAULT;
 296         if( get_user(addrlen, &((struct cegw_ioctl __user *)arg)->udp_addrlen) != 0 )
 297                 return -EFAULT;
 298
 299         memsz = sizeof(*gwctl) + dstcnt * addrlen;
 300         /* ToDo: memory limit */
 301         gwctl = kmalloc(GFP_KERNEL, memsz);
 302         if (gwctl==NULL)
 303                 return -ENOMEM;
 304
 305         err = copy_from_user(gwctl, (void __user *)arg, memsz);
 306         if (err != 0)
 307         {
 308                 return -EFAULT;
 309         }
 310         /**/
 311         job = kmalloc(GFP_KERNEL, sizeof(*job));
 312         if (job == NULL)
 313                 return -ENOMEM;
 314
 315         job->udp_dst = kmalloc(GFP_KERNEL, dstcnt * addrlen); /* ToDo: limit */
 316         if (job->udp_dst == NULL)
 317                 return -ENOMEM;
 318
 319         err = copy_from_user(job->udp_dst, (void __user *)(arg + sizeof(struct cegw_ioctl)), dstcnt*addrlen);
 320         if (err != 0)
 321                 return -EFAULT;
 322
 323         job->udp_sock = sockfd_lookup( gwctl->udp_sock, &err );
 324         if (job->udp_sock == NULL)
 325                 return err;
 326
 327         job->can_sock = sockfd_lookup( gwctl->can_sock, &err );
 328         if (job->can_sock == NULL)
 329                 return err;
 330
 331         job->udp_dstcnt = dstcnt;
 332
 333         /* ToDo: sin_family? */
 334
 335         file->private_data = job;
 336         cegw_thread_start(job);
 337
 338         /* process udp destinations */
 339         switch (cmd) {
 340                 case CEGW_IOCTL_START:
 341                         printk("udp_dstcnt=%i\n", gwctl->udp_dstcnt);
 342                         printk("udp_dst[0] family=%i\n", job->udp_dst[0].sin_family);
 343                         printk("udp_dst[1] family=%i\n", job->udp_dst[1].sin_family);
 344                         break;
 345                 default:
 346                         printk("undefined ioctl command\n");
 347                         break;
 348         }
 349
 350         return 0;
 351 }
 352
 353 static const struct file_operations cegw_fops = {
 354         .owner = THIS_MODULE,
 355         .open = cegw_open,
 356         .release = cegw_release,
 357         .unlocked_ioctl = cegw_ioctl
 358 };
 359
 360 static struct miscdevice cegw_device = {
 361         .minor = MISC_DYNAMIC_MINOR,
 362         .name = "cegw",
 363         .fops = &cegw_fops
 364 };
 365
 366 static int __init cegw_init(void)
 367 {
 368         misc_register(&cegw_device);
 369
 370         return 0;
 371         notifier.notifier_call = cegw_notifier;
 372         register_netdevice_notifier(&notifier);
 373
 374         return 0;
 375 }
 376
 377 static void __exit cegw_exit(void)
 378 {
 379         misc_deregister(&cegw_device);
 380
 381         return;
 382         /* ToDo: effect on cangw? */
 383         rtnl_unregister_all(PF_CAN);
 384
 385         /* wait for rtnl callbacks */
 386         rtnl_lock();
 387         rtnl_unlock();
 388
 389         mutex_lock(&cegw_mutex);
 390         cegw_thread_stop();
 391         cegw_state = CEGW_EXIT;
 392         mutex_unlock(&cegw_mutex);
 393
 394         unregister_netdevice_notifier(&notifier);
 395         //cegw_flush();
 396 }
 397
 398 module_init(cegw_init);
 399 module_exit(cegw_exit);
 400