/* The start and stop message parameters can be specified. This makes it possible to implement every possible project-specific service. */
static int handle_routine_control(struct upgrade_uds_t *self_p, int length)
/* The start and stop message parameters can be specified. This makes it possible to implement every possible project-specific service. */
static int handle_routine_control(struct upgrade_uds_t *self_p, int length)
{
    uint8_t sub_function;
    uint16_t routine_id;
    int res;
    if (length < 3) {
        ignore_and_write_negative_response(self_p,
                                           length,
                                           ROUTINE_CONTROL,
                                           INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
        return (-1);
    }
    chan_read(self_p->chin_p, &sub_function, sizeof(sub_function));
    chan_read(self_p->chin_p, &routine_id, sizeof(routine_id));
    routine_id = ntohs(routine_id);
    length -= 3;
    switch (routine_id) {
    case ROUTINE_ID_ERASE:
        res = handle_routine_control_erase(self_p, sub_function, length);
        break;
    default:
        ignore_and_write_negative_response(self_p,
                                           length,
                                           ROUTINE_CONTROL,
                                           SERVICE_NOT_SUPPORTED);
        res = -1;
        break;
    }
    return (res);
}