V4L/DVB (3693): Fix msp3400c and bttv stereo/mono/bilingual detection/handling

- msp3400c did not detect the second carrier, thus being always mono.
- properly mute the msp3400c while detecting the carrier.
- fix checks on the presence of scart2/3 inputs and scart 2 output.
- implement proper audio mode fallbacks for msp3400c/d, identical to the
  way msp3400g works.
- MODE_STEREO no longer produces dual languages when set for a bilingual
  transmission, instead it falls back to LANG1. Use LANG1_LANG2 to hear
  both languages of a bilingual transmission. This is much more intuitive
  for the user and is in accordance with the preferred usage in the v4l2
  specification.
- bttv tried to implement v4l2 calls with v4l1 calls to the i2c devices,
  completely mangling the audmode/rxsubchans handling. v4l2 calls now do
  v4l2 calls to the i2c devices.
- fixed broken i2c_vidiocschan in bttv.
- add start/end lines to LOG_STATUS.

Signed-off-by: Hans Verkuil <hverkuil@xs4all.nl>
Signed-off-by: Mauro Carvalho Chehab <mchehab@infradead.org>

4 files changed, 138 insertions, 139 deletions

diff --git a/drivers/media/video/bt8xx/bttv-driver.c b/drivers/media/video/bt8xx/bttv-driver.c
index 74def9c23952..7913e2ec7a5b 100644
--- a/drivers/media/video/bt8xx/bttv-driver.c
+++ b/drivers/media/video/bt8xx/bttv-driver.c
@@ -1023,14 +1023,12 @@ audio_input(struct bttv *btv, int input)
 static void
 i2c_vidiocschan(struct bttv *btv)
 {
-	struct video_channel c;
+	v4l2_std_id std = bttv_tvnorms[btv->tvnorm].v4l2_id;
 
-	memset(&c,0,sizeof(c));
-	c.norm    = btv->tvnorm;
-	c.channel = btv->input;
-	bttv_call_i2c_clients(btv,VIDIOCSCHAN,&c);
+	bttv_call_i2c_clients(btv, VIDIOC_S_INPUT, &btv->input);
+	bttv_call_i2c_clients(btv, VIDIOC_S_STD, &std);
 	if (btv->c.type == BTTV_BOARD_VOODOOTV_FM)
-		bttv_tda9880_setnorm(btv,c.norm);
+		bttv_tda9880_setnorm(btv,btv->tvnorm);
 }
 
 static int
@@ -1184,11 +1182,27 @@ static int get_control(struct bttv *btv, struct v4l2_control *c)
 			break;
 	if (i == BTTV_CTLS)
 		return -EINVAL;
-	if (i >= 4 && i <= 8) {
+	if (btv->audio_hook && i >= 4 && i <= 8) {
 		memset(&va,0,sizeof(va));
-		bttv_call_i2c_clients(btv, VIDIOCGAUDIO, &va);
-		if (btv->audio_hook)
-			btv->audio_hook(btv,&va,0);
+		btv->audio_hook(btv,&va,0);
+		switch (c->id) {
+		case V4L2_CID_AUDIO_MUTE:
+			c->value = (VIDEO_AUDIO_MUTE & va.flags) ? 1 : 0;
+			break;
+		case V4L2_CID_AUDIO_VOLUME:
+			c->value = va.volume;
+			break;
+		case V4L2_CID_AUDIO_BALANCE:
+			c->value = va.balance;
+			break;
+		case V4L2_CID_AUDIO_BASS:
+			c->value = va.bass;
+			break;
+		case V4L2_CID_AUDIO_TREBLE:
+			c->value = va.treble;
+			break;
+		}
+		return 0;
 	}
 	switch (c->id) {
 	case V4L2_CID_BRIGHTNESS:
@@ -1205,19 +1219,11 @@ static int get_control(struct bttv *btv, struct v4l2_control *c)
 		break;
 
 	case V4L2_CID_AUDIO_MUTE:
-		c->value = (VIDEO_AUDIO_MUTE & va.flags) ? 1 : 0;
-		break;
 	case V4L2_CID_AUDIO_VOLUME:
-		c->value = va.volume;
-		break;
 	case V4L2_CID_AUDIO_BALANCE:
-		c->value = va.balance;
-		break;
 	case V4L2_CID_AUDIO_BASS:
-		c->value = va.bass;
-		break;
 	case V4L2_CID_AUDIO_TREBLE:
-		c->value = va.treble;
+		bttv_call_i2c_clients(btv,VIDIOC_G_CTRL,c);
 		break;
 
 	case V4L2_CID_PRIVATE_CHROMA_AGC:
@@ -1269,11 +1275,35 @@ static int set_control(struct bttv *btv, struct v4l2_control *c)
 			break;
 	if (i == BTTV_CTLS)
 		return -EINVAL;
-	if (i >= 4 && i <= 8) {
+	if (btv->audio_hook && i >= 4 && i <= 8) {
 		memset(&va,0,sizeof(va));
-		bttv_call_i2c_clients(btv, VIDIOCGAUDIO, &va);
-		if (btv->audio_hook)
-			btv->audio_hook(btv,&va,0);
+		btv->audio_hook(btv,&va,0);
+		switch (c->id) {
+		case V4L2_CID_AUDIO_MUTE:
+			if (c->value) {
+				va.flags |= VIDEO_AUDIO_MUTE;
+				audio_mute(btv, 1);
+			} else {
+				va.flags &= ~VIDEO_AUDIO_MUTE;
+				audio_mute(btv, 0);
+			}
+			break;
+
+		case V4L2_CID_AUDIO_VOLUME:
+			va.volume = c->value;
+			break;
+		case V4L2_CID_AUDIO_BALANCE:
+			va.balance = c->value;
+			break;
+		case V4L2_CID_AUDIO_BASS:
+			va.bass = c->value;
+			break;
+		case V4L2_CID_AUDIO_TREBLE:
+			va.treble = c->value;
+			break;
+		}
+		btv->audio_hook(btv,&va,1);
+		return 0;
 	}
 	switch (c->id) {
 	case V4L2_CID_BRIGHTNESS:
@@ -1289,26 +1319,13 @@ static int set_control(struct bttv *btv, struct v4l2_control *c)
 		bt848_sat(btv,c->value);
 		break;
 	case V4L2_CID_AUDIO_MUTE:
-		if (c->value) {
-			va.flags |= VIDEO_AUDIO_MUTE;
-			audio_mute(btv, 1);
-		} else {
-			va.flags &= ~VIDEO_AUDIO_MUTE;
-			audio_mute(btv, 0);
-		}
-		break;
-
+		audio_mute(btv, c->value);
+		/* fall through */
 	case V4L2_CID_AUDIO_VOLUME:
-		va.volume = c->value;
-		break;
 	case V4L2_CID_AUDIO_BALANCE:
-		va.balance = c->value;
-		break;
 	case V4L2_CID_AUDIO_BASS:
-		va.bass = c->value;
-		break;
 	case V4L2_CID_AUDIO_TREBLE:
-		va.treble = c->value;
+		bttv_call_i2c_clients(btv,VIDIOC_S_CTRL,c);
 		break;
 
 	case V4L2_CID_PRIVATE_CHROMA_AGC:
@@ -1364,11 +1381,6 @@ static int set_control(struct bttv *btv, struct v4l2_control *c)
 	default:
 		return -EINVAL;
 	}
-	if (i >= 4 && i <= 8) {
-		bttv_call_i2c_clients(btv, VIDIOCSAUDIO, &va);
-		if (btv->audio_hook)
-			btv->audio_hook(btv,&va,1);
-	}
 	return 0;
 }
 
@@ -1827,33 +1839,26 @@ static int bttv_common_ioctls(struct bttv *btv, unsigned int cmd, void *arg)
 			return -EINVAL;
 		mutex_lock(&btv->lock);
 		memset(t,0,sizeof(*t));
+		t->rxsubchans = V4L2_TUNER_SUB_MONO;
+		bttv_call_i2c_clients(btv, VIDIOC_G_TUNER, t);
 		strcpy(t->name, "Television");
-		t->type       = V4L2_TUNER_ANALOG_TV;
 		t->capability = V4L2_TUNER_CAP_NORM;
-		t->rxsubchans = V4L2_TUNER_SUB_MONO;
+		t->type       = V4L2_TUNER_ANALOG_TV;
 		if (btread(BT848_DSTATUS)&BT848_DSTATUS_HLOC)
 			t->signal = 0xffff;
-		{
-			struct video_tuner tuner;
-
-			memset(&tuner, 0, sizeof (tuner));
-			tuner.rangehigh = 0xffffffffUL;
-			bttv_call_i2c_clients(btv, VIDIOCGTUNER, &tuner);
-			t->rangelow = tuner.rangelow;
-			t->rangehigh = tuner.rangehigh;
-		}
-		{
+
+		if (btv->audio_hook) {
 			/* Hmmm ... */
 			struct video_audio va;
 			memset(&va, 0, sizeof(struct video_audio));
-			bttv_call_i2c_clients(btv, VIDIOCGAUDIO, &va);
-			if (btv->audio_hook)
-				btv->audio_hook(btv,&va,0);
+			btv->audio_hook(btv,&va,0);
+			t->audmode    = V4L2_TUNER_MODE_MONO;
+			t->rxsubchans = V4L2_TUNER_SUB_MONO;
 			if(va.mode & VIDEO_SOUND_STEREO) {
-				t->audmode     = V4L2_TUNER_MODE_STEREO;
-				t->rxsubchans |= V4L2_TUNER_SUB_STEREO;
+				t->audmode    = V4L2_TUNER_MODE_STEREO;
+				t->rxsubchans = V4L2_TUNER_SUB_STEREO;
 			}
-			if(va.mode & VIDEO_SOUND_LANG1) {
+			if(va.mode & VIDEO_SOUND_LANG2) {
 				t->audmode    = V4L2_TUNER_MODE_LANG1;
 				t->rxsubchans = V4L2_TUNER_SUB_LANG1
 					| V4L2_TUNER_SUB_LANG2;
@@ -1872,10 +1877,10 @@ static int bttv_common_ioctls(struct bttv *btv, unsigned int cmd, void *arg)
 		if (0 != t->index)
 			return -EINVAL;
 		mutex_lock(&btv->lock);
-		{
+		bttv_call_i2c_clients(btv, VIDIOC_S_TUNER, t);
+		if (btv->audio_hook) {
 			struct video_audio va;
 			memset(&va, 0, sizeof(struct video_audio));
-			bttv_call_i2c_clients(btv, VIDIOCGAUDIO, &va);
 			if (t->audmode == V4L2_TUNER_MODE_MONO)
 				va.mode = VIDEO_SOUND_MONO;
 			else if (t->audmode == V4L2_TUNER_MODE_STEREO ||
@@ -1885,9 +1890,7 @@ static int bttv_common_ioctls(struct bttv *btv, unsigned int cmd, void *arg)
 				va.mode = VIDEO_SOUND_LANG1;
 			else if (t->audmode == V4L2_TUNER_MODE_LANG2)
 				va.mode = VIDEO_SOUND_LANG2;
-			bttv_call_i2c_clients(btv, VIDIOCSAUDIO, &va);
-			if (btv->audio_hook)
-				btv->audio_hook(btv,&va,1);
+			btv->audio_hook(btv,&va,1);
 		}
 		mutex_unlock(&btv->lock);
 		return 0;
@@ -1912,7 +1915,7 @@ static int bttv_common_ioctls(struct bttv *btv, unsigned int cmd, void *arg)
 			return -EINVAL;
 		mutex_lock(&btv->lock);
 		btv->freq = f->frequency;
-		bttv_call_i2c_clients(btv,VIDIOCSFREQ,&btv->freq);
+		bttv_call_i2c_clients(btv,VIDIOC_S_FREQUENCY,f);
 		if (btv->has_matchbox && btv->radio_user)
 			tea5757_set_freq(btv,btv->freq);
 		mutex_unlock(&btv->lock);
@@ -1920,7 +1923,9 @@ static int bttv_common_ioctls(struct bttv *btv, unsigned int cmd, void *arg)
 	}
 	case VIDIOC_LOG_STATUS:
 	{
+		printk(KERN_INFO "bttv%d: =================  START STATUS CARD #%d  =================\n", btv->c.nr, btv->c.nr);
 		bttv_call_i2c_clients(btv, VIDIOC_LOG_STATUS, NULL);
+		printk(KERN_INFO "bttv%d: ==================  END STATUS CARD #%d  ==================\n", btv->c.nr, btv->c.nr);
 		return 0;
 	}
 
@@ -2870,12 +2875,10 @@ static int bttv_do_ioctl(struct inode *inode, struct file *file,
 			return 0;
 		}
 		*c = bttv_ctls[i];
-		if (i >= 4 && i <= 8) {
+		if (btv->audio_hook && i >= 4 && i <= 8) {
 			struct video_audio va;
 			memset(&va,0,sizeof(va));
-			bttv_call_i2c_clients(btv, VIDIOCGAUDIO, &va);
-			if (btv->audio_hook)
-				btv->audio_hook(btv,&va,0);
+			btv->audio_hook(btv,&va,0);
 			switch (bttv_ctls[i].id) {
 			case V4L2_CID_AUDIO_VOLUME:
 				if (!(va.flags & VIDEO_AUDIO_VOLUME))
diff --git a/drivers/media/video/msp3400-driver.c b/drivers/media/video/msp3400-driver.c
index 12a83ec7872c..027c3d35eaa5 100644
--- a/drivers/media/video/msp3400-driver.c
+++ b/drivers/media/video/msp3400-driver.c
@@ -283,19 +283,6 @@ void msp_set_scart(struct i2c_client *client, int in, int out)
 		msp_write_dem(client, 0x40, state->i2s_mode);
 }
 
-void msp_set_mute(struct i2c_client *client)
-{
-	struct msp_state *state = i2c_get_clientdata(client);
-
-	v4l_dbg(1, msp_debug, client, "mute audio\n");
-	msp_write_dsp(client, 0x0000, 0);
-	msp_write_dsp(client, 0x0007, 1);
-	if (state->has_scart2_out_volume)
-		msp_write_dsp(client, 0x0040, 1);
-	if (state->has_headphones)
-		msp_write_dsp(client, 0x0006, 0);
-}
-
 void msp_set_audio(struct i2c_client *client)
 {
 	struct msp_state *state = i2c_get_clientdata(client);
@@ -347,7 +334,6 @@ static void msp_wake_thread(struct i2c_client *client)
 
 	if (NULL == state->kthread)
 		return;
-	msp_set_mute(client);
 	state->watch_stereo = 0;
 	state->restart = 1;
 	wake_up_interruptible(&state->wq);
@@ -375,19 +361,15 @@ int msp_sleep(struct msp_state *state, int timeout)
 
 /* ------------------------------------------------------------------------ */
 
-static int msp_mode_v4l2_to_v4l1(int rxsubchans)
+static int msp_mode_v4l2_to_v4l1(int rxsubchans, int audmode)
 {
-	int mode = 0;
-
-	if (rxsubchans & V4L2_TUNER_SUB_STEREO)
-		mode |= VIDEO_SOUND_STEREO;
-	if (rxsubchans & V4L2_TUNER_SUB_LANG2)
-		mode |= VIDEO_SOUND_LANG2 | VIDEO_SOUND_STEREO;
-	if (rxsubchans & V4L2_TUNER_SUB_LANG1)
-		mode |= VIDEO_SOUND_LANG1 | VIDEO_SOUND_STEREO;
-	if (mode == 0)
-		mode |= VIDEO_SOUND_MONO;
-	return mode;
+	if (rxsubchans == V4L2_TUNER_SUB_MONO)
+		return VIDEO_SOUND_MONO;
+	if (rxsubchans == V4L2_TUNER_SUB_STEREO)
+		return VIDEO_SOUND_STEREO;
+	if (audmode == V4L2_TUNER_MODE_LANG2)
+		return VIDEO_SOUND_LANG2;
+	return VIDEO_SOUND_LANG1;
 }
 
 static int msp_mode_v4l1_to_v4l2(int mode)
@@ -606,7 +588,7 @@ static int msp_command(struct i2c_client *client, unsigned int cmd, void *arg)
 			break;
 		if (state->opmode == OPMODE_AUTOSELECT)
 			msp_detect_stereo(client);
-		va->mode = msp_mode_v4l2_to_v4l1(state->rxsubchans);
+		va->mode = msp_mode_v4l2_to_v4l1(state->rxsubchans, state->audmode);
 		break;
 	}
 
@@ -621,7 +603,8 @@ static int msp_command(struct i2c_client *client, unsigned int cmd, void *arg)
 		state->treble = va->treble;
 		msp_set_audio(client);
 
-		if (va->mode != 0 && state->radio == 0) {
+		if (va->mode != 0 && state->radio == 0 &&
+		    state->audmode != msp_mode_v4l1_to_v4l2(va->mode)) {
 			state->audmode = msp_mode_v4l1_to_v4l2(va->mode);
 			msp_set_audmode(client);
 		}
@@ -727,6 +710,8 @@ static int msp_command(struct i2c_client *client, unsigned int cmd, void *arg)
 
 		if (state->radio)  /* TODO: add mono/stereo support for radio */
 			break;
+		if (state->audmode == vt->audmode)
+			break;
 		state->audmode = vt->audmode;
 		/* only set audmode */
 		msp_set_audmode(client);
@@ -888,7 +873,7 @@ static int msp_attach(struct i2c_adapter *adapter, int address, int kind)
 
 	memset(state, 0, sizeof(*state));
 	state->v4l2_std = V4L2_STD_NTSC;
-	state->audmode = V4L2_TUNER_MODE_LANG1;
+	state->audmode = V4L2_TUNER_MODE_STEREO;
 	state->volume = 58880;	/* 0db gain */
 	state->balance = 32768;	/* 0db gain */
 	state->bass = 32768;
@@ -932,13 +917,16 @@ static int msp_attach(struct i2c_adapter *adapter, int address, int kind)
 	state->has_radio = msp_revision >= 'G';
 	/* Has headphones output: not for stripped down products */
 	state->has_headphones = msp_prod_lo < 5;
+	/* Has scart2 input: not in stripped down products of the '3' family */
+	state->has_scart2 = msp_family >= 4 || msp_prod_lo < 7;
+	/* Has scart3 input: not in stripped down products of the '3' family */
+	state->has_scart3 = msp_family >= 4 || msp_prod_lo < 5;
 	/* Has scart4 input: not in pre D revisions, not in stripped D revs */
 	state->has_scart4 = msp_family >= 4 || (msp_revision >= 'D' && msp_prod_lo < 5);
-	/* Has scart2 and scart3 inputs and scart2 output: not in stripped
-	   down products of the '3' family */
-	state->has_scart23_in_scart2_out = msp_family >= 4 || msp_prod_lo < 5;
+	/* Has scart2 output: not in stripped down products of the '3' family */
+	state->has_scart2_out = msp_family >= 4 || msp_prod_lo < 5;
 	/* Has scart2 a volume control? Not in pre-D revisions. */
-	state->has_scart2_out_volume = msp_revision > 'C' && state->has_scart23_in_scart2_out;
+	state->has_scart2_out_volume = msp_revision > 'C' && state->has_scart2_out;
 	/* Has a configurable i2s out? */
 	state->has_i2s_conf = msp_revision >= 'G' && msp_prod_lo < 7;
 	/* Has subwoofer output: not in pre-D revs and not in stripped down products */
diff --git a/drivers/media/video/msp3400-driver.h b/drivers/media/video/msp3400-driver.h
index 1940748bb633..b5834938df7d 100644
--- a/drivers/media/video/msp3400-driver.h
+++ b/drivers/media/video/msp3400-driver.h
@@ -54,8 +54,10 @@ struct msp_state {
 	u8 has_radio;
 	u8 has_headphones;
 	u8 has_ntsc_jp_d_k3;
+	u8 has_scart2;
+	u8 has_scart3;
 	u8 has_scart4;
-	u8 has_scart23_in_scart2_out;
+	u8 has_scart2_out;
 	u8 has_scart2_out_volume;
 	u8 has_i2s_conf;
 	u8 has_subwoofer;
@@ -98,7 +100,6 @@ int msp_read_dem(struct i2c_client *client, int addr);
 int msp_read_dsp(struct i2c_client *client, int addr);
 int msp_reset(struct i2c_client *client);
 void msp_set_scart(struct i2c_client *client, int in, int out);
-void msp_set_mute(struct i2c_client *client);
 void msp_set_audio(struct i2c_client *client);
 int msp_sleep(struct msp_state *state, int timeout);
 
diff --git a/drivers/media/video/msp3400-kthreads.c b/drivers/media/video/msp3400-kthreads.c
index 1a4564a7e92f..926095c6d826 100644
--- a/drivers/media/video/msp3400-kthreads.c
+++ b/drivers/media/video/msp3400-kthreads.c
@@ -170,7 +170,7 @@ static void msp_set_source(struct i2c_client *client, u16 src)
 	msp_write_dsp(client, 0x000a, src);
 	msp_write_dsp(client, 0x000b, src);
 	msp_write_dsp(client, 0x000c, src);
-	if (state->has_scart23_in_scart2_out)
+	if (state->has_scart2_out)
 		msp_write_dsp(client, 0x0041, src);
 }
 
@@ -240,15 +240,22 @@ static void msp3400c_set_audmode(struct i2c_client *client)
 		return;
 	}
 
-	/* If no second language is available, switch to the first language */
-	if ((audmode == V4L2_TUNER_MODE_LANG2 ||
-	     audmode == V4L2_TUNER_MODE_LANG1_LANG2) &&
-	    !(state->rxsubchans & V4L2_TUNER_SUB_LANG2))
-		audmode = V4L2_TUNER_MODE_LANG1;
-	/* switch to stereo for stereo transmission, otherwise
-	   keep first language */
-	if (audmode == V4L2_TUNER_MODE_LANG1 &&
-	    (state->rxsubchans & V4L2_TUNER_SUB_STEREO))
+	/* Note: for the C and D revs no NTSC stereo + SAP is possible as
+	   the hardware does not support SAP. So the rxsubchans combination
+	   of STEREO | LANG2 does not occur. */
+
+	/* switch to mono if only mono is available */
+	if (state->rxsubchans == V4L2_TUNER_SUB_MONO)
+		audmode = V4L2_TUNER_MODE_MONO;
+	/* if bilingual */
+	else if (state->rxsubchans & V4L2_TUNER_SUB_LANG2) {
+		/* and mono or stereo, then fallback to lang1 */
+		if (audmode == V4L2_TUNER_MODE_MONO ||
+		    audmode == V4L2_TUNER_MODE_STEREO)
+			audmode = V4L2_TUNER_MODE_LANG1;
+	}
+	/* if stereo, and audmode is not mono, then switch to stereo */
+	else if (audmode != V4L2_TUNER_MODE_MONO)
 		audmode = V4L2_TUNER_MODE_STEREO;
 
 	/* switch demodulator */
@@ -308,6 +315,7 @@ static void msp3400c_set_audmode(struct i2c_client *client)
 	}
 
 	/* switch audio */
+	v4l_dbg(1, msp_debug, client, "set audmode %d\n", audmode);
 	switch (audmode) {
 	case V4L2_TUNER_MODE_STEREO:
 	case V4L2_TUNER_MODE_LANG1_LANG2:
@@ -476,8 +484,9 @@ int msp3400c_thread(void *data)
 			continue;
 		}
 
-		/* mute */
-		msp_set_mute(client);
+		/* put into sane state (and mute) */
+		msp_reset(client);
+
 		msp3400c_set_mode(client, MSP_MODE_AM_DETECT);
 		val1 = val2 = 0;
 		max1 = max2 = -1;
@@ -560,7 +569,6 @@ int msp3400c_thread(void *data)
 				/* B/G NICAM */
 				state->second = msp3400c_carrier_detect_55[max2].cdo;
 				msp3400c_set_mode(client, MSP_MODE_FM_NICAM1);
-				msp3400c_set_carrier(client, state->second, state->main);
 				state->nicam_on = 1;
 				state->watch_stereo = 1;
 			} else {
@@ -571,7 +579,6 @@ int msp3400c_thread(void *data)
 			/* PAL I NICAM */
 			state->second = MSP_CARRIER(6.552);
 			msp3400c_set_mode(client, MSP_MODE_FM_NICAM2);
-			msp3400c_set_carrier(client, state->second, state->main);
 			state->nicam_on = 1;
 			state->watch_stereo = 1;
 			break;
@@ -585,13 +592,11 @@ int msp3400c_thread(void *data)
 				/* L NICAM or AM-mono */
 				state->second = msp3400c_carrier_detect_65[max2].cdo;
 				msp3400c_set_mode(client, MSP_MODE_AM_NICAM);
-				msp3400c_set_carrier(client, state->second, state->main);
 				state->watch_stereo = 1;
 			} else if (max2 == 0 && state->has_nicam) {
 				/* D/K NICAM */
 				state->second = msp3400c_carrier_detect_65[max2].cdo;
 				msp3400c_set_mode(client, MSP_MODE_FM_NICAM1);
-				msp3400c_set_carrier(client, state->second, state->main);
 				state->nicam_on = 1;
 				state->watch_stereo = 1;
 			} else {
@@ -603,13 +608,15 @@ int msp3400c_thread(void *data)
 		no_second:
 			state->second = msp3400c_carrier_detect_main[max1].cdo;
 			msp3400c_set_mode(client, MSP_MODE_FM_TERRA);
-			msp3400c_set_carrier(client, state->second, state->main);
 			state->rxsubchans = V4L2_TUNER_SUB_MONO;
 			break;
 		}
+		msp3400c_set_carrier(client, state->second, state->main);
 
-		/* unmute */
+		/* unmute, restore misc registers */
 		msp_set_audio(client);
+
+		msp_write_dsp(client, 0x13, state->acb);
 		msp3400c_set_audmode(client);
 
 		if (msp_debug)
@@ -617,12 +624,12 @@ int msp3400c_thread(void *data)
 
 		/* monitor tv audio mode, the first time don't wait
 		   so long to get a quick stereo/bilingual result */
-		if (msp_sleep(state, 1000))
-			goto restart;
+		count = 20;
 		while (state->watch_stereo) {
 			watch_stereo(client);
-			if (msp_sleep(state, 5000))
+			if (msp_sleep(state, count ? 200 : 5000))
 				goto restart;
+			if (count) count--;
 		}
 	}
 	v4l_dbg(1, msp_debug, client, "thread: exit\n");
@@ -634,7 +641,7 @@ int msp3410d_thread(void *data)
 {
 	struct i2c_client *client = data;
 	struct msp_state *state = i2c_get_clientdata(client);
-	int val, i, std;
+	int val, i, std, count;
 
 	v4l_dbg(1, msp_debug, client, "msp3410 daemon started\n");
 
@@ -775,12 +782,12 @@ int msp3410d_thread(void *data)
 
 		/* monitor tv audio mode, the first time don't wait
 		   so long to get a quick stereo/bilingual result */
-		if (msp_sleep(state, 1000))
-			goto restart;
+		count = 20;
 		while (state->watch_stereo) {
 			watch_stereo(client);
-			if (msp_sleep(state, 5000))
+			if (msp_sleep(state, count ? 200 : 5000))
 				goto restart;
+			if (count) count--;
 		}
 	}
 	v4l_dbg(1, msp_debug, client, "thread: exit\n");
@@ -837,20 +844,20 @@ static void msp34xxg_set_source(struct i2c_client *client, u16 reg, int in)
 		source = 0; /* mono only */
 		matrix = 0x30;
 		break;
-	case V4L2_TUNER_MODE_LANG1:
-		source = 3; /* stereo or A */
-		matrix = 0x00;
-		break;
 	case V4L2_TUNER_MODE_LANG2:
 		source = 4; /* stereo or B */
 		matrix = 0x10;
 		break;
-	case V4L2_TUNER_MODE_STEREO:
 	case V4L2_TUNER_MODE_LANG1_LANG2:
-	default:
 		source = 1; /* stereo or A|B */
 		matrix = 0x20;
 		break;
+	case V4L2_TUNER_MODE_STEREO:
+	case V4L2_TUNER_MODE_LANG1:
+	default:
+		source = 3; /* stereo or A */
+		matrix = 0x00;
+		break;
 	}
 
 	if (in == MSP_DSP_OUT_TUNER)
@@ -877,7 +884,7 @@ static void msp34xxg_set_sources(struct i2c_client *client)
 	msp34xxg_set_source(client, 0x000c, (in >> 4) & 0xf);
 	msp34xxg_set_source(client, 0x0009, (in >> 8) & 0xf);
 	msp34xxg_set_source(client, 0x000a, (in >> 12) & 0xf);
-	if (state->has_scart23_in_scart2_out)
+	if (state->has_scart2_out)
 		msp34xxg_set_source(client, 0x0041, (in >> 16) & 0xf);
 	msp34xxg_set_source(client, 0x000b, (in >> 20) & 0xf);
 }