Merge with Stefan Roese's branch with CFI driver fixes