pktgen: correctly handle failures when adding a device
[linux-2.6.git] / drivers / mfd / anatop-mfd.c
1 /*
2  * Anatop MFD driver
3  *
4  * Copyright (C) 2012 Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>
5  * Copyright (C) 2012 Linaro
6  *
7  *  This program is free software; you can redistribute it and/or modify
8  *  it under the terms of the GNU General Public License as published by
9  *  the Free Software Foundation; either version 2 of the License, or
10  *   (at your option) any later version.
11  *
12  *  This program is distributed in the hope that it will be useful,
13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15  *  GNU General Public License for more details.
16  *
17  *  You should have received a copy of the GNU General Public License along
18  *  with this program; if not, write to the Free Software Foundation, Inc.,
19  *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20  *  This program is free software; you can redistribute it and/or modify
21  *  it under the terms of the GNU General Public License as published by
22  *  the Free Software Foundation; either version 2 of the License, or
23  *  (at your option) any later version.
24  *
25  *  This program is distributed in the hope that it will be useful,
26  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
27  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
28  *  GNU General Public License for more details.
29  *
30  *  You should have received a copy of the GNU General Public License along
31  *  with this program; if not, write to the Free Software Foundation, Inc.,
32  *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
33  *
34  */
35
36 #include <linux/io.h>
37 #include <linux/module.h>
38 #include <linux/platform_device.h>
39 #include <linux/of.h>
40 #include <linux/of_platform.h>
41 #include <linux/of_address.h>
42 #include <linux/mfd/anatop.h>
43
44 u32 anatop_get_bits(struct anatop *adata, u32 addr, int bit_shift,
45                     int bit_width)
46 {
47         u32 val, mask;
48
49         if (bit_width == 32)
50                 mask = ~0;
51         else
52                 mask = (1 << bit_width) - 1;
53
54         val = readl(adata->ioreg + addr);
55         val = (val >> bit_shift) & mask;
56
57         return val;
58 }
59 EXPORT_SYMBOL_GPL(anatop_get_bits);
60
61 void anatop_set_bits(struct anatop *adata, u32 addr, int bit_shift,
62                      int bit_width, u32 data)
63 {
64         u32 val, mask;
65
66         if (bit_width == 32)
67                 mask = ~0;
68         else
69                 mask = (1 << bit_width) - 1;
70
71         spin_lock(&adata->reglock);
72         val = readl(adata->ioreg + addr) & ~(mask << bit_shift);
73         writel((data << bit_shift) | val, adata->ioreg + addr);
74         spin_unlock(&adata->reglock);
75 }
76 EXPORT_SYMBOL_GPL(anatop_set_bits);
77
78 static const struct of_device_id of_anatop_match[] = {
79         { .compatible = "fsl,imx6q-anatop", },
80         { },
81 };
82
83 static int __devinit of_anatop_probe(struct platform_device *pdev)
84 {
85         struct device *dev = &pdev->dev;
86         struct device_node *np = dev->of_node;
87         void *ioreg;
88         struct anatop *drvdata;
89
90         ioreg = of_iomap(np, 0);
91         if (!ioreg)
92                 return -EADDRNOTAVAIL;
93         drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
94         if (!drvdata)
95                 return -ENOMEM;
96         drvdata->ioreg = ioreg;
97         spin_lock_init(&drvdata->reglock);
98         platform_set_drvdata(pdev, drvdata);
99         of_platform_populate(np, of_anatop_match, NULL, dev);
100
101         return 0;
102 }
103
104 static int __devexit of_anatop_remove(struct platform_device *pdev)
105 {
106         struct anatop *drvdata;
107         drvdata = platform_get_drvdata(pdev);
108         iounmap(drvdata->ioreg);
109
110         return 0;
111 }
112
113 static struct platform_driver anatop_of_driver = {
114         .driver = {
115                 .name = "anatop-mfd",
116                 .owner = THIS_MODULE,
117                 .of_match_table = of_anatop_match,
118         },
119         .probe          = of_anatop_probe,
120         .remove         = of_anatop_remove,
121 };
122
123 static int __init anatop_init(void)
124 {
125         return platform_driver_register(&anatop_of_driver);
126 }
127 postcore_initcall(anatop_init);
128
129 static void __exit anatop_exit(void)
130 {
131         platform_driver_unregister(&anatop_of_driver);
132 }
133 module_exit(anatop_exit);
134
135 MODULE_AUTHOR("Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>");
136 MODULE_DESCRIPTION("ANATOP MFD driver");
137 MODULE_LICENSE("GPL v2");