From 92c371470649d58e1e76e6c255fe3c010fc8ef54 Mon Sep 17 00:00:00 2001 From: barthess Date: Fri, 28 Oct 2016 14:25:08 +0300 Subject: USB_MSD. Added READ_FORMAT_CAPACITIES handler --- os/various/lib_scsi.c | 107 ++++++++++++++++++++++++++++++++++---------------- os/various/lib_scsi.h | 11 ++++++ os/various/ramdisk.c | 10 ++--- 3 files changed, 90 insertions(+), 38 deletions(-) (limited to 'os/various') diff --git a/os/various/lib_scsi.c b/os/various/lib_scsi.c index 11842ef..55aeb7e 100644 --- a/os/various/lib_scsi.c +++ b/os/various/lib_scsi.c @@ -25,10 +25,17 @@ #include #include "hal.h" -//#include "chprintf.h" #include "lib_scsi.h" +#define DEBUG_TRACE_PRINT FALSE +#define DEBUG_TRACE_WARNING FALSE +#define DEBUG_TRACE_ERROR FALSE +#include "dbgtrace.h" + +#define ARCH_LITTLE_ENDIAN +#include "bswap.h" + /*===========================================================================*/ /* Driver local definitions. */ /*===========================================================================*/ @@ -50,25 +57,6 @@ typedef struct { /* Driver local functions. */ /*===========================================================================*/ -/** - * @brief Byte swapping function. - * - * @notapi - */ -static uint32_t swap_uint32(uint32_t val) { - val = ((val << 8) & 0xFF00FF00 ) | ((val >> 8) & 0x00FF00FF); - return ((val << 16) & 0xFFFF0000) | ((val >> 16) & 0x0000FFFF); -} - -/** - * @brief Byte swapping function. - * - * @notapi - */ -static uint16_t swap_uint16(uint16_t val) { - return ((val >> 8) & 0xff) | ((val & 0xff) << 8); -} - /** * @brief Combines data request from byte array. * @@ -83,8 +71,8 @@ static data_request_t decode_data_request(const uint8_t *cmd) { memcpy(&lba, &cmd[2], sizeof(lba)); memcpy(&blk, &cmd[7], sizeof(blk)); - req.first_lba = swap_uint32(lba); - req.blk_cnt = swap_uint16(blk); + req.first_lba = be32_to_cpu(lba); + req.blk_cnt = be16_to_cpu(blk); return req; } @@ -253,6 +241,51 @@ static bool mode_sense6(SCSITarget *scsip, const uint8_t *cmd) { sizeof(scsi_mode_sense6_response_t)); } +/** + * @brief SCSI read format capacities command handler. + * + * @param[in] scsip pointer to @p SCSITarget structure + * @param[in] cmd pointer to SCSI command data + * + * @return The operation status. + * + * @notapi + */ +static bool read_format_capacities(SCSITarget *scsip, const uint8_t *cmd) { + + /* An Allocation Length of zero indicates that no data shall be transferred. + This condition shall not be considered as an error. The Logical Unit + shall terminate the data transfer when Allocation Length bytes have + been transferred or when all available data have been transferred to + the Initiator, whatever is less. */ + + uint16_t len = cmd[7] << 8 | cmd[8]; + + if (0 == len) { + return SCSI_SUCCESS; + } + else { + scsi_read_format_capacities_response_t ret; + BlockDeviceInfo bdi; + blkGetInfo(scsip->config->blkdev, &bdi); + + uint32_t tmp = cpu_to_be32(bdi.blk_num); + memcpy(ret.blocknum, &tmp, 4); + + uint8_t formatted_media = 0b10; + uint16_t blocklen = bdi.blk_size; + ret.blocklen[0] = formatted_media; + ret.blocklen[1] = 0; + ret.blocklen[2] = blocklen >> 8; + ret.blocklen[3] = blocklen & 0xFF; + + ret.header[3] = 1 * 8; + + return transmit_data(scsip, (uint8_t *)&ret, + sizeof(scsi_read_format_capacities_response_t)); + } +} + /** * @brief SCSI read capacity (10) command handler. * @@ -270,8 +303,8 @@ static bool read_capacity10(SCSITarget *scsip, const uint8_t *cmd) { BlockDeviceInfo bdi; blkGetInfo(scsip->config->blkdev, &bdi); scsi_read_capacity10_response_t ret; - ret.block_size = swap_uint32(bdi.blk_size); - ret.last_block_addr = swap_uint32(bdi.blk_num - 1); + ret.block_size = cpu_to_be32(bdi.blk_size); + ret.last_block_addr = cpu_to_be32(bdi.blk_num - 1); return transmit_data(scsip, (uint8_t *)&ret, sizeof(scsi_read_capacity10_response_t)); @@ -370,39 +403,47 @@ bool scsiExecCmd(SCSITarget *scsip, const uint8_t *cmd) { switch (cmd[0]) { case SCSI_CMD_INQUIRY: - //chprintf(SDDBG, "SCSI_CMD_INQUIRY\r\n"); + dbgprintf("SCSI_CMD_INQUIRY\r\n"); return inquiry(scsip, cmd); case SCSI_CMD_REQUEST_SENSE: - //chprintf(SDDBG, "SCSI_CMD_REQUEST_SENSE\r\n"); + dbgprintf("SCSI_CMD_REQUEST_SENSE\r\n"); return request_sense(scsip, cmd); case SCSI_CMD_READ_CAPACITY_10: - //chprintf(SDDBG, "SCSI_CMD_READ_CAPACITY_10\r\n"); + dbgprintf("SCSI_CMD_READ_CAPACITY_10\r\n"); return read_capacity10(scsip, cmd); case SCSI_CMD_READ_10: - //chprintf(SDDBG, "SCSI_CMD_READ_10\r\n"); + dbgprintf("SCSI_CMD_READ_10\r\n"); return data_read_write10(scsip, cmd); case SCSI_CMD_WRITE_10: - //chprintf(SDDBG, "SCSI_CMD_WRITE_10\r\n"); + dbgprintf("SCSI_CMD_WRITE_10\r\n"); return data_read_write10(scsip, cmd); case SCSI_CMD_TEST_UNIT_READY: - //chprintf(SDDBG, "SCSI_CMD_TEST_UNIT_READY\r\n"); + dbgprintf("SCSI_CMD_TEST_UNIT_READY\r\n"); return cmd_ignored(scsip, cmd); case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL: - //chprintf(SDDBG, "SCSI_CMD_ALLOW_MEDIUM_REMOVAL\r\n"); + dbgprintf("SCSI_CMD_ALLOW_MEDIUM_REMOVAL\r\n"); return cmd_ignored(scsip, cmd); case SCSI_CMD_MODE_SENSE_6: - //chprintf(SDDBG, "SCSI_CMD_MODE_SENSE_6\r\n"); + dbgprintf("SCSI_CMD_MODE_SENSE_6\r\n"); return mode_sense6(scsip, cmd); + case SCSI_CMD_READ_FORMAT_CAPACITIES: + dbgprintf("SCSI_CMD_READ_FORMAT_CAPACITIES\r\n"); + return read_format_capacities(scsip, cmd); + + case SCSI_CMD_VERIFY_10: + dbgprintf("SCSI_CMD_VERIFY_10\r\n"); + return cmd_ignored(scsip, cmd); + default: - //(SDDBG, "SCSI unhandled command: %d\r\n", cmd[0]); + warnprintf("SCSI unhandled command: %X\r\n", cmd[0]); return cmd_unhandled(scsip, cmd); } } diff --git a/os/various/lib_scsi.h b/os/various/lib_scsi.h index 21ab5d8..97badb0 100644 --- a/os/various/lib_scsi.h +++ b/os/various/lib_scsi.h @@ -37,6 +37,7 @@ #define SCSI_CMD_SEND_DIAGNOSTIC 0x1D #define SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL 0x1E #define SCSI_CMD_READ_CAPACITY_10 0x25 +#define SCSI_CMD_READ_FORMAT_CAPACITIES 0x23 #define SCSI_CMD_READ_10 0x28 #define SCSI_CMD_WRITE_10 0x2A #define SCSI_CMD_VERIFY_10 0x2F @@ -156,6 +157,16 @@ typedef struct PACKED_VAR { uint32_t block_size; } scsi_read_capacity10_response_t; +/** + * @brief Represents SCSI read format capacity response structure. + * @details See SCSI specification. + */ +typedef struct PACKED_VAR { + uint8_t header[4]; + uint8_t blocknum[4]; + uint8_t blocklen[4]; +} scsi_read_format_capacities_response_t; + /** * @brief Type of a SCSI transport transmit call. * diff --git a/os/various/ramdisk.c b/os/various/ramdisk.c index b9a40ef..08abdca 100644 --- a/os/various/ramdisk.c +++ b/os/various/ramdisk.c @@ -58,7 +58,7 @@ static bool is_inserted(void *instance) { static bool is_protected(void *instance) { RamDisk *rd = instance; - if (rd->state != BLK_READY) { + if (BLK_READY == rd->state) { return rd->readonly; } else { @@ -68,7 +68,7 @@ static bool is_protected(void *instance) { static bool connect(void *instance) { RamDisk *rd = instance; - if (rd->state == BLK_STOP) { + if (BLK_STOP == rd->state) { rd->state = BLK_READY; } return HAL_SUCCESS; @@ -76,7 +76,7 @@ static bool connect(void *instance) { static bool disconnect(void *instance) { RamDisk *rd = instance; - if (rd->state != BLK_STOP) { + if (BLK_STOP != rd->state) { rd->state = BLK_STOP; } return HAL_SUCCESS; @@ -114,7 +114,7 @@ static bool write(void *instance, uint32_t startblk, static bool sync(void *instance) { RamDisk *rd = instance; - if (rd->state != BLK_READY) { + if (BLK_READY != rd->state) { return HAL_FAILED; } else { @@ -125,7 +125,7 @@ static bool sync(void *instance) { static bool get_info(void *instance, BlockDeviceInfo *bdip) { RamDisk *rd = instance; - if (rd->state != BLK_READY) { + if (BLK_READY != rd->state) { return HAL_FAILED; } else { -- cgit v1.2.3