text: #include <string.h>
  #include <crc/crc32.h>
  #include <cmp_mem_access/cmp_mem_access.h>
  #include <platform.h>
  #include "flash_writer.h"
  #include "boot_arg.h"
  #include "config.h"
  #include "command.h"
  
  void command_erase_flash_page(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      void* address;
      uint64_t tmp = 0;
      char device_class[64];
  
      cmp_read_uinteger(args, &tmp);
      address = (void*)(uintptr_t)tmp;
  
      // refuse to overwrite bootloader or config pages
      if (address < memory_get_app_addr()) {
          goto command_fail;
      }
  
      // Refuse to erase past end of flash memory
      if (address >= memory_get_app_addr() + memory_get_app_size()) {
          goto command_fail;
      }
  
      uint32_t size = 64;
      cmp_read_str(args, device_class, &size);
  
      if (strcmp(device_class, config->device_class) != 0) {
          goto command_fail;
      }
  
      flash_writer_unlock();
  
      flash_writer_page_erase(address);
  
      flash_writer_lock();
  
      cmp_write_bool(out, 1);
      return;
  
  command_fail:
      cmp_write_bool(out, 0);
      return;
  }
  
  void command_write_flash(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      void* address;
      void* src;
      uint64_t tmp = 0;
      uint32_t size;
      char device_class[64];
  
      cmp_read_uinteger(args, &tmp);
      address = (void*)(uintptr_t)tmp;
  
      // refuse to overwrite bootloader or config pages
      if (address < memory_get_app_addr()) {
          goto command_fail;
      }
  
      // Refuse to erase past end of flash memory
      if (address >= memory_get_app_addr() + memory_get_app_size()) {
          goto command_fail;
      }
  
      size = 64;
      cmp_read_str(args, device_class, &size);
  
      if (strcmp(device_class, config->device_class) != 0) {
          goto command_fail;
      }
  
      if (!cmp_read_bin_size(args, &size)) {
          goto command_fail;
      }
  
      /* This is ugly, yet required to achieve zero copy. */
      cmp_mem_access_t* cma = (cmp_mem_access_t*)(args->buf);
      src = cmp_mem_access_get_ptr_at_pos(cma, cmp_mem_access_get_pos(cma));
  
      flash_writer_unlock();
  
      flash_writer_page_write(address, src, size);
  
      flash_writer_lock();
  
      cmp_write_bool(out, 1);
      return;
  
  command_fail:
      cmp_write_bool(out, 0);
      return;
  }
  
  void command_read_flash(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      void* address;
      uint64_t tmp;
      uint32_t size;
  
      cmp_read_uinteger(args, &tmp);
      address = (void*)(uintptr_t)tmp;
  
      cmp_read_u32(args, &size);
  
      cmp_write_bin(out, address, size);
  }
  
  void command_jump_to_application(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      if (crc32(0, memory_get_app_addr(), config->application_size) == config->application_crc) {
          reboot_system(BOOT_ARG_START_APPLICATION);
      } else {
          reboot_system(BOOT_ARG_START_BOOTLOADER_NO_TIMEOUT);
      }
  }
  
  void command_crc_region(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      uint32_t crc;
      void* address;
      uint32_t size;
      uint64_t tmp;
  
      cmp_read_uinteger(args, &tmp);
      address = (void*)(uintptr_t)tmp;
      cmp_read_uint(args, &size);
  
      crc = crc32(0, address, size);
      cmp_write_uint(out, crc);
  }
  
  void command_config_update(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      config_update_from_serialized(config, args);
      cmp_write_bool(out, 1);
  }
  
  void command_ping(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      cmp_write_bool(out, 1);
  }
  
  static bool flash_write_and_verify(void* addr, void* data, size_t len)
  {
      flash_writer_unlock();
      flash_writer_page_erase(addr);
      flash_writer_page_write(addr, data, len);
      flash_writer_lock();
      return config_is_valid(addr, len);
  }
  
  void command_config_write_to_flash(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      config->update_count += 1;
  
      memset(config_page_buffer, 0, CONFIG_PAGE_SIZE);
  
      config_write(config_page_buffer, config, CONFIG_PAGE_SIZE);
  
      void* config1 = memory_get_config1_addr();
      void* config2 = memory_get_config2_addr();
  
      bool success = false;
      if (config_is_valid(config2, CONFIG_PAGE_SIZE)) {
          if (flash_write_and_verify(config1, config_page_buffer, CONFIG_PAGE_SIZE)) {
              if (flash_write_and_verify(config2, config_page_buffer, CONFIG_PAGE_SIZE)) {
                  success = true;
              }
          }
      } else if (config_is_valid(config1, CONFIG_PAGE_SIZE)) {
          if (flash_write_and_verify(config2, config_page_buffer, CONFIG_PAGE_SIZE)) {
              if (flash_write_and_verify(config1, config_page_buffer, CONFIG_PAGE_SIZE)) {
                  success = true;
              }
          }
      } else {
          success = flash_write_and_verify(config1, config_page_buffer, CONFIG_PAGE_SIZE);
          success &= flash_write_and_verify(config2, config_page_buffer, CONFIG_PAGE_SIZE);
      }
  
      if (success) {
          cmp_write_bool(out, 1);
      } else {
          cmp_write_bool(out, 0);
      }
  }
  
  void command_config_read(int argc, cmp_ctx_t* args, cmp_ctx_t* out, bootloader_config_t* config)
  {
      config_write_messagepack(out, config);
  }
  
  int protocol_execute_command(char* data, size_t data_len, const command_t* commands, int command_len, char* out_buf, size_t out_len, bootloader_config_t* config)
  {
      cmp_mem_access_t command_cma;
      cmp_ctx_t command_reader;
      int32_t commmand_index, i;
      uint32_t argc;
      int32_t command_version;
      bool read_success;
  
      cmp_mem_access_t out_cma;
      cmp_ctx_t out_writer;
  
      cmp_mem_access_ro_init(&command_reader, &command_cma, data, data_len);
  
      cmp_mem_access_init(&out_writer, &out_cma, out_buf, out_len);
  
      cmp_read_int(&command_reader, &command_version);
  
      if (command_version != COMMAND_SET_VERSION) {
          return -ERR_INVALID_COMMAND_SET_VERSION;
      }
  
      read_success = cmp_read_int(&command_reader, &commmand_index);
  
      if (!read_success) {
          return -ERR_INVALID_COMMAND;
      }
  
      read_success = cmp_read_array(&command_reader, &argc);
  
      /* If we cannot read array size, assume it is because we don't have
       * arguments. */
      if (!read_success) {
          argc = 0;
      }
  
      for (i = 0; i < command_len; ++i) {
          if (commands[i].index == commmand_index) {
              commands[i].callback(argc, &command_reader, &out_writer, config);
              return cmp_mem_access_get_pos(&out_cma);
          }
      }
  
      return -ERR_COMMAND_NOT_FOUND;
  }
  