drivers/media/dvb-frontends/cxd2880/cxd2880_tnrdmd_dvbt_mon.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * cxd2880_tnrdmd_dvbt_mon.c
   4  * Sony CXD2880 DVB-T2/T tuner + demodulator driver
   5  * DVB-T monitor functions
   6  *
   7  * Copyright (C) 2016, 2017, 2018 Sony Semiconductor Solutions Corporation
   8  */
   9
  10 #include "cxd2880_tnrdmd_mon.h"
  11 #include "cxd2880_tnrdmd_dvbt.h"
  12 #include "cxd2880_tnrdmd_dvbt_mon.h"
  13
  14 #include <media/dvb_math.h>
  15
  16 static const int ref_dbm_1000[3][5] = {
  17         {-93000, -91000, -90000, -89000, -88000},
  18         {-87000, -85000, -84000, -83000, -82000},
  19         {-82000, -80000, -78000, -77000, -76000},
  20 };
  21
  22 static int is_tps_locked(struct cxd2880_tnrdmd *tnr_dmd);
  23
  24 int cxd2880_tnrdmd_dvbt_mon_sync_stat(struct cxd2880_tnrdmd
  25                                       *tnr_dmd, u8 *sync_stat,
  26                                       u8 *ts_lock_stat,
  27                                       u8 *unlock_detected)
  28 {
  29         u8 rdata = 0x00;
  30         int ret;
  31
  32         if (!tnr_dmd || !sync_stat || !ts_lock_stat || !unlock_detected)
  33                 return -EINVAL;
  34
  35         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
  36                 return -EINVAL;
  37         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
  38                 return -EINVAL;
  39
  40         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
  41                                      CXD2880_IO_TGT_DMD,
  42                                      0x00, 0x0d);
  43         if (ret)
  44                 return ret;
  45
  46         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
  47                                      CXD2880_IO_TGT_DMD,
  48                                      0x10, &rdata, 1);
  49         if (ret)
  50                 return ret;
  51
  52         *unlock_detected = (rdata & 0x10) ? 1 : 0;
  53         *sync_stat = rdata & 0x07;
  54         *ts_lock_stat = (rdata & 0x20) ? 1 : 0;
  55
  56         if (*sync_stat == 0x07)
  57                 return -EAGAIN;
  58
  59         return ret;
  60 }
  61
  62 int cxd2880_tnrdmd_dvbt_mon_sync_stat_sub(struct cxd2880_tnrdmd
  63                                           *tnr_dmd, u8 *sync_stat,
  64                                           u8 *unlock_detected)
  65 {
  66         u8 ts_lock_stat = 0;
  67
  68         if (!tnr_dmd || !sync_stat || !unlock_detected)
  69                 return -EINVAL;
  70
  71         if (tnr_dmd->diver_mode != CXD2880_TNRDMD_DIVERMODE_MAIN)
  72                 return -EINVAL;
  73
  74         return cxd2880_tnrdmd_dvbt_mon_sync_stat(tnr_dmd->diver_sub,
  75                                                  sync_stat,
  76                                                  &ts_lock_stat,
  77                                                  unlock_detected);
  78 }
  79
  80 int cxd2880_tnrdmd_dvbt_mon_mode_guard(struct cxd2880_tnrdmd
  81                                        *tnr_dmd,
  82                                        enum cxd2880_dvbt_mode
  83                                        *mode,
  84                                        enum cxd2880_dvbt_guard
  85                                        *guard)
  86 {
  87         u8 rdata = 0x00;
  88         int ret;
  89
  90         if (!tnr_dmd || !mode || !guard)
  91                 return -EINVAL;
  92
  93         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
  94                 return -EINVAL;
  95
  96         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
  97                 return -EINVAL;
  98
  99         ret = slvt_freeze_reg(tnr_dmd);
 100         if (ret)
 101                 return ret;
 102
 103         ret = is_tps_locked(tnr_dmd);
 104         if (ret) {
 105                 slvt_unfreeze_reg(tnr_dmd);
 106
 107                 if (tnr_dmd->diver_mode == CXD2880_TNRDMD_DIVERMODE_MAIN)
 108                         ret =
 109                             cxd2880_tnrdmd_dvbt_mon_mode_guard(tnr_dmd->diver_sub,
 110                                                                mode, guard);
 111
 112                 return ret;
 113         }
 114
 115         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 116                                      CXD2880_IO_TGT_DMD,
 117                                      0x00, 0x0d);
 118         if (ret) {
 119                 slvt_unfreeze_reg(tnr_dmd);
 120                 return ret;
 121         }
 122
 123         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 124                                      CXD2880_IO_TGT_DMD,
 125                                      0x1b, &rdata, 1);
 126         if (ret) {
 127                 slvt_unfreeze_reg(tnr_dmd);
 128                 return ret;
 129         }
 130
 131         slvt_unfreeze_reg(tnr_dmd);
 132
 133         *mode = (enum cxd2880_dvbt_mode)((rdata >> 2) & 0x03);
 134         *guard = (enum cxd2880_dvbt_guard)(rdata & 0x03);
 135
 136         return ret;
 137 }
 138
 139 int cxd2880_tnrdmd_dvbt_mon_carrier_offset(struct cxd2880_tnrdmd
 140                                            *tnr_dmd, int *offset)
 141 {
 142         u8 rdata[4];
 143         u32 ctl_val = 0;
 144         int ret;
 145
 146         if (!tnr_dmd || !offset)
 147                 return -EINVAL;
 148
 149         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 150                 return -EINVAL;
 151
 152         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 153                 return -EINVAL;
 154
 155         ret = slvt_freeze_reg(tnr_dmd);
 156         if (ret)
 157                 return ret;
 158
 159         ret = is_tps_locked(tnr_dmd);
 160         if (ret) {
 161                 slvt_unfreeze_reg(tnr_dmd);
 162                 return ret;
 163         }
 164
 165         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 166                                      CXD2880_IO_TGT_DMD,
 167                                      0x00, 0x0d);
 168         if (ret) {
 169                 slvt_unfreeze_reg(tnr_dmd);
 170                 return ret;
 171         }
 172
 173         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 174                                      CXD2880_IO_TGT_DMD,
 175                                      0x1d, rdata, 4);
 176         if (ret) {
 177                 slvt_unfreeze_reg(tnr_dmd);
 178                 return ret;
 179         }
 180
 181         slvt_unfreeze_reg(tnr_dmd);
 182
 183         ctl_val =
 184             ((rdata[0] & 0x1f) << 24) | (rdata[1] << 16) | (rdata[2] << 8) |
 185             (rdata[3]);
 186         *offset = cxd2880_convert2s_complement(ctl_val, 29);
 187         *offset = -1 * ((*offset) * tnr_dmd->bandwidth / 235);
 188
 189         return ret;
 190 }
 191
 192 int cxd2880_tnrdmd_dvbt_mon_carrier_offset_sub(struct
 193                                                cxd2880_tnrdmd
 194                                                *tnr_dmd,
 195                                                int *offset)
 196 {
 197         if (!tnr_dmd || !offset)
 198                 return -EINVAL;
 199
 200         if (tnr_dmd->diver_mode != CXD2880_TNRDMD_DIVERMODE_MAIN)
 201                 return -EINVAL;
 202
 203         return cxd2880_tnrdmd_dvbt_mon_carrier_offset(tnr_dmd->diver_sub,
 204                                                       offset);
 205 }
 206
 207 int cxd2880_tnrdmd_dvbt_mon_tps_info(struct cxd2880_tnrdmd
 208                                      *tnr_dmd,
 209                                      struct cxd2880_dvbt_tpsinfo
 210                                      *info)
 211 {
 212         u8 rdata[7];
 213         u8 cell_id_ok = 0;
 214         int ret;
 215
 216         if (!tnr_dmd || !info)
 217                 return -EINVAL;
 218
 219         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 220                 return -EINVAL;
 221
 222         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 223                 return -EINVAL;
 224
 225         ret = slvt_freeze_reg(tnr_dmd);
 226         if (ret)
 227                 return ret;
 228
 229         ret = is_tps_locked(tnr_dmd);
 230         if (ret) {
 231                 slvt_unfreeze_reg(tnr_dmd);
 232
 233                 if (tnr_dmd->diver_mode == CXD2880_TNRDMD_DIVERMODE_MAIN)
 234                         ret =
 235                             cxd2880_tnrdmd_dvbt_mon_tps_info(tnr_dmd->diver_sub,
 236                                                              info);
 237
 238                 return ret;
 239         }
 240
 241         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 242                                      CXD2880_IO_TGT_DMD,
 243                                      0x00, 0x0d);
 244         if (ret) {
 245                 slvt_unfreeze_reg(tnr_dmd);
 246                 return ret;
 247         }
 248
 249         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 250                                      CXD2880_IO_TGT_DMD,
 251                                      0x29, rdata, 7);
 252         if (ret) {
 253                 slvt_unfreeze_reg(tnr_dmd);
 254                 return ret;
 255         }
 256
 257         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 258                                      CXD2880_IO_TGT_DMD,
 259                                      0x00, 0x11);
 260         if (ret) {
 261                 slvt_unfreeze_reg(tnr_dmd);
 262                 return ret;
 263         }
 264
 265         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 266                                      CXD2880_IO_TGT_DMD,
 267                                      0xd5, &cell_id_ok, 1);
 268         if (ret) {
 269                 slvt_unfreeze_reg(tnr_dmd);
 270                 return ret;
 271         }
 272
 273         slvt_unfreeze_reg(tnr_dmd);
 274
 275         info->constellation =
 276             (enum cxd2880_dvbt_constellation)((rdata[0] >> 6) & 0x03);
 277         info->hierarchy = (enum cxd2880_dvbt_hierarchy)((rdata[0] >> 3) & 0x07);
 278         info->rate_hp = (enum cxd2880_dvbt_coderate)(rdata[0] & 0x07);
 279         info->rate_lp = (enum cxd2880_dvbt_coderate)((rdata[1] >> 5) & 0x07);
 280         info->guard = (enum cxd2880_dvbt_guard)((rdata[1] >> 3) & 0x03);
 281         info->mode = (enum cxd2880_dvbt_mode)((rdata[1] >> 1) & 0x03);
 282         info->fnum = (rdata[2] >> 6) & 0x03;
 283         info->length_indicator = rdata[2] & 0x3f;
 284         info->cell_id = (rdata[3] << 8) | rdata[4];
 285         info->reserved_even = rdata[5] & 0x3f;
 286         info->reserved_odd = rdata[6] & 0x3f;
 287
 288         info->cell_id_ok = cell_id_ok & 0x01;
 289
 290         return ret;
 291 }
 292
 293 int cxd2880_tnrdmd_dvbt_mon_packet_error_number(struct
 294                                                 cxd2880_tnrdmd
 295                                                 *tnr_dmd,
 296                                                 u32 *pen)
 297 {
 298         u8 rdata[3];
 299         int ret;
 300
 301         if (!tnr_dmd || !pen)
 302                 return -EINVAL;
 303
 304         if (tnr_dmd->diver_mode == CXD2880_TNRDMD_DIVERMODE_SUB)
 305                 return -EINVAL;
 306
 307         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 308                 return -EINVAL;
 309
 310         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 311                 return -EINVAL;
 312
 313         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 314                                      CXD2880_IO_TGT_DMD,
 315                                      0x00, 0x0d);
 316         if (ret)
 317                 return ret;
 318
 319         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 320                                      CXD2880_IO_TGT_DMD,
 321                                      0x26, rdata, 3);
 322         if (ret)
 323                 return ret;
 324
 325         if (!(rdata[0] & 0x01))
 326                 return -EAGAIN;
 327
 328         *pen = (rdata[1] << 8) | rdata[2];
 329
 330         return ret;
 331 }
 332
 333 int cxd2880_tnrdmd_dvbt_mon_spectrum_sense(struct cxd2880_tnrdmd
 334                                            *tnr_dmd,
 335                                             enum
 336                                             cxd2880_tnrdmd_spectrum_sense
 337                                             *sense)
 338 {
 339         u8 data = 0;
 340         int ret;
 341
 342         if (!tnr_dmd || !sense)
 343                 return -EINVAL;
 344
 345         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 346                 return -EINVAL;
 347
 348         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 349                 return -EINVAL;
 350
 351         ret = slvt_freeze_reg(tnr_dmd);
 352         if (ret)
 353                 return ret;
 354
 355         ret = is_tps_locked(tnr_dmd);
 356         if (ret) {
 357                 slvt_unfreeze_reg(tnr_dmd);
 358
 359                 if (tnr_dmd->diver_mode == CXD2880_TNRDMD_DIVERMODE_MAIN)
 360                         ret = cxd2880_tnrdmd_dvbt_mon_spectrum_sense(tnr_dmd->diver_sub,
 361                                                                      sense);
 362
 363                 return ret;
 364         }
 365
 366         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 367                                      CXD2880_IO_TGT_DMD,
 368                                      0x00, 0x0d);
 369         if (ret) {
 370                 slvt_unfreeze_reg(tnr_dmd);
 371                 return ret;
 372         }
 373
 374         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 375                                      CXD2880_IO_TGT_DMD,
 376                                      0x1c, &data, sizeof(data));
 377         if (ret) {
 378                 slvt_unfreeze_reg(tnr_dmd);
 379                 return ret;
 380         }
 381
 382         slvt_unfreeze_reg(tnr_dmd);
 383
 384         *sense =
 385             (data & 0x01) ? CXD2880_TNRDMD_SPECTRUM_INV :
 386             CXD2880_TNRDMD_SPECTRUM_NORMAL;
 387
 388         return ret;
 389 }
 390
 391 static int dvbt_read_snr_reg(struct cxd2880_tnrdmd *tnr_dmd,
 392                              u16 *reg_value)
 393 {
 394         u8 rdata[2];
 395         int ret;
 396
 397         if (!tnr_dmd || !reg_value)
 398                 return -EINVAL;
 399
 400         ret = slvt_freeze_reg(tnr_dmd);
 401         if (ret)
 402                 return ret;
 403
 404         ret = is_tps_locked(tnr_dmd);
 405         if (ret) {
 406                 slvt_unfreeze_reg(tnr_dmd);
 407                 return ret;
 408         }
 409
 410         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 411                                      CXD2880_IO_TGT_DMD,
 412                                      0x00, 0x0d);
 413         if (ret) {
 414                 slvt_unfreeze_reg(tnr_dmd);
 415                 return ret;
 416         }
 417
 418         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 419                                      CXD2880_IO_TGT_DMD,
 420                                      0x13, rdata, 2);
 421         if (ret) {
 422                 slvt_unfreeze_reg(tnr_dmd);
 423                 return ret;
 424         }
 425
 426         slvt_unfreeze_reg(tnr_dmd);
 427
 428         *reg_value = (rdata[0] << 8) | rdata[1];
 429
 430         return ret;
 431 }
 432
 433 static int dvbt_calc_snr(struct cxd2880_tnrdmd *tnr_dmd,
 434                          u32 reg_value, int *snr)
 435 {
 436         if (!tnr_dmd || !snr)
 437                 return -EINVAL;
 438
 439         if (reg_value == 0)
 440                 return -EAGAIN;
 441
 442         if (reg_value > 4996)
 443                 reg_value = 4996;
 444
 445         *snr = intlog10(reg_value) - intlog10(5350 - reg_value);
 446         *snr = (*snr + 839) / 1678 + 28500;
 447
 448         return 0;
 449 }
 450
 451 int cxd2880_tnrdmd_dvbt_mon_snr(struct cxd2880_tnrdmd *tnr_dmd,
 452                                 int *snr)
 453 {
 454         u16 reg_value = 0;
 455         int ret;
 456
 457         if (!tnr_dmd || !snr)
 458                 return -EINVAL;
 459
 460         *snr = -1000 * 1000;
 461
 462         if (tnr_dmd->diver_mode == CXD2880_TNRDMD_DIVERMODE_SUB)
 463                 return -EINVAL;
 464
 465         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 466                 return -EINVAL;
 467
 468         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 469                 return -EINVAL;
 470
 471         if (tnr_dmd->diver_mode == CXD2880_TNRDMD_DIVERMODE_SINGLE) {
 472                 ret = dvbt_read_snr_reg(tnr_dmd, &reg_value);
 473                 if (ret)
 474                         return ret;
 475
 476                 ret = dvbt_calc_snr(tnr_dmd, reg_value, snr);
 477         } else {
 478                 int snr_main = 0;
 479                 int snr_sub = 0;
 480
 481                 ret =
 482                     cxd2880_tnrdmd_dvbt_mon_snr_diver(tnr_dmd, snr, &snr_main,
 483                                                       &snr_sub);
 484         }
 485
 486         return ret;
 487 }
 488
 489 int cxd2880_tnrdmd_dvbt_mon_snr_diver(struct cxd2880_tnrdmd
 490                                       *tnr_dmd, int *snr,
 491                                       int *snr_main, int *snr_sub)
 492 {
 493         u16 reg_value = 0;
 494         u32 reg_value_sum = 0;
 495         int ret;
 496
 497         if (!tnr_dmd || !snr || !snr_main || !snr_sub)
 498                 return -EINVAL;
 499
 500         *snr = -1000 * 1000;
 501         *snr_main = -1000 * 1000;
 502         *snr_sub = -1000 * 1000;
 503
 504         if (tnr_dmd->diver_mode != CXD2880_TNRDMD_DIVERMODE_MAIN)
 505                 return -EINVAL;
 506
 507         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 508                 return -EINVAL;
 509
 510         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 511                 return -EINVAL;
 512
 513         ret = dvbt_read_snr_reg(tnr_dmd, &reg_value);
 514         if (!ret) {
 515                 ret = dvbt_calc_snr(tnr_dmd, reg_value, snr_main);
 516                 if (ret)
 517                         reg_value = 0;
 518         } else if (ret == -EAGAIN) {
 519                 reg_value = 0;
 520         } else {
 521                 return ret;
 522         }
 523
 524         reg_value_sum += reg_value;
 525
 526         ret = dvbt_read_snr_reg(tnr_dmd->diver_sub, &reg_value);
 527         if (!ret) {
 528                 ret = dvbt_calc_snr(tnr_dmd->diver_sub, reg_value, snr_sub);
 529                 if (ret)
 530                         reg_value = 0;
 531         } else if (ret == -EAGAIN) {
 532                 reg_value = 0;
 533         } else {
 534                 return ret;
 535         }
 536
 537         reg_value_sum += reg_value;
 538
 539         return dvbt_calc_snr(tnr_dmd, reg_value_sum, snr);
 540 }
 541
 542 int cxd2880_tnrdmd_dvbt_mon_sampling_offset(struct cxd2880_tnrdmd
 543                                             *tnr_dmd, int *ppm)
 544 {
 545         u8 ctl_val_reg[5];
 546         u8 nominal_rate_reg[5];
 547         u32 trl_ctl_val = 0;
 548         u32 trcg_nominal_rate = 0;
 549         int num;
 550         int den;
 551         s8 diff_upper = 0;
 552         int ret;
 553
 554         if (!tnr_dmd || !ppm)
 555                 return -EINVAL;
 556
 557         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 558                 return -EINVAL;
 559
 560         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 561                 return -EINVAL;
 562
 563         ret = slvt_freeze_reg(tnr_dmd);
 564         if (ret)
 565                 return ret;
 566
 567         ret = is_tps_locked(tnr_dmd);
 568         if (ret) {
 569                 slvt_unfreeze_reg(tnr_dmd);
 570                 return ret;
 571         }
 572
 573         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 574                                      CXD2880_IO_TGT_DMD,
 575                                      0x00, 0x0d);
 576         if (ret) {
 577                 slvt_unfreeze_reg(tnr_dmd);
 578                 return ret;
 579         }
 580
 581         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 582                                      CXD2880_IO_TGT_DMD,
 583                                      0x21, ctl_val_reg,
 584                                      sizeof(ctl_val_reg));
 585         if (ret) {
 586                 slvt_unfreeze_reg(tnr_dmd);
 587                 return ret;
 588         }
 589
 590         ret = tnr_dmd->io->write_reg(tnr_dmd->io,
 591                                      CXD2880_IO_TGT_DMD,
 592                                      0x00, 0x04);
 593         if (ret) {
 594                 slvt_unfreeze_reg(tnr_dmd);
 595                 return ret;
 596         }
 597
 598         ret = tnr_dmd->io->read_regs(tnr_dmd->io,
 599                                      CXD2880_IO_TGT_DMD,
 600                                      0x60, nominal_rate_reg,
 601                                      sizeof(nominal_rate_reg));
 602         if (ret) {
 603                 slvt_unfreeze_reg(tnr_dmd);
 604                 return ret;
 605         }
 606
 607         slvt_unfreeze_reg(tnr_dmd);
 608
 609         diff_upper =
 610             (ctl_val_reg[0] & 0x7f) - (nominal_rate_reg[0] & 0x7f);
 611
 612         if (diff_upper < -1 || diff_upper > 1)
 613                 return -EAGAIN;
 614
 615         trl_ctl_val = ctl_val_reg[1] << 24;
 616         trl_ctl_val |= ctl_val_reg[2] << 16;
 617         trl_ctl_val |= ctl_val_reg[3] << 8;
 618         trl_ctl_val |= ctl_val_reg[4];
 619
 620         trcg_nominal_rate = nominal_rate_reg[1] << 24;
 621         trcg_nominal_rate |= nominal_rate_reg[2] << 16;
 622         trcg_nominal_rate |= nominal_rate_reg[3] << 8;
 623         trcg_nominal_rate |= nominal_rate_reg[4];
 624
 625         trl_ctl_val >>= 1;
 626         trcg_nominal_rate >>= 1;
 627
 628         if (diff_upper == 1)
 629                 num =
 630                     (int)((trl_ctl_val + 0x80000000u) -
 631                           trcg_nominal_rate);
 632         else if (diff_upper == -1)
 633                 num =
 634                     -(int)((trcg_nominal_rate + 0x80000000u) -
 635                            trl_ctl_val);
 636         else
 637                 num = (int)(trl_ctl_val - trcg_nominal_rate);
 638
 639         den = (nominal_rate_reg[0] & 0x7f) << 24;
 640         den |= nominal_rate_reg[1] << 16;
 641         den |= nominal_rate_reg[2] << 8;
 642         den |= nominal_rate_reg[3];
 643         den = (den + (390625 / 2)) / 390625;
 644
 645         den >>= 1;
 646
 647         if (num >= 0)
 648                 *ppm = (num + (den / 2)) / den;
 649         else
 650                 *ppm = (num - (den / 2)) / den;
 651
 652         return ret;
 653 }
 654
 655 int cxd2880_tnrdmd_dvbt_mon_sampling_offset_sub(struct
 656                                                 cxd2880_tnrdmd
 657                                                 *tnr_dmd, int *ppm)
 658 {
 659         if (!tnr_dmd || !ppm)
 660                 return -EINVAL;
 661
 662         if (tnr_dmd->diver_mode != CXD2880_TNRDMD_DIVERMODE_MAIN)
 663                 return -EINVAL;
 664
 665         return cxd2880_tnrdmd_dvbt_mon_sampling_offset(tnr_dmd->diver_sub, ppm);
 666 }
 667
 668 static int dvbt_calc_ssi(struct cxd2880_tnrdmd *tnr_dmd,
 669                          int rf_lvl, u8 *ssi)
 670 {
 671         struct cxd2880_dvbt_tpsinfo tps;
 672         int prel;
 673         int temp_ssi = 0;
 674         int ret;
 675
 676         if (!tnr_dmd || !ssi)
 677                 return -EINVAL;
 678
 679         ret = cxd2880_tnrdmd_dvbt_mon_tps_info(tnr_dmd, &tps);
 680         if (ret)
 681                 return ret;
 682
 683         if (tps.constellation >= CXD2880_DVBT_CONSTELLATION_RESERVED_3 ||
 684             tps.rate_hp >= CXD2880_DVBT_CODERATE_RESERVED_5)
 685                 return -EINVAL;
 686
 687         prel = rf_lvl - ref_dbm_1000[tps.constellation][tps.rate_hp];
 688
 689         if (prel < -15000)
 690                 temp_ssi = 0;
 691         else if (prel < 0)
 692                 temp_ssi = ((2 * (prel + 15000)) + 1500) / 3000;
 693         else if (prel < 20000)
 694                 temp_ssi = (((4 * prel) + 500) / 1000) + 10;
 695         else if (prel < 35000)
 696                 temp_ssi = (((2 * (prel - 20000)) + 1500) / 3000) + 90;
 697         else
 698                 temp_ssi = 100;
 699
 700         *ssi = (temp_ssi > 100) ? 100 : (u8)temp_ssi;
 701
 702         return ret;
 703 }
 704
 705 int cxd2880_tnrdmd_dvbt_mon_ssi(struct cxd2880_tnrdmd *tnr_dmd,
 706                                 u8 *ssi)
 707 {
 708         int rf_lvl = 0;
 709         int ret;
 710
 711         if (!tnr_dmd || !ssi)
 712                 return -EINVAL;
 713
 714         if (tnr_dmd->diver_mode == CXD2880_TNRDMD_DIVERMODE_SUB)
 715                 return -EINVAL;
 716
 717         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 718                 return -EINVAL;
 719
 720         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 721                 return -EINVAL;
 722
 723         ret = cxd2880_tnrdmd_mon_rf_lvl(tnr_dmd, &rf_lvl);
 724         if (ret)
 725                 return ret;
 726
 727         return dvbt_calc_ssi(tnr_dmd, rf_lvl, ssi);
 728 }
 729
 730 int cxd2880_tnrdmd_dvbt_mon_ssi_sub(struct cxd2880_tnrdmd *tnr_dmd,
 731                                     u8 *ssi)
 732 {
 733         int rf_lvl = 0;
 734         int ret;
 735
 736         if (!tnr_dmd || !ssi)
 737                 return -EINVAL;
 738
 739         if (tnr_dmd->diver_mode != CXD2880_TNRDMD_DIVERMODE_MAIN)
 740                 return -EINVAL;
 741
 742         if (tnr_dmd->state != CXD2880_TNRDMD_STATE_ACTIVE)
 743                 return -EINVAL;
 744
 745         if (tnr_dmd->sys != CXD2880_DTV_SYS_DVBT)
 746                 return -EINVAL;
 747
 748         ret = cxd2880_tnrdmd_mon_rf_lvl(tnr_dmd->diver_sub, &rf_lvl);
 749         if (ret)
 750                 return ret;
 751
 752         return dvbt_calc_ssi(tnr_dmd, rf_lvl, ssi);
 753 }
 754
 755 static int is_tps_locked(struct cxd2880_tnrdmd *tnr_dmd)
 756 {
 757         u8 sync = 0;
 758         u8 tslock = 0;
 759         u8 early_unlock = 0;
 760         int ret;
 761
 762         if (!tnr_dmd)
 763                 return -EINVAL;
 764
 765         ret =
 766             cxd2880_tnrdmd_dvbt_mon_sync_stat(tnr_dmd, &sync, &tslock,
 767                                               &early_unlock);
 768         if (ret)
 769                 return ret;
 770
 771         if (sync != 6)
 772                 return -EAGAIN;
 773
 774         return 0;
 775 }