--- /home/nmisaghian/ata_patch/atacontrol.old	2011-11-22 10:38:44.900307700 -0500
+++ /home/nmisaghian/ata_patch/atacontrol.new	2011-11-22 10:39:13.091819900 -0500
@@ -1,4 +1,5 @@
 /*-
+ * Copyright (c) 2011 Sandvine Incorporated. All rights reserved.
  * Copyright (c) 2000 - 2006 Søren Schmidt <sos@FreeBSD.org>
  * All rights reserved.
  *
@@ -27,11 +28,13 @@
  */
 
 #include <sys/types.h>
+#include <sys/stat.h>
 #include <sys/ata.h>
 
 #include <err.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <inttypes.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -39,6 +42,9 @@
 #include <sysexits.h>
 #include <unistd.h>
 
+#define	MAX_FW_IMAGE_SIZE	(0xFFFF * 512)
+#define	DOWNLOAD_FW_TIMEOUT	20	/* 20 seconds */
+
 static const char *
 mode2str(int mode)
 {
@@ -125,6 +131,7 @@
 		"        atacontrol mode device [mode]\n"
 		"        atacontrol cap device\n"
 		"        atacontrol spindown device [seconds]\n"
+		"        atacontrol fwdownload device fw_image_path\n"
 	);
 	exit(EX_USAGE);
 }
@@ -280,6 +287,142 @@
 	cap_print(&params);
 }
 
+static int
+ata_fw_download(int fd, const char *fw_img_path)
+{
+	struct ata_ioc_request ioc_request;
+	struct ata_params params;
+	struct stat stbuf;
+	caddr_t buf;
+	off_t img_size;
+	off_t offset;
+	int fw_fd;
+	int transfer_mode;
+	int transfer_bytes;
+	int transfer_blocks;
+	int min_transfer_blocks;
+
+	if (ioctl(fd, IOCATAGPARM, &params) < 0) {
+		warn("Cound not get ATA identify data");
+		return (1);
+	}
+	if (!((params.support.command2 & ATA_SUPPORT_MICROCODE) &&
+	    (params.enabled.command2 & ATA_SUPPORT_MICROCODE))) {
+		warnx("Firmware download not supported by device");
+		return (1);
+	}
+	if ((fw_fd = open(fw_img_path, O_RDONLY)) < 0) {
+		warn("Could not open %s", fw_img_path);
+		return (1);
+	}
+	if (fstat(fw_fd, &stbuf) < 0) {
+		warn("Could not stat %s", fw_img_path);
+		goto bailout;
+	}
+	img_size = stbuf.st_size;
+	if (img_size == 0) {
+		warnx("File (%s) has zero size", fw_img_path);
+		goto bailout;
+	}
+	if (img_size > MAX_FW_IMAGE_SIZE) {
+		warnx("Image file (%s) is too large (max = %d)", fw_img_path,
+		    MAX_FW_IMAGE_SIZE);
+		goto bailout;
+	}
+	if (img_size % 512 != 0) {
+		warnx("Size of %s is not a multiple of 512", fw_img_path);
+		goto bailout;
+	}
+	if ((buf = malloc(img_size)) == NULL) {
+		warnx("Could not allocate buffer to read %s",
+		    fw_img_path);
+		goto bailout;
+	}
+	/* Read image into a buffer. */
+	if (read(fw_fd, buf, img_size) != img_size) {
+		warn("Could not read image file %s", fw_img_path);
+		goto bailout1;
+	}
+	/* Determine download transfer mode. */
+	if (((params.support2 & ATA_SUPPORT_MICROCODE3) != 0) &&
+	    ((params.enabled2 & ATA_SUPPORT_MICROCODE3) != 0)) {
+		transfer_mode = 3;
+		min_transfer_blocks = params.reserved224[234 - 224];
+		if ((min_transfer_blocks == 0) ||
+		    (min_transfer_blocks == 0xffff))
+			min_transfer_blocks = 1;
+		/* Always choose minimum transfer size for mode 3. */
+		transfer_blocks = min_transfer_blocks;
+	} else {
+		transfer_mode = 7;
+		transfer_blocks = img_size / 512;
+	}
+	transfer_bytes = transfer_blocks * 512;
+	ioc_request.flags |= ATA_CMD_WRITE;
+	ioc_request.u.ata.command = 0x92; /* DOWNLOAD MICROCODE */
+	ioc_request.u.ata.feature = transfer_mode;
+	ioc_request.count = transfer_bytes;
+	ioc_request.data = buf;
+	ioc_request.timeout = DOWNLOAD_FW_TIMEOUT;
+	ioc_request.u.ata.count = transfer_blocks & 0xff;
+	/* Download the firmware image into the device. */
+	fprintf(stdout, "Downloading image %s  size(bytes)= "
+	    "%"PRId64" transfer_mode= %d block_size(bytes)= %d\n",
+	    fw_img_path, img_size, transfer_mode, transfer_bytes);
+	for (offset = 0; offset < img_size; offset += transfer_bytes) {
+		ioc_request.u.ata.lba = ((offset / 512) << 8) |
+		    ((transfer_blocks >> 8) & 0xff);
+		if (ioctl(fd, IOCATAREQUEST, &ioc_request) < 0) {
+			warn("ioctl(IOCATAREQUEST) failed");
+			goto bailout1;
+		}
+		if (transfer_mode == 3) {
+			switch (ioc_request.error) {
+			case 0:	/* No error */
+				break;
+			case 1:	/* Drive needs more segments. */
+				if (offset + transfer_bytes >= img_size) 
+					warnx("Drive indicates it needs more"
+					    " segments after download was"
+					    " complete");
+				break;
+			case 2: /* Drive has had enough of it! */
+				if (offset + transfer_bytes < img_size) {
+					warnx("Firmware transfer aborted at %"PRId64"",
+					    offset + transfer_bytes);
+					goto bailout1;
+				}
+				break;
+			default:
+				warnx("DOWNLOAD_MICROCODE returned error"
+				    " value %d", ioc_request.error);
+				goto bailout1;
+			}
+		}
+		ioc_request.data += transfer_bytes;
+	}
+	if (ioc_request.error == 2 || ioc_request.error == 0) {
+		fprintf(stdout, "Firmware download successful.\n");
+		/* 
+		 * Call Get Param to update firmware revision
+		 * number in ATA driver cache
+		 */
+		if (ioctl(fd, IOCATAGPARM, &params) < 0)
+			warnx("ioctl(IOCATAGPARM) failed");
+	} else {
+		warnx("Firmware download failed.");
+		goto bailout1;
+	}
+	free(buf);
+	close(fw_fd);
+	return (0);
+bailout1:
+	free(buf);
+bailout:
+	close(fw_fd);
+        return (1);
+}
+
 static void
 info_print(int fd, int channel, int prchan)
 {
@@ -410,6 +553,14 @@
 		exit(EX_OK);
 	}
 
+	if (!strcmp(argv[1], "fwdownload") && argc == 4) {
+		int error;
+
+		fd = open_dev(argv[2], O_RDONLY);
+		error = ata_fw_download(fd, argv[3]);
+		exit(error);
+	}
+
 	if ((fd = open("/dev/ata", O_RDWR)) < 0)
 		err(1, "control device not found");