aboutsummaryrefslogtreecommitdiff
path: root/drivers/input/touchscreen/ts_filter_median.c
blob: 6f8aae5941da7daccaafe21bf178a20bde56e167 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * Copyright (c) 2008 Andy Green <andy@warmcat.com>
 *
 *
 * Median averaging stuff.  We sort incoming raw samples into an array of
 * MEDIAN_SIZE length, discarding the oldest sample each time once we are full.
 * We then return the sum of the middle three samples for X and Y.  It means
 * the final result must be divided by (3 * scaling factor) to correct for
 * avoiding the repeated /3.
 *
 * This strongly rejects brief excursions away from a central point that is
 * sticky in time compared to the excursion duration.
 *
 * Thanks to Dale Schumacher (who wrote some example code) and Carl-Daniel
 * Halifinger who pointed out this would be a good method.
 */

#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/slab.h>
#include "ts_filter_median.h"

struct ts_filter_median {
	/* Private configuration. */
	struct ts_filter_median_configuration *config;
	/* Generic Filter API. */
	struct ts_filter tsf;

	/* Count raw samples we get. */
	int samples_count;
	/*
	 * Remember the last coordinates we got in order to know if
	 * we are moving slow or fast.
	 */
	int last_issued[MAX_TS_FILTER_COORDS];
	/* How many samples in the sort buffer are valid. */
	int valid;
	/* Samples taken for median in sorted form. */
	int *sort[MAX_TS_FILTER_COORDS];
	/* Samples taken for median. */
	int *fifo[MAX_TS_FILTER_COORDS];
	/* Where we are in the fifo sample memory. */
	int pos;
	/* Do we have a sample to deliver? */
	int ready;
};

#define ts_filter_to_filter_median(f) \
	container_of(f, struct ts_filter_median, tsf)


static void ts_filter_median_insert(int *p, int sample, int count)
{
	int n;

	/* Search through what we got so far to find where to put sample. */
	for (n = 0; n < count; n++)
		if (sample < p[n]) {	/* We met somebody bigger than us? */
			/* Starting from the end, push bigger guys down one. */
			for (count--; count >= n; count--)
				p[count + 1] = p[count];
			p[n] = sample; /* Put us in place of first bigger. */
			return;
		}

	p[count] = sample; /* Nobody was bigger than us, add us on the end. */
}

static void ts_filter_median_del(int *p, int value, int count)
{
	int index;

	for (index = 0; index < count; index++)
		if (p[index] == value) {
			for (; index < count; index++)
				p[index] = p[index + 1];
			return;
		}
}


static void ts_filter_median_clear(struct ts_filter *tsf)
{
	struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);

	tsfm->pos = 0;
	tsfm->valid = 0;
	tsfm->ready = 0;
	memset(&tsfm->last_issued[0], 1, tsf->count_coords * sizeof(int));
}

static struct ts_filter *ts_filter_median_create(
	struct platform_device *pdev,
	const struct ts_filter_configuration *conf,
	int count_coords)
{
	int *p;
	int n;
	struct ts_filter_median *tsfm = kzalloc(sizeof(struct ts_filter_median),
								    GFP_KERNEL);

	if (!tsfm)
		return NULL;

	tsfm->config = container_of(conf,
				    struct ts_filter_median_configuration,
				    config);

	tsfm->tsf.count_coords = count_coords;

	tsfm->config->midpoint = (tsfm->config->extent >> 1) + 1;

	p = kmalloc(2 * count_coords * sizeof(int) * (tsfm->config->extent + 1),
		    GFP_KERNEL);
	if (!p) {
		kfree(tsfm);
		return NULL;
	}

	for (n = 0; n < count_coords; n++) {
		tsfm->sort[n] = p;
		p += tsfm->config->extent + 1;
		tsfm->fifo[n] = p;
		p += tsfm->config->extent + 1;
	}

	ts_filter_median_clear(&tsfm->tsf);

	dev_info(&pdev->dev,
		 "Created Median filter len:%d coords:%d dec_threshold:%d\n",
		 tsfm->config->extent, count_coords,
		 tsfm->config->decimation_threshold);

	return &tsfm->tsf;
}

static void ts_filter_median_destroy(struct ts_filter *tsf)
{
	struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);

	kfree(tsfm->sort[0]); /* First guy has pointer from kmalloc. */
	kfree(tsf);
}

static void ts_filter_median_scale(struct ts_filter *tsf, int *coords)
{
	int n;

	for (n = 0; n < tsf->count_coords; n++)
		coords[n] = (coords[n] + 2) / 3;
}

/*
 * Give us the raw sample data coords, and if we return 1 then you can
 * get a filtered coordinate from coords. If we return 0 you didn't
 * fill all the filters with samples yet.
 */

static int ts_filter_median_process(struct ts_filter *tsf, int *coords)
{
	struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);
	int n;
	int movement = 1;

	for (n = 0; n < tsf->count_coords; n++) {
		/* Grab copy in insertion order to remove when oldest. */
		tsfm->fifo[n][tsfm->pos] = coords[n];
		/* Insert these samples in sorted order in the median arrays. */
		ts_filter_median_insert(tsfm->sort[n], coords[n], tsfm->valid);
	}
	/* Move us on in the fifo. */
	if (++tsfm->pos == (tsfm->config->extent + 1))
		tsfm->pos = 0;

	/* Have we finished a median sampling? */
	if (++tsfm->valid < tsfm->config->extent)
		goto process_exit; /* No valid sample to use. */

	BUG_ON(tsfm->valid != tsfm->config->extent);

	tsfm->valid--;

	/*
	 * Sum the middle 3 in the median sorted arrays. We don't divide back
	 * down which increases the sum resolution by a factor of 3 until the
	 * scale API function is called.
	 */
	for (n = 0; n < tsf->count_coords; n++)
		/* Perform the deletion of the oldest sample. */
		ts_filter_median_del(tsfm->sort[n], tsfm->fifo[n][tsfm->pos],
				     tsfm->valid);

	tsfm->samples_count--;
	if (tsfm->samples_count >= 0)
		goto process_exit;

	for (n = 0; n < tsf->count_coords; n++) {
		/* Give the coordinate result from summing median 3. */
		coords[n] = tsfm->sort[n][tsfm->config->midpoint - 1] +
			    tsfm->sort[n][tsfm->config->midpoint] +
			    tsfm->sort[n][tsfm->config->midpoint + 1];

		movement += abs(tsfm->last_issued[n] - coords[n]);
	}

	if (movement > tsfm->config->decimation_threshold) /* Moving fast. */
		tsfm->samples_count = tsfm->config->decimation_above;
	else
		tsfm->samples_count = tsfm->config->decimation_below;

	memcpy(&tsfm->last_issued[0], coords, tsf->count_coords * sizeof(int));

	tsfm->ready = 1;

process_exit:
	return 0;
}

static int ts_filter_median_haspoint(struct ts_filter *tsf)
{
	struct ts_filter_median *priv = ts_filter_to_filter_median(tsf);

	return priv->ready;
}

static void ts_filter_median_getpoint(struct ts_filter *tsf, int *point)
{
	struct ts_filter_median *priv = ts_filter_to_filter_median(tsf);

	BUG_ON(!priv->ready);

	memcpy(point, &priv->last_issued[0], tsf->count_coords * sizeof(int));

	priv->ready = 0;
}

const struct ts_filter_api ts_filter_median_api = {
	.create =	ts_filter_median_create,
	.destroy =	ts_filter_median_destroy,
	.clear =	ts_filter_median_clear,
	.process =	ts_filter_median_process,
	.scale =	ts_filter_median_scale,
	.haspoint =	ts_filter_median_haspoint,
	.getpoint =	ts_filter_median_getpoint,
};
EXPORT_SYMBOL_GPL(ts_filter_median_api);