storage.c

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

    data[0] = PERIPHERAL_DEVICE_TYPE;
    data[1] = 0x80;           /* removal */
    data[2] = 0x00;
    data[3] = 0x01;
    data[4] = 4 + 8 + 16 + 4;
    data[5] = 0x00;
    data[6] = 0x00;
    data[7] = 0x00;
    memset(&data[8], ' ', 8 + 16 + 4);
    memcpy(&data[8], storage_device_description.iManufacturer,
	   min_t(int, 8, strlen(storage_device_description.iManufacturer)));
    memcpy(&data[16], storage_device_description.iProduct,
	   min_t(int, 16, strlen(storage_device_description.iProduct)));
    add_in_data(tdata, data, min(len, (unsigned int)CB[4]));
    return 0;

  invalid_cdb_field:
    printk(KERN_WARNING STORAGE_MOD_NAME ": Invalid CDB field\n");
    tdata->sense.key = ILLEGAL_REQUEST;
    tdata->sense.code = SENCODE_INVALID_CDB_FIELD;
    return -EINVAL;
}

static int do_mode_select(struct usb_storage_threaddata *tdata)
{
    u8 *CB = tdata->cbw.CB;
    if (tdata->data_len < CB[4]) {
	printk(KERN_ERR STORAGE_MOD_NAME ": phase error on MODE_SELECT\n");
	tdata->csw.bStatus = CSW_STAT_PERR;
	return -EINVAL;
    }
    /* nothing changeable */
    tdata->csw.dDataResidue -= CB[4];
    return 0;
}

static int do_mode_sense(struct usb_storage_threaddata *tdata)
{
    u8 *CB = tdata->cbw.CB;
    u8 data[4 + 13];
    unsigned int len = 4 + 13;
    int page = CB[2] & 0x3f;

    if ((CB[2] & 0xc0) == 0x40)	/* PC=01b: changable values */
	goto invalid_cdb_field;
    if (page != 0x06 && page != 0x3f)	/* page 6 or all */
	goto invalid_cdb_field;
    /* mode parameter header (c.f. SPC-2 8.3, RBC 5.8.2) */
    data[0] = len - 1;
    data[1] = 0;
    data[2] = 0;
    data[3] = 0;
    /* parameter page 6 (c.f. RBC 5.8.3) */
    data[4] = 0x80 | 0x06;
    data[5] = 13 - 2;
    data[6] = 0;
    put_unaligned(cpu_to_be16(STORAGE_BLOCK_SIZE), (u16 *)&data[7]);
    data[9] = 0;
    put_unaligned(cpu_to_be32(tdata->num_blocks), (u32 *)&data[10]);
    data[14] = 0;
    data[15] = 0x03;	/* FORMATD,LOCKD */
    data[16] = 0;
    add_in_data(tdata, data, min(len, (unsigned int)CB[4]));
    return 0;

  invalid_cdb_field:
    printk(KERN_WARNING STORAGE_MOD_NAME ": Invalid CDB field\n");
    tdata->sense.key = ILLEGAL_REQUEST;
    tdata->sense.code = SENCODE_INVALID_CDB_FIELD;
    return -EINVAL;
}

static int do_read(struct usb_storage_threaddata *tdata)
{
    u8 *CB = tdata->cbw.CB;
    unsigned int lba = be32_to_cpu(get_unaligned((u32 *)&CB[2]));
    unsigned int llen = be16_to_cpu(get_unaligned((u16 *)&CB[7]));
    unsigned int len = llen * STORAGE_BLOCK_SIZE;
    loff_t  tmp_offset;
    int rlen;
    mm_segment_t fs;

    if (tdata->cbw.dDataTransferLength < len)
	goto phase_error;
    if (lba + llen > tdata->num_blocks)
	goto lba_out_of_range;
    if (  ( tmp_offset =  tdata->real_fd->f_op->llseek(tdata->real_fd, (off_t)lba * STORAGE_BLOCK_SIZE,
	      0 /* SEEK_SET */) )   == (off_t)-1) {
	printk (KERN_ERR STORAGE_MOD_NAME ": %s: lseek error on LBA %u \n",
	       tdata->filename, lba);
	goto medium_error;
    }
    fs = get_fs(); set_fs( KERNEL_DS );
    rlen=tdata->real_fd->f_op->read(tdata->real_fd, tdata->data_buf, len, &tmp_offset );
    set_fs( fs );
    if (rlen != len) {
	printk (KERN_ERR STORAGE_MOD_NAME ": %s: read error on LBA %u (read %d) \n",
	       tdata->filename, lba, rlen);
	goto medium_error;
    }
    tdata->data_len = len;
    tdata->stat.read_blocks += llen;
    return 0;

  phase_error:
    printk(KERN_ERR STORAGE_MOD_NAME ": phase error on READ\n");
    tdata->csw.bStatus = CSW_STAT_PERR;
    return -EINVAL;
  lba_out_of_range:
    printk(KERN_WARNING STORAGE_MOD_NAME ": LBA out of range\n");
    tdata->sense.key = ILLEGAL_REQUEST;
    tdata->sense.code = SENCODE_LBA_OUT_OF_RANGE;
    return -EINVAL;
  medium_error:
    printk(KERN_WARNING STORAGE_MOD_NAME ": Medium error\n");
    tdata->sense.key = MEDIUM_ERROR;
    return -EINVAL;
}

static int do_readcapacity(struct usb_storage_threaddata *tdata)
{
    u32 data[2];
    data[0] = cpu_to_be32(tdata->num_blocks - 1);
    data[1] = cpu_to_be32(STORAGE_BLOCK_SIZE);
    add_in_data(tdata, &data, sizeof(data));
    return 0;
}

static int do_request_sense(struct usb_storage_threaddata *tdata)
{
    u8 *CB = tdata->cbw.CB;
    u8 data[13];
    unsigned int len = sizeof(data);
    data[0] = 0x70;	/* current errors */
    data[1] = 0;
    data[2] = tdata->sense.key;
    put_unaligned(cpu_to_be32(tdata->sense.info), (u32 *)&data[3]);
    data[7] = len - 7;;
    put_unaligned(cpu_to_be32(tdata->sense.cmdinfo), (u32 *)&data[8]);
    data[12] = tdata->sense.code;
    add_in_data(tdata, data, min(len, (unsigned int)CB[4]));
    return 0;
}

static int do_write(struct usb_storage_threaddata *tdata)
{
    u8 *CB = tdata->cbw.CB;
    unsigned int lba = be32_to_cpu(get_unaligned((u32 *)&CB[2]));
    unsigned int llen = be16_to_cpu(get_unaligned((u16 *)&CB[7]));
    unsigned int len = llen * STORAGE_BLOCK_SIZE;
    loff_t  tmp_offset;
    int rlen;
    mm_segment_t fs;

    if (tdata->data_len < len)
	goto phase_error;
    if (lba + llen > tdata->num_blocks)
	goto lba_out_of_range;
    if (  ( tmp_offset = tdata->real_fd->f_op->llseek(tdata->real_fd, (off_t)lba * STORAGE_BLOCK_SIZE,
	      0 /* SEEK_SET */) ) == (off_t)-1) {
	printk (KERN_ERR STORAGE_MOD_NAME ": %s: lseek error on LBA %u (errno %d)\n",
	       tdata->filename, lba, errno);
	goto medium_error;
    }
    fs = get_fs(); set_fs( KERNEL_DS );
    rlen=tdata->real_fd->f_op->write(tdata->real_fd, tdata->data_buf, len, &tmp_offset);
    set_fs( fs );
    if (rlen != len) {
	printk (KERN_ERR STORAGE_MOD_NAME ": %s: write error on LBA %u (errno %d)\n",
	       tdata->filename, lba, errno);
	goto medium_error;
    }
    tdata->csw.dDataResidue -= len;
    tdata->stat.write_blocks += llen;
    return 0;

  phase_error:
    printk(KERN_ERR STORAGE_MOD_NAME ": phase error on WRITE\n");
    tdata->csw.bStatus = CSW_STAT_PERR;
    return -EINVAL;
  lba_out_of_range:
    printk(KERN_WARNING STORAGE_MOD_NAME ": LBA out of range\n");
    tdata->sense.key = ILLEGAL_REQUEST;
    tdata->sense.code = SENCODE_LBA_OUT_OF_RANGE;
    return -EINVAL;
  medium_error:
    printk(KERN_WARNING STORAGE_MOD_NAME ": Medium error\n");
    tdata->sense.key = MEDIUM_ERROR;
    return -EINVAL;
}

/* process RBC Command Block */
static int storage_process_CB(struct usb_storage_threaddata *tdata)
{
    u8 *CB = tdata->cbw.CB;

    if (CB[0] != REQUEST_SENSE) {
	/* initialize sense data */
	tdata->sense.key = NO_SENSE;
	tdata->sense.info = 0;
	tdata->sense.code = SENCODE_NO_SENSE;
	tdata->sense.cmdinfo = 0;
    }

    switch (CB[0]) {
    case FORMAT_UNIT:
	dbg_rx(2, "FORMAT_UNIT");
	goto unsupported;
    case INQUIRY:
	dbg_rx(2, "INQUIRY");
	tdata->stat.inquiry++;
	if (tdata->data_len)
	    goto phase_error;
	return do_inquiry(tdata);
    case MODE_SELECT:
	dbg_rx(2, "MODE_SELECT");
	tdata->stat.mode_select++;
	return do_mode_select(tdata);
    case MODE_SENSE:
	dbg_rx(2, "MODE_SENSE");
	tdata->stat.mode_sense++;
	if (tdata->data_len)
	    goto phase_error;
	return do_mode_sense(tdata);
    case PERSISTENT_RESERVE_IN:
	dbg_rx(2, "PERSISTENT_RESERVE_IN");
	goto unsupported;
    case PERSISTENT_RESERVE_OUT:
	dbg_rx(2, "PERSISTENT_RESERVE_OUT");
	goto unsupported;
    case ALLOW_MEDIUM_REMOVAL:
	dbg_rx(2, "ALLOW_MEDIUM_REMOVAL");
	goto unsupported;
    case READ_10:
	dbg_rx(2, "READ_10");
	tdata->stat.read_10++;
	if (tdata->data_len)
	    goto phase_error;
	return do_read(tdata);
    case READ_CAPACITY:
	dbg_rx(2, "READ_CAPACITY");
	tdata->stat.read_capacity++;
	if (tdata->data_len)
	    goto phase_error;
	return do_readcapacity(tdata);
    case RELEASE:
	dbg_rx(2, "RELEASE");
	goto unsupported;
    case REQUEST_SENSE:
	dbg_rx(2, "REQUEST_SENSE");
	tdata->stat.request_sense++;
	if (tdata->data_len)
	    goto phase_error;
	return do_request_sense(tdata);
    case RESERVE:
	dbg_rx(2, "RESERVE");
	goto unsupported;
    case START_STOP:
	dbg_rx(2, "START_STOP");
	tdata->stat.start_stop++;
	if (tdata->data_len)
	    goto phase_error;
	/* do nothing */
	break;
    case SYNCHRONIZE_CACHE:
	dbg_rx(2, "SYNCHRONIZE_CACHE");
	goto unsupported;
    case TEST_UNIT_READY:
	dbg_rx(2, "TEST_UNIT_READY");
	tdata->stat.test_unit_ready++;
	if (tdata->data_len)
	    goto phase_error;
	/* do nothing */
	break;
    case VERIFY:
	dbg_rx(2, "VERIFY");
	tdata->stat.verify++;
	if (tdata->data_len)
	    goto phase_error;
	/* do nothing */
	break;
    case WRITE_10:
	dbg_rx(2, "WRITE_10");
	tdata->stat.write_10++;
	return do_write(tdata);
    case WRITE_BUFFER:
	dbg_rx(2, "WRITE_BUFFER");
	tdata->stat.write_buffer++;
	if (tdata->data_len)
	    goto phase_error;
	/* do nothing */
	break;
    default:
	printk(KERN_ERR STORAGE_MOD_NAME ": unknown RBC command (%02x)\n",
	       CB[0]);
	goto unsupported;
    }

    return 0;

  phase_error:
    printk(KERN_ERR STORAGE_MOD_NAME ": phase error on RBC command %02x\n", CB[0]);
    tdata->csw.bStatus = CSW_STAT_PERR;
    return -EINVAL;
  unsupported:
    tdata->stat.unsupported++;
    tdata->sense.key = ILLEGAL_REQUEST;
    tdata->sense.code = SENCODE_INVALID_COMMAND;
    return -EINVAL;
}

static int storage_thread(void *data) 
{
    struct usb_storage_private *private;
    off_t off;

    daemonize ();
    reparent_to_init();
    spin_lock_irq(&current->sigmask_lock);
    sigemptyset(&current->blocked);
    recalc_sigpending(current);
    spin_unlock_irq(&current->sigmask_lock);

    strcpy (current->comm, STORAGE_MOD_NAME);

    private = &storage_private;
    memset(private, 0, sizeof(*private));
    if (!filename)
	filename = STORAGE_DEFAULT_FILENAME;
    strcpy(private->tdata.filename, filename);
    private->tdata.real_fd = filp_open (private->tdata.filename, O_RDWR, 0);
    if (IS_ERR(private->tdata.real_fd)) {
	printk(KERN_ERR STORAGE_MOD_NAME ": %s open failed (errno %d)\n",
	       private->tdata.filename, errno);
	storage_thread_terminating = 1;
	goto end;
    }
    if ((off = private->tdata.real_fd->f_op->llseek(private->tdata.real_fd, 0, 2 /* SEEK_END*/)) == (off_t)-1) {
	printk (KERN_ERR STORAGE_MOD_NAME ": %s lseek failed (errno %d)\n",
	       private->tdata.filename, errno);
	filp_close(private->tdata.real_fd,0);
	private->tdata.real_fd = NULL;
	storage_thread_terminating = 1;
	goto end;
    }
    private->tdata.num_blocks = off / STORAGE_BLOCK_SIZE;
    private->devstate = STATE_INVALID;
    printk(KERN_INFO STORAGE_MOD_NAME ": storage filename %s, %d blocks\n",
	   private->tdata.filename, private->tdata.num_blocks);

    // let startup continue
    up(&storage_sem_start);

    // process loop
    for (storage_thread_terminating = 0; !storage_thread_terminating;) {
        // wait for someone to tell us to do something
        down(&storage_sem_work);

	spin_lock_bh(&storage_lock);	/* prevent event handler */
	if (private->device && private->tdata.busy) {
	    spin_unlock_bh(&storage_lock);
	    /* this function CAN sleep */
	    if (storage_process_CB(&private->tdata) < 0) {
		if (private->tdata.csw.bStatus == CSW_STAT_GOOD)
		    private->tdata.csw.bStatus = CSW_STAT_FAILED;
	    }
	    spin_lock_bh(&storage_lock);
	    private->tdata.busy = 0;
	    /* kick event routine */
	    usbd_device_event(private->device, DEVICE_FUNCTION_PRIVATE, 1);
	}
	spin_unlock_bh(&storage_lock);
    }

    /* shutdown */
    if (private->tdata.data_buf)
	kfree(private->tdata.data_buf);
    filp_close(private->tdata.real_fd,0);

    // let the process stopping us know we are done and return
 end:
    up(&storage_sem_start);
    complete_and_exit (NULL, 0);
    return 0;
}

/**
 * storage_thread_kickoff - start command processing thread
 */
static int storage_thread_kickoff(void)
{
    storage_thread_terminating = 0;
    kernel_thread(&storage_thread, NULL, CLONE_FS | CLONE_FILES);
    down(&storage_sem_start);
    if (storage_thread_terminating)
	return -1;
    return 0;
}

/**
 * storage_thread_killoff - stop command processing thread
 */
static void storage_thread_killoff(void)
{
    if (!storage_thread_terminating) {
        storage_thread_terminating = 1;
        up(&storage_sem_work);
        down(&storage_sem_start);
    }
}

#define STORAGE_SERIAL_CHAR_MIN	12
static int __init storage_check_serial(const char *s)
{
    // c.f. USB Mass Storage Class Bulk-Only Transport 4.1.1 Serial Number
    if (strlen(s) < STORAGE_SERIAL_CHAR_MIN)
	return 0;
    while (*s) {
	// '0'-'9', 'A'-'F'
	if (!isxdigit(*s) || islower(*s))
	    return 0;
	s++;
    }
    return 1;
}

/*
 * storage_modinit - module init
 *
 */
static int __init storage_modinit(void)
{
    static char serial_str[STORAGE_SERIAL_CHAR_MIN + 1];
    int i, len;

    printk(KERN_INFO "%s (dbg=\"%s\")\n", __usbd_module_info, dbg?dbg:"");

    if (vendor_id) {
        storage_device_description.idVendor = vendor_id;
    }
    if (product_id) {
        storage_device_description.idProduct = product_id;
    }
    if (!serial_number)
	serial_number = storage_device_description.iSerialNumber;
    len = strlen(serial_number);
    while (len++ < STORAGE_SERIAL_CHAR_MIN)
	strcat(serial_str, "0");
    strcat(serial_str, serial_number);
    for (i = 0; serial_str[i]; i++) {
	if (islower(serial_str[i]))
	    serial_str[i] = toupper(serial_str[i]);
    }
    storage_device_description.iSerialNumber = serial_str;

    printk(KERN_INFO "vendor_id: %04x product_id: %04x, serial_number: %s\n",
	   storage_device_description.idVendor,
	   storage_device_description.idProduct,
	   storage_device_description.iSerialNumber);

    if (0 != scan_debug_options(STORAGE_MOD_NAME, dbg_table,dbg)) {
        return(-EINVAL);
    }

    if (!storage_check_serial(storage_device_description.iSerialNumber)) {
	printk(KERN_ERR "bad serial number (%s)\n",
	       storage_device_description.iSerialNumber);
	return -EINVAL;
    }

    spin_lock_init(&storage_lock);
    create_proc_read_entry(STORAGE_PROC_NAME, 0, NULL, storage_read_proc, 0);

    if (storage_thread_kickoff()) {
	return -ENODEV;
    }

    // register us with the usb device support layer
    //
    if (usbd_register_function(&function_driver)) {
        return -EINVAL;
    }

    // return
    return 0;
}


/* storage_modexit - module cleanup
 */
static void __exit storage_modexit(void)
{
    // de-register us with the usb device support layer
    // 
    usbd_deregister_function(&function_driver);
    remove_proc_entry(STORAGE_PROC_NAME, NULL);
    storage_thread_killoff();
}

module_init(storage_modinit);
module_exit(storage_modexit);

/*
 * Local variables:
 * c-basic-offset: 4
 * End:
 */