From fc1b676ce64cec7274ea985ceae1fc3e2f1e1d86 Mon Sep 17 00:00:00 2001
From: Bernd Herzog <bernd@nachregenkommtsonne.de>
Date: Fri, 31 Mar 2023 22:34:55 +0200
Subject: [PATCH] implemented rest of scsi stack
---
firmware/baseband/sd_over_usb/scsi.c | 378 ++++++++++++++++++++++-----
firmware/baseband/sd_over_usb/scsi.h | 78 ++++++
2 files changed, 396 insertions(+), 60 deletions(-)
diff --git a/firmware/baseband/sd_over_usb/scsi.c b/firmware/baseband/sd_over_usb/scsi.c
index b4b035364..18397ecdc 100644
--- a/firmware/baseband/sd_over_usb/scsi.c
+++ b/firmware/baseband/sd_over_usb/scsi.c
@@ -62,23 +62,9 @@ volatile bool inquiry_stage_one_send = false;
void inquiry_cb(void* user_data, unsigned int bytes_transferred)
{
inquiry_stage_one_send = true;
-
- //HALT_UNTIL_DEBUGGING();
-
- //faulty
- // usb_transfer_schedule_block(
- // &usb_endpoint_bulk_out,
- // &usb_bulk_buffer[0],
- // USB_TRANSFER_SIZE,
- // handle_inquiry_3,
- // msd_cbw_data);
-
-
-//does not send
-
}
-void handle_inquiry(msd_cbw_t *msd_cbw_data){
+void handle_inquiry(msd_cbw_t *msd_cbw_data) {
inquiry_stage_one_send = false;
memcpy(&usb_bulk_buffer[0], &default_scsi_inquiry_response, sizeof(scsi_inquiry_response_t));
usb_transfer_schedule_block(
@@ -98,16 +84,290 @@ void handle_inquiry(msd_cbw_t *msd_cbw_data){
};
memcpy(&usb_bulk_buffer[0x4000], &csw, sizeof(msd_csw_t));
- // does not gets send
usb_transfer_schedule_block(
&usb_endpoint_bulk_in,
&usb_bulk_buffer[0x4000],
sizeof(msd_csw_t),
- NULL, //handle_inquiry_3,
- NULL); //msd_cbw_data);
+ NULL,
+ NULL);
}
+typedef struct {
+ uint8_t header[4];
+ uint8_t blocknum[4];
+ uint8_t blocklen[4];
+} scsi_read_format_capacities_response_t;
+
+volatile bool capacities_stage_one_send = false;
+void capacities_cb(void* user_data, unsigned int bytes_transferred)
+{
+ capacities_stage_one_send = true;
+}
+
+void read_format_capacities(msd_cbw_t *msd_cbw_data) {
+ uint16_t len = msd_cbw_data->cmd_data[7] << 8 | msd_cbw_data->cmd_data[8];
+
+ if (len != 0) {
+ scsi_read_format_capacities_response_t ret = {
+ .header = {0, 0, 0, 1 * 8 /* num_entries * 8 */},
+ .blocknum = {0, 0, (1024 * 8) >> 8, 0}, // 32GB
+ .blocklen = {0b10 /* formated */, 0, (512) >> 8, 0}
+ };
+
+ capacities_stage_one_send = false;
+ memcpy(&usb_bulk_buffer[0], &ret, sizeof(scsi_read_format_capacities_response_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0],
+ sizeof(scsi_inquiry_response_t),
+ capacities_cb,
+ msd_cbw_data);
+
+ while (!capacities_stage_one_send);
+ }
+
+
+ msd_csw_t csw = {
+ .signature = MSD_CSW_SIGNATURE,
+ .tag = msd_cbw_data->tag,
+ .data_residue = 0,
+ .status = 0
+ };
+
+ memcpy(&usb_bulk_buffer[0x4000], &csw, sizeof(msd_csw_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0x4000],
+ sizeof(msd_csw_t),
+ NULL,
+ NULL);
+
+}
+
+typedef struct {
+ uint32_t last_block_addr;
+ uint32_t block_size;
+} scsi_read_capacity10_response_t;
+
+volatile bool capacity10_stage_one_send = false;
+void capacity10_cb(void* user_data, unsigned int bytes_transferred)
+{
+ capacity10_stage_one_send = true;
+}
+
+void read_capacity10(msd_cbw_t *msd_cbw_data) {
+ capacity10_stage_one_send = false;
+
+ scsi_read_capacity10_response_t ret = {
+ .last_block_addr = cpu_to_be32(8 * 1024 * 1024 - 1),
+ .block_size = cpu_to_be32(512)
+ };
+
+ memcpy(&usb_bulk_buffer[0], &ret, sizeof(scsi_read_capacity10_response_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0],
+ sizeof(scsi_read_capacity10_response_t),
+ capacity10_cb,
+ msd_cbw_data);
+
+ while (!capacity10_stage_one_send);
+
+ msd_csw_t csw = {
+ .signature = MSD_CSW_SIGNATURE,
+ .tag = msd_cbw_data->tag,
+ .data_residue = 0,
+ .status = 0
+ };
+
+ memcpy(&usb_bulk_buffer[0x4000], &csw, sizeof(msd_csw_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0x4000],
+ sizeof(msd_csw_t),
+ NULL,
+ NULL);
+
+}
+
+
+typedef struct {
+ uint8_t byte[18];
+} scsi_sense_response_t;
+
+volatile bool sense_stage_one_send = false;
+void sense_cb(void* user_data, unsigned int bytes_transferred)
+{
+ sense_stage_one_send = true;
+}
+
+
+void request_sense(msd_cbw_t *msd_cbw_data) {
+ sense_stage_one_send = false;
+
+ scsi_sense_response_t ret = {
+ .byte = { 0x70, 0, SCSI_SENSE_KEY_GOOD, 0,
+ 0, 0, 0, 8,
+ 0, 0 ,0 ,0,
+ SCSI_ASENSE_NO_ADDITIONAL_INFORMATION, SCSI_ASENSEQ_NO_QUALIFIER, 0, 0,
+ 0, 0 }
+ };
+
+ memcpy(&usb_bulk_buffer[0], &ret, sizeof(scsi_sense_response_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0],
+ sizeof(scsi_sense_response_t),
+ sense_cb,
+ msd_cbw_data);
+
+ while (!sense_stage_one_send);
+
+ msd_csw_t csw = {
+ .signature = MSD_CSW_SIGNATURE,
+ .tag = msd_cbw_data->tag,
+ .data_residue = 0,
+ .status = 0
+ };
+
+ memcpy(&usb_bulk_buffer[0x4000], &csw, sizeof(msd_csw_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0x4000],
+ sizeof(msd_csw_t),
+ NULL,
+ NULL);
+}
+
+typedef struct {
+ uint8_t byte[4];
+} scsi_mode_sense6_response_t;
+
+volatile bool sense6_stage_one_send = false;
+void sense6_cb(void* user_data, unsigned int bytes_transferred)
+{
+ sense6_stage_one_send = true;
+}
+
+void mode_sense6 (msd_cbw_t *msd_cbw_data) {
+ sense6_stage_one_send = false;
+
+ scsi_mode_sense6_response_t ret = {
+ .byte = {
+ sizeof(scsi_mode_sense6_response_t) - 1,
+ 0,
+ 0x01 << 7, // 0 for not write protected
+ 0 }
+ };
+
+ memcpy(&usb_bulk_buffer[0], &ret, sizeof(scsi_mode_sense6_response_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0],
+ sizeof(scsi_mode_sense6_response_t),
+ sense6_cb,
+ msd_cbw_data);
+
+ while (!sense6_stage_one_send);
+
+ msd_csw_t csw = {
+ .signature = MSD_CSW_SIGNATURE,
+ .tag = msd_cbw_data->tag,
+ .data_residue = 0,
+ .status = 0
+ };
+
+ memcpy(&usb_bulk_buffer[0x4000], &csw, sizeof(msd_csw_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0x4000],
+ sizeof(msd_csw_t),
+ NULL,
+ NULL);
+}
+
+typedef struct {
+ uint32_t first_lba;
+ uint16_t blk_cnt;
+} data_request_t;
+
+static data_request_t decode_data_request(const uint8_t *cmd) {
+
+ data_request_t req;
+ uint32_t lba;
+ uint16_t blk;
+
+ memcpy(&lba, &cmd[2], sizeof(lba));
+ memcpy(&blk, &cmd[7], sizeof(blk));
+
+ req.first_lba = be32_to_cpu(lba);
+ req.blk_cnt = be16_to_cpu(blk);
+
+ return req;
+}
+
+volatile uint32_t read10_blocks_send = 0;
+void read10_cb(void* user_data, unsigned int bytes_transferred)
+{
+ read10_blocks_send++;
+}
+
+
+void data_read10(msd_cbw_t *msd_cbw_data) {
+ read10_blocks_send = 0;
+
+ data_request_t req = decode_data_request(msd_cbw_data->cmd_data);
+
+ for (size_t block_index = 0; block_index < req.blk_cnt; block_index++) {
+ memset(&usb_bulk_buffer[0], 0, 512);
+
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0],
+ 512,
+ read10_cb,
+ msd_cbw_data);
+
+ while (read10_blocks_send <= block_index);
+ }
+
+ msd_csw_t csw = {
+ .signature = MSD_CSW_SIGNATURE,
+ .tag = msd_cbw_data->tag,
+ .data_residue = 0,
+ .status = 0
+ };
+
+ memcpy(&usb_bulk_buffer[0x4000], &csw, sizeof(msd_csw_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0x4000],
+ sizeof(msd_csw_t),
+ NULL,
+ NULL);
+}
+
+
+
+
+void test_unit_ready(msd_cbw_t *msd_cbw_data) {
+ msd_csw_t csw = {
+ .signature = MSD_CSW_SIGNATURE,
+ .tag = msd_cbw_data->tag,
+ .data_residue = 0,
+ .status = 0
+ };
+
+ memcpy(&usb_bulk_buffer[0x4000], &csw, sizeof(msd_csw_t));
+ usb_transfer_schedule_block(
+ &usb_endpoint_bulk_in,
+ &usb_bulk_buffer[0x4000],
+ sizeof(msd_csw_t),
+ NULL,
+ NULL);
+}
+
void scsi_command(msd_cbw_t *msd_cbw_data) {
switch (msd_cbw_data->cmd_data[0]) {
@@ -117,50 +377,48 @@ void scsi_command(msd_cbw_t *msd_cbw_data) {
break;
- default:
- HALT_UNTIL_DEBUGGING();
+ case SCSI_CMD_REQUEST_SENSE:
+ request_sense(msd_cbw_data);
+ break;
+
+ case SCSI_CMD_READ_CAPACITY_10:
+ read_capacity10(msd_cbw_data);
+ break;
+
+ case SCSI_CMD_READ_10:
+ data_read10(msd_cbw_data);
+ break;
+
+ /*
+ case SCSI_CMD_WRITE_10:
+ ret = data_read_write10(scsip, cmd);
+ break;
+*/
+ case SCSI_CMD_TEST_UNIT_READY:
+ test_unit_ready(msd_cbw_data);
+ break;
+
+ case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL:
+ test_unit_ready(msd_cbw_data);
+ // ret = cmd_ignored(scsip, cmd);
+ break;
+
+ case SCSI_CMD_MODE_SENSE_6:
+ mode_sense6(msd_cbw_data);
+ break;
+
+ case SCSI_CMD_READ_FORMAT_CAPACITIES:
+ read_format_capacities(msd_cbw_data);
+ break;
+
+ case SCSI_CMD_VERIFY_10:
+ test_unit_ready(msd_cbw_data);
break;
/*
- case SCSI_CMD_REQUEST_SENSE:
- ret = request_sense(scsip, cmd);
- break;
-
- case SCSI_CMD_READ_CAPACITY_10:
- ret = read_capacity10(scsip, cmd);
- break;
-
- case SCSI_CMD_READ_10:
- ret = data_read_write10(scsip, cmd);
- break;
-
- case SCSI_CMD_WRITE_10:
- ret = data_read_write10(scsip, cmd);
- break;
-
- case SCSI_CMD_TEST_UNIT_READY:
- ret = test_unit_ready(scsip, cmd);
- break;
-
- case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL:
- ret = cmd_ignored(scsip, cmd);
- break;
-
- case SCSI_CMD_MODE_SENSE_6:
- ret = mode_sense6(scsip, cmd);
- break;
-
- case SCSI_CMD_READ_FORMAT_CAPACITIES:
- ret = read_format_capacities(scsip, cmd);
- break;
-
- case SCSI_CMD_VERIFY_10:
- ret = cmd_ignored(scsip, cmd);
- break;
-
- default:
- ret = cmd_unhandled(scsip, cmd);
- break;
- */
+ default:
+ ret = cmd_unhandled(scsip, cmd);
+ break;
+ */
}
// if (ret == SCSI_SUCCESS)
diff --git a/firmware/baseband/sd_over_usb/scsi.h b/firmware/baseband/sd_over_usb/scsi.h
index 616c9aff9..85dedd1a2 100644
--- a/firmware/baseband/sd_over_usb/scsi.h
+++ b/firmware/baseband/sd_over_usb/scsi.h
@@ -26,6 +26,36 @@
#define SCSI_CMD_WRITE_10 0x2A
#define SCSI_CMD_VERIFY_10 0x2F
+#define SCSI_SENSE_KEY_GOOD 0x00
+#define SCSI_SENSE_KEY_RECOVERED_ERROR 0x01
+#define SCSI_SENSE_KEY_NOT_READY 0x02
+#define SCSI_SENSE_KEY_MEDIUM_ERROR 0x03
+#define SCSI_SENSE_KEY_HARDWARE_ERROR 0x04
+#define SCSI_SENSE_KEY_ILLEGAL_REQUEST 0x05
+#define SCSI_SENSE_KEY_UNIT_ATTENTION 0x06
+#define SCSI_SENSE_KEY_DATA_PROTECT 0x07
+#define SCSI_SENSE_KEY_BLANK_CHECK 0x08
+#define SCSI_SENSE_KEY_VENDOR_SPECIFIC 0x09
+#define SCSI_SENSE_KEY_COPY_ABORTED 0x0A
+#define SCSI_SENSE_KEY_ABORTED_COMMAND 0x0B
+#define SCSI_SENSE_KEY_VOLUME_OVERFLOW 0x0D
+#define SCSI_SENSE_KEY_MISCOMPARE 0x0E
+
+#define SCSI_ASENSE_NO_ADDITIONAL_INFORMATION 0x00
+#define SCSI_ASENSE_LOGICAL_UNIT_NOT_READY 0x04
+#define SCSI_ASENSE_INVALID_FIELD_IN_CDB 0x24
+#define SCSI_ASENSE_NOT_READY_TO_READY_CHANGE 0x28
+#define SCSI_ASENSE_WRITE_PROTECTED 0x27
+#define SCSI_ASENSE_FORMAT_ERROR 0x31
+#define SCSI_ASENSE_INVALID_COMMAND 0x20
+#define SCSI_ASENSE_LBA_OUT_OF_RANGE 0x21
+#define SCSI_ASENSE_MEDIUM_NOT_PRESENT 0x3A
+
+#define SCSI_ASENSEQ_NO_QUALIFIER 0x00
+#define SCSI_ASENSEQ_FORMAT_COMMAND_FAILED 0x01
+#define SCSI_ASENSEQ_INIT_COMMAND_REQUIRED 0x02
+#define SCSI_ASENSEQ_OPERATION_IN_PROGRESS 0x07
+
#define MSD_CBW_SIGNATURE 0x43425355
#define MSD_CSW_SIGNATURE 0x53425355
@@ -41,6 +71,54 @@ typedef struct {
uint8_t cmd_data[16];
} __attribute__((packed)) msd_cbw_t;
+static inline uint16_t bswap_16(const uint16_t x)
+ __attribute__ ((warn_unused_result))
+ __attribute__ ((const))
+ __attribute__ ((always_inline));
+
+static inline uint16_t bswap_16(const uint16_t x) {
+
+ uint8_t tmp;
+ union { uint16_t x; uint8_t b[2]; } data;
+
+ data.x = x;
+ tmp = data.b[0];
+ data.b[0] = data.b[1];
+ data.b[1] = tmp;
+
+ return data.x;
+}
+
+static inline uint32_t bswap_32(const uint32_t x)
+ __attribute__ ((warn_unused_result))
+ __attribute__ ((const))
+ __attribute__ ((always_inline));
+
+
+static inline uint32_t bswap_32(const uint32_t x) {
+
+ uint8_t tmp;
+ union { uint32_t x; uint8_t b[4]; } data;
+
+ data.x = x;
+ tmp = data.b[0];
+ data.b[0] = data.b[3];
+ data.b[3] = tmp;
+ tmp = data.b[1];
+ data.b[1] = data.b[2];
+ data.b[2] = tmp;
+
+ return data.x;
+}
+
+#define be16_to_cpu(x) bswap_16(x)
+#define be32_to_cpu(x) bswap_32(x)
+
+#define cpu_to_be16(x) bswap_16(x)
+#define cpu_to_be32(x) bswap_32(x)
+
+
+
void scsi_command(msd_cbw_t *msd_cbw_data);
#endif /* __SCSI_H__ */
\ No newline at end of file