/* $NetBSD: common.c,v 1.1 2021/12/07 17:39:55 brad Exp $ */ /* * Copyright (c) 2021 Brad Spencer <brad@anduin.eldar.org> * * Permission to use, copy, modify, and distribute this software for any * purpose with or without fee is hereby granted, provided that the above * copyright notice and this permission notice appear in all copies. * * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */ #ifdef __RCSID __RCSID("$NetBSD: common.c,v 1.1 2021/12/07 17:39:55 brad Exp $"); #endif /* Common functions dealing with the SCMD devices. This does not * know how to talk to anything in particular, it calls out to the * functions defined in the function blocks for that. */ #include <inttypes.h> #include <stdbool.h> #include <stdio.h> #include <stdlib.h> #include <unistd.h> #include <err.h> #include <fcntl.h> #include <string.h> #include <limits.h> #include <errno.h> #include <dev/ic/scmdreg.h> #define EXTERN #include "common.h" #include "responses.h" #include "scmdctl.h" int decode_motor_level(int raw) { int r; r = abs(128 - raw); if (raw < 128) r = r * -1; return r; } int common_clear(struct function_block *fb, int fd, bool debug) { return (*(fb->func_clear))(fd, debug); } int common_identify(struct function_block *fb, int fd, bool debug, int a_module, struct scmd_identify_response *r) { uint8_t b; int err; err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_read))(fd, debug, a_module, SCMD_REG_ID, SCMD_REG_ID, &b); if (! err) r->id = b; err = (*(fb->func_phy_read))(fd, debug, a_module, SCMD_REG_FID, SCMD_REG_FID, &b); if (! err) r->fwversion = b; err = (*(fb->func_phy_read))(fd, debug, a_module, SCMD_REG_CONFIG_BITS, SCMD_REG_CONFIG_BITS, &b); if (! err) r->config_bits = b; err = (*(fb->func_phy_read))(fd, debug, a_module, SCMD_REG_SLAVE_ADDR, SCMD_REG_SLAVE_ADDR, &b); if (! err) r->slv_i2c_address = b; } return err; } int common_diag(struct function_block *fb, int fd, bool debug, int a_module, struct scmd_diag_response *r) { uint8_t b; int err, m; err = (*(fb->func_clear))(fd, debug); if (! err) { m = 0; for(uint8_t n = SCMD_REG_U_I2C_RD_ERR; n <= SCMD_REG_GEN_TEST_WORD; n++) { err = (*(fb->func_phy_read))(fd, debug, a_module, n, n, &b); if (! err) { r->diags[m] = b; } m++; } } return err; } /* This tries to avoid reading just every register if only one * motor is asked about. */ int common_get_motor(struct function_block *fb, int fd, bool debug, int a_module, struct scmd_motor_response *r) { uint8_t b; int err = 0,m; if (a_module != SCMD_ANY_MODULE && (a_module < 0 || a_module > 16)) return EINVAL; err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_DRIVER_ENABLE, SCMD_REG_DRIVER_ENABLE, &b); if (! err) r->driver = b; m = 0; for(uint8_t n = SCMD_REG_MA_DRIVE; n <= SCMD_REG_S16B_DRIVE; n++) { r->motorlevels[m] = SCMD_NO_MOTOR; if (a_module != SCMD_ANY_MODULE && (m / 2) != a_module) goto skip; err = (*(fb->func_phy_read))(fd, debug, 0, n, n, &b); if (! err) r->motorlevels[m] = b; skip: m++; } if (a_module == SCMD_ANY_MODULE || a_module == 0) { err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_MOTOR_A_INVERT, SCMD_REG_MOTOR_A_INVERT, &b); if (!err) r->invert[0] = (b & 0x01); err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_MOTOR_B_INVERT, SCMD_REG_MOTOR_B_INVERT, &b); if (!err) r->invert[1] = (b & 0x01); } if (a_module != 0) { m = 2; for(uint8_t n = SCMD_REG_INV_2_9; n <= SCMD_REG_INV_26_33; n++) { err = (*(fb->func_phy_read))(fd, debug, 0, n, n, &b); if (!err) { for(uint8_t j = 0; j < 8;j++) { r->invert[m] = (b & (1 << j)); m++; } } } } if (a_module == SCMD_ANY_MODULE || a_module == 0) { err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_BRIDGE, SCMD_REG_BRIDGE, &b); if (!err) r->bridge[0] = (b & 0x01); } if (a_module != 0) { m = 1; for(uint8_t n = SCMD_REG_BRIDGE_SLV_L; n <= SCMD_REG_BRIDGE_SLV_H; n++) { err = (*(fb->func_phy_read))(fd, debug, 0, n, n, &b); if (!err) { for(uint8_t j = 0; j < 8;j++) { r->bridge[m] = (b & (1 << j)); m++; } } } } } return err; } int common_set_motor(struct function_block *fb, int fd, bool debug, int a_module, char a_motor, int8_t reg_v) { uint8_t reg; int err; int reg_index; if (a_module < 0 || a_module > 16) return EINVAL; if (!(a_motor == 'A' || a_motor == 'B')) return EINVAL; err = (*(fb->func_clear))(fd, debug); if (! err) { reg_index = a_module * 2; if (a_motor == 'B') reg_index++; reg = SCMD_REG_MA_DRIVE + reg_index; reg_v = reg_v + 128; if (debug) fprintf(stderr,"common_set_motor: reg_index: %d ; reg: %02X ; reg_v: %d\n",reg_index,reg,reg_v); err = (*(fb->func_phy_write))(fd, debug, 0, reg, reg_v); } return err; } int common_invert_motor(struct function_block *fb, int fd, bool debug, int a_module, char a_motor) { uint8_t b; int err; uint8_t reg, reg_index = 0, reg_offset = 0; int motor_index; err = (*(fb->func_clear))(fd, debug); if (! err) { if (a_module == 0) { if (a_motor == 'A') { reg = SCMD_REG_MOTOR_A_INVERT; } else { reg = SCMD_REG_MOTOR_B_INVERT; } err = (*(fb->func_phy_read))(fd, debug, 0, reg, reg, &b); if (!err) { b = b ^ 0x01; err = (*(fb->func_phy_write))(fd, debug, 0, reg, b); } } else { motor_index = (a_module * 2) - 2; if (a_motor == 'B') motor_index++; reg_offset = motor_index / 8; motor_index = motor_index % 8; reg_index = 1 << motor_index; reg = SCMD_REG_INV_2_9 + reg_offset; if (debug) fprintf(stderr,"common_invert_motor: remote invert: motor_index: %d ; reg_offset: %d ; reg_index: %02X ; reg: %02X\n",motor_index,reg_offset,reg_index,reg); err = (*(fb->func_phy_read))(fd, debug, 0, reg, reg, &b); if (!err) { b = b ^ reg_index; err = (*(fb->func_phy_write))(fd, debug, 0, reg, b); } } } return err; } int common_bridge_motor(struct function_block *fb, int fd, bool debug, int a_module) { uint8_t b; int err = 0; uint8_t reg, reg_index = 0, reg_offset = 0; int module_index; err = (*(fb->func_clear))(fd, debug); if (! err) { if (a_module == 0) { err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_BRIDGE, SCMD_REG_BRIDGE, &b); if (!err) { b = b ^ 0x01; err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_BRIDGE, b); } } else { module_index = a_module - 1; reg_offset = module_index / 8; module_index = module_index % 8; reg_index = 1 << module_index; reg = SCMD_REG_BRIDGE_SLV_L + reg_offset; if (debug) fprintf(stderr,"common_bridge_motor: remote bridge: module_index: %d ; reg_offset: %d ; reg_index: %02X ; reg: %02X\n",module_index,reg_offset,reg_index,reg); err = (*(fb->func_phy_read))(fd, debug, 0, reg, reg, &b); if (!err) { b = b ^ reg_index; err = (*(fb->func_phy_write))(fd, debug, 0, reg, b); } } } return err; } int common_enable_disable(struct function_block *fb, int fd, bool debug, int subcmd) { int err; uint8_t reg_v; if (!(subcmd == SCMD_ENABLE || subcmd == SCMD_DISABLE)) return EINVAL; err = (*(fb->func_clear))(fd, debug); if (! err) { switch (subcmd) { case SCMD_ENABLE: reg_v = SCMD_DRIVER_ENABLE; break; case SCMD_DISABLE: reg_v = SCMD_DRIVER_DISABLE; break; default: return EINVAL; } err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_DRIVER_ENABLE, reg_v); } return err; } /* These control commands can take a very long time and the restart * make cause the device to become unresponsive for a bit. */ int common_control_1(struct function_block *fb, int fd, bool debug, int subcmd) { int err; uint8_t reg_v; if (!(subcmd == SCMD_RESTART || subcmd == SCMD_ENUMERATE)) return EINVAL; err = (*(fb->func_clear))(fd, debug); if (! err) { switch (subcmd) { case SCMD_RESTART: reg_v = SCMD_CONTROL_1_RESTART; break; case SCMD_ENUMERATE: reg_v = SCMD_CONTROL_1_REENUMERATE; break; default: return EINVAL; } err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_CONTROL_1, reg_v); } return err; } int common_get_update_rate(struct function_block *fb, int fd, bool debug, uint8_t *rate) { uint8_t b; int err; err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_UPDATE_RATE, SCMD_REG_UPDATE_RATE, &b); if (!err) *rate = b; } return err; } int common_set_update_rate(struct function_block *fb, int fd, bool debug, uint8_t rate) { int err; err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_UPDATE_RATE, rate); } return err; } int common_force_update(struct function_block *fb, int fd, bool debug) { int err; err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_FORCE_UPDATE, 0x01); } return err; } int common_get_ebus_speed(struct function_block *fb, int fd, bool debug, uint8_t *speed) { uint8_t b; int err; err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_E_BUS_SPEED, SCMD_REG_E_BUS_SPEED, &b); if (!err) *speed = b; } return err; } int common_set_ebus_speed(struct function_block *fb, int fd, bool debug, uint8_t speed) { int err; if (speed > 0x03) return EINVAL; err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_E_BUS_SPEED, speed); } return err; } int common_get_lock_state(struct function_block *fb, int fd, bool debug, int ltype, uint8_t *lstate) { uint8_t b; uint8_t reg; int err; switch (ltype) { case SCMD_LOCAL_USER_LOCK: reg = SCMD_REG_LOCAL_USER_LOCK; break; case SCMD_LOCAL_MASTER_LOCK: reg = SCMD_REG_LOCAL_MASTER_LOCK; break; case SCMD_GLOBAL_USER_LOCK: reg = SCMD_REG_USER_LOCK; break; case SCMD_GLOBAL_MASTER_LOCK: reg = SCMD_REG_MASTER_LOCK; break; default: return EINVAL; } err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_read))(fd, debug, 0, reg, reg, &b); if (!err) *lstate = b; } return err; } int common_set_lock_state(struct function_block *fb, int fd, bool debug, int ltype, uint8_t lstate) { uint8_t reg; uint8_t state; int err; switch (ltype) { case SCMD_LOCAL_USER_LOCK: reg = SCMD_REG_LOCAL_USER_LOCK; break; case SCMD_LOCAL_MASTER_LOCK: reg = SCMD_REG_LOCAL_MASTER_LOCK; break; case SCMD_GLOBAL_USER_LOCK: reg = SCMD_REG_USER_LOCK; break; case SCMD_GLOBAL_MASTER_LOCK: reg = SCMD_REG_MASTER_LOCK; break; default: return EINVAL; } switch (lstate) { case SCMD_LOCK_LOCKED: state = SCMD_ANY_LOCK_LOCKED; break; case SCMD_LOCK_UNLOCK: state = SCMD_MASTER_LOCK_UNLOCKED; if (ltype == SCMD_LOCAL_USER_LOCK || ltype == SCMD_GLOBAL_USER_LOCK) state = SCMD_USER_LOCK_UNLOCKED; break; default: return EINVAL; } err = (*(fb->func_clear))(fd, debug); if (! err) { err = (*(fb->func_phy_write))(fd, debug, 0, reg, state); } return err; }