drivers/video/matrox/matroxfb_maven.c

   1 /*
   2  *
   3  * Hardware accelerated Matrox Millennium I, II, Mystique, G100, G200, G400 and G450.
   4  *
   5  * (c) 1998-2002 Petr Vandrovec <vandrove@vc.cvut.cz>
   6  *
   7  * Portions Copyright (c) 2001 Matrox Graphics Inc.
   8  *
   9  * Version: 1.65 2002/08/14
  10  *
  11  * See matroxfb_base.c for contributors.
  12  *
  13  */
  14
  15 #include "matroxfb_maven.h"
  16 #include "matroxfb_misc.h"
  17 #include "matroxfb_DAC1064.h"
  18 #include <linux/i2c.h>
  19 #include <linux/matroxfb.h>
  20 #include <asm/div64.h>
  21 #include <asm/uaccess.h>
  22
  23 #define MAVEN_I2CID     (0x1B)
  24
  25 #define MGATVO_B        1
  26 #define MGATVO_C        2
  27
  28 static const struct maven_gamma {
  29   unsigned char reg83;
  30   unsigned char reg84;
  31   unsigned char reg85;
  32   unsigned char reg86;
  33   unsigned char reg87;
  34   unsigned char reg88;
  35   unsigned char reg89;
  36   unsigned char reg8a;
  37   unsigned char reg8b;
  38 } maven_gamma[] = {
  39   { 131, 57, 223, 15, 117, 212, 251, 91, 156},
  40   { 133, 61, 128, 63, 180, 147, 195, 100, 180},
  41   { 131, 19, 63, 31, 50, 66, 171, 64, 176},
  42   { 0, 0, 0, 31, 16, 16, 16, 100, 200},
  43   { 8, 23, 47, 73, 147, 244, 220, 80, 195},
  44   { 22, 43, 64, 80, 147, 115, 58, 85, 168},
  45   { 34, 60, 80, 214, 147, 212, 188, 85, 167},
  46   { 45, 77, 96, 216, 147, 99, 91, 85, 159},
  47   { 56, 76, 112, 107, 147, 212, 148, 64, 144},
  48   { 65, 91, 128, 137, 147, 196, 17, 69, 148},
  49   { 72, 104, 136, 138, 147, 180, 245, 73, 147},
  50   { 87, 116, 143, 126, 16, 83, 229, 77, 144},
  51   { 95, 119, 152, 254, 244, 83, 221, 77, 151},
  52   { 100, 129, 159, 156, 244, 148, 197, 77, 160},
  53   { 105, 141, 167, 247, 244, 132, 181, 84, 166},
  54   { 105, 147, 168, 247, 244, 245, 181, 90, 170},
  55   { 120, 153, 175, 248, 212, 229, 165, 90, 180},
  56   { 119, 156, 176, 248, 244, 229, 84, 74, 160},
  57   { 119, 158, 183, 248, 244, 229, 149, 78, 165}
  58 };
  59
  60 /* Definition of the various controls */
  61 struct mctl {
  62         struct v4l2_queryctrl desc;
  63         size_t control;
  64 };
  65
  66 #define BLMIN   0x0FF
  67 #define WLMAX   0x3FF
  68
  69 static const struct mctl maven_controls[] =
  70 {       { { V4L2_CID_BRIGHTNESS, V4L2_CTRL_TYPE_INTEGER,
  71           "brightness",
  72           0, WLMAX - BLMIN, 1, 379 - BLMIN,
  73           0,
  74         }, offsetof(struct matrox_fb_info, altout.tvo_params.brightness) },
  75         { { V4L2_CID_CONTRAST, V4L2_CTRL_TYPE_INTEGER,
  76           "contrast",
  77           0, 1023, 1, 127,
  78           0,
  79         }, offsetof(struct matrox_fb_info, altout.tvo_params.contrast) },
  80         { { V4L2_CID_SATURATION, V4L2_CTRL_TYPE_INTEGER,
  81           "saturation",
  82           0, 255, 1, 155,
  83           0,
  84         }, offsetof(struct matrox_fb_info, altout.tvo_params.saturation) },
  85         { { V4L2_CID_HUE, V4L2_CTRL_TYPE_INTEGER,
  86           "hue",
  87           0, 255, 1, 0,
  88           0,
  89         }, offsetof(struct matrox_fb_info, altout.tvo_params.hue) },
  90         { { V4L2_CID_GAMMA, V4L2_CTRL_TYPE_INTEGER,
  91           "gamma",
  92           0, ARRAY_SIZE(maven_gamma) - 1, 1, 3,
  93           0,
  94         }, offsetof(struct matrox_fb_info, altout.tvo_params.gamma) },
  95         { { MATROXFB_CID_TESTOUT, V4L2_CTRL_TYPE_BOOLEAN,
  96           "test output",
  97           0, 1, 1, 0,
  98           0,
  99         }, offsetof(struct matrox_fb_info, altout.tvo_params.testout) },
 100         { { MATROXFB_CID_DEFLICKER, V4L2_CTRL_TYPE_INTEGER,
 101           "deflicker mode",
 102           0, 2, 1, 0,
 103           0,
 104         }, offsetof(struct matrox_fb_info, altout.tvo_params.deflicker) },
 105
 106 };
 107
 108 #define MAVCTRLS ARRAY_SIZE(maven_controls)
 109
 110 /* Return: positive number: id found
 111            -EINVAL:         id not found, return failure
 112            -ENOENT:         id not found, create fake disabled control */
 113 static int get_ctrl_id(__u32 v4l2_id) {
 114         int i;
 115
 116         for (i = 0; i < MAVCTRLS; i++) {
 117                 if (v4l2_id < maven_controls[i].desc.id) {
 118                         if (maven_controls[i].desc.id == 0x08000000) {
 119                                 return -EINVAL;
 120                         }
 121                         return -ENOENT;
 122                 }
 123                 if (v4l2_id == maven_controls[i].desc.id) {
 124                         return i;
 125                 }
 126         }
 127         return -EINVAL;
 128 }
 129
 130 struct maven_data {
 131         struct matrox_fb_info*          primary_head;
 132         struct i2c_client               client;
 133         int                             version;
 134 };
 135
 136 static int* get_ctrl_ptr(struct maven_data* md, int idx) {
 137         return (int*)((char*)(md->primary_head) + maven_controls[idx].control);
 138 }
 139
 140 static int maven_get_reg(struct i2c_client* c, char reg) {
 141         char dst;
 142         struct i2c_msg msgs[] = {{ c->addr, I2C_M_REV_DIR_ADDR, sizeof(reg), &reg },
 143                                  { c->addr, I2C_M_RD | I2C_M_NOSTART, sizeof(dst), &dst }};
 144         s32 err;
 145
 146         err = i2c_transfer(c->adapter, msgs, 2);
 147         if (err < 0)
 148                 printk(KERN_INFO "ReadReg(%d) failed\n", reg);
 149         return dst & 0xFF;
 150 }
 151
 152 static int maven_set_reg(struct i2c_client* c, int reg, int val) {
 153         s32 err;
 154
 155         err = i2c_smbus_write_byte_data(c, reg, val);
 156         if (err)
 157                 printk(KERN_INFO "WriteReg(%d) failed\n", reg);
 158         return err;
 159 }
 160
 161 static int maven_set_reg_pair(struct i2c_client* c, int reg, int val) {
 162         s32 err;
 163
 164         err = i2c_smbus_write_word_data(c, reg, val);
 165         if (err)
 166                 printk(KERN_INFO "WriteRegPair(%d) failed\n", reg);
 167         return err;
 168 }
 169
 170 static const struct matrox_pll_features maven_pll = {
 171         50000,
 172         27000,
 173         4, 127,
 174         2, 31,
 175         3
 176 };
 177
 178 struct matrox_pll_features2 {
 179         unsigned int    vco_freq_min;
 180         unsigned int    vco_freq_max;
 181         unsigned int    feed_div_min;
 182         unsigned int    feed_div_max;
 183         unsigned int    in_div_min;
 184         unsigned int    in_div_max;
 185         unsigned int    post_shift_max;
 186 };
 187
 188 struct matrox_pll_ctl {
 189         unsigned int    ref_freq;
 190         unsigned int    den;
 191 };
 192
 193 static const struct matrox_pll_features2 maven1000_pll = {
 194          50000000,
 195         300000000,
 196          5, 128,
 197          3,  32,
 198          3
 199 };
 200
 201 static const struct matrox_pll_ctl maven_PAL = {
 202         540000,
 203             50
 204 };
 205
 206 static const struct matrox_pll_ctl maven_NTSC = {
 207         450450, /* 27027000/60 == 27000000/59.94005994 */
 208             60
 209 };
 210
 211 static int matroxfb_PLL_mavenclock(const struct matrox_pll_features2* pll,
 212                 const struct matrox_pll_ctl* ctl,
 213                 unsigned int htotal, unsigned int vtotal,
 214                 unsigned int* in, unsigned int* feed, unsigned int* post,
 215                 unsigned int* h2) {
 216         unsigned int besth2 = 0;
 217         unsigned int fxtal = ctl->ref_freq;
 218         unsigned int fmin = pll->vco_freq_min / ctl->den;
 219         unsigned int fwant;
 220         unsigned int p;
 221         unsigned int scrlen;
 222         unsigned int fmax;
 223
 224         DBG(__FUNCTION__)
 225
 226         scrlen = htotal * (vtotal - 1);
 227         fwant = htotal * vtotal;
 228         fmax = pll->vco_freq_max / ctl->den;
 229
 230         dprintk(KERN_DEBUG "want: %u, xtal: %u, h: %u, v: %u, fmax: %u\n",
 231                 fwant, fxtal, htotal, vtotal, fmax);
 232         for (p = 1; p <= pll->post_shift_max; p++) {
 233                 if (fwant * 2 > fmax)
 234                         break;
 235                 fwant *= 2;
 236         }
 237         if (fwant > fmax)
 238                 return 0;
 239         for (; p-- > 0; fwant >>= 1) {
 240                 unsigned int m;
 241
 242                 if (fwant < fmin) break;
 243                 for (m = pll->in_div_min; m <= pll->in_div_max; m++) {
 244                         unsigned int n;
 245                         unsigned int dvd;
 246                         unsigned int ln;
 247
 248                         n = (fwant * m) / fxtal;
 249                         if (n < pll->feed_div_min)
 250                                 continue;
 251                         if (n > pll->feed_div_max)
 252                                 break;
 253
 254                         ln = fxtal * n;
 255                         dvd = m << p;
 256
 257                         if (ln % dvd)
 258                                 continue;
 259                         ln = ln / dvd;
 260
 261                         if (ln < scrlen + 2)
 262                                 continue;
 263                         ln = ln - scrlen;
 264                         if (ln > htotal)
 265                                 continue;
 266                         dprintk(KERN_DEBUG "Match: %u / %u / %u / %u\n", n, m, p, ln);
 267                         if (ln > besth2) {
 268                                 dprintk(KERN_DEBUG "Better...\n");
 269                                 *h2 = besth2 = ln;
 270                                 *post = p;
 271                                 *in = m;
 272                                 *feed = n;
 273                         }
 274                 }
 275         }
 276
 277         /* if h2/post/in/feed have not been assigned, return zero (error) */
 278         if (besth2 < 2)
 279                 return 0;
 280
 281         dprintk(KERN_ERR "clk: %02X %02X %02X %d %d\n", *in, *feed, *post, fxtal, fwant);
 282         return fxtal * (*feed) / (*in) * ctl->den;
 283 }
 284
 285 static unsigned int matroxfb_mavenclock(const struct matrox_pll_ctl* ctl,
 286                 unsigned int htotal, unsigned int vtotal,
 287                 unsigned int* in, unsigned int* feed, unsigned int* post,
 288                 unsigned int* htotal2) {
 289         unsigned int fvco;
 290         unsigned int uninitialized_var(p);
 291
 292         fvco = matroxfb_PLL_mavenclock(&maven1000_pll, ctl, htotal, vtotal, in, feed, &p, htotal2);
 293         if (!fvco)
 294                 return -EINVAL;
 295         p = (1 << p) - 1;
 296         if (fvco <= 100000000)
 297                 ;
 298         else if (fvco <= 140000000)
 299                 p |= 0x08;
 300         else if (fvco <= 180000000)
 301                 p |= 0x10;
 302         else
 303                 p |= 0x18;
 304         *post = p;
 305         return 0;
 306 }
 307
 308 static void DAC1064_calcclock(unsigned int freq, unsigned int fmax,
 309                 unsigned int* in, unsigned int* feed, unsigned int* post) {
 310         unsigned int fvco;
 311         unsigned int p;
 312
 313         fvco = matroxfb_PLL_calcclock(&maven_pll, freq, fmax, in, feed, &p);
 314         p = (1 << p) - 1;
 315         if (fvco <= 100000)
 316                 ;
 317         else if (fvco <= 140000)
 318                 p |= 0x08;
 319         else if (fvco <= 180000)
 320                 p |= 0x10;
 321         else
 322                 p |= 0x18;
 323         *post = p;
 324         return;
 325 }
 326
 327 static unsigned char maven_compute_deflicker (const struct maven_data* md) {
 328         unsigned char df;
 329
 330         df = (md->version == MGATVO_B?0x40:0x00);
 331         switch (md->primary_head->altout.tvo_params.deflicker) {
 332                 case 0:
 333 /*                      df |= 0x00; */
 334                         break;
 335                 case 1:
 336                         df |= 0xB1;
 337                         break;
 338                 case 2:
 339                         df |= 0xA2;
 340                         break;
 341         }
 342         return df;
 343 }
 344
 345 static void maven_compute_bwlevel (const struct maven_data* md,
 346                                    int *bl, int *wl) {
 347         const int b = md->primary_head->altout.tvo_params.brightness + BLMIN;
 348         const int c = md->primary_head->altout.tvo_params.contrast;
 349
 350         *bl = max(b - c, BLMIN);
 351         *wl = min(b + c, WLMAX);
 352 }
 353
 354 static const struct maven_gamma* maven_compute_gamma (const struct maven_data* md) {
 355         return maven_gamma + md->primary_head->altout.tvo_params.gamma;
 356 }
 357
 358
 359 static void maven_init_TVdata(const struct maven_data* md, struct mavenregs* data) {
 360         static struct mavenregs palregs = { {
 361                 0x2A, 0x09, 0x8A, 0xCB, /* 00: chroma subcarrier */
 362                 0x00,
 363                 0x00,   /* ? not written */
 364                 0x00,   /* modified by code (F9 written...) */
 365                 0x00,   /* ? not written */
 366                 0x7E,   /* 08 */
 367                 0x44,   /* 09 */
 368                 0x9C,   /* 0A */
 369                 0x2E,   /* 0B */
 370                 0x21,   /* 0C */
 371                 0x00,   /* ? not written */
 372                 0x3F, 0x03, /* 0E-0F */
 373                 0x3F, 0x03, /* 10-11 */
 374                 0x1A,   /* 12 */
 375                 0x2A,   /* 13 */
 376                 0x1C, 0x3D, 0x14, /* 14-16 */
 377                 0x9C, 0x01, /* 17-18 */
 378                 0x00,   /* 19 */
 379                 0xFE,   /* 1A */
 380                 0x7E,   /* 1B */
 381                 0x60,   /* 1C */
 382                 0x05,   /* 1D */
 383                 0x89, 0x03, /* 1E-1F */
 384                 0x72,   /* 20 */
 385                 0x07,   /* 21 */
 386                 0x72,   /* 22 */
 387                 0x00,   /* 23 */
 388                 0x00,   /* 24 */
 389                 0x00,   /* 25 */
 390                 0x08,   /* 26 */
 391                 0x04,   /* 27 */
 392                 0x00,   /* 28 */
 393                 0x1A,   /* 29 */
 394                 0x55, 0x01, /* 2A-2B */
 395                 0x26,   /* 2C */
 396                 0x07, 0x7E, /* 2D-2E */
 397                 0x02, 0x54, /* 2F-30 */
 398                 0xB0, 0x00, /* 31-32 */
 399                 0x14,   /* 33 */
 400                 0x49,   /* 34 */
 401                 0x00,   /* 35 written multiple times */
 402                 0x00,   /* 36 not written */
 403                 0xA3,   /* 37 */
 404                 0xC8,   /* 38 */
 405                 0x22,   /* 39 */
 406                 0x02,   /* 3A */
 407                 0x22,   /* 3B */
 408                 0x3F, 0x03, /* 3C-3D */
 409                 0x00,   /* 3E written multiple times */
 410                 0x00,   /* 3F not written */
 411         }, MATROXFB_OUTPUT_MODE_PAL, 625, 50 };
 412         static struct mavenregs ntscregs = { {
 413                 0x21, 0xF0, 0x7C, 0x1F, /* 00: chroma subcarrier */
 414                 0x00,
 415                 0x00,   /* ? not written */
 416                 0x00,   /* modified by code (F9 written...) */
 417                 0x00,   /* ? not written */
 418                 0x7E,   /* 08 */
 419                 0x43,   /* 09 */
 420                 0x7E,   /* 0A */
 421                 0x3D,   /* 0B */
 422                 0x00,   /* 0C */
 423                 0x00,   /* ? not written */
 424                 0x41, 0x00, /* 0E-0F */
 425                 0x3C, 0x00, /* 10-11 */
 426                 0x17,   /* 12 */
 427                 0x21,   /* 13 */
 428                 0x1B, 0x1B, 0x24, /* 14-16 */
 429                 0x83, 0x01, /* 17-18 */
 430                 0x00,   /* 19 */
 431                 0x0F,   /* 1A */
 432                 0x0F,   /* 1B */
 433                 0x60,   /* 1C */
 434                 0x05,   /* 1D */
 435                 0x89, 0x02, /* 1E-1F */
 436                 0x5F,   /* 20 */
 437                 0x04,   /* 21 */
 438                 0x5F,   /* 22 */
 439                 0x01,   /* 23 */
 440                 0x02,   /* 24 */
 441                 0x00,   /* 25 */
 442                 0x0A,   /* 26 */
 443                 0x05,   /* 27 */
 444                 0x00,   /* 28 */
 445                 0x10,   /* 29 */
 446                 0xFF, 0x03, /* 2A-2B */
 447                 0x24,   /* 2C */
 448                 0x0F, 0x78, /* 2D-2E */
 449                 0x00, 0x00, /* 2F-30 */
 450                 0xB2, 0x04, /* 31-32 */
 451                 0x14,   /* 33 */
 452                 0x02,   /* 34 */
 453                 0x00,   /* 35 written multiple times */
 454                 0x00,   /* 36 not written */
 455                 0xA3,   /* 37 */
 456                 0xC8,   /* 38 */
 457                 0x15,   /* 39 */
 458                 0x05,   /* 3A */
 459                 0x3B,   /* 3B */
 460                 0x3C, 0x00, /* 3C-3D */
 461                 0x00,   /* 3E written multiple times */
 462                 0x00,   /* never written */
 463         }, MATROXFB_OUTPUT_MODE_NTSC, 525, 60 };
 464         MINFO_FROM(md->primary_head);
 465
 466         if (ACCESS_FBINFO(outputs[1]).mode == MATROXFB_OUTPUT_MODE_PAL)
 467                 *data = palregs;
 468         else
 469                 *data = ntscregs;
 470
 471         /* Set deflicker */
 472         data->regs[0x93] = maven_compute_deflicker(md);
 473
 474         /* set gamma */
 475         {
 476                 const struct maven_gamma* g;
 477                 g = maven_compute_gamma(md);
 478                 data->regs[0x83] = g->reg83;
 479                 data->regs[0x84] = g->reg84;
 480                 data->regs[0x85] = g->reg85;
 481                 data->regs[0x86] = g->reg86;
 482                 data->regs[0x87] = g->reg87;
 483                 data->regs[0x88] = g->reg88;
 484                 data->regs[0x89] = g->reg89;
 485                 data->regs[0x8A] = g->reg8a;
 486                 data->regs[0x8B] = g->reg8b;
 487         }
 488
 489         /* Set contrast / brightness */
 490         {
 491                 int bl, wl;
 492                 maven_compute_bwlevel (md, &bl, &wl);
 493                 data->regs[0x0e] = bl >> 2;
 494                 data->regs[0x0f] = bl & 3;
 495                 data->regs[0x1e] = wl >> 2;
 496                 data->regs[0x1f] = wl & 3;
 497         }
 498
 499         /* Set saturation */
 500         {
 501                 data->regs[0x20] =
 502                 data->regs[0x22] = ACCESS_FBINFO(altout.tvo_params.saturation);
 503         }
 504
 505         /* Set HUE */
 506         data->regs[0x25] = ACCESS_FBINFO(altout.tvo_params.hue);
 507         return;
 508 }
 509
 510 #define LR(x) maven_set_reg(c, (x), m->regs[(x)])
 511 #define LRP(x) maven_set_reg_pair(c, (x), m->regs[(x)] | (m->regs[(x)+1] << 8))
 512 static void maven_init_TV(struct i2c_client* c, const struct mavenregs* m) {
 513         int val;
 514
 515
 516         maven_set_reg(c, 0x3E, 0x01);
 517         maven_get_reg(c, 0x82); /* fetch oscillator state? */
 518         maven_set_reg(c, 0x8C, 0x00);
 519         maven_get_reg(c, 0x94); /* get 0x82 */
 520         maven_set_reg(c, 0x94, 0xA2);
 521         /* xmiscctrl */
 522
 523         maven_set_reg_pair(c, 0x8E, 0x1EFF);
 524         maven_set_reg(c, 0xC6, 0x01);
 525
 526         /* removed code... */
 527
 528         maven_get_reg(c, 0x06);
 529         maven_set_reg(c, 0x06, 0xF9);   /* or read |= 0xF0 ? */
 530
 531         /* removed code here... */
 532
 533         /* real code begins here? */
 534         /* chroma subcarrier */
 535         LR(0x00); LR(0x01); LR(0x02); LR(0x03);
 536
 537         LR(0x04);
 538
 539         LR(0x2C);
 540         LR(0x08);
 541         LR(0x0A);
 542         LR(0x09);
 543         LR(0x29);
 544         LRP(0x31);
 545         LRP(0x17);
 546         LR(0x0B);
 547         LR(0x0C);
 548         if (m->mode == MATROXFB_OUTPUT_MODE_PAL) {
 549                 maven_set_reg(c, 0x35, 0x10); /* ... */
 550         } else {
 551                 maven_set_reg(c, 0x35, 0x0F); /* ... */
 552         }
 553
 554         LRP(0x10);
 555
 556         LRP(0x0E);
 557         LRP(0x1E);
 558
 559         LR(0x20);       /* saturation #1 */
 560         LR(0x22);       /* saturation #2 */
 561         LR(0x25);       /* hue */
 562         LR(0x34);
 563         LR(0x33);
 564         LR(0x19);
 565         LR(0x12);
 566         LR(0x3B);
 567         LR(0x13);
 568         LR(0x39);
 569         LR(0x1D);
 570         LR(0x3A);
 571         LR(0x24);
 572         LR(0x14);
 573         LR(0x15);
 574         LR(0x16);
 575         LRP(0x2D);
 576         LRP(0x2F);
 577         LR(0x1A);
 578         LR(0x1B);
 579         LR(0x1C);
 580         LR(0x23);
 581         LR(0x26);
 582         LR(0x28);
 583         LR(0x27);
 584         LR(0x21);
 585         LRP(0x2A);
 586         if (m->mode == MATROXFB_OUTPUT_MODE_PAL)
 587                 maven_set_reg(c, 0x35, 0x1D);   /* ... */
 588         else
 589                 maven_set_reg(c, 0x35, 0x1C);
 590
 591         LRP(0x3C);
 592         LR(0x37);
 593         LR(0x38);
 594         maven_set_reg(c, 0xB3, 0x01);
 595
 596         maven_get_reg(c, 0xB0); /* read 0x80 */
 597         maven_set_reg(c, 0xB0, 0x08);   /* ugh... */
 598         maven_get_reg(c, 0xB9); /* read 0x7C */
 599         maven_set_reg(c, 0xB9, 0x78);
 600         maven_get_reg(c, 0xBF); /* read 0x00 */
 601         maven_set_reg(c, 0xBF, 0x02);
 602         maven_get_reg(c, 0x94); /* read 0x82 */
 603         maven_set_reg(c, 0x94, 0xB3);
 604
 605         LR(0x80); /* 04 1A 91 or 05 21 91 */
 606         LR(0x81);
 607         LR(0x82);
 608
 609         maven_set_reg(c, 0x8C, 0x20);
 610         maven_get_reg(c, 0x8D);
 611         maven_set_reg(c, 0x8D, 0x10);
 612
 613         LR(0x90); /* 4D 50 52 or 4E 05 45 */
 614         LR(0x91);
 615         LR(0x92);
 616
 617         LRP(0x9A); /* 0049 or 004F */
 618         LRP(0x9C); /* 0004 or 0004 */
 619         LRP(0x9E); /* 0458 or 045E */
 620         LRP(0xA0); /* 05DA or 051B */
 621         LRP(0xA2); /* 00CC or 00CF */
 622         LRP(0xA4); /* 007D or 007F */
 623         LRP(0xA6); /* 007C or 007E */
 624         LRP(0xA8); /* 03CB or 03CE */
 625         LRP(0x98); /* 0000 or 0000 */
 626         LRP(0xAE); /* 0044 or 003A */
 627         LRP(0x96); /* 05DA or 051B */
 628         LRP(0xAA); /* 04BC or 046A */
 629         LRP(0xAC); /* 004D or 004E */
 630
 631         LR(0xBE);
 632         LR(0xC2);
 633
 634         maven_get_reg(c, 0x8D);
 635         maven_set_reg(c, 0x8D, 0x04);
 636
 637         LR(0x20);       /* saturation #1 */
 638         LR(0x22);       /* saturation #2 */
 639         LR(0x93);       /* whoops */
 640         LR(0x20);       /* oh, saturation #1 again */
 641         LR(0x22);       /* oh, saturation #2 again */
 642         LR(0x25);       /* hue */
 643         LRP(0x0E);
 644         LRP(0x1E);
 645         LRP(0x0E);      /* problems with memory? */
 646         LRP(0x1E);      /* yes, matrox must have problems in memory area... */
 647
 648         /* load gamma correction stuff */
 649         LR(0x83);
 650         LR(0x84);
 651         LR(0x85);
 652         LR(0x86);
 653         LR(0x87);
 654         LR(0x88);
 655         LR(0x89);
 656         LR(0x8A);
 657         LR(0x8B);
 658
 659         val = maven_get_reg(c, 0x8D);
 660         val &= 0x14;                    /* 0x10 or anything ored with it */
 661         maven_set_reg(c, 0x8D, val);
 662
 663         LR(0x33);
 664         LR(0x19);
 665         LR(0x12);
 666         LR(0x3B);
 667         LR(0x13);
 668         LR(0x39);
 669         LR(0x1D);
 670         LR(0x3A);
 671         LR(0x24);
 672         LR(0x14);
 673         LR(0x15);
 674         LR(0x16);
 675         LRP(0x2D);
 676         LRP(0x2F);
 677         LR(0x1A);
 678         LR(0x1B);
 679         LR(0x1C);
 680         LR(0x23);
 681         LR(0x26);
 682         LR(0x28);
 683         LR(0x27);
 684         LR(0x21);
 685         LRP(0x2A);
 686         if (m->mode == MATROXFB_OUTPUT_MODE_PAL)
 687                 maven_set_reg(c, 0x35, 0x1D);
 688         else
 689                 maven_set_reg(c, 0x35, 0x1C);
 690         LRP(0x3C);
 691         LR(0x37);
 692         LR(0x38);
 693
 694         maven_get_reg(c, 0xB0);
 695         LR(0xB0);       /* output mode */
 696         LR(0x90);
 697         LR(0xBE);
 698         LR(0xC2);
 699
 700         LRP(0x9A);
 701         LRP(0xA2);
 702         LRP(0x9E);
 703         LRP(0xA6);
 704         LRP(0xAA);
 705         LRP(0xAC);
 706         maven_set_reg(c, 0x3E, 0x00);
 707         maven_set_reg(c, 0x95, 0x20);
 708 }
 709
 710 static int maven_find_exact_clocks(unsigned int ht, unsigned int vt,
 711                 struct mavenregs* m) {
 712         unsigned int x;
 713         unsigned int err = ~0;
 714
 715         /* 1:1 */
 716         m->regs[0x80] = 0x0F;
 717         m->regs[0x81] = 0x07;
 718         m->regs[0x82] = 0x81;
 719
 720         for (x = 0; x < 8; x++) {
 721                 unsigned int c;
 722                 unsigned int uninitialized_var(a), uninitialized_var(b),
 723                              uninitialized_var(h2);
 724                 unsigned int h = ht + 2 + x;
 725
 726                 if (!matroxfb_mavenclock((m->mode == MATROXFB_OUTPUT_MODE_PAL) ? &maven_PAL : &maven_NTSC, h, vt, &a, &b, &c, &h2)) {
 727                         unsigned int diff = h - h2;
 728
 729                         if (diff < err) {
 730                                 err = diff;
 731                                 m->regs[0x80] = a - 1;
 732                                 m->regs[0x81] = b - 1;
 733                                 m->regs[0x82] = c | 0x80;
 734                                 m->hcorr = h2 - 2;
 735                                 m->htotal = h - 2;
 736                         }
 737                 }
 738         }
 739         return err != ~0U;
 740 }
 741
 742 static inline int maven_compute_timming(struct maven_data* md,
 743                 struct my_timming* mt,
 744                 struct mavenregs* m) {
 745         unsigned int tmpi;
 746         unsigned int a, bv, c;
 747         MINFO_FROM(md->primary_head);
 748
 749         m->mode = ACCESS_FBINFO(outputs[1]).mode;
 750         if (m->mode != MATROXFB_OUTPUT_MODE_MONITOR) {
 751                 unsigned int lmargin;
 752                 unsigned int umargin;
 753                 unsigned int vslen;
 754                 unsigned int hcrt;
 755                 unsigned int slen;
 756
 757                 maven_init_TVdata(md, m);
 758
 759                 if (maven_find_exact_clocks(mt->HTotal, mt->VTotal, m) == 0)
 760                         return -EINVAL;
 761
 762                 lmargin = mt->HTotal - mt->HSyncEnd;
 763                 slen = mt->HSyncEnd - mt->HSyncStart;
 764                 hcrt = mt->HTotal - slen - mt->delay;
 765                 umargin = mt->VTotal - mt->VSyncEnd;
 766                 vslen = mt->VSyncEnd - mt->VSyncStart;
 767
 768                 if (m->hcorr < mt->HTotal)
 769                         hcrt += m->hcorr;
 770                 if (hcrt > mt->HTotal)
 771                         hcrt -= mt->HTotal;
 772                 if (hcrt + 2 > mt->HTotal)
 773                         hcrt = 0;       /* or issue warning? */
 774
 775                 /* last (first? middle?) line in picture can have different length */
 776                 /* hlen - 2 */
 777                 m->regs[0x96] = m->hcorr;
 778                 m->regs[0x97] = m->hcorr >> 8;
 779                 /* ... */
 780                 m->regs[0x98] = 0x00; m->regs[0x99] = 0x00;
 781                 /* hblanking end */
 782                 m->regs[0x9A] = lmargin;        /* 100% */
 783                 m->regs[0x9B] = lmargin >> 8;   /* 100% */
 784                 /* who knows */
 785                 m->regs[0x9C] = 0x04;
 786                 m->regs[0x9D] = 0x00;
 787                 /* htotal - 2 */
 788                 m->regs[0xA0] = m->htotal;
 789                 m->regs[0xA1] = m->htotal >> 8;
 790                 /* vblanking end */
 791                 m->regs[0xA2] = mt->VTotal - mt->VSyncStart - 1;        /* stop vblanking */
 792                 m->regs[0xA3] = (mt->VTotal - mt->VSyncStart - 1) >> 8;
 793                 /* something end... [A6]+1..[A8] */
 794                 if (md->version == MGATVO_B) {
 795                         m->regs[0xA4] = 0x04;
 796                         m->regs[0xA5] = 0x00;
 797                 } else {
 798                         m->regs[0xA4] = 0x01;
 799                         m->regs[0xA5] = 0x00;
 800                 }
 801                 /* something start... 0..[A4]-1 */
 802                 m->regs[0xA6] = 0x00;
 803                 m->regs[0xA7] = 0x00;
 804                 /* vertical line count - 1 */
 805                 m->regs[0xA8] = mt->VTotal - 1;
 806                 m->regs[0xA9] = (mt->VTotal - 1) >> 8;
 807                 /* horizontal vidrst pos */
 808                 m->regs[0xAA] = hcrt;           /* 0 <= hcrt <= htotal - 2 */
 809                 m->regs[0xAB] = hcrt >> 8;
 810                 /* vertical vidrst pos */
 811                 m->regs[0xAC] = mt->VTotal - 2;
 812                 m->regs[0xAD] = (mt->VTotal - 2) >> 8;
 813                 /* moves picture up/down and so on... */
 814                 m->regs[0xAE] = 0x01; /* Fix this... 0..VTotal */
 815                 m->regs[0xAF] = 0x00;
 816                 {
 817                         int hdec;
 818                         int hlen;
 819                         unsigned int ibmin = 4 + lmargin + mt->HDisplay;
 820                         unsigned int ib;
 821                         int i;
 822
 823                         /* Verify! */
 824                         /* Where 94208 came from? */
 825                         if (mt->HTotal)
 826                                 hdec = 94208 / (mt->HTotal);
 827                         else
 828                                 hdec = 0x81;
 829                         if (hdec > 0x81)
 830                                 hdec = 0x81;
 831                         if (hdec < 0x41)
 832                                 hdec = 0x41;
 833                         hdec--;
 834                         hlen = 98304 - 128 - ((lmargin + mt->HDisplay - 8) * hdec);
 835                         if (hlen < 0)
 836                                 hlen = 0;
 837                         hlen = hlen >> 8;
 838                         if (hlen > 0xFF)
 839                                 hlen = 0xFF;
 840                         /* Now we have to compute input buffer length.
 841                            If you want any picture, it must be between
 842                              4 + lmargin + xres
 843                            and
 844                              94208 / hdec
 845                            If you want perfect picture even on the top
 846                            of screen, it must be also
 847                              0x3C0000 * i / hdec + Q - R / hdec
 848                            where
 849                                 R      Qmin   Qmax
 850                              0x07000   0x5AE  0x5BF
 851                              0x08000   0x5CF  0x5FF
 852                              0x0C000   0x653  0x67F
 853                              0x10000   0x6F8  0x6FF
 854                          */
 855                         i = 1;
 856                         do {
 857                                 ib = ((0x3C0000 * i - 0x8000)/ hdec + 0x05E7) >> 8;
 858                                 i++;
 859                         } while (ib < ibmin);
 860                         if (ib >= m->htotal + 2) {
 861                                 ib = ibmin;
 862                         }
 863
 864                         m->regs[0x90] = hdec;   /* < 0x40 || > 0x80 is bad... 0x80 is questionable */
 865                         m->regs[0xC2] = hlen;
 866                         /* 'valid' input line length */
 867                         m->regs[0x9E] = ib;
 868                         m->regs[0x9F] = ib >> 8;
 869                 }
 870                 {
 871                         int vdec;
 872                         int vlen;
 873
 874 #define MATROX_USE64BIT_DIVIDE
 875                         if (mt->VTotal) {
 876 #ifdef MATROX_USE64BIT_DIVIDE
 877                                 u64 f1;
 878                                 u32 a;
 879                                 u32 b;
 880
 881                                 a = m->vlines * (m->htotal + 2);
 882                                 b = (mt->VTotal - 1) * (m->htotal + 2) + m->hcorr + 2;
 883
 884                                 f1 = ((u64)a) << 15;    /* *32768 */
 885                                 do_div(f1, b);
 886                                 vdec = f1;
 887 #else
 888                                 vdec = m->vlines * 32768 / mt->VTotal;
 889 #endif
 890                         } else
 891                                 vdec = 0x8000;
 892                         if (vdec > 0x8000)
 893                                 vdec = 0x8000;
 894                         vlen = (vslen + umargin + mt->VDisplay) * vdec;
 895                         vlen = (vlen >> 16) - 146; /* FIXME: 146?! */
 896                         if (vlen < 0)
 897                                 vlen = 0;
 898                         if (vlen > 0xFF)
 899                                 vlen = 0xFF;
 900                         vdec--;
 901                         m->regs[0x91] = vdec;
 902                         m->regs[0x92] = vdec >> 8;
 903                         m->regs[0xBE] = vlen;
 904                 }
 905                 m->regs[0xB0] = 0x08;   /* output: SVideo/Composite */
 906                 return 0;
 907         }
 908
 909         DAC1064_calcclock(mt->pixclock, 450000, &a, &bv, &c);
 910         m->regs[0x80] = a;
 911         m->regs[0x81] = bv;
 912         m->regs[0x82] = c | 0x80;
 913
 914         m->regs[0xB3] = 0x01;
 915         m->regs[0x94] = 0xB2;
 916
 917         /* htotal... */
 918         m->regs[0x96] = mt->HTotal;
 919         m->regs[0x97] = mt->HTotal >> 8;
 920         /* ?? */
 921         m->regs[0x98] = 0x00;
 922         m->regs[0x99] = 0x00;
 923         /* hsync len */
 924         tmpi = mt->HSyncEnd - mt->HSyncStart;
 925         m->regs[0x9A] = tmpi;
 926         m->regs[0x9B] = tmpi >> 8;
 927         /* hblank end */
 928         tmpi = mt->HTotal - mt->HSyncStart;
 929         m->regs[0x9C] = tmpi;
 930         m->regs[0x9D] = tmpi >> 8;
 931         /* hblank start */
 932         tmpi += mt->HDisplay;
 933         m->regs[0x9E] = tmpi;
 934         m->regs[0x9F] = tmpi >> 8;
 935         /* htotal + 1 */
 936         tmpi = mt->HTotal + 1;
 937         m->regs[0xA0] = tmpi;
 938         m->regs[0xA1] = tmpi >> 8;
 939         /* vsync?! */
 940         tmpi = mt->VSyncEnd - mt->VSyncStart - 1;
 941         m->regs[0xA2] = tmpi;
 942         m->regs[0xA3] = tmpi >> 8;
 943         /* ignored? */
 944         tmpi = mt->VTotal - mt->VSyncStart;
 945         m->regs[0xA4] = tmpi;
 946         m->regs[0xA5] = tmpi >> 8;
 947         /* ignored? */
 948         tmpi = mt->VTotal - 1;
 949         m->regs[0xA6] = tmpi;
 950         m->regs[0xA7] = tmpi >> 8;
 951         /* vtotal - 1 */
 952         m->regs[0xA8] = tmpi;
 953         m->regs[0xA9] = tmpi >> 8;
 954         /* hor vidrst */
 955         tmpi = mt->HTotal - mt->delay;
 956         m->regs[0xAA] = tmpi;
 957         m->regs[0xAB] = tmpi >> 8;
 958         /* vert vidrst */
 959         tmpi = mt->VTotal - 2;
 960         m->regs[0xAC] = tmpi;
 961         m->regs[0xAD] = tmpi >> 8;
 962         /* ignored? */
 963         m->regs[0xAE] = 0x00;
 964         m->regs[0xAF] = 0x00;
 965
 966         m->regs[0xB0] = 0x03;   /* output: monitor */
 967         m->regs[0xB1] = 0xA0;   /* ??? */
 968         m->regs[0x8C] = 0x20;   /* must be set... */
 969         m->regs[0x8D] = 0x04;   /* defaults to 0x10: test signal */
 970         m->regs[0xB9] = 0x1A;   /* defaults to 0x2C: too bright */
 971         m->regs[0xBF] = 0x22;   /* makes picture stable */
 972
 973         return 0;
 974 }
 975
 976 static int maven_program_timming(struct maven_data* md,
 977                 const struct mavenregs* m) {
 978         struct i2c_client* c = &md->client;
 979
 980         if (m->mode == MATROXFB_OUTPUT_MODE_MONITOR) {
 981                 LR(0x80);
 982                 LR(0x81);
 983                 LR(0x82);
 984
 985                 LR(0xB3);
 986                 LR(0x94);
 987
 988                 LRP(0x96);
 989                 LRP(0x98);
 990                 LRP(0x9A);
 991                 LRP(0x9C);
 992                 LRP(0x9E);
 993                 LRP(0xA0);
 994                 LRP(0xA2);
 995                 LRP(0xA4);
 996                 LRP(0xA6);
 997                 LRP(0xA8);
 998                 LRP(0xAA);
 999                 LRP(0xAC);
1000                 LRP(0xAE);
1001
1002                 LR(0xB0);       /* output: monitor */
1003                 LR(0xB1);       /* ??? */
1004                 LR(0x8C);       /* must be set... */
1005                 LR(0x8D);       /* defaults to 0x10: test signal */
1006                 LR(0xB9);       /* defaults to 0x2C: too bright */
1007                 LR(0xBF);       /* makes picture stable */
1008         } else {
1009                 maven_init_TV(c, m);
1010         }
1011         return 0;
1012 }
1013
1014 static inline int maven_resync(struct maven_data* md) {
1015         struct i2c_client* c = &md->client;
1016         maven_set_reg(c, 0x95, 0x20);   /* start whole thing */
1017         return 0;
1018 }
1019
1020 static int maven_get_queryctrl (struct maven_data* md,
1021                                 struct v4l2_queryctrl *p) {
1022         int i;
1023
1024         i = get_ctrl_id(p->id);
1025         if (i >= 0) {
1026                 *p = maven_controls[i].desc;
1027                 return 0;
1028         }
1029         if (i == -ENOENT) {
1030                 static const struct v4l2_queryctrl disctrl =
1031                         { .flags = V4L2_CTRL_FLAG_DISABLED };
1032
1033                 i = p->id;
1034                 *p = disctrl;
1035                 p->id = i;
1036                 sprintf(p->name, "Ctrl #%08X", i);
1037                 return 0;
1038         }
1039         return -EINVAL;
1040 }
1041
1042 static int maven_set_control (struct maven_data* md,
1043                               struct v4l2_control *p) {
1044         int i;
1045
1046         i = get_ctrl_id(p->id);
1047         if (i < 0) return -EINVAL;
1048
1049         /*
1050          * Check if changed.
1051          */
1052         if (p->value == *get_ctrl_ptr(md, i)) return 0;
1053
1054         /*
1055          * Check limits.
1056          */
1057         if (p->value > maven_controls[i].desc.maximum) return -EINVAL;
1058         if (p->value < maven_controls[i].desc.minimum) return -EINVAL;
1059
1060         /*
1061          * Store new value.
1062          */
1063         *get_ctrl_ptr(md, i) = p->value;
1064
1065         switch (p->id) {
1066                 case V4L2_CID_BRIGHTNESS:
1067                 case V4L2_CID_CONTRAST:
1068                 {
1069                   int blacklevel, whitelevel;
1070                   maven_compute_bwlevel(md, &blacklevel, &whitelevel);
1071                   blacklevel = (blacklevel >> 2) | ((blacklevel & 3) << 8);
1072                   whitelevel = (whitelevel >> 2) | ((whitelevel & 3) << 8);
1073                   maven_set_reg_pair(&md->client, 0x0e, blacklevel);
1074                   maven_set_reg_pair(&md->client, 0x1e, whitelevel);
1075                 }
1076                 break;
1077                 case V4L2_CID_SATURATION:
1078                 {
1079                   maven_set_reg(&md->client, 0x20, p->value);
1080                   maven_set_reg(&md->client, 0x22, p->value);
1081                 }
1082                 break;
1083                 case V4L2_CID_HUE:
1084                 {
1085                   maven_set_reg(&md->client, 0x25, p->value);
1086                 }
1087                 break;
1088                 case V4L2_CID_GAMMA:
1089                 {
1090                   const struct maven_gamma* g;
1091                   g = maven_compute_gamma(md);
1092                   maven_set_reg(&md->client, 0x83, g->reg83);
1093                   maven_set_reg(&md->client, 0x84, g->reg84);
1094                   maven_set_reg(&md->client, 0x85, g->reg85);
1095                   maven_set_reg(&md->client, 0x86, g->reg86);
1096                   maven_set_reg(&md->client, 0x87, g->reg87);
1097                   maven_set_reg(&md->client, 0x88, g->reg88);
1098                   maven_set_reg(&md->client, 0x89, g->reg89);
1099                   maven_set_reg(&md->client, 0x8a, g->reg8a);
1100                   maven_set_reg(&md->client, 0x8b, g->reg8b);
1101                 }
1102                 break;
1103                 case MATROXFB_CID_TESTOUT:
1104                 {
1105                         unsigned char val
1106                           = maven_get_reg(&md->client,0x8d);
1107                         if (p->value) val |= 0x10;
1108                         else          val &= ~0x10;
1109                         maven_set_reg(&md->client, 0x8d, val);
1110                 }
1111                 break;
1112                 case MATROXFB_CID_DEFLICKER:
1113                 {
1114                   maven_set_reg(&md->client, 0x93, maven_compute_deflicker(md));
1115                 }
1116                 break;
1117         }
1118
1119
1120         return 0;
1121 }
1122
1123 static int maven_get_control (struct maven_data* md,
1124                               struct v4l2_control *p) {
1125         int i;
1126
1127         i = get_ctrl_id(p->id);
1128         if (i < 0) return -EINVAL;
1129         p->value = *get_ctrl_ptr(md, i);
1130         return 0;
1131 }
1132
1133 /******************************************************/
1134
1135 static int maven_out_compute(void* md, struct my_timming* mt) {
1136 #define mdinfo ((struct maven_data*)md)
1137 #define minfo (mdinfo->primary_head)
1138         return maven_compute_timming(md, mt, &ACCESS_FBINFO(hw).maven);
1139 #undef minfo
1140 #undef mdinfo
1141 }
1142
1143 static int maven_out_program(void* md) {
1144 #define mdinfo ((struct maven_data*)md)
1145 #define minfo (mdinfo->primary_head)
1146         return maven_program_timming(md, &ACCESS_FBINFO(hw).maven);
1147 #undef minfo
1148 #undef mdinfo
1149 }
1150
1151 static int maven_out_start(void* md) {
1152         return maven_resync(md);
1153 }
1154
1155 static int maven_out_verify_mode(void* md, u_int32_t arg) {
1156         switch (arg) {
1157                 case MATROXFB_OUTPUT_MODE_PAL:
1158                 case MATROXFB_OUTPUT_MODE_NTSC:
1159                 case MATROXFB_OUTPUT_MODE_MONITOR:
1160                         return 0;
1161         }
1162         return -EINVAL;
1163 }
1164
1165 static int maven_out_get_queryctrl(void* md, struct v4l2_queryctrl* p) {
1166         return maven_get_queryctrl(md, p);
1167 }
1168
1169 static int maven_out_get_ctrl(void* md, struct v4l2_control* p) {
1170         return maven_get_control(md, p);
1171 }
1172
1173 static int maven_out_set_ctrl(void* md, struct v4l2_control* p) {
1174         return maven_set_control(md, p);
1175 }
1176
1177 static struct matrox_altout maven_altout = {
1178         .name           = "Secondary output",
1179         .compute        = maven_out_compute,
1180         .program        = maven_out_program,
1181         .start          = maven_out_start,
1182         .verifymode     = maven_out_verify_mode,
1183         .getqueryctrl   = maven_out_get_queryctrl,
1184         .getctrl        = maven_out_get_ctrl,
1185         .setctrl        = maven_out_set_ctrl,
1186 };
1187
1188 static int maven_init_client(struct i2c_client* clnt) {
1189         struct maven_data* md = i2c_get_clientdata(clnt);
1190         MINFO_FROM(container_of(clnt->adapter, struct i2c_bit_adapter, adapter)->minfo);
1191
1192         md->primary_head = MINFO;
1193         down_write(&ACCESS_FBINFO(altout.lock));
1194         ACCESS_FBINFO(outputs[1]).output = &maven_altout;
1195         ACCESS_FBINFO(outputs[1]).src = ACCESS_FBINFO(outputs[1]).default_src;
1196         ACCESS_FBINFO(outputs[1]).data = md;
1197         ACCESS_FBINFO(outputs[1]).mode = MATROXFB_OUTPUT_MODE_MONITOR;
1198         up_write(&ACCESS_FBINFO(altout.lock));
1199         if (maven_get_reg(clnt, 0xB2) < 0x14) {
1200                 md->version = MGATVO_B;
1201                 /* Tweak some things for this old chip */
1202         } else {
1203                 md->version = MGATVO_C;
1204         }
1205         /*
1206          * Set all parameters to its initial values.
1207          */
1208         {
1209                 unsigned int i;
1210
1211                 for (i = 0; i < MAVCTRLS; ++i) {
1212                         *get_ctrl_ptr(md, i) = maven_controls[i].desc.default_value;
1213                 }
1214         }
1215
1216         return 0;
1217 }
1218
1219 static int maven_shutdown_client(struct i2c_client* clnt) {
1220         struct maven_data* md = i2c_get_clientdata(clnt);
1221
1222         if (md->primary_head) {
1223                 MINFO_FROM(md->primary_head);
1224
1225                 down_write(&ACCESS_FBINFO(altout.lock));
1226                 ACCESS_FBINFO(outputs[1]).src = MATROXFB_SRC_NONE;
1227                 ACCESS_FBINFO(outputs[1]).output = NULL;
1228                 ACCESS_FBINFO(outputs[1]).data = NULL;
1229                 ACCESS_FBINFO(outputs[1]).mode = MATROXFB_OUTPUT_MODE_MONITOR;
1230                 up_write(&ACCESS_FBINFO(altout.lock));
1231                 md->primary_head = NULL;
1232         }
1233         return 0;
1234 }
1235
1236 static unsigned short normal_i2c[] = { MAVEN_I2CID, I2C_CLIENT_END };
1237 I2C_CLIENT_INSMOD;
1238
1239 static struct i2c_driver maven_driver;
1240
1241 static int maven_detect_client(struct i2c_adapter* adapter, int address, int kind) {
1242         int err = 0;
1243         struct i2c_client* new_client;
1244         struct maven_data* data;
1245
1246         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WRITE_WORD_DATA |
1247                                               I2C_FUNC_SMBUS_BYTE_DATA |
1248                                               I2C_FUNC_PROTOCOL_MANGLING))
1249                 goto ERROR0;
1250         if (!(data = kzalloc(sizeof(*data), GFP_KERNEL))) {
1251                 err = -ENOMEM;
1252                 goto ERROR0;
1253         }
1254         new_client = &data->client;
1255         i2c_set_clientdata(new_client, data);
1256         new_client->addr = address;
1257         new_client->adapter = adapter;
1258         new_client->driver = &maven_driver;
1259         new_client->flags = 0;
1260         strlcpy(new_client->name, "maven", I2C_NAME_SIZE);
1261         if ((err = i2c_attach_client(new_client)))
1262                 goto ERROR3;
1263         err = maven_init_client(new_client);
1264         if (err)
1265                 goto ERROR4;
1266         return 0;
1267 ERROR4:;
1268         i2c_detach_client(new_client);
1269 ERROR3:;
1270         kfree(new_client);
1271 ERROR0:;
1272         return err;
1273 }
1274
1275 static int maven_attach_adapter(struct i2c_adapter* adapter) {
1276         if (adapter->id == I2C_HW_B_G400)
1277                 return i2c_probe(adapter, &addr_data, &maven_detect_client);
1278         return 0;
1279 }
1280
1281 static int maven_detach_client(struct i2c_client* client) {
1282         int err;
1283
1284         if ((err = i2c_detach_client(client)))
1285                 return err;
1286         maven_shutdown_client(client);
1287         kfree(i2c_get_clientdata(client));
1288         return 0;
1289 }
1290
1291 static struct i2c_driver maven_driver={
1292         .driver = {
1293                 .name   = "maven",
1294         },
1295         .id             = I2C_DRIVERID_MGATVO,
1296         .attach_adapter = maven_attach_adapter,
1297         .detach_client  = maven_detach_client,
1298 };
1299
1300 static int __init matroxfb_maven_init(void)
1301 {
1302         return i2c_add_driver(&maven_driver);
1303 }
1304
1305 static void __exit matroxfb_maven_exit(void)
1306 {
1307         i2c_del_driver(&maven_driver);
1308 }
1309
1310 MODULE_AUTHOR("(c) 1999-2002 Petr Vandrovec <vandrove@vc.cvut.cz>");
1311 MODULE_DESCRIPTION("Matrox G200/G400 Matrox MGA-TVO driver");
1312 MODULE_LICENSE("GPL");
1313 module_init(matroxfb_maven_init);
1314 module_exit(matroxfb_maven_exit);
1315 /* we do not have __setup() yet */