dl3: add encryption for km first (uses gcrypt)

4 files changed, 109 insertions, 68 deletions

diff --git a/Makefile b/Makefile
index 3fd0315..02ee03f 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,10 @@
-CFLAGS := -g3 -Wall -O2 `pkg-config libusb-1.0 --cflags` `pkg-config openssl --cflags`
-LIBS := `pkg-config libusb-1.0 --libs` `pkg-config openssl --libs`
+CFLAGS := -g3 -Wall -O2 `pkg-config libusb-1.0 --cflags` `pkg-config openssl --cflags` `libgcrypt-config --cflags`
+LIBS := `pkg-config libusb-1.0 --libs` `pkg-config openssl --libs` `libgcrypt-config --libs`
 
 dl3: dl3.c dl3_hdcp.c
 	$(CC) $(CFLAGS) -o $@ dl3.c dl3_hdcp.c $(LIBS)
 
 all: dl3
+
+clean:
+	rm dl3
diff --git a/dl3.c b/dl3.c
index ddde8b3..df09865 100644
--- a/dl3.c
+++ b/dl3.c
@@ -15,74 +15,81 @@
 #include <openssl/sha.h>
 #include <openssl/rand.h>
 
-static int MGF1_SHA256(unsigned char *mask, long len, const unsigned char *seed, long seedlen)
-{
-    return PKCS1_MGF1(mask, len, seed, seedlen, EVP_sha256());
-}
+#include <gcrypt.h>
+
+static int VID = 0x17e9;
+static int PID = 0x4301;
+
+/* make km all 0s for now */
+static unsigned char mykm[16] = { 0xcd, 0xef, 0x65, 0x33, 0x69, 0x23, 0xfa, 0x3e,
+				0x60, 0xee, 0xdd, 0x5c, 0xce, 0xfb, 0x39, 0x19 };
 
-static int RSA_padding_add_PKCS1_OAEP_SHA256(unsigned char *to, int tlen,
-                                             const unsigned char *from, int flen,
-                                             const unsigned char *param, int plen)
+static int create_gcrypt_pubkey(gcry_sexp_t *pub_key,
+				const uint8_t *n,
+				const uint8_t *e,
+				int n_len, int e_len)
 {
-    int i, emlen = tlen - 1;
-    unsigned char *db, *seed;
-    unsigned char *dbmask, seedmask[SHA256_DIGEST_LENGTH];
-
-    if (flen > emlen - 2 * SHA256_DIGEST_LENGTH - 1)
-    {
-        RSAerr(RSA_F_RSA_PADDING_ADD_PKCS1_OAEP,
-            RSA_R_DATA_TOO_LARGE_FOR_KEY_SIZE);
-        return 0;
-    }
-
-    if (emlen < 2 * SHA256_DIGEST_LENGTH + 1)
-    {
-        RSAerr(RSA_F_RSA_PADDING_ADD_PKCS1_OAEP, RSA_R_KEY_SIZE_TOO_SMALL);
-        return 0;
-    }
-
-    to[0] = 0;
-    seed = to + 1;
-    db = to + SHA256_DIGEST_LENGTH + 1;
-
-    if (!EVP_Digest((void *)param, plen, db, NULL, EVP_sha256(), NULL))
-        return 0;
-    memset(db + SHA256_DIGEST_LENGTH, 0,
-        emlen - flen - 2 * SHA256_DIGEST_LENGTH - 1);
-    db[emlen - flen - SHA256_DIGEST_LENGTH - 1] = 0x01;
-    memcpy(db + emlen - flen - SHA256_DIGEST_LENGTH, from, (unsigned int) flen);
-    if (RAND_bytes(seed, SHA256_DIGEST_LENGTH) <= 0)
-        return 0;
-
-    dbmask = (unsigned char*)OPENSSL_malloc(emlen - SHA256_DIGEST_LENGTH);
-    if (dbmask == NULL)
-    {
-        RSAerr(RSA_F_RSA_PADDING_ADD_PKCS1_OAEP, ERR_R_MALLOC_FAILURE);
-        return 0;
-    }
-
-    if (MGF1_SHA256(dbmask, emlen - SHA256_DIGEST_LENGTH, seed, SHA256_DIGEST_LENGTH) < 0)
-        return 0;
-    for (i = 0; i < emlen - SHA256_DIGEST_LENGTH; i++)
-        db[i] ^= dbmask[i];
-
-    if (MGF1_SHA256(seedmask, SHA256_DIGEST_LENGTH, db, emlen - SHA256_DIGEST_LENGTH) < 0)
-        return 0;
-    for (i = 0; i < SHA256_DIGEST_LENGTH; i++)
-        seed[i] ^= seedmask[i];
-
-    OPENSSL_free(dbmask);
-    return 1;
+	char buf[65536];
+	char temp[10];
+	int ret;
+	int i;
+
+	buf[0] = 0;
+	strcat(buf, "(public-key\n");
+	strcat(buf, " (rsa\n");
+	strcat(buf, "  (n #");
+
+	for (i = 0; i < n_len; i++) {
+		sprintf(temp,"%02x", n[i]);
+		strcat(buf, temp);
+	}
+	strcat(buf, "#)\n");
+	strcat(buf, "  (e #");
+	for (i = 0; i < e_len; i++) {
+		sprintf(temp,"%02x", e[i]);
+		strcat(buf, temp);
+	}
+	strcat(buf, "#)\n");
+	strcat(buf, " )\n");
+	strcat(buf, ")\n");
+
+	printf("pub key %s\n", buf);
+
+	ret = gcry_sexp_sscan(pub_key, NULL, buf, strlen(buf));
+	if (ret != 0)
+		return -1;
+	return 0;
 }
 
-static int VID = 0x17e9;
-static int PID = 0x4301;
+static int create_gcrypt_km(gcry_sexp_t *km,
+			    const uint8_t *km_str,
+			    int km_len)
+{
+	char buf[65536];
+	char temp[10];
+	int i, ret;
+	buf[0] = 0;
+
+	strcat(buf, "(data\n");
+	strcat(buf, "    (flags oaep)\n");
+	strcat(buf, "    (hash-algo sha256)\n");
+	strcat(buf, "    (labal \"km\")\n");
+	strcat(buf, "    (value #");
+	for (i = 0; i < km_len; i++) {
+		sprintf(temp,"%02x", km_str[i]);
+		strcat(buf, temp);
+	}
+	strcat(buf, "#))\n");
 
+	printf("km buf %s\n", buf);
 
-/* make km all 0s for now */
-static unsigned char km[16];
+	ret = gcry_sexp_sscan(km, NULL, buf, strlen(buf));
+	if (ret != 0)
+		return -1;
+	return 0;
+}
 
-static void decode_cert(const unsigned char *buf, int len)
+static void decode_cert(const unsigned char *buf, int len, gcry_sexp_t *pub_key)
 {
 	const unsigned char *start = buf + (len - 522);
 	BIGNUM *e, *m;
@@ -124,6 +131,14 @@ static void decode_cert(const unsigned char *buf, int len)
 	}
 	printf("\n");
 
+	ret = create_gcrypt_pubkey(pub_key,
+				   &start[5],
+				   &start[133],
+				   128, 3);
+	if (ret != 0) {
+		fprintf(stderr, "failed to create pubkey %d\n", ret);
+	}
+
 	ctx = EVP_PKEY_CTX_new(pRsaKey, NULL);
 	if (EVP_PKEY_verify_init(ctx) <= 0) {
 		printf("fail 1\n");
@@ -193,6 +208,7 @@ static int send_buffer(libusb_device_handle *handle, int endpoint_out, int endpo
 	int r, i = 0;
 	int size;
 	int len2, len;
+	gcry_sexp_t pub_key, km, result;
 
 	dl3_empty_packet(buf, &len);
 	r = write_to_usb(handle, endpoint_out, buf, len, &size);
@@ -231,9 +247,28 @@ static int send_buffer(libusb_device_handle *handle, int endpoint_out, int endpo
 	}
 	printf("}\n");
 
-	decode_cert(buf, size);
+	decode_cert(buf, size, &pub_key);
+
+	r = create_gcrypt_km(&km, mykm, 16);
+	if (r != 0)
+		fprintf(stderr, "failed to create km\n");
+
+	r = gcry_pk_encrypt(&result, km, pub_key);
+
+	GCRY_SEXP list = gcry_sexp_find_token( result, "a", 0);
+
+	gcry_mpi_t out_msg = gcry_sexp_nth_mpi(list, 1, GCRYMPI_FMT_USG);
+
+	gcry_mpi_dump(out_msg);
+
+	unsigned char obuf[1024] = { 0 };
+	r = gcry_mpi_print(GCRYMPI_FMT_USG, (unsigned char*) &obuf,
+			     sizeof(obuf), &size, out_msg);
+	if (r) {
+		printf("failed to stringify mpi");
+	}
 
-	dl3_packet_hdcp_ake_no_stored_km(buf, &len);
+	dl3_packet_hdcp_ake_no_stored_km(buf, &len, obuf);
 	len = ROUND_TO_4(len);
 	r = write_to_usb(handle, endpoint_out, buf, len, &size);
 	if (r != 0)
diff --git a/dl3_hdcp.c b/dl3_hdcp.c
index 811b43f..e3a3bb1 100644
--- a/dl3_hdcp.c
+++ b/dl3_hdcp.c
@@ -142,10 +142,10 @@ void dl3_packet_hdcp_ake_init(uint8_t *buf, int *len_p)
 }
 
 /* send an empty km for now */
-void dl3_packet_hdcp_ake_no_stored_km(uint8_t *buf, int *len_p)
+void dl3_packet_hdcp_ake_no_stored_km(uint8_t *buf, int *len_p, uint8_t *ekpubkm)
 {
 	int nsk_len = 128 + 1;
-	int total_len, mid_len;
+	int total_len, mid_len, i;
 
 	total_len = pkt4_mid_len(nsk_len, &mid_len);
 
@@ -158,5 +158,8 @@ void dl3_packet_hdcp_ake_no_stored_km(uint8_t *buf, int *len_p)
 	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;
 }
diff --git a/dl_pkts.h b/dl_pkts.h
index a4c3bb9..fa8a685 100644
--- a/dl_pkts.h
+++ b/dl_pkts.h
@@ -6,7 +6,7 @@ void dl3_empty_packet(uint8_t *buf, int *len);
 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);
-void dl3_packet_hdcp_ake_no_stored_km(uint8_t *buf, int *len_p);
 
+void dl3_packet_hdcp_ake_no_stored_km(uint8_t *buf, int *len_p, uint8_t *ekpubkm);
 #define ROUND_TO_4(x) ((((x) + 3) / 4) * 4)
 #endif