diff options
Diffstat (limited to 'drivers/sensor/yas_mag_driver-yas532.c')
| -rw-r--r-- | drivers/sensor/yas_mag_driver-yas532.c | 2909 |
1 files changed, 2909 insertions, 0 deletions
diff --git a/drivers/sensor/yas_mag_driver-yas532.c b/drivers/sensor/yas_mag_driver-yas532.c new file mode 100644 index 0000000..14259b8 --- /dev/null +++ b/drivers/sensor/yas_mag_driver-yas532.c @@ -0,0 +1,2909 @@ +/* + * Copyright (c) 2010-2011 Yamaha Corporation + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +#include <linux/sensor/yas.h> + +struct utimeval { + int32_t tv_sec; + int32_t tv_msec; +}; + +struct utimer { + struct utimeval prev_time; + struct utimeval total_time; + struct utimeval delay_ms; +}; + +static int utimeval_init(struct utimeval *val); +static int utimeval_is_initial(struct utimeval *val); +static int utimeval_is_overflow(struct utimeval *val); +static struct utimeval utimeval_plus(struct utimeval *first, + struct utimeval *second); +static struct utimeval utimeval_minus(struct utimeval *first, + struct utimeval *second); +static int utimeval_greater_than(struct utimeval *first, + struct utimeval *second); +static int utimeval_greater_or_equal(struct utimeval *first, + struct utimeval *second); +static int utimeval_greater_than_zero(struct utimeval *val); +static int utimeval_less_than_zero(struct utimeval *val); +static struct utimeval *msec_to_utimeval(struct utimeval *result, + uint32_t msec); +static uint32_t utimeval_to_msec(struct utimeval *val); + +static struct utimeval utimer_calc_next_time(struct utimer *ut, + struct utimeval *cur); +static struct utimeval utimer_current_time(void); +static int utimer_is_timeout(struct utimer *ut); +static int utimer_clear_timeout(struct utimer *ut); +static uint32_t utimer_get_total_time(struct utimer *ut); +static uint32_t utimer_get_delay(struct utimer *ut); +static int utimer_set_delay(struct utimer *ut, uint32_t delay_ms); +static int utimer_update(struct utimer *ut); +static int utimer_update_with_curtime(struct utimer *ut, struct utimeval *cur); +static uint32_t utimer_sleep_time(struct utimer *ut); +static uint32_t utimer_sleep_time_with_curtime(struct utimer *ut, + struct utimeval *cur); +static int utimer_init(struct utimer *ut, uint32_t delay_ms); +static int utimer_clear(struct utimer *ut); +static void utimer_lib_init(void (*func) (int *sec, int *msec)); + +#define YAS_REGADDR_DEVICE_ID (0x80) +#define YAS_REGADDR_ACTUATE_INIT_COIL (0x81) +#define YAS_REGADDR_MEASURE_COMMAND (0x82) +#define YAS_REGADDR_CONFIG (0x83) +#define YAS_REGADDR_MEASURE_INTERVAL (0x84) +#define YAS_REGADDR_OFFSET_X (0x85) +#define YAS_REGADDR_OFFSET_Y1 (0x86) +#define YAS_REGADDR_OFFSET_Y2 (0x87) +#define YAS_REGADDR_TEST1 (0x88) +#define YAS_REGADDR_TEST2 (0x89) +#define YAS_REGADDR_CAL (0x90) +#define YAS_REGADDR_MEASURE_DATA (0xb0) +#define YAS_YAS530_DEVICE_ID (0x01) /* YAS530 (MS-3E) */ +#define YAS_YAS530_VERSION_A (0) /* YAS530 (MS-3E Aver) */ +#define YAS_YAS530_VERSION_B (1) /* YAS530B (MS-3E Bver) */ +#define YAS_YAS530_VERSION_A_COEF (380) +#define YAS_YAS530_VERSION_B_COEF (550) +#define YAS_YAS530_DATA_CENTER (2048) +#define YAS_YAS530_DATA_OVERFLOW (4095) + +#define YAS_YAS532_DEVICE_ID (0x02) /* YAS532 (MS-3R) */ +#define YAS_YAS532_VERSION_AB (0) /* YAS532AB (MS-3R ABver) */ +#define YAS_YAS532_VERSION_AC (1) /* YAS532AC (MS-3R ACver) */ +#define YAS_YAS532_VERSION_AB_COEF (1800) +#define YAS_YAS532_VERSION_AC_COEF (900) +#define YAS_YAS532_DATA_CENTER (4096) +#define YAS_YAS532_DATA_OVERFLOW (8190) + +#undef YAS_YAS530_CAL_SINGLE_READ + +struct yas_machdep_func { + int (*device_open) (void); + int (*device_close) (void); + int (*device_write) (uint8_t addr, const uint8_t *buf, int len); + int (*device_read) (uint8_t addr, uint8_t *buf, int len); + void (*msleep) (int msec); +}; + +static int yas_cdrv_actuate_initcoil(void); +static int yas_cdrv_set_offset(const int8_t *offset); +static int yas_cdrv_recalc_calib_offset(int32_t *prev_calib_offset, + int32_t *new_calib_offset, + int8_t *prev_offset, + int8_t *new_offset); +static int yas_cdrv_set_transformatiom_matrix(const int8_t *transform); +static int yas_cdrv_measure_and_set_offset(int8_t *offset); +static int yas_cdrv_measure(int32_t *msens, int32_t *raw, int16_t *t); +static int yas_cdrv_init(const int8_t *transform, + struct yas_machdep_func *func); +static int yas_cdrv_term(void); + +static void (*current_time) (int *sec, int *msec) = { +0}; + +static int utimeval_init(struct utimeval *val) +{ + if (unlikely(!val)) + return -1; + val->tv_sec = val->tv_msec = 0; + return 0; +} + +static int utimeval_is_initial(struct utimeval *val) +{ + if (unlikely(!val)) + return 0; + return val->tv_sec == 0 && val->tv_msec == 0; +} + +static int utimeval_is_overflow(struct utimeval *val) +{ + int32_t max; + + if (unlikely(!val)) + return 0; + + max = (int32_t) (0xffffffff / 1000); + if (val->tv_sec > max) { + return 1; /* overflow */ + } else if (val->tv_sec == max) { + if (val->tv_msec > + (int32_t) (0xffffffff % 1000)) { + return 1; /* overflow */ + } + } + + return 0; +} + +static struct utimeval +utimeval_plus(struct utimeval *first, struct utimeval *second) +{ + struct utimeval result = { 0, 0 }; + int32_t tmp; + + if (unlikely(!first || !second)) + return result; + + tmp = first->tv_sec + second->tv_sec; + if (first->tv_sec >= 0 && second->tv_sec >= 0 && tmp < 0) + goto overflow; + if (first->tv_sec < 0 && second->tv_sec < 0 && tmp >= 0) + goto underflow; + + result.tv_sec = tmp; + result.tv_msec = first->tv_msec + second->tv_msec; + if (1000 <= result.tv_msec) { + tmp = result.tv_sec + result.tv_msec / 1000; + if (result.tv_sec >= 0 && result.tv_msec >= 0 && tmp < 0) + goto overflow; + result.tv_sec = tmp; + result.tv_msec = result.tv_msec % 1000; + } + if (result.tv_msec < 0) { + tmp = result.tv_sec + result.tv_msec / 1000 - 1; + if (result.tv_sec < 0 && result.tv_msec < 0 && tmp >= 0) + goto underflow; + result.tv_sec = tmp; + result.tv_msec = result.tv_msec % 1000 + 1000; + } + + return result; + +overflow: + result.tv_sec = 0x7fffffff; + result.tv_msec = 999; + return result; + +underflow: + result.tv_sec = 0x80000000; + result.tv_msec = 0; + return result; +} + +static struct utimeval +utimeval_minus(struct utimeval *first, struct utimeval *second) +{ + struct utimeval result = { 0, 0 }, tmp; + + if (first == NULL || second == NULL + || second->tv_sec == (int)0x80000000) + return result; + + tmp.tv_sec = -second->tv_sec; + tmp.tv_msec = -second->tv_msec; + return utimeval_plus(first, &tmp); +} + +static int utimeval_less_than(struct utimeval *first, struct utimeval *second) +{ + if (unlikely(!first || !second)) + return 0; + + if (first->tv_sec > second->tv_sec) + return 1; + else if (first->tv_sec < second->tv_sec) + return 0; + else + if (first->tv_msec > second->tv_msec) + return 1; + else + return 0; +} + +static int +utimeval_greater_than(struct utimeval *first, struct utimeval *second) +{ + if (unlikely(!first || !second)) + return 0; + + if (first->tv_sec < second->tv_sec) + return 1; + else if (first->tv_sec > second->tv_sec) + return 0; + else + if (first->tv_msec < second->tv_msec) + return 1; + else + return 0; +} + +static int +utimeval_greater_or_equal(struct utimeval *first, struct utimeval *second) +{ + return !utimeval_less_than(first, second); +} + +static int utimeval_greater_than_zero(struct utimeval *val) +{ + struct utimeval zero = { 0, 0 }; + return utimeval_greater_than(&zero, val); +} + +static int utimeval_less_than_zero(struct utimeval *val) +{ + struct utimeval zero = { 0, 0 }; + return utimeval_less_than(&zero, val); +} + +static struct utimeval *msec_to_utimeval(struct utimeval *result, uint32_t msec) +{ + if (unlikely(!result)) + return result; + result->tv_sec = msec / 1000; + result->tv_msec = msec % 1000; + + return result; +} + +static uint32_t utimeval_to_msec(struct utimeval *val) +{ + if (unlikely(!val)) + return 0; + if (utimeval_less_than_zero(val)) + return 0; + + if (utimeval_is_overflow(val)) + return 0xffffffff; + + return val->tv_sec * 1000 + val->tv_msec; +} + +static struct utimeval +utimer_calc_next_time(struct utimer *ut, struct utimeval *cur) +{ + struct utimeval result = { 0, 0 }, delay; + + if (ut == NULL || cur == NULL) + return result; + utimer_update_with_curtime(ut, cur); + if (utimer_is_timeout(ut)) { + result = *cur; + } else { + delay = utimeval_minus(&ut->delay_ms, &ut->total_time); + result = utimeval_plus(cur, &delay); + } + + return result; +} + +static struct utimeval utimer_current_time(void) +{ + struct utimeval tv; + int sec, msec; + + if (current_time != NULL) + current_time(&sec, &msec); + else + sec = 0, msec = 0; + tv.tv_sec = sec; + tv.tv_msec = msec; + + return tv; +} + +static int utimer_clear(struct utimer *ut) +{ + if (unlikely(!ut)) + return -1; + utimeval_init(&ut->prev_time); + utimeval_init(&ut->total_time); + + return 0; +} + +static int utimer_update_with_curtime(struct utimer *ut, struct utimeval *cur) +{ + struct utimeval tmp; + + if (unlikely(!ut || !cur)) + return -1; + if (utimeval_is_initial(&ut->prev_time)) + ut->prev_time = *cur; + if (utimeval_greater_than_zero(&ut->delay_ms)) { + tmp = utimeval_minus(cur, &ut->prev_time); + if (utimeval_less_than_zero(&tmp)) + utimeval_init(&ut->total_time); + else { + ut->total_time = utimeval_plus(&tmp, &ut->total_time); + if (utimeval_is_overflow(&ut->total_time)) + utimeval_init(&ut->total_time); + } + ut->prev_time = *cur; + } + + return 0; +} + +static int utimer_update(struct utimer *ut) +{ + struct utimeval cur; + + if (unlikely(!ut)) + return -1; + + cur = utimer_current_time(); + utimer_update_with_curtime(ut, &cur); + return 0; +} + +static int utimer_is_timeout(struct utimer *ut) +{ + if (unlikely(!ut)) + return 0; + + if (utimeval_greater_than_zero(&ut->delay_ms)) + return utimeval_greater_or_equal(&ut->delay_ms, + &ut->total_time); + else + return 1; +} + +static int utimer_clear_timeout(struct utimer *ut) +{ + uint32_t delay, total; + + if (unlikely(!ut)) + return -1; + + delay = utimeval_to_msec(&ut->delay_ms); + if (delay == 0 || utimeval_is_overflow(&ut->total_time)) + total = 0; + else + if (utimeval_is_overflow(&ut->total_time)) + total = 0; + else { + total = utimeval_to_msec(&ut->total_time); + total = total % delay; + } + msec_to_utimeval(&ut->total_time, total); + + return 0; +} + +static uint32_t +utimer_sleep_time_with_curtime(struct utimer *ut, struct utimeval *cur) +{ + struct utimeval tv; + + if (unlikely(!ut || !cur)) + return 0; + + tv = utimer_calc_next_time(ut, cur); + tv = utimeval_minus(&tv, cur); + if (utimeval_less_than_zero(&tv)) + return 0; + + return utimeval_to_msec(&tv); +} + +static uint32_t utimer_sleep_time(struct utimer *ut) +{ + struct utimeval cur; + + if (unlikely(!ut)) + return 0; + + cur = utimer_current_time(); + return utimer_sleep_time_with_curtime(ut, &cur); +} + +static int utimer_init(struct utimer *ut, uint32_t delay_ms) +{ + if (unlikely(!ut)) + return -1; + utimer_clear(ut); + msec_to_utimeval(&ut->delay_ms, delay_ms); + + return 0; +} + +static uint32_t utimer_get_total_time(struct utimer *ut) +{ + return utimeval_to_msec(&ut->total_time); +} + +static uint32_t utimer_get_delay(struct utimer *ut) +{ + if (unlikely(!ut)) + return -1; + return utimeval_to_msec(&ut->delay_ms); +} + +static int utimer_set_delay(struct utimer *ut, uint32_t delay_ms) +{ + return utimer_init(ut, delay_ms); +} + +static void utimer_lib_init(void (*func) (int *sec, int *msec)) +{ + current_time = func; +} + +struct yas_cal_data { + uint8_t dx, dy1, dy2; + uint8_t d2, d3, d4, d5, d6, d7, d8, d9, d0; + uint8_t dck; + uint8_t ver; +}; +struct yas_correction_data { + int32_t Cx, Cy1, Cy2; + int32_t a2, a3, a4, a5, a6, a7, a8, a9, k; +}; +struct yas_cdriver { + struct yas_cal_data cal; + struct yas_correction_data correct; + struct yas_machdep_func func; + int8_t transform[9]; + int16_t temperature; + uint8_t dev_id; + int32_t coef; + int16_t center; + int16_t overflow; +}; +static struct yas_cdriver cdriver; + +static int device_open(void) +{ + if (cdriver.func.device_open == NULL) + return -1; + return cdriver.func.device_open(); +} + +static int device_close(void) +{ + if (cdriver.func.device_close == NULL) + return -1; + return cdriver.func.device_close(); +} + +static int device_write(uint8_t addr, const uint8_t *buf, int len) +{ + if (cdriver.func.device_write == NULL) + return -1; + return cdriver.func.device_write(addr, buf, len); +} + +static int device_read(uint8_t addr, uint8_t *buf, int len) +{ + if (cdriver.func.device_read == NULL) + return -1; + return cdriver.func.device_read(addr, buf, len); +} + +static void sleep(int millisec) +{ + if (cdriver.func.msleep == NULL) + return; + cdriver.func.msleep(millisec); +} + +static int init_test_register(void) +{ + uint8_t data; + + data = 0x00; + if (device_write(YAS_REGADDR_TEST1, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + data = 0x00; + if (device_write(YAS_REGADDR_TEST2, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int get_device_id(uint8_t *id) +{ + uint8_t data = 0; + + if (device_read(YAS_REGADDR_DEVICE_ID, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + *id = data; + + return YAS_NO_ERROR; +} + +static int get_cal_data_yas530(struct yas_cal_data *cal) +{ + uint8_t data[16]; +#ifdef YAS_YAS530_CAL_SINGLE_READ + int i; + + for (i = 0; i < 16; i++) { /* dummy read */ + if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + for (i = 0; i < 16; i++) { + if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } +#else + if (device_read(YAS_REGADDR_CAL, data, 16) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (device_read(YAS_REGADDR_CAL, data, 16) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; +#endif + + cal->dx = data[0]; + cal->dy1 = data[1]; + cal->dy2 = data[2]; + cal->d2 = (data[3] >> 2) & 0x03f; + cal->d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); + cal->d4 = data[4] & 0x3f; + cal->d5 = (data[5] >> 2) & 0x3f; + cal->d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); + cal->d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); + cal->d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); + cal->d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); + cal->d0 = (data[9] >> 2) & 0x1f; + cal->dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); + cal->ver = (data[15]) & 0x03; + + return YAS_NO_ERROR; +} + +static void +get_correction_value_yas530(struct yas_cal_data *cal, + struct yas_correction_data *correct) +{ + correct->Cx = cal->dx * 6 - 768; + correct->Cy1 = cal->dy1 * 6 - 768; + correct->Cy2 = cal->dy2 * 6 - 768; + correct->a2 = cal->d2 - 32; + correct->a3 = cal->d3 - 8; + correct->a4 = cal->d4 - 32; + correct->a5 = cal->d5 + 38; + correct->a6 = cal->d6 - 32; + correct->a7 = cal->d7 - 64; + correct->a8 = cal->d8 - 32; + correct->a9 = cal->d9; + correct->k = cal->d0 + 10; +} + +static int get_cal_data_yas532(struct yas_cal_data *cal) +{ + uint8_t data[14]; +#ifdef YAS_YAS530_CAL_SINGLE_READ + int i; + + for (i = 0; i < 14; i++) { /* dummy read */ + if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + for (i = 0; i < 14; i++) { + if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } +#else + if (device_read(YAS_REGADDR_CAL, data, 14) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (device_read(YAS_REGADDR_CAL, data, 14) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; +#endif + + cal->dx = data[0]; + cal->dy1 = data[1]; + cal->dy2 = data[2]; + cal->d2 = (data[3] >> 2) & 0x03f; + cal->d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); + cal->d4 = data[4] & 0x3f; + cal->d5 = (data[5] >> 2) & 0x3f; + cal->d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); + cal->d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); + cal->d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); + cal->d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); + cal->d0 = (data[9] >> 2) & 0x1f; + cal->dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); + cal->ver = (data[13]) & 0x01; + + return YAS_NO_ERROR; +} + +static void +get_correction_value_yas532(struct yas_cal_data *cal, + struct yas_correction_data *correct) +{ + correct->Cx = cal->dx * 10 - 1280; + correct->Cy1 = cal->dy1 * 10 - 1280; + correct->Cy2 = cal->dy2 * 10 - 1280; + correct->a2 = cal->d2 - 32; + correct->a3 = cal->d3 - 8; + correct->a4 = cal->d4 - 32; + correct->a5 = cal->d5 + 38; + correct->a6 = cal->d6 - 32; + correct->a7 = cal->d7 - 64; + correct->a8 = cal->d8 - 32; + correct->a9 = cal->d9; + correct->k = cal->d0; +} + +static int set_configuration(int inton, int inthact, int cck) +{ + uint8_t data = 0; + + data |= (!!inton) & 0x01; + data |= ((!!inthact) << 1) & 0x02; + data |= (cck << 2) & 0x1c; + + if (device_write(YAS_REGADDR_CONFIG, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int get_measure_interval(int32_t *msec) +{ + uint8_t data; + int mult = 7; + + if (device_read(YAS_REGADDR_MEASURE_INTERVAL, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + switch (cdriver.dev_id) { + case YAS_YAS532_DEVICE_ID: + mult = 4; + break; + case YAS_YAS530_DEVICE_ID: + default: + mult = 7; + break; + } + + *msec = data * mult; + + return YAS_NO_ERROR; +} + +static int set_measure_interval(int32_t msec) +{ + uint8_t data = 0; + int mult = 7; + + switch (cdriver.dev_id) { + case YAS_YAS532_DEVICE_ID: + mult = 4; + break; + case YAS_YAS530_DEVICE_ID: + default: + mult = 7; + break; + } + + if (msec > mult*0xff) + data = 0xff; + else + if (msec % mult == 0) + data = (uint8_t)(msec / mult); + else + data = (uint8_t)(msec / mult + 1); + if (device_write(YAS_REGADDR_MEASURE_INTERVAL, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int set_measure_command(int ldtc, int fors, int dlymes) +{ + uint8_t data = 0; + + data |= 0x01; /* bit 0 must be 1 */ + data |= ((!(!ldtc)) << 1) & 0x02; + data |= ((!(!fors)) << 2) & 0x04; + data |= ((!(!dlymes)) << 4) & 0x10; + + if (device_write(YAS_REGADDR_MEASURE_COMMAND, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int +measure_normal_yas530(int *busy, int16_t *t, int16_t *x, int16_t *y1, + int16_t *y2) +{ + uint8_t data[8]; + + if (set_measure_command(0, 0, 0) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + sleep(2); + + if (device_read(YAS_REGADDR_MEASURE_DATA, data, 8) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + *busy = (data[0] >> 7) & 0x01; + *t = (((int32_t) data[0] << 2) & 0x1fc) | ((data[1] >> 6) & 0x03); + *x = (((int32_t) data[2] << 5) & 0xfe0) | ((data[3] >> 3) & 0x1f); + *y1 = (((int32_t) data[4] << 5) & 0xfe0) | ((data[5] >> 3) & 0x1f); + *y2 = (((int32_t) data[6] << 5) & 0xfe0) | ((data[7] >> 3) & 0x1f); + /*YLOGD(("f[%d] t[%d] x[%d] y1[%d] y2[%d]\n", + *busy, *t, *x, *y1, *y2)); */ + + return YAS_NO_ERROR; +} + +static int +measure_normal_yas532(int *busy, int16_t *t, int16_t *x, int16_t *y1, + int16_t *y2) +{ + uint8_t data[8]; + + if (set_measure_command(0, 0, 0) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + sleep(2); + + if (device_read(YAS_REGADDR_MEASURE_DATA, data, 8) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + *busy = (data[0] >> 7) & 0x01; + *t = (((int32_t) data[0] << 3) & 0x3f8) | ((data[1] >> 5) & 0x07); + *x = (((int32_t) data[2] << 6) & 0x1fc0) | ((data[3] >> 2) & 0x3f); + *y1 = (((int32_t) data[4] << 6) & 0x1fc0) | ((data[5] >> 2) & 0x3f); + *y2 = (((int32_t) data[6] << 6) & 0x1fc0) | ((data[7] >> 2) & 0x3f); + /*YLOGD(("f[%d] t[%d] x[%d] y1[%d] y2[%d]\n", + *busy, *t, *x, *y1, *y2)); */ + + return YAS_NO_ERROR; +} + +static int +measure_normal(int *busy, int16_t *t, int16_t *x, int16_t *y1, int16_t *y2) +{ + int result; + + switch (cdriver.dev_id) { + case YAS_YAS532_DEVICE_ID: + result = measure_normal_yas532(busy, t, x, y1, y2); + break; + case YAS_YAS530_DEVICE_ID: + default: + result = measure_normal_yas530(busy, t, x, y1, y2); + break; + } + + return result; +} + +static int +coordinate_conversion(int32_t x, int32_t y1, int32_t y2, int16_t t, + int32_t *xo, int32_t *yo, int32_t *zo, + struct yas_correction_data *c) +{ + int32_t sx, sy1, sy2, sy, sz; + int32_t hx, hy, hz; + + sx = x - (c->Cx * t) / 100; + sy1 = y1 - (c->Cy1 * t) / 100; + sy2 = y2 - (c->Cy2 * t) / 100; + + sy = sy1 - sy2; + sz = -sy1 - sy2; + + hx = c->k * ((100 * sx + c->a2 * sy + c->a3 * sz) / 10); + hy = c->k * ((c->a4 * sx + c->a5 * sy + c->a6 * sz) / 10); + hz = c->k * ((c->a7 * sx + c->a8 * sy + c->a9 * sz) / 10); + + *xo = cdriver.transform[0] * hx + + cdriver.transform[1] * hy + cdriver.transform[2] * hz; + *yo = cdriver.transform[3] * hx + + cdriver.transform[4] * hy + cdriver.transform[5] * hz; + *zo = cdriver.transform[6] * hx + + cdriver.transform[7] * hy + cdriver.transform[8] * hz; + + return YAS_NO_ERROR; +} + +static int +set_hardware_offset(int8_t offset_x, int8_t offset_y1, int8_t offset_y2) +{ + uint8_t data; + + data = offset_x & 0x3f; + if (device_write(YAS_REGADDR_OFFSET_X, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + data = offset_y1 & 0x3f; + if (device_write(YAS_REGADDR_OFFSET_Y1, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + data = offset_y2 & 0x3f; + if (device_write(YAS_REGADDR_OFFSET_Y2, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + return YAS_NO_ERROR; +} + +static int yas_cdrv_actuate_initcoil(void) +{ + uint8_t data = 0; + + if (device_write(YAS_REGADDR_ACTUATE_INIT_COIL, &data, 1) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + return YAS_NO_ERROR; +} + +static int +check_offset(int8_t offset_x, int8_t offset_y1, int8_t offset_y2, + int *flag_x, int *flag_y1, int *flag_y2) +{ + int busy; + int16_t t, x, y1, y2; + + if (set_hardware_offset(offset_x, offset_y1, offset_y2) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (measure_normal(&busy, &t, &x, &y1, &y2) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + *flag_x = *flag_y1 = *flag_y2 = 0; + if (x > cdriver.center) + *flag_x = 1; + if (y1 > cdriver.center) + *flag_y1 = 1; + if (y2 > cdriver.center) + *flag_y2 = 1; + if (x < cdriver.center) + *flag_x = -1; + if (y1 < cdriver.center) + *flag_y1 = -1; + if (y2 < cdriver.center) + *flag_y2 = -1; + + return YAS_NO_ERROR; +} + +static int yas_cdrv_measure_and_set_offset(int8_t *offset) +{ + int i; + int8_t offset_x = 0, offset_y1 = 0, offset_y2 = 0; + int flag_x = 0, flag_y1 = 0, flag_y2 = 0; + static const int correct[5] = { 16, 8, 4, 2, 1 }; + + for (i = 0; i < 5; i++) { + if (check_offset(offset_x, offset_y1, offset_y2, + &flag_x, &flag_y1, &flag_y2) < 0) { + return YAS_ERROR_DEVICE_COMMUNICATION; + } + YLOGD(("offset[%d][%d][%d] flag[%d][%d][%d]\n", + offset_x, offset_y1, offset_y2, + flag_x, flag_y1, flag_y2)); + if (flag_x) + offset_x += flag_x * correct[i]; + if (flag_y1) + offset_y1 += flag_y1 * correct[i]; + if (flag_y2) + offset_y2 += flag_y2 * correct[i]; + } + if (set_hardware_offset(offset_x, offset_y1, offset_y2) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + offset[0] = offset_x; + offset[1] = offset_y1; + offset[2] = offset_y2; + + return YAS_NO_ERROR; +} + +static int yas_cdrv_set_offset(const int8_t *offset) +{ + if (set_hardware_offset(offset[0], offset[1], offset[2]) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + return YAS_NO_ERROR; +} + +static int +yas_cdrv_measure(int32_t *data, int32_t *raw, int16_t *temperature) +{ + int busy; + int16_t x, y1, y2, t; + int32_t xx, yy1, yy2; + int result = 0; + + if (measure_normal(&busy, &t, &x, &y1, &y2) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + if (x == 0) + result |= 0x01; + if (x == cdriver.overflow) + result |= 0x02; + if (y1 == 0) + result |= 0x04; + if (y1 == cdriver.overflow) + result |= 0x08; + if (y2 == 0) + result |= 0x10; + if (y2 == cdriver.overflow) + result |= 0x20; + + xx = x; + yy1 = y1; + yy2 = y2; + + if (coordinate_conversion(xx, yy1, yy2, t, &data[0], &data[1], + &data[2], &cdriver.correct) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + cdriver.temperature = t; + + if (raw != NULL) + raw[0] = xx, raw[1] = yy1, raw[2] = yy2; + if (temperature != NULL) + *temperature = t; + + return result; +} + +static int +yas_cdrv_recalc_calib_offset(int32_t *prev_calib_offset, + int32_t *new_calib_offset, + int8_t *prev_offset, int8_t *new_offset) +{ + int32_t tmp[3], resolution[9], base[3]; + int32_t raw[3]; + int32_t diff, i; + + if (prev_calib_offset == NULL || new_calib_offset == NULL + || prev_offset == NULL || new_offset == NULL) + return YAS_ERROR_ARG; + + raw[0] = raw[1] = raw[2] = 0; + if (coordinate_conversion(raw[0], raw[1], raw[2], cdriver.temperature, + &base[0], &base[1], &base[2], + &cdriver.correct) < 0) + return YAS_ERROR_ERROR; + for (i = 0; i < 3; i++) { + raw[0] = raw[1] = raw[2] = 0; + raw[i] = cdriver.coef; + if (coordinate_conversion(raw[0], raw[1], raw[2], + cdriver.temperature, + &resolution[i * 3 + 0], + &resolution[i * 3 + 1], + &resolution[i * 3 + 2], + &cdriver.correct) < 0) + return YAS_ERROR_ERROR; + resolution[i * 3 + 0] -= base[0]; + resolution[i * 3 + 1] -= base[1]; + resolution[i * 3 + 2] -= base[2]; + } + + for (i = 0; i < 3; i++) + tmp[i] = prev_calib_offset[i]; + + for (i = 0; i < 3; i++) { + diff = (int32_t) new_offset[i] - (int32_t) prev_offset[i]; + while (diff > 0) { + tmp[0] -= resolution[i * 3 + 0]; + tmp[1] -= resolution[i * 3 + 1]; + tmp[2] -= resolution[i * 3 + 2]; + diff--; + } + while (diff < 0) { + tmp[0] += resolution[i * 3 + 0]; + tmp[1] += resolution[i * 3 + 1]; + tmp[2] += resolution[i * 3 + 2]; + diff++; + } + } + for (i = 0; i < 3; i++) + new_calib_offset[i] = tmp[i]; + + return YAS_NO_ERROR; +} + +static int yas_cdrv_set_transformatiom_matrix(const int8_t *transform) +{ + int i; + + if (transform == NULL) + return YAS_ERROR_ARG; + for (i = 0; i < 9; i++) + cdriver.transform[i] = transform[i]; + + return YAS_NO_ERROR; +} + +static int +yas_cdrv_init(const int8_t *transform, struct yas_machdep_func *func) +{ + int interval, i; + uint8_t id; + + if (transform == NULL || func == NULL) + return YAS_ERROR_ARG; + + for (i = 0; i < 9; i++) + cdriver.transform[i] = transform[i]; + + cdriver.func = *func; + + if (device_open() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + if (init_test_register() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + if (get_device_id(&id) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + YLOGD(("device id:%02x\n", id)); + + switch (id) { + case YAS_YAS530_DEVICE_ID: + if (get_cal_data_yas530(&cdriver.cal) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + + get_correction_value_yas530(&cdriver.cal, &cdriver.correct); + cdriver.center = YAS_YAS530_DATA_CENTER; + cdriver.overflow = YAS_YAS530_DATA_OVERFLOW; + switch (cdriver.cal.ver) { + case YAS_YAS530_VERSION_B: + cdriver.coef = YAS_YAS530_VERSION_B_COEF; + break; + case YAS_YAS530_VERSION_A: + default: + cdriver.coef = YAS_YAS530_VERSION_A_COEF; + break; + } + break; + + case YAS_YAS532_DEVICE_ID: + if (get_cal_data_yas532(&cdriver.cal) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + get_correction_value_yas532(&cdriver.cal, &cdriver.correct); + cdriver.center = YAS_YAS532_DATA_CENTER; + cdriver.overflow = YAS_YAS532_DATA_OVERFLOW; + switch (cdriver.cal.ver) { + case YAS_YAS532_VERSION_AC: + cdriver.coef = YAS_YAS532_VERSION_AC_COEF; + break; + case YAS_YAS532_VERSION_AB: + default: + cdriver.coef = YAS_YAS532_VERSION_AB_COEF; + break; + } + break; + + default: + return YAS_ERROR_DEVICE_COMMUNICATION; + } + cdriver.dev_id = id; + + if (set_configuration(0, 0, cdriver.cal.dck) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (set_measure_interval(0) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (get_measure_interval(&interval) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + YLOGD(("interval[%d]\n", interval)); + + return YAS_NO_ERROR; +} + +static int yas_cdrv_term(void) +{ + device_close(); + return YAS_NO_ERROR; +} + +#define YAS_DEFAULT_CALIB_INTERVAL (50) /* 50 msecs */ +#define YAS_DEFAULT_DATA_INTERVAL (200) /* 200 msecs */ +#define YAS_INITCOIL_INTERVAL (200) /* 200 msec */ +#define YAS_INITCOIL_GIVEUP_INTERVAL (180000) /* 180 seconds */ +#define YAS_DETECT_OVERFLOW_INTERVAL (0) /* 0 second */ + +#define YAS_MAG_ERROR_DELAY (200) +#define YAS_MAG_STATE_NORMAL (0) +#define YAS_MAG_STATE_INIT_COIL (1) +#define YAS_MAG_STATE_MEASURE_OFFSET (2) + +static const int8_t YAS_TRANSFORMATION[][9] = { + {0, 1, 0, -1, 0, 0, 0, 0, 1}, + {-1, 0, 0, 0, -1, 0, 0, 0, 1}, + {0, -1, 0, 1, 0, 0, 0, 0, 1}, + {1, 0, 0, 0, 1, 0, 0, 0, 1}, + {0, -1, 0, -1, 0, 0, 0, 0, -1}, + {1, 0, 0, 0, -1, 0, 0, 0, -1}, + {0, 1, 0, 1, 0, 0, 0, 0, -1}, + {-1, 0, 0, 0, 1, 0, 0, 0, -1}, +}; + +static const int supported_data_interval[] = { 10, 20, 50, 60, 100, 200, 1000 }; +static const int supported_calib_interval[] = { 60, 60, 50, 60, 50, 50, 50 }; +static const int32_t INVALID_CALIB_OFFSET[] = { 0x7fffffff, + 0x7fffffff, 0x7fffffff }; +static const int8_t INVALID_OFFSET[] = { 0x7f, 0x7f, 0x7f }; + +struct yas_adaptive_filter { + int num; + int index; + int filter_len; + int filter_noise; + int32_t sequence[YAS_MAG_MAX_FILTER_LEN]; +}; + +struct yas_thresh_filter { + int32_t threshold; + int32_t last; +}; + +struct yas_driver { + int initialized; + struct yas_mag_driver_callback callback; + struct utimer data_timer; + struct utimer initcoil_timer; + struct utimer initcoil_giveup_timer; + struct utimer detect_overflow_timer; + int32_t prev_mag[3]; + int32_t prev_xy1y2[3]; + int32_t prev_mag_w_offset[3]; + int16_t prev_temperature; + int measure_state; + int active; + int overflow; + int initcoil_gaveup; + int position; + int delay_timer_use_data; + int delay_timer_interval; + int delay_timer_counter; + int filter_enable; + int filter_len; + int filter_thresh; + int filter_noise[3]; + struct yas_adaptive_filter adap_filter[3]; + struct yas_thresh_filter thresh_filter[3]; + struct yas_mag_offset offset; +#ifdef YAS_MAG_MANUAL_OFFSET + struct yas_vector manual_offset; +#endif + struct yas_matrix static_matrix; + struct yas_matrix dynamic_matrix; +}; + +static struct yas_driver this_driver; + +static int lock(void) +{ + if (this_driver.callback.lock != NULL) { + if (this_driver.callback.lock() < 0) + return YAS_ERROR_RESTARTSYS; + } + return 0; +} + +static int unlock(void) +{ + if (this_driver.callback.unlock != NULL) { + if (this_driver.callback.unlock() < 0) + return YAS_ERROR_RESTARTSYS; + } + return 0; +} + +static int32_t square(int32_t data) +{ + return data * data; +} + +static void +adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, + int noise) +{ + int i; + + adap_filter->num = 0; + adap_filter->index = 0; + adap_filter->filter_noise = noise; + adap_filter->filter_len = len; + + for (i = 0; i < adap_filter->filter_len; ++i) + adap_filter->sequence[i] = 0; +} + +static int32_t +adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, int32_t in) +{ + int32_t avg, sum; + int i; + + if (adap_filter->filter_len == 0) + return in; + + if (adap_filter->num < adap_filter->filter_len) { + adap_filter->sequence[adap_filter->index++] = in / 100; + adap_filter->num++; + return in; + } + if (adap_filter->filter_len <= adap_filter->index) + adap_filter->index = 0; + + adap_filter->sequence[adap_filter->index++] = in / 100; + + avg = 0; + for (i = 0; i < adap_filter->filter_len; i++) + avg += adap_filter->sequence[i]; + + avg /= adap_filter->filter_len; + + sum = 0; + for (i = 0; i < adap_filter->filter_len; i++) + sum += square(avg - adap_filter->sequence[i]); + + sum /= adap_filter->filter_len; + + if (sum <= adap_filter->filter_noise) + return avg * 100; + + return ((in / 100 - avg) * (sum - adap_filter->filter_noise) / sum + + avg) * 100; +} + +static void +thresh_filter_init(struct yas_thresh_filter *thresh_filter, int threshold) +{ + thresh_filter->threshold = threshold; + thresh_filter->last = 0; +} + +static int32_t +thresh_filter_filter(struct yas_thresh_filter *thresh_filter, int32_t in) +{ + if (in < thresh_filter->last - thresh_filter->threshold + || thresh_filter->last + thresh_filter->threshold < in) { + thresh_filter->last = in; + return in; + } else + return thresh_filter->last; +} + +static void filter_init(struct yas_driver *d) +{ + int i; + + for (i = 0; i < 3; i++) { + adaptive_filter_init(&d->adap_filter[i], d->filter_len, + d->filter_noise[i]); + thresh_filter_init(&d->thresh_filter[i], d->filter_thresh); + } +} + +static void +filter_filter(struct yas_driver *d, int32_t *orig, int32_t *filtered) +{ + int i; + + for (i = 0; i < 3; i++) { + filtered[i] = + adaptive_filter_filter(&d->adap_filter[i], orig[i]); + filtered[i] = + thresh_filter_filter(&d->thresh_filter[i], filtered[i]); + } +} + +static int is_valid_offset(const int8_t *p) +{ + return (p != NULL && (p[0] <= 31) && (p[1] <= 31) && (p[2] <= 31) + && (-31 <= p[0]) && (-31 <= p[1]) && (-31 <= p[2])); +} + +static int is_valid_calib_offset(const int32_t *p) +{ + int i; + for (i = 0; i < 3; i++) { + if (p[i] != INVALID_CALIB_OFFSET[i]) + return 1; + } + return 0; +} + +static int is_offset_differ(const int8_t *p0, const int8_t *p1) +{ + if (p0[0] != p1[0] || p0[1] != p1[1] || p0[2] != p1[2]) + return 1; + else + return 0; +} + +static int is_calib_offset_differ(const int32_t *p0, const int32_t *p1) +{ + if (p0[0] != p1[0] || p0[1] != p1[1] || p0[2] != p1[2]) + return 1; + else + return 0; +} + +static int get_overflow(struct yas_driver *d) +{ + return d->overflow; +} + +static void set_overflow(struct yas_driver *d, const int overflow) +{ + if (d->overflow != overflow) + d->overflow = overflow; +} + +static int get_initcoil_gaveup(struct yas_driver *d) +{ + return d->initcoil_gaveup; +} + +static void set_initcoil_gaveup(struct yas_driver *d, const int initcoil_gaveup) +{ + d->initcoil_gaveup = initcoil_gaveup; +} + +static int32_t *get_calib_offset(struct yas_driver *d) +{ + return d->offset.calib_offset.v; +} + +static void set_calib_offset(struct yas_driver *d, const int32_t *offset) +{ + int i; + + if (is_calib_offset_differ(d->offset.calib_offset.v, offset)) { + for (i = 0; i < 3; i++) + d->offset.calib_offset.v[i] = offset[i]; + } +} + +#ifdef YAS_MAG_MANUAL_OFFSET +static int32_t *get_manual_offset(struct yas_driver *d) +{ + return d->manual_offset.v; +} + +static void set_manual_offset(struct yas_driver *d, const int32_t *offset) +{ + int i; + + for (i = 0; i < 3; i++) + d->manual_offset.v[i] = offset[i]; +} +#endif + +static int32_t *get_static_matrix(struct yas_driver *d) +{ + return d->static_matrix.matrix; +} + +static void set_static_matrix(struct yas_driver *d, const int32_t *matrix) +{ + int i; + + for (i = 0; i < 9; i++) + d->static_matrix.matrix[i] = matrix[i]; +} + +static int32_t *get_dynamic_matrix(struct yas_driver *d) +{ + return d->dynamic_matrix.matrix; +} + +static void set_dynamic_matrix(struct yas_driver *d, const int32_t *matrix) +{ + int i; + + for (i = 0; i < 9; i++) + d->dynamic_matrix.matrix[i] = matrix[i]; +} + +static int8_t *get_offset(struct yas_driver *d) +{ + return d->offset.hard_offset; +} + +static void set_offset(struct yas_driver *d, const int8_t *offset) +{ + int i; + + if (is_offset_differ(d->offset.hard_offset, offset)) { + for (i = 0; i < 3; i++) + d->offset.hard_offset[i] = offset[i]; + } +} + +static int get_active(struct yas_driver *d) +{ + return d->active; +} + +static void set_active(struct yas_driver *d, const int active) +{ + d->active = active; +} + +static int get_position(struct yas_driver *d) +{ + return d->position; +} + +static void set_position(struct yas_driver *d, const int position) +{ + d->position = position; +} + +static int get_measure_state(struct yas_driver *d) +{ + return d->measure_state; +} + +static void set_measure_state(struct yas_driver *d, const int state) +{ + d->measure_state = state; +} + +static struct utimer *get_data_timer(struct yas_driver *d) +{ + return &d->data_timer; +} + +static struct utimer *get_initcoil_timer(struct yas_driver *d) +{ + return &d->initcoil_timer; +} + +static struct utimer *get_initcoil_giveup_timer(struct yas_driver *d) +{ + return &d->initcoil_giveup_timer; +} + +static struct utimer *get_detect_overflow_timer(struct yas_driver *d) +{ + return &d->detect_overflow_timer; +} + +static int get_delay_timer_use_data(struct yas_driver *d) +{ + return d->delay_timer_use_data; +} + +static void set_delay_timer_use_data(struct yas_driver *d, int flag) +{ + d->delay_timer_use_data = !(!flag); +} + +static int get_delay_timer_interval(struct yas_driver *d) +{ + return d->delay_timer_interval; +} + +static void set_delay_timer_interval(struct yas_driver *d, int interval) +{ + d->delay_timer_interval = interval; +} + +static int get_delay_timer_counter(struct yas_driver *d) +{ + return d->delay_timer_counter; +} + +static void set_delay_timer_counter(struct yas_driver *d, int counter) +{ + d->delay_timer_counter = counter; +} + +static int get_filter_enable(struct yas_driver *d) +{ + return d->filter_enable; +} + +static void set_filter_enable(struct yas_driver *d, int enable) +{ + if (enable) + filter_init(d); + + d->filter_enable = !(!enable); +} + +static int get_filter_len(struct yas_driver *d) +{ + return d->filter_len; +} + +static void set_filter_len(struct yas_driver *d, int len) +{ + if (len < 0) + return; + + if (len > YAS_MAG_MAX_FILTER_LEN) + return; + + d->filter_len = len; + filter_init(d); +} + +static int get_filter_noise(struct yas_driver *d, int *noise) +{ + int i; + + for (i = 0; i < 3; i++) + noise[i] = d->filter_noise[i]; + + return 0; +} + +static void set_filter_noise(struct yas_driver *d, int *noise) +{ + int i; + + if (noise == NULL) + return; + + for (i = 0; i < 3; i++) { + if (noise[i] < 0) + noise[i] = 0; + + d->filter_noise[i] = noise[i]; + } + filter_init(d); +} + +static int get_filter_thresh(struct yas_driver *d) +{ + return d->filter_thresh; +} + +static void set_filter_thresh(struct yas_driver *d, int threshold) +{ + if (threshold < 0) + return; + + d->filter_thresh = threshold; + filter_init(d); +} + +static int32_t *get_previous_mag(struct yas_driver *d) +{ + return d->prev_mag; +} + +static void set_previous_mag(struct yas_driver *d, int32_t *data) +{ + int i; + for (i = 0; i < 3; i++) + d->prev_mag[i] = data[i]; +} + +static int32_t *get_previous_xy1y2(struct yas_driver *d) +{ + return d->prev_xy1y2; +} + +static void set_previous_xy1y2(struct yas_driver *d, int32_t *data) +{ + int i; + for (i = 0; i < 3; i++) + d->prev_xy1y2[i] = data[i]; +} + +static int32_t *get_previous_mag_w_offset(struct yas_driver *d) +{ + return d->prev_mag_w_offset; +} + +static void set_previous_mag_w_offset(struct yas_driver *d, int32_t *data) +{ + int i; + for (i = 0; i < 3; i++) + d->prev_mag_w_offset[i] = data[i]; +} + +static int16_t get_previous_temperature(struct yas_driver *d) +{ + return d->prev_temperature; +} + +static void set_previous_temperature(struct yas_driver *d, int16_t temperature) +{ + d->prev_temperature = temperature; +} + +static int init_coil(struct yas_driver *d) +{ + int rt; + + YLOGD(("init_coil IN\n")); + + utimer_update(get_initcoil_timer(d)); + if (!get_initcoil_gaveup(d)) { + utimer_update(get_initcoil_giveup_timer(d)); + if (utimer_is_timeout(get_initcoil_giveup_timer(d))) { + utimer_clear_timeout(get_initcoil_giveup_timer(d)); + set_initcoil_gaveup(d, TRUE); + } + } + if (utimer_is_timeout(get_initcoil_timer(d)) && + !get_initcoil_gaveup(d)) { + utimer_clear_timeout(get_initcoil_timer(d)); + YLOGI(("init_coil!\n")); + rt = yas_cdrv_actuate_initcoil(); + if (rt < 0) { + YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); + return rt; + } + if (get_overflow(d) || !is_valid_offset(get_offset(d))) + set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); + else + set_measure_state(d, YAS_MAG_STATE_NORMAL); + } + + YLOGD(("init_coil OUT\n")); + + return 0; +} + +static int measure_offset(struct yas_driver *d) +{ + int8_t offset[3]; + int32_t moffset[3]; + int rt, result = 0, i; + + YLOGI(("measure_offset IN\n")); + rt = yas_cdrv_measure_and_set_offset(offset); + if (rt < 0) { + YLOGE(("yas_cdrv_measure_offset failed[%d]\n", rt)); + return rt; + } + + YLOGI(("offset[%d][%d][%d]\n", offset[0], offset[1], offset[2])); + + for (i = 0; i < 3; i++) + moffset[i] = get_calib_offset(d)[i]; + + if (is_offset_differ(get_offset(d), offset)) { + if (is_valid_offset(get_offset(d)) + && is_valid_calib_offset(get_calib_offset(d))) { + yas_cdrv_recalc_calib_offset(get_calib_offset(d), + moffset, + get_offset(d), offset); + result |= YAS_REPORT_CALIB_OFFSET_CHANGED; + } + } + result |= YAS_REPORT_HARD_OFFSET_CHANGED; + + set_offset(d, offset); + if (is_valid_calib_offset(moffset)) + set_calib_offset(d, moffset); + + set_measure_state(d, YAS_MAG_STATE_NORMAL); + + YLOGI(("measure_offset OUT\n")); + + return result; +} + +static int +measure_msensor_normal(struct yas_driver *d, int32_t *magnetic, + int32_t *mag_w_offset, int32_t * xy1y2, + int16_t *temperature) +{ + int rt = 0, result, i; + int32_t tmp[3]; + + YLOGD(("measure_msensor_normal IN\n")); + + result = 0; + rt = yas_cdrv_measure(mag_w_offset, tmp, temperature); + if (rt < 0) { + YLOGE(("yas_cdrv_measure failed[%d]\n", rt)); + return rt; + } + for (i = 0; i < 3; i++) + xy1y2[i] = tmp[i]; + +#ifdef YAS_MAG_MANUAL_OFFSET + for (i = 0; i < 3; i++) + mag_w_offset[i] -= get_manual_offset(d)[i]; +#endif + if (rt > 0) { + utimer_update(get_detect_overflow_timer(d)); + set_overflow(d, TRUE); + if (utimer_is_timeout(get_detect_overflow_timer(d))) { + utimer_clear_timeout(get_detect_overflow_timer(d)); + result |= YAS_REPORT_OVERFLOW_OCCURED; + } + if (get_measure_state(d) == YAS_MAG_STATE_NORMAL) + set_measure_state(d, YAS_MAG_STATE_INIT_COIL); + + } else { + utimer_clear(get_detect_overflow_timer(d)); + set_overflow(d, FALSE); + if (get_measure_state(d) == YAS_MAG_STATE_NORMAL) { + utimer_clear(get_initcoil_timer(d)); + utimer_clear(get_initcoil_giveup_timer(d)); + } + } + + for (i = 0; i < 3; i++) { + tmp[i] + = (get_static_matrix(d)[i * 3 + 0] / 10 * + (mag_w_offset[0] / 10)) / 100 + + (get_static_matrix(d)[i * 3 + 1] / 10 * + (mag_w_offset[1] / 10)) / 100 + + (get_static_matrix(d)[i * 3 + 2] / 10 * + (mag_w_offset[2] / 10)) / 100; + } + for (i = 0; i < 3; i++) + magnetic[i] = mag_w_offset[i] = tmp[i]; + if (is_valid_calib_offset(get_calib_offset(d))) { + for (i = 0; i < 3; i++) + magnetic[i] -= get_calib_offset(d)[i]; + } + for (i = 0; i < 3; i++) { + tmp[i] + = (get_dynamic_matrix(d)[i * 3 + 0] / 10 * + (magnetic[0] / 10)) / 100 + + (get_dynamic_matrix(d)[i * 3 + 1] / 10 * + (magnetic[1] / 10)) / 100 + (get_dynamic_matrix(d)[i * 3 + + 2] / + 10 * (magnetic[2] / 10)) / + 100; + } + for (i = 0; i < 3; i++) + magnetic[i] = tmp[i]; + + if (get_filter_enable(d)) + filter_filter(d, magnetic, magnetic); + + YLOGD(("measure_msensor_normal OUT\n")); + + return result; +} + +static int +measure_msensor(struct yas_driver *d, int32_t *magnetic, + int32_t *mag_w_offset, int32_t *xy1y2, int16_t *temperature) +{ + int result, i; + + YLOGD(("measure_msensor IN\n")); + + for (i = 0; i < 3; i++) { + magnetic[i] = get_previous_mag(d)[i]; + mag_w_offset[i] = get_previous_mag_w_offset(d)[i]; + xy1y2[i] = get_previous_xy1y2(d)[i]; + *temperature = get_previous_temperature(d); + } + + result = 0; + switch (get_measure_state(d)) { + case YAS_MAG_STATE_INIT_COIL: + result = init_coil(d); + break; + case YAS_MAG_STATE_MEASURE_OFFSET: + result = measure_offset(d); + break; + case YAS_MAG_STATE_NORMAL: + result = 0; + break; + default: + result = -1; + break; + } + + if (result < 0) + return result; + + if (!(result & YAS_REPORT_OVERFLOW_OCCURED)) + result |= + measure_msensor_normal(d, magnetic, mag_w_offset, xy1y2, + temperature); + set_previous_mag(d, magnetic); + set_previous_xy1y2(d, xy1y2); + set_previous_mag_w_offset(d, mag_w_offset); + set_previous_temperature(d, *temperature); + + YLOGD(("measure_msensor OUT\n")); + + return result; +} + +static int +measure(struct yas_driver *d, int32_t *magnetic, int32_t *mag_w_offset, + int32_t *xy1y2, int16_t *temperature, uint32_t *time_delay) +{ + int result; + int counter; + uint32_t total = 0; + + YLOGD(("measure IN\n")); + + utimer_update(get_data_timer(d)); + result = measure_msensor(d, magnetic, mag_w_offset, xy1y2, + temperature); + if (result < 0) + return result; + + counter = get_delay_timer_counter(d); + total = utimer_get_total_time(get_data_timer(d)); + if (utimer_get_delay(get_data_timer(d)) > 0) + counter -= total / utimer_get_delay(get_data_timer(d)); + else + counter = 0; + + if (utimer_is_timeout(get_data_timer(d))) { + utimer_clear_timeout(get_data_timer(d)); + + if (get_delay_timer_use_data(d)) { + result |= YAS_REPORT_DATA; + if (counter <= 0) + result |= YAS_REPORT_CALIB; + } else { + result |= YAS_REPORT_CALIB; + if (counter <= 0) + result |= YAS_REPORT_DATA; + } + } + + if (counter <= 0) + set_delay_timer_counter(d, get_delay_timer_interval(d)); + else + set_delay_timer_counter(d, counter); + + *time_delay = utimer_sleep_time(get_data_timer(d)); + + YLOGD(("measure OUT [%d]\n", result)); + + return result; +} + +static int resume(struct yas_driver *d) +{ + int32_t zero[] = { 0, 0, 0 }; + struct yas_machdep_func func; + int rt; + + YLOGI(("resume IN\n")); + + func.device_open = d->callback.device_open; + func.device_close = d->callback.device_close; + func.device_write = d->callback.device_write; + func.device_read = d->callback.device_read; + func.msleep = d->callback.msleep; + + rt = + yas_cdrv_init(YAS_TRANSFORMATION[get_position(d)], &func); + + if (rt < 0) { + YLOGE(("yas_cdrv_init failed[%d]\n", rt)); + return rt; + } + + utimer_clear(get_data_timer(d)); + utimer_clear(get_initcoil_giveup_timer(d)); + utimer_clear(get_initcoil_timer(d)); + utimer_clear(get_detect_overflow_timer(d)); + + set_previous_mag(d, zero); + set_previous_xy1y2(d, zero); + set_previous_mag_w_offset(d, zero); + set_previous_temperature(d, 0); + set_overflow(d, FALSE); + set_initcoil_gaveup(d, FALSE); + + filter_init(d); + + if (is_valid_offset(d->offset.hard_offset)) { + yas_cdrv_set_offset(d->offset.hard_offset); + rt = yas_cdrv_actuate_initcoil(); + if (rt < 0) { + YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); + set_measure_state(d, YAS_MAG_STATE_INIT_COIL); + } else + set_measure_state(d, YAS_MAG_STATE_NORMAL); + } else { + rt = yas_cdrv_actuate_initcoil(); + if (rt < 0) { + YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); + set_measure_state(d, YAS_MAG_STATE_INIT_COIL); + } else + set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); + } + + YLOGI(("resume OUT\n")); + return 0; +} + +static int suspend(struct yas_driver *d) +{ + YLOGI(("suspend IN\n")); + + (void)d; + yas_cdrv_term(); + + YLOGI(("suspend OUT\n")); + return 0; +} + +static int check_interval(int ms) +{ + int index = -1; + + if (ms <= supported_data_interval[0]) + ms = supported_data_interval[0]; + for (index = 0; index < NELEMS(supported_data_interval); index++) { + if (ms == supported_data_interval[index]) + goto done; + else if (ms < supported_data_interval[index]) { + if (index != 0) + index -= 1; + goto done; + } + } +done: + return index; +} + +static int yas_get_delay_nolock(struct yas_driver *d, int *ms) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (get_delay_timer_use_data(d)) + *ms = utimer_get_delay(get_data_timer(d)); + else { + *ms = + utimer_get_delay(get_data_timer(d)) * + get_delay_timer_interval(d); + } + return YAS_NO_ERROR; +} + +static int yas_set_delay_nolock(struct yas_driver *d, int ms) +{ + int index; + uint32_t delay_data, delay_calib; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + index = check_interval(ms); + if (index < 0) + return YAS_ERROR_ARG; + delay_data = supported_data_interval[index]; + delay_calib = supported_calib_interval[index]; + set_delay_timer_use_data(d, delay_data < delay_calib); + if (delay_data < delay_calib) { + set_delay_timer_interval(d, delay_calib / delay_data); + set_delay_timer_counter(d, delay_calib / delay_data); + utimer_set_delay(get_data_timer(d), + supported_data_interval[index]); + } else { + set_delay_timer_interval(d, delay_data / delay_calib); + set_delay_timer_counter(d, delay_data / delay_calib); + utimer_set_delay(get_data_timer(d), + supported_calib_interval[index]); + } + + return YAS_NO_ERROR; +} + +static int +yas_get_offset_nolock(struct yas_driver *d, struct yas_mag_offset *offset) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + *offset = d->offset; + return YAS_NO_ERROR; +} + +static int +yas_set_offset_nolock(struct yas_driver *d, struct yas_mag_offset *offset) +{ + int32_t zero[] = { 0, 0, 0 }; + int rt; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (!get_active(d)) { + d->offset = *offset; + return YAS_NO_ERROR; + } + + if (!is_valid_offset(offset->hard_offset) + || is_offset_differ(offset->hard_offset, d->offset.hard_offset)) { + filter_init(d); + + utimer_clear(get_data_timer(d)); + utimer_clear(get_initcoil_giveup_timer(d)); + utimer_clear(get_initcoil_timer(d)); + utimer_clear(get_detect_overflow_timer(d)); + + set_previous_mag(d, zero); + set_previous_xy1y2(d, zero); + set_previous_mag_w_offset(d, zero); + set_previous_temperature(d, 0); + set_overflow(d, FALSE); + set_initcoil_gaveup(d, FALSE); + } + d->offset = *offset; + + if (is_valid_offset(d->offset.hard_offset)) + yas_cdrv_set_offset(d->offset.hard_offset); + else { + rt = yas_cdrv_actuate_initcoil(); + if (rt < 0) { + YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); + set_measure_state(d, YAS_MAG_STATE_INIT_COIL); + } else + set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); + } + + return YAS_NO_ERROR; +} + +#ifdef YAS_MAG_MANUAL_OFFSET + +static int +yas_get_manual_offset_nolock(struct yas_driver *d, struct yas_vector *offset) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + *offset = d->manual_offset; + + return YAS_NO_ERROR; +} + +static int +yas_set_manual_offset_nolock(struct yas_driver *d, struct yas_vector *offset) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + set_manual_offset(d, offset->v); + + return YAS_NO_ERROR; +} + +#endif + +static int +yas_get_static_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) +{ + int i; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + for (i = 0; i < 9; i++) + matrix->matrix[i] = get_static_matrix(d)[i]; + + return YAS_NO_ERROR; +} + +static int +yas_set_static_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + set_static_matrix(d, matrix->matrix); + + return YAS_NO_ERROR; +} + +static int +yas_get_dynamic_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) +{ + int i; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + for (i = 0; i < 9; i++) + matrix->matrix[i] = get_dynamic_matrix(d)[i]; + + return YAS_NO_ERROR; +} + +static int +yas_set_dynamic_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + set_dynamic_matrix(d, matrix->matrix); + + return YAS_NO_ERROR; +} + +static int yas_get_enable_nolock(struct yas_driver *d) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + return get_active(d); +} + +static int yas_set_enable_nolock(struct yas_driver *d, int active) +{ + int rt; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + if (active) { + if (get_active(d)) + return YAS_NO_ERROR; + rt = resume(d); + if (rt < 0) + return rt; + set_active(d, TRUE); + } else { + if (!get_active(d)) + return YAS_NO_ERROR; + rt = suspend(d); + if (rt < 0) + return rt; + set_active(d, FALSE); + } + return YAS_NO_ERROR; +} + +static int +yas_get_filter_nolock(struct yas_driver *d, struct yas_mag_filter *filter) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + filter->len = get_filter_len(d); + get_filter_noise(d, filter->noise); + filter->threshold = get_filter_thresh(d); + return YAS_NO_ERROR; +} + +static int +yas_set_filter_nolock(struct yas_driver *d, struct yas_mag_filter *filter) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + set_filter_len(d, filter->len); + set_filter_noise(d, filter->noise); + set_filter_thresh(d, filter->threshold); + return YAS_NO_ERROR; +} + +static int yas_get_filter_enable_nolock(struct yas_driver *d) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + return get_filter_enable(d); +} + +static int yas_set_filter_enable_nolock(struct yas_driver *d, int enable) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + set_filter_enable(d, enable); + return YAS_NO_ERROR; +} + +static int yas_get_position_nolock(struct yas_driver *d, int *position) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + *position = get_position(d); + return YAS_NO_ERROR; +} + +static int yas_set_position_nolock(struct yas_driver *d, int position) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (get_active(d)) + yas_cdrv_set_transformatiom_matrix(YAS_TRANSFORMATION + [position]); + set_position(d, position); + filter_init(d); + return YAS_NO_ERROR; +} + +static int +yas_read_reg_nolock(struct yas_driver *d, uint8_t addr, uint8_t * buf, int len) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (!get_active(d)) { + if (d->callback.device_open() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + if (d->callback.device_read(addr, buf, len) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (!get_active(d)) { + if (d->callback.device_close() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + + return YAS_NO_ERROR; +} + +static int +yas_write_reg_nolock(struct yas_driver *d, uint8_t addr, const uint8_t * buf, + int len) +{ + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + if (!get_active(d)) { + if (d->callback.device_open() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + if (d->callback.device_write(addr, buf, len) < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + if (!get_active(d)) { + if (d->callback.device_close() < 0) + return YAS_ERROR_DEVICE_COMMUNICATION; + } + + return YAS_NO_ERROR; +} + +static int +yas_measure_nolock(struct yas_driver *d, struct yas_mag_data *data, + int *time_delay_ms) +{ + uint32_t time_delay = YAS_MAG_ERROR_DELAY; + int rt, i; + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + *time_delay_ms = YAS_MAG_ERROR_DELAY; + + if (!get_active(d)) { + for (i = 0; i < 3; i++) { + data->xyz.v[i] = get_previous_mag(d)[i]; + data->raw.v[i] = get_previous_mag_w_offset(d)[i]; + data->xy1y2.v[i] = get_previous_xy1y2(d)[i]; + } + data->temperature = get_previous_temperature(d); + return YAS_NO_ERROR; + } + + rt = measure(d, data->xyz.v, data->raw.v, data->xy1y2.v, + &data->temperature, &time_delay); + if (rt >= 0) { + *time_delay_ms = time_delay; + if (*time_delay_ms > 0) + *time_delay_ms += 1; + } + + return rt; +} + +static int yas_init_nolock(struct yas_driver *d) +{ +#ifdef YAS_MAG_MANUAL_OFFSET + int32_t zero[] = { 0, 0, 0 }; +#endif + int32_t notransform[] = { 10000, 0, 0, 0, 10000, 0, 0, 0, 10000 }; + int noise[] = { + YAS_MAG_DEFAULT_FILTER_NOISE_X, + YAS_MAG_DEFAULT_FILTER_NOISE_Y, + YAS_MAG_DEFAULT_FILTER_NOISE_Z + }; + + YLOGI(("yas_init_nolock IN\n")); + + utimer_lib_init(this_driver.callback.current_time); + utimer_init(get_data_timer(d), 50); + utimer_init(get_initcoil_timer(d), YAS_INITCOIL_INTERVAL); + utimer_init(get_initcoil_giveup_timer(d), YAS_INITCOIL_GIVEUP_INTERVAL); + utimer_init(get_detect_overflow_timer(d), YAS_DETECT_OVERFLOW_INTERVAL); + + set_delay_timer_use_data(d, 0); + set_delay_timer_interval(d, + YAS_DEFAULT_DATA_INTERVAL / + YAS_DEFAULT_CALIB_INTERVAL); + set_delay_timer_counter(d, + YAS_DEFAULT_DATA_INTERVAL / + YAS_DEFAULT_CALIB_INTERVAL); + + set_filter_enable(d, FALSE); + set_filter_len(d, YAS_MAG_DEFAULT_FILTER_LEN); + set_filter_thresh(d, YAS_MAG_DEFAULT_FILTER_THRESH); + set_filter_noise(d, noise); + filter_init(d); + set_calib_offset(d, INVALID_CALIB_OFFSET); +#ifdef YAS_MAG_MANUAL_OFFSET + set_manual_offset(d, zero); +#endif + set_static_matrix(d, notransform); + set_dynamic_matrix(d, notransform); + set_offset(d, INVALID_OFFSET); + set_active(d, FALSE); + set_position(d, 0); + + d->initialized = 1; + + YLOGI(("yas_init_nolock OUT\n")); + + return YAS_NO_ERROR; +} + +static int yas_term_nolock(struct yas_driver *d) +{ + YLOGI(("yas_term_nolock\n")); + + if (unlikely(!d->initialized)) + return YAS_ERROR_NOT_INITIALIZED; + + if (get_active(d)) + suspend(d); + d->initialized = 0; + + YLOGI(("yas_term_nolock out\n")); + return YAS_NO_ERROR; +} + +static int yas_get_delay(void) +{ + int ms = 0, rt; + + YLOGI(("yas_get_delay\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_delay_nolock(&this_driver, &ms); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_delay[%d] OUT\n", ms)); + + return (rt < 0 ? rt : ms); +} + +static int yas_set_delay(int delay) +{ + int rt; + + YLOGI(("yas_set_delay\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_delay_nolock(&this_driver, delay); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_delay OUT\n")); + + return rt; +} + +static int yas_get_offset(struct yas_mag_offset *offset) +{ + int rt; + + YLOGI(("yas_get_offset\n")); + + if (offset == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_offset_nolock(&this_driver, offset); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_offset[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_offset(struct yas_mag_offset *offset) +{ + int rt; + + YLOGI(("yas_set_offset IN\n")); + + if (offset == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_offset_nolock(&this_driver, offset); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_offset OUT\n")); + + return rt; +} + +#ifdef YAS_MAG_MANUAL_OFFSET + +static int yas_get_manual_offset(struct yas_vector *offset) +{ + int rt; + + YLOGI(("yas_get_manual_offset\n")); + + if (offset == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_manual_offset_nolock(&this_driver, offset); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_manual_offset[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_manual_offset(struct yas_vector *offset) +{ + int rt; + + YLOGI(("yas_set_manual_offset IN\n")); + + if (offset == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_manual_offset_nolock(&this_driver, offset); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_manual_offset OUT\n")); + + return rt; +} + +#endif + +static int yas_get_static_matrix(struct yas_matrix *matrix) +{ + int rt; + + YLOGI(("yas_get_static_matrix\n")); + + if (matrix == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_static_matrix_nolock(&this_driver, matrix); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_static_matrix[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_static_matrix(struct yas_matrix *matrix) +{ + int rt; + + YLOGI(("yas_set_static_matrix IN\n")); + + if (matrix == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_static_matrix_nolock(&this_driver, matrix); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_static_matrix OUT\n")); + + return rt; +} + +static int yas_get_dynamic_matrix(struct yas_matrix *matrix) +{ + int rt; + + YLOGI(("yas_get_dynamic_matrix\n")); + + if (matrix == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_dynamic_matrix_nolock(&this_driver, matrix); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_dynamic_matrix[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_dynamic_matrix(struct yas_matrix *matrix) +{ + int rt; + + YLOGI(("yas_set_dynamic_matrix IN\n")); + + if (matrix == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_dynamic_matrix_nolock(&this_driver, matrix); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_dynamic_matrix OUT\n")); + + return rt; +} + +static int yas_get_enable(void) +{ + int rt; + + YLOGI(("yas_get_enable\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_enable_nolock(&this_driver); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_enable OUT[%d]\n", rt)); + + return rt; +} + +static int yas_set_enable(int enable) +{ + int rt; + + YLOGI(("yas_set_enable IN\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_enable_nolock(&this_driver, enable); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_enable OUT\n")); + + return rt; +} + +static int yas_get_filter(struct yas_mag_filter *filter) +{ + int rt; + + YLOGI(("yas_get_filter\n")); + + if (filter == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_filter_nolock(&this_driver, filter); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_filter[%d] OUT\n", rt)); + + return rt; +} + +static int yas_set_filter(struct yas_mag_filter *filter) +{ + int rt, i; + + YLOGI(("yas_set_filter IN\n")); + + if (filter == NULL + || filter->len < 0 + || YAS_MAG_MAX_FILTER_LEN < filter->len || filter->threshold < 0) { + return YAS_ERROR_ARG; + } + for (i = 0; i < 3; i++) { + if (filter->noise[i] < 0) + return YAS_ERROR_ARG; + } + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_filter_nolock(&this_driver, filter); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_filter OUT\n")); + + return rt; +} + +static int yas_get_filter_enable(void) +{ + int rt; + + YLOGI(("yas_get_filter_enable\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_filter_enable_nolock(&this_driver); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_filter_enable OUT[%d]\n", rt)); + + return rt; +} + +static int yas_set_filter_enable(int enable) +{ + int rt; + + YLOGI(("yas_set_filter_enable IN\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_filter_enable_nolock(&this_driver, enable); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_filter_enable OUT\n")); + + return rt; +} + +static int yas_get_position(void) +{ + int position = 0; + int rt; + + YLOGI(("yas_get_position\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_get_position_nolock(&this_driver, &position); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_get_position[%d] OUT\n", position)); + + return (rt < 0 ? rt : position); +} + +static int yas_set_position(int position) +{ + int rt; + + YLOGI(("yas_set_position\n")); + + if (position < 0 || 7 < position) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_set_position_nolock(&this_driver, position); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_set_position[%d] OUT\n", position)); + + return rt; +} + +static int yas_read_reg(uint8_t addr, uint8_t *buf, int len) +{ + int rt; + + YLOGI(("yas_read_reg\n")); + + if (buf == NULL || len <= 0) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_read_reg_nolock(&this_driver, addr, buf, len); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_read_reg[%d] OUT\n", rt)); + + return rt; +} + +static int yas_write_reg(uint8_t addr, const uint8_t *buf, int len) +{ + int rt; + + YLOGI(("yas_write_reg\n")); + + if (buf == NULL || len <= 0) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_write_reg_nolock(&this_driver, addr, buf, len); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGI(("yas_write_reg[%d] OUT\n", rt)); + + return rt; +} + +static int yas_measure(struct yas_mag_data *data, int *time_delay_ms) +{ + int rt; + + YLOGD(("yas_measure IN\n")); + + if (data == NULL || time_delay_ms == NULL) + return YAS_ERROR_ARG; + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_measure_nolock(&this_driver, data, time_delay_ms); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + YLOGD(("yas_measure OUT[%d]\n", rt)); + + return rt; +} + +static int yas_init(void) +{ + int rt; + + YLOGI(("yas_init\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_init_nolock(&this_driver); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + return rt; +} + +static int yas_term(void) +{ + int rt; + YLOGI(("yas_term\n")); + + if (lock() < 0) + return YAS_ERROR_RESTARTSYS; + + rt = yas_term_nolock(&this_driver); + + if (unlock() < 0) + return YAS_ERROR_RESTARTSYS; + + return rt; +} + +int yas_mag_driver_init(struct yas_mag_driver *f) +{ + if (f == NULL) + return YAS_ERROR_ARG; + if (f->callback.device_open == NULL + || f->callback.device_close == NULL + || f->callback.device_read == NULL + || f->callback.device_write == NULL + || f->callback.msleep == NULL || f->callback.current_time == NULL) + return YAS_ERROR_ARG; + + f->init = yas_init; + f->term = yas_term; + f->get_delay = yas_get_delay; + f->set_delay = yas_set_delay; + f->get_offset = yas_get_offset; + f->set_offset = yas_set_offset; +#ifdef YAS_MAG_MANUAL_OFFSET + f->get_manual_offset = yas_get_manual_offset; + f->set_manual_offset = yas_set_manual_offset; +#endif + f->get_static_matrix = yas_get_static_matrix; + f->set_static_matrix = yas_set_static_matrix; + f->get_dynamic_matrix = yas_get_dynamic_matrix; + f->set_dynamic_matrix = yas_set_dynamic_matrix; + f->get_enable = yas_get_enable; + f->set_enable = yas_set_enable; + f->get_filter = yas_get_filter; + f->set_filter = yas_set_filter; + f->get_filter_enable = yas_get_filter_enable; + f->set_filter_enable = yas_set_filter_enable; + f->get_position = yas_get_position; + f->set_position = yas_set_position; + f->read_reg = yas_read_reg; + f->write_reg = yas_write_reg; + f->measure = yas_measure; + + if ((f->callback.lock == NULL && f->callback.unlock != NULL) + || (f->callback.lock != NULL && f->callback.unlock == NULL)) { + this_driver.callback.lock = NULL; + this_driver.callback.unlock = NULL; + } else { + this_driver.callback.lock = f->callback.lock; + this_driver.callback.unlock = f->callback.unlock; + } + this_driver.callback.device_open = f->callback.device_open; + this_driver.callback.device_close = f->callback.device_close; + this_driver.callback.device_write = f->callback.device_write; + this_driver.callback.device_read = f->callback.device_read; + this_driver.callback.msleep = f->callback.msleep; + this_driver.callback.current_time = f->callback.current_time; + yas_term(); + + return YAS_NO_ERROR; +} |