2 * This program is free software; you can redistribute it and/or modify
3 * it under the terms of the GNU General Public License as published by
4 * the Free Software Foundation; either version 2 of the License, or
5 * (at your option) any later version.
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software
14 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
16 * Copyright (c) 2008 Andy Green <andy@openmoko.com>
19 * Median averaging stuff. We sort incoming raw samples into an array of
20 * MEDIAN_SIZE length, discarding the oldest sample each time once we are full.
21 * We then return the sum of the middle three samples for X and Y. It means
22 * the final result must be divided by (3 * scaling factor) to correct for
23 * avoiding the repeated /3.
25 * This strongly rejects brief excursions away from a central point that is
26 * sticky in time compared to the excursion duration.
28 * Thanks to Dale Schumacher (who wrote some example code) and Carl-Daniel
29 * Halifinger who pointed out this would be a good method.
32 #include <linux/errno.h>
33 #include <linux/kernel.h>
34 #include <linux/slab.h>
35 #include <linux/touchscreen/ts_filter_median.h>
37 struct ts_filter_median {
38 /* Private configuration. */
39 struct ts_filter_median_configuration *config;
40 /* Generic Filter API. */
43 /* Count raw samples we get. */
46 * Remember the last coordinates we got in order to know if
47 * we are moving slow or fast.
49 int last_issued[MAX_TS_FILTER_COORDS];
50 /* How many samples in the sort buffer are valid. */
52 /* Samples taken for median in sorted form. */
53 int *sort[MAX_TS_FILTER_COORDS];
54 /* Samples taken for median. */
55 int *fifo[MAX_TS_FILTER_COORDS];
56 /* Where we are in the fifo sample memory. */
58 /* Do we have a sample to deliver? */
62 #define ts_filter_to_filter_median(f) \
63 container_of(f, struct ts_filter_median, tsf)
66 static void ts_filter_median_insert(int *p, int sample, int count)
70 /* Search through what we got so far to find where to put sample. */
71 for (n = 0; n < count; n++)
72 if (sample < p[n]) { /* We met somebody bigger than us? */
73 /* Starting from the end, push bigger guys down one. */
74 for (count--; count >= n; count--)
75 p[count + 1] = p[count];
76 p[n] = sample; /* Put us in place of first bigger. */
80 p[count] = sample; /* Nobody was bigger than us, add us on the end. */
83 static void ts_filter_median_del(int *p, int value, int count)
87 for (index = 0; index < count; index++)
88 if (p[index] == value) {
89 for (; index < count; index++)
90 p[index] = p[index + 1];
96 static void ts_filter_median_clear(struct ts_filter *tsf)
98 struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);
103 memset(&tsfm->last_issued[0], 1, tsf->count_coords * sizeof(int));
106 static struct ts_filter *ts_filter_median_create(
107 struct platform_device *pdev,
108 const struct ts_filter_configuration *conf,
113 struct ts_filter_median *tsfm = kzalloc(sizeof(struct ts_filter_median),
119 tsfm->config = container_of(conf,
120 struct ts_filter_median_configuration,
123 tsfm->tsf.count_coords = count_coords;
125 tsfm->config->midpoint = (tsfm->config->extent >> 1) + 1;
127 p = kmalloc(2 * count_coords * sizeof(int) * (tsfm->config->extent + 1),
134 for (n = 0; n < count_coords; n++) {
136 p += tsfm->config->extent + 1;
138 p += tsfm->config->extent + 1;
141 ts_filter_median_clear(&tsfm->tsf);
144 "Created Median filter len:%d coords:%d dec_threshold:%d\n",
145 tsfm->config->extent, count_coords,
146 tsfm->config->decimation_threshold);
151 static void ts_filter_median_destroy(struct ts_filter *tsf)
153 struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);
155 kfree(tsfm->sort[0]); /* First guy has pointer from kmalloc. */
159 static void ts_filter_median_scale(struct ts_filter *tsf, int *coords)
163 for (n = 0; n < tsf->count_coords; n++)
164 coords[n] = (coords[n] + 2) / 3;
168 * Give us the raw sample data coords, and if we return 1 then you can
169 * get a filtered coordinate from coords. If we return 0 you didn't
170 * fill all the filters with samples yet.
173 static int ts_filter_median_process(struct ts_filter *tsf, int *coords)
175 struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);
179 for (n = 0; n < tsf->count_coords; n++) {
180 /* Grab copy in insertion order to remove when oldest. */
181 tsfm->fifo[n][tsfm->pos] = coords[n];
182 /* Insert these samples in sorted order in the median arrays. */
183 ts_filter_median_insert(tsfm->sort[n], coords[n], tsfm->valid);
185 /* Move us on in the fifo. */
186 if (++tsfm->pos == (tsfm->config->extent + 1))
189 /* Have we finished a median sampling? */
190 if (++tsfm->valid < tsfm->config->extent)
191 goto process_exit; /* No valid sample to use. */
193 BUG_ON(tsfm->valid != tsfm->config->extent);
198 * Sum the middle 3 in the median sorted arrays. We don't divide back
199 * down which increases the sum resolution by a factor of 3 until the
200 * scale API function is called.
202 for (n = 0; n < tsf->count_coords; n++)
203 /* Perform the deletion of the oldest sample. */
204 ts_filter_median_del(tsfm->sort[n], tsfm->fifo[n][tsfm->pos],
207 tsfm->samples_count--;
208 if (tsfm->samples_count >= 0)
211 for (n = 0; n < tsf->count_coords; n++) {
212 /* Give the coordinate result from summing median 3. */
213 coords[n] = tsfm->sort[n][tsfm->config->midpoint - 1] +
214 tsfm->sort[n][tsfm->config->midpoint] +
215 tsfm->sort[n][tsfm->config->midpoint + 1];
217 movement += abs(tsfm->last_issued[n] - coords[n]);
220 if (movement > tsfm->config->decimation_threshold) /* Moving fast. */
221 tsfm->samples_count = tsfm->config->decimation_above;
223 tsfm->samples_count = tsfm->config->decimation_below;
225 memcpy(&tsfm->last_issued[0], coords, tsf->count_coords * sizeof(int));
233 static int ts_filter_median_haspoint(struct ts_filter *tsf)
235 struct ts_filter_median *priv = ts_filter_to_filter_median(tsf);
240 static void ts_filter_median_getpoint(struct ts_filter *tsf, int *point)
242 struct ts_filter_median *priv = ts_filter_to_filter_median(tsf);
244 BUG_ON(!priv->ready);
246 memcpy(point, &priv->last_issued[0], tsf->count_coords * sizeof(int));
251 const struct ts_filter_api ts_filter_median_api = {
252 .create = ts_filter_median_create,
253 .destroy = ts_filter_median_destroy,
254 .clear = ts_filter_median_clear,
255 .process = ts_filter_median_process,
256 .scale = ts_filter_median_scale,
257 .haspoint = ts_filter_median_haspoint,
258 .getpoint = ts_filter_median_getpoint,
260 EXPORT_SYMBOL_GPL(ts_filter_median_api);