@@ -524,16 +524,78 @@ static __init char **gpio_mockup_make_line_names(const char *label,
return names;
}
-static int __init gpio_mockup_init(void)
+static int __init gpio_mockup_register_device(struct property_entry *properties)
{
- struct property_entry properties[GPIO_MOCKUP_MAX_PROP];
- char chip_label[GPIO_MOCKUP_LABEL_SIZE];
struct gpio_mockup_device *mockup_dev;
- int i, prop, num_chips, err = 0, base;
struct platform_device_info pdevinfo;
- char **line_names;
+
+ memset(&pdevinfo, 0, sizeof(pdevinfo));
+
+ mockup_dev = kzalloc(sizeof(*mockup_dev), GFP_KERNEL);
+ if (!mockup_dev)
+ return -ENOMEM;
+
+ pdevinfo.name = "gpio-mockup";
+ pdevinfo.properties = properties;
+
+ pdevinfo.id = ida_alloc(&gpio_mockup_ida, GFP_KERNEL);
+ if (pdevinfo.id < 0) {
+ kfree(mockup_dev);
+ return pdevinfo.id;
+ }
+
+ mockup_dev->pdev = platform_device_register_full(&pdevinfo);
+ if (IS_ERR(mockup_dev->pdev)) {
+ ida_free(&gpio_mockup_ida, pdevinfo.id);
+ kfree(mockup_dev);
+ return PTR_ERR(mockup_dev->pdev);
+ }
+
+ list_add(&mockup_dev->list, &gpio_mockup_devices);
+
+ return 0;
+}
+
+static int __init gpio_mockup_register_one_chip_from_params(int idx)
+{
+ char chip_label[GPIO_MOCKUP_LABEL_SIZE], **line_names = NULL;
+ struct property_entry properties[GPIO_MOCKUP_MAX_PROP];
+ int prop = 0, base, ret;
u16 ngpio;
+ memset(properties, 0, sizeof(properties));
+
+ snprintf(chip_label, sizeof(chip_label), "gpio-mockup-%c", idx + 'A');
+ properties[prop++] = PROPERTY_ENTRY_STRING("chip-label",
+ chip_label);
+
+ base = gpio_mockup_range_base(idx);
+ if (base >= 0)
+ properties[prop++] = PROPERTY_ENTRY_U32("gpio-base",
+ base);
+
+ ngpio = base < 0 ? gpio_mockup_range_ngpio(idx)
+ : gpio_mockup_range_ngpio(idx) - base;
+ properties[prop++] = PROPERTY_ENTRY_U16("nr-gpios", ngpio);
+
+ if (gpio_mockup_named_lines) {
+ line_names = gpio_mockup_make_line_names(chip_label, ngpio);
+ if (!line_names)
+ return -ENOMEM;
+
+ properties[prop++] = PROPERTY_ENTRY_STRING_ARRAY_LEN(
+ "gpio-line-names", line_names, ngpio);
+ }
+
+ ret = gpio_mockup_register_device(properties);
+ kfree_strarray(line_names, ngpio);
+ return ret;
+}
+
+static int __init gpio_mockup_register_chips_from_params(void)
+{
+ int num_chips, i, ret;
+
if ((gpio_mockup_num_ranges < 2) ||
(gpio_mockup_num_ranges % 2) ||
(gpio_mockup_num_ranges > GPIO_MOCKUP_MAX_RANGES))
@@ -551,86 +613,39 @@ static int __init gpio_mockup_init(void)
return -EINVAL;
}
- gpio_mockup_dbg_dir = debugfs_create_dir("gpio-mockup", NULL);
-
- err = platform_driver_register(&gpio_mockup_driver);
- if (err) {
- pr_err("error registering platform driver\n");
- debugfs_remove_recursive(gpio_mockup_dbg_dir);
- return err;
- }
-
for (i = 0; i < num_chips; i++) {
- memset(properties, 0, sizeof(properties));
- memset(&pdevinfo, 0, sizeof(pdevinfo));
- prop = 0;
- line_names = NULL;
-
- snprintf(chip_label, sizeof(chip_label),
- "gpio-mockup-%c", i + 'A');
- properties[prop++] = PROPERTY_ENTRY_STRING("chip-label",
- chip_label);
-
- base = gpio_mockup_range_base(i);
- if (base >= 0)
- properties[prop++] = PROPERTY_ENTRY_U32("gpio-base",
- base);
-
- ngpio = base < 0 ? gpio_mockup_range_ngpio(i)
- : gpio_mockup_range_ngpio(i) - base;
- properties[prop++] = PROPERTY_ENTRY_U16("nr-gpios", ngpio);
-
- if (gpio_mockup_named_lines) {
- line_names = gpio_mockup_make_line_names(chip_label,
- ngpio);
- if (!line_names) {
- err = -ENOMEM;
- goto err_out;
- }
-
- properties[prop++] = PROPERTY_ENTRY_STRING_ARRAY_LEN(
- "gpio-line-names",
- line_names, ngpio);
+ ret = gpio_mockup_register_one_chip_from_params(i);
+ if (ret) {
+ gpio_mockup_unregister_devices();
+ return ret;
}
+ }
- pdevinfo.name = "gpio-mockup";
- pdevinfo.properties = properties;
+ return 0;
+}
- pdevinfo.id = ida_alloc(&gpio_mockup_ida, GFP_KERNEL);
- if (pdevinfo.id < 0) {
- kfree_strarray(line_names, ngpio);
- err = pdevinfo.id;
- goto err_out;
- }
+static int __init gpio_mockup_init(void)
+{
+ int ret;
- mockup_dev = kzalloc(sizeof(*mockup_dev), GFP_KERNEL);
- if (!mockup_dev) {
- kfree_strarray(line_names, ngpio);
- ida_free(&gpio_mockup_ida, pdevinfo.id);
- err = -ENOMEM;
- goto err_out;
- }
+ gpio_mockup_dbg_dir = debugfs_create_dir("gpio-mockup", NULL);
- mockup_dev->pdev = platform_device_register_full(&pdevinfo);
- kfree_strarray(line_names, ngpio);
- if (IS_ERR(mockup_dev->pdev)) {
- pr_err("error registering device");
- ida_free(&gpio_mockup_ida, pdevinfo.id);
- kfree(mockup_dev);
- err = PTR_ERR(mockup_dev->pdev);
- goto err_out;
- }
+ ret = platform_driver_register(&gpio_mockup_driver);
+ if (ret) {
+ pr_err("error registering platform driver\n");
+ debugfs_remove_recursive(gpio_mockup_dbg_dir);
+ return ret;
+ }
- list_add(&mockup_dev->list, &gpio_mockup_devices);
+ ret = gpio_mockup_register_chips_from_params();
+ if (ret) {
+ pr_err("error registering device");
+ debugfs_remove_recursive(gpio_mockup_dbg_dir);
+ platform_driver_unregister(&gpio_mockup_driver);
+ return ret;
}
return 0;
-
-err_out:
- platform_driver_unregister(&gpio_mockup_driver);
- gpio_mockup_unregister_devices();
- debugfs_remove_recursive(gpio_mockup_dbg_dir);
- return err;
}
static void __exit gpio_mockup_exit(void)