add LC support

next time to add SKE

3 files changed, 121 insertions, 2 deletions

diff --git a/dl3.c b/dl3.c
index 6b4a362..7f3874e 100644
--- a/dl3.c
+++ b/dl3.c
@@ -23,8 +23,12 @@
 struct hdcp_session_info {
 	gcry_sexp_t pub_key;
 	uint8_t calc_h[32];
+	uint8_t calc_l[32];
+	uint8_t rrx[8];
 	bool h_matches;
+	bool l_matches;
 	bool is_repeater;
+	bool got_h_prime;
 };
 static int VID = 0x17e9;
 static int PID = 0x4301;
@@ -35,6 +39,8 @@ static unsigned char mykm[16] = { 0xcd, 0xef, 0x65, 0x33, 0x69, 0x23, 0xfa, 0x3e
 
 const uint8_t Rtx[] = { 0x84, 0xd7, 0xcf, 0x64, 0x2d, 0x68, 0xff, 0xd5 };
 
+const uint8_t lc_Rn[] = { 0xd7, 0xfa, 0xf6, 0x99, 0x14, 0xd2, 0x51, 0xe0 };
+
 static int create_gcrypt_pubkey(struct hdcp_session_info *info,
 				const uint8_t *n,
 				const uint8_t *e,
@@ -174,6 +180,35 @@ static int calculate_h(struct hdcp_session_info *info)
 	memcpy(info->calc_h, res, resLen);
 }
 
+static int calculate_l(struct hdcp_session_info *info)
+{
+	HMAC_CTX ctx;
+	unsigned char res[200];
+	unsigned int resLen;
+	int i;
+	uint8_t input[8];
+	unsigned char *kdbuf;
+
+	kdbuf = kd(mykm, Rtx);
+
+	for (i = 0; i < 8; i++) {
+		kdbuf[24 + i] ^= info->rrx[i];
+	}
+	memcpy(input, lc_Rn, 8);
+
+	HMAC_CTX_init(&ctx);
+	HMAC_Init_ex(&ctx, kdbuf, KD_SIZE, EVP_sha256(), NULL);
+	HMAC_Update(&ctx, input, 8);
+	HMAC_Final(&ctx, res, &resLen);
+
+	HMAC_CTX_cleanup(&ctx);
+
+	if (resLen != 32)
+		return -1;
+
+	memcpy(info->calc_l, res, resLen);
+}
+
 static void decode_cert(struct hdcp_session_info *info, const uint8_t *buf, int len)
 {
 	const uint8_t *start = buf + 2;
@@ -256,6 +291,13 @@ static void decode_cert(struct hdcp_session_info *info, const uint8_t *buf, int
 	printf("ret is %d\n", ret);
 }
 
+static void decode_rrx(struct hdcp_session_info *info,
+		       const uint8_t *buf, int len)
+{
+	const uint8_t *start = buf + 1;
+	memcpy(info->rrx, start, 8);
+}
+
 static void decode_h_prime(struct hdcp_session_info *info,
 			   const uint8_t *buf, int len)
 {
@@ -279,6 +321,34 @@ static void decode_h_prime(struct hdcp_session_info *info,
 
 }
 
+static void decode_l_prime(struct hdcp_session_info *info,
+			   const uint8_t *buf, int len)
+{
+	const uint8_t *start = buf + 1;
+	int i;
+
+	if (!memcmp(info->calc_l, start, 32)) {
+		info->l_matches = true;
+		printf("L values matched\n");
+		return;
+	} else {
+		info->l_matches = false;
+		printf("L values failed to match\n");
+	}
+	printf("l: ");
+	for (i = 0; i < 32; i++) {
+		printf("%02x", info->calc_l[i]);
+	}
+	printf("\n");
+	printf("l_prime: ");
+	for (i = 0; i < 32; i++) {
+		printf("%02x", start[i]);
+	}
+
+	printf("\n");
+
+}
+
 static void decode_rx(struct hdcp_session_info *info,
 		      const uint8_t *buf, int len)
 {
@@ -301,7 +371,7 @@ static void decode_rx(struct hdcp_session_info *info,
 		decode_cert(info, buf + 22, len - 22);
 		break;
 	case HDCP_AKE_SEND_RRX:
-		printf("send rrx\n");
+		decode_rrx(info, buf + 22, len - 22);
 		break;
 	case HDCP_AKE_SEND_H_PRIME:
 		decode_h_prime(info, buf + 22, len - 22);
@@ -309,6 +379,9 @@ static void decode_rx(struct hdcp_session_info *info,
 	case HDCP_AKE_SEND_PAIRING_INFO:
 		printf("send pairing info\n");
 		break;
+	case HDCP_LC_SEND_L_PRIME:
+		decode_l_prime(info, buf + 22, len - 22);
+		break;
 	}
 
 }
@@ -424,10 +497,28 @@ static int send_buffer(libusb_device_handle *handle, int endpoint_out, int endpo
 	do {
 		r = block_read_usb(handle, endpoint_in, buf, &size);	
 		if (r < 0)
-			return r;
+			break;
 		decode_rx(&info, buf, size);
 		/* find H' */
 	} while (1);
+
+	if (!info.h_matches)
+		return -1;
+
+	/* send LC prime */
+	dl3_packet_hdcp_lc_init(buf, &len, lc_Rn);
+	r = write_to_usb(handle, endpoint_out, buf, len, &size);
+	if (r != 0)
+		return r;
+
+	calculate_l(&info);
+	do {
+		r = block_read_usb(handle, endpoint_in, buf, &size);
+		if (r < 0)
+			break;
+		decode_rx(&info, buf, size);
+		/* find L' */
+	} while (1);
 	return 0;
 }
 
diff --git a/dl3_hdcp.c b/dl3_hdcp.c
index 1668c6f..769e406 100644
--- a/dl3_hdcp.c
+++ b/dl3_hdcp.c
@@ -162,3 +162,30 @@ void dl3_packet_hdcp_ake_no_stored_km(uint8_t *buf, int *len_p, uint8_t *ekpubkm
 	}
 	*len_p = total_len + 4;
 }
+
+void dl3_packet_hdcp_lc_init(uint8_t *buf, int *len_p, 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, 0);
+
+	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;
+}
diff --git a/dl_pkts.h b/dl_pkts.h
index c8fcfae..75f705f 100644
--- a/dl_pkts.h
+++ b/dl_pkts.h
@@ -7,5 +7,6 @@ void dl3_msg2_hdr25(uint8_t *buf, int *len_p);
 void dl3_msg2_hdr04(uint8_t *buf, int *len_p);
 void dl3_packet_hdcp_ake_init(uint8_t *buf, int *len_p, uint8_t *Rtx);
 void dl3_packet_hdcp_ake_no_stored_km(uint8_t *buf, int *len_p, uint8_t *ekpubkm);
+void dl3_packet_hdcp_lc_init(uint8_t *buf, int *len_p, uint8_t *Rn);
 #define ROUND_TO_4(x) ((((x) + 3) / 4) * 4)
 #endif