1/*
2 * fix_standby.c  by Mark Lord, March 2006.
3 *
4 * This frees a drive from the "power up in standby" condition.
5 * It tries to be safe, but is still quite risky when run
6 * on a preemptive kernel, because it could preempt something
7 * that was in the middle of issuing an IDE/SATA command.
8 *
9 * Hardcoded to work only on the first (master) drive
10 * on the first (primary) IDE/SATA interface.
11 */
12#include <stdio.h>
13#include <errno.h>
14#include <stdlib.h>
15#include <unistd.h>
16#include <sys/io.h>
17
18typedef unsigned char  u8;
19typedef unsigned short u16;
20
21enum {
22	BUSY_STAT	= 0x80,
23	READY_STAT	= 0x40,
24	WRERR_STAT	= 0x20,
25	SEEK_STAT	= 0x10,
26	DRQ_STAT	= 0x08,
27	ECC_STAT	= 0x04,
28	INDEX_STAT	= 0x02,
29	ERR_STAT	= 0x01,
30};
31
32enum {
33	CHECKPOWERMODE	= 0xE5,
34	SETFEATURES	= 0xEF,
35	SF_SPINUP_NOW	= 0x07,
36	SF_NO_STANDBY	= 0x86,
37};
38
39static unsigned int ide_base, ide_control, ide_altstatus;
40
41enum {
42	ide_data	= 0,
43	ide_feature	= 1,
44	ide_error	= ide_feature,
45	ide_nsectors	= 2,
46	ide_lbal	= 3,
47	ide_lbam	= 4,
48	ide_lbah	= 5,
49	ide_select	= 6,
50	ide_command	= 7,
51	ide_status	= ide_command,
52};
53
54static inline void
55cli (void)
56{
57	__asm__ __volatile__("cli" : : : "memory");
58}
59
60static inline void
61sti (void)
62{
63	__asm__ __volatile__("sti" : : : "memory");
64}
65
66static void
67wait_400ns (void)
68{
69	inb(ide_altstatus);
70	inb(ide_altstatus);
71	inb(ide_altstatus);
72	inb(ide_altstatus);
73	inb(ide_altstatus);
74}
75
76static inline void
77OUTB (u8 val, unsigned int reg)
78{
79	outb(val, ide_base + reg);
80}
81
82static inline u8
83INB (unsigned int reg)
84{
85	return inb(ide_base + reg);
86}
87
88static inline u16
89INW (unsigned int reg)
90{
91	return inw(ide_base + reg);
92}
93
94static inline void
95OUTW (u16 val, unsigned int reg)
96{
97	outw(val, ide_base + reg);
98}
99
100static void
101dump_regs (void)
102{
103	printf("ide_data      = 0x%04x\n", INW(ide_data));
104	printf("ide_error     = 0x%02x\n", INB(ide_error));
105	printf("ide_nsectors  = 0x%02x\n", INB(ide_nsectors));
106	printf("ide_lbal      = 0x%02x\n", INB(ide_lbal));
107	printf("ide_lbam      = 0x%02x\n", INB(ide_lbam));
108	printf("ide_lbah      = 0x%02x\n", INB(ide_lbah));
109	printf("ide_select    = 0x%02x\n", INB(ide_select));
110	printf("ide_status    = 0x%02x\n", INB(ide_status));
111	printf("ide_altstatus = 0x%02x\n", INB(ide_altstatus));
112}
113
114static int
115wait_for_status (u8 ones, u8 zeros, u8 *stat_r, unsigned int timeout)
116{
117	u8 stat;
118	int result;
119
120	wait_400ns();
121	timeout *= 1000000;
122	do {
123		stat = INB(ide_status);
124		result = (stat & (ones|zeros)) != ones;
125	} while (result && --timeout);
126	if (stat_r)
127		*stat_r = stat;
128	return result ? EIO : 0;
129}
130
131static int
132require_status (u8 ones, u8 zeros, u8 *stat_r, unsigned int timeout, const char *msg)
133{
134	int result = 0;
135	u8 stat;
136
137	if (wait_for_status(ones, zeros, &stat, timeout)) {
138		fprintf(stderr, "status timeout: %s, stat=0x%02x\n", msg, stat);
139		dump_regs();
140		result = EIO;
141	}
142	if (stat_r)
143		*stat_r = stat;
144	return result;
145}
146
147static int
148do_drive_select (int master_slave)
149{
150	if (require_status(READY_STAT, BUSY_STAT|DRQ_STAT, NULL, 1, "do_drive_select1"))
151		return EIO;
152	OUTB(0xe0 | ((master_slave & 1) << 4), ide_select);
153	if (require_status(READY_STAT, BUSY_STAT|DRQ_STAT, NULL, 1, "do_drive_select2"))
154		return EIO;
155	return 0;
156}
157
158static int
159do_nondata_command (u8 command)
160{
161	u8 stat, err;
162
163	if (require_status(READY_STAT, BUSY_STAT|DRQ_STAT, NULL, 1, "do_nondata_command1"))
164		return EIO;
165	OUTB(command, ide_command);
166	if (require_status(READY_STAT, BUSY_STAT|DRQ_STAT, &stat, 40, "do_nondata_command2"))
167		return EIO;
168	if ((stat & ERR_STAT)) {
169		err = INB(ide_error);
170		fprintf(stderr, "command 0x%02x failed, status=0x%02x, error=0x%02x\n", command, stat, err);
171		dump_regs();
172		return EIO;
173	}
174	printf("command 0x%02x succeeded\n", command);
175	return 0;
176}
177
178static int
179do_setfeatures (u8 subcommand, u8 parameter)
180{
181	if (require_status(READY_STAT, BUSY_STAT|DRQ_STAT, NULL, 1, "do_setfeatures1"))
182		return EIO;
183	OUTB(subcommand, ide_feature);
184	OUTB(parameter,  ide_nsectors);
185	return do_nondata_command(SETFEATURES);
186}
187
188static int
189do_rw_test (void)
190{
191	OUTW(0x1234, ide_data);
192	wait_400ns();
193	if (INW(ide_data) == 0x1234) {
194		OUTW(0x4321, ide_data);
195		wait_400ns();
196		if (INW(ide_data) == 0x4321)
197			return 0; /* success */
198	}
199	fprintf(stderr, "register read/write test failed\n");
200	return EIO;
201}
202
203static int
204do_fix_drive (void)
205{
206	u8 stat;
207
208	printf("\nTrying with ide_base=0x%x:\n", ide_base);
209
210	ide_control = (ide_base < 0x1000) ? 0x206 : 0x102;
211	ide_altstatus = ide_control;
212
213	stat = INB(ide_altstatus);
214	if (stat == 0xff || stat == 0x00) {
215		fprintf(stderr, "No drive detected, try a different ide_base?\n");
216		return ENODEV;
217	}
218
219	if ((stat & (BUSY_STAT|DRQ_STAT))) {
220		fprintf(stderr, "IDE interface is busy, try again later\n");
221		return EAGAIN;
222	}
223	if (do_drive_select(0))
224		return EIO;
225	do_rw_test();
226
227	OUTB(0x02, ide_control);		/* set NIEN */
228	wait_400ns();
229
230	if (do_setfeatures(SF_SPINUP_NOW, 0))	/* spin-up the drive */
231		return EIO;
232	if (do_setfeatures(SF_NO_STANDBY, 0))	/* disable powerup-in-standby feature */
233		return EIO;
234	if (do_nondata_command(CHECKPOWERMODE))	/* clear any leftover errors */
235		return EIO;
236	dump_regs();
237
238	INB(ide_status);			/* clear IRQ */
239	OUTB(0x00, ide_control);		/* reeanble NIEN */
240	wait_400ns();
241	INB(ide_status);			/* clear IRQ */
242
243	printf("Done; the drive may take another 30 seconds to come to life now.\n");
244	return 0;
245}
246
247int  main (int argc, char **argv)
248{
249	int result;
250
251	ide_base = 0x1f0;
252	if (argc > 1) {
253		char *end = NULL;
254		ide_base = strtol(argv[1], &end, 0);
255		if (argc > 2 || !ide_base || ide_base > 0xffff || !end || *end) {
256			fprintf(stderr, "Bad argument; expected ide_base, Eg:  0x1f0\n");
257			return EINVAL;
258		}
259	}
260
261	sync();
262	sleep(2);
263	iopl(3);
264	cli();
265
266	result = do_fix_drive();
267
268	sti();
269	sync();
270	return result;
271}
272
273