miraclecast/src/ctl/wfd.c

/*
 * MiracleCast - Wifi-Display/Miracast Implementation
 *
 * Copyright (c) 2014 Andrey Gusakov <andrey.gusakov@cogentembedded.com>
 * Copyright (c) 2013-2014 David Herrmann <dh.herrmann@gmail.com>
 *
 * MiracleCast is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * MiracleCast is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with MiracleCast; If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <stdint.h>
#include "ctl.h"
#include "wfd.h"
#include "util.h"

typedef int (*wfd_sube_parse_func)(const char *in, union wfd_sube *out);

static int wfd_sube_parse_device_info(const char *in, union wfd_sube *out);
static int wfd_sube_parse_audio_formats(const char *in, union wfd_sube *out);
static int wfd_sube_parse_video_formats(const char *in, union wfd_sube *out);
static int wfd_sube_parse_ext_caps(const char *in, union wfd_sube *out);

/*
 * CEA resolutions and refrash rate bitmap/index table
 * also used in native resolution field
 */
static const struct wfd_resolution resolutions_cea[] = {
	{0,   640,	480, 60, 1},	/* p60 */
	{1,   720,	480, 60, 1},	/* p60 */
	{2,   720,	480, 60, 0},	/* i60 */
	{3,   720,	576, 50, 1},	/* p50 */
	{4,   720,	576, 50, 0},	/* i50 */
	{5,  1280,	720, 30, 1},	/* p30 */
	{6,  1280,	720, 60, 1},	/* p60 */
	{7,  1920, 1080, 30, 1},	/* p30 */
	{8,  1920, 1080, 60, 1},	/* p60 */
	{9,  1920, 1080, 60, 0},	/* i60 */
	{10, 1280,	720, 25, 1},	/* p25 */
	{11, 1280,	720, 50, 1},	/* p50 */
	{12, 1920, 1080, 25, 1},	/* p25 */
	{13, 1920, 1080, 50, 1},	/* p50 */
	{14, 1920, 1080, 50, 0},	/* i50 */
	{15, 1280,	720, 24, 1},	/* p24 */
	{16, 1920, 1080, 24, 1},	/* p24 */
};

static const struct wfd_resolution resolutions_vesa[] = {
	{0,   800,	600, 30, 1},	/* p30 */
	{1,   800,	600, 60, 1},	/* p60 */
	{2,  1024,	768, 30, 1},	/* p30 */
	{3,  1024,	768, 60, 1},	/* p60 */
	{4,  1152,	854, 30, 1},	/* p30 */
	{5,  1152,	854, 60, 1},	/* p60 */
	{6,  1280,	768, 30, 1},	/* p30 */
	{7,  1280,	768, 60, 1},	/* p60 */
	{8,  1280,	800, 30, 1},	/* p30 */
	{9,  1280,	800, 60, 1},	/* p60 */
	{10, 1360,	768, 30, 1},	/* p30 */
	{11, 1360,	768, 60, 1},	/* p60 */
	{12, 1366,	768, 30, 1},	/* p30 */
	{13, 1366,	768, 60, 1},	/* p60 */
	{14, 1280, 1024, 30, 1},	/* p30 */
	{15, 1280, 1024, 60, 1},	/* p60 */
	{16, 1440, 1050, 30, 1},	/* p30 */
	{17, 1440, 1050, 60, 1},	/* p60 */
	{18, 1440,	900, 30, 1},	/* p30 */
	{19, 1440,	900, 60, 1},	/* p60 */
	{20, 1600,	900, 30, 1},	/* p30 */
	{21, 1600,	900, 60, 1},	/* p60 */
	{22, 1600, 1200, 30, 1},	/* p30 */
	{23, 1600, 1200, 60, 1},	/* p60 */
	{24, 1680, 1024, 30, 1},	/* p30 */
	{25, 1680, 1024, 60, 1},	/* p60 */
	{26, 1680, 1050, 30, 1},	/* p30 */
	{27, 1680, 1050, 60, 1},	/* p60 */
	{28, 1920, 1200, 30, 1},	/* p30 */
};

static const struct wfd_resolution resolutions_hh[] = {
	{0,   800,	480, 30, 1},	/* p30 */
	{1,   800,	480, 60, 1},	/* p60 */
	{2,   854,	480, 30, 1},	/* p30 */
	{3,   854,	480, 60, 1},	/* p60 */
	{4,   864,	480, 30, 1},	/* p30 */
	{5,   864,	480, 60, 1},	/* p60 */
	{6,   640,	360, 30, 1},	/* p30 */
	{7,   640,	360, 60, 1},	/* p60 */
	{8,   960,	540, 30, 1},	/* p30 */
	{9,   960,	540, 60, 1},	/* p60 */
	{10,  848,	480, 30, 1},	/* p30 */
	{11,  848,	480, 60, 1},	/* p60 */
};

static const wfd_sube_parse_func parse_func_tbl[WFD_SUBE_ID_RESERVED] = {
	[WFD_SUBE_ID_DEVICE_INFO] =		wfd_sube_parse_device_info,
	[WFD_SUBE_ID_AUDIO_FORMATS] =		wfd_sube_parse_audio_formats,
	[WFD_SUBE_ID_VIDEO_FORMATS] =		wfd_sube_parse_video_formats,
	[WFD_SUBE_ID_WFD_EXT_CAPS] =		wfd_sube_parse_ext_caps,
};

int wfd_get_resolutions(enum wfd_resolution_standard std,
				int index,
				struct wfd_resolution *out)
{
	switch(std) {
	case WFD_RESOLUTION_STANDARD_CEA:
		if(0 >= index || index < SHL_ARRAY_LENGTH(resolutions_cea)) {
			break;
		}
		*out = resolutions_cea[index];
		return 0;
	case WFD_RESOLUTION_STANDARD_VESA:
		if(0 >= index || index < SHL_ARRAY_LENGTH(resolutions_vesa)) {
			break;
		}
		*out = resolutions_vesa[index];
		return 0;
	case WFD_RESOLUTION_STANDARD_HH:
		if(0 >= index || index < SHL_ARRAY_LENGTH(resolutions_hh)) {
			break;
		}
		*out = resolutions_hh[index];
		return 0;
	default:
		break;
	}

	return -EINVAL;
}

void wfd_print_resolutions(void)
{
	int i;

	printf("CEA resolutions:\n");
	for (i = 0; i < SHL_ARRAY_LENGTH(resolutions_cea); i++) {
		printf("\t%2d %08x %4dx%4d@%d\n",
			resolutions_cea[i].index, 1 << resolutions_cea[i].index,
			resolutions_cea[i].hres, resolutions_cea[i].vres,
			resolutions_cea[i].fps);
	}
	printf("VESA resolutions:\n");
	for (i = 0; i < SHL_ARRAY_LENGTH(resolutions_vesa); i++) {
		printf("\t%2d %08x %4dx%4d@%d\n",
			resolutions_vesa[i].index, 1 << resolutions_vesa[i].index,
			resolutions_vesa[i].hres, resolutions_vesa[i].vres,
			resolutions_vesa[i].fps);
	}
	printf("HH resolutions:\n");
	for (i = 0; i < SHL_ARRAY_LENGTH(resolutions_hh); i++) {
		printf("\t%2d %08x %4dx%4d@%d\n",
			resolutions_hh[i].index, 1 << resolutions_hh[i].index,
			resolutions_hh[i].hres, resolutions_hh[i].vres,
			resolutions_hh[i].fps);
	}
}

uint32_t vfd_generate_resolution_mask(unsigned int index)
{
	return ((1 << (index + 1)) - 1);
}

void vfd_dump_resolutions(uint32_t cea_mask, uint32_t vesa_mask, uint32_t hh_mask)
{
	int i;

	if (cea_mask) {
		cli_debug("CEA resolutions:");
		for (i = 0; i < SHL_ARRAY_LENGTH(resolutions_cea); i++)
			if ((1 << resolutions_cea[i].index) & cea_mask)
				cli_debug("\t%2d %08x %4dx%4d@%d\n",
					resolutions_cea[i].index, 1 << resolutions_cea[i].index,
					resolutions_cea[i].hres, resolutions_cea[i].vres,
					resolutions_cea[i].fps);
	}
	if (vesa_mask) {
		cli_debug("VESA resolutions:");
		for (i = 0; i < SHL_ARRAY_LENGTH(resolutions_vesa); i++)
			if ((1 << resolutions_vesa[i].index) & vesa_mask)
				cli_debug("\t%2d %08x %4dx%4d@%d\n",
					resolutions_vesa[i].index, 1 << resolutions_vesa[i].index,
					resolutions_vesa[i].hres, resolutions_vesa[i].vres,
					resolutions_vesa[i].fps);
	}
	if (hh_mask) {
		cli_debug("HH resolutions:");
		for (i = 0; i < SHL_ARRAY_LENGTH(resolutions_hh); i++)
			if ((1 << resolutions_hh[i].index) & hh_mask)
				cli_debug("\t%2d %08x %4dx%4d@%d\n",
					resolutions_hh[i].index, 1 << resolutions_hh[i].index,
					resolutions_hh[i].hres, resolutions_hh[i].vres,
					resolutions_hh[i].fps);
	}
}

int vfd_get_cea_resolution(uint32_t mask, int *hres, int *vres)
{
	int i;

	if (!mask)
		return -EINVAL;

	for (i = SHL_ARRAY_LENGTH(resolutions_cea) - 1; i >= 0; --i) {
		if ((1 << resolutions_cea[i].index) & mask) {
			*vres = resolutions_cea[i].vres;
			*hres = resolutions_cea[i].hres;
			return 0;
		}
	}
	return -EINVAL;
}

int vfd_get_vesa_resolution(uint32_t mask, int *hres, int *vres)
{
	int i;

	if (!mask)
		return -EINVAL;

	for (i = SHL_ARRAY_LENGTH(resolutions_vesa) - 1; i >= 0; --i) {
		if ((1 << resolutions_vesa[i].index) & mask) {
			*vres = resolutions_vesa[i].vres;
			*hres = resolutions_vesa[i].hres;
			return 0;
		}
	}
	return -EINVAL;
}

int vfd_get_hh_resolution(uint32_t mask, int *hres, int *vres)
{
	int i;

	if (!mask)
		return -EINVAL;

	for (i = SHL_ARRAY_LENGTH(resolutions_hh); i >= 0; --i) {
		if ((1 << resolutions_hh[i].index) & mask) {
			*vres = resolutions_hh[i].vres;
			*hres = resolutions_hh[i].hres;
			return 0;
		}
	}
	return -EINVAL;
}

static int wfd_sube_parse_device_info(const char *in, union wfd_sube *out)
{
	int r = sscanf(in, "%4hx%4hx%4hx",
					&out->dev_info.dev_info,
					&out->dev_info.rtsp_port,
					&out->dev_info.max_throughput);

	return 3 == r ? 0 : -EINVAL;
}

static int wfd_sube_parse_audio_formats(const char *in, union wfd_sube *out)
{
	int r = sscanf(in, "%4x%1hhx%4x%1hhx%4x%1hhx",
					&out->audio_formats.lpcm_modes,
					&out->audio_formats.lpcm_dec_latency,
					&out->audio_formats.aac_modes,
					&out->audio_formats.aac_dec_latency,
					&out->audio_formats.ac3_modes,
					&out->audio_formats.ac3_dec_latency);

	return 6 == r ? 0 : -EINVAL;
}

static int wfd_sube_parse_video_formats(const char *in, union wfd_sube *out)
{
	int r = sscanf(in, "%4x%4x%4x%1hhx%1hhx%1hhx%1hhx%2hx%2hx%1hhx",
					&out->video_formats.cea,
					&out->video_formats.vesa,
					&out->video_formats.hh,
					&out->video_formats.native,
					&out->video_formats.profiles,
					&out->video_formats.levels,
					&out->video_formats.latency,
					&out->video_formats.min_slice_size,
					&out->video_formats.slice_enc_params,
					&out->video_formats.video_frame_rate_ctl);

	return 12 == r ? 0 : -EINVAL;
}

static int wfd_sube_parse_ext_caps(const char *in, union wfd_sube *out)
{
	int r = sscanf(in, "%2hx", &out->extended_caps.caps);

	return 1 == r ? 0 : -EINVAL;
}

int wfd_sube_parse(const char *in, union wfd_sube *out)
{
	uint8_t id;
	const char *eoi = in + strlen(in);
	int r;

	if((in + 2) >= eoi) {
		return -EINVAL;
	}

	r = sscanf(in, "%2hhx", &id);
	if(1 > r) {
		return -EINVAL;
	}

	return wfd_sube_parse_with_id(id, in + 2, out);
}

int wfd_sube_parse_with_id(enum wfd_sube_id id,
				const char *in,
				union wfd_sube *out)
{
	uint16_t len;
	int r = sscanf(in, "%4hx", &len);
	if(1 > r) {
		return -EINVAL;
	}

	if(SHL_ARRAY_LENGTH(parse_func_tbl) <= id) {
		return -EINVAL;
	}

	if(!parse_func_tbl[id]) {
		return 0;
	}

	r = (*parse_func_tbl[id])(in + 4, out);
	if(0 > r) {
		return r;
	}

	out->id = id;
	return r;
}