path: root/dl3.c

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>

#include "dl_pkts.h"
#include "libusb.h"

#include "pkts.h"
#include "hdcp.h"
#include <openssl/bn.h>
#include <openssl/rsa.h>
#include <openssl/evp.h>
#include <openssl/err.h>
#include <openssl/sha.h>
#include <openssl/rand.h>

#include <gcrypt.h>

struct hdcp_session_info {
	gcry_sexp_t pub_key;
};
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 create_gcrypt_pubkey(struct hdcp_session_info *info,
				const uint8_t *n,
				const uint8_t *e,
				int n_len, int e_len)
{
	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(&info->pub_key, NULL, buf, strlen(buf));
	if (ret != 0)
		return -1;
	return 0;
}

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);

	ret = gcry_sexp_sscan(km, NULL, buf, strlen(buf));
	if (ret != 0)
		return -1;
	return 0;
}

static void decode_cert(struct hdcp_session_info *info, const uint8_t *buf, int len)
{
	const uint8_t *start = buf + 2;
	BIGNUM *e, *m;
	int i, ret;
	EVP_PKEY *pRsaKey = EVP_PKEY_new();
	EVP_PKEY_CTX *ctx;
	EVP_MD_CTX *mdctx = NULL;

	RSA *rsa = RSA_new();
	printf("recv id: %02x %02x %02x %02x %02x\n",
	       start[0], start[1], start[2], start[3], start[4]);

	printf("rect pub key:\n");

	m = BN_bin2bn(ddccert_m, 384, NULL);

	e = BN_bin2bn(ddccert_e, 1, NULL);

	rsa->n = m;
	rsa->e = e;

	EVP_PKEY_assign_RSA(pRsaKey, rsa);

	for (i = 5; i < 5 + 128; i++) {
		printf("%02x", start[i]);
	}
	printf("\n");
	for (i = 133; i < 133 + 3; i++) {
		printf("%02x", start[i]);
	}
	printf("\n");
	printf("proto descrip + rsvd: \n");
	for (i = 136; i < 136 + 2; i++) {
		printf("%02x", start[i]);
	}
	printf("\nllc sig: \n");
	for (i = 138; i < 136 + 384; i++) {
		printf("%02x", start[i]);
	}
	printf("\n");

	if (info) {
	  ret = create_gcrypt_pubkey(info,
				     &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");
		return;
	}

	mdctx = EVP_MD_CTX_create();
	EVP_DigestVerifyInit(mdctx, NULL, EVP_sha256(), NULL, pRsaKey);

	EVP_DigestVerifyUpdate(mdctx, start, 138);
	ret = EVP_DigestVerifyFinal(mdctx, start + 138, 384);
#if 0
	if (EVP_PKEY_CTX_set_rsa_padding(ctx, RSA_PKCS1_PADDING) <= 0) {
		printf("fail passing\n");
		return;
	}
#if 1
	if (EVP_PKEY_CTX_set_signature_md(ctx, EVP_sha256()) <= 0) {
		printf("fail sig\n");
		return;
	}
#endif	
	ret = EVP_PKEY_verify(ctx, start + 138, 384, start, 138);
#endif
	printf("ret is %d\n", ret);
}

static void decode_h_prime(struct hdcp_session_info *info,
			   const uint8_t *buf, int len)
{
	const uint8_t *start = buf + 1;
	int i;

	printf("h_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)
{
	/**/
	int msglen = buf[2] | (buf[3] << 8);
	int midlen;

	if (buf[0] != 0 || buf[1] != 0)
		return;

	if (buf[4] != 0x4 || buf[7] != 0x80)
		return;

	/* hdcp packet */
	if (buf[18] != 0x84 && buf[20] != 0x30)
		return;

	switch (buf[22]) {
	case HDCP_AKE_SEND_CERT:
		decode_cert(info, buf + 22, len - 22);
		break;
	case HDCP_AKE_SEND_RRX:
		printf("send rrx\n");
		break;
	case HDCP_AKE_SEND_H_PRIME:
		decode_h_prime(info, buf + 22, len - 22);
		break;
	case HDCP_AKE_SEND_PAIRING_INFO:
		printf("send pairing info\n");
		break;
	}

}

static int write_to_usb(libusb_device_handle *handle, int endpoint_out,
			const uint8_t *buf, int len, int *size)
{
	int i = 0, r;
  	do {
		r = libusb_bulk_transfer(handle, endpoint_out, buf, len, size, 1000);
		if (r == LIBUSB_ERROR_PIPE)
			libusb_clear_halt(handle, endpoint_out);
		i++;
	} while ((r == LIBUSB_ERROR_PIPE) && (i < 5));

	printf("tx: %d:", *size);
	for (i = 0; i < *size; i++) {
		printf("%02x ", buf[i]);
	}
	printf("\n");
	return r;
}

static int block_read_usb(libusb_device_handle *handle, int endpoint_in,
			  uint8_t *buf, int *size)
{
	int r, i;

	r = libusb_bulk_transfer(handle, endpoint_in, buf, 1024, size, 1000);
	if (r < 0) {
		printf("libusb_bulk_transfer failed: %s\n", libusb_error_name(r));
	}
	printf("rx: %d:", *size);
	for (i = 0; i < *size; i++) {
		printf("%02x ", buf[i]);
	}
	printf("\n");
	return r;
}

static int send_buffer(libusb_device_handle *handle, int endpoint_out, int endpoint_in)
{
	unsigned char buf[1024];
	int r, i = 0;
	int size;
	int len2, len;
	gcry_sexp_t pub_key, km, result;
	struct hdcp_session_info info;

	memset(&info, 0, sizeof(info));

	dl3_empty_packet(buf, &len);
	r = write_to_usb(handle, endpoint_out, buf, len, &size);
	if (r != 0)
		return r;

	dl3_msg2_hdr25(buf, &len);
	r = write_to_usb(handle, endpoint_out, buf, len, &size);
	if (r != 0)
		return r;

	memset(buf, 0, 1024);
	dl3_msg2_hdr04(buf, &len);
	len2 = len;
	dl3_packet_hdcp_ake_init(&buf[len], &len);
	len2 += len;
	len2 = ROUND_TO_4(len2);
	r = write_to_usb(handle, endpoint_out, buf, len2, &size);
	if (r != 0)
		return r;

	r = block_read_usb(handle, endpoint_in, buf, &size);
	if (r < 0)
		return r;

	decode_rx(&info, buf, size);

	r = block_read_usb(handle, endpoint_in, buf, &size);	
	if (r < 0)
		return r;

	decode_rx(&info, buf, size);

	r = create_gcrypt_km(&km, mykm, 16);
	if (r != 0)
		fprintf(stderr, "failed to create km\n");

	r = gcry_pk_encrypt(&result, km, info.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, obuf);
	len = ROUND_TO_4(len);
	r = write_to_usb(handle, endpoint_out, buf, len, &size);
	if (r != 0)
		return r;

	do {
		r = block_read_usb(handle, endpoint_in, buf, &size);	
		if (r < 0)
			return r;
		decode_rx(&info, buf, size);
		/* find H' */
	} while (1);
	return 0;
}

int test_device(uint16_t vid, uint16_t pid)
{
	libusb_device_handle *handle;
	libusb_device *dev;
	uint8_t bus;
	int r;
	struct libusb_config_descriptor *conf_desc;
	int nb_ifaces;
	int iface;

	handle = libusb_open_device_with_vid_pid(NULL, vid, pid);
	if (handle == NULL) {
		return -1;
	}

	dev = libusb_get_device(handle);
	bus = libusb_get_bus_number(dev);

	r = libusb_get_config_descriptor(dev, 0, &conf_desc);
	if (r < 0)
		return -1;

	nb_ifaces = conf_desc->bNumInterfaces;
	libusb_free_config_descriptor(conf_desc);
	for (iface = 0; iface < nb_ifaces; iface++)
	{
		printf("\nClaiming interface %d...\n", iface);
		r = libusb_claim_interface(handle, iface);
		if (r != LIBUSB_SUCCESS) {
			perror("   Failed.\n");
		}
	}

	printf("num interfaces %d\n", nb_ifaces);

	
				
	send_buffer(handle, 0x02, 0x84);
	printf("\n");
	for (iface = 0; iface<nb_ifaces; iface++) {
		printf("Releasing interface %d...\n", iface);
		libusb_release_interface(handle, iface);
	}



	libusb_close(handle);
	return 0;
}

int main(int argc, char** argv)
{
	int r;

	//	decode_cert(NULL, dplinkcert, 546);
#if 1
	r = libusb_init(NULL);
	if (r < 0)
		return r;
		libusb_set_debug(NULL, LIBUSB_LOG_LEVEL_DEBUG);
	test_device(VID, PID);

	libusb_exit(NULL);
#endif
}