The supported way to add custom dialplan like this is to use a dialplan hook: Hooking for fun and income
But even better, if these are calls arriving on a specific trunk that need this treatment, then you should have a special context for the trunk.
You can test for the value of CALLERID(number) before setting with a line like this:
SetIf($["CALLERID(number)"=""]?CALLERID(number)=${SIP_HEADER(cid)})
lr;dt = looks right; didn’t test