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;
}

int csp_route_security_chek(...) {
  if (packet->id.flags & CSP_FXTEA) {
    csp_log_error("Received XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet");
  }
  
  // ...
  
  if (packet->id.flags & CSP_FHMAC) {
    csp_log_error("Received packet with HMAC, but CSP was compiled without HMAC support. Discarding packet");
  }
  
   // ...
}

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 can_rx_task_gmv(void) {
	// ...
  if(frame_type == START) {
    // Allocate packet
    if (curr_buf->packet == (CFP_Packet_t *)0x0) {
      tmp = csp_buffer_size();
      packet = (CFP_Packet_t *)csp_buffer_get(tmp - 0xe);
      curr_buf->packet = packet;
    } else { ... }
    
    curr_buf->rx_count = 0;
    curr_buf->remain = frame_id >> 0xd & 0x1f;
    // Copy to Global Buffer
    memcpy(datablockGlobalRx.Data, &can_frame.data, can_frame.dlc);
    datablockGlobalRx.Size = can_frame.dlc + datablockGlobalRx.Size;
    // ...
  }
  else if(frame_type == CONTINUE) {
    can_remain = frame_id >> 0xd & 0x1f;
    // Check continous ID
    if (can_remain == curr_buf->remain - 1) {
      curr_buf->remain = can_remain;
      curr_buf->rx_count = curr_buf->rx_count + (ushort)can_frame.dlc;
      // Copy to Global Buffer
      memcpy(datablockGlobalRx.Data + datablockGlobalRx.Size, 
             &can_frame.data, can_frame.dlc);
      datablockGlobalRx.Size = (uint)can_frame.dlc + datablockGlobalRx.Size;
      // ...
    }
  }
  else if(frame_type == END) {
    can_remain = frame_id >> 0xd & 0x1f;
    if (can_remain != curr_buf->remain - 1) {
      memcpy(datablockGlobalRx.Data + datablockGlobalRx.Size, 
             &can_frame.data, can_frame.dlc);
      datablockGlobalRx.Size = can_frame.dlc + datablockGlobalRx.Size;
      memcpy(datablockGlobalRxFinal.Data, datablockGlobalRx.Data, 
             datablockGlobalRx.Size);
      datablockGlobalRxFinal.Size = datablockGlobalRx.Size;
      datablockGlobalFlag = 1;
      CAN_AddPacketToCanStore(&datablockGlobalRxFinal);
      // ...
    }
  }
    
}

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);
}