/* Copyright 2012 Fredrik Wikstrom. All rights reserved.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions
** are met:
**
** 1. Redistributions of source code must retain the above copyright
**    notice, this list of conditions and the following disclaimer.
**
** 2. Redistributions in binary form must reproduce the above copyright
**    notice, this list of conditions and the following disclaimer in the
**    documentation and/or other materials provided with the distribution.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS `AS IS'
** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
** ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
** LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
** CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
** SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
** INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
** CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
** ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
** POSSIBILITY OF SUCH DAMAGE.
*/

#include "cddapatch.h"
#include <proto/exec.h>
#include <proto/dos.h>
#include <proto/utility.h>
#include <proto/icon.h>
#include <proto/commodities.h>
#include <proto/cdplayer.h>
#include <string.h>
#include <stddef.h>
#include "endian.h"
#include "bytepack.h"
#include "CDDAPatch_rev.h"

CONST TEXT USED verstag[] = VERSTAG;
struct DiskObject *Icon;

struct Library *CDPlayerBase;
struct CDPlayerIFace *ICDPlayer;

#define MAX_DEVICES 8
#define MAX_DRIVES 32

struct PatchedDevice {
	CONST_STRPTR name;
	struct IORequest *ior;
	struct Interface *interface;
	APTR newbeginio;
	APTR oldbeginio;
};

struct PatchedDrive {
	CONST_STRPTR devicename;
	uint32 deviceunit;
};

static APTR mempool;
static struct PatchedDevice devices[MAX_DEVICES];
static struct PatchedDrive drives[MAX_DRIVES];
static int32 num_devices;
static int32 num_drives;

static struct PatchedDevice *DeviceIsPatched (CONST_STRPTR devicename);
static struct PatchedDevice *PatchDevice (CONST_STRPTR devicename, uint32 deviceunit, struct MsgPort *ioport);
static void UnPatchDevice (struct PatchedDevice *device);
static struct PatchedDrive *DriveIsPatched (CONST_STRPTR devicename, uint32 deviceunit);
static struct PatchedDrive *PatchDrive (CONST_STRPTR devicename, uint32 deviceunit, struct MsgPort *dmport);
static void NewBeginIO (struct DeviceManagerInterface *Self, struct IOStdReq *ior,
	void (*OldBeginIO) (struct DeviceManagerInterface *Self, struct IOStdReq *ior));
static struct IOStdReq *CreateCDDAIORequest (struct IOStdReq *ior);
static void DeleteCDDAIORequest (struct IOStdReq *cdda_io);
static BOOL CheckCDDAAddress (struct SCSICmd *scsi, uint32 addr, struct IOStdReq *cdda_io, BytePackBuffer *sense);
static uint32 GetTrackNumber (const struct CD_TOC *toc, const uint32 addr);

int main (int argc, STRPTR *argv) {
	int rc = RETURN_FAIL;
	struct MsgPort *ioport = NULL;
	struct MsgPort *dmport = NULL;
	STRPTR *tooltypes, tt;
	int32 i;
	uint32 sigs;

	SetAmiUpdateENVVariable(PROGNAME);

	Icon = IIcon->GetDiskObjectNew("PROGDIR:"PROGNAME);
	if (!Icon) {
		goto error;
	}

	if (!RegisterCxBroker()) {
		goto error;
	}

	if (!(ioport = IExec->AllocSysObject(ASOT_PORT, NULL)) ||
		!(dmport = IExec->AllocSysObject(ASOT_PORT, NULL)))
	{
		goto error;
	}

	if (!(CDPlayerBase = IExec->OpenLibrary("cdplayer.library", 52)) ||
		!CheckLib(CDPlayerBase, 52, 7) ||
		!(ICDPlayer = (struct CDPlayerIFace *)IExec->GetInterface(CDPlayerBase, "main", 1, NULL)))
	{
		goto error;
	}

	mempool = IExec->AllocSysObject(ASOT_MEMPOOL, NULL);
	if (!mempool) {
		goto error;
	}

	tooltypes = Icon->do_ToolTypes;
	while (tt = *tooltypes++) {
		if (IUtility->Strnicmp(tt, "DRIVE=", 6) == 0 && tt[6]) {
			TEXT drivename[20];
			int32 len;
			struct FileSystemData *fsd;
			STRPTR devicename;
			uint32 deviceunit;
			struct PatchedDevice *device;
			struct PatchedDrive *drive;
			IUtility->Strlcpy(drivename, tt+6, sizeof(drivename)-1);
			len = strlen(drivename)-1;
			if (drivename[len] != ':') {
				drivename[++len] = ':';
				drivename[len+1] = 0;
			}
			fsd = IDOS->GetDiskFileSystemData(drivename);
			if (!fsd) {
				goto error;
			}
			devicename = ASPrintfPooled(mempool, "%s", fsd->fsd_DeviceName);
			deviceunit = fsd->fsd_DeviceUnit;
			IDOS->FreeDiskFileSystemData(fsd);
			if (!devicename) {
				goto error;
			}
			if ((!(device = DeviceIsPatched(devicename)) &&
				!(device = PatchDevice(devicename, deviceunit, ioport))) ||
				(!(drive = DriveIsPatched(devicename, deviceunit)) &&
				!(drive = PatchDrive(devicename, deviceunit, dmport))))
			{
				goto error;
			}
		}
	}

	rc = RETURN_OK;

	while (TRUE) {
		sigs = (1UL << BrokerPort->mp_SigBit);
		sigs = IExec->Wait(sigs|SIGBREAKF_CTRL_C);

		if (sigs & SIGBREAKF_CTRL_C) {
			break;
		}

		if (sigs & (1UL << BrokerPort->mp_SigBit)) {
			CxMsg *msg;
			BOOL quit = FALSE;
			while (msg = (CxMsg *)IExec->GetMsg(BrokerPort)) {
				switch (ICommodities->CxMsgType(msg)) {
					case CXM_COMMAND:
						switch (ICommodities->CxMsgID(msg)) {
							case CXCMD_DISABLE:
								ICommodities->ActivateCxObj(Broker, FALSE);
								break;
							case CXCMD_ENABLE:
								ICommodities->ActivateCxObj(Broker, TRUE);
								break;
							case CXCMD_UNIQUE:
							case CXCMD_KILL:
								quit = TRUE;
								break;
						}
						break;
				}
				IExec->ReplyMsg((struct Message *)msg);
			}
			if (quit) {
				break;
			}
		}
	}

error:
	for (i = 0; i < num_devices; i++) {
		UnPatchDevice(&devices[i]);
	}
	IExec->FreeSysObject(ASOT_MEMPOOL, mempool);
	IExec->DropInterface((struct Interface *)ICDPlayer);
	IExec->CloseLibrary(CDPlayerBase);
	IExec->FreeSysObject(ASOT_PORT, ioport);
	IExec->FreeSysObject(ASOT_PORT, dmport);
	UnregisterCxBroker();
	IIcon->FreeDiskObject(Icon);

	return rc;
}

static struct PatchedDevice *DeviceIsPatched (CONST_STRPTR devicename) {
	int32 i;
	for (i = 0; i < num_devices; i++) {
		if (strcmp(devices[i].name, devicename) == 0) {
			return &devices[i];
		}
	}
	return NULL;
}

static struct PatchedDevice *PatchDevice (CONST_STRPTR devicename, uint32 deviceunit, struct MsgPort *ioport) {
	struct PatchedDevice *device = &devices[num_devices++];
	uint32 *opcodes;
	device->name = devicename;
	device->ior = IExec->AllocSysObjectTags(ASOT_IOREQUEST,
		ASOIOR_ReplyPort,	ioport,
		ASOIOR_Size,		sizeof(struct IOExtTD),
		TAG_END);
	if (!device->ior) {
		return NULL;
	}
	if (IExec->OpenDevice(devicename, deviceunit, device->ior, 0) != IOERR_SUCCESS) {
		device->ior->io_Device = NULL;
		return NULL;
	}
	device->interface = IExec->GetInterface(&device->ior->io_Device->dd_Library,
		"__device", 1, NULL);
	if (!device->interface) {
		return NULL;
	}
	device->newbeginio = IExec->AllocVecTags(6 * sizeof(uint32),
		AVT_Type,	MEMF_EXECUTABLE,
		TAG_END);
	if (!device->newbeginio) {
		return NULL;
	}
	opcodes = device->newbeginio;
	opcodes[0] = 0x3ca00000; // lis   r5,0
	opcodes[1] = 0x60a50000; // ori   r5,r5,0
	opcodes[2] = 0x3cc00000; // lis   r6,0
	opcodes[3] = 0x60c60000; // ori   r6,r6,0
	opcodes[4] = 0x7cc903a6; // mtctr r6
	opcodes[5] = 0x4e800420; // bctr
	IExec->Forbid();
	device->oldbeginio = IExec->SetMethod(device->interface,
		offsetof(struct DeviceManagerInterface, BeginIO), device->newbeginio);
	opcodes[0] |= ((uint32)device->oldbeginio >> 16);
	opcodes[1] |= ((uint32)device->oldbeginio & 0xffff);
	opcodes[2] |= ((uint32)NewBeginIO >> 16);
	opcodes[3] |= ((uint32)NewBeginIO & 0xffff);
	IExec->Permit();
	if (!device->oldbeginio) {
		return NULL;
	}
	return device;
}

static void UnPatchDevice (struct PatchedDevice *device) {
	if (device->ior) {
		if (device->ior->io_Device) {
			if (device->interface) {
				if (device->newbeginio) {
					if (device->oldbeginio) {
						IExec->SetMethod(device->interface,
							offsetof(struct DeviceManagerInterface, BeginIO), device->oldbeginio);
					}
					IExec->FreeVec(device->newbeginio);
				}
				IExec->DropInterface(device->interface);
			}
			IExec->CloseDevice(device->ior);
		}
		IExec->FreeSysObject(ASOT_IOREQUEST, device->ior);
	}
}

static struct PatchedDrive *DriveIsPatched (CONST_STRPTR devicename, uint32 deviceunit) {
	int32 i;
	for (i = 0; i < num_drives; i++) {
		if (strcmp(drives[i].devicename, devicename) == 0 &&
			drives[i].deviceunit == deviceunit)
		{
			return &drives[i];
		}
	}
	return NULL;
}

static struct PatchedDrive *PatchDrive (CONST_STRPTR devicename, uint32 deviceunit, struct MsgPort *dmport) {
	struct PatchedDrive *drive = &drives[num_drives++];
	drive->devicename = devicename;
	drive->deviceunit = deviceunit;
	return drive;
}

static void NewBeginIO (struct DeviceManagerInterface *Self, struct IOStdReq *ior,
	void (*OldBeginIO) (struct DeviceManagerInterface *Self, struct IOStdReq *ior))
{
	if (ior->io_Command == HD_SCSICMD) {
		struct SCSICmd *scsi = ior->io_Data;
		const UBYTE *cmd = scsi->scsi_Command;
		struct IOStdReq *cdda_io;
		switch (cmd[0]) {
			case SCSICMD_READ_SUBCHANNEL:
				if (scsi->scsi_CmdLength >= 10 && (cmd[1] == 0x00 || cmd[1] == 0x02) && cmd[2] == 0x40 &&
					cmd[3] == 0x01 && cmd[4] == 0 && cmd[5] == 0 && cmd[6] == 0 && cmd[9] == 0)
				{
					if (cdda_io = CreateCDDAIORequest(ior)) {
						BytePackBuffer data;
						BOOL active;
						uint32 track;
						uint8 status;
						struct CD_Time cdt;
			
						active = ICDPlayer->CDActive(cdda_io);
						track = ICDPlayer->CDCurrentTitle(cdda_io);
						ICDPlayer->CDTitleTime(&cdt, cdda_io);
						DeleteCDDAIORequest(cdda_io);

						BytePackInit(&data, scsi->scsi_Data, scsi->scsi_Length);

						status = active ? 0x11 : (track ? 0x12 : 0x13);
						BytePackWrite8(&data, 0);
						BytePackWrite8(&data, status);
						BytePackWrite16MSB(&data, 12);
						BytePackWrite8(&data, 0x01);
						BytePackWrite8(&data, 0x10);
						BytePackWrite8(&data, track);
						BytePackWrite8(&data, 0);
						if (cmd[1] & 2) {
							uint8 m, s, f;
							ADDR2MSF(cdt.cdt_AllCurBase, m, s, f);
							BytePackWrite8(&data, 0);
							BytePackWrite8(&data, m);
							BytePackWrite8(&data, s);
							BytePackWrite8(&data, f);
							ADDR2MSF(cdt.cdt_TrackCurBase, m, s, f);
							BytePackWrite8(&data, 0);
							BytePackWrite8(&data, m);
							BytePackWrite8(&data, s);
							BytePackWrite8(&data, f);
						} else {
							BytePackWrite32MSB(&data, cdt.cdt_AllCurBase);
							BytePackWrite32MSB(&data, cdt.cdt_TrackCurBase);
						}

						ior->io_Error = IOERR_SUCCESS;
						scsi->scsi_Actual = BytePackBytesWritten(&data);
						scsi->scsi_CmdActual = 10;
						scsi->scsi_Status = 0;
						scsi->scsi_SenseActual = 0;
						if (!(ior->io_Flags & IOF_QUICK)) {
							IExec->ReplyMsg(&ior->io_Message);
						}
						return;
					}
				}
				break;
			case SCSICMD_PLAY_AUDIO:
				if (scsi->scsi_CmdLength >= 10 && cmd[1] == 0 && cmd[6] == 0 && cmd[9] == 0) {
					if (cdda_io = CreateCDDAIORequest(ior)) {
						BytePackBuffer sense;
						uint32 startaddr = rbe32(&cmd[2]);
						uint32 endaddr = startaddr + rbe16(&cmd[7]);

						if (scsi->scsi_Flags & SCSIF_AUTOSENSE) {
							BytePackInit(&sense, scsi->scsi_SenseData, scsi->scsi_SenseLength);
						} else {
							BytePackInit(&sense, NULL, 0);
						}

						if (CheckCDDAAddress(scsi, startaddr, cdda_io, &sense)) {
							ICDPlayer->CDPlayAddr(startaddr, endaddr, cdda_io);
							DeleteCDDAIORequest(cdda_io);

							ior->io_Error = IOERR_SUCCESS;
						} else {
							ior->io_Error = HFERR_BadStatus;
						}
						scsi->scsi_Actual = 0;
						scsi->scsi_CmdActual = 12;
						scsi->scsi_Status = 0;
						scsi->scsi_SenseActual = BytePackBytesWritten(&sense);
						if (!(ior->io_Flags & IOF_QUICK)) {
							IExec->ReplyMsg(&ior->io_Message);
						}
						return;
					}
				}
				break;
			case SCSICMD_PLAY_AUDIO_MSF:
				if (scsi->scsi_CmdLength >= 10 && cmd[1] == 0 && cmd[2] == 0 && cmd[9] == 0) {
					if (cdda_io = CreateCDDAIORequest(ior)) {
						BytePackBuffer sense;
						uint32 startaddr = MSF2ADDR(cmd[3], cmd[4], cmd[5]);
						uint32 endaddr = MSF2ADDR(cmd[6], cmd[7], cmd[8]);

						IExec->DebugPrintF("start: %02ld:%02ld:%02ld end: %02ld:%02ld:%02ld\n",
							 cmd[3], cmd[4], cmd[5], cmd[6], cmd[7], cmd[8]);

						if (scsi->scsi_Flags & SCSIF_AUTOSENSE) {
							BytePackInit(&sense, scsi->scsi_SenseData, scsi->scsi_SenseLength);
						} else {
							BytePackInit(&sense, NULL, 0);
						}

						if (CheckCDDAAddress(scsi, startaddr, cdda_io, &sense)) {
							ICDPlayer->CDPlayAddr(startaddr, endaddr, cdda_io);
							DeleteCDDAIORequest(cdda_io);

							ior->io_Error = IOERR_SUCCESS;
						} else {
							ior->io_Error = HFERR_BadStatus;
						}
						scsi->scsi_Actual = 0;
						scsi->scsi_CmdActual = 10;
						scsi->scsi_Status = 0;
						scsi->scsi_SenseActual = BytePackBytesWritten(&sense);
						if (!(ior->io_Flags & IOF_QUICK)) {
							IExec->ReplyMsg(&ior->io_Message);
						}
						return;
					}
				}
				break;
			case SCSICMD_PAUSE_RESUME:
				if (scsi->scsi_CmdLength >= 10 && cmd[1] == 0 && cmd[2] == 0 && cmd[3] == 0 &&
					cmd[4] == 0 && cmd[5] == 0 && cmd[6] == 0 && cmd[7] == 0 && (cmd[8] & 0xfe) == 0 &&
					cmd[9] == 0)
				{
					if (cdda_io = CreateCDDAIORequest(ior)) {
						ICDPlayer->CDResume(!cmd[8], cdda_io);
						DeleteCDDAIORequest(cdda_io);

						ior->io_Error = IOERR_SUCCESS;
						scsi->scsi_Actual = 0;
						scsi->scsi_CmdActual = 10;
						scsi->scsi_Status = 0;
						scsi->scsi_SenseActual = 0;
						if (!(ior->io_Flags & IOF_QUICK)) {
							IExec->ReplyMsg(&ior->io_Message);
						}
						return;
					}
				}
				break;
			case SCSICMD_STOP_PLAY_SCAN:
				if (scsi->scsi_CmdLength >= 10 && cmd[1] == 0 && cmd[2] == 0 && cmd[3] == 0 &&
					cmd[4] == 0 && cmd[5] == 0 && cmd[6] == 0 && cmd[7] == 0 && cmd[8] == 0 &&
					cmd[9] == 0)
				{
					if (cdda_io = CreateCDDAIORequest(ior)) {
						ICDPlayer->CDStop(cdda_io);
						DeleteCDDAIORequest(cdda_io);

						ior->io_Error = IOERR_SUCCESS;
						scsi->scsi_Actual = 0;
						scsi->scsi_CmdActual = 10;
						scsi->scsi_Status = 0;
						scsi->scsi_SenseActual = 0;
						if (!(ior->io_Flags & IOF_QUICK)) {
							IExec->ReplyMsg(&ior->io_Message);
						}
						return;
					}
				}
				break;
			case SCSICMD_PLAY_AUDIO_12:
				if (scsi->scsi_CmdLength >= 12 && cmd[1] == 0 && cmd[10] == 0 && cmd[11] == 0) {
					if (cdda_io = CreateCDDAIORequest(ior)) {
						BytePackBuffer sense;
						uint32 startaddr = rbe32(&cmd[2]);
						uint32 endaddr = startaddr + rbe32(&cmd[6]);

						if (scsi->scsi_Flags & SCSIF_AUTOSENSE) {
							BytePackInit(&sense, scsi->scsi_SenseData, scsi->scsi_SenseLength);
						} else {
							BytePackInit(&sense, NULL, 0);
						}

						if (CheckCDDAAddress(scsi, startaddr, cdda_io, &sense)) {
							ICDPlayer->CDPlayAddr(startaddr, endaddr, cdda_io);
							DeleteCDDAIORequest(cdda_io);

							ior->io_Error = IOERR_SUCCESS;
						} else {
							ior->io_Error = HFERR_BadStatus;
						}
						scsi->scsi_Actual = 0;
						scsi->scsi_CmdActual = 12;
						scsi->scsi_Status = 0;
						scsi->scsi_SenseActual = BytePackBytesWritten(&sense);
						if (!(ior->io_Flags & IOF_QUICK)) {
							IExec->ReplyMsg(&ior->io_Message);
						}
						return;
					}
				}
				break;
			case SCSICMD_START_STOP_UNIT:
				if (scsi->scsi_CmdLength >= 6 && (cmd[1] & 0xfe) == 0 && cmd[2] == 0 &&
					cmd[3] == 0 && cmd[4] == 0 && cmd[5] == 0)
				{
					if (cdda_io = CreateCDDAIORequest(ior)) {
						ICDPlayer->CDStop(cdda_io);
						DeleteCDDAIORequest(cdda_io);
					}
				}
				break;
			case SCSICMD_MODE_SENSE:
				if (scsi->scsi_CmdLength >= 6 && (cmd[1] & 0xf7) == 0 && cmd[2] == 0x0e &&
					cmd[3] == 0 && cmd[5] == 0)
				{
					if (cdda_io = CreateCDDAIORequest(ior)) {
						BytePackBuffer data;
						struct CD_Volume vol;

						ICDPlayer->CDGetVolume(&vol, cdda_io);
						DeleteCDDAIORequest(cdda_io);

						BytePackInit(&data, scsi->scsi_Data, scsi->scsi_Length);

						BytePackWrite8(&data, 0x13);
						BytePackWrite8(&data, 0x70);
						BytePackWrite8(&data, 0x00);
						BytePackWrite8(&data, 0x00);
						BytePackWrite8(&data, 0x0e);
						BytePackWrite8(&data, 0x0e);
						BytePackWrite8(&data, 0x04);
						BytePackWrite8(&data, 0x00);
						BytePackWrite8(&data, 0x00);
						BytePackWrite8(&data, 0x00);
						BytePackWrite8(&data, 0x00);
						BytePackWrite8(&data, 0x4b);
						BytePackWrite8(&data, 0x01);
						BytePackWrite8(&data, vol.cdv_Chan0);
						BytePackWrite8(&data, 0x02);
						BytePackWrite8(&data, vol.cdv_Chan1);
						BytePackWrite8(&data, 0x04);
						BytePackWrite8(&data, vol.cdv_Chan2);
						BytePackWrite8(&data, 0x08);
						BytePackWrite8(&data, vol.cdv_Chan3);

						ior->io_Error = IOERR_SUCCESS;
						scsi->scsi_Actual = BytePackBytesWritten(&data);
						scsi->scsi_CmdActual = 6;
						scsi->scsi_Status = 0;
						scsi->scsi_SenseActual = 0;
						if (!(ior->io_Flags & IOF_QUICK)) {
							IExec->ReplyMsg(&ior->io_Message);
						}
						return;
					}
				}
				break;
			case SCSICMD_MODE_SELECT:
				if (scsi->scsi_CmdLength >= 6 && cmd[1] == 0x10 && cmd[2] == 0 &&
					cmd[3] == 0 && cmd[4] == 20 && cmd[5] == 0)
				{
					if (cdda_io = CreateCDDAIORequest(ior)) {
						UBYTE *data = (UBYTE *)scsi->scsi_Data;
						struct CD_Volume vol;

						vol.cdv_Chan0 = data[13];
						vol.cdv_Chan1 = data[15];
						vol.cdv_Chan2 = data[17];
						vol.cdv_Chan3 = data[19];

						ICDPlayer->CDSetVolume(&vol, cdda_io);
						DeleteCDDAIORequest(cdda_io);

						ior->io_Error = IOERR_SUCCESS;
						scsi->scsi_Actual = 20;
						scsi->scsi_CmdActual = 6;
						scsi->scsi_Status = 0;
						scsi->scsi_SenseActual = 0;
						if (!(ior->io_Flags & IOF_QUICK)) {
							IExec->ReplyMsg(&ior->io_Message);
						}
						return;
					}
				}
				break;
		}
	}
	OldBeginIO(Self, ior);
}

static struct IOStdReq *CreateCDDAIORequest (struct IOStdReq *ior) {
	struct MsgPort *cdda_mp;
	struct IOStdReq *cdda_io;
	if ((cdda_mp = IExec->AllocSysObject(ASOT_PORT, NULL)) &&
		(cdda_io = IExec->AllocSysObjectTags(ASOT_IOREQUEST,
			ASOIOR_Duplicate,	ior,
			ASOIOR_Size,		sizeof(struct IOStdReq),
			ASOIOR_ReplyPort,	cdda_mp,
			TAG_END)))
	{
		return cdda_io;
	}
	IExec->FreeSysObject(ASOT_PORT, cdda_mp);
	return NULL;
}

static void DeleteCDDAIORequest (struct IOStdReq *cdda_io) {
	if (cdda_io) {
		struct MsgPort *cdda_mp = cdda_io->io_Message.mn_ReplyPort;
		IExec->FreeSysObject(ASOT_IOREQUEST, cdda_io);
		IExec->FreeSysObject(ASOT_PORT, cdda_mp);
	}
}

static void WriteSenseData (BytePackBuffer *sense, uint8 sensekey, uint8 asc, uint8 ascq) {
	BytePackWrite8(sense, 0x70);
	BytePackWrite8(sense, 0);
	BytePackWrite8(sense, sensekey);
	BytePackWrite32MSB(sense, 0);
	BytePackWrite8(sense, 10);
	BytePackWrite32MSB(sense, 0);
	BytePackWrite8(sense, asc);
	BytePackWrite8(sense, ascq);
	BytePackWrite8(sense, 0);
	BytePackWrite24MSB(sense, 0);
}

static BOOL CheckCDDAAddress (struct SCSICmd *scsi, uint32 addr, struct IOStdReq *cdda_io, BytePackBuffer *sense) {
	struct CD_TOC toc;
	uint32 track;
	BOOL res = FALSE;
	if (ICDPlayer->CDReadTOC(&toc, cdda_io) == IOERR_SUCCESS && toc.cdc_NumTracks > 0 &&
		(track = GetTrackNumber(&toc, addr)))
	{
		IExec->DebugPrintF("track: %ld flags: %ld\n", track, toc.cdc_Flags[track-1]);
		if (toc.cdc_Flags[track-1] == 0) {
			res = TRUE;
		} else {
			scsi->scsi_Status = SCSI_CheckCondition;
			WriteSenseData(sense, SENSEKEY_IllegalRequest, 0x64, 0x00);
		}
	} else {
		scsi->scsi_Status = SCSI_CheckCondition;
		WriteSenseData(sense, SENSEKEY_IllegalRequest, 0x21, 0x00);
	}
	return res;
}

static uint32 GetTrackNumber (const struct CD_TOC *toc, const uint32 addr) {
	const uint32 num_tracks = toc->cdc_NumTracks;
	uint32 track;
	for (track = 0; track < num_tracks; track++) {
		if (addr >= toc->cdc_Addr[track] && addr < toc->cdc_Addr[track + 1]) {
			return track + 1;
		}
	}
	return 0;
}
