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