1 files changed, 64 insertions, 7 deletions

diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index 30ee5905215..be4bec2a964 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -170,18 +170,18 @@ static int af9033_rd_reg_mask(struct af9033_state *state, u32 reg, u8 *val,
 static int af9033_wr_reg_val_tab(struct af9033_state *state,
 		const struct reg_val *tab, int tab_len)
 {
+#define MAX_TAB_LEN 212
 	int ret, i, j;
-	u8 buf[MAX_XFER_SIZE];
+	u8 buf[1 + MAX_TAB_LEN];
+
+	dev_dbg(&state->i2c->dev, "%s: tab_len=%d\n", __func__, tab_len);
 
 	if (tab_len > sizeof(buf)) {
-		dev_warn(&state->i2c->dev,
-			 "%s: i2c wr len=%d is too big!\n",
-			 KBUILD_MODNAME, tab_len);
+		dev_warn(&state->i2c->dev, "%s: tab len %d is too big\n",
+				KBUILD_MODNAME, tab_len);
 		return -EINVAL;
 	}
 
-	dev_dbg(&state->i2c->dev, "%s: tab_len=%d\n", __func__, tab_len);
-
 	for (i = 0, j = 0; i < tab_len; i++) {
 		buf[j] = tab[i].val;
 
@@ -989,10 +989,62 @@ err:
 	return ret;
 }
 
+static int af9033_pid_filter_ctrl(struct dvb_frontend *fe, int onoff)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+
+	dev_dbg(&state->i2c->dev, "%s: onoff=%d\n", __func__, onoff);
+
+	ret = af9033_wr_reg_mask(state, 0x80f993, onoff, 0x01);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_pid_filter(struct dvb_frontend *fe, int index, u16 pid, int onoff)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+	u8 wbuf[2] = {(pid >> 0) & 0xff, (pid >> 8) & 0xff};
+
+	dev_dbg(&state->i2c->dev, "%s: index=%d pid=%04x onoff=%d\n",
+			__func__, index, pid, onoff);
+
+	if (pid > 0x1fff)
+		return 0;
+
+	ret = af9033_wr_regs(state, 0x80f996, wbuf, 2);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x80f994, onoff);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x80f995, index);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
 static struct dvb_frontend_ops af9033_ops;
 
 struct dvb_frontend *af9033_attach(const struct af9033_config *config,
-		struct i2c_adapter *i2c)
+				   struct i2c_adapter *i2c,
+				   struct af9033_ops *ops)
 {
 	int ret;
 	struct af9033_state *state;
@@ -1067,6 +1119,11 @@ struct dvb_frontend *af9033_attach(const struct af9033_config *config,
 	memcpy(&state->fe.ops, &af9033_ops, sizeof(struct dvb_frontend_ops));
 	state->fe.demodulator_priv = state;
 
+	if (ops) {
+		ops->pid_filter = af9033_pid_filter;
+		ops->pid_filter_ctrl = af9033_pid_filter_ctrl;
+	}
+
 	return &state->fe;
 
 err: