kernel/canethgw.c

   1 #include <linux/module.h>
   2 #include <linux/kernel.h>
   3 #include <linux/kthread.h>
   4 #include <linux/sched.h>
   5 #include <linux/delay.h>
   6 #include <linux/wait.h>
   7 #include <linux/netdevice.h>
   8 #include <linux/socket.h>
   9 #include <linux/net.h>
  10 #include <linux/can/core.h>
  11 #include <linux/can.h>
  12 #include <net/rtnetlink.h>
  13 #include "canethgw.h"
  14
  15 /**
  16  * ToDo
  17  *  [ ] check every input
  18  *  [ ] refactor
  19  */
  20
  21 MODULE_LICENSE( "GPL" );
  22
  23 static int  gw_udp_recv( void* data );
  24 static void gw_udp_send( struct can_frame* cf, struct in_addr ipaddr, u16 port );
  25 static int  gw_can_recv( void* data );
  26 static void gw_can_send( struct can_frame* cf, int ifidx );
  27 static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port );
  28
  29 #define CEGW_STOPPED 0
  30 #define CEGW_RUNNING 1
  31
  32 static struct task_struct* eth_to_can, * can_to_eth;
  33 static struct socket* udp_sock = NULL;
  34 static struct socket* can_sock = NULL;
  35 static int gw_state = CEGW_STOPPED;
  36
  37 struct can_eth_gw
  38 {
  39         int src_if_idx;
  40         struct in_addr dst_addr;
  41         unsigned short dst_port;
  42         struct hlist_node list;
  43 };
  44
  45 struct eth_can_gw
  46 {
  47         int dst_if_idx;
  48         struct hlist_node list;
  49 };
  50
  51 HLIST_HEAD( can_eth_job );
  52 HLIST_HEAD( eth_can_job );
  53
  54 struct cegw_setting
  55 {
  56         struct can_filter filter;
  57         int src_idx;
  58         /* bind on if */
  59         struct in_addr dst_addr;
  60         unsigned short dst_port;
  61 };
  62
  63 /***********************
  64  *   UDP
  65  ***********************/
  66
  67 static int gw_udp_recv( void* data )
  68 {
  69         struct can_frame cf;
  70         struct kvec vec;
  71         struct msghdr mh;
  72         struct eth_can_gw* job;
  73         struct hlist_node* pos;
  74         int can_ifidx;
  75
  76         vec.iov_base = &cf;
  77         vec.iov_len = sizeof(cf);
  78
  79         mh.msg_name = NULL;
  80         mh.msg_namelen = 0;
  81         mh.msg_iov = NULL;
  82         mh.msg_iovlen = 0;
  83         mh.msg_control = NULL;
  84         mh.msg_controllen = 0;
  85         mh.msg_flags = 0;
  86
  87         while( 1 )
  88         {
  89                 if( kthread_should_stop() ) /* up() ?, recv is blocking */
  90                         break;
  91                 kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* todo: handle error */
  92                 printk( "received udp msg_id:%d\n", cf.can_id );
  93                 hlist_for_each_entry_rcu( job, pos, &eth_can_job, list )
  94                 {
  95                         rcu_read_lock(); /**/
  96                         can_ifidx = job->dst_if_idx;
  97                         rcu_read_unlock();
  98                         /* ToDo from filter */
  99                         gw_can_send( &cf, can_ifidx );
 100                 }
 101         }
 102
 103         return 0;
 104 }
 105
 106 inline static void gw_udp_send( struct can_frame* cf, struct in_addr ipaddr, u16 port )
 107 {
 108         struct msghdr mh;
 109         struct sockaddr_in addr;
 110         struct kvec vec;
 111
 112         addr.sin_family = AF_INET;
 113         addr.sin_port = htons( port );
 114         addr.sin_addr = ipaddr;
 115
 116         mh.msg_name = &addr;
 117         mh.msg_namelen = sizeof( addr );
 118         mh.msg_control = NULL;
 119         mh.msg_controllen = 0;
 120         mh.msg_flags = 0;
 121
 122         vec.iov_base = cf;
 123         vec.iov_len = sizeof( *cf );
 124
 125         kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
 126 }
 127
 128 /***********************
 129  *   CAN
 130  ***********************/
 131
 132 static int gw_can_recv( void* data )
 133 {
 134         struct msghdr mh;
 135         struct kvec vec;
 136         struct can_frame cf;
 137         struct sockaddr_can ca;
 138         struct can_eth_gw* job;
 139         struct hlist_node* pos;
 140         struct in_addr eth_addr;
 141         u16 eth_port;
 142
 143         mh.msg_name = &ca;
 144         mh.msg_namelen = sizeof( ca );
 145         mh.msg_control = NULL;
 146         mh.msg_controllen = 0;
 147         mh.msg_flags = 0;
 148
 149         vec.iov_base = &cf;
 150         vec.iov_len = sizeof( cf );
 151
 152         while( 1 )
 153         {
 154                 if( kthread_should_stop() ) /**/
 155                         break;
 156                 kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
 157                 printk( "received can msg_id:%d, from:%d\n", cf.can_id, ca.can_ifindex );
 158                 hlist_for_each_entry_rcu( job, pos, &can_eth_job, list )
 159                 {
 160                         rcu_read_lock();
 161                         eth_addr = job->dst_addr;
 162                         eth_port = job->dst_port;
 163                         rcu_read_unlock();
 164                         printk( KERN_INFO "%x\n", eth_addr );
 165                         if( job->src_if_idx == ca.can_ifindex )
 166                                 gw_udp_send( &cf, eth_addr, eth_port );
 167                 }
 168         }
 169
 170         return 0;
 171 }
 172
 173 inline static void gw_can_send( struct can_frame* cf, int ifidx )
 174 {
 175         struct msghdr mh;
 176         struct kvec vec;
 177         struct sockaddr_can ca =
 178         {
 179                 .can_family = AF_CAN,
 180                 .can_ifindex = ifidx
 181         };
 182
 183         mh.msg_name = &ca;
 184         mh.msg_namelen = sizeof( ca );
 185         mh.msg_control = NULL;
 186         mh.msg_controllen = 0;
 187         mh.msg_flags = 0;
 188
 189         vec.iov_base = cf;
 190         vec.iov_len = sizeof( *cf );
 191
 192         kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) );
 193 }
 194
 195 /* NetLink */
 196
 197 static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 198 {
 199         struct nlattr* tb[ CGW_MAX+1 ];
 200         struct rtmsg *r;
 201         struct can_eth_gw* cethgw = NULL;
 202         struct eth_can_gw* ecangw = NULL;
 203         int err = 0;
 204         int type = 0;
 205
 206         /* ToDo: size check
 207         if (nlmsg_len(nlh) < sizeof(*r))
 208                 return -EINVAL;
 209         */
 210
 211         err = nlmsg_parse( nlh, sizeof( struct rtmsg ), tb, CGW_MAX, NULL );
 212         if( err < 0 )
 213         {
 214                 printk( KERN_ERR "error: nlmsg_parse\n" );
 215                 return err;
 216         }
 217
 218         if( tb[CGW_CMD_INFO] == NULL )
 219         {
 220                 printk( "error: bad cmd\n" );
 221                 return -EINVAL;
 222         }
 223
 224         switch( *(int*)nla_data( tb[CGW_CMD_INFO] ) )
 225         {
 226                 case CEGW_LISTEN:
 227                         listen( 0,  *(struct in_addr*)nla_data( tb[CGW_LISTEN_IP] ),
 228                                     *(u16*)nla_data( tb[CGW_LISTEN_PORT] ) );
 229                         break;
 230                 case CGW_TYPE_CAN_ETH_UDP:
 231                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
 232                         printk( KERN_INFO "eth addr:%x\n", *(u32*)nla_data( tb[CGW_ETH_IP] ) );
 233                         printk( KERN_INFO "eth port:%hu\n", *(u16*)nla_data( tb[CGW_ETH_PORT] ) );
 234                         cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
 235                         if( cethgw == NULL )
 236                         {
 237                                 printk( KERN_ERR "error: kmalloc\n" );
 238                                 break;
 239                         }
 240                         cethgw->src_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
 241                         cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CGW_ETH_IP] );
 242                         cethgw->dst_port = *(u16*)nla_data( tb[CGW_ETH_PORT] );
 243
 244                         hlist_add_head_rcu( &cethgw->list, &can_eth_job );
 245                         break;
 246                 case CGW_TYPE_ETH_CAN_UDP:
 247                         printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
 248                         ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
 249                         if( ecangw == NULL )
 250                         {
 251                                 printk( KERN_ERR "error: kmalloc\n" );
 252                                 break;
 253                         }
 254                         ecangw->dst_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
 255                         hlist_add_head_rcu( &ecangw->list, &eth_can_job );
 256                         break;
 257                 default:
 258                         printk( "default" );
 259                         /* ToDo undef operation */
 260                         break;
 261         }
 262
 263         return 0;
 264 }
 265
 266 static int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
 267 {
 268         struct rtmsg* r;
 269         struct nlattr* tb[ CGW_MAX+1 ];
 270         struct hlist_node* pos,* n;
 271         struct can_eth_gw* ceth;
 272         struct eth_can_gw* ecan;
 273
 274         int err = 0;
 275
 276         if( nlmsg_len(nlh) < sizeof(*r) )
 277                 return -EINVAL;
 278
 279         r = nlmsg_data( nlh );
 280
 281         if( r->rtm_family != AF_CAN )
 282                 return -EPFNOSUPPORT;
 283
 284         /*
 285         if( r->gwtype != CGW_TYPE_CAN_ETH_UDP )
 286                 return -EINVAL;
 287         */
 288         printk( "attrsize=%d\n", nlmsg_attrlen(nlh, sizeof(struct rtmsg)) );
 289
 290         err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CGW_MAX, NULL );
 291         if( err != 0 )
 292                 return -EINVAL;
 293
 294         if( tb[CGW_CMD_INFO] == NULL )
 295                 return -EINVAL;
 296
 297         if( *(int*)nla_data( tb[CGW_CMD_INFO] ) == CEGW_FLUSH )
 298         {
 299                 hlist_for_each_entry_safe( ceth, pos, n, &can_eth_job, list )
 300                 {
 301                         hlist_del( &ceth->list );
 302                         kfree( ceth );
 303                 }
 304                 hlist_for_each_entry_safe( ecan, pos, n, &eth_can_job, list )
 305                 {
 306                         hlist_del( &ecan->list );
 307                         kfree( ecan );
 308                 }
 309         }
 310         //      tb[]
 311         return 0;
 312 }
 313
 314 static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port )
 315 {
 316         struct sockaddr_in udp_addr;
 317         struct sockaddr_can can_addr;
 318         struct socket* tmp;
 319
 320         printk( KERN_INFO "listen called\n" );
 321
 322         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &tmp) != 0 )
 323         {
 324                 printk( KERN_ERR "error: can_sock creation failed\n" );
 325                 return -1;
 326         }
 327
 328         can_addr.can_family = AF_CAN;
 329         can_addr.can_ifindex = can_ifidx;
 330
 331         if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 332         {
 333                 printk( KERN_ERR "can_sock bind failed\n" );
 334                 return -1;
 335         }
 336
 337         printk( KERN_INFO "can socket success\n" );
 338
 339         udp_addr.sin_family = AF_INET;
 340         udp_addr.sin_port = htons( eth_port );
 341         udp_addr.sin_addr = eth_addr;
 342
 343         printk( KERN_INFO "trying to bind\n" );
 344         if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
 345         {
 346                 printk( "error: binding failed\n" );
 347                 sock_release( udp_sock );
 348                 sock_release( can_sock );
 349                 return -1;
 350         }
 351
 352         printk( KERN_INFO "socket established\n" );
 353
 354         /* run threads */
 355         eth_to_can = kthread_run( gw_udp_recv, NULL, "ethcangw" );
 356         can_to_eth = kthread_run( gw_can_recv, NULL, "canethgw" );
 357
 358         printk( KERN_INFO "threads are running\n" );
 359
 360         gw_state = CEGW_RUNNING;
 361
 362         return 0;
 363 }
 364
 365 /***********************
 366  *   module init/exit
 367  ***********************/
 368
 369 static int __init cangw_init( void )
 370 {
 371         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 372         {
 373                 printk( KERN_ERR "error: can_sock creation failed\n" );
 374                 return -1;
 375         }
 376
 377         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
 378         {
 379                 printk( KERN_ERR "error: udp_sock creation failed\n" );
 380                 sock_release( can_sock );
 381                 return -1;
 382         }
 383
 384         /* subscribe to netlink */
 385         //if( __rtnl_register( PF_CAN, RTM_GETROUTE,  ) != 0 )
 386         __rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL );
 387         __rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL );
 388         //__rtnl_register( PF_CAN, RTM_DELROUTE,  )
 389
 390         /*
 391         if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 )
 392         {s
 393                 printk( "error: can_sock creation failed\n" );
 394         }
 395         */
 396
 397         return 0;
 398 }
 399
 400 static void __exit cangw_exit( void )
 401 {
 402         if( gw_state == CEGW_RUNNING )
 403         {
 404                 sock_release( udp_sock );
 405                 sock_release( can_sock );
 406                 /* ToDo: stop threads */
 407         }
 408
 409         /* ToDo: unregister netlink
 410          *       free jobs          */
 411         printk( "cangw: exit\n" );
 412         //kthread_stop( ts );
 413 }
 414
 415 module_init( cangw_init );
 416 module_exit( cangw_exit );
 417