/*      $NetBSD: common.c,v 1.1 2021/12/07 17:39:55 brad Exp $  */

/*
* Copyright (c) 2021 Brad Spencer <[email protected]>
*
* 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;
}

You are viewing proxied material from ftp.icm.edu.pl. The copyright of proxied material belongs to its original authors. Any comments or complaints in relation to proxied material should be directed to the original authors of the content concerned. Please see the disclaimer for more details.