10.0.0.21 Git - firmerware/blob - src/function/develtest/netconsole.c

   1 /* netconsole.c -- run a console over LLC over ethernet
   2  *
   3  * This test program includes a very small (and very selfish)
   4  * network stack: It only does LLC2, and welcomes one LAN peer
   5  * at a time to connect to SAP 20 where a console is running.
   6  *
   7  * Console log events are saved up in a buffer, and reported
   8  * as soon as the client connects.  From then on, new entries
   9  * sent to the console are also sent immediately.  An entry is
  10  * made for incoming broadcast messages.
  11  *
  12  * From: Rick van Rein <rick@openfortress.nl>
  13  */
  14
  15
  16 #include <stdint.h>
  17 #include <stdbool.h>
  18 #include <stdarg.h>
  19
  20 #include <config.h>
  21
  22 #include <0cpm/cpu.h>
  23 #include <0cpm/timer.h>
  24 #include <0cpm/led.h>
  25 #include <0cpm/kbd.h>
  26 #include <0cpm/app.h>
  27 #include <0cpm/show.h>
  28 #include <0cpm/cons.h>
  29
  30
  31 #define onlinetest_top_main top_main
  32
  33
  34 bool online = false;
  35
  36
  37 void top_timer_expiration (timing_t timeout) {
  38         /* Keep the linker happy */ ;
  39 }
  40
  41 void top_hook_update (bool offhook) {
  42         /* Keep the linker happy */ ;
  43 }
  44
  45 void top_button_press (buttonclass_t bcl, buttoncode_t cde) {
  46         /* Keep the linker happy */ ;
  47 }
  48
  49 void top_button_release (void) {
  50         /* Keep the linker happy */ ;
  51 }
  52
  53 void top_network_online (void) {
  54         online = true;
  55 }
  56
  57 void top_network_offline (void) {
  58         online = false;
  59 }
  60
  61 void top_network_can_send (void) {
  62         /* Keep the linker happy */ ;
  63 }
  64
  65 void top_network_can_recv (void) {
  66         /* Keep the linker happy */ ;
  67 }
  68
  69
  70 static bool llc_connected = false;
  71 static uint8_t peer_sap;
  72 static uint8_t peer_mac [6];
  73 static uint8_t llc_ua [6 + 6 + 2 + 3];
  74 static uint8_t llc_rr [6 + 6 + 2 + 4];
  75 static uint8_t llc_sent;
  76 static uint8_t llc_received;
  77
  78
  79 /* Dummy LLC2 send routine, ignoring "cnx" as there is just one LLC2 connection.
  80  * This is out-only, so N(R) always sends as 0x00 and N(S) increments.
  81  * Before sending, the routine will first establish whether the last send
  82  * was successful; if not, it will repeat that.  The return value is true if
  83  * at least the send was done, relying on future calls to resend if need be.
  84  */
  85 uint8_t llc_pkt [100];
  86 uint16_t llc_pktlen;
  87 bool netsend_llc2 (struct llc2 *cnx, uint8_t *data, uint16_t datalen) {
  88         bool newpkt;
  89         if (datalen > 80) {
  90                 return false;
  91         }
  92         if (!llc_connected) {
  93                 return false;
  94         }
  95         newpkt = (llc_sent == llc_received);
  96         if (newpkt) {
  97                 // Sending is complete, construct new packet as requested
  98                 memcpy (llc_pkt +  0, peer_mac, 6);
  99                 memcpy (llc_pkt +  6, "\x00\x0b\x82\x19\xa0\xf4", 6);
 100                 llc_pkt [12] = 0x00;
 101                 llc_pkt [13] = datalen + 4;
 102                 llc_pkt [14] = peer_sap;                // DSAP
 103                 llc_pkt [15] = 20;                      // SSAP
 104                 llc_pkt [16] = llc_sent << 1;           // N(S) = 0x00, information frame
 105                 llc_pkt [17] = 0x00;                    // N(R) = sent-up-to-here, low bit reset
 106                 memcpy (llc_pkt + 18, data, datalen);
 107                 llc_pktlen = 6 + 6 + 2 + 4 + datalen;
 108                 llc_sent++;
 109                 llc_sent &= 0x7f;
 110         }
 111         bottom_network_send (llc_pkt, llc_pktlen);
 112         return newpkt;
 113 }
 114
 115 struct llc2 {
 116         uint8_t dummy;
 117 };
 118 static struct llc2 llc2_dummy_handle;
 119
 120 /* LLC2-only network packet handling */
 121 void selfish_llc2_handler (uint8_t *pkt, uint16_t pktlen) {
 122         uint16_t typelen;
 123         uint16_t cmd;
 124         bool ack = false;
 125         typelen = (pkt [12] << 8) | pkt [13];
 126 #if 0
 127         if ((pktlen < 14) || (typelen < 46)) {
 128                 bottom_printf ("Unpadded packet length %d received\n", (unsigned int) pktlen);
 129                 return;
 130         }
 131 #endif
 132         if (typelen > 1500) {
 133                 // bottom_printf ("Traffic is not LLC but protocol 0x%x\n", (unsigned int) typelen);
 134                 return;
 135         }
 136         if ((typelen > 64) && (typelen != pktlen)) {
 137                 bottom_printf ("Illegal length %d received (pktlen = %d)\n", (unsigned int) typelen, (unsigned int) pktlen);
 138                 return;
 139         }
 140         if (pkt [14] != 20) {
 141                 bottom_printf ("Received LLC traffic for SAP %d instead of 20\n", (unsigned int) pkt [14]);
 142                 return;
 143         }
 144         if (llc_connected) {
 145                 if (memcmp (pkt + 6, peer_mac, 6) != 0) {
 146                         // Peer MAC should be the constant while connected
 147                         return;
 148                 }
 149                 if ((pkt [15] & 0xfe) != peer_sap) {
 150                         // Peer SAP should be constant while connected
 151                         return;
 152                 }
 153         }
 154         cmd = pkt [16];
 155         if (typelen > 3) {
 156                 cmd |= pkt [17] << 8;
 157         }
 158         if (cmd == 0x007f) {                            // SABME (llc.connect)
 159                 memcpy (peer_mac, pkt + 6, 6);
 160                 peer_sap = pkt [15] & 0xfe;
 161                 llc_sent = llc_received = 0x00;
 162                 llc_connected = true;
 163                 netcons_connect (&llc2_dummy_handle);
 164                 ack = true;
 165         } else if (cmd == 0x0053) {                     // DISC  (llc.disconnect)
 166                 llc_connected = false;
 167                 netcons_close ();
 168                 ack = true;
 169         } else if ((cmd & 0x0001) == 0x0000) {          // Data sent back (will be ignored)
 170                 memcpy (llc_rr +  0, peer_mac, 6);
 171                 memcpy (llc_rr +  6, "\x00\x0b\x82\x19\xa0\xf4", 6);
 172                 memcpy (llc_rr + 12, "\x00\x04\x00\x15", 4);
 173                 llc_rr [14] = peer_sap;
 174                 // memcpy (llc_rr + 12, "\x00\x04\x14\x00", 4);
 175                 // llc_rr [15] = peer_sap | 0x01;
 176                 llc_rr [16] = 0x01;                     // supervisory, RR
 177                 llc_rr [17] = (cmd + 2) & 0xfe;         // outgoing N(R) = incoming N(S) + 1
 178                 bottom_network_send (llc_rr, sizeof (llc_rr));
 179         } else if ((cmd & 0x0007) == 0x0001) {          // Receiver ready / Receiver Reject
 180                 llc_received = (cmd >> 9);
 181         } else {
 182                 bottom_printf ("Selfishly ignoring LLC traffic with cmd bytes 0x%x 0x%x\n", (uint32_t) pkt [16], (uint32_t) pkt [17]);
 183         }
 184         if (ack) {
 185                 memcpy (llc_ua +  0, peer_mac, 6);
 186                 memcpy (llc_ua +  6, "\x00\x0b\x82\x19\xa0\xf4", 6);
 187                 memcpy (llc_ua + 12, "\x00\x03\x00\x15\x73", 5);
 188                 llc_ua [14] = peer_sap;
 189                 // Try sending; assume repeat will be caused by other/smarter side
 190                 bottom_network_send (llc_ua, sizeof (llc_ua));
 191         }
 192 }
 193
 194
 195 uint8_t netinput [1000];
 196 uint16_t netinputlen;
 197
 198 void onlinetest_top_main (void) {
 199         bottom_critical_region_end ();
 200         bottom_led_set (LED_IDX_BACKLIGHT, LED_STABLE_ON);
 201         while (true) {
 202                 if (online) {
 203                         bottom_show_fixed_msg (APP_LEVEL_BACKGROUNDED, FIXMSG_READY);
 204                         netinputlen = sizeof (netinput);
 205                         if (bottom_network_recv (netinput, &netinputlen)) {
 206                                 if (memcmp (netinput, "\xff\xff\xff\xff\xff\xff", 6) == 0) {
 207                                         bottom_printf ("Broadcast!\n");
 208 #if 0
 209                                         bottom_printf ("Broadcast from %x:%x:%x:%x:%x:%x\n",
 210                                                         (unsigned int) netinput [ 6],
 211                                                         (unsigned int) netinput [ 7],
 212                                                         (unsigned int) netinput [ 8],
 213                                                         (unsigned int) netinput [ 9],
 214                                                         (unsigned int) netinput [10],
 215                                                         (unsigned int) netinput [11]);
 216 #endif
 217                                 } else {
 218                                         bottom_led_set (LED_IDX_BACKLIGHT, LED_STABLE_OFF);
 219                                         selfish_llc2_handler (netinput, netinputlen);
 220                                 }
 221                         }
 222                 } else {
 223                         bottom_show_fixed_msg (APP_LEVEL_BACKGROUNDED, FIXMSG_OFFLINE);
 224                         bottom_led_set (LED_IDX_BACKLIGHT, LED_STABLE_ON);
 225                 }
 226         }
 227 }
 228