From fc48f556f87d99f628cb96fa4c432392ec13d217 Mon Sep 17 00:00:00 2001
From: Athanasios Oikonomou <athoik@gmail.com>
Date: Mon, 8 Feb 2016 21:58:24 +0200
Subject: [PATCH] STV: Add PLS support


diff --git a/drivers/media/dvb-frontends/stv0900_core.c b/drivers/media/dvb-frontends/stv0900_core.c
index e5a87b5..805da45 100644
--- a/drivers/media/dvb-frontends/stv0900_core.c
+++ b/drivers/media/dvb-frontends/stv0900_core.c
@@ -1552,6 +1552,19 @@ static int stv0900_status(struct stv0900_internal *intp,
 	return locked;
 }
 
+static int stv0900_set_pls(struct stv0900_internal *intp,
+				enum fe_stv0900_demod_num demod, u8 pls_mode, u32 pls_code)
+{
+	enum fe_stv0900_error error = STV0900_NO_ERROR;
+
+	dprintk("Set PLS code %d (mode %d)", pls_code, pls_mode);
+	stv0900_write_reg(intp, PLROOT2, (pls_mode<<2) | (pls_code>>16));
+	stv0900_write_reg(intp, PLROOT1, pls_code>>8);
+	stv0900_write_reg(intp, PLROOT0, pls_code);
+
+	return error;
+}
+
 static int stv0900_set_mis(struct stv0900_internal *intp,
 				enum fe_stv0900_demod_num demod, int mis)
 {
@@ -1559,7 +1572,7 @@ static int stv0900_set_mis(struct stv0900_internal *intp,
 
 	dprintk("%s\n", __func__);
 
-	if (mis < 0 || mis > 255) {
+	if (mis == NO_STREAM_ID_FILTER) {
 		dprintk("Disable MIS filtering\n");
 		stv0900_write_bits(intp, FILTER_EN, 0);
 	} else {
@@ -1593,6 +1606,7 @@ static enum dvbfe_search stv0900_search(struct dvb_frontend *fe)
 	if (state->config->set_ts_params)
 		state->config->set_ts_params(fe, 0);
 
+	stv0900_set_pls(intp, demod, (c->stream_id>>26) & 0x3, (c->stream_id>>8) & 0x3FFFF);
 	stv0900_set_mis(intp, demod, c->stream_id);
 
 	p_result.locked = FALSE;
diff --git a/drivers/media/dvb-frontends/stv090x.c b/drivers/media/dvb-frontends/stv090x.c
index 970375a..416b749 100644
--- a/drivers/media/dvb-frontends/stv090x.c
+++ b/drivers/media/dvb-frontends/stv090x.c
@@ -3425,18 +3425,40 @@ err:
 	return -1;
 }
 
+static int stv090x_set_pls(struct stv090x_state *state, u8 pls_mode, u32 pls_code)
+{
+	if (pls_mode == 0 && pls_code == 0)
+		pls_code = 1;
+	pls_mode &= 0x03;
+	pls_code &= 0x3FFFF;
+
+	dprintk(FE_DEBUG, 1, "Set PLS code %d (mode %d)", pls_code, pls_mode);
+	if (STV090x_WRITE_DEMOD(state, PLROOT2, (pls_mode<<2) | (pls_code>>16)) < 0)
+		goto err;
+	if (STV090x_WRITE_DEMOD(state, PLROOT1, pls_code>>8) < 0)
+		goto err;
+	if (STV090x_WRITE_DEMOD(state, PLROOT0, pls_code) < 0)
+		goto err;
+	return 0;
+err:
+	dprintk(FE_ERROR, 1, "I/O error");
+	return -1;
+}
+
 static int stv090x_set_mis(struct stv090x_state *state, int mis)
 {
 	u32 reg;
 
-	if (mis < 0 || mis > 255) {
+	if (mis == NO_STREAM_ID_FILTER) {
 		dprintk(FE_DEBUG, 1, "Disable MIS filtering");
+		stv090x_set_pls(state, 0, 0);
 		reg = STV090x_READ_DEMOD(state, PDELCTRL1);
 		STV090x_SETFIELD_Px(reg, FILTER_EN_FIELD, 0x00);
 		if (STV090x_WRITE_DEMOD(state, PDELCTRL1, reg) < 0)
 			goto err;
 	} else {
 		dprintk(FE_DEBUG, 1, "Enable MIS filtering - %d", mis);
+		stv090x_set_pls(state, (mis>>26) & 0x3, (mis>>8) & 0x3FFFF);
 		reg = STV090x_READ_DEMOD(state, PDELCTRL1);
 		STV090x_SETFIELD_Px(reg, FILTER_EN_FIELD, 0x01);
 		if (STV090x_WRITE_DEMOD(state, PDELCTRL1, reg) < 0)
-- 
2.1.4