Commit | Line | Data |
---|---|---|
6de8653f HV |
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> | |
f335c3f2 | 25 | #include <linux/math64.h> |
6de8653f HV |
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> | |
4e30a373 | 31 | #include <linux/fixp-arith.h> |
6de8653f HV |
32 | |
33 | #include "vivid-core.h" | |
34 | #include "vivid-ctrls.h" | |
35 | #include "vivid-sdr-cap.h" | |
36 | ||
7615f4bc AP |
37 | /* stream formats */ |
38 | struct vivid_format { | |
39 | u32 pixelformat; | |
40 | u32 buffersize; | |
41 | }; | |
42 | ||
43 | /* format descriptions for capture and preview */ | |
60f68735 | 44 | static const struct vivid_format formats[] = { |
7615f4bc AP |
45 | { |
46 | .pixelformat = V4L2_SDR_FMT_CU8, | |
47 | .buffersize = SDR_CAP_SAMPLES_PER_BUF * 2, | |
48 | }, { | |
49 | .pixelformat = V4L2_SDR_FMT_CS8, | |
50 | .buffersize = SDR_CAP_SAMPLES_PER_BUF * 2, | |
51 | }, | |
52 | }; | |
53 | ||
6de8653f HV |
54 | static const struct v4l2_frequency_band bands_adc[] = { |
55 | { | |
56 | .tuner = 0, | |
57 | .type = V4L2_TUNER_ADC, | |
58 | .index = 0, | |
59 | .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS, | |
60 | .rangelow = 300000, | |
61 | .rangehigh = 300000, | |
62 | }, | |
63 | { | |
64 | .tuner = 0, | |
65 | .type = V4L2_TUNER_ADC, | |
66 | .index = 1, | |
67 | .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS, | |
68 | .rangelow = 900001, | |
69 | .rangehigh = 2800000, | |
70 | }, | |
71 | { | |
72 | .tuner = 0, | |
73 | .type = V4L2_TUNER_ADC, | |
74 | .index = 2, | |
75 | .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS, | |
76 | .rangelow = 3200000, | |
77 | .rangehigh = 3200000, | |
78 | }, | |
79 | }; | |
80 | ||
81 | /* ADC band midpoints */ | |
82 | #define BAND_ADC_0 ((bands_adc[0].rangehigh + bands_adc[1].rangelow) / 2) | |
83 | #define BAND_ADC_1 ((bands_adc[1].rangehigh + bands_adc[2].rangelow) / 2) | |
84 | ||
85 | static const struct v4l2_frequency_band bands_fm[] = { | |
86 | { | |
87 | .tuner = 1, | |
88 | .type = V4L2_TUNER_RF, | |
89 | .index = 0, | |
90 | .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS, | |
91 | .rangelow = 50000000, | |
92 | .rangehigh = 2000000000, | |
93 | }, | |
94 | }; | |
95 | ||
96 | static void vivid_thread_sdr_cap_tick(struct vivid_dev *dev) | |
97 | { | |
98 | struct vivid_buffer *sdr_cap_buf = NULL; | |
99 | ||
100 | dprintk(dev, 1, "SDR Capture Thread Tick\n"); | |
101 | ||
102 | /* Drop a certain percentage of buffers. */ | |
103 | if (dev->perc_dropped_buffers && | |
104 | prandom_u32_max(100) < dev->perc_dropped_buffers) | |
105 | return; | |
106 | ||
107 | spin_lock(&dev->slock); | |
108 | if (!list_empty(&dev->sdr_cap_active)) { | |
109 | sdr_cap_buf = list_entry(dev->sdr_cap_active.next, | |
110 | struct vivid_buffer, list); | |
111 | list_del(&sdr_cap_buf->list); | |
112 | } | |
113 | spin_unlock(&dev->slock); | |
114 | ||
115 | if (sdr_cap_buf) { | |
2d700715 | 116 | sdr_cap_buf->vb.sequence = dev->sdr_cap_seq_count; |
6de8653f | 117 | vivid_sdr_cap_process(dev, sdr_cap_buf); |
d6dd645e JS |
118 | sdr_cap_buf->vb.vb2_buf.timestamp = |
119 | ktime_get_ns() + dev->time_wrap_offset; | |
2d700715 | 120 | vb2_buffer_done(&sdr_cap_buf->vb.vb2_buf, dev->dqbuf_error ? |
6de8653f HV |
121 | VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE); |
122 | dev->dqbuf_error = false; | |
123 | } | |
124 | } | |
125 | ||
126 | static int vivid_thread_sdr_cap(void *data) | |
127 | { | |
128 | struct vivid_dev *dev = data; | |
129 | u64 samples_since_start; | |
130 | u64 buffers_since_start; | |
131 | u64 next_jiffies_since_start; | |
132 | unsigned long jiffies_since_start; | |
133 | unsigned long cur_jiffies; | |
134 | unsigned wait_jiffies; | |
135 | ||
136 | dprintk(dev, 1, "SDR Capture Thread Start\n"); | |
137 | ||
138 | set_freezable(); | |
139 | ||
140 | /* Resets frame counters */ | |
141 | dev->sdr_cap_seq_offset = 0; | |
142 | if (dev->seq_wrap) | |
143 | dev->sdr_cap_seq_offset = 0xffffff80U; | |
144 | dev->jiffies_sdr_cap = jiffies; | |
145 | dev->sdr_cap_seq_resync = false; | |
146 | ||
147 | for (;;) { | |
148 | try_to_freeze(); | |
149 | if (kthread_should_stop()) | |
150 | break; | |
151 | ||
152 | mutex_lock(&dev->mutex); | |
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 */ | |
2d700715 JS |
163 | buffers_since_start = |
164 | (u64)jiffies_since_start * dev->sdr_adc_freq + | |
6de8653f HV |
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 | } | |
2d700715 JS |
179 | dev->sdr_cap_seq_count = |
180 | buffers_since_start + dev->sdr_cap_seq_offset; | |
6de8653f HV |
181 | |
182 | vivid_thread_sdr_cap_tick(dev); | |
183 | mutex_unlock(&dev->mutex); | |
184 | ||
185 | /* | |
186 | * Calculate the number of samples streamed since we started, | |
187 | * not including the current buffer. | |
188 | */ | |
189 | samples_since_start = buffers_since_start * SDR_CAP_SAMPLES_PER_BUF; | |
190 | ||
191 | /* And the number of jiffies since we started */ | |
192 | jiffies_since_start = jiffies - dev->jiffies_sdr_cap; | |
193 | ||
194 | /* Increase by the number of samples in one buffer */ | |
195 | samples_since_start += SDR_CAP_SAMPLES_PER_BUF; | |
196 | /* | |
197 | * Calculate when that next buffer is supposed to start | |
198 | * in jiffies since we started streaming. | |
199 | */ | |
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; | |
206 | ||
207 | wait_jiffies = next_jiffies_since_start - jiffies_since_start; | |
208 | schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1); | |
209 | } | |
210 | dprintk(dev, 1, "SDR Capture Thread End\n"); | |
211 | return 0; | |
212 | } | |
213 | ||
df9ecb0c | 214 | static int sdr_cap_queue_setup(struct vb2_queue *vq, |
6de8653f | 215 | unsigned *nbuffers, unsigned *nplanes, |
36c0f8b3 | 216 | unsigned sizes[], struct device *alloc_devs[]) |
6de8653f HV |
217 | { |
218 | /* 2 = max 16-bit sample returned */ | |
219 | sizes[0] = SDR_CAP_SAMPLES_PER_BUF * 2; | |
220 | *nplanes = 1; | |
221 | return 0; | |
222 | } | |
223 | ||
224 | static int sdr_cap_buf_prepare(struct vb2_buffer *vb) | |
225 | { | |
226 | struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue); | |
227 | unsigned size = SDR_CAP_SAMPLES_PER_BUF * 2; | |
228 | ||
229 | dprintk(dev, 1, "%s\n", __func__); | |
230 | ||
231 | if (dev->buf_prepare_error) { | |
232 | /* | |
233 | * Error injection: test what happens if buf_prepare() returns | |
234 | * an error. | |
235 | */ | |
236 | dev->buf_prepare_error = false; | |
237 | return -EINVAL; | |
238 | } | |
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); | |
242 | return -EINVAL; | |
243 | } | |
244 | vb2_set_plane_payload(vb, 0, size); | |
245 | ||
246 | return 0; | |
247 | } | |
248 | ||
249 | static void sdr_cap_buf_queue(struct vb2_buffer *vb) | |
250 | { | |
2d700715 | 251 | struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); |
6de8653f | 252 | struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue); |
2d700715 | 253 | struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb); |
6de8653f HV |
254 | |
255 | dprintk(dev, 1, "%s\n", __func__); | |
256 | ||
257 | spin_lock(&dev->slock); | |
258 | list_add_tail(&buf->list, &dev->sdr_cap_active); | |
259 | spin_unlock(&dev->slock); | |
260 | } | |
261 | ||
262 | static int sdr_cap_start_streaming(struct vb2_queue *vq, unsigned count) | |
263 | { | |
264 | struct vivid_dev *dev = vb2_get_drv_priv(vq); | |
265 | int err = 0; | |
266 | ||
267 | dprintk(dev, 1, "%s\n", __func__); | |
268 | dev->sdr_cap_seq_count = 0; | |
269 | if (dev->start_streaming_error) { | |
270 | dev->start_streaming_error = false; | |
271 | err = -EINVAL; | |
272 | } else if (dev->kthread_sdr_cap == NULL) { | |
273 | dev->kthread_sdr_cap = kthread_run(vivid_thread_sdr_cap, dev, | |
274 | "%s-sdr-cap", dev->v4l2_dev.name); | |
275 | ||
276 | if (IS_ERR(dev->kthread_sdr_cap)) { | |
277 | v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n"); | |
278 | err = PTR_ERR(dev->kthread_sdr_cap); | |
279 | dev->kthread_sdr_cap = NULL; | |
280 | } | |
281 | } | |
282 | if (err) { | |
283 | struct vivid_buffer *buf, *tmp; | |
284 | ||
285 | list_for_each_entry_safe(buf, tmp, &dev->sdr_cap_active, list) { | |
286 | list_del(&buf->list); | |
2d700715 JS |
287 | vb2_buffer_done(&buf->vb.vb2_buf, |
288 | VB2_BUF_STATE_QUEUED); | |
6de8653f HV |
289 | } |
290 | } | |
291 | return err; | |
292 | } | |
293 | ||
294 | /* abort streaming and wait for last buffer */ | |
295 | static void sdr_cap_stop_streaming(struct vb2_queue *vq) | |
296 | { | |
297 | struct vivid_dev *dev = vb2_get_drv_priv(vq); | |
298 | ||
299 | if (dev->kthread_sdr_cap == NULL) | |
300 | return; | |
301 | ||
302 | while (!list_empty(&dev->sdr_cap_active)) { | |
303 | struct vivid_buffer *buf; | |
304 | ||
2d700715 JS |
305 | buf = list_entry(dev->sdr_cap_active.next, |
306 | struct vivid_buffer, list); | |
6de8653f | 307 | list_del(&buf->list); |
2d700715 | 308 | vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); |
6de8653f HV |
309 | } |
310 | ||
311 | /* shutdown control thread */ | |
312 | mutex_unlock(&dev->mutex); | |
313 | kthread_stop(dev->kthread_sdr_cap); | |
314 | dev->kthread_sdr_cap = NULL; | |
315 | mutex_lock(&dev->mutex); | |
316 | } | |
317 | ||
318 | const struct vb2_ops vivid_sdr_cap_qops = { | |
319 | .queue_setup = sdr_cap_queue_setup, | |
320 | .buf_prepare = sdr_cap_buf_prepare, | |
321 | .buf_queue = sdr_cap_buf_queue, | |
322 | .start_streaming = sdr_cap_start_streaming, | |
323 | .stop_streaming = sdr_cap_stop_streaming, | |
6dfa5131 PL |
324 | .wait_prepare = vb2_ops_wait_prepare, |
325 | .wait_finish = vb2_ops_wait_finish, | |
6de8653f HV |
326 | }; |
327 | ||
2d700715 JS |
328 | int vivid_sdr_enum_freq_bands(struct file *file, void *fh, |
329 | struct v4l2_frequency_band *band) | |
6de8653f HV |
330 | { |
331 | switch (band->tuner) { | |
332 | case 0: | |
333 | if (band->index >= ARRAY_SIZE(bands_adc)) | |
334 | return -EINVAL; | |
335 | *band = bands_adc[band->index]; | |
336 | return 0; | |
337 | case 1: | |
338 | if (band->index >= ARRAY_SIZE(bands_fm)) | |
339 | return -EINVAL; | |
340 | *band = bands_fm[band->index]; | |
341 | return 0; | |
342 | default: | |
343 | return -EINVAL; | |
344 | } | |
345 | } | |
346 | ||
2d700715 JS |
347 | int vivid_sdr_g_frequency(struct file *file, void *fh, |
348 | struct v4l2_frequency *vf) | |
6de8653f HV |
349 | { |
350 | struct vivid_dev *dev = video_drvdata(file); | |
351 | ||
352 | switch (vf->tuner) { | |
353 | case 0: | |
354 | vf->frequency = dev->sdr_adc_freq; | |
355 | vf->type = V4L2_TUNER_ADC; | |
356 | return 0; | |
357 | case 1: | |
358 | vf->frequency = dev->sdr_fm_freq; | |
359 | vf->type = V4L2_TUNER_RF; | |
360 | return 0; | |
361 | default: | |
362 | return -EINVAL; | |
363 | } | |
364 | } | |
365 | ||
2d700715 JS |
366 | int vivid_sdr_s_frequency(struct file *file, void *fh, |
367 | const struct v4l2_frequency *vf) | |
6de8653f HV |
368 | { |
369 | struct vivid_dev *dev = video_drvdata(file); | |
370 | unsigned freq = vf->frequency; | |
371 | unsigned band; | |
372 | ||
373 | switch (vf->tuner) { | |
374 | case 0: | |
375 | if (vf->type != V4L2_TUNER_ADC) | |
376 | return -EINVAL; | |
377 | if (freq < BAND_ADC_0) | |
378 | band = 0; | |
379 | else if (freq < BAND_ADC_1) | |
380 | band = 1; | |
381 | else | |
382 | band = 2; | |
383 | ||
384 | freq = clamp_t(unsigned, freq, | |
385 | bands_adc[band].rangelow, | |
386 | bands_adc[band].rangehigh); | |
387 | ||
388 | if (vb2_is_streaming(&dev->vb_sdr_cap_q) && | |
389 | freq != dev->sdr_adc_freq) { | |
390 | /* resync the thread's timings */ | |
391 | dev->sdr_cap_seq_resync = true; | |
392 | } | |
393 | dev->sdr_adc_freq = freq; | |
394 | return 0; | |
395 | case 1: | |
396 | if (vf->type != V4L2_TUNER_RF) | |
397 | return -EINVAL; | |
398 | dev->sdr_fm_freq = clamp_t(unsigned, freq, | |
399 | bands_fm[0].rangelow, | |
400 | bands_fm[0].rangehigh); | |
401 | return 0; | |
402 | default: | |
403 | return -EINVAL; | |
404 | } | |
405 | } | |
406 | ||
407 | int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt) | |
408 | { | |
409 | switch (vt->index) { | |
410 | case 0: | |
411 | strlcpy(vt->name, "ADC", sizeof(vt->name)); | |
412 | vt->type = V4L2_TUNER_ADC; | |
2d700715 JS |
413 | vt->capability = |
414 | V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS; | |
6de8653f HV |
415 | vt->rangelow = bands_adc[0].rangelow; |
416 | vt->rangehigh = bands_adc[2].rangehigh; | |
417 | return 0; | |
418 | case 1: | |
419 | strlcpy(vt->name, "RF", sizeof(vt->name)); | |
420 | vt->type = V4L2_TUNER_RF; | |
2d700715 JS |
421 | vt->capability = |
422 | V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS; | |
6de8653f HV |
423 | vt->rangelow = bands_fm[0].rangelow; |
424 | vt->rangehigh = bands_fm[0].rangehigh; | |
425 | return 0; | |
426 | default: | |
427 | return -EINVAL; | |
428 | } | |
429 | } | |
430 | ||
431 | int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt) | |
432 | { | |
433 | if (vt->index > 1) | |
434 | return -EINVAL; | |
435 | return 0; | |
436 | } | |
437 | ||
438 | int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f) | |
439 | { | |
7615f4bc | 440 | if (f->index >= ARRAY_SIZE(formats)) |
6de8653f | 441 | return -EINVAL; |
7615f4bc | 442 | f->pixelformat = formats[f->index].pixelformat; |
6de8653f HV |
443 | return 0; |
444 | } | |
445 | ||
446 | int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) | |
447 | { | |
7615f4bc AP |
448 | struct vivid_dev *dev = video_drvdata(file); |
449 | ||
450 | f->fmt.sdr.pixelformat = dev->sdr_pixelformat; | |
451 | f->fmt.sdr.buffersize = dev->sdr_buffersize; | |
6de8653f HV |
452 | memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved)); |
453 | return 0; | |
454 | } | |
455 | ||
7615f4bc AP |
456 | int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) |
457 | { | |
458 | struct vivid_dev *dev = video_drvdata(file); | |
459 | struct vb2_queue *q = &dev->vb_sdr_cap_q; | |
460 | int i; | |
461 | ||
462 | if (vb2_is_busy(q)) | |
463 | return -EBUSY; | |
464 | ||
465 | memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved)); | |
466 | for (i = 0; i < ARRAY_SIZE(formats); i++) { | |
467 | if (formats[i].pixelformat == f->fmt.sdr.pixelformat) { | |
468 | dev->sdr_pixelformat = formats[i].pixelformat; | |
469 | dev->sdr_buffersize = formats[i].buffersize; | |
470 | f->fmt.sdr.buffersize = formats[i].buffersize; | |
471 | return 0; | |
472 | } | |
473 | } | |
474 | dev->sdr_pixelformat = formats[0].pixelformat; | |
475 | dev->sdr_buffersize = formats[0].buffersize; | |
476 | f->fmt.sdr.pixelformat = formats[0].pixelformat; | |
477 | f->fmt.sdr.buffersize = formats[0].buffersize; | |
478 | return 0; | |
479 | } | |
480 | ||
481 | int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f) | |
482 | { | |
483 | int i; | |
484 | ||
485 | memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved)); | |
486 | for (i = 0; i < ARRAY_SIZE(formats); i++) { | |
487 | if (formats[i].pixelformat == f->fmt.sdr.pixelformat) { | |
488 | f->fmt.sdr.buffersize = formats[i].buffersize; | |
489 | return 0; | |
490 | } | |
491 | } | |
492 | f->fmt.sdr.pixelformat = formats[0].pixelformat; | |
493 | f->fmt.sdr.buffersize = formats[0].buffersize; | |
494 | return 0; | |
495 | } | |
496 | ||
4e30a373 PL |
497 | #define FIXP_N (15) |
498 | #define FIXP_FRAC (1 << FIXP_N) | |
499 | #define FIXP_2PI ((int)(2 * 3.141592653589 * FIXP_FRAC)) | |
f335c3f2 | 500 | #define M_100000PI (3.14159 * 100000) |
6de8653f HV |
501 | |
502 | void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf) | |
503 | { | |
2d700715 | 504 | u8 *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0); |
6de8653f | 505 | unsigned long i; |
2d700715 | 506 | unsigned long plane_size = vb2_plane_size(&buf->vb.vb2_buf, 0); |
f335c3f2 | 507 | s64 s64tmp; |
4e30a373 PL |
508 | s32 src_phase_step; |
509 | s32 mod_phase_step; | |
510 | s32 fixp_i; | |
511 | s32 fixp_q; | |
6de8653f | 512 | |
6de8653f HV |
513 | /* calculate phase step */ |
514 | #define BEEP_FREQ 1000 /* 1kHz beep */ | |
4e30a373 | 515 | src_phase_step = DIV_ROUND_CLOSEST(FIXP_2PI * BEEP_FREQ, |
60f68735 | 516 | dev->sdr_adc_freq); |
6de8653f HV |
517 | |
518 | for (i = 0; i < plane_size; i += 2) { | |
4e30a373 PL |
519 | mod_phase_step = fixp_cos32_rad(dev->sdr_fixp_src_phase, |
520 | FIXP_2PI) >> (31 - FIXP_N); | |
521 | ||
522 | dev->sdr_fixp_src_phase += src_phase_step; | |
f335c3f2 AP |
523 | s64tmp = (s64) mod_phase_step * dev->sdr_fm_deviation; |
524 | dev->sdr_fixp_mod_phase += div_s64(s64tmp, M_100000PI); | |
6de8653f HV |
525 | |
526 | /* | |
60f68735 | 527 | * Transfer phase angle to [0, 2xPI] in order to avoid variable |
6de8653f HV |
528 | * overflow and make it suitable for cosine implementation |
529 | * used, which does not support negative angles. | |
530 | */ | |
60f68735 AP |
531 | dev->sdr_fixp_src_phase %= FIXP_2PI; |
532 | dev->sdr_fixp_mod_phase %= FIXP_2PI; | |
4e30a373 | 533 | |
60f68735 AP |
534 | if (dev->sdr_fixp_mod_phase < 0) |
535 | dev->sdr_fixp_mod_phase += FIXP_2PI; | |
6de8653f | 536 | |
4e30a373 PL |
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); | |
6de8653f | 539 | |
4e30a373 PL |
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); | |
6de8653f | 544 | |
7615f4bc AP |
545 | switch (dev->sdr_pixelformat) { |
546 | case V4L2_SDR_FMT_CU8: | |
60f68735 | 547 | /* convert 'fixp float' to u8 [0, +255] */ |
7615f4bc AP |
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); | |
553 | break; | |
554 | case V4L2_SDR_FMT_CS8: | |
60f68735 AP |
555 | /* convert 'fixp float' to s8 [-128, +127] */ |
556 | /* s8 = X * 127.5 - 0.5; X is float [-1.0, +1.0] */ | |
557 | fixp_i = fixp_i * 1275 - FIXP_FRAC * 5; | |
558 | fixp_q = fixp_q * 1275 - FIXP_FRAC * 5; | |
7615f4bc AP |
559 | *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10); |
560 | *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10); | |
561 | break; | |
562 | default: | |
563 | break; | |
564 | } | |
6de8653f HV |
565 | } |
566 | } |