drivers/input/touchscreen/ts_filter_mean.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  * Mean has no effect if the samples are changing by more that the
  20  * threshold set by averaging_threshold in the configuration.
  21  *
  22  * However while samples come in that don't go outside this threshold from
  23  * the last reported sample, Mean replaces the samples with a simple mean
  24  * of a configurable number of samples (set by bits_filter_length in config,
  25  * which is 2^n, so 5 there makes 32 sample averaging).
  26  *
  27  * Mean works well if the input data is already good quality, reducing + / - 1
  28  * sample jitter when the stylus is still, or moving very slowly, without
  29  * introducing abrupt transitions or reducing ability to follow larger
  30  * movements.  If you set the threshold higher than the dynamic range of the
  31  * coordinates, you can just use it as a simple mean average.
  32  */
  33
  34 #include <linux/errno.h>
  35 #include <linux/kernel.h>
  36 #include <linux/slab.h>
  37 #include "ts_filter_mean.h"
  38
  39 static void ts_filter_mean_clear_internal(struct ts_filter *tsf)
  40 {
  41         struct ts_filter_mean *tsfs = (struct ts_filter_mean *)tsf;
  42         int n;
  43
  44         for (n = 0; n < tsfs->tsf.count_coords; n++) {
  45                 tsfs->fhead[n] = 0;
  46                 tsfs->ftail[n] = 0;
  47                 tsfs->lowpass[n] = 0;
  48         }
  49 }
  50
  51 static void ts_filter_mean_clear(struct ts_filter *tsf)
  52 {
  53         ts_filter_mean_clear_internal(tsf);
  54
  55         if (tsf->next) /* chain */
  56                 (tsf->next->api->clear)(tsf->next);
  57 }
  58
  59 static struct ts_filter *ts_filter_mean_create(struct platform_device *pdev,
  60                                                void *config, int count_coords)
  61 {
  62         int *p;
  63         int n;
  64         struct ts_filter_mean *tsfs = kzalloc(
  65                                   sizeof(struct ts_filter_mean), GFP_KERNEL);
  66
  67         if (!tsfs)
  68                 return NULL;
  69
  70         BUG_ON((count_coords < 1) || (count_coords > MAX_TS_FILTER_COORDS));
  71         tsfs->tsf.count_coords = count_coords;
  72
  73         tsfs->config = (struct ts_filter_mean_configuration *)config;
  74
  75         tsfs->config->extent = 1 << tsfs->config->bits_filter_length;
  76         BUG_ON((tsfs->config->extent > 256) || (!tsfs->config->extent));
  77
  78         p = kmalloc(tsfs->config->extent * sizeof(int) * count_coords,
  79                                                                     GFP_KERNEL);
  80         if (!p)
  81                 return NULL;
  82
  83         for (n = 0; n < count_coords; n++) {
  84                 tsfs->fifo[n] = p;
  85                 p += tsfs->config->extent;
  86         }
  87
  88         if (!tsfs->config->averaging_threshold)
  89                 tsfs->config->averaging_threshold = 0xffff; /* always active */
  90
  91         ts_filter_mean_clear_internal(&tsfs->tsf);
  92
  93         printk(KERN_INFO"  Created Mean ts filter len %d depth %d thresh %d\n",
  94                tsfs->config->extent, count_coords,
  95                tsfs->config->averaging_threshold);
  96
  97         return &tsfs->tsf;
  98 }
  99
 100 static void ts_filter_mean_destroy(struct platform_device *pdev,
 101                                    struct ts_filter *tsf)
 102 {
 103         struct ts_filter_mean *tsfs = (struct ts_filter_mean *)tsf;
 104
 105         kfree(tsfs->fifo[0]); /* first guy has pointer from kmalloc */
 106         kfree(tsf);
 107 }
 108
 109 static void ts_filter_mean_scale(struct ts_filter *tsf, int *coords)
 110 {
 111         if (tsf->next) /* chain */
 112                 (tsf->next->api->scale)(tsf->next, coords);
 113 }
 114
 115 /*
 116  * Give us the raw sample data in x and y, and if we return 1 then you can
 117  * get a filtered coordinate from tsm->x and tsm->y. If we return 0 you didn't
 118  * fill the filter with samples yet.
 119  */
 120
 121 static int ts_filter_mean_process(struct ts_filter *tsf, int *coords)
 122 {
 123         struct ts_filter_mean *tsfs = (struct ts_filter_mean *)tsf;
 124         int n;
 125         int len;
 126
 127         for (n = 0; n < tsf->count_coords; n++) {
 128
 129                 /*
 130                  * Has he moved far enough away that we should abandon current
 131                  * low pass filtering state?
 132                  */
 133                 if ((coords[n] < (tsfs->reported[n] -
 134                                           tsfs->config->averaging_threshold)) ||
 135                     (coords[n] > (tsfs->reported[n] +
 136                                           tsfs->config->averaging_threshold))) {
 137                         tsfs->fhead[n] = 0;
 138                         tsfs->ftail[n] = 0;
 139                         tsfs->lowpass[n] = 0;
 140                 }
 141
 142                 /* capture this sample into fifo and sum */
 143                 tsfs->fifo[n][tsfs->fhead[n]++] = coords[n];
 144                 if (tsfs->fhead[n] == tsfs->config->extent)
 145                         tsfs->fhead[n] = 0;
 146                 tsfs->lowpass[n] += coords[n];
 147
 148                 /* adjust the sum into an average and use that*/
 149                 len = (tsfs->fhead[n] - tsfs->ftail[n]) &
 150                                                      (tsfs->config->extent - 1);
 151                 coords[n] = (tsfs->lowpass[n] + (len >> 1)) / len;
 152                 tsfs->reported[n] = coords[n];
 153
 154                 /* remove oldest sample if we are full */
 155                 if (len == (tsfs->config->extent - 1)) {
 156                         tsfs->lowpass[n] -= tsfs->fifo[n][tsfs->ftail[n]++];
 157                         if (tsfs->ftail[n] == tsfs->config->extent)
 158                                 tsfs->ftail[n] = 0;
 159                 }
 160         }
 161
 162         if (tsf->next) /* chain */
 163                 return (tsf->next->api->process)(tsf->next, coords);
 164
 165         return 1;
 166 }
 167
 168 struct ts_filter_api ts_filter_mean_api = {
 169         .create = ts_filter_mean_create,
 170         .destroy = ts_filter_mean_destroy,
 171         .clear = ts_filter_mean_clear,
 172         .process = ts_filter_mean_process,
 173         .scale = ts_filter_mean_scale,
 174 };