2 * vivid-sdr-cap.c - software defined radio support functions.
4 * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
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.
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
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/math64.h>
26 #include <linux/videodev2.h>
27 #include <linux/v4l2-dv-timings.h>
28 #include <media/v4l2-common.h>
29 #include <media/v4l2-event.h>
30 #include <media/v4l2-dv-timings.h>
31 #include <linux/fixp-arith.h>
33 #include "vivid-core.h"
34 #include "vivid-ctrls.h"
35 #include "vivid-sdr-cap.h"
43 /* format descriptions for capture and preview */
44 static struct vivid_format formats
[] = {
46 .pixelformat
= V4L2_SDR_FMT_CU8
,
47 .buffersize
= SDR_CAP_SAMPLES_PER_BUF
* 2,
49 .pixelformat
= V4L2_SDR_FMT_CS8
,
50 .buffersize
= SDR_CAP_SAMPLES_PER_BUF
* 2,
54 static const unsigned int NUM_FORMATS
= ARRAY_SIZE(formats
);
56 static const struct v4l2_frequency_band bands_adc
[] = {
59 .type
= V4L2_TUNER_ADC
,
61 .capability
= V4L2_TUNER_CAP_1HZ
| V4L2_TUNER_CAP_FREQ_BANDS
,
67 .type
= V4L2_TUNER_ADC
,
69 .capability
= V4L2_TUNER_CAP_1HZ
| V4L2_TUNER_CAP_FREQ_BANDS
,
75 .type
= V4L2_TUNER_ADC
,
77 .capability
= V4L2_TUNER_CAP_1HZ
| V4L2_TUNER_CAP_FREQ_BANDS
,
83 /* ADC band midpoints */
84 #define BAND_ADC_0 ((bands_adc[0].rangehigh + bands_adc[1].rangelow) / 2)
85 #define BAND_ADC_1 ((bands_adc[1].rangehigh + bands_adc[2].rangelow) / 2)
87 static const struct v4l2_frequency_band bands_fm
[] = {
90 .type
= V4L2_TUNER_RF
,
92 .capability
= V4L2_TUNER_CAP_1HZ
| V4L2_TUNER_CAP_FREQ_BANDS
,
94 .rangehigh
= 2000000000,
98 static void vivid_thread_sdr_cap_tick(struct vivid_dev
*dev
)
100 struct vivid_buffer
*sdr_cap_buf
= NULL
;
102 dprintk(dev
, 1, "SDR Capture Thread Tick\n");
104 /* Drop a certain percentage of buffers. */
105 if (dev
->perc_dropped_buffers
&&
106 prandom_u32_max(100) < dev
->perc_dropped_buffers
)
109 spin_lock(&dev
->slock
);
110 if (!list_empty(&dev
->sdr_cap_active
)) {
111 sdr_cap_buf
= list_entry(dev
->sdr_cap_active
.next
,
112 struct vivid_buffer
, list
);
113 list_del(&sdr_cap_buf
->list
);
115 spin_unlock(&dev
->slock
);
118 sdr_cap_buf
->vb
.v4l2_buf
.sequence
= dev
->sdr_cap_seq_count
;
119 vivid_sdr_cap_process(dev
, sdr_cap_buf
);
120 v4l2_get_timestamp(&sdr_cap_buf
->vb
.v4l2_buf
.timestamp
);
121 sdr_cap_buf
->vb
.v4l2_buf
.timestamp
.tv_sec
+= dev
->time_wrap_offset
;
122 vb2_buffer_done(&sdr_cap_buf
->vb
, dev
->dqbuf_error
?
123 VB2_BUF_STATE_ERROR
: VB2_BUF_STATE_DONE
);
124 dev
->dqbuf_error
= false;
128 static int vivid_thread_sdr_cap(void *data
)
130 struct vivid_dev
*dev
= data
;
131 u64 samples_since_start
;
132 u64 buffers_since_start
;
133 u64 next_jiffies_since_start
;
134 unsigned long jiffies_since_start
;
135 unsigned long cur_jiffies
;
136 unsigned wait_jiffies
;
138 dprintk(dev
, 1, "SDR Capture Thread Start\n");
142 /* Resets frame counters */
143 dev
->sdr_cap_seq_offset
= 0;
145 dev
->sdr_cap_seq_offset
= 0xffffff80U
;
146 dev
->jiffies_sdr_cap
= jiffies
;
147 dev
->sdr_cap_seq_resync
= false;
151 if (kthread_should_stop())
154 mutex_lock(&dev
->mutex
);
155 cur_jiffies
= jiffies
;
156 if (dev
->sdr_cap_seq_resync
) {
157 dev
->jiffies_sdr_cap
= cur_jiffies
;
158 dev
->sdr_cap_seq_offset
= dev
->sdr_cap_seq_count
+ 1;
159 dev
->sdr_cap_seq_count
= 0;
160 dev
->sdr_cap_seq_resync
= false;
162 /* Calculate the number of jiffies since we started streaming */
163 jiffies_since_start
= cur_jiffies
- dev
->jiffies_sdr_cap
;
164 /* Get the number of buffers streamed since the start */
165 buffers_since_start
= (u64
)jiffies_since_start
* dev
->sdr_adc_freq
+
166 (HZ
* SDR_CAP_SAMPLES_PER_BUF
) / 2;
167 do_div(buffers_since_start
, HZ
* SDR_CAP_SAMPLES_PER_BUF
);
170 * After more than 0xf0000000 (rounded down to a multiple of
171 * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
172 * jiffies have passed since we started streaming reset the
173 * counters and keep track of the sequence offset.
175 if (jiffies_since_start
> JIFFIES_RESYNC
) {
176 dev
->jiffies_sdr_cap
= cur_jiffies
;
177 dev
->sdr_cap_seq_offset
= buffers_since_start
;
178 buffers_since_start
= 0;
180 dev
->sdr_cap_seq_count
= buffers_since_start
+ dev
->sdr_cap_seq_offset
;
182 vivid_thread_sdr_cap_tick(dev
);
183 mutex_unlock(&dev
->mutex
);
186 * Calculate the number of samples streamed since we started,
187 * not including the current buffer.
189 samples_since_start
= buffers_since_start
* SDR_CAP_SAMPLES_PER_BUF
;
191 /* And the number of jiffies since we started */
192 jiffies_since_start
= jiffies
- dev
->jiffies_sdr_cap
;
194 /* Increase by the number of samples in one buffer */
195 samples_since_start
+= SDR_CAP_SAMPLES_PER_BUF
;
197 * Calculate when that next buffer is supposed to start
198 * in jiffies since we started streaming.
200 next_jiffies_since_start
= samples_since_start
* HZ
+
201 dev
->sdr_adc_freq
/ 2;
202 do_div(next_jiffies_since_start
, dev
->sdr_adc_freq
);
203 /* If it is in the past, then just schedule asap */
204 if (next_jiffies_since_start
< jiffies_since_start
)
205 next_jiffies_since_start
= jiffies_since_start
;
207 wait_jiffies
= next_jiffies_since_start
- jiffies_since_start
;
208 schedule_timeout_interruptible(wait_jiffies
? wait_jiffies
: 1);
210 dprintk(dev
, 1, "SDR Capture Thread End\n");
214 static int sdr_cap_queue_setup(struct vb2_queue
*vq
, const struct v4l2_format
*fmt
,
215 unsigned *nbuffers
, unsigned *nplanes
,
216 unsigned sizes
[], void *alloc_ctxs
[])
218 /* 2 = max 16-bit sample returned */
219 sizes
[0] = SDR_CAP_SAMPLES_PER_BUF
* 2;
224 static int sdr_cap_buf_prepare(struct vb2_buffer
*vb
)
226 struct vivid_dev
*dev
= vb2_get_drv_priv(vb
->vb2_queue
);
227 unsigned size
= SDR_CAP_SAMPLES_PER_BUF
* 2;
229 dprintk(dev
, 1, "%s\n", __func__
);
231 if (dev
->buf_prepare_error
) {
233 * Error injection: test what happens if buf_prepare() returns
236 dev
->buf_prepare_error
= false;
239 if (vb2_plane_size(vb
, 0) < size
) {
240 dprintk(dev
, 1, "%s data will not fit into plane (%lu < %u)\n",
241 __func__
, vb2_plane_size(vb
, 0), size
);
244 vb2_set_plane_payload(vb
, 0, size
);
249 static void sdr_cap_buf_queue(struct vb2_buffer
*vb
)
251 struct vivid_dev
*dev
= vb2_get_drv_priv(vb
->vb2_queue
);
252 struct vivid_buffer
*buf
= container_of(vb
, struct vivid_buffer
, vb
);
254 dprintk(dev
, 1, "%s\n", __func__
);
256 spin_lock(&dev
->slock
);
257 list_add_tail(&buf
->list
, &dev
->sdr_cap_active
);
258 spin_unlock(&dev
->slock
);
261 static int sdr_cap_start_streaming(struct vb2_queue
*vq
, unsigned count
)
263 struct vivid_dev
*dev
= vb2_get_drv_priv(vq
);
266 dprintk(dev
, 1, "%s\n", __func__
);
267 dev
->sdr_cap_seq_count
= 0;
268 if (dev
->start_streaming_error
) {
269 dev
->start_streaming_error
= false;
271 } else if (dev
->kthread_sdr_cap
== NULL
) {
272 dev
->kthread_sdr_cap
= kthread_run(vivid_thread_sdr_cap
, dev
,
273 "%s-sdr-cap", dev
->v4l2_dev
.name
);
275 if (IS_ERR(dev
->kthread_sdr_cap
)) {
276 v4l2_err(&dev
->v4l2_dev
, "kernel_thread() failed\n");
277 err
= PTR_ERR(dev
->kthread_sdr_cap
);
278 dev
->kthread_sdr_cap
= NULL
;
282 struct vivid_buffer
*buf
, *tmp
;
284 list_for_each_entry_safe(buf
, tmp
, &dev
->sdr_cap_active
, list
) {
285 list_del(&buf
->list
);
286 vb2_buffer_done(&buf
->vb
, VB2_BUF_STATE_QUEUED
);
292 /* abort streaming and wait for last buffer */
293 static void sdr_cap_stop_streaming(struct vb2_queue
*vq
)
295 struct vivid_dev
*dev
= vb2_get_drv_priv(vq
);
297 if (dev
->kthread_sdr_cap
== NULL
)
300 while (!list_empty(&dev
->sdr_cap_active
)) {
301 struct vivid_buffer
*buf
;
303 buf
= list_entry(dev
->sdr_cap_active
.next
, struct vivid_buffer
, list
);
304 list_del(&buf
->list
);
305 vb2_buffer_done(&buf
->vb
, VB2_BUF_STATE_ERROR
);
308 /* shutdown control thread */
309 mutex_unlock(&dev
->mutex
);
310 kthread_stop(dev
->kthread_sdr_cap
);
311 dev
->kthread_sdr_cap
= NULL
;
312 mutex_lock(&dev
->mutex
);
315 const struct vb2_ops vivid_sdr_cap_qops
= {
316 .queue_setup
= sdr_cap_queue_setup
,
317 .buf_prepare
= sdr_cap_buf_prepare
,
318 .buf_queue
= sdr_cap_buf_queue
,
319 .start_streaming
= sdr_cap_start_streaming
,
320 .stop_streaming
= sdr_cap_stop_streaming
,
321 .wait_prepare
= vb2_ops_wait_prepare
,
322 .wait_finish
= vb2_ops_wait_finish
,
325 int vivid_sdr_enum_freq_bands(struct file
*file
, void *fh
, struct v4l2_frequency_band
*band
)
327 switch (band
->tuner
) {
329 if (band
->index
>= ARRAY_SIZE(bands_adc
))
331 *band
= bands_adc
[band
->index
];
334 if (band
->index
>= ARRAY_SIZE(bands_fm
))
336 *band
= bands_fm
[band
->index
];
343 int vivid_sdr_g_frequency(struct file
*file
, void *fh
, struct v4l2_frequency
*vf
)
345 struct vivid_dev
*dev
= video_drvdata(file
);
349 vf
->frequency
= dev
->sdr_adc_freq
;
350 vf
->type
= V4L2_TUNER_ADC
;
353 vf
->frequency
= dev
->sdr_fm_freq
;
354 vf
->type
= V4L2_TUNER_RF
;
361 int vivid_sdr_s_frequency(struct file
*file
, void *fh
, const struct v4l2_frequency
*vf
)
363 struct vivid_dev
*dev
= video_drvdata(file
);
364 unsigned freq
= vf
->frequency
;
369 if (vf
->type
!= V4L2_TUNER_ADC
)
371 if (freq
< BAND_ADC_0
)
373 else if (freq
< BAND_ADC_1
)
378 freq
= clamp_t(unsigned, freq
,
379 bands_adc
[band
].rangelow
,
380 bands_adc
[band
].rangehigh
);
382 if (vb2_is_streaming(&dev
->vb_sdr_cap_q
) &&
383 freq
!= dev
->sdr_adc_freq
) {
384 /* resync the thread's timings */
385 dev
->sdr_cap_seq_resync
= true;
387 dev
->sdr_adc_freq
= freq
;
390 if (vf
->type
!= V4L2_TUNER_RF
)
392 dev
->sdr_fm_freq
= clamp_t(unsigned, freq
,
393 bands_fm
[0].rangelow
,
394 bands_fm
[0].rangehigh
);
401 int vivid_sdr_g_tuner(struct file
*file
, void *fh
, struct v4l2_tuner
*vt
)
405 strlcpy(vt
->name
, "ADC", sizeof(vt
->name
));
406 vt
->type
= V4L2_TUNER_ADC
;
407 vt
->capability
= V4L2_TUNER_CAP_1HZ
| V4L2_TUNER_CAP_FREQ_BANDS
;
408 vt
->rangelow
= bands_adc
[0].rangelow
;
409 vt
->rangehigh
= bands_adc
[2].rangehigh
;
412 strlcpy(vt
->name
, "RF", sizeof(vt
->name
));
413 vt
->type
= V4L2_TUNER_RF
;
414 vt
->capability
= V4L2_TUNER_CAP_1HZ
| V4L2_TUNER_CAP_FREQ_BANDS
;
415 vt
->rangelow
= bands_fm
[0].rangelow
;
416 vt
->rangehigh
= bands_fm
[0].rangehigh
;
423 int vivid_sdr_s_tuner(struct file
*file
, void *fh
, const struct v4l2_tuner
*vt
)
430 int vidioc_enum_fmt_sdr_cap(struct file
*file
, void *fh
, struct v4l2_fmtdesc
*f
)
432 if (f
->index
>= ARRAY_SIZE(formats
))
434 f
->pixelformat
= formats
[f
->index
].pixelformat
;
438 int vidioc_g_fmt_sdr_cap(struct file
*file
, void *fh
, struct v4l2_format
*f
)
440 struct vivid_dev
*dev
= video_drvdata(file
);
442 f
->fmt
.sdr
.pixelformat
= dev
->sdr_pixelformat
;
443 f
->fmt
.sdr
.buffersize
= dev
->sdr_buffersize
;
444 memset(f
->fmt
.sdr
.reserved
, 0, sizeof(f
->fmt
.sdr
.reserved
));
448 int vidioc_s_fmt_sdr_cap(struct file
*file
, void *fh
, struct v4l2_format
*f
)
450 struct vivid_dev
*dev
= video_drvdata(file
);
451 struct vb2_queue
*q
= &dev
->vb_sdr_cap_q
;
457 memset(f
->fmt
.sdr
.reserved
, 0, sizeof(f
->fmt
.sdr
.reserved
));
458 for (i
= 0; i
< ARRAY_SIZE(formats
); i
++) {
459 if (formats
[i
].pixelformat
== f
->fmt
.sdr
.pixelformat
) {
460 dev
->sdr_pixelformat
= formats
[i
].pixelformat
;
461 dev
->sdr_buffersize
= formats
[i
].buffersize
;
462 f
->fmt
.sdr
.buffersize
= formats
[i
].buffersize
;
466 dev
->sdr_pixelformat
= formats
[0].pixelformat
;
467 dev
->sdr_buffersize
= formats
[0].buffersize
;
468 f
->fmt
.sdr
.pixelformat
= formats
[0].pixelformat
;
469 f
->fmt
.sdr
.buffersize
= formats
[0].buffersize
;
473 int vidioc_try_fmt_sdr_cap(struct file
*file
, void *fh
, struct v4l2_format
*f
)
477 memset(f
->fmt
.sdr
.reserved
, 0, sizeof(f
->fmt
.sdr
.reserved
));
478 for (i
= 0; i
< ARRAY_SIZE(formats
); i
++) {
479 if (formats
[i
].pixelformat
== f
->fmt
.sdr
.pixelformat
) {
480 f
->fmt
.sdr
.buffersize
= formats
[i
].buffersize
;
484 f
->fmt
.sdr
.pixelformat
= formats
[0].pixelformat
;
485 f
->fmt
.sdr
.buffersize
= formats
[0].buffersize
;
490 #define FIXP_FRAC (1 << FIXP_N)
491 #define FIXP_2PI ((int)(2 * 3.141592653589 * FIXP_FRAC))
492 #define M_100000PI (3.14159 * 100000)
494 void vivid_sdr_cap_process(struct vivid_dev
*dev
, struct vivid_buffer
*buf
)
496 u8
*vbuf
= vb2_plane_vaddr(&buf
->vb
, 0);
498 unsigned long plane_size
= vb2_plane_size(&buf
->vb
, 0);
506 * TODO: Generated beep tone goes very crackly when sample rate is
507 * increased to ~1Msps or more. That is because of huge rounding error
508 * of phase angle caused by used cosine implementation.
511 /* calculate phase step */
512 #define BEEP_FREQ 1000 /* 1kHz beep */
513 src_phase_step
= DIV_ROUND_CLOSEST(FIXP_2PI
* BEEP_FREQ
,
516 for (i
= 0; i
< plane_size
; i
+= 2) {
517 mod_phase_step
= fixp_cos32_rad(dev
->sdr_fixp_src_phase
,
518 FIXP_2PI
) >> (31 - FIXP_N
);
520 dev
->sdr_fixp_src_phase
+= src_phase_step
;
521 s64tmp
= (s64
) mod_phase_step
* dev
->sdr_fm_deviation
;
522 dev
->sdr_fixp_mod_phase
+= div_s64(s64tmp
, M_100000PI
);
525 * Transfer phases to [0 / 2xPI] in order to avoid variable
526 * overflow and make it suitable for cosine implementation
527 * used, which does not support negative angles.
529 while (dev
->sdr_fixp_mod_phase
< FIXP_2PI
)
530 dev
->sdr_fixp_mod_phase
+= FIXP_2PI
;
531 while (dev
->sdr_fixp_mod_phase
> FIXP_2PI
)
532 dev
->sdr_fixp_mod_phase
-= FIXP_2PI
;
534 while (dev
->sdr_fixp_src_phase
> FIXP_2PI
)
535 dev
->sdr_fixp_src_phase
-= FIXP_2PI
;
537 fixp_i
= fixp_cos32_rad(dev
->sdr_fixp_mod_phase
, FIXP_2PI
);
538 fixp_q
= fixp_sin32_rad(dev
->sdr_fixp_mod_phase
, FIXP_2PI
);
540 /* Normalize fraction values represented with 32 bit precision
541 * to fixed point representation with FIXP_N bits */
542 fixp_i
>>= (31 - FIXP_N
);
543 fixp_q
>>= (31 - FIXP_N
);
545 switch (dev
->sdr_pixelformat
) {
546 case V4L2_SDR_FMT_CU8
:
547 /* convert 'fixp float' to u8 */
548 /* u8 = X * 127.5 + 127.5; X is float [-1.0, +1.0] */
549 fixp_i
= fixp_i
* 1275 + FIXP_FRAC
* 1275;
550 fixp_q
= fixp_q
* 1275 + FIXP_FRAC
* 1275;
551 *vbuf
++ = DIV_ROUND_CLOSEST(fixp_i
, FIXP_FRAC
* 10);
552 *vbuf
++ = DIV_ROUND_CLOSEST(fixp_q
, FIXP_FRAC
* 10);
554 case V4L2_SDR_FMT_CS8
:
555 /* convert 'fixp float' to s8 */
556 fixp_i
= fixp_i
* 1275;
557 fixp_q
= fixp_q
* 1275;
558 *vbuf
++ = DIV_ROUND_CLOSEST(fixp_i
, FIXP_FRAC
* 10);
559 *vbuf
++ = DIV_ROUND_CLOSEST(fixp_q
, FIXP_FRAC
* 10);