1 /* 2 * bcm.c - Broadcast Manager to filter/send (cyclic) CAN content 3 * 4 * Copyright (c) 2002-2017 Volkswagen Group Electronic Research 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 3. Neither the name of Volkswagen nor the names of its contributors 16 * may be used to endorse or promote products derived from this software 17 * without specific prior written permission. 18 * 19 * Alternatively, provided that this notice is retained in full, this 20 * software may be distributed under the terms of the GNU General 21 * Public License ("GPL") version 2, in which case the provisions of the 22 * GPL apply INSTEAD OF those given above. 23 * 24 * The provided data structures and external interfaces from this code 25 * are not restricted to be used by modules with a GPL compatible license. 26 * 27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 30 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 31 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 32 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 33 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 34 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 35 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 36 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 37 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 38 * DAMAGE. 39 * 40 */ 41 42 #include <linux/module.h> 43 #include <linux/init.h> 44 #include <linux/interrupt.h> 45 #include <linux/hrtimer.h> 46 #include <linux/list.h> 47 #include <linux/proc_fs.h> 48 #include <linux/seq_file.h> 49 #include <linux/uio.h> 50 #include <linux/net.h> 51 #include <linux/netdevice.h> 52 #include <linux/socket.h> 53 #include <linux/if_arp.h> 54 #include <linux/skbuff.h> 55 #include <linux/can.h> 56 #include <linux/can/core.h> 57 #include <linux/can/skb.h> 58 #include <linux/can/bcm.h> 59 #include <linux/slab.h> 60 #include <net/sock.h> 61 #include <net/net_namespace.h> 62 63 /* 64 * To send multiple CAN frame content within TX_SETUP or to filter 65 * CAN messages with multiplex index within RX_SETUP, the number of 66 * different filters is limited to 256 due to the one byte index value. 67 */ 68 #define MAX_NFRAMES 256 69 70 /* use of last_frames[index].flags */ 71 #define RX_RECV 0x40 /* received data for this element */ 72 #define RX_THR 0x80 /* element not been sent due to throttle feature */ 73 #define BCM_CAN_FLAGS_MASK 0x3F /* to clean private flags after usage */ 74 75 /* get best masking value for can_rx_register() for a given single can_id */ 76 #define REGMASK(id) ((id & CAN_EFF_FLAG) ? \ 77 (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \ 78 (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG)) 79 80 #define CAN_BCM_VERSION "20170425" 81 82 MODULE_DESCRIPTION("PF_CAN broadcast manager protocol"); 83 MODULE_LICENSE("Dual BSD/GPL"); 84 MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>"); 85 MODULE_ALIAS("can-proto-2"); 86 87 /* 88 * easy access to the first 64 bit of can(fd)_frame payload. cp->data is 89 * 64 bit aligned so the offset has to be multiples of 8 which is ensured 90 * by the only callers in bcm_rx_cmp_to_index() bcm_rx_handler(). 91 */ 92 static inline u64 get_u64(const struct canfd_frame *cp, int offset) 93 { 94 return *(u64 *)(cp->data + offset); 95 } 96 97 struct bcm_op { 98 struct list_head list; 99 int ifindex; 100 canid_t can_id; 101 u32 flags; 102 unsigned long frames_abs, frames_filtered; 103 struct bcm_timeval ival1, ival2; 104 struct hrtimer timer, thrtimer; 105 struct tasklet_struct tsklet, thrtsklet; 106 ktime_t rx_stamp, kt_ival1, kt_ival2, kt_lastmsg; 107 int rx_ifindex; 108 int cfsiz; 109 u32 count; 110 u32 nframes; 111 u32 currframe; 112 /* void pointers to arrays of struct can[fd]_frame */ 113 void *frames; 114 void *last_frames; 115 struct canfd_frame sframe; 116 struct canfd_frame last_sframe; 117 struct sock *sk; 118 struct net_device *rx_reg_dev; 119 }; 120 121 struct bcm_sock { 122 struct sock sk; 123 int bound; 124 int ifindex; 125 struct notifier_block notifier; 126 struct list_head rx_ops; 127 struct list_head tx_ops; 128 unsigned long dropped_usr_msgs; 129 struct proc_dir_entry *bcm_proc_read; 130 char procname [32]; /* inode number in decimal with \0 */ 131 }; 132 133 static inline struct bcm_sock *bcm_sk(const struct sock *sk) 134 { 135 return (struct bcm_sock *)sk; 136 } 137 138 static inline ktime_t bcm_timeval_to_ktime(struct bcm_timeval tv) 139 { 140 return ktime_set(tv.tv_sec, tv.tv_usec * NSEC_PER_USEC); 141 } 142 143 #define CFSIZ(flags) ((flags & CAN_FD_FRAME) ? CANFD_MTU : CAN_MTU) 144 #define OPSIZ sizeof(struct bcm_op) 145 #define MHSIZ sizeof(struct bcm_msg_head) 146 147 /* 148 * procfs functions 149 */ 150 #if IS_ENABLED(CONFIG_PROC_FS) 151 static char *bcm_proc_getifname(struct net *net, char *result, int ifindex) 152 { 153 struct net_device *dev; 154 155 if (!ifindex) 156 return "any"; 157 158 rcu_read_lock(); 159 dev = dev_get_by_index_rcu(net, ifindex); 160 if (dev) 161 strcpy(result, dev->name); 162 else 163 strcpy(result, "???"); 164 rcu_read_unlock(); 165 166 return result; 167 } 168 169 static int bcm_proc_show(struct seq_file *m, void *v) 170 { 171 char ifname[IFNAMSIZ]; 172 struct net *net = m->private; 173 struct sock *sk = (struct sock *)PDE_DATA(m->file->f_inode); 174 struct bcm_sock *bo = bcm_sk(sk); 175 struct bcm_op *op; 176 177 seq_printf(m, ">>> socket %pK", sk->sk_socket); 178 seq_printf(m, " / sk %pK", sk); 179 seq_printf(m, " / bo %pK", bo); 180 seq_printf(m, " / dropped %lu", bo->dropped_usr_msgs); 181 seq_printf(m, " / bound %s", bcm_proc_getifname(net, ifname, bo->ifindex)); 182 seq_printf(m, " <<<\n"); 183 184 list_for_each_entry(op, &bo->rx_ops, list) { 185 186 unsigned long reduction; 187 188 /* print only active entries & prevent division by zero */ 189 if (!op->frames_abs) 190 continue; 191 192 seq_printf(m, "rx_op: %03X %-5s ", op->can_id, 193 bcm_proc_getifname(net, ifname, op->ifindex)); 194 195 if (op->flags & CAN_FD_FRAME) 196 seq_printf(m, "(%u)", op->nframes); 197 else 198 seq_printf(m, "[%u]", op->nframes); 199 200 seq_printf(m, "%c ", (op->flags & RX_CHECK_DLC) ? 'd' : ' '); 201 202 if (op->kt_ival1) 203 seq_printf(m, "timeo=%lld ", 204 (long long)ktime_to_us(op->kt_ival1)); 205 206 if (op->kt_ival2) 207 seq_printf(m, "thr=%lld ", 208 (long long)ktime_to_us(op->kt_ival2)); 209 210 seq_printf(m, "# recv %ld (%ld) => reduction: ", 211 op->frames_filtered, op->frames_abs); 212 213 reduction = 100 - (op->frames_filtered * 100) / op->frames_abs; 214 215 seq_printf(m, "%s%ld%%\n", 216 (reduction == 100) ? "near " : "", reduction); 217 } 218 219 list_for_each_entry(op, &bo->tx_ops, list) { 220 221 seq_printf(m, "tx_op: %03X %s ", op->can_id, 222 bcm_proc_getifname(net, ifname, op->ifindex)); 223 224 if (op->flags & CAN_FD_FRAME) 225 seq_printf(m, "(%u) ", op->nframes); 226 else 227 seq_printf(m, "[%u] ", op->nframes); 228 229 if (op->kt_ival1) 230 seq_printf(m, "t1=%lld ", 231 (long long)ktime_to_us(op->kt_ival1)); 232 233 if (op->kt_ival2) 234 seq_printf(m, "t2=%lld ", 235 (long long)ktime_to_us(op->kt_ival2)); 236 237 seq_printf(m, "# sent %ld\n", op->frames_abs); 238 } 239 seq_putc(m, '\n'); 240 return 0; 241 } 242 243 static int bcm_proc_open(struct inode *inode, struct file *file) 244 { 245 return single_open_net(inode, file, bcm_proc_show); 246 } 247 248 static const struct file_operations bcm_proc_fops = { 249 .owner = THIS_MODULE, 250 .open = bcm_proc_open, 251 .read = seq_read, 252 .llseek = seq_lseek, 253 .release = single_release, 254 }; 255 #endif /* CONFIG_PROC_FS */ 256 257 /* 258 * bcm_can_tx - send the (next) CAN frame to the appropriate CAN interface 259 * of the given bcm tx op 260 */ 261 static void bcm_can_tx(struct bcm_op *op) 262 { 263 struct sk_buff *skb; 264 struct net_device *dev; 265 struct canfd_frame *cf = op->frames + op->cfsiz * op->currframe; 266 267 /* no target device? => exit */ 268 if (!op->ifindex) 269 return; 270 271 dev = dev_get_by_index(sock_net(op->sk), op->ifindex); 272 if (!dev) { 273 /* RFC: should this bcm_op remove itself here? */ 274 return; 275 } 276 277 skb = alloc_skb(op->cfsiz + sizeof(struct can_skb_priv), gfp_any()); 278 if (!skb) 279 goto out; 280 281 can_skb_reserve(skb); 282 can_skb_prv(skb)->ifindex = dev->ifindex; 283 can_skb_prv(skb)->skbcnt = 0; 284 285 skb_put_data(skb, cf, op->cfsiz); 286 287 /* send with loopback */ 288 skb->dev = dev; 289 can_skb_set_owner(skb, op->sk); 290 can_send(skb, 1); 291 292 /* update statistics */ 293 op->currframe++; 294 op->frames_abs++; 295 296 /* reached last frame? */ 297 if (op->currframe >= op->nframes) 298 op->currframe = 0; 299 out: 300 dev_put(dev); 301 } 302 303 /* 304 * bcm_send_to_user - send a BCM message to the userspace 305 * (consisting of bcm_msg_head + x CAN frames) 306 */ 307 static void bcm_send_to_user(struct bcm_op *op, struct bcm_msg_head *head, 308 struct canfd_frame *frames, int has_timestamp) 309 { 310 struct sk_buff *skb; 311 struct canfd_frame *firstframe; 312 struct sockaddr_can *addr; 313 struct sock *sk = op->sk; 314 unsigned int datalen = head->nframes * op->cfsiz; 315 int err; 316 317 skb = alloc_skb(sizeof(*head) + datalen, gfp_any()); 318 if (!skb) 319 return; 320 321 skb_put_data(skb, head, sizeof(*head)); 322 323 if (head->nframes) { 324 /* CAN frames starting here */ 325 firstframe = (struct canfd_frame *)skb_tail_pointer(skb); 326 327 skb_put_data(skb, frames, datalen); 328 329 /* 330 * the BCM uses the flags-element of the canfd_frame 331 * structure for internal purposes. This is only 332 * relevant for updates that are generated by the 333 * BCM, where nframes is 1 334 */ 335 if (head->nframes == 1) 336 firstframe->flags &= BCM_CAN_FLAGS_MASK; 337 } 338 339 if (has_timestamp) { 340 /* restore rx timestamp */ 341 skb->tstamp = op->rx_stamp; 342 } 343 344 /* 345 * Put the datagram to the queue so that bcm_recvmsg() can 346 * get it from there. We need to pass the interface index to 347 * bcm_recvmsg(). We pass a whole struct sockaddr_can in skb->cb 348 * containing the interface index. 349 */ 350 351 sock_skb_cb_check_size(sizeof(struct sockaddr_can)); 352 addr = (struct sockaddr_can *)skb->cb; 353 memset(addr, 0, sizeof(*addr)); 354 addr->can_family = AF_CAN; 355 addr->can_ifindex = op->rx_ifindex; 356 357 err = sock_queue_rcv_skb(sk, skb); 358 if (err < 0) { 359 struct bcm_sock *bo = bcm_sk(sk); 360 361 kfree_skb(skb); 362 /* don't care about overflows in this statistic */ 363 bo->dropped_usr_msgs++; 364 } 365 } 366 367 static void bcm_tx_start_timer(struct bcm_op *op) 368 { 369 if (op->kt_ival1 && op->count) 370 hrtimer_start(&op->timer, 371 ktime_add(ktime_get(), op->kt_ival1), 372 HRTIMER_MODE_ABS); 373 else if (op->kt_ival2) 374 hrtimer_start(&op->timer, 375 ktime_add(ktime_get(), op->kt_ival2), 376 HRTIMER_MODE_ABS); 377 } 378 379 static void bcm_tx_timeout_tsklet(unsigned long data) 380 { 381 struct bcm_op *op = (struct bcm_op *)data; 382 struct bcm_msg_head msg_head; 383 384 if (op->kt_ival1 && (op->count > 0)) { 385 386 op->count--; 387 if (!op->count && (op->flags & TX_COUNTEVT)) { 388 389 /* create notification to user */ 390 msg_head.opcode = TX_EXPIRED; 391 msg_head.flags = op->flags; 392 msg_head.count = op->count; 393 msg_head.ival1 = op->ival1; 394 msg_head.ival2 = op->ival2; 395 msg_head.can_id = op->can_id; 396 msg_head.nframes = 0; 397 398 bcm_send_to_user(op, &msg_head, NULL, 0); 399 } 400 bcm_can_tx(op); 401 402 } else if (op->kt_ival2) 403 bcm_can_tx(op); 404 405 bcm_tx_start_timer(op); 406 } 407 408 /* 409 * bcm_tx_timeout_handler - performs cyclic CAN frame transmissions 410 */ 411 static enum hrtimer_restart bcm_tx_timeout_handler(struct hrtimer *hrtimer) 412 { 413 struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer); 414 415 tasklet_schedule(&op->tsklet); 416 417 return HRTIMER_NORESTART; 418 } 419 420 /* 421 * bcm_rx_changed - create a RX_CHANGED notification due to changed content 422 */ 423 static void bcm_rx_changed(struct bcm_op *op, struct canfd_frame *data) 424 { 425 struct bcm_msg_head head; 426 427 /* update statistics */ 428 op->frames_filtered++; 429 430 /* prevent statistics overflow */ 431 if (op->frames_filtered > ULONG_MAX/100) 432 op->frames_filtered = op->frames_abs = 0; 433 434 /* this element is not throttled anymore */ 435 data->flags &= (BCM_CAN_FLAGS_MASK|RX_RECV); 436 437 head.opcode = RX_CHANGED; 438 head.flags = op->flags; 439 head.count = op->count; 440 head.ival1 = op->ival1; 441 head.ival2 = op->ival2; 442 head.can_id = op->can_id; 443 head.nframes = 1; 444 445 bcm_send_to_user(op, &head, data, 1); 446 } 447 448 /* 449 * bcm_rx_update_and_send - process a detected relevant receive content change 450 * 1. update the last received data 451 * 2. send a notification to the user (if possible) 452 */ 453 static void bcm_rx_update_and_send(struct bcm_op *op, 454 struct canfd_frame *lastdata, 455 const struct canfd_frame *rxdata) 456 { 457 memcpy(lastdata, rxdata, op->cfsiz); 458 459 /* mark as used and throttled by default */ 460 lastdata->flags |= (RX_RECV|RX_THR); 461 462 /* throttling mode inactive ? */ 463 if (!op->kt_ival2) { 464 /* send RX_CHANGED to the user immediately */ 465 bcm_rx_changed(op, lastdata); 466 return; 467 } 468 469 /* with active throttling timer we are just done here */ 470 if (hrtimer_active(&op->thrtimer)) 471 return; 472 473 /* first reception with enabled throttling mode */ 474 if (!op->kt_lastmsg) 475 goto rx_changed_settime; 476 477 /* got a second frame inside a potential throttle period? */ 478 if (ktime_us_delta(ktime_get(), op->kt_lastmsg) < 479 ktime_to_us(op->kt_ival2)) { 480 /* do not send the saved data - only start throttle timer */ 481 hrtimer_start(&op->thrtimer, 482 ktime_add(op->kt_lastmsg, op->kt_ival2), 483 HRTIMER_MODE_ABS); 484 return; 485 } 486 487 /* the gap was that big, that throttling was not needed here */ 488 rx_changed_settime: 489 bcm_rx_changed(op, lastdata); 490 op->kt_lastmsg = ktime_get(); 491 } 492 493 /* 494 * bcm_rx_cmp_to_index - (bit)compares the currently received data to formerly 495 * received data stored in op->last_frames[] 496 */ 497 static void bcm_rx_cmp_to_index(struct bcm_op *op, unsigned int index, 498 const struct canfd_frame *rxdata) 499 { 500 struct canfd_frame *cf = op->frames + op->cfsiz * index; 501 struct canfd_frame *lcf = op->last_frames + op->cfsiz * index; 502 int i; 503 504 /* 505 * no one uses the MSBs of flags for comparison, 506 * so we use it here to detect the first time of reception 507 */ 508 509 if (!(lcf->flags & RX_RECV)) { 510 /* received data for the first time => send update to user */ 511 bcm_rx_update_and_send(op, lcf, rxdata); 512 return; 513 } 514 515 /* do a real check in CAN frame data section */ 516 for (i = 0; i < rxdata->len; i += 8) { 517 if ((get_u64(cf, i) & get_u64(rxdata, i)) != 518 (get_u64(cf, i) & get_u64(lcf, i))) { 519 bcm_rx_update_and_send(op, lcf, rxdata); 520 return; 521 } 522 } 523 524 if (op->flags & RX_CHECK_DLC) { 525 /* do a real check in CAN frame length */ 526 if (rxdata->len != lcf->len) { 527 bcm_rx_update_and_send(op, lcf, rxdata); 528 return; 529 } 530 } 531 } 532 533 /* 534 * bcm_rx_starttimer - enable timeout monitoring for CAN frame reception 535 */ 536 static void bcm_rx_starttimer(struct bcm_op *op) 537 { 538 if (op->flags & RX_NO_AUTOTIMER) 539 return; 540 541 if (op->kt_ival1) 542 hrtimer_start(&op->timer, op->kt_ival1, HRTIMER_MODE_REL); 543 } 544 545 static void bcm_rx_timeout_tsklet(unsigned long data) 546 { 547 struct bcm_op *op = (struct bcm_op *)data; 548 struct bcm_msg_head msg_head; 549 550 /* create notification to user */ 551 msg_head.opcode = RX_TIMEOUT; 552 msg_head.flags = op->flags; 553 msg_head.count = op->count; 554 msg_head.ival1 = op->ival1; 555 msg_head.ival2 = op->ival2; 556 msg_head.can_id = op->can_id; 557 msg_head.nframes = 0; 558 559 bcm_send_to_user(op, &msg_head, NULL, 0); 560 } 561 562 /* 563 * bcm_rx_timeout_handler - when the (cyclic) CAN frame reception timed out 564 */ 565 static enum hrtimer_restart bcm_rx_timeout_handler(struct hrtimer *hrtimer) 566 { 567 struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer); 568 569 /* schedule before NET_RX_SOFTIRQ */ 570 tasklet_hi_schedule(&op->tsklet); 571 572 /* no restart of the timer is done here! */ 573 574 /* if user wants to be informed, when cyclic CAN-Messages come back */ 575 if ((op->flags & RX_ANNOUNCE_RESUME) && op->last_frames) { 576 /* clear received CAN frames to indicate 'nothing received' */ 577 memset(op->last_frames, 0, op->nframes * op->cfsiz); 578 } 579 580 return HRTIMER_NORESTART; 581 } 582 583 /* 584 * bcm_rx_do_flush - helper for bcm_rx_thr_flush 585 */ 586 static inline int bcm_rx_do_flush(struct bcm_op *op, int update, 587 unsigned int index) 588 { 589 struct canfd_frame *lcf = op->last_frames + op->cfsiz * index; 590 591 if ((op->last_frames) && (lcf->flags & RX_THR)) { 592 if (update) 593 bcm_rx_changed(op, lcf); 594 return 1; 595 } 596 return 0; 597 } 598 599 /* 600 * bcm_rx_thr_flush - Check for throttled data and send it to the userspace 601 * 602 * update == 0 : just check if throttled data is available (any irq context) 603 * update == 1 : check and send throttled data to userspace (soft_irq context) 604 */ 605 static int bcm_rx_thr_flush(struct bcm_op *op, int update) 606 { 607 int updated = 0; 608 609 if (op->nframes > 1) { 610 unsigned int i; 611 612 /* for MUX filter we start at index 1 */ 613 for (i = 1; i < op->nframes; i++) 614 updated += bcm_rx_do_flush(op, update, i); 615 616 } else { 617 /* for RX_FILTER_ID and simple filter */ 618 updated += bcm_rx_do_flush(op, update, 0); 619 } 620 621 return updated; 622 } 623 624 static void bcm_rx_thr_tsklet(unsigned long data) 625 { 626 struct bcm_op *op = (struct bcm_op *)data; 627 628 /* push the changed data to the userspace */ 629 bcm_rx_thr_flush(op, 1); 630 } 631 632 /* 633 * bcm_rx_thr_handler - the time for blocked content updates is over now: 634 * Check for throttled data and send it to the userspace 635 */ 636 static enum hrtimer_restart bcm_rx_thr_handler(struct hrtimer *hrtimer) 637 { 638 struct bcm_op *op = container_of(hrtimer, struct bcm_op, thrtimer); 639 640 tasklet_schedule(&op->thrtsklet); 641 642 if (bcm_rx_thr_flush(op, 0)) { 643 hrtimer_forward(hrtimer, ktime_get(), op->kt_ival2); 644 return HRTIMER_RESTART; 645 } else { 646 /* rearm throttle handling */ 647 op->kt_lastmsg = 0; 648 return HRTIMER_NORESTART; 649 } 650 } 651 652 /* 653 * bcm_rx_handler - handle a CAN frame reception 654 */ 655 static void bcm_rx_handler(struct sk_buff *skb, void *data) 656 { 657 struct bcm_op *op = (struct bcm_op *)data; 658 const struct canfd_frame *rxframe = (struct canfd_frame *)skb->data; 659 unsigned int i; 660 661 if (op->can_id != rxframe->can_id) 662 return; 663 664 /* make sure to handle the correct frame type (CAN / CAN FD) */ 665 if (skb->len != op->cfsiz) 666 return; 667 668 /* disable timeout */ 669 hrtimer_cancel(&op->timer); 670 671 /* save rx timestamp */ 672 op->rx_stamp = skb->tstamp; 673 /* save originator for recvfrom() */ 674 op->rx_ifindex = skb->dev->ifindex; 675 /* update statistics */ 676 op->frames_abs++; 677 678 if (op->flags & RX_RTR_FRAME) { 679 /* send reply for RTR-request (placed in op->frames[0]) */ 680 bcm_can_tx(op); 681 return; 682 } 683 684 if (op->flags & RX_FILTER_ID) { 685 /* the easiest case */ 686 bcm_rx_update_and_send(op, op->last_frames, rxframe); 687 goto rx_starttimer; 688 } 689 690 if (op->nframes == 1) { 691 /* simple compare with index 0 */ 692 bcm_rx_cmp_to_index(op, 0, rxframe); 693 goto rx_starttimer; 694 } 695 696 if (op->nframes > 1) { 697 /* 698 * multiplex compare 699 * 700 * find the first multiplex mask that fits. 701 * Remark: The MUX-mask is stored in index 0 - but only the 702 * first 64 bits of the frame data[] are relevant (CAN FD) 703 */ 704 705 for (i = 1; i < op->nframes; i++) { 706 if ((get_u64(op->frames, 0) & get_u64(rxframe, 0)) == 707 (get_u64(op->frames, 0) & 708 get_u64(op->frames + op->cfsiz * i, 0))) { 709 bcm_rx_cmp_to_index(op, i, rxframe); 710 break; 711 } 712 } 713 } 714 715 rx_starttimer: 716 bcm_rx_starttimer(op); 717 } 718 719 /* 720 * helpers for bcm_op handling: find & delete bcm [rx|tx] op elements 721 */ 722 static struct bcm_op *bcm_find_op(struct list_head *ops, 723 struct bcm_msg_head *mh, int ifindex) 724 { 725 struct bcm_op *op; 726 727 list_for_each_entry(op, ops, list) { 728 if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) && 729 (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) 730 return op; 731 } 732 733 return NULL; 734 } 735 736 static void bcm_remove_op(struct bcm_op *op) 737 { 738 if (op->tsklet.func) { 739 while (test_bit(TASKLET_STATE_SCHED, &op->tsklet.state) || 740 test_bit(TASKLET_STATE_RUN, &op->tsklet.state) || 741 hrtimer_active(&op->timer)) { 742 hrtimer_cancel(&op->timer); 743 tasklet_kill(&op->tsklet); 744 } 745 } 746 747 if (op->thrtsklet.func) { 748 while (test_bit(TASKLET_STATE_SCHED, &op->thrtsklet.state) || 749 test_bit(TASKLET_STATE_RUN, &op->thrtsklet.state) || 750 hrtimer_active(&op->thrtimer)) { 751 hrtimer_cancel(&op->thrtimer); 752 tasklet_kill(&op->thrtsklet); 753 } 754 } 755 756 if ((op->frames) && (op->frames != &op->sframe)) 757 kfree(op->frames); 758 759 if ((op->last_frames) && (op->last_frames != &op->last_sframe)) 760 kfree(op->last_frames); 761 762 kfree(op); 763 } 764 765 static void bcm_rx_unreg(struct net_device *dev, struct bcm_op *op) 766 { 767 if (op->rx_reg_dev == dev) { 768 can_rx_unregister(dev_net(dev), dev, op->can_id, 769 REGMASK(op->can_id), bcm_rx_handler, op); 770 771 /* mark as removed subscription */ 772 op->rx_reg_dev = NULL; 773 } else 774 printk(KERN_ERR "can-bcm: bcm_rx_unreg: registered device " 775 "mismatch %p %p\n", op->rx_reg_dev, dev); 776 } 777 778 /* 779 * bcm_delete_rx_op - find and remove a rx op (returns number of removed ops) 780 */ 781 static int bcm_delete_rx_op(struct list_head *ops, struct bcm_msg_head *mh, 782 int ifindex) 783 { 784 struct bcm_op *op, *n; 785 786 list_for_each_entry_safe(op, n, ops, list) { 787 if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) && 788 (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) { 789 790 /* 791 * Don't care if we're bound or not (due to netdev 792 * problems) can_rx_unregister() is always a save 793 * thing to do here. 794 */ 795 if (op->ifindex) { 796 /* 797 * Only remove subscriptions that had not 798 * been removed due to NETDEV_UNREGISTER 799 * in bcm_notifier() 800 */ 801 if (op->rx_reg_dev) { 802 struct net_device *dev; 803 804 dev = dev_get_by_index(sock_net(op->sk), 805 op->ifindex); 806 if (dev) { 807 bcm_rx_unreg(dev, op); 808 dev_put(dev); 809 } 810 } 811 } else 812 can_rx_unregister(sock_net(op->sk), NULL, 813 op->can_id, 814 REGMASK(op->can_id), 815 bcm_rx_handler, op); 816 817 list_del(&op->list); 818 bcm_remove_op(op); 819 return 1; /* done */ 820 } 821 } 822 823 return 0; /* not found */ 824 } 825 826 /* 827 * bcm_delete_tx_op - find and remove a tx op (returns number of removed ops) 828 */ 829 static int bcm_delete_tx_op(struct list_head *ops, struct bcm_msg_head *mh, 830 int ifindex) 831 { 832 struct bcm_op *op, *n; 833 834 list_for_each_entry_safe(op, n, ops, list) { 835 if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) && 836 (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) { 837 list_del(&op->list); 838 bcm_remove_op(op); 839 return 1; /* done */ 840 } 841 } 842 843 return 0; /* not found */ 844 } 845 846 /* 847 * bcm_read_op - read out a bcm_op and send it to the user (for bcm_sendmsg) 848 */ 849 static int bcm_read_op(struct list_head *ops, struct bcm_msg_head *msg_head, 850 int ifindex) 851 { 852 struct bcm_op *op = bcm_find_op(ops, msg_head, ifindex); 853 854 if (!op) 855 return -EINVAL; 856 857 /* put current values into msg_head */ 858 msg_head->flags = op->flags; 859 msg_head->count = op->count; 860 msg_head->ival1 = op->ival1; 861 msg_head->ival2 = op->ival2; 862 msg_head->nframes = op->nframes; 863 864 bcm_send_to_user(op, msg_head, op->frames, 0); 865 866 return MHSIZ; 867 } 868 869 /* 870 * bcm_tx_setup - create or update a bcm tx op (for bcm_sendmsg) 871 */ 872 static int bcm_tx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg, 873 int ifindex, struct sock *sk) 874 { 875 struct bcm_sock *bo = bcm_sk(sk); 876 struct bcm_op *op; 877 struct canfd_frame *cf; 878 unsigned int i; 879 int err; 880 881 /* we need a real device to send frames */ 882 if (!ifindex) 883 return -ENODEV; 884 885 /* check nframes boundaries - we need at least one CAN frame */ 886 if (msg_head->nframes < 1 || msg_head->nframes > MAX_NFRAMES) 887 return -EINVAL; 888 889 /* check the given can_id */ 890 op = bcm_find_op(&bo->tx_ops, msg_head, ifindex); 891 if (op) { 892 /* update existing BCM operation */ 893 894 /* 895 * Do we need more space for the CAN frames than currently 896 * allocated? -> This is a _really_ unusual use-case and 897 * therefore (complexity / locking) it is not supported. 898 */ 899 if (msg_head->nframes > op->nframes) 900 return -E2BIG; 901 902 /* update CAN frames content */ 903 for (i = 0; i < msg_head->nframes; i++) { 904 905 cf = op->frames + op->cfsiz * i; 906 err = memcpy_from_msg((u8 *)cf, msg, op->cfsiz); 907 908 if (op->flags & CAN_FD_FRAME) { 909 if (cf->len > 64) 910 err = -EINVAL; 911 } else { 912 if (cf->len > 8) 913 err = -EINVAL; 914 } 915 916 if (err < 0) 917 return err; 918 919 if (msg_head->flags & TX_CP_CAN_ID) { 920 /* copy can_id into frame */ 921 cf->can_id = msg_head->can_id; 922 } 923 } 924 op->flags = msg_head->flags; 925 926 } else { 927 /* insert new BCM operation for the given can_id */ 928 929 op = kzalloc(OPSIZ, GFP_KERNEL); 930 if (!op) 931 return -ENOMEM; 932 933 op->can_id = msg_head->can_id; 934 op->cfsiz = CFSIZ(msg_head->flags); 935 op->flags = msg_head->flags; 936 937 /* create array for CAN frames and copy the data */ 938 if (msg_head->nframes > 1) { 939 op->frames = kmalloc(msg_head->nframes * op->cfsiz, 940 GFP_KERNEL); 941 if (!op->frames) { 942 kfree(op); 943 return -ENOMEM; 944 } 945 } else 946 op->frames = &op->sframe; 947 948 for (i = 0; i < msg_head->nframes; i++) { 949 950 cf = op->frames + op->cfsiz * i; 951 err = memcpy_from_msg((u8 *)cf, msg, op->cfsiz); 952 953 if (op->flags & CAN_FD_FRAME) { 954 if (cf->len > 64) 955 err = -EINVAL; 956 } else { 957 if (cf->len > 8) 958 err = -EINVAL; 959 } 960 961 if (err < 0) { 962 if (op->frames != &op->sframe) 963 kfree(op->frames); 964 kfree(op); 965 return err; 966 } 967 968 if (msg_head->flags & TX_CP_CAN_ID) { 969 /* copy can_id into frame */ 970 cf->can_id = msg_head->can_id; 971 } 972 } 973 974 /* tx_ops never compare with previous received messages */ 975 op->last_frames = NULL; 976 977 /* bcm_can_tx / bcm_tx_timeout_handler needs this */ 978 op->sk = sk; 979 op->ifindex = ifindex; 980 981 /* initialize uninitialized (kzalloc) structure */ 982 hrtimer_init(&op->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); 983 op->timer.function = bcm_tx_timeout_handler; 984 985 /* initialize tasklet for tx countevent notification */ 986 tasklet_init(&op->tsklet, bcm_tx_timeout_tsklet, 987 (unsigned long) op); 988 989 /* currently unused in tx_ops */ 990 hrtimer_init(&op->thrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); 991 992 /* add this bcm_op to the list of the tx_ops */ 993 list_add(&op->list, &bo->tx_ops); 994 995 } /* if ((op = bcm_find_op(&bo->tx_ops, msg_head->can_id, ifindex))) */ 996 997 if (op->nframes != msg_head->nframes) { 998 op->nframes = msg_head->nframes; 999 /* start multiple frame transmission with index 0 */ 1000 op->currframe = 0; 1001 } 1002 1003 /* check flags */ 1004 1005 if (op->flags & TX_RESET_MULTI_IDX) { 1006 /* start multiple frame transmission with index 0 */ 1007 op->currframe = 0; 1008 } 1009 1010 if (op->flags & SETTIMER) { 1011 /* set timer values */ 1012 op->count = msg_head->count; 1013 op->ival1 = msg_head->ival1; 1014 op->ival2 = msg_head->ival2; 1015 op->kt_ival1 = bcm_timeval_to_ktime(msg_head->ival1); 1016 op->kt_ival2 = bcm_timeval_to_ktime(msg_head->ival2); 1017 1018 /* disable an active timer due to zero values? */ 1019 if (!op->kt_ival1 && !op->kt_ival2) 1020 hrtimer_cancel(&op->timer); 1021 } 1022 1023 if (op->flags & STARTTIMER) { 1024 hrtimer_cancel(&op->timer); 1025 /* spec: send CAN frame when starting timer */ 1026 op->flags |= TX_ANNOUNCE; 1027 } 1028 1029 if (op->flags & TX_ANNOUNCE) { 1030 bcm_can_tx(op); 1031 if (op->count) 1032 op->count--; 1033 } 1034 1035 if (op->flags & STARTTIMER) 1036 bcm_tx_start_timer(op); 1037 1038 return msg_head->nframes * op->cfsiz + MHSIZ; 1039 } 1040 1041 /* 1042 * bcm_rx_setup - create or update a bcm rx op (for bcm_sendmsg) 1043 */ 1044 static int bcm_rx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg, 1045 int ifindex, struct sock *sk) 1046 { 1047 struct bcm_sock *bo = bcm_sk(sk); 1048 struct bcm_op *op; 1049 int do_rx_register; 1050 int err = 0; 1051 1052 if ((msg_head->flags & RX_FILTER_ID) || (!(msg_head->nframes))) { 1053 /* be robust against wrong usage ... */ 1054 msg_head->flags |= RX_FILTER_ID; 1055 /* ignore trailing garbage */ 1056 msg_head->nframes = 0; 1057 } 1058 1059 /* the first element contains the mux-mask => MAX_NFRAMES + 1 */ 1060 if (msg_head->nframes > MAX_NFRAMES + 1) 1061 return -EINVAL; 1062 1063 if ((msg_head->flags & RX_RTR_FRAME) && 1064 ((msg_head->nframes != 1) || 1065 (!(msg_head->can_id & CAN_RTR_FLAG)))) 1066 return -EINVAL; 1067 1068 /* check the given can_id */ 1069 op = bcm_find_op(&bo->rx_ops, msg_head, ifindex); 1070 if (op) { 1071 /* update existing BCM operation */ 1072 1073 /* 1074 * Do we need more space for the CAN frames than currently 1075 * allocated? -> This is a _really_ unusual use-case and 1076 * therefore (complexity / locking) it is not supported. 1077 */ 1078 if (msg_head->nframes > op->nframes) 1079 return -E2BIG; 1080 1081 if (msg_head->nframes) { 1082 /* update CAN frames content */ 1083 err = memcpy_from_msg(op->frames, msg, 1084 msg_head->nframes * op->cfsiz); 1085 if (err < 0) 1086 return err; 1087 1088 /* clear last_frames to indicate 'nothing received' */ 1089 memset(op->last_frames, 0, msg_head->nframes * op->cfsiz); 1090 } 1091 1092 op->nframes = msg_head->nframes; 1093 op->flags = msg_head->flags; 1094 1095 /* Only an update -> do not call can_rx_register() */ 1096 do_rx_register = 0; 1097 1098 } else { 1099 /* insert new BCM operation for the given can_id */ 1100 op = kzalloc(OPSIZ, GFP_KERNEL); 1101 if (!op) 1102 return -ENOMEM; 1103 1104 op->can_id = msg_head->can_id; 1105 op->nframes = msg_head->nframes; 1106 op->cfsiz = CFSIZ(msg_head->flags); 1107 op->flags = msg_head->flags; 1108 1109 if (msg_head->nframes > 1) { 1110 /* create array for CAN frames and copy the data */ 1111 op->frames = kmalloc(msg_head->nframes * op->cfsiz, 1112 GFP_KERNEL); 1113 if (!op->frames) { 1114 kfree(op); 1115 return -ENOMEM; 1116 } 1117 1118 /* create and init array for received CAN frames */ 1119 op->last_frames = kzalloc(msg_head->nframes * op->cfsiz, 1120 GFP_KERNEL); 1121 if (!op->last_frames) { 1122 kfree(op->frames); 1123 kfree(op); 1124 return -ENOMEM; 1125 } 1126 1127 } else { 1128 op->frames = &op->sframe; 1129 op->last_frames = &op->last_sframe; 1130 } 1131 1132 if (msg_head->nframes) { 1133 err = memcpy_from_msg(op->frames, msg, 1134 msg_head->nframes * op->cfsiz); 1135 if (err < 0) { 1136 if (op->frames != &op->sframe) 1137 kfree(op->frames); 1138 if (op->last_frames != &op->last_sframe) 1139 kfree(op->last_frames); 1140 kfree(op); 1141 return err; 1142 } 1143 } 1144 1145 /* bcm_can_tx / bcm_tx_timeout_handler needs this */ 1146 op->sk = sk; 1147 op->ifindex = ifindex; 1148 1149 /* ifindex for timeout events w/o previous frame reception */ 1150 op->rx_ifindex = ifindex; 1151 1152 /* initialize uninitialized (kzalloc) structure */ 1153 hrtimer_init(&op->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); 1154 op->timer.function = bcm_rx_timeout_handler; 1155 1156 /* initialize tasklet for rx timeout notification */ 1157 tasklet_init(&op->tsklet, bcm_rx_timeout_tsklet, 1158 (unsigned long) op); 1159 1160 hrtimer_init(&op->thrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); 1161 op->thrtimer.function = bcm_rx_thr_handler; 1162 1163 /* initialize tasklet for rx throttle handling */ 1164 tasklet_init(&op->thrtsklet, bcm_rx_thr_tsklet, 1165 (unsigned long) op); 1166 1167 /* add this bcm_op to the list of the rx_ops */ 1168 list_add(&op->list, &bo->rx_ops); 1169 1170 /* call can_rx_register() */ 1171 do_rx_register = 1; 1172 1173 } /* if ((op = bcm_find_op(&bo->rx_ops, msg_head->can_id, ifindex))) */ 1174 1175 /* check flags */ 1176 1177 if (op->flags & RX_RTR_FRAME) { 1178 struct canfd_frame *frame0 = op->frames; 1179 1180 /* no timers in RTR-mode */ 1181 hrtimer_cancel(&op->thrtimer); 1182 hrtimer_cancel(&op->timer); 1183 1184 /* 1185 * funny feature in RX(!)_SETUP only for RTR-mode: 1186 * copy can_id into frame BUT without RTR-flag to 1187 * prevent a full-load-loopback-test ... ;-] 1188 */ 1189 if ((op->flags & TX_CP_CAN_ID) || 1190 (frame0->can_id == op->can_id)) 1191 frame0->can_id = op->can_id & ~CAN_RTR_FLAG; 1192 1193 } else { 1194 if (op->flags & SETTIMER) { 1195 1196 /* set timer value */ 1197 op->ival1 = msg_head->ival1; 1198 op->ival2 = msg_head->ival2; 1199 op->kt_ival1 = bcm_timeval_to_ktime(msg_head->ival1); 1200 op->kt_ival2 = bcm_timeval_to_ktime(msg_head->ival2); 1201 1202 /* disable an active timer due to zero value? */ 1203 if (!op->kt_ival1) 1204 hrtimer_cancel(&op->timer); 1205 1206 /* 1207 * In any case cancel the throttle timer, flush 1208 * potentially blocked msgs and reset throttle handling 1209 */ 1210 op->kt_lastmsg = 0; 1211 hrtimer_cancel(&op->thrtimer); 1212 bcm_rx_thr_flush(op, 1); 1213 } 1214 1215 if ((op->flags & STARTTIMER) && op->kt_ival1) 1216 hrtimer_start(&op->timer, op->kt_ival1, 1217 HRTIMER_MODE_REL); 1218 } 1219 1220 /* now we can register for can_ids, if we added a new bcm_op */ 1221 if (do_rx_register) { 1222 if (ifindex) { 1223 struct net_device *dev; 1224 1225 dev = dev_get_by_index(sock_net(sk), ifindex); 1226 if (dev) { 1227 err = can_rx_register(sock_net(sk), dev, 1228 op->can_id, 1229 REGMASK(op->can_id), 1230 bcm_rx_handler, op, 1231 "bcm", sk); 1232 1233 op->rx_reg_dev = dev; 1234 dev_put(dev); 1235 } 1236 1237 } else 1238 err = can_rx_register(sock_net(sk), NULL, op->can_id, 1239 REGMASK(op->can_id), 1240 bcm_rx_handler, op, "bcm", sk); 1241 if (err) { 1242 /* this bcm rx op is broken -> remove it */ 1243 list_del(&op->list); 1244 bcm_remove_op(op); 1245 return err; 1246 } 1247 } 1248 1249 return msg_head->nframes * op->cfsiz + MHSIZ; 1250 } 1251 1252 /* 1253 * bcm_tx_send - send a single CAN frame to the CAN interface (for bcm_sendmsg) 1254 */ 1255 static int bcm_tx_send(struct msghdr *msg, int ifindex, struct sock *sk, 1256 int cfsiz) 1257 { 1258 struct sk_buff *skb; 1259 struct net_device *dev; 1260 int err; 1261 1262 /* we need a real device to send frames */ 1263 if (!ifindex) 1264 return -ENODEV; 1265 1266 skb = alloc_skb(cfsiz + sizeof(struct can_skb_priv), GFP_KERNEL); 1267 if (!skb) 1268 return -ENOMEM; 1269 1270 can_skb_reserve(skb); 1271 1272 err = memcpy_from_msg(skb_put(skb, cfsiz), msg, cfsiz); 1273 if (err < 0) { 1274 kfree_skb(skb); 1275 return err; 1276 } 1277 1278 dev = dev_get_by_index(sock_net(sk), ifindex); 1279 if (!dev) { 1280 kfree_skb(skb); 1281 return -ENODEV; 1282 } 1283 1284 can_skb_prv(skb)->ifindex = dev->ifindex; 1285 can_skb_prv(skb)->skbcnt = 0; 1286 skb->dev = dev; 1287 can_skb_set_owner(skb, sk); 1288 err = can_send(skb, 1); /* send with loopback */ 1289 dev_put(dev); 1290 1291 if (err) 1292 return err; 1293 1294 return cfsiz + MHSIZ; 1295 } 1296 1297 /* 1298 * bcm_sendmsg - process BCM commands (opcodes) from the userspace 1299 */ 1300 static int bcm_sendmsg(struct socket *sock, struct msghdr *msg, size_t size) 1301 { 1302 struct sock *sk = sock->sk; 1303 struct bcm_sock *bo = bcm_sk(sk); 1304 int ifindex = bo->ifindex; /* default ifindex for this bcm_op */ 1305 struct bcm_msg_head msg_head; 1306 int cfsiz; 1307 int ret; /* read bytes or error codes as return value */ 1308 1309 if (!bo->bound) 1310 return -ENOTCONN; 1311 1312 /* check for valid message length from userspace */ 1313 if (size < MHSIZ) 1314 return -EINVAL; 1315 1316 /* read message head information */ 1317 ret = memcpy_from_msg((u8 *)&msg_head, msg, MHSIZ); 1318 if (ret < 0) 1319 return ret; 1320 1321 cfsiz = CFSIZ(msg_head.flags); 1322 if ((size - MHSIZ) % cfsiz) 1323 return -EINVAL; 1324 1325 /* check for alternative ifindex for this bcm_op */ 1326 1327 if (!ifindex && msg->msg_name) { 1328 /* no bound device as default => check msg_name */ 1329 DECLARE_SOCKADDR(struct sockaddr_can *, addr, msg->msg_name); 1330 1331 if (msg->msg_namelen < sizeof(*addr)) 1332 return -EINVAL; 1333 1334 if (addr->can_family != AF_CAN) 1335 return -EINVAL; 1336 1337 /* ifindex from sendto() */ 1338 ifindex = addr->can_ifindex; 1339 1340 if (ifindex) { 1341 struct net_device *dev; 1342 1343 dev = dev_get_by_index(sock_net(sk), ifindex); 1344 if (!dev) 1345 return -ENODEV; 1346 1347 if (dev->type != ARPHRD_CAN) { 1348 dev_put(dev); 1349 return -ENODEV; 1350 } 1351 1352 dev_put(dev); 1353 } 1354 } 1355 1356 lock_sock(sk); 1357 1358 switch (msg_head.opcode) { 1359 1360 case TX_SETUP: 1361 ret = bcm_tx_setup(&msg_head, msg, ifindex, sk); 1362 break; 1363 1364 case RX_SETUP: 1365 ret = bcm_rx_setup(&msg_head, msg, ifindex, sk); 1366 break; 1367 1368 case TX_DELETE: 1369 if (bcm_delete_tx_op(&bo->tx_ops, &msg_head, ifindex)) 1370 ret = MHSIZ; 1371 else 1372 ret = -EINVAL; 1373 break; 1374 1375 case RX_DELETE: 1376 if (bcm_delete_rx_op(&bo->rx_ops, &msg_head, ifindex)) 1377 ret = MHSIZ; 1378 else 1379 ret = -EINVAL; 1380 break; 1381 1382 case TX_READ: 1383 /* reuse msg_head for the reply to TX_READ */ 1384 msg_head.opcode = TX_STATUS; 1385 ret = bcm_read_op(&bo->tx_ops, &msg_head, ifindex); 1386 break; 1387 1388 case RX_READ: 1389 /* reuse msg_head for the reply to RX_READ */ 1390 msg_head.opcode = RX_STATUS; 1391 ret = bcm_read_op(&bo->rx_ops, &msg_head, ifindex); 1392 break; 1393 1394 case TX_SEND: 1395 /* we need exactly one CAN frame behind the msg head */ 1396 if ((msg_head.nframes != 1) || (size != cfsiz + MHSIZ)) 1397 ret = -EINVAL; 1398 else 1399 ret = bcm_tx_send(msg, ifindex, sk, cfsiz); 1400 break; 1401 1402 default: 1403 ret = -EINVAL; 1404 break; 1405 } 1406 1407 release_sock(sk); 1408 1409 return ret; 1410 } 1411 1412 /* 1413 * notification handler for netdevice status changes 1414 */ 1415 static int bcm_notifier(struct notifier_block *nb, unsigned long msg, 1416 void *ptr) 1417 { 1418 struct net_device *dev = netdev_notifier_info_to_dev(ptr); 1419 struct bcm_sock *bo = container_of(nb, struct bcm_sock, notifier); 1420 struct sock *sk = &bo->sk; 1421 struct bcm_op *op; 1422 int notify_enodev = 0; 1423 1424 if (!net_eq(dev_net(dev), sock_net(sk))) 1425 return NOTIFY_DONE; 1426 1427 if (dev->type != ARPHRD_CAN) 1428 return NOTIFY_DONE; 1429 1430 switch (msg) { 1431 1432 case NETDEV_UNREGISTER: 1433 lock_sock(sk); 1434 1435 /* remove device specific receive entries */ 1436 list_for_each_entry(op, &bo->rx_ops, list) 1437 if (op->rx_reg_dev == dev) 1438 bcm_rx_unreg(dev, op); 1439 1440 /* remove device reference, if this is our bound device */ 1441 if (bo->bound && bo->ifindex == dev->ifindex) { 1442 bo->bound = 0; 1443 bo->ifindex = 0; 1444 notify_enodev = 1; 1445 } 1446 1447 release_sock(sk); 1448 1449 if (notify_enodev) { 1450 sk->sk_err = ENODEV; 1451 if (!sock_flag(sk, SOCK_DEAD)) 1452 sk->sk_error_report(sk); 1453 } 1454 break; 1455 1456 case NETDEV_DOWN: 1457 if (bo->bound && bo->ifindex == dev->ifindex) { 1458 sk->sk_err = ENETDOWN; 1459 if (!sock_flag(sk, SOCK_DEAD)) 1460 sk->sk_error_report(sk); 1461 } 1462 } 1463 1464 return NOTIFY_DONE; 1465 } 1466 1467 /* 1468 * initial settings for all BCM sockets to be set at socket creation time 1469 */ 1470 static int bcm_init(struct sock *sk) 1471 { 1472 struct bcm_sock *bo = bcm_sk(sk); 1473 1474 bo->bound = 0; 1475 bo->ifindex = 0; 1476 bo->dropped_usr_msgs = 0; 1477 bo->bcm_proc_read = NULL; 1478 1479 INIT_LIST_HEAD(&bo->tx_ops); 1480 INIT_LIST_HEAD(&bo->rx_ops); 1481 1482 /* set notifier */ 1483 bo->notifier.notifier_call = bcm_notifier; 1484 1485 register_netdevice_notifier(&bo->notifier); 1486 1487 return 0; 1488 } 1489 1490 /* 1491 * standard socket functions 1492 */ 1493 static int bcm_release(struct socket *sock) 1494 { 1495 struct sock *sk = sock->sk; 1496 struct net *net = sock_net(sk); 1497 struct bcm_sock *bo; 1498 struct bcm_op *op, *next; 1499 1500 if (sk == NULL) 1501 return 0; 1502 1503 bo = bcm_sk(sk); 1504 1505 /* remove bcm_ops, timer, rx_unregister(), etc. */ 1506 1507 unregister_netdevice_notifier(&bo->notifier); 1508 1509 lock_sock(sk); 1510 1511 list_for_each_entry_safe(op, next, &bo->tx_ops, list) 1512 bcm_remove_op(op); 1513 1514 list_for_each_entry_safe(op, next, &bo->rx_ops, list) { 1515 /* 1516 * Don't care if we're bound or not (due to netdev problems) 1517 * can_rx_unregister() is always a save thing to do here. 1518 */ 1519 if (op->ifindex) { 1520 /* 1521 * Only remove subscriptions that had not 1522 * been removed due to NETDEV_UNREGISTER 1523 * in bcm_notifier() 1524 */ 1525 if (op->rx_reg_dev) { 1526 struct net_device *dev; 1527 1528 dev = dev_get_by_index(net, op->ifindex); 1529 if (dev) { 1530 bcm_rx_unreg(dev, op); 1531 dev_put(dev); 1532 } 1533 } 1534 } else 1535 can_rx_unregister(net, NULL, op->can_id, 1536 REGMASK(op->can_id), 1537 bcm_rx_handler, op); 1538 1539 bcm_remove_op(op); 1540 } 1541 1542 #if IS_ENABLED(CONFIG_PROC_FS) 1543 /* remove procfs entry */ 1544 if (net->can.bcmproc_dir && bo->bcm_proc_read) 1545 remove_proc_entry(bo->procname, net->can.bcmproc_dir); 1546 #endif /* CONFIG_PROC_FS */ 1547 1548 /* remove device reference */ 1549 if (bo->bound) { 1550 bo->bound = 0; 1551 bo->ifindex = 0; 1552 } 1553 1554 sock_orphan(sk); 1555 sock->sk = NULL; 1556 1557 release_sock(sk); 1558 sock_put(sk); 1559 1560 return 0; 1561 } 1562 1563 static int bcm_connect(struct socket *sock, struct sockaddr *uaddr, int len, 1564 int flags) 1565 { 1566 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; 1567 struct sock *sk = sock->sk; 1568 struct bcm_sock *bo = bcm_sk(sk); 1569 struct net *net = sock_net(sk); 1570 int ret = 0; 1571 1572 if (len < sizeof(*addr)) 1573 return -EINVAL; 1574 1575 lock_sock(sk); 1576 1577 if (bo->bound) { 1578 ret = -EISCONN; 1579 goto fail; 1580 } 1581 1582 /* bind a device to this socket */ 1583 if (addr->can_ifindex) { 1584 struct net_device *dev; 1585 1586 dev = dev_get_by_index(net, addr->can_ifindex); 1587 if (!dev) { 1588 ret = -ENODEV; 1589 goto fail; 1590 } 1591 if (dev->type != ARPHRD_CAN) { 1592 dev_put(dev); 1593 ret = -ENODEV; 1594 goto fail; 1595 } 1596 1597 bo->ifindex = dev->ifindex; 1598 dev_put(dev); 1599 1600 } else { 1601 /* no interface reference for ifindex = 0 ('any' CAN device) */ 1602 bo->ifindex = 0; 1603 } 1604 1605 #if IS_ENABLED(CONFIG_PROC_FS) 1606 if (net->can.bcmproc_dir) { 1607 /* unique socket address as filename */ 1608 sprintf(bo->procname, "%lu", sock_i_ino(sk)); 1609 bo->bcm_proc_read = proc_create_data(bo->procname, 0644, 1610 net->can.bcmproc_dir, 1611 &bcm_proc_fops, sk); 1612 if (!bo->bcm_proc_read) { 1613 ret = -ENOMEM; 1614 goto fail; 1615 } 1616 } 1617 #endif /* CONFIG_PROC_FS */ 1618 1619 bo->bound = 1; 1620 1621 fail: 1622 release_sock(sk); 1623 1624 return ret; 1625 } 1626 1627 static int bcm_recvmsg(struct socket *sock, struct msghdr *msg, size_t size, 1628 int flags) 1629 { 1630 struct sock *sk = sock->sk; 1631 struct sk_buff *skb; 1632 int error = 0; 1633 int noblock; 1634 int err; 1635 1636 noblock = flags & MSG_DONTWAIT; 1637 flags &= ~MSG_DONTWAIT; 1638 skb = skb_recv_datagram(sk, flags, noblock, &error); 1639 if (!skb) 1640 return error; 1641 1642 if (skb->len < size) 1643 size = skb->len; 1644 1645 err = memcpy_to_msg(msg, skb->data, size); 1646 if (err < 0) { 1647 skb_free_datagram(sk, skb); 1648 return err; 1649 } 1650 1651 sock_recv_ts_and_drops(msg, sk, skb); 1652 1653 if (msg->msg_name) { 1654 __sockaddr_check_size(sizeof(struct sockaddr_can)); 1655 msg->msg_namelen = sizeof(struct sockaddr_can); 1656 memcpy(msg->msg_name, skb->cb, msg->msg_namelen); 1657 } 1658 1659 skb_free_datagram(sk, skb); 1660 1661 return size; 1662 } 1663 1664 static const struct proto_ops bcm_ops = { 1665 .family = PF_CAN, 1666 .release = bcm_release, 1667 .bind = sock_no_bind, 1668 .connect = bcm_connect, 1669 .socketpair = sock_no_socketpair, 1670 .accept = sock_no_accept, 1671 .getname = sock_no_getname, 1672 .poll = datagram_poll, 1673 .ioctl = can_ioctl, /* use can_ioctl() from af_can.c */ 1674 .listen = sock_no_listen, 1675 .shutdown = sock_no_shutdown, 1676 .setsockopt = sock_no_setsockopt, 1677 .getsockopt = sock_no_getsockopt, 1678 .sendmsg = bcm_sendmsg, 1679 .recvmsg = bcm_recvmsg, 1680 .mmap = sock_no_mmap, 1681 .sendpage = sock_no_sendpage, 1682 }; 1683 1684 static struct proto bcm_proto __read_mostly = { 1685 .name = "CAN_BCM", 1686 .owner = THIS_MODULE, 1687 .obj_size = sizeof(struct bcm_sock), 1688 .init = bcm_init, 1689 }; 1690 1691 static const struct can_proto bcm_can_proto = { 1692 .type = SOCK_DGRAM, 1693 .protocol = CAN_BCM, 1694 .ops = &bcm_ops, 1695 .prot = &bcm_proto, 1696 }; 1697 1698 static int canbcm_pernet_init(struct net *net) 1699 { 1700 #if IS_ENABLED(CONFIG_PROC_FS) 1701 /* create /proc/net/can-bcm directory */ 1702 net->can.bcmproc_dir = proc_net_mkdir(net, "can-bcm", net->proc_net); 1703 #endif /* CONFIG_PROC_FS */ 1704 1705 return 0; 1706 } 1707 1708 static void canbcm_pernet_exit(struct net *net) 1709 { 1710 #if IS_ENABLED(CONFIG_PROC_FS) 1711 /* remove /proc/net/can-bcm directory */ 1712 if (net->can.bcmproc_dir) 1713 remove_proc_entry("can-bcm", net->proc_net); 1714 #endif /* CONFIG_PROC_FS */ 1715 } 1716 1717 static struct pernet_operations canbcm_pernet_ops __read_mostly = { 1718 .init = canbcm_pernet_init, 1719 .exit = canbcm_pernet_exit, 1720 }; 1721 1722 static int __init bcm_module_init(void) 1723 { 1724 int err; 1725 1726 pr_info("can: broadcast manager protocol (rev " CAN_BCM_VERSION " t)\n"); 1727 1728 err = can_proto_register(&bcm_can_proto); 1729 if (err < 0) { 1730 printk(KERN_ERR "can: registration of bcm protocol failed\n"); 1731 return err; 1732 } 1733 1734 register_pernet_subsys(&canbcm_pernet_ops); 1735 return 0; 1736 } 1737 1738 static void __exit bcm_module_exit(void) 1739 { 1740 can_proto_unregister(&bcm_can_proto); 1741 unregister_pernet_subsys(&canbcm_pernet_ops); 1742 } 1743 1744 module_init(bcm_module_init); 1745 module_exit(bcm_module_exit); 1746