static int ide_floppy_get_capacity(ide_drive_t *drive)
{
struct ide_disk_obj *floppy = drive->driver_data;
struct gendisk *disk = floppy->disk;
struct ide_atapi_pc pc;
u8 *cap_desc;
u8 pc_buf[256], header_len, desc_cnt;
int i, rc = 1, blocks, length;
ide_debug_log(IDE_DBG_FUNC, "enter");
drive->bios_cyl = 0;
drive->bios_head = drive->bios_sect = 0;
floppy->blocks = 0;
floppy->bs_factor = 1;
drive->capacity64 = 0;
ide_floppy_create_read_capacity_cmd(&pc);
if (ide_queue_pc_tail(drive, disk, &pc, pc_buf, pc.req_xfer)) {
printk(KERN_ERR PFX "Can't get floppy parameters\n");
return 1;
}
header_len = pc_buf[3];
cap_desc = &pc_buf[4];
desc_cnt = header_len / 8;
for (i = 0; i < desc_cnt; i++) {
unsigned int desc_start = 4 + i*8;
blocks = be32_to_cpup((__be32 *)&pc_buf[desc_start]);
length = be16_to_cpup((__be16 *)&pc_buf[desc_start + 6]);
ide_debug_log(IDE_DBG_PROBE, "Descriptor %d: %dkB, %d blocks, "
"%d sector size",
i, blocks * length / 1024,
blocks, length);
if (i)
continue;
switch (pc_buf[desc_start + 4] & 0x03) {
case CAPACITY_UNFORMATTED:
if (!(drive->atapi_flags & IDE_AFLAG_CLIK_DRIVE))
break;
case CAPACITY_CURRENT:
if (memcmp(cap_desc, &floppy->cap_desc, 8))
printk(KERN_INFO PFX "%s: %dkB, %d blocks, %d "
"sector size\n",
drive->name, blocks * length / 1024,
blocks, length);
memcpy(&floppy->cap_desc, cap_desc, 8);
if (!length || length % 512) {
printk(KERN_NOTICE PFX "%s: %d bytes block size"
" not supported\n", drive->name, length);
} else {
floppy->blocks = blocks;
floppy->block_size = length;
floppy->bs_factor = length / 512;
if (floppy->bs_factor != 1)
printk(KERN_NOTICE PFX "%s: Warning: "
"non 512 bytes block size not "
"fully supported\n",
drive->name);
drive->capacity64 =
floppy->blocks * floppy->bs_factor;
rc = 0;
}
break;
case CAPACITY_NO_CARTRIDGE:
printk(KERN_ERR PFX "%s: No disk in drive\n",
drive->name);
break;
case CAPACITY_INVALID:
printk(KERN_ERR PFX "%s: Invalid capacity for disk "
"in drive\n", drive->name);
break;
}
ide_debug_log(IDE_DBG_PROBE, "Descriptor 0 Code: %d",
pc_buf[desc_start + 4] & 0x03);
}
if (!(drive->atapi_flags & IDE_AFLAG_CLIK_DRIVE))
(void) ide_floppy_get_flexible_disk_page(drive, &pc);
return rc;
}
/*
* Determine if a media is present in the floppy drive, and if so, its LBA
* capacity.
*/
static int ide_floppy_get_capacity(ide_drive_t *drive)
{
struct ide_disk_obj *floppy = drive->driver_data;
struct gendisk *disk = floppy->disk;
struct ide_atapi_pc pc;
u8 *cap_desc;
u8 pc_buf[256], header_len, desc_cnt;
int i, rc = 1, blocks, length;
pax_track_stack();
ide_debug_log(IDE_DBG_FUNC, "enter");
drive->bios_cyl = 0;
drive->bios_head = drive->bios_sect = 0;
floppy->blocks = 0;
floppy->bs_factor = 1;
drive->capacity64 = 0;
ide_floppy_create_read_capacity_cmd(&pc);
if (ide_queue_pc_tail(drive, disk, &pc, pc_buf, pc.req_xfer)) {
printk(KERN_ERR PFX "Can't get floppy parameters\n");
return 1;
}
header_len = pc_buf[3];
cap_desc = &pc_buf[4];
desc_cnt = header_len / 8; /* capacity descriptor of 8 bytes */
for (i = 0; i < desc_cnt; i++) {
unsigned int desc_start = 4 + i*8;
blocks = be32_to_cpup((__be32 *)&pc_buf[desc_start]);
length = be16_to_cpup((__be16 *)&pc_buf[desc_start + 6]);
ide_debug_log(IDE_DBG_PROBE, "Descriptor %d: %dkB, %d blocks, "
"%d sector size",
i, blocks * length / 1024,
blocks, length);
if (i)
continue;
/*
* the code below is valid only for the 1st descriptor, ie i=0
*/
switch (pc_buf[desc_start + 4] & 0x03) {
/* Clik! drive returns this instead of CAPACITY_CURRENT */
case CAPACITY_UNFORMATTED:
if (!(drive->atapi_flags & IDE_AFLAG_CLIK_DRIVE))
/*
* If it is not a clik drive, break out
* (maintains previous driver behaviour)
*/
break;
case CAPACITY_CURRENT:
/* Normal Zip/LS-120 disks */
if (memcmp(cap_desc, &floppy->cap_desc, 8))
printk(KERN_INFO PFX "%s: %dkB, %d blocks, %d "
"sector size\n",
drive->name, blocks * length / 1024,
blocks, length);
memcpy(&floppy->cap_desc, cap_desc, 8);
if (!length || length % 512) {
printk(KERN_NOTICE PFX "%s: %d bytes block size"
" not supported\n", drive->name, length);
} else {
floppy->blocks = blocks;
floppy->block_size = length;
floppy->bs_factor = length / 512;
if (floppy->bs_factor != 1)
printk(KERN_NOTICE PFX "%s: Warning: "
"non 512 bytes block size not "
"fully supported\n",
drive->name);
drive->capacity64 =
floppy->blocks * floppy->bs_factor;
rc = 0;
}
break;
case CAPACITY_NO_CARTRIDGE:
/*
* This is a KERN_ERR so it appears on screen
* for the user to see
*/
printk(KERN_ERR PFX "%s: No disk in drive\n",
drive->name);
break;
case CAPACITY_INVALID:
printk(KERN_ERR PFX "%s: Invalid capacity for disk "
"in drive\n", drive->name);
break;
}
ide_debug_log(IDE_DBG_PROBE, "Descriptor 0 Code: %d",
pc_buf[desc_start + 4] & 0x03);
}
/* Clik! disk does not support get_flexible_disk_page */
if (!(drive->atapi_flags & IDE_AFLAG_CLIK_DRIVE))
(void) ide_floppy_get_flexible_disk_page(drive, &pc);
return rc;
}
