1 files changed, 52 insertions, 18 deletions

diff --git a/drivers/net/ksz8864rmn.c b/drivers/net/ksz8864rmn.c
index 85063ff0d8..c4c30377af 100644
--- a/drivers/net/ksz8864rmn.c
+++ b/drivers/net/ksz8864rmn.c
@@ -31,48 +31,55 @@
 #define CMD_WRITE		0x02
 #define CMD_READ		0x03
 
+enum ksz_type {
+	unknown,
+	ksz87,
+	ksz88
+};
+
 struct micrel_switch_priv {
 	struct cdev             cdev;
 	struct spi_device       *spi;
 	unsigned int		p_enable;
+	unsigned int		addr_width;
+	unsigned int		pad;
 };
 
-static int micrel_switch_read_reg(struct spi_device *spi, uint8_t reg)
+static int micrel_switch_read_reg(const struct micrel_switch_priv *priv, uint8_t reg)
 {
 	uint8_t tx[2];
 	uint8_t rx[1];
 	int ret;
 
-	tx[0] = CMD_READ;
-	tx[1] = reg;
+	tx[0] = CMD_READ << (priv->addr_width + priv->pad - 8) | reg >> (8 - priv->pad);
+	tx[1] = reg << priv->pad;
 
-	ret = spi_write_then_read(spi, tx, 2, rx, 1);
+	ret = spi_write_then_read(priv->spi, tx, 2, rx, 1);
 	if (ret < 0)
 		return ret;
 
 	return rx[0];
 }
 
-static void micrel_switch_write_reg(struct spi_device *spi, uint8_t reg, uint8_t val)
+static void micrel_switch_write_reg(const struct micrel_switch_priv *priv, uint8_t reg, uint8_t val)
 {
 	uint8_t tx[3];
 
-	tx[0] = CMD_WRITE;
-	tx[1] = reg;
+	tx[0] = CMD_WRITE << (priv->addr_width + priv->pad - 8) | reg >> (8 - priv->pad);
+	tx[1] = reg << priv->pad;
 	tx[2] = val;
 
-	spi_write_then_read(spi, tx, 3, NULL, 0);
+	spi_write_then_read(priv->spi, tx, 3, NULL, 0);
 }
 
 static int micrel_switch_enable_set(struct param_d *param, void *_priv)
 {
 	struct micrel_switch_priv *priv = _priv;
-	struct spi_device *spi = priv->spi;
 
 	if (priv->p_enable)
-		micrel_switch_write_reg(spi, REG_ID1, 1);
+		micrel_switch_write_reg(priv, REG_ID1, 1);
 	else
-		micrel_switch_write_reg(spi, REG_ID1, 0);
+		micrel_switch_write_reg(priv, REG_ID1, 0);
 
 	return 0;
 }
@@ -84,7 +91,7 @@ static ssize_t micel_switch_read(struct cdev *cdev, void *_buf, size_t count, lo
 	struct micrel_switch_priv *priv = cdev->priv;
 
 	for (i = 0; i < count; i++) {
-		ret = micrel_switch_read_reg(priv->spi, offset);
+		ret = micrel_switch_read_reg(priv, offset);
 		if (ret < 0)
 			return ret;
 		*buf = ret;
@@ -102,7 +109,7 @@ static ssize_t micel_switch_write(struct cdev *cdev, const void *_buf, size_t co
 	struct micrel_switch_priv *priv = cdev->priv;
 
 	for (i = 0; i < count; i++) {
-		micrel_switch_write_reg(priv->spi, offset, *buf);
+		micrel_switch_write_reg(priv, offset, *buf);
 		buf++;
 		offset++;
 	}
@@ -115,10 +122,15 @@ static struct cdev_operations micrel_switch_ops = {
 	.write = micel_switch_write,
 };
 
-static int micrel_switch_probe(struct device_d *dev)
+static int micrel_switch_probe(struct device *dev)
 {
 	struct micrel_switch_priv *priv;
 	int ret = 0;
+	enum ksz_type kind = (enum ksz_type)device_get_match_data(dev);
+	uint8_t id;
+
+	if (kind == unknown)
+		return -ENODEV;
 
 	priv = xzalloc(sizeof(*priv));
 
@@ -128,12 +140,27 @@ static int micrel_switch_probe(struct device_d *dev)
 	priv->spi->mode = SPI_MODE_0;
 	priv->spi->bits_per_word = 8;
 
-	ret = micrel_switch_read_reg(priv->spi, REG_ID0);
+	switch (kind) {
+	case ksz87:
+		priv->addr_width = 12;
+		priv->pad = 1;
+		id = 0x87;
+		break;
+	case ksz88:
+		priv->addr_width = 8;
+		priv->pad = 0;
+		id = 0x95;
+		break;
+	default:
+		return -ENODEV;
+	};
+
+	ret = micrel_switch_read_reg(priv, REG_ID0);
 	if (ret < 0) {
 		dev_err(&priv->spi->dev, "failed to read device id\n");
 		return ret;
 	}
-	if (ret != 0x95) {
+	if (ret != id) {
 		dev_err(&priv->spi->dev, "unknown device id: %02x\n", ret);
 		return -ENODEV;
 	}
@@ -149,13 +176,20 @@ static int micrel_switch_probe(struct device_d *dev)
 			NULL, &priv->p_enable, priv);
 
 	priv->p_enable = 1;
-	micrel_switch_write_reg(priv->spi, REG_ID1, 1);
+	micrel_switch_write_reg(priv, REG_ID1, 1);
 
 	return 0;
 }
 
-static struct driver_d micrel_switch_driver = {
+static const struct platform_device_id ksz_ids[] = {
+	{ .name = "ksz8864rmn", .driver_data = ksz88 },
+	{ .name = "ksz8795", .driver_data = ksz87 },
+	{ }
+};
+
+static struct driver micrel_switch_driver = {
 	.name  = "ksz8864rmn",
 	.probe = micrel_switch_probe,
+	.id_table = ksz_ids,
 };
 device_spi_driver(micrel_switch_driver);