int remap_vr()

in meta-facebook/meta-fbal/recipes-fbal/plat-libs/files/pal/pal_sensors.c [3006:3179]

• 154 lines of code
• 24 McCabe index (conditional complexity)

int remap_vr(uint8_t addr) { //8bit addr
  int fd = 0, ret = -1;
  uint8_t tbuf[64] = {0};
  uint8_t rbuf[64] = {0};
  uint8_t bus = I2C_BUS_1;
  uint8_t ver[4];
  uint8_t tmp[32] = {0};
  uint8_t retry = 5, clr_mode = 0;

  fd = i2c_cdev_slave_open(bus, addr >> 1, I2C_SLAVE_FORCE_CLAIM);
  if (fd < 0) {
    syslog(LOG_WARNING, "%s() Failed to open %d", __func__, bus);
    return ret;
  }

  do {
    tbuf[0] = 0x00;
    tbuf[1] = 0x00;
    if ( vr_block_rw(fd, addr, tbuf, 2, rbuf, 0) ) {
      syslog(LOG_WARNING, "set page fail slavaddr=%x cmd=%x\n", addr, tbuf[0]);
      break;
    }

    while ( retry > 0 ) {
      tbuf[0] = TI_ONLY_CONFIG2;
      ret = vr_block_rw(fd, addr, tbuf, 1, rbuf, 32);
      if ( ret != 0 ) {
        retry--;
        syslog(LOG_WARNING, "read vr id fail\n");
        continue;
      } else {
        if ( memcmp(tmp, rbuf, 32) == 0 ) {
#ifdef DEBUG
          syslog(LOG_WARNING, "vr data compare success\n");
#endif
          break;
        } else {
          memcpy(tmp, rbuf, 32);
          memset(rbuf, 0, sizeof(rbuf));
          retry--;
#ifdef DEBUG
          syslog(LOG_WARNING, "vr data compare fail retry=%d\n", retry);
#endif
        }
      }
    }
    if ( retry == 0 ) {
      break;
    }
#ifdef DEBUG
    for (int i=0; i<32; i++) {
      syslog(LOG_WARNING, "rbuf[%d] = %x\n", i, rbuf[i]);
    }
#endif

//Set TI Only config2
    tbuf[0] = TI_ONLY_CONFIG2;
    rbuf[10] |= 0x40;
    memcpy(tbuf+1, rbuf, 32);
    if ( vr_block_rw(fd, addr, tbuf, 33, rbuf, 0) ) {
      syslog(LOG_WARNING, "err1 bus=%x slavaddr=%x cmd=%x\n", bus, addr, tbuf[0]);
      break;
    }
    clr_mode = 1;

//Read FW Version.
    memset(tbuf,0,sizeof(tbuf));
    tbuf[0] = 0xf5;
    tbuf[1] = 0x04;
    tbuf[2] = 0xf0;
    tbuf[3] = 0x1F;
    tbuf[4] = 0x2C;
    tbuf[5] = 0x03;
    if ( vr_block_rw(fd, addr, tbuf, 6, rbuf, 0) ) {
      syslog(LOG_WARNING, "err2 bus=%x slavaddr=%x cmd=%x\n", bus, addr, tbuf[0]);
      break;
    }

    memset(tbuf,0,sizeof(tbuf));
    tbuf[0] = 0xf6;
    if ( vr_block_rw(fd, addr, tbuf, 1, rbuf, 5) ) {
      syslog(LOG_WARNING, "err3 bus=%x slavaddr=%x cmd=%x\n", bus, addr, tbuf[0]);
      break;
    }
    ver[0] = rbuf[1];
    ver[1] = rbuf[3];

    memset(tbuf,0,sizeof(tbuf));
    tbuf[0] = 0xf5;
    tbuf[1] = 0x04;
    tbuf[2] = 0xF4;
    tbuf[3] = 0x1F;
    tbuf[4] = 0x2C;
    tbuf[5] = 0x03;
    if ( vr_block_rw(fd, addr, tbuf, 6, rbuf, 0) ) {
      syslog(LOG_WARNING, "err4 bus=%x slavaddr=%x cmd=%x\n", bus, addr, tbuf[0]);
      break;
    }

    memset(tbuf,0,sizeof(tbuf));
    tbuf[0] = 0xf6;
    if ( vr_block_rw(fd, addr, tbuf, 1, rbuf, 5) ) {
      syslog(LOG_WARNING, "err5 bus=%x slavaddr=%x cmd=%x\n", bus, addr, tbuf[0]);
      break;
    }
    ver[2] = rbuf[1];
    ver[3] = rbuf[3];

//Set VR workaround version=2.7.10.0
    if( (ver[0] != 2) || (ver[1] != 7) || (ver[2] != 10) || (ver[3] != 0) ) {
      syslog(LOG_WARNING, "vr addr=0x%X version=%u.%u.%u.%u", addr, ver[0], ver[1], ver[2], ver[3]);
      break;
    }

    memset(tbuf,0,sizeof(tbuf));
    tbuf[0] = 0xf5;
    tbuf[1] = 0x04;
    tbuf[2] = 0xf0;
    tbuf[3] = 0x86;
    tbuf[4] = 0x64;
    tbuf[5] = 0x03;
    if ( vr_block_rw_check(fd, addr, tbuf, 6) ) {
      break;
    }

    memset(tbuf,0,sizeof(tbuf));
    tbuf[0] = 0xf6;
    tbuf[1] = 0x04;
    tbuf[2] = 0x01;
    tbuf[3] = 0xAA;
    tbuf[4] = 0x00;
    tbuf[5] = 0x00;
    if ( vr_block_rw_check(fd, addr, tbuf, 6) ) {
      break;
    }

    memset(tbuf,0,sizeof(tbuf));
    tbuf[0] = 0xf5;
    tbuf[1] = 0x04;
    tbuf[2] = 0xD8;
    tbuf[3] = 0x8A;
    tbuf[4] = 0x64;
    tbuf[5] = 0x03;
    if ( vr_block_rw_check(fd, addr, tbuf, 6) ) {
      break;
    }

    memset(tbuf,0,sizeof(tbuf));
    tbuf[0] = 0xf6;
    tbuf[1] = 0x04;
    tbuf[2] = 0x49;
    tbuf[3] = 0x1A;
    tbuf[4] = 0x00;
    tbuf[5] = 0x00;
    if ( vr_block_rw_check(fd, addr, tbuf, 6) ) {
      break;
    }

    syslog(LOG_WARNING, "vr workaround addr=0x%X version=%u.%u.%u.%u", addr, ver[0], ver[1], ver[2], ver[3]);
  } while(0);

//Restore TI_ONLY_CONFIG2
  if (clr_mode) {
    tbuf[0] = TI_ONLY_CONFIG2;
    tmp[10] &= 0xBF;
    memcpy(tbuf+1, tmp, 32);
    if ( vr_block_rw_check(fd, addr, tbuf, 33) ) {
      syslog(LOG_WARNING, "restore vr failed, addr=0x%X", addr);
    }
  }

  i2c_cdev_slave_close(fd);
  return ret;
}