diff --git a/tools/sdbus++ b/tools/sdbus++
index 6606d07..6051046 100755
--- a/tools/sdbus++
+++ b/tools/sdbus++
@@ -6,7 +6,8 @@
 def main():
     valid_types = { 'interface': sdbusplus.Interface }
     valid_processes = { 'markdown' : "markdown",
-                        'server-header' : "server_header" }
+                        'server-header' : "server_header",
+                        'server-cpp' : "server_cpp" }
 
     parser = argparse.ArgumentParser(description='Process sdbus++ YAML files.')
 
diff --git a/tools/sdbusplus/interface.py b/tools/sdbusplus/interface.py
index fa56eb9..7bbae38 100644
--- a/tools/sdbusplus/interface.py
+++ b/tools/sdbusplus/interface.py
@@ -33,3 +33,6 @@
 
     def server_header(self, loader):
         return self.render(loader, "interface.mako.server.hpp", interface=self)
+
+    def server_cpp(self, loader):
+        return self.render(loader, "interface.mako.server.cpp", interface=self)
diff --git a/tools/templates/interface.mako.server.cpp b/tools/templates/interface.mako.server.cpp
new file mode 100644
index 0000000..16f7f43
--- /dev/null
+++ b/tools/templates/interface.mako.server.cpp
@@ -0,0 +1,24 @@
+#include <sdbusplus/bus.hpp>
+#include <sdbusplus/message.hpp>
+#include <${"/".join(interface.name.split('.') + [ 'server.hpp' ])}>
+    <%
+        namespaces = interface.name.split('.')
+        classname = namespaces.pop()
+    %>
+namespace sdbusplus
+{
+namespace server
+{
+    % for s in namespaces:
+namespace ${s}
+{
+    % endfor
+    % for m in interface.methods:
+${ m.cpp_prototype(loader, interface=interface, ptype='callback-cpp') }
+    % endfor
+
+    % for s in namespaces:
+} // namespace ${s}
+    % endfor
+} // namespace server
+} // namespace sdbusplus
diff --git a/tools/templates/method.mako.prototype.hpp b/tools/templates/method.mako.prototype.hpp
index 2718812..e26e726 100644
--- a/tools/templates/method.mako.prototype.hpp
+++ b/tools/templates/method.mako.prototype.hpp
@@ -13,17 +13,30 @@
         return ",\n            ".\
             join([ parameter(p, defaultValue) for p in method.parameters ])
 
+    def parameters_as_local():
+        return "{};\n    ".join([ parameter(p) for p in method.parameters ])
+
+    def parameters_as_list():
+        return ", ".join([ p.camelCase for p in method.parameters ])
+
     def parameter(p, defaultValue=False):
         r = "%s %s" % (p.typeName, p.camelCase)
         if defaultValue:
             r += default_value(p)
         return r
 
+    def returns_as_tuple_index(tuple):
+        return ", ".join([ "std::move(std::get<%d>(%s))" % (i,tuple) \
+                for i in range(len(method.returns))])
+
     def default_value(p):
         if p.defaultValue != None:
             return " = " + str(p.defaultValue)
         else:
             return ""
+
+    def interface_name():
+        return interface.name.split('.').pop()
 %>
 ###
 ### Emit 'header'
@@ -54,4 +67,39 @@
          */
         static int _callback_${ method.CamelCase }(
             sd_bus_message*, void*, sd_bus_error*);
+###
+### Emit 'callback-cpp'
+###
+    % elif ptype == 'callback-cpp':
+int ${interface_name()}::_callback_${ method.CamelCase }(
+        sd_bus_message* msg, void* context, sd_bus_error* error)
+{
+    ### Need to add a ref to msg since we attached it to an sdbusplus::message.
+    auto m = sdbusplus::message::message(sd_bus_message_ref(msg));
+
+    % if len(method.parameters) != 0:
+    ${parameters_as_local()}{};
+
+    m.read(${parameters_as_list()});
+    % endif
+
+    auto o = static_cast<${interface_name()}*>(context);
+    % if len(method.returns) != 0:
+    auto r = \
+    %endif
+    o->${ method.camelCase }(${parameters_as_list()});
+
+    auto reply = m.new_method_return();
+    % if len(method.returns) == 0:
+    // No data to append on reply.
+    % elif len(method.returns) == 1:
+    reply.append(std::move(r));
+    % else:
+    reply.append(${returns_as_tuple_index("r")});
+    % endif
+
+    sdbusplus::bus::method_return(reply);
+
+    return 0;
+}
     % endif