can: dev: fix crtlmode_supported check

[pandora-kernel.git] / drivers / net / can / dev.c

diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index 2cfe501..847c1f8 100644 (file)
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -273,6 +273,84 @@ static int can_get_bittiming(struct net_device *dev, struct can_bittiming *bt,
        return err;
 }
 
+static void can_update_state_error_stats(struct net_device *dev,
+                                        enum can_state new_state)
+{
+       struct can_priv *priv = netdev_priv(dev);
+
+       if (new_state <= priv->state)
+               return;
+
+       switch (new_state) {
+       case CAN_STATE_ERROR_WARNING:
+               priv->can_stats.error_warning++;
+               break;
+       case CAN_STATE_ERROR_PASSIVE:
+               priv->can_stats.error_passive++;
+               break;
+       case CAN_STATE_BUS_OFF:
+       default:
+               break;
+       };
+}
+
+static int can_tx_state_to_frame(struct net_device *dev, enum can_state state)
+{
+       switch (state) {
+       case CAN_STATE_ERROR_ACTIVE:
+               return CAN_ERR_CRTL_ACTIVE;
+       case CAN_STATE_ERROR_WARNING:
+               return CAN_ERR_CRTL_TX_WARNING;
+       case CAN_STATE_ERROR_PASSIVE:
+               return CAN_ERR_CRTL_TX_PASSIVE;
+       default:
+               return 0;
+       }
+}
+
+static int can_rx_state_to_frame(struct net_device *dev, enum can_state state)
+{
+       switch (state) {
+       case CAN_STATE_ERROR_ACTIVE:
+               return CAN_ERR_CRTL_ACTIVE;
+       case CAN_STATE_ERROR_WARNING:
+               return CAN_ERR_CRTL_RX_WARNING;
+       case CAN_STATE_ERROR_PASSIVE:
+               return CAN_ERR_CRTL_RX_PASSIVE;
+       default:
+               return 0;
+       }
+}
+
+void can_change_state(struct net_device *dev, struct can_frame *cf,
+                     enum can_state tx_state, enum can_state rx_state)
+{
+       struct can_priv *priv = netdev_priv(dev);
+       enum can_state new_state = max(tx_state, rx_state);
+
+       if (unlikely(new_state == priv->state)) {
+               netdev_warn(dev, "%s: oops, state did not change", __func__);
+               return;
+       }
+
+       netdev_dbg(dev, "New error state: %d\n", new_state);
+
+       can_update_state_error_stats(dev, new_state);
+       priv->state = new_state;
+
+       if (unlikely(new_state == CAN_STATE_BUS_OFF)) {
+               cf->can_id |= CAN_ERR_BUSOFF;
+               return;
+       }
+
+       cf->can_id |= CAN_ERR_CRTL;
+       cf->data[1] |= tx_state >= rx_state ?
+                      can_tx_state_to_frame(dev, tx_state) : 0;
+       cf->data[1] |= tx_state <= rx_state ?
+                      can_rx_state_to_frame(dev, rx_state) : 0;
+}
+EXPORT_SYMBOL_GPL(can_change_state);
+
 /*
  * Local echo of CAN messages
  *
@@ -729,10 +807,14 @@ static int can_changelink(struct net_device *dev,
                if (dev->flags & IFF_UP)
                        return -EBUSY;
                cm = nla_data(data[IFLA_CAN_CTRLMODE]);
-               if (cm->flags & ~priv->ctrlmode_supported)
+
+               /* check whether changed bits are allowed to be modified */
+               if (cm->mask & ~priv->ctrlmode_supported)
                        return -EOPNOTSUPP;
+
+               /* clear bits to be modified and copy the flag values */
                priv->ctrlmode &= ~cm->mask;
-               priv->ctrlmode |= cm->flags;
+               priv->ctrlmode |= (cm->flags & cm->mask);
 
                /* CAN_CTRLMODE_FD can only be set when driver supports FD */
                if (priv->ctrlmode & CAN_CTRLMODE_FD)