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 .open = bcm_proc_open, 250 .read = seq_read, 251 .llseek = seq_lseek, 252 .release = single_release, 253 }; 254 #endif /* CONFIG_PROC_FS */ 255 256 /* 257 * bcm_can_tx - send the (next) CAN frame to the appropriate CAN interface 258 * of the given bcm tx op 259 */ 260 static void bcm_can_tx(struct bcm_op *op) 261 { 262 struct sk_buff *skb; 263 struct net_device *dev; 264 struct canfd_frame *cf = op->frames + op->cfsiz * op->currframe; 265 266 /* no target device? => exit */ 267 if (!op->ifindex) 268 return; 269 270 dev = dev_get_by_index(sock_net(op->sk), op->ifindex); 271 if (!dev) { 272 /* RFC: should this bcm_op remove itself here? */ 273 return; 274 } 275 276 skb = alloc_skb(op->cfsiz + sizeof(struct can_skb_priv), gfp_any()); 277 if (!skb) 278 goto out; 279 280 can_skb_reserve(skb); 281 can_skb_prv(skb)->ifindex = dev->ifindex; 282 can_skb_prv(skb)->skbcnt = 0; 283 284 skb_put_data(skb, cf, op->cfsiz); 285 286 /* send with loopback */ 287 skb->dev = dev; 288 can_skb_set_owner(skb, op->sk); 289 can_send(skb, 1); 290 291 /* update statistics */ 292 op->currframe++; 293 op->frames_abs++; 294 295 /* reached last frame? */ 296 if (op->currframe >= op->nframes) 297 op->currframe = 0; 298 out: 299 dev_put(dev); 300 } 301 302 /* 303 * bcm_send_to_user - send a BCM message to the userspace 304 * (consisting of bcm_msg_head + x CAN frames) 305 */ 306 static void bcm_send_to_user(struct bcm_op *op, struct bcm_msg_head *head, 307 struct canfd_frame *frames, int has_timestamp) 308 { 309 struct sk_buff *skb; 310 struct canfd_frame *firstframe; 311 struct sockaddr_can *addr; 312 struct sock *sk = op->sk; 313 unsigned int datalen = head->nframes * op->cfsiz; 314 int err; 315 316 skb = alloc_skb(sizeof(*head) + datalen, gfp_any()); 317 if (!skb) 318 return; 319 320 skb_put_data(skb, head, sizeof(*head)); 321 322 if (head->nframes) { 323 /* CAN frames starting here */ 324 firstframe = (struct canfd_frame *)skb_tail_pointer(skb); 325 326 skb_put_data(skb, frames, datalen); 327 328 /* 329 * the BCM uses the flags-element of the canfd_frame 330 * structure for internal purposes. This is only 331 * relevant for updates that are generated by the 332 * BCM, where nframes is 1 333 */ 334 if (head->nframes == 1) 335 firstframe->flags &= BCM_CAN_FLAGS_MASK; 336 } 337 338 if (has_timestamp) { 339 /* restore rx timestamp */ 340 skb->tstamp = op->rx_stamp; 341 } 342 343 /* 344 * Put the datagram to the queue so that bcm_recvmsg() can 345 * get it from there. We need to pass the interface index to 346 * bcm_recvmsg(). We pass a whole struct sockaddr_can in skb->cb 347 * containing the interface index. 348 */ 349 350 sock_skb_cb_check_size(sizeof(struct sockaddr_can)); 351 addr = (struct sockaddr_can *)skb->cb; 352 memset(addr, 0, sizeof(*addr)); 353 addr->can_family = AF_CAN; 354 addr->can_ifindex = op->rx_ifindex; 355 356 err = sock_queue_rcv_skb(sk, skb); 357 if (err < 0) { 358 struct bcm_sock *bo = bcm_sk(sk); 359 360 kfree_skb(skb); 361 /* don't care about overflows in this statistic */ 362 bo->dropped_usr_msgs++; 363 } 364 } 365 366 static void bcm_tx_start_timer(struct bcm_op *op) 367 { 368 if (op->kt_ival1 && op->count) 369 hrtimer_start(&op->timer, 370 ktime_add(ktime_get(), op->kt_ival1), 371 HRTIMER_MODE_ABS); 372 else if (op->kt_ival2) 373 hrtimer_start(&op->timer, 374 ktime_add(ktime_get(), op->kt_ival2), 375 HRTIMER_MODE_ABS); 376 } 377 378 static void bcm_tx_timeout_tsklet(unsigned long data) 379 { 380 struct bcm_op *op = (struct bcm_op *)data; 381 struct bcm_msg_head msg_head; 382 383 if (op->kt_ival1 && (op->count > 0)) { 384 385 op->count--; 386 if (!op->count && (op->flags & TX_COUNTEVT)) { 387 388 /* create notification to user */ 389 msg_head.opcode = TX_EXPIRED; 390 msg_head.flags = op->flags; 391 msg_head.count = op->count; 392 msg_head.ival1 = op->ival1; 393 msg_head.ival2 = op->ival2; 394 msg_head.can_id = op->can_id; 395 msg_head.nframes = 0; 396 397 bcm_send_to_user(op, &msg_head, NULL, 0); 398 } 399 bcm_can_tx(op); 400 401 } else if (op->kt_ival2) 402 bcm_can_tx(op); 403 404 bcm_tx_start_timer(op); 405 } 406 407 /* 408 * bcm_tx_timeout_handler - performs cyclic CAN frame transmissions 409 */ 410 static enum hrtimer_restart bcm_tx_timeout_handler(struct hrtimer *hrtimer) 411 { 412 struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer); 413 414 tasklet_schedule(&op->tsklet); 415 416 return HRTIMER_NORESTART; 417 } 418 419 /* 420 * bcm_rx_changed - create a RX_CHANGED notification due to changed content 421 */ 422 static void bcm_rx_changed(struct bcm_op *op, struct canfd_frame *data) 423 { 424 struct bcm_msg_head head; 425 426 /* update statistics */ 427 op->frames_filtered++; 428 429 /* prevent statistics overflow */ 430 if (op->frames_filtered > ULONG_MAX/100) 431 op->frames_filtered = op->frames_abs = 0; 432 433 /* this element is not throttled anymore */ 434 data->flags &= (BCM_CAN_FLAGS_MASK|RX_RECV); 435 436 head.opcode = RX_CHANGED; 437 head.flags = op->flags; 438 head.count = op->count; 439 head.ival1 = op->ival1; 440 head.ival2 = op->ival2; 441 head.can_id = op->can_id; 442 head.nframes = 1; 443 444 bcm_send_to_user(op, &head, data, 1); 445 } 446 447 /* 448 * bcm_rx_update_and_send - process a detected relevant receive content change 449 * 1. update the last received data 450 * 2. send a notification to the user (if possible) 451 */ 452 static void bcm_rx_update_and_send(struct bcm_op *op, 453 struct canfd_frame *lastdata, 454 const struct canfd_frame *rxdata) 455 { 456 memcpy(lastdata, rxdata, op->cfsiz); 457 458 /* mark as used and throttled by default */ 459 lastdata->flags |= (RX_RECV|RX_THR); 460 461 /* throttling mode inactive ? */ 462 if (!op->kt_ival2) { 463 /* send RX_CHANGED to the user immediately */ 464 bcm_rx_changed(op, lastdata); 465 return; 466 } 467 468 /* with active throttling timer we are just done here */ 469 if (hrtimer_active(&op->thrtimer)) 470 return; 471 472 /* first reception with enabled throttling mode */ 473 if (!op->kt_lastmsg) 474 goto rx_changed_settime; 475 476 /* got a second frame inside a potential throttle period? */ 477 if (ktime_us_delta(ktime_get(), op->kt_lastmsg) < 478 ktime_to_us(op->kt_ival2)) { 479 /* do not send the saved data - only start throttle timer */ 480 hrtimer_start(&op->thrtimer, 481 ktime_add(op->kt_lastmsg, op->kt_ival2), 482 HRTIMER_MODE_ABS); 483 return; 484 } 485 486 /* the gap was that big, that throttling was not needed here */ 487 rx_changed_settime: 488 bcm_rx_changed(op, lastdata); 489 op->kt_lastmsg = ktime_get(); 490 } 491 492 /* 493 * bcm_rx_cmp_to_index - (bit)compares the currently received data to formerly 494 * received data stored in op->last_frames[] 495 */ 496 static void bcm_rx_cmp_to_index(struct bcm_op *op, unsigned int index, 497 const struct canfd_frame *rxdata) 498 { 499 struct canfd_frame *cf = op->frames + op->cfsiz * index; 500 struct canfd_frame *lcf = op->last_frames + op->cfsiz * index; 501 int i; 502 503 /* 504 * no one uses the MSBs of flags for comparison, 505 * so we use it here to detect the first time of reception 506 */ 507 508 if (!(lcf->flags & RX_RECV)) { 509 /* received data for the first time => send update to user */ 510 bcm_rx_update_and_send(op, lcf, rxdata); 511 return; 512 } 513 514 /* do a real check in CAN frame data section */ 515 for (i = 0; i < rxdata->len; i += 8) { 516 if ((get_u64(cf, i) & get_u64(rxdata, i)) != 517 (get_u64(cf, i) & get_u64(lcf, i))) { 518 bcm_rx_update_and_send(op, lcf, rxdata); 519 return; 520 } 521 } 522 523 if (op->flags & RX_CHECK_DLC) { 524 /* do a real check in CAN frame length */ 525 if (rxdata->len != lcf->len) { 526 bcm_rx_update_and_send(op, lcf, rxdata); 527 return; 528 } 529 } 530 } 531 532 /* 533 * bcm_rx_starttimer - enable timeout monitoring for CAN frame reception 534 */ 535 static void bcm_rx_starttimer(struct bcm_op *op) 536 { 537 if (op->flags & RX_NO_AUTOTIMER) 538 return; 539 540 if (op->kt_ival1) 541 hrtimer_start(&op->timer, op->kt_ival1, HRTIMER_MODE_REL); 542 } 543 544 static void bcm_rx_timeout_tsklet(unsigned long data) 545 { 546 struct bcm_op *op = (struct bcm_op *)data; 547 struct bcm_msg_head msg_head; 548 549 /* create notification to user */ 550 msg_head.opcode = RX_TIMEOUT; 551 msg_head.flags = op->flags; 552 msg_head.count = op->count; 553 msg_head.ival1 = op->ival1; 554 msg_head.ival2 = op->ival2; 555 msg_head.can_id = op->can_id; 556 msg_head.nframes = 0; 557 558 bcm_send_to_user(op, &msg_head, NULL, 0); 559 } 560 561 /* 562 * bcm_rx_timeout_handler - when the (cyclic) CAN frame reception timed out 563 */ 564 static enum hrtimer_restart bcm_rx_timeout_handler(struct hrtimer *hrtimer) 565 { 566 struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer); 567 568 /* schedule before NET_RX_SOFTIRQ */ 569 tasklet_hi_schedule(&op->tsklet); 570 571 /* no restart of the timer is done here! */ 572 573 /* if user wants to be informed, when cyclic CAN-Messages come back */ 574 if ((op->flags & RX_ANNOUNCE_RESUME) && op->last_frames) { 575 /* clear received CAN frames to indicate 'nothing received' */ 576 memset(op->last_frames, 0, op->nframes * op->cfsiz); 577 } 578 579 return HRTIMER_NORESTART; 580 } 581 582 /* 583 * bcm_rx_do_flush - helper for bcm_rx_thr_flush 584 */ 585 static inline int bcm_rx_do_flush(struct bcm_op *op, int update, 586 unsigned int index) 587 { 588 struct canfd_frame *lcf = op->last_frames + op->cfsiz * index; 589 590 if ((op->last_frames) && (lcf->flags & RX_THR)) { 591 if (update) 592 bcm_rx_changed(op, lcf); 593 return 1; 594 } 595 return 0; 596 } 597 598 /* 599 * bcm_rx_thr_flush - Check for throttled data and send it to the userspace 600 * 601 * update == 0 : just check if throttled data is available (any irq context) 602 * update == 1 : check and send throttled data to userspace (soft_irq context) 603 */ 604 static int bcm_rx_thr_flush(struct bcm_op *op, int update) 605 { 606 int updated = 0; 607 608 if (op->nframes > 1) { 609 unsigned int i; 610 611 /* for MUX filter we start at index 1 */ 612 for (i = 1; i < op->nframes; i++) 613 updated += bcm_rx_do_flush(op, update, i); 614 615 } else { 616 /* for RX_FILTER_ID and simple filter */ 617 updated += bcm_rx_do_flush(op, update, 0); 618 } 619 620 return updated; 621 } 622 623 static void bcm_rx_thr_tsklet(unsigned long data) 624 { 625 struct bcm_op *op = (struct bcm_op *)data; 626 627 /* push the changed data to the userspace */ 628 bcm_rx_thr_flush(op, 1); 629 } 630 631 /* 632 * bcm_rx_thr_handler - the time for blocked content updates is over now: 633 * Check for throttled data and send it to the userspace 634 */ 635 static enum hrtimer_restart bcm_rx_thr_handler(struct hrtimer *hrtimer) 636 { 637 struct bcm_op *op = container_of(hrtimer, struct bcm_op, thrtimer); 638 639 tasklet_schedule(&op->thrtsklet); 640 641 if (bcm_rx_thr_flush(op, 0)) { 642 hrtimer_forward(hrtimer, ktime_get(), op->kt_ival2); 643 return HRTIMER_RESTART; 644 } else { 645 /* rearm throttle handling */ 646 op->kt_lastmsg = 0; 647 return HRTIMER_NORESTART; 648 } 649 } 650 651 /* 652 * bcm_rx_handler - handle a CAN frame reception 653 */ 654 static void bcm_rx_handler(struct sk_buff *skb, void *data) 655 { 656 struct bcm_op *op = (struct bcm_op *)data; 657 const struct canfd_frame *rxframe = (struct canfd_frame *)skb->data; 658 unsigned int i; 659 660 if (op->can_id != rxframe->can_id) 661 return; 662 663 /* make sure to handle the correct frame type (CAN / CAN FD) */ 664 if (skb->len != op->cfsiz) 665 return; 666 667 /* disable timeout */ 668 hrtimer_cancel(&op->timer); 669 670 /* save rx timestamp */ 671 op->rx_stamp = skb->tstamp; 672 /* save originator for recvfrom() */ 673 op->rx_ifindex = skb->dev->ifindex; 674 /* update statistics */ 675 op->frames_abs++; 676 677 if (op->flags & RX_RTR_FRAME) { 678 /* send reply for RTR-request (placed in op->frames[0]) */ 679 bcm_can_tx(op); 680 return; 681 } 682 683 if (op->flags & RX_FILTER_ID) { 684 /* the easiest case */ 685 bcm_rx_update_and_send(op, op->last_frames, rxframe); 686 goto rx_starttimer; 687 } 688 689 if (op->nframes == 1) { 690 /* simple compare with index 0 */ 691 bcm_rx_cmp_to_index(op, 0, rxframe); 692 goto rx_starttimer; 693 } 694 695 if (op->nframes > 1) { 696 /* 697 * multiplex compare 698 * 699 * find the first multiplex mask that fits. 700 * Remark: The MUX-mask is stored in index 0 - but only the 701 * first 64 bits of the frame data[] are relevant (CAN FD) 702 */ 703 704 for (i = 1; i < op->nframes; i++) { 705 if ((get_u64(op->frames, 0) & get_u64(rxframe, 0)) == 706 (get_u64(op->frames, 0) & 707 get_u64(op->frames + op->cfsiz * i, 0))) { 708 bcm_rx_cmp_to_index(op, i, rxframe); 709 break; 710 } 711 } 712 } 713 714 rx_starttimer: 715 bcm_rx_starttimer(op); 716 } 717 718 /* 719 * helpers for bcm_op handling: find & delete bcm [rx|tx] op elements 720 */ 721 static struct bcm_op *bcm_find_op(struct list_head *ops, 722 struct bcm_msg_head *mh, int ifindex) 723 { 724 struct bcm_op *op; 725 726 list_for_each_entry(op, ops, list) { 727 if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) && 728 (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) 729 return op; 730 } 731 732 return NULL; 733 } 734 735 static void bcm_remove_op(struct bcm_op *op) 736 { 737 if (op->tsklet.func) { 738 while (test_bit(TASKLET_STATE_SCHED, &op->tsklet.state) || 739 test_bit(TASKLET_STATE_RUN, &op->tsklet.state) || 740 hrtimer_active(&op->timer)) { 741 hrtimer_cancel(&op->timer); 742 tasklet_kill(&op->tsklet); 743 } 744 } 745 746 if (op->thrtsklet.func) { 747 while (test_bit(TASKLET_STATE_SCHED, &op->thrtsklet.state) || 748 test_bit(TASKLET_STATE_RUN, &op->thrtsklet.state) || 749 hrtimer_active(&op->thrtimer)) { 750 hrtimer_cancel(&op->thrtimer); 751 tasklet_kill(&op->thrtsklet); 752 } 753 } 754 755 if ((op->frames) && (op->frames != &op->sframe)) 756 kfree(op->frames); 757 758 if ((op->last_frames) && (op->last_frames != &op->last_sframe)) 759 kfree(op->last_frames); 760 761 kfree(op); 762 } 763 764 static void bcm_rx_unreg(struct net_device *dev, struct bcm_op *op) 765 { 766 if (op->rx_reg_dev == dev) { 767 can_rx_unregister(dev_net(dev), dev, op->can_id, 768 REGMASK(op->can_id), bcm_rx_handler, op); 769 770 /* mark as removed subscription */ 771 op->rx_reg_dev = NULL; 772 } else 773 printk(KERN_ERR "can-bcm: bcm_rx_unreg: registered device " 774 "mismatch %p %p\n", op->rx_reg_dev, dev); 775 } 776 777 /* 778 * bcm_delete_rx_op - find and remove a rx op (returns number of removed ops) 779 */ 780 static int bcm_delete_rx_op(struct list_head *ops, struct bcm_msg_head *mh, 781 int ifindex) 782 { 783 struct bcm_op *op, *n; 784 785 list_for_each_entry_safe(op, n, ops, list) { 786 if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) && 787 (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) { 788 789 /* 790 * Don't care if we're bound or not (due to netdev 791 * problems) can_rx_unregister() is always a save 792 * thing to do here. 793 */ 794 if (op->ifindex) { 795 /* 796 * Only remove subscriptions that had not 797 * been removed due to NETDEV_UNREGISTER 798 * in bcm_notifier() 799 */ 800 if (op->rx_reg_dev) { 801 struct net_device *dev; 802 803 dev = dev_get_by_index(sock_net(op->sk), 804 op->ifindex); 805 if (dev) { 806 bcm_rx_unreg(dev, op); 807 dev_put(dev); 808 } 809 } 810 } else 811 can_rx_unregister(sock_net(op->sk), NULL, 812 op->can_id, 813 REGMASK(op->can_id), 814 bcm_rx_handler, op); 815 816 list_del(&op->list); 817 bcm_remove_op(op); 818 return 1; /* done */ 819 } 820 } 821 822 return 0; /* not found */ 823 } 824 825 /* 826 * bcm_delete_tx_op - find and remove a tx op (returns number of removed ops) 827 */ 828 static int bcm_delete_tx_op(struct list_head *ops, struct bcm_msg_head *mh, 829 int ifindex) 830 { 831 struct bcm_op *op, *n; 832 833 list_for_each_entry_safe(op, n, ops, list) { 834 if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) && 835 (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) { 836 list_del(&op->list); 837 bcm_remove_op(op); 838 return 1; /* done */ 839 } 840 } 841 842 return 0; /* not found */ 843 } 844 845 /* 846 * bcm_read_op - read out a bcm_op and send it to the user (for bcm_sendmsg) 847 */ 848 static int bcm_read_op(struct list_head *ops, struct bcm_msg_head *msg_head, 849 int ifindex) 850 { 851 struct bcm_op *op = bcm_find_op(ops, msg_head, ifindex); 852 853 if (!op) 854 return -EINVAL; 855 856 /* put current values into msg_head */ 857 msg_head->flags = op->flags; 858 msg_head->count = op->count; 859 msg_head->ival1 = op->ival1; 860 msg_head->ival2 = op->ival2; 861 msg_head->nframes = op->nframes; 862 863 bcm_send_to_user(op, msg_head, op->frames, 0); 864 865 return MHSIZ; 866 } 867 868 /* 869 * bcm_tx_setup - create or update a bcm tx op (for bcm_sendmsg) 870 */ 871 static int bcm_tx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg, 872 int ifindex, struct sock *sk) 873 { 874 struct bcm_sock *bo = bcm_sk(sk); 875 struct bcm_op *op; 876 struct canfd_frame *cf; 877 unsigned int i; 878 int err; 879 880 /* we need a real device to send frames */ 881 if (!ifindex) 882 return -ENODEV; 883 884 /* check nframes boundaries - we need at least one CAN frame */ 885 if (msg_head->nframes < 1 || msg_head->nframes > MAX_NFRAMES) 886 return -EINVAL; 887 888 /* check the given can_id */ 889 op = bcm_find_op(&bo->tx_ops, msg_head, ifindex); 890 if (op) { 891 /* update existing BCM operation */ 892 893 /* 894 * Do we need more space for the CAN frames than currently 895 * allocated? -> This is a _really_ unusual use-case and 896 * therefore (complexity / locking) it is not supported. 897 */ 898 if (msg_head->nframes > op->nframes) 899 return -E2BIG; 900 901 /* update CAN frames content */ 902 for (i = 0; i < msg_head->nframes; i++) { 903 904 cf = op->frames + op->cfsiz * i; 905 err = memcpy_from_msg((u8 *)cf, msg, op->cfsiz); 906 907 if (op->flags & CAN_FD_FRAME) { 908 if (cf->len > 64) 909 err = -EINVAL; 910 } else { 911 if (cf->len > 8) 912 err = -EINVAL; 913 } 914 915 if (err < 0) 916 return err; 917 918 if (msg_head->flags & TX_CP_CAN_ID) { 919 /* copy can_id into frame */ 920 cf->can_id = msg_head->can_id; 921 } 922 } 923 op->flags = msg_head->flags; 924 925 } else { 926 /* insert new BCM operation for the given can_id */ 927 928 op = kzalloc(OPSIZ, GFP_KERNEL); 929 if (!op) 930 return -ENOMEM; 931 932 op->can_id = msg_head->can_id; 933 op->cfsiz = CFSIZ(msg_head->flags); 934 op->flags = msg_head->flags; 935 936 /* create array for CAN frames and copy the data */ 937 if (msg_head->nframes > 1) { 938 op->frames = kmalloc(msg_head->nframes * op->cfsiz, 939 GFP_KERNEL); 940 if (!op->frames) { 941 kfree(op); 942 return -ENOMEM; 943 } 944 } else 945 op->frames = &op->sframe; 946 947 for (i = 0; i < msg_head->nframes; i++) { 948 949 cf = op->frames + op->cfsiz * i; 950 err = memcpy_from_msg((u8 *)cf, msg, op->cfsiz); 951 952 if (op->flags & CAN_FD_FRAME) { 953 if (cf->len > 64) 954 err = -EINVAL; 955 } else { 956 if (cf->len > 8) 957 err = -EINVAL; 958 } 959 960 if (err < 0) { 961 if (op->frames != &op->sframe) 962 kfree(op->frames); 963 kfree(op); 964 return err; 965 } 966 967 if (msg_head->flags & TX_CP_CAN_ID) { 968 /* copy can_id into frame */ 969 cf->can_id = msg_head->can_id; 970 } 971 } 972 973 /* tx_ops never compare with previous received messages */ 974 op->last_frames = NULL; 975 976 /* bcm_can_tx / bcm_tx_timeout_handler needs this */ 977 op->sk = sk; 978 op->ifindex = ifindex; 979 980 /* initialize uninitialized (kzalloc) structure */ 981 hrtimer_init(&op->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); 982 op->timer.function = bcm_tx_timeout_handler; 983 984 /* initialize tasklet for tx countevent notification */ 985 tasklet_init(&op->tsklet, bcm_tx_timeout_tsklet, 986 (unsigned long) op); 987 988 /* currently unused in tx_ops */ 989 hrtimer_init(&op->thrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); 990 991 /* add this bcm_op to the list of the tx_ops */ 992 list_add(&op->list, &bo->tx_ops); 993 994 } /* if ((op = bcm_find_op(&bo->tx_ops, msg_head->can_id, ifindex))) */ 995 996 if (op->nframes != msg_head->nframes) { 997 op->nframes = msg_head->nframes; 998 /* start multiple frame transmission with index 0 */ 999 op->currframe = 0; 1000 } 1001 1002 /* check flags */ 1003 1004 if (op->flags & TX_RESET_MULTI_IDX) { 1005 /* start multiple frame transmission with index 0 */ 1006 op->currframe = 0; 1007 } 1008 1009 if (op->flags & SETTIMER) { 1010 /* set timer values */ 1011 op->count = msg_head->count; 1012 op->ival1 = msg_head->ival1; 1013 op->ival2 = msg_head->ival2; 1014 op->kt_ival1 = bcm_timeval_to_ktime(msg_head->ival1); 1015 op->kt_ival2 = bcm_timeval_to_ktime(msg_head->ival2); 1016 1017 /* disable an active timer due to zero values? */ 1018 if (!op->kt_ival1 && !op->kt_ival2) 1019 hrtimer_cancel(&op->timer); 1020 } 1021 1022 if (op->flags & STARTTIMER) { 1023 hrtimer_cancel(&op->timer); 1024 /* spec: send CAN frame when starting timer */ 1025 op->flags |= TX_ANNOUNCE; 1026 } 1027 1028 if (op->flags & TX_ANNOUNCE) { 1029 bcm_can_tx(op); 1030 if (op->count) 1031 op->count--; 1032 } 1033 1034 if (op->flags & STARTTIMER) 1035 bcm_tx_start_timer(op); 1036 1037 return msg_head->nframes * op->cfsiz + MHSIZ; 1038 } 1039 1040 /* 1041 * bcm_rx_setup - create or update a bcm rx op (for bcm_sendmsg) 1042 */ 1043 static int bcm_rx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg, 1044 int ifindex, struct sock *sk) 1045 { 1046 struct bcm_sock *bo = bcm_sk(sk); 1047 struct bcm_op *op; 1048 int do_rx_register; 1049 int err = 0; 1050 1051 if ((msg_head->flags & RX_FILTER_ID) || (!(msg_head->nframes))) { 1052 /* be robust against wrong usage ... */ 1053 msg_head->flags |= RX_FILTER_ID; 1054 /* ignore trailing garbage */ 1055 msg_head->nframes = 0; 1056 } 1057 1058 /* the first element contains the mux-mask => MAX_NFRAMES + 1 */ 1059 if (msg_head->nframes > MAX_NFRAMES + 1) 1060 return -EINVAL; 1061 1062 if ((msg_head->flags & RX_RTR_FRAME) && 1063 ((msg_head->nframes != 1) || 1064 (!(msg_head->can_id & CAN_RTR_FLAG)))) 1065 return -EINVAL; 1066 1067 /* check the given can_id */ 1068 op = bcm_find_op(&bo->rx_ops, msg_head, ifindex); 1069 if (op) { 1070 /* update existing BCM operation */ 1071 1072 /* 1073 * Do we need more space for the CAN frames than currently 1074 * allocated? -> This is a _really_ unusual use-case and 1075 * therefore (complexity / locking) it is not supported. 1076 */ 1077 if (msg_head->nframes > op->nframes) 1078 return -E2BIG; 1079 1080 if (msg_head->nframes) { 1081 /* update CAN frames content */ 1082 err = memcpy_from_msg(op->frames, msg, 1083 msg_head->nframes * op->cfsiz); 1084 if (err < 0) 1085 return err; 1086 1087 /* clear last_frames to indicate 'nothing received' */ 1088 memset(op->last_frames, 0, msg_head->nframes * op->cfsiz); 1089 } 1090 1091 op->nframes = msg_head->nframes; 1092 op->flags = msg_head->flags; 1093 1094 /* Only an update -> do not call can_rx_register() */ 1095 do_rx_register = 0; 1096 1097 } else { 1098 /* insert new BCM operation for the given can_id */ 1099 op = kzalloc(OPSIZ, GFP_KERNEL); 1100 if (!op) 1101 return -ENOMEM; 1102 1103 op->can_id = msg_head->can_id; 1104 op->nframes = msg_head->nframes; 1105 op->cfsiz = CFSIZ(msg_head->flags); 1106 op->flags = msg_head->flags; 1107 1108 if (msg_head->nframes > 1) { 1109 /* create array for CAN frames and copy the data */ 1110 op->frames = kmalloc(msg_head->nframes * op->cfsiz, 1111 GFP_KERNEL); 1112 if (!op->frames) { 1113 kfree(op); 1114 return -ENOMEM; 1115 } 1116 1117 /* create and init array for received CAN frames */ 1118 op->last_frames = kzalloc(msg_head->nframes * op->cfsiz, 1119 GFP_KERNEL); 1120 if (!op->last_frames) { 1121 kfree(op->frames); 1122 kfree(op); 1123 return -ENOMEM; 1124 } 1125 1126 } else { 1127 op->frames = &op->sframe; 1128 op->last_frames = &op->last_sframe; 1129 } 1130 1131 if (msg_head->nframes) { 1132 err = memcpy_from_msg(op->frames, msg, 1133 msg_head->nframes * op->cfsiz); 1134 if (err < 0) { 1135 if (op->frames != &op->sframe) 1136 kfree(op->frames); 1137 if (op->last_frames != &op->last_sframe) 1138 kfree(op->last_frames); 1139 kfree(op); 1140 return err; 1141 } 1142 } 1143 1144 /* bcm_can_tx / bcm_tx_timeout_handler needs this */ 1145 op->sk = sk; 1146 op->ifindex = ifindex; 1147 1148 /* ifindex for timeout events w/o previous frame reception */ 1149 op->rx_ifindex = ifindex; 1150 1151 /* initialize uninitialized (kzalloc) structure */ 1152 hrtimer_init(&op->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); 1153 op->timer.function = bcm_rx_timeout_handler; 1154 1155 /* initialize tasklet for rx timeout notification */ 1156 tasklet_init(&op->tsklet, bcm_rx_timeout_tsklet, 1157 (unsigned long) op); 1158 1159 hrtimer_init(&op->thrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); 1160 op->thrtimer.function = bcm_rx_thr_handler; 1161 1162 /* initialize tasklet for rx throttle handling */ 1163 tasklet_init(&op->thrtsklet, bcm_rx_thr_tsklet, 1164 (unsigned long) op); 1165 1166 /* add this bcm_op to the list of the rx_ops */ 1167 list_add(&op->list, &bo->rx_ops); 1168 1169 /* call can_rx_register() */ 1170 do_rx_register = 1; 1171 1172 } /* if ((op = bcm_find_op(&bo->rx_ops, msg_head->can_id, ifindex))) */ 1173 1174 /* check flags */ 1175 1176 if (op->flags & RX_RTR_FRAME) { 1177 struct canfd_frame *frame0 = op->frames; 1178 1179 /* no timers in RTR-mode */ 1180 hrtimer_cancel(&op->thrtimer); 1181 hrtimer_cancel(&op->timer); 1182 1183 /* 1184 * funny feature in RX(!)_SETUP only for RTR-mode: 1185 * copy can_id into frame BUT without RTR-flag to 1186 * prevent a full-load-loopback-test ... ;-] 1187 */ 1188 if ((op->flags & TX_CP_CAN_ID) || 1189 (frame0->can_id == op->can_id)) 1190 frame0->can_id = op->can_id & ~CAN_RTR_FLAG; 1191 1192 } else { 1193 if (op->flags & SETTIMER) { 1194 1195 /* set timer value */ 1196 op->ival1 = msg_head->ival1; 1197 op->ival2 = msg_head->ival2; 1198 op->kt_ival1 = bcm_timeval_to_ktime(msg_head->ival1); 1199 op->kt_ival2 = bcm_timeval_to_ktime(msg_head->ival2); 1200 1201 /* disable an active timer due to zero value? */ 1202 if (!op->kt_ival1) 1203 hrtimer_cancel(&op->timer); 1204 1205 /* 1206 * In any case cancel the throttle timer, flush 1207 * potentially blocked msgs and reset throttle handling 1208 */ 1209 op->kt_lastmsg = 0; 1210 hrtimer_cancel(&op->thrtimer); 1211 bcm_rx_thr_flush(op, 1); 1212 } 1213 1214 if ((op->flags & STARTTIMER) && op->kt_ival1) 1215 hrtimer_start(&op->timer, op->kt_ival1, 1216 HRTIMER_MODE_REL); 1217 } 1218 1219 /* now we can register for can_ids, if we added a new bcm_op */ 1220 if (do_rx_register) { 1221 if (ifindex) { 1222 struct net_device *dev; 1223 1224 dev = dev_get_by_index(sock_net(sk), ifindex); 1225 if (dev) { 1226 err = can_rx_register(sock_net(sk), dev, 1227 op->can_id, 1228 REGMASK(op->can_id), 1229 bcm_rx_handler, op, 1230 "bcm", sk); 1231 1232 op->rx_reg_dev = dev; 1233 dev_put(dev); 1234 } 1235 1236 } else 1237 err = can_rx_register(sock_net(sk), NULL, op->can_id, 1238 REGMASK(op->can_id), 1239 bcm_rx_handler, op, "bcm", sk); 1240 if (err) { 1241 /* this bcm rx op is broken -> remove it */ 1242 list_del(&op->list); 1243 bcm_remove_op(op); 1244 return err; 1245 } 1246 } 1247 1248 return msg_head->nframes * op->cfsiz + MHSIZ; 1249 } 1250 1251 /* 1252 * bcm_tx_send - send a single CAN frame to the CAN interface (for bcm_sendmsg) 1253 */ 1254 static int bcm_tx_send(struct msghdr *msg, int ifindex, struct sock *sk, 1255 int cfsiz) 1256 { 1257 struct sk_buff *skb; 1258 struct net_device *dev; 1259 int err; 1260 1261 /* we need a real device to send frames */ 1262 if (!ifindex) 1263 return -ENODEV; 1264 1265 skb = alloc_skb(cfsiz + sizeof(struct can_skb_priv), GFP_KERNEL); 1266 if (!skb) 1267 return -ENOMEM; 1268 1269 can_skb_reserve(skb); 1270 1271 err = memcpy_from_msg(skb_put(skb, cfsiz), msg, cfsiz); 1272 if (err < 0) { 1273 kfree_skb(skb); 1274 return err; 1275 } 1276 1277 dev = dev_get_by_index(sock_net(sk), ifindex); 1278 if (!dev) { 1279 kfree_skb(skb); 1280 return -ENODEV; 1281 } 1282 1283 can_skb_prv(skb)->ifindex = dev->ifindex; 1284 can_skb_prv(skb)->skbcnt = 0; 1285 skb->dev = dev; 1286 can_skb_set_owner(skb, sk); 1287 err = can_send(skb, 1); /* send with loopback */ 1288 dev_put(dev); 1289 1290 if (err) 1291 return err; 1292 1293 return cfsiz + MHSIZ; 1294 } 1295 1296 /* 1297 * bcm_sendmsg - process BCM commands (opcodes) from the userspace 1298 */ 1299 static int bcm_sendmsg(struct socket *sock, struct msghdr *msg, size_t size) 1300 { 1301 struct sock *sk = sock->sk; 1302 struct bcm_sock *bo = bcm_sk(sk); 1303 int ifindex = bo->ifindex; /* default ifindex for this bcm_op */ 1304 struct bcm_msg_head msg_head; 1305 int cfsiz; 1306 int ret; /* read bytes or error codes as return value */ 1307 1308 if (!bo->bound) 1309 return -ENOTCONN; 1310 1311 /* check for valid message length from userspace */ 1312 if (size < MHSIZ) 1313 return -EINVAL; 1314 1315 /* read message head information */ 1316 ret = memcpy_from_msg((u8 *)&msg_head, msg, MHSIZ); 1317 if (ret < 0) 1318 return ret; 1319 1320 cfsiz = CFSIZ(msg_head.flags); 1321 if ((size - MHSIZ) % cfsiz) 1322 return -EINVAL; 1323 1324 /* check for alternative ifindex for this bcm_op */ 1325 1326 if (!ifindex && msg->msg_name) { 1327 /* no bound device as default => check msg_name */ 1328 DECLARE_SOCKADDR(struct sockaddr_can *, addr, msg->msg_name); 1329 1330 if (msg->msg_namelen < sizeof(*addr)) 1331 return -EINVAL; 1332 1333 if (addr->can_family != AF_CAN) 1334 return -EINVAL; 1335 1336 /* ifindex from sendto() */ 1337 ifindex = addr->can_ifindex; 1338 1339 if (ifindex) { 1340 struct net_device *dev; 1341 1342 dev = dev_get_by_index(sock_net(sk), ifindex); 1343 if (!dev) 1344 return -ENODEV; 1345 1346 if (dev->type != ARPHRD_CAN) { 1347 dev_put(dev); 1348 return -ENODEV; 1349 } 1350 1351 dev_put(dev); 1352 } 1353 } 1354 1355 lock_sock(sk); 1356 1357 switch (msg_head.opcode) { 1358 1359 case TX_SETUP: 1360 ret = bcm_tx_setup(&msg_head, msg, ifindex, sk); 1361 break; 1362 1363 case RX_SETUP: 1364 ret = bcm_rx_setup(&msg_head, msg, ifindex, sk); 1365 break; 1366 1367 case TX_DELETE: 1368 if (bcm_delete_tx_op(&bo->tx_ops, &msg_head, ifindex)) 1369 ret = MHSIZ; 1370 else 1371 ret = -EINVAL; 1372 break; 1373 1374 case RX_DELETE: 1375 if (bcm_delete_rx_op(&bo->rx_ops, &msg_head, ifindex)) 1376 ret = MHSIZ; 1377 else 1378 ret = -EINVAL; 1379 break; 1380 1381 case TX_READ: 1382 /* reuse msg_head for the reply to TX_READ */ 1383 msg_head.opcode = TX_STATUS; 1384 ret = bcm_read_op(&bo->tx_ops, &msg_head, ifindex); 1385 break; 1386 1387 case RX_READ: 1388 /* reuse msg_head for the reply to RX_READ */ 1389 msg_head.opcode = RX_STATUS; 1390 ret = bcm_read_op(&bo->rx_ops, &msg_head, ifindex); 1391 break; 1392 1393 case TX_SEND: 1394 /* we need exactly one CAN frame behind the msg head */ 1395 if ((msg_head.nframes != 1) || (size != cfsiz + MHSIZ)) 1396 ret = -EINVAL; 1397 else 1398 ret = bcm_tx_send(msg, ifindex, sk, cfsiz); 1399 break; 1400 1401 default: 1402 ret = -EINVAL; 1403 break; 1404 } 1405 1406 release_sock(sk); 1407 1408 return ret; 1409 } 1410 1411 /* 1412 * notification handler for netdevice status changes 1413 */ 1414 static int bcm_notifier(struct notifier_block *nb, unsigned long msg, 1415 void *ptr) 1416 { 1417 struct net_device *dev = netdev_notifier_info_to_dev(ptr); 1418 struct bcm_sock *bo = container_of(nb, struct bcm_sock, notifier); 1419 struct sock *sk = &bo->sk; 1420 struct bcm_op *op; 1421 int notify_enodev = 0; 1422 1423 if (!net_eq(dev_net(dev), sock_net(sk))) 1424 return NOTIFY_DONE; 1425 1426 if (dev->type != ARPHRD_CAN) 1427 return NOTIFY_DONE; 1428 1429 switch (msg) { 1430 1431 case NETDEV_UNREGISTER: 1432 lock_sock(sk); 1433 1434 /* remove device specific receive entries */ 1435 list_for_each_entry(op, &bo->rx_ops, list) 1436 if (op->rx_reg_dev == dev) 1437 bcm_rx_unreg(dev, op); 1438 1439 /* remove device reference, if this is our bound device */ 1440 if (bo->bound && bo->ifindex == dev->ifindex) { 1441 bo->bound = 0; 1442 bo->ifindex = 0; 1443 notify_enodev = 1; 1444 } 1445 1446 release_sock(sk); 1447 1448 if (notify_enodev) { 1449 sk->sk_err = ENODEV; 1450 if (!sock_flag(sk, SOCK_DEAD)) 1451 sk->sk_error_report(sk); 1452 } 1453 break; 1454 1455 case NETDEV_DOWN: 1456 if (bo->bound && bo->ifindex == dev->ifindex) { 1457 sk->sk_err = ENETDOWN; 1458 if (!sock_flag(sk, SOCK_DEAD)) 1459 sk->sk_error_report(sk); 1460 } 1461 } 1462 1463 return NOTIFY_DONE; 1464 } 1465 1466 /* 1467 * initial settings for all BCM sockets to be set at socket creation time 1468 */ 1469 static int bcm_init(struct sock *sk) 1470 { 1471 struct bcm_sock *bo = bcm_sk(sk); 1472 1473 bo->bound = 0; 1474 bo->ifindex = 0; 1475 bo->dropped_usr_msgs = 0; 1476 bo->bcm_proc_read = NULL; 1477 1478 INIT_LIST_HEAD(&bo->tx_ops); 1479 INIT_LIST_HEAD(&bo->rx_ops); 1480 1481 /* set notifier */ 1482 bo->notifier.notifier_call = bcm_notifier; 1483 1484 register_netdevice_notifier(&bo->notifier); 1485 1486 return 0; 1487 } 1488 1489 /* 1490 * standard socket functions 1491 */ 1492 static int bcm_release(struct socket *sock) 1493 { 1494 struct sock *sk = sock->sk; 1495 struct net *net; 1496 struct bcm_sock *bo; 1497 struct bcm_op *op, *next; 1498 1499 if (!sk) 1500 return 0; 1501 1502 net = sock_net(sk); 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