misc.c

/*-----------------------------------------------------------------------------
 *  File: misc.c
 *
 *  This file is part of Sensapex smcp1_proxy codebase.
 *
 *  Created by Veli-Matti Kananen
 *  Copyright (c) 2024 Sensapex Oy. All rights reserved.
 *---------------------------------------------------------------------------*/
#include <stdio.h>
#include <assert.h>
#include <signal.h>
#include "misc.h"
#include "constants.h"
#include "integration.h"
#include "port.h"
#include "functions.h"

// Defines
#define ACTUATOR_STATUS_BUSY 0x00000001

// program closing signal
volatile sig_atomic_t SHOULD_EXIT = 0;

volatile sig_atomic_t PROXY_INIT_COMPLETE = 0;

// Static function prototypes
static mcu_error initActuator(mcu_actuator act);

// Static variables
static hwInfo mcu_hw_info = {0, 0, 0};
static systemInfo mcu_system_info = {0, 0, 7, 0x10, 55555, MCU_COMMON_FEATURES_MASK};
static pthread_mutex_t actMutexBusyIndHandle = PTHREAD_MUTEX_INITIALIZER;
static uint32_t actStatus = 0;
volatile uint32_t gUmxStatus = 0;

const hwInfo *mcu_commonGetHwInfo(bool refreshCache)
{
    if (refreshCache)
    {
        // TODO. Update the cache from ... somewhere.
        mcu_hw_info.hwid = 50;
    }
    return &mcu_hw_info;
}

systemInfo *mcu_commonGetSystemConf(bool refreshCache)
{
    systemInfo *retval = NULL;
    mcu_error status = MCU_ERROR_NONE;

    if (refreshCache)
    {
        // TODO
    }
    if (status == MCU_ERROR_NONE)
    {
        retval = &mcu_system_info;
    }
    return retval;
}

mcu_error mcu_commonSetSystemConf(systemInfo *info, bool writeThrough)
{
    mcu_error retval = MCU_ERROR_NONE;
    mcu_system_info = *info;
    if (writeThrough)
    {
        // TODO
    }
    return retval;
}
bool mcu_pwmIsBusy(const mcu_actuator actuator, int *state)
{
    UNUSED(actuator);
    UNUSED(state);
    return false;
}

bool mcu_commonIsExpired(uint32_t current_ms, uint32_t start_ms,
                         uint32_t period_ms)
{
    return ((current_ms - start_ms) >= period_ms || start_ms > current_ms);
}

mcu_error initSystem()
{
    mcu_error retval = MCU_ERROR_NONE;

    // Init a crash log mutex
    systemInfo *info = mcu_commonGetSystemConf(true);

    // Read mcu system configuration
    if (info->udp_port == MCU_16_BIT_DATA_UNDEFINED)
    {
        // Uninitialized. Set default values and save the configuration.
        info->features = 0;
        info->rd_features = 0;
        info->axis = 0;
        info->smcp_version = 0x10;
        info->udp_port = SMCP1_DEF_UDP_PORT;
        info->features_mask = MCU_COMMON_FEATURES_MASK;
        // Resolve the used head configuration.
        // Store the configuration in EEPROM.
        retval = mcu_commonSetSystemConf(info, true);
    }

    // Initialize connected actuators
    if (retval == MCU_ERROR_NONE)
    {
        int axisMask = 0;
        for (mcu_actuator act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++)
        {
            mcu_error initStatus = initActuator(actuators[act]);
            if (initStatus == MCU_ERROR_NONE)
            {
                axisMask |= (1 << act);
            }
            else
            {
                break;
            }
        }
        info->axis = axisMask;
    }

    // Initialize the supported SMCP version
    if (retval == MCU_ERROR_NONE)
    {
        retval = mcu_smcp10_init();
        assert(retval == MCU_ERROR_NONE);

        /*
        if (retval == MCU_ERROR_NONE) {
            // Set the default hello sender callback
            switch (info->smcp_version)
            {
                case 0x10:
                    heartbeat_fptr = &mcu_smcp10_heartbeat;
                    pos_notify_fptr = &mcu_smcp10_handlePosNotify;
                    break;
                default:
                    heartbeat_fptr = NULL;
                    pos_notify_fptr = NULL;
                    break;
            }
        }
        */
    }

    // Read HW info from EEPROM to cache
    if (retval == MCU_ERROR_NONE)
    {
        const hwInfo *hw_info = mcu_commonGetHwInfo(true);
        info->features_mask = MCU_COMMON_FEATURES_MASK;
    }

    return retval;
}

mcu_error initActuator(mcu_actuator act)
{
    return integrationInitActuator(act);
}

bool mcu_commonIsActuatorEnabled(const mcu_actuator act, const systemInfo *info)
{
    const systemInfo *sysInfoPtr = info ? info : mcu_commonGetSystemConf(false);
    return sysInfoPtr->axis & (1 << act);
}

mcu_error mcu_actuatorGetPosition(const mcu_actuator actuatorId, int *position, bool rawData)
{
    int posValue;
    mcu_error retval = integrationGetPosition(actuatorId, &posValue, rawData);
    if (retval == MCU_ERROR_NONE)
    {
        *position = posValue;
    }
    return retval;
}

static uint32_t mcu_waveformGetStatus()
{
    return gUmxStatus;
}

mcu_error mcu_waveformSetStatus(uint32_t statusBits, bool enable)
{
    mcu_error retval = MCU_ERROR_NONE;

    volatile uint32_t prevStatus = mcu_waveformGetStatus();
    volatile uint32_t newStatus = prevStatus;

    if (enable)
    {
        newStatus |= statusBits;
    }
    else
    {
        newStatus &= ~(statusBits);
    }

    gUmxStatus = newStatus;

    if (prevStatus != newStatus)
    {
        retval = mcu_smcp10BroadcastStatusChangedNotification(newStatus);
    }

    return retval;
}

mcu_error mcu_actuatorSetBusyStatus(const mcu_actuator actuator, const uint32_t status)
{
    mcu_error retval = MCU_ERROR_NONE;

    osStatus mutexState = osMutexWait(&actMutexBusyIndHandle, 10); // max wait time 10 ms
    retval = mutexState == osOK ? retval : MCU_ERROR_GENERAL;
    assert(mutexState == osOK);
    if (retval == MCU_ERROR_NONE)
    {
        if (status == 0)
        {
            actStatus &= ~(1 << actuator);
        }
        else
        {
            actStatus |= (1 << actuator);
        }

        bool busy = actStatus == 0 ? false : true;

        retval = mcu_waveformSetStatus(ACTUATOR_STATUS_BUSY, busy);

        mutexState = osMutexRelease(&actMutexBusyIndHandle);
        assert(mutexState == osOK);
        if (retval == MCU_ERROR_NONE)
        {
            retval = mutexState == osOK ? retval : MCU_ERROR_GENERAL;
        }
    }
    return retval;
}

uint32_t mcu_actuatorGetBusyStatus(const mcu_actuator actuator)
{
    return actStatus & (1 << actuator);
}