For the methodology, see the following article:
Estimating Panel Data Models in the Presence of Endogeneity and Selection (with Jeffrey M. Wooldridge), Journal of Econometrics 157, August 2010, pp. 375-380.
You can find the Stata do-files that were used to implement the parametric estimators from the website below:
http://myweb.fsu.edu/asemykina/two_step_se_parametric.do