diff --git a/src/json_rpc.c b/src/json_rpc.c
index 67c5bb8..a85cb8a 100644
--- a/src/json_rpc.c
+++ b/src/json_rpc.c
@@ -12,7 +12,6 @@
 #include "json_rpc.h"
 
 static void _service_request(json_rpc_t *jrpc);
-static void _generic_error_response(json_rpc_t *jrpc, json_rpc_request_t *req);
 static size_t _read_bytes(int fd, void *buf, size_t sz, int timeout_ms);
 static size_t _read_until(int fd, void *buf, size_t sz, char c, int timeout_ms);
 
@@ -186,8 +185,28 @@ _service_request(json_rpc_t *jrpc)
     }
 
     {
+        cJSON *response = cJSON_CreateObject();
+        cJSON *val = NULL;
+        cJSON *body = NULL;
+        const char *resp_token = "error";
         json_rpc_method_t *method = NULL;
 
+        if (response == NULL) {
+            cJSON_Delete(request.request);
+            return;
+        }
+
+        if (cJSON_IsNumber(request.id)) {
+            val = cJSON_AddNumberToObject(response, "id", request.id->valuedouble);
+        } else {
+            val = cJSON_AddStringToObject(response, "id", request.id->valuestring);
+        }
+        if (val == NULL) {
+            cJSON_Delete(response);
+            cJSON_Delete(request.request);
+            return;
+        }
+
         for (size_t i = 0; i < jrpc->method_cnt; i++) {
             if (!strcmp(jrpc->methods[i].method, request.method->valuestring)) {
                 method = &jrpc->methods[i];
@@ -197,75 +216,64 @@ _service_request(json_rpc_t *jrpc)
 
         if (!method) {
             log_warn("No handler for method \"%s\"\r\n", request.method->valuestring);
-            _generic_error_response(jrpc, &request);
+        } else if (method->callback(&request, &body, method->arg) == 0) {
+            resp_token = "response";
+
+            if (body == NULL) {
+                log_error("No response body given for successful callback\r\n");
+                cJSON_Delete(response);
+                cJSON_Delete(request.request);
+                return;
+            }
+        }
+
+        if (body) {
+            cJSON_AddItemToObject(response, resp_token, body);
         } else {
-            /* TODO */
-        }
-    }
+            /* Generic method not found error */
+            body = cJSON_CreateObject();
+            if (body == NULL) {
+                cJSON_Delete(response);
+                cJSON_Delete(request.request);
+                return;
+            }
 
-    cJSON_Delete(request.request);
-}
+            if ((cJSON_AddNumberToObject(body, "code", -32601) == NULL) ||
+                (cJSON_AddStringToObject(body, "message", "Not implemented.") == NULL)) {
+                cJSON_Delete(body);
+                cJSON_Delete(response);
+                cJSON_Delete(request.request);
+                return;
+            }
 
-static void
-_generic_error_response(json_rpc_t *jrpc, json_rpc_request_t *req)
-{
-    cJSON *resp = cJSON_CreateObject();
-    cJSON *err_obj;
-    cJSON *val;
-
-    if (!resp) {
-        return;
-    }
-
-    if (cJSON_IsNumber(req->id)) {
-        val = cJSON_AddNumberToObject(resp, "id", req->id->valuedouble);
-    } else {
-        val = cJSON_AddStringToObject(resp, "id", req->id->valuestring);
-    }
-    if (!val) {
-        cJSON_Delete(resp);
-        return;
-    }
-
-    err_obj = cJSON_AddObjectToObject(resp, "error");
-    if (!err_obj) {
-        cJSON_Delete(resp);
-        return;
-    }
-
-    val = cJSON_AddNumberToObject(err_obj, "code", -32803);
-    if (!val) {
-        cJSON_Delete(resp);
-        return;
-    }
-    val = cJSON_AddStringToObject(err_obj, "message", "Not implemented");
-    if (!val) {
-        cJSON_Delete(resp);
-        return;
-    }
-
-    {
-        char *resp_str = cJSON_PrintUnformatted(resp);
-        size_t resp_str_len;
-
-        if (!resp_str) {
-            cJSON_Delete(resp);
-            return;
+            cJSON_AddItemToObject(response, resp_token, body);
         }
 
-        resp_str_len = strlen(resp_str);
-        dprintf(jrpc->outfd, "Content-Length: %ld\r\n", resp_str_len);
-        write(jrpc->outfd, resp_str, resp_str_len);
+        {
+            char *resp_str = cJSON_PrintUnformatted(response);
+            size_t resp_str_len;
 
-        free(resp_str);
-        resp_str = cJSON_Print(resp);
-        if (resp_str) {
+            if (resp_str == NULL) {
+                cJSON_Delete(response);
+                cJSON_Delete(request.request);
+                return;
+            }
+
+            resp_str_len = strlen(resp_str);
+            dprintf(jrpc->outfd, "Content-Length: %ld\r\n\r\n", resp_str_len);
+            write(jrpc->outfd, resp_str, strlen(resp_str));
+            free(resp_str);
+
+            resp_str = cJSON_Print(response);
             log_info(">> %s\r\n", resp_str);
             free(resp_str);
         }
+
+        cJSON_Delete(body);
+        cJSON_Delete(response);
     }
 
-    cJSON_Delete(resp);
+    cJSON_Delete(request.request);
 }