3 files changed, 75 insertions, 1 deletions

diff --git a/v4l2-apps/lib/v4l2_driver.c b/v4l2-apps/lib/v4l2_driver.c
index 3f7a1188f..94f826968 100644
--- a/v4l2-apps/lib/v4l2_driver.c
+++ b/v4l2-apps/lib/v4l2_driver.c
@@ -745,3 +745,60 @@ int v4l2_close (struct v4l2_driver *drv)
 
 	return (close(drv->fd));
 }
+
+/****************************************************************************
+	Get/Set frequency
+ ****************************************************************************/
+
+int v4l2_getset_freq (struct v4l2_driver *drv, enum v4l2_direction dir,
+		      double *freq)
+{
+	struct v4l2_tuner     tun;
+	struct v4l2_frequency frq;
+	double d = 62500;
+	unsigned int counter;
+
+	memset(&tun, 0, sizeof(tun));
+
+	if (-1 == xioctl (drv->fd, VIDIOC_G_TUNER, &tun)) {
+		perror ("g_tuner");
+		printf("Assuming 62.5 kHz step\n");
+	} else {
+		if (tun.capability & V4L2_TUNER_CAP_LOW)
+			d=62.5;
+	}
+
+	if (drv->debug) {
+		if (tun.capability & V4L2_TUNER_CAP_LOW)
+			printf("62.5 Hz step\n");
+		else
+			printf("62.5 kHz step\n");
+	}
+
+	memset(&frq, 0, sizeof(frq));
+
+	frq.type = V4L2_TUNER_ANALOG_TV;
+
+	if (dir & V4L2_GET) {
+		if (-1 == xioctl (drv->fd, VIDIOC_G_FREQUENCY, &frq)) {
+			perror ("s_frequency");
+			return errno;
+		}
+		*freq = frq.frequency * d;
+		if (drv->debug)
+			printf("board is at freq %4.3f MHz (%d)\n",
+				*freq/1000000, frq.frequency);
+	} else {
+		frq.frequency = (uint32_t)(((*freq)+d/2) / d);
+
+		if (-1 == xioctl (drv->fd, VIDIOC_S_FREQUENCY, &frq)) {
+			perror ("s_frequency");
+			return errno;
+		}
+		if (drv->debug)
+			printf("board set to freq %4.3f MHz (%d)\n",
+				*freq/1000000, frq.frequency);
+
+	}
+	return 0;
+}
diff --git a/v4l2-apps/lib/v4l2_driver.h b/v4l2-apps/lib/v4l2_driver.h
index ef6a754f4..74d5b8358 100644
--- a/v4l2-apps/lib/v4l2_driver.h
+++ b/v4l2-apps/lib/v4l2_driver.h
@@ -75,3 +75,6 @@ int v4l2_free_bufs(struct v4l2_driver *drv);
 int v4l2_start_streaming(struct v4l2_driver *drv);
 int v4l2_stop_streaming(struct v4l2_driver *drv);
 int v4l2_rcvbuf(struct v4l2_driver *drv, v4l2_recebe_buffer *v4l2_rec_buf);
+int v4l2_getset_freq (struct v4l2_driver *drv, enum v4l2_direction dir,
+		      double *freq);
+
diff --git a/v4l2-apps/test/driver-test.c b/v4l2-apps/test/driver-test.c
index a2ac365a3..e4c16ed54 100644
--- a/v4l2-apps/test/driver-test.c
+++ b/v4l2-apps/test/driver-test.c
@@ -31,12 +31,13 @@ int main(void)
 	struct v4l2_driver drv;
 	struct drv_list *cur;
 	unsigned int count = 10, i;
+	double freq;
 
 	if (v4l2_open ("/dev/video0", 1,&drv)<0) {
 		perror("open /dev/video0");
 		return -1;
 	}
-	if (v4l2_enum_stds (&drv)) {
+	if (v4l2_enum_stds (&drv)<0) {
 		perror("enum_stds");
 		printf("Error! Driver is not reporting supported STD, frames/sec and number of lines!\n Trying to continue anyway...\n");
 	} else {
@@ -83,6 +84,19 @@ int main(void)
 		perror("get_parm");
 	}
 
+
+	v4l2_getset_freq (&drv,V4L2_GET, &freq);
+
+	freq=0;
+	v4l2_getset_freq (&drv,V4L2_SET, &freq);
+
+	freq=121250000; /* 121.250 MHz */
+	v4l2_getset_freq (&drv,V4L2_SET, &freq);
+
+	printf("Preparing for frames...\n");
+	fflush (stdout);
+	sleep(1);
+
 	v4l2_mmap_bufs(&drv, 2);
 
 	v4l2_start_streaming(&drv);