#define MMC_BLK_WRITE BIT(1)
#define MMC_BLK_DISCARD BIT(2)
#define MMC_BLK_SECDISCARD BIT(3)
+#define MMC_BLK_RPMB BIT(4)
/*
* Only set in main mmc_blk_data associated
int ret = -EINVAL;
if (cmd == MMC_IOC_CMD)
ret = mmc_blk_ioctl_cmd(bdev, (struct mmc_ioc_cmd __user *)arg);
+
return ret;
}
md->reset_done &= ~type;
}
+int mmc_rpmb_req_handle(struct device *emmc, struct mmc_rpmb_req *req)
+{
+ int ret = 0;
+ struct gendisk *disk = NULL;
+ struct mmc_card *card = NULL;
+ struct mmc_blk_data *md = NULL;
+ unsigned int part_curr;
+
+ if (!emmc)
+ return -ENODEV;
+
+ if (!req)
+ return -EINVAL;
+
+ disk = dev_to_disk(emmc);
+ if (!disk) {
+ pr_err("%s: NO eMMC disk found. Try it later\n",
+ __func__);
+ return -ENODEV;
+ }
+
+ md = mmc_blk_get(disk);
+ if (!md) {
+ pr_err("%s: NO eMMC block data. Try it later\n",
+ __func__);
+ return -ENODEV;
+ }
+ if (!(md->flags & MMC_BLK_CMD23)) {
+ pr_err("%s: CMD23 is not supported by host or device\n",
+ mmc_hostname(card->host));
+ ret = -EOPNOTSUPP;
+ goto err;
+ }
+
+ card = mmc_dev_to_card(disk->driverfs_dev);
+ if (IS_ERR(card)) {
+ ret = PTR_ERR(card);
+ pr_err("%s: encounter error when getting eMMC card\n",
+ __func__);
+ ret = -ENODEV;
+ goto err;
+ }
+
+ if (!mmc_card_mmc(card) || !card->ext_csd.rpmb_size) {
+ ret = -ENODEV;
+ goto err;
+ }
+
+ /* check request */
+ ret = mmc_rpmb_pre_frame(req, card);
+ if (ret) {
+ pr_err("%s: prepare frame failed\n", mmc_hostname(card->host));
+ goto err;
+ }
+
+ mmc_claim_host(card->host);
+
+ /* * before start, let's change to RPMB partition first
+ */
+ part_curr = md->part_type;
+ md->part_type = EXT_CSD_PART_CONFIG_RPMB;
+ ret = mmc_blk_part_switch(card, md);
+ if (ret) {
+ pr_err("%s: Invalid RPMB partition switch (%d)!\n",
+ mmc_hostname(card->host), ret);
+ /*
+ * In case partition is not in user data area, make
+ * a force partition switch
+ */
+ goto out;
+ }
+
+ ret = mmc_rpmb_partition_ops(req, card);
+ if (ret)
+ pr_err("%s: failed (%d) to handle RPMB request type (%d)!\n",
+ mmc_hostname(card->host), ret, req->type);
+out:
+ /*
+ * switch back
+ */
+ md->part_type = part_curr;
+ if (mmc_blk_part_switch(card, md)) {
+ int err;
+ /*
+ * we need reset eMMC card at here
+ */
+ err = mmc_blk_reset(md, card->host, MMC_BLK_RPMB);
+ if (!err)
+ mmc_blk_reset_success(md, MMC_BLK_RPMB);
+ else {
+ pr_err("%s: eMMC card reset failed (%d)\n",
+ mmc_hostname(card->host), err);
+ if (!ret)
+ ret = err;
+ }
+ }
+ mmc_release_host(card->host);
+ mmc_rpmb_post_frame(req);
+err:
+ mmc_blk_put(md);
+
+ return ret;
+}
+EXPORT_SYMBOL_GPL(mmc_rpmb_req_handle);
+
static int mmc_blk_issue_discard_rq(struct mmc_queue *mq, struct request *req)
{
struct mmc_blk_data *md = mq->data;
return 0;
}
+
+static int mmc_rpmb_send_command(struct mmc_card *card, u8 *buf, __u16 blks,
+ __u16 type, u8 req_type)
+{
+ struct mmc_request mrq = {NULL};
+ struct mmc_command cmd = {0};
+ struct mmc_command sbc = {0};
+ struct mmc_data data = {0};
+ struct scatterlist sg;
+ u8 *transfer_buf = NULL;
+
+ mrq.sbc = &sbc;
+ mrq.cmd = &cmd;
+ mrq.data = &data;
+ mrq.stop = NULL;
+ transfer_buf = kzalloc(512 * blks, GFP_KERNEL);
+ if (!transfer_buf)
+ return -ENOMEM;
+
+ /*
+ * set CMD23
+ */
+ sbc.opcode = MMC_SET_BLOCK_COUNT;
+ sbc.arg = blks;
+ if ((req_type == RPMB_REQ) && (type == RPMB_WRITE_DATA ||
+ type == RPMB_PROGRAM_KEY))
+ sbc.arg |= 1 << 31;
+ sbc.flags = MMC_RSP_R1 | MMC_CMD_AC;
+
+ /*
+ * set CMD25/18
+ */
+ sg_init_one(&sg, transfer_buf, 512 * blks);
+ if (req_type == RPMB_REQ) {
+ cmd.opcode = MMC_WRITE_MULTIPLE_BLOCK;
+ sg_copy_from_buffer(&sg, 1, buf, 512 * blks);
+ data.flags |= MMC_DATA_WRITE;
+ } else {
+ cmd.opcode = MMC_READ_MULTIPLE_BLOCK;
+ data.flags |= MMC_DATA_READ;
+ }
+ cmd.arg = 0;
+ cmd.flags = MMC_RSP_R1 | MMC_CMD_ADTC;
+ data.blksz = 512;
+ data.blocks = blks;
+ data.sg = &sg;
+ data.sg_len = 1;
+
+ mmc_set_data_timeout(&data, card);
+
+ mmc_wait_for_req(card->host, &mrq);
+
+ if (req_type != RPMB_REQ)
+ sg_copy_to_buffer(&sg, 1, buf, 512 * blks);
+
+ kfree(transfer_buf);
+
+ if (cmd.error)
+ return cmd.error;
+ if (data.error)
+ return data.error;
+ return 0;
+}
+
+void mmc_rpmb_post_frame(struct mmc_rpmb_req *p_req)
+{
+ int i;
+ __u8 *ptr, *buf_frame = p_req->frame;
+
+ if (!p_req->ready || !buf_frame)
+ return;
+ /*
+ * Regarding to the check rules, here is the post
+ * rules
+ * All will return result.
+ * GET_WRITE_COUNTER:
+ * must: write counter, nonce
+ * optional: MAC
+ * WRITE_DATA:
+ * must: MAC, write counter
+ * READ_DATA:
+ * must: nonce, data
+ * optional: MAC
+ * PROGRAM_KEY:
+ * must: Nothing
+ *
+ * Except READ_DATA, all of these operations only need to parse
+ * one frame. READ_DATA needs blks frames to get DATA
+ */
+
+ memcpy(p_req->result, buf_frame + RPMB_RES_BEG, 2);
+ *p_req->result = be16_to_cpup(p_req->result);
+
+ if (p_req->type == RPMB_PROGRAM_KEY)
+ goto out;
+
+ if (p_req->type == RPMB_GET_WRITE_COUNTER ||
+ p_req->type == RPMB_WRITE_DATA) {
+ memcpy(p_req->wc, buf_frame + RPMB_WCOUNTER_BEG, 4);
+ *p_req->wc = be32_to_cpup(p_req->wc);
+ }
+
+ if (p_req->type == RPMB_GET_WRITE_COUNTER ||
+ p_req->type == RPMB_READ_DATA) {
+ /* nonce copy */
+ ptr = buf_frame + RPMB_NONCE_END;
+ for (i = 0; i < 16; i++, ptr--)
+ p_req->nonce[i] = *ptr;
+ }
+ /*
+ * Take MAC within the last package
+ */
+ if (p_req->type == RPMB_READ_DATA) {
+ int j;
+ __u8 *data = p_req->data;
+ for (i = 0; i < p_req->blk_cnt; i++) {
+ ptr = buf_frame + i * 512 + RPMB_DATA_END;
+ for (j = 0; j < 256; j++, ptr--, data++)
+ *data = *ptr;
+ }
+ /*
+ * MAC stored in the last package
+ */
+ if (p_req->mac) {
+ ptr = buf_frame + 512 * i + RPMB_MAC_END;
+ for (i = 0; i < 32; i++, ptr--)
+ p_req->mac[i] = *ptr;
+ }
+ } else if (p_req->mac) {
+ ptr = buf_frame + RPMB_MAC_END;
+ for (i = 0; i < 32; i++, ptr--)
+ p_req->mac[i] = *ptr;
+ }
+out:
+ kfree(buf_frame);
+ p_req->frame = NULL;
+ return;
+}
+EXPORT_SYMBOL_GPL(mmc_rpmb_post_frame);
+
+static int mmc_rpmb_request_check(struct mmc_card *card,
+ struct mmc_rpmb_req *p_req)
+{
+ /*
+ * Some paramter is a must for the operation. Different
+ * operation expect different paramters. Below code is
+ * used for checking this.
+ *
+ * All operations will need result.
+ * GET_WRITE_COUNTER:
+ * must: write counter, nonce
+ * optional: MAC
+ * WRITE_DATA:
+ * must: MAC, data, write counter
+ * READ_DATA:
+ * must: nonce, data
+ * optional: MAC
+ * PROGRAM_KEY:
+ * must: MAC
+ *
+ * So here, we only check the 'must' paramters
+ */
+ if (!p_req->result) {
+ pr_err("%s: Type %d has NULL pointer for result\n",
+ mmc_hostname(card->host), p_req->type);
+ return -EINVAL;
+ }
+
+ if (p_req->type == RPMB_GET_WRITE_COUNTER) {
+ if (!p_req->nonce || !p_req->wc) {
+ pr_err("%s: Type %d has NULL pointer for nonce/wc\n",
+ mmc_hostname(card->host), p_req->type);
+ return -EINVAL;
+ }
+ p_req->blk_cnt = 1;
+ } else if (p_req->type == RPMB_WRITE_DATA ||
+ p_req->type == RPMB_READ_DATA) {
+ if ((__u32)(p_req->addr + p_req->blk_cnt) >
+ card->ext_csd.rpmb_size) {
+ pr_err("%s Type %d: beyond the RPMB partition rang "
+ "addr %d, blk_cnt %d, rpmb_size %d\n",
+ mmc_hostname(card->host),
+ p_req->type,
+ p_req->addr,
+ p_req->blk_cnt,
+ card->ext_csd.rpmb_size);
+ return -EINVAL;
+ }
+ if (!p_req->data) {
+ pr_err("%s: Type %d has NULL pointer for data\n",
+ mmc_hostname(card->host), p_req->type);
+ return -EINVAL;
+ }
+ if (p_req->type == RPMB_WRITE_DATA) {
+ if (!p_req->wc || !p_req->mac) {
+ pr_err("%s: Type %d has NULL pointer for"
+ " write counter/MAC\n",
+ mmc_hostname(card->host),
+ p_req->type);
+ return -EINVAL;
+ }
+ } else {
+ if (!p_req->nonce) {
+ pr_err("%s: Type %d has NULL pointer for"
+ " nonce\n",
+ mmc_hostname(card->host),
+ p_req->type);
+ return -EINVAL;
+ }
+ }
+ } else if (p_req->type == RPMB_PROGRAM_KEY) {
+ if (!p_req->mac) {
+ pr_err("%s: Type %d has NULL pointer for MAC\n",
+ mmc_hostname(card->host), p_req->type);
+ return -EINVAL;
+ }
+ p_req->blk_cnt = 1;
+ } else
+ return -EOPNOTSUPP;
+
+ if (p_req->blk_cnt == 0) {
+ pr_err("%s: Type %d has zero block count\n",
+ mmc_hostname(card->host), p_req->blk_cnt);
+ return -EINVAL;
+ }
+
+ if (p_req->blk_cnt > card->rpmb_max_req) {
+ pr_err("%s: Type %d has invalid block count, "
+ "cannot large than %d\n",
+ mmc_hostname(card->host),
+ p_req->blk_cnt,
+ card->rpmb_max_req);
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+/*
+ * prepare the request of RPMB frame
+ * RPMB frame is MSB first
+ * convert needed bytes
+ * return how many frames will be prepared
+ */
+int mmc_rpmb_pre_frame(struct mmc_rpmb_req *p_req,
+ struct mmc_card *card)
+{
+ int i, j, ret;
+ __u8 *ptr = NULL, *buf_frame;
+ __u8 *data = p_req->data;
+ __u16 blk_cnt, addr, type;
+ __u32 w_counter;
+
+ /*
+ * make sure these two items are clear
+ */
+ p_req->ready = 0;
+ p_req->frame = NULL;
+
+ ret = mmc_rpmb_request_check(card, p_req);
+ if (ret)
+ return ret;
+
+ buf_frame = kzalloc(512 * p_req->blk_cnt, GFP_KERNEL);
+ if (!buf_frame) {
+ pr_err("%s: cannot allocate frame for type %d\n",
+ mmc_hostname(card->host), p_req->type);
+ return -ENOMEM;
+ }
+
+ type = cpu_to_be16p(&p_req->type);
+ if (p_req->type == RPMB_GET_WRITE_COUNTER ||
+ p_req->type == RPMB_READ_DATA) {
+ /*
+ * One package prepared
+ * This request needs Nonce and type
+ * If is data read, then also need addr
+ */
+ memcpy(buf_frame + RPMB_TYPE_BEG, &type, 2);
+ if (p_req->type == RPMB_READ_DATA) {
+ addr = cpu_to_be16p(&p_req->addr);
+ memcpy(buf_frame + RPMB_ADDR_BEG, &addr, 2);
+ }
+ /* convert Nonce code */
+ ptr = buf_frame + RPMB_NONCE_END;
+ for (i = 0; i < 16; i++, ptr--)
+ *ptr = p_req->nonce[i];
+ } else if (p_req->type == RPMB_WRITE_DATA) {
+ /*
+ * multiple package prepared
+ * This request nees blk_cnt, addr, write_counter,
+ * data and mac
+ */
+ blk_cnt = cpu_to_be16p(&p_req->blk_cnt);
+ addr = cpu_to_be16p(&p_req->addr);
+ w_counter = cpu_to_be32p(p_req->wc);
+ for (i = 0; i < p_req->blk_cnt; i++) {
+ memcpy(buf_frame + i * 512 + RPMB_TYPE_BEG,
+ &type, 2);
+ memcpy(buf_frame + i * 512 + RPMB_BLKS_BEG,
+ &blk_cnt, 2);
+ memcpy(buf_frame + i * 512 + RPMB_ADDR_BEG,
+ &addr, 2);
+ memcpy(buf_frame + i * 512 + RPMB_WCOUNTER_BEG,
+ &w_counter, 4);
+ ptr = buf_frame + i * 512 + RPMB_DATA_END;
+ for (j = 0; j < 256; j++, ptr--, data++)
+ *ptr = *data;
+ }
+ ptr = buf_frame + 512 * (i - 1) + RPMB_MAC_END;
+ /* convert MAC code */
+ for (i = 0; i < 32; i++, ptr--)
+ *ptr = p_req->mac[i];
+ } else if (p_req->type == RPMB_PROGRAM_KEY) {
+ /*
+ * One package prepared
+ * This request only need mac
+ */
+ memcpy(buf_frame + RPMB_TYPE_BEG, &type, 2);
+ ptr = buf_frame + RPMB_MAC_END;
+ /* convert MAC code */
+ for (i = 0; i < 32; i++, ptr--)
+ *ptr = p_req->mac[i];
+ } else {
+ pr_err("%s: We shouldn't be here\n", mmc_hostname(card->host));
+ kfree(buf_frame);
+ return -EINVAL;
+ }
+ p_req->ready = 1;
+ p_req->frame = buf_frame;
+ return 0;
+}
+EXPORT_SYMBOL_GPL(mmc_rpmb_pre_frame);
+
+int mmc_rpmb_partition_ops(struct mmc_rpmb_req *p_req,
+ struct mmc_card *card)
+{
+ int err = 0;
+ __u16 type, blks;
+ __u8 *buf_frame = p_req->frame;
+
+ if (!p_req->ready || !buf_frame) {
+ pr_err("%s: mmc_rpmb_req is not prepared\n",
+ mmc_hostname(card->host));
+ return -EINVAL;
+ }
+
+ type = p_req->type;
+ blks = p_req->blk_cnt;
+
+ /*
+ * STEP 1: send request to RPMB partition
+ */
+ if (type == RPMB_READ_DATA)
+ err = mmc_rpmb_send_command(card, buf_frame, 1, type, RPMB_REQ);
+ else
+ err = mmc_rpmb_send_command(card, buf_frame,
+ blks, type, RPMB_REQ);
+ if (err) {
+ pr_err("%s: request write counter failed (%d)\n",
+ mmc_hostname(card->host), err);
+ goto out;
+ }
+
+ memset(buf_frame, 0, 512 * blks);
+ /*
+ * STEP 2: check write result
+ * Only for WRITE_DATA or Program key
+ */
+ if (type == RPMB_WRITE_DATA ||
+ type == RPMB_PROGRAM_KEY) {
+ buf_frame[RPMB_TYPE_BEG + 1] = RPMB_RESULT_READ;
+ err = mmc_rpmb_send_command(card, buf_frame, 1,
+ RPMB_RESULT_READ, RPMB_REQ);
+ if (err) {
+ pr_err("%s: request write counter failed (%d)\n",
+ mmc_hostname(card->host), err);
+ goto out;
+ }
+ }
+
+ /*
+ * STEP 3: get response from RPMB partition
+ */
+
+ if (type == RPMB_READ_DATA)
+ err = mmc_rpmb_send_command(card, buf_frame,
+ blks, type, RPMB_RESP);
+ else
+ err = mmc_rpmb_send_command(card, buf_frame,
+ 1, type, RPMB_RESP);
+ if (err) {
+ pr_err("%s: response write counter failed (%d)\n",
+ mmc_hostname(card->host), err);
+ }
+out:
+ return err;
+}
+EXPORT_SYMBOL_GPL(mmc_rpmb_partition_ops);
projects / kernel / kernel-mfld-blackbay.git / commitdiff