Update core_cm3.c from upstream