path: root/hw/xfree86/i2c/msp3430.c

#ifdef HAVE_XORG_CONFIG_H
#include <xorg-config.h>
#endif

#include <string.h>
#include <unistd.h>

#include "xf86.h"
#include "xf86i2c.h"
#include "msp3430.h"
#include "i2c_def.h"

#define CONTROL         0x00
#define WR_DEM          0x10
#define RD_DEM          0x11
#define WR_DSP          0x12
#define RD_DSP          0x13


void InitMSP34xxG(MSP3430Ptr m);
void InitMSP34x5D(MSP3430Ptr m);
void CheckModeMSP34x5D(MSP3430Ptr m);
char *MSP_getProductName (CARD16 product_id);
void mpause(int milliseconds);

#define __MSPDEBUG__	0

#if __MSPDEBUG__ > 3

void MSPBeep(MSP3430Ptr m, CARD8 freq);
#define __MSPBEEP MSPBeep(m,0x14);

#else

#define __MSPBEEP 
#endif

static void SetMSP3430Control(MSP3430Ptr m, CARD8 RegAddress, CARD8 RegValueHigh, CARD8 RegValueLow)
{
   I2CByte data[3];
   
   data[0]=RegAddress;
   data[1]=RegValueHigh;
   data[2]=RegValueLow;
   
   I2C_WriteRead(&(m->d),data,3,NULL,0);
}

static void SetMSP3430Data(MSP3430Ptr m, CARD8 RegAddress, CARD8 RegSubAddressHigh, CARD8 RegSubAddressLow, 
     CARD8 RegValueHigh, CARD8 RegValueLow)
{
   I2CByte data[5];
#ifdef MSP_DEBUG
   if(!m->registers_present[RegSubAddressLow]){
   	xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_ERROR, "Attempt to access non-existent register in MSP34xxX: 0x%02x 0x%02x 0x%02x <- 0x%02x 0x%02x\n",
		RegAddress, RegSubAddressHigh, RegSubAddressLow, RegValueHigh, RegValueLow);
   	}
#endif
   
   data[0] = RegAddress;
   data[1] = RegSubAddressHigh;
   data[2] = RegSubAddressLow;
   data[3] = RegValueHigh;
   data[4] = RegValueLow;
   
   I2C_WriteRead(&(m->d),data,5,NULL,0);
}

static void GetMSP3430Data(MSP3430Ptr m, CARD8 RegAddress, CARD8 RegSubAddressHigh, CARD8 RegSubAddressLow,
            CARD8 *RegValueHigh, CARD8 *RegValueLow)
{
   I2CByte  send[3];
   I2CByte  receive[2];
   
   send[0] = RegAddress;
   send[1] = RegSubAddressHigh;
   send[2] = RegSubAddressLow;
   
   I2C_WriteRead(&(m->d), send, 3, receive, 2);
   
   *RegValueHigh = receive[0];
   *RegValueLow = receive[1];
}

#if __MSPDEBUG__ > 2
static void MSP3430DumpStatus(MSP3430Ptr m)
{
CARD8 status_hi, status_lo;
CARD8 subaddr, data[2];

GetMSP3430Data(m, RD_DEM, 0x02, 0x00, &status_hi, &status_lo);
xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP34xx: SAP(8)=%d mono/NICAM(7)=%d stereo=%d %s O_1=%d O_0=%d 2nd car=%d 1st car=%d\n",
		status_hi & 1, (status_lo>>7) & 1, (status_lo>>6)&1, 
		(status_lo>>5)? ( (status_hi>>1)&1? "bad NICAM reception" : "NICAM" ) : 
		                ((status_hi>>1)&1 ? "bogus" : "ANALOG FM/AM") , 
		(status_lo>>4)&1, (status_lo>>3)&1,!( (status_lo>>2)&1), !((status_lo>>1)&1));

GetMSP3430Data(m, RD_DEM, 0x00, 0x7E, &status_hi, &status_lo);
xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP34xx: standard result=0x%02x%02x\n",
		status_hi, status_lo);
subaddr=0x0;
I2C_WriteRead(&(m->d), &subaddr, 1, data, 2);
xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP34xx: control=0x%02x%02x\n",
		data[1], data[0]);
}
#endif

/* wrapper */
void InitMSP3430(MSP3430Ptr m)
{
  #if __MSPDEBUG__ > 1
  xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"InitMSP3430(m->connector=%d, m->standard=%d, m->chip_family=%d)\n",
  		m->connector, m->standard, m->chip_family);
  #endif
	switch (m->chip_family) {
		case MSPFAMILY_34x0G:
			InitMSP34xxG(m);
			break;
		case MSPFAMILY_34x5G:
			InitMSP34xxG(m);
			break;
		case MSPFAMILY_34x5D:
			InitMSP34x5D(m);
			break;
	}
}

/*-----------------------------------------------------------------
| common functions for all MSP34xx chips 
|----------------------------------------------------------------*/

MSP3430Ptr DetectMSP3430(I2CBusPtr b, I2CSlaveAddr addr)
{
   MSP3430Ptr m;
   I2CByte a;
   CARD8 hardware_version, major_revision, product_code, rom_version;
   Bool supported;
   
   m = xcalloc(1,sizeof(MSP3430Rec));
   if(m == NULL)return NULL;
   m->d.DevName = strdup("MSP34xx");
   m->d.SlaveAddr = addr;
   m->d.pI2CBus = b;
   m->d.NextDev = NULL;
   m->d.StartTimeout = b->StartTimeout;
   m->d.BitTimeout = b->BitTimeout;
   m->d.AcknTimeout = b->AcknTimeout;
   m->d.ByteTimeout = b->ByteTimeout;
   
   if(!I2C_WriteRead(&(m->d), NULL, 0, &a, 1))
   {
   	xfree(m->d.DevName);
   	xfree(m);
	return NULL;
    }


	m->standard=MSP3430_NTSC;
	m->connector=MSP3430_CONNECTOR_1;
	m->mode=MSPMODE_STEREO_A;	/*stereo or chanel A if avail. */
    	m->c_format=MSPFORMAT_UNKNOWN;
    	m->c_standard=MSPSTANDARD_UNKNOWN;
    	m->c_matrix=m->c_fmmatrix=m->c_source=0;
	m->volume=0;
	m->recheck=FALSE;

   GetMSP3430Data(m, RD_DSP, 0x00, 0x1E, &hardware_version, &major_revision);
   GetMSP3430Data(m, RD_DSP, 0x00, 0x1F, &product_code, &rom_version);
   m->hardware_version=hardware_version;
   m->major_revision=major_revision;
   m->product_code=product_code;
   m->rom_version=rom_version;

   m->chip_id=((major_revision << 8) | product_code);
   
   supported=FALSE;
   switch (major_revision) {
   case 4:	/* 34xxD */
      switch (product_code) {
	  case 0x05: /* 3405D */
	  case 0x0A: /* 3410D */
	  case 0x0F: /* 3415D */
	  	m->chip_family=MSPFAMILY_34x5D;
		m->recheck=TRUE;
		supported=TRUE;
		break;
      default:
	  	m->chip_family=MSPFAMILY_34x0D;
      }
	  break;
   case 7:	/* 34xxG */
   	switch(product_code){
		case 0x00:
		case 0x0A:
		case 0x1E:
		case 0x28:
		case 0x32:
		  	m->chip_family=MSPFAMILY_34x0G;
			supported=TRUE;
			break;
		case 0x0f:
		case 0x19:
		case 0x2d:
		case 0x37:
		case 0x41:
		  	m->chip_family=MSPFAMILY_34x5G;
			supported=TRUE;
			#ifdef MSP_DEBUG
			memset(m->registers_present, 0, 256);
			#define A(num) m->registers_present[(num)]=1;
			#define B(num1, num2) memset(&(m->registers_present[num1]), 1, num2-num1);
			A(0x20)
			A(0x30)
			A(0x40)
			A(0x00)
			B(0x01, 0x08)
			B(0x0B, 0x0E)
			A(0x10)
			B(0x12,0x14)
			A(0x16)
			A(0x29)
			#undef B
			#undef A
			#endif
			break;
		default:
		  	m->chip_family=MSPFAMILY_UNKNOWN;
		}
		break;
   default:
	  	m->chip_family=MSPFAMILY_UNKNOWN;
   }
   
	xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "Found %s%s, rom version 0x%02x, chip_id=0x%04x\n",
		MSP_getProductName(m->chip_id), supported?"":" (unsupported)", rom_version, m->chip_id);

	if (!supported) {
       		xfree(m->d.DevName);
	   	xfree(m);
		return NULL;
	}	
   if(!I2CDevInit(&(m->d)))
   {
       xfree(m->d.DevName);
       xfree(m);
       return NULL;
   }

   return m;  
}

void ResetMSP3430(MSP3430Ptr m)
{
    /* Reset the MSP3430 */
    SetMSP3430Control(m, 0x00, 0x80, 0x00);
    /* Set it back to normal operation */
    SetMSP3430Control(m, 0x00, 0x00, 0x00);

    m->c_format=MSPFORMAT_UNKNOWN;
    m->c_standard=MSPSTANDARD_UNKNOWN;
    m->c_matrix=m->c_fmmatrix=m->c_source=0;
	m->volume=0;
}   

void MSP3430SetVolume (MSP3430Ptr m, CARD8 value)
{
    CARD8 result;
#if 0
    CARD8 old_volume;
    GetMSP3430Data(m, RD_DSP, 0x00, 0x00, &old_volume, &result);   
    xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP3430 result 0x%02x\n", result);
#endif
    /* save an extra Get call */
    result=0;

    SetMSP3430Data(m, WR_DSP, 0x00, 0x00, value, result);

    SetMSP3430Data(m, WR_DSP, 0x00, 0x07, value, 0);
    m->volume=value;

#if __MSPDEBUG__ > 2
    MSP3430DumpStatus(m); 
    __MSPBEEP
    GetMSP3430Data(m, RD_DSP, 0x00, 0x00, &old_volume, &result);
    xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP3430 volume 0x%02x\n",value);
#endif
}


void MSP3430SetSAP (MSP3430Ptr m, int mode)
{
	xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "Put actual code to change SAP here\n");

      SetMSP3430Data(m, WR_DSP, 0x00, 0x08, mode & 0xff, 0x20); 
}


#if 0
void MSP3430SetSource(MSP3430Ptr m, CARD8 value)
{
    /* Write to DSP, register 0x0008, (loudspeaker channel source/matrix) */
    /* This sets the source to the TV tuner, for stereo operation */
    SetMSP3430Data(m, WR_DSP, 0x00, 0x08, value, 0x20);
}
#endif


char *MSP_getProductName (CARD16 product_id)
{
	switch (product_id) {
		case 0x0400: return "MSP3400D";
		case 0x040a: return "MSP3410D";
		case 0x0405: return "MSP3405D";
		case 0x040f: return "MSP3415D";
		case 0x0700: return "MSP3400G";
		case 0x070a: return "MSP3410G";
		case 0x071e: return "MSP3430G";
		case 0x0728: return "MSP3440G";
		case 0x0732: return "MSP3450G";
		case 0x070f: return "MSP3415G";
		case 0x0719: return "MSP3425G";
		case 0x072d: return "MSP3445G";
		case 0x0737: return "MSP3455G";
		case 0x0741: return "MSP3465G";
	}
	return "MSP - unknown type";
}

#if __MSPDEBUG__ > 2
/*puts beep in MSP output
    freq = 0x01 - 16Hz ... 0x40 - 1kHz ... 0xff - 4kHz  
*/
void MSPBeep(MSP3430Ptr m, CARD8 freq) {
    SetMSP3430Data (m, WR_DSP, 0x00, freq, 0x7f, 0x40);
    mpause(100);
    SetMSP3430Data (m, WR_DSP, 0x00, 0x14, 0x00, 0x00);
}
#endif

void mpause(int milliseconds) {
    int i,m;
    m=milliseconds/20;
    for (i=0;i<m;i++) usleep(20000);
}

/*-----------------------------------------------------------------
| specific functions for all MSP34xxG chips 
|----------------------------------------------------------------*/

void InitMSP34xxG(MSP3430Ptr m)
{

  #if __MSPDEBUG__ > 1
  xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"InitMSP34xxG(m->connector=%d, m->standard=%d, m->chip_family=%d)\n",
  		m->connector, m->standard, m->chip_family);
  #endif
   /* Reset MSP3430 */
   SetMSP3430Control(m, 0x00, 0x80, 0x00);
   /* Set it back to normal operation */
   SetMSP3430Control(m, 0x00, 0x00, 0x00);
   
   /*set MODUS register */
   /* bits: 0 - automatic sound detection */
   /*       1 - enable STATUS change */
   /*       12 - detect 6.5 Mhz carrier as D/K1, D/K2 or D/K NICAM  (does not seem to work ) */
   /*       13 - detect 4.5 Mhz carrier as BTSC */ 
   if ( (m->standard & 0xff) == MSP3430_PAL )
   {
      SetMSP3430Data(m, WR_DEM, 0x00, 0x30, 0x30, 0x03|0x08);    /* make O_ pins tristate */
      /* PAL standard */
      SetMSP3430Data(m, WR_DEM, 0x00, 0x20, 0x00, 0x01); /* possibly wrong */
   } else {
      SetMSP3430Data(m, WR_DEM, 0x00, 0x30, 0x20, 0x03|0x08);
      /* standard selection is M-BTSC-Stereo */
      SetMSP3430Data(m, WR_DEM, 0x00, 0x20, 0x00, 0x20); 
   }
   
   switch(m->connector){
         case MSP3430_CONNECTOR_1:
	        SetMSP3430Data(m, WR_DSP, 0x00, 0x08, 0x03, 0x20);
	        break;
	 case MSP3430_CONNECTOR_2:
		/* this has not been checked yet.. could be bogus */
    		/* SCART Input Prescale: 0 dB gain */
		SetMSP3430Data(m, WR_DSP, 0x00, 0x0d, 0x19, 0x00);
    		SetMSP3430Data(m, WR_DSP, 0x00, 0x08, 0x02, 0x20);   	
		break;
	case MSP3430_CONNECTOR_3:
	default:
    		/* SCART Input Prescale: 0 dB gain */
		SetMSP3430Data(m, WR_DSP, 0x00, 0x0d, 0x19, 0x00);
		
    		SetMSP3430Data(m, WR_DSP, 0x00, 0x08, 0x02, 0x20);
    		break;
	}
	      
    switch(m->standard){
        case MSP3430_PAL:
     		SetMSP3430Data(m, WR_DSP, 0x00, 0x0e, 0x24, 0x03);
     		SetMSP3430Data(m, WR_DSP, 0x00, 0x10, 0x00, 0x5a);
                SetMSP3430Data(m, WR_DEM, 0x00, 0x20, 0x00, 0x03);
                /* Set volume to FAST_MUTE. */
     	        SetMSP3430Data(m, WR_DSP, 0x00, 0x00, 0xFF, 0x00);
	        break;
        case MSP3430_PAL_DK1:
     		SetMSP3430Data(m, WR_DSP, 0x00, 0x0e, 0x24, 0x03);
     		SetMSP3430Data(m, WR_DSP, 0x00, 0x10, 0x00, 0x5a);
		SetMSP3430Data(m, WR_DEM, 0x00, 0x20, 0x00, 0x04);
                /* Set volume to FAST_MUTE. */
     	        SetMSP3430Data(m, WR_DSP, 0x00, 0x00, 0xFF, 0x00);
		break;
	case MSP3430_SECAM: /* is this right ? */
        case MSP3430_NTSC:         
                /* Write to DSP, register 0x000E, (prescale FM/FM matrix) */
                SetMSP3430Data(m, WR_DSP, 0x00, 0x0e, 0x24, 0x03);

                /* Set volume to FAST_MUTE. */
                SetMSP3430Data(m, WR_DSP, 0x00, 0x00, 0xFF, 0x00);
		break;
	}

}

/*-----------------------------------------------------------------
| specific functions for all MSP34x5D chips 
|----------------------------------------------------------------*/

void InitMSP34x5D(MSP3430Ptr m)
{
int count;
CARD8 high,low;
CARD16 result,standard;
CARD16 peak;


if (m->c_format==MSPFORMAT_UNKNOWN) ResetMSP3430(m);
else {
    /*mute volume*/
    SetMSP3430Data (m, WR_DSP, 0x00, 0x00, 0x00, 0x00);
}



    switch(m->connector){
	case MSP3430_CONNECTOR_2:
	case MSP3430_CONNECTOR_3:
	    if (m->c_format!=MSPFORMAT_SCART) {
    		/* SCART Input Prescale: 0 dB gain */
			SetMSP3430Data (m, WR_DSP, 0x00, 0x0d, 0x19, 0x00);
			/* this has not been checked yet.. could be bogus */
			m->c_format=MSPFORMAT_SCART;	/*stereo*/
		}
	    break;
	case MSP3430_CONNECTOR_1:
	default:
	
	    switch ( m->standard & 0x00ff ) {
		case MSP3430_PAL:
			switch( m->standard ) {
			case MSP3430_PAL_DK1:
			    standard=MSPSTANDARD_FM_DK1;
			    break;
/*			case MSP3430_PAL_DK2:
			    standard=MSPSTANDARD_FM_DK2;
			    break;
			case MSP3430_PAL_BG:
			may be FM stereo (Germany) or FM NICAM (Scandinavia,spain)
			    standard=MSPSTANDARD_AUTO;
			    break;
*/
			default:
			    standard=MSPSTANDARD_AUTO;
			}
		    break;
		case MSP3430_SECAM:
		    standard=MSPSTANDARD_AUTO;
		case MSP3430_NTSC:
			    /* Only MSP34x5 supported format - Korean NTSC-M*/
			 standard=MSPSTANDARD_FM_M;
		default:
		    standard=MSPSTANDARD_AUTO;
		}
		
	    /*no NICAM support in MSP3410D - force to autodetect*/
	    if ((m->chip_id==0x405) && (standard>=MSPSTANDARD_NICAM_BG))
    		standard=MSPSTANDARD_AUTO;
		    
	    if (m->c_standard != standard) {

   	        SetMSP3430Data (m, WR_DEM, 0x00, 0x20, standard>>8, standard & 0xFF);
	        if (standard==MSPSTANDARD_AUTO) {
			    count = 50; /* time shouldn't exceed 1s, just in case */
			    do {
     		        usleep(20000);
     		        GetMSP3430Data (m, RD_DEM, 0x00, 0x7e, &high, &low);
			        result = ( high << 8 ) | low;
     		        --count;
	    	    } while( result > 0x07ff && count > 0 );
		    
		    	if ((result > MSPSTANDARD_AUTO))
					standard=result;
		    	else standard=MSPSTANDARD_UNKNOWN;
#if __MSPDEBUG__ > 1
				xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"Detected audio standard: %d\n",result);
#endif		    	
		    	/* result = MSPSTANDARD_NICAM_L can be one of:
		    	SECAM_L - MSPSTANDARD_NICAM_L
		    	D/K1 - MSPSTANDARD_FM_DK1
		    	D/K2 - MSPSTANDARD_FM_DK2
		    	D/K-NICAM - MSPSTANDARD_NICAM_DK*/
		    	if( standard == MSPSTANDARD_NICAM_L ) {
		        	if ((m->standard & 0x00ff)==MSP3430_PAL) {
			    		/* force PAL D/K  */
			    		standard=MSPSTANDARD_FM_DK1;
		            	SetMSP3430Data (m, WR_DEM, 0x00, 0x20, standard>>8, standard & 0xFF);
#if __MSPDEBUG__ > 1
			        	xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO, "Detected 6.5MHz carrier - forced to D/K1 !!!\n" );
#endif
					}
		    	}
			}
			m->c_standard=standard;
	    } /*end - standard changed*/
	    else {
			if (standard<MSPSTANDARD_NICAM_BG) {
    		    /* get old value of ident. mode register*/
		    	GetMSP3430Data (m, RD_DSP, 0x00, 0x15, &high, &low);
    		    /* reset Ident-Filter */
    		    SetMSP3430Data (m, WR_DSP, 0x00, 0x14, 0x00, 0x3F);
		    	/* put back old value to ident. mode register*/
    		    SetMSP3430Data (m, WR_DSP, 0x00, 0x14, 0x00, low);
			}
	    }

	    if (standard<=MSPSTANDARD_AUTO) {
    	   	m->c_format=MSPFORMAT_1xFM;
	    }
	    else if (standard<MSPSTANDARD_NICAM_BG) {
			/* set FM prescale */
			SetMSP3430Data (m, WR_DSP, 0x00, 0x0e, 0x30, 0);
			/* set FM deemphasis*/
			SetMSP3430Data (m, WR_DSP, 0x00, 0x0f, ((standard==MSPSTANDARD_FM_M)?0:1), 0);
		    	
			/* check if FM2 carrier is present */
			/*turn off FM DC Notch*/
			SetMSP3430Data (m, WR_DSP, 0x00, 0x17, 0x00, 0x3f);
			/*matrix source for Quasi-Peak Detector - stereo: ch2->L ch1->R*/
			SetMSP3430Data (m, WR_DSP, 0x00, 0x0c, 0x00, 0x20);
			
			mpause(250);
   		    GetMSP3430Data (m, RD_DSP, 0x00, 0x1A, &high, &low);
			peak = (high << 8) | low;
#if __MSPDEBUG__ > 1
			xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"Second carrier Quasi-Peak detection: %d\n",peak);
#endif	    	
			/*turn on FM DC Notch*/
			SetMSP3430Data (m, WR_DSP, 0x00, 0x17, 0x00, 0x00);
	    	
			if (peak<5) {
	    	    /* if second carrier not detected - only mono from first carrier*/
	    	    m->c_format=MSPFORMAT_1xFM;
			}
			else {
	    	    m->c_format=MSPFORMAT_2xFM;
    	   	    /*start of FM identification process - FM_WAIT
	    	    wait at least 0.5s - used 1s - gives beter resolution*/
    	   	    mpause(1000);
			}
	    }
	    else {
			if (standard==MSPSTANDARD_NICAM_L) {
	    	    m->c_format=MSPFORMAT_NICAM_AM;
		    	/* set AM prescale */
		    	SetMSP3430Data (m, WR_DSP, 0x00, 0x0e, 0x7C, 0);
			}
			else {
	    	    m->c_format=MSPFORMAT_NICAM_FM;
		    	/* set FM prescale */
			    SetMSP3430Data (m, WR_DSP, 0x00, 0x0e, 0x30, 0);
			}
			/* set FM deemphasis*/
			SetMSP3430Data (m, WR_DSP, 0x00, 0x0f, 0x00, 0);
			/* set NICAM prescale to 0dB */
			SetMSP3430Data (m, WR_DSP, 0x00, 0x10, 0x20, 0);
	    }
	    
	    break;
	} /*end - case conector*/

    CheckModeMSP34x5D(m);
    
    /* Set volume to FAST_MUTE. */
    /*SetMSP3430Data(m, WR_DSP, 0x00, 0x00, 0xFF, 0x00);*/
    /*set volume*/
	MSP3430SetVolume(m,m->volume);
    
    __MSPBEEP


} /* EnableMSP34x5D ()... */




void CheckModeMSP34x5D(MSP3430Ptr m) {
    const char stereo_on=25;
    const char stereo_off=20;
    const char dual_on=-stereo_on;
    const char dual_off=-stereo_off;
    char detect;
    CARD8 matrix, fmmatrix, source, high, low;

    fmmatrix=0;		/*no matrix*/
    source=0;		/*FM*/
    switch (m->c_format) {
	case MSPFORMAT_NICAM_FM:
	case MSPFORMAT_NICAM_AM:
	case MSPFORMAT_SCART:
	    source=( (m->c_format == MSPFORMAT_SCART)?2:1 );
	    switch (m->mode) {
		case MSPMODE_MONO:
		    matrix=0x30;	/*MONO*/
		    break;
		case MSPMODE_A:
		    matrix=0x00;	/*A*/
		    break;
		case MSPMODE_B:
		    matrix=0x10;	/*B*/
		    break;
		default:
		    matrix=0x20;	/*STEREO*/
		    break;
		}
	    break;
	default:
	case MSPFORMAT_1xFM:
	    matrix=0x00;	/*A*/
	    break;
	case MSPFORMAT_2xFM:
	    switch (m->mode) {
		case MSPMODE_MONO:
		    matrix=0x30;	/*MONO*/
		    break;
		case MSPMODE_STEREO:
		    matrix=0x20;	/*STEREO*/
		    fmmatrix=((m->c_standard==MSPSTANDARD_FM_M)?2:1);
		    break;
		case MSPMODE_AB:
		    matrix=0x20;	/*STEREO*/
		    break;
		case MSPMODE_A:
		    matrix=0x00;	/*A*/
		    break;
		case MSPMODE_B:
		    matrix=0x10;	/*B*/
		    break;
		default:
    		/*FM_IDENT_CHECK*/
    		GetMSP3430Data (m, RD_DSP, 0x00, 0x18, &high, &low);
    		detect=(char)high;
#if __MSPDEBUG__ > 1
    		xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"Stereo Detection Register: %d\n",detect);
#endif
    		if (detect>=((m->c_mode==MSPMODE_STEREO)?stereo_off:stereo_on)) {
				m->c_mode=MSPMODE_STEREO;
				matrix=0x20;	/*STEREO*/
				fmmatrix=((m->c_standard==MSPSTANDARD_FM_M)?2:1);
		    }
			else if (detect<=((m->c_mode==MSPMODE_AB)?dual_off:dual_on)) {
				m->c_mode=MSPMODE_AB;
    			switch (m->mode) {
			    case MSPMODE_STEREO_AB: matrix=0x20; break;
			    case MSPMODE_STEREO_B: matrix=0x10; break;
			    default:
				case MSPMODE_A: matrix=0x00; break;
				}
			}
    		else {
				m->c_mode=MSPMODE_MONO;
				matrix=0x30;	/*MONO*/
			}
		    break;
	    } /* end - case mode*/
	    break;
    }

    if (m->c_fmmatrix != fmmatrix) {
        GetMSP3430Data (m, RD_DSP, 0x00, 0x0e, &high, &low);
		SetMSP3430Data (m, WR_DSP, 0x00, 0x0e, high, fmmatrix);
		m->c_fmmatrix = fmmatrix;
    }

    if ((m->c_matrix != matrix) || (m->c_source != source)) {
        /*set chanel source and matrix for loudspeaker*/
		SetMSP3430Data (m, WR_DSP, 0x00, 0x08, source, matrix);
    	
		m->c_matrix = matrix;
		m->c_source = source;
    }

	if ( ((m->c_format) & 0xF0) == MSPFORMAT_NICAM)
			SetMSP3430Data (m, WR_DEM, 0x00, 0x21, 0, 1);

#if __MSPDEBUG__ > 0
		    char *msg;
		    switch (matrix) {
			case 0x30: /*MONO*/
			    msg="MONO";
			    break;
			case 0x00: /*LEFT*/
			    msg="MONO/CHANNEL_1";
			    break;
			case 0x10: /*RIGHT*/
			    msg="MONO/CHANNEL_2";
			    break;
			case 0x20: /*LEFT*/
			    msg="STEREO";
			    break;
			default:
			    msg="unknown";
			    break;
		    }
    		    xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"Audio mode set to: %s\n",msg);
#endif
}