Added new socketoptions to force the isotp protocol to intentionally

[socketcan-devel.git] / kernel / 2.6 / net / can / isotp.c

diff --git a/kernel/2.6/net/can/isotp.c b/kernel/2.6/net/can/isotp.c
index 7493af6d5b3820cddcbbe9bfd48ba0bca6d15bb0..4d7f98b0e6ffae9b2f81c4e1cbb4e230c87198b3 100644 (file)
--- a/kernel/2.6/net/can/isotp.c
+++ b/kernel/2.6/net/can/isotp.c
@@ -132,10 +132,13 @@ struct isotp_sock {
        canid_t txid;
        canid_t rxid;
        ktime_t tx_gap;
+       ktime_t last_cf_tstamp;
        struct hrtimer rxtimer, txtimer;
        struct tasklet_struct txtsklet;
        struct can_isotp_options opt;
        struct can_isotp_fc_options rxfc, txfc;
+       __u32 force_tx_stmin;
+       __u32 force_rx_stmin;
        struct tpcon rx, tx;
        struct notifier_block notifier;
        wait_queue_head_t wait;
@@ -276,11 +279,17 @@ static int isotp_rcv_fc(struct isotp_sock *so, struct can_frame *cf, int ae)
                    ((so->txfc.stmin < 0xF1) || (so->txfc.stmin > 0xF9)))
                        so->txfc.stmin = 0x7F;
 
+               /* reset CF frame rx timestamp for rx stmin enforcement */
+               so->last_cf_tstamp = ktime_set(0,0);
+
                so->tx_gap = ktime_set(0,0);
                /* add transmission time for CAN frame N_As */
                so->tx_gap = ktime_add_ns(so->tx_gap, so->opt.frame_txtime);
                /* add waiting time for consecutive frames N_Cs */
-               if (so->txfc.stmin < 0x80)
+               if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN) 
+                       so->tx_gap = ktime_add_ns(so->tx_gap,
+                                                 so->force_tx_stmin);
+               else if (so->txfc.stmin < 0x80)
                        so->tx_gap = ktime_add_ns(so->tx_gap,
                                                  so->txfc.stmin * 1000000);
                else
@@ -397,6 +406,16 @@ static int isotp_rcv_cf(struct sock *sk, struct can_frame *cf, int ae,
        if (so->rx.state != ISOTP_WAIT_DATA)
                return 0;
 
+       /* drop if timestamp gap is less than force_rx_stmin nano secs */
+       if (so->opt.flags & CAN_ISOTP_FORCE_RXSTMIN) {
+
+               if (ktime_to_ns(ktime_sub(skb->tstamp, so->last_cf_tstamp)) <
+                   so->force_rx_stmin)
+                       return 0;
+
+               so->last_cf_tstamp = skb->tstamp; 
+       }
+
        hrtimer_cancel(&so->rxtimer);
 
        if ((cf->data[ae] & 0x0F) != so->rx.sn) {
@@ -915,8 +934,13 @@ static int isotp_getname(struct socket *sock, struct sockaddr *uaddr,
        return 0;
 }
 
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+static int isotp_setsockopt(struct socket *sock, int level, int optname,
+                           char __user *optval, unsigned int optlen)
+#else
 static int isotp_setsockopt(struct socket *sock, int level, int optname,
                            char __user *optval, int optlen)
+#endif
 {
        struct sock *sk = sock->sk;
        struct isotp_sock *so = isotp_sk(sk);
@@ -945,6 +969,22 @@ static int isotp_setsockopt(struct socket *sock, int level, int optname,
                        return -EFAULT;
                break;
 
+       case CAN_ISOTP_TX_STMIN:
+               if (optlen != sizeof(__u32))
+                       return -EINVAL;
+
+               if (copy_from_user(&so->force_tx_stmin, optval, optlen))
+                       return -EFAULT;
+               break;
+
+       case CAN_ISOTP_RX_STMIN:
+               if (optlen != sizeof(__u32))
+                       return -EINVAL;
+
+               if (copy_from_user(&so->force_rx_stmin, optval, optlen))
+                       return -EFAULT;
+               break;
+
        default:
                ret = -ENOPROTOOPT;
        }
@@ -979,6 +1019,16 @@ static int isotp_getsockopt(struct socket *sock, int level, int optname,
                val = &so->rxfc;
                break;
 
+       case CAN_ISOTP_TX_STMIN:
+               len = min_t(int, len, sizeof(__u32));
+               val = &so->force_tx_stmin;
+               break;
+
+       case CAN_ISOTP_RX_STMIN:
+               len = min_t(int, len, sizeof(__u32));
+               val = &so->force_rx_stmin;
+               break;
+
        default:
                return -ENOPROTOOPT;
        }