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
ion_memalloc(struct ion_buf_handle * buf,uint32_t size,uint32_t heap)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
ion_memfree(struct ion_buf_handle * handle)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
ion_buffer_clean_inval(struct ion_buf_handle * buf,uint32_t cmd)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
is_encrypted(int smcmod_fd,struct ion_buf_handle * buf,uint32_t len,uint32_t * ctx_id,uint32_t * end_offset)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
decrypt(int smcmod_fd,struct ion_buf_handle * buf,uint32_t len,uint32_t md_ctx,uint32_t offset)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
save_file(const char * fname,unsigned char * buf,size_t len)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
decrypt_image(const char * src_file,const char * dst_file)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