1 /* $Id: pbms.c,v 1.20 2022/03/28 12:43:12 riastradh Exp $ */
2
3 /*
4 * Copyright (c) 2005, Johan Wall�n
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions are
9 * met:
10 *
11 * 1. Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 *
14 * 2. Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 *
19 * 3. The name of the copyright holder may not be used to endorse or
20 * promote products derived from this software without specific
21 * prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER "AS IS" AND ANY
24 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
26 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER BE
27 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
28 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
29 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
30 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
31 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
32 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
33 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /*
37 * The pbms driver provides support for the trackpad on new (post
38 * February 2005) Apple PowerBooks (and iBooks?) that are not standard
39 * USB HID mice.
40 */
41
42 /*
43 * The protocol (that is, the interpretation of the data generated by
44 * the trackpad) is taken from the Linux appletouch driver version
45 * 0.08 by Johannes Berg, Stelian Pop and Frank Arnold. The method
46 * used to detect fingers on the trackpad is also taken from that
47 * driver.
48 */
49
50 /*
51 * To add support for other devices using the same protocol, add an
52 * entry to the pbms_devices table below. See the comments for
53 * pbms_devices and struct pbms_devs.
54 */
55
56 /*
57 * PROTOCOL:
58 *
59 * The driver transfers continuously 81 byte events. The last byte is
60 * 1 if the button is pressed, and is 0 otherwise. Of the remaining
61 * bytes, 26 + 16 = 42 are sensors detecting pressure in the X or
62 * horizontal, and Y or vertical directions, respectively. On 12 and
63 * 15 inch PowerBooks, only the 16 first sensors in the X-direction
64 * are used. In the X-direction, the sensors correspond to byte
65 * positions
66 *
67 * 2, 7, 12, 17, 22, 27, 32, 37, 4, 9, 14, 19, 24, 29, 34, 39, 42,
68 * 47, 52, 57, 62, 67, 72, 77, 44 and 49;
69 *
70 * In the Y direction, the sensors correspond to byte positions
71 *
72 * 1, 6, 11, 16, 21, 26, 31, 36, 3, 8, 13, 18, 23, 28, 33 and 38.
73 *
74 * On 12 inch iBooks only the 9 first sensors in Y-direction are used.
75 * The change in the sensor values over time is more interesting than
76 * their absolute values: if the pressure increases, we know that the
77 * finger has just moved there.
78 *
79 * We keep track of the previous sample (of sensor values in the X and
80 * Y directions) and the accumulated change for each sensor. When we
81 * receive a new sample, we add the difference of the new sensor value
82 * and the old value to the accumulated change. If the accumulator
83 * becomes negative, we set it to zero. The effect is that the
84 * accumulator is large for sensors whose pressure has recently
85 * increased. If there is little change in pressure (or if the
86 * pressure decreases), the accumulator drifts back to zero.
87 *
88 * Since there is some fluctuations, we ignore accumulator values
89 * below a threshold. The raw finger position is computed as a
90 * weighted average of the other sensors (the weights are the
91 * accumulated changes).
92 *
93 * For smoothing, we keep track of the previous raw finger position,
94 * and the virtual position reported to wsmouse. The new raw position
95 * is computed as a weighted average of the old raw position and the
96 * computed raw position. Since this still generates some noise, we
97 * compute a new virtual position as a weighted average of the previous
98 * virtual position and the new raw position. The weights are
99 * controlled by the raw change and a noise parameter. The position
100 * is reported as a relative position.
101 */
102
103 /*
104 * TODO:
105 *
106 * Add support for other drivers of the same type.
107 *
108 * Add support for tapping and two-finger scrolling? The
109 * implementation already detects two fingers, so this should be
110 * relatively easy.
111 *
112 * Implement some of the mouse ioctls?
113 *
114 * Take care of the XXXs.
115 *
116 */
117
118 #include <sys/cdefs.h>
119
120 #include <sys/param.h>
121 #include <sys/device.h>
122 #include <sys/errno.h>
123
124 #include <sys/ioctl.h>
125 #include <sys/systm.h>
126 #include <sys/tty.h>
127
128 #include <dev/usb/usb.h>
129 #include <dev/usb/usbdi.h>
130 #include <dev/usb/usbdevs.h>
131 #include <dev/usb/uhidev.h>
132 #include <dev/hid/hid.h>
133
134 #include <dev/wscons/wsconsio.h>
135 #include <dev/wscons/wsmousevar.h>
136
137 /*
138 * Magic numbers.
139 */
140
141
142 /* The amount of data transferred by the USB device. */
143 #define PBMS_DATA_LEN 81
144
145 /* The maximum number of sensors. */
146 #define PBMS_X_SENSORS 26
147 #define PBMS_Y_SENSORS 16
148 #define PBMS_SENSORS (PBMS_X_SENSORS + PBMS_Y_SENSORS)
149
150 /*
151 * Parameters for supported devices. For generality, these parameters
152 * can be different for each device. The meanings of the parameters
153 * are as follows.
154 *
155 * desc: A printable description used for dmesg output.
156 *
157 * noise: Amount of noise in the computed position. This controls
158 * how large a change must be to get reported, and how
159 * large enough changes are smoothed. A good value can
160 * probably only be found experimentally, but something around
161 * 16 seems suitable.
162 *
163 * product: The product ID of the trackpad.
164 *
165 *
166 * threshold: Accumulated changes less than this are ignored. A good
167 * value could be determined experimentally, but 5 is a
168 * reasonable guess.
169 *
170 * vendor: The vendor ID. Currently USB_VENDOR_APPLE for all devices.
171 *
172 * x_factor: Factor used in computations with X-coordinates. If the
173 * x-resolution of the display is x, this should be
174 * (x + 1) / (x_sensors - 1). Other values work fine, but
175 * then the aspect ratio is not necessarily kept.
176 *
177 * x_sensors: The number of sensors in the X-direction.
178 *
179 * y_factor: As x_factors, but for Y-coordinates.
180 *
181 * y_sensors: The number of sensors in the Y-direction.
182 */
183
184 struct pbms_dev {
185 const char *descr; /* Description of the driver (for dmesg). */
186 int noise; /* Amount of noise in the computed position. */
187 int threshold; /* Changes less than this are ignored. */
188 int x_factor; /* Factor used in computation with X-coordinates. */
189 int x_sensors; /* The number of X-sensors. */
190 int y_factor; /* Factor used in computation with Y-coordinates. */
191 int y_sensors; /* The number of Y-sensors. */
192 uint16_t product; /* Product ID. */
193 uint16_t vendor; /* The vendor ID. */
194 };
195
196 /* Devices supported by this driver. */
197 static struct pbms_dev pbms_devices[] =
198 {
199 #define POWERBOOK_TOUCHPAD(inches, prod, x_fact, x_sens, y_fact) \
200 { \
201 .descr = #inches " inch PowerBook Trackpad", \
202 .vendor = USB_VENDOR_APPLE, \
203 .product = (prod), \
204 .noise = 16, \
205 .threshold = 5, \
206 .x_factor = (x_fact), \
207 .x_sensors = (x_sens), \
208 .y_factor = (y_fact), \
209 .y_sensors = 16 \
210 }
211 /* 12 inch PowerBooks/iBooks */
212 POWERBOOK_TOUCHPAD(12, 0x030a, 69, 16, 52), /* XXX Not tested. */
213 POWERBOOK_TOUCHPAD(12, 0x030b, 73, 15, 96),
214 /* 15 inch PowerBooks */
215 POWERBOOK_TOUCHPAD(15, 0x020e, 85, 16, 57), /* XXX Not tested. */
216 POWERBOOK_TOUCHPAD(15, 0x020f, 85, 16, 57),
217 POWERBOOK_TOUCHPAD(15, 0x0215, 90, 15, 107),
218 /* 17 inch PowerBooks */
219 POWERBOOK_TOUCHPAD(17, 0x020d, 71, 26, 68) /* XXX Not tested. */
220 #undef POWERBOOK_TOUCHPAD
221 };
222
223 /* The number of supported devices. */
224 #define PBMS_NUM_DEVICES (sizeof(pbms_devices) / sizeof(pbms_devices[0]))
225
226
227 /*
228 * Types and prototypes.
229 */
230
231
232 /* Device data. */
233 struct pbms_softc {
234 struct uhidev sc_hdev; /* USB parent */
235 int is_geyser2;
236 int sc_datalen; /* Size of a data packet */
237 int sc_bufusage; /* Number of bytes in sc_databuf */
238 int sc_acc[PBMS_SENSORS]; /* Accumulated sensor values. */
239 unsigned char sc_prev[PBMS_SENSORS]; /* Previous sample. */
240 unsigned char sc_sample[PBMS_SENSORS]; /* Current sample. */
241 uint8_t sc_databuf[PBMS_DATA_LEN]; /* Buffer for a data packet */
242 device_t sc_wsmousedev; /* WSMouse device. */
243 int sc_noise; /* Amount of noise. */
244 int sc_theshold; /* Threshold value. */
245 int sc_x; /* Virtual position in horizontal
246 * direction (wsmouse position). */
247 int sc_x_factor; /* X-coordinate factor. */
248 int sc_x_raw; /* X-position of finger on trackpad. */
249 int sc_x_sensors; /* Number of X-sensors. */
250 int sc_y; /* Virtual position in vertical direction
251 * (wsmouse position). */
252 int sc_y_factor; /* Y-coordinate factor. */
253 int sc_y_raw; /* Y-position of finger on trackpad. */
254 int sc_y_sensors; /* Number of Y-sensors. */
255 uint32_t sc_buttons; /* Button state. */
256 uint32_t sc_status; /* Status flags. */
257 #define PBMS_ENABLED 1 /* Is the device enabled? */
258 #define PBMS_DYING 2 /* Is the device dying? */
259 #define PBMS_VALID 4 /* Is the previous sample valid? */
260 };
261
262
263 /* Static function prototypes. */
264 static void pbms_intr(struct uhidev *, void *, unsigned int);
265 static int pbms_enable(void *);
266 static void pbms_disable(void *);
267 static int pbms_ioctl(void *, unsigned long, void *, int, struct lwp *);
268 static void reorder_sample(struct pbms_softc *, unsigned char *, unsigned char *);
269 static int compute_delta(struct pbms_softc *, int *, int *, int *, uint32_t *);
270 static int detect_pos(int *, int, int, int, int *, int *);
271 static int smooth_pos(int, int, int);
272
273 /* Access methods for wsmouse. */
274 const struct wsmouse_accessops pbms_accessops = {
275 pbms_enable,
276 pbms_ioctl,
277 pbms_disable,
278 };
279
280 /* This take cares also of the basic device registration. */
281 int pbms_match(device_t, cfdata_t, void *);
282 void pbms_attach(device_t, device_t, void *);
283 int pbms_detach(device_t, int);
284 void pbms_childdet(device_t, device_t);
285 int pbms_activate(device_t, enum devact);
286 extern struct cfdriver pbms_cd;
287 CFATTACH_DECL2_NEW(pbms, sizeof(struct pbms_softc), pbms_match, pbms_attach,
288 pbms_detach, pbms_activate, NULL, pbms_childdet);
289
290 /*
291 * Basic driver.
292 */
293
294
295 /* Try to match the device at some uhidev. */
296
297 int
pbms_match(device_t parent,cfdata_t match,void * aux)298 pbms_match(device_t parent, cfdata_t match, void *aux)
299 {
300 struct uhidev_attach_arg *uha = aux;
301 usb_device_descriptor_t *udd;
302 int i;
303 uint16_t vendor, product;
304
305 /*
306 * We just check if the vendor and product IDs have the magic numbers
307 * we expect.
308 */
309 if (uha->uiaa->uiaa_proto == UIPROTO_MOUSE &&
310 ((udd = usbd_get_device_descriptor(uha->uiaa->uiaa_device))
311 != NULL)) {
312 vendor = UGETW(udd->idVendor);
313 product = UGETW(udd->idProduct);
314 for (i = 0; i < PBMS_NUM_DEVICES; i++) {
315 if (vendor == pbms_devices[i].vendor &&
316 product == pbms_devices[i].product)
317 return UMATCH_IFACECLASS;
318 }
319 }
320 return UMATCH_NONE;
321 }
322
323
324 /* Attach the device. */
325
326 void
pbms_attach(device_t parent,device_t self,void * aux)327 pbms_attach(device_t parent, device_t self, void *aux)
328 {
329 struct wsmousedev_attach_args a;
330 struct uhidev_attach_arg *uha = aux;
331 struct pbms_dev *pd;
332 struct pbms_softc *sc = device_private(self);
333 struct usbd_device *udev;
334 usb_device_descriptor_t *udd;
335 int i;
336 uint16_t vendor, product;
337
338 sc->sc_hdev.sc_intr = pbms_intr;
339 sc->sc_hdev.sc_parent = uha->parent;
340 sc->sc_hdev.sc_report_id = uha->reportid;
341
342 sc->is_geyser2 = 0;
343 sc->sc_datalen = PBMS_DATA_LEN;
344
345 /* Fill in device-specific parameters. */
346 udev = uha->uiaa->uiaa_udevice;
347 if ((udd = usbd_get_device_descriptor(udev)) != NULL) {
348 product = UGETW(udd->idProduct);
349 vendor = UGETW(udd->idVendor);
350 for (i = 0; i < PBMS_NUM_DEVICES; i++) {
351 pd = &pbms_devices[i];
352 if (product == pd->product && vendor == pd->vendor) {
353 printf(": %s\n", pd->descr);
354 sc->sc_noise = pd->noise;
355 sc->sc_theshold = pd->threshold;
356 sc->sc_x_factor = pd->x_factor;
357 sc->sc_x_sensors = pd->x_sensors;
358 sc->sc_y_factor = pd->y_factor;
359 sc->sc_y_sensors = pd->y_sensors;
360 if (product == 0x0215) {
361 sc->is_geyser2 = 1;
362 sc->sc_datalen = 64;
363 sc->sc_y_sensors = 9;
364 }
365 else if (product == 0x030b)
366 sc->sc_y_sensors = 9;
367 break;
368 }
369 }
370 }
371 KASSERT(0 <= sc->sc_x_sensors && sc->sc_x_sensors <= PBMS_X_SENSORS);
372 KASSERT(0 <= sc->sc_y_sensors && sc->sc_y_sensors <= PBMS_Y_SENSORS);
373
374 sc->sc_status = 0;
375
376 a.accessops = &pbms_accessops;
377 a.accesscookie = sc;
378
379 sc->sc_wsmousedev = config_found(self, &a, wsmousedevprint, CFARGS_NONE);
380
381 return;
382 }
383
384
385 /* Detach the device. */
386
387 void
pbms_childdet(device_t self,device_t child)388 pbms_childdet(device_t self, device_t child)
389 {
390 struct pbms_softc *sc = device_private(self);
391
392 if (sc->sc_wsmousedev == child)
393 sc->sc_wsmousedev = NULL;
394 }
395
396 int
pbms_detach(device_t self,int flags)397 pbms_detach(device_t self, int flags)
398 {
399 /* XXX This could not possibly be sufficient! */
400 return config_detach_children(self, flags);
401 }
402
403
404 /* Activate the device. */
405
406 int
pbms_activate(device_t self,enum devact act)407 pbms_activate(device_t self, enum devact act)
408 {
409 struct pbms_softc *sc = device_private(self);
410
411 if (act != DVACT_DEACTIVATE)
412 return EOPNOTSUPP;
413
414 sc->sc_status |= PBMS_DYING;
415 return 0;
416 }
417
418
419 /* Enable the device. */
420
421 static int
pbms_enable(void * v)422 pbms_enable(void *v)
423 {
424 struct pbms_softc *sc = v;
425
426 /* Check that we are not detaching or already enabled. */
427 if (sc->sc_status & PBMS_DYING)
428 return EIO;
429 if (sc->sc_status & PBMS_ENABLED)
430 return EBUSY;
431
432 sc->sc_status |= PBMS_ENABLED;
433 sc->sc_status &= ~PBMS_VALID;
434 sc->sc_bufusage = 0;
435 sc->sc_buttons = 0;
436 memset(sc->sc_sample, 0, sizeof(sc->sc_sample));
437
438 return uhidev_open(&sc->sc_hdev);
439 }
440
441
442 /* Disable the device. */
443
444 static void
pbms_disable(void * v)445 pbms_disable(void *v)
446 {
447 struct pbms_softc *sc = v;
448
449 if (!(sc->sc_status & PBMS_ENABLED))
450 return;
451
452 sc->sc_status &= ~PBMS_ENABLED;
453 uhidev_close(&sc->sc_hdev);
454 }
455
456
457 /* XXX ioctl not implemented. */
458
459 static int
pbms_ioctl(void * v,unsigned long cmd,void * data,int flag,struct lwp * p)460 pbms_ioctl(void *v, unsigned long cmd, void *data, int flag, struct lwp *p)
461 {
462 return EPASSTHROUGH;
463 }
464
465
466 /*
467 * Interrupts & pointer movement.
468 */
469
470
471 /* Handle interrupts. */
472
473 void
pbms_intr(struct uhidev * addr,void * ibuf,unsigned int len)474 pbms_intr(struct uhidev *addr, void *ibuf, unsigned int len)
475 {
476 struct pbms_softc *sc = (struct pbms_softc *)addr;
477 uint8_t *data;
478 int dx, dy, dz, i, s;
479 uint32_t buttons;
480
481 /*
482 * We may have to construct the full data packet over two or three
483 * sequential interrupts, as the device only sends us chunks of
484 * 32 or 64 bytes of data.
485 * This also requires some synchronization, to make sure we place
486 * the first protocol-byte at the first byte in the bufffer.
487 */
488 if (sc->is_geyser2) {
489 /* XXX Need to check this. */
490 } else {
491 /* the last chunk is always 17 bytes */
492 if (len == 17 && sc->sc_bufusage + len != sc->sc_datalen) {
493 sc->sc_bufusage = 0; /* discard bad packet */
494 return;
495 }
496 }
497
498 memcpy(sc->sc_databuf + sc->sc_bufusage, ibuf, len);
499 sc->sc_bufusage += len;
500 if (sc->sc_bufusage != sc->sc_datalen)
501 return; /* wait until packet is complete */
502
503 /* process the now complete protocol and clear the buffer */
504 data = sc->sc_databuf;
505 sc->sc_bufusage = 0;
506 #if 0
507 for (i = 0; i < sc->sc_datalen; i++)
508 printf(" %02x", data[i]);
509 printf("\n");
510 #endif
511
512 /* The last byte is 1 if the button is pressed and 0 otherwise. */
513 buttons = !!data[sc->sc_datalen - 1];
514
515 /* Everything below assumes that the sample is reordered. */
516 reorder_sample(sc, sc->sc_sample, data);
517
518 /* Is this the first sample? */
519 if (!(sc->sc_status & PBMS_VALID)) {
520 sc->sc_status |= PBMS_VALID;
521 sc->sc_x = sc->sc_y = -1;
522 sc->sc_x_raw = sc->sc_y_raw = -1;
523 memcpy(sc->sc_prev, sc->sc_sample, sizeof(sc->sc_prev));
524 memset(sc->sc_acc, 0, sizeof(sc->sc_acc));
525 return;
526 }
527 /* Accumulate the sensor change while keeping it nonnegative. */
528 for (i = 0; i < PBMS_SENSORS; i++) {
529 sc->sc_acc[i] +=
530 (signed char) (sc->sc_sample[i] - sc->sc_prev[i]);
531 if (sc->sc_acc[i] < 0)
532 sc->sc_acc[i] = 0;
533 }
534 memcpy(sc->sc_prev, sc->sc_sample, sizeof(sc->sc_prev));
535
536 /* Compute change. */
537 dx = dy = dz = 0;
538 if (!compute_delta(sc, &dx, &dy, &dz, &buttons))
539 return;
540
541 /* Report to wsmouse. */
542 if ((dx != 0 || dy != 0 || dz != 0 || buttons != sc->sc_buttons) &&
543 sc->sc_wsmousedev != NULL) {
544 s = spltty();
545 wsmouse_input(sc->sc_wsmousedev, buttons, dx, -dy, dz, 0,
546 WSMOUSE_INPUT_DELTA);
547 splx(s);
548 }
549 sc->sc_buttons = buttons;
550 }
551
552
553 /*
554 * Reorder the sensor values so that all the X-sensors are before the
555 * Y-sensors in the natural order. Note that this might have to be
556 * rewritten if PBMS_X_SENSORS or PBMS_Y_SENSORS change.
557 */
558
559 static void
reorder_sample(struct pbms_softc * sc,unsigned char * to,unsigned char * from)560 reorder_sample(struct pbms_softc *sc, unsigned char *to, unsigned char *from)
561 {
562 int i;
563
564 if (sc->is_geyser2) {
565 int j;
566
567 memset(to, 0, PBMS_SENSORS);
568 for (i = 0, j = 19; i < 20; i += 2, j += 3) {
569 to[i] = from[j];
570 to[i + 1] = from[j + 1];
571 }
572 for (i = 0, j = 1; i < 9; i += 2, j += 3) {
573 to[PBMS_X_SENSORS + i] = from[j];
574 to[PBMS_X_SENSORS + i + 1] = from[j + 1];
575 }
576 } else {
577 for (i = 0; i < 8; i++) {
578 /* X-sensors. */
579 to[i] = from[5 * i + 2];
580 to[i + 8] = from[5 * i + 4];
581 to[i + 16] = from[5 * i + 42];
582 #if 0
583 /*
584 * XXX This seems to introduce random ventical jumps, so
585 * we ignore these sensors until we figure out their meaning.
586 */
587 if (i < 2)
588 to[i + 24] = from[5 * i + 44];
589 #endif /* 0 */
590 /* Y-sensors. */
591 to[i + 26] = from[5 * i + 1];
592 to[i + 34] = from[5 * i + 3];
593 }
594 }
595 }
596
597
598 /*
599 * Compute the change in x, y and z direction, update the button state
600 * (to simulate more than one button, scrolling etc.), and update the
601 * history. Note that dx, dy, dz and buttons are modified only if
602 * corresponding pressure is detected and should thus be initialised
603 * before the call. Return 0 on error.
604 */
605
606 /* XXX Could we report something useful in dz? */
607
608 static int
compute_delta(struct pbms_softc * sc,int * dx,int * dy,int * dz,uint32_t * buttons)609 compute_delta(struct pbms_softc *sc, int *dx, int *dy, int *dz,
610 uint32_t * buttons)
611 {
612 int x_det, y_det, x_raw, y_raw, x_fingers, y_fingers, fingers, x, y;
613
614 x_det = detect_pos(sc->sc_acc, sc->sc_x_sensors, sc->sc_theshold,
615 sc->sc_x_factor, &x_raw, &x_fingers);
616 y_det = detect_pos(sc->sc_acc + PBMS_X_SENSORS, sc->sc_y_sensors,
617 sc->sc_theshold, sc->sc_y_factor,
618 &y_raw, &y_fingers);
619 fingers = uimax(x_fingers, y_fingers);
620
621 /* Check the number of fingers and if we have detected a position. */
622 if (fingers > 1) {
623 /* More than one finger detected, resetting. */
624 memset(sc->sc_acc, 0, sizeof(sc->sc_acc));
625 sc->sc_x_raw = sc->sc_y_raw = sc->sc_x = sc->sc_y = -1;
626 return 0;
627 } else if (x_det == 0 && y_det == 0) {
628 /* No position detected, resetting. */
629 memset(sc->sc_acc, 0, sizeof(sc->sc_acc));
630 sc->sc_x_raw = sc->sc_y_raw = sc->sc_x = sc->sc_y = -1;
631 } else if (x_det > 0 && y_det > 0) {
632 /* Smooth position. */
633 if (sc->sc_x_raw >= 0) {
634 sc->sc_x_raw = (3 * sc->sc_x_raw + x_raw) / 4;
635 sc->sc_y_raw = (3 * sc->sc_y_raw + y_raw) / 4;
636 /*
637 * Compute virtual position and change if we already
638 * have a decent position.
639 */
640 if (sc->sc_x >= 0) {
641 x = smooth_pos(sc->sc_x, sc->sc_x_raw,
642 sc->sc_noise);
643 y = smooth_pos(sc->sc_y, sc->sc_y_raw,
644 sc->sc_noise);
645 *dx = x - sc->sc_x;
646 *dy = y - sc->sc_y;
647 sc->sc_x = x;
648 sc->sc_y = y;
649 } else {
650 /* Initialise virtual position. */
651 sc->sc_x = sc->sc_x_raw;
652 sc->sc_y = sc->sc_y_raw;
653 }
654 } else {
655 /* Initialise raw position. */
656 sc->sc_x_raw = x_raw;
657 sc->sc_y_raw = y_raw;
658 }
659 }
660 return 1;
661 }
662
663
664 /*
665 * Compute the new smoothed position from the previous smoothed position
666 * and the raw position.
667 */
668
669 static int
smooth_pos(int pos_old,int pos_raw,int noise)670 smooth_pos(int pos_old, int pos_raw, int noise)
671 {
672 int ad, delta;
673
674 delta = pos_raw - pos_old;
675 ad = abs(delta);
676
677 /* Too small changes are ignored. */
678 if (ad < noise / 2)
679 delta = 0;
680 /* A bit larger changes are smoothed. */
681 else if (ad < noise)
682 delta /= 4;
683 else if (ad < 2 * noise)
684 delta /= 2;
685
686 return pos_old + delta;
687 }
688
689
690 /*
691 * Detect the position of the finger. Returns the total pressure.
692 * The position is returned in pos_ret and the number of fingers
693 * is returned in fingers_ret. The position returned in pos_ret
694 * is in [0, (n_sensors - 1) * factor - 1].
695 */
696
697 static int
detect_pos(int * sensors,int n_sensors,int threshold,int fact,int * pos_ret,int * fingers_ret)698 detect_pos(int *sensors, int n_sensors, int threshold, int fact,
699 int *pos_ret, int *fingers_ret)
700 {
701 int i, w, s;
702
703 /*
704 * Compute the number of fingers, total pressure, and weighted
705 * position of the fingers.
706 */
707 *fingers_ret = 0;
708 w = s = 0;
709 for (i = 0; i < n_sensors; i++) {
710 if (sensors[i] >= threshold) {
711 if (i == 0 || sensors[i - 1] < threshold)
712 *fingers_ret += 1;
713 s += sensors[i];
714 w += sensors[i] * i;
715 }
716 }
717
718 if (s > 0)
719 *pos_ret = w * fact / s;
720
721 return s;
722 }
723