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(ceil: 100) < dev->perc_dropped_buffers) |
94 | return; |
95 | |
96 | spin_lock(lock: &dev->slock); |
97 | if (!list_empty(head: &dev->sdr_cap_active)) { |
98 | sdr_cap_buf = list_entry(dev->sdr_cap_active.next, |
99 | struct vivid_buffer, list); |
100 | list_del(entry: &sdr_cap_buf->list); |
101 | } |
102 | spin_unlock(lock: &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(req: sdr_cap_buf->vb.vb2_buf.req_obj.req, |
107 | parent: &dev->ctrl_hdl_sdr_cap); |
108 | v4l2_ctrl_request_complete(req: sdr_cap_buf->vb.vb2_buf.req_obj.req, |
109 | parent: &dev->ctrl_hdl_sdr_cap); |
110 | vivid_sdr_cap_process(dev, buf: sdr_cap_buf); |
111 | sdr_cap_buf->vb.vb2_buf.timestamp = |
112 | ktime_get_ns() + dev->time_wrap_offset; |
113 | vb2_buffer_done(vb: &sdr_cap_buf->vb.vb2_buf, state: 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(lock: &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(lock: &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(q: 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, plane_no: 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, plane_no: 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(q: 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(lock: &dev->slock); |
261 | list_add_tail(new: &buf->list, head: &dev->sdr_cap_active); |
262 | spin_unlock(lock: &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(q: 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(ptr: dev->kthread_sdr_cap)) { |
280 | v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n" ); |
281 | err = PTR_ERR(ptr: 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(entry: &buf->list); |
290 | vb2_buffer_done(vb: &buf->vb.vb2_buf, |
291 | state: 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(q: vq); |
301 | |
302 | if (dev->kthread_sdr_cap == NULL) |
303 | return; |
304 | |
305 | while (!list_empty(head: &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(entry: &buf->list); |
311 | v4l2_ctrl_request_complete(req: buf->vb.vb2_buf.req_obj.req, |
312 | parent: &dev->ctrl_hdl_sdr_cap); |
313 | vb2_buffer_done(vb: &buf->vb.vb2_buf, state: VB2_BUF_STATE_ERROR); |
314 | } |
315 | |
316 | /* shutdown control thread */ |
317 | kthread_stop(k: 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(q: vb->vb2_queue); |
324 | |
325 | v4l2_ctrl_request_complete(req: vb->req_obj.req, parent: &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(q: &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(vb: &buf->vb.vb2_buf, plane_no: 0); |
513 | unsigned long i; |
514 | unsigned long plane_size = vb2_plane_size(vb: &buf->vb.vb2_buf, plane_no: 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(dividend: 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(radians: 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 | |