Merge changes I928ddebc,Iade322a2 am: 4fd0ff9756 (2f8528e0) · Commits · e / os / android_system_core

trusty/storage/proxy/rpmb.c

+120 −13

Original line number	Original line	Diff line number	Diff line
	@@ -16,7 +16,10 @@

	#include <errno.h>		#include <errno.h>
	#include <fcntl.h>		#include <fcntl.h>
			#include <scsi/scsi.h>
			#include <scsi/scsi_proto.h>
	#include <scsi/sg.h>		#include <scsi/sg.h>
			#include <stdbool.h>
	#include <stdint.h>		#include <stdint.h>
	#include <stdio.h>		#include <stdio.h>
	#include <stdlib.h>		#include <stdlib.h>
	@@ -104,22 +107,18 @@ static enum dev_type dev_type = UNKNOWN_RPMB;

	static const char* UFS_WAKE_LOCK_NAME = "ufs_seq_wakelock";		static const char* UFS_WAKE_LOCK_NAME = "ufs_seq_wakelock";

	#ifdef RPMB_DEBUG		static void print_buf(FILE* handle, const char* prefix, const uint8_t* buf, size_t size) {

	static void print_buf(const char* prefix, const uint8_t* buf, size_t size) {
	size_t i;		size_t i;

	printf("%s @%p [%zu]", prefix, buf, size);		fprintf(handle, "%s @%p [%zu]", prefix, buf, size);
	for (i = 0; i < size; i++) {		for (i = 0; i < size; i++) {
	if (i && i % 32 == 0) printf("\n%*s", (int)strlen(prefix), "");		if (i && i % 32 == 0) fprintf(handle, "\n%*s", (int)strlen(prefix), "");
	printf(" %02x", buf[i]);		fprintf(handle, " %02x", buf[i]);
	}		}
	printf("\n");		fprintf(handle, "\n");
	fflush(stdout);		fflush(handle);
	}		}

	#endif

	static void set_sg_io_hdr(sg_io_hdr_t* io_hdrp, int dxfer_direction, unsigned char cmd_len,		static void set_sg_io_hdr(sg_io_hdr_t* io_hdrp, int dxfer_direction, unsigned char cmd_len,
	unsigned char mx_sb_len, unsigned int dxfer_len, void* dxferp,		unsigned char mx_sb_len, unsigned int dxfer_len, void* dxferp,
	unsigned char* cmdp, void* sbp) {		unsigned char* cmdp, void* sbp) {
	@@ -135,6 +134,111 @@ static void set_sg_io_hdr(sg_io_hdr_t* io_hdrp, int dxfer_direction, unsigned ch
	io_hdrp->timeout = TIMEOUT;		io_hdrp->timeout = TIMEOUT;
	}		}

			/* Returns false if the sense data was valid and no errors were present */
			static bool check_scsi_sense(const uint8_t* sense_buf, size_t len) {
			uint8_t response_code = 0;
			uint8_t sense_key = 0;
			uint8_t additional_sense_code = 0;
			uint8_t additional_sense_code_qualifier = 0;
			uint8_t additional_length = 0;

			if (!sense_buf \|\| len == 0) {
			ALOGE("Invalid SCSI sense buffer, length: %zu\n", len);
			return false;
			}

			response_code = 0x7f & sense_buf[0];

			if (response_code < 0x70 \|\| response_code > 0x73) {
			ALOGE("Invalid SCSI sense response code: %hhu\n", response_code);
			return false;
			}

			if (response_code >= 0x72) {
			/* descriptor format, SPC-6 4.4.2 */
			if (len > 1) {
			sense_key = 0xf & sense_buf[1];
			}
			if (len > 2) {
			additional_sense_code = sense_buf[2];
			}
			if (len > 3) {
			additional_sense_code_qualifier = sense_buf[3];
			}
			if (len > 7) {
			additional_length = sense_buf[7];
			}
			} else {
			/* fixed format, SPC-6 4.4.3 */
			if (len > 2) {
			sense_key = 0xf & sense_buf[2];
			}
			if (len > 7) {
			additional_length = sense_buf[7];
			}
			if (len > 12) {
			additional_sense_code = sense_buf[12];
			}
			if (len > 13) {
			additional_sense_code_qualifier = sense_buf[13];
			}
			}

			switch (sense_key) {
			case NO_SENSE:
			case 0x0f: /* COMPLETED, not present in kernel headers */
			ALOGD("SCSI success with sense data: key=%hhu, asc=%hhu, ascq=%hhu\n", sense_key,
			additional_sense_code, additional_sense_code_qualifier);
			return true;
			}

			ALOGE("Unexpected SCSI sense data: key=%hhu, asc=%hhu, ascq=%hhu\n", sense_key,
			additional_sense_code, additional_sense_code_qualifier);
			print_buf(stderr, "sense buffer: ", sense_buf, len);
			return false;
			}

			static void check_sg_io_hdr(const sg_io_hdr_t* io_hdrp) {
			if (io_hdrp->status == 0 && io_hdrp->host_status == 0 && io_hdrp->driver_status == 0) {
			return;
			}

			if (io_hdrp->status & 0x01) {
			ALOGE("SG_IO received unknown status, LSB is set: %hhu", io_hdrp->status);
			}

			if (io_hdrp->masked_status != GOOD && io_hdrp->sb_len_wr > 0) {
			bool sense_error = check_scsi_sense(io_hdrp->sbp, io_hdrp->sb_len_wr);
			if (sense_error) {
			ALOGE("Unexpected SCSI sense. masked_status: %hhu, host_status: %hu, driver_status: "
			"%hu\n",
			io_hdrp->masked_status, io_hdrp->host_status, io_hdrp->driver_status);
			return;
			}
			}

			switch (io_hdrp->masked_status) {
			case GOOD:
			break;
			case CHECK_CONDITION:
			/* handled by check_sg_sense above */
			break;
			default:
			ALOGE("SG_IO failed with masked_status: %hhu, host_status: %hu, driver_status: %hu\n",
			io_hdrp->masked_status, io_hdrp->host_status, io_hdrp->driver_status);
			return;
			}

			if (io_hdrp->host_status != 0) {
			ALOGE("SG_IO failed with host_status: %hu, driver_status: %hu\n", io_hdrp->host_status,
			io_hdrp->driver_status);
			}

			if (io_hdrp->resid != 0) {
			ALOGE("SG_IO resid was non-zero: %d\n", io_hdrp->resid);
			}
			}

	static int send_mmc_rpmb_req(int mmc_fd, const struct storage_rpmb_send_req* req) {		static int send_mmc_rpmb_req(int mmc_fd, const struct storage_rpmb_send_req* req) {
	struct {		struct {
	struct mmc_ioc_multi_cmd multi;		struct mmc_ioc_multi_cmd multi;
	@@ -153,7 +257,7 @@ static int send_mmc_rpmb_req(int mmc_fd, const struct storage_rpmb_send_req* req
	mmc_ioc_cmd_set_data((*cmd), write_buf);		mmc_ioc_cmd_set_data((*cmd), write_buf);
	#ifdef RPMB_DEBUG		#ifdef RPMB_DEBUG
	ALOGI("opcode: 0x%x, write_flag: 0x%x\n", cmd->opcode, cmd->write_flag);		ALOGI("opcode: 0x%x, write_flag: 0x%x\n", cmd->opcode, cmd->write_flag);
	print_buf("request: ", write_buf, req->reliable_write_size);		print_buf(stdout, "request: ", write_buf, req->reliable_write_size);
	#endif		#endif
	write_buf += req->reliable_write_size;		write_buf += req->reliable_write_size;
	mmc.multi.num_of_cmds++;		mmc.multi.num_of_cmds++;
	@@ -169,7 +273,7 @@ static int send_mmc_rpmb_req(int mmc_fd, const struct storage_rpmb_send_req* req
	mmc_ioc_cmd_set_data((*cmd), write_buf);		mmc_ioc_cmd_set_data((*cmd), write_buf);
	#ifdef RPMB_DEBUG		#ifdef RPMB_DEBUG
	ALOGI("opcode: 0x%x, write_flag: 0x%x\n", cmd->opcode, cmd->write_flag);		ALOGI("opcode: 0x%x, write_flag: 0x%x\n", cmd->opcode, cmd->write_flag);
	print_buf("request: ", write_buf, req->write_size);		print_buf(stdout, "request: ", write_buf, req->write_size);
	#endif		#endif
	write_buf += req->write_size;		write_buf += req->write_size;
	mmc.multi.num_of_cmds++;		mmc.multi.num_of_cmds++;
	@@ -225,6 +329,7 @@ static int send_ufs_rpmb_req(int sg_fd, const struct storage_rpmb_send_req* req)
	ALOGE("%s: ufs ioctl failed: %d, %s\n", __func__, rc, strerror(errno));		ALOGE("%s: ufs ioctl failed: %d, %s\n", __func__, rc, strerror(errno));
	goto err_op;		goto err_op;
	}		}
			check_sg_io_hdr(&io_hdr);
	write_buf += req->reliable_write_size;		write_buf += req->reliable_write_size;
	}		}

	@@ -239,6 +344,7 @@ static int send_ufs_rpmb_req(int sg_fd, const struct storage_rpmb_send_req* req)
	ALOGE("%s: ufs ioctl failed: %d, %s\n", __func__, rc, strerror(errno));		ALOGE("%s: ufs ioctl failed: %d, %s\n", __func__, rc, strerror(errno));
	goto err_op;		goto err_op;
	}		}
			check_sg_io_hdr(&io_hdr);
	write_buf += req->write_size;		write_buf += req->write_size;
	}		}

	@@ -252,6 +358,7 @@ static int send_ufs_rpmb_req(int sg_fd, const struct storage_rpmb_send_req* req)
	if (rc < 0) {		if (rc < 0) {
	ALOGE("%s: ufs ioctl failed: %d, %s\n", __func__, rc, strerror(errno));		ALOGE("%s: ufs ioctl failed: %d, %s\n", __func__, rc, strerror(errno));
	}		}
			check_sg_io_hdr(&io_hdr);
	}		}

	err_op:		err_op:
	@@ -353,7 +460,7 @@ int rpmb_send(struct storage_msg* msg, const void* r, size_t req_len) {
	goto err_response;		goto err_response;
	}		}
	#ifdef RPMB_DEBUG		#ifdef RPMB_DEBUG
	if (req->read_size) print_buf("response: ", read_buf, req->read_size);		if (req->read_size) print_buf(stdout, "response: ", read_buf, req->read_size);
	#endif		#endif

	if (msg->flags & STORAGE_MSG_FLAG_POST_COMMIT) {		if (msg->flags & STORAGE_MSG_FLAG_POST_COMMIT) {