visus
/
framework-arduinoteensy-cxx20
Watch 1
Star 0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
PlatformIO package of the Teensy core framework compatible with GCC 10 & C++20
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
4 Commits
1 Branch
Tree: 16a0c39090
Branches Tags
${ item.name }
Create branch ${ searchTerm }
from '16a0c39090'
${ noResults }
framework-arduinoteensy-cxx20/libraries/FNET/examples/ASIXEthernet_Test/Functions.ino

560 lines
18KB
Raw Blame History 

  1. #define HACKED //Limits number of times usbthread loops per second

  2. 

  3. void usbthread() {

  4.   uint32_t cc = 0;

  5.   while(1) {

  6.     myusb.Task();

  7.     asix1.read();

  8.     checkLink();

  9.     if(fnet_netif_is_initialized(current_netif)){

  10.       fnet_poll();

  11.       fnet_service_poll();

  12.     }

  13.     #ifdef STATS

  14.     LoopedUSB++;

  15.     #endif

  16.     #ifdef HACKED

  17.     cc++;

  18.     if ( cc > 20 ) {

  19.       cc=0;

  20.       threads.yield();

  21.     }

  22.     #endif

  23.   }

  24. }

  25. 

  26. void dhcp_cln_callback_updated(fnet_dhcp_cln_desc_t _dhcp_desc, fnet_netif_desc_t netif, void *p ) { //Called when DHCP updates

  27.   struct fnet_dhcp_cln_options current_options;

  28.   fnet_dhcp_cln_get_options(dhcp_desc, &current_options);

  29.   

  30.   uint8_t *ip = (uint8_t*)&current_options.ip_address.s_addr;

  31.   Serial.print("IPAddress: ");

  32.   Serial.print((uint8_t)*ip++);

  33.   Serial.print(".");

  34.   Serial.print((uint8_t)*ip++);

  35.   Serial.print(".");

  36.   Serial.print((uint8_t)*ip++);

  37.   Serial.print(".");

  38.   Serial.println((uint8_t)*ip);

  39. 

  40.   ip = (uint8_t*)&current_options.netmask.s_addr;

  41.   Serial.print("SubnetMask: ");

  42.   Serial.print((uint8_t)*ip++);

  43.   Serial.print(".");

  44.   Serial.print((uint8_t)*ip++);

  45.   Serial.print(".");

  46.   Serial.print((uint8_t)*ip++);

  47.   Serial.print(".");

  48.   Serial.println((uint8_t)*ip);

  49. 

  50.   ip = (uint8_t*)&current_options.gateway.s_addr;

  51.   Serial.print("Gateway: ");

  52.   Serial.print((uint8_t)*ip++);

  53.   Serial.print(".");

  54.   Serial.print((uint8_t)*ip++);

  55.   Serial.print(".");

  56.   Serial.print((uint8_t)*ip++);

  57.   Serial.print(".");

  58.   Serial.println((uint8_t)*ip);

  59. 

  60.   ip = (uint8_t*)&current_options.dhcp_server.s_addr;

  61.   Serial.print("DHCPServer: ");

  62.   Serial.print((uint8_t)*ip++);

  63.   Serial.print(".");

  64.   Serial.print((uint8_t)*ip++);

  65.   Serial.print(".");

  66.   Serial.print((uint8_t)*ip++);

  67.   Serial.print(".");

  68.   Serial.println((uint8_t)*ip);

  69. 

  70.   

  71.   Serial.print("State: ");

  72.   Serial.println(fnet_dhcp_cln_get_state(_dhcp_desc));

  73.   Serial.println();

  74.   Serial.println();

  75. 

  76.   

  77. 

  78. }

  79. 

  80. void checkLink(){

  81. //  Serial.print("Link: ");

  82. //  Serial.println(asix1.connected);

  83.   if(asix1.connected && fnet_dhcp_cln_is_enabled(dhcp_desc)){

  84. //    Serial.println("DHCP already running!");

  85.   }

  86.   else if(asix1.connected){

  87.     struct fnet_init_params     init_params;

  88.   

  89.     static const fnet_timer_api_t timer_api = { //Setup multi-thread timer

  90.       .timer_get_ms = timer_get_ms,

  91.       .timer_delay = 0,

  92.     };

  93.     /* Input parameters for FNET stack initialization */

  94.     init_params.netheap_ptr = stack_heap;

  95.     init_params.netheap_size = sizeof(stack_heap);

  96.     init_params.mutex_api = &teensy_mutex_api;

  97.     init_params.timer_api = &timer_api;

  98.     /* FNET Initialization */

  99.     if (fnet_init(&init_params) != FNET_ERR) {

  100.       Serial.println("TCP/IP stack initialization is done.\n");

  101.       /* You may use FNET stack API */

  102.       /* Initialize networking interfaces using fnet_netif_init(). */

  103. //        Get current net interface.

  104.       if(fnet_netif_init(FNET_CPU_USB0_IF, MacAddress, 6) != FNET_ERR){

  105.         Serial.println("netif Initialized");

  106.         if((current_netif = fnet_netif_get_default()) == 0){

  107.           Serial.println("ERROR: Network Interface is not configurated!");

  108.         }

  109.         else {

  110.           Serial.println("Initializing DHCP");

  111.           fnet_memset_zero(&dhcp_desc, sizeof(dhcp_desc));

  112.           fnet_memset_zero(&dhcp_params, sizeof(dhcp_params));

  113.           dhcp_params.netif = current_netif;

  114.           // Enable DHCP client.

  115.           if((dhcp_desc = fnet_dhcp_cln_init(&dhcp_params))){

  116.               /*Register DHCP event handler callbacks.*/

  117.              fnet_dhcp_cln_set_callback_updated(dhcp_desc, dhcp_cln_callback_updated, (void*)dhcp_desc);

  118.              fnet_dhcp_cln_set_callback_discover(dhcp_desc, dhcp_cln_callback_updated, (void*)dhcp_desc);

  119.              Serial.println("DHCP initialization done!");

  120.              bench_srv_init();

  121.           }

  122.           else{

  123.             Serial.println("ERROR: DHCP initialization failed!");

  124.           }

  125.         }

  126.       }

  127.       else {

  128.         Serial.println("Error:netif initialization failed.\n");

  129.       }

  130.   }

  131.     else {

  132.       Serial.println("Error:TCP/IP stack initialization failed.\n");

  133.     }

  134.   }

  135.   else if(!asix1.connected && fnet_dhcp_cln_is_enabled(dhcp_desc)){

  136.     Serial.println("DHCP Released");

  137.     fnet_dhcp_cln_release(dhcp_desc);

  138.     fnet_memset_zero(dhcp_desc, sizeof(dhcp_desc));

  139.     bench_srv_release();

  140.     fnet_release();

  141.   }

  142.   else if(!asix1.connected){

  143. //    Serial.println("DHCP already released!");

  144.   }

  145. }

  146. 

  147. void handleOutput(fnet_netif_t *netif, fnet_netbuf_t *nb) { //Called when a message is sent

  148.   if(nb && (nb->total_length >= FNET_ETH_HDR_SIZE)){

  149.     uint8_t* p = (uint8_t*)sbuf;

  150.     _fnet_netbuf_to_buf(nb, 0u, FNET_NETBUF_COPYALL, p);

  151. 

  152.     if(nb->total_length >= 12){

  153. //      Serial.print("Message Transmitted: ");

  154. //      Serial.println(nb->total_length);

  155. //      Serial.print("QueuedBefore: ");

  156. //      Serial.println(asix1.txQueued());

  157. //      const uint8_t* end = p + nb->total_length;

  158. //      while(p < end){

  159. //        if(*p <= 0x0F) Serial.print("0");

  160. //        Serial.print(*p, HEX);

  161. //        Serial.print(" ");

  162. //        p++;

  163. //      }

  164. //      Serial.println();

  165. //    p = (uint8_t*)sbuf;

  166.     }

  167.     myusb.Task();

  168.     asix1.sendPacket(p, nb->total_length);

  169. //    Serial.print("QueuedAfter: ");

  170. //    Serial.println(asix1.txQueued());

  171.     myusb.Task();

  172.   }

  173. }

  174. 

  175. void handleWait() {

  176.   myusb.Task();

  177. }

  178. 

  179. uint32_t _totalLength;

  180. int32_t _remainingLength = 0;

  181. uint8_t* _lastIndex;

  182. uint8_t* _rxEnd;

  183. uint8_t* _rxStart;

  184. void handleRecieve(const uint8_t* data, uint32_t length) { //Called when ASIX gets a message

  185.   if(length == 0) return;

  186.   RECIEVE:

  187.   if(_remainingLength < 0){

  188.     Serial.println("Remaining");

  189.     while(_lastIndex < _rxEnd){

  190.       *_lastIndex = *data;

  191.       data++;

  192.       _lastIndex++;

  193.     }

  194.     _fnet_eth_input(&fnet_usb0_if, (uint8_t*)rbuf, _totalLength);

  195.     _remainingLength = abs(_remainingLength);

  196.     data += _remainingLength;

  197.     _remainingLength = length - ((_remainingLength + 3) & 0xFFFFFFFC);

  198.     length -= _remainingLength;

  199.     if(length) {

  200.       goto RECIEVE;

  201.     }

  202.   }

  203.   else if(((data[0] | data[2]) == 0xFF) && ((data[1] | data[3]) == 0xFF)) { //Check for header

  204.     _lastIndex = (uint8_t*)rbuf;

  205.     _totalLength = (data[1] << 8) | data[0];

  206.     _remainingLength = length - ((_totalLength + 9) & 0xFFFFFFFC);

  207.     if(_remainingLength < 0){

  208. //      Serial.print("Remaining: ");

  209. //      Serial.println(_remainingLength);

  210.       const uint8_t* end = data + length;

  211.       _rxEnd = (uint8_t*)rbuf + _totalLength;

  212.       data += 6;

  213.       while(data < end){

  214.         *_lastIndex = *data;

  215.         data++;

  216.         _lastIndex++;

  217.       }

  218.     }

  219.     else {

  220.       data += 6;

  221.       _fnet_eth_input(&fnet_usb0_if, (uint8_t*)data, _totalLength);

  222.       if(_remainingLength) {        

  223. //        Serial.println("Remaining data");

  224. //        Serial.print("length: ");

  225. //        Serial.print(length);

  226. //        Serial.print("  rlength: ");

  227. //        Serial.println(_remainingLength);

  228.         data -= 6;

  229.         data += ((_totalLength + 9) & 0xFFFFFFFC);

  230.         length -= ((_totalLength + 9) & 0xFFFFFFFC);

  231. //        asix1.print_hexbytes((uint8_t*)data, ((_totalLength + 9) & 0xFFFFFFFC));

  232.         goto RECIEVE;

  233.       }

  234.     }

  235.   }

  236.   else if(length == abs(_remainingLength)) return; //Technically an error and I don't know where it's happening, but this poses no problems so it's fine

  237.   else{ //Error edgecase unknown cause

  238.     Serial.println("Message Recieve Error, searching for next header");

  239.     Serial.print("length: ");

  240.     Serial.print(length);

  241.     Serial.print("  rlength: ");

  242.     Serial.println(_remainingLength);

  243.     return;

  244.   }

  245. }

  246. 

  247. void handleSetMACAddress(uint8_t * hw_addr) { //Gets called on initialization

  248.   Serial.print("SetMACAddress: ");

  249.   for(uint8_t i = 0; i < 6; i++) {

  250.     if(hw_addr[i] <= 0x0F) Serial.print("0");

  251.     Serial.print(hw_addr[i], HEX);

  252.   }

  253.   Serial.println();

  254. }

  255. 

  256. void handleGetMACAddress(fnet_mac_addr_t * hw_addr) { //Gets called everytime a message is sent

  257.   (*hw_addr)[0] = asix1.nodeID[0];

  258.   (*hw_addr)[1] = asix1.nodeID[1];

  259.   (*hw_addr)[2] = asix1.nodeID[2];

  260.   (*hw_addr)[3] = asix1.nodeID[3];

  261.   (*hw_addr)[4] = asix1.nodeID[4];

  262.   (*hw_addr)[5] = asix1.nodeID[5];

  263.   MacAddress[0] = asix1.nodeID[0];

  264.   MacAddress[1] = asix1.nodeID[1];

  265.   MacAddress[2] = asix1.nodeID[2];

  266.   MacAddress[3] = asix1.nodeID[3];

  267.   MacAddress[4] = asix1.nodeID[4];

  268.   MacAddress[5] = asix1.nodeID[5];

  269. //  Serial.print("GetMACAddress: ");

  270. //  for(uint8_t i = 0; i < 6; i++) {

  271. //    if(hw_addr[i] <= 0x0F) Serial.print("0");

  272. //    Serial.print(hw_addr[i], HEX);

  273. //  }

  274. //  Serial.println();

  275. }

  276. 

  277. void handlePHYRead(fnet_uint32_t reg_addr, fnet_uint16_t *data) { //Could be called, don't think it works correctly

  278.   asix1.readPHY(reg_addr, data);

  279.   Serial.print("PHYRead: ");

  280.   Serial.print(reg_addr, HEX);

  281.   Serial.print("  ");

  282.   Serial.println(*data, HEX);

  283. }

  284. 

  285. void handlePHYWrite(fnet_uint32_t reg_addr, fnet_uint16_t data) { //Could be called, might work correctly

  286.   asix1.writePHY(reg_addr, data);

  287.   Serial.println("PHYWrite");

  288. }

  289. 

  290. uint8_t mcHashTable[8] = {0,0,0,0,0,0,0,0}; //Whole table needs resent when one multicast address is changed

  291. void handleMulticastJoin(fnet_netif_t *netif, fnet_mac_addr_t multicast_addr) { //Called when joining multicast group

  292.   //Not setup yet

  293.   Serial.print("MulticastJoin: ");

  294.   for(uint8_t i = 0; i < 6; i++) {

  295.     if(multicast_addr[i] <= 0x0F) Serial.print("0");

  296.     Serial.print(multicast_addr[i], HEX);

  297.   }

  298.   Serial.println();

  299.   uint8_t crc = byteReverse((uint8_t)(fnet_usb_crc_hash(multicast_addr) & 0x000000FF)) >> 2;

  300. 

  301.   crc &= 0x3F;

  302.   mcHashTable[crc >> 3] |= 1 << (crc & 7);

  303.   asix1.setMulticast((uint8_t*)&mcHashTable);

  304. //  Serial.println("MulticastTable: ");

  305. //  for(uint8_t i = 0; i < 8; i++) {

  306. //    Serial.println(mcHashTable[i],BIN);

  307. //  }

  308. }

  309. 

  310. void handleMulticastLeave(fnet_netif_t *netif, fnet_mac_addr_t multicast_addr) { //Called when leaving multicast group

  311.   //Not setup yet

  312.   Serial.println("MulticastLeave: ");

  313.   for(uint8_t i = 0; i < 6; i++) {

  314.     if(multicast_addr[i] <= 0x0F) Serial.print("0");

  315.     Serial.print(multicast_addr[i], HEX);

  316.   }

  317.   Serial.println();

  318.   uint8_t crc = byteReverse((uint8_t)(fnet_usb_crc_hash(multicast_addr) & 0x000000FF)) >> 2;

  319.   crc &= 0x3F;

  320.   mcHashTable[crc >> 3] ^= 1 << (crc & 7);

  321.   asix1.setMulticast((uint8_t*)&mcHashTable);

  322. //  Serial.println("MulticastTable: ");

  323. //  for(uint8_t i = 0; i < 8; i++) {

  324. //    Serial.println(mcHashTable[i],BIN);

  325. //  }

  326. }

  327. 

  328. uint8_t byteReverse(uint8_t x) { 

  329.    x = ((x >> 1) & 0x55) | ((x << 1) & 0xaa); 

  330.    x = ((x >> 2) & 0x33) | ((x << 2) & 0xcc); 

  331.    x = ((x >> 4) & 0x0f) | ((x << 4) & 0xf0); 

  332.    return x;    

  333. }

  334. 

  335. fnet_bool_t handleIsConnected() {

  336.   return asix1.connected ? FNET_TRUE : FNET_FALSE;

  337. }

  338. 

  339. fnet_bench_srv_params_t bench_srv_params;

  340. fnet_bench_srv_desc_t bench_srv_desc;

  341. void bench_srv_init(){

  342.     /* Set Benchmark server parameters.*/

  343.     fnet_memset_zero(&bench_srv_params, sizeof(bench_srv_params));

  344.     bench_srv_params.type = SOCK_STREAM; /* TCP by default */

  345.         if(current_netif){ /* Only on one interface */

  346.             bench_srv_params.address.sa_scope_id = fnet_netif_get_scope_id(current_netif);

  347.         }

  348. 

  349.         /* Start Benchmark server. */

  350.         bench_srv_desc = fnet_bench_srv_init(&bench_srv_params);

  351.         if(bench_srv_desc){

  352.             /* Instal callbacks */

  353. //            fnet_bench_srv_set_callback_session_begin (fapp_bench_srv_desc, fapp_bench_srv_callback_session_begin, shell_desc);

  354.             fnet_bench_srv_set_callback_session_end (bench_srv_desc, bench_srv_callback_session_end, bench_srv_desc);

  355.         }

  356. }

  357. 

  358. void bench_srv_release(void){

  359.     fnet_bench_srv_release(bench_srv_desc);

  360.     bench_srv_desc = 0;

  361. }

  362. 

  363. static void bench_srv_callback_session_end(fnet_bench_srv_desc_t desc, const struct fnet_bench_srv_result *bench_srv_result, void *cookie) {

  364.   if(bench_srv_result){

  365.     uint64_t totalBytes = (bench_srv_result->megabytes * 1000000) + bench_srv_result->bytes;

  366. //    Serial.println("Benchmark results:");

  367.     Serial.print("Megabytes: ");

  368.     Serial.print(totalBytes / 1000000.0, 6);

  369.     Serial.print("  Seconds: ");

  370.     Serial.print(bench_srv_result->time_ms/1000.0, 4);

  371. //    Serial.print("Bytes/Sec: ");

  372. //    Serial.println(totalBytes/(bench_srv_result->time_ms/1000.0), 4);

  373. //    Serial.print("KBytes/Sec: ");

  374. //    Serial.println((totalBytes/(bench_srv_result->time_ms/1000.0))/1000.0, 4);

  375.     Serial.print("  KBits/Sec: ");

  376.     Serial.println((((totalBytes/(bench_srv_result->time_ms/1000.0))/1000.0)*8), 4);

  377. //    Serial.println();

  378.   }

  379. }

  380. 

  381. fnet_bench_cln_desc_t teensy_bench_cln_desc;

  382. void bench_cln_cmd(uint8_t argc, char* argv){

  383.     fnet_bench_cln_params_t         bench_cln_params;

  384.     fnet_netif_desc_t               netif = current_netif;

  385.     fnet_bench_cln_desc_t           bench_cln_desc;

  386.     fnet_char_t                     ip_str[FNET_IP_ADDR_STR_SIZE_MAX];

  387.     uint8_t i;

  388. 

  389.     /* Set Benchmark client parameters.*/

  390.     fnet_memset_zero(&bench_cln_params, sizeof(bench_cln_params));

  391. 

  392.     /* Default values */

  393.     bench_cln_params.type = SOCK_STREAM; /* TCP by default */

  394.     bench_cln_params.message_size = 1272;     /* Default message size */

  395.     bench_cln_params.message_number = 10000; /* Default number of messages */

  396. 

  397.     /* -a <remote ip> [tcp|udp] [-m <message size>] [-mn <number of messages>] */

  398.     for(i = 0; i < (argc); i++){

  399.         

  400.         if (!fnet_strcmp(&argv[i], "-a")){ /*[-a <remote ip>] */

  401.             i += 3;

  402.             if(i < argc){

  403.                 if(fnet_inet_ptos(&argv[i], &bench_cln_params.address) == FNET_ERR){

  404.                     goto ERROR_PARAMETER;

  405.                 }

  406.                 while(argv[i] != '\0'){

  407.                   i++;

  408.                 }

  409.             }

  410.             else{

  411.                 goto ERROR_PARAMETER;

  412.             }

  413.         }

  414.         else if (!fnet_strcmp(&argv[i], "tcp")){ /* TCP */

  415.             bench_cln_params.type = SOCK_STREAM;

  416.             while(argv[i] != '\0'){

  417.               i++;

  418.             }

  419.         }

  420.         else if (!fnet_strcmp(&argv[i], "udp")){ /* udp */

  421.             bench_cln_params.type = SOCK_DGRAM;

  422.             while(argv[i] != '\0'){

  423.               i++;

  424.             }

  425.         }

  426.         else if (!fnet_strcmp(&argv[i], "-m")){ /* [-m <message size>] */

  427.             fnet_char_t *p = 0;

  428. 

  429.             i += 3;

  430.             if(i < argc){

  431.                 bench_cln_params.message_size = fnet_strtoul(&argv[i], &p, 0);

  432.                 if (bench_cln_params.message_size == 0){

  433.                     goto ERROR_PARAMETER;

  434.                 }

  435.             }

  436.             else{

  437.                 goto ERROR_PARAMETER;

  438.             }

  439.             while(argv[i] != '\0'){

  440.               i++;

  441.             }

  442.         }

  443.         else if (!fnet_strcmp(&argv[i], "-mn")){ /* [-mn <number of messages>] */

  444.             fnet_char_t *p = 0;

  445. 

  446.             i += 4;

  447.             if(i < argc){

  448.                 bench_cln_params.message_number = fnet_strtoul(&argv[i], &p, 0);

  449.                 if (bench_cln_params.message_number == 0){

  450.                     goto ERROR_PARAMETER;

  451.                 }

  452.             }

  453.             else{

  454.                 goto ERROR_PARAMETER;

  455.             }

  456.             while(argv[i] != '\0'){

  457.               i++;

  458.             }

  459.         }

  460.         else if (argv[i] == '\0'){

  461.         

  462.         }

  463.         else{/* Wrong parameter.*/

  464.             goto ERROR_PARAMETER;

  465.         }

  466.     }

  467. 

  468.     if(fnet_socket_addr_is_unspecified(&bench_cln_params.address) == FNET_TRUE){ /* Address is not set. */

  469.         Serial.println("Error: <remote ip> is not set");

  470.     }

  471.     else{

  472.         if(netif){ /* Only on one interface */

  473.             bench_cln_params.address.sa_scope_id = fnet_netif_get_scope_id(netif);

  474.         }

  475. 

  476.         bench_cln_params.callback = bench_cln_callback_session_end;    /* Callback function.*/

  477. 

  478.         /* Run Benchmark client. */

  479.         bench_cln_desc = fnet_bench_cln_init(&bench_cln_params);

  480.         if(bench_cln_desc){

  481.             teensy_bench_cln_desc = bench_cln_desc;

  482. //

  483.             Serial.println("Benchmark client started.");

  484.             Serial.print("Protocol: ");

  485.             Serial.println((bench_cln_params.type == SOCK_STREAM) ? "TCP" : "UDP");

  486.             Serial.print("Remote IP Addr: ");

  487.             Serial.println(fnet_inet_ntop(bench_cln_params.address.sa_family, bench_cln_params.address.sa_data, ip_str, sizeof(ip_str)));

  488.             Serial.print("Remote Port: ");

  489.             Serial.println(FNET_NTOHS(FNET_CFG_BENCH_CLN_PORT));

  490.             Serial.print("Message Size: ");

  491.             Serial.println(bench_cln_params.message_size);

  492.             Serial.print("Num. of messages: ");

  493.             Serial.println(bench_cln_params.message_number);

  494.             Serial.println();

  495.         }

  496.         else{

  497.             Serial.println("Benchmark client failed to start.");

  498.         }

  499.     }

  500.     return;

  501. 

  502. ERROR_PARAMETER:

  503.     Serial.print("Bad parameter: ");

  504.     Serial.println(&argv[i]);

  505.     return;

  506. }

  507. 

  508. static void bench_cln_callback_session_end(fnet_bench_cln_desc_t bench_cln_desc, const fnet_bench_cln_result_t *bench_cln_result, void *cookie){

  509.   if(bench_cln_result){

  510. //        fapp_bench_print_results (shell_desc, bench_cln_result->megabytes, bench_cln_result->bytes, bench_cln_result->time_ms);

  511.       uint64_t totalBytes = (bench_cln_result->megabytes * 1000000) + bench_cln_result->bytes;

  512. //    Serial.println("Benchmark results:");

  513.     Serial.print("Megabytes: ");

  514.     Serial.print(totalBytes / 1000000.0, 6);

  515.     Serial.print("  Seconds: ");

  516.     Serial.print(bench_cln_result->time_ms/1000.0, 4);

  517.     Serial.print("  KBits/Sec: ");

  518.     Serial.println((((totalBytes/(bench_cln_result->time_ms/1000.0))/1000.0)*8), 4);

  519.   }

  520. }

  521. 

  522. void serialParser(uint8_t argc, char* argv){

  523.   for(uint8_t i = 0; i < (argc) /*avoid the last parameter.*/; i++){

  524.     if (!fnet_strcmp(&argv[i], "benchtx")){ /*benchtx */

  525.       i++;

  526.       if(i < argc){

  527.         argv += 8; //Advance length of command + 1

  528.         argc -= 8; //Decrease length same amount

  529.         Serial.println("Starting benchtx...");

  530.         bench_cln_cmd(argc, argv);

  531.         return;

  532.       }

  533.       else{

  534.         goto ERROR_PARAMETER;

  535.       }

  536.     }

  537.     else     if (!fnet_strcmp(&argv[i], "benchrx")){ /*benchrx */

  538.       i++;

  539.       if(i < argc){

  540.         argv += 8; //Advance length of command + 1

  541.         argc -= 8; //Decrease length same amount

  542. //        bench_cln_cmd(argc, argv);

  543.         Serial.println("Starting benchrx...");

  544.         return;

  545.       }

  546.       else{

  547.         goto ERROR_PARAMETER;

  548.       }

  549.     }

  550.     else{/* Wrong parameter.*/

  551.       goto ERROR_PARAMETER;

  552.     }

  553.   }

  554.   return;

  555. 

  556. ERROR_PARAMETER:

  557.     Serial.println("Invalid command");

  558.     return;

  559. }