Commit | Line | Data |
---|---|---|
b6d6454f BD |
1 | /* linux/drivers/mfd/sm501.c |
2 | * | |
3 | * Copyright (C) 2006 Simtec Electronics | |
4 | * Ben Dooks <ben@simtec.co.uk> | |
5 | * Vincent Sanders <vince@simtec.co.uk> | |
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 version 2 as | |
9 | * published by the Free Software Foundation. | |
10 | * | |
11 | * SM501 MFD driver | |
12 | */ | |
13 | ||
14 | #include <linux/kernel.h> | |
15 | #include <linux/module.h> | |
16 | #include <linux/delay.h> | |
17 | #include <linux/init.h> | |
18 | #include <linux/list.h> | |
19 | #include <linux/device.h> | |
20 | #include <linux/platform_device.h> | |
21 | #include <linux/pci.h> | |
22 | ||
23 | #include <linux/sm501.h> | |
24 | #include <linux/sm501-regs.h> | |
25 | ||
26 | #include <asm/io.h> | |
27 | ||
28 | struct sm501_device { | |
29 | struct list_head list; | |
30 | struct platform_device pdev; | |
31 | }; | |
32 | ||
33 | struct sm501_devdata { | |
34 | spinlock_t reg_lock; | |
35 | struct mutex clock_lock; | |
36 | struct list_head devices; | |
37 | ||
38 | struct device *dev; | |
39 | struct resource *io_res; | |
40 | struct resource *mem_res; | |
41 | struct resource *regs_claim; | |
42 | struct sm501_platdata *platdata; | |
43 | ||
331d7475 BD |
44 | unsigned int in_suspend; |
45 | unsigned long pm_misc; | |
46 | ||
b6d6454f BD |
47 | int unit_power[20]; |
48 | unsigned int pdev_id; | |
49 | unsigned int irq; | |
50 | void __iomem *regs; | |
51 | }; | |
52 | ||
53 | #define MHZ (1000 * 1000) | |
54 | ||
55 | #ifdef DEBUG | |
245904a4 | 56 | static const unsigned int div_tab[] = { |
b6d6454f BD |
57 | [0] = 1, |
58 | [1] = 2, | |
59 | [2] = 4, | |
60 | [3] = 8, | |
61 | [4] = 16, | |
62 | [5] = 32, | |
63 | [6] = 64, | |
64 | [7] = 128, | |
65 | [8] = 3, | |
66 | [9] = 6, | |
67 | [10] = 12, | |
68 | [11] = 24, | |
69 | [12] = 48, | |
70 | [13] = 96, | |
71 | [14] = 192, | |
72 | [15] = 384, | |
73 | [16] = 5, | |
74 | [17] = 10, | |
75 | [18] = 20, | |
76 | [19] = 40, | |
77 | [20] = 80, | |
78 | [21] = 160, | |
79 | [22] = 320, | |
80 | [23] = 604, | |
81 | }; | |
82 | ||
83 | static unsigned long decode_div(unsigned long pll2, unsigned long val, | |
84 | unsigned int lshft, unsigned int selbit, | |
245904a4 | 85 | unsigned long mask) |
b6d6454f BD |
86 | { |
87 | if (val & selbit) | |
88 | pll2 = 288 * MHZ; | |
89 | ||
245904a4 | 90 | return pll2 / div_tab[(val >> lshft) & mask]; |
b6d6454f BD |
91 | } |
92 | ||
93 | #define fmt_freq(x) ((x) / MHZ), ((x) % MHZ), (x) | |
94 | ||
95 | /* sm501_dump_clk | |
96 | * | |
97 | * Print out the current clock configuration for the device | |
98 | */ | |
99 | ||
100 | static void sm501_dump_clk(struct sm501_devdata *sm) | |
101 | { | |
102 | unsigned long misct = readl(sm->regs + SM501_MISC_TIMING); | |
103 | unsigned long pm0 = readl(sm->regs + SM501_POWER_MODE_0_CLOCK); | |
104 | unsigned long pm1 = readl(sm->regs + SM501_POWER_MODE_1_CLOCK); | |
105 | unsigned long pmc = readl(sm->regs + SM501_POWER_MODE_CONTROL); | |
106 | unsigned long sdclk0, sdclk1; | |
107 | unsigned long pll2 = 0; | |
108 | ||
109 | switch (misct & 0x30) { | |
110 | case 0x00: | |
111 | pll2 = 336 * MHZ; | |
112 | break; | |
113 | case 0x10: | |
114 | pll2 = 288 * MHZ; | |
115 | break; | |
116 | case 0x20: | |
117 | pll2 = 240 * MHZ; | |
118 | break; | |
119 | case 0x30: | |
120 | pll2 = 192 * MHZ; | |
121 | break; | |
122 | } | |
123 | ||
124 | sdclk0 = (misct & (1<<12)) ? pll2 : 288 * MHZ; | |
245904a4 | 125 | sdclk0 /= div_tab[((misct >> 8) & 0xf)]; |
b6d6454f BD |
126 | |
127 | sdclk1 = (misct & (1<<20)) ? pll2 : 288 * MHZ; | |
245904a4 | 128 | sdclk1 /= div_tab[((misct >> 16) & 0xf)]; |
b6d6454f BD |
129 | |
130 | dev_dbg(sm->dev, "MISCT=%08lx, PM0=%08lx, PM1=%08lx\n", | |
131 | misct, pm0, pm1); | |
132 | ||
133 | dev_dbg(sm->dev, "PLL2 = %ld.%ld MHz (%ld), SDCLK0=%08lx, SDCLK1=%08lx\n", | |
134 | fmt_freq(pll2), sdclk0, sdclk1); | |
135 | ||
136 | dev_dbg(sm->dev, "SDRAM: PM0=%ld, PM1=%ld\n", sdclk0, sdclk1); | |
137 | ||
138 | dev_dbg(sm->dev, "PM0[%c]: " | |
139 | "P2 %ld.%ld MHz (%ld), V2 %ld.%ld (%ld), " | |
48986f06 | 140 | "M %ld.%ld (%ld), MX1 %ld.%ld (%ld)\n", |
b6d6454f | 141 | (pmc & 3 ) == 0 ? '*' : '-', |
245904a4 VS |
142 | fmt_freq(decode_div(pll2, pm0, 24, 1<<29, 31)), |
143 | fmt_freq(decode_div(pll2, pm0, 16, 1<<20, 15)), | |
144 | fmt_freq(decode_div(pll2, pm0, 8, 1<<12, 15)), | |
145 | fmt_freq(decode_div(pll2, pm0, 0, 1<<4, 15))); | |
b6d6454f BD |
146 | |
147 | dev_dbg(sm->dev, "PM1[%c]: " | |
148 | "P2 %ld.%ld MHz (%ld), V2 %ld.%ld (%ld), " | |
149 | "M %ld.%ld (%ld), MX1 %ld.%ld (%ld)\n", | |
150 | (pmc & 3 ) == 1 ? '*' : '-', | |
245904a4 VS |
151 | fmt_freq(decode_div(pll2, pm1, 24, 1<<29, 31)), |
152 | fmt_freq(decode_div(pll2, pm1, 16, 1<<20, 15)), | |
153 | fmt_freq(decode_div(pll2, pm1, 8, 1<<12, 15)), | |
154 | fmt_freq(decode_div(pll2, pm1, 0, 1<<4, 15))); | |
b6d6454f | 155 | } |
331d7475 BD |
156 | |
157 | static void sm501_dump_regs(struct sm501_devdata *sm) | |
158 | { | |
159 | void __iomem *regs = sm->regs; | |
160 | ||
161 | dev_info(sm->dev, "System Control %08x\n", | |
162 | readl(regs + SM501_SYSTEM_CONTROL)); | |
163 | dev_info(sm->dev, "Misc Control %08x\n", | |
164 | readl(regs + SM501_MISC_CONTROL)); | |
165 | dev_info(sm->dev, "GPIO Control Low %08x\n", | |
166 | readl(regs + SM501_GPIO31_0_CONTROL)); | |
167 | dev_info(sm->dev, "GPIO Control Hi %08x\n", | |
168 | readl(regs + SM501_GPIO63_32_CONTROL)); | |
169 | dev_info(sm->dev, "DRAM Control %08x\n", | |
170 | readl(regs + SM501_DRAM_CONTROL)); | |
171 | dev_info(sm->dev, "Arbitration Ctrl %08x\n", | |
172 | readl(regs + SM501_ARBTRTN_CONTROL)); | |
173 | dev_info(sm->dev, "Misc Timing %08x\n", | |
174 | readl(regs + SM501_MISC_TIMING)); | |
175 | } | |
176 | ||
177 | static void sm501_dump_gate(struct sm501_devdata *sm) | |
b6d6454f | 178 | { |
331d7475 BD |
179 | dev_info(sm->dev, "CurrentGate %08x\n", |
180 | readl(sm->regs + SM501_CURRENT_GATE)); | |
181 | dev_info(sm->dev, "CurrentClock %08x\n", | |
182 | readl(sm->regs + SM501_CURRENT_CLOCK)); | |
183 | dev_info(sm->dev, "PowerModeControl %08x\n", | |
184 | readl(sm->regs + SM501_POWER_MODE_CONTROL)); | |
b6d6454f | 185 | } |
331d7475 BD |
186 | |
187 | #else | |
188 | static inline void sm501_dump_gate(struct sm501_devdata *sm) { } | |
189 | static inline void sm501_dump_regs(struct sm501_devdata *sm) { } | |
190 | static inline void sm501_dump_clk(struct sm501_devdata *sm) { } | |
b6d6454f BD |
191 | #endif |
192 | ||
193 | /* sm501_sync_regs | |
194 | * | |
195 | * ensure the | |
196 | */ | |
197 | ||
198 | static void sm501_sync_regs(struct sm501_devdata *sm) | |
199 | { | |
200 | readl(sm->regs); | |
201 | } | |
202 | ||
331d7475 BD |
203 | static inline void sm501_mdelay(struct sm501_devdata *sm, unsigned int delay) |
204 | { | |
205 | /* during suspend/resume, we are currently not allowed to sleep, | |
206 | * so change to using mdelay() instead of msleep() if we | |
207 | * are in one of these paths */ | |
208 | ||
209 | if (sm->in_suspend) | |
210 | mdelay(delay); | |
211 | else | |
212 | msleep(delay); | |
213 | } | |
214 | ||
b6d6454f BD |
215 | /* sm501_misc_control |
216 | * | |
331d7475 | 217 | * alters the miscellaneous control parameters |
b6d6454f BD |
218 | */ |
219 | ||
220 | int sm501_misc_control(struct device *dev, | |
221 | unsigned long set, unsigned long clear) | |
222 | { | |
223 | struct sm501_devdata *sm = dev_get_drvdata(dev); | |
224 | unsigned long misc; | |
225 | unsigned long save; | |
226 | unsigned long to; | |
227 | ||
228 | spin_lock_irqsave(&sm->reg_lock, save); | |
229 | ||
230 | misc = readl(sm->regs + SM501_MISC_CONTROL); | |
231 | to = (misc & ~clear) | set; | |
232 | ||
233 | if (to != misc) { | |
234 | writel(to, sm->regs + SM501_MISC_CONTROL); | |
235 | sm501_sync_regs(sm); | |
236 | ||
237 | dev_dbg(sm->dev, "MISC_CONTROL %08lx\n", misc); | |
238 | } | |
239 | ||
240 | spin_unlock_irqrestore(&sm->reg_lock, save); | |
241 | return to; | |
242 | } | |
243 | ||
244 | EXPORT_SYMBOL_GPL(sm501_misc_control); | |
245 | ||
246 | /* sm501_modify_reg | |
247 | * | |
248 | * Modify a register in the SM501 which may be shared with other | |
249 | * drivers. | |
250 | */ | |
251 | ||
252 | unsigned long sm501_modify_reg(struct device *dev, | |
253 | unsigned long reg, | |
254 | unsigned long set, | |
255 | unsigned long clear) | |
256 | { | |
257 | struct sm501_devdata *sm = dev_get_drvdata(dev); | |
258 | unsigned long data; | |
259 | unsigned long save; | |
260 | ||
261 | spin_lock_irqsave(&sm->reg_lock, save); | |
262 | ||
263 | data = readl(sm->regs + reg); | |
264 | data |= set; | |
265 | data &= ~clear; | |
266 | ||
267 | writel(data, sm->regs + reg); | |
268 | sm501_sync_regs(sm); | |
269 | ||
270 | spin_unlock_irqrestore(&sm->reg_lock, save); | |
271 | ||
272 | return data; | |
273 | } | |
274 | ||
275 | EXPORT_SYMBOL_GPL(sm501_modify_reg); | |
276 | ||
277 | unsigned long sm501_gpio_get(struct device *dev, | |
278 | unsigned long gpio) | |
279 | { | |
280 | struct sm501_devdata *sm = dev_get_drvdata(dev); | |
281 | unsigned long result; | |
282 | unsigned long reg; | |
283 | ||
284 | reg = (gpio > 32) ? SM501_GPIO_DATA_HIGH : SM501_GPIO_DATA_LOW; | |
285 | result = readl(sm->regs + reg); | |
286 | ||
287 | result >>= (gpio & 31); | |
288 | return result & 1UL; | |
289 | } | |
290 | ||
291 | EXPORT_SYMBOL_GPL(sm501_gpio_get); | |
292 | ||
293 | void sm501_gpio_set(struct device *dev, | |
294 | unsigned long gpio, | |
295 | unsigned int to, | |
296 | unsigned int dir) | |
297 | { | |
298 | struct sm501_devdata *sm = dev_get_drvdata(dev); | |
299 | ||
300 | unsigned long bit = 1 << (gpio & 31); | |
301 | unsigned long base; | |
302 | unsigned long save; | |
303 | unsigned long val; | |
304 | ||
305 | base = (gpio > 32) ? SM501_GPIO_DATA_HIGH : SM501_GPIO_DATA_LOW; | |
306 | base += SM501_GPIO; | |
307 | ||
308 | spin_lock_irqsave(&sm->reg_lock, save); | |
309 | ||
310 | val = readl(sm->regs + base) & ~bit; | |
311 | if (to) | |
312 | val |= bit; | |
313 | writel(val, sm->regs + base); | |
314 | ||
315 | val = readl(sm->regs + SM501_GPIO_DDR_LOW) & ~bit; | |
316 | if (dir) | |
317 | val |= bit; | |
318 | ||
319 | writel(val, sm->regs + SM501_GPIO_DDR_LOW); | |
320 | sm501_sync_regs(sm); | |
321 | ||
322 | spin_unlock_irqrestore(&sm->reg_lock, save); | |
323 | ||
324 | } | |
325 | ||
326 | EXPORT_SYMBOL_GPL(sm501_gpio_set); | |
327 | ||
328 | ||
329 | /* sm501_unit_power | |
330 | * | |
331 | * alters the power active gate to set specific units on or off | |
332 | */ | |
333 | ||
334 | int sm501_unit_power(struct device *dev, unsigned int unit, unsigned int to) | |
335 | { | |
336 | struct sm501_devdata *sm = dev_get_drvdata(dev); | |
337 | unsigned long mode; | |
338 | unsigned long gate; | |
339 | unsigned long clock; | |
340 | ||
341 | mutex_lock(&sm->clock_lock); | |
342 | ||
343 | mode = readl(sm->regs + SM501_POWER_MODE_CONTROL); | |
344 | gate = readl(sm->regs + SM501_CURRENT_GATE); | |
345 | clock = readl(sm->regs + SM501_CURRENT_CLOCK); | |
346 | ||
347 | mode &= 3; /* get current power mode */ | |
348 | ||
bf703c3f | 349 | if (unit >= ARRAY_SIZE(sm->unit_power)) { |
b6d6454f BD |
350 | dev_err(dev, "%s: bad unit %d\n", __FUNCTION__, unit); |
351 | goto already; | |
352 | } | |
353 | ||
354 | dev_dbg(sm->dev, "%s: unit %d, cur %d, to %d\n", __FUNCTION__, unit, | |
355 | sm->unit_power[unit], to); | |
356 | ||
357 | if (to == 0 && sm->unit_power[unit] == 0) { | |
358 | dev_err(sm->dev, "unit %d is already shutdown\n", unit); | |
359 | goto already; | |
360 | } | |
361 | ||
362 | sm->unit_power[unit] += to ? 1 : -1; | |
363 | to = sm->unit_power[unit] ? 1 : 0; | |
364 | ||
365 | if (to) { | |
366 | if (gate & (1 << unit)) | |
367 | goto already; | |
368 | gate |= (1 << unit); | |
369 | } else { | |
370 | if (!(gate & (1 << unit))) | |
371 | goto already; | |
372 | gate &= ~(1 << unit); | |
373 | } | |
374 | ||
375 | switch (mode) { | |
376 | case 1: | |
377 | writel(gate, sm->regs + SM501_POWER_MODE_0_GATE); | |
378 | writel(clock, sm->regs + SM501_POWER_MODE_0_CLOCK); | |
379 | mode = 0; | |
380 | break; | |
381 | case 2: | |
382 | case 0: | |
383 | writel(gate, sm->regs + SM501_POWER_MODE_1_GATE); | |
384 | writel(clock, sm->regs + SM501_POWER_MODE_1_CLOCK); | |
385 | mode = 1; | |
386 | break; | |
387 | ||
388 | default: | |
389 | return -1; | |
390 | } | |
391 | ||
392 | writel(mode, sm->regs + SM501_POWER_MODE_CONTROL); | |
393 | sm501_sync_regs(sm); | |
394 | ||
395 | dev_dbg(sm->dev, "gate %08lx, clock %08lx, mode %08lx\n", | |
396 | gate, clock, mode); | |
397 | ||
331d7475 | 398 | sm501_mdelay(sm, 16); |
b6d6454f BD |
399 | |
400 | already: | |
401 | mutex_unlock(&sm->clock_lock); | |
402 | return gate; | |
403 | } | |
404 | ||
405 | EXPORT_SYMBOL_GPL(sm501_unit_power); | |
406 | ||
407 | ||
408 | /* Perform a rounded division. */ | |
409 | static long sm501fb_round_div(long num, long denom) | |
410 | { | |
411 | /* n / d + 1 / 2 = (2n + d) / 2d */ | |
412 | return (2 * num + denom) / (2 * denom); | |
413 | } | |
414 | ||
415 | /* clock value structure. */ | |
416 | struct sm501_clock { | |
417 | unsigned long mclk; | |
418 | int divider; | |
419 | int shift; | |
420 | }; | |
421 | ||
422 | /* sm501_select_clock | |
423 | * | |
424 | * selects nearest discrete clock frequency the SM501 can achive | |
425 | * the maximum divisor is 3 or 5 | |
426 | */ | |
427 | static unsigned long sm501_select_clock(unsigned long freq, | |
428 | struct sm501_clock *clock, | |
429 | int max_div) | |
430 | { | |
431 | unsigned long mclk; | |
432 | int divider; | |
433 | int shift; | |
434 | long diff; | |
435 | long best_diff = 999999999; | |
436 | ||
437 | /* Try 288MHz and 336MHz clocks. */ | |
438 | for (mclk = 288000000; mclk <= 336000000; mclk += 48000000) { | |
439 | /* try dividers 1 and 3 for CRT and for panel, | |
440 | try divider 5 for panel only.*/ | |
441 | ||
442 | for (divider = 1; divider <= max_div; divider += 2) { | |
443 | /* try all 8 shift values.*/ | |
444 | for (shift = 0; shift < 8; shift++) { | |
445 | /* Calculate difference to requested clock */ | |
446 | diff = sm501fb_round_div(mclk, divider << shift) - freq; | |
447 | if (diff < 0) | |
448 | diff = -diff; | |
449 | ||
450 | /* If it is less than the current, use it */ | |
451 | if (diff < best_diff) { | |
452 | best_diff = diff; | |
453 | ||
454 | clock->mclk = mclk; | |
455 | clock->divider = divider; | |
456 | clock->shift = shift; | |
457 | } | |
458 | } | |
459 | } | |
460 | } | |
461 | ||
462 | /* Return best clock. */ | |
463 | return clock->mclk / (clock->divider << clock->shift); | |
464 | } | |
465 | ||
466 | /* sm501_set_clock | |
467 | * | |
468 | * set one of the four clock sources to the closest available frequency to | |
469 | * the one specified | |
470 | */ | |
471 | ||
472 | unsigned long sm501_set_clock(struct device *dev, | |
473 | int clksrc, | |
474 | unsigned long req_freq) | |
475 | { | |
476 | struct sm501_devdata *sm = dev_get_drvdata(dev); | |
477 | unsigned long mode = readl(sm->regs + SM501_POWER_MODE_CONTROL); | |
478 | unsigned long gate = readl(sm->regs + SM501_CURRENT_GATE); | |
479 | unsigned long clock = readl(sm->regs + SM501_CURRENT_CLOCK); | |
480 | unsigned char reg; | |
481 | unsigned long sm501_freq; /* the actual frequency acheived */ | |
482 | ||
483 | struct sm501_clock to; | |
484 | ||
485 | /* find achivable discrete frequency and setup register value | |
486 | * accordingly, V2XCLK, MCLK and M1XCLK are the same P2XCLK | |
487 | * has an extra bit for the divider */ | |
488 | ||
489 | switch (clksrc) { | |
490 | case SM501_CLOCK_P2XCLK: | |
491 | /* This clock is divided in half so to achive the | |
492 | * requested frequency the value must be multiplied by | |
493 | * 2. This clock also has an additional pre divisor */ | |
494 | ||
495 | sm501_freq = (sm501_select_clock(2 * req_freq, &to, 5) / 2); | |
496 | reg=to.shift & 0x07;/* bottom 3 bits are shift */ | |
497 | if (to.divider == 3) | |
498 | reg |= 0x08; /* /3 divider required */ | |
499 | else if (to.divider == 5) | |
500 | reg |= 0x10; /* /5 divider required */ | |
501 | if (to.mclk != 288000000) | |
502 | reg |= 0x20; /* which mclk pll is source */ | |
503 | break; | |
504 | ||
505 | case SM501_CLOCK_V2XCLK: | |
506 | /* This clock is divided in half so to achive the | |
507 | * requested frequency the value must be multiplied by 2. */ | |
508 | ||
509 | sm501_freq = (sm501_select_clock(2 * req_freq, &to, 3) / 2); | |
510 | reg=to.shift & 0x07; /* bottom 3 bits are shift */ | |
511 | if (to.divider == 3) | |
512 | reg |= 0x08; /* /3 divider required */ | |
513 | if (to.mclk != 288000000) | |
514 | reg |= 0x10; /* which mclk pll is source */ | |
515 | break; | |
516 | ||
517 | case SM501_CLOCK_MCLK: | |
518 | case SM501_CLOCK_M1XCLK: | |
519 | /* These clocks are the same and not further divided */ | |
520 | ||
521 | sm501_freq = sm501_select_clock( req_freq, &to, 3); | |
522 | reg=to.shift & 0x07; /* bottom 3 bits are shift */ | |
523 | if (to.divider == 3) | |
524 | reg |= 0x08; /* /3 divider required */ | |
525 | if (to.mclk != 288000000) | |
526 | reg |= 0x10; /* which mclk pll is source */ | |
527 | break; | |
528 | ||
529 | default: | |
530 | return 0; /* this is bad */ | |
531 | } | |
532 | ||
533 | mutex_lock(&sm->clock_lock); | |
534 | ||
535 | mode = readl(sm->regs + SM501_POWER_MODE_CONTROL); | |
536 | gate = readl(sm->regs + SM501_CURRENT_GATE); | |
537 | clock = readl(sm->regs + SM501_CURRENT_CLOCK); | |
538 | ||
539 | clock = clock & ~(0xFF << clksrc); | |
540 | clock |= reg<<clksrc; | |
541 | ||
542 | mode &= 3; /* find current mode */ | |
543 | ||
544 | switch (mode) { | |
545 | case 1: | |
546 | writel(gate, sm->regs + SM501_POWER_MODE_0_GATE); | |
547 | writel(clock, sm->regs + SM501_POWER_MODE_0_CLOCK); | |
548 | mode = 0; | |
549 | break; | |
550 | case 2: | |
551 | case 0: | |
552 | writel(gate, sm->regs + SM501_POWER_MODE_1_GATE); | |
553 | writel(clock, sm->regs + SM501_POWER_MODE_1_CLOCK); | |
554 | mode = 1; | |
555 | break; | |
556 | ||
557 | default: | |
558 | mutex_unlock(&sm->clock_lock); | |
559 | return -1; | |
560 | } | |
561 | ||
562 | writel(mode, sm->regs + SM501_POWER_MODE_CONTROL); | |
563 | sm501_sync_regs(sm); | |
564 | ||
565 | dev_info(sm->dev, "gate %08lx, clock %08lx, mode %08lx\n", | |
566 | gate, clock, mode); | |
567 | ||
331d7475 | 568 | sm501_mdelay(sm, 16); |
b6d6454f BD |
569 | mutex_unlock(&sm->clock_lock); |
570 | ||
571 | sm501_dump_clk(sm); | |
572 | ||
573 | return sm501_freq; | |
574 | } | |
575 | ||
576 | EXPORT_SYMBOL_GPL(sm501_set_clock); | |
577 | ||
578 | /* sm501_find_clock | |
579 | * | |
580 | * finds the closest available frequency for a given clock | |
581 | */ | |
582 | ||
583 | unsigned long sm501_find_clock(int clksrc, | |
584 | unsigned long req_freq) | |
585 | { | |
586 | unsigned long sm501_freq; /* the frequency achiveable by the 501 */ | |
587 | struct sm501_clock to; | |
588 | ||
589 | switch (clksrc) { | |
590 | case SM501_CLOCK_P2XCLK: | |
591 | sm501_freq = (sm501_select_clock(2 * req_freq, &to, 5) / 2); | |
592 | break; | |
593 | ||
594 | case SM501_CLOCK_V2XCLK: | |
595 | sm501_freq = (sm501_select_clock(2 * req_freq, &to, 3) / 2); | |
596 | break; | |
597 | ||
598 | case SM501_CLOCK_MCLK: | |
599 | case SM501_CLOCK_M1XCLK: | |
600 | sm501_freq = sm501_select_clock(req_freq, &to, 3); | |
601 | break; | |
602 | ||
603 | default: | |
604 | sm501_freq = 0; /* error */ | |
605 | } | |
606 | ||
607 | return sm501_freq; | |
608 | } | |
609 | ||
610 | EXPORT_SYMBOL_GPL(sm501_find_clock); | |
611 | ||
612 | static struct sm501_device *to_sm_device(struct platform_device *pdev) | |
613 | { | |
614 | return container_of(pdev, struct sm501_device, pdev); | |
615 | } | |
616 | ||
617 | /* sm501_device_release | |
618 | * | |
619 | * A release function for the platform devices we create to allow us to | |
620 | * free any items we allocated | |
621 | */ | |
622 | ||
623 | static void sm501_device_release(struct device *dev) | |
624 | { | |
625 | kfree(to_sm_device(to_platform_device(dev))); | |
626 | } | |
627 | ||
628 | /* sm501_create_subdev | |
629 | * | |
630 | * Create a skeleton platform device with resources for passing to a | |
631 | * sub-driver | |
632 | */ | |
633 | ||
634 | static struct platform_device * | |
635 | sm501_create_subdev(struct sm501_devdata *sm, | |
636 | char *name, unsigned int res_count) | |
637 | { | |
638 | struct sm501_device *smdev; | |
639 | ||
640 | smdev = kzalloc(sizeof(struct sm501_device) + | |
641 | sizeof(struct resource) * res_count, GFP_KERNEL); | |
642 | if (!smdev) | |
643 | return NULL; | |
644 | ||
645 | smdev->pdev.dev.release = sm501_device_release; | |
646 | ||
647 | smdev->pdev.name = name; | |
648 | smdev->pdev.id = sm->pdev_id; | |
649 | smdev->pdev.resource = (struct resource *)(smdev+1); | |
650 | smdev->pdev.num_resources = res_count; | |
651 | ||
652 | smdev->pdev.dev.parent = sm->dev; | |
653 | ||
654 | return &smdev->pdev; | |
655 | } | |
656 | ||
657 | /* sm501_register_device | |
658 | * | |
659 | * Register a platform device created with sm501_create_subdev() | |
660 | */ | |
661 | ||
662 | static int sm501_register_device(struct sm501_devdata *sm, | |
663 | struct platform_device *pdev) | |
664 | { | |
665 | struct sm501_device *smdev = to_sm_device(pdev); | |
666 | int ptr; | |
667 | int ret; | |
668 | ||
669 | for (ptr = 0; ptr < pdev->num_resources; ptr++) { | |
670 | printk("%s[%d] flags %08lx: %08llx..%08llx\n", | |
671 | pdev->name, ptr, | |
672 | pdev->resource[ptr].flags, | |
673 | (unsigned long long)pdev->resource[ptr].start, | |
674 | (unsigned long long)pdev->resource[ptr].end); | |
675 | } | |
676 | ||
677 | ret = platform_device_register(pdev); | |
678 | ||
679 | if (ret >= 0) { | |
680 | dev_dbg(sm->dev, "registered %s\n", pdev->name); | |
681 | list_add_tail(&smdev->list, &sm->devices); | |
682 | } else | |
683 | dev_err(sm->dev, "error registering %s (%d)\n", | |
684 | pdev->name, ret); | |
685 | ||
686 | return ret; | |
687 | } | |
688 | ||
689 | /* sm501_create_subio | |
690 | * | |
691 | * Fill in an IO resource for a sub device | |
692 | */ | |
693 | ||
694 | static void sm501_create_subio(struct sm501_devdata *sm, | |
695 | struct resource *res, | |
696 | resource_size_t offs, | |
697 | resource_size_t size) | |
698 | { | |
699 | res->flags = IORESOURCE_MEM; | |
700 | res->parent = sm->io_res; | |
701 | res->start = sm->io_res->start + offs; | |
702 | res->end = res->start + size - 1; | |
703 | } | |
704 | ||
705 | /* sm501_create_mem | |
706 | * | |
707 | * Fill in an MEM resource for a sub device | |
708 | */ | |
709 | ||
710 | static void sm501_create_mem(struct sm501_devdata *sm, | |
711 | struct resource *res, | |
712 | resource_size_t *offs, | |
713 | resource_size_t size) | |
714 | { | |
715 | *offs -= size; /* adjust memory size */ | |
716 | ||
717 | res->flags = IORESOURCE_MEM; | |
718 | res->parent = sm->mem_res; | |
719 | res->start = sm->mem_res->start + *offs; | |
720 | res->end = res->start + size - 1; | |
721 | } | |
722 | ||
723 | /* sm501_create_irq | |
724 | * | |
725 | * Fill in an IRQ resource for a sub device | |
726 | */ | |
727 | ||
728 | static void sm501_create_irq(struct sm501_devdata *sm, | |
729 | struct resource *res) | |
730 | { | |
731 | res->flags = IORESOURCE_IRQ; | |
732 | res->parent = NULL; | |
733 | res->start = res->end = sm->irq; | |
734 | } | |
735 | ||
736 | static int sm501_register_usbhost(struct sm501_devdata *sm, | |
737 | resource_size_t *mem_avail) | |
738 | { | |
739 | struct platform_device *pdev; | |
740 | ||
741 | pdev = sm501_create_subdev(sm, "sm501-usb", 3); | |
742 | if (!pdev) | |
743 | return -ENOMEM; | |
744 | ||
745 | sm501_create_subio(sm, &pdev->resource[0], 0x40000, 0x20000); | |
746 | sm501_create_mem(sm, &pdev->resource[1], mem_avail, 256*1024); | |
747 | sm501_create_irq(sm, &pdev->resource[2]); | |
748 | ||
749 | return sm501_register_device(sm, pdev); | |
750 | } | |
751 | ||
752 | static int sm501_register_display(struct sm501_devdata *sm, | |
753 | resource_size_t *mem_avail) | |
754 | { | |
755 | struct platform_device *pdev; | |
756 | ||
757 | pdev = sm501_create_subdev(sm, "sm501-fb", 4); | |
758 | if (!pdev) | |
759 | return -ENOMEM; | |
760 | ||
761 | sm501_create_subio(sm, &pdev->resource[0], 0x80000, 0x10000); | |
762 | sm501_create_subio(sm, &pdev->resource[1], 0x100000, 0x50000); | |
763 | sm501_create_mem(sm, &pdev->resource[2], mem_avail, *mem_avail); | |
764 | sm501_create_irq(sm, &pdev->resource[3]); | |
765 | ||
766 | return sm501_register_device(sm, pdev); | |
767 | } | |
768 | ||
769 | /* sm501_dbg_regs | |
770 | * | |
771 | * Debug attribute to attach to parent device to show core registers | |
772 | */ | |
773 | ||
774 | static ssize_t sm501_dbg_regs(struct device *dev, | |
775 | struct device_attribute *attr, char *buff) | |
776 | { | |
777 | struct sm501_devdata *sm = dev_get_drvdata(dev) ; | |
778 | unsigned int reg; | |
779 | char *ptr = buff; | |
780 | int ret; | |
781 | ||
782 | for (reg = 0x00; reg < 0x70; reg += 4) { | |
783 | ret = sprintf(ptr, "%08x = %08x\n", | |
784 | reg, readl(sm->regs + reg)); | |
785 | ptr += ret; | |
786 | } | |
787 | ||
788 | return ptr - buff; | |
789 | } | |
790 | ||
791 | ||
792 | static DEVICE_ATTR(dbg_regs, 0666, sm501_dbg_regs, NULL); | |
793 | ||
794 | /* sm501_init_reg | |
795 | * | |
796 | * Helper function for the init code to setup a register | |
5136237b BD |
797 | * |
798 | * clear the bits which are set in r->mask, and then set | |
799 | * the bits set in r->set. | |
b6d6454f BD |
800 | */ |
801 | ||
802 | static inline void sm501_init_reg(struct sm501_devdata *sm, | |
803 | unsigned long reg, | |
804 | struct sm501_reg_init *r) | |
805 | { | |
806 | unsigned long tmp; | |
807 | ||
808 | tmp = readl(sm->regs + reg); | |
b6d6454f | 809 | tmp &= ~r->mask; |
5136237b | 810 | tmp |= r->set; |
b6d6454f BD |
811 | writel(tmp, sm->regs + reg); |
812 | } | |
813 | ||
814 | /* sm501_init_regs | |
815 | * | |
816 | * Setup core register values | |
817 | */ | |
818 | ||
819 | static void sm501_init_regs(struct sm501_devdata *sm, | |
820 | struct sm501_initdata *init) | |
821 | { | |
822 | sm501_misc_control(sm->dev, | |
823 | init->misc_control.set, | |
824 | init->misc_control.mask); | |
825 | ||
826 | sm501_init_reg(sm, SM501_MISC_TIMING, &init->misc_timing); | |
827 | sm501_init_reg(sm, SM501_GPIO31_0_CONTROL, &init->gpio_low); | |
828 | sm501_init_reg(sm, SM501_GPIO63_32_CONTROL, &init->gpio_high); | |
829 | ||
b6d6454f BD |
830 | if (init->m1xclk) { |
831 | dev_info(sm->dev, "setting M1XCLK to %ld\n", init->m1xclk); | |
832 | sm501_set_clock(sm->dev, SM501_CLOCK_M1XCLK, init->m1xclk); | |
833 | } | |
b5913bbd BD |
834 | |
835 | if (init->mclk) { | |
836 | dev_info(sm->dev, "setting MCLK to %ld\n", init->mclk); | |
837 | sm501_set_clock(sm->dev, SM501_CLOCK_MCLK, init->mclk); | |
838 | } | |
81906221 BD |
839 | |
840 | } | |
841 | ||
842 | /* Check the PLL sources for the M1CLK and M1XCLK | |
843 | * | |
844 | * If the M1CLK and M1XCLKs are not sourced from the same PLL, then | |
845 | * there is a risk (see errata AB-5) that the SM501 will cease proper | |
846 | * function. If this happens, then it is likely the SM501 will | |
847 | * hang the system. | |
848 | */ | |
849 | ||
850 | static int sm501_check_clocks(struct sm501_devdata *sm) | |
851 | { | |
852 | unsigned long pwrmode = readl(sm->regs + SM501_CURRENT_CLOCK); | |
853 | unsigned long msrc = (pwrmode & SM501_POWERMODE_M_SRC); | |
854 | unsigned long m1src = (pwrmode & SM501_POWERMODE_M1_SRC); | |
855 | ||
856 | return ((msrc == 0 && m1src != 0) || (msrc != 0 && m1src == 0)); | |
b6d6454f BD |
857 | } |
858 | ||
859 | static unsigned int sm501_mem_local[] = { | |
860 | [0] = 4*1024*1024, | |
861 | [1] = 8*1024*1024, | |
862 | [2] = 16*1024*1024, | |
863 | [3] = 32*1024*1024, | |
864 | [4] = 64*1024*1024, | |
865 | [5] = 2*1024*1024, | |
866 | }; | |
867 | ||
868 | /* sm501_init_dev | |
869 | * | |
870 | * Common init code for an SM501 | |
871 | */ | |
872 | ||
873 | static int sm501_init_dev(struct sm501_devdata *sm) | |
874 | { | |
875 | resource_size_t mem_avail; | |
876 | unsigned long dramctrl; | |
1e27dbe7 | 877 | unsigned long devid; |
b6d6454f BD |
878 | int ret; |
879 | ||
880 | mutex_init(&sm->clock_lock); | |
881 | spin_lock_init(&sm->reg_lock); | |
882 | ||
883 | INIT_LIST_HEAD(&sm->devices); | |
884 | ||
1e27dbe7 | 885 | devid = readl(sm->regs + SM501_DEVICEID); |
b6d6454f | 886 | |
1e27dbe7 BD |
887 | if ((devid & SM501_DEVICEID_IDMASK) != SM501_DEVICEID_SM501) { |
888 | dev_err(sm->dev, "incorrect device id %08lx\n", devid); | |
889 | return -EINVAL; | |
890 | } | |
891 | ||
892 | dramctrl = readl(sm->regs + SM501_DRAM_CONTROL); | |
b6d6454f BD |
893 | mem_avail = sm501_mem_local[(dramctrl >> 13) & 0x7]; |
894 | ||
1e27dbe7 BD |
895 | dev_info(sm->dev, "SM501 At %p: Version %08lx, %ld Mb, IRQ %d\n", |
896 | sm->regs, devid, (unsigned long)mem_avail >> 20, sm->irq); | |
b6d6454f | 897 | |
331d7475 | 898 | sm501_dump_gate(sm); |
b6d6454f BD |
899 | |
900 | ret = device_create_file(sm->dev, &dev_attr_dbg_regs); | |
901 | if (ret) | |
902 | dev_err(sm->dev, "failed to create debug regs file\n"); | |
903 | ||
904 | sm501_dump_clk(sm); | |
905 | ||
906 | /* check to see if we have some device initialisation */ | |
907 | ||
908 | if (sm->platdata) { | |
909 | struct sm501_platdata *pdata = sm->platdata; | |
910 | ||
911 | if (pdata->init) { | |
912 | sm501_init_regs(sm, sm->platdata->init); | |
913 | ||
914 | if (pdata->init->devices & SM501_USE_USB_HOST) | |
915 | sm501_register_usbhost(sm, &mem_avail); | |
916 | } | |
917 | } | |
918 | ||
81906221 BD |
919 | ret = sm501_check_clocks(sm); |
920 | if (ret) { | |
921 | dev_err(sm->dev, "M1X and M clocks sourced from different " | |
922 | "PLLs\n"); | |
923 | return -EINVAL; | |
924 | } | |
925 | ||
b6d6454f BD |
926 | /* always create a framebuffer */ |
927 | sm501_register_display(sm, &mem_avail); | |
928 | ||
929 | return 0; | |
930 | } | |
931 | ||
932 | static int sm501_plat_probe(struct platform_device *dev) | |
933 | { | |
934 | struct sm501_devdata *sm; | |
935 | int err; | |
936 | ||
937 | sm = kzalloc(sizeof(struct sm501_devdata), GFP_KERNEL); | |
938 | if (sm == NULL) { | |
939 | dev_err(&dev->dev, "no memory for device data\n"); | |
940 | err = -ENOMEM; | |
941 | goto err1; | |
942 | } | |
943 | ||
944 | sm->dev = &dev->dev; | |
945 | sm->pdev_id = dev->id; | |
946 | sm->irq = platform_get_irq(dev, 0); | |
947 | sm->io_res = platform_get_resource(dev, IORESOURCE_MEM, 1); | |
948 | sm->mem_res = platform_get_resource(dev, IORESOURCE_MEM, 0); | |
949 | sm->platdata = dev->dev.platform_data; | |
950 | ||
951 | if (sm->irq < 0) { | |
952 | dev_err(&dev->dev, "failed to get irq resource\n"); | |
953 | err = sm->irq; | |
954 | goto err_res; | |
955 | } | |
956 | ||
957 | if (sm->io_res == NULL || sm->mem_res == NULL) { | |
958 | dev_err(&dev->dev, "failed to get IO resource\n"); | |
959 | err = -ENOENT; | |
960 | goto err_res; | |
961 | } | |
962 | ||
963 | sm->regs_claim = request_mem_region(sm->io_res->start, | |
964 | 0x100, "sm501"); | |
965 | ||
966 | if (sm->regs_claim == NULL) { | |
967 | dev_err(&dev->dev, "cannot claim registers\n"); | |
968 | err= -EBUSY; | |
969 | goto err_res; | |
970 | } | |
971 | ||
972 | platform_set_drvdata(dev, sm); | |
973 | ||
974 | sm->regs = ioremap(sm->io_res->start, | |
975 | (sm->io_res->end - sm->io_res->start) - 1); | |
976 | ||
977 | if (sm->regs == NULL) { | |
978 | dev_err(&dev->dev, "cannot remap registers\n"); | |
979 | err = -EIO; | |
980 | goto err_claim; | |
981 | } | |
982 | ||
983 | return sm501_init_dev(sm); | |
984 | ||
985 | err_claim: | |
986 | release_resource(sm->regs_claim); | |
987 | kfree(sm->regs_claim); | |
988 | err_res: | |
989 | kfree(sm); | |
990 | err1: | |
991 | return err; | |
992 | ||
993 | } | |
994 | ||
331d7475 BD |
995 | #ifdef CONFIG_PM |
996 | /* power management support */ | |
997 | ||
998 | static int sm501_plat_suspend(struct platform_device *pdev, pm_message_t state) | |
999 | { | |
1000 | struct sm501_devdata *sm = platform_get_drvdata(pdev); | |
1001 | ||
1002 | sm->in_suspend = 1; | |
1003 | sm->pm_misc = readl(sm->regs + SM501_MISC_CONTROL); | |
1004 | ||
1005 | sm501_dump_regs(sm); | |
1006 | return 0; | |
1007 | } | |
1008 | ||
1009 | static int sm501_plat_resume(struct platform_device *pdev) | |
1010 | { | |
1011 | struct sm501_devdata *sm = platform_get_drvdata(pdev); | |
1012 | ||
1013 | sm501_dump_regs(sm); | |
1014 | sm501_dump_gate(sm); | |
1015 | sm501_dump_clk(sm); | |
1016 | ||
1017 | /* check to see if we are in the same state as when suspended */ | |
1018 | ||
1019 | if (readl(sm->regs + SM501_MISC_CONTROL) != sm->pm_misc) { | |
1020 | dev_info(sm->dev, "SM501_MISC_CONTROL changed over sleep\n"); | |
1021 | writel(sm->pm_misc, sm->regs + SM501_MISC_CONTROL); | |
1022 | ||
1023 | /* our suspend causes the controller state to change, | |
1024 | * either by something attempting setup, power loss, | |
1025 | * or an external reset event on power change */ | |
1026 | ||
1027 | if (sm->platdata && sm->platdata->init) { | |
1028 | sm501_init_regs(sm, sm->platdata->init); | |
1029 | } | |
1030 | } | |
1031 | ||
1032 | /* dump our state from resume */ | |
1033 | ||
1034 | sm501_dump_regs(sm); | |
1035 | sm501_dump_clk(sm); | |
1036 | ||
1037 | sm->in_suspend = 0; | |
1038 | ||
1039 | return 0; | |
1040 | } | |
1041 | #else | |
1042 | #define sm501_plat_suspend NULL | |
1043 | #define sm501_plat_resume NULL | |
1044 | #endif | |
1045 | ||
b6d6454f BD |
1046 | /* Initialisation data for PCI devices */ |
1047 | ||
1048 | static struct sm501_initdata sm501_pci_initdata = { | |
1049 | .gpio_high = { | |
1050 | .set = 0x3F000000, /* 24bit panel */ | |
1051 | .mask = 0x0, | |
1052 | }, | |
1053 | .misc_timing = { | |
1054 | .set = 0x010100, /* SDRAM timing */ | |
1055 | .mask = 0x1F1F00, | |
1056 | }, | |
1057 | .misc_control = { | |
1058 | .set = SM501_MISC_PNL_24BIT, | |
1059 | .mask = 0, | |
1060 | }, | |
1061 | ||
1062 | .devices = SM501_USE_ALL, | |
81906221 BD |
1063 | |
1064 | /* Errata AB-3 says that 72MHz is the fastest available | |
1065 | * for 33MHZ PCI with proper bus-mastering operation */ | |
1066 | ||
1067 | .mclk = 72 * MHZ, | |
1068 | .m1xclk = 144 * MHZ, | |
b6d6454f BD |
1069 | }; |
1070 | ||
1071 | static struct sm501_platdata_fbsub sm501_pdata_fbsub = { | |
1072 | .flags = (SM501FB_FLAG_USE_INIT_MODE | | |
1073 | SM501FB_FLAG_USE_HWCURSOR | | |
1074 | SM501FB_FLAG_USE_HWACCEL | | |
1075 | SM501FB_FLAG_DISABLE_AT_EXIT), | |
1076 | }; | |
1077 | ||
1078 | static struct sm501_platdata_fb sm501_fb_pdata = { | |
1079 | .fb_route = SM501_FB_OWN, | |
1080 | .fb_crt = &sm501_pdata_fbsub, | |
1081 | .fb_pnl = &sm501_pdata_fbsub, | |
1082 | }; | |
1083 | ||
1084 | static struct sm501_platdata sm501_pci_platdata = { | |
1085 | .init = &sm501_pci_initdata, | |
1086 | .fb = &sm501_fb_pdata, | |
1087 | }; | |
1088 | ||
1089 | static int sm501_pci_probe(struct pci_dev *dev, | |
1090 | const struct pci_device_id *id) | |
1091 | { | |
1092 | struct sm501_devdata *sm; | |
1093 | int err; | |
1094 | ||
1095 | sm = kzalloc(sizeof(struct sm501_devdata), GFP_KERNEL); | |
1096 | if (sm == NULL) { | |
1097 | dev_err(&dev->dev, "no memory for device data\n"); | |
1098 | err = -ENOMEM; | |
1099 | goto err1; | |
1100 | } | |
1101 | ||
1102 | /* set a default set of platform data */ | |
1103 | dev->dev.platform_data = sm->platdata = &sm501_pci_platdata; | |
1104 | ||
1105 | /* set a hopefully unique id for our child platform devices */ | |
1106 | sm->pdev_id = 32 + dev->devfn; | |
1107 | ||
1108 | pci_set_drvdata(dev, sm); | |
1109 | ||
1110 | err = pci_enable_device(dev); | |
1111 | if (err) { | |
1112 | dev_err(&dev->dev, "cannot enable device\n"); | |
1113 | goto err2; | |
1114 | } | |
1115 | ||
1116 | sm->dev = &dev->dev; | |
1117 | sm->irq = dev->irq; | |
1118 | ||
1119 | #ifdef __BIG_ENDIAN | |
1120 | /* if the system is big-endian, we most probably have a | |
1121 | * translation in the IO layer making the PCI bus little endian | |
1122 | * so make the framebuffer swapped pixels */ | |
1123 | ||
1124 | sm501_fb_pdata.flags |= SM501_FBPD_SWAP_FB_ENDIAN; | |
1125 | #endif | |
1126 | ||
1127 | /* check our resources */ | |
1128 | ||
1129 | if (!(pci_resource_flags(dev, 0) & IORESOURCE_MEM)) { | |
1130 | dev_err(&dev->dev, "region #0 is not memory?\n"); | |
1131 | err = -EINVAL; | |
1132 | goto err3; | |
1133 | } | |
1134 | ||
1135 | if (!(pci_resource_flags(dev, 1) & IORESOURCE_MEM)) { | |
1136 | dev_err(&dev->dev, "region #1 is not memory?\n"); | |
1137 | err = -EINVAL; | |
1138 | goto err3; | |
1139 | } | |
1140 | ||
1141 | /* make our resources ready for sharing */ | |
1142 | ||
1143 | sm->io_res = &dev->resource[1]; | |
1144 | sm->mem_res = &dev->resource[0]; | |
1145 | ||
1146 | sm->regs_claim = request_mem_region(sm->io_res->start, | |
1147 | 0x100, "sm501"); | |
1148 | if (sm->regs_claim == NULL) { | |
1149 | dev_err(&dev->dev, "cannot claim registers\n"); | |
1150 | err= -EBUSY; | |
1151 | goto err3; | |
1152 | } | |
1153 | ||
1154 | sm->regs = ioremap(pci_resource_start(dev, 1), | |
1155 | pci_resource_len(dev, 1)); | |
1156 | ||
1157 | if (sm->regs == NULL) { | |
1158 | dev_err(&dev->dev, "cannot remap registers\n"); | |
1159 | err = -EIO; | |
1160 | goto err4; | |
1161 | } | |
1162 | ||
1163 | sm501_init_dev(sm); | |
1164 | return 0; | |
1165 | ||
1166 | err4: | |
1167 | release_resource(sm->regs_claim); | |
1168 | kfree(sm->regs_claim); | |
1169 | err3: | |
1170 | pci_disable_device(dev); | |
1171 | err2: | |
1172 | pci_set_drvdata(dev, NULL); | |
1173 | kfree(sm); | |
1174 | err1: | |
1175 | return err; | |
1176 | } | |
1177 | ||
1178 | static void sm501_remove_sub(struct sm501_devdata *sm, | |
1179 | struct sm501_device *smdev) | |
1180 | { | |
1181 | list_del(&smdev->list); | |
1182 | platform_device_unregister(&smdev->pdev); | |
1183 | } | |
1184 | ||
1185 | static void sm501_dev_remove(struct sm501_devdata *sm) | |
1186 | { | |
1187 | struct sm501_device *smdev, *tmp; | |
1188 | ||
1189 | list_for_each_entry_safe(smdev, tmp, &sm->devices, list) | |
1190 | sm501_remove_sub(sm, smdev); | |
1191 | ||
1192 | device_remove_file(sm->dev, &dev_attr_dbg_regs); | |
1193 | } | |
1194 | ||
1195 | static void sm501_pci_remove(struct pci_dev *dev) | |
1196 | { | |
1197 | struct sm501_devdata *sm = pci_get_drvdata(dev); | |
1198 | ||
1199 | sm501_dev_remove(sm); | |
1200 | iounmap(sm->regs); | |
1201 | ||
1202 | release_resource(sm->regs_claim); | |
1203 | kfree(sm->regs_claim); | |
1204 | ||
1205 | pci_set_drvdata(dev, NULL); | |
1206 | pci_disable_device(dev); | |
1207 | } | |
1208 | ||
1209 | static int sm501_plat_remove(struct platform_device *dev) | |
1210 | { | |
1211 | struct sm501_devdata *sm = platform_get_drvdata(dev); | |
1212 | ||
1213 | sm501_dev_remove(sm); | |
1214 | iounmap(sm->regs); | |
1215 | ||
1216 | release_resource(sm->regs_claim); | |
1217 | kfree(sm->regs_claim); | |
1218 | ||
1219 | return 0; | |
1220 | } | |
1221 | ||
1222 | static struct pci_device_id sm501_pci_tbl[] = { | |
1223 | { 0x126f, 0x0501, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 }, | |
1224 | { 0, }, | |
1225 | }; | |
1226 | ||
1227 | MODULE_DEVICE_TABLE(pci, sm501_pci_tbl); | |
1228 | ||
1229 | static struct pci_driver sm501_pci_drv = { | |
1230 | .name = "sm501", | |
1231 | .id_table = sm501_pci_tbl, | |
1232 | .probe = sm501_pci_probe, | |
1233 | .remove = sm501_pci_remove, | |
1234 | }; | |
1235 | ||
1236 | static struct platform_driver sm501_plat_drv = { | |
1237 | .driver = { | |
1238 | .name = "sm501", | |
1239 | .owner = THIS_MODULE, | |
1240 | }, | |
1241 | .probe = sm501_plat_probe, | |
1242 | .remove = sm501_plat_remove, | |
331d7475 BD |
1243 | .suspend = sm501_plat_suspend, |
1244 | .resume = sm501_plat_resume, | |
b6d6454f BD |
1245 | }; |
1246 | ||
1247 | static int __init sm501_base_init(void) | |
1248 | { | |
1249 | platform_driver_register(&sm501_plat_drv); | |
f15e66b9 | 1250 | return pci_register_driver(&sm501_pci_drv); |
b6d6454f BD |
1251 | } |
1252 | ||
1253 | static void __exit sm501_base_exit(void) | |
1254 | { | |
1255 | platform_driver_unregister(&sm501_plat_drv); | |
1256 | pci_unregister_driver(&sm501_pci_drv); | |
1257 | } | |
1258 | ||
1259 | module_init(sm501_base_init); | |
1260 | module_exit(sm501_base_exit); | |
1261 | ||
1262 | MODULE_DESCRIPTION("SM501 Core Driver"); | |
1263 | MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>, Vincent Sanders"); | |
1264 | MODULE_LICENSE("GPL v2"); |