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;

weston_ctx *weston_get_ctx()
{
	if (!gCtx) {
		gCtx = (weston_ctx *)calloc(1, sizeof(*gCtx));
		gCtx->need_update_hdmi_param = false;
		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);
}

static 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->head && ctx->head->output) {
		if (ctx->head->output->current_scale != scaling)
			ctx->head->output->current_scale = 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 drmModeCrtc *weston_get_crtc_for_conn(int drm_fd, drmModeConnector *conn)
{
	drmModeEncoder *encoder;
	drmModeCrtc *crtc = NULL;

	/* Get the current mode on the crtc that's currently driving
	 * this connector. */
	encoder = drmModeGetEncoder(drm_fd, conn->encoder_id);
	if (encoder != NULL) {
		crtc = drmModeGetCrtc(drm_fd, encoder->crtc_id);
		drmModeFreeEncoder(encoder);
	}
	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;

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

		initModePolicyFun(crtc, conn, callback);
		if (ctx->crtc)
			drmModeFreeCrtc(ctx->crtc);
		ctx->drm_fd = fd;
		ctx->crtc = crtc;
		ctx->conn = conn;
		ctx->head = head;
		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);
}

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

	wl_list_for_each(info, &ctx->prop_list, link) {
		if (info->need_change) {
			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;
			ctx->need_update_hdmi_param = true;
		}
	}
	return ret;
}

void mode_policy_set_hotplug(int plug)
{
	weston_ctx *ctx = weston_get_ctx();
	if (ctx->hotplug == AML_WESTON_HOTPLUG_INIT)
		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
		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 };
	drmModeModeInfo info = { 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)
			initModePolicy(NULL, NULL, callback);
		ctx->hotplug = AML_WESTON_HOTPLUG_INIT;
	}
	if (ctx->mode_update)
		return &ctx->next_mode;
	return NULL;
}

static void * wstUpdatenvThread(void *arg )
{
	long long delay = 16667LL;
	weston_ctx *ctx = (weston_ctx *)arg;
	char * attr = NULL;
	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);
}