storage.c

Linux2.4.20针对三星公司的s3c2440内核基础上的一些设备驱动代码

 */
int storage_urb_sent (struct urb *urb, int status)
{
    dbg_tx(2,"%s length: %d status : %x",urb->device->name, urb->actual_length, status);
    usbd_dealloc_urb(urb);

    return 0;
}

/* storage_recv_urb - called to indicate URB has been received
 * @urb - pointer to struct urb
 *
 * Return non-zero if we failed and urb is still valid (not disposed)
 * NOTE: This function is called in bottom half context.
 */
int storage_recv_urb (struct urb *urb)
{
    int port = 0; // XXX compound device
    struct usb_device_instance *device = urb->device;
    struct usb_storage_private *private = (device->function_instance_array+port)->privdata;
    struct usb_storage_threaddata *tdata = &private->tdata;
#if 0	/* bad bi driver */
    unsigned int datalen;
#endif

    if (urb->status != RECV_OK)
	return 1;
    dbg_rx(2,"length=%d",urb->actual_length);
    dbgPRINTmem(dbgflg_usbdfd_rx,3,urb->buffer,min(urb->actual_length, 32u));

    // push the data up
    spin_lock(&storage_lock);
    if (private->tdata.busy) {
	printk(KERN_ERR STORAGE_MOD_NAME ": receive in busy state (%d)\n",
	       private->devstate);
	private->devstate = STATE_INVALID;
	spin_unlock(&storage_lock);
	usbd_recycle_urb(urb);	// free urb
	return 0;
    }
    switch (private->devstate) {
    case STATE_IDLE:
	tdata->data_len = 0;
	if (storage_receive_CBW(private, urb->buffer, urb->actual_length) < 0) {
	    private->devstate = STATE_INVALID;
	    break;
	}
#if 0	/* bad bi driver */
	datalen = 0;
	if (urb->actual_length > sizeof(struct CBW)) {
	    datalen = urb->actual_length - sizeof(struct CBW);
	    /* buf contains CBW and (part of) DATA */
	    if (tdata->cbw.dDataTransferLength == 0 ||
		(tdata->cbw.bmFlags & 0x80) ||
		tdata->cbw.dDataTransferLength < datalen) {
		printk(KERN_ERR STORAGE_MOD_NAME ": bad DATA length (%d)\n",
		       datalen);
		private->devstate = STATE_INVALID;
		break;
	    }
	}
#endif

	if (tdata->cbw.dDataTransferLength == 0) {
	    private->devstate = STATE_DN;
	    private->tdata.busy = 1;
	    storage_thread_poke();
	    break;
	}
	if (tdata->data_alloclen < tdata->cbw.dDataTransferLength) {
	    /* expand data buffer */
	    if (tdata->data_buf)
		kfree(tdata->data_buf);
	    tdata->data_buf = kmalloc(tdata->cbw.dDataTransferLength, GFP_ATOMIC);
	    if (!tdata->data_buf) {
		printk(KERN_ERR STORAGE_MOD_NAME ": failed to expand buffer (%d)\n",
		       tdata->cbw.dDataTransferLength);
		tdata->data_alloclen = 0;
		private->devstate = STATE_INVALID;
		break;
	    }
	    tdata->data_alloclen = tdata->cbw.dDataTransferLength;
	}
	if (tdata->cbw.bmFlags & 0x80) {	/* IN */
	    private->devstate = STATE_DI;
	    private->tdata.busy = 1;
	    storage_thread_poke();
	} else {	/* OUT */
#if 0	/* bad bi driver */
	    if (datalen > 0) {
		memcpy(tdata->data_buf, urb->buffer + sizeof(struct CBW),
		       datalen);
		tdata->data_len = datalen;
		if (tdata->data_len >= tdata->cbw.dDataTransferLength) {
		    /* all data received */
		    private->devstate = STATE_DN;
		    private->tdata.busy = 1;
		    storage_thread_poke();
		    break;
		}
	    }
#endif
	    private->devstate = STATE_DO;
	}
	break;
    case STATE_DO:
	if (tdata->data_len + urb->actual_length > tdata->cbw.dDataTransferLength) {
	    printk(KERN_ERR STORAGE_MOD_NAME ": bad DATA length (%d + %d > %d)\n",
		   tdata->data_len, urb->actual_length,
		   tdata->cbw.dDataTransferLength);
	    private->devstate = STATE_INVALID;
	    break;
	}
	memcpy(tdata->data_buf + tdata->data_len,
	       urb->buffer, urb->actual_length);
	tdata->data_len += urb->actual_length;
	if (tdata->data_len >= tdata->cbw.dDataTransferLength) {
	    /* all data received */
	    private->devstate = STATE_DN;
	    private->tdata.busy = 1;
	    storage_thread_poke();
	}
	break;
    case STATE_DI:
    default:
	printk(KERN_ERR STORAGE_MOD_NAME ": receive in bad state (%d)\n",
	       private->devstate);
	private->devstate = STATE_INVALID;
    }
    spin_unlock(&storage_lock);

    usbd_recycle_urb(urb);	// free urb
    return(0);
}

/**
 * storage_recv_setup - called with a control URB 
 * @urb - pointer to struct urb
 *
 * Check if this is a setup packet, process the device request, put results
 * back into the urb and return zero or non-zero to indicate success (DATA)
 * or failure (STALL).
 *
 * This routine IS called at interrupt time. Please use the usual precautions.
 *
 */
int storage_recv_setup (struct urb *urb)
{
    struct usb_device_request *request;
    struct usb_device_instance *device = urb->device;
    int port = 0;
    struct usb_storage_private *private = (device->function_instance_array+port)->privdata;

    request = &urb->device_request;

    // handle Mass Storage Class-Specific Request
    // c.f. USB Mass Storage Class Bulk-Only Transport 3.1, 3.2
    if ((request->bmRequestType & (USB_REQ_TYPE_MASK|USB_REQ_RECIPIENT_MASK)) !=
	 (USB_REQ_TYPE_CLASS | USB_REQ_RECIPIENT_INTERFACE)) {
        dbg_ep0(1, "not class/interface request: %x", request->bmRequestType);
        return 0; // XXX
    }

    if ((request->bmRequestType&USB_REQ_DIRECTION_MASK)) {
        dbg_ep0(1, "Device-to-Host");
        switch (request->bRequest) {
        case USB_REQ_GET_MAX_LUN:
	    urb->actual_length = 1;
	    urb->buffer[0] = 0;	// only one LUN
	    break;
	default:
	    dbg_ep0(1, "Unknown request: %x", request->bRequest);
	}
    }
    else {
        dbg_ep0(1, "Host-to-Device");
        switch (request->bRequest) {
        case USB_REQ_BO_MASS_STORAGE_RESET:
	    /* do job in event handler */
	    usbd_device_event(private->device, DEVICE_FUNCTION_PRIVATE, 0);
            break;
	default:
	    dbg_ep0(1, "Unknown request: %x", request->bRequest);
        }
    }
    return 0;
}

/* USB Device Functions ************************************************************************ */

/* proc interface */
static int storage_read_proc(char *page, char **start, off_t off,
			     int count, int *eof, void *data)
{
    char *p = page;
    struct usb_storage_private *private = &storage_private;

    spin_lock_bh(&storage_lock);	/* prevent event handler */
    if (private->device) {
	struct usb_storage_threaddata *tdata = &private->tdata;
	p += sprintf(p, "storage \"%s\" %d blocks\n",
		     tdata->filename, tdata->num_blocks);
	p += sprintf(p, "state: %d\n", private->devstate);
	p += sprintf(p, "transfer statistics:\n");
	p += sprintf(p, "read_blocks\t%u\n", tdata->stat.read_blocks);
	p += sprintf(p, "write_blocks\t%u\n", tdata->stat.write_blocks);
	p += sprintf(p, "command statistics:\n");
	p += sprintf(p, "inquiry\t%u\n", tdata->stat.inquiry);
	p += sprintf(p, "mode_select\t%u\n", tdata->stat.mode_select);
	p += sprintf(p, "mode_sense\t%u\n", tdata->stat.mode_sense);
	p += sprintf(p, "read_10\t%u\n", tdata->stat.read_10);
	p += sprintf(p, "read_capacity\t%u\n", tdata->stat.read_capacity);
	p += sprintf(p, "request_sense\t%u\n", tdata->stat.request_sense);
	p += sprintf(p, "start_stop\t%u\n", tdata->stat.start_stop);
	p += sprintf(p, "test_unit_ready\t%u\n", tdata->stat.test_unit_ready);
	p += sprintf(p, "verify\t%u\n", tdata->stat.verify);
	p += sprintf(p, "write_10\t%u\n", tdata->stat.write_10);
	p += sprintf(p, "write_buffer\t%u\n", tdata->stat.write_buffer);
	p += sprintf(p, "unsupported\t%u\n", tdata->stat.unsupported);
    }
    spin_unlock_bh(&storage_lock);

    return p - page;
}

/* storage_event - process a device event
 *
 * NOTE: This function is called from keventd kernel thread.
 */
void storage_event(struct usb_device_instance *device, usb_device_event_t event, int data)
{
    int port = 0; // XXX compound device
    struct usb_function_instance *function;

    dbg_usbe(5,"%d",event);

    if ((function = device->function_instance_array+port)==NULL){
        dbg_usbe(1,"no function");
        return;
    }

    dbg_usbe(3,"---> %s %d", device->name, event);
    switch (event) {

    case DEVICE_UNKNOWN:
    case DEVICE_INIT:
        dbg_usbe(1,"---> INIT %s %d", device->name, event);
        break;

    case DEVICE_CREATE:
        dbg_usbe(1,"---> CREATE %s %d", device->name, event);
        {
            struct usb_storage_private *private;

            // There is no way to indicate error, so make this unconditional
            // and undo it in the DESTROY event unconditionally as well.
            // It the responsibility of the USBD core and the bus interface
            // to see that there is a matching DESTROY for every CREATE.

	    spin_lock(&storage_lock);
	    if (storage_private.device) {
                dbg_usbe(1,"---> CREATE no free storage");
		spin_unlock(&storage_lock);
		return;
	    }
	    private = &storage_private;
	    private->device = device;
	    private->devstate = STATE_IDLE;
            function->privdata = private;
	    spin_unlock(&storage_lock);

            dbg_usbe(1,"---> START %s privdata assigned: %p", device->name, private); 
            return;
        }
        break;

    case DEVICE_HUB_CONFIGURED:
        break;
    case DEVICE_RESET:
        break;
    case DEVICE_ADDRESS_ASSIGNED:
        break;
    case DEVICE_CONFIGURED:
        dbg_usbe(1,"---> CONFIGURED %s %d", device->name, event);
        break;
    case DEVICE_SET_INTERFACE:
        break;
    case DEVICE_SET_FEATURE:
        break;
    case DEVICE_CLEAR_FEATURE:
        break;
    case DEVICE_DE_CONFIGURED:
        dbg_usbe(1,"---> DECONFIGURED %s %d", device->name, event);
        break;
    case DEVICE_BUS_INACTIVE:
        break;
    case DEVICE_BUS_ACTIVITY:
        break;
    case DEVICE_POWER_INTERRUPTION:
        break;
    case DEVICE_HUB_RESET:
        break;

    case DEVICE_DESTROY:
        dbg_usbe(1, "---> DESTROY %s %d", device->name, event);
        {
            struct usb_storage_private *private;
	    struct usb_storage_threaddata *tdata;

            if ((private = (device->function_instance_array+port)->privdata) == NULL) {
                dbg_usbe(1, "---> DESTROY %s private null", device->name);
                return;
            }
            dbg_usbe(1, "---> DESTROY %s private %p", device->name, private);
	    tdata = &private->tdata;

	    spin_lock(&storage_lock);
	    private->device = NULL;
	    private->devstate = STATE_INVALID;
	    if (tdata->busy) {
		/* free on DEVICE_FUNCTION_PRIVATE */
	    } else {
		if (tdata->data_buf)
		    kfree(tdata->data_buf);
		tdata->data_buf = NULL;
		tdata->data_len = tdata->data_alloclen = 0;
	    }
	    spin_unlock(&storage_lock);

            dbg_usbe(1,"---> STOP %s",device->name); 
            return;
        }
        break;

    case DEVICE_FUNCTION_PRIVATE:
        dbg_usbe(2,"---> FUNCTION_PRIVATE %s %d", device->name, event);
	{
            struct usb_storage_private *private;
	    struct usb_storage_threaddata *tdata;
            if ((private = (device->function_instance_array+port)->privdata) == NULL) {
                dbg_usbe(1, "---> PRIVATE %s private null", device->name);
                return;
            }
            dbg_usbe(2, "---> PRIVATE %s private %p", device->name, private);
	    tdata = &private->tdata;

	    spin_lock_bh(&storage_lock);
	    if (data == 0) {
		dbg_rx(1, "USB_REQ_BO_MASS_STORAGE_RESET received.");
		private->devstate = STATE_IDLE;
	    } else {
		/* kicked by storage_thread */
		struct urb *urb;

		if (!private->tdata.busy) {
		    /* command completed. send data and status */
		    switch (private->devstate) {
		    case STATE_DI:
			urb = usbd_alloc_urb(private->device,
					     private->device->function_instance_array+port,
					     CONFIG_USBD_STORAGE_IN_ENDPOINT | IN,
					     tdata->cbw.dDataTransferLength);
			if (!urb) {
			    printk(KERN_ERR STORAGE_MOD_NAME ": failed to alloc DATA urb\n");
			    private->devstate = STATE_INVALID;
			    break;
			}
#if 0
			if (tdata->data_len < tdata->cbw.dDataTransferLength) {
			    dbg_tx(2, "fill data to pad up (%d < %d)",
				   tdata->data_len,
				   tdata->cbw.dDataTransferLength);
			    memset(tdata->data_buf + tdata->data_len, 0,
				   tdata->cbw.dDataTransferLength - tdata->data_len);
			    tdata->data_len = tdata->cbw.dDataTransferLength;
			}
#endif
			memcpy(urb->buffer, tdata->data_buf, tdata->data_len);
			urb->actual_length = tdata->data_len;
			tdata->csw.dDataResidue -= urb->actual_length;
			dbg_rx(3,"DATA (length %d)", urb->actual_length);
			dbgPRINTmem(dbgflg_usbdfd_tx,3,urb->buffer,min(urb->actual_length, 32u));
			if (usbd_send_urb(urb)) {
			    private->devstate = STATE_INVALID;
			    break;
			}
			/* FALLTHRU */
		    case STATE_DN:
			/* It's important to stall endpoint here */
			if(tdata->sense.key==ILLEGAL_REQUEST)  {
			     if ( tdata->cbw.CB[0] != REQUEST_SENSE  || tdata->sense.code == SENCODE_LBA_OUT_OF_RANGE  ) {
				  usbd_device_feature(private->device, CONFIG_USBD_STORAGE_IN_ENDPOINT | IN, 1);
				  usbd_device_feature(private->device, CONFIG_USBD_STORAGE_IN_ENDPOINT | IN, 0);
			     }

			}
			if (storage_send_CSW(private) < 0) {
			    private->devstate = STATE_INVALID;
			    break;
			}
			private->devstate = STATE_IDLE;
			break;
		    case STATE_IDLE:
			/* USB_REQ_BO_MASS_STORAGE_RESET received during
			   command processing */
			break;
		    default:
			private->devstate = STATE_INVALID;
			if (tdata->data_buf)
			    kfree(tdata->data_buf);
			tdata->data_buf = NULL;
			tdata->data_len = tdata->data_alloclen = 0;
		    }
		}
	    }
	    spin_unlock_bh(&storage_lock);
	}
        break;

    }
}


struct usb_function_operations function_ops = {
    event: storage_event,
    recv_urb: storage_recv_urb,
    urb_sent: storage_urb_sent,
    recv_setup: storage_recv_setup
};

struct usb_function_driver function_driver = {
    name: "usbd storage",
    ops: &function_ops,
    device_description: &storage_device_description,
    configurations: sizeof(storage_description)/sizeof(struct usb_configuration_description),
    configuration_description: storage_description,
    this_module: THIS_MODULE,
};


/*
 * RBC functions
 */

static void add_in_data(struct usb_storage_threaddata *tdata,
			void *buf, unsigned int len)
{
    if (tdata->data_len < tdata->cbw.dDataTransferLength) {
	memcpy(tdata->data_buf + tdata->data_len, buf,
	       min(tdata->cbw.dDataTransferLength - tdata->data_len, len));
    }
    tdata->data_len += len;
}

static int do_vpd(struct usb_storage_threaddata *tdata,
		  int page, unsigned int alloclen)
{
    u8 data[256];
    unsigned int len = 4;

    switch (page) {
    case 0x00:	/* Supported vital product data pages */
	data[4+0] = 0x00;	/* page list */
	data[4+1] = 0x80;
	data[4+2] = 0x83;
	len += 3;
	break;
    case 0x80:	/* Unit Serial Number */
	strcpy((char *)&data[4], storage_device_description.iSerialNumber);
	len += strlen(storage_device_description.iSerialNumber);
	break;
    case 0x83:	/* Vital Product Data Device Identification */
	sprintf(&data[4+4], "%s %s %s",
		storage_device_description.iManufacturer,
		storage_device_description.iProduct,
		storage_device_description.iSerialNumber);
	data[4+0] = 0x02;	/* ASCII */
	data[4+1] = 0x01;
	data[4+3] = strlen((char *)&data[4+4]);
	len += 4 + data[3];
	break;
    default:
	return -EINVAL;
    }
    data[0] = PERIPHERAL_DEVICE_TYPE;
    data[1] = page;	/* page code */
    data[2] = 0;
    data[3] = len - 4;
    add_in_data(tdata, data, min(len, alloclen));
    return 0;
}

static int do_inquiry(struct usb_storage_threaddata *tdata)
{
    u8 *CB = tdata->cbw.CB;
    u8 data[4 + 4 + 8 + 16 + 4];
    unsigned int len = 4 + 4 + 8 + 16 + 4;

    if (CB[1] & 0x02)	/* CMDDT */
	goto invalid_cdb_field;
    if (CB[1] & 0x01) {	/* EVPD */
	if (do_vpd(tdata, CB[2], CB[4]) < 0)
	    goto invalid_cdb_field;
	return 0;
    }
    if (CB[2])
	goto invalid_cdb_field;