target/linux/s3c24xx/files-2.6.31/drivers/input/touchscreen/ts_filter_median.c

   1 /*
   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.
   6  *
   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.
  11  *
  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
  15  *
  16  * Copyright (c) 2008 Andy Green <andy@openmoko.com>
  17  *
  18  *
  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.
  24  *
  25  * This strongly rejects brief excursions away from a central point that is
  26  * sticky in time compared to the excursion duration.
  27  *
  28  * Thanks to Dale Schumacher (who wrote some example code) and Carl-Daniel
  29  * Halifinger who pointed out this would be a good method.
  30  */
  31
  32 #include <linux/errno.h>
  33 #include <linux/kernel.h>
  34 #include <linux/slab.h>
  35 #include <linux/touchscreen/ts_filter_median.h>
  36
  37 struct ts_filter_median {
  38         /* Private configuration. */
  39         struct ts_filter_median_configuration *config;
  40         /* Generic Filter API. */
  41         struct ts_filter tsf;
  42
  43         /* Count raw samples we get. */
  44         int samples_count;
  45         /*
  46          * Remember the last coordinates we got in order to know if
  47          * we are moving slow or fast.
  48          */
  49         int last_issued[MAX_TS_FILTER_COORDS];
  50         /* How many samples in the sort buffer are valid. */
  51         int 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. */
  57         int pos;
  58         /* Do we have a sample to deliver? */
  59         int ready;
  60 };
  61
  62 #define ts_filter_to_filter_median(f) \
  63         container_of(f, struct ts_filter_median, tsf)
  64
  65
  66 static void ts_filter_median_insert(int *p, int sample, int count)
  67 {
  68         int n;
  69
  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. */
  77                         return;
  78                 }
  79
  80         p[count] = sample; /* Nobody was bigger than us, add us on the end. */
  81 }
  82
  83 static void ts_filter_median_del(int *p, int value, int count)
  84 {
  85         int index;
  86
  87         for (index = 0; index < count; index++)
  88                 if (p[index] == value) {
  89                         for (; index < count; index++)
  90                                 p[index] = p[index + 1];
  91                         return;
  92                 }
  93 }
  94
  95
  96 static void ts_filter_median_clear(struct ts_filter *tsf)
  97 {
  98         struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);
  99
 100         tsfm->pos = 0;
 101         tsfm->valid = 0;
 102         tsfm->ready = 0;
 103         memset(&tsfm->last_issued[0], 1, tsf->count_coords * sizeof(int));
 104 }
 105
 106 static struct ts_filter *ts_filter_median_create(
 107         struct platform_device *pdev,
 108         const struct ts_filter_configuration *conf,
 109         int count_coords)
 110 {
 111         int *p;
 112         int n;
 113         struct ts_filter_median *tsfm = kzalloc(sizeof(struct ts_filter_median),
 114                                                                     GFP_KERNEL);
 115
 116         if (!tsfm)
 117                 return NULL;
 118
 119         tsfm->config = container_of(conf,
 120                                     struct ts_filter_median_configuration,
 121                                     config);
 122
 123         tsfm->tsf.count_coords = count_coords;
 124
 125         tsfm->config->midpoint = (tsfm->config->extent >> 1) + 1;
 126
 127         p = kmalloc(2 * count_coords * sizeof(int) * (tsfm->config->extent + 1),
 128                     GFP_KERNEL);
 129         if (!p) {
 130                 kfree(tsfm);
 131                 return NULL;
 132         }
 133
 134         for (n = 0; n < count_coords; n++) {
 135                 tsfm->sort[n] = p;
 136                 p += tsfm->config->extent + 1;
 137                 tsfm->fifo[n] = p;
 138                 p += tsfm->config->extent + 1;
 139         }
 140
 141         ts_filter_median_clear(&tsfm->tsf);
 142
 143         dev_info(&pdev->dev,
 144                  "Created Median filter len:%d coords:%d dec_threshold:%d\n",
 145                  tsfm->config->extent, count_coords,
 146                  tsfm->config->decimation_threshold);
 147
 148         return &tsfm->tsf;
 149 }
 150
 151 static void ts_filter_median_destroy(struct ts_filter *tsf)
 152 {
 153         struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);
 154
 155         kfree(tsfm->sort[0]); /* First guy has pointer from kmalloc. */
 156         kfree(tsf);
 157 }
 158
 159 static void ts_filter_median_scale(struct ts_filter *tsf, int *coords)
 160 {
 161         int n;
 162
 163         for (n = 0; n < tsf->count_coords; n++)
 164                 coords[n] = (coords[n] + 2) / 3;
 165 }
 166
 167 /*
 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.
 171  */
 172
 173 static int ts_filter_median_process(struct ts_filter *tsf, int *coords)
 174 {
 175         struct ts_filter_median *tsfm = ts_filter_to_filter_median(tsf);
 176         int n;
 177         int movement = 1;
 178
 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);
 184         }
 185         /* Move us on in the fifo. */
 186         if (++tsfm->pos == (tsfm->config->extent + 1))
 187                 tsfm->pos = 0;
 188
 189         /* Have we finished a median sampling? */
 190         if (++tsfm->valid < tsfm->config->extent)
 191                 goto process_exit; /* No valid sample to use. */
 192
 193         BUG_ON(tsfm->valid != tsfm->config->extent);
 194
 195         tsfm->valid--;
 196
 197         /*
 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.
 201          */
 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],
 205                                      tsfm->valid);
 206
 207         tsfm->samples_count--;
 208         if (tsfm->samples_count >= 0)
 209                 goto process_exit;
 210
 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];
 216
 217                 movement += abs(tsfm->last_issued[n] - coords[n]);
 218         }
 219
 220         if (movement > tsfm->config->decimation_threshold) /* Moving fast. */
 221                 tsfm->samples_count = tsfm->config->decimation_above;
 222         else
 223                 tsfm->samples_count = tsfm->config->decimation_below;
 224
 225         memcpy(&tsfm->last_issued[0], coords, tsf->count_coords * sizeof(int));
 226
 227         tsfm->ready = 1;
 228
 229 process_exit:
 230         return 0;
 231 }
 232
 233 static int ts_filter_median_haspoint(struct ts_filter *tsf)
 234 {
 235         struct ts_filter_median *priv = ts_filter_to_filter_median(tsf);
 236
 237         return priv->ready;
 238 }
 239
 240 static void ts_filter_median_getpoint(struct ts_filter *tsf, int *point)
 241 {
 242         struct ts_filter_median *priv = ts_filter_to_filter_median(tsf);
 243
 244         BUG_ON(!priv->ready);
 245
 246         memcpy(point, &priv->last_issued[0], tsf->count_coords * sizeof(int));
 247
 248         priv->ready = 0;
 249 }
 250
 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,
 259 };
 260 EXPORT_SYMBOL_GPL(ts_filter_median_api);
 261