cc3dsfs/source/CaptureDeviceSpecific/usb_ds_3ds_capture.cpp

#include "usb_ds_3ds_capture.hpp"
#include "devicecapture.hpp"
#include "usb_generic.hpp"

#include <libusb.h>
#include <cstring>
#include <thread>
#include <chrono>
#include <iostream>

// Adapted from sample code provided by Loopy

#define REAL_SERIAL_NUMBER_SIZE 32
#define SERIAL_NUMBER_SIZE (REAL_SERIAL_NUMBER_SIZE+1)

#define FIRST_3D_VERSION 6
#define CAPTURE_SKIP_TIMEOUT_SECONDS 1.0

#define NUM_DS_3DS_CONCURRENT_DATA_BUFFERS 4
#define NUM_DS_3DS_CONCURRENT_REQUESTS 4

enum usb_capture_status {
	USB_CAPTURE_SUCCESS = 0,
	USB_CAPTURE_SKIP,
	USB_CAPTURE_PIPE_ERROR,
	USB_CAPTURE_FRAMEINFO_ERROR,
	USB_CAPTURE_ERROR
};

struct usb_device {
	bool is_3ds;
	int vid;
	int pid;
	int default_config;
	int capture_interface;
	int control_timeout;
	int bulk_timeout;
	int ep2_in;
	int cmdin_status;
	int cmdin_frameinfo;
	int cmdout_capture_start;
	int cmdout_capture_stop;
	int cmdin_i2c_read;
	int cmdout_i2c_write;
	int i2caddr_3dsconfig;
	int bitstream_3dscfg_ver;
};

struct usb_ds_status {
	uint32_t framecount;
	uint8_t lcd_on;
	uint8_t capture_in_progress;
};

struct usb_ds_3ds_general_data {
	SharedConsumerMutex* is_there_ready_buffer;
	SharedConsumerMutex* is_buffer_usage_done;
	uint32_t index;
	uint32_t* last_index;
	int real_index;
	bool in_use;
	CaptureData* capture_data;
	CaptureReceived capture_buf;
	size_t read_amount;
	double time_in;
};

static const usb_device usb_3ds_desc = {
.is_3ds = true,
.vid = 0x16D0, .pid = 0x06A3,
.default_config = 1, .capture_interface = 0,
.control_timeout = 30, .bulk_timeout = 50,
.ep2_in = 2 | LIBUSB_ENDPOINT_IN,
.cmdin_status = 0, .cmdin_frameinfo = 0,
.cmdout_capture_start = 0x40, .cmdout_capture_stop = 0,
.cmdin_i2c_read = 0x21, .cmdout_i2c_write = 0x21,
.i2caddr_3dsconfig = 0x14, .bitstream_3dscfg_ver = 1
};

static const usb_device usb_old_ds_desc = {
.is_3ds = false,
.vid = 0x16D0, .pid = 0x0647,
.default_config = 1, .capture_interface = 0,
.control_timeout = 500, .bulk_timeout = 500,
.ep2_in = 2 | LIBUSB_ENDPOINT_IN,
.cmdin_status = 0x31, .cmdin_frameinfo = 0x30,
.cmdout_capture_start = 0x30, .cmdout_capture_stop = 0x31,
.cmdin_i2c_read = 0, .cmdout_i2c_write = 0,
.i2caddr_3dsconfig = 0, .bitstream_3dscfg_ver = 0
};

static const usb_device* usb_devices_desc_list[] = {
	&usb_3ds_desc,
	&usb_old_ds_desc,
};

static void unlock_buffer(usb_ds_3ds_general_data* buffer_data) {
	buffer_data->in_use = false;
	buffer_data->is_buffer_usage_done->specific_unlock(buffer_data->real_index);
}

static void ds_3ds_usb_thread_function(bool* usb_thread_run, usb_ds_3ds_general_data* buffer_data) {
	if(!usb_is_initialized())
		return;
	while(*usb_thread_run) {
		int processing_index = *buffer_data[0].last_index;
		int dummy = 0;
		for(int i = 0; i < NUM_DS_3DS_CONCURRENT_DATA_BUFFERS; i++) {
			if(!buffer_data[i].in_use)
				continue;
			int diff = (int)(processing_index - buffer_data[i].index);
			if(diff < 0)
				unlock_buffer(&buffer_data[i]);
			else if(diff == 0) {
				buffer_data[i].capture_data->data_buffers.WriteToBuffer(&buffer_data[i].capture_buf, buffer_data[i].read_amount, buffer_data[i].time_in, &buffer_data[i].capture_data->status.device);
				if(buffer_data[i].capture_data->status.cooldown_curr_in)
					buffer_data[i].capture_data->status.cooldown_curr_in = buffer_data[i].capture_data->status.cooldown_curr_in - 1;
				buffer_data[i].capture_data->status.video_wait.unlock();
				buffer_data[i].capture_data->status.audio_wait.unlock();
				unlock_buffer(&buffer_data[i]);
			}
		}
		buffer_data[0].is_there_ready_buffer->general_timed_lock(&dummy);
	}
}

static void ds_3ds_start_thread(std::thread* thread_ptr, bool* usb_thread_run, usb_ds_3ds_general_data* buffer_data) {
	if(!usb_is_initialized())
		return;
	*usb_thread_run = true;
	*thread_ptr = std::thread(ds_3ds_usb_thread_function, usb_thread_run, buffer_data);
}

static void ds_3ds_close_thread(std::thread* thread_ptr, bool* usb_thread_run, usb_ds_3ds_general_data* buffer_data) {
	if(!usb_is_initialized())
		return;
	*usb_thread_run = false;
	buffer_data[0].is_there_ready_buffer->specific_unlock(0);
	thread_ptr->join();
}

// Read vendor request from control endpoint.  Returns bytes transferred (<0 = libusb error)
static int vend_in(libusb_device_handle *handle, const usb_device* usb_device_desc, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLength, uint8_t *buf) {
	return libusb_control_transfer(handle, ((uint8_t)LIBUSB_REQUEST_TYPE_VENDOR | (uint8_t)LIBUSB_ENDPOINT_IN), bRequest, wValue, wIndex, buf, wLength, usb_device_desc->control_timeout);
}

// Write vendor request to control endpoint.  Returns bytes transferred (<0 = libusb error)
static int vend_out(libusb_device_handle *handle, const usb_device* usb_device_desc, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLength, uint8_t *buf) {
	return libusb_control_transfer(handle, ((uint8_t)LIBUSB_REQUEST_TYPE_VENDOR | (uint8_t)LIBUSB_ENDPOINT_OUT), bRequest, wValue, wIndex, buf, wLength, usb_device_desc->control_timeout);
}

// Read from bulk endpoint.  Returns libusb error code
static int bulk_in(libusb_device_handle *handle, const usb_device* usb_device_desc, uint8_t *buf, int length, int *transferred) {
	return libusb_bulk_transfer(handle, usb_device_desc->ep2_in, buf, length, transferred, usb_device_desc->bulk_timeout);
}

// Read FPGA configuration regs
static bool read_config(libusb_device_handle *handle, const usb_device* usb_device_desc, uint8_t cfgAddr, uint8_t *buf, int count) {
	if(!usb_device_desc->is_3ds)
		return false;
	if((count <= 0) || (count >= 256))
		return false;
	if(vend_out(handle, usb_device_desc, usb_device_desc->cmdout_i2c_write, usb_device_desc->i2caddr_3dsconfig, 0, 1, &cfgAddr) <= 0)
		return false;
   	return vend_in(handle, usb_device_desc, usb_device_desc->cmdin_i2c_read, usb_device_desc->i2caddr_3dsconfig, 0, count, buf) == count;
}

static uint8_t capture_get_version(libusb_device_handle *handle, const usb_device* usb_device_desc) {
	if(!usb_device_desc->is_3ds)
		return 0;
	uint8_t version=0;
	read_config(handle, usb_device_desc, usb_device_desc->bitstream_3dscfg_ver, &version, 1);
	return version;
}

static bool capture_get_has_3d(libusb_device_handle *handle, const usb_device* usb_device_desc) {
	if(!usb_device_desc->is_3ds)
		return false;
	return capture_get_version(handle, usb_device_desc) >= FIRST_3D_VERSION;
}

static const usb_device* _get_usb_device_desc(CaptureDevice* device) {
	return (usb_device*)device->descriptor;
}

static const usb_device* get_usb_device_desc(CaptureData* capture_data) {
	return _get_usb_device_desc(&capture_data->status.device);
}

static std::string get_serial(libusb_device_handle *handle, libusb_device_descriptor *usb_descriptor, int &curr_serial_extra_id) {
	uint8_t data[SERIAL_NUMBER_SIZE];
	std::string serial_str = std::to_string(curr_serial_extra_id);
	if(libusb_get_string_descriptor_ascii(handle, usb_descriptor->iSerialNumber, data, REAL_SERIAL_NUMBER_SIZE-1) >= 0) {
		data[REAL_SERIAL_NUMBER_SIZE] = '\0';
		serial_str = std::string((const char*)data);
	}
	else
		curr_serial_extra_id += 1;
	return serial_str;
}

static int insert_device(std::vector<CaptureDevice> &devices_list, const usb_device* usb_device_desc, libusb_device *usb_device, libusb_device_descriptor *usb_descriptor, int &curr_serial_extra_id) {
	libusb_device_handle *handle = NULL;
	uint8_t data[SERIAL_NUMBER_SIZE];
	if((usb_descriptor->idVendor != usb_device_desc->vid) || (usb_descriptor->idProduct != usb_device_desc->pid))
		return LIBUSB_ERROR_NOT_FOUND;
	int result = libusb_open(usb_device, &handle);
	if(result || (handle == NULL))
		return result;
	result = libusb_claim_interface(handle, usb_device_desc->capture_interface);
	if(result == LIBUSB_SUCCESS)
		libusb_release_interface(handle, usb_device_desc->capture_interface);
	if(result < 0) {
		libusb_close(handle);
		return result;
	}
	std::string serial_str = get_serial(handle, usb_descriptor, curr_serial_extra_id);
	if(usb_device_desc->is_3ds)
		devices_list.emplace_back(serial_str, "3DS", CAPTURE_CONN_USB, (void*)usb_device_desc, true, capture_get_has_3d(handle, usb_device_desc), true, HEIGHT_3DS, TOP_WIDTH_3DS + BOT_WIDTH_3DS, O3DS_SAMPLES_IN, 90, 0, 0, TOP_WIDTH_3DS, 0, VIDEO_DATA_RGB);
	else
		devices_list.emplace_back(serial_str, "DS", CAPTURE_CONN_USB, (void*)usb_device_desc, false, false, false, WIDTH_DS, HEIGHT_DS + HEIGHT_DS, 0, 0, 0, 0, 0, HEIGHT_DS, VIDEO_DATA_RGB16);
	libusb_close(handle);
	return result;
}

static libusb_device_handle* usb_find_by_serial_number(const usb_device* usb_device_desc, std::string &serial_number) {
	if(!usb_is_initialized())
		return NULL;
	libusb_device **usb_devices;
	int num_devices = libusb_get_device_list(get_usb_ctx(), &usb_devices);
	libusb_device_descriptor usb_descriptor{};
	libusb_device_handle *final_handle = NULL;

	int curr_serial_extra_id = 0;
	for(int i = 0; i < num_devices; i++) {
		libusb_device_handle *handle = NULL;
		uint8_t data[SERIAL_NUMBER_SIZE];
		int result = libusb_get_device_descriptor(usb_devices[i], &usb_descriptor);
		if(result < 0)
			continue;
		if((usb_descriptor.idVendor != usb_device_desc->vid) || (usb_descriptor.idProduct != usb_device_desc->pid))
			continue;
		result = libusb_open(usb_devices[i], &handle);
		if(result || (handle == NULL))
			continue;
		std::string device_serial_number = get_serial(handle, &usb_descriptor, curr_serial_extra_id);
		if(serial_number == device_serial_number) {
			final_handle = handle;
			break;
		}
		libusb_close(handle);
	}

	if(num_devices >= 0)
		libusb_free_device_list(usb_devices, 1);
	return final_handle;
}

static void capture_end(libusb_device_handle *dev, const usb_device* usb_device_desc, bool interface_claimed = true) {
	if(interface_claimed)
		libusb_release_interface(dev, usb_device_desc->capture_interface);
	libusb_close(dev);
}

static uint64_t _usb_get_video_in_size(const usb_device* usb_device_desc, bool enabled_3d) {
	if(!usb_device_desc->is_3ds)
		return sizeof(USBOldDSVideoInputData);
	if(enabled_3d)
		return sizeof(RGB83DSVideoInputData_3D);
	return sizeof(RGB83DSVideoInputData);
}

static uint64_t get_capture_size(const usb_device* usb_device_desc, bool enabled_3d) {
	if(!usb_device_desc->is_3ds)
		return sizeof(USBOldDSVideoInputData);
	if(enabled_3d)
		return sizeof(USB3DSCaptureReceived_3D) - EXTRA_DATA_BUFFER_USB_SIZE;
	return sizeof(USB3DSCaptureReceived) - EXTRA_DATA_BUFFER_USB_SIZE;
}

static usb_capture_status capture_read_oldds_3ds(usb_ds_3ds_general_data* chosen_buffer) {
	CaptureData* capture_data = chosen_buffer->capture_data;
	CaptureReceived* data_buffer = &chosen_buffer->capture_buf;
	libusb_device_handle* handle = (libusb_device_handle*)capture_data->handle;
	const usb_device* usb_device_desc = get_usb_device_desc(capture_data);
	const bool enabled_3d = capture_data->status.enabled_3d;
	int bytesIn = 0;
	int transferred = 0;
	int result = 0;
	uint64_t video_size = _usb_get_video_in_size(usb_device_desc, enabled_3d);
	uint64_t full_in_size = get_capture_size(usb_device_desc, enabled_3d);
	uint8_t *video_data_ptr = (uint8_t*)data_buffer->usb_received_3ds.video_in.screen_data;
	if(!usb_device_desc->is_3ds)
		video_data_ptr = (uint8_t*)data_buffer->usb_received_old_ds.video_in.screen_data;

	uint8_t dummy;
	result = vend_out(handle, usb_device_desc, usb_device_desc->cmdout_capture_start, 0, 0, 0, &dummy);
	if(result < 0)
		return (result == LIBUSB_ERROR_TIMEOUT) ? USB_CAPTURE_SKIP: USB_CAPTURE_ERROR;

	// Both DS and 3DS old CCs send data until end of frame, followed by 0-length packets
	do {
		int transferSize = ((full_in_size - bytesIn) + (EXTRA_DATA_BUFFER_USB_SIZE - 1)) & ~(EXTRA_DATA_BUFFER_USB_SIZE - 1); // multiple of maxPacketSize
		result = bulk_in(handle, usb_device_desc, video_data_ptr + bytesIn, transferSize, &transferred);
		if(result == LIBUSB_SUCCESS)
			bytesIn += transferred;
	} while((bytesIn < full_in_size) && (result == LIBUSB_SUCCESS) && (transferred > 0));

	if(result==LIBUSB_ERROR_PIPE) {
		libusb_clear_halt(handle, usb_device_desc->ep2_in);
		return USB_CAPTURE_PIPE_ERROR;
	} else if(result == LIBUSB_ERROR_TIMEOUT || (usb_device_desc->is_3ds && (bytesIn < video_size))) {
		return USB_CAPTURE_SKIP;
	} else if(result != LIBUSB_SUCCESS) {
		return USB_CAPTURE_ERROR;
	}

	if(!usb_device_desc->is_3ds) {
		#ifndef SIMPLE_DS_FRAME_SKIP
		if(bytesIn < video_size) {
			if(vend_in(handle, usb_device_desc, usb_device_desc->cmdin_frameinfo, 0, 0, sizeof(data_buffer->usb_received_old_ds.frameinfo), (uint8_t*)&data_buffer->usb_received_old_ds.frameinfo) < 0)
				return USB_CAPTURE_FRAMEINFO_ERROR;
		}
		else {
			data_buffer->usb_received_old_ds.frameinfo.valid = 1;
			for(int i = 0; i < (HEIGHT_DS >> 3) << 1; i++)
				data_buffer->usb_received_old_ds.frameinfo.half_line_flags[i] = 0xFF;
		}
		#else
		if(bytesIn < video_size)
			return USB_CAPTURE_SKIP;
		#endif
	}

	chosen_buffer->read_amount = bytesIn;
	return USB_CAPTURE_SUCCESS;
}

static void process_usb_capture_result(usb_capture_status result, std::chrono::time_point<std::chrono::high_resolution_clock>* clock_start, bool* done, usb_ds_3ds_general_data* chosen_buffer) {
	const auto curr_time = std::chrono::high_resolution_clock::now();
	const std::chrono::duration<double> diff = curr_time - (*clock_start);

	switch(result) {
		case USB_CAPTURE_SKIP:
			/*
			if(diff.count() >= CAPTURE_SKIP_TIMEOUT_SECONDS) {
				capture_error_print(true, chosen_buffer->capture_data, "Disconnected: Too long since last read");
				done = true;
			}
			*/
			break;
		case USB_CAPTURE_PIPE_ERROR:
			capture_error_print(true, chosen_buffer->capture_data, "Disconnected: Pipe error");
			*done = true;
			break;
		case USB_CAPTURE_FRAMEINFO_ERROR:
			capture_error_print(true, chosen_buffer->capture_data, "Disconnected: Frameinfo error");
			*done = true;
			break;
		case USB_CAPTURE_SUCCESS:
			*clock_start = curr_time;
			chosen_buffer->time_in = diff.count();
			chosen_buffer->index = (*chosen_buffer->last_index) + 1;
			chosen_buffer->is_buffer_usage_done->specific_try_lock(chosen_buffer->real_index);
			chosen_buffer->in_use = true;
			(*chosen_buffer->last_index) += 1;
			chosen_buffer->is_there_ready_buffer->specific_unlock(chosen_buffer->real_index);
			break;
		default:
			capture_error_print(true, chosen_buffer->capture_data, "Disconnected: Error");
			*done = true;
			break;
	}
}

static usb_ds_3ds_general_data* get_free_buffer(usb_ds_3ds_general_data* buffer_data) {
	usb_ds_3ds_general_data* out_data = NULL;
	int dummy = 0;
	while(!out_data) {
		for(int i = 0; i < NUM_DS_3DS_CONCURRENT_DATA_BUFFERS; i++) {
			if(!buffer_data[i].in_use) {
				buffer_data[i].is_there_ready_buffer->specific_try_lock(i);
				out_data = &buffer_data[i];
				break;
			}
		}
		if(!out_data)
			buffer_data[0].is_buffer_usage_done->general_timed_lock(&dummy);
	}
	return out_data;
}

static void wait_all_free_buffer(usb_ds_3ds_general_data* buffer_data) {
	for(int i = 0; i < NUM_DS_3DS_CONCURRENT_DATA_BUFFERS; i++)
		buffer_data[i].index = (*buffer_data[0].last_index) - 1;
	for(int i = 0; i < NUM_DS_3DS_CONCURRENT_DATA_BUFFERS; i++) {
		while(buffer_data[i].in_use) {
			buffer_data[i].is_there_ready_buffer->specific_unlock(i);
			buffer_data[i].is_buffer_usage_done->specific_timed_lock(i);
		}
	}
}

void list_devices_usb_ds_3ds(std::vector<CaptureDevice> &devices_list, std::vector<no_access_recap_data> &no_access_list) {
	if(!usb_is_initialized())
		return;
	libusb_device **usb_devices;
	int num_devices = libusb_get_device_list(get_usb_ctx(), &usb_devices);
	libusb_device_descriptor usb_descriptor{};

	const size_t num_usb_desc = sizeof(usb_devices_desc_list) / sizeof(usb_devices_desc_list[0]);
	bool no_access_elems[num_usb_desc];
	int curr_serial_extra_ids[num_usb_desc];
	for (int i = 0; i < num_usb_desc; i++) {
		no_access_elems[i] = false;
		curr_serial_extra_ids[i] = 0;
	}
	for(int i = 0; i < num_devices; i++) {
		int result = libusb_get_device_descriptor(usb_devices[i], &usb_descriptor);
		if(result < 0)
			continue;
		for(int j = 0; j < num_usb_desc; j++)
			if(insert_device(devices_list, usb_devices_desc_list[j], usb_devices[i], &usb_descriptor, curr_serial_extra_ids[j]) != LIBUSB_ERROR_NOT_FOUND) {
				if (result == LIBUSB_ERROR_ACCESS)
					no_access_elems[j] = true;
				break;
			}
	}

	if(num_devices >= 0)
		libusb_free_device_list(usb_devices, 1);

	for(int i = 0; i < num_usb_desc; i++)
		if(no_access_elems[i])
			no_access_list.emplace_back(usb_devices_desc_list[i]->vid, usb_devices_desc_list[i]->pid);
}

bool connect_usb(bool print_failed, CaptureData* capture_data, CaptureDevice* device) {
	if(!usb_is_initialized())
		return false;
	const usb_device* usb_device_desc = _get_usb_device_desc(device);
	libusb_device_handle *dev = usb_find_by_serial_number(usb_device_desc, device->serial_number);
	if(!dev) {
		capture_error_print(true, capture_data, "Device not found");
		return false;
	}
	if(libusb_set_configuration(dev, usb_device_desc->default_config) != LIBUSB_SUCCESS) {
		capture_error_print(true, capture_data, "Configuration failed");
		capture_end(dev, usb_device_desc, false);
		return false;
	}
	if(libusb_claim_interface(dev, usb_device_desc->capture_interface) != LIBUSB_SUCCESS) {
		capture_error_print(true, capture_data, "Interface claim failed");
		capture_end(dev, usb_device_desc, false);
		return false;
	}

	if(usb_device_desc->is_3ds) {
		//FW bug(?) workaround- first read times out sometimes
		uint8_t dummy;
		vend_out(dev, usb_device_desc, usb_device_desc->cmdout_capture_start, 0, 0, 0, &dummy);
		default_sleep(usb_device_desc->bulk_timeout);
	}

	capture_data->handle = (void*)dev;

	return true;
}

uint64_t usb_get_video_in_size(CaptureData* capture_data) {
	return _usb_get_video_in_size(get_usb_device_desc(capture_data), capture_data->status.enabled_3d);
}

void usb_capture_main_loop(CaptureData* capture_data) {
	if(!usb_is_initialized())
		return;

	std::chrono::time_point<std::chrono::high_resolution_clock> clock_start = std::chrono::high_resolution_clock::now();
	bool done = false;
	usb_ds_3ds_general_data* general_data = new usb_ds_3ds_general_data[NUM_DS_3DS_CONCURRENT_DATA_BUFFERS];
	SharedConsumerMutex is_there_ready_buffer(NUM_DS_3DS_CONCURRENT_DATA_BUFFERS);
	SharedConsumerMutex is_buffer_usage_done(NUM_DS_3DS_CONCURRENT_DATA_BUFFERS);
	uint32_t last_index = -1;
	for(int i = 0; i < NUM_DS_3DS_CONCURRENT_DATA_BUFFERS; i++) {
		general_data[i].is_there_ready_buffer = &is_there_ready_buffer;
		general_data[i].is_buffer_usage_done = &is_buffer_usage_done;
		general_data[i].last_index = &last_index;
		general_data[i].capture_data = capture_data;
		general_data[i].in_use = false;
		general_data[i].real_index = i;
	}
	bool usb_thread_run;
	std::thread processing_thread;
	ds_3ds_start_thread(&processing_thread, &usb_thread_run, general_data);

	usb_ds_3ds_general_data* chosen_buffer = get_free_buffer(general_data);
	while((!done) && capture_data->status.connected && capture_data->status.running) {
		usb_capture_status result = capture_read_oldds_3ds(chosen_buffer);

		process_usb_capture_result(result, &clock_start, &done, chosen_buffer);
		if(!done)
			chosen_buffer = get_free_buffer(general_data);
	}
	wait_all_free_buffer(general_data);
	ds_3ds_close_thread(&processing_thread, &usb_thread_run, general_data);
	delete []general_data;
}

void usb_capture_cleanup(CaptureData* capture_data) {
	if(!usb_is_initialized())
		return;
	const usb_device* usb_device_desc = get_usb_device_desc(capture_data);
	capture_end((libusb_device_handle*)capture_data->handle, usb_device_desc);
}

void usb_ds_3ds_init() {
	return usb_init();
}

void usb_ds_3ds_close() {
	usb_close();
}