kernel_3.2_mcp251x_hwfilters.patch

diff -uprN linux-3.2.bach.orig/drivers/net/can/mcp251x.c linux-3.2.bach/drivers/net/can/mcp251x.c
--- linux-3.2.bach.orig/drivers/net/can/mcp251x.c	2015-05-25 15:14:19.979952273 +0200
+++ linux-3.2.bach/drivers/net/can/mcp251x.c	2015-06-05 14:54:41.025912625 +0200
@@ -184,10 +184,11 @@
 #define RXBEID0_OFF 4
 #define RXBDLC_OFF  5
 #define RXBDAT_OFF  6
-#define RXFSIDH(n) ((n) * 4)
-#define RXFSIDL(n) ((n) * 4 + 1)
-#define RXFEID8(n) ((n) * 4 + 2)
-#define RXFEID0(n) ((n) * 4 + 3)
+#define RXFSID(n)  ((n < 3) ? 0 : 4)
+#define RXFSIDH(n) ((n) * 4 + RXFSID(n))
+#define RXFSIDL(n) ((n) * 4 + 1 + RXFSID(n))
+#define RXFEID8(n) ((n) * 4 + 2 + RXFSID(n))
+#define RXFEID0(n) ((n) * 4 + 3 + RXFSID(n))
 #define RXMSIDH(n) ((n) * 4 + 0x20)
 #define RXMSIDL(n) ((n) * 4 + 0x21)
 #define RXMEID8(n) ((n) * 4 + 0x22)
@@ -258,6 +259,7 @@ struct mcp251x_priv {
 #define AFTER_SUSPEND_POWER 4
 #define AFTER_SUSPEND_RESTART 8
 	int restart_tx;
+	u8 hwfilterstate;
 };
 
 #define MCP251X_IS(_model) \
@@ -486,6 +488,11 @@ static void mcp251x_hw_rx(struct spi_dev
 	priv->net->stats.rx_packets++;
 	priv->net->stats.rx_bytes += frame->can_dlc;
 	netif_rx_ni(skb);
+
+    dev_dbg(&spi->dev, "CTRL: 0x%02x 0x%02x\n",
+              mcp251x_read_reg(spi, RXBCTRL(0)),
+             mcp251x_read_reg(spi, RXBCTRL(1)));
+
 }
 
 static void mcp251x_hw_sleep(struct spi_device *spi)
@@ -535,6 +542,25 @@ static int mcp251x_do_set_mode(struct ne
 	return 0;
 }
 
+static int mcp251x_set_conf_mode(struct spi_device *spi)
+{
+	unsigned long timeout;
+	mcp251x_write_reg(spi, CANCTRL, CANCTRL_REQOP_CONF);
+	
+	/* Wait for the device to enter config mode */
+	timeout = jiffies + HZ;
+	while ((mcp251x_read_reg(spi, CANSTAT) & CANCTRL_REQOP_MASK) != CANCTRL_REQOP_CONF) {
+		schedule();
+		if (time_after(jiffies, timeout)) {
+			dev_err(&spi->dev, "MCP251x didn't"
+				" enter in conf mode\n");
+			return -EBUSY;
+		}
+	}
+
+	return 0;
+}
+
 static int mcp251x_set_normal_mode(struct spi_device *spi)
 {
 	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
@@ -570,6 +596,321 @@ static int mcp251x_set_normal_mode(struc
 	return 0;
 }
 
+/************************
+ * SysFs - HW filters
+ */
+
+#define MCP251X_STDFILTER		5
+#define MCP251X_STDFILTER_MASK	1
+#define MCP251X_EXTFILTER		0
+#define MCP251X_EXTFILTER_MASK	0
+
+static void mcp251x_setmask(struct spi_device *spi,u8 n, u16 mask)
+{
+	mcp251x_write_reg(spi, RXMSIDH(n), mask >> 3);
+	mcp251x_write_reg(spi, RXMSIDL(n), (u8) (mask << 5));
+
+	mcp251x_write_reg(spi, RXMEID8(n), 0);
+	mcp251x_write_reg(spi, RXMEID0(n), 0);
+
+}
+
+static void mcp251x_setfilter(struct spi_device *spi,u8 n, u16 value)
+{
+	mcp251x_write_reg(spi, RXFSIDH(n), (u8) (value >> 3));
+	mcp251x_write_reg(spi, RXFSIDL(n), (u8) (value << 5));
+
+	mcp251x_write_reg(spi, RXFEID8(n), 0);
+	mcp251x_write_reg(spi, RXFEID0(n), 0);
+
+}
+
+static void mcp251x_setexfilter(struct spi_device *spi,u8 n, u32 value)
+{
+	u8 mdl;
+
+	mcp251x_write_reg(spi, RXFSIDH(n), (u8) (value >> 21));
+
+	mdl = (((value >> 16) & 0b11100) << 3) | ((value >> 16) & 0b11) | 0b1000;
+	mcp251x_write_reg(spi, RXFSIDL(n), mdl);
+
+
+	mcp251x_write_reg(spi, RXFEID8(n), (u8) (value >> 8));
+	mcp251x_write_reg(spi, RXFEID0(n), (u8) value);
+
+}
+
+static void mcp251x_setexmask(struct spi_device *spi,u8 n, u32 mask)
+{
+	u8 mdl;
+
+	mcp251x_write_reg(spi, RXMSIDH(n), (u8) (mask >> 21));
+
+	mdl = (((mask >> 16) & 0b11100) << 3) | ((mask >> 16) & 0b11);
+
+	mcp251x_write_reg(spi, RXMSIDL(n), (u8) mdl);
+
+	mcp251x_write_reg(spi, RXMEID8(n), (u8) (mask >> 8));
+	mcp251x_write_reg(spi, RXMEID0(n), (u8) mask);
+
+}
+
+
+static ssize_t mcp251x_sysfs_show_rxfsid(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+	u8 high = mcp251x_read_reg(spi, RXFSIDH(MCP251X_STDFILTER));
+	u8 low = mcp251x_read_reg(spi, RXFSIDL(MCP251X_STDFILTER));
+	u16 data = (high << 3) | (low >> 5);
+	return snprintf(buf, PAGE_SIZE, "%u\n", data);
+}
+
+static ssize_t mcp251x_sysfs_store_rxfsid(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+	u16 val;
+	int ret;
+
+	ret = kstrtou16(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	mcp251x_setfilter(spi, MCP251X_STDFILTER, val);
+	return count;
+}
+
+static DEVICE_ATTR(rxfsid, S_IRUGO | S_IWUSR, mcp251x_sysfs_show_rxfsid, mcp251x_sysfs_store_rxfsid);
+
+
+static ssize_t mcp251x_sysfs_show_rxmsid(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+	u8 high = mcp251x_read_reg(spi, RXMSIDH(MCP251X_STDFILTER_MASK));
+	u8 low = mcp251x_read_reg(spi, RXMSIDL(MCP251X_STDFILTER_MASK));
+
+	u16 data = (high << 3) | (low >> 5);
+	return snprintf(buf, PAGE_SIZE, "%u\n", data);
+}
+
+static ssize_t mcp251x_sysfs_store_rxmsid(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+	u16 val;
+	int ret;
+
+	ret = kstrtou16(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+	mcp251x_setmask(spi, MCP251X_STDFILTER_MASK, val);
+	return count;
+}
+static DEVICE_ATTR(rxmsid, S_IRUGO | S_IWUSR, mcp251x_sysfs_show_rxmsid, mcp251x_sysfs_store_rxmsid);
+
+
+static ssize_t mcp251x_sysfs_show_rxmeid(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+
+	u8 high = mcp251x_read_reg(spi, RXMSIDH(MCP251X_EXTFILTER_MASK));
+	u8 low = mcp251x_read_reg(spi, RXMSIDL(MCP251X_EXTFILTER_MASK));
+	u8 e8 = mcp251x_read_reg(spi, RXMEID8(MCP251X_EXTFILTER_MASK));
+	u8 e0 = mcp251x_read_reg(spi, RXMEID0(MCP251X_EXTFILTER_MASK));
+
+	u32 data = (high << 21) | ((((low & 0b11100000) >> 3) | (low & 0b11)) << 16) | (e8 << 8) | e0;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data);
+}
+
+static ssize_t mcp251x_sysfs_store_rxmeid(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+	u32 val;
+	int ret;
+
+	ret = kstrtou32(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	mcp251x_setexmask(spi, MCP251X_EXTFILTER_MASK, val);
+	return count;
+}
+static DEVICE_ATTR(rxmeid, S_IRUGO | S_IWUSR,
+		mcp251x_sysfs_show_rxmeid, mcp251x_sysfs_store_rxmeid);
+
+
+static ssize_t mcp251x_sysfs_show_rxfeid(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+
+	u8 high = mcp251x_read_reg(spi, RXFSIDH(MCP251X_EXTFILTER));
+	u8 low = mcp251x_read_reg(spi, RXFSIDL(MCP251X_EXTFILTER));
+	u8 e8 = mcp251x_read_reg(spi, RXFEID8(MCP251X_EXTFILTER));
+	u8 e0 = mcp251x_read_reg(spi, RXFEID0(MCP251X_EXTFILTER));
+	u32 data = (high << 21) | ((((low & 0b11100000) >> 3) | (low & 0b11)) << 16) | (e8 << 8) | e0;
+	return snprintf(buf, PAGE_SIZE, "%u\n", data);
+}
+
+static ssize_t mcp251x_sysfs_store_rxfeid(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+	u32 val;
+	int ret;
+
+	ret = kstrtou32(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+
+	mcp251x_setexfilter(spi, MCP251X_EXTFILTER, val);
+	return count;
+}
+static DEVICE_ATTR(rxfeid, S_IRUGO | S_IWUSR,
+		mcp251x_sysfs_show_rxfeid, mcp251x_sysfs_store_rxfeid);
+
+
+static ssize_t mcp251x_sysfs_show_confstate(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+	int res = 1;
+
+
+	if ((mcp251x_read_reg(spi, CANSTAT) & CANCTRL_REQOP_MASK) != CANCTRL_REQOP_CONF) {
+		res = 0;
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", res);
+}
+
+static ssize_t mcp251x_sysfs_store_confstate(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+
+	int val, ret;
+
+	ret = kstrtoint(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val) {
+		if (mcp251x_set_conf_mode(spi) < 0)
+			return -1;
+	} else {
+		mcp251x_set_normal_mode(spi);
+	}
+
+	return count;
+}
+static DEVICE_ATTR(confstate, S_IRUGO | S_IWUSR, mcp251x_sysfs_show_confstate, mcp251x_sysfs_store_confstate);
+
+
+/**
+ *
+ * 00 - off hw filter
+ * 11 - filter on both
+ * 01 - filter on for 11bit
+ * 10 - filter on for 29bit (standard will not be filtered)
+ */
+static ssize_t mcp251x_sysfs_store_hwfilter(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	struct spi_device *spi = priv->spi;
+	u8 reg = 0;
+	u8 i, state, res;
+
+	res = kstrtou8(buf, 10, &state);
+
+	if (res < 0)
+		return res;
+
+	if (!state) {
+		reg = RXBCTRL_RXM0 | RXBCTRL_RXM1;
+	}
+	else {
+		mcp251x_setexmask(spi, MCP251X_EXTFILTER_MASK, 0);
+		mcp251x_setmask(spi, MCP251X_STDFILTER_MASK, 0);
+
+		for (i = 0; i < 2; i++) {
+			mcp251x_setexfilter(spi, i, 0x1FFFFFFF);
+		}
+		for (i = 2; i < 6; i++) {
+			mcp251x_setfilter(spi, i, 0x7ff);
+		}
+
+		if (state == 0b01) {
+			mcp251x_setmask(spi, MCP251X_STDFILTER_MASK, 0x7ff);
+		} else if (state == 0b10) {
+			mcp251x_setexmask(spi, MCP251X_EXTFILTER_MASK, 0x1FFFFFFF);
+		}
+	}
+
+	priv->hwfilterstate = state;
+	mcp251x_write_reg(spi, RXBCTRL(0), RXBCTRL_BUKT | reg);
+	mcp251x_write_reg(spi, RXBCTRL(1), reg);
+
+	return count;
+}
+
+static ssize_t mcp251x_sysfs_show_hwfilter(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct mcp251x_priv *priv = netdev_priv(to_net_dev(dev));
+	return snprintf(buf, PAGE_SIZE, "%d\n", priv->hwfilterstate);
+}
+
+static DEVICE_ATTR(hwfilter, S_IRUGO | S_IWUSR, mcp251x_sysfs_show_hwfilter, mcp251x_sysfs_store_hwfilter);
+
+
+static struct attribute *mcp251x_sysfs_attrs[] = {
+	&dev_attr_confstate.attr,
+	&dev_attr_rxfsid.attr,
+	&dev_attr_rxmsid.attr,
+	&dev_attr_rxfeid.attr,
+	&dev_attr_rxmeid.attr,
+	&dev_attr_hwfilter.attr,
+	NULL,
+};
+
+static struct attribute_group mcp251x_sysfs_attr_group = {
+	.name = "hwfilters",
+	.attrs = mcp251x_sysfs_attrs,
+};
+
+/***********************
+ * END SysFs for HW filters
+ ***********/
+
 static int mcp251x_do_set_bittiming(struct net_device *net)
 {
 	struct mcp251x_priv *priv = netdev_priv(net);
@@ -598,10 +939,9 @@ static int mcp251x_setup(struct net_devi
 {
 	mcp251x_do_set_bittiming(net);
 
-	mcp251x_write_reg(spi, RXBCTRL(0),
-			  RXBCTRL_BUKT | RXBCTRL_RXM0 | RXBCTRL_RXM1);
-	mcp251x_write_reg(spi, RXBCTRL(1),
-			  RXBCTRL_RXM0 | RXBCTRL_RXM1);
+	mcp251x_write_reg(spi, RXBCTRL(0), RXBCTRL_BUKT | RXBCTRL_RXM0 | RXBCTRL_RXM1);
+	mcp251x_write_reg(spi, RXBCTRL(1), RXBCTRL_RXM0 | RXBCTRL_RXM1);
+	priv->hwfilterstate = 0;
 	return 0;
 }
 
@@ -929,6 +1269,7 @@ static int mcp251x_open(struct net_devic
 	priv->force_quit = 0;
 	priv->tx_skb = NULL;
 	priv->tx_len = 0;
+	priv->hwfilterstate = 0;
 
 	ret = request_threaded_irq(spi->irq, NULL, mcp251x_can_ist,
 		  pdata->irq_flags ? pdata->irq_flags : IRQF_TRIGGER_FALLING,
@@ -950,16 +1291,19 @@ static int mcp251x_open(struct net_devic
 		mcp251x_open_clean(net);
 		goto open_unlock;
 	}
+
 	ret = mcp251x_setup(net, priv, spi);
 	if (ret) {
 		mcp251x_open_clean(net);
 		goto open_unlock;
 	}
+
 	ret = mcp251x_set_normal_mode(spi);
 	if (ret) {
 		mcp251x_open_clean(net);
 		goto open_unlock;
 	}
+
 	netif_wake_queue(net);
 
 open_unlock:
@@ -1058,6 +1402,8 @@ static int __devinit mcp251x_can_probe(s
 	spi->mode = SPI_MODE_0;
 	spi->bits_per_word = 8;
 	spi_setup(spi);
+	
+	net->sysfs_groups[0] = &mcp251x_sysfs_attr_group;
 
 	/* Here is OK to not lock the MCP, no one knows about it yet */
 	if (!mcp251x_hw_probe(spi)) {

HW CAN filters are not support in linux kernel. This is patch for mcp251x which enabling hwfilters and it can be set via sysfs.