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 }; 341 342 int vivid_sdr_enum_freq_bands(struct file *file, void *fh, 343 struct v4l2_frequency_band *band) 344 { 345 switch (band->tuner) { 346 case 0: 347 if (band->index >= ARRAY_SIZE(bands_adc)) 348 return -EINVAL; 349 *band = bands_adc[band->index]; 350 return 0; 351 case 1: 352 if (band->index >= ARRAY_SIZE(bands_fm)) 353 return -EINVAL; 354 *band = bands_fm[band->index]; 355 return 0; 356 default: 357 return -EINVAL; 358 } 359 } 360 361 int vivid_sdr_g_frequency(struct file *file, void *fh, 362 struct v4l2_frequency *vf) 363 { 364 struct vivid_dev *dev = video_drvdata(file); 365 366 switch (vf->tuner) { 367 case 0: 368 vf->frequency = dev->sdr_adc_freq; 369 vf->type = V4L2_TUNER_ADC; 370 return 0; 371 case 1: 372 vf->frequency = dev->sdr_fm_freq; 373 vf->type = V4L2_TUNER_RF; 374 return 0; 375 default: 376 return -EINVAL; 377 } 378 } 379 380 int vivid_sdr_s_frequency(struct file *file, void *fh, 381 const struct v4l2_frequency *vf) 382 { 383 struct vivid_dev *dev = video_drvdata(file); 384 unsigned freq = vf->frequency; 385 unsigned band; 386 387 switch (vf->tuner) { 388 case 0: 389 if (vf->type != V4L2_TUNER_ADC) 390 return -EINVAL; 391 if (freq < BAND_ADC_0) 392 band = 0; 393 else if (freq < BAND_ADC_1) 394 band = 1; 395 else 396 band = 2; 397 398 freq = clamp_t(unsigned, freq, 399 bands_adc[band].rangelow, 400 bands_adc[band].rangehigh); 401 402 if (vb2_is_streaming(&dev->vb_sdr_cap_q) && 403 freq != dev->sdr_adc_freq) { 404 /* resync the thread's timings */ 405 dev->sdr_cap_seq_resync = true; 406 } 407 dev->sdr_adc_freq = freq; 408 return 0; 409 case 1: 410 if (vf->type != V4L2_TUNER_RF) 411 return -EINVAL; 412 dev->sdr_fm_freq = clamp_t(unsigned, freq, 413 bands_fm[0].rangelow, 414 bands_fm[0].rangehigh); 415 return 0; 416 default: 417 return -EINVAL; 418 } 419 } 420 421 int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt) 422 { 423 switch (vt->index) { 424 case 0: 425 strscpy(vt->name, "ADC", sizeof(vt->name)); 426 vt->type = V4L2_TUNER_ADC; 427 vt->capability = 428 V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS; 429 vt->rangelow = bands_adc[0].rangelow; 430 vt->rangehigh = bands_adc[2].rangehigh; 431 return 0; 432 case 1: 433 strscpy(vt->name, "RF", sizeof(vt->name)); 434 vt->type = V4L2_TUNER_RF; 435 vt->capability = 436 V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS; 437 vt->rangelow = bands_fm[0].rangelow; 438 vt->rangehigh = bands_fm[0].rangehigh; 439 return 0; 440 default: 441 return -EINVAL; 442 } 443 } 444 445 int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt) 446 { 447 if (vt->index > 1) 448 return -EINVAL; 449 return 0; 450 } 451 452 int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f) 453 { 454 if (f->index >= ARRAY_SIZE(formats)) 455 return -EINVAL; 456 f->pixelformat = formats[f->index].pixelformat; 457 return 0; 458 } 459 460 int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 461 { 462 struct vivid_dev *dev = video_drvdata(file); 463 464 f->fmt.sdr.pixelformat = dev->sdr_pixelformat; 465 f->fmt.sdr.buffersize = dev->sdr_buffersize; 466 return 0; 467 } 468 469 int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 470 { 471 struct vivid_dev *dev = video_drvdata(file); 472 struct vb2_queue *q = &dev->vb_sdr_cap_q; 473 int i; 474 475 if (vb2_is_busy(q)) 476 return -EBUSY; 477 478 for (i = 0; i < ARRAY_SIZE(formats); i++) { 479 if (formats[i].pixelformat == f->fmt.sdr.pixelformat) { 480 dev->sdr_pixelformat = formats[i].pixelformat; 481 dev->sdr_buffersize = formats[i].buffersize; 482 f->fmt.sdr.buffersize = formats[i].buffersize; 483 return 0; 484 } 485 } 486 dev->sdr_pixelformat = formats[0].pixelformat; 487 dev->sdr_buffersize = formats[0].buffersize; 488 f->fmt.sdr.pixelformat = formats[0].pixelformat; 489 f->fmt.sdr.buffersize = formats[0].buffersize; 490 return 0; 491 } 492 493 int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) 494 { 495 int i; 496 497 for (i = 0; i < ARRAY_SIZE(formats); i++) { 498 if (formats[i].pixelformat == f->fmt.sdr.pixelformat) { 499 f->fmt.sdr.buffersize = formats[i].buffersize; 500 return 0; 501 } 502 } 503 f->fmt.sdr.pixelformat = formats[0].pixelformat; 504 f->fmt.sdr.buffersize = formats[0].buffersize; 505 return 0; 506 } 507 508 #define FIXP_N (15) 509 #define FIXP_FRAC (1 << FIXP_N) 510 #define FIXP_2PI ((int)(2 * 3.141592653589 * FIXP_FRAC)) 511 #define M_100000PI (3.14159 * 100000) 512 513 void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf) 514 { 515 u8 *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0); 516 unsigned long i; 517 unsigned long plane_size = vb2_plane_size(&buf->vb.vb2_buf, 0); 518 s64 s64tmp; 519 s32 src_phase_step; 520 s32 mod_phase_step; 521 s32 fixp_i; 522 s32 fixp_q; 523 524 /* calculate phase step */ 525 #define BEEP_FREQ 1000 /* 1kHz beep */ 526 src_phase_step = DIV_ROUND_CLOSEST(FIXP_2PI * BEEP_FREQ, 527 dev->sdr_adc_freq); 528 529 for (i = 0; i < plane_size; i += 2) { 530 mod_phase_step = fixp_cos32_rad(dev->sdr_fixp_src_phase, 531 FIXP_2PI) >> (31 - FIXP_N); 532 533 dev->sdr_fixp_src_phase += src_phase_step; 534 s64tmp = (s64) mod_phase_step * dev->sdr_fm_deviation; 535 dev->sdr_fixp_mod_phase += div_s64(s64tmp, M_100000PI); 536 537 /* 538 * Transfer phase angle to [0, 2xPI] in order to avoid variable 539 * overflow and make it suitable for cosine implementation 540 * used, which does not support negative angles. 541 */ 542 dev->sdr_fixp_src_phase %= FIXP_2PI; 543 dev->sdr_fixp_mod_phase %= FIXP_2PI; 544 545 if (dev->sdr_fixp_mod_phase < 0) 546 dev->sdr_fixp_mod_phase += FIXP_2PI; 547 548 fixp_i = fixp_cos32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI); 549 fixp_q = fixp_sin32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI); 550 551 /* Normalize fraction values represented with 32 bit precision 552 * to fixed point representation with FIXP_N bits */ 553 fixp_i >>= (31 - FIXP_N); 554 fixp_q >>= (31 - FIXP_N); 555 556 switch (dev->sdr_pixelformat) { 557 case V4L2_SDR_FMT_CU8: 558 /* convert 'fixp float' to u8 [0, +255] */ 559 /* u8 = X * 127.5 + 127.5; X is float [-1.0, +1.0] */ 560 fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275; 561 fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275; 562 *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10); 563 *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10); 564 break; 565 case V4L2_SDR_FMT_CS8: 566 /* convert 'fixp float' to s8 [-128, +127] */ 567 /* s8 = X * 127.5 - 0.5; X is float [-1.0, +1.0] */ 568 fixp_i = fixp_i * 1275 - FIXP_FRAC * 5; 569 fixp_q = fixp_q * 1275 - FIXP_FRAC * 5; 570 *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10); 571 *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10); 572 break; 573 default: 574 break; 575 } 576 } 577 } 578