In this paper, we consider a planning problem for a hierarchical finite state machine (HFSM) and develop an algorithm for efficiently computing optimal plans between any two states. The algorithm consists of an offline and an online step. In the offline step, one computes exit costs for each machine in the HFSM. It needs to be done only once for a given HFSM, and it is shown to have time complexity scaling linearly with the number of machines in the HFSM. In the online step, one computes an optimal plan from an initial state to a goal state, by first reducing the HFSM (using the exit costs), computing an optimal trajectory for the reduced HFSM, and then expand this trajectory to an optimal plan for the original HFSM. The time complexity is near-linearly with the depth of the HFSM. It is argued that HFSMs arise naturally for large-scale control systems, exemplified by an application where a robot moves between houses to complete tasks. We compare our algorithm with Dijkstra's algorithm on HFSMs consisting of up to 2 million states, where our algorithm outperforms the latter, being several orders of magnitude faster.