5 files changed, 176 insertions, 10 deletions

diff --git a/drivers/soc/ti/knav_qmss.h b/drivers/soc/ti/knav_qmss.h
index bc9dcc8cc3ce..51da2341280d 100644
--- a/drivers/soc/ti/knav_qmss.h
+++ b/drivers/soc/ti/knav_qmss.h
@@ -348,15 +348,15 @@ struct knav_range_info {
 	list_for_each_entry(region, &kdev->regions, list)
 
 #define first_region(kdev)					\
-	list_first_entry(&kdev->regions, \
-			struct knav_region, list)
+	list_first_entry_or_null(&kdev->regions, \
+				 struct knav_region, list)
 
 #define for_each_queue_range(kdev, range)			\
 	list_for_each_entry(range, &kdev->queue_ranges, list)
 
 #define first_queue_range(kdev)					\
-	list_first_entry(&kdev->queue_ranges, \
-			struct knav_range_info, list)
+	list_first_entry_or_null(&kdev->queue_ranges, \
+				 struct knav_range_info, list)
 
 #define for_each_pool(kdev, pool)				\
 	list_for_each_entry(pool, &kdev->pools, list)
diff --git a/drivers/soc/ti/knav_qmss_queue.c b/drivers/soc/ti/knav_qmss_queue.c
index 0a2c8634c48b..9b8dd6732681 100644
--- a/drivers/soc/ti/knav_qmss_queue.c
+++ b/drivers/soc/ti/knav_qmss_queue.c
@@ -785,7 +785,7 @@ void *knav_pool_create(const char *name,
 		dev_err(kdev->dev, "out of descs in region(%d) for pool(%s)\n",
 			region_id, name);
 		ret = -ENOMEM;
-		goto err;
+		goto err_unlock;
 	}
 
 	/* Region maintains a sorted (by region offset) list of pools
@@ -815,15 +815,16 @@ void *knav_pool_create(const char *name,
 		dev_err(kdev->dev, "pool(%s) create failed: fragmented desc pool in region(%d)\n",
 			name, region_id);
 		ret = -ENOMEM;
-		goto err;
+		goto err_unlock;
 	}
 
 	mutex_unlock(&knav_dev_lock);
 	kdesc_fill_pool(pool);
 	return pool;
 
-err:
+err_unlock:
 	mutex_unlock(&knav_dev_lock);
+err:
 	kfree(pool->name);
 	devm_kfree(kdev->dev, pool);
 	return ERR_PTR(ret);
@@ -1305,14 +1306,14 @@ static void knav_free_queue_ranges(struct knav_device *kdev)
 static void knav_queue_free_regions(struct knav_device *kdev)
 {
 	struct knav_region *region;
-	struct knav_pool *pool;
+	struct knav_pool *pool, *tmp;
 	unsigned size;
 
 	for (;;) {
 		region = first_region(kdev);
 		if (!region)
 			break;
-		list_for_each_entry(pool, &region->pools, region_inst)
+		list_for_each_entry_safe(pool, tmp, &region->pools, region_inst)
 			knav_pool_destroy(pool);
 
 		size = region->virt_end - region->virt_start;
@@ -1639,7 +1640,7 @@ static int knav_queue_init_queues(struct knav_device *kdev)
 	size = (1 << kdev->inst_shift) * kdev->num_queues_in_use;
 	kdev->instances = devm_kzalloc(kdev->dev, size, GFP_KERNEL);
 	if (!kdev->instances)
-		return -1;
+		return -ENOMEM;
 
 	for_each_queue_range(kdev, range) {
 		if (range->ops && range->ops->init_range)
diff --git a/drivers/soc/versatile/Kconfig b/drivers/soc/versatile/Kconfig
index bf5ee9c85330..a928a7fc6be4 100644
--- a/drivers/soc/versatile/Kconfig
+++ b/drivers/soc/versatile/Kconfig
@@ -1,6 +1,15 @@
 #
 # ARM Versatile SoC drivers
 #
+config SOC_INTEGRATOR_CM
+	bool "SoC bus device for the ARM Integrator platform core modules"
+	depends on ARCH_INTEGRATOR
+	select SOC_BUS
+	help
+	  Include support for the SoC bus on the ARM Integrator platform
+	  core modules providing some sysfs information about the ASIC
+	  variant.
+
 config SOC_REALVIEW
 	bool "SoC bus device for the ARM RealView platforms"
 	depends on ARCH_REALVIEW
diff --git a/drivers/soc/versatile/Makefile b/drivers/soc/versatile/Makefile
index ad547435648e..cf612fe3a659 100644
--- a/drivers/soc/versatile/Makefile
+++ b/drivers/soc/versatile/Makefile
@@ -1 +1,2 @@
+obj-$(CONFIG_SOC_INTEGRATOR_CM)	+= soc-integrator.o
 obj-$(CONFIG_SOC_REALVIEW)	+= soc-realview.o
diff --git a/drivers/soc/versatile/soc-integrator.c b/drivers/soc/versatile/soc-integrator.c
new file mode 100644
index 000000000000..a5d7d39ae0ad
--- /dev/null
+++ b/drivers/soc/versatile/soc-integrator.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2014 Linaro Ltd.
+ *
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/sys_soc.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+
+#define INTEGRATOR_HDR_ID_OFFSET	0x00
+
+static u32 integrator_coreid;
+
+static const struct of_device_id integrator_cm_match[] = {
+	{ .compatible = "arm,core-module-integrator", },
+	{ }
+};
+
+static const char *integrator_arch_str(u32 id)
+{
+	switch ((id >> 16) & 0xff) {
+	case 0x00:
+		return "ASB little-endian";
+	case 0x01:
+		return "AHB little-endian";
+	case 0x03:
+		return "AHB-Lite system bus, bi-endian";
+	case 0x04:
+		return "AHB";
+	case 0x08:
+		return "AHB system bus, ASB processor bus";
+	default:
+		return "Unknown";
+	}
+}
+
+static const char *integrator_fpga_str(u32 id)
+{
+	switch ((id >> 12) & 0xf) {
+	case 0x01:
+		return "XC4062";
+	case 0x02:
+		return "XC4085";
+	case 0x03:
+		return "XVC600";
+	case 0x04:
+		return "EPM7256AE (Altera PLD)";
+	default:
+		return "Unknown";
+	}
+}
+
+static ssize_t integrator_get_manf(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	return sprintf(buf, "%02x\n", integrator_coreid >> 24);
+}
+
+static struct device_attribute integrator_manf_attr =
+	__ATTR(manufacturer,  S_IRUGO, integrator_get_manf,  NULL);
+
+static ssize_t integrator_get_arch(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	return sprintf(buf, "%s\n", integrator_arch_str(integrator_coreid));
+}
+
+static struct device_attribute integrator_arch_attr =
+	__ATTR(arch,  S_IRUGO, integrator_get_arch,  NULL);
+
+static ssize_t integrator_get_fpga(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	return sprintf(buf, "%s\n", integrator_fpga_str(integrator_coreid));
+}
+
+static struct device_attribute integrator_fpga_attr =
+	__ATTR(fpga,  S_IRUGO, integrator_get_fpga,  NULL);
+
+static ssize_t integrator_get_build(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	return sprintf(buf, "%02x\n", (integrator_coreid >> 4) & 0xFF);
+}
+
+static struct device_attribute integrator_build_attr =
+	__ATTR(build,  S_IRUGO, integrator_get_build,  NULL);
+
+static int __init integrator_soc_init(void)
+{
+	static struct regmap *syscon_regmap;
+	struct soc_device *soc_dev;
+	struct soc_device_attribute *soc_dev_attr;
+	struct device_node *np;
+	struct device *dev;
+	u32 val;
+	int ret;
+
+	np = of_find_matching_node(NULL, integrator_cm_match);
+	if (!np)
+		return -ENODEV;
+
+	syscon_regmap = syscon_node_to_regmap(np);
+	if (IS_ERR(syscon_regmap))
+		return PTR_ERR(syscon_regmap);
+
+	ret = regmap_read(syscon_regmap, INTEGRATOR_HDR_ID_OFFSET,
+			  &val);
+	if (ret)
+		return -ENODEV;
+	integrator_coreid = val;
+
+	soc_dev_attr = kzalloc(sizeof(*soc_dev_attr), GFP_KERNEL);
+	if (!soc_dev_attr)
+		return -ENOMEM;
+
+	soc_dev_attr->soc_id = "Integrator";
+	soc_dev_attr->machine = "Integrator";
+	soc_dev_attr->family = "Versatile";
+	soc_dev = soc_device_register(soc_dev_attr);
+	if (IS_ERR(soc_dev)) {
+		kfree(soc_dev_attr);
+		return -ENODEV;
+	}
+	dev = soc_device_to_device(soc_dev);
+
+	device_create_file(dev, &integrator_manf_attr);
+	device_create_file(dev, &integrator_arch_attr);
+	device_create_file(dev, &integrator_fpga_attr);
+	device_create_file(dev, &integrator_build_attr);
+
+	dev_info(dev, "Detected ARM core module:\n");
+	dev_info(dev, "    Manufacturer: %02x\n", (val >> 24));
+	dev_info(dev, "    Architecture: %s\n", integrator_arch_str(val));
+	dev_info(dev, "    FPGA: %s\n", integrator_fpga_str(val));
+	dev_info(dev, "    Build: %02x\n", (val >> 4) & 0xFF);
+	dev_info(dev, "    Rev: %c\n", ('A' + (val & 0x03)));
+
+	return 0;
+}
+device_initcall(integrator_soc_init);