[SeaBIOS] [RFC PATCH 10/16] scsi: get physical chs geometry from mode page 0x04

[Paolo Bonzini]

The mode page is marked as obsolete, but QEMU can provide the information.

Signed-off-by: Paolo Bonzini 
---
 src/blockcmd.c |   33 ++++++++++++++++++++++++++++++++-
 src/blockcmd.h |   29 +++++++++++++++++++++++++++++
 2 files changed, 61 insertions(+), 1 deletions(-)

diff --git a/src/blockcmd.c b/src/blockcmd.c
index 8e82f09..39adcfc 100644
--- a/src/blockcmd.c
+++ b/src/blockcmd.c
@@ -120,9 +120,25 @@ scsi_init_drive(struct drive_s *drive, const char *s, int *pdt, char **desc)
     if (*pdt == SCSI_TYPE_CDROM)
         *desc = znprintf(MAXDESCSIZE, "DVD/CD [%s Drive %s %s %s]"
                               , s, vendor, product, rev);
-    else
+    else {
+        struct cdbres_mode_sense_geom geomdata;
+        ret = cdb_mode_sense_geom(&dop, &geomdata);
+        if (ret == 0) {
+            u32 cylinders;
+            cylinders = geomdata.cyl[0] << 16;
+            cylinders |= geomdata.cyl[1] << 8;
+            cylinders |= geomdata.cyl[2];
+            if (cylinders && geomdata.heads &&
+                ((u32)drive->sectors % (geomdata.heads * cylinders) == 0)) {
+                drive->pchs.cylinders = cylinders;
+                drive->pchs.heads = geomdata.heads;
+                drive->pchs.spt = (u32)drive->sectors
+		       / (geomdata.heads * cylinders);
+            }
+        }
         *desc = znprintf(MAXDESCSIZE, "%s Drive %s %s %s"
                               , s, vendor, product, rev);
+    }

return 0;
 }
@@ -176,6 +192,21 @@ cdb_read_capacity(struct disk_op_s *op, struct cdbres_read_capacity *data)
     return cdb_cmd_data(op, &cmd, sizeof(*data));
 }

+// Mode sense, geometry page.
+int
+cdb_mode_sense_geom(struct disk_op_s *op, struct cdbres_mode_sense_geom *data)
+{
+    struct cdb_mode_sense cmd;
+    memset(&cmd, 0, sizeof(cmd));
+    cmd.command = CDB_CMD_MODE_SENSE;
+    cmd.flags = 8; /* DBD */
+    cmd.page = MODE_PAGE_HD_GEOMETRY;
+    cmd.count = htons(sizeof(*data));
+    op->count = 1;
+    op->buf_fl = data;
+    return cdb_cmd_data(op, &cmd, sizeof(*data));
+}
+
 // Read sectors.
 int
 cdb_read(struct disk_op_s *op)
diff --git a/src/blockcmd.h b/src/blockcmd.h
index 13ae991..bace649 100644
--- a/src/blockcmd.h
+++ b/src/blockcmd.h
@@ -71,11 +71,40 @@ struct cdbres_inquiry {
     char rev[4];
 } PACKED;

+#define CDB_CMD_MODE_SENSE    0x5A
+#define MODE_PAGE_HD_GEOMETRY 0x04
+
+struct cdb_mode_sense {
+    u8 command;
+    u8 flags;
+    u8 page;
+    u32 reserved_03;
+    u16 count;
+    u8 reserved_09;
+    u8 pad[6];
+} PACKED;
+
+struct cdbres_mode_sense_geom {
+    u8 unused_00[3];
+    u8 read_only;
+    u32 unused_04;
+    u8 page;
+    u8 length;
+    u8 cyl[3];
+    u8 heads;
+    u8 precomp[3];
+    u8 reduced[3];
+    u16 step_rate;
+    u8 landing[3];
+    u16 rpm;
+} PACKED;
+
 // blockcmd.c
 int cdb_get_inquiry(struct disk_op_s *op, struct cdbres_inquiry *data);
 int cdb_get_sense(struct disk_op_s *op, struct cdbres_request_sense *data);
 int cdb_test_unit_ready(struct disk_op_s *op);
 int cdb_read_capacity(struct disk_op_s *op, struct cdbres_read_capacity *data);
+int cdb_mode_sense_geom(struct disk_op_s *op, struct cdbres_mode_sense_geom *data);
 int cdb_inquiry(struct disk_op_s *op, struct cdbres_inquiry *data);
 int cdb_read(struct disk_op_s *op);
 int cdb_write(struct disk_op_s *op);