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 <linux/int_log.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 
cxd2880_tnrdmd_dvbt_mon_sync_stat(struct cxd2880_tnrdmd * tnr_dmd,u8 * sync_stat,u8 * ts_lock_stat,u8 * unlock_detected)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 
cxd2880_tnrdmd_dvbt_mon_sync_stat_sub(struct cxd2880_tnrdmd * tnr_dmd,u8 * sync_stat,u8 * unlock_detected)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 
cxd2880_tnrdmd_dvbt_mon_mode_guard(struct cxd2880_tnrdmd * tnr_dmd,enum cxd2880_dvbt_mode * mode,enum cxd2880_dvbt_guard * guard)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 
cxd2880_tnrdmd_dvbt_mon_carrier_offset(struct cxd2880_tnrdmd * tnr_dmd,int * offset)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 
cxd2880_tnrdmd_dvbt_mon_carrier_offset_sub(struct cxd2880_tnrdmd * tnr_dmd,int * offset)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 
cxd2880_tnrdmd_dvbt_mon_tps_info(struct cxd2880_tnrdmd * tnr_dmd,struct cxd2880_dvbt_tpsinfo * info)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 
cxd2880_tnrdmd_dvbt_mon_packet_error_number(struct cxd2880_tnrdmd * tnr_dmd,u32 * pen)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 
cxd2880_tnrdmd_dvbt_mon_spectrum_sense(struct cxd2880_tnrdmd * tnr_dmd,enum cxd2880_tnrdmd_spectrum_sense * sense)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 
dvbt_read_snr_reg(struct cxd2880_tnrdmd * tnr_dmd,u16 * reg_value)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 
dvbt_calc_snr(struct cxd2880_tnrdmd * tnr_dmd,u32 reg_value,int * snr)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 
cxd2880_tnrdmd_dvbt_mon_snr(struct cxd2880_tnrdmd * tnr_dmd,int * snr)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 
cxd2880_tnrdmd_dvbt_mon_snr_diver(struct cxd2880_tnrdmd * tnr_dmd,int * snr,int * snr_main,int * snr_sub)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 
cxd2880_tnrdmd_dvbt_mon_sampling_offset(struct cxd2880_tnrdmd * tnr_dmd,int * ppm)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 
cxd2880_tnrdmd_dvbt_mon_sampling_offset_sub(struct cxd2880_tnrdmd * tnr_dmd,int * ppm)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 
dvbt_calc_ssi(struct cxd2880_tnrdmd * tnr_dmd,int rf_lvl,u8 * ssi)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 
cxd2880_tnrdmd_dvbt_mon_ssi(struct cxd2880_tnrdmd * tnr_dmd,u8 * ssi)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 
cxd2880_tnrdmd_dvbt_mon_ssi_sub(struct cxd2880_tnrdmd * tnr_dmd,u8 * ssi)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 
is_tps_locked(struct cxd2880_tnrdmd * tnr_dmd)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 }
776