diff --git a/src/code.cpp b/src/code.cpp
index a0a14a9..c983fe5 100644
--- a/src/code.cpp
+++ b/src/code.cpp
@@ -258,6 +258,151 @@ static int translateIfGoTo(std::ostringstream &stream, Command cmd) {
   return 0;
 }
 
+static int translateFunctionDefinition(std::ostringstream &stream,
+                                       Command cmd) {
+  stream << "(" << cmd.label << ")" << std::endl;
+
+  for (int i = 0; i < cmd.index; ++i) {
+    stream << "@0" << std::endl;
+    stream << "D=A" << std::endl;
+
+    if (pushFromDRegisterToStack(stream) != 0)
+      return 1;
+  }
+
+  return 0;
+}
+
+int translateFunctionCall(std::ostringstream &stream, Command cmd) {
+  static int callCounter = 0;
+  std::string returnLabel = cmd.label + "$ret" + std::to_string(callCounter++);
+
+  stream << "@" << returnLabel << std::endl;
+  stream << "D=A" << std::endl;
+
+  if (pushFromDRegisterToStack(stream) != 0)
+    return 1;
+
+  stream << "@LCL" << std::endl;
+  stream << "D=M" << std::endl;
+
+  if (pushFromDRegisterToStack(stream) != 0)
+    return 1;
+
+  stream << "@ARG" << std::endl;
+  stream << "D=M" << std::endl;
+
+  if (pushFromDRegisterToStack(stream) != 0)
+    return 1;
+
+  stream << "@THIS" << std::endl;
+  stream << "D=M" << std::endl;
+
+  if (pushFromDRegisterToStack(stream) != 0)
+    return 1;
+
+  stream << "@THAT" << std::endl;
+  stream << "D=M" << std::endl;
+
+  if (pushFromDRegisterToStack(stream) != 0)
+    return 1;
+
+  stream << "@SP" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@5" << std::endl;
+  stream << "D=D-A" << std::endl;
+  stream << "@" << cmd.index << std::endl;
+  stream << "D=D-A" << std::endl;
+  stream << "@ARG" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@SP" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@LCL" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@" << cmd.label << std::endl;
+  stream << "0;JMP" << std::endl;
+  stream << "(" << returnLabel << ")" << std::endl;
+
+  return 0;
+}
+
+static int translateFunctionReturn(std::ostringstream &stream) {
+  stream << "@LCL" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@FRAME" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@5" << std::endl;
+  stream << "D=A" << std::endl;
+  stream << "@FRAME" << std::endl;
+  stream << "D=M-D" << std::endl;
+  stream << "A=D" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@RETURN_ADDRESS" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@ARG" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@ADDR" << std::endl;
+  stream << "M=D" << std::endl;
+
+  if (popFromStackToDRegister(stream) != 0)
+    return 1;
+  
+  stream << "@ADDR" << std::endl;
+  stream << "A=M" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@ARG" << std::endl;
+  stream << "D=M+1" << std::endl;
+  stream << "@SP" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@FRAME" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@1" << std::endl;
+  stream << "D=D-A" << std::endl;
+  stream << "A=D" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@THAT" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@FRAME" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@2" << std::endl;
+  stream << "D=D-A" << std::endl;
+  stream << "A=D" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@THIS" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@FRAME" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@3" << std::endl;
+  stream << "D=D-A" << std::endl;
+  stream << "A=D" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@ARG" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@FRAME" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@4" << std::endl;
+  stream << "D=D-A" << std::endl;
+  stream << "A=D" << std::endl;
+  stream << "D=M" << std::endl;
+  stream << "@LCL" << std::endl;
+  stream << "M=D" << std::endl;
+
+  stream << "@RETURN_ADDRESS" << std::endl;
+  stream << "A=M" << std::endl;
+  stream << "M;JMP" << std::endl;
+
+  return 0;
+}
+
 int translateCommand(std::string &output, const std::string fileName,
                      Command cmd) {
   std::ostringstream stream;
@@ -298,6 +443,15 @@ int translateCommand(std::string &output, const std::string fileName,
   case CommandType::IFGOTO:
     translateIfGoTo(stream, cmd);
     break;
+  case CommandType::FUNCTION:
+    translateFunctionDefinition(stream, cmd);
+    break;
+  case CommandType::CALL:
+    translateFunctionCall(stream, cmd);
+    break;
+  case CommandType::RETURN:
+    translateFunctionReturn(stream);
+    break;
   default:
     break;
   }
diff --git a/src/code.hpp b/src/code.hpp
index 45ff8cb..3c6830d 100644
--- a/src/code.hpp
+++ b/src/code.hpp
@@ -1,3 +1,4 @@
 #include "types.hpp"
 
+int translateFunctionCall(std::ostringstream&, Command);
 int translateCommand(std::string&, const std::string, Command);
diff --git a/src/main.cpp b/src/main.cpp
index 510201a..3a93909 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -3,6 +3,7 @@
 #include <fstream>
 #include <iostream>
 #include <regex>
+#include <sstream>
 #include <string>
 #include <vector>
 
@@ -11,13 +12,43 @@
 #include "utils.hpp"
 
 int process(std::string inputPath, std::string outputPath,
-            std::ios_base::openmode outputStreamMode) {
+            std::ios_base::openmode outputStreamMode, bool &init) {
   std::ifstream ifs(inputPath);
   std::ofstream ofs(outputPath, outputStreamMode);
   std::string fileName = getFileNameFromFilePath(inputPath);
   std::string line;
   std::string output;
 
+  if (init) {
+    std::ostringstream initStream;
+    Command cmd;
+
+    initStream << "// " << "SP = 256" << std::endl;
+    initStream << std::endl;
+
+    initStream << "@256" << std::endl;
+    initStream << "D=A" << std::endl;
+    initStream << "@SP" << std::endl;
+    initStream << "M=D" << std::endl;
+
+    cmd.label = "Sys.init";
+    cmd.commandType = CommandType::CALL;
+    cmd.index = 0;
+
+    initStream << std::endl;
+    initStream << "// " << "call Sys.init 0" << std::endl;
+    initStream << std::endl;
+
+    if (translateFunctionCall(initStream, cmd) != 0)
+      exit(1);
+
+    initStream << std::endl;
+
+    output = initStream.str();
+    ofs << output;
+    init = false;
+  }
+
   std::vector<Command> commands;
 
   while (getline(ifs, line)) {
@@ -32,17 +63,15 @@ int process(std::string inputPath, std::string outputPath,
   }
 
   for (const Command &cmd : commands) {
-    int translateResult;
-
-    if ((translateResult = translateCommand(output, fileName, cmd)) != 0) {
-      return translateResult;
-    }
+    if (translateCommand(output, fileName, cmd) != 0)
+      exit(1);
 
     ofs << output;
   }
 
   ifs.close();
   ofs.close();
+
   return 0;
 }
 
@@ -53,8 +82,10 @@ int main(int argc, char **argv) {
     exit(1);
   }
 
+  bool init = false;
+
   if (regex_match(argv[1], std::regex("^.+\\.vm")))
-    return process(argv[1], getOutputPath(argv[1]), std::ofstream::trunc);
+    return process(argv[1], getOutputPath(argv[1]), std::ofstream::trunc, init);
 
   if (!std::filesystem::is_directory(argv[1])) {
     std::cerr << "Argument is not a VM file!" << std::endl;
@@ -67,9 +98,16 @@ int main(int argc, char **argv) {
     std::filesystem::remove(outputPath);
   }
 
+  for (const auto &entry : std::filesystem::directory_iterator(argv[1])) {
+    if (entry.is_regular_file() && entry.path().filename() == "Sys.vm") {
+      init = true;
+    }
+  }
+
   for (const auto &entry : std::filesystem::directory_iterator(argv[1])) {
     if (entry.is_regular_file() && entry.path().extension() == ".vm") {
-      if (process(entry.path().string(), outputPath, std::ofstream::app) != 0)
+      if (process(entry.path().string(), outputPath, std::ofstream::app,
+                  init) != 0)
         exit(1);
     }
   }
diff --git a/src/parser.cpp b/src/parser.cpp
index c9666ab..e75c162 100644
--- a/src/parser.cpp
+++ b/src/parser.cpp
@@ -15,9 +15,15 @@ int parseCommand(std::vector<Command> &commands, std::string line) {
       std::string segment = matched[2];
       cmd.line = line;
       cmd.commandType = commandTypes.at(matched[1]);
-      cmd.segmentType = segmentTypes.at(segment);
-      cmd.segmentName = segmentNames.at(segment);
       cmd.index = std::stoi(std::string(matched[3]));
+
+      if (cmd.commandType == CommandType::FUNCTION ||
+          cmd.commandType == CommandType::CALL) {
+        cmd.label = matched[2];
+      } else {
+        cmd.segmentType = segmentTypes.at(segment);
+        cmd.segmentName = segmentNames.at(segment);
+      }
     }
 
     commands.push_back(cmd);