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 if (!time_is_after_jiffies(cur_jiffies + wait_jiffies)) 210 continue; 211 212 wait_queue_head_t wait; 213 214 init_waitqueue_head(&wait); 215 wait_event_interruptible_timeout(wait, kthread_should_stop(), 216 cur_jiffies + wait_jiffies - jiffies); 217 } 218 dprintk(dev, 1, "SDR Capture Thread End\n"); 219 return 0; 220 } 221 222 static int sdr_cap_queue_setup(struct vb2_queue *vq, 223 unsigned *nbuffers, unsigned *nplanes, 224 unsigned sizes[], struct device *alloc_devs[]) 225 { 226 /* 2 = max 16-bit sample returned */ 227 u32 size = SDR_CAP_SAMPLES_PER_BUF * 2; 228 229 if (*nplanes) 230 return sizes[0] < size ? -EINVAL : 0; 231 232 *nplanes = 1; 233 sizes[0] = size; 234 return 0; 235 } 236 237 static int sdr_cap_buf_prepare(struct vb2_buffer *vb) 238 { 239 struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 240 unsigned size = SDR_CAP_SAMPLES_PER_BUF * 2; 241 242 dprintk(dev, 1, "%s\n", __func__); 243 244 if (dev->buf_prepare_error) { 245 /* 246 * Error injection: test what happens if buf_prepare() returns 247 * an error. 248 */ 249 dev->buf_prepare_error = false; 250 return -EINVAL; 251 } 252 if (vb2_plane_size(vb, 0) < size) { 253 dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n", 254 __func__, vb2_plane_size(vb, 0), size); 255 return -EINVAL; 256 } 257 vb2_set_plane_payload(vb, 0, size); 258 259 return 0; 260 } 261 262 static void sdr_cap_buf_queue(struct vb2_buffer *vb) 263 { 264 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 265 struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 266 struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb); 267 268 dprintk(dev, 1, "%s\n", __func__); 269 270 spin_lock(&dev->slock); 271 list_add_tail(&buf->list, &dev->sdr_cap_active); 272 spin_unlock(&dev->slock); 273 } 274 275 static int sdr_cap_start_streaming(struct vb2_queue *vq, unsigned count) 276 { 277 struct vivid_dev *dev = vb2_get_drv_priv(vq); 278 int err = 0; 279 280 dprintk(dev, 1, "%s\n", __func__); 281 dev->sdr_cap_seq_start = dev->seq_wrap * 128; 282 if (dev->start_streaming_error) { 283 dev->start_streaming_error = false; 284 err = -EINVAL; 285 } else if (dev->kthread_sdr_cap == NULL) { 286 dev->kthread_sdr_cap = kthread_run(vivid_thread_sdr_cap, dev, 287 "%s-sdr-cap", dev->v4l2_dev.name); 288 289 if (IS_ERR(dev->kthread_sdr_cap)) { 290 v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n"); 291 err = PTR_ERR(dev->kthread_sdr_cap); 292 dev->kthread_sdr_cap = NULL; 293 } 294 } 295 if (err) { 296 struct vivid_buffer *buf, *tmp; 297 298 list_for_each_entry_safe(buf, tmp, &dev->sdr_cap_active, list) { 299 list_del(&buf->list); 300 vb2_buffer_done(&buf->vb.vb2_buf, 301 VB2_BUF_STATE_QUEUED); 302 } 303 } 304 return err; 305 } 306 307 /* abort streaming and wait for last buffer */ 308 static void sdr_cap_stop_streaming(struct vb2_queue *vq) 309 { 310 struct vivid_dev *dev = vb2_get_drv_priv(vq); 311 312 if (dev->kthread_sdr_cap == NULL) 313 return; 314 315 while (!list_empty(&dev->sdr_cap_active)) { 316 struct vivid_buffer *buf; 317 318 buf = list_entry(dev->sdr_cap_active.next, 319 struct vivid_buffer, list); 320 list_del(&buf->list); 321 v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req, 322 &dev->ctrl_hdl_sdr_cap); 323 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 324 } 325 326 /* shutdown control thread */ 327 kthread_stop(dev->kthread_sdr_cap); 328 dev->kthread_sdr_cap = NULL; 329 } 330 331 static void sdr_cap_buf_request_complete(struct vb2_buffer *vb) 332 { 333 struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue); 334 335 v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_sdr_cap); 336 } 337 338 const struct vb2_ops vivid_sdr_cap_qops = { 339 .queue_setup = sdr_cap_queue_setup, 340 .buf_prepare = sdr_cap_buf_prepare, 341 .buf_queue = sdr_cap_buf_queue, 342 .start_streaming = sdr_cap_start_streaming, 343 .stop_streaming = sdr_cap_stop_streaming, 344 .buf_request_complete = sdr_cap_buf_request_complete, 345 }; 346 347 int vivid_sdr_enum_freq_bands(struct file *file, void *fh, 348 struct v4l2_frequency_band *band) 349 { 350 switch (band->tuner) { 351 case 0: 352 if (band->index >= ARRAY_SIZE(bands_adc)) 353 return -EINVAL; 354 *band = bands_adc[band->index]; 355 return 0; 356 case 1: 357 if (band->index >= ARRAY_SIZE(bands_fm)) 358 return -EINVAL; 359 *band = bands_fm[band->index]; 360 return 0; 361 default: 362 return -EINVAL; 363 } 364 } 365 366 int vivid_sdr_g_frequency(struct file *file, void *fh, 367 struct v4l2_frequency *vf) 368 { 369 struct vivid_dev *dev = video_drvdata(file); 370 371 switch (vf->tuner) { 372 case 0: 373 vf->frequency = dev->sdr_adc_freq; 374 vf->type = V4L2_TUNER_ADC; 375 return 0; 376 case 1: 377 vf->frequency = dev->sdr_fm_freq; 378 vf->type = V4L2_TUNER_RF; 379 return 0; 380 default: 381 return -EINVAL; 382 } 383 } 384 385 int vivid_sdr_s_frequency(struct file *file, void *fh, 386 const struct v4l2_frequency *vf) 387 { 388 struct vivid_dev *dev = video_drvdata(file); 389 unsigned freq = vf->frequency; 390 unsigned band; 391 392 switch (vf->tuner) { 393 case 0: 394 if (vf->type != V4L2_TUNER_ADC) 395 return -EINVAL; 396 if (freq < BAND_ADC_0) 397 band = 0; 398 else if (freq < BAND_ADC_1) 399 band = 1; 400 else 401 band = 2; 402 403 freq = clamp_t(unsigned, freq, 404 bands_adc[band].rangelow, 405 bands_adc[band].rangehigh); 406 407 if (vb2_is_streaming(&dev->vb_sdr_cap_q) && 408 freq != dev->sdr_adc_freq) { 409 /* resync the thread's timings */ 410 dev->sdr_cap_seq_resync = true; 411 } 412 dev->sdr_adc_freq = freq; 413 return 0; 414 case 1: 415 if (vf->type != V4L2_TUNER_RF) 416 return -EINVAL; 417 dev->sdr_fm_freq = clamp_t(unsigned, freq, 418 bands_fm[0].rangelow, 419 bands_fm[0].rangehigh); 420 return 0; 421 default: 422 return -EINVAL; 423 } 424 } 425 426 int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt) 427 { 428 switch (vt->index) { 429 case 0: 430 strscpy(vt->name, "ADC", sizeof(vt->name)); 431 vt->type = V4L2_TUNER_ADC; 432 vt->capability = 433 V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS; 434 vt->rangelow = bands_adc[0].rangelow; 435 vt->rangehigh = bands_adc[2].rangehigh; 436 return 0; 437 case 1: 438 strscpy(vt->name, "RF", sizeof(vt->name)); 439 vt->type = V4L2_TUNER_RF; 440 vt->capability = 441 V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS; 442 vt->rangelow = bands_fm[0].rangelow; 443 vt->rangehigh = bands_fm[0].rangehigh; 444 return 0; 445 default: 446 return -EINVAL; 447 } 448 } 449 450 int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt) 451 { 452 if (vt->index > 1) 453 return -EINVAL; 454 return 0; 455 } 456 457 int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f) 458 { 459 if (f->index >= ARRAY_SIZE(formats)) 460 return -EINVAL; 461 f->pixelformat = formats[f->index].pixelformat; 462 return 0; 463 } 464 465 int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 466 { 467 struct vivid_dev *dev = video_drvdata(file); 468 469 f->fmt.sdr.pixelformat = dev->sdr_pixelformat; 470 f->fmt.sdr.buffersize = dev->sdr_buffersize; 471 return 0; 472 } 473 474 int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 475 { 476 struct vivid_dev *dev = video_drvdata(file); 477 struct vb2_queue *q = &dev->vb_sdr_cap_q; 478 int i; 479 480 if (vb2_is_busy(q)) 481 return -EBUSY; 482 483 for (i = 0; i < ARRAY_SIZE(formats); i++) { 484 if (formats[i].pixelformat == f->fmt.sdr.pixelformat) { 485 dev->sdr_pixelformat = formats[i].pixelformat; 486 dev->sdr_buffersize = formats[i].buffersize; 487 f->fmt.sdr.buffersize = formats[i].buffersize; 488 return 0; 489 } 490 } 491 dev->sdr_pixelformat = formats[0].pixelformat; 492 dev->sdr_buffersize = formats[0].buffersize; 493 f->fmt.sdr.pixelformat = formats[0].pixelformat; 494 f->fmt.sdr.buffersize = formats[0].buffersize; 495 return 0; 496 } 497 498 int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 499 { 500 int i; 501 502 for (i = 0; i < ARRAY_SIZE(formats); i++) { 503 if (formats[i].pixelformat == f->fmt.sdr.pixelformat) { 504 f->fmt.sdr.buffersize = formats[i].buffersize; 505 return 0; 506 } 507 } 508 f->fmt.sdr.pixelformat = formats[0].pixelformat; 509 f->fmt.sdr.buffersize = formats[0].buffersize; 510 return 0; 511 } 512 513 #define FIXP_N (15) 514 #define FIXP_FRAC (1 << FIXP_N) 515 #define FIXP_2PI ((int)(2 * 3.141592653589 * FIXP_FRAC)) 516 #define M_100000PI (3.14159 * 100000) 517 518 void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf) 519 { 520 u8 *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0); 521 unsigned long i; 522 unsigned long plane_size = vb2_plane_size(&buf->vb.vb2_buf, 0); 523 s64 s64tmp; 524 s32 src_phase_step; 525 s32 mod_phase_step; 526 s32 fixp_i; 527 s32 fixp_q; 528 529 /* calculate phase step */ 530 #define BEEP_FREQ 1000 /* 1kHz beep */ 531 src_phase_step = DIV_ROUND_CLOSEST(FIXP_2PI * BEEP_FREQ, 532 dev->sdr_adc_freq); 533 534 for (i = 0; i < plane_size; i += 2) { 535 mod_phase_step = fixp_cos32_rad(dev->sdr_fixp_src_phase, 536 FIXP_2PI) >> (31 - FIXP_N); 537 538 dev->sdr_fixp_src_phase += src_phase_step; 539 s64tmp = (s64) mod_phase_step * dev->sdr_fm_deviation; 540 dev->sdr_fixp_mod_phase += div_s64(s64tmp, M_100000PI); 541 542 /* 543 * Transfer phase angle to [0, 2xPI] in order to avoid variable 544 * overflow and make it suitable for cosine implementation 545 * used, which does not support negative angles. 546 */ 547 dev->sdr_fixp_src_phase %= FIXP_2PI; 548 dev->sdr_fixp_mod_phase %= FIXP_2PI; 549 550 if (dev->sdr_fixp_mod_phase < 0) 551 dev->sdr_fixp_mod_phase += FIXP_2PI; 552 553 fixp_i = fixp_cos32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI); 554 fixp_q = fixp_sin32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI); 555 556 /* Normalize fraction values represented with 32 bit precision 557 * to fixed point representation with FIXP_N bits */ 558 fixp_i >>= (31 - FIXP_N); 559 fixp_q >>= (31 - FIXP_N); 560 561 switch (dev->sdr_pixelformat) { 562 case V4L2_SDR_FMT_CU8: 563 /* convert 'fixp float' to u8 [0, +255] */ 564 /* u8 = X * 127.5 + 127.5; X is float [-1.0, +1.0] */ 565 fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275; 566 fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275; 567 *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10); 568 *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10); 569 break; 570 case V4L2_SDR_FMT_CS8: 571 /* convert 'fixp float' to s8 [-128, +127] */ 572 /* s8 = X * 127.5 - 0.5; X is float [-1.0, +1.0] */ 573 fixp_i = fixp_i * 1275 - FIXP_FRAC * 5; 574 fixp_q = fixp_q * 1275 - FIXP_FRAC * 5; 575 *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10); 576 *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10); 577 break; 578 default: 579 break; 580 } 581 } 582 } 583