1 /* 2 * Copyright (c) 2013, The Linux Foundation. All rights reserved. 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions are 6 * met: 7 * * Redistributions of source code must retain the above copyright 8 * notice, this list of conditions and the following disclaimer. 9 * * Redistributions in binary form must reproduce the above 10 * copyright notice, this list of conditions and the following 11 * disclaimer in the documentation and/or other materials provided 12 * with the distribution. 13 * * Neither the name of The Linux Foundation nor the names of its 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 17 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 18 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 19 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 20 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 21 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 24 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 25 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 26 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 27 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 */ 29 30 #include <stdio.h> 31 #include <fcntl.h> 32 #include <errno.h> 33 #include <string.h> 34 #include <stdlib.h> 35 #include <unistd.h> 36 #include <sys/mman.h> 37 #include <linux/qseecom.h> 38 #include <linux/msm_ion.h> 39 40 /* Service IDs */ 41 #define SCM_SVC_SSD 0x07 42 43 /* Service specific command IDs */ 44 #define SSD_PARSE_MD_ID 0x06 45 #define SSD_DECRYPT_IMG_FRAG_ID 0x07 46 47 /* SSD parsing status messages from TZ */ 48 #define SSD_PMD_ENCRYPTED 0 49 #define SSD_PMD_NOT_ENCRYPTED 1 50 #define SSD_PMD_PARSING_INCOMPLETE 6 51 52 #define SSD_HEADER_MIN_SIZE 128 53 #define MULTIPLICATION_FACTOR 2 54 55 #define SMCMOD_DECRYPT_REQ_OP_METADATA 1 56 #define SMCMOD_DECRYPT_REQ_OP_IMG_FRAG 2 57 58 struct smcmod_decrypt_req { 59 uint32_t service_id; /* in */ 60 uint32_t command_id; /* in */ 61 uint32_t operation; /* in */ 62 63 union { 64 struct { 65 uint32_t len; 66 uint32_t ion_fd; 67 } metadata; 68 struct { 69 uint32_t ctx_id; 70 uint32_t last_frag; 71 uint32_t frag_len; 72 uint32_t ion_fd; 73 uint32_t offset; 74 } img_frag; 75 } request; 76 77 union { 78 struct { 79 uint32_t status; 80 uint32_t ctx_id; 81 uint32_t end_offset; 82 } metadata; 83 struct { 84 uint32_t status; 85 } img_frag; 86 } response; 87 }; 88 89 #define SMCMOD_IOC_MAGIC 0x97 90 #define SMCMOD_IOCTL_DECRYPT_CMD \ 91 _IOWR(SMCMOD_IOC_MAGIC, 37, struct smcmod_decrypt_req) 92 93 struct ion_buf_handle { 94 unsigned char *buffer; 95 uint32_t buffer_len; 96 int ion_fd; 97 int ifd_data_fd; 98 struct ion_handle_data ion_alloc_handle; 99 }; 100 101 static int 102 ion_memalloc(struct ion_buf_handle *buf, uint32_t size, uint32_t heap) 103 { 104 struct ion_allocation_data alloc_data; 105 struct ion_fd_data fd_data; 106 unsigned char *va; 107 struct ion_handle_data handle_data; 108 int ion_fd; 109 int rc; 110 111 ion_fd = open("/dev/ion", O_RDONLY); 112 if (ion_fd < 0) { 113 fprintf(stderr, "Cannot open ION device (%s)\n", strerror(errno)); 114 return -1; 115 } 116 117 alloc_data.len = (size + 4095) & ~4095; 118 alloc_data.align = 4096; 119 120 alloc_data.flags = 0; 121 alloc_data.heap_id_mask = ION_HEAP(heap); 122 123 /* Set the buffers to be uncached */ 124 alloc_data.flags = 0; 125 126 rc = ioctl(ion_fd, ION_IOC_ALLOC, &alloc_data); 127 if (rc) { 128 fprintf(stderr, "ION buffer allocation failed (%s)\n", 129 strerror(errno)); 130 goto alloc_fail; 131 } 132 133 if (alloc_data.handle) { 134 fd_data.handle = alloc_data.handle; 135 } else { 136 fprintf(stderr, "ION alloc data returned NULL\n"); 137 rc = -1; 138 goto alloc_fail; 139 } 140 141 rc = ioctl(ion_fd, ION_IOC_MAP, &fd_data); 142 if (rc) { 143 fprintf(stderr, "ION map call failed(%s)\n", strerror(errno)); 144 goto ioctl_fail; 145 } 146 147 va = (unsigned char*)mmap(NULL, alloc_data.len, PROT_READ | PROT_WRITE, 148 MAP_SHARED, fd_data.fd, 0); 149 if (va == MAP_FAILED) { 150 fprintf(stderr, "ION memory map failed (%s)\n", strerror(errno)); 151 rc = -1; 152 goto map_fail; 153 } 154 155 buf->ion_fd = ion_fd; 156 buf->ifd_data_fd = fd_data.fd; 157 buf->buffer = va; 158 buf->ion_alloc_handle.handle = alloc_data.handle; 159 buf->buffer_len = alloc_data.len; 160 161 memset(buf->buffer, 0, buf->buffer_len); 162 return 0; 163 164 map_fail: 165 ioctl_fail: 166 handle_data.handle = alloc_data.handle; 167 if (buf->ifd_data_fd) 168 close(buf->ifd_data_fd); 169 rc = ioctl(ion_fd, ION_IOC_FREE, &handle_data); 170 if (rc) 171 fprintf(stderr, "ION free failed (%s)\n", strerror(errno)); 172 alloc_fail: 173 if (ion_fd >= 0) 174 close(ion_fd); 175 buf->ion_fd = -1; 176 return rc; 177 } 178 179 static int ion_memfree(struct ion_buf_handle *handle) 180 { 181 struct ion_handle_data handle_data; 182 int ret; 183 184 ret = munmap(handle->buffer, (handle->buffer_len + 4095) & ~4095); 185 if (ret) 186 fprintf(stderr, "munmap failed (%s)\n", strerror(errno)); 187 188 handle_data.handle = handle->ion_alloc_handle.handle; 189 close(handle->ifd_data_fd); 190 ret = ioctl(handle->ion_fd, ION_IOC_FREE, &handle_data); 191 if (ret) 192 fprintf(stderr, "ION free failed (%s)\n", strerror(errno)); 193 close(handle->ion_fd); 194 195 return ret; 196 } 197 198 static int ion_buffer_clean_inval(struct ion_buf_handle *buf, uint32_t cmd) 199 { 200 struct ion_flush_data data; 201 int rc; 202 203 data.fd = buf->ifd_data_fd; 204 data.vaddr = buf->buffer; 205 data.length = buf->buffer_len; 206 data.offset = 0; 207 data.handle = buf->ion_alloc_handle.handle; 208 209 rc = ioctl(buf->ion_fd, cmd, &data); 210 if (rc < 0) 211 fprintf(stderr, "clean_inval cache failed (%s)\n", strerror(errno)); 212 213 return rc; 214 } 215 216 static int is_encrypted(int smcmod_fd, struct ion_buf_handle *buf, 217 uint32_t len, uint32_t *ctx_id, uint32_t *end_offset) 218 { 219 struct smcmod_decrypt_req req; 220 uint32_t status; 221 int ret; 222 223 req.service_id = SCM_SVC_SSD; 224 req.command_id = SSD_PARSE_MD_ID; 225 req.operation = SMCMOD_DECRYPT_REQ_OP_METADATA; 226 req.request.metadata.len = 227 (len >= SSD_HEADER_MIN_SIZE) ? SSD_HEADER_MIN_SIZE : len; 228 req.request.metadata.ion_fd = buf->ifd_data_fd; 229 230 do { 231 ret = ioctl(smcmod_fd, SMCMOD_IOCTL_DECRYPT_CMD, &req); 232 if (ret < 0) 233 fprintf(stderr, "%s: ioctl ret=%d, %s\n", __func__, ret, 234 strerror(errno)); 235 236 status = req.response.metadata.status; 237 238 if (!ret && (status == SSD_PMD_PARSING_INCOMPLETE)) { 239 req.request.metadata.len *= MULTIPLICATION_FACTOR; 240 continue; 241 } else { 242 break; 243 } 244 } while (1); 245 246 if (!ret) { 247 if (status == SSD_PMD_ENCRYPTED) { 248 *ctx_id = req.response.metadata.ctx_id; 249 *end_offset = req.response.metadata.end_offset; 250 ret = 1; 251 } else { 252 fprintf(stderr, "Image is not encrypted (response status %d)\n", 253 status); 254 } 255 } else { 256 fprintf(stderr, "%s: call failed\n", __func__); 257 } 258 259 return ret; 260 } 261 262 static int decrypt(int smcmod_fd, struct ion_buf_handle *buf, uint32_t len, 263 uint32_t md_ctx, uint32_t offset) 264 { 265 struct smcmod_decrypt_req req; 266 int ret; 267 268 req.service_id = SCM_SVC_SSD; 269 req.command_id = SSD_DECRYPT_IMG_FRAG_ID; 270 req.operation = SMCMOD_DECRYPT_REQ_OP_IMG_FRAG; 271 req.request.img_frag.ctx_id = md_ctx; 272 req.request.img_frag.last_frag = 1; 273 req.request.img_frag.ion_fd = buf->ifd_data_fd; 274 req.request.img_frag.frag_len = len - offset; 275 req.request.img_frag.offset = offset; 276 277 ret = ioctl(smcmod_fd, SMCMOD_IOCTL_DECRYPT_CMD, &req); 278 if (ret < 0) { 279 fprintf(stderr, "decrypt ioctl failed (%s)\n", strerror(errno)); 280 return ret; 281 } 282 283 return 0; 284 } 285 286 static int save_file(const char *fname, unsigned char *buf, size_t len) 287 { 288 FILE *file; 289 size_t written; 290 291 file = fopen(fname, "wb"); 292 if (!file) { 293 fprintf(stderr, "Failed to open %s (%s)\n", fname, strerror(errno)); 294 return -errno; 295 } 296 297 written = fwrite(buf, len, 1, file); 298 if (written != 1) { 299 fclose(file); 300 fprintf(stderr, "Failed to write %s (%s)\n", fname, strerror(errno)); 301 return -errno; 302 } 303 fflush(file); 304 fclose(file); 305 fprintf(stdout, "%s written %d bytes\n", fname, len); 306 return 0; 307 } 308 309 int decrypt_image(const char *src_file, const char *dst_file) 310 { 311 int ret = -1; 312 uint32_t md_ctx = 0, offset = 0; 313 uint32_t fsize = 0; 314 FILE *file = NULL; 315 struct ion_buf_handle ionbuf; 316 int smcmod_fd = -1; 317 int qseecom_fd = -1; 318 size_t read; 319 320 memset(&ionbuf, 0, sizeof(ionbuf)); 321 ionbuf.ion_fd = -1; 322 323 qseecom_fd = open("/dev/qseecom", O_RDWR); 324 if (qseecom_fd < 0) { 325 fprintf(stderr, "Failed to open /dev/qseecom device (%s)\n", 326 strerror(errno)); 327 goto exit; 328 } 329 330 smcmod_fd = open("/dev/smcmod", O_RDWR); 331 if (smcmod_fd < 0) { 332 fprintf(stderr, "Failed to open /dev/smcmod device (%s)\n", 333 strerror(errno)); 334 goto exit; 335 } 336 337 file = fopen(src_file, "rb"); 338 if (!file) { 339 fprintf(stderr, "Failed to open %s (%s)\n", src_file, strerror(errno)); 340 goto exit; 341 } 342 343 fseek(file, 0, SEEK_END); 344 fsize = ftell(file); 345 fseek(file, 0, SEEK_SET); 346 347 ret = ion_memalloc(&ionbuf, fsize, ION_PIL1_HEAP_ID); 348 if (ret) 349 goto exit; 350 351 read = fread(ionbuf.buffer, fsize, 1, file); 352 if (read != 1) { 353 fprintf(stderr, "Failed to read %s (%s)\n", src_file, strerror(errno)); 354 ret = -errno; 355 goto exit; 356 } 357 358 ret = ion_buffer_clean_inval(&ionbuf, ION_IOC_CLEAN_INV_CACHES); 359 if (ret < 0) 360 goto exit; 361 362 ret = ioctl(qseecom_fd, QSEECOM_IOCTL_PERF_ENABLE_REQ); 363 if (ret < 0) 364 goto exit; 365 366 ret = is_encrypted(smcmod_fd, &ionbuf, fsize, &md_ctx, &offset); 367 if (ret < 0) 368 goto exit; 369 370 if (ret == 1) { 371 fprintf(stdout, "decrypting %s ...\n", src_file); 372 ret = decrypt(smcmod_fd, &ionbuf, fsize, md_ctx, offset); 373 if (ret < 0) 374 goto exit; 375 376 ion_buffer_clean_inval(&ionbuf, ION_IOC_INV_CACHES); 377 378 ret = save_file(dst_file, ionbuf.buffer + offset, fsize - offset); 379 if (ret < 0) 380 goto exit; 381 382 fprintf(stdout, "decrypting done!\n"); 383 } 384 385 exit: 386 if (ionbuf.ion_fd >= 0) 387 ion_memfree(&ionbuf); 388 389 if (qseecom_fd >= 0) { 390 ioctl(qseecom_fd, QSEECOM_IOCTL_PERF_DISABLE_REQ); 391 close(qseecom_fd); 392 } 393 394 if (smcmod_fd >= 0) 395 close(smcmod_fd); 396 397 if (file) 398 fclose(file); 399 400 return ret; 401 } 402
