1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * vivid-sdr-cap.c - software defined radio support functions. 4 * 5 * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved. 6 */ 7 8 #include <linux/errno.h> 9 #include <linux/kernel.h> 10 #include <linux/delay.h> 11 #include <linux/kthread.h> 12 #include <linux/freezer.h> 13 #include <linux/math64.h> 14 #include <linux/videodev2.h> 15 #include <linux/v4l2-dv-timings.h> 16 #include <media/v4l2-common.h> 17 #include <media/v4l2-event.h> 18 #include <media/v4l2-dv-timings.h> 19 #include <linux/fixp-arith.h> 20 #include <linux/jiffies.h> 21 22 #include "vivid-core.h" 23 #include "vivid-ctrls.h" 24 #include "vivid-sdr-cap.h" 25 26 /* stream formats */ 27 struct vivid_format { 28 u32 pixelformat; 29 u32 buffersize; 30 }; 31 32 /* format descriptions for capture and preview */ 33 static const struct vivid_format formats[] = { 34 { 35 .pixelformat = V4L2_SDR_FMT_CU8, 36 .buffersize = SDR_CAP_SAMPLES_PER_BUF * 2, 37 }, { 38 .pixelformat = V4L2_SDR_FMT_CS8, 39 .buffersize = SDR_CAP_SAMPLES_PER_BUF * 2, 40 }, 41 }; 42 43 static const struct v4l2_frequency_band bands_adc[] = { 44 { 45 .tuner = 0, 46 .type = V4L2_TUNER_ADC, 47 .index = 0, 48 .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS, 49 .rangelow = 300000, 50 .rangehigh = 300000, 51 }, 52 { 53 .tuner = 0, 54 .type = V4L2_TUNER_ADC, 55 .index = 1, 56 .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS, 57 .rangelow = 900001, 58 .rangehigh = 2800000, 59 }, 60 { 61 .tuner = 0, 62 .type = V4L2_TUNER_ADC, 63 .index = 2, 64 .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS, 65 .rangelow = 3200000, 66 .rangehigh = 3200000, 67 }, 68 }; 69 70 /* ADC band midpoints */ 71 #define BAND_ADC_0 ((bands_adc[0].rangehigh + bands_adc[1].rangelow) / 2) 72 #define BAND_ADC_1 ((bands_adc[1].rangehigh + bands_adc[2].rangelow) / 2) 73 74 static const struct v4l2_frequency_band bands_fm[] = { 75 { 76 .tuner = 1, 77 .type = V4L2_TUNER_RF, 78 .index = 0, 79 .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS, 80 .rangelow = 50000000, 81 .rangehigh = 2000000000, 82 }, 83 }; 84 85 static void vivid_thread_sdr_cap_tick(struct vivid_dev *dev) 86 { 87 struct vivid_buffer *sdr_cap_buf = NULL; 88 89 dprintk(dev, 1, "SDR Capture Thread Tick\n"); 90 91 /* Drop a certain percentage of buffers. */ 92 if (dev->perc_dropped_buffers && 93 get_random_u32_below(100) < dev->perc_dropped_buffers) 94 return; 95 96 spin_lock(&dev->slock); 97 if (!list_empty(&dev->sdr_cap_active)) { 98 sdr_cap_buf = list_entry(dev->sdr_cap_active.next, 99 struct vivid_buffer, list); 100 list_del(&sdr_cap_buf->list); 101 } 102 spin_unlock(&dev->slock); 103 104 if (sdr_cap_buf) { 105 sdr_cap_buf->vb.sequence = dev->sdr_cap_with_seq_wrap_count; 106 v4l2_ctrl_request_setup(sdr_cap_buf->vb.vb2_buf.req_obj.req, 107 &dev->ctrl_hdl_sdr_cap); 108 v4l2_ctrl_request_complete(sdr_cap_buf->vb.vb2_buf.req_obj.req, 109 &dev->ctrl_hdl_sdr_cap); 110 vivid_sdr_cap_process(dev, sdr_cap_buf); 111 sdr_cap_buf->vb.vb2_buf.timestamp = 112 ktime_get_ns() + dev->time_wrap_offset; 113 vb2_buffer_done(&sdr_cap_buf->vb.vb2_buf, dev->dqbuf_error ? 114 VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE); 115 dev->dqbuf_error = false; 116 } 117 } 118 119 static int vivid_thread_sdr_cap(void *data) 120 { 121 struct vivid_dev *dev = data; 122 u64 samples_since_start; 123 u64 buffers_since_start; 124 u64 next_jiffies_since_start; 125 unsigned long jiffies_since_start; 126 unsigned long cur_jiffies; 127 unsigned wait_jiffies; 128 129 dprintk(dev, 1, "SDR Capture Thread Start\n"); 130 131 set_freezable(); 132 133 /* Resets frame counters */ 134 dev->sdr_cap_seq_offset = 0; 135 dev->sdr_cap_seq_count = 0; 136 dev->jiffies_sdr_cap = jiffies; 137 dev->sdr_cap_seq_resync = false; 138 if (dev->time_wrap) 139 dev->time_wrap_offset = dev->time_wrap - ktime_get_ns(); 140 else 141 dev->time_wrap_offset = 0; 142 143 for (;;) { 144 try_to_freeze(); 145 if (kthread_should_stop()) 146 break; 147 148 if (!mutex_trylock(&dev->mutex)) { 149 schedule(); 150 continue; 151 } 152 153 cur_jiffies = jiffies; 154 if (dev->sdr_cap_seq_resync) { 155 dev->jiffies_sdr_cap = cur_jiffies; 156 dev->sdr_cap_seq_offset = dev->sdr_cap_seq_count + 1; 157 dev->sdr_cap_seq_count = 0; 158 dev->sdr_cap_seq_resync = false; 159 } 160 /* Calculate the number of jiffies since we started streaming */ 161 jiffies_since_start = cur_jiffies - dev->jiffies_sdr_cap; 162 /* Get the number of buffers streamed since the start */ 163 buffers_since_start = 164 (u64)jiffies_since_start * dev->sdr_adc_freq + 165 (HZ * SDR_CAP_SAMPLES_PER_BUF) / 2; 166 do_div(buffers_since_start, HZ * SDR_CAP_SAMPLES_PER_BUF); 167 168 /* 169 * After more than 0xf0000000 (rounded down to a multiple of 170 * 'jiffies-per-day' to ease jiffies_to_msecs calculation) 171 * jiffies have passed since we started streaming reset the 172 * counters and keep track of the sequence offset. 173 */ 174 if (jiffies_since_start > JIFFIES_RESYNC) { 175 dev->jiffies_sdr_cap = cur_jiffies; 176 dev->sdr_cap_seq_offset = buffers_since_start; 177 buffers_since_start = 0; 178 } 179 dev->sdr_cap_seq_count = 180 buffers_since_start + dev->sdr_cap_seq_offset; 181 dev->sdr_cap_with_seq_wrap_count = dev->sdr_cap_seq_count - dev->sdr_cap_seq_start; 182 183 vivid_thread_sdr_cap_tick(dev); 184 mutex_unlock(&dev->mutex); 185 186 /* 187 * Calculate the number of samples streamed since we started, 188 * not including the current buffer. 189 */ 190 samples_since_start = buffers_since_start * SDR_CAP_SAMPLES_PER_BUF; 191 192 /* And the number of jiffies since we started */ 193 jiffies_since_start = jiffies - dev->jiffies_sdr_cap; 194 195 /* Increase by the number of samples in one buffer */ 196 samples_since_start += SDR_CAP_SAMPLES_PER_BUF; 197 /* 198 * Calculate when that next buffer is supposed to start 199 * in jiffies since we started streaming. 200 */ 201 next_jiffies_since_start = samples_since_start * HZ + 202 dev->sdr_adc_freq / 2; 203 do_div(next_jiffies_since_start, dev->sdr_adc_freq); 204 /* If it is in the past, then just schedule asap */ 205 if (next_jiffies_since_start < jiffies_since_start) 206 next_jiffies_since_start = jiffies_since_start; 207 208 wait_jiffies = next_jiffies_since_start - jiffies_since_start; 209 while (time_is_after_jiffies(cur_jiffies + wait_jiffies) && 210 !kthread_should_stop()) 211 schedule(); 212 } 213 dprintk(dev, 1, "SDR Capture Thread End\n"); 214 return 0; 215 } 216 217 static int sdr_cap_queue_setup(struct vb2_queue *vq, 218 unsigned *nbuffers, unsigned *nplanes, 219 unsigned sizes[], struct device *alloc_devs[]) 220 { 221 /* 2 = max 16-bit sample returned */ 222 u32 size = SDR_CAP_SAMPLES_PER_BUF * 2; 223 224 if (*nplanes) 225 return sizes[0] < size ? -EINVAL : 0; 226 227 *nplanes = 1; 228 sizes[0] = size; 229 return 0; 230 } 231 232 static int sdr_cap_buf_prepare(struct vb2_buffer *vb) 233 { 234 struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 235 unsigned size = SDR_CAP_SAMPLES_PER_BUF * 2; 236 237 dprintk(dev, 1, "%s\n", __func__); 238 239 if (dev->buf_prepare_error) { 240 /* 241 * Error injection: test what happens if buf_prepare() returns 242 * an error. 243 */ 244 dev->buf_prepare_error = false; 245 return -EINVAL; 246 } 247 if (vb2_plane_size(vb, 0) < size) { 248 dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n", 249 __func__, vb2_plane_size(vb, 0), size); 250 return -EINVAL; 251 } 252 vb2_set_plane_payload(vb, 0, size); 253 254 return 0; 255 } 256 257 static void sdr_cap_buf_queue(struct vb2_buffer *vb) 258 { 259 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 260 struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 261 struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb); 262 263 dprintk(dev, 1, "%s\n", __func__); 264 265 spin_lock(&dev->slock); 266 list_add_tail(&buf->list, &dev->sdr_cap_active); 267 spin_unlock(&dev->slock); 268 } 269 270 static int sdr_cap_start_streaming(struct vb2_queue *vq, unsigned count) 271 { 272 struct vivid_dev *dev = vb2_get_drv_priv(vq); 273 int err = 0; 274 275 dprintk(dev, 1, "%s\n", __func__); 276 dev->sdr_cap_seq_start = dev->seq_wrap * 128; 277 if (dev->start_streaming_error) { 278 dev->start_streaming_error = false; 279 err = -EINVAL; 280 } else if (dev->kthread_sdr_cap == NULL) { 281 dev->kthread_sdr_cap = kthread_run(vivid_thread_sdr_cap, dev, 282 "%s-sdr-cap", dev->v4l2_dev.name); 283 284 if (IS_ERR(dev->kthread_sdr_cap)) { 285 v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n"); 286 err = PTR_ERR(dev->kthread_sdr_cap); 287 dev->kthread_sdr_cap = NULL; 288 } 289 } 290 if (err) { 291 struct vivid_buffer *buf, *tmp; 292 293 list_for_each_entry_safe(buf, tmp, &dev->sdr_cap_active, list) { 294 list_del(&buf->list); 295 vb2_buffer_done(&buf->vb.vb2_buf, 296 VB2_BUF_STATE_QUEUED); 297 } 298 } 299 return err; 300 } 301 302 /* abort streaming and wait for last buffer */ 303 static void sdr_cap_stop_streaming(struct vb2_queue *vq) 304 { 305 struct vivid_dev *dev = vb2_get_drv_priv(vq); 306 307 if (dev->kthread_sdr_cap == NULL) 308 return; 309 310 while (!list_empty(&dev->sdr_cap_active)) { 311 struct vivid_buffer *buf; 312 313 buf = list_entry(dev->sdr_cap_active.next, 314 struct vivid_buffer, list); 315 list_del(&buf->list); 316 v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req, 317 &dev->ctrl_hdl_sdr_cap); 318 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 319 } 320 321 /* shutdown control thread */ 322 kthread_stop(dev->kthread_sdr_cap); 323 dev->kthread_sdr_cap = NULL; 324 } 325 326 static void sdr_cap_buf_request_complete(struct vb2_buffer *vb) 327 { 328 struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 329 330 v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_sdr_cap); 331 } 332 333 const struct vb2_ops vivid_sdr_cap_qops = { 334 .queue_setup = sdr_cap_queue_setup, 335 .buf_prepare = sdr_cap_buf_prepare, 336 .buf_queue = sdr_cap_buf_queue, 337 .start_streaming = sdr_cap_start_streaming, 338 .stop_streaming = sdr_cap_stop_streaming, 339 .buf_request_complete = sdr_cap_buf_request_complete, 340 .wait_prepare = vb2_ops_wait_prepare, 341 .wait_finish = vb2_ops_wait_finish, 342 }; 343 344 int vivid_sdr_enum_freq_bands(struct file *file, void *fh, 345 struct v4l2_frequency_band *band) 346 { 347 switch (band->tuner) { 348 case 0: 349 if (band->index >= ARRAY_SIZE(bands_adc)) 350 return -EINVAL; 351 *band = bands_adc[band->index]; 352 return 0; 353 case 1: 354 if (band->index >= ARRAY_SIZE(bands_fm)) 355 return -EINVAL; 356 *band = bands_fm[band->index]; 357 return 0; 358 default: 359 return -EINVAL; 360 } 361 } 362 363 int vivid_sdr_g_frequency(struct file *file, void *fh, 364 struct v4l2_frequency *vf) 365 { 366 struct vivid_dev *dev = video_drvdata(file); 367 368 switch (vf->tuner) { 369 case 0: 370 vf->frequency = dev->sdr_adc_freq; 371 vf->type = V4L2_TUNER_ADC; 372 return 0; 373 case 1: 374 vf->frequency = dev->sdr_fm_freq; 375 vf->type = V4L2_TUNER_RF; 376 return 0; 377 default: 378 return -EINVAL; 379 } 380 } 381 382 int vivid_sdr_s_frequency(struct file *file, void *fh, 383 const struct v4l2_frequency *vf) 384 { 385 struct vivid_dev *dev = video_drvdata(file); 386 unsigned freq = vf->frequency; 387 unsigned band; 388 389 switch (vf->tuner) { 390 case 0: 391 if (vf->type != V4L2_TUNER_ADC) 392 return -EINVAL; 393 if (freq < BAND_ADC_0) 394 band = 0; 395 else if (freq < BAND_ADC_1) 396 band = 1; 397 else 398 band = 2; 399 400 freq = clamp_t(unsigned, freq, 401 bands_adc[band].rangelow, 402 bands_adc[band].rangehigh); 403 404 if (vb2_is_streaming(&dev->vb_sdr_cap_q) && 405 freq != dev->sdr_adc_freq) { 406 /* resync the thread's timings */ 407 dev->sdr_cap_seq_resync = true; 408 } 409 dev->sdr_adc_freq = freq; 410 return 0; 411 case 1: 412 if (vf->type != V4L2_TUNER_RF) 413 return -EINVAL; 414 dev->sdr_fm_freq = clamp_t(unsigned, freq, 415 bands_fm[0].rangelow, 416 bands_fm[0].rangehigh); 417 return 0; 418 default: 419 return -EINVAL; 420 } 421 } 422 423 int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt) 424 { 425 switch (vt->index) { 426 case 0: 427 strscpy(vt->name, "ADC", sizeof(vt->name)); 428 vt->type = V4L2_TUNER_ADC; 429 vt->capability = 430 V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS; 431 vt->rangelow = bands_adc[0].rangelow; 432 vt->rangehigh = bands_adc[2].rangehigh; 433 return 0; 434 case 1: 435 strscpy(vt->name, "RF", sizeof(vt->name)); 436 vt->type = V4L2_TUNER_RF; 437 vt->capability = 438 V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS; 439 vt->rangelow = bands_fm[0].rangelow; 440 vt->rangehigh = bands_fm[0].rangehigh; 441 return 0; 442 default: 443 return -EINVAL; 444 } 445 } 446 447 int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt) 448 { 449 if (vt->index > 1) 450 return -EINVAL; 451 return 0; 452 } 453 454 int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f) 455 { 456 if (f->index >= ARRAY_SIZE(formats)) 457 return -EINVAL; 458 f->pixelformat = formats[f->index].pixelformat; 459 return 0; 460 } 461 462 int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 463 { 464 struct vivid_dev *dev = video_drvdata(file); 465 466 f->fmt.sdr.pixelformat = dev->sdr_pixelformat; 467 f->fmt.sdr.buffersize = dev->sdr_buffersize; 468 return 0; 469 } 470 471 int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 472 { 473 struct vivid_dev *dev = video_drvdata(file); 474 struct vb2_queue *q = &dev->vb_sdr_cap_q; 475 int i; 476 477 if (vb2_is_busy(q)) 478 return -EBUSY; 479 480 for (i = 0; i < ARRAY_SIZE(formats); i++) { 481 if (formats[i].pixelformat == f->fmt.sdr.pixelformat) { 482 dev->sdr_pixelformat = formats[i].pixelformat; 483 dev->sdr_buffersize = formats[i].buffersize; 484 f->fmt.sdr.buffersize = formats[i].buffersize; 485 return 0; 486 } 487 } 488 dev->sdr_pixelformat = formats[0].pixelformat; 489 dev->sdr_buffersize = formats[0].buffersize; 490 f->fmt.sdr.pixelformat = formats[0].pixelformat; 491 f->fmt.sdr.buffersize = formats[0].buffersize; 492 return 0; 493 } 494 495 int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 496 { 497 int i; 498 499 for (i = 0; i < ARRAY_SIZE(formats); i++) { 500 if (formats[i].pixelformat == f->fmt.sdr.pixelformat) { 501 f->fmt.sdr.buffersize = formats[i].buffersize; 502 return 0; 503 } 504 } 505 f->fmt.sdr.pixelformat = formats[0].pixelformat; 506 f->fmt.sdr.buffersize = formats[0].buffersize; 507 return 0; 508 } 509 510 #define FIXP_N (15) 511 #define FIXP_FRAC (1 << FIXP_N) 512 #define FIXP_2PI ((int)(2 * 3.141592653589 * FIXP_FRAC)) 513 #define M_100000PI (3.14159 * 100000) 514 515 void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf) 516 { 517 u8 *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0); 518 unsigned long i; 519 unsigned long plane_size = vb2_plane_size(&buf->vb.vb2_buf, 0); 520 s64 s64tmp; 521 s32 src_phase_step; 522 s32 mod_phase_step; 523 s32 fixp_i; 524 s32 fixp_q; 525 526 /* calculate phase step */ 527 #define BEEP_FREQ 1000 /* 1kHz beep */ 528 src_phase_step = DIV_ROUND_CLOSEST(FIXP_2PI * BEEP_FREQ, 529 dev->sdr_adc_freq); 530 531 for (i = 0; i < plane_size; i += 2) { 532 mod_phase_step = fixp_cos32_rad(dev->sdr_fixp_src_phase, 533 FIXP_2PI) >> (31 - FIXP_N); 534 535 dev->sdr_fixp_src_phase += src_phase_step; 536 s64tmp = (s64) mod_phase_step * dev->sdr_fm_deviation; 537 dev->sdr_fixp_mod_phase += div_s64(s64tmp, M_100000PI); 538 539 /* 540 * Transfer phase angle to [0, 2xPI] in order to avoid variable 541 * overflow and make it suitable for cosine implementation 542 * used, which does not support negative angles. 543 */ 544 dev->sdr_fixp_src_phase %= FIXP_2PI; 545 dev->sdr_fixp_mod_phase %= FIXP_2PI; 546 547 if (dev->sdr_fixp_mod_phase < 0) 548 dev->sdr_fixp_mod_phase += FIXP_2PI; 549 550 fixp_i = fixp_cos32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI); 551 fixp_q = fixp_sin32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI); 552 553 /* Normalize fraction values represented with 32 bit precision 554 * to fixed point representation with FIXP_N bits */ 555 fixp_i >>= (31 - FIXP_N); 556 fixp_q >>= (31 - FIXP_N); 557 558 switch (dev->sdr_pixelformat) { 559 case V4L2_SDR_FMT_CU8: 560 /* convert 'fixp float' to u8 [0, +255] */ 561 /* u8 = X * 127.5 + 127.5; X is float [-1.0, +1.0] */ 562 fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275; 563 fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275; 564 *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10); 565 *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10); 566 break; 567 case V4L2_SDR_FMT_CS8: 568 /* convert 'fixp float' to s8 [-128, +127] */ 569 /* s8 = X * 127.5 - 0.5; X is float [-1.0, +1.0] */ 570 fixp_i = fixp_i * 1275 - FIXP_FRAC * 5; 571 fixp_q = fixp_q * 1275 - FIXP_FRAC * 5; 572 *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10); 573 *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10); 574 break; 575 default: 576 break; 577 } 578 } 579 } 580