xorg-server-1.4/hw/xfree86/i2c/msp3430.c

   1 #ifdef HAVE_XORG_CONFIG_H
   2 #include <xorg-config.h>
   3 #endif
   4
   5 #include <string.h>
   6 #include <unistd.h>
   7
   8 #include "xf86.h"
   9 #include "xf86i2c.h"
  10 #include "msp3430.h"
  11 #include "i2c_def.h"
  12
  13 #define CONTROL         0x00
  14 #define WR_DEM          0x10
  15 #define RD_DEM          0x11
  16 #define WR_DSP          0x12
  17 #define RD_DSP          0x13
  18
  19
  20 void InitMSP34xxG(MSP3430Ptr m);
  21 void InitMSP34x5D(MSP3430Ptr m);
  22 void CheckModeMSP34x5D(MSP3430Ptr m);
  23 char *MSP_getProductName (CARD16 product_id);
  24 void mpause(int milliseconds);
  25
  26 #define __MSPDEBUG__    0
  27
  28 #if __MSPDEBUG__ > 3
  29
  30 void MSPBeep(MSP3430Ptr m, CARD8 freq);
  31 #define __MSPBEEP MSPBeep(m,0x14);
  32
  33 #else
  34
  35 #define __MSPBEEP
  36 #endif
  37
  38 static void SetMSP3430Control(MSP3430Ptr m, CARD8 RegAddress, CARD8 RegValueHigh, CARD8 RegValueLow)
  39 {
  40    I2CByte data[3];
  41
  42    data[0]=RegAddress;
  43    data[1]=RegValueHigh;
  44    data[2]=RegValueLow;
  45
  46    I2C_WriteRead(&(m->d),data,3,NULL,0);
  47 }
  48
  49 static void SetMSP3430Data(MSP3430Ptr m, CARD8 RegAddress, CARD8 RegSubAddressHigh, CARD8 RegSubAddressLow,
  50      CARD8 RegValueHigh, CARD8 RegValueLow)
  51 {
  52    I2CByte data[5];
  53 #ifdef MSP_DEBUG
  54    if(!m->registers_present[RegSubAddressLow]){
  55         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",
  56                 RegAddress, RegSubAddressHigh, RegSubAddressLow, RegValueHigh, RegValueLow);
  57         }
  58 #endif
  59
  60    data[0] = RegAddress;
  61    data[1] = RegSubAddressHigh;
  62    data[2] = RegSubAddressLow;
  63    data[3] = RegValueHigh;
  64    data[4] = RegValueLow;
  65
  66    I2C_WriteRead(&(m->d),data,5,NULL,0);
  67 }
  68
  69 static void GetMSP3430Data(MSP3430Ptr m, CARD8 RegAddress, CARD8 RegSubAddressHigh, CARD8 RegSubAddressLow,
  70             CARD8 *RegValueHigh, CARD8 *RegValueLow)
  71 {
  72    I2CByte  send[3];
  73    I2CByte  receive[2];
  74
  75    send[0] = RegAddress;
  76    send[1] = RegSubAddressHigh;
  77    send[2] = RegSubAddressLow;
  78
  79    I2C_WriteRead(&(m->d), send, 3, receive, 2);
  80
  81    *RegValueHigh = receive[0];
  82    *RegValueLow = receive[1];
  83 }
  84
  85 #if __MSPDEBUG__ > 2
  86 static void MSP3430DumpStatus(MSP3430Ptr m)
  87 {
  88 CARD8 status_hi, status_lo;
  89 CARD8 subaddr, data[2];
  90
  91 GetMSP3430Data(m, RD_DEM, 0x02, 0x00, &status_hi, &status_lo);
  92 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",
  93                 status_hi & 1, (status_lo>>7) & 1, (status_lo>>6)&1,
  94                 (status_lo>>5)? ( (status_hi>>1)&1? "bad NICAM reception" : "NICAM" ) :
  95                                 ((status_hi>>1)&1 ? "bogus" : "ANALOG FM/AM") ,
  96                 (status_lo>>4)&1, (status_lo>>3)&1,!( (status_lo>>2)&1), !((status_lo>>1)&1));
  97
  98 GetMSP3430Data(m, RD_DEM, 0x00, 0x7E, &status_hi, &status_lo);
  99 xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP34xx: standard result=0x%02x%02x\n",
 100                 status_hi, status_lo);
 101 subaddr=0x0;
 102 I2C_WriteRead(&(m->d), &subaddr, 1, data, 2);
 103 xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP34xx: control=0x%02x%02x\n",
 104                 data[1], data[0]);
 105 }
 106 #endif
 107
 108 /* wrapper */
 109 void InitMSP3430(MSP3430Ptr m)
 110 {
 111   #if __MSPDEBUG__ > 1
 112   xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"InitMSP3430(m->connector=%d, m->standard=%d, m->chip_family=%d)\n",
 113                 m->connector, m->standard, m->chip_family);
 114   #endif
 115         switch (m->chip_family) {
 116                 case MSPFAMILY_34x0G:
 117                         InitMSP34xxG(m);
 118                         break;
 119                 case MSPFAMILY_34x5G:
 120                         InitMSP34xxG(m);
 121                         break;
 122                 case MSPFAMILY_34x5D:
 123                         InitMSP34x5D(m);
 124                         break;
 125         }
 126 }
 127
 128 /*-----------------------------------------------------------------
 129 | common functions for all MSP34xx chips
 130 |----------------------------------------------------------------*/
 131
 132 MSP3430Ptr DetectMSP3430(I2CBusPtr b, I2CSlaveAddr addr)
 133 {
 134    MSP3430Ptr m;
 135    I2CByte a;
 136    CARD8 hardware_version, major_revision, product_code, rom_version;
 137    Bool supported;
 138
 139    m = xcalloc(1,sizeof(MSP3430Rec));
 140    if(m == NULL)return NULL;
 141    m->d.DevName = strdup("MSP34xx");
 142    m->d.SlaveAddr = addr;
 143    m->d.pI2CBus = b;
 144    m->d.NextDev = NULL;
 145    m->d.StartTimeout = b->StartTimeout;
 146    m->d.BitTimeout = b->BitTimeout;
 147    m->d.AcknTimeout = b->AcknTimeout;
 148    m->d.ByteTimeout = b->ByteTimeout;
 149
 150    if(!I2C_WriteRead(&(m->d), NULL, 0, &a, 1))
 151    {
 152         xfree(m->d.DevName);
 153         xfree(m);
 154         return NULL;
 155     }
 156
 157
 158         m->standard=MSP3430_NTSC;
 159         m->connector=MSP3430_CONNECTOR_1;
 160         m->mode=MSPMODE_STEREO_A;       /*stereo or chanel A if avail. */
 161         m->c_format=MSPFORMAT_UNKNOWN;
 162         m->c_standard=MSPSTANDARD_UNKNOWN;
 163         m->c_matrix=m->c_fmmatrix=m->c_source=0;
 164         m->volume=0;
 165         m->recheck=FALSE;
 166
 167    GetMSP3430Data(m, RD_DSP, 0x00, 0x1E, &hardware_version, &major_revision);
 168    GetMSP3430Data(m, RD_DSP, 0x00, 0x1F, &product_code, &rom_version);
 169    m->hardware_version=hardware_version;
 170    m->major_revision=major_revision;
 171    m->product_code=product_code;
 172    m->rom_version=rom_version;
 173
 174    m->chip_id=((major_revision << 8) | product_code);
 175
 176    supported=FALSE;
 177    switch (major_revision) {
 178    case 4:      /* 34xxD */
 179       switch (product_code) {
 180           case 0x05: /* 3405D */
 181           case 0x0A: /* 3410D */
 182           case 0x0F: /* 3415D */
 183                 m->chip_family=MSPFAMILY_34x5D;
 184                 m->recheck=TRUE;
 185                 supported=TRUE;
 186                 break;
 187       default:
 188                 m->chip_family=MSPFAMILY_34x0D;
 189       }
 190           break;
 191    case 7:      /* 34xxG */
 192         switch(product_code){
 193                 case 0x00:
 194                 case 0x0A:
 195                 case 0x1E:
 196                 case 0x28:
 197                 case 0x32:
 198                         m->chip_family=MSPFAMILY_34x0G;
 199                         supported=TRUE;
 200                         break;
 201                 case 0x0f:
 202                 case 0x19:
 203                 case 0x2d:
 204                 case 0x37:
 205                 case 0x41:
 206                         m->chip_family=MSPFAMILY_34x5G;
 207                         supported=TRUE;
 208                         #ifdef MSP_DEBUG
 209                         memset(m->registers_present, 0, 256);
 210                         #define A(num) m->registers_present[(num)]=1;
 211                         #define B(num1, num2) memset(&(m->registers_present[num1]), 1, num2-num1);
 212                         A(0x20)
 213                         A(0x30)
 214                         A(0x40)
 215                         A(0x00)
 216                         B(0x01, 0x08)
 217                         B(0x0B, 0x0E)
 218                         A(0x10)
 219                         B(0x12,0x14)
 220                         A(0x16)
 221                         A(0x29)
 222                         #undef B
 223                         #undef A
 224                         #endif
 225                         break;
 226                 default:
 227                         m->chip_family=MSPFAMILY_UNKNOWN;
 228                 }
 229                 break;
 230    default:
 231                 m->chip_family=MSPFAMILY_UNKNOWN;
 232    }
 233
 234         xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "Found %s%s, rom version 0x%02x, chip_id=0x%04x\n",
 235                 MSP_getProductName(m->chip_id), supported?"":" (unsupported)", rom_version, m->chip_id);
 236
 237         if (!supported) {
 238                 xfree(m->d.DevName);
 239                 xfree(m);
 240                 return NULL;
 241         }
 242    if(!I2CDevInit(&(m->d)))
 243    {
 244        xfree(m->d.DevName);
 245        xfree(m);
 246        return NULL;
 247    }
 248
 249    return m;
 250 }
 251
 252 void ResetMSP3430(MSP3430Ptr m)
 253 {
 254     /* Reset the MSP3430 */
 255     SetMSP3430Control(m, 0x00, 0x80, 0x00);
 256     /* Set it back to normal operation */
 257     SetMSP3430Control(m, 0x00, 0x00, 0x00);
 258
 259     m->c_format=MSPFORMAT_UNKNOWN;
 260     m->c_standard=MSPSTANDARD_UNKNOWN;
 261     m->c_matrix=m->c_fmmatrix=m->c_source=0;
 262         m->volume=0;
 263 }
 264
 265 void MSP3430SetVolume (MSP3430Ptr m, CARD8 value)
 266 {
 267     CARD8 result;
 268 #if 0
 269     CARD8 old_volume;
 270     GetMSP3430Data(m, RD_DSP, 0x00, 0x00, &old_volume, &result);
 271     xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP3430 result 0x%02x\n", result);
 272 #endif
 273     /* save an extra Get call */
 274     result=0;
 275
 276     SetMSP3430Data(m, WR_DSP, 0x00, 0x00, value, result);
 277
 278     SetMSP3430Data(m, WR_DSP, 0x00, 0x07, value, 0);
 279     m->volume=value;
 280
 281 #if __MSPDEBUG__ > 2
 282     MSP3430DumpStatus(m);
 283     __MSPBEEP
 284     GetMSP3430Data(m, RD_DSP, 0x00, 0x00, &old_volume, &result);
 285     xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "MSP3430 volume 0x%02x\n",value);
 286 #endif
 287 }
 288
 289
 290 void MSP3430SetSAP (MSP3430Ptr m, int mode)
 291 {
 292         xf86DrvMsg(m->d.pI2CBus->scrnIndex, X_INFO, "Put actual code to change SAP here\n");
 293
 294       SetMSP3430Data(m, WR_DSP, 0x00, 0x08, mode & 0xff, 0x20);
 295 }
 296
 297
 298 #if 0
 299 void MSP3430SetSource(MSP3430Ptr m, CARD8 value)
 300 {
 301     /* Write to DSP, register 0x0008, (loudspeaker channel source/matrix) */
 302     /* This sets the source to the TV tuner, for stereo operation */
 303     SetMSP3430Data(m, WR_DSP, 0x00, 0x08, value, 0x20);
 304 }
 305 #endif
 306
 307
 308 char *MSP_getProductName (CARD16 product_id)
 309 {
 310         switch (product_id) {
 311                 case 0x0400: return "MSP3400D";
 312                 case 0x040a: return "MSP3410D";
 313                 case 0x0405: return "MSP3405D";
 314                 case 0x040f: return "MSP3415D";
 315                 case 0x0700: return "MSP3400G";
 316                 case 0x070a: return "MSP3410G";
 317                 case 0x071e: return "MSP3430G";
 318                 case 0x0728: return "MSP3440G";
 319                 case 0x0732: return "MSP3450G";
 320                 case 0x070f: return "MSP3415G";
 321                 case 0x0719: return "MSP3425G";
 322                 case 0x072d: return "MSP3445G";
 323                 case 0x0737: return "MSP3455G";
 324                 case 0x0741: return "MSP3465G";
 325         }
 326         return "MSP - unknown type";
 327 }
 328
 329 #if __MSPDEBUG__ > 2
 330 /*puts beep in MSP output
 331     freq = 0x01 - 16Hz ... 0x40 - 1kHz ... 0xff - 4kHz
 332 */
 333 void MSPBeep(MSP3430Ptr m, CARD8 freq) {
 334     SetMSP3430Data (m, WR_DSP, 0x00, freq, 0x7f, 0x40);
 335     mpause(100);
 336     SetMSP3430Data (m, WR_DSP, 0x00, 0x14, 0x00, 0x00);
 337 }
 338 #endif
 339
 340 void mpause(int milliseconds) {
 341     int i,m;
 342     m=milliseconds/20;
 343     for (i=0;i<m;i++) usleep(20000);
 344 }
 345
 346 /*-----------------------------------------------------------------
 347 | specific functions for all MSP34xxG chips
 348 |----------------------------------------------------------------*/
 349
 350 void InitMSP34xxG(MSP3430Ptr m)
 351 {
 352
 353   #if __MSPDEBUG__ > 1
 354   xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"InitMSP34xxG(m->connector=%d, m->standard=%d, m->chip_family=%d)\n",
 355                 m->connector, m->standard, m->chip_family);
 356   #endif
 357    /* Reset MSP3430 */
 358    SetMSP3430Control(m, 0x00, 0x80, 0x00);
 359    /* Set it back to normal operation */
 360    SetMSP3430Control(m, 0x00, 0x00, 0x00);
 361
 362    /*set MODUS register */
 363    /* bits: 0 - automatic sound detection */
 364    /*       1 - enable STATUS change */
 365    /*       12 - detect 6.5 Mhz carrier as D/K1, D/K2 or D/K NICAM  (does not seem to work ) */
 366    /*       13 - detect 4.5 Mhz carrier as BTSC */
 367    if ( (m->standard & 0xff) == MSP3430_PAL )
 368    {
 369       SetMSP3430Data(m, WR_DEM, 0x00, 0x30, 0x30, 0x03|0x08);    /* make O_ pins tristate */
 370       /* PAL standard */
 371       SetMSP3430Data(m, WR_DEM, 0x00, 0x20, 0x00, 0x01); /* possibly wrong */
 372    } else {
 373       SetMSP3430Data(m, WR_DEM, 0x00, 0x30, 0x20, 0x03|0x08);
 374       /* standard selection is M-BTSC-Stereo */
 375       SetMSP3430Data(m, WR_DEM, 0x00, 0x20, 0x00, 0x20);
 376    }
 377
 378    switch(m->connector){
 379          case MSP3430_CONNECTOR_1:
 380                 SetMSP3430Data(m, WR_DSP, 0x00, 0x08, 0x03, 0x20);
 381                 break;
 382          case MSP3430_CONNECTOR_2:
 383                 /* this has not been checked yet.. could be bogus */
 384                 /* SCART Input Prescale: 0 dB gain */
 385                 SetMSP3430Data(m, WR_DSP, 0x00, 0x0d, 0x19, 0x00);
 386                 SetMSP3430Data(m, WR_DSP, 0x00, 0x08, 0x02, 0x20);
 387                 break;
 388         case MSP3430_CONNECTOR_3:
 389         default:
 390                 /* SCART Input Prescale: 0 dB gain */
 391                 SetMSP3430Data(m, WR_DSP, 0x00, 0x0d, 0x19, 0x00);
 392
 393                 SetMSP3430Data(m, WR_DSP, 0x00, 0x08, 0x02, 0x20);
 394                 break;
 395         }
 396
 397     switch(m->standard){
 398         case MSP3430_PAL:
 399                 SetMSP3430Data(m, WR_DSP, 0x00, 0x0e, 0x24, 0x03);
 400                 SetMSP3430Data(m, WR_DSP, 0x00, 0x10, 0x00, 0x5a);
 401                 SetMSP3430Data(m, WR_DEM, 0x00, 0x20, 0x00, 0x03);
 402                 /* Set volume to FAST_MUTE. */
 403                 SetMSP3430Data(m, WR_DSP, 0x00, 0x00, 0xFF, 0x00);
 404                 break;
 405         case MSP3430_PAL_DK1:
 406                 SetMSP3430Data(m, WR_DSP, 0x00, 0x0e, 0x24, 0x03);
 407                 SetMSP3430Data(m, WR_DSP, 0x00, 0x10, 0x00, 0x5a);
 408                 SetMSP3430Data(m, WR_DEM, 0x00, 0x20, 0x00, 0x04);
 409                 /* Set volume to FAST_MUTE. */
 410                 SetMSP3430Data(m, WR_DSP, 0x00, 0x00, 0xFF, 0x00);
 411                 break;
 412         case MSP3430_SECAM: /* is this right ? */
 413         case MSP3430_NTSC:
 414                 /* Write to DSP, register 0x000E, (prescale FM/FM matrix) */
 415                 SetMSP3430Data(m, WR_DSP, 0x00, 0x0e, 0x24, 0x03);
 416
 417                 /* Set volume to FAST_MUTE. */
 418                 SetMSP3430Data(m, WR_DSP, 0x00, 0x00, 0xFF, 0x00);
 419                 break;
 420         }
 421
 422 }
 423
 424 /*-----------------------------------------------------------------
 425 | specific functions for all MSP34x5D chips
 426 |----------------------------------------------------------------*/
 427
 428 void InitMSP34x5D(MSP3430Ptr m)
 429 {
 430 int count;
 431 CARD8 high,low;
 432 CARD16 result,standard;
 433 CARD16 peak;
 434
 435
 436 if (m->c_format==MSPFORMAT_UNKNOWN) ResetMSP3430(m);
 437 else {
 438     /*mute volume*/
 439     SetMSP3430Data (m, WR_DSP, 0x00, 0x00, 0x00, 0x00);
 440 }
 441
 442
 443
 444     switch(m->connector){
 445         case MSP3430_CONNECTOR_2:
 446         case MSP3430_CONNECTOR_3:
 447             if (m->c_format!=MSPFORMAT_SCART) {
 448                 /* SCART Input Prescale: 0 dB gain */
 449                         SetMSP3430Data (m, WR_DSP, 0x00, 0x0d, 0x19, 0x00);
 450                         /* this has not been checked yet.. could be bogus */
 451                         m->c_format=MSPFORMAT_SCART;    /*stereo*/
 452                 }
 453             break;
 454         case MSP3430_CONNECTOR_1:
 455         default:
 456
 457             switch ( m->standard & 0x00ff ) {
 458                 case MSP3430_PAL:
 459                         switch( m->standard ) {
 460                         case MSP3430_PAL_DK1:
 461                             standard=MSPSTANDARD_FM_DK1;
 462                             break;
 463 /*                      case MSP3430_PAL_DK2:
 464                             standard=MSPSTANDARD_FM_DK2;
 465                             break;
 466                         case MSP3430_PAL_BG:
 467                         may be FM stereo (Germany) or FM NICAM (Scandinavia,spain)
 468                             standard=MSPSTANDARD_AUTO;
 469                             break;
 470 */
 471                         default:
 472                             standard=MSPSTANDARD_AUTO;
 473                         }
 474                     break;
 475                 case MSP3430_SECAM:
 476                     standard=MSPSTANDARD_AUTO;
 477                 case MSP3430_NTSC:
 478                             /* Only MSP34x5 supported format - Korean NTSC-M*/
 479                          standard=MSPSTANDARD_FM_M;
 480                 default:
 481                     standard=MSPSTANDARD_AUTO;
 482                 }
 483
 484             /*no NICAM support in MSP3410D - force to autodetect*/
 485             if ((m->chip_id==0x405) && (standard>=MSPSTANDARD_NICAM_BG))
 486                 standard=MSPSTANDARD_AUTO;
 487
 488             if (m->c_standard != standard) {
 489
 490                 SetMSP3430Data (m, WR_DEM, 0x00, 0x20, standard>>8, standard & 0xFF);
 491                 if (standard==MSPSTANDARD_AUTO) {
 492                             count = 50; /* time shouldn't exceed 1s, just in case */
 493                             do {
 494                         usleep(20000);
 495                         GetMSP3430Data (m, RD_DEM, 0x00, 0x7e, &high, &low);
 496                                 result = ( high << 8 ) | low;
 497                         --count;
 498                     } while( result > 0x07ff && count > 0 );
 499
 500                         if ((result > MSPSTANDARD_AUTO))
 501                                         standard=result;
 502                         else standard=MSPSTANDARD_UNKNOWN;
 503 #if __MSPDEBUG__ > 1
 504                                 xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"Detected audio standard: %d\n",result);
 505 #endif
 506                         /* result = MSPSTANDARD_NICAM_L can be one of:
 507                         SECAM_L - MSPSTANDARD_NICAM_L
 508                         D/K1 - MSPSTANDARD_FM_DK1
 509                         D/K2 - MSPSTANDARD_FM_DK2
 510                         D/K-NICAM - MSPSTANDARD_NICAM_DK*/
 511                         if( standard == MSPSTANDARD_NICAM_L ) {
 512                                 if ((m->standard & 0x00ff)==MSP3430_PAL) {
 513                                         /* force PAL D/K  */
 514                                         standard=MSPSTANDARD_FM_DK1;
 515                                 SetMSP3430Data (m, WR_DEM, 0x00, 0x20, standard>>8, standard & 0xFF);
 516 #if __MSPDEBUG__ > 1
 517                                         xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO, "Detected 6.5MHz carrier - forced to D/K1 !!!\n" );
 518 #endif
 519                                         }
 520                         }
 521                         }
 522                         m->c_standard=standard;
 523             } /*end - standard changed*/
 524             else {
 525                         if (standard<MSPSTANDARD_NICAM_BG) {
 526                     /* get old value of ident. mode register*/
 527                         GetMSP3430Data (m, RD_DSP, 0x00, 0x15, &high, &low);
 528                     /* reset Ident-Filter */
 529                     SetMSP3430Data (m, WR_DSP, 0x00, 0x14, 0x00, 0x3F);
 530                         /* put back old value to ident. mode register*/
 531                     SetMSP3430Data (m, WR_DSP, 0x00, 0x14, 0x00, low);
 532                         }
 533             }
 534
 535             if (standard<=MSPSTANDARD_AUTO) {
 536                 m->c_format=MSPFORMAT_1xFM;
 537             }
 538             else if (standard<MSPSTANDARD_NICAM_BG) {
 539                         /* set FM prescale */
 540                         SetMSP3430Data (m, WR_DSP, 0x00, 0x0e, 0x30, 0);
 541                         /* set FM deemphasis*/
 542                         SetMSP3430Data (m, WR_DSP, 0x00, 0x0f, ((standard==MSPSTANDARD_FM_M)?0:1), 0);
 543
 544                         /* check if FM2 carrier is present */
 545                         /*turn off FM DC Notch*/
 546                         SetMSP3430Data (m, WR_DSP, 0x00, 0x17, 0x00, 0x3f);
 547                         /*matrix source for Quasi-Peak Detector - stereo: ch2->L ch1->R*/
 548                         SetMSP3430Data (m, WR_DSP, 0x00, 0x0c, 0x00, 0x20);
 549
 550                         mpause(250);
 551                     GetMSP3430Data (m, RD_DSP, 0x00, 0x1A, &high, &low);
 552                         peak = (high << 8) | low;
 553 #if __MSPDEBUG__ > 1
 554                         xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"Second carrier Quasi-Peak detection: %d\n",peak);
 555 #endif
 556                         /*turn on FM DC Notch*/
 557                         SetMSP3430Data (m, WR_DSP, 0x00, 0x17, 0x00, 0x00);
 558
 559                         if (peak<5) {
 560                     /* if second carrier not detected - only mono from first carrier*/
 561                     m->c_format=MSPFORMAT_1xFM;
 562                         }
 563                         else {
 564                     m->c_format=MSPFORMAT_2xFM;
 565                     /*start of FM identification process - FM_WAIT
 566                     wait at least 0.5s - used 1s - gives beter resolution*/
 567                     mpause(1000);
 568                         }
 569             }
 570             else {
 571                         if (standard==MSPSTANDARD_NICAM_L) {
 572                     m->c_format=MSPFORMAT_NICAM_AM;
 573                         /* set AM prescale */
 574                         SetMSP3430Data (m, WR_DSP, 0x00, 0x0e, 0x7C, 0);
 575                         }
 576                         else {
 577                     m->c_format=MSPFORMAT_NICAM_FM;
 578                         /* set FM prescale */
 579                             SetMSP3430Data (m, WR_DSP, 0x00, 0x0e, 0x30, 0);
 580                         }
 581                         /* set FM deemphasis*/
 582                         SetMSP3430Data (m, WR_DSP, 0x00, 0x0f, 0x00, 0);
 583                         /* set NICAM prescale to 0dB */
 584                         SetMSP3430Data (m, WR_DSP, 0x00, 0x10, 0x20, 0);
 585             }
 586
 587             break;
 588         } /*end - case conector*/
 589
 590     CheckModeMSP34x5D(m);
 591
 592     /* Set volume to FAST_MUTE. */
 593     /*SetMSP3430Data(m, WR_DSP, 0x00, 0x00, 0xFF, 0x00);*/
 594     /*set volume*/
 595         MSP3430SetVolume(m,m->volume);
 596
 597     __MSPBEEP
 598
 599
 600 } /* EnableMSP34x5D ()... */
 601
 602
 603
 604
 605 void CheckModeMSP34x5D(MSP3430Ptr m) {
 606     const char stereo_on=25;
 607     const char stereo_off=20;
 608     const char dual_on=-stereo_on;
 609     const char dual_off=-stereo_off;
 610     char detect;
 611     CARD8 matrix, fmmatrix, source, high, low;
 612
 613     fmmatrix=0;         /*no matrix*/
 614     source=0;           /*FM*/
 615     switch (m->c_format) {
 616         case MSPFORMAT_NICAM_FM:
 617         case MSPFORMAT_NICAM_AM:
 618         case MSPFORMAT_SCART:
 619             source=( (m->c_format == MSPFORMAT_SCART)?2:1 );
 620             switch (m->mode) {
 621                 case MSPMODE_MONO:
 622                     matrix=0x30;        /*MONO*/
 623                     break;
 624                 case MSPMODE_A:
 625                     matrix=0x00;        /*A*/
 626                     break;
 627                 case MSPMODE_B:
 628                     matrix=0x10;        /*B*/
 629                     break;
 630                 default:
 631                     matrix=0x20;        /*STEREO*/
 632                     break;
 633                 }
 634             break;
 635         default:
 636         case MSPFORMAT_1xFM:
 637             matrix=0x00;        /*A*/
 638             break;
 639         case MSPFORMAT_2xFM:
 640             switch (m->mode) {
 641                 case MSPMODE_MONO:
 642                     matrix=0x30;        /*MONO*/
 643                     break;
 644                 case MSPMODE_STEREO:
 645                     matrix=0x20;        /*STEREO*/
 646                     fmmatrix=((m->c_standard==MSPSTANDARD_FM_M)?2:1);
 647                     break;
 648                 case MSPMODE_AB:
 649                     matrix=0x20;        /*STEREO*/
 650                     break;
 651                 case MSPMODE_A:
 652                     matrix=0x00;        /*A*/
 653                     break;
 654                 case MSPMODE_B:
 655                     matrix=0x10;        /*B*/
 656                     break;
 657                 default:
 658                 /*FM_IDENT_CHECK*/
 659                 GetMSP3430Data (m, RD_DSP, 0x00, 0x18, &high, &low);
 660                 detect=(char)high;
 661 #if __MSPDEBUG__ > 1
 662                 xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"Stereo Detection Register: %d\n",detect);
 663 #endif
 664                 if (detect>=((m->c_mode==MSPMODE_STEREO)?stereo_off:stereo_on)) {
 665                                 m->c_mode=MSPMODE_STEREO;
 666                                 matrix=0x20;    /*STEREO*/
 667                                 fmmatrix=((m->c_standard==MSPSTANDARD_FM_M)?2:1);
 668                     }
 669                         else if (detect<=((m->c_mode==MSPMODE_AB)?dual_off:dual_on)) {
 670                                 m->c_mode=MSPMODE_AB;
 671                         switch (m->mode) {
 672                             case MSPMODE_STEREO_AB: matrix=0x20; break;
 673                             case MSPMODE_STEREO_B: matrix=0x10; break;
 674                             default:
 675                                 case MSPMODE_A: matrix=0x00; break;
 676                                 }
 677                         }
 678                 else {
 679                                 m->c_mode=MSPMODE_MONO;
 680                                 matrix=0x30;    /*MONO*/
 681                         }
 682                     break;
 683             } /* end - case mode*/
 684             break;
 685     }
 686
 687     if (m->c_fmmatrix != fmmatrix) {
 688         GetMSP3430Data (m, RD_DSP, 0x00, 0x0e, &high, &low);
 689                 SetMSP3430Data (m, WR_DSP, 0x00, 0x0e, high, fmmatrix);
 690                 m->c_fmmatrix = fmmatrix;
 691     }
 692
 693     if ((m->c_matrix != matrix) || (m->c_source != source)) {
 694         /*set chanel source and matrix for loudspeaker*/
 695                 SetMSP3430Data (m, WR_DSP, 0x00, 0x08, source, matrix);
 696
 697                 m->c_matrix = matrix;
 698                 m->c_source = source;
 699     }
 700
 701         if ( ((m->c_format) & 0xF0) == MSPFORMAT_NICAM)
 702                         SetMSP3430Data (m, WR_DEM, 0x00, 0x21, 0, 1);
 703
 704 #if __MSPDEBUG__ > 0
 705                     char *msg;
 706                     switch (matrix) {
 707                         case 0x30: /*MONO*/
 708                             msg="MONO";
 709                             break;
 710                         case 0x00: /*LEFT*/
 711                             msg="MONO/CHANNEL_1";
 712                             break;
 713                         case 0x10: /*RIGHT*/
 714                             msg="MONO/CHANNEL_2";
 715                             break;
 716                         case 0x20: /*LEFT*/
 717                             msg="STEREO";
 718                             break;
 719                         default:
 720                             msg="unknown";
 721                             break;
 722                     }
 723                     xf86DrvMsg(m->d.pI2CBus->scrnIndex,X_INFO,"Audio mode set to: %s\n",msg);
 724 #endif
 725 }
 726