For help in writing a plugin see the GStreamer website. To perform the actual focusing commands you'd ideally use the GStreamer photography interface, then it's just a case of writing the contrast optimisation code to drive the focus back and forward and find the best position.