1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <string.h>
4 #include <assert.h>
5 #include "aes_core.h"
6 #include "aes_locl.h"
7
8 #define CCM_DEBUG 0
9 #if 1
10
11 typedef void (*block128_f)(const unsigned char in[16],
12 unsigned char out[16],
13 const void *key);
14
15 typedef void (*ccm128_f)(const unsigned char *in, unsigned char *out,
16 size_t blocks, const void *key,
17 const unsigned char ivec[16],unsigned char cmac[16]);
18
19
20 struct ccm128_context {
21 union { u64 u[2]; u8 c[16]; } nonce, cmac;
22 u64 blocks;
23 block128_f block;
24 void *key;
25 };
26
27 //#define U64(C) C##UL
28
29 typedef struct ccm128_context CCM128_CONTEXT;
30
31 /* First you setup M and L parameters and pass the key schedule.
32 * This is called once per session setup... */
rk_crypto_ccm128_init(CCM128_CONTEXT * ctx,unsigned int M,unsigned int L,void * key,block128_f block)33 static void rk_crypto_ccm128_init(CCM128_CONTEXT *ctx,
34 unsigned int M,unsigned int L,void *key,block128_f block)
35 {
36 // printf("m = %d,L = %d\n",M,L);
37 memset(ctx->nonce.c,0,sizeof(ctx->nonce.c));
38 ctx->nonce.c[0] = ((u8)(L-1)&7) | (u8)(((M-2)/2)&7)<<3;
39 ctx->blocks = 0;
40 ctx->block = block;
41 ctx->key = key;
42 }
43
44 /* !!! Following interfaces are to be called *once* per packet !!! */
45
46 /* Then you setup per-message nonce and pass the length of the message */
rk_crypto_ccm128_setiv(CCM128_CONTEXT * ctx,const unsigned char * nonce,size_t nlen,size_t mlen)47 static int rk_crypto_ccm128_setiv(CCM128_CONTEXT *ctx,
48 const unsigned char *nonce,size_t nlen,size_t mlen)
49 {
50 unsigned int L = ctx->nonce.c[0]&7; /* the L parameter */
51
52 if (nlen<(14-L)) return -1; /* nonce is too short */
53
54 if (sizeof(mlen)==8 && L>=3) {
55 ctx->nonce.c[8] = (u8)(mlen>>(56%(sizeof(mlen)*8)));
56 ctx->nonce.c[9] = (u8)(mlen>>(48%(sizeof(mlen)*8)));
57 ctx->nonce.c[10] = (u8)(mlen>>(40%(sizeof(mlen)*8)));
58 ctx->nonce.c[11] = (u8)(mlen>>(32%(sizeof(mlen)*8)));
59 }
60 else
61 ctx->nonce.u[1] = 0;
62
63 ctx->nonce.c[12] = (u8)(mlen>>24);
64 ctx->nonce.c[13] = (u8)(mlen>>16);
65 ctx->nonce.c[14] = (u8)(mlen>>8);
66 ctx->nonce.c[15] = (u8)mlen;
67
68 ctx->nonce.c[0] &= ~0x40; /* clear Adata flag */
69 memcpy(&ctx->nonce.c[1],nonce,14-L);
70
71 return 0;
72 }
73
74 /* Then you pass additional authentication data, this is optional */
rk_crypto_ccm128_aad(CCM128_CONTEXT * ctx,const unsigned char * aad,size_t alen)75 static void rk_crypto_ccm128_aad(CCM128_CONTEXT *ctx,
76 const unsigned char *aad,size_t alen)
77 { unsigned int i;
78 block128_f block = ctx->block;
79
80 if (alen==0) return;
81
82 ctx->nonce.c[0] |= 0x40; /* set Adata flag */
83 (*block)(ctx->nonce.c,ctx->cmac.c,ctx->key),
84 ctx->blocks++;
85
86 if (alen<(0x10000-0x100)) {
87 ctx->cmac.c[0] ^= (u8)(alen>>8);
88 ctx->cmac.c[1] ^= (u8)alen;
89 i=2;
90 }
91 else if (sizeof(alen)==8 && alen>=(size_t)1<<(32%(sizeof(alen)*8))) {
92 ctx->cmac.c[0] ^= 0xFF;
93 ctx->cmac.c[1] ^= 0xFF;
94 ctx->cmac.c[2] ^= (u8)(alen>>(56%(sizeof(alen)*8)));
95 ctx->cmac.c[3] ^= (u8)(alen>>(48%(sizeof(alen)*8)));
96 ctx->cmac.c[4] ^= (u8)(alen>>(40%(sizeof(alen)*8)));
97 ctx->cmac.c[5] ^= (u8)(alen>>(32%(sizeof(alen)*8)));
98 ctx->cmac.c[6] ^= (u8)(alen>>24);
99 ctx->cmac.c[7] ^= (u8)(alen>>16);
100 ctx->cmac.c[8] ^= (u8)(alen>>8);
101 ctx->cmac.c[9] ^= (u8)alen;
102 i=10;
103 }
104 else {
105 ctx->cmac.c[0] ^= 0xFF;
106 ctx->cmac.c[1] ^= 0xFE;
107 ctx->cmac.c[2] ^= (u8)(alen>>24);
108 ctx->cmac.c[3] ^= (u8)(alen>>16);
109 ctx->cmac.c[4] ^= (u8)(alen>>8);
110 ctx->cmac.c[5] ^= (u8)alen;
111 i=6;
112 }
113
114 do {
115 for(;i<16 && alen;++i,++aad,--alen)
116 ctx->cmac.c[i] ^= *aad;
117 (*block)(ctx->cmac.c,ctx->cmac.c,ctx->key),
118 ctx->blocks++;
119 i=0;
120 } while (alen);
121 }
122
123 /* Finally you encrypt or decrypt the message */
124
125 /* counter part of nonce may not be larger than L*8 bits,
126 * L is not larger than 8, therefore 64-bit counter... */
rk_ctr64_inc(unsigned char * counter)127 static void rk_ctr64_inc(unsigned char *counter) {
128 unsigned int n=8;
129 u8 c;
130
131 counter += 8;
132 do {
133 --n;
134 c = counter[n];
135 ++c;
136 counter[n] = c;
137 if (c) return;
138 } while (n);
139 }
140
rk_crypto_ccm128_encrypt(CCM128_CONTEXT * ctx,const unsigned char * inp,unsigned char * out,size_t len)141 static int rk_crypto_ccm128_encrypt(CCM128_CONTEXT *ctx,
142 const unsigned char *inp, unsigned char *out,
143 size_t len)
144 {
145 size_t n;
146 unsigned int i,L;
147 unsigned char flags0 = ctx->nonce.c[0];
148 block128_f block = ctx->block;
149 void * key = ctx->key;
150 union { u64 u[2]; u8 c[16]; } scratch;
151
152 if (!(flags0&0x40))
153 (*block)(ctx->nonce.c,ctx->cmac.c,key),
154 ctx->blocks++;
155
156 ctx->nonce.c[0] = L = flags0&7;
157 for (n=0,i=15-L;i<15;++i) {
158 n |= ctx->nonce.c[i];
159 ctx->nonce.c[i]=0;
160 n <<= 8;
161 }
162 n |= ctx->nonce.c[15]; /* reconstructed length */
163 ctx->nonce.c[15]=1;
164
165 // printf("n = %d,len = %d\n",n,len);
166
167 if (n!=len) return -1; /* length mismatch */
168
169 ctx->blocks += ((len+15)>>3)|1;
170 if (ctx->blocks > (U64(1)<<61)) return -2; /* too much data */
171
172 while (len>=16) {
173 #if defined(STRICT_ALIGNMENT)
174 union { u64 u[2]; u8 c[16]; } temp;
175
176 memcpy (temp.c,inp,16);
177 ctx->cmac.u[0] ^= temp.u[0];
178 ctx->cmac.u[1] ^= temp.u[1];
179 #else
180 ctx->cmac.u[0] ^= ((u64*)inp)[0];
181 ctx->cmac.u[1] ^= ((u64*)inp)[1];
182 #endif
183 (*block)(ctx->cmac.c,ctx->cmac.c,key);
184 (*block)(ctx->nonce.c,scratch.c,key);
185 rk_ctr64_inc(ctx->nonce.c);
186 #if defined(STRICT_ALIGNMENT)
187 temp.u[0] ^= scratch.u[0];
188 temp.u[1] ^= scratch.u[1];
189 memcpy(out,temp.c,16);
190 #else
191 ((u64*)out)[0] = scratch.u[0]^((u64*)inp)[0];
192 ((u64*)out)[1] = scratch.u[1]^((u64*)inp)[1];
193 #endif
194 inp += 16;
195 out += 16;
196 len -= 16;
197 }
198
199 if (len) {
200 for (i=0; i<len; ++i) ctx->cmac.c[i] ^= inp[i];
201 (*block)(ctx->cmac.c,ctx->cmac.c,key);
202 (*block)(ctx->nonce.c,scratch.c,key);
203 for (i=0; i<len; ++i) out[i] = scratch.c[i]^inp[i];
204 }
205
206 for (i=15-L;i<16;++i)
207 ctx->nonce.c[i]=0;
208
209 (*block)(ctx->nonce.c,scratch.c,key);
210 ctx->cmac.u[0] ^= scratch.u[0];
211 ctx->cmac.u[1] ^= scratch.u[1];
212
213 ctx->nonce.c[0] = flags0;
214
215 return 0;
216 }
217
rk_crypto_ccm128_decrypt(CCM128_CONTEXT * ctx,const unsigned char * inp,unsigned char * out,size_t len)218 static int rk_crypto_ccm128_decrypt(CCM128_CONTEXT *ctx,
219 const unsigned char *inp, unsigned char *out,
220 size_t len)
221 {
222 size_t n;
223 unsigned int i,L;
224 unsigned char flags0 = ctx->nonce.c[0];
225 block128_f block = ctx->block;
226 void * key = ctx->key;
227 union { u64 u[2]; u8 c[16]; } scratch;
228
229 if (!(flags0&0x40))
230 (*block)(ctx->nonce.c,ctx->cmac.c,key);
231
232 ctx->nonce.c[0] = L = flags0&7;
233 for (n=0,i=15-L;i<15;++i) {
234 n |= ctx->nonce.c[i];
235 ctx->nonce.c[i]=0;
236 n <<= 8;
237 }
238 n |= ctx->nonce.c[15]; /* reconstructed length */
239 ctx->nonce.c[15]=1;
240
241 // printf("n = %d,len = %d\n",n,len);
242
243 if (n!=len) return -1;
244
245 while (len>=16) {
246 #if defined(STRICT_ALIGNMENT)
247 union { u64 u[2]; u8 c[16]; } temp;
248 #endif
249 (*block)(ctx->nonce.c,scratch.c,key);
250 rk_ctr64_inc(ctx->nonce.c);
251 #if defined(STRICT_ALIGNMENT)
252 memcpy (temp.c,inp,16);
253 ctx->cmac.u[0] ^= (scratch.u[0] ^= temp.u[0]);
254 ctx->cmac.u[1] ^= (scratch.u[1] ^= temp.u[1]);
255 memcpy (out,scratch.c,16);
256 #else
257 ctx->cmac.u[0] ^= (((u64*)out)[0] = scratch.u[0]^((u64*)inp)[0]);
258 ctx->cmac.u[1] ^= (((u64*)out)[1] = scratch.u[1]^((u64*)inp)[1]);
259 #endif
260 (*block)(ctx->cmac.c,ctx->cmac.c,key);
261
262 inp += 16;
263 out += 16;
264 len -= 16;
265 }
266
267 if (len) {
268 (*block)(ctx->nonce.c,scratch.c,key);
269 for (i=0; i<len; ++i)
270 ctx->cmac.c[i] ^= (out[i] = scratch.c[i]^inp[i]);
271 (*block)(ctx->cmac.c,ctx->cmac.c,key);
272 }
273
274 for (i=15-L;i<16;++i)
275 ctx->nonce.c[i]=0;
276
277 (*block)(ctx->nonce.c,scratch.c,key);
278 ctx->cmac.u[0] ^= scratch.u[0];
279 ctx->cmac.u[1] ^= scratch.u[1];
280
281 ctx->nonce.c[0] = flags0;
282
283 return 0;
284 }
285
286 #if 0
287 static void rk_ctr64_add (unsigned char *counter,size_t inc)
288 { size_t n=8, val=0;
289
290 counter += 8;
291 do {
292 --n;
293 val += counter[n] + (inc&0xff);
294 counter[n] = (unsigned char)val;
295 val >>= 8; /* carry bit */
296 inc >>= 8;
297 } while(n && (inc || val));
298 }
299
300
301 static int rk_crypto_ccm128_encrypt_ccm64(CCM128_CONTEXT *ctx,
302 const unsigned char *inp, unsigned char *out,
303 size_t len,ccm128_f stream)
304 {
305 size_t n;
306 unsigned int i,L;
307 unsigned char flags0 = ctx->nonce.c[0];
308 block128_f block = ctx->block;
309 void * key = ctx->key;
310 union { u64 u[2]; u8 c[16]; } scratch;
311
312 if (!(flags0&0x40))
313 (*block)(ctx->nonce.c,ctx->cmac.c,key),
314 ctx->blocks++;
315
316 ctx->nonce.c[0] = L = flags0&7;
317 for (n=0,i=15-L;i<15;++i) {
318 n |= ctx->nonce.c[i];
319 ctx->nonce.c[i]=0;
320 n <<= 8;
321 }
322 n |= ctx->nonce.c[15]; /* reconstructed length */
323 ctx->nonce.c[15]=1;
324
325 if (n!=len) return -1; /* length mismatch */
326
327 ctx->blocks += ((len+15)>>3)|1;
328 if (ctx->blocks > (U64(1)<<61)) return -2; /* too much data */
329
330 n=len/16;
331 if (n) {
332 (*stream)(inp,out,n,key,ctx->nonce.c,ctx->cmac.c);
333 n *= 16;
334 inp += n;
335 out += n;
336 len -= n;
337 if (len) rk_ctr64_add(ctx->nonce.c,n/16);
338 }
339
340 if (len) {
341 for (i=0; i<len; ++i) ctx->cmac.c[i] ^= inp[i];
342 (*block)(ctx->cmac.c,ctx->cmac.c,key);
343 (*block)(ctx->nonce.c,scratch.c,key);
344 for (i=0; i<len; ++i) out[i] = scratch.c[i]^inp[i];
345 }
346
347 for (i=15-L;i<16;++i)
348 ctx->nonce.c[i]=0;
349
350 (*block)(ctx->nonce.c,scratch.c,key);
351 ctx->cmac.u[0] ^= scratch.u[0];
352 ctx->cmac.u[1] ^= scratch.u[1];
353
354 ctx->nonce.c[0] = flags0;
355
356 return 0;
357 }
358
359 static int rk_crypto_ccm128_decrypt_ccm64(CCM128_CONTEXT *ctx,
360 const unsigned char *inp, unsigned char *out,
361 size_t len,ccm128_f stream)
362 {
363 size_t n;
364 unsigned int i,L;
365 unsigned char flags0 = ctx->nonce.c[0];
366 block128_f block = ctx->block;
367 void * key = ctx->key;
368 union { u64 u[2]; u8 c[16]; } scratch;
369
370 if (!(flags0&0x40))
371 (*block)(ctx->nonce.c,ctx->cmac.c,key);
372
373 ctx->nonce.c[0] = L = flags0&7;
374 for (n=0,i=15-L;i<15;++i) {
375 n |= ctx->nonce.c[i];
376 ctx->nonce.c[i]=0;
377 n <<= 8;
378 }
379 n |= ctx->nonce.c[15]; /* reconstructed length */
380 ctx->nonce.c[15]=1;
381
382 if (n!=len) return -1;
383
384 n=len/16;
385 if (n) {
386 (*stream)(inp,out,n,key,ctx->nonce.c,ctx->cmac.c);
387 n *= 16;
388 inp += n;
389 out += n;
390 len -= n;
391 if (len) rk_ctr64_add(ctx->nonce.c,n/16);
392 }
393
394 if (len) {
395 (*block)(ctx->nonce.c,scratch.c,key);
396 for (i=0; i<len; ++i)
397 ctx->cmac.c[i] ^= (out[i] = scratch.c[i]^inp[i]);
398 (*block)(ctx->cmac.c,ctx->cmac.c,key);
399 }
400
401 for (i=15-L;i<16;++i)
402 ctx->nonce.c[i]=0;
403
404 (*block)(ctx->nonce.c,scratch.c,key);
405 ctx->cmac.u[0] ^= scratch.u[0];
406 ctx->cmac.u[1] ^= scratch.u[1];
407
408 ctx->nonce.c[0] = flags0;
409
410 return 0;
411 }
412 #endif
rk_crypto_ccm128_tag(CCM128_CONTEXT * ctx,unsigned char * tag,size_t len)413 size_t rk_crypto_ccm128_tag(CCM128_CONTEXT *ctx,unsigned char *tag,size_t len)
414 { unsigned int M = (ctx->nonce.c[0]>>3)&7; /* the M parameter */
415
416 M *= 2; M += 2;
417 if (len<M) return 0;
418 memcpy(tag,ctx->cmac.c,M);
419 return M;
420 }
421
422 #endif
423
424
425
426
427 /*m is the lengh of tag*/
428
429 #if 0
430 int rk_aes_ccm_encrypt(struct aes_ae_in *in, struct aes_ae_out *out, const int enc)
431 {
432 int time = 0;
433 int i = 0;
434 RK_AES_KEY ks1, ks2;
435 CCM128_CONTEXT ctx;
436 int ret = 0;
437 //unsigned int m = 12;
438 unsigned int l = 0;
439
440 if (in->key == NULL || in->iv == NULL || in->src == NULL || in->aad == NULL)
441 return -1;
442
443 if (in->key_len!= 128/8 && in->key_len != 192/8 && in->key_len != 256/8)
444 return -2;
445
446 if(in->src_len % 16 != 0)
447 return -3;
448
449 if(out->dest == NULL || out->tag == NULL)
450 return -4;
451 printf("-----param sucess-----\n");
452
453
454 l = out->dest_len;/* dest_len = inlength */
455
456 ret = rk_aes_set_encrypt_key(in->key, in->key_len * 8, &ks1);
457 if(ret != 0)
458 printf("-----set_encrypt_key fail-----\n");
459
460 rk_crypto_ccm128_init(&ctx, in->tag_size, l, &ks1, (block128_f)rk_aes_encrypt);
461
462 ret = rk_crypto_ccm128_setiv(&ctx, in->iv, in->iv_len, l);/*l ?*/
463 if (ret != 0)
464 printf("========rk_crypto_ccm128_setiv ret = %d================\n",ret);
465
466 rk_crypto_ccm128_aad(&ctx, in->aad, in->aad_len);
467
468 if(enc){
469 if((ret = rk_crypto_ccm128_encrypt(&ctx, in->src , out->dest,in->src_len)) != 0)
470 printf("=========rk_crypto_ccm128_encrypt ret = %d===\n",ret);
471 rk_crypto_ccm128_tag(&ctx, out->tag, 12); /*tag is length*/
472 }
473 else{if((ret = rk_crypto_ccm128_decrypt(&ctx, out->dest, in->src, out->dest_len)) != 0)
474 printf("=========rk_crypto_ccm128_decrypt ret = %d===\n",ret);
475
476 }
477
478 printf("----op done------------");
479
480 return 0;
481 }
482 #endif
483
compare_string(unsigned char * a,unsigned char * b,unsigned int len)484 static int compare_string(unsigned char *a, unsigned char *b, unsigned int len)
485 {
486 unsigned int i;
487
488 if((a == NULL) || (b == NULL))
489 return -1;
490
491 for (i = 0; i < len; i++){
492 if(*a != *b)
493 return -1;
494 a++;
495 b++;
496 }
497 return 0;
498 }
499
rk_aes_ccm_encrypt(struct aes_ae_in * in,struct aes_ae_out * out,const int enc)500 int rk_aes_ccm_encrypt(struct aes_ae_in *in, struct aes_ae_out *out, const int enc)
501 {
502 RK_AES_KEY ks1;
503 CCM128_CONTEXT ctx;
504 int ret = 0;
505
506 unsigned int m = 0;
507 unsigned int l = 0;
508
509 unsigned char tag_tmp[16]= {0};
510
511 if (in->key == NULL || in->iv == NULL || in->src == NULL || in->aad == NULL)
512 return -1;
513
514 if (in->key_len!= 128/8 && in->key_len != 192/8 && in->key_len != 256/8)
515 return -2;
516
517 if(in->src_len % 16 != 0)
518 return -3;
519
520 if(out->dest == NULL || out->tag == NULL)
521 return -4;
522
523 m = in->tag_size;
524 // tag_tmp = malloc(m);
525
526 l = 15 - in->iv_len; /* l + iv_len = 15 */
527 ret = rk_aes_set_encrypt_key(in->key, in->key_len * 8, &ks1);
528 if(ret != 0) {
529 printf("-----set_encrypt_key fail-----\n");
530 goto exit;
531 }
532
533
534
535 /* M :tag size ,L = 8? src_Len*/
536 rk_crypto_ccm128_init(&ctx, in->tag_size, l, &ks1, (block128_f)rk_aes_encrypt);
537
538
539
540 // ret = rk_crypto_ccm128_setiv(&ctx, in->iv, in->iv_len, l);/*l ?*/
541 ret = rk_crypto_ccm128_setiv(&ctx, in->iv, in->iv_len, in->src_len);
542 if (ret != 0) {
543 printf("========rk_crypto_ccm128_setiv ret = %d================\n",ret);
544 goto exit;
545 }
546
547 rk_crypto_ccm128_aad(&ctx, in->aad, in->aad_len);
548
549 if(enc){
550 if((ret = rk_crypto_ccm128_encrypt(&ctx, in->src , out->dest,in->src_len)) != 0) {
551 printf("=========rk_crypto_ccm128_encrypt ret = %d===\n",ret);
552 goto exit;
553 }
554
555 rk_crypto_ccm128_tag(&ctx, out->tag, m); /*tag is length*/
556 } else {
557 if((ret = rk_crypto_ccm128_decrypt(&ctx, in->src, out->dest, in->src_len)) != 0) {
558 printf("=========rk_crypto_ccm128_decrypt ret = %d===\n",ret);
559 goto exit;
560 }
561
562 rk_crypto_ccm128_tag(&ctx, tag_tmp, m);
563 return compare_string(tag_tmp, out->tag, m);
564 }
565
566 exit:
567 return ret;
568 }
569
570
571
572