#include "dvd_io.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "config.h"
#include "ka_error.h"
#include "ka_log.h"
#include "ka_mem.h"

#include "swis.h"

typedef struct
{
	uint32_t	r[5];
} CDControlBlock_t;

struct dvd_io_s
{
  ka_error_t* pErrorBlock;
  // DVD device
  CDControlBlock_t CDBlock;
  uint32_t scsiDeviceId;
  uint8_t  scsiCb[12];
  uint8_t  agid;               // Authentication Grant ID
  uint8_t  dvdFlags;
  dvd_key_t busKey;
  dvd_key_t discKey;
  dvd_key_t titleKey;
  // Word aligned so buffer can be reused as parameter block
  uint32_t dummy;
  uint8_t  buffer[DVD_BLOCK_SIZE + 4];
  uint32_t discKeysHdr;
  uint8_t  discKeys[DVD_DISCKEYS_SIZE]; // 5-byte hash of decrypted disc key + 408 keys encrypted by player lkey
};

#define DVD_flag_opCDFS      0x80
#define DVD_flag_opSCSI      0x40
#define DVD_flag_isProtected 0x01
#define DVD_flag_hasAgid     0x02
#define DVD_flag_hasDiscKey  0x04
#define DVD_flag_wrongRegion 0x08

#define CMD_CB_LEN 12 // Valid for all commands we use
#define CMD_READ_DISC_TRUCTURE 0xAD
#define CMD_REPORT_KEY         0xA4
#define CMD_SEND_KEY           0xA3

/**
 * Checks how to access DVD.
 */
static int setDccessMethod(dvd_io_t* io)
{
  io->dvdFlags &= ~(DVD_flag_opCDFS | DVD_flag_opSCSI);

  // CDFSDriver before 2.42 has a broken SCSIUserOp
  if (rm_version("CDFSDriver") >= 242)
  {
    io->dvdFlags |= DVD_flag_opCDFS;
    return 1;
  }

  // Use SCSI_Op directly? Ok, but what if it's not a SCSI drive ?
  uint8_t buf[16];

  // See source of CDFS_SCSIUserOp
  io->scsiDeviceId = io->CDBlock.r[0]       // device id
                   | io->CDBlock.r[1] >> 3  // card nr
                   | io->CDBlock.r[2] >> 5; // lun
  // Use SCSI 'Determine Device' to check if known device
  if (_swix(SCSI_Initialise, _INR(0, 2), 2, io->scsiDeviceId, buf) == 0)
  {
    io->dvdFlags |= DVD_flag_opSCSI;
    return 1;
  }

  // Invalid SCSI device ID.
  // Probably an ATAPI ATAPI but there is no standard ATAPI module,
  //  so dunno how to access them.
  return 0;
}

/**
 * Perform CSS related SCSI commands (all the commands use a 12 bytes control block).
 *
 * @param f     dvdin structure of which
 *               - CDBlock is the control block of the drive used
 *               - scsiCB  12 bytes, is prefilled with the operation parameters
 * @param buf   buffer for input/output
 * @param len   buffer len
 * @param read  non-0 if read operation, 0 if write operation
 */
static const _kernel_oserror* scsi_op(dvd_io_t* io, void* buf, int len, int read)
{
  const _kernel_oserror* err = NULL;
  uint32_t op;

  if (read)
    op = 1 << 24;
  else
    op = 2 << 24;
  io->scsiCb[8] = len >> 8;
  io->scsiCb[9] = len;

  if (io->dvdFlags & DVD_flag_opCDFS)
  {
    err = _swix( CD_SCSIUserOp, _INR(0,7)
               , op, CMD_CB_LEN, io->scsiCb, buf, len, 0, 0, &io->CDBlock
               );
  }
  else
  {
    op |= io->scsiDeviceId;
    op |= 1 << 27; // escape poll off
    err = _swix( SCSI_Op, _INR(0,8)
               , op, CMD_CB_LEN, io->scsiCb, buf, len, 0, 0, &io->CDBlock, 1
               );
  }

  return err;
}

/**
 * If we have a valid AGID, invalidate it.
 */
static void invalidateAuthenticationGrantId(dvd_io_t* io)
{
  if (io->dvdFlags & DVD_flag_hasAgid)
  {
    if (config.debug & cfg_printnavstats)
      ka_log(ka_log_nav, "DVD InvalidateAuthenticationGrantId");

    memset(io->scsiCb, 0, CMD_CB_LEN);
    io->scsiCb[0] = CMD_REPORT_KEY;
    io->scsiCb[7] = 0; // KEY CLASS CSS/CPPM
    io->scsiCb[10] = io->agid | 0x3f; // Invalidate AGID
    scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
    io->dvdFlags &= ~DVD_flag_hasAgid;
  }
}

/**
 * Invalidate all possible AGIDs.
 */
static void invalidateAllAuthenticationGrantIds(dvd_io_t* io)
{
  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD InvalidateAllAuthenticationGrantIds");

  for (int i = 0; i < 4; i++)
  {
    memset(io->scsiCb, 0, CMD_CB_LEN);
    io->scsiCb[0] = CMD_REPORT_KEY;
    io->scsiCb[7] = 0; // KEY CLASS CSS/CPPM
    io->scsiCb[10] = (i << 6) | 0x3f; // Invalidate AGID
    scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  }
}

/**
 * Read disc copyright info and akkiwed regions.
 */
static const _kernel_oserror* readCopyright(dvd_io_t* io, unsigned int* pprot, unsigned int* pregs)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReadCopyright");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_READ_DISC_TRUCTURE;
  io->scsiCb[1] = 0; // Media type DVD
  io->scsiCb[6] = 0; // Layer 0
  io->scsiCb[7] = 1; // Copyright protection
  err = scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  if (err != NULL) return err;

  *pprot = io->buffer[4]; // 0 no, 1 CSS/CPPM, 2 CPRM, 16 AACS (BD)
  *pregs = io->buffer[5]; // 1 bit per region, off = not playable in that region

  return NULL;
}

/**
 * Read encrypted list of potential disc keys.
 */
static const _kernel_oserror* readDiscKeys(dvd_io_t* io)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReadDiscKey");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_READ_DISC_TRUCTURE;
  io->scsiCb[1] = 0; // Media type DVD
  io->scsiCb[7] = 2; // Disc Key
  io->scsiCb[10] = io->agid;
  err = scsi_op(io, &io->discKeysHdr, 4 + DVD_DISCKEYS_SIZE, 1);
  if (err != NULL) return err;

  return NULL;
}

/**
 * Get an Authentication Grant ID (AGID), CSS/CPPM version.
 */
static const _kernel_oserror* reportAuthenticationGrantId(dvd_io_t* io)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReportAuthenticationGrantId");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_REPORT_KEY;
  io->scsiCb[7] = 0; // KEY CLASS CSS/CPPM
  io->scsiCb[10] = 0; // KEY FORMAT = AGID for CSS/CPPM
  err = scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  if (err != NULL) return err;

  io->agid = io->buffer[7] & 0xC0;
  io->dvdFlags |= DVD_flag_hasAgid;

  return NULL;
}

/**
 * Authentication Grant ID (AGID), CPRM veersion.
 *//*
static const _kernel_oserror* reportAuthenticationGrantIdForCPRM(dvd_io_t* io)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReportAuthenticationGrantIdForCPRM");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_REPORT_KEY;
  io->scsiCb[7] = 0; // KEY CLASS CSS/CPPM
  io->scsiCb[10] = 17; // KEY FORMAT = AGID for CPRM
  err = scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  if (err != NULL) return err;

  io->agid = io->buffer[7] & 0xC0;
  io->dvdFlags |= DVD_flag_hasAgid;

  return NULL;
}
*/

/**
 * Send a challenge to the drive (during authentication process).
 */
static const _kernel_oserror* sendChallengeKey(dvd_io_t* io, const uint8_t challenge[10])
{
  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD SendChallengeKey");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_SEND_KEY;
  io->scsiCb[7] = 0; // KEY CLASS CSS/CPPM
  io->scsiCb[10] = io->agid | 1; // Challenge Key

  io->buffer[0] = 0;
  io->buffer[1] = 0xe;
  io->buffer[2] = 0;
  io->buffer[3] = 0;
  for (int i = 0; i < 10; i++)
    io->buffer[i + 4] = challenge[9-i];
  io->buffer[14] = 0;
  io->buffer[15] = 0;

  return scsi_op(io, io->buffer, 16, 0);
}

/**
 * Retrieve a challenge from the drive (during authentication process).
 */
static const _kernel_oserror* reportChallengeKey(dvd_io_t* io, uint8_t challenge[10])
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReportChallengeKey");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_REPORT_KEY;
  io->scsiCb[7] = 0; // KEY CLASS CSS/CPPM
  io->scsiCb[10] = io->agid | 1; // Challenge Key
  err = scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  if (err != NULL) return err;

  for (int i = 0; i < 10; i++)
    challenge[9-i] = io->buffer[i + 4];

  return NULL;
}

/**
 * Retrieve encrypted key corresponding to challenge sent to the drive (during authentication process).
 */
static const _kernel_oserror* reportKey1(dvd_io_t* io, dvd_key_t key1)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReportKey1");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_REPORT_KEY;
  io->scsiCb[7] = 0; // KEY CLASS CSS/CPPM
  io->scsiCb[10] = io->agid | 2; // Key1
  err = scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  if (err != NULL) return err;

  key1[0] = io->buffer[8];
  key1[1] = io->buffer[7];
  key1[2] = io->buffer[6];
  key1[3] = io->buffer[5];
  key1[4] = io->buffer[4];

  return NULL;
}

/**
 * Send encrypted key corresponding to challenge retrieved from the drive (during authentication process).
 */
static const _kernel_oserror* sendKey2(dvd_io_t* io, const dvd_key_t key2)
{
  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD SendKey2");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_SEND_KEY;
  io->scsiCb[7] = 0; // KEY CLASS CSS
  io->scsiCb[10] = io->agid | 3; // Key2

  io->buffer[0] = 0;
  io->buffer[1] = 0xa;
  io->buffer[2] = 0;
  io->buffer[3] = 0;
  io->buffer[4] = key2[4];
  io->buffer[5] = key2[3];
  io->buffer[6] = key2[2];
  io->buffer[7] = key2[1];
  io->buffer[8] = key2[0];
  io->buffer[9] = 0;
  io->buffer[10] = 0;
  io->buffer[11] = 0;

  return scsi_op(io, io->buffer, 12, 0);
}

/**
 * Retrieve encrypted title key for a given drive lba position.
 */
static const _kernel_oserror* reportTitleKey(dvd_io_t* io, uint32_t lba, dvd_key_t tkey)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReportTitleKey");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_REPORT_KEY;
  io->scsiCb[2] = lba >> 24;
  io->scsiCb[3] = lba >> 16;
  io->scsiCb[4] = lba >>  8;
  io->scsiCb[5] = lba;
  io->scsiCb[7] = 0; // KEY CLASS CSS
  io->scsiCb[10] = io->agid | 4; // Title Key
  err = scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  if (err != NULL) return err;

  memcpy(tkey, io->buffer + 5, DVD_KEY_SIZE);

  return NULL;
}

/**
 * Check if authentication is still valid.
 */
static const _kernel_oserror* reportAuthenticationSuccessFlag(dvd_io_t* io, int* flag)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReportAuthenticationSuccessFlag");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_REPORT_KEY;
  io->scsiCb[7] = 0; // KEY CLASS CSS
  io->scsiCb[10] = io->agid | 5; // Authentication Success Flag, ASF
  err = scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  if (err != NULL) return err;

  *flag = io->buffer[7] & 1;

  return NULL;
}

/**
 * Retrieve prefered region code structure.
 */
static const _kernel_oserror* reportPreferedRegionCodeStructure(dvd_io_t* io, uint32_t* regionState)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD ReportPreferedRegionCodeStructure");

  memset(io->scsiCb, 0, CMD_CB_LEN);
  io->scsiCb[0] = CMD_REPORT_KEY;
  io->scsiCb[7] = 0; // KEY CLASS CSS
  io->scsiCb[10] = io-> agid | 8; // Prefered Region Code
  err = scsi_op(io, io->buffer, DVD_BLOCK_SIZE, 1);
  if (err != NULL) return err;

  *regionState = (io->buffer[4] << 24)
               | (io->buffer[5] << 16)
               | (io->buffer[6] <<  8)
               | io->buffer[7];

  return NULL;
}

/*
 * Obtain Authentication Grant Id (AGID).
 */
static const _kernel_oserror* getAuthenticationGrantId(dvd_io_t* io)
{
  const _kernel_oserror* err = NULL;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD GetAuthenticationGrantId");

  // Release existing AGID if any
  invalidateAuthenticationGrantId(io);

  // Request an AGID
  err = reportAuthenticationGrantId(io);
  if (err != NULL)
  {
    // Release all AGIDS and retry
    invalidateAllAuthenticationGrantIds(io);
    err = reportAuthenticationGrantId(io);
  }

  return err;
}

/*
 * Obtain session/bus key.
 */
static const _kernel_oserror* getBusKey(dvd_io_t* io)
{
  const _kernel_oserror* err = NULL;
  unsigned int variant;
  dvd_key_t key1, key2;
  uint8_t  challenge[10];

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD GetBusKey");

  for (int i = 0; i < 10; i++)
    challenge[i] = i;

  // Send challenge
  err = sendChallengeKey(io, challenge);
  if (err != NULL)
    goto error;

  // Get corresponding key
  err = reportKey1(io, key1);
  if (err != NULL)
    goto error;

  // Determine variant
  for (variant = 0; variant < 32; variant++)
  {
    css_cryptKey(0, variant, challenge, key2);
    if (!memcmp(key1, key2, DVD_KEY_SIZE))
      break;
  }

  if (variant >= 32)
  {
    err = ka_error_fill(io->pErrorBlock, "error205");
    goto error;
  }

  // Obtain challenge
  err = reportChallengeKey(io, challenge);
  if (err != NULL)
    goto error;

  // Generate key2 corresponding to challenge and variant
  css_cryptKey(1, variant, challenge, key2);

  // Send key2 so that the drive trusts us
  err = sendKey2(io, key2);
  if (err != NULL)
    goto error;

  // Encrypt key1 from variant to obtain bus key
  memcpy(challenge, key1, 5);
  memcpy(challenge + 5, key2, 5);
  css_cryptKey(2, variant, challenge, io->busKey); // key1 + key2 -> busKey

  return NULL;

error:
  return err;
}

static const _kernel_oserror* getDiscKey(dvd_io_t* io)
{
  const _kernel_oserror* err = NULL;
  int i, flag;

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD GetDiscKey");

  // Read list of possible disc keys
  err = readDiscKeys(io);
  if (err) goto error;

  // Check that we are still trusted
  err = reportAuthenticationSuccessFlag(io, &flag);
  if (err) goto error;

  if (!flag)
  {
    err = ka_error_fill(io->pErrorBlock, "error206");
    goto error;
  }

  // Shuffle disc key using the bus key
  for(i = 0 ; i < DVD_DISCKEYS_SIZE; i++)
  {
    io->discKeys[i] ^= io->busKey[4 - (i % DVD_KEY_SIZE)];
  }

  if (css_decryptDiscKey(io->discKeys, io->discKey))
  {
    io->dvdFlags |= DVD_flag_hasDiscKey;
  }
  else
  {
    err = ka_error_fill(io->pErrorBlock, "error207");
    goto error;
  }

  return NULL;

error:
  return err;
}

/*****************************************************************************
 * CrackTitleKey: try to crack title key from the contents of a VOB.
 *****************************************************************************
 * This function is called by dvdcss_titlekey to find a title key, if we've
 * chosen to crack title key instead of decrypting it with the disc key.
 * The DVD should have been opened and be in an authenticated state.
 * i_pos is the starting sector, i_len is the maximum number of sectors to read
 *****************************************************************************/

static const _kernel_oserror* crackTitleKey(dvd_io_t* io, uint32_t pos, uint32_t len, dvd_key_t titlekey)
{
  const _kernel_oserror* err = NULL;
  int reads = 0;
  int encrypted = 0;
  int tries = 0;
  int success = 0;
  uint32_t tlen;
  uint32_t spos = pos;

  if (len > 2000)
    len = 2000;
  tlen = len;

  do
  {
    err = dvd_io_readBlocks(io, pos, io->buffer, 1);
    if (err)
      break;

    // PES_scrambling_control does not exist in a system_header,
    // a padding_stream or a private_stream2 (and others?).
    if (io->buffer[0x14] & 0x30  && ! (io->buffer[0x11] == 0xbb
                                    || io->buffer[0x11] == 0xbe
                                    || io->buffer[0x11] == 0xbf))
    {
      encrypted++;

      if (css_attackPattern(io->buffer, titlekey, &tries) > 0)
      {
        success = 1;
        break;
      }
    }

    pos++;
    len--;
    reads++;

    // Stop after 2000 blocks if we haven't seen any encrypted blocks.
    if (reads >= 2000 && encrypted == 0)
      break;

  } while (len > 0);

  if (success > 0)
  {
    if (config.debug & cfg_printnavstats)
      ka_log(ka_log_nav, "DVD TitleKey cracked");
    return NULL;
  }

  memset(titlekey, 0, DVD_KEY_SIZE);

  if (encrypted || err)
  {
    if (config.debug & cfg_printnavstats)
      ka_log(ka_log_nav, "DVD TitleKey cracking start LBA %08x read %d (max %d), encrypted %d", spos, reads, tlen, encrypted);
  }

  if (err)
  {
    if (config.debug & cfg_printnavstats)
      ka_log(ka_log_nav, "DVD TitleKey cracking read error: %08x %s", err->errnum, err->errmess);
    return err;
  }

  if (config.debug & cfg_printnavstats)
  {
    if (encrypted)
      ka_log(ka_log_nav, "DVD TitleKey cracking failed");
    else
      ka_log(ka_log_nav, "DVD No TitleKey");
  }

  return NULL;
}

static uint32_t read32(uint8_t* p)
{
  return p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);
}

/**
 * Get title ky corresponding to a given lba position.
 */
const _kernel_oserror* dvd_io_getTitleKey(dvd_io_t* io, uint32_t lbaStart, uint32_t lbaEnd, dvd_key_t titleKey)
{
  const _kernel_oserror* err = NULL;
  int i, flag;
  dvd_key_t key;

  if ((io->dvdFlags & DVD_flag_isProtected) == 0)
  {
    memset(titleKey, 0, sizeof(titleKey));
    return NULL;
  }

  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_nav, "DVD GetTitleKey");

  err = getAuthenticationGrantId(io);
  if (err) goto exit;

  err = getBusKey(io);
  if (err) goto exit;

  // Get encrypted title key
  err = reportTitleKey(io, lbaStart, titleKey);
  if (err)
  {
    if (config.debug & cfg_printnavstats)
      ka_log(ka_log_nav, "DVD title key error: %08x %s", err->errnum, err->errmess);

    invalidateAuthenticationGrantId(io);

    err = getAuthenticationGrantId(io);
    if (err) goto exit;

    err = getBusKey(io);
    if (err) goto exit;

    // Try cracking key
    err = crackTitleKey(io, lbaStart, lbaEnd - lbaStart, titleKey);
    if (err) goto exit;
  }
  else
  {
    // Shuffle the title key using the bus key
    for(i = 0 ; i < DVD_KEY_SIZE; i++)
    {
      key[i] = titleKey[i] ^ io->busKey[4 - (i % DVD_KEY_SIZE)];
    }

    // If p_key is all zero then there really wasn't any key present
    // even though we got to read it without an error.
    if(key[0] | key[1] | key[2] | key[3] | key[4])
    {
      // Decrypt the title key using the bys key
      css_decryptKey(0xff, io->discKey, key, titleKey);
    }
    else
    {
      if (config.debug & cfg_printnavstats)
        ka_log(ka_log_nav, "DVD No TitleKey");
    }
  }

  // Check that we are still trusted
  err = reportAuthenticationSuccessFlag(io, &flag);
  if (err) goto exit;

  if (!flag)
  {
    err = ka_error_fill(io->pErrorBlock, "error206");
    goto exit;
  }

exit:
  invalidateAuthenticationGrantId(io);

  return err;
}

int dvd_io_parseIso(dvd_io_t* io, dvd_desc_t* dvd)
{
  const _kernel_oserror* err = NULL;
  uint32_t lba;
  int size, i, j;
  uint8_t* p;
  uint8_t* end;

  // Find ISO9660 Primary Volume Descriptor
  lba = 16;

  do
  {
    err = _swix(CD_ReadData, _INR(0,4)|_IN(7), 0, lba, 1, io->buffer, DVD_BLOCK_SIZE, &io->CDBlock);
    if(err) goto exit;

    if ((strncmp((char*) io->buffer + 1, "CD001", 5))
    ||  (io->buffer[0] == 255))
    {
      err = ka_error_fill(io->pErrorBlock, "error208");
      goto exit;
    }
  } while (io->buffer[0] != 1);

  // Note: Volume is at [40, 40 + 32[ padded with blanks

  // Read root folder
  p = io->buffer + 156;
  lba = read32(p + 2);
  size = read32(p + 10);
  err = _swix(CD_ReadData, _INR(0,4)|_IN(7), 0, lba, 1, io->buffer, DVD_BLOCK_SIZE, &io->CDBlock);
  if(err) goto exit;

  // Locate VIDEO_TS folder
  p = io->buffer;
  end = p + DVD_BLOCK_SIZE;
  while (size > 0)
  {
    int len = p[0];

    if (p + len > end)
    {
      // Corrupted
      size = 0;
      break;
    }
    if (len > 32)
    {
      if ((p[32] == 8)
      &&  !strncmp((char*) p + 33, "VIDEO_TS",8))
        break;

      // Next entry
      size -= p[0];
      p += len;
    }
    else if (len)
    {
      // Corrupted
      size = 0;
      break;
    }
    else
    {
      // Zero padded sector (when not enough for 1 more entry or after last entry)
      size -= (end - p);
      p = end;
    }

    // Next sector ?
    if ((size > 0) && (p >= end))
    {
      lba += 1;
      err = _swix(CD_ReadData, _INR(0,4)|_IN(7), 0, lba, 1, io->buffer, DVD_BLOCK_SIZE, &io->CDBlock);
      if(err) goto exit;
      p = io->buffer;
    }
  }

  if (size <= 0)
  {
    err = ka_error_fill(io->pErrorBlock, "error208");
    goto exit;
  }

  // Read VIDEO_TS folder
  lba = read32(p + 2);
  size = read32(p + 10);
  err = _swix(CD_ReadData, _INR(0,4)|_IN(7), 0, lba, 1, io->buffer, DVD_BLOCK_SIZE, &io->CDBlock);
  if(err) goto exit;

  // Locate VIDEO_TS/IFO file
  p = io->buffer;
  end = p + DVD_BLOCK_SIZE;
  while (size > 0)
  {
    int len = p[0];

    if (p + len > end)
    {
      // Corrupted
      break;
    }
    if (len > 32)
    {
      char* name = (char*) (p + 33);
      dvd_file_desc_t* fi = NULL;

      // In 99% of DVDs, files are suffixed by ";1"
      if ((p[32] > 12) && (name[12] == ';'))
      {
        p[32] = 12;
        name[12] = 0;
      }

      if (p[32] == 12)
      {
        if (!stricmp("VIDEO_TS.BUP", name))
          fi = &dvd->ifos[0].bup;
        else if (!stricmp("VIDEO_TS.IFO", name))
          fi = &dvd->ifos[0].ifo;
        else if (!stricmp("VIDEO_TS.VOB", name))
          fi = &dvd->ifos[0].vobs[0];
        else
        {
          char c = name[4];
          name[4] = 0;

          if (!stricmp("VTS_", name))
          {
            name[4] = c;
            if ((name[4] >= '0') && (name[4] <= '9')
            &&  (name[5] >= '0') && (name[5] <= '9')
            &&  (name[6] = '_')
            &&  (name[7] >= '0') && (name[7] <= '9'))
            {
              i = 10 * (name[4] - '0') + (name[5] - '0');
              j = (name[7] - '0');

              if (!stricmp(".VOB", name + 8))
                fi = &dvd->ifos[i].vobs[j];
              else if (j == 0)
              {
                if (!stricmp(".BUP", name + 8))
                  fi = &dvd->ifos[i].bup;
                else if (!stricmp(".IFO", name + 8))
                  fi = &dvd->ifos[i].ifo;
              }
            }
          }

          name[4] = c;
        }
      }

      if (fi)
      {
        strncpy(fi->name, name, 12);
        fi->name[8] = '/'; // RISC OS
        fi->name[12] = 0;
        uint32_t flba = read32(p + 2);
        fi->start = flba * (uint64_t) DVD_BLOCK_SIZE;
        uint32_t flen = read32(p + 10);
        fi->size = flen;
        fi->end = fi->start + fi->size;
      }

      // Next entry
      size -= p[0];
      p += len;
    }
    else if (len)
    {
      // Corrupted
      break;
    }
    else
    {
      // Zero padded sector (when not enough for 1 more entry or after last entry)
      size -= (end - p);
      p = end;
    }

    // Next sector ?
    if ((size > 0) && (p >= end))
    {
      lba += 1;
      err = _swix(CD_ReadData, _INR(0,4)|_IN(7), 0, lba, 1, io->buffer, DVD_BLOCK_SIZE, &io->CDBlock);
      if(err) goto exit;
      p = io->buffer;
    }
  }

  dvd->nr_ifos = 0;
  for(i = 0; i <= DVD_IFOS_MAX; i++)
  {
    dvd_ifo_desc_t* idesc = &dvd->ifos[i];
    if (!idesc->ifo.size && !idesc->bup.size)
      break;
    dvd->nr_ifos++;

    for(j = 1; j <= DVD_VOBS_MAX; j++)
    {
      if (!idesc->vobs[j].size)
        break;
    }

    idesc->nr_vobs = j;
    idesc->vob_menu_start = idesc->vobs[0].start;
    idesc->vob_menu_end = idesc->vobs[0].end;
    idesc->vob_menu_size = idesc->vobs[0].size;
    if (j > 1)
    {
      idesc->vob_title_start = idesc->vobs[1].start;
      idesc->vob_title_end = idesc->vobs[j - 1].end;
      idesc->vob_title_size = idesc->vob_title_end - idesc->vob_title_start;
    }
    else
    {
      idesc->vob_title_start = 0l;
      idesc->vob_title_end = 0l;
      idesc->vob_title_size = 0l;
    }
  }

  dvd_desc_stats(dvd);

  if ((!dvd->ifos[0].ifo.size && !dvd->ifos[0].bup.size)
  ||  (!dvd->ifos[1].ifo.size && !dvd->ifos[1].bup.size)
  ||  (!dvd->ifos[1].vobs[1].size))
  {
    err = ka_error_fill(io->pErrorBlock, "error208");
    goto exit;
  }

exit:
  if (err != NULL)
  {
    if (err != io->pErrorBlock)
      *io->pErrorBlock = *err;
    return -1;
  }

  return 0;
}

static int loadIfo(dvd_io_t* io, dvd_file_desc_t* ifo, uint8_t** pp, uint8_t** ppend)
{
  const _kernel_oserror* err = NULL;
  uint32_t size = (uint32_t) ifo->size;
  uint32_t nr = size / DVD_BLOCK_SIZE;
  uint32_t lba =(uint32_t) (ifo->start / DVD_BLOCK_SIZE);
  uint8_t* p = ka_mem_alloc(size);
  if (p == NULL)
  {
    ka_error_fill(io->pErrorBlock, ka_error_nomem);
    return -1;
  }

  err = _swix(CD_ReadData, _INR(0,4)|_IN(7), 0, lba, nr, p, DVD_BLOCK_SIZE, &io->CDBlock);
  if(err != NULL)
  {
    sprintf(&io->pErrorBlock->errmess[0], "Could not read 0%d bytes from LBA %08x", size, lba);
    goto error;
  }

  *pp = p;
  *ppend = p + size;

  return 0;

error:
  ka_mem_free(p);
  return -1;
}

int dvd_io_loadVmgIfo(dvd_io_t* io, dvd_ifo_desc_t* ifo)
{
  uint8_t* buf = NULL;
  uint8_t* end;
  int ret = -1;

  if (!loadIfo(io, &ifo->ifo, &buf, &end))
  {
    if(!dvd_read_vmg_ifo(io->pErrorBlock, &ifo->vmg, buf, end))
    {
      ret = 0;
      goto exit;
    }
    ka_mem_free(buf);
    buf = NULL;
  }

  // Try with BUP
  if (!loadIfo(io, &ifo->bup, &buf, &end)
  &&  !dvd_read_vmg_ifo(io->pErrorBlock, &ifo->vmg, buf, end))
    ret = 0;

exit:
  if (buf) ka_mem_free(buf);

  return ret;
}

int dvd_io_loadVtsIfo(dvd_io_t* io, dvd_ifo_desc_t* ifo, int i)
{
  uint8_t* buf = NULL;
  uint8_t* end;
  int ret = -1;

  if (!loadIfo(io, &ifo->ifo, &buf, &end))
  {
    if(!dvd_read_vts_ifo(io->pErrorBlock, &ifo->vts, i, buf, end))
    {
      ret = 0;
      goto exit;
    }
    ka_mem_free(buf);
    buf = NULL;
  }

  // Try with BUP
  if (!loadIfo(io, &ifo->bup, &buf, &end)
  &&  !dvd_read_vts_ifo(io->pErrorBlock, &ifo->vts, i, buf, end))
    ret = 0;

exit:
  if (buf) ka_mem_free(buf);

  return ret;
}

dvd_io_t* dvd_io_new(ka_error_t* pErrorBlock, int drive)
{
  const _kernel_oserror* err = NULL;
  uint32_t val, prot, regs;
  dvd_io_t* io = ka_mem_calloc(sizeof(*io));

  if (!io)
  {
    ka_error_fill(pErrorBlock, ka_error_nomem);
    return NULL;
  }

  io->pErrorBlock = pErrorBlock;
  io->dvdFlags = 0;

  err = _swix(CDFS_ConvertDriveToDevice, _IN(0)|_OUT(1), drive, &val);
  if (err) goto exit;

  io->CDBlock.r[0] = (val & 0x00000007);
  io->CDBlock.r[1] = (val & 0x00000018) >> 3;
  io->CDBlock.r[2] = (val & 0x000000e0) >> 5;
  io->CDBlock.r[3] = (val & 0x0000ff00) >> 8;
  io->CDBlock.r[4] = (val & 0xffff0000) >> 16;

  // Check we can process the necessary commands
  if (!setDccessMethod(io))
  {
    err = ka_error_fill(pErrorBlock, "error202");
    goto exit;
  }

  // Check if disc present and is data
  err = _swix(CD_EnquireDataMode,_INR(0,1)|_IN(7)|_OUT(0)
            , 0, 0, &io->CDBlock
            , &val);
  if (err) goto exit;

  if (!val)
  {
    err = ka_error_fill(pErrorBlock, "error203");
    goto exit;
  }

  err = _swix(CD_GetParameters,_IN(0)|_IN(7)
            , io->buffer, &io->CDBlock
            );
  if (err) goto exit;

  io->buffer[8] = val;
  err = _swix(CD_SetParameters,_IN(0)|_IN(7)
            , io->buffer, &io->CDBlock
            );
  if (err) goto exit;

  // Read disc protection and allowed regions
  err = readCopyright(io, &prot, &regs);
  if (err)
  {
    // Assume unprotected (CDFaker ?)
    if (config.debug & cfg_printnavstats)
      ka_log(ka_log_nav, "Cound not read DVD Copyright, assume the drive is unprotected");

    err = NULL;
  }

  // Read disc key
  if (prot)
  {
    io->dvdFlags |= DVD_flag_isProtected;

    // Read disc prefered regions
    err = reportPreferedRegionCodeStructure(io, &val);
    if (err) goto exit;

    if (config.debug & cfg_printnavstats)
      ka_log(ka_log_nav, "DVD Region struct: %08x", val);

    if (val >> 30)
      val = 0xff & (val >> 16); // 1 bit set to 0 for active region
    else
      val = 0xff; // Drive has no set region, i.e. will refuse all regions

    if (config.debug & cfg_printnavstats)
      ka_log(ka_log_nav, "DVD vs Drive region bits: %02x vs %02x", regs, val);

    if ((val | regs) == 0xff)
    {
      io->dvdFlags |= DVD_flag_wrongRegion;
      // Drive does not support the disc's region
      err = ka_error_fill(pErrorBlock, "error204");
      goto exit;
    }

    err = getAuthenticationGrantId(io);
    if (err) goto exit;

    err = getBusKey(io);
    if (err) goto exit;

    err = getDiscKey(io);
    if (err) goto exit;
  }

exit:
  if (err)
  {
    if (err != pErrorBlock)
    {
      pErrorBlock->errnum = err->errnum;
      strncpy(pErrorBlock->errmess, err->errmess, sizeof(pErrorBlock->errmess));
    }

    dvd_io_delete(&io);
    return NULL;
  }

  invalidateAuthenticationGrantId(io);
  return io;
}

void dvd_io_delete(dvd_io_t** pio)
{
  dvd_io_t* io = *pio;

  *pio = NULL;

  if (!io)
    return;

  invalidateAuthenticationGrantId(io);
  ka_mem_free(io);
}

const _kernel_oserror* dvd_io_readBlocks(dvd_io_t* io, uint32_t lba, uint8_t* buffer, int nr)
{
  return _swix(CD_ReadData, _INR(0,4)|_IN(7), 0, lba, nr, buffer, DVD_BLOCK_SIZE, &io->CDBlock);
}

void dvd_io_unscramble(dvd_io_t* io, const dvd_key_t titleKey, uint8_t* sector)
{
  if ((io->dvdFlags & DVD_flag_isProtected) == 0)
    return;

  if ((io->dvdFlags & DVD_flag_hasDiscKey) == 0)
    return;

  css_unscramble(titleKey, sector);
}

/**
 * Returns the highest vobu address from the table of cell addresses.
 */
/*
static uint32_t end_cell_address(dvd_cell_address_table_t* cas)
{
  if (cas == NULL) return 0;

  uint32_t max = 0;
  for (int i = 0; i < cas->nr_cells; i++)
  {
    if (max < cas->cells[i].lb_vob_last)
      max = cas->cells[i].lb_vob_last;
  }
  if (config.debug & cfg_printnavstats)
    ka_log(ka_log_note | ka_log_nav, "cells count %d, last lba %08x", cas->nr_cells, max);

  return max + 1;
}
*/

/**
 * Attempts to locate DVD sectors corresponding to the VIDEO_TS/IFO,
 * parse the IFO and from it derive the location of all other IFO, BUP and VOB.
 * Parse the other IFOs.
 */
int dvd_io_load_desc(dvd_io_t* io, dvd_desc_t* dvd)
{
  if (dvd_io_parseIso(io, dvd))
    return -1;

  if (dvd_io_loadVmgIfo(io, &dvd->ifos[0]))
    return -1;

  for (int i = 1; i < dvd->nr_ifos; i++)
  {
    if (dvd_io_loadVtsIfo(io, &dvd->ifos[i], i))
      return -1;
  }

  return 0;
}
