csd_bs_list_remove: fix removal logic

Related: OS#4394
Change-Id: I638d4e063fee6bad45ab14d8ad6b9ad847a7127a
diff --git a/src/libmsc/csd_bs.c b/src/libmsc/csd_bs.c
index 2322f20..967bd6d 100644
--- a/src/libmsc/csd_bs.c
+++ b/src/libmsc/csd_bs.c
@@ -340,14 +340,14 @@
 	bool found = false;
 
 	for (i = 0; i < list->count; i++) {
-		if (list->bs[i] == bs) {
+		if (list->bs[i] == bs)
 			found = true;
-			list->count--;
-			continue;
-		}
-		if (i && found)
-			list->bs[i-1] = list->bs[i];
+		if (found && i + 1 < list->count)
+			list->bs[i] = list->bs[i + 1];
 	}
+
+	if (found)
+		list->count--;
 }
 
 void csd_bs_list_intersection(struct csd_bs_list *dest, const struct csd_bs_list *other)
diff --git a/tests/csd/csd_test.ok b/tests/csd/csd_test.ok
index 8ba59b3..0beac46 100644
--- a/tests/csd/csd_test.ok
+++ b/tests/csd/csd_test.ok
@@ -4,8 +4,8 @@
 removing BS25T (not in the list):
   BS21T,BS22T,BS24T
 removing BS22T:
-  BS21T,BS22T
+  BS21T,BS24T
 removing BS24T:
-  BS21T,BS22T
-removing BS21T:
   BS21T
+removing BS21T:
+  (no-bearer-services)