| /* |
| * Secure RPMB Driver for Exynos scsi rpmb |
| * |
| * Copyright (C) 2016 Samsung Electronics Co., Ltd. |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License as published by |
| * the Free Software Foundation; either version 2 of the License, or |
| * (at your option) any later version. |
| */ |
| |
| #include <linux/cdev.h> |
| #include <linux/interrupt.h> |
| #include <linux/module.h> |
| #include <linux/of_irq.h> |
| #include <linux/platform_device.h> |
| #include <linux/slab.h> |
| #include <linux/suspend.h> |
| #include <linux/smc.h> |
| |
| #include <scsi/scsi_cmnd.h> |
| #include <scsi/scsi_ioctl.h> |
| |
| #include "scsi_srpmb.h" |
| |
| #define SRPMB_DEVICE_PROPNAME "samsung,ufs-srpmb" |
| |
| struct platform_device *sr_pdev; |
| |
| #if defined(DEBUG_SRPMB) |
| static void dump_packet(u8 *data, int len) |
| { |
| u8 s[17]; |
| int i, j; |
| |
| s[16] = '\0'; |
| for (i = 0; i < len; i += 16) { |
| printk("%06x :", i); |
| for (j = 0; j < 16; j++) { |
| printk(" %02x", data[i+j]); |
| s[j] = (data[i+j] < ' ' ? '.' : (data[i+j] > '}' ? '.' : data[i+j])); |
| } |
| printk(" |%s|\n", s); |
| } |
| printk("\n"); |
| } |
| #endif |
| |
| static void swap_packet(u8 *p, u8 *d) |
| { |
| int i; |
| for (i = 0; i < RPMB_PACKET_SIZE; i++) |
| d[i] = p[RPMB_PACKET_SIZE - 1 - i]; |
| } |
| |
| static void update_rpmb_status_flag(struct rpmb_irq_ctx *ctx, |
| Rpmb_Req *req, int status) |
| { |
| unsigned long flags; |
| |
| spin_lock_irqsave(&ctx->lock, flags); |
| req->status_flag = status; |
| spin_unlock_irqrestore(&ctx->lock, flags); |
| } |
| |
| static void srpmb_worker(struct work_struct *data) |
| { |
| int ret; |
| struct rpmb_packet packet; |
| struct rpmb_irq_ctx *rpmb_ctx; |
| struct scsi_device *sdp; |
| Rpmb_Req *req; |
| |
| if (!data) { |
| dev_err(&sr_pdev->dev, "rpmb work_struct data invalid\n"); |
| return ; |
| } |
| rpmb_ctx = container_of(data, struct rpmb_irq_ctx, work); |
| if (!rpmb_ctx->dev) { |
| dev_err(&sr_pdev->dev, "rpmb_ctx->dev invalid\n"); |
| return ; |
| } |
| sdp = to_scsi_device(rpmb_ctx->dev); |
| |
| if (!rpmb_ctx->vir_addr) { |
| dev_err(&sr_pdev->dev, "rpmb_ctx->vir_addr invalid\n"); |
| return ; |
| } |
| req = (Rpmb_Req *)rpmb_ctx->vir_addr; |
| |
| __pm_stay_awake(&rpmb_ctx->wakesrc); |
| |
| dev_info(&sr_pdev->dev, "start rpmb workqueue with command(%d)\n", req->type); |
| |
| switch (req->type) { |
| case GET_WRITE_COUNTER: |
| if (req->data_len != RPMB_PACKET_SIZE) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| WRITE_COUTNER_DATA_LEN_ERROR); |
| dev_err(&sr_pdev->dev, "data len is invalid\n"); |
| break; |
| } |
| |
| req->cmd = SCSI_IOCTL_SECURITY_PROTOCOL_OUT; |
| req->outlen = RPMB_PACKET_SIZE; |
| |
| ret = srpmb_scsi_ioctl(sdp, req); |
| if (ret < 0) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| WRITE_COUTNER_SECURITY_OUT_ERROR); |
| dev_err(&sr_pdev->dev, "ioctl read_counter error: %x\n", ret); |
| break; |
| } |
| |
| memset(req->rpmb_data, 0x0, req->data_len); |
| req->cmd = SCSI_IOCTL_SECURITY_PROTOCOL_IN; |
| req->inlen = req->data_len; |
| |
| ret = srpmb_scsi_ioctl(sdp, req); |
| if (ret < 0) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| WRITE_COUTNER_SECURITY_IN_ERROR); |
| dev_err(&sr_pdev->dev, "ioctl error : %x\n", ret); |
| break; |
| } |
| if (req->rpmb_data[RPMB_RESULT] || req->rpmb_data[RPMB_RESULT+1]) { |
| dev_info(&sr_pdev->dev, "GET_WRITE_COUNTER: REQ/RES = %02x%02x, RESULT = %02x%02x\n", |
| req->rpmb_data[RPMB_REQRES], req->rpmb_data[RPMB_REQRES+1], |
| req->rpmb_data[RPMB_RESULT], req->rpmb_data[RPMB_RESULT+1]); |
| } |
| |
| update_rpmb_status_flag(rpmb_ctx, req, RPMB_PASSED); |
| |
| break; |
| case WRITE_DATA: |
| if (req->data_len < RPMB_PACKET_SIZE || |
| req->data_len > RPMB_PACKET_SIZE * 64) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| WRITE_DATA_LEN_ERROR); |
| dev_err(&sr_pdev->dev, "data len is invalid\n"); |
| break; |
| } |
| |
| req->cmd = SCSI_IOCTL_SECURITY_PROTOCOL_OUT; |
| req->outlen = req->data_len; |
| |
| ret = srpmb_scsi_ioctl(sdp, req); |
| if (ret < 0) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| WRITE_DATA_SECURITY_OUT_ERROR); |
| dev_err(&sr_pdev->dev, "ioctl write data error: %x\n", ret); |
| break; |
| } |
| |
| memset(req->rpmb_data, 0x0, req->data_len); |
| memset(&packet, 0x0, RPMB_PACKET_SIZE); |
| packet.request = RESULT_READ_REQ; |
| swap_packet((uint8_t *)&packet, req->rpmb_data); |
| req->cmd = SCSI_IOCTL_SECURITY_PROTOCOL_OUT; |
| req->outlen = RPMB_PACKET_SIZE; |
| |
| ret = srpmb_scsi_ioctl(sdp, req); |
| if (ret < 0) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| WRITE_DATA_RESULT_SECURITY_OUT_ERROR); |
| dev_err(&sr_pdev->dev, |
| "ioctl write_data result error: %x\n", ret); |
| break; |
| } |
| |
| memset(req->rpmb_data, 0x0, req->data_len); |
| req->cmd = SCSI_IOCTL_SECURITY_PROTOCOL_IN; |
| req->inlen = RPMB_PACKET_SIZE; |
| |
| ret = srpmb_scsi_ioctl(sdp, req); |
| if (ret < 0) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| WRITE_DATA_SECURITY_IN_ERROR); |
| dev_err(&sr_pdev->dev, |
| "ioctl write_data result error: %x\n", ret); |
| break; |
| } |
| if (req->rpmb_data[RPMB_RESULT] || req->rpmb_data[RPMB_RESULT+1]) { |
| dev_info(&sr_pdev->dev, "WRITE_DATA: REQ/RES = %02x%02x, RESULT = %02x%02x\n", |
| req->rpmb_data[RPMB_REQRES], req->rpmb_data[RPMB_REQRES+1], |
| req->rpmb_data[RPMB_RESULT], req->rpmb_data[RPMB_RESULT+1]); |
| } |
| |
| update_rpmb_status_flag(rpmb_ctx, req, RPMB_PASSED); |
| |
| break; |
| case READ_DATA: |
| if (req->data_len < RPMB_PACKET_SIZE || |
| req->data_len > RPMB_PACKET_SIZE * 64) { |
| update_rpmb_status_flag(rpmb_ctx, req, READ_LEN_ERROR); |
| dev_err(&sr_pdev->dev, "data len is invalid\n"); |
| break; |
| } |
| |
| req->cmd = SCSI_IOCTL_SECURITY_PROTOCOL_OUT; |
| req->outlen = RPMB_PACKET_SIZE; |
| |
| ret = srpmb_scsi_ioctl(sdp, req); |
| if (ret < 0) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| READ_DATA_SECURITY_OUT_ERROR); |
| dev_err(&sr_pdev->dev, "ioctl read data error: %x\n", ret); |
| break; |
| } |
| |
| memset(req->rpmb_data, 0x0, req->data_len); |
| req->cmd = SCSI_IOCTL_SECURITY_PROTOCOL_IN; |
| req->inlen = req->data_len; |
| |
| ret = srpmb_scsi_ioctl(sdp, req); |
| if (ret < 0) { |
| update_rpmb_status_flag(rpmb_ctx, req, |
| READ_DATA_SECURITY_IN_ERROR); |
| dev_err(&sr_pdev->dev, |
| "ioctl result read data error : %x\n", ret); |
| break; |
| } |
| if (req->rpmb_data[RPMB_RESULT] || req->rpmb_data[RPMB_RESULT+1]) { |
| dev_info(&sr_pdev->dev, "READ_DATA: REQ/RES = %02x%02x, RESULT = %02x%02x\n", |
| req->rpmb_data[RPMB_REQRES], req->rpmb_data[RPMB_REQRES+1], |
| req->rpmb_data[RPMB_RESULT], req->rpmb_data[RPMB_RESULT+1]); |
| } |
| |
| update_rpmb_status_flag(rpmb_ctx, req, RPMB_PASSED); |
| |
| break; |
| default: |
| dev_err(&sr_pdev->dev, "invalid requset type : %x\n", req->type); |
| } |
| |
| __pm_relax(&rpmb_ctx->wakesrc); |
| dev_info(&sr_pdev->dev, "finish rpmb workqueue with command(%d)\n", req->type); |
| } |
| |
| static int srpmb_suspend_notifier(struct notifier_block *nb, unsigned long event, |
| void *dummy) |
| { |
| struct rpmb_irq_ctx *rpmb_ctx; |
| struct device *dev; |
| Rpmb_Req *req; |
| |
| if (!nb) { |
| dev_err(&sr_pdev->dev, "noti_blk work_struct data invalid\n"); |
| return -1; |
| } |
| rpmb_ctx = container_of(nb, struct rpmb_irq_ctx, pm_notifier); |
| dev = rpmb_ctx->dev; |
| req = (Rpmb_Req *)rpmb_ctx->vir_addr; |
| if (!req) { |
| dev_err(dev, "Invalid wsm address for rpmb\n"); |
| return -EINVAL; |
| } |
| |
| switch (event) { |
| case PM_HIBERNATION_PREPARE: |
| case PM_SUSPEND_PREPARE: |
| case PM_RESTORE_PREPARE: |
| flush_workqueue(rpmb_ctx->srpmb_queue); |
| update_rpmb_status_flag(rpmb_ctx, req, RPMB_FAIL_SUSPEND_STATUS); |
| break; |
| case PM_POST_SUSPEND: |
| case PM_POST_HIBERNATION: |
| case PM_POST_RESTORE: |
| update_rpmb_status_flag(rpmb_ctx, req, 0); |
| break; |
| default: |
| break; |
| } |
| |
| return 0; |
| } |
| |
| static irqreturn_t rpmb_irq_handler(int intr, void *arg) |
| { |
| struct rpmb_irq_ctx *rpmb_ctx = (struct rpmb_irq_ctx *)arg; |
| struct device *dev; |
| Rpmb_Req *req; |
| |
| dev = rpmb_ctx->dev; |
| req = (Rpmb_Req *)rpmb_ctx->vir_addr; |
| if (!req) { |
| dev_err(dev, "Invalid wsm address for rpmb\n"); |
| return IRQ_HANDLED; |
| } |
| |
| update_rpmb_status_flag(rpmb_ctx, req, RPMB_IN_PROGRESS); |
| queue_work(rpmb_ctx->srpmb_queue, &rpmb_ctx->work); |
| |
| return IRQ_HANDLED; |
| } |
| |
| int init_wsm(struct device *dev) |
| { |
| int ret; |
| struct rpmb_irq_ctx *rpmb_ctx; |
| struct irq_data *rpmb_irqd = NULL; |
| irq_hw_number_t hwirq = 0; |
| |
| rpmb_ctx = kzalloc(sizeof(struct rpmb_irq_ctx), GFP_KERNEL); |
| if (!rpmb_ctx) { |
| dev_err(&sr_pdev->dev, "kzalloc failed\n"); |
| goto out_srpmb_ctx_alloc_fail; |
| } |
| |
| /* buffer init */ |
| rpmb_ctx->vir_addr = dma_alloc_coherent(&sr_pdev->dev, |
| sizeof(Rpmb_Req) + RPMB_BUF_MAX_SIZE, |
| &rpmb_ctx->phy_addr, GFP_KERNEL); |
| |
| if (rpmb_ctx->vir_addr && rpmb_ctx->phy_addr) { |
| dev_info(dev, "srpmb dma addr: virt_pK(%pK), phy(%llx)\n", |
| rpmb_ctx->vir_addr, (uint64_t)rpmb_ctx->phy_addr); |
| |
| rpmb_ctx->irq = irq_of_parse_and_map(sr_pdev->dev.of_node, 0); |
| if (rpmb_ctx->irq <= 0) { |
| dev_err(&sr_pdev->dev, "No IRQ number, aborting\n"); |
| goto out_srpmb_init_fail; |
| } |
| |
| /* Get irq_data for secure log */ |
| rpmb_irqd = irq_get_irq_data(rpmb_ctx->irq); |
| if (!rpmb_irqd) { |
| dev_err(&sr_pdev->dev, "Fail to get irq_data\n"); |
| goto out_srpmb_init_fail; |
| } |
| |
| /* Get hardware interrupt number */ |
| hwirq = irqd_to_hwirq(rpmb_irqd); |
| dev_dbg(&sr_pdev->dev, "hwirq for srpmb (%ld)\n", hwirq); |
| |
| rpmb_ctx->dev = dev; |
| rpmb_ctx->srpmb_queue = alloc_workqueue("srpmb_wq", |
| WQ_MEM_RECLAIM | WQ_UNBOUND | WQ_HIGHPRI, 1); |
| if (!rpmb_ctx->srpmb_queue) { |
| dev_err(&sr_pdev->dev, |
| "Fail to alloc workqueue for ufs sprmb\n"); |
| goto out_srpmb_init_fail; |
| } |
| |
| ret = request_irq(rpmb_ctx->irq, rpmb_irq_handler, |
| IRQF_TRIGGER_RISING, sr_pdev->name, rpmb_ctx); |
| if (ret) { |
| dev_err(&sr_pdev->dev, "request irq failed: %x\n", ret); |
| goto out_srpmb_init_fail; |
| } |
| |
| rpmb_ctx->pm_notifier.notifier_call = srpmb_suspend_notifier; |
| ret = register_pm_notifier(&rpmb_ctx->pm_notifier); |
| if (ret) { |
| dev_err(&sr_pdev->dev, "Failed to setup pm notifier\n"); |
| goto out_srpmb_free_irq_req; |
| } |
| |
| ret = exynos_smc(SMC_SRPMB_WSM, rpmb_ctx->phy_addr, hwirq, 0); |
| if (ret) { |
| dev_err(&sr_pdev->dev, "wsm smc init failed: %x\n", ret); |
| goto out_srpmb_unregister_pm; |
| } |
| |
| wakeup_source_init(&rpmb_ctx->wakesrc, "srpmb"); |
| spin_lock_init(&rpmb_ctx->lock); |
| INIT_WORK(&rpmb_ctx->work, srpmb_worker); |
| |
| } else { |
| dev_err(&sr_pdev->dev, "wsm dma alloc failed\n"); |
| goto out_srpmb_dma_alloc_fail; |
| } |
| |
| return 0; |
| |
| out_srpmb_unregister_pm: |
| unregister_pm_notifier(&rpmb_ctx->pm_notifier); |
| out_srpmb_free_irq_req: |
| free_irq(rpmb_ctx->irq, rpmb_ctx); |
| out_srpmb_init_fail: |
| if (rpmb_ctx->srpmb_queue) |
| destroy_workqueue(rpmb_ctx->srpmb_queue); |
| |
| dma_free_coherent(&sr_pdev->dev, RPMB_BUF_MAX_SIZE, |
| rpmb_ctx->vir_addr, rpmb_ctx->phy_addr); |
| |
| out_srpmb_dma_alloc_fail: |
| kfree(rpmb_ctx); |
| |
| out_srpmb_ctx_alloc_fail: |
| return -ENOMEM; |
| } |
| |
| static int srpmb_probe(struct platform_device *pdev) |
| { |
| sr_pdev = pdev; |
| |
| return 0; |
| } |
| |
| static const struct of_device_id of_match_table[] = { |
| { .compatible = SRPMB_DEVICE_PROPNAME }, |
| { } |
| }; |
| |
| static struct platform_driver srpmb_plat_driver = { |
| .probe = srpmb_probe, |
| .driver = { |
| .name = "exynos-ufs-srpmb", |
| .owner = THIS_MODULE, |
| .of_match_table = of_match_table, |
| } |
| }; |
| |
| static int __init srpmb_init(void) |
| { |
| return platform_driver_register(&srpmb_plat_driver); |
| } |
| |
| static void __exit srpmb_exit(void) |
| { |
| platform_driver_unregister(&srpmb_plat_driver); |
| } |
| |
| subsys_initcall(srpmb_init); |
| module_exit(srpmb_exit); |
| |
| MODULE_AUTHOR("Yongtaek Kwon <ycool.kwon@samsung.com>"); |
| MODULE_LICENSE("GPL"); |
| MODULE_DESCRIPTION("UFS SRPMB driver"); |