usbmass.c

printer usb

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

        reply[4+4] = 2;                             /* "formatted media" code */

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

        /* Formattable Capacity Descriptor */
        reply[12+0] = (c->LastSector >> 24) & 0xff; /* No. of blocks MSB */
        reply[12+1] = (c->LastSector >> 16) & 0xff; /* No. of blocks MSB */
        reply[12+2] = (c->LastSector >>  8) & 0xff; /* No. of blocks     */
        reply[12+3] = (c->LastSector >>  0) & 0xff; /* No. of blocks LSB */

        reply[12+4] = 0;                            /* "formatted media" code */

        reply[12+5] = (c->SectorSize >> 16) & 0xff; /* block length MSB */
        reply[12+6] = (c->SectorSize >>  8) & 0xff; /* block length     */
        reply[12+7] = (c->SectorSize >>  0) & 0xff; /* block length LSB */
        if (AllocLen)
            USBWriteCard(reply, AllocLen);
    }

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

static Uint8 cachable_mem_reply6[18];

static void DoRequestSense(COMMAND *c)
{
    int i;
    Uint16 AllocLen;
    Uint8 *reply = (Uint8 *)NON_CACHABLE_MEMORY(cachable_mem_reply6);

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

    if (c->cmd_length < 4 || c->cmd[1] || c->cmd[2] || !c->INflag ||
        c->data_length < AllocLen)
    {
        DBPRINT("RequestSense 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;
    }

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

    if (sense_data != NO_SENSE)
        reply[0] = 0x70;
    reply[2] = (sense_data >> 16) & 0xff;
    reply[7] = 10;
    reply[12] = (sense_data >> 8) & 0xff;
    reply[13] = sense_data & 0xff;

#ifdef MC_DBPRINT
{
    static struct {
        int code;
        char *str;
    } rs[10] = {
        {0x00, "NO SENSE"},
        {0x03, "WRITE_FAULT"},
        {0x11, "READ_ERROR"},
        {0x20, "INVALID_CMD_OPCODE"},
        {0x21, "ADDRESS_OUT_OF_RANGE"},
        {0x24, "INVALID_FIELD_IN_CMD"},
        {0x27, "WRITE_PROTECTED_MEDIA"},
        {0x28, "MEDIA CHANGED"},
        {0x3A, "MEDIUM_NOT_PRESENT"},
        {0x54, "USB_TO_HOST_FAILURE"}
    };
    char *string = "SENSE CODE ERROR";

    for (i = 0; i < 10; i++) {
        if (reply[12] == rs[i].code) {
            string = rs[i].str;
            break;
        }
    }
    DBPRINT("ReqSense %02x %02x %02x %s\n",
             reply[2], reply[12], reply[13], string);
}
#endif

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

static void DoVerify(COMMAND *c)
{
    Uint32 sector, count;
#ifdef REALLY_VERIFY_DATA
	Uint32 i;
    MEM_STATUS status;
#endif

    /* Extract sector/count and check if valid; return if not */
    if (SectorCheck(c, c->cmd[1], &sector, &count))
        return;
#ifdef REALLY_VERIFY_DATA
    DBPRINT("Verify: A %d (0x%x) C %d\n", sector, sector, count);

    /* Read the verify region to make certain all is well */
    for (i = 0; i < count; i++) {
        status = MEMORYCARD_READ(sectbuff, sector + i, 1);
        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("Verify error %x\n", sense_data);
            return;
        }
    }
#endif
    sense_data = NO_SENSE;
    SendStatus(COMMAND_PASSED, NO_STALL, c->cmdtag, c->data_length);
}

static void DoWrite(COMMAND *c, Boolean *CardChangeFlag)
{
    Uint8 *ptr1, *ptr2;
    Uint32 cnt1, cnt2, cnt;
    Uint32 sector, count, left, SectorSize;
    MEM_STATUS status;
    Boolean NewCard;
    int i;

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

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

    left = count;
    status = *CardChangeFlag ? NOT_PRESENT : OK;
    SectorSize = c->SectorSize;
    while (status == OK && left) {
        /* Read data to write, abort if USB connection fails */
        if (!USBReadCard(&ptr1, &cnt1, &ptr2, &cnt2))
            return;

        status = GET_MEMORYCARD_STAT(&c->LastSector, &c->SectorSize,
                                     &NewCard, &c->WriteProtected);
        if (status == NOT_PRESENT || NewCard) {
            DBPRINT("WR: status %s, NewCard %d -> TRUE\n",
                    (status == NOT_PRESENT ? "not present" : "OK"), NewCard);
            *CardChangeFlag = TRUE;
            break;
        }

        cnt = cnt1 / SectorSize;
        if (cnt) {
            if (cnt > left)
                cnt = left;

            if (status == OK) {
#if WRITE_PROTECT_FLASH
                status = WR_PROTECT_ERR;
#else
                if (c->WriteProtected)
                    status = WR_PROTECT_ERR;
                else
                    status = MEMORYCARD_WRITE(sector, cnt, ptr1);
#endif
            }
            sector += cnt;
            left -= cnt;
            USBConsumeCard(cnt*SectorSize);
        }
        else if ((cnt1 + cnt2) / SectorSize) {
            /* Sector split across ring buffer */
            if (SectorSize > MAX_SECTOR_SIZE) {
                status = WRITE_FAULT;
                break;
            }

            for (i = 0; i < SectorSize; i++)
                sectbuff[i] = (i < cnt1) ? ptr1[i] : ptr2[i - cnt1];

            if (status == OK) {
#if WRITE_PROTECT_FLASH
                status = WR_PROTECT_ERR;
#else
                if (c->WriteProtected)
                    status = WR_PROTECT_ERR;
                else
                    status = MEMORYCARD_WRITE(sector, 1, sectbuff);
#endif
            }
            sector++;
            left--;
            USBConsumeCard(SectorSize);
        }
    }

    if (status == OK) {
        sense_data = NO_SENSE;
        SendStatus(COMMAND_PASSED, NO_STALL, c->cmdtag,
                   c->data_length - count*SectorSize);
    }
    else {
#ifdef MC_DBPRINT
        DBPRINT("WR: status ERROR: ");
        switch (status) {
        case OK:             DBPRINT("OK\n");             break;
        case NOT_PRESENT:    DBPRINT("NOT_PRESENT\n");    break;
        case WR_PROTECT_ERR: DBPRINT("WR_PROTECT_ERR\n"); break;
        case WRITE_FAULT:    DBPRINT("WRITE_FAULT\n");    break;
        case READ_ERROR:     DBPRINT("READ_ERROR\n");     break;
        default:             DBPRINT("???\n");
        }
#endif
        if (status == WR_PROTECT_ERR)
            sense_data = SENSE_WRITE_PROTECTED_MEDIA;
        else
            sense_data = SENSE_MEDIUM_NOT_PRESENT;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        DBPRINT("Write error %x\n", sense_data);
    }
}

static Uint8 cachable_mem_cmd_status[13];

static void SendStatus(Uint8 status, Boolean stall, Uint32 tag, Uint32 residue)
{
    Uint8 *cmd_status = (Uint8 *)NON_CACHABLE_MEMORY(cachable_mem_cmd_status);

    /* If command didn't pass, stall endpoint before sending status */
    if (stall) {
        Uint8 *ptr1, *ptr2;
        Uint32 cnt1, cnt2;
        TimeVal sec = {1, 0};

        /* Flush any in-coming data first */
        while (1) {
            USBReadCard(&ptr1, &cnt1, &ptr2, &cnt2);
            if (!cnt1)
                break;
            USBConsumeCard(cnt1 + cnt2);
            DBPRINT("FLUSH %d bytes\n", cnt1+cnt2);
            TaskSleep(&sec);
        }
        ARM_INTS_OFF;
        STALL_CARD_ENDPOINT;
        ARM_INTS_ON;
    }

    cmd_status[0] = 0x55;
    cmd_status[1] = 0x53;
    cmd_status[2] = 0x42;
    cmd_status[3] = 0x53;

    cmd_status[4] = (tag >>  0) & 0xff;
    cmd_status[5] = (tag >>  8) & 0xff;
    cmd_status[6] = (tag >> 16) & 0xff;
    cmd_status[7] = (tag >> 24) & 0xff;

    cmd_status[ 8] = (residue >>  0) & 0xff;
    cmd_status[ 9] = (residue >>  8) & 0xff;
    cmd_status[10] = (residue >> 16) & 0xff;
    cmd_status[11] = (residue >> 24) & 0xff;
    cmd_status[12] = status;
    USBWriteCard(cmd_status, 13);
}

static Boolean SectorCheck(COMMAND *c, Boolean FieldFlag, Uint32 *sector,
                           Uint32 *count)
{
    Uint8 opcode = c->cmd[0], minlength;

    if (c->CardStatus == NOT_PRESENT) {
        sense_data = SENSE_MEDIUM_NOT_PRESENT;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return TRUE;
    }

    if (FieldFlag) {
        sense_data = SENSE_INVALID_FIELD_IN_CMD;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return TRUE;
    }

    *sector = ((Uint32)c->cmd[2] << 24) | ((Uint32)c->cmd[3] << 16) |
              ((Uint32)c->cmd[4] <<  8) |  (Uint32)c->cmd[5];

    if (opcode == READ12 || opcode == WRITE12) {
        minlength = 10;
        *count = ((Uint32)c->cmd[6] << 24) | ((Uint32)c->cmd[7] << 16) |
                 ((Uint32)c->cmd[8] <<  8) |  (Uint32)c->cmd[9];
    }
    else {
        minlength = 9;
        *count = ((Uint32)c->cmd[7] <<  8) | (Uint32)c->cmd[8];
    }

    if (c->cmd_length < minlength) {
        DBPRINT("RD/WRT/VERIFY(%d) length failed %d\n", opcode, c->cmd_length);
        sense_data = SENSE_INVALID_FIELD_IN_CMD;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return TRUE;
    }

    if (*sector > c->LastSector || *sector + *count > c->LastSector+1) {
        DBPRINT("SectorCheck bad sector/count: %d %d\n", sector, count);
        sense_data = SENSE_ADDRESS_OUT_OF_RANGE;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return TRUE;
    }

    if (opcode != VERIFY && *count > c->data_length) {
        DBPRINT("SectorCheck overrun: %d %d\n", c->cmd_length, c->cmd[1]);
        sense_data = SENSE_INVALID_FIELD_IN_CMD;
        SendStatus(COMMAND_FAILED, STALL, c->cmdtag, c->data_length);
        return TRUE;
    }

    /* Keep LED blinking during memory card access */
    DoNotRemoveFlashMemory = TRUE;

    /*
     * Because when the host is reading a flash memory over USB 2.0
     * the USB task can consume all the CPU cycles and this task is
     * at a high priority, it can lock out the communication with the
     * printer and scanner tasks and look as though the printer is not
     * responding.  To allow these tasks to have a short time slice we
     * sleep briefly every 5 seconds. It would probably be better to
     * run the USB Task at a priority lower than the Print task but
     * there may be bad side effects -- this is something to look into
     * in the future.
     */
#if USB_MASS_OCCASIONAL_HS_SLEEP
    if (USB20_HighSpeed) {
        Uint32 current_time;
        static Uint32 last_time = 0, start_time = 0;

        /*
         * Should be OK if it has been at least 2 seconds since
         * any read or write was processed.
         */
        current_time = BIOSTimerMS();
        if (current_time - last_time > 2000)
            start_time = current_time;
        /*
         * But if the card reader is hogging the CPU, let other tasks
         * in every 8 seconds.
         */
        else if (current_time - start_time > 8000) {
            TASKSLEEP_MILLISECONDS(500);
            start_time = current_time;
        }
        last_time = current_time;
    }
#endif
    return FALSE;
}
#endif