src/multi2.c

   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 }