2 files changed, 91 insertions, 11 deletions

diff --git a/target/linux/ramips/files/drivers/net/ramips.c b/target/linux/ramips/files/drivers/net/ramips.c
index 812507328f..63570c7914 100644
--- a/target/linux/ramips/files/drivers/net/ramips.c
+++ b/target/linux/ramips/files/drivers/net/ramips.c
@@ -33,6 +33,9 @@
 
 #ifdef CONFIG_RALINK_RT305X
 #include "ramips_esw.c"
+#else
+static inline int rt305x_esw_init(void) { return 0; }
+static inline void rt305x_esw_exit(void) { }
 #endif
 
 #define phys_to_bus(a)  (a & 0x1FFFFFFF)
@@ -507,9 +510,6 @@ ramips_eth_plat_probe(struct platform_device *plat)
 		goto err_free_dev;
 	}
 
-#ifdef CONFIG_RALINK_RT305X
-	rt305x_esw_init();
-#endif
 	printk(KERN_DEBUG "ramips_eth: loaded\n");
 	return 0;
 
@@ -541,10 +541,23 @@ static struct platform_driver ramips_eth_driver = {
 static int __init
 ramips_eth_init(void)
 {
-	int ret = platform_driver_register(&ramips_eth_driver);
+	int ret;
+
+	ret = rt305x_esw_init();
 	if (ret)
+		return ret;
+
+	ret = platform_driver_register(&ramips_eth_driver);
+	if (ret) {
 		printk(KERN_ERR
 		       "ramips_eth: Error registering platfom driver!\n");
+		goto esw_cleanup;
+	}
+
+	return 0;
+
+esw_cleanup:
+	rt305x_esw_exit();
 	return ret;
 }
 
@@ -552,6 +565,7 @@ static void __exit
 ramips_eth_cleanup(void)
 {
 	platform_driver_unregister(&ramips_eth_driver);
+	rt305x_esw_exit();
 }
 
 module_init(ramips_eth_init);
diff --git a/target/linux/ramips/files/drivers/net/ramips_esw.c b/target/linux/ramips/files/drivers/net/ramips_esw.c
index e8041b48df..87479c6544 100644
--- a/target/linux/ramips/files/drivers/net/ramips_esw.c
+++ b/target/linux/ramips/files/drivers/net/ramips_esw.c
@@ -1,5 +1,7 @@
-#include <rt305x.h>
+#include <linux/ioport.h>
+
 #include <rt305x_regs.h>
+#include <rt305x_esw_platform.h>
 
 #define GPIO_PRUPOSE           0x60
 #define GPIO_MDIO_BIT          (1<<7)
@@ -10,10 +12,9 @@
 
 struct rt305x_esw {
 	void __iomem *base;
+	struct rt305x_esw_platform_data *pdata;
 };
 
-static struct rt305x_esw rt305x_esw;
-
 static inline void
 ramips_esw_wr(struct rt305x_esw *esw, u32 val, unsigned reg)
 {
@@ -121,15 +122,80 @@ rt305x_esw_hw_init(struct rt305x_esw *esw)
 }
 
 static int
-rt305x_esw_init(void)
+rt305x_esw_probe(struct platform_device *pdev)
 {
+	struct rt305x_esw_platform_data *pdata;
 	struct rt305x_esw *esw;
+	struct resource *res;
+	int err;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata)
+		return -EINVAL;
 
-	esw = &rt305x_esw;
-	esw->base = ioremap_nocache(RT305X_SWITCH_BASE, PAGE_SIZE);
-	if(!esw->base)
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no memory resource found\n");
 		return -ENOMEM;
+	}
+
+	esw = kzalloc(sizeof (struct rt305x_esw), GFP_KERNEL);
+	if (!esw) {
+		dev_err(&pdev->dev, "no memory for private data\n");
+		return -ENOMEM;
+	}
 
+	esw->base = ioremap(res->start, resource_size(res));
+	if (!esw->base) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		err = -ENOMEM;
+		goto free_esw;
+	}
+
+	platform_set_drvdata(pdev, esw);
+
+	esw->pdata = pdata;
 	rt305x_esw_hw_init(esw);
+
 	return 0;
+
+free_esw:
+	kfree(esw);
+	return err;
+}
+
+static int
+rt305x_esw_remove(struct platform_device *pdev)
+{
+	struct rt305x_esw *esw;
+
+	esw = platform_get_drvdata(pdev);
+	if (esw) {
+		platform_set_drvdata(pdev, NULL);
+		iounmap(esw->base);
+		kfree(esw);
+	}
+
+	return 0;
+}
+
+static struct platform_driver rt305x_esw_driver = {
+	.probe = rt305x_esw_probe,
+	.remove = rt305x_esw_remove,
+	.driver = {
+		.name = "rt305x-esw",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init
+rt305x_esw_init(void)
+{
+	return platform_driver_register(&rt305x_esw_driver);
+}
+
+static void __exit
+rt305x_esw_exit(void)
+{
+	platform_driver_unregister(&rt305x_esw_driver);
 }