cores/rp2040: _freertos.cpp: Avoid memory leak and unnecessary allocation (#948)

There's a memory leak around the corner: creating the mutex means
that we allocate memory for it, but if we cannot find any free slot in the
FMMap array, we're simply returning a nullptr without ever freeing the
previously allocated memory.

Instead of allocating it out of the free-slot check, do it inside.
While at it, also check if the mutex creation succeeded before assigning
it to a FMMap slot.

Author: kholk

cores/rp2040/_freertos.cpp

@@ -38,10 +38,15 @@ extern "C" SemaphoreHandle_t __get_freertos_mutex_for_ptr(mutex_t *m) {
             return _map[i].dst;
         }
     }
-    // Make a new mutex
-    auto fm = __freertos_mutex_create();
+
     for (int i = 0; i < 16; i++) {
         if (_map[i].src == nullptr) {
+            // Make a new mutex
+            SemaphoreHandle_t fm = __freertos_mutex_create();
+            if (fm == nullptr) {
+                return nullptr;
+            }
+
             _map[i].src = m;
             _map[i].dst = fm;
             return fm;