#include <stdint.h>
#include <string.h>
#include "hdcp.h"
#include "dl_pkts.h"

static uint16_t out_hdcp_counter;
/* all outgoing packets in bytes - 4-8 - contains one of these values,
   4 seems to be used the most, for HDCP packets and data packets.
   4 seems to have a subpacket type */
#define MSG_TYPE_1 0x1
#define MSG_TYPE_2 0x2
#define MSG_TYPE_4 0x4

/* seems to be an initial 8 byte header on USB outgoing packets */
static void fill_hdcp_header1(uint8_t *buf, uint16_t length, uint32_t msg_type)
{
	buf[0] = 0;
	buf[1] = 0;

	buf[2] = (length & 0xff);
	buf[3] = (length >> 8) & 0xff;

	/* msg type (??) */
	buf[4] = msg_type & 0xff;
	buf[5] = 0;
	buf[6] = 0;
	buf[7] = 0;
}

/* header 2 values seen in traces */
/* MSG1 hdr2 values */
#define MSG1_0 0x0    /* null header - no more data */

/* MSG2 hdr2 values */
#define MSG2_04 0x04  /* appears first at init time - has 16 bytes following. */
#define MSG2_06 0x06  /* appears first at init time - has 16 bytes following. */
#define MSG2_25 0x25  /* appears first at init time - has 16 bytes following. */
/* hdr25 appears to have a sequence number following it. */
/* 4 appears in MSG2 and MSG4 headers */
#define MSG2_45 0x45 /* mostly seems to be input - sent once */

/* 4 in the header seems to have a subtype */
#define MSG4_PKT4  0x04 /* use in HDCP transactions, AKE etc */
#define MSG4_PKT6  0x06 /* use in HDCP data transactions with enc data */

#define MSG4_PKT25 0x25 /* used for replies */
#define MSG4_PKT45 0x45 /* used for replies */

/* MSG4 HDR2 4/25 - has a secondary length/type field at 0x16
   0x10 - non HDCP
   0x84 - HDCP msg
*/
/* seems to be a secondary 8 byte header on outgoing packets */
/* appears to get filled in for type 6 dwords */
static void fill_hdcp_header2(uint8_t *buf, uint32_t dword)
{
	int i;

	buf[8] = dword & 0xff;
	for (i = 9; i < 16; i++)
		buf[i] = 0x0;
}

void dl3_empty_packet(uint8_t *buf, int *len)
{
	memset(buf, 0, 16);
	fill_hdcp_header1(buf, 12, MSG_TYPE_1);
	fill_hdcp_header2(buf, MSG1_0);
	*len = 16;
}

void dl3_msg2_hdr(uint8_t *buf, int *len_p, int hdr_num,
		  uint16_t val1, uint16_t val2)
{
	int len = 0x1c;
	memset(buf, 0, len + 4);
	fill_hdcp_header1(buf, len, MSG_TYPE_2);
	fill_hdcp_header2(buf, hdr_num);

	buf[16] = val1 & 0xff;
	buf[18] = val2 & 0xff;

	*len_p = len + 4;
}

void dl3_msg4_hdr4(uint8_t *buf, uint16_t length, uint16_t type, uint16_t seq)
{
	buf[16] = (length & 0xff);
	buf[17] = (length >> 8) & 0xff;
	buf[18] = (type & 0xff);
	buf[19] = (type >> 8) & 0xff;
	buf[20] = (seq & 0xff);
	buf[21] = (seq >> 8) & 0xff;
}

int pkt4_mid_len(int hdcp_msg_len, int *mid_len)
{
	int total;
	total = hdcp_msg_len;
	total += 22;
	*mid_len = total;
	total += 14;
	return total;
}

void dl3_packet_hdcp_ake_init(uint8_t *buf, int *len_p, const uint8_t *Rtx)
{
	int ake_init_len = 8 + 1;
	int total_len = 0;
	int i;
	int mid_len;

	/* 9 bytes for AKE */
	total_len = pkt4_mid_len(ake_init_len, &mid_len);

	memset(buf, 0, ROUND_TO_4(total_len + 4));
	fill_hdcp_header1(buf, total_len, MSG_TYPE_4);
	fill_hdcp_header2(buf, MSG4_PKT4);

	dl3_msg4_hdr4(buf, mid_len, 0x10, out_hdcp_counter++);

	buf[38] = 0x30;
	buf[40] = HDCP_AKE_INIT;

	/* write Rtx */
	for (i = 0; i < 8; i++) {
		buf[41 + i] = Rtx[i];
	}

	*len_p = total_len + 4;
}

/* send an empty km for now */
void dl3_packet_hdcp_ake_no_stored_km(uint8_t *buf, int *len_p, const uint8_t *ekpubkm)
{
	int nsk_len = 128 + 1;
	int total_len, mid_len, i;

	total_len = pkt4_mid_len(nsk_len, &mid_len);

	memset(buf, 0, ROUND_TO_4(total_len + 4));
	fill_hdcp_header1(buf, total_len, MSG_TYPE_4);
	fill_hdcp_header2(buf, MSG4_PKT4);

	dl3_msg4_hdr4(buf, mid_len, 0x10, out_hdcp_counter++);

	buf[38] = 0x30;
	buf[40] = HDCP_AKE_NO_STORED_KM;

	for (i = 0; i < 128; i++) {
	  buf[41 + i] = ekpubkm[i];
	}
	*len_p = total_len + 4;
}

void dl3_packet_hdcp_lc_init(uint8_t *buf, int *len_p, const uint8_t *Rn)
{
	int lc_init_len = 8 + 1;
	int total_len = 0;
	int i;
	int mid_len;

	/* 9 bytes for AKE */
	total_len = pkt4_mid_len(lc_init_len, &mid_len);

	memset(buf, 0, ROUND_TO_4(total_len + 4));
	fill_hdcp_header1(buf, total_len, MSG_TYPE_4);
	fill_hdcp_header2(buf, MSG4_PKT4);

	dl3_msg4_hdr4(buf, mid_len, 0x10, out_hdcp_counter++);

	buf[38] = 0x30;
	buf[40] = HDCP_LC_INIT;

	/* write Rtx */
	for (i = 0; i < 8; i++) {
		buf[41 + i] = Rn[i];
	}

	*len_p = total_len + 4;
}

void dl3_packet_hdcp_ske_send_eks(uint8_t *buf, int *len_p, const uint8_t *edkey_ks, const uint8_t *riv)
{
	int ske_send_eks_len = 24 + 1;
	int total_len = 0;
	int i;
	int mid_len;

	/* 24 bytes for ske send eks */
	total_len = pkt4_mid_len(ske_send_eks_len, &mid_len);

	memset(buf, 0, ROUND_TO_4(total_len + 4));
	fill_hdcp_header1(buf, total_len, MSG_TYPE_4);
	fill_hdcp_header2(buf, MSG4_PKT4);

	dl3_msg4_hdr4(buf, mid_len, 0x10, out_hdcp_counter++);

	buf[38] = 0x30;
	buf[40] = HDCP_SKE_SEND_EKS;

	/* write Rtx */
	for (i = 0; i < 16; i++) {
		buf[41 + i] = edkey_ks[i];
	}
	for (i = 0; i < 8; i++) {
		buf[41 + 16 + i] = riv[i];
	}

	*len_p = total_len + 4;
}

void dl3_packet_send_enc_data(uint8_t *buf, int *len_p, const uint8_t *enc_data)
{
	int total_len = 0;

	total_len = 48 + 12;

	memset(buf, 0, ROUND_TO_4(total_len + 4));
	fill_hdcp_header1(buf, total_len, MSG_TYPE_4);
	fill_hdcp_header2(buf, MSG4_PKT6);

	buf[10] = 0xa;

	memcpy(&buf[16], enc_data, 48);
	*len_p = total_len + 4;
}