// Initialize command state if needed
    if (commandState->state == 0) {

        commandState->state = SBC_STATE_WRITE;
    }

    // Check current command state
    switch (commandState->state) {
    //-------------------
    case SBC_STATE_WRITE:
    //-------------------
        // Start transfer
        status = USBD_Write(MSDDriverDescriptors_BULKIN,
                            (void *) &modeParameterHeader6,
                            commandState->length,
                            (TransferCallback) MSDDriver_Callback,
                            (void *) transfer);

        // Check operation result code
        if (status != USBD_STATUS_SUCCESS) {

            trace_LOG(trace_WARNING, "W: SPC_ModeSense6: Cannot start data transfer\n\r");
            result = MSDDriver_STATUS_ERROR;
        }
        else {

            // Proceed to next state
            commandState->state = SBC_STATE_WAIT_WRITE;
        }
        break;

    //------------------------
    case SBC_STATE_WAIT_WRITE:
    //------------------------
        trace_LOG(trace_INFO, "Wait ");

        // Check semaphore value
        if (transfer->semaphore > 0) {

            // Take semaphore and terminate command
            transfer->semaphore--;

            if (transfer->status != USBD_STATUS_SUCCESS) {

                trace_LOG(trace_WARNING, "W: SPC_ModeSense6: Data transfer failed\n\r");
                result = MSDDriver_STATUS_ERROR;
            }
            else {

                result = MSDDriver_STATUS_SUCCESS;
            }

            // Update length field
            commandState->length -= transfer->transferred;

        }
        break;
    }

    return result;
}

//------------------------------------------------------------------------------
//! \brief  Performs a TEST UNIT READY COMMAND command.
//! \param  lun          Pointer to the LUN affected by the command
//! \return Operation result code (SUCCESS, ERROR, INCOMPLETE or PARAMETER)
//! \see    MSDLun
//------------------------------------------------------------------------------
static unsigned char SBC_TestUnitReady(MSDLun *lun)
{
    unsigned char result = MSDDriver_STATUS_ERROR;

    // Check current media state
    switch(lun->media->state) {
    //-------------------
    case MED_STATE_READY:
    //-------------------
        // Nothing to do
        trace_LOG(trace_INFO, "Rdy ");
        result = MSDDriver_STATUS_SUCCESS;
        break;

    //------------------
    case MED_STATE_BUSY:
    //------------------
        trace_LOG(trace_INFO, "Bsy ");
        SBC_UpdateSenseData(&(lun->requestSenseData),
                            SBC_SENSE_KEY_NOT_READY,
                            0,
                            0);
        break;

    //------
    default:
    //------
        trace_LOG(trace_INFO, "? ");
        SBC_UpdateSenseData(&(lun->requestSenseData),
                            SBC_SENSE_KEY_NOT_READY,
                            SBC_ASC_MEDIUM_NOT_PRESENT,
                            0);
        break;
    }

    return result;
}

//------------------------------------------------------------------------------
//      Exported functions
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//! \brief  Updates the sense data of a LUN with the given key and codes
//! \param  requestSenseData             Pointer to the sense data to update
//! \param  senseKey                     Sense key
//! \param  additionalSenseCode          Additional sense code
//! \param  additionalSenseCodeQualifier Additional sense code qualifier
//------------------------------------------------------------------------------
void SBC_UpdateSenseData(SBCRequestSenseData *requestSenseData,
                         unsigned char senseKey,
                         unsigned char additionalSenseCode,
                         unsigned char additionalSenseCodeQualifier)
{
    requestSenseData->bSenseKey = senseKey;
    requestSenseData->bAdditionalSenseCode = additionalSenseCode;
    requestSenseData->bAdditionalSenseCodeQualifier
        = additionalSenseCodeQualifier;
}

//------------------------------------------------------------------------------
//! \brief  Return information about the transfer length and direction expected
//!         by the device for a particular command.
//! \param  command Pointer to a buffer holding the command to evaluate
//! \param  length  Expected length of the data transfer
//! \param  type    Expected direction of data transfer
//! \param  lun     Pointer to the LUN affected by the command
//------------------------------------------------------------------------------
unsigned char SBC_GetCommandInformation(void          *command,
                               unsigned int  *length,
                               unsigned char *type,
                               MSDLun         *lun)
{
    SBCCommand *sbcCommand = (SBCCommand *) command;
    unsigned char          isCommandSupported = 1;

    // Identify command
    switch (sbcCommand->bOperationCode) {
    //---------------
    case SBC_INQUIRY:
    //---------------
        (*type) = MSDDriver_DEVICE_TO_HOST;

        // Allocation length is stored in big-endian format
        (*length) = WORDB(sbcCommand->inquiry.pAllocationLength);
        break;

    //--------------------
    case SBC_MODE_SENSE_6:
    //--------------------
        (*type) = MSDDriver_DEVICE_TO_HOST;
        if (sbcCommand->modeSense6.bAllocationLength >
            sizeof(SBCModeParameterHeader6)) {

            *length = sizeof(SBCModeParameterHeader6);
        }
        else {

            *length = sbcCommand->modeSense6.bAllocationLength;
        }

        // Only "return all pages" command is supported
        if (sbcCommand->modeSense6.bPageCode != SBC_PAGE_RETURN_ALL) {

            // Unsupported page
            trace_LOG(trace_WARNING, "W: SBC_GetCommandInformation: Page code not supported (0x%02X)\n\r",
                          sbcCommand->modeSense6.bPageCode);
            isCommandSupported = 0;
            (*length) = 0;
        }
        break;

    //------------------------------------
    case SBC_PREVENT_ALLOW_MEDIUM_REMOVAL:
    //------------------------------------
        (*type) = MSDDriver_NO_TRANSFER;
        break;

    //---------------------
    case SBC_REQUEST_SENSE:
    //---------------------
        (*type) = MSDDriver_DEVICE_TO_HOST;
        (*length) = sbcCommand->requestSense.bAllocationLength;
        break;

    //-----------------------
    case SBC_TEST_UNIT_READY:
    //-----------------------
        (*type) = MSDDriver_NO_TRANSFER;
        break;

    //---------------------
    case SBC_READ_CAPACITY_10:
    //---------------------
        (*type) = MSDDriver_DEVICE_TO_HOST;
        (*length) = sizeof(SBCReadCapacity10Data);
        break;

    //---------------
    case SBC_READ_10:
    //---------------
        (*type) = MSDDriver_DEVICE_TO_HOST;
        (*length) = WORDB(sbcCommand->read10.pTransferLength)
                     * lun->blockSize;
        break;

    //----------------
    case SBC_WRITE_10:
    //----------------
        (*type) = MSDDriver_HOST_TO_DEVICE;
        (*length) = WORDB(sbcCommand->write10.pTransferLength)
                     * lun->blockSize;
        break;

    //-----------------
    case SBC_VERIFY_10:
    //-----------------
        (*type) = MSDDriver_NO_TRANSFER;
        break;

    //------
    default:
    //------
        isCommandSupported = 0;
    }

    // If length is 0, no transfer is expected
    if ((*length) == 0) {

        (*type) = MSDDriver_NO_TRANSFER;
    }

    return isCommandSupported;
}

//------------------------------------------------------------------------------
//! \brief  Processes a SBC command by dispatching it to a subfunction.
//! \param  pUsb          Pointer to a S_usb instance
//! \param  lun          Pointer to the affected LUN
//! \param  commandState Pointer to the current command state
//! \return Operation result code
//------------------------------------------------------------------------------
unsigned char SBC_ProcessCommand(MSDLun               *lun,
                                 MSDCommandState *commandState)
{
    unsigned char result = MSDDriver_STATUS_INCOMPLETE;
    SBCCommand *command = (SBCCommand *) commandState->cbw.pCommand;

    // Identify command
    switch (command->bOperationCode) {
    //---------------
    case SBC_READ_10:
    //---------------
        trace_LOG(trace_INFO, "Read(10) ");

        // Perform the Read10 command
        result = SBC_Read10(lun, commandState);
        break;

    //----------------
    case SBC_WRITE_10:
    //----------------
        trace_LOG(trace_INFO, "Write(10) ");

        // Perform the Write10 command
        result = SBC_Write10(lun, commandState);
        break;

    //---------------------
    case SBC_READ_CAPACITY_10:
    //---------------------
        trace_LOG(trace_INFO, "RdCapacity(10) ");

        // Perform the ReadCapacity command
        result = SBC_ReadCapacity10(lun, commandState);
        break;

    //---------------------
    case SBC_VERIFY_10:
    //---------------------
        trace_LOG(trace_INFO, "Verify(10) ");

        // Flush media
        MED_Flush(lun->media);
        result = MSDDriver_STATUS_SUCCESS;
        break;

    //---------------
    case SBC_INQUIRY:
    //---------------
        trace_LOG(trace_INFO, "Inquiry ");

        // Process Inquiry command
        result = SBC_Inquiry(lun, commandState);
        break;

    //--------------------
    case SBC_MODE_SENSE_6:
    //--------------------
        trace_LOG(trace_INFO, "ModeSense(6) ");

        // Process ModeSense6 command
        result = SBC_ModeSense6(commandState);
        break;

    //-----------------------
    case SBC_TEST_UNIT_READY:
    //-----------------------
        trace_LOG(trace_INFO, "TstUnitRdy ");

        // Process TestUnitReady command
        //MED_Flush(lun->media);
        result = SBC_TestUnitReady(lun);
        break;

    //---------------------
    case SBC_REQUEST_SENSE:
    //---------------------
        trace_LOG(trace_INFO, "ReqSense ");

        // Perform the RequestSense command
        result = SBC_RequestSense(lun, commandState);
        break;

    //------------------------------------
    case SBC_PREVENT_ALLOW_MEDIUM_REMOVAL:
    //------------------------------------
        trace_LOG(trace_INFO, "PrevAllowRem ");

        // Nothing to do
        result = MSDDriver_STATUS_SUCCESS;
        break;

    //------
    default:
    //------
        result = MSDDriver_STATUS_PARAMETER;
    }

    return result;
}