503 lines
11 KiB
C
503 lines
11 KiB
C
/* $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;
|
|
}
|