#define USBAT_ATA       UT_VENDOR

#define ATA_REG_DATA		0x10
#define ATA_REG_FEATURES	0x11
#define ATA_REG_ERROR		0x11
#define ATA_REG_SECCNT		0x12
#define ATA_REG_LBA_LO		0x13
#define ATA_REG_LBA_ME		0x14
#define ATA_REG_LBA_HI		0x15
#define ATA_REG_DEVICE		0x16
#define ATA_REG_STATUS		0x17
#define ATA_REG_CMD			0x17

#define ATA_CMD_DEVICE_RESET                    0x08
#define ATA_CMD_READ_SECTORS                    0x20
#define ATA_CMD_IDENTIFY_PACKET_DEVICE			0xa1
#define ATA_CMD_SET_MULTIPLE_MODE               0xc6
#define ATA_CMD_GET_MEDIA_STATUS                0xda
#define ATA_CMD_IDENTIFY_DEVICE                 0xec

#define ATA_LBA_MASK            0x0FFFFFFF

#define ATA_STATUS_DRDY         0x50            /* device ready */
#define ATA_STATUS_DRQ          0x08            /* data ready */

#define MIN(x,y) (((x) <= (y))?(x):(y))

#ifdef USB_DEBUG
#define US_DEBUGP	if(umassdebug) printf
#else
#define US_DEBUGP	if(0) printf
#endif

#define wait_ms(x) usbd_delay_ms(us, x) /* DELAY((x)*1000) */
#define CHK_ERR(rc) if(rc != USBD_NORMAL_COMPLETION) return rc

Static usbd_status umass_usbat2_bulk_xfer(struct umass_softc *sc,
										usbd_pipe_handle pipe,
										unsigned char * data, int len)
{
	usbd_xfer_handle reqh;
	int err, actlen;

	if( len == 0 )
		return USBD_NORMAL_COMPLETION;

	reqh = usbd_alloc_xfer(sc->sc_udev);
	if (reqh == 0)
		return USBD_NOMEM;

	actlen = len;
	err = usbd_bulk_transfer(reqh, pipe, 0, UMASS_TIMEOUT, data, &actlen, "usbat2b");

	usbd_free_xfer(reqh);

	return err;
}

Static usbd_status ziocf_request(usbd_device_handle dev,
								int request, int type, int value, int index,
								void *data, int len)
{
	usb_device_request_t req;

	req.bmRequestType = type;
	req.bRequest = request;
	USETW(req.wValue, value);
	USETW(req.wIndex, index);
	USETW(req.wLength, len);
	return usbd_do_request(dev, &req, data);
}

Static usbd_status ziocf_read(usbd_device_handle dev,
							u_char access, int reg, u_char * data)
{
	return ziocf_request(dev, access, 0xc0, reg, 0, data, 1);
}

Static usbd_status ziocf_write(usbd_device_handle dev,
							u_char access, int reg, u_char data)
{
	return ziocf_request(dev, access|0x01, 0x40, (data<<8)|reg, 0, NULL, 0);
}

static void ziocf_regdata(u_char *data, u_char *reg, u_char *cmd, int regs)
{
	int i;

	for (i=0; i<regs; i++) {
		data[   i<<1 ] = reg[i];
		data[1+(i<<1)] = cmd[i];
	}
}

static int ziocf_read_data(struct umass_softc * sc,
						u_int32_t lba, u_int32_t blks,
						unsigned char *dest, int datalen, int *actlen)
{
	int ssize = 512;
	u_char ctrl_mreg_wr[16] = {
		0x40, USBAT_ATA|0x07, 0x01, 0x17, 0xf0, 0xee,
		(7*2) & 0xFF, ((7*2)>>8),   /* # of ata_registers */
		0xc0, 0x44,                 /* bit compare reg blk read */
		ATA_REG_DATA, ATA_REG_STATUS,
		0xfd, 0x20,                 /* timeout, qualifier, */
		0x00, 0x02,                 /* length */
	};
	u_char registers[7]  = { 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17 };
	u_char command[7]    = { 0, 0, 0, 0, 0, 0xe0, 0x20 };
	u_char data[7*2];
	u_char thistime;
	int len, result;

	*actlen = 0;
	if (lba > ATA_LBA_MASK)
		return USBD_IOERROR;

	blks = MIN(blks, datalen/ssize);
	do {
		thistime = MIN(blks, (2*16384)/ssize);
		len = thistime * ssize;
		USETW(ctrl_mreg_wr+14, len);

		command[0] = 0;
		command[1] = thistime;
		USETDW(command+2, lba);
		command[5] |= 0xe0;

		result = ziocf_request(sc->sc_udev, 0x80, 0x40, 0, 0,ctrl_mreg_wr, 16);

		ziocf_regdata( data, registers, command, 7);
		result = umass_usbat2_bulk_xfer(sc, sc->bulkout_pipe, data, 14);
		CHK_ERR(result);

		result = umass_usbat2_bulk_xfer(sc, sc->bulkin_pipe, dest, len);
		CHK_ERR(result);

		US_DEBUGP("ziocf_read_data:  %d bytes\n", len);

		blks -= thistime;
		lba  += thistime;
		dest += len;
		*actlen += len;
	} while (blks > 0);

	return USBD_NORMAL_COMPLETION;
}

Static usbd_status umass_usbat2_get_status(struct umass_softc *sc)
{
	unsigned char reply;
	int rc;

	/* send the setup */
	rc = ziocf_read(sc->sc_udev, USBAT_ATA, 0x17, &reply);
	CHK_ERR(rc);

	if ( (reply != 0x50) && (reply != 0x51) ){
		printf("ziocf_get_status:  0x%02x\n", (unsigned char) (reply));
		return USBD_IOERROR;
	}

	return USBD_NORMAL_COMPLETION;
}

static int ziocf_id_device(struct umass_softc * sc)
{
	usbd_device_handle us;
	u_char status;
	u_char ctrl_command[8] = {0x40, 0x47, 0,0,  0, 0, 6, 0};
	u_char ctrl_4210[8] = {0xc0, 0x42, 0x10, 0x00, 0x00, 0x00, 0x00, 0x02};
	u_char command[6] = { 0x12, 0x01, 0x16, 0xa0, 0x17, 0xec };	
	u_char reply[512];
	int rc, sectors;

	us = sc->sc_udev;
	rc = ziocf_request(us, 0x80, 0x40, 0,0,	ctrl_command, 8);
	CHK_ERR(rc);
	wait_ms(5);

	rc = umass_usbat2_bulk_xfer(sc, sc->bulkout_pipe, command, 6);
	CHK_ERR(rc);
	US_DEBUGP("ziocf_id_device:  bulk_write for read_capacity ok\n");

	wait_ms(5);
	rc = ziocf_read(us, USBAT_ATA, 0x17, &status);
	CHK_ERR(rc);
	
	if ( status == 0x58 ) {
		rc = ziocf_request(us, 0x80,0x40, 0,0,	ctrl_4210, 8);
		wait_ms(50);
	}
	CHK_ERR(rc);
	wait_ms(50);
	
	rc = umass_usbat2_bulk_xfer(sc, sc->bulkin_pipe, reply, sizeof(reply));
	CHK_ERR(rc);
	wait_ms(150);

	rc = ziocf_read(us, USBAT_ATA, 0x17, &status);
	CHK_ERR(rc);

	US_DEBUGP("ziocf_id_device: ATA IDENTIFY: %02X %02X %02X %02X\n",
		reply[0], reply[1], reply[2], reply[3] );

	sectors = UGETDW(reply+120);

	return sectors;
}

Static void umass_usbat2_transfer	(struct umass_softc *sc, int lun,
				void *cmd, int cmdlen,
				void *data, int datalen, int dir,
				transfer_cb_f cb, void *priv)
{
	unsigned char * rcmd = (unsigned char *)cmd;
	unsigned char response[36] = {
		0x00, 0x80, 0x00, 0x01, 0x1F, 0x00, 0x00, 0x00
	};
	unsigned char * rdata = (unsigned char *)data;
	usbd_device_handle dev = sc->sc_udev;
	usb_device_descriptor_t *udd = usbd_get_device_descriptor(dev);
	char v[USB_MAX_STRING_LEN];
	char p[USB_MAX_STRING_LEN];
	int bcdDevice;
	unsigned int sectors, ssize, err;

	switch( rcmd[0] ) {
	case INQUIRY:
		memcpy(rdata, response, 8);
		memset(rdata+8, 0, 28);
		
		usbd_devinfo_vp(dev, v, p, 0);
		
		strncpy(rdata+8, v, 8);
		strncpy(rdata+16, p, 16);
		
		bcdDevice = UGETW(udd->bcdDevice);
		rdata[32] = 0x30 + ((bcdDevice>>12) & 0x0F);
		rdata[33] = 0x30 + ((bcdDevice>>8) & 0x0F);
		rdata[34] = 0x30 + ((bcdDevice>>4) & 0x0F);
		rdata[35] = 0x30 + ((bcdDevice) & 0x0F);
		
		cb(sc, priv, datalen-36, STATUS_CMD_OK);
		break;

	case TEST_UNIT_READY:
		umass_usbat2_get_status(sc);
		cb(sc, priv, datalen, STATUS_CMD_OK);
		break;

	case READ_CAPACITY:
		sectors = ziocf_id_device(sc);

		/* build the reply */
		ssize = 0x200; /* hard coded 512B/sec as per ATA spec */
		rdata[0] = (sectors >> 24) & 0xFF;
		rdata[1] = (sectors >> 16) & 0xFF;
		rdata[2] = (sectors >> 8) & 0xFF;
		rdata[3] = (sectors) & 0xFF;

		rdata[4] = (ssize >> 24) & 0xFF;
		rdata[5] = (ssize >> 16) & 0xFF;
		rdata[6] = (ssize >> 8) & 0xFF;
		rdata[7] = (ssize) & 0xFF;

		cb(sc, priv, datalen-8, STATUS_CMD_OK);
		break;

	case PREVENT_ALLOW:
		cb(sc, priv, datalen, STATUS_CMD_OK);
		break;

	case READ_6:
		{
		int lba, blks, len;

		lba  = ((rcmd[1] << 16) | (rcmd[2] << 8) | rcmd[3]);
		blks = ((rcmd[4] << 8) | rcmd[5]);

		err = ziocf_read_data(sc, lba, blks, data, datalen, &len);

		if (err != USBD_NORMAL_COMPLETION )
			cb(sc, priv, datalen, STATUS_CMD_FAILED);
		else
			cb(sc, priv, datalen-len, STATUS_CMD_OK);
		}
		break;

	case READ_10:
		{
			u_int32_t lba, blks, len;

			lba = ((rcmd[2] << 24)|(rcmd[3] << 16)|(rcmd[4] << 8) |(rcmd[5]));
			blks = ((rcmd[7] << 8) | (rcmd[8]));

			err = ziocf_read_data(sc, lba, blks, data, datalen, &len);

			if (err != USBD_NORMAL_COMPLETION )
				cb(sc, priv, datalen, STATUS_CMD_FAILED);
			else
				cb(sc, priv, datalen-len, STATUS_CMD_OK);
		}
		break;

	case SYNCHRONIZE_CACHE:
		cb(sc, priv, datalen, STATUS_CMD_OK);
		break;

	case REQUEST_SENSE:
		rdata[0] = 0xf0;
		rdata[2] = SSD_KEY_NO_SENSE;
		rdata[7] = 11;
		rdata[12] = 0;
		rdata[13] = 0;
		cb(sc, priv, datalen-14, STATUS_CMD_OK);
		break;

	case WRITE_6:
	case MODE_SELECT_6:
	case MODE_SENSE_6:
	case START_STOP_UNIT:
	case RESERVE:
	case RELEASE:
	case RECEIVE_DIAGNOSTIC:
	case SEND_DIAGNOSTIC:
	case WRITE_10:
	case POSITION_TO_ELEMENT:
	case WRITE_BUFFER:
	case READ_BUFFER:
	case CHANGE_DEFINITION:
	case MODE_SELECT_10:
	case MODE_SENSE_10:
	case MOVE_MEDIUM:
	case READ_12:
	case WRITE_12:
	case READ_ELEMENT_STATUS:
		printf("transfer cmd 0x%x unsupported\n", rcmd[0]);
		cb(sc, priv, datalen, STATUS_CMD_OK);
		break;

	default:
		panic("unknown scsi cmd in usbat2_transfer");
	}
}

Static void umass_usbat2_state	(usbd_xfer_handle xfer,
				usbd_private_handle priv, usbd_status err)
{
	panic("usbat2_state called");
}

int umass_usbat2_init(struct umass_softc *sc)
{
	usbd_device_handle us;
	u_char ctrl_uio_reset[] = { 0x40, 0x82, 0x30, 0xa0,  0, 0, 0, 0};
	u_char ctrl_feature[] = { 0x40, 0x81, 0x00, 0x83, 0x08, 0x88, 0x00, 0x02};
	int rc, i;
	u_char status;

	us = sc->sc_udev;
	rc = ziocf_request(us, 0x80, 0x40, 0, 0, ctrl_uio_reset, 8);
	wait_ms(300);
	rc = ziocf_request(us, 0x82, 0xC0, 0, 0, &status, 1);

	CHK_ERR(rc);
	US_DEBUGP("ziocf_init 1 : 30a0, status=%02X\n", status);

	rc = ziocf_write(us, USBAT_ATA, ATA_REG_DEVICE, 0xa0);
	CHK_ERR(rc);
	wait_ms(50);
	
	rc = ziocf_read(us, USBAT_ATA, ATA_REG_STATUS, &status);
	US_DEBUGP("ziocf_init 6 : write 16:a0,       ok, status%02X\n", status );

	rc = ziocf_write(us,USBAT_ATA, ATA_REG_CMD,ATA_CMD_IDENTIFY_PACKET_DEVICE);
	CHK_ERR(rc);
		
	rc = ziocf_read(us, USBAT_ATA, ATA_REG_STATUS, &status);
	US_DEBUGP("ziocf_init 6 : write        17:a1 ok, status%02X\n", status );

	rc = ziocf_write(us, USBAT_ATA, ATA_REG_DEVICE, 0xa0);
	CHK_ERR(rc);
	wait_ms(50);

	rc = ziocf_write(us, USBAT_ATA, ATA_REG_CMD, ATA_CMD_DEVICE_RESET);
	CHK_ERR(rc);
		
	rc = ziocf_read(us, USBAT_ATA, ATA_REG_STATUS, &status);
	CHK_ERR(rc);
	US_DEBUGP("ziocf_init 7 : write 16:a0, 17:a1 ok, status 17=%02X\n",status);
	rc = ziocf_read(us, USBAT_ATA, ATA_REG_ERROR, &status);
	CHK_ERR(rc);
	US_DEBUGP("ziocf_init 7 : write 16:a0, 17:a1 ok, status 11=%02X\n",status);

	status = 0xff;
	for(i=0; (i<30) && (status!=0x50); i++) {
		wait_ms(250);
		rc = ziocf_read(us, USBAT_ATA, ATA_REG_STATUS, &status);
	}

	rc = ziocf_write(us, USBAT_ATA, ATA_REG_SECCNT, 0x01);
	CHK_ERR(rc);
	wait_ms(20);

	rc = ziocf_write(us, USBAT_ATA, ATA_REG_CMD, ATA_CMD_SET_MULTIPLE_MODE);
	CHK_ERR(rc);
	wait_ms(120);

	rc = ziocf_read(us, USBAT_ATA, ATA_REG_STATUS, &status);
	CHK_ERR(rc);
	US_DEBUGP("ziocf_init 13: cmnd c6 ok, status=%02X\n", status);

	rc = ziocf_request(us, 0x80, 0x40, 0, 0, ctrl_feature, 8);
	US_DEBUGP("ziocf_init 14: ctrl feature ok\n");
	wait_ms(100);

	return rc;
}