/*      $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;
}