can: slcan: extend the protocol with CAN state info

It extends the protocol to receive the adapter CAN state changes
(warning, busoff, etc.) and forward them to the netdev upper levels.

Link: https://lore.kernel.org/all/20220628163137.413025-13-dario.binacchi@amarulasolutions.com
Signed-off-by: Dario Binacchi <dario.binacchi@amarulasolutions.com>
Signed-off-by: Marc Kleine-Budde <mkl@pengutronix.de>

diff --git a/drivers/net/can/slcan/slcan-core.c b/drivers/net/can/slcan/slcan-core.c
index 4269b2267be2229aa4e36f4d3f8f5c2ddc7e3ba0..54d29a410ad54b38a5c43b8dad297eac6311689c 100644 (file)
--- a/drivers/net/can/slcan/slcan-core.c
+++ b/drivers/net/can/slcan/slcan-core.c
@@ -78,7 +78,11 @@ MODULE_PARM_DESC(maxdev, "Maximum number of slcan interfaces");
 #define SLC_CMD_LEN 1
 #define SLC_SFF_ID_LEN 3
 #define SLC_EFF_ID_LEN 8
-
+#define SLC_STATE_LEN 1
+#define SLC_STATE_BE_RXCNT_LEN 3
+#define SLC_STATE_BE_TXCNT_LEN 3
+#define SLC_STATE_FRAME_LEN       (1 + SLC_CMD_LEN + SLC_STATE_BE_RXCNT_LEN + \
+                                  SLC_STATE_BE_TXCNT_LEN)
 struct slcan {
        struct can_priv         can;
        int                     magic;
@@ -254,6 +258,72 @@ decode_failed:
        dev_kfree_skb(skb);
 }
 
+/* A change state frame must contain state info and receive and transmit
+ * error counters.
+ *
+ * Examples:
+ *
+ * sb256256 : state bus-off: rx counter 256, tx counter 256
+ * sa057033 : state active, rx counter 57, tx counter 33
+ */
+static void slc_bump_state(struct slcan *sl)
+{
+       struct net_device *dev = sl->dev;
+       struct sk_buff *skb;
+       struct can_frame *cf;
+       char *cmd = sl->rbuff;
+       u32 rxerr, txerr;
+       enum can_state state, rx_state, tx_state;
+
+       switch (cmd[1]) {
+       case 'a':
+               state = CAN_STATE_ERROR_ACTIVE;
+               break;
+       case 'w':
+               state = CAN_STATE_ERROR_WARNING;
+               break;
+       case 'p':
+               state = CAN_STATE_ERROR_PASSIVE;
+               break;
+       case 'b':
+               state = CAN_STATE_BUS_OFF;
+               break;
+       default:
+               return;
+       }
+
+       if (state == sl->can.state || sl->rcount < SLC_STATE_FRAME_LEN)
+               return;
+
+       cmd += SLC_STATE_BE_RXCNT_LEN + SLC_CMD_LEN + 1;
+       cmd[SLC_STATE_BE_TXCNT_LEN] = 0;
+       if (kstrtou32(cmd, 10, &txerr))
+               return;
+
+       *cmd = 0;
+       cmd -= SLC_STATE_BE_RXCNT_LEN;
+       if (kstrtou32(cmd, 10, &rxerr))
+               return;
+
+       skb = alloc_can_err_skb(dev, &cf);
+       if (skb) {
+               cf->data[6] = txerr;
+               cf->data[7] = rxerr;
+       } else {
+               cf = NULL;
+       }
+
+       tx_state = txerr >= rxerr ? state : 0;
+       rx_state = txerr <= rxerr ? state : 0;
+       can_change_state(dev, cf, tx_state, rx_state);
+
+       if (state == CAN_STATE_BUS_OFF)
+               can_bus_off(dev);
+
+       if (skb)
+               netif_rx(skb);
+}
+
 /* An error frame can contain more than one type of error.
  *
  * Examples:
@@ -387,6 +457,8 @@ static void slc_bump(struct slcan *sl)
                return slc_bump_frame(sl);
        case 'e':
                return slc_bump_err(sl);
+       case 's':
+               return slc_bump_state(sl);
        default:
                return;
        }