[PATCH net-next-2.6] can: Proper ctrlmode handling for CAN devices

@@ -1073,6 +1073,7 @@ static int __init at91_can_probe(struct platform_device *pdev)
 	priv->can.bittiming_const = &at91_bittiming_const;
 	priv->can.do_set_bittiming = at91_set_bittiming;
 	priv->can.do_set_mode = at91_set_mode;
+	priv->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES;
 	priv->reg_base = addr;
 	priv->dev = dev;
 	priv->clk = clk;

@@ -603,6 +603,7 @@ struct net_device *alloc_bfin_candev(void)
 	priv->can.bittiming_const = &bfin_can_bittiming_const;
 	priv->can.do_set_bittiming = bfin_can_set_bittiming;
 	priv->can.do_set_mode = bfin_can_set_mode;
+	priv->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES;
 
 	return dev;
 }

@@ -587,13 +587,22 @@ static int can_changelink(struct net_device *dev,
 
 	if (data[IFLA_CAN_CTRLMODE]) {
 		struct can_ctrlmode *cm;
+		u32 ctrlmode;
 
 		/* Do not allow changing controller mode while running */
 		if (dev->flags & IFF_UP)
 			return -EBUSY;
 		cm = nla_data(data[IFLA_CAN_CTRLMODE]);
-		priv->ctrlmode &= ~cm->mask;
-		priv->ctrlmode |= cm->flags;
+		if (cm->flags & ~priv->ctrlmode_supported)
+			return -EOPNOTSUPP;
+		ctrlmode = priv->ctrlmode & ~cm->mask;
+		ctrlmode |= cm->flags;
+		if (priv->check_ctrlmode) {
+			err = priv->check_ctrlmode(ctrlmode);
+			if (err < 0)
+				return err;
+		}
+		priv->ctrlmode = ctrlmode;
 	}
 
 	if (data[IFLA_CAN_BITTIMING]) {

@@ -538,10 +538,14 @@ static void mcp251x_set_normal_mode(struct spi_device *spi)
 
 	if (priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK) {
 		/* Put device into loopback mode */
-		mcp251x_write_reg(spi, CANCTRL, CANCTRL_REQOP_LOOPBACK);
+	} else if (priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) {
+		/* Put device into liste-only mode */
+		mcp251x_write_reg(spi, CANCTRL, CANCTRL_REQOP_LISTEN_ONLY);
 	} else {
 		/* Put device into normal mode */
-		mcp251x_write_reg(spi, CANCTRL, CANCTRL_REQOP_NORMAL);
+		mcp251x_write_reg(spi, CANCTRL, CANCTRL_REQOP_NORMAL |
+				  (priv->can.ctrlmode & CAN_CTRLMODE_ONE_SHOT ?
+				   CANCTRL_OSM : 0));
 
 		/* Wait for the device to enter normal mode */
 		timeout = jiffies + HZ;

@@ -917,6 +921,25 @@ static void mcp251x_irq_work_handler(struct work_struct *ws)
 	}
 }
 
+static inline int mcp251x_match_bits(u32 arg, u32 mask)
+{
+	if ((arg & mask) == mask)
+		return 1;
+	return 0;
+}
+
+static int mcp251x_check_ctrlmode(u32 ctrlmode)
+{
+	if (mcp251x_match_bits(ctrlmode, CAN_CTRLMODE_LOOPBACK |
+			       CAN_CTRLMODE_LISTENONLY) ||
+	    mcp251x_match_bits(ctrlmode, CAN_CTRLMODE_LOOPBACK |
+			       CAN_CTRLMODE_ONE_SHOT) ||
+	    mcp251x_match_bits(ctrlmode, CAN_CTRLMODE_ONE_SHOT |
+			       CAN_CTRLMODE_LISTENONLY))
+		return -EOPNOTSUPP;
+	return 0;
+}
+
 static const struct net_device_ops mcp251x_netdev_ops = {
 	.ndo_open = mcp251x_open,
 	.ndo_stop = mcp251x_stop,

@@ -948,6 +971,11 @@ static int __devinit mcp251x_can_probe(struct spi_device *spi)
 	priv->can.bittiming_const = &mcp251x_bittiming_const;
 	priv->can.do_set_mode = mcp251x_do_set_mode;
 	priv->can.clock.freq = pdata->oscillator_frequency / 2;
+	priv->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES |
+		CAN_CTRLMODE_LOOPBACK | CAN_CTRLMODE_LISTENONLY;
+	if (pdata->model == CAN_MCP251X_MCP2515)
+		priv->can.ctrlmode_supported |= CAN_CTRLMODE_ONE_SHOT;
+	priv->can.check_ctrlmode = mcp251x_check_ctrlmode;
 	priv->net = net;
 	dev_set_drvdata(&spi->dev, priv);
 

@@ -686,6 +686,7 @@ struct net_device *alloc_mscandev(void)
 	priv->can.bittiming_const = &mscan_bittiming_const;
 	priv->can.do_set_bittiming = mscan_do_set_bittiming;
 	priv->can.do_set_mode = mscan_do_set_mode;
+	priv->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES;
 
 	for (i = 0; i < TX_QUEUE_SIZE; i++) {
 		priv->tx_queue[i].id = i;

@@ -567,6 +567,7 @@ struct net_device *alloc_sja1000dev(int sizeof_priv)
 	priv->can.bittiming_const = &sja1000_bittiming_const;
 	priv->can.do_set_bittiming = sja1000_set_bittiming;
 	priv->can.do_set_mode = sja1000_set_mode;
+	priv->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES;
 
 	if (sizeof_priv)
 		priv->priv = (void *)priv + sizeof(struct sja1000_priv);

@@ -909,6 +909,7 @@ static int ti_hecc_probe(struct platform_device *pdev)
 	priv->can.bittiming_const = &ti_hecc_bittiming_const;
 	priv->can.do_set_mode = ti_hecc_do_set_mode;
 	priv->can.do_get_state = ti_hecc_get_state;
+	priv->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES;
 
 	ndev->irq = irq->start;
 	ndev->flags |= IFF_ECHO;

@@ -1022,6 +1022,7 @@ static int ems_usb_probe(struct usb_interface *intf,
 	dev->can.bittiming_const = &ems_usb_bittiming_const;
 	dev->can.do_set_bittiming = ems_usb_set_bittiming;
 	dev->can.do_set_mode = ems_usb_set_mode;
+	dev->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES;
 
 	netdev->flags |= IFF_ECHO; /* we support local echo */
 

@@ -38,6 +38,7 @@ struct can_priv {
 
 	enum can_state state;
 	u32 ctrlmode;
+	u32 ctrlmode_supported;
 
 	int restart_ms;
 	struct timer_list restart_timer;

@@ -46,6 +47,7 @@ struct can_priv {
 	int (*do_set_mode)(struct net_device *dev, enum can_mode mode);
 	int (*do_get_state)(const struct net_device *dev,
 			    enum can_state *state);
+	int (*check_ctrlmode) (u32 ctrlmode);
 
 	unsigned int echo_skb_max;
 	struct sk_buff **echo_skb;