void csp_i2c_rx(i2c_frame_t *frame,void *pxTaskWoken) {
  // ...  
  if (frame) {
    frame_len = frame->len - 4;
    if (frame_len > 0xfc) {
      csp_if_i2c.frame = csp_if_i2c.frame + 1;
      csp_buffer_free_isr(frame);
      return;
    }
    frame->len = frame_len;
    i2c_rx_csp_packet = (csp_packet_t *) frame;
    h32 = csp_ntoh32(frame->data[3] | frame->data[1] << 0x10 | 
                        frame->data[0] << 0x18 | frame->data[2] << 8);
    frame->data[3] = (uint8_t)h32;
    frame->data[0] = (uint8_t)(h32 >> 0x18);
    frame->data[1] = (uint8_t)(h32 >> 0x10);
    frame->data[2] = (uint8_t)(h32 >> 8);
    csp_qfifo_write(i2c_rx_csp_packet, &csp_if_i2c, pxTaskWoken);
  }
  return;
}

if (cspServerInitialised == false) {
  cspSocket = csp_socket(0);
  if (!cspSocket) { return; }
  
  ret = csp_bind(cspSocket, CSP_ANY_PORT);
  if (!ret) { return; }
  
  ret = csp_listen(cspSocket, 10);
  if (!ret) { return; }
  
  cspServerInitialised = true;
}

cspServerConn = csp_accept(cspSocket, 10);
if (cspServerConn) {
  while (request_packet = csp_read(cspServerConn,0), packet) {
    dest_port = csp_conn_dport(cspServerConn);
    switch(dest_port) {
      // ...
    }
  }
  csp_close(cspServerConn);
}

switch(csp_conn_dport(conn)) {
	case 0: // Network information handlers
    	csp_cmp_handler(conn, packet);
        break;
    case 1: // Ping
        do_csp_debug(2,"SERVICE: Ping received");
        break;
    case 2: // OS Tasklist
        csp_sys_tasklist(str, size);
        // ...
        csp_send(conn, packet, 0);
        break;
    case 3: // Remaining Memory
        val = csp_sys_memfree();
        // ...
        csp_send(conn, packet, 0);
        break;
    case 4: // System Reboot
    	if(packet->data[0..4] == BYTESEQ) { csp_sys_reboot(); }
   // ...
}

dest_port = csp_conn_dport(conn);
switch(dest_port) {
  case 0x00 - 0x06:
    csp_service_handler(conn, pkt);
  case 0x07:
    rparam_service_handler(conn, pkt);
  case 0x10:
    CSP_ProcessReceivedSPP(pkt);
}

// csp_listen, _bind(0x14), _accept
switch(val) {
  case 0x1: // Set ADCS Mode
    memcpy(packet->data + 2, _adcs_mode, 7);
    packet->data[1] = '\0';
    packet->length = 0;
    goto send_packet_set_len;
  case 0x1c:
    gs_adcs_gps_on();
    break;
  case '\x14': // Set ADCS Wheel position
    gs_adcs_wheels_diag(packet->data[2],&val0,&val1);
    packet->data[1] = '\0';
    h16 = util_hton16(val0);
    packet->data[5] = (char)(h16 & 0xffff);
    packet->data[4] = (char)((h16 & 0xffff) >> 8);
    h16 = util_hton16(val1);
    packet->data[7] = (char)(h16 & 0xffff);
    packet->data[6] = (char)((h16 & 0xffff) >> 8);

    packet->length = 0;
    goto send_packet_set_len;
}

send_packet_set_len:
        *(char *)((int)&packet->length + 1) = len;
send_packet:
        ret = csp_send(conn,packet,0);
        if (!ret) goto failed;;

void TCTA_Cycle(void) {
  TCMA_CleanTCBuffers();
  TCMA_ReadCommand();
  TCMA_VerifyCommand();
  TCMA_ExecuteCommand();
  return;
}

int TCMA_ReadCommand(void) {
  packet = gRawPacketBuffer;
  ret = COMTT_GetReceivedTCPacket(&packet,0xff,&source_channel);
  if (ret == SUCCESS) {
    CKSM_ComputeCRC(packet, &offset);
    // ...
    do {
      currentState = &gTelecommandsInProcess[i].currentState;
      if(*currentState == EMPTY) {
        *currentState = READING;
        ret = SPP_ReadSpacePacket(packet, packet.Size,
                       &gTelecommandsInProcess[i].telecommand)
        if(ret == SUCCESS) {
          // Read and set more fields ...
          *currentState = READ;
          i = 0x14;
        } else {
          *currentState = REJECTED;
        }
      }
    } while (i < 0x14);
}

if (serviceArea == 0x20001) {
  // ...
  if ((operation & 0xffffff00) == 0x18100) { PUBSUB_MonitorEvent(msg); }
} else if (serviceArea == 0x40001) {
  // ...
  if ((operation & 0xffffff00) == 0x18100) { SUBMIT_SubmitAction(msg); }
} else if (serviceArea == 0x4a000f) {
  // ...
  operation = operation & 0xffffff00;
  if (operation == 0x10100) { SUBMIT_CreateFile(msg); }
  if (operation == 0x20100) { SUBMIT_RemoveFile(msg); }
  if (operation == 0x30100) { REQUEST_WriteFile(msg); }
  if (operation == 0x50100) { PROGRESS_ReadFile(msg); }
}
// ...
 
  

void SUBMIT_CreateFile(MAL_Message_t *pMessage) {
  msg = (pMessage->body).data;
  ret = MAL_ReadBoolean(msg, &offset,(pMessage->body).length, &report_acceptance);
  if(ret == SUCCESS) {
    ret = MAL_ReadString(msg, &offset, (pMessage->body).length, &unused_str);
    if(ret == SUCCESS) {
      // Inform about Acceptance
      COMActivityTracking_PublishAcceptanceEvent(...);
    }
  }
  
  ret = MAL_ReadBoolean(msg, &offset,(pMessage->body).length, &boolVal);
  if(ret == SUCCESS) {
    ret = MAL_ReadString(msg, &offset, (pMessage->body).length, &filename);
    if(ret == SUCCESS) {
      strcpy(full_filename,"/flash/");
      strncpy(full_filename + 7,filename.character, filename.length);
      full_filename[filename.length + 7] = 0;
      file_handle = fopen(full_filename,"r");
      // ...
      
      MAL_WriteUInteger(...);
      MAL_WriteBoolean(...);
      MOSManager_SendMessage(&outputMessage_SUBMIT_CreateFile);
    }
  }
  
}