Previously, only the first out of multiple telnet commands with
TELNETSRV_CMDFLAG_PUSHINTPOOLQ option would get a pointer to the fifo
queue, which is fixed in this commit.
Further, add_telnetcmd() now checks whether TELNET_MAXCMD is reached,
and prints and error.
Reformatted/simplified the function.