339 goto already;
340 }
341
342 sm->unit_power[unit] += to ? 1 : -1;
343 to = sm->unit_power[unit] ? 1 : 0;
344
345 if (to) {
346 if (gate & (1 << unit))
347 goto already;
348 gate |= (1 << unit);
349 } else {
350 if (!(gate & (1 << unit)))
351 goto already;
352 gate &= ~(1 << unit);
353 }
354
355 switch (mode) {
356 case 1:
357 writel(gate, sm->regs + SM501_POWER_MODE_0_GATE);
358 writel(clock, sm->regs + SM501_POWER_MODE_0_CLOCK);
359 mode = 0;
360 break;
361 case 2:
362 case 0:
363 writel(gate, sm->regs + SM501_POWER_MODE_1_GATE);
364 writel(clock, sm->regs + SM501_POWER_MODE_1_CLOCK);
365 mode = 1;
366 break;
367
368 default:
369 return -1;
370 }
371
372 writel(mode, sm->regs + SM501_POWER_MODE_CONTROL);
373 sm501_sync_regs(sm);
374
375 dev_dbg(sm->dev, "gate %08lx, clock %08lx, mode %08lx\n",
376 gate, clock, mode);
377
378 sm501_mdelay(sm, 16);
379