usbmass.c

printer usb

    case WRITE10:
    case WRITE12:
        DoWrite(c, &CardChangeFlag);
        break;

#ifdef FUNAI_MODIFY_022704
    case CARD_CHECK_TEST:
        /* for Memory Card Check Tool */
        DoMediaCheck(c);
        break;
#endif /* FUNAI_MODIFY_022704 */

    default:
        DBPRINT("INVALID OP CODE\n");
        sense_data = SENSE_INVALID_CMD_OPCODE;
        SendStatus(COMMAND_FAILED, NO_STALL, c->cmdtag, c->data_length);
        break;
    }
}

#ifdef FUNAI_MODIFY_022704
/* for Memory Card Check Tool */
static Uint8 cachable_mem_reply1[40];

static void DoMediaCheck(COMMAND *c)
{
    int i;
    Boolean flag;
    Uint8 Mfg[10];
    Uint8 AllocLen;
    Uint8 *reply = (Uint8 *)NON_CACHABLE_MEMORY(cachable_mem_reply1);

    MemcardType cardType = NoMemoryCard;

    AllocLen = c->cmd[4];

    if (AllocLen > 40)
        AllocLen = 40;

    cardType = GetInsertedMemoryCardType();

    OMgetString(omManufacturerName, OMCURRENT, (char *)Mfg, 9);
    flag = FALSE;
    for (i = 0; i < 8; i++) {
        if (!Mfg[i])
            flag = TRUE;
        if (flag)
            Mfg[i] = ' ';
    }

    reply[ 0] = 0;          /* direct-access device */
    reply[ 1] = 0x80;       /* removable media */
    reply[ 2] = 0;          /* version: device does not claim conformance */
    reply[ 3] = 0;          /* response data format */
    reply[ 4] = 0;          /* additional length */
    reply[ 5] = 0;          /* reserved */
    reply[ 6] = 0;
    reply[ 7] = 0;
    memcpy(&reply[ 8], Mfg, 8);
    memcpy(&reply[16], PRODUCT_ID_16, 16);
    memcpy(&reply[32], PRODUCT_REV_4, 4);
    reply[36] = 0;
    reply[37] = 0;
    reply[38] = 0;

    switch (cardType) {
    case NoMemoryCard:
        reply[39] = 0x0B;
        break;

    case CompactFlash:
        reply[39] = 0;
        break;

    case SmartMedia:
        reply[39] = 0x01;
        break;

    case MemoryStick:
        reply[39] = 0x02;
        break;

    case SecureDigital:
        reply[39] = 0x04;
        break;

    case XD:
        reply[39] = 0x05;
        break;

    default:
        reply[39] = 0xFF;
        break;
    }

    if (AllocLen)
        USBWriteCard(reply, AllocLen);
    SendStatus(COMMAND_PASSED, NO_STALL, c->cmdtag, c->data_length - AllocLen);
}
#endif /* FUNAI_MODIFY_022704 */

static Uint8 cachable_mem_reply2[36];

static void DoInquiry(COMMAND *c)
{
    int i;
    Boolean flag;
    Uint8 Mfg[10];
    Uint8 AllocLen;
    Uint8 *reply = (Uint8 *)NON_CACHABLE_MEMORY(cachable_mem_reply2);

    DBPRINT("INQUIRY\n");

    AllocLen = c->cmd[4];
    if (AllocLen > 36)
        AllocLen = 36;

    if (c->cmd_length < 4 || c->cmd[1] || c->cmd[2] || !c->INflag ||
        c->data_length < AllocLen)
    {
        DBPRINT("Inquiry failed %d %d %d %d %d %d\n",
                 c->cmd_length, c->cmd[1], c->cmd[2], c->INflag,
                 c->data_length, AllocLen);
        sense_data = SENSE_INVALID_FIELD_IN_CMD;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return;
    }

    OMgetString(omManufacturerName, OMCURRENT, (char *)Mfg, 9);
    flag = FALSE;
    for (i = 0; i < 8; i++) {
        if (!Mfg[i])
            flag = TRUE;
        if (flag)
            Mfg[i] = ' ';
    }

    reply[ 0] = 0;          /* direct-access device */
    reply[ 1] = 0x80;       /* removable media */
    reply[ 2] = 0;          /* version: device does not claim conformance */
    reply[ 3] = 0;          /* response data format */
    reply[ 4] = 0;          /* additional length */
    reply[ 5] = 0;          /* reserved */
    reply[ 6] = 0;
    reply[ 7] = 0;
    memcpy(&reply[ 8], Mfg, 8);
    memcpy(&reply[16], PRODUCT_ID_16, 16);
    memcpy(&reply[32], PRODUCT_REV_4, 4);

    if (AllocLen)
        USBWriteCard(reply, AllocLen);
    sense_data = NO_SENSE;
    SendStatus(COMMAND_PASSED, NO_STALL, c->cmdtag, c->data_length - AllocLen);
}

static Uint8 cachable_mem_reply3[512];

static void DoModeSense(COMMAND *c)
{
    Uint8 AllocLen;
    Uint8 *reply = (Uint8 *)NON_CACHABLE_MEMORY(cachable_mem_reply3);
    Uint32 SectorLimit = c->LastSector + 1;
    int i;

    DBPRINT("MODE_SENSE6 %x %x\n", SectorLimit, c->SectorSize);

    if (c->cmd[0] == MODE_SENSE6)
        AllocLen = c->cmd[4];
    else
        AllocLen = (c->cmd[7]<<8) | c->cmd[8];

    if (c->cmd_length < 5 || c->cmd[1] || !c->INflag ||
        c->data_length < AllocLen)
    {
        DBPRINT("MODE_SENSE6 failed %d %d %d %d %d\n",
                 c->cmd_length, c->cmd[1], c->INflag, c->data_length, AllocLen);
        sense_data = SENSE_INVALID_FIELD_IN_CMD;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return;
    }

    for (i = 0; i < 512; i++)
        reply[i] = 0;


    /* MODE PARAMETER LIST */

    if (c->CardStatus == NOT_PRESENT)
        AllocLen = 6;
    else if (c->cmd[0] == MODE_SENSE6) {
        /* Mode parameter header(6) */
        reply[ 0] = 24;     /* mode data length */
        reply[ 1] = 0;      /* medium type */

#if WRITE_PROTECT_FLASH
        /* Until the "Flash sector 0 write" bug is fixed, write protect flash */
        reply[ 2] = 0x80;   /* device specific parameter - WRITE PROTECT */
#else
        /* device specific parameter - WRITE PROTECT */
        reply[ 2] = (c->WriteProtected) ? 0x80 : 0;
#endif

        reply[ 3] = 8;      /* block descriptor length */

            /* Block descriptor */
            reply[ 4] = 0;                              /* Density code (dev sp.) */
            reply[ 5] = (SectorLimit  >> 16) & 0xff;    /* No. of blocks MSB */
            reply[ 6] = (SectorLimit  >>  8) & 0xff;    /* No. of blocks     */
            reply[ 7] = (SectorLimit  >>  0) & 0xff;    /* No. of blocks LSB */
            reply[ 8] = 0;                              /* reserved */
            reply[ 9] = (c->SectorSize >> 16) & 0xff;   /* block length MSB */
            reply[10] = (c->SectorSize >>  8) & 0xff;   /* block length     */
            reply[11] = (c->SectorSize >>  0) & 0xff;   /* block length LSB */

        /* Pages: Read-Write Error Recovery Page */
        reply[12] = 1;      /* page type 1 */
        reply[13] = 10;     /* page length */
        reply[14] = 0x80;
        reply[15] = 8;      /* read retry count */
        reply[20] = 1;      /* write retry count */
    }

    if (AllocLen)
        USBWriteCard(reply, AllocLen);
    sense_data = NO_SENSE;
    SendStatus(COMMAND_PASSED, NO_STALL, c->cmdtag, c->data_length - AllocLen);
}


#define RDBUFF_SIZE 8192

static Uint8 rbuff[RDBUFF_SIZE];

static void DoRead(COMMAND *c)
{
    Uint32 sector, count, cnt;
    MEM_STATUS status;
    Uint8 *readbuff = (Uint8 *)NON_CACHABLE_MEMORY(rbuff);

    /* Extract sector/count and check if valid; return if not */
    if (SectorCheck(c, (c->cmd[1] || !c->INflag), &sector, &count))
        return;

    DBPRINT("RD: A %d (0x%x) C %d\n", sector, sector, count);

    while (count) {
        /* Transfer as many sectors at a time as there is room */
        cnt = count;
        if (cnt > (RDBUFF_SIZE / c->SectorSize))
            cnt = RDBUFF_SIZE / c->SectorSize;

        /* Read memory card data into read buffer */
        status = MEMORYCARD_READ(readbuff, sector, cnt);
        if (status != OK) {
            if (status == READ_ERROR)
                sense_data = SENSE_READ_ERROR;
            else
                sense_data = SENSE_MEDIUM_NOT_PRESENT;
            SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
            DBPRINT("Read10 error %x\n", sense_data);
            return;
        }
        sector += cnt;

        /* Write it back to host */
        if (!USBWriteCard(readbuff, cnt*c->SectorSize)) {
            PSPRINTF("FAKE DRIVE NOT READY\n");
            sense_data = SENSE_DRIVE_NOT_READY;
            SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
            DBPRINT("USBWriteCard failed - lost USB channel\n");
            return;
        }
        count -= cnt;
        c->data_length -= cnt*c->SectorSize;
    }

    sense_data = NO_SENSE;
    SendStatus(COMMAND_PASSED, NO_STALL, c->cmdtag, c->data_length);
}

static Uint8 cachable_mem_reply4[8];

static void DoReadCapacity(COMMAND *c)
{
    Uint8 *reply = (Uint8 *)NON_CACHABLE_MEMORY(cachable_mem_reply4);
    int count;

    DBPRINT("ReadCap: 0x%x %x\n", c->LastSector, c->SectorSize);

    if (c->cmd_length < 6 || c->cmd[1] || !c->INflag) {
        DBPRINT("ReadCap failed %d %d %d\n", c->cmd_length, c->cmd[1], c->INflag);
        sense_data = SENSE_INVALID_FIELD_IN_CMD;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return;
    }

    if (c->CardStatus == NOT_PRESENT) {
        c->LastSector = 0x800000;
        c->SectorSize = 0x200;
    }
    reply[0] = (c->LastSector >> 24) & 0xff;    /* No. of blocks MSB */
    reply[1] = (c->LastSector >> 16) & 0xff;    /* No. of blocks     */
    reply[2] = (c->LastSector >>  8) & 0xff;    /* No. of blocks     */
    reply[3] = (c->LastSector >>  0) & 0xff;    /* No. of blocks LSB */

    reply[4] = (c->SectorSize >> 24) & 0xff;    /* block length MSB */
    reply[5] = (c->SectorSize >> 16) & 0xff;    /* block length     */
    reply[6] = (c->SectorSize >>  8) & 0xff;    /* block length     */
    reply[7] = (c->SectorSize >>  0) & 0xff;    /* block length LSB */

    count = c->data_length;
    if (count > 8)
        count = 8;
    USBWriteCard(reply, count);

    if (c->CardStatus == NOT_PRESENT) {
        sense_data = SENSE_MEDIUM_NOT_PRESENT;
        SendStatus(COMMAND_FAILED, NO_STALL, c->cmdtag, c->data_length - count);
    }
    else {
        sense_data = NO_SENSE;
        SendStatus(COMMAND_PASSED, NO_STALL, c->cmdtag, c->data_length - count);
    }
}

static Uint8 cachable_mem_reply5[36];

static void DoReadFormatCapacities(COMMAND *c)
{
    Uint16 AllocLen;
    Uint8 *reply = (Uint8 *)NON_CACHABLE_MEMORY(cachable_mem_reply5);

    DBPRINT("ReadFormCap OK\n");

    AllocLen = (c->cmd[7] << 8) | c->cmd[8];
    if (AllocLen > 20)
        AllocLen = 20;

    if (c->cmd_length < 9 || c->cmd[1] || !c->INflag ||
        c->data_length < AllocLen)
    {
        DBPRINT("ReadFormatCap failed %d %d %d %d %d\n",
                 c->cmd_length, c->cmd[1], c->INflag, c->data_length, AllocLen);
        sense_data = SENSE_INVALID_FIELD_IN_CMD;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return;
    }

    if (c->CardStatus == NOT_PRESENT) {
        /* Capacity List Header */
        reply[ 0] = 0;  /* reserved */
        reply[ 1] = 0;  /* reserved */
        reply[ 2] = 0;  /* reserved */
        reply[ 3] = 8;  /* Capacity List Length */

        /* Current/Maximum Capacity Descriptor */
        reply[4+0] = (0x800000 >> 24) & 0xff;  /* No. of blocks MSB */
        reply[4+1] = (0x800000 >> 16) & 0xff;  /* No. of blocks MSB */
        reply[4+2] = (0x800000 >>  8) & 0xff;  /* No. of blocks     */
        reply[4+3] = (0x800000 >>  0) & 0xff;  /* No. of blocks LSB */

        reply[4+4] = 3;     /* no cartridge, max formattable capacity */

        reply[4+5] = (0x200 >> 16) & 0xff;  /* block length MSB */
        reply[4+6] = (0x200 >>  8) & 0xff;  /* block length     */
        reply[4+7] = (0x200 >>  0) & 0xff;  /* block length LSB */
        if (AllocLen > 12)
            AllocLen = 12;
        if (AllocLen)
            USBWriteCard(reply, AllocLen);
    }
    else {
        /* Capacity List Header */
        reply[ 0] = 0;  /* reserved */
        reply[ 1] = 0;  /* reserved */
        reply[ 2] = 0;  /* reserved */
        reply[ 3] = 16; /* Capacity List Length */