UTS6710/drivers/hdmi/6710/hdmi_hdcp.c

#include "drv_types.h"
#include <asm/io.h>
#include "hdmi_hdcp.h"
#include "../kmf/kmflib/bf.h"
#include "drv_spi.h"	/*spi_read_flash()*/
#include <linux/slab.h>	/*kmalloc(), kfree()*/

#define HDCP_RAWKEY_SIZE    (288)
#define HDCP_CKEY_SIZE      (512)
#define HDCP_VKEY_SIZE      (32)
#define HDCP_RKEY_SIZE      (40)
#define HDCP_CRC_SIZE       (4)
#define HDCP_KEY_SIZE       (HDCP_CKEY_SIZE+HDCP_VKEY_SIZE+HDCP_RKEY_SIZE)

#ifdef CONFIG_SUPPORT_STORE_HDCP2X_TO_FLASH
#define HDCP2X_RAWKEY_SIZE  (898)
#define HDCP2X_CKEY_SIZE    (1024)
#define HDCP2X_VKEY_SIZE    (32)
#define HDCP2X_RKEY_SIZE    (40)
#define HDCP2X_CRC_SIZE     (4)
#define HDCP2X_KEY_SIZE     (HDCP2X_CKEY_SIZE+HDCP2X_VKEY_SIZE+HDCP2X_RKEY_SIZE)

#define HDCP2X_HEADER_SIZE       (4)
#define HDCP2X_LIC_CONST_SIZE    (36)    //Licensed Constant
#define HDCP2X_PUB_CERT_SIZE     (522)   //Public Certificate
#define HDCP2X_PVT_KEY_SIZE      (340)   //Private Key
#endif

//debug message
//#define SUPPORT_PRINT_HDCP_MESSAGE

// ----- local static variables/functions -----

#pragma pack(push,1)

typedef struct __HDCPKEY_FMT
{
	union
	{
		UINT32   data[(HDCP_KEY_SIZE/4)];
		struct
		{
			UINT8   ckey[HDCP_CKEY_SIZE];
			UINT8   vkey[HDCP_VKEY_SIZE];
			UINT8   rkey[HDCP_RKEY_SIZE];
		};
	};
	UINT32	crc;

} HDCPKEY_FMT, *HDCPKEY_FMT_PTR;

#ifdef CONFIG_SUPPORT_STORE_HDCP2X_TO_FLASH
typedef struct __HDCP2X_KEY_FMT
{
	union
	{
		UINT32   data[(HDCP2X_KEY_SIZE/4)];
		struct
		{
			UINT8   ckey[HDCP2X_CKEY_SIZE];
			UINT8   vkey[HDCP2X_VKEY_SIZE];
			UINT8   rkey[HDCP2X_RKEY_SIZE];
		};
	};
	UINT32	crc;

} HDCP2X_KEY_FMT, *HDCP2X_KEY_FMT_PTR;

typedef struct __HDCP2X_REV_KEY_SET
{
    struct
    {
        UINT8   Header[HDCP2X_HEADER_SIZE];
        UINT8   LicConst[HDCP2X_LIC_CONST_SIZE];
        UINT8   PubCert[HDCP2X_PUB_CERT_SIZE];
        UINT8   PvtKey[HDCP2X_PVT_KEY_SIZE];
	};
} HDCP2X_REV_KEY_SET, *HDCP2X_REV_KEY_SET_PTR;
#endif

#pragma pack(pop)


static BOOL bHDCP_CRC = false;
static UINT8 local_ksv[5];
static UINT8 local_key_data[HDCP_RAWKEY_SIZE];
#ifdef CONFIG_SUPPORT_STORE_HDCP2X_TO_FLASH
static UINT8 local_2Xkey_data[HDCP2X_RAWKEY_SIZE];
#endif

#ifdef CONFIG_DISABLE_CIKEY_HDCPKEY_SPI_ENCRYPT
extern void do_spi_encrypt(char *, unsigned int, unsigned int);
#endif

#ifdef SUPPORT_PRINT_HDCP_MESSAGE
static void dump(const UINT8 *ptr, INT32 size)
{
	INT32 i, n;
	INT8 str[3 * 0x10 + 8];

	for (n = 0, i = 0; i < size; i++)
	{
		if (n >= 0)
		{
			n += sprintf(&str[n], "%02x ", ptr[i]);
		}

		if (n >= 3 * 0x10 || i + 1 == size)
		{
			n = 0;
			printk("%s\n", str);
		}
	}
}
#endif

/* Keep the encrypt functions for future use */
#if 0
static void cmN_arraBye_shift_inc(INT8 *pChar, INT32 byteLen, INT32 bitShift)
{
    //limit 1024 byte
    UINT8 *pBye;
    UINT8 pBAck[1024];
    INT32 i;
    INT32 offByte, offBit;
    ULONG tempDW;
    pBye = (UINT8 *)pChar;

    if (byteLen > 1024)
    {
        return;
    }

    offByte = bitShift / 8;
    offByte = offByte % byteLen;
    offBit = bitShift & 0x07;//%8

    if (offByte)
    {
        memcpy(pBAck, pBye, byteLen);

        for (i = 0; i < byteLen ; i++)
        {
            if ((i + offByte) < byteLen)
            {
                pBye[i + offByte] = pBAck[i];
            }
            else
            {
                pBye[i + offByte - byteLen] = pBAck[i];
            }
        }
    }

    if (offBit)
    {
        memcpy(pBAck, pBye, byteLen);

        for (i = 0; i < byteLen ; i++)
        {
            if (i == 0)
            {
                tempDW = (((UINT32)pBAck[0]) << 8) + pBAck[byteLen - 1];
            }
            else
            {
                tempDW = (((UINT32)pBAck[i]) << 8) + pBAck[i - 1];
            }

            tempDW = tempDW >> (8 - offBit);
            pBye[i] = (UINT8)tempDW;
        }
    }
}

static void byteEncryption(UINT8 *pByte, INT32 length)
{
    UINT8 seed1, seed2, seed3;
    INT32 i;
    UINT8 temp1, temp2;
    UINT8 temp3, temp4;
    UINT8 temp5, temp6;
    UINT8 tt;

    if (length > 4)
    {
        pByte[1] = (pByte[1] + pByte[0]) & 0xff;
        pByte[length - 2] = (pByte[length - 2] + pByte[length - 1]) & 0xff;
        cmN_arraBye_shift_inc((INT8 *)(pByte + 1), (length - 2), (INT32)(((pByte[0] + pByte[length - 1])) * 19) & 0xff);
    }

    tt = 0x98;

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ tt;
        tt++;
    }

    seed1 = 0xee;
    seed2 = 0x5A;
    seed3 = 0x63;

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ seed1;
    }

    if (length > 2)
    {
        for (i = 0; i < length / 2; i++)
        {
            //1234 to 41 23
            temp1 = (*(pByte + i)) & 0xF0;
            temp2 = (*(pByte + i)) & 0x0F;
            temp3 = (*(pByte + i + (length / 2))) & 0xF0;
            temp4 = (*(pByte + i + (length / 2))) & 0x0F;
            (*(pByte + i)) = (temp4 << 4) | (temp1 >> 4);
            (*(pByte + i + (length / 2))) = (temp2 << 4) | (temp3 >> 4);
        }
    }

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ seed2;
    }

    tt = 0x6c;

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ tt;
        tt += 0xf9;
    }

    if (length > 3)
    {
        for (i = 0; i < length / 3; i++)
        {
            //123456 to 54 16 32
            temp1 = (*(pByte + i)) & 0xF0;
            temp2 = (*(pByte + i)) & 0x0F;
            temp3 = (*(pByte + i + (length / 3) * 1)) & 0xF0;
            temp4 = (*(pByte + i + (length / 3) * 1)) & 0x0F;
            temp5 = (*(pByte + i + (length / 3) * 2)) & 0xF0;
            temp6 = (*(pByte + i + (length / 3) * 2)) & 0x0F;
            (*(pByte + i)) = (temp5) | (temp4);
            (*(pByte + i + (length / 3) * 1)) = (temp1) | (temp6);
            (*(pByte + i + (length / 3) * 2)) = (temp3) | (temp2);
        }
    }

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ seed3;
    }
}
#endif

static void cmN_arraBye_shift_dec(INT8 *pChar, INT32 byteLen, INT32 bitShift)
{
    //limit 1024 byte
    UINT8 *pBye;
    INT32 i;
    INT32 offByte, offBit;
    ULONG tempDW;
    UINT8 *pBAck = kmalloc(1024, GFP_KERNEL);
    pBye = (UINT8*)pChar;

    if (pBAck == NULL)
    {
        return;
    }
    if (byteLen > 1024)
    {
        kfree(pBAck) ;
        return;
    }

    offByte = bitShift / 8;
    offByte = offByte % byteLen;
    offBit = bitShift & 0x07;//%8

    if (offByte)
    {
        memcpy(pBAck, pBye, byteLen);

        for (i = 0; i < byteLen ; i++)
        {
            if ((i + offByte) < byteLen)
            {
                pBye[i] = pBAck[i + offByte];
            }
            else
            {
                pBye[i] = pBAck[i + offByte - byteLen];
            }
        }
    }

    if (offBit)
    {
        memcpy(pBAck, pBye, byteLen);

        for (i = 0; i < byteLen ; i++)
        {
            if (i == byteLen - 1)
            {
                tempDW = (((UINT32)pBAck[0]) << 8) + pBAck[byteLen - 1];
            }
            else
            {
                tempDW = (((UINT32)pBAck[i + 1]) << 8) + pBAck[i];
            }

            tempDW = tempDW >> offBit;
            pBye[i] = (UINT8)tempDW;
        }
    }
    kfree(pBAck) ;
}

static void byteDecryption(UINT8 *pByte, INT32 length)
{
    UINT8 seed1, seed2, seed3;
    INT32 i;
    UINT8 temp1, temp2;
    UINT8 temp3, temp4;
    UINT8 temp5, temp6;
    UINT8 tt;
    seed1 = 0xee;
    seed2 = 0x5A;
    seed3 = 0x63;

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ seed3;
    }

    if (length > 3)
    {
        for (i = 0; i < length / 3; i++)
        {
            //123456 to 54 16 32
            temp5 = (*(pByte + i)) & 0xF0;
            temp4 = (*(pByte + i)) & 0x0F;
            temp1 = (*(pByte + i + (length / 3) * 1)) & 0xF0;
            temp6 = (*(pByte + i + (length / 3) * 1)) & 0x0F;
            temp3 = (*(pByte + i + (length / 3) * 2)) & 0xF0;
            temp2 = (*(pByte + i + (length / 3) * 2)) & 0x0F;
            (*(pByte + i)) = (temp1) | (temp2);
            (*(pByte + i + (length / 3) * 1)) = (temp3) | (temp4);
            (*(pByte + i + (length / 3) * 2)) = (temp5) | (temp6);
        }
    }

    tt = 0x6c;

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ tt;
        tt += 0xf9;
    }

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ seed2;
    }

    if (length > 2)
    {
        for (i = 0; i < length / 2; i++)
        {
            //1234 to 41 23
            temp4 = (*(pByte + i)) & 0xF0;
            temp1 = (*(pByte + i)) & 0x0F;
            temp2 = (*(pByte + i + (length / 2))) & 0xF0;
            temp3 = (*(pByte + i + (length / 2))) & 0x0F;
            (*(pByte + i)) = (temp1 << 4) | (temp2 >> 4);
            (*(pByte + i + (length / 2))) = (temp3 << 4) | (temp4 >> 4);
        }
    }

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ seed1;
    }

    tt = 0x98;

    for (i = 0; i < length; i++)
    {
        *(pByte + i) = (*(pByte + i)) ^ tt;
        tt++;
    }

    if (length > 4)
    {
        cmN_arraBye_shift_dec((INT8 *)(pByte + 1), (length - 2), (INT32)(((pByte[0] + pByte[length - 1])) * 19) & 0xff);
        pByte[length - 2] = (pByte[length - 2] - pByte[length - 1]) & 0xff;
        pByte[1] = (pByte[1] - pByte[0]) & 0xff;
    }
}

static void henx(UINT8* key_addr)
{
	UINT8* dk_addr = key_addr + 8;
	UINT8 xor_bytes[7] =
	{
		0x4a, 0x65, 0x67, 0x72, 0x6f, 0x65, 0x47
	};
	UINT8 map[40] =
	{
		17, 36, 12, 29, 21, 10, 18, 37, 31, 1 ,
		2, 23, 35, 7, 27, 39, 3, 16, 8, 33,
		28, 25, 6, 14, 13, 4, 0, 20, 30, 9 ,
		5, 32, 38, 11, 15, 26, 19, 22, 34, 24
	};
	UINT8 temp[280];
	INT32 i = 0, j = 0;

	for (i = 0; i < 40; i++)
	{
		for (j = 0; j < 7; j++)
		{
			temp[map[i] * 7 + j] = dk_addr[i * 7 + j] ^ xor_bytes[j];
		}
	}

	memcpy(dk_addr, temp, 280);
}

#ifdef CONFIG_SUPPORT_STORE_HDCP2X_TO_FLASH
static INT32 xcrypt_HDCP2X(UINT8 *in, UINT8 *out, UINT32 len)
{
	// input 1024-byte HDCP encoded data, output 898-byte HDCP key.
	UINT32 SAMPLE_KEY1_DW = 0x01234567;
	UINT32 SAMPLE_KEY2_DW = 0x89abcdef;
	INT32 i = 0;
	UINT8 pInSeed[sizeof(ULONG)];
	memcpy(pInSeed, &SAMPLE_KEY1_DW, sizeof(ULONG));

	for (i = 0 ; i < 36 ; i++)
	{
		*(out + i) = *(in + i) ^ pInSeed[i % 4];
	}

	memcpy(pInSeed, &SAMPLE_KEY2_DW, sizeof(ULONG));

	for (i = 36 ; i < 898 ; i++)
	{
		*(out + i) = *(in + i) ^ pInSeed[i % 4];
	}

	return 0;
}
#endif

static INT32 xcrypt(UINT8 *in, UINT8 *out, UINT32 len)
{
	// input 512-byte HDCP encoded data, output 288-byte HDCP key.
	UINT32 SAMPLE_KEY1_DW = 0x01234567;
	UINT32 SAMPLE_KEY2_DW = 0x89abcdef;
	INT32 i = 0;
	UINT8 pInSeed[sizeof(ULONG)];
	memcpy(pInSeed, &SAMPLE_KEY1_DW, sizeof(ULONG));

	for (i = 0 ; i < 5 ; i++)
	{
		*(out + i) = *(in + i) ^ pInSeed[i % 4];
	}

	memcpy(pInSeed, &SAMPLE_KEY2_DW, sizeof(ULONG));

	for (i = 8 ; i < 288 ; i++)
	{
		*(out + i) = *(in + i) ^ pInSeed[i % 4];
	}

	return 0;
}

UINT32 hdmi_get_hdcp_data(INT32 offset)
{
	UINT32 *data = (UINT32*)local_key_data;
	return data[offset];
}

BOOL DRV_HDMI_CheckHdcpKey(void)
{
	return bHDCP_CRC;
}

void DRV_HDMI_GetHDCPKeyKSV(UINT8 *ksv)
{
	memcpy(ksv, &local_ksv, 5);
}

void DRV_HDMI_UpdateHDCPKey(UINT8 *key_ptr)
{
	INT32 i;
	UINT32 calculated_crc;
	UINT8 *pbHdcpKey;
	HDCPKEY_FMT_PTR key;
#ifdef CONFIG_DISABLE_CIKEY_HDCPKEY_SPI_ENCRYPT
    UINT8 *pbHdcpKey_tmp;
#endif

	pbHdcpKey = kmalloc(sizeof(UINT8) * (HDCP_KEY_SIZE+HDCP_CRC_SIZE), GFP_KERNEL);
	spi_read_flash(pbHdcpKey, (UINT8*)key_ptr, (HDCP_KEY_SIZE+HDCP_CRC_SIZE));

#ifdef CONFIG_DISABLE_CIKEY_HDCPKEY_SPI_ENCRYPT
    pbHdcpKey_tmp = kmalloc((((HDCP_KEY_SIZE+HDCP_CRC_SIZE+15)>>4)<<4), GFP_KERNEL);
    memcpy((void *)pbHdcpKey_tmp, (void *)pbHdcpKey, (HDCP_KEY_SIZE+HDCP_CRC_SIZE));
    do_spi_encrypt((INT8 *)pbHdcpKey_tmp, (((HDCP_KEY_SIZE+HDCP_CRC_SIZE+15)>>4)<<4), 0);
    memcpy((void *)pbHdcpKey, (void *)pbHdcpKey_tmp, (HDCP_KEY_SIZE+HDCP_CRC_SIZE));
    kfree(pbHdcpKey_tmp);
#endif

	key = (HDCPKEY_FMT_PTR)pbHdcpKey;
	/* Input data is encrypted and decryption needed */
	calculated_crc = 0;
	for (i = 0; i < (HDCP_KEY_SIZE/4); i++)
	{
		calculated_crc += (UINT32)key->data[i];
	}

	bHDCP_CRC = (calculated_crc == key->crc) ? 1 : 0;
	byteDecryption(key->vkey, HDCP_VKEY_SIZE);
	byteDecryption(key->rkey, HDCP_RKEY_SIZE);
	bf_decrypt(key->ckey, HDCP_CKEY_SIZE, key->rkey, HDCP_RKEY_SIZE);
	xcrypt(key->ckey, key->ckey, HDCP_CKEY_SIZE);
	/* Update local ksv data */
	memcpy(local_ksv, key->ckey, 5);
	/* Update to local key data */
	memcpy(local_key_data, key->ckey, HDCP_RAWKEY_SIZE);

#ifdef SUPPORT_PRINT_HDCP_MESSAGE
	printk("hdcp raw key\n");
	dump(local_key_data, HDCP_RAWKEY_SIZE);
#endif
	
	/* Transform raw hdcp key into hw format */
	henx(local_key_data);

#ifdef SUPPORT_PRINT_HDCP_MESSAGE
	printk("henx\n");
	dump(local_key_data, HDCP_RAWKEY_SIZE);
#endif
    kfree(pbHdcpKey);
}

#ifdef CONFIG_SUPPORT_STORE_HDCP2X_TO_FLASH
void DRV_HDMI_UpdateHDCP2XKey(UINT8 *key_ptr)
{
	INT32 i;
	UINT32 calculated_crc;
	UINT8 *pbHdcpKey;
	HDCP2X_KEY_FMT_PTR key;
    HDCP2X_REV_KEY_SET Hdcp2X_Key;
#ifdef CONFIG_DISABLE_CIKEY_HDCPKEY_SPI_ENCRYPT
    UINT8 *pbHdcpKey_tmp;
#endif

	pbHdcpKey = kmalloc(sizeof(UINT8) * (HDCP2X_KEY_SIZE+HDCP2X_CRC_SIZE), GFP_KERNEL);
	spi_read_flash(pbHdcpKey, (UINT8*)key_ptr, (HDCP2X_KEY_SIZE+HDCP2X_CRC_SIZE));

#ifdef CONFIG_DISABLE_CIKEY_HDCPKEY_SPI_ENCRYPT
    pbHdcpKey_tmp = kmalloc((((HDCP2X_KEY_SIZE+HDCP2X_CRC_SIZE+15)>>4)<<4), GFP_KERNEL);
    memcpy((void *)pbHdcpKey_tmp, (void *)pbHdcpKey, (HDCP2X_KEY_SIZE+HDCP2X_CRC_SIZE));
    do_spi_encrypt((INT8 *)pbHdcpKey_tmp, (((HDCP2X_KEY_SIZE+HDCP2X_CRC_SIZE+15)>>4)<<4), 0);
    memcpy((void *)pbHdcpKey, (void *)pbHdcpKey_tmp, (HDCP2X_KEY_SIZE+HDCP2X_CRC_SIZE));
    kfree(pbHdcpKey_tmp);
#endif

	key = (HDCP2X_KEY_FMT_PTR)pbHdcpKey;
	/* Input data is encrypted and decryption needed */
	calculated_crc = 0;
	for (i = 0; i < (HDCP2X_KEY_SIZE/4); i++)
	{
		calculated_crc += (UINT32)key->data[i];
	}

	bHDCP_CRC = (calculated_crc == key->crc) ? 1 : 0;
	byteDecryption(key->vkey, HDCP2X_VKEY_SIZE);
	byteDecryption(key->rkey, HDCP2X_RKEY_SIZE);
	bf_decrypt(key->ckey, HDCP2X_CKEY_SIZE, key->rkey, HDCP2X_RKEY_SIZE);
	xcrypt_HDCP2X(key->ckey, key->ckey, HDCP2X_CKEY_SIZE);
	/* Update to local key data */
	memcpy(local_2Xkey_data, key->ckey, HDCP2X_RAWKEY_SIZE);

#ifdef SUPPORT_PRINT_HDCP_MESSAGE
	printk("hdcp2X raw key\n");
	dump(local_2Xkey_data, HDCP2X_RAWKEY_SIZE);
#endif
    Hdcp2X_Key.Header[0]=0x00;
    Hdcp2X_Key.Header[1]=0x00;
    Hdcp2X_Key.Header[2]=0x00;
    Hdcp2X_Key.Header[3]=0x02;
    memcpy(Hdcp2X_Key.LicConst, &(key->ckey[0]), HDCP2X_LIC_CONST_SIZE);
    memcpy(Hdcp2X_Key.PubCert, &(key->ckey[36]), HDCP2X_PUB_CERT_SIZE);
    memcpy(Hdcp2X_Key.PvtKey, &(key->ckey[558]), HDCP2X_PVT_KEY_SIZE);
#ifdef SUPPORT_PRINT_HDCP_MESSAGE
	printk("hdcp2X key header\n");
	dump(Hdcp2X_Key.Header, 4);
	printk("hdcp2X key Licensed Constant\n");
	dump(Hdcp2X_Key.LicConst, HDCP2X_LIC_CONST_SIZE);
	printk("hdcp2X key Public Certificate\n");
	dump(Hdcp2X_Key.PubCert, HDCP2X_PUB_CERT_SIZE);
	printk("hdcp2X key Private Key\n");
	dump(Hdcp2X_Key.PvtKey, HDCP2X_PVT_KEY_SIZE);
#endif
    kfree(pbHdcpKey);
}
#endif