comparison arib25v021/arib25/src/multi2.c @ 0:67e8eca28a80

initial import
author Yoshiki Yazawa <yaz@honeyplanet.jp>
date Mon, 16 Feb 2009 15:41:49 +0900
parents
children
comparison
equal deleted inserted replaced
-1:000000000000 0:67e8eca28a80
1 #include <stdlib.h>
2 #include <string.h>
3
4 #include "multi2.h"
5 #include "multi2_error_code.h"
6
7 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
8 inline functions
9 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
10 static __inline uint8_t *load_be_uint32(uint32_t *dst, uint8_t *src)
11 {
12 *dst = ((src[0]<<24)|(src[1]<<16)|(src[2]<<8)|src[3]);
13 return src+4;
14 }
15
16 static __inline uint8_t *save_be_uint32(uint8_t *dst, uint32_t src)
17 {
18 dst[0] = (uint8_t)((src>>24) & 0xff);
19 dst[1] = (uint8_t)((src>>16) & 0xff);
20 dst[2] = (uint8_t)((src>> 8) & 0xff);
21 dst[3] = (uint8_t)( src & 0xff);
22 return dst+4;
23 }
24
25 static __inline uint32_t left_rotate_uint32(uint32_t val, uint32_t count)
26 {
27 return ((val << count) | (val >> (32-count)));
28 }
29
30 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31 inner structures
32 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
33 typedef struct {
34 uint32_t key[8];
35 } CORE_PARAM;
36
37 typedef struct {
38 uint32_t l;
39 uint32_t r;
40 } CORE_DATA;
41
42 typedef struct {
43
44 int32_t ref_count;
45
46 CORE_DATA cbc_init;
47
48 CORE_PARAM sys;
49 CORE_DATA scr[2]; /* 0: odd, 1: even */
50 CORE_PARAM wrk[2]; /* 0: odd, 1: even */
51
52 uint32_t round;
53 uint32_t state;
54
55 } MULTI2_PRIVATE_DATA;
56
57 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
58 constant values
59 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
60 #define MULTI2_STATE_CBC_INIT_SET (0x0001)
61 #define MULTI2_STATE_SYSTEM_KEY_SET (0x0002)
62 #define MULTI2_STATE_SCRAMBLE_KEY_SET (0x0004)
63
64 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
65 function prottypes (interface method)
66 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
67 static void release_multi2(void *m2);
68 static int add_ref_multi2(void *m2);
69 static int set_round_multi2(void *m2, int32_t val);
70 static int set_system_key_multi2(void *m2, uint8_t *val);
71 static int set_init_cbc_multi2(void *m2, uint8_t *val);
72 static int set_scramble_key_multi2(void *m2, uint8_t *val);
73 static int clear_scramble_key_multi2(void *m2);
74 static int encrypt_multi2(void *m2, int32_t type, uint8_t *buf, int32_t size);
75 static int decrypt_multi2(void *m2, int32_t type, uint8_t *buf, int32_t size);
76
77 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
78 global function implementation
79 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
80 MULTI2 *create_multi2()
81 {
82 int n;
83
84 MULTI2 *r;
85 MULTI2_PRIVATE_DATA *prv;
86
87 n = sizeof(MULTI2_PRIVATE_DATA);
88 n += sizeof(MULTI2);
89
90 prv = (MULTI2_PRIVATE_DATA *)calloc(1, n);
91 if(prv == NULL){
92 return NULL;
93 }
94
95 r = (MULTI2 *)(prv+1);
96 r->private_data = prv;
97
98 prv->ref_count = 1;
99 prv->round = 4;
100
101 r->release = release_multi2;
102 r->add_ref = add_ref_multi2;
103 r->set_round = set_round_multi2;
104 r->set_system_key = set_system_key_multi2;
105 r->set_init_cbc = set_init_cbc_multi2;
106 r->set_scramble_key = set_scramble_key_multi2;
107 r->clear_scramble_key = clear_scramble_key_multi2;
108 r->encrypt = encrypt_multi2;
109 r->decrypt = decrypt_multi2;
110
111 return r;
112 }
113
114 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
115 function prottypes (private method)
116 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
117 static MULTI2_PRIVATE_DATA *private_data(void *m2);
118
119 static void core_schedule(CORE_PARAM *work, CORE_PARAM *skey, CORE_DATA *dkey);
120
121 static void core_encrypt(CORE_DATA *dst, CORE_DATA *src, CORE_PARAM *w, int32_t round);
122 static void core_decrypt(CORE_DATA *dst, CORE_DATA *src, CORE_PARAM *w, int32_t round);
123
124 static void core_pi1(CORE_DATA *dst, CORE_DATA *src);
125 static void core_pi2(CORE_DATA *dst, CORE_DATA *src, uint32_t a);
126 static void core_pi3(CORE_DATA *dst, CORE_DATA *src, uint32_t a, uint32_t b);
127 static void core_pi4(CORE_DATA *dst, CORE_DATA *src, uint32_t a);
128
129 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
130 interface method implementation
131 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
132 static void release_multi2(void *m2)
133 {
134 MULTI2_PRIVATE_DATA *prv;
135
136 prv = private_data(m2);
137 if(prv == NULL){
138 /* do nothing */
139 return;
140 }
141
142 prv->ref_count -= 1;
143 if(prv->ref_count == 0){
144 free(prv);
145 }
146 }
147
148 static int add_ref_multi2(void *m2)
149 {
150 MULTI2_PRIVATE_DATA *prv;
151
152 prv = private_data(m2);
153 if(prv == NULL){
154 return MULTI2_ERROR_INVALID_PARAMETER;
155 }
156
157 prv->ref_count += 1;
158
159 return 0;
160 }
161
162 static int set_round_multi2(void *m2, int32_t val)
163 {
164 MULTI2_PRIVATE_DATA *prv;
165
166 prv = private_data(m2);
167 if(prv == NULL){
168 /* do nothing */
169 return MULTI2_ERROR_INVALID_PARAMETER;
170 }
171
172 prv->round = val;
173
174 return 0;
175 }
176
177 static int set_system_key_multi2(void *m2, uint8_t *val)
178 {
179 int i;
180 uint8_t *p;
181
182 MULTI2_PRIVATE_DATA *prv;
183
184 prv = private_data(m2);
185 if( (prv == NULL) || (val == NULL) ){
186 return MULTI2_ERROR_INVALID_PARAMETER;
187 }
188
189 p = val;
190 for(i=0;i<8;i++){
191 p = load_be_uint32(prv->sys.key+i, p);
192 }
193
194 prv->state |= MULTI2_STATE_SYSTEM_KEY_SET;
195
196 return 0;
197 }
198
199 static int set_init_cbc_multi2(void *m2, uint8_t *val)
200 {
201 uint8_t *p;
202
203 MULTI2_PRIVATE_DATA *prv;
204
205 prv = private_data(m2);
206 if( (prv == NULL) || (val == NULL) ){
207 return MULTI2_ERROR_INVALID_PARAMETER;
208 }
209
210 p = val;
211
212 p = load_be_uint32(&(prv->cbc_init.l), p);
213 p = load_be_uint32(&(prv->cbc_init.r), p);
214
215 prv->state |= MULTI2_STATE_CBC_INIT_SET;
216
217 return 0;
218 }
219
220 static int set_scramble_key_multi2(void *m2, uint8_t *val)
221 {
222 uint8_t *p;
223
224 MULTI2_PRIVATE_DATA *prv;
225
226 prv = private_data(m2);
227 if( (prv == NULL) || (val == NULL) ){
228 return MULTI2_ERROR_INVALID_PARAMETER;
229 }
230
231 p = val;
232
233 p = load_be_uint32(&(prv->scr[0].l), p);
234 p = load_be_uint32(&(prv->scr[0].r), p);
235 p = load_be_uint32(&(prv->scr[1].l), p);
236 p = load_be_uint32(&(prv->scr[1].r), p);
237
238 core_schedule(prv->wrk+0, &(prv->sys), prv->scr+0);
239 core_schedule(prv->wrk+1, &(prv->sys), prv->scr+1);
240
241 prv->state |= MULTI2_STATE_SCRAMBLE_KEY_SET;
242
243 return 0;
244 }
245
246 static int clear_scramble_key_multi2(void *m2)
247 {
248 MULTI2_PRIVATE_DATA *prv;
249
250 prv = private_data(m2);
251 if(prv == NULL){
252 return MULTI2_ERROR_INVALID_PARAMETER;
253 }
254
255 memset(prv->scr, 0, sizeof(prv->scr));
256 memset(prv->wrk, 0, sizeof(prv->wrk));
257
258 prv->state &= (~MULTI2_STATE_SCRAMBLE_KEY_SET);
259
260 return 0;
261 }
262
263 static int encrypt_multi2(void *m2, int32_t type, uint8_t *buf, int32_t size)
264 {
265 CORE_DATA src,dst;
266 CORE_PARAM *prm;
267
268 uint8_t *p;
269
270 MULTI2_PRIVATE_DATA *prv;
271
272 prv = private_data(m2);
273 if( (prv == NULL) || (buf == NULL) || (size < 1) ){
274 return MULTI2_ERROR_INVALID_PARAMETER;
275 }
276
277 if(prv->state != (MULTI2_STATE_CBC_INIT_SET|MULTI2_STATE_SYSTEM_KEY_SET|MULTI2_STATE_SCRAMBLE_KEY_SET)){
278 if( (prv->state & MULTI2_STATE_CBC_INIT_SET) == 0 ){
279 return MULTI2_ERROR_UNSET_CBC_INIT;
280 }
281 if( (prv->state & MULTI2_STATE_SYSTEM_KEY_SET) == 0 ){
282 return MULTI2_ERROR_UNSET_SYSTEM_KEY;
283 }
284 if( (prv->state & MULTI2_STATE_SCRAMBLE_KEY_SET) == 0 ){
285 return MULTI2_ERROR_UNSET_SCRAMBLE_KEY;
286 }
287 }
288
289 if(type == 0x02){
290 prm = prv->wrk+1;
291 }else{
292 prm = prv->wrk+0;
293 }
294
295 dst.l = prv->cbc_init.l;
296 dst.r = prv->cbc_init.r;
297
298 p = buf;
299 while(size >= 8){
300 load_be_uint32(&(src.l), p+0);
301 load_be_uint32(&(src.r), p+4);
302 src.l = src.l ^ dst.l;
303 src.r = src.r ^ dst.r;
304 core_encrypt(&dst, &src, prm, prv->round);
305 p = save_be_uint32(p, dst.l);
306 p = save_be_uint32(p, dst.r);
307 size -= 8;
308 }
309
310 if(size > 0){
311 int i;
312 uint8_t tmp[8];
313
314 src.l = dst.l;
315 src.r = dst.r;
316 core_encrypt(&dst, &src, prm, prv->round);
317 save_be_uint32(tmp+0, dst.l);
318 save_be_uint32(tmp+4, dst.r);
319
320 for(i=0;i<size;i++){
321 p[i] = (uint8_t)(p[i] ^ tmp[i]);
322 }
323 }
324
325 return 0;
326 }
327
328 static int decrypt_multi2(void *m2, int32_t type, uint8_t *buf, int32_t size)
329 {
330 CORE_DATA src,dst,cbc;
331 CORE_PARAM *prm;
332
333 uint8_t *p;
334
335 MULTI2_PRIVATE_DATA *prv;
336
337 prv = private_data(m2);
338 if( (prv == NULL) || (buf == NULL) || (size < 1) ){
339 return MULTI2_ERROR_INVALID_PARAMETER;
340 }
341
342 if(prv->state != (MULTI2_STATE_CBC_INIT_SET|MULTI2_STATE_SYSTEM_KEY_SET|MULTI2_STATE_SCRAMBLE_KEY_SET)){
343 if( (prv->state & MULTI2_STATE_CBC_INIT_SET) == 0 ){
344 return MULTI2_ERROR_UNSET_CBC_INIT;
345 }
346 if( (prv->state & MULTI2_STATE_SYSTEM_KEY_SET) == 0 ){
347 return MULTI2_ERROR_UNSET_SYSTEM_KEY;
348 }
349 if( (prv->state & MULTI2_STATE_SCRAMBLE_KEY_SET) == 0 ){
350 return MULTI2_ERROR_UNSET_SCRAMBLE_KEY;
351 }
352 }
353
354 if(type == 0x02){
355 prm = prv->wrk+1;
356 }else{
357 prm = prv->wrk+0;
358 }
359
360 cbc.l = prv->cbc_init.l;
361 cbc.r = prv->cbc_init.r;
362
363 p = buf;
364 while(size >= 8){
365 load_be_uint32(&(src.l), p+0);
366 load_be_uint32(&(src.r), p+4);
367 core_decrypt(&dst, &src, prm, prv->round);
368 dst.l = dst.l ^ cbc.l;
369 dst.r = dst.r ^ cbc.r;
370 cbc.l = src.l;
371 cbc.r = src.r;
372 p = save_be_uint32(p, dst.l);
373 p = save_be_uint32(p, dst.r);
374 size -= 8;
375 }
376
377 if(size > 0){
378 int i;
379 uint8_t tmp[8];
380
381 core_encrypt(&dst, &cbc, prm, prv->round);
382 save_be_uint32(tmp+0, dst.l);
383 save_be_uint32(tmp+4, dst.r);
384
385 for(i=0;i<size;i++){
386 p[i] = (uint8_t)(p[i] ^ tmp[i]);
387 }
388 }
389
390 return 0;
391 }
392
393 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
394 private method implementation
395 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
396 static MULTI2_PRIVATE_DATA *private_data(void *m2)
397 {
398 MULTI2_PRIVATE_DATA *r;
399 MULTI2 *p;
400
401 p = (MULTI2 *)m2;
402 if(p == NULL){
403 return NULL;
404 }
405
406 r = (MULTI2_PRIVATE_DATA *)(p->private_data);
407 if( ((void *)(r+1)) != ((void *)p) ){
408 return NULL;
409 }
410
411 return r;
412 }
413
414 static void core_schedule(CORE_PARAM *work, CORE_PARAM *skey, CORE_DATA *dkey)
415 {
416 CORE_DATA b1,b2,b3,b4,b5,b6,b7,b8,b9;
417
418 core_pi1(&b1, dkey);
419
420 core_pi2(&b2, &b1, skey->key[0]);
421 work->key[0] = b2.l;
422
423 core_pi3(&b3, &b2, skey->key[1], skey->key[2]);
424 work->key[1] = b3.r;
425
426 core_pi4(&b4, &b3, skey->key[3]);
427 work->key[2] = b4.l;
428
429 core_pi1(&b5, &b4);
430 work->key[3] = b5.r;
431
432 core_pi2(&b6, &b5, skey->key[4]);
433 work->key[4] = b6.l;
434
435 core_pi3(&b7, &b6, skey->key[5], skey->key[6]);
436 work->key[5] = b7.r;
437
438 core_pi4(&b8, &b7, skey->key[7]);
439 work->key[6] = b8.l;
440
441 core_pi1(&b9, &b8);
442 work->key[7] = b9.r;
443 }
444
445 static void core_encrypt(CORE_DATA *dst, CORE_DATA *src, CORE_PARAM *w, int32_t round)
446 {
447 int32_t i;
448
449 CORE_DATA tmp;
450
451 dst->l = src->l;
452 dst->r = src->r;
453 for(i=0;i<round;i++){
454 core_pi1(&tmp, dst);
455 core_pi2( dst, &tmp, w->key[0]);
456 core_pi3(&tmp, dst, w->key[1], w->key[2]);
457 core_pi4( dst, &tmp, w->key[3]);
458 core_pi1(&tmp, dst);
459 core_pi2( dst, &tmp, w->key[4]);
460 core_pi3(&tmp, dst, w->key[5], w->key[6]);
461 core_pi4( dst, &tmp, w->key[7]);
462 }
463 }
464
465 static void core_decrypt(CORE_DATA *dst, CORE_DATA *src, CORE_PARAM *w, int32_t round)
466 {
467 int32_t i;
468
469 CORE_DATA tmp;
470
471 dst->l = src->l;
472 dst->r = src->r;
473 for(i=0;i<round;i++){
474 core_pi4(&tmp, dst, w->key[7]);
475 core_pi3( dst, &tmp, w->key[5], w->key[6]);
476 core_pi2(&tmp, dst, w->key[4]);
477 core_pi1( dst, &tmp);
478 core_pi4(&tmp, dst, w->key[3]);
479 core_pi3( dst, &tmp, w->key[1], w->key[2]);
480 core_pi2(&tmp, dst, w->key[0]);
481 core_pi1( dst, &tmp);
482 }
483 }
484
485 static void core_pi1(CORE_DATA *dst, CORE_DATA *src)
486 {
487 dst->l = src->l;
488 dst->r = src->r ^ src->l;
489 }
490
491 static void core_pi2(CORE_DATA *dst, CORE_DATA *src, uint32_t a)
492 {
493 uint32_t t0,t1,t2;
494
495 t0 = src->r + a;
496 t1 = left_rotate_uint32(t0, 1) + t0 - 1;
497 t2 = left_rotate_uint32(t1, 4) ^ t1;
498
499 dst->l = src->l ^ t2;
500 dst->r = src->r;
501 }
502
503 static void core_pi3(CORE_DATA *dst, CORE_DATA *src, uint32_t a, uint32_t b)
504 {
505 uint32_t t0,t1,t2,t3,t4,t5;
506
507 t0 = src->l + a;
508 t1 = left_rotate_uint32(t0, 2) + t0 + 1;
509 t2 = left_rotate_uint32(t1, 8) ^ t1;
510 t3 = t2 + b;
511 t4 = left_rotate_uint32(t3, 1) - t3;
512 t5 = left_rotate_uint32(t4, 16) ^ (t4 | src->l);
513
514 dst->l = src->l;
515 dst->r = src->r ^ t5;
516 }
517
518 static void core_pi4(CORE_DATA *dst, CORE_DATA *src, uint32_t a)
519 {
520 uint32_t t0,t1;
521
522 t0 = src->r + a;
523 t1 = left_rotate_uint32(t0, 2) + t0 + 1;
524
525 dst->l = src->l ^ t1;
526 dst->r = src->r;
527 }