drivers/media/platform/vivid/vivid-sdr-cap.c

   1 /*
   2  * vivid-sdr-cap.c - software defined radio support functions.
   3  *
   4  * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
   5  *
   6  * This program is free software; you may redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; version 2 of the License.
   9  *
  10  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  11  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  12  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  13  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
  14  * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
  15  * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  16  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  17  * SOFTWARE.
  18  */
  19
  20 #include <linux/errno.h>
  21 #include <linux/kernel.h>
  22 #include <linux/delay.h>
  23 #include <linux/kthread.h>
  24 #include <linux/freezer.h>
  25 #include <linux/videodev2.h>
  26 #include <linux/v4l2-dv-timings.h>
  27 #include <media/v4l2-common.h>
  28 #include <media/v4l2-event.h>
  29 #include <media/v4l2-dv-timings.h>
  30
  31 #include "vivid-core.h"
  32 #include "vivid-ctrls.h"
  33 #include "vivid-sdr-cap.h"
  34
  35 static const struct v4l2_frequency_band bands_adc[] = {
  36         {
  37                 .tuner = 0,
  38                 .type = V4L2_TUNER_ADC,
  39                 .index = 0,
  40                 .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
  41                 .rangelow   =  300000,
  42                 .rangehigh  =  300000,
  43         },
  44         {
  45                 .tuner = 0,
  46                 .type = V4L2_TUNER_ADC,
  47                 .index = 1,
  48                 .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
  49                 .rangelow   =  900001,
  50                 .rangehigh  = 2800000,
  51         },
  52         {
  53                 .tuner = 0,
  54                 .type = V4L2_TUNER_ADC,
  55                 .index = 2,
  56                 .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
  57                 .rangelow   = 3200000,
  58                 .rangehigh  = 3200000,
  59         },
  60 };
  61
  62 /* ADC band midpoints */
  63 #define BAND_ADC_0 ((bands_adc[0].rangehigh + bands_adc[1].rangelow) / 2)
  64 #define BAND_ADC_1 ((bands_adc[1].rangehigh + bands_adc[2].rangelow) / 2)
  65
  66 static const struct v4l2_frequency_band bands_fm[] = {
  67         {
  68                 .tuner = 1,
  69                 .type = V4L2_TUNER_RF,
  70                 .index = 0,
  71                 .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
  72                 .rangelow   =    50000000,
  73                 .rangehigh  =  2000000000,
  74         },
  75 };
  76
  77 static void vivid_thread_sdr_cap_tick(struct vivid_dev *dev)
  78 {
  79         struct vivid_buffer *sdr_cap_buf = NULL;
  80
  81         dprintk(dev, 1, "SDR Capture Thread Tick\n");
  82
  83         /* Drop a certain percentage of buffers. */
  84         if (dev->perc_dropped_buffers &&
  85             prandom_u32_max(100) < dev->perc_dropped_buffers)
  86                 return;
  87
  88         spin_lock(&dev->slock);
  89         if (!list_empty(&dev->sdr_cap_active)) {
  90                 sdr_cap_buf = list_entry(dev->sdr_cap_active.next,
  91                                          struct vivid_buffer, list);
  92                 list_del(&sdr_cap_buf->list);
  93         }
  94         spin_unlock(&dev->slock);
  95
  96         if (sdr_cap_buf) {
  97                 sdr_cap_buf->vb.v4l2_buf.sequence = dev->sdr_cap_seq_count;
  98                 vivid_sdr_cap_process(dev, sdr_cap_buf);
  99                 v4l2_get_timestamp(&sdr_cap_buf->vb.v4l2_buf.timestamp);
 100                 sdr_cap_buf->vb.v4l2_buf.timestamp.tv_sec += dev->time_wrap_offset;
 101                 vb2_buffer_done(&sdr_cap_buf->vb, dev->dqbuf_error ?
 102                                 VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 103                 dev->dqbuf_error = false;
 104         }
 105 }
 106
 107 static int vivid_thread_sdr_cap(void *data)
 108 {
 109         struct vivid_dev *dev = data;
 110         u64 samples_since_start;
 111         u64 buffers_since_start;
 112         u64 next_jiffies_since_start;
 113         unsigned long jiffies_since_start;
 114         unsigned long cur_jiffies;
 115         unsigned wait_jiffies;
 116
 117         dprintk(dev, 1, "SDR Capture Thread Start\n");
 118
 119         set_freezable();
 120
 121         /* Resets frame counters */
 122         dev->sdr_cap_seq_offset = 0;
 123         if (dev->seq_wrap)
 124                 dev->sdr_cap_seq_offset = 0xffffff80U;
 125         dev->jiffies_sdr_cap = jiffies;
 126         dev->sdr_cap_seq_resync = false;
 127
 128         for (;;) {
 129                 try_to_freeze();
 130                 if (kthread_should_stop())
 131                         break;
 132
 133                 mutex_lock(&dev->mutex);
 134                 cur_jiffies = jiffies;
 135                 if (dev->sdr_cap_seq_resync) {
 136                         dev->jiffies_sdr_cap = cur_jiffies;
 137                         dev->sdr_cap_seq_offset = dev->sdr_cap_seq_count + 1;
 138                         dev->sdr_cap_seq_count = 0;
 139                         dev->sdr_cap_seq_resync = false;
 140                 }
 141                 /* Calculate the number of jiffies since we started streaming */
 142                 jiffies_since_start = cur_jiffies - dev->jiffies_sdr_cap;
 143                 /* Get the number of buffers streamed since the start */
 144                 buffers_since_start = (u64)jiffies_since_start * dev->sdr_adc_freq +
 145                                       (HZ * SDR_CAP_SAMPLES_PER_BUF) / 2;
 146                 do_div(buffers_since_start, HZ * SDR_CAP_SAMPLES_PER_BUF);
 147
 148                 /*
 149                  * After more than 0xf0000000 (rounded down to a multiple of
 150                  * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
 151                  * jiffies have passed since we started streaming reset the
 152                  * counters and keep track of the sequence offset.
 153                  */
 154                 if (jiffies_since_start > JIFFIES_RESYNC) {
 155                         dev->jiffies_sdr_cap = cur_jiffies;
 156                         dev->sdr_cap_seq_offset = buffers_since_start;
 157                         buffers_since_start = 0;
 158                 }
 159                 dev->sdr_cap_seq_count = buffers_since_start + dev->sdr_cap_seq_offset;
 160
 161                 vivid_thread_sdr_cap_tick(dev);
 162                 mutex_unlock(&dev->mutex);
 163
 164                 /*
 165                  * Calculate the number of samples streamed since we started,
 166                  * not including the current buffer.
 167                  */
 168                 samples_since_start = buffers_since_start * SDR_CAP_SAMPLES_PER_BUF;
 169
 170                 /* And the number of jiffies since we started */
 171                 jiffies_since_start = jiffies - dev->jiffies_sdr_cap;
 172
 173                 /* Increase by the number of samples in one buffer */
 174                 samples_since_start += SDR_CAP_SAMPLES_PER_BUF;
 175                 /*
 176                  * Calculate when that next buffer is supposed to start
 177                  * in jiffies since we started streaming.
 178                  */
 179                 next_jiffies_since_start = samples_since_start * HZ +
 180                                            dev->sdr_adc_freq / 2;
 181                 do_div(next_jiffies_since_start, dev->sdr_adc_freq);
 182                 /* If it is in the past, then just schedule asap */
 183                 if (next_jiffies_since_start < jiffies_since_start)
 184                         next_jiffies_since_start = jiffies_since_start;
 185
 186                 wait_jiffies = next_jiffies_since_start - jiffies_since_start;
 187                 schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
 188         }
 189         dprintk(dev, 1, "SDR Capture Thread End\n");
 190         return 0;
 191 }
 192
 193 static int sdr_cap_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 194                        unsigned *nbuffers, unsigned *nplanes,
 195                        unsigned sizes[], void *alloc_ctxs[])
 196 {
 197         /* 2 = max 16-bit sample returned */
 198         sizes[0] = SDR_CAP_SAMPLES_PER_BUF * 2;
 199         *nplanes = 1;
 200         return 0;
 201 }
 202
 203 static int sdr_cap_buf_prepare(struct vb2_buffer *vb)
 204 {
 205         struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 206         unsigned size = SDR_CAP_SAMPLES_PER_BUF * 2;
 207
 208         dprintk(dev, 1, "%s\n", __func__);
 209
 210         if (dev->buf_prepare_error) {
 211                 /*
 212                  * Error injection: test what happens if buf_prepare() returns
 213                  * an error.
 214                  */
 215                 dev->buf_prepare_error = false;
 216                 return -EINVAL;
 217         }
 218         if (vb2_plane_size(vb, 0) < size) {
 219                 dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
 220                                 __func__, vb2_plane_size(vb, 0), size);
 221                 return -EINVAL;
 222         }
 223         vb2_set_plane_payload(vb, 0, size);
 224
 225         return 0;
 226 }
 227
 228 static void sdr_cap_buf_queue(struct vb2_buffer *vb)
 229 {
 230         struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 231         struct vivid_buffer *buf = container_of(vb, struct vivid_buffer, vb);
 232
 233         dprintk(dev, 1, "%s\n", __func__);
 234
 235         spin_lock(&dev->slock);
 236         list_add_tail(&buf->list, &dev->sdr_cap_active);
 237         spin_unlock(&dev->slock);
 238 }
 239
 240 static int sdr_cap_start_streaming(struct vb2_queue *vq, unsigned count)
 241 {
 242         struct vivid_dev *dev = vb2_get_drv_priv(vq);
 243         int err = 0;
 244
 245         dprintk(dev, 1, "%s\n", __func__);
 246         dev->sdr_cap_seq_count = 0;
 247         if (dev->start_streaming_error) {
 248                 dev->start_streaming_error = false;
 249                 err = -EINVAL;
 250         } else if (dev->kthread_sdr_cap == NULL) {
 251                 dev->kthread_sdr_cap = kthread_run(vivid_thread_sdr_cap, dev,
 252                                 "%s-sdr-cap", dev->v4l2_dev.name);
 253
 254                 if (IS_ERR(dev->kthread_sdr_cap)) {
 255                         v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
 256                         err = PTR_ERR(dev->kthread_sdr_cap);
 257                         dev->kthread_sdr_cap = NULL;
 258                 }
 259         }
 260         if (err) {
 261                 struct vivid_buffer *buf, *tmp;
 262
 263                 list_for_each_entry_safe(buf, tmp, &dev->sdr_cap_active, list) {
 264                         list_del(&buf->list);
 265                         vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
 266                 }
 267         }
 268         return err;
 269 }
 270
 271 /* abort streaming and wait for last buffer */
 272 static void sdr_cap_stop_streaming(struct vb2_queue *vq)
 273 {
 274         struct vivid_dev *dev = vb2_get_drv_priv(vq);
 275
 276         if (dev->kthread_sdr_cap == NULL)
 277                 return;
 278
 279         while (!list_empty(&dev->sdr_cap_active)) {
 280                 struct vivid_buffer *buf;
 281
 282                 buf = list_entry(dev->sdr_cap_active.next, struct vivid_buffer, list);
 283                 list_del(&buf->list);
 284                 vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 285         }
 286
 287         /* shutdown control thread */
 288         mutex_unlock(&dev->mutex);
 289         kthread_stop(dev->kthread_sdr_cap);
 290         dev->kthread_sdr_cap = NULL;
 291         mutex_lock(&dev->mutex);
 292 }
 293
 294 const struct vb2_ops vivid_sdr_cap_qops = {
 295         .queue_setup            = sdr_cap_queue_setup,
 296         .buf_prepare            = sdr_cap_buf_prepare,
 297         .buf_queue              = sdr_cap_buf_queue,
 298         .start_streaming        = sdr_cap_start_streaming,
 299         .stop_streaming         = sdr_cap_stop_streaming,
 300         .wait_prepare           = vivid_unlock,
 301         .wait_finish            = vivid_lock,
 302 };
 303
 304 int vivid_sdr_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band)
 305 {
 306         switch (band->tuner) {
 307         case 0:
 308                 if (band->index >= ARRAY_SIZE(bands_adc))
 309                         return -EINVAL;
 310                 *band = bands_adc[band->index];
 311                 return 0;
 312         case 1:
 313                 if (band->index >= ARRAY_SIZE(bands_fm))
 314                         return -EINVAL;
 315                 *band = bands_fm[band->index];
 316                 return 0;
 317         default:
 318                 return -EINVAL;
 319         }
 320 }
 321
 322 int vivid_sdr_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
 323 {
 324         struct vivid_dev *dev = video_drvdata(file);
 325
 326         switch (vf->tuner) {
 327         case 0:
 328                 vf->frequency = dev->sdr_adc_freq;
 329                 vf->type = V4L2_TUNER_ADC;
 330                 return 0;
 331         case 1:
 332                 vf->frequency = dev->sdr_fm_freq;
 333                 vf->type = V4L2_TUNER_RF;
 334                 return 0;
 335         default:
 336                 return -EINVAL;
 337         }
 338 }
 339
 340 int vivid_sdr_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
 341 {
 342         struct vivid_dev *dev = video_drvdata(file);
 343         unsigned freq = vf->frequency;
 344         unsigned band;
 345
 346         switch (vf->tuner) {
 347         case 0:
 348                 if (vf->type != V4L2_TUNER_ADC)
 349                         return -EINVAL;
 350                 if (freq < BAND_ADC_0)
 351                         band = 0;
 352                 else if (freq < BAND_ADC_1)
 353                         band = 1;
 354                 else
 355                         band = 2;
 356
 357                 freq = clamp_t(unsigned, freq,
 358                                 bands_adc[band].rangelow,
 359                                 bands_adc[band].rangehigh);
 360
 361                 if (vb2_is_streaming(&dev->vb_sdr_cap_q) &&
 362                     freq != dev->sdr_adc_freq) {
 363                         /* resync the thread's timings */
 364                         dev->sdr_cap_seq_resync = true;
 365                 }
 366                 dev->sdr_adc_freq = freq;
 367                 return 0;
 368         case 1:
 369                 if (vf->type != V4L2_TUNER_RF)
 370                         return -EINVAL;
 371                 dev->sdr_fm_freq = clamp_t(unsigned, freq,
 372                                 bands_fm[0].rangelow,
 373                                 bands_fm[0].rangehigh);
 374                 return 0;
 375         default:
 376                 return -EINVAL;
 377         }
 378 }
 379
 380 int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
 381 {
 382         switch (vt->index) {
 383         case 0:
 384                 strlcpy(vt->name, "ADC", sizeof(vt->name));
 385                 vt->type = V4L2_TUNER_ADC;
 386                 vt->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
 387                 vt->rangelow = bands_adc[0].rangelow;
 388                 vt->rangehigh = bands_adc[2].rangehigh;
 389                 return 0;
 390         case 1:
 391                 strlcpy(vt->name, "RF", sizeof(vt->name));
 392                 vt->type = V4L2_TUNER_RF;
 393                 vt->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
 394                 vt->rangelow = bands_fm[0].rangelow;
 395                 vt->rangehigh = bands_fm[0].rangehigh;
 396                 return 0;
 397         default:
 398                 return -EINVAL;
 399         }
 400 }
 401
 402 int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
 403 {
 404         if (vt->index > 1)
 405                 return -EINVAL;
 406         return 0;
 407 }
 408
 409 int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 410 {
 411         if (f->index)
 412                 return -EINVAL;
 413         f->pixelformat = V4L2_SDR_FMT_CU8;
 414         strlcpy(f->description, "IQ U8", sizeof(f->description));
 415         return 0;
 416 }
 417
 418 int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
 419 {
 420         f->fmt.sdr.pixelformat = V4L2_SDR_FMT_CU8;
 421         f->fmt.sdr.buffersize = SDR_CAP_SAMPLES_PER_BUF * 2;
 422         memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
 423         return 0;
 424 }
 425
 426 #define FIXP_FRAC    (1 << 15)
 427 #define FIXP_PI      ((int)(FIXP_FRAC * 3.141592653589))
 428
 429 /* cos() from cx88 driver: cx88-dsp.c */
 430 static s32 fixp_cos(unsigned int x)
 431 {
 432         u32 t2, t4, t6, t8;
 433         u16 period = x / FIXP_PI;
 434
 435         if (period % 2)
 436                 return -fixp_cos(x - FIXP_PI);
 437         x = x % FIXP_PI;
 438         if (x > FIXP_PI/2)
 439                 return -fixp_cos(FIXP_PI/2 - (x % (FIXP_PI/2)));
 440         /* Now x is between 0 and FIXP_PI/2.
 441          * To calculate cos(x) we use it's Taylor polinom. */
 442         t2 = x*x/FIXP_FRAC/2;
 443         t4 = t2*x/FIXP_FRAC*x/FIXP_FRAC/3/4;
 444         t6 = t4*x/FIXP_FRAC*x/FIXP_FRAC/5/6;
 445         t8 = t6*x/FIXP_FRAC*x/FIXP_FRAC/7/8;
 446         return FIXP_FRAC-t2+t4-t6+t8;
 447 }
 448
 449 static inline s32 fixp_sin(unsigned int x)
 450 {
 451         return -fixp_cos(x + (FIXP_PI / 2));
 452 }
 453
 454 void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
 455 {
 456         u8 *vbuf = vb2_plane_vaddr(&buf->vb, 0);
 457         unsigned long i;
 458         unsigned long plane_size = vb2_plane_size(&buf->vb, 0);
 459         int fixp_src_phase_step, fixp_i, fixp_q;
 460
 461         /*
 462          * TODO: Generated beep tone goes very crackly when sample rate is
 463          * increased to ~1Msps or more. That is because of huge rounding error
 464          * of phase angle caused by used cosine implementation.
 465          */
 466
 467         /* calculate phase step */
 468         #define BEEP_FREQ 1000 /* 1kHz beep */
 469         fixp_src_phase_step = DIV_ROUND_CLOSEST(2 * FIXP_PI * BEEP_FREQ,
 470                         dev->sdr_adc_freq);
 471
 472         for (i = 0; i < plane_size; i += 2) {
 473                 dev->sdr_fixp_mod_phase += fixp_cos(dev->sdr_fixp_src_phase);
 474                 dev->sdr_fixp_src_phase += fixp_src_phase_step;
 475
 476                 /*
 477                  * Transfer phases to [0 / 2xPI] in order to avoid variable
 478                  * overflow and make it suitable for cosine implementation
 479                  * used, which does not support negative angles.
 480                  */
 481                 while (dev->sdr_fixp_mod_phase < (0 * FIXP_PI))
 482                         dev->sdr_fixp_mod_phase += (2 * FIXP_PI);
 483                 while (dev->sdr_fixp_mod_phase > (2 * FIXP_PI))
 484                         dev->sdr_fixp_mod_phase -= (2 * FIXP_PI);
 485
 486                 while (dev->sdr_fixp_src_phase > (2 * FIXP_PI))
 487                         dev->sdr_fixp_src_phase -= (2 * FIXP_PI);
 488
 489                 fixp_i = fixp_cos(dev->sdr_fixp_mod_phase);
 490                 fixp_q = fixp_sin(dev->sdr_fixp_mod_phase);
 491
 492                 /* convert 'fixp float' to u8 */
 493                 /* u8 = X * 127.5f + 127.5f; where X is float [-1.0 / +1.0] */
 494                 fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275;
 495                 fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275;
 496                 *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
 497                 *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
 498         }
 499 }