pico_cnc.c

/*
  pico_cnc.c - driver code for RP2040 ARM processors

  Part of grblHAL

  Copyright (c) 2021 Terje Io
  
  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  Grbl is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "driver.h"

#if defined(BOARD_PICO_CNC)

#include "hardware/pio.h"

#include "driverPIO.pio.h"
#include "grbl/protocol.h"

static output_sr_t *sr;
static bool state[AUX_N_OUT];

static char *pnum = NULL;
static uint8_t n_in, n_out, *in_map = NULL, *out_map = NULL;
static volatile uint32_t event_bits;
static volatile bool spin_lock = false;
static input_signal_t *aux_in;
static output_signal_t *aux_out;
static char input_ports[50] = "", output_ports[50] = "";

static void aux_settings_load (void);
static bool is_setting_available (const setting_detail_t *setting);
static status_code_t aux_set_invert_out (setting_id_t id, uint_fast16_t int_value);
static uint32_t aux_get_invert_out (setting_id_t setting);
static void digital_out (uint8_t port, bool on);

static const setting_group_detail_t aux_groups[] = {
    { Group_Root, Group_AuxPorts, "Aux ports"}
};

static const setting_detail_t aux_settings[] = {
    { Settings_IoPort_InvertOut, Group_AuxPorts, "Invert I/O Port outputs", NULL, Format_Bitfield, output_ports, NULL, NULL, Setting_NonCoreFn, aux_set_invert_out, aux_get_invert_out },
    { Settings_IoPort_InvertIn, Group_AuxPorts, "Invert I/O Port inputs", NULL, Format_Bitfield, input_ports, NULL, NULL, Setting_NonCore, &settings.ioport.invert_in.mask, NULL, is_setting_available },
};

static setting_details_t setting_details = {
    .groups = aux_groups,
    .n_groups = sizeof(aux_groups) / sizeof(setting_group_detail_t),
    .settings = aux_settings,
    .n_settings = sizeof(aux_settings) / sizeof(setting_detail_t),
    .load = aux_settings_load,
    .save = settings_write_global
};

static bool is_setting_available (const setting_detail_t *setting)
{
    bool available = false;

    switch(setting->id) {

        case Settings_IoPort_InvertIn:
//        case Settings_IoPort_Pullup_Disable:
            available = n_in > 0;
            break;

        default:
            break;
    }

    return available;
}

static void aux_settings_load (void)
{
    uint_fast8_t idx = AUX_N_OUT;

    do {
        idx--;
		digital_out(idx, false);
    } while(idx);
}

static status_code_t aux_set_invert_out (setting_id_t id, uint_fast16_t value)
{
    ioport_bus_t invert;
    invert.mask = (uint8_t)value & AUX_OUT_MASK;

    if(invert.mask != settings.ioport.invert_out.mask) {
        uint_fast8_t idx = AUX_N_OUT;
        do {
            idx--;
            if(((settings.ioport.invert_out.mask >> idx) & 0x01) != ((invert.mask >> idx) & 0x01))
                digital_out(idx, !state[idx]);
        } while(idx);

        settings.ioport.invert_out.mask = invert.mask;
    }

    return Status_OK;
}

static uint32_t aux_get_invert_out (setting_id_t setting)
{
    return settings.ioport.invert_out.mask;
}

static void digital_out (uint8_t port, bool on)
{
    if(port < n_out) {

        if(out_map)
            port = out_map[port];

        on = ((settings.ioport.invert_out.mask >> port) & 0x01) ? !on : on;

        state[port] = on;

        switch(port)
        {
            case 0:
                sr->aux0_out = on;
                break;

            case 1:
                sr->aux1_out = on;
                break;	

            case 2:
                sr->aux2_out = on;
                break;	

            case 3:
                sr->aux3_out = on;
                break;	

            case 4:
                sr->aux4_out = on;
                break;	

            case 5:
                sr->aux5_out = on;
                break;	

            case 6:
                sr->aux6_out = on;
                break;

            case 7:
                sr->aux7_out = on;
                break;
                
            default:
                break;
        }

        out_sr16_write(pio1, 1, sr->value);
    }
}

inline static __attribute__((always_inline)) int32_t get_input (const input_signal_t *input, bool invert, wait_mode_t wait_mode, float timeout)
{
    if(wait_mode == WaitMode_Immediate)
        return DIGITAL_IN(input->bit) ^ invert;

    int32_t value = -1;
    uint_fast16_t delay = (uint_fast16_t)ceilf((1000.0f / 50.0f) * timeout) + 1;

    if(wait_mode == WaitMode_Rise || wait_mode == WaitMode_Fall) {

        pin_irq_mode_t irq_mode = wait_mode == WaitMode_Rise ? IRQ_Mode_Rising : IRQ_Mode_Falling;

        if(input->cap.irq_mode & irq_mode) {

            event_bits &= ~input->bit;
            pinEnableIRQ(input, irq_mode);

            do {
                if(event_bits & input->bit) {
                    value = DIGITAL_IN(input->bit) ^ invert;
                    break;
                }
                if(delay) {
                    protocol_execute_realtime();
                    hal.delay_ms(50, NULL);
                } else
                    break;
            } while(--delay && !sys.abort);

            pinEnableIRQ(input, IRQ_Mode_None);    // Restore pin interrupt status
        }

    } else {

        bool wait_for = wait_mode != WaitMode_Low;

        do {
            if((DIGITAL_IN(input->bit) ^ invert) == wait_for) {
                value = DIGITAL_IN(input->bit);
                break;
            }
            if(delay) {
                protocol_execute_realtime();
                hal.delay_ms(50, NULL);
            } else
                break;
        } while(--delay && !sys.abort);
    }

    return value;
}

inline static __attribute__((always_inline)) uint8_t in_map_rev (uint8_t port)
{
    if(in_map) {
        uint_fast8_t idx = n_in;
        do {
            if(in_map[--idx] == port) {
                port = idx;
                break;
            }
        } while(idx);
    }

    return port;
}

inline static __attribute__((always_inline)) uint8_t out_map_rev (uint8_t port)
{
    if(out_map) {
        uint_fast8_t idx = n_out;
        do {
            if(out_map[--idx] == port) {
                port = idx;
                break;
            }
        } while(idx);
    }

    return port;
}

void ioports_event (input_signal_t *input)
{
    spin_lock = true;
    event_bits |= input->bit;

    if(input->interrupt_callback)
        input->interrupt_callback(in_map_rev(input->id - Input_Aux0), DIGITAL_IN(input->bit));

    spin_lock = false;
}

static int32_t wait_on_input (io_port_type_t type, uint8_t port, wait_mode_t wait_mode, float timeout)
{
    int32_t value = -1;

    if(type == Port_Digital && port < n_in) {
        if(in_map)
            port = in_map[port];
        value = get_input(&aux_in[port], (settings.ioport.invert_in.mask << port) & 0x01, wait_mode, timeout);
    }
//    else if(port == 0)
//        value = analogRead(41);

    return value;
}

static bool register_interrupt_handler (uint8_t port, pin_irq_mode_t irq_mode, ioport_interrupt_callback_ptr interrupt_callback)
{
    bool ok;

    if(in_map)
        port = in_map[port];

    if((ok = port < n_in && aux_in[port].cap.irq_mode != IRQ_Mode_None)) {

        input_signal_t *input = &aux_in[port];

        if(irq_mode != IRQ_Mode_None && (ok = interrupt_callback != NULL)) {
            input->irq_mode = irq_mode;
            input->interrupt_callback = interrupt_callback;
            pinEnableIRQ(input, irq_mode);
        }

        if(irq_mode == IRQ_Mode_None || !ok) {
            while(spin_lock);
            pinEnableIRQ(input, IRQ_Mode_None);
            input->irq_mode = IRQ_Mode_None;
            input->interrupt_callback = NULL;
        }
    }

    return ok;
}

static xbar_t *get_pin_info (io_port_type_t type, io_port_direction_t dir, uint8_t port)
{
    static xbar_t pin;
    xbar_t *info = NULL;

    if(type == Port_Digital) {

        memset(&pin, 0, sizeof(xbar_t));

        if(dir == Port_Input && port < n_in) {
            if(in_map)
                port = in_map[port];
            pin.mode.input = On;
            pin.mode.irq_mode = aux_in[port].irq_mode;
            pin.mode.can_remap = !aux_in[port].cap.remapped;
            pin.cap = aux_in[port].cap;
            pin.function = aux_in[port].id;
            pin.group = aux_in[port].group;
            pin.pin = aux_in[port].pin;
            pin.bit = aux_in[port].bit;
            pin.description = aux_in[port].description;
            info = &pin;
        }

        if(dir == Port_Output && port < n_out) {
            if(out_map)
                port = out_map[port];
            pin.mode = aux_out[port].mode;
            pin.mode.output = On;
            pin.function = aux_out[port].id;
            pin.group = aux_out[port].group;
            pin.pin = aux_out[port].pin;
            pin.bit = 1 << aux_out[port].pin;
 //           pin.port = (void *)aux_out[port].port;
            pin.description = aux_out[port].description;
            info = &pin;
        }
    }

    return info;
}

static void set_pin_description (io_port_type_t type, io_port_direction_t dir, uint8_t port, const char *s)
{
    if(type == Port_Digital) {
        if(dir == Port_Input && port < n_in)
            aux_in[in_map ? in_map[port] : port].description = s;

        if(dir == Port_Output && port < n_out)
            aux_out[out_map ? out_map[port] : port].description = s;
    }
}

static char *get_pnum (uint8_t port)
{
    return pnum ? (pnum + (port * 3) + (port > 9 ? port - 10 : 0)) : NULL;
}

static bool claim (io_port_type_t type, io_port_direction_t dir, uint8_t *port, const char *description)
{
    bool ok = false;

    if(type == Port_Digital) {

        if(dir == Port_Input) {

            if((ok = in_map && *port < n_in && !aux_in[*port].cap.claimed)) {

                uint8_t i;

                hal.port.num_digital_in--;

                for(i = in_map_rev(*port); i < hal.port.num_digital_in ; i++) {
                    in_map[i] = in_map[i + 1];
                    aux_in[in_map[i]].description = get_pnum(i);
                }

                aux_in[*port].cap.claimed = On;
                aux_in[*port].description = description;

                in_map[hal.port.num_digital_in] = *port;
                *port = hal.port.num_digital_in;
            }

        } else if((ok = out_map && *port < n_out && !aux_out[*port].mode.claimed)) {

            uint8_t i;

            hal.port.num_digital_out--;

            for(i = out_map_rev(*port); i < hal.port.num_digital_out; i++) {
                out_map[i] = out_map[i + 1];
                aux_out[out_map[i]].description = get_pnum(i);
            }

            aux_out[*port].mode.claimed = On;
            aux_out[*port].description = description;

            out_map[hal.port.num_digital_out] = *port;
            *port = hal.port.num_digital_out;
        }
    }

    return ok;
}

bool swap_pins (io_port_type_t type, io_port_direction_t dir, uint8_t port_a, uint8_t port_b)
{
    bool ok = port_a == port_b;

    if(!ok && type == Port_Digital) {

        if((ok = dir == Port_Input && port_a < n_in && port_b < n_in &&
                   aux_in[port_a].interrupt_callback == NULL &&
                    aux_in[port_b].interrupt_callback == NULL)) {

            input_signal_t tmp;

            memcpy(&tmp, &aux_in[port_a], sizeof(input_signal_t));
            memcpy(&aux_in[port_a], &aux_in[port_b], sizeof(input_signal_t));
            aux_in[port_a].description = tmp.description;
            tmp.description = aux_in[port_b].description;
            memcpy(&aux_in[port_b], &tmp, sizeof(input_signal_t));
        }

        if((ok = dir == Port_Output && port_a < n_out && port_b < n_out)) {

            output_signal_t tmp;

            memcpy(&tmp, &aux_out[port_a], sizeof(output_signal_t));
            memcpy(&aux_out[port_a], &aux_out[port_b], sizeof(output_signal_t));
            aux_out[port_a].description = tmp.description;
            tmp.description = aux_out[port_b].description;
            memcpy(&aux_out[port_b], &tmp, sizeof(output_signal_t));
        }
    }

    return ok;
}

void board_init (pin_group_pins_t *aux_inputs, pin_group_pins_t *aux_outputs, output_sr_t *reg)
{
    uint_fast8_t i, ports;

    sr = reg;
    aux_in = aux_inputs->pins.inputs;
    aux_out = aux_outputs->pins.outputs;

    if((hal.port.num_digital_in = n_in = aux_inputs->n_pins)) {
        hal.port.wait_on_input = wait_on_input;
        hal.port.register_interrupt_handler = register_interrupt_handler;
        in_map = malloc(n_in * sizeof(uint8_t));
    }

    if((hal.port.num_digital_out = n_out = aux_outputs->n_pins)) {
        hal.port.digital_out = digital_out;
        out_map = malloc(n_out * sizeof(uint8_t));
    }

    if((ports = max(n_in, n_out)) > 0)  {

        char *pn;

        hal.port.claim = claim;
        hal.port.swap_pins = swap_pins;
        hal.port.get_pin_info = get_pin_info;
        hal.port.set_pin_description = set_pin_description;

        settings_register(&setting_details);

        // Add M62-M65 port number mappings (P<n>) to description
        pnum = pn = malloc((3 * ports + (ports > 9 ? ports - 10 : 0)) + 1);

        for(i = 0; i < ports; i++) {

            if(pn) {
                *pn = 'P';
                strcpy(pn + 1, uitoa(i));
            }

            if(hal.port.num_digital_in && i < hal.port.num_digital_in) {
                if(in_map)
                    in_map[i] = i;
                if(pn)
                    aux_in[i].description = pn;
            }

            if(hal.port.num_digital_out && i < hal.port.num_digital_out) {
                if(out_map)
                    out_map[i] = i;
                if(pn)
                    aux_out[i].description = pn;
            }

            if(pn)
                pn += i > 9 ? 4 : 3;
        }

        // Add port names for ports up to 8 for $-setting flags
        for(i = 0; i < min(hal.port.num_digital_in, 8); i++) {
            strcat(input_ports, i == 0 ? "Aux " : ",Aux ");
            strcat(input_ports, uitoa(i));
        }

        for(i = 0; i < min(hal.port.num_digital_out, 8) ; i++) {
         //   out.mask = (out.mask << 1) + 1;
            strcat(output_ports, i == 0 ? "Aux " : ",Aux ");
            strcat(output_ports, uitoa(i));
        }
    }
}

#endif