mesa xserver pixman git update 25 nov 2011

1 files changed, 727 insertions, 726 deletions

diff --git a/xorg-server/hw/xfree86/i2c/msp3430.c b/xorg-server/hw/xfree86/i2c/msp3430.c
index b58c3f09c..df8adc435 100644
--- a/xorg-server/hw/xfree86/i2c/msp3430.c
+++ b/xorg-server/hw/xfree86/i2c/msp3430.c
@@ -1,726 +1,727 @@
-#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 = calloc(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))

-   {

-       free(m->d.DevName);

-       free(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) {

-            free(m->d.DevName);

-            free(m);

-            return NULL;

-	}

-   if(!I2CDevInit(&(m->d)))

-   {

-       free(m->d.DevName);

-       free(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

-}

-

+#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);
+static const 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 = calloc(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))
+   {
+       free(m->d.DevName);
+       free(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) {
+            free(m->d.DevName);
+            free(m);
+            return NULL;
+	}
+   if(!I2CDevInit(&(m->d)))
+   {
+       free(m->d.DevName);
+       free(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
+
+
+static const 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
+}
+