On 08/12/14(Mon) 09:35, David Higgs wrote: > On Mon, Dec 8, 2014 at 9:29 AM, Martin Pieuchot <mpieuc...@nolizard.org> > wrote: > [...] > > Now I'd like to finish the transition that started with the import of > > upd(4) and move away from the actual 1 reportID = 1 driver model. > > Because in the end they are all sharing the same USB device actually > > represented by an uhidev(4) instance. So I'll ride the refactoring > > needed by your buggy firmware to also change that. Since all the > > uhidev_*_report() will be modified, we can also pass a pointer to > > the uhidev(4) device instead of one of its children. Is it clearer? > > Sounds great to me.
As promised, here's a second and last diff that changes the various uhidev_*_report() functions to provide the transfered length to the caller if requested. The *get* variant now also handles the first byte required for non-0 reportID. And last but not least, it includes the hack from apcupsd to deal with buggy firmwares like yours. Could you try it and let me know how it goes? Index: ucycom.c =================================================================== RCS file: /cvs/src/sys/dev/usb/ucycom.c,v retrieving revision 1.29 diff -u -p -r1.29 ucycom.c --- ucycom.c 12 Jul 2014 20:26:33 -0000 1.29 +++ ucycom.c 8 Dec 2014 22:54:53 -0000 @@ -453,8 +453,8 @@ ucycom_param(void *addr, int portno, str report[2] = (baud >> 16) & 0xff; report[3] = (baud >> 24) & 0xff; report[4] = cfg; - err = uhidev_set_report(&sc->sc_hdev, UHID_FEATURE_REPORT, - sc->sc_hdev.sc_report_id, report, sc->sc_flen); + err = uhidev_set_report(sc->sc_hdev.sc_parent, UHID_FEATURE_REPORT, + sc->sc_hdev.sc_report_id, report, sc->sc_flen, NULL); if (err != 0) { DPRINTF(("ucycom_param: uhidev_set_report %d %s\n", err, usbd_errstr(err))); @@ -553,11 +553,11 @@ ucycom_set(void *addr, int portno, int r void ucycom_get_cfg(struct ucycom_softc *sc) { - int err, cfg, baud; + int cfg, baud; uint8_t report[5]; - err = uhidev_get_report(&sc->sc_hdev, UHID_FEATURE_REPORT, - sc->sc_hdev.sc_report_id, report, sc->sc_flen); + uhidev_get_report(sc->sc_hdev.sc_parent, UHID_FEATURE_REPORT, + sc->sc_hdev.sc_report_id, report, sc->sc_flen, NULL); cfg = report[4]; baud = (report[3] << 24) + (report[2] << 16) + (report[1] << 8) + report[0]; DPRINTF(("ucycom_configure: device reports %d baud, %d-%c-%d (%d)\n", baud, Index: ugold.c =================================================================== RCS file: /cvs/src/sys/dev/usb/ugold.c,v retrieving revision 1.6 diff -u -p -r1.6 ugold.c --- ugold.c 29 Apr 2014 12:53:33 -0000 1.6 +++ ugold.c 8 Dec 2014 22:54:53 -0000 @@ -260,6 +260,6 @@ ugold_refresh(void *arg) int ugold_issue_cmd(struct ugold_softc *sc, uint8_t *cmd, int len) { - return uhidev_set_report_async(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, cmd, len); + return uhidev_set_report_async(sc->sc_hdev.sc_parent, + UHID_OUTPUT_REPORT, sc->sc_hdev.sc_report_id, cmd, len, NULL); } Index: uhid.c =================================================================== RCS file: /cvs/src/sys/dev/usb/uhid.c,v retrieving revision 1.58 diff -u -p -r1.58 uhid.c --- uhid.c 12 Jul 2014 18:48:52 -0000 1.58 +++ uhid.c 8 Dec 2014 22:54:53 -0000 @@ -259,7 +259,6 @@ uhid_do_read(struct uhid_softc *sc, stru { int s; int error = 0; - int extra; size_t length; u_char buffer[UHID_CHUNK]; usbd_status err; @@ -267,13 +266,12 @@ uhid_do_read(struct uhid_softc *sc, stru DPRINTFN(1, ("uhidread\n")); if (sc->sc_state & UHID_IMMED) { DPRINTFN(1, ("uhidread immed\n")); - extra = sc->sc_hdev.sc_report_id != 0; - err = uhidev_get_report(&sc->sc_hdev, UHID_INPUT_REPORT, - sc->sc_hdev.sc_report_id, buffer, - sc->sc_hdev.sc_isize + extra); + err = uhidev_get_report(sc->sc_hdev.sc_parent, + UHID_INPUT_REPORT, sc->sc_hdev.sc_report_id, buffer, + sc->sc_hdev.sc_isize, NULL); if (err) return (EIO); - return (uiomove(buffer+extra, sc->sc_hdev.sc_isize, uio)); + return (uiomove(buffer, sc->sc_hdev.sc_isize, uio)); } s = splusb(); @@ -346,8 +344,9 @@ uhid_do_write(struct uhid_softc *sc, str return (EINVAL); error = uiomove(sc->sc_obuf, size, uio); if (!error) { - err = uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, sc->sc_obuf, size); + err = uhidev_set_report(sc->sc_hdev.sc_parent, + UHID_OUTPUT_REPORT, sc->sc_hdev.sc_report_id, sc->sc_obuf, + size, NULL); if (err) error = EIO; } @@ -375,9 +374,8 @@ uhid_do_ioctl(struct uhid_softc *sc, u_l int flag, struct proc *p) { u_char buffer[UHID_CHUNK]; - int size, extra; usbd_status err; - int rc; + int rc, size; DPRINTFN(2, ("uhidioctl: cmd=%lx\n", cmd)); @@ -409,10 +407,9 @@ uhid_do_ioctl(struct uhid_softc *sc, u_l case USB_SET_IMMED: if (*(int *)addr) { - extra = sc->sc_hdev.sc_report_id != 0; - err = uhidev_get_report(&sc->sc_hdev, UHID_INPUT_REPORT, - sc->sc_hdev.sc_report_id, buffer, - sc->sc_hdev.sc_isize + extra); + err = uhidev_get_report(sc->sc_hdev.sc_parent, + UHID_INPUT_REPORT, sc->sc_hdev.sc_report_id, buffer, + sc->sc_hdev.sc_isize, NULL); if (err) return (EOPNOTSUPP); Index: uhidev.c =================================================================== RCS file: /cvs/src/sys/dev/usb/uhidev.c,v retrieving revision 1.64 diff -u -p -r1.64 uhidev.c --- uhidev.c 8 Dec 2014 22:00:11 -0000 1.64 +++ uhidev.c 8 Dec 2014 22:54:53 -0000 @@ -270,11 +270,6 @@ uhidev_use_rdesc(struct uhidev_softc *sc int size; if (vendor == USB_VENDOR_WACOM) { - struct uhidev wacom; - - /* XXX until we pass the parent directly. */ - wacom.sc_parent = sc; - /* The report descriptor for the Wacom Graphire is broken. */ switch (product) { case USB_PRODUCT_WACOM_GRAPHIRE: @@ -283,8 +278,8 @@ uhidev_use_rdesc(struct uhidev_softc *sc break; case USB_PRODUCT_WACOM_GRAPHIRE3_4X5: case USB_PRODUCT_WACOM_GRAPHIRE4_4X5: - uhidev_set_report(&wacom, UHID_FEATURE_REPORT, - 2, &reportbuf, sizeof(reportbuf)); + uhidev_set_report(sc, UHID_FEATURE_REPORT, + 2, &reportbuf, sizeof(reportbuf), NULL); size = sizeof(uhid_graphire3_4x5_report_descr); descptr = uhid_graphire3_4x5_report_descr; break; @@ -629,9 +624,9 @@ uhidev_close(struct uhidev *scd) } usbd_status -uhidev_set_report(struct uhidev *scd, int type, int id, void *data, int len) +uhidev_set_report(struct uhidev_softc *sc, int type, int id, void *data, + int len, int *actlen) { - struct uhidev_softc *sc = scd->sc_parent; usb_device_request_t req; usbd_status err; char *buf = data; @@ -650,7 +645,8 @@ uhidev_set_report(struct uhidev *scd, in USETW(req.wIndex, sc->sc_ifaceno); USETW(req.wLength, len); - err = usbd_do_request(sc->sc_udev, &req, buf); + err = usbd_do_request_flags(sc->sc_udev, &req, buf, 0, actlen, + USBD_DEFAULT_TIMEOUT); if (id > 0) free(buf, M_TEMP, len); @@ -659,10 +655,9 @@ uhidev_set_report(struct uhidev *scd, in } usbd_status -uhidev_set_report_async(struct uhidev *scd, int type, int id, void *data, - int len) +uhidev_set_report_async(struct uhidev_softc *sc, int type, int id, void *data, + int len, int *actlen) { - struct uhidev_softc *sc = scd->sc_parent; usb_device_request_t req; usbd_status err; char *buf = data; @@ -683,7 +678,8 @@ uhidev_set_report_async(struct uhidev *s USETW(req.wIndex, sc->sc_ifaceno); USETW(req.wLength, len); - err = usbd_do_request_async(sc->sc_udev, &req, buf); + err = usbd_do_request_flags(sc->sc_udev, &req, buf, 0, actlen, + USBD_DEFAULT_TIMEOUT); /* * Since report requests are write-only it is safe to free @@ -697,10 +693,17 @@ uhidev_set_report_async(struct uhidev *s } usbd_status -uhidev_get_report(struct uhidev *scd, int type, int id, void *data, int len) +uhidev_get_report(struct uhidev_softc *sc, int type, int id, void *data, + int len, int *actlen) { - struct uhidev_softc *sc = scd->sc_parent; usb_device_request_t req; + char *buf = data; + usbd_status err; + + if (id > 0) { + len++; + buf = malloc(len, M_TEMP, M_WAITOK|M_ZERO); + } req.bmRequestType = UT_READ_CLASS_INTERFACE; req.bRequest = UR_GET_REPORT; @@ -708,7 +711,16 @@ uhidev_get_report(struct uhidev *scd, in USETW(req.wIndex, sc->sc_ifaceno); USETW(req.wLength, len); - return (usbd_do_request(sc->sc_udev, &req, data)); + err = usbd_do_request_flags(sc->sc_udev, &req, buf, 0, actlen, + USBD_DEFAULT_TIMEOUT); + + /* Skip the reportID. */ + if (id > 0) { + memcpy(data, buf + 1, len - 1); + free(buf, M_TEMP, len); + } + + return (err); } usbd_status @@ -748,7 +760,7 @@ uhidev_ioctl(struct uhidev *sc, u_long c { struct usb_ctl_report_desc *rd; struct usb_ctl_report *re; - int size, extra; + int size; usbd_status err; void *desc; @@ -775,11 +787,8 @@ uhidev_ioctl(struct uhidev *sc, u_long c default: return EINVAL; } - extra = sc->sc_report_id != 0; - err = uhidev_get_report(sc, re->ucr_report, sc->sc_report_id, - re->ucr_data, size + extra); - if (extra) - memcpy(re->ucr_data, re->ucr_data + 1, size); + err = uhidev_get_report(sc->sc_parent, re->ucr_report, + sc->sc_report_id, re->ucr_data, size, NULL); if (err) return EIO; break; @@ -798,8 +807,8 @@ uhidev_ioctl(struct uhidev *sc, u_long c default: return EINVAL; } - err = uhidev_set_report(sc, re->ucr_report, - sc->sc_report_id, re->ucr_data, size); + err = uhidev_set_report(sc->sc_parent, re->ucr_report, + sc->sc_report_id, re->ucr_data, size, NULL); if (err) return EIO; break; Index: uhidev.h =================================================================== RCS file: /cvs/src/sys/dev/usb/uhidev.h,v retrieving revision 1.20 diff -u -p -r1.20 uhidev.h --- uhidev.h 24 Apr 2014 09:40:28 -0000 1.20 +++ uhidev.h 8 Dec 2014 22:54:53 -0000 @@ -88,7 +88,10 @@ void uhidev_get_report_desc(struct uhide int uhidev_open(struct uhidev *); void uhidev_close(struct uhidev *); int uhidev_ioctl(struct uhidev *, u_long, caddr_t, int, struct proc *); -usbd_status uhidev_set_report(struct uhidev *, int, int, void *, int); -usbd_status uhidev_set_report_async(struct uhidev *, int, int, void *, int); -usbd_status uhidev_get_report(struct uhidev *, int, int, void *, int); +usbd_status uhidev_set_report(struct uhidev_softc *, int, int, void *, int, + int *); +usbd_status uhidev_set_report_async(struct uhidev_softc *, int, int, void *, + int, int *); +usbd_status uhidev_get_report(struct uhidev_softc *, int, int, void *, int, + int *); usbd_status uhidev_write(struct uhidev_softc *, void *, int); Index: ukbd.c =================================================================== RCS file: /cvs/src/sys/dev/usb/ukbd.c,v retrieving revision 1.68 diff -u -p -r1.68 ukbd.c --- ukbd.c 21 Aug 2014 14:52:55 -0000 1.68 +++ ukbd.c 8 Dec 2014 22:54:53 -0000 @@ -363,8 +363,9 @@ ukbd_set_leds(void *v, int leds) return; if (sc->sc_ledsize && hidkbd_set_leds(kbd, leds, &res) != 0) - uhidev_set_report_async(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, &res, 1); + uhidev_set_report_async(sc->sc_hdev.sc_parent, + UHID_OUTPUT_REPORT, sc->sc_hdev.sc_report_id, &res, 1, + NULL); } int Index: uoak_subr.c =================================================================== RCS file: /cvs/src/sys/dev/usb/uoak_subr.c,v retrieving revision 1.4 diff -u -p -r1.4 uoak_subr.c --- uoak_subr.c 15 Apr 2014 09:14:27 -0000 1.4 +++ uoak_subr.c 8 Dec 2014 22:54:53 -0000 @@ -51,8 +51,8 @@ int uoak_check_device_ready(struct uoak_softc *sc) { - if (uhidev_get_report(sc->sc_hdev, UHID_FEATURE_REPORT, - sc->sc_hdev->sc_report_id, &sc->sc_buf, sc->sc_flen)) + if (uhidev_get_report(sc->sc_hdev->sc_parent, UHID_FEATURE_REPORT, + sc->sc_hdev->sc_report_id, &sc->sc_buf, sc->sc_flen, NULL)) return EIO; if (sc->sc_buf[0] != 0xff) @@ -69,8 +69,8 @@ uoak_set_cmd(struct uoak_softc *sc) while (uoak_check_device_ready(sc) < 0) usbd_delay_ms(sc->sc_udev, UOAK_RETRY_DELAY); - if (uhidev_set_report(sc->sc_hdev, UHID_FEATURE_REPORT, - sc->sc_hdev->sc_report_id, &sc->sc_rcmd, sc->sc_flen)) + if (uhidev_set_report(sc->sc_hdev->sc_parent, UHID_FEATURE_REPORT, + sc->sc_hdev->sc_report_id, &sc->sc_rcmd, sc->sc_flen, NULL)) return EIO; return 0; @@ -86,8 +86,8 @@ uoak_get_cmd(struct uoak_softc *sc) usbd_delay_ms(sc->sc_udev, UOAK_RETRY_DELAY); /* issue request */ - if (uhidev_set_report(sc->sc_hdev, UHID_FEATURE_REPORT, - sc->sc_hdev->sc_report_id, &sc->sc_rcmd, sc->sc_flen)) + if (uhidev_set_report(sc->sc_hdev->sc_parent, UHID_FEATURE_REPORT, + sc->sc_hdev->sc_report_id, &sc->sc_rcmd, sc->sc_flen, NULL)) return EIO; /* wait till the device ready to return the request */ Index: upd.c =================================================================== RCS file: /cvs/src/sys/dev/usb/upd.c,v retrieving revision 1.10 diff -u -p -r1.10 upd.c --- upd.c 12 Jul 2014 18:48:52 -0000 1.10 +++ upd.c 8 Dec 2014 22:54:54 -0000 @@ -263,7 +263,8 @@ upd_refresh(void *arg) struct upd_softc *sc = (struct upd_softc *)arg; struct upd_report *report; uint8_t buf[256]; - int repid, err; + int repid, actlen; + usbd_status err; for (repid = 0; repid < sc->sc_max_repid; repid++) { report = &sc->sc_reports[repid]; @@ -271,15 +272,16 @@ upd_refresh(void *arg) continue; memset(buf, 0x0, sizeof(buf)); - /* - * XXX uhidev_get_report() is not clever enough to handle - * non-NUl reportID, so add an extra byte for it. - */ - err = uhidev_get_report(&sc->sc_hdev, UHID_FEATURE_REPORT, - repid, buf, report->size + 1); - if (err) { - DPRINTF(("read failure: reportid=%02x err=%d\n", repid, - err)); + err = uhidev_get_report(sc->sc_hdev.sc_parent, + UHID_FEATURE_REPORT, repid, buf, report->size, &actlen); + + /* Deal with buggy firmwares, */ + if (err == USBD_SHORT_XFER) + report->size = actlen; + + if (err != USBD_NORMAL_COMPLETION && err != USBD_SHORT_XFER) { + DPRINTF(("read failure: reportid=%02x err=%s\n", repid, + usbd_status(err))); continue; } Index: uthum.c =================================================================== RCS file: /cvs/src/sys/dev/usb/uthum.c,v retrieving revision 1.27 diff -u -p -r1.27 uthum.c --- uthum.c 15 Apr 2014 09:14:27 -0000 1.27 +++ uthum.c 8 Dec 2014 22:54:54 -0000 @@ -290,20 +290,20 @@ uthum_issue_cmd(struct uthum_softc *sc, bzero(cmdbuf, sizeof(cmdbuf)); memcpy(cmdbuf, cmd_issue, sizeof(cmd_issue)); - if (uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_OUTPUT_REPORT, + sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen, NULL)) return EIO; bzero(cmdbuf, sizeof(cmdbuf)); cmdbuf[0] = target_cmd; - if (uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_OUTPUT_REPORT, + sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen, NULL)) return EIO; bzero(cmdbuf, sizeof(cmdbuf)); for (i = 0; i < 7; i++) { - if (uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_OUTPUT_REPORT, + sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen, NULL)) return EIO; } @@ -329,8 +329,8 @@ uthum_read_data(struct uthum_softc *sc, bzero(cmdbuf, sizeof(cmdbuf)); memcpy(cmdbuf, cmd_query, sizeof(cmd_query)); - if (uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_OUTPUT_REPORT, + sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen, NULL)) return EIO; /* wait if required */ @@ -338,8 +338,8 @@ uthum_read_data(struct uthum_softc *sc, tsleep(&sc->sc_sensortask, 0, "uthum", (delay*hz+999)/1000 + 1); /* get answer */ - if (uhidev_get_report(&sc->sc_hdev, UHID_FEATURE_REPORT, - sc->sc_hdev.sc_report_id, report, sc->sc_flen)) + if (uhidev_get_report(sc->sc_hdev.sc_parent, UHID_FEATURE_REPORT, + sc->sc_hdev.sc_report_id, report, sc->sc_flen, NULL)) return EIO; memcpy(buf, report, len); return 0; Index: utrh.c =================================================================== RCS file: /cvs/src/sys/dev/usb/utrh.c,v retrieving revision 1.16 diff -u -p -r1.16 utrh.c --- utrh.c 12 Jul 2014 18:48:53 -0000 1.16 +++ utrh.c 8 Dec 2014 22:54:54 -0000 @@ -211,14 +211,14 @@ utrh_refresh(void *arg) bzero(ledbuf, sizeof(ledbuf)); ledbuf[0] = 0x3; ledbuf[1] = 0x1; - if (uhidev_set_report(&sc->sc_hdev, UHID_FEATURE_REPORT, - sc->sc_hdev.sc_report_id, ledbuf, sc->sc_flen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_FEATURE_REPORT, + sc->sc_hdev.sc_report_id, ledbuf, sc->sc_flen, NULL)) printf("LED request failed\n"); /* issue query */ uint8_t cmdbuf[] = {0x31, 0x15, 0x00, 0x00, 0x00, 0x00, 0x00}; - if (uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_OUTPUT_REPORT, + sc->sc_hdev.sc_report_id, cmdbuf, sc->sc_olen, NULL)) return; /* wait till sensor data are updated, 1s will be enough */ @@ -226,8 +226,8 @@ utrh_refresh(void *arg) /* turn off LED 1 */ ledbuf[1] = 0x0; - if (uhidev_set_report(&sc->sc_hdev, UHID_FEATURE_REPORT, - sc->sc_hdev.sc_report_id, ledbuf, sc->sc_flen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_FEATURE_REPORT, + sc->sc_hdev.sc_report_id, ledbuf, sc->sc_flen, NULL)) printf("LED request failed\n"); temp_tick = (sc->sc_ibuf[2] * 256 + sc->sc_ibuf[3]) & 0x3fff; Index: utwitch.c =================================================================== RCS file: /cvs/src/sys/dev/usb/utwitch.c,v retrieving revision 1.13 diff -u -p -r1.13 utwitch.c --- utwitch.c 12 Jul 2014 18:48:53 -0000 1.13 +++ utwitch.c 8 Dec 2014 22:54:54 -0000 @@ -273,8 +273,8 @@ utwitch_set_mode(struct utwitch_softc *s req[0] = CMD_MODE; req[1] = val; req[2] = CMD_EOF; - err = uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, req, sc->sc_olen); + err = uhidev_set_report(sc->sc_hdev.sc_parent, UHID_OUTPUT_REPORT, + sc->sc_hdev.sc_report_id, req, sc->sc_olen, NULL); if (err) { printf("uhidev_set_report error:EIO\n"); return; @@ -294,8 +294,8 @@ utwitch_read_value_request(struct utwitc req[1] = CMD_EOF; sc->issueing_cmd = CMD_READ; sc->accepted_cmd = CMD_NONE; - if (uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, req, sc->sc_olen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_OUTPUT_REPORT, + sc->sc_hdev.sc_report_id, req, sc->sc_olen, NULL)) return; /* wait till sensor data are updated, 500ms will be enough */ @@ -317,8 +317,8 @@ utwitch_write_value_request(struct utwit sc->issueing_cmd = CMD_WRITE; sc->accepted_cmd = CMD_NONE; - if (uhidev_set_report(&sc->sc_hdev, UHID_OUTPUT_REPORT, - sc->sc_hdev.sc_report_id, req, sc->sc_olen)) + if (uhidev_set_report(sc->sc_hdev.sc_parent, UHID_OUTPUT_REPORT, + sc->sc_hdev.sc_report_id, req, sc->sc_olen, NULL)) return; /* wait till sensor data are updated, 250ms will be enough */