// check - check if M-code is handled here.
// parameters: mcode - M-code to check for (some are predefined in user_mcode_t in grbl/gcode.h), use a cast if not.
// returns:    mcode if handled, UserMCode_Ignore otherwise (UserMCode_Ignore is defined in grbl/gcode.h).
static user_mcode_t check (user_mcode_t mcode)
{
    return mcode == RGB_Inspection_Light 
                     ? mcode                                                            // Handled by us.
                     : (user_mcode.check ? user_mcode.check(mcode) : UserMCode_Ignore); // If another handler present then call it or return ignore.
}

// validate - validate parameters
// parameters: gc_block - pointer to parser_block_t struct (defined in grbl/gcode.h).
//             gc_block->words - holds a bitfield of parameter words available.
//             If float values are NAN (Not A Number) this means they are not available.
//             If integer values has all bits set to 1 this means they are not available.
// returns:    status_code_t enum (defined in grbl/gcode.h): Status_OK if validated ok, appropriate status from enum if not.

static status_code_t validate (parser_block_t *gc_block, parameter_words_t *deprecated)
{
    status_code_t state = Status_GcodeValueWordMissing;

    switch(gc_block->user_mcode) {

        case RGB_Inspection_Light:
            if(gc_block->words.p && !isnan(gc_block->values.p))             // Check if P parameter value is supplied.
                state = Status_BadNumberFormat;                             // Return error if so.

            if(gc_block->words.q && isnan(gc_block->values.q))              // Check if Q parameter value is supplied.
                state = Status_BadNumberFormat;                             // Return error if not.

            if(state != Status_BadNumberFormat && gc_block->words.q) {      // Are required parameters provided?
                if(gc_block->values.q > 0.0f && gc_block->values.q <= 5.0f) // Yes, is Q parameter value in range (1-5)?
                    state = Status_OK;                                      // Yes - return ok status.
                else
                    state = Status_GcodeValueOutOfRange;                    // No - return error status.
                if(gc_block->words.q)                                       // If P parameter is present set
                    gc_block->values.p = 1.0f;                              // value to 1 for execution.
                gc_block->words.p = gc_block->words.q = Off;                // Claim parameters.
                gc_block->user_mcode_sync = true;                           // Optional: execute command synchronized
            }
            break;

        default:
            state = Status_Unhandled;
            break;
    }

    // If not handled by us and another handler present then call it.
    return state == Status_Unhandled && user_mcode.validate ? user_mcode.validate(gc_block, deprecated) : state;
}

// execute - execute M-code
// parameters: state - sys.state (bitmap, defined in system.h)
//             gc_block - pointer to parser_block_t struct (defined in grbl/gcode.h).
// returns:    -
static void execute (sys_state_t state, parser_block_t *gc_block) {

    bool handled = true;

    switch(gc_block->user_mcode) {

        case RGB_Inspection_Light:
            // do something: Q parameter value can be found in gc_block->values.q.
            //               P parameter has its value in gc_block->values.p set to 1 if present, NAN if not.
            if (gc_block->values.q == 1) {
                rgb_set_state(RGB_WHITE);
            }
            else {
                if (gc_block->values.q == 2)
                   rgb_set_state(RGB_OFF);
            }
            break;

        default:
            handled = false;
            break;
    }


    if(!handled && user_mcode.execute)          // If not handled by us and another handler present
        user_mcode.execute(state, gc_block);    // then call it.
}

And I have the relevant sections in my init function:

`void my_plugin_init() // Init function called from drivers_init()
{
    if(hal.port.num_digital_out >= 3) {

        hal.port.num_digital_out -= 3;  // Remove the our outputs from the list of available outputs
        base_port = hal.port.num_digital_out;

        if(hal.port.set_pin_description) {  // Viewable from $PINS command in MDI
            uint32_t idx = 0;
            do {
                hal.port.set_pin_description(true, true, base_port + idx, rgb[idx]);
                if (idx == 0) { red_port = idx; } else
                if (idx == 1) { blue_port = idx; } else
                if (idx == 2) { green_port = idx; }
                idx++;                
            } while(idx <= 2);
        }
        startMS = hal.get_elapsed_ticks();

        // Save away current HAL pointers so that we can use them to keep
        // any chain of M-code handlers intact.
        memcpy(&user_mcode, &hal.user_mcode, sizeof(user_mcode_ptrs_t));

        // Redirect HAL pointers to our code.
        hal.user_mcode.check = check;
        hal.user_mcode.validate = validate;
        hal.user_mcode.execute = execute;
       
        driver_reset = hal.driver_reset;                // Subscribe to driver reset event
        hal.driver_reset = driverReset;

        on_report_options = grbl.on_report_options;     // Subscribe to report options event
        grbl.on_report_options = onReportOptions;

        on_state_change = grbl.on_state_change;         // Subscribe to the state changed event by saving away the original
        grbl.on_state_change = onStateChanged;          // function pointer and adding ours to the chain.

        on_realtime_report = grbl.on_realtime_report;   // Subscribe to realtime report events AKA ? reports
        grbl.on_realtime_report = onRealtimeReport;     

        on_program_completed = grbl.on_program_completed; // Subscribe to on program completed events (lightshow on complete?)
        grbl.on_program_completed = onProgramCompleted;   // Not using this yet, will add back if needed

        on_execute_realtime = grbl.on_execute_realtime;     // Subscribe to the realtime execution event
        grbl.on_execute_realtime = realtimeIndicators;      // Spindle monitoring, flashing LEDs etc live here

    } else
        protocol_enqueue_rt_command(warning_msg);
}`