QNAP-TS419p system daemon
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

493 lines
13 KiB

  1. /* sgio.c - by Mark Lord (C) 2007 -- freely distributable */
  2. #include <stdio.h>
  3. #include <stdlib.h>
  4. #include <unistd.h>
  5. #include <fcntl.h>
  6. #include <errno.h>
  7. #include <string.h>
  8. #include <sys/ioctl.h>
  9. #include <sys/stat.h>
  10. #include <sys/types.h>
  11. #include <scsi/scsi.h>
  12. #include <scsi/sg.h>
  13. #include "sgio.h"
  14. //#include "hdparm.h"
  15. #include <linux/hdreg.h>
  16. extern int verbose;
  17. extern int prefer_ata12;
  18. /*
  19. * Taskfile layout for SG_ATA_16 cdb:
  20. *
  21. * LBA48:
  22. * cdb[ 3] = hob_feat
  23. * cdb[ 5] = hob_nsect
  24. * cdb[ 7] = hob_lbal
  25. * cdb[ 9] = hob_lbam
  26. * cdb[11] = hob_lbah
  27. *
  28. * LBA28/LBA48:
  29. * cdb[ 4] = feat
  30. * cdb[ 6] = nsect
  31. * cdb[ 8] = lbal
  32. * cdb[10] = lbam
  33. * cdb[12] = lbah
  34. * cdb[13] = device
  35. * cdb[14] = command
  36. *
  37. * Taskfile layout for SG_ATA_12 cdb:
  38. *
  39. * cdb[ 3] = feat
  40. * cdb[ 4] = nsect
  41. * cdb[ 5] = lbal
  42. * cdb[ 6] = lbam
  43. * cdb[ 7] = lbah
  44. * cdb[ 8] = device
  45. * cdb[ 9] = command
  46. *
  47. * dxfer_direction choices:
  48. * SG_DXFER_TO_DEV, SG_DXFER_FROM_DEV, SG_DXFER_NONE
  49. */
  50. static inline int needs_lba48 (__u8 ata_op, __u64 lba, unsigned int nsect)
  51. {
  52. const __u64 lba28_limit = (1<<28) - 1;
  53. switch (ata_op) {
  54. case ATA_OP_READ_PIO_EXT:
  55. case ATA_OP_READ_DMA_EXT:
  56. case ATA_OP_WRITE_PIO_EXT:
  57. case ATA_OP_WRITE_DMA_EXT:
  58. case ATA_OP_READ_VERIFY_EXT:
  59. case ATA_OP_WRITE_UNC_EXT:
  60. case ATA_OP_READ_NATIVE_MAX_EXT:
  61. case ATA_OP_SET_MAX_EXT:
  62. case ATA_OP_FLUSHCACHE_EXT:
  63. return 1;
  64. }
  65. if (lba >= lba28_limit)
  66. return 1;
  67. if (nsect) {
  68. if (nsect > 0xff)
  69. return 1;
  70. if ((lba + nsect - 1) >= lba28_limit)
  71. return 1;
  72. }
  73. return 0;
  74. }
  75. void tf_init (struct ata_tf *tf, __u8 ata_op, __u64 lba, unsigned int nsect)
  76. {
  77. memset(tf, 0, sizeof(*tf));
  78. tf->command = ata_op;
  79. tf->dev = ATA_USING_LBA;
  80. tf->lob.lbal = lba;
  81. tf->lob.lbam = lba >> 8;
  82. tf->lob.lbah = lba >> 16;
  83. tf->lob.nsect = nsect;
  84. if (needs_lba48(ata_op, lba, nsect)) {
  85. tf->is_lba48 = 1;
  86. tf->hob.nsect = nsect >> 8;
  87. tf->hob.lbal = lba >> 24;
  88. tf->hob.lbam = lba >> 32;
  89. tf->hob.lbah = lba >> 40;
  90. } else {
  91. tf->dev |= (lba >> 24) & 0x0f;
  92. }
  93. }
  94. #ifdef SG_IO
  95. __u64 tf_to_lba (struct ata_tf *tf)
  96. {
  97. __u32 lba24, lbah;
  98. __u64 lba64;
  99. lba24 = (tf->lob.lbah << 16) | (tf->lob.lbam << 8) | (tf->lob.lbal);
  100. if (tf->is_lba48)
  101. lbah = (tf->hob.lbah << 16) | (tf->hob.lbam << 8) | (tf->hob.lbal);
  102. else
  103. lbah = (tf->dev & 0x0f);
  104. lba64 = (((__u64)lbah) << 24) | (__u64)lba24;
  105. return lba64;
  106. }
  107. enum {
  108. SG_CDB2_TLEN_NODATA = 0 << 0,
  109. SG_CDB2_TLEN_FEAT = 1 << 0,
  110. SG_CDB2_TLEN_NSECT = 2 << 0,
  111. SG_CDB2_TLEN_BYTES = 0 << 2,
  112. SG_CDB2_TLEN_SECTORS = 1 << 2,
  113. SG_CDB2_TDIR_TO_DEV = 0 << 3,
  114. SG_CDB2_TDIR_FROM_DEV = 1 << 3,
  115. SG_CDB2_CHECK_COND = 1 << 5,
  116. };
  117. static void dump_bytes (const char *prefix, unsigned char *p, int len)
  118. {
  119. int i;
  120. if (prefix)
  121. fprintf(stderr, "%s: ", prefix);
  122. for (i = 0; i < len; ++i)
  123. fprintf(stderr, " %02x", p[i]);
  124. fprintf(stderr, "\n");
  125. }
  126. int sg16 (int fd, int rw, int dma, struct ata_tf *tf,
  127. void *data, unsigned int data_bytes, unsigned int timeout_secs)
  128. {
  129. unsigned char cdb[SG_ATA_16_LEN];
  130. unsigned char sb[32], *desc;
  131. struct scsi_sg_io_hdr io_hdr;
  132. memset(&cdb, 0, sizeof(cdb));
  133. memset(&sb, 0, sizeof(sb));
  134. memset(&io_hdr, 0, sizeof(struct scsi_sg_io_hdr));
  135. if (dma) {
  136. //cdb[1] = data ? (rw ? SG_ATA_PROTO_UDMA_OUT : SG_ATA_PROTO_UDMA_IN) : SG_ATA_PROTO_NON_DATA;
  137. cdb[1] = data ? SG_ATA_PROTO_DMA : SG_ATA_PROTO_NON_DATA;
  138. } else {
  139. cdb[1] = data ? (rw ? SG_ATA_PROTO_PIO_OUT : SG_ATA_PROTO_PIO_IN) : SG_ATA_PROTO_NON_DATA;
  140. }
  141. cdb[ 2] = SG_CDB2_CHECK_COND;
  142. if (data) {
  143. cdb[2] |= SG_CDB2_TLEN_NSECT | SG_CDB2_TLEN_SECTORS;
  144. cdb[2] |= rw ? SG_CDB2_TDIR_TO_DEV : SG_CDB2_TDIR_FROM_DEV;
  145. }
  146. if (!prefer_ata12 || tf->is_lba48) {
  147. cdb[ 0] = SG_ATA_16;
  148. cdb[ 4] = tf->lob.feat;
  149. cdb[ 6] = tf->lob.nsect;
  150. cdb[ 8] = tf->lob.lbal;
  151. cdb[10] = tf->lob.lbam;
  152. cdb[12] = tf->lob.lbah;
  153. cdb[13] = tf->dev;
  154. cdb[14] = tf->command;
  155. if (tf->is_lba48) {
  156. cdb[ 1] |= SG_ATA_LBA48;
  157. cdb[ 3] = tf->hob.feat;
  158. cdb[ 5] = tf->hob.nsect;
  159. cdb[ 7] = tf->hob.lbal;
  160. cdb[ 9] = tf->hob.lbam;
  161. cdb[11] = tf->hob.lbah;
  162. }
  163. io_hdr.cmd_len = SG_ATA_16_LEN;
  164. } else {
  165. cdb[ 0] = SG_ATA_12;
  166. cdb[ 3] = tf->lob.feat;
  167. cdb[ 4] = tf->lob.nsect;
  168. cdb[ 5] = tf->lob.lbal;
  169. cdb[ 6] = tf->lob.lbam;
  170. cdb[ 7] = tf->lob.lbah;
  171. cdb[ 8] = tf->dev;
  172. cdb[ 9] = tf->command;
  173. io_hdr.cmd_len = SG_ATA_12_LEN;
  174. }
  175. io_hdr.interface_id = 'S';
  176. io_hdr.mx_sb_len = sizeof(sb);
  177. io_hdr.dxfer_direction = data ? (rw ? SG_DXFER_TO_DEV : SG_DXFER_FROM_DEV) : SG_DXFER_NONE;
  178. io_hdr.dxfer_len = data ? data_bytes : 0;
  179. io_hdr.dxferp = data;
  180. io_hdr.cmdp = cdb;
  181. io_hdr.sbp = sb;
  182. io_hdr.pack_id = tf_to_lba(tf);
  183. io_hdr.timeout = (timeout_secs ? timeout_secs : 5) * 1000; /* msecs */
  184. if (verbose)
  185. dump_bytes("outgoing cdb", cdb, sizeof(cdb));
  186. if (ioctl(fd, SG_IO, &io_hdr) == -1) {
  187. if (verbose)
  188. perror("ioctl(fd,SG_IO)");
  189. return -1; /* SG_IO not supported */
  190. }
  191. if (verbose)
  192. fprintf(stderr, "SG_IO: ATA_%u status=0x%x, host_status=0x%x, driver_status=0x%x\n",
  193. io_hdr.cmd_len, io_hdr.status, io_hdr.host_status, io_hdr.driver_status);
  194. if (io_hdr.host_status || io_hdr.driver_status != SG_DRIVER_SENSE
  195. || (io_hdr.status && io_hdr.status != SG_CHECK_CONDITION))
  196. {
  197. if (verbose)
  198. fprintf(stderr, "SG_IO: bad response (not CHECK_CONDITION)\n");
  199. errno = EBADE;
  200. return -1;
  201. }
  202. desc = sb + 8;
  203. if (sb[0] != 0x72 || sb[7] < 14 || desc[0] != 0x09 || desc[1] < 0x0c) {
  204. if (verbose)
  205. dump_bytes("SG_IO: bad/missing sense data, sb[]", sb, sizeof(sb));
  206. errno = EBADE;
  207. return -1;
  208. }
  209. if (verbose)
  210. dump_bytes("SG_IO: sb[]", sb, sizeof(sb));
  211. if (verbose) {
  212. int len = desc[1], maxlen = sizeof(sb) - 8 - 2;
  213. if (len > maxlen)
  214. len = maxlen;
  215. dump_bytes("SG_IO: desc[]", desc, len);
  216. }
  217. tf->is_lba48 = desc[ 2] & 1;
  218. tf->error = desc[ 3];
  219. tf->lob.nsect = desc[ 5];
  220. tf->lob.lbal = desc[ 7];
  221. tf->lob.lbam = desc[ 9];
  222. tf->lob.lbah = desc[11];
  223. tf->dev = desc[12];
  224. tf->status = desc[13];
  225. tf->hob.feat = 0;
  226. if (tf->is_lba48) {
  227. tf->hob.nsect = desc[ 4];
  228. tf->hob.lbal = desc[ 6];
  229. tf->hob.lbam = desc[ 8];
  230. tf->hob.lbah = desc[10];
  231. } else {
  232. tf->hob.nsect = 0;
  233. tf->hob.lbal = 0;
  234. tf->hob.lbam = 0;
  235. tf->hob.lbah = 0;
  236. }
  237. if (verbose)
  238. fprintf(stderr, " ATA_%u stat=%02x err=%02x nsect=%02x lbal=%02x lbam=%02x lbah=%02x dev=%02x\n",
  239. io_hdr.cmd_len, tf->status, tf->error, tf->lob.nsect, tf->lob.lbal, tf->lob.lbam, tf->lob.lbah, tf->dev);
  240. if (tf->status & (ATA_STAT_ERR | ATA_STAT_DRQ)) {
  241. if (verbose) {
  242. fprintf(stderr, "I/O error, ata_op=0x%02x ata_status=0x%02x ata_error=0x%02x\n",
  243. tf->command, tf->status, tf->error);
  244. }
  245. errno = EIO;
  246. return -1;
  247. }
  248. return 0;
  249. }
  250. #endif /* SG_IO */
  251. int do_drive_cmd (int fd, unsigned char *args)
  252. {
  253. #ifdef SG_IO
  254. struct ata_tf tf;
  255. void *data = NULL;
  256. unsigned int data_bytes = 0;
  257. int rc;
  258. if (args == NULL)
  259. goto use_legacy_ioctl;
  260. /*
  261. * Reformat and try to issue via SG_IO:
  262. */
  263. if (args[3]) {
  264. data_bytes = args[3] * 512;
  265. data = args + 4;
  266. }
  267. tf_init(&tf, args[0], 0, args[1]);
  268. tf.lob.feat = args[2];
  269. if (tf.command == ATA_OP_SMART) {
  270. tf.lob.nsect = args[3];
  271. tf.lob.lbal = args[1];
  272. tf.lob.lbam = 0x4f;
  273. tf.lob.lbah = 0xc2;
  274. }
  275. rc = sg16(fd, SG_READ, SG_PIO, &tf, data, data_bytes, 0);
  276. if (rc == -1) {
  277. if (errno == EINVAL || errno == ENODEV)
  278. goto use_legacy_ioctl;
  279. }
  280. if (rc == 0 || errno == EIO) {
  281. args[0] = tf.status;
  282. args[1] = tf.error;
  283. args[2] = tf.lob.nsect;
  284. }
  285. return rc;
  286. use_legacy_ioctl:
  287. #endif /* SG_IO */
  288. if (verbose)
  289. fprintf(stderr, "Trying legacy HDIO_DRIVE_CMD\n");
  290. return ioctl(fd, HDIO_DRIVE_CMD, args);
  291. }
  292. int do_taskfile_cmd (int fd, struct hdio_taskfile *r, unsigned int timeout_secs)
  293. {
  294. int rc;
  295. #ifdef SG_IO
  296. struct ata_tf tf;
  297. void *data = NULL;
  298. unsigned int data_bytes = 0;
  299. int rw = SG_READ;
  300. /*
  301. * Reformat and try to issue via SG_IO:
  302. */
  303. tf_init(&tf, 0, 0, 0);
  304. #if 1 /* debugging */
  305. if (verbose) {
  306. printf("oflags.lob_all=0x%02x, flags={", r->oflags.lob_all);
  307. if (r->oflags.lob.feat) printf(" feat");
  308. if (r->oflags.lob.lbal) printf(" lbal");
  309. if (r->oflags.lob.nsect)printf(" nsect");
  310. if (r->oflags.lob.lbam) printf(" lbam");
  311. if (r->oflags.lob.lbah) printf(" lbah");
  312. if (r->oflags.lob.dev) printf(" dev");
  313. if (r->oflags.lob.command) printf(" command");
  314. printf(" }\n");
  315. printf("oflags.hob_all=0x%02x, flags={", r->oflags.hob_all);
  316. if (r->oflags.hob.feat) printf(" feat");
  317. if (r->oflags.hob.lbal) printf(" lbal");
  318. if (r->oflags.hob.nsect)printf(" nsect");
  319. if (r->oflags.hob.lbam) printf(" lbam");
  320. if (r->oflags.hob.lbah) printf(" lbah");
  321. printf(" }\n");
  322. }
  323. #endif
  324. if (r->oflags.lob.feat) tf.lob.feat = r->lob.feat;
  325. if (r->oflags.lob.lbal) tf.lob.lbal = r->lob.lbal;
  326. if (r->oflags.lob.nsect) tf.lob.nsect = r->lob.nsect;
  327. if (r->oflags.lob.lbam) tf.lob.lbam = r->lob.lbam;
  328. if (r->oflags.lob.lbah) tf.lob.lbah = r->lob.lbah;
  329. if (r->oflags.lob.dev) tf.dev = r->lob.dev;
  330. if (r->oflags.lob.command) tf.command = r->lob.command;
  331. if (r->oflags.hob_all || r->iflags.hob_all) {
  332. tf.is_lba48 = 1;
  333. if (r->oflags.hob.feat) tf.hob.feat = r->hob.feat;
  334. if (r->oflags.hob.lbal) tf.hob.lbal = r->hob.lbal;
  335. if (r->oflags.hob.nsect)tf.hob.nsect = r->hob.nsect;
  336. if (r->oflags.hob.lbam) tf.hob.lbam = r->hob.lbam;
  337. if (r->oflags.hob.lbah) tf.hob.lbah = r->hob.lbah;
  338. if (verbose)
  339. fprintf(stderr, "using LBA48 taskfile\n");
  340. }
  341. switch (r->cmd_req) {
  342. case TASKFILE_CMD_REQ_OUT:
  343. case TASKFILE_CMD_REQ_RAW_OUT:
  344. data_bytes = r->obytes;
  345. data = r->data;
  346. rw = SG_WRITE;
  347. break;
  348. case TASKFILE_CMD_REQ_IN:
  349. data_bytes = r->ibytes;
  350. data = r->data;
  351. break;
  352. }
  353. rc = sg16(fd, rw, SG_PIO, &tf, data, data_bytes, timeout_secs);
  354. if (rc == -1) {
  355. if (errno == EINVAL || errno == ENODEV)
  356. goto use_legacy_ioctl;
  357. }
  358. if (rc == 0 || errno == EIO) {
  359. if (r->iflags.lob.feat) r->lob.feat = tf.error;
  360. if (r->iflags.lob.lbal) r->lob.lbal = tf.lob.lbal;
  361. if (r->iflags.lob.nsect) r->lob.nsect = tf.lob.nsect;
  362. if (r->iflags.lob.lbam) r->lob.lbam = tf.lob.lbam;
  363. if (r->iflags.lob.lbah) r->lob.lbah = tf.lob.lbah;
  364. if (r->iflags.lob.dev) r->lob.dev = tf.dev;
  365. if (r->iflags.lob.command) r->lob.command = tf.status;
  366. if (r->iflags.hob.feat) r->hob.feat = tf.hob.feat;
  367. if (r->iflags.hob.lbal) r->hob.lbal = tf.hob.lbal;
  368. if (r->iflags.hob.nsect) r->hob.nsect = tf.hob.nsect;
  369. if (r->iflags.hob.lbam) r->hob.lbam = tf.hob.lbam;
  370. if (r->iflags.hob.lbah) r->hob.lbah = tf.hob.lbah;
  371. }
  372. return rc;
  373. use_legacy_ioctl:
  374. #else
  375. timeout_secs = 0; /* keep compiler happy */
  376. #endif /* SG_IO */
  377. if (verbose)
  378. fprintf(stderr, "trying legacy HDIO_DRIVE_TASKFILE\n");
  379. errno = 0;
  380. rc = ioctl(fd, HDIO_DRIVE_TASKFILE, r);
  381. if (verbose) {
  382. int err = errno;
  383. fprintf(stderr, "rc=%d, errno=%d, returned ATA registers: ", rc, err);
  384. if (r->iflags.lob.feat) fprintf(stderr, " er=%02x", r->lob.feat);
  385. if (r->iflags.lob.nsect) fprintf(stderr, " ns=%02x", r->lob.nsect);
  386. if (r->iflags.lob.lbal) fprintf(stderr, " ll=%02x", r->lob.lbal);
  387. if (r->iflags.lob.lbam) fprintf(stderr, " lm=%02x", r->lob.lbam);
  388. if (r->iflags.lob.lbah) fprintf(stderr, " lh=%02x", r->lob.lbah);
  389. if (r->iflags.lob.dev) fprintf(stderr, " dh=%02x", r->lob.dev);
  390. if (r->iflags.lob.command) fprintf(stderr, " st=%02x", r->lob.command);
  391. if (r->iflags.hob.feat) fprintf(stderr, " err=%02x", r->hob.feat);
  392. if (r->iflags.hob.nsect) fprintf(stderr, " err=%02x", r->hob.nsect);
  393. if (r->iflags.hob.lbal) fprintf(stderr, " err=%02x", r->hob.lbal);
  394. if (r->iflags.hob.lbam) fprintf(stderr, " err=%02x", r->hob.lbam);
  395. if (r->iflags.hob.lbah) fprintf(stderr, " err=%02x", r->hob.lbah);
  396. fprintf(stderr, "\n");
  397. errno = err;
  398. }
  399. if (rc == -1 && errno == EINVAL) {
  400. fprintf(stderr, "The running kernel lacks CONFIG_IDE_TASK_IOCTL support for this device.\n");
  401. errno = EINVAL;
  402. }
  403. return rc;
  404. }
  405. void init_hdio_taskfile (struct hdio_taskfile *r, __u8 ata_op, int rw, int force_lba48,
  406. __u64 lba, unsigned int nsect, int data_bytes)
  407. {
  408. memset(r, 0, sizeof(struct hdio_taskfile) + data_bytes);
  409. if (!data_bytes) {
  410. r->dphase = TASKFILE_DPHASE_NONE;
  411. r->cmd_req = TASKFILE_CMD_REQ_NODATA;
  412. } else if (rw == RW_WRITE) {
  413. r->dphase = TASKFILE_DPHASE_PIO_OUT;
  414. r->cmd_req = TASKFILE_CMD_REQ_RAW_OUT;
  415. r->obytes = data_bytes;
  416. } else { /* rw == RW_READ */
  417. r->dphase = TASKFILE_DPHASE_PIO_IN;
  418. r->cmd_req = TASKFILE_CMD_REQ_IN;
  419. r->ibytes = data_bytes;
  420. }
  421. r->lob.command = ata_op;
  422. r->oflags.lob.command = 1;
  423. r->oflags.lob.dev = 1;
  424. r->oflags.lob.lbal = 1;
  425. r->oflags.lob.lbam = 1;
  426. r->oflags.lob.lbah = 1;
  427. r->oflags.lob.nsect = 1;
  428. r->iflags.lob.command = 1;
  429. r->iflags.lob.feat = 1;
  430. r->lob.nsect = nsect;
  431. r->lob.lbal = lba;
  432. r->lob.lbam = lba >> 8;
  433. r->lob.lbah = lba >> 16;
  434. r->lob.dev = 0xa0 | ATA_USING_LBA;
  435. if (needs_lba48(ata_op, lba, nsect) || force_lba48) {
  436. r->hob.nsect = nsect >> 8;
  437. r->hob.lbal = lba >> 24;
  438. r->hob.lbam = lba >> 32;
  439. r->hob.lbah = lba >> 40;
  440. r->oflags.hob.nsect = 1;
  441. r->oflags.hob.lbal = 1;
  442. r->oflags.hob.lbam = 1;
  443. r->oflags.hob.lbah = 1;
  444. } else {
  445. r->lob.dev |= (lba >> 24) & 0x0f;
  446. }
  447. }