libweston/modepolicy/modepolicy_aml.cpp

#include <pthread.h>

#include "modepolicyfunc.h"
#include "DisplayAdapter.h"
#include "modepolicy_aml.h"

weston_ctx *gCtx = NULL;
static int g_activeLevel = 3;

static char *prop_changed_and_mode[] = {
	"color_space",
	"color_depth"
};

weston_ctx *weston_get_ctx()
{
	if (!gCtx) {
		gCtx = (weston_ctx *)calloc(1, sizeof(*gCtx));
		gCtx->need_update_hdmi_param = false;
		gCtx->scaling = -1;
		wl_list_init(&gCtx->prop_list);
	}
	return gCtx;
}

static void weston_prop_list_init(weston_ctx *ctx)
{
	prop_info *info, *tmp;
	if (!wl_list_empty(&ctx->prop_list)) {
		wl_list_for_each_safe(info, tmp, &ctx->prop_list, link) {
			wl_list_remove(&info->link);
			free(info);
		}
	}
	wl_list_init(&ctx->prop_list);
}

static int weston_add_property_item(weston_ctx *ctx,
			 const char *name, int id, int prop_id)
{
	prop_info *info;

	wl_list_for_each(info, &ctx->prop_list, link) {
		if (!strcmp(name, info->name) && info->item_id == id)
			return 0;
	}

	info = (prop_info *)calloc(1, sizeof(*info));
	memcpy(info->name, name, sizeof(info->name));
	info->item_id = id;
	info->prop_id = prop_id;
	info->need_change = 0;
	MESON_LOGD("ctx: %p, name: %s, id: %d, prop_id: %d\n", ctx, name, id, prop_id);
	wl_list_insert(&ctx->prop_list, &info->link);
	return 0;
}

static int weston_add_property(weston_ctx *ctx, int id, int type)
{
	int i, len;
	int value = 0;
	drmModePropertyRes *propRes;
	drmModeObjectProperties *props;

	props = drmModeObjectGetProperties( ctx->drm_fd, id, type );
	if ( props ) {
		for ( i= 0; i < props->count_props; ++i ) {
			propRes = drmModeGetProperty( ctx->drm_fd, props->props[i] );
			if ( propRes ) {
				weston_add_property_item(ctx, propRes->name, id, props->props[i]);
				drmModeFreeProperty( propRes );
				propRes = 0;
			}
		}
		drmModeFreeObjectProperties( props );
		props = 0;
	}
	return value;
}

static int weston_get_property_value(weston_ctx *ctx, const char * key, int id, int type)
{
	int i, len;
	int value = 0;
	drmModePropertyRes *propRes;
	drmModeObjectProperties *props;

	props = drmModeObjectGetProperties( ctx->drm_fd, id, type );
	if ( props ) {
		for ( i= 0; i < props->count_props; ++i ) {
			propRes = drmModeGetProperty( ctx->drm_fd, props->props[i] );
			if ( propRes ) {
				len = strlen(propRes->name);
				if ( !strncmp( propRes->name, key, len) ) {
					MESON_LOGD("property %d name (%s) value (%lld)",
						props->props[i], propRes->name, props->prop_values[i] );
					value = props->prop_values[i];
					drmModeFreeProperty( propRes );
					propRes = 0;
					break;
				}
				drmModeFreeProperty( propRes );
				propRes = 0;
			}
		}
		drmModeFreeObjectProperties( props );
		props = 0;
	}
	return value;
}

static int weston_get_connector_property(const char * key)
{
	weston_ctx *ctx = weston_get_ctx();

	return weston_get_property_value(ctx, key, ctx->conn->connector_id, DRM_MODE_OBJECT_CONNECTOR);
}

static int weston_get_crtc_property(const char * key)
{
	weston_ctx *ctx = weston_get_ctx();

	return weston_get_property_value(ctx, key, ctx->crtc->crtc_id, DRM_MODE_OBJECT_CRTC);
}

static bool get_mode_name_for_weston_mode(
			 weston_ctx *ctx,
			 struct weston_mode *mode,
			 char *out)
{
	int i;
	int current_interlaced;
	const char *value;

	if (mode && getModeNameForPix(out, mode->width, mode->height, mode->refresh, mode->flags))
		return true;

	value = bootenv_get("hdmimode");
	if (value)
		strcpy(out, value);
	else
		strcpy(out, "none");
	MESON_LOGD("out: %s", out);
	return false;
}

static bool weston_get_mode(char *mode)
{
	weston_ctx *ctx = weston_get_ctx();
	struct weston_mode *wmode = NULL;

	if (!wmode && ctx->current_mode.width != 0)
		wmode = &ctx->current_mode;

	return get_mode_name_for_weston_mode(ctx, wmode, mode);
}

bool WestonGetUbootIsBestmode()
{
	const char *isbestmode = bootenv_get("is.bestmode");
	if ( isbestmode != NULL ) {
		if (strcmp("false", isbestmode) == 0)
			return false;
		else
			return true;
	} else
		return true;
}

bool westonGetDrmModeInfoByName( weston_ctx *ctx, const char *mode, drmModeModeInfo *out_mode )
{
	bool result= false;

	if ( ctx && mode && out_mode ) {
		int width = -1, height = -1, rate = -1;
		bool interlaced = false;
		bool haveTarget = false;
		bool useBestRate = true;

		MESON_LOGD("%s: mode (%s)", __func__, mode);

		if ( sscanf( mode, "%dx%dp%d", &width, &height, &rate ) == 3 ) {
			interlaced = false;
		} else if ( sscanf( mode, "%dx%di%d", &width, &height, &rate ) == 3 ) {
			interlaced = true;
		} else if ( sscanf( mode, "%dx%dx%d", &width, &height, &rate ) == 3 ) {
			interlaced = false;
		} else if ( sscanf( mode, "%dx%d", &width, &height ) == 2 ) {
			int len = strlen(mode);
			interlaced = (mode[len - 1] == 'i');
		} else if (sscanf( mode, "%dp%dhz", &height,&rate ) == 2) {
			interlaced = false;
			width = -1;
		} else if (sscanf( mode, "%di%dhz", &height,&rate ) == 2) {
			interlaced = true;
			width = -1;
		} else if ( sscanf( mode, "%dp", &height ) == 1 ) {
			int len = strlen(mode);
			interlaced = (mode[len - 1] == 'i');
			width = -1;
		} else if (sscanf( mode, "smpte%dhz", &rate ) == 1) {
			interlaced = false;
			height = 2160;
			width = 4096;
		}

		if ( height > 0 ) {
			if ( width < 0 ) {
				switch ( height ) {
					case 480:
					case 576:
						width = 720;
						break;
					case 720:
						width = 1280;
						break;
					case 1080:
						width = 1920;
						break;
					case 1440:
						width = 2560;
						break;
					case 2160:
						width = 3840;
						break;
					case 2880:
						width = 5120;
						break;
					case 4320:
						width = 7680;
						break;
					default:
						break;
				}
			}
		}
		MESON_LOGD("%s w %d h %d rate %d", __func__, width, height, rate);
		if ( rate >= 0 )
			useBestRate = false;

		if ( (width > 0) && (height > 0) ) {
			if ( ctx->drm_fd >= 0 ) {
				drmModeRes *res = 0;
				drmModeConnector *conn = 0;

				res = drmModeGetResources( ctx->drm_fd );
				if ( res ) {
					int i;
					for ( i = 0; i < res->count_connectors; ++i ) {
						conn = drmModeGetConnector( ctx->drm_fd, res->connectors[i] );
						if ( conn ) {
							if ( conn->count_modes && (conn->connection == DRM_MODE_CONNECTED) )
								break;
							drmModeFreeConnector(conn);
							conn = 0;
						}
					}
					if ( conn ) {
						uint32_t rateBest = 0;
						int miBest = -1;

						MESON_LOGD("%s: want %dx%dx%d interlaced %d use best rate %d",
							__func__, width, height, rate, interlaced, useBestRate);
						for ( i = 0; i < conn->count_modes; ++i ) {
							MESON_LOGD("%s: consider mode %d: %dx%dx%d (%s) type 0x%x flags 0x%x",
								__func__, i, conn->modes[i].hdisplay, conn->modes[i].vdisplay,
								conn->modes[i].vrefresh, conn->modes[i].name,
								conn->modes[i].type, conn->modes[i].flags );

							if ( (conn->modes[i].hdisplay == width) &&
								(conn->modes[i].vdisplay == height) ) {
								bool modeIsInterlaced = (conn->modes[i].flags & DRM_MODE_FLAG_INTERLACE);
								if ( modeIsInterlaced != interlaced )
									continue;

								if ( useBestRate ) {
									if ( conn->modes[i].vrefresh > rateBest ) {
										rateBest = conn->modes[i].vrefresh;
										miBest = i;
									}
								} else if ( conn->modes[i].vrefresh == rate ) {
									miBest = i;
									break;
								}
							}
						}

						if ( miBest >= 0 ) {
							*out_mode = conn->modes[miBest];

							MESON_LOGI("%s: choosing output mode: %dx%dx%d (%s) flags 0x%x",
								__func__,
								out_mode->hdisplay,
								out_mode->vdisplay,
								out_mode->vrefresh,
								out_mode->name,
								out_mode->flags );

							result= true;
						} else {
							MESON_LOGE("%s: failed to find a mode matching (%s)", __func__, mode);
						}

						drmModeFreeConnector( conn );
					} else {
						MESON_LOGE("%s: unable to get connector for card", __func__);
					}
					drmModeFreeResources(res);
				} else {
					MESON_LOGE("%s: unable to get card resources", __func__);
				}
			} else {
				MESON_LOGE("%s: no open device", __func__);
			}
		} else {
			MESON_LOGE("%s: unable to parse mode (%s)", __func__, mode);
		}
	}

	return result;
}

static bool weston_set_mode(const char *mode)
{
	weston_ctx *ctx = weston_get_ctx();
	drmModeModeInfo info = {0};

	if (westonGetDrmModeInfoByName(ctx, mode, &info)) {
		ctx->next_mode.width = info.hdisplay;
		ctx->next_mode.height = info.vdisplay;
		ctx->next_mode.refresh = info.vrefresh * 1000;
		ctx->next_mode.flags = info.flags;
		setBootConfig(info.name, WestonGetUbootIsBestmode());
		ctx->mode_update = true;
		ctx->mode_changed = true;
	}
	return 0;
}

static int weston_get_property(int id, const char *name, char *buf)
{
	weston_ctx *ctx = weston_get_ctx();
	int value = 0;

	if (!ctx->crtc || !ctx->conn)
		return 0;

	if (ctx->crtc->crtc_id == id)
		value = weston_get_crtc_property(name);

	if (ctx->conn->connector_id == id)
		value = weston_get_connector_property(name);

	snprintf(buf, MAX_BUF_LEN, "%d", value);
	return 0;
}

static int weston_set_property(int id, const char *name, int value)
{
	prop_info *info;
	weston_ctx *ctx = weston_get_ctx();

	wl_list_for_each(info, &ctx->prop_list, link) {
		if (!strcmp(name, info->name) && info->item_id == id) {
			MESON_LOGD("name: %s, id: %d, prop: %d, value: %d\n",
				name, id, info->prop_id, value);
			info->value = value;
			info->need_change = 1;
			ctx->prop_changed = true;
			break;
		}
	}

	return 0;
}

static bool weston_set_colorattribute(const char *attr)
{
	weston_ctx *ctx = weston_get_ctx();
	int depth = -1, value = -1; // default none

	if ( attr && strlen(attr) > 3 ) {
		if ( strstr (attr, "rgb") )
			value = 0;
		else if ( strstr (attr, "422") )
			value = 1;
		else if ( strstr (attr, "444") )
			value = 2;
		else if ( strstr (attr, "420") )
			value = 3;
		sscanf(attr + 4, "%dbit", &depth);
	}
	if (ctx && ctx->conn && ctx->conn->connector_type == DRM_MODE_CONNECTOR_HDMIA) {
		weston_set_property(ctx->conn->connector_id, "color_space", value);
		weston_set_property(ctx->conn->connector_id, "color_depth", depth);
	}
	return 0;
}

void weston_set_scaling_position(int x, int y, int w, int h)
{
	weston_ctx *ctx = weston_get_ctx();
	const char *value = bootenv_get("scaling");
	int scaling = 1;

	if (value)
		scaling = atoi(value);

	if (ctx && ctx->scaling != scaling)
		ctx->scaling = scaling;
}

CompositorFunctionCallBack callback = {
	.get_mode = weston_get_mode,
	.set_mode = weston_set_mode,
	.set_colorattribute = weston_set_colorattribute,
	.get_property = weston_get_property,
	.set_property = weston_set_property,
	.set_scaling_position = weston_set_scaling_position,
};

static char *get_color_space_by_value(int value)
{
	char *str;

	switch ( value ) {
		case 0:
			str = "rgb";
			break;
		case 1:
			str = "422";
			break;
		case 2:
			str = "444";
			break;
		case 3:
		default:
			str = "420";
			break;
	}
	return str;
}

static int get_property_value(char *name)
{
	prop_info *info;
	weston_ctx *ctx = weston_get_ctx();

	wl_list_for_each(info, &ctx->prop_list, link) {
		if (!strcmp(name, info->name)) {
			if (info->need_change == 1)
				return info->value;
		}
	}
	return -1;
}

static int get_id_by_name(const char *name)
{
	prop_info *info;
	weston_ctx *ctx = weston_get_ctx();

	wl_list_for_each(info, &ctx->prop_list, link) {
		if (!strcmp(name, info->name))
				return info->item_id;
	}
	return -1;
}

static int mode_policy_parse_other(const char *name, int value)
{
	weston_ctx *ctx = weston_get_ctx();
	char attrvalue[32] = { 0 };

	if (STRCMPS(name, "scaling") == 0) {
		if (ctx && ctx->scaling != value)
			ctx->scaling = value;

		sprintf(attrvalue, "%d", value);
		bootenv_update("scaling", attrvalue);
		return 0;
	}
	return -1;
}

static void set_color_attribute(char *space, int depth)
{
	weston_ctx *ctx = weston_get_ctx();
	char attrvalue[32] = { 0 };
	char mode[] = { 0 };

	sprintf(attrvalue, "%s,%dbit", space, depth);
	setColorSpace(attrvalue);
	weston_get_mode(mode);
	setBootConfig(mode, false);
}

int mode_policy_set_property(const char *name, int value)
{
	char attrvalue[32] = { 0 };
	int color_space = 0;
	int color_depth = 0;
	int id = get_id_by_name(name);
	MESON_LOGI("id: %d, name: %s, value: %d", id, name, value);

	if (value < 0 || id < 0)
		return mode_policy_parse_other(name, value);

	if (strncmp(name, "dv_mode", sizeof("dv_mode")) == 0) {
		setDvMode(value == 0 ? 1 : (value == 1 ? 2 : 0));
		return 0;
	} else if (strncmp(name, "color_depth", sizeof("color_depth")) == 0) {
		color_space = get_property_value("color_space");
		if (color_space >= 0) {
			set_color_attribute(get_color_space_by_value(color_space), value);
			return 0;
		}
	} else if (strncmp(name, "color_space", sizeof("color_space")) == 0) {
		color_depth = get_property_value("color_depth");
		if (color_depth >= 0) {
			set_color_attribute(get_color_space_by_value(value), color_depth);
			return 0;
		}
	} else if (strncmp(name, "meson.crtc.hdr_policy", sizeof("meson.crtc.hdr_policy")) == 0) {
		sprintf(attrvalue, "%d", value);
		bootenv_update("hdr_policy", attrvalue);
	}

	return weston_set_property(id, name, value);
}

int mode_policy_get_scaling()
{
	weston_ctx *ctx = weston_get_ctx();

	if (ctx->scaling < 0)
		return 100;

	return ctx->scaling;
}

static struct weston_mode
drm_mode_to_weston_mode(drmModeModeInfoPtr info)
{
	struct weston_mode m;

	m.width = info->hdisplay;
	m.height = info->vdisplay;
	//mhz:refresh * 1000
	m.refresh = info->vrefresh * 1000;
	//hack: use weston_mode's flags as drm_mode's flags.
	m.flags = info->flags;
	m.aspect_ratio = WESTON_MODE_PIC_AR_NONE;

	return m;
}

static drmModeCrtc *weston_get_crtc_for_conn(int drm_fd, drmModeConnector *conn)
{
	drmModeEncoder *encoder;
	drmModeCrtc *crtc = NULL;
	drmModeRes *res = 0;
	int i, j, k;
	/* Get the current mode on the crtc that's currently driving
	 * this connector. */
	res = drmModeGetResources(drm_fd);
	if ( res ) {
		for (i= 0; i < res->count_encoders; ++i ) {
			uint32_t crtcId= 0;
			bool found= false;
			encoder = drmModeGetEncoder(drm_fd, res->encoders[i]);
			if (encoder && conn && (encoder->encoder_id == conn->encoder_id)) {
				found = true;
				break;
			}
			for (j = 0; j < res->count_crtcs; j++) {
				if (encoder->possible_crtcs & (1 << j)) {
					crtcId= res->crtcs[j];
					for (k = 0; k < res->count_crtcs; k++) {
						if (res->crtcs[k] == crtcId) {
							drmModeFreeEncoder(encoder);
							encoder= drmModeGetEncoder(drm_fd, res->encoders[k]);
							encoder->crtc_id= crtcId;
							MESON_LOGD("got enc %p crtc id %d", drm_fd, crtcId);
							found = true;
							break;
						}
					}
					if (found) {
						break;
					}
				}
			}
			if (!found) {
				drmModeFreeEncoder(encoder);
				encoder= 0;
			}
			if (found) {
				break;
			}
		}
		if (encoder != NULL) {
			crtc = drmModeGetCrtc(drm_fd, encoder->crtc_id);
			drmModeFreeEncoder(encoder);
		}
		drmModeFreeResources( res );
		res = 0;
	} else {
		MESON_LOGD("unable to get card resources");
	}
	return crtc;
}

void init_mode_policy_without_mode(struct weston_head *head, int fd, drmModeConnector *conn)
{
	weston_ctx *ctx = weston_get_ctx();
	drmModeCrtc *crtc = NULL;
	bool changed = false;

	if (conn && conn->connector_type != DRM_MODE_CONNECTOR_TV) {
		crtc = weston_get_crtc_for_conn(fd, conn);
		if (!crtc)
			return;

		initModePolicyFun(crtc, conn, callback);
		if ((ctx->crtc && ctx->crtc->crtc_id != crtc->crtc_id) || !ctx->crtc)
			changed = true;

		if (ctx->crtc)
			drmModeFreeCrtc(ctx->crtc);
		ctx->drm_fd = fd;
		ctx->crtc = crtc;
		ctx->conn = conn;
		ctx->head = head;
		ctx->current_mode = drm_mode_to_weston_mode(&ctx->crtc->mode);
		if (changed) {
			weston_prop_list_init(ctx);
			weston_add_property(ctx, ctx->crtc->crtc_id, DRM_MODE_OBJECT_CRTC);
			weston_add_property(ctx, ctx->conn->connector_id, DRM_MODE_OBJECT_CONNECTOR);
		}
	}
}

void init_mode_policy(int fd, drmModeConnector *conn)
{
	weston_ctx *ctx = weston_get_ctx();
	drmModeCrtc *crtc = weston_get_crtc_for_conn(fd, conn);
	if (ctx->crtc)
		drmModeFreeCrtc(ctx->crtc);
	ctx->drm_fd = fd;
	ctx->crtc = crtc;
	ctx->conn = conn;
	initModePolicy(crtc, conn, callback);
}

static bool need_mode_changed(char *name)
{
	int size = sizeof(prop_changed_and_mode) / sizeof(prop_changed_and_mode[0]);
	int i;

	for (i = 0; i < size; i++) {
		if (STRCMPS(name, prop_changed_and_mode[i]) == 0)
			return true;
	}
	return false;
}

int mode_policy_add_prop(drmModeAtomicReq *req, bool mode_changed)
{
	weston_ctx *ctx = weston_get_ctx();
	prop_info *info;
	int err = 0, ret = 0;
	int count = 0;

	wl_list_for_each(info, &ctx->prop_list, link) {
		if (info->need_change) {
			if (need_mode_changed(info->name) && !mode_changed)
				continue;

			err = drmModeAtomicAddProperty(req, info->item_id, info->prop_id, info->value);
			if (!err)
				MESON_LOGE("drmModeAtomicAddProperty %s fail: %d(%d)\n",
					info->name, err, errno);
			info->need_change = 0;
			ret |= (err <= 0) ? -1 : 0;
			if (!ret)
				count++;
			ctx->need_update_hdmi_param = true;
		}
	}
	return ret >= 0 ? count : ret;
}

void mode_policy_set_hotplug(int plug, bool force)
{
	weston_ctx *ctx = weston_get_ctx();

	if (force) {
		ctx->hotplug = plug;
	} else {
		if (ctx->hotplug & plug)
			ctx->hotplug = plug;
	}
}

void mode_policy_update_mode(struct weston_mode *mode)
{
	weston_ctx *ctx = weston_get_ctx();
	if (mode)
		ctx->current_mode = *mode;
	else if (ctx->mode_changed)
		ctx->current_mode = ctx->next_mode;
	MESON_LOGD("curr: %dx%d@%d",
		ctx->current_mode.width,
		ctx->current_mode.height,
		ctx->current_mode.refresh);
}

struct weston_mode *mode_policy_choose_mode(struct weston_mode *mode)
{
	weston_ctx *ctx = weston_get_ctx();
	char name[32] = { 0 };

	if (mode) {
		if (get_mode_name_for_weston_mode(ctx, mode, name))
			setActiveConfig(name);
	} else {
		if (ctx->hotplug & (AML_WESTON_HOTPLUG_PLUG | AML_WESTON_HOTPLUG_UNPLUG))
			onHotplug(ctx->hotplug & AML_WESTON_HOTPLUG_PLUG);
		else if (ctx->hotplug & (AML_WESTON_HOTPLUG_BOOT | AML_WESTON_HOTPLUG_SET))
			initModePolicy(NULL, NULL, callback);
		ctx->hotplug = AML_WESTON_HOTPLUG_INIT;
	}
	if (ctx->mode_update)
		return &ctx->next_mode;
	return &ctx->current_mode;
}

void mode_policy_update_bestmode(bool bestmode)
{
	bootenv_update("is.bestmode", bestmode ? "true" : "false");
}

void mode_policy_update_modeset(bool *state, bool *modeset)
{
	weston_ctx *ctx = weston_get_ctx();

	MESON_LOGI("state: %d, modeset: %d, mode_changed: %d", *state, *modeset, ctx->mode_changed);
	if (ctx->mode_changed) {
		if (!*state)
			*state = true;
		if (!*modeset)
			*modeset = true;
	}
}

void mode_policy_resume()
{
	weston_ctx *ctx = weston_get_ctx();
	int flag;

	flag = (ctx->conn &&
		(ctx->conn->connector_type == DRM_MODE_CONNECTOR_HDMIA ||
		ctx->conn->connector_type == DRM_MODE_CONNECTOR_TV)) ?
		AML_WESTON_HOTPLUG_PLUG : AML_WESTON_HOTPLUG_SET;

	mode_policy_set_hotplug(flag, true);
}

static void * wstUpdatenvThread(void *arg )
{
	long long delay = 16667LL;
	weston_ctx *ctx = (weston_ctx *)arg;

	while (!ctx->update_env_thread_stop_requested)
	{
		if ((ctx->mode_changed && !ctx->prop_changed) ||
			(ctx->prop_changed && ctx->need_update_hdmi_param))
		{
			weston_log("%s[%d]\n", __func__, __LINE__);
			updateEnv();
			ctx->need_update_hdmi_param = false;
			ctx->mode_changed = false;
			ctx->prop_changed = false;
		}
		usleep( delay );
	}
	MESON_LOGD("update env thread exit");
	return NULL;
}

void weston_start_update_env_thread()
{
	int rc;
	weston_ctx *ctx = weston_get_ctx();

	ctx->update_env_thread_stop_requested = false;
	rc = pthread_create(&ctx->update_env_thread_id, NULL, wstUpdatenvThread, ctx);
	if ( rc )
		MESON_LOGE("unable to start updatenv thread: rc %d errno %d", rc, errno);
}