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