view arib25v023/arib25/src/multi2.c @ 50:23b6f99f65b2

propagate upstream change: - adapted to FastBoot. use spinlock to avoid resource contention between multiple number of cards.

#include <stdlib.h>
#include <string.h>

#include "multi2.h"
#include "multi2_error_code.h"

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 inline functions
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static __inline uint8_t *load_be_uint32(uint32_t *dst, uint8_t *src)
{
	*dst = ((src[0]<<24)|(src[1]<<16)|(src[2]<<8)|src[3]);
	return src+4;
}

static __inline uint8_t *save_be_uint32(uint8_t *dst, uint32_t src)
{
	dst[0] = (uint8_t)((src>>24) & 0xff);
	dst[1] = (uint8_t)((src>>16) & 0xff);
	dst[2] = (uint8_t)((src>> 8) & 0xff);
	dst[3] = (uint8_t)( src      & 0xff);
	return dst+4;
}

static __inline uint32_t left_rotate_uint32(uint32_t val, uint32_t count)
{
	return ((val << count) | (val >> (32-count)));
}

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 inner structures
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
typedef struct {
	uint32_t key[8];
} CORE_PARAM;

typedef struct {
	uint32_t l;
	uint32_t r;
} CORE_DATA;

typedef struct {

	int32_t    ref_count;

	CORE_DATA  cbc_init;
	
	CORE_PARAM sys;
	CORE_DATA  scr[2]; /* 0: odd, 1: even */
	CORE_PARAM wrk[2]; /* 0: odd, 1: even */

	uint32_t   round;
	uint32_t   state;
	
} MULTI2_PRIVATE_DATA;

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 constant values
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#define MULTI2_STATE_CBC_INIT_SET     (0x0001)
#define MULTI2_STATE_SYSTEM_KEY_SET   (0x0002)
#define MULTI2_STATE_SCRAMBLE_KEY_SET (0x0004)

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 function prottypes (interface method)
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static void release_multi2(void *m2);
static int add_ref_multi2(void *m2);
static int set_round_multi2(void *m2, int32_t val);
static int set_system_key_multi2(void *m2, uint8_t *val);
static int set_init_cbc_multi2(void *m2, uint8_t *val);
static int set_scramble_key_multi2(void *m2, uint8_t *val);
static int clear_scramble_key_multi2(void *m2);
static int encrypt_multi2(void *m2, int32_t type, uint8_t *buf, int32_t size);
static int decrypt_multi2(void *m2, int32_t type, uint8_t *buf, int32_t size);

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 global function implementation
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
MULTI2 *create_multi2()
{
	int n;
	
	MULTI2 *r;
	MULTI2_PRIVATE_DATA *prv;

	n  = sizeof(MULTI2_PRIVATE_DATA);
	n += sizeof(MULTI2);
	
	prv = (MULTI2_PRIVATE_DATA *)calloc(1, n);
	if(prv == NULL){
		return NULL;
	}

	r = (MULTI2 *)(prv+1);
	r->private_data = prv;

	prv->ref_count = 1;
	prv->round = 4;

	r->release = release_multi2;
	r->add_ref = add_ref_multi2;
	r->set_round = set_round_multi2;
	r->set_system_key = set_system_key_multi2;
	r->set_init_cbc = set_init_cbc_multi2;
	r->set_scramble_key = set_scramble_key_multi2;
	r->clear_scramble_key = clear_scramble_key_multi2;
	r->encrypt = encrypt_multi2;
	r->decrypt = decrypt_multi2;

	return r;
}

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 function prottypes (private method)
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static MULTI2_PRIVATE_DATA *private_data(void *m2);

static void core_schedule(CORE_PARAM *work, CORE_PARAM *skey, CORE_DATA *dkey);

static void core_encrypt(CORE_DATA *dst, CORE_DATA *src, CORE_PARAM *w, int32_t round);
static void core_decrypt(CORE_DATA *dst, CORE_DATA *src, CORE_PARAM *w, int32_t round);

static void core_pi1(CORE_DATA *dst, CORE_DATA *src);
static void core_pi2(CORE_DATA *dst, CORE_DATA *src, uint32_t a);
static void core_pi3(CORE_DATA *dst, CORE_DATA *src, uint32_t a, uint32_t b);
static void core_pi4(CORE_DATA *dst, CORE_DATA *src, uint32_t a);

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 interface method implementation
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static void release_multi2(void *m2)
{
	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if(prv == NULL){
		/* do nothing */
		return;
	}

	prv->ref_count -= 1;
	if(prv->ref_count == 0){
		free(prv);
	}
}

static int add_ref_multi2(void *m2)
{
	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if(prv == NULL){
		return MULTI2_ERROR_INVALID_PARAMETER;
	}

	prv->ref_count += 1;

	return 0;
}

static int set_round_multi2(void *m2, int32_t val)
{
	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if(prv == NULL){
		/* do nothing */
		return MULTI2_ERROR_INVALID_PARAMETER;
	}

	prv->round = val;

	return 0;
}

static int set_system_key_multi2(void *m2, uint8_t *val)
{
	int i;
	uint8_t *p;
	
	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if( (prv == NULL) || (val == NULL) ){
		return MULTI2_ERROR_INVALID_PARAMETER;
	}

	p = val;
	for(i=0;i<8;i++){
		p = load_be_uint32(prv->sys.key+i, p);
	}

	prv->state |= MULTI2_STATE_SYSTEM_KEY_SET;

	return 0;
}

static int set_init_cbc_multi2(void *m2, uint8_t *val)
{
	uint8_t *p;
	
	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if( (prv == NULL) || (val == NULL) ){
		return MULTI2_ERROR_INVALID_PARAMETER;
	}

	p = val;

	p = load_be_uint32(&(prv->cbc_init.l), p);
	p = load_be_uint32(&(prv->cbc_init.r), p);

	prv->state |= MULTI2_STATE_CBC_INIT_SET;

	return 0;
}

static int set_scramble_key_multi2(void *m2, uint8_t *val)
{
	uint8_t *p;
	
	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if( (prv == NULL) || (val == NULL) ){
		return MULTI2_ERROR_INVALID_PARAMETER;
	}

	p = val;

	p = load_be_uint32(&(prv->scr[0].l), p);
	p = load_be_uint32(&(prv->scr[0].r), p);
	p = load_be_uint32(&(prv->scr[1].l), p);
	p = load_be_uint32(&(prv->scr[1].r), p);
	
	core_schedule(prv->wrk+0, &(prv->sys), prv->scr+0);
	core_schedule(prv->wrk+1, &(prv->sys), prv->scr+1);

	prv->state |= MULTI2_STATE_SCRAMBLE_KEY_SET;

	return 0;
}

static int clear_scramble_key_multi2(void *m2)
{
	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if(prv == NULL){
		return MULTI2_ERROR_INVALID_PARAMETER;
	}

	memset(prv->scr, 0, sizeof(prv->scr));
	memset(prv->wrk, 0, sizeof(prv->wrk));

	prv->state &= (~MULTI2_STATE_SCRAMBLE_KEY_SET);

	return 0;
}

static int encrypt_multi2(void *m2, int32_t type, uint8_t *buf, int32_t size)
{
	CORE_DATA src,dst;
	CORE_PARAM *prm;

	uint8_t *p;

	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if( (prv == NULL) || (buf == NULL) || (size < 1) ){
		return MULTI2_ERROR_INVALID_PARAMETER;
	}

	if(prv->state != (MULTI2_STATE_CBC_INIT_SET|MULTI2_STATE_SYSTEM_KEY_SET|MULTI2_STATE_SCRAMBLE_KEY_SET)){
		if( (prv->state & MULTI2_STATE_CBC_INIT_SET) == 0 ){
			return MULTI2_ERROR_UNSET_CBC_INIT;
		}
		if( (prv->state & MULTI2_STATE_SYSTEM_KEY_SET) == 0 ){
			return MULTI2_ERROR_UNSET_SYSTEM_KEY;
		}
		if( (prv->state & MULTI2_STATE_SCRAMBLE_KEY_SET) == 0 ){
			return MULTI2_ERROR_UNSET_SCRAMBLE_KEY;
		}
	}
	
	if(type == 0x02){
		prm = prv->wrk+1;
	}else{
		prm = prv->wrk+0;
	}

	dst.l = prv->cbc_init.l;
	dst.r = prv->cbc_init.r;

	p = buf;
	while(size >= 8){
		load_be_uint32(&(src.l), p+0);
		load_be_uint32(&(src.r), p+4);
		src.l = src.l ^ dst.l;
		src.r = src.r ^ dst.r;
		core_encrypt(&dst, &src, prm, prv->round);
		p = save_be_uint32(p, dst.l);
		p = save_be_uint32(p, dst.r);
		size -= 8;
	}

	if(size > 0){
		int i;
		uint8_t tmp[8];
		
		src.l = dst.l;
		src.r = dst.r;
		core_encrypt(&dst, &src, prm, prv->round);
		save_be_uint32(tmp+0, dst.l);
		save_be_uint32(tmp+4, dst.r);

		for(i=0;i<size;i++){
			p[i] = (uint8_t)(p[i] ^ tmp[i]);
		}
	}

	return 0;
}

static int decrypt_multi2(void *m2, int32_t type, uint8_t *buf, int32_t size)
{
	CORE_DATA src,dst,cbc;
	CORE_PARAM *prm;

	uint8_t *p;

	MULTI2_PRIVATE_DATA *prv;

	prv = private_data(m2);
	if( (prv == NULL) || (buf == NULL) || (size < 1) ){
		return MULTI2_ERROR_INVALID_PARAMETER;
	}

	if(prv->state != (MULTI2_STATE_CBC_INIT_SET|MULTI2_STATE_SYSTEM_KEY_SET|MULTI2_STATE_SCRAMBLE_KEY_SET)){
		if( (prv->state & MULTI2_STATE_CBC_INIT_SET) == 0 ){
			return MULTI2_ERROR_UNSET_CBC_INIT;
		}
		if( (prv->state & MULTI2_STATE_SYSTEM_KEY_SET) == 0 ){
			return MULTI2_ERROR_UNSET_SYSTEM_KEY;
		}
		if( (prv->state & MULTI2_STATE_SCRAMBLE_KEY_SET) == 0 ){
			return MULTI2_ERROR_UNSET_SCRAMBLE_KEY;
		}
	}
	
	if(type == 0x02){
		prm = prv->wrk+1;
	}else{
		prm = prv->wrk+0;
	}

	cbc.l = prv->cbc_init.l;
	cbc.r = prv->cbc_init.r;

	p = buf;
	while(size >= 8){
		load_be_uint32(&(src.l), p+0);
		load_be_uint32(&(src.r), p+4);
		core_decrypt(&dst, &src, prm, prv->round);
		dst.l = dst.l ^ cbc.l;
		dst.r = dst.r ^ cbc.r;
		cbc.l = src.l;
		cbc.r = src.r;
		p = save_be_uint32(p, dst.l);
		p = save_be_uint32(p, dst.r);
		size -= 8;
	}

	if(size > 0){
		int i;
		uint8_t tmp[8];
		
		core_encrypt(&dst, &cbc, prm, prv->round);
		save_be_uint32(tmp+0, dst.l);
		save_be_uint32(tmp+4, dst.r);

		for(i=0;i<size;i++){
			p[i] = (uint8_t)(p[i] ^ tmp[i]);
		}
	}

	return 0;
}

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 private method implementation
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static MULTI2_PRIVATE_DATA *private_data(void *m2)
{
	MULTI2_PRIVATE_DATA *r;
	MULTI2 *p;

	p = (MULTI2 *)m2;
	if(p == NULL){
		return NULL;
	}

	r = (MULTI2_PRIVATE_DATA *)(p->private_data);
	if( ((void *)(r+1)) != ((void *)p) ){
		return NULL;
	}

	return r;
}

static void core_schedule(CORE_PARAM *work, CORE_PARAM *skey, CORE_DATA *dkey)
{
	CORE_DATA b1,b2,b3,b4,b5,b6,b7,b8,b9;

	core_pi1(&b1, dkey);
	
	core_pi2(&b2, &b1, skey->key[0]);
	work->key[0] = b2.l;
	
	core_pi3(&b3, &b2, skey->key[1], skey->key[2]);
	work->key[1] = b3.r;
	
	core_pi4(&b4, &b3, skey->key[3]);
	work->key[2] = b4.l;

	core_pi1(&b5, &b4);
	work->key[3] = b5.r;

	core_pi2(&b6, &b5, skey->key[4]);
	work->key[4] = b6.l;

	core_pi3(&b7, &b6, skey->key[5], skey->key[6]);
	work->key[5] = b7.r;

	core_pi4(&b8, &b7, skey->key[7]);
	work->key[6] = b8.l;

	core_pi1(&b9, &b8);
	work->key[7] = b9.r;
}

static void core_encrypt(CORE_DATA *dst, CORE_DATA *src, CORE_PARAM *w, int32_t round)
{
	int32_t i;
	
	CORE_DATA tmp;
	
	dst->l = src->l;
	dst->r = src->r;
	for(i=0;i<round;i++){
		core_pi1(&tmp,  dst);
		core_pi2( dst, &tmp, w->key[0]);
		core_pi3(&tmp,  dst, w->key[1], w->key[2]);
		core_pi4( dst, &tmp, w->key[3]);
		core_pi1(&tmp,  dst);
		core_pi2( dst, &tmp, w->key[4]);
		core_pi3(&tmp,  dst, w->key[5], w->key[6]);
		core_pi4( dst, &tmp, w->key[7]);
	}
}

static void core_decrypt(CORE_DATA *dst, CORE_DATA *src, CORE_PARAM *w, int32_t round)
{
	int32_t i;
	
	CORE_DATA tmp;

	dst->l = src->l;
	dst->r = src->r;
	for(i=0;i<round;i++){
		core_pi4(&tmp,  dst, w->key[7]);
		core_pi3( dst, &tmp, w->key[5], w->key[6]);
		core_pi2(&tmp,  dst, w->key[4]);
		core_pi1( dst, &tmp);
		core_pi4(&tmp,  dst, w->key[3]);
		core_pi3( dst, &tmp, w->key[1], w->key[2]);
		core_pi2(&tmp,  dst, w->key[0]);
		core_pi1( dst, &tmp);
	}
}

static void core_pi1(CORE_DATA *dst, CORE_DATA *src)
{
	dst->l = src->l;
	dst->r = src->r ^ src->l;
}

static void core_pi2(CORE_DATA *dst, CORE_DATA *src, uint32_t a)
{
	uint32_t t0,t1,t2;

	t0 = src->r + a;
	t1 = left_rotate_uint32(t0, 1) + t0 - 1;
	t2 = left_rotate_uint32(t1, 4) ^ t1;

	dst->l = src->l ^ t2;
	dst->r = src->r;
}

static void core_pi3(CORE_DATA *dst, CORE_DATA *src, uint32_t a, uint32_t b)
{
	uint32_t t0,t1,t2,t3,t4,t5;

	t0 = src->l + a;
	t1 = left_rotate_uint32(t0, 2) + t0 + 1;
	t2 = left_rotate_uint32(t1, 8) ^ t1;
	t3 = t2 + b;
	t4 = left_rotate_uint32(t3, 1) - t3;
	t5 = left_rotate_uint32(t4, 16) ^ (t4 | src->l);

	dst->l = src->l;
	dst->r = src->r ^ t5;
}

static void core_pi4(CORE_DATA *dst, CORE_DATA *src, uint32_t a)
{
	uint32_t t0,t1;

	t0 = src->r + a;
	t1 = left_rotate_uint32(t0, 2) + t0 + 1;

	dst->l = src->l ^ t1;
	dst->r = src->r;
}