Infinite loop in Servo::attach under certain conditions

[LandryNorris]

A dynamically allocated Servo will infinite loop when attached if the program uses a large amount of static memory.

I am using platform.io for the build system with the following platformio.ini file for all snippets below. Each snippet is in main.cpp

; PlatformIO Project Configuration File
;
;   Build options: build flags, source filter
;   Upload options: custom upload port, speed and extra flags
;   Library options: dependencies, extra library storages
;   Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html

[env:pico]
platform = https://github.com/maxgerhardt/platform-raspberrypi.git
board = pico
framework = arduino
board_build.core = earlephilhower
board_build.filesystem_size = 0.5m

monitor_speed = 115200

platform_packages =
    framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master

There are multiple factors at play here, so this issue will unfortunately have to be a wall of text.

Minimum reproducible example

#include "Arduino.h"
#include "Servo.h"

typedef struct {
    //can have more fields or no additional fields. Reproduces either way.
    Servo servo;
} ServoHolder;

ServoHolder* holder; // create servo holder object to hold a servo
// twelve servo objects can be created on most boards

double foo[20000];

void setup() {
    Serial.begin(115200);
    while(!Serial);
    delay(1000);
    Serial.println("Starting");

    holder = static_cast<ServoHolder*>(malloc(sizeof(ServoHolder)));

    Serial.println("Attaching servo");
    Serial.println(sizeof(ServoHolder));
    Serial.println(sizeof(Servo));
    Serial.println((uint32_t) holder);
    delay(1000);
    holder->servo.attach(16);  // attaches the servo on GIO16 to the servo object

    Serial.println("Attached servo");
    holder->servo.writeMicroseconds(1500);
    delay(1000);

    for(int i = 0; i < 10000; i++) {
        Serial.print(foo[i]);
    }
}

void loop() {
    Serial.println("looping");
    int pos;

    for (pos = 1000; pos <= 2000; pos += 10) { // goes from 0 degrees to 180 degrees
        // in steps of 1 degree
        holder->servo.writeMicroseconds(pos);              // tell servo to go to position in variable 'pos'
        delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos = 2000; pos >= 1000; pos -= 10) { // goes from 180 degrees to 0 degrees
        holder->servo.writeMicroseconds(pos);              // tell servo to go to position in variable 'pos'
        delay(15);                       // waits 15ms for the servo to reach the position
    }
}

Actual

With the code above, I get the following output:

Starting
Attaching servo
28
28
537041248

After this, there is no more output (adding code turning on the built-in LED shows that no code following this runs at all).

Expected

When I remove the foo array and comment the line printing items, I get the following output:

Starting
Attaching servo
28
28
536881248
Attached servo
looping
looping
looping

This shows that the allocated memory is large enough to hold a servo object, and that the object is allocated properly.

Global Servo

I also modified the program to use a global servo object as shown below.

#include "Arduino.h"
#include "Servo.h"

typedef struct {
    Servo servo;
} ServoHolder;

ServoHolder* holder; // create servo holder object to hold a servo
// twelve servo objects can be created on most boards

Servo servo;

double foo[20000];

void setup() {
    Serial.begin(115200);
    while(!Serial);
    delay(1000);
    Serial.println("Starting");

    holder = static_cast<ServoHolder*>(malloc(sizeof(ServoHolder)));

    Serial.println("Attaching servo");
    Serial.println(sizeof(ServoHolder));
    Serial.println(sizeof(Servo));
    Serial.println((uint32_t) holder);
    delay(1000);
    servo.attach(16);  // attaches the servo on GIO16 to the servo object

    Serial.println("Attached servo");
    servo.writeMicroseconds(1500);
    delay(1000);

    for(int i = 0; i < 10000; i++) {
        Serial.print(foo[i]);
    }
}

void loop() {
    Serial.println("looping");
    int pos;

    for (pos = 1000; pos <= 2000; pos += 10) { // goes from 0 degrees to 180 degrees
        // in steps of 1 degree
        servo.writeMicroseconds(pos);              // tell servo to go to position in variable 'pos'
        delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos = 2000; pos >= 1000; pos -= 10) { // goes from 180 degrees to 0 degrees
        servo.writeMicroseconds(pos);              // tell servo to go to position in variable 'pos'
        delay(15);                       // waits 15ms for the servo to reach the position
    }
}

The output is as expected:

Starting
Attaching servo
28
28
537041280
Attached servo
0.000.000.000.000.000.000.000.00{many more 0.00s omitted to save space}looping
looping

Static memory allocation

For completeness's sake, I also wrote a version of this using dynamically allocated memory for foo.

#include "Arduino.h"
#include "Servo.h"

typedef struct {
    Servo servo;
} ServoHolder;

ServoHolder* holder; // create servo holder object to hold a servo
// twelve servo objects can be created on most boards

double* foo;

void setup() {
    Serial.begin(115200);
    while(!Serial);
    delay(1000);
    Serial.println("Starting");

    foo = static_cast<double*>(malloc(20000 * sizeof(double)));

    holder = static_cast<ServoHolder*>(malloc(sizeof(ServoHolder)));

    Serial.println("Attaching servo");
    Serial.println(sizeof(ServoHolder));
    Serial.println(sizeof(Servo));
    Serial.println((uint32_t) holder);
    delay(1000);
    holder->servo.attach(16);  // attaches the servo on GIO16 to the servo object

    Serial.println("Attached servo");
    holder->servo.writeMicroseconds(1500);
    delay(1000);

    for(int i = 0; i < 10000; i++) {
        Serial.print(foo[i]);
    }
}

void loop() {
    Serial.println("looping");
    int pos;

    for (pos = 1000; pos <= 2000; pos += 10) { // goes from 0 degrees to 180 degrees
        // in steps of 1 degree
        holder->servo.writeMicroseconds(pos);              // tell servo to go to position in variable 'pos'
        delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos = 2000; pos >= 1000; pos -= 10) { // goes from 180 degrees to 0 degrees
        holder->servo.writeMicroseconds(pos);              // tell servo to go to position in variable 'pos'
        delay(15);                       // waits 15ms for the servo to reach the position
    }
}

The attach call freezes as before:

Starting
Attaching servo
28
28
537041256

[LandryNorris]

I can reproduce this on both of the two picos I currently have access to. Please let me know if any more information is required.

[LandryNorris]

In any of the modified cases, the servo moves as expected. In the unmodified main.cpp, the servo never spins, since it is infinite looping in attach.

[earlephilhower]

I'm traveling so won't be able to look at this for a while, but if you could hook up a Picoprobe and get a dump of the stack traces that would be helpful. At some point you start running into OOM errors just because of fragmentation and other things coming in, especially since there is use of the STL in the core (i.e. std::map makes a tree of alloc'd pointers). ~160KB (the double array) out of ~190KB free (normal range) does seem like there should be plenty of space, but given what you're seeing I'd say you're hitting a fragmentation OOM since it works w/less allocated space...

[LandryNorris]

I just noticed the dynamic allocation test is using the stack-allocated servo. Switched it to the dynamically allocated servo, and it reproduces the issue. I'll look at using the second pico as a picoprobe to get a report.

[LandryNorris]

After debugging, I realized that I'm using malloc, which does not initialize the memory, and won't call C++ constructors. Because of this, _attached is not always initialized to 0, causing it to skip setting up the PIO state machines. The infinite loop appears to happen at the call to pio_sm_clear_fifos on Servo.cpp:154. I guess I'm too used to doing things in a C way, where in an API like this, attach would initialize the fields, or there would be an initialize function for servos that would act like the constructor.

As for why this works if the memory usage is low, it appears that there's a zeroed out region (64 bytes long in my testing) that malloc was allocating from. In this case, _attached is zero as expected, and the _minUs and _maxUs fields get set to avoid extremes.

Given that the issue is from me misusing memory and not an issue in the library, I'm happy to close this issue, but it would be nice if there were some extra protections, or if possible, if there could be a C-like initialization method, or if attach handled this case.

[earlephilhower]

Thanks for digging into this. I believe the proper way to make everything work is to use new (which should work on classes IIRC and call embedded object constructors)

    holder = new ServoHolder();

If that barfs, then making the struct a class would definitely call the proper constructors when new is called.