git.gag.com Git - fw/altos/blob - src/core/ao_radio_cmac.c

   1 /*
   2  * Copyright © 2011 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include "ao.h"
  19
  20 #define AO_CMAC_KEY_LEN         AO_AES_LEN
  21 #define AO_CMAC_MAX_LEN         (128 - AO_CMAC_KEY_LEN)
  22
  23 static __xdata uint8_t ao_radio_cmac_mutex;
  24 __pdata int16_t ao_radio_cmac_rssi;
  25 static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN];
  26 static __pdata uint8_t ao_radio_cmac_len;
  27
  28 static uint8_t
  29 getnibble(void)
  30 {
  31         int8_t  b;
  32
  33         b = ao_cmd_hexchar(getchar());
  34         if (b < 0) {
  35                 ao_cmd_status = ao_cmd_lex_error;
  36                 return 0;
  37         }
  38         return (uint8_t) b;
  39 }
  40
  41 static uint8_t
  42 getbyte(void)
  43 {
  44         uint8_t b;
  45         b = getnibble() << 4;
  46         b |= getnibble();
  47         return b;
  48 }
  49
  50 static uint8_t
  51 round_len(uint8_t len)
  52 {
  53         uint8_t rem;
  54
  55         /* Make sure we transfer at least one packet, and
  56          * then make sure every packet is full. Note that
  57          * there is no length encoded, and that the receiver
  58          * must deal with any extra bytes in the packet
  59          */
  60         if (len < AO_CMAC_KEY_LEN)
  61                 len = AO_CMAC_KEY_LEN;
  62         rem = len % AO_CMAC_KEY_LEN;
  63         if (rem != 0)
  64                 len += (AO_CMAC_KEY_LEN - rem);
  65         return len;
  66 }
  67
  68 /*
  69  * Sign and deliver the data sitting in the cmac buffer
  70  */
  71 static void
  72 radio_cmac_send(uint8_t len) __reentrant
  73 {
  74         uint8_t i;
  75
  76         len = round_len(len);
  77         /* Make sure the AES key is loaded */
  78         ao_config_get();
  79
  80 #if HAS_MONITOR
  81         ao_monitor_set(0);
  82 #endif
  83
  84         ao_mutex_get(&ao_aes_mutex);
  85         ao_aes_set_mode(ao_aes_mode_cbc_mac);
  86         ao_aes_set_key(ao_config.aes_key);
  87         ao_aes_zero_iv();
  88         for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
  89                 if (i + AO_CMAC_KEY_LEN < len)
  90                         ao_aes_run(&cmac_data[i], NULL);
  91                 else
  92                         ao_aes_run(&cmac_data[i], &cmac_data[len]);
  93         }
  94         ao_mutex_put(&ao_aes_mutex);
  95
  96         ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN);
  97 }
  98
  99 /*
 100  * Receive and validate an incoming packet
 101  */
 102
 103 static int8_t
 104 radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant
 105 {
 106         uint8_t i;
 107
 108         len = round_len(len);
 109 #if HAS_MONITOR
 110         ao_monitor_set(0);
 111 #endif
 112         if (timeout)
 113                 ao_alarm(timeout);
 114
 115         i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2);
 116         ao_clear_alarm();
 117
 118         if (!i) {
 119                 ao_radio_cmac_rssi = 0;
 120                 return AO_RADIO_CMAC_TIMEOUT;
 121         }
 122
 123         ao_radio_cmac_rssi = (int16_t) (((int8_t) cmac_data[len + AO_CMAC_KEY_LEN]) >> 1) - 74;
 124         if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & AO_RADIO_STATUS_CRC_OK))
 125                 return AO_RADIO_CMAC_CRC_ERROR;
 126
 127         ao_config_get();
 128
 129         /* Compute the packet signature
 130          */
 131         ao_mutex_get(&ao_aes_mutex);
 132         ao_aes_set_mode(ao_aes_mode_cbc_mac);
 133         ao_aes_set_key(ao_config.aes_key);
 134         ao_aes_zero_iv();
 135         for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
 136                 if (i + AO_CMAC_KEY_LEN < len)
 137                         ao_aes_run(&cmac_data[i], NULL);
 138                 else
 139                         ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]);
 140         }
 141         ao_mutex_put(&ao_aes_mutex);
 142
 143         /* Check the packet signature against the signature provided
 144          * over the link
 145          */
 146
 147         if (memcmp(&cmac_data[len],
 148                    &cmac_data[len + AO_CMAC_KEY_LEN + 2],
 149                    AO_CMAC_KEY_LEN) != 0) {
 150                 return AO_RADIO_CMAC_MAC_ERROR;
 151         }
 152
 153         return AO_RADIO_CMAC_OK;
 154 }
 155
 156 int8_t
 157 ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant
 158 {
 159         if (len > AO_CMAC_MAX_LEN)
 160                 return AO_RADIO_CMAC_LEN_ERROR;
 161         ao_mutex_get(&ao_radio_cmac_mutex);
 162         ao_xmemcpy(cmac_data, packet, len);
 163 #if AO_LED_TX
 164         ao_led_on(AO_LED_TX);
 165 #endif
 166         radio_cmac_send(len);
 167 #if AO_LED_TX
 168         ao_led_off(AO_LED_TX);
 169 #endif
 170         ao_mutex_put(&ao_radio_cmac_mutex);
 171         return AO_RADIO_CMAC_OK;
 172 }
 173
 174 int8_t
 175 ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant
 176 {
 177         uint8_t i;
 178         if (len > AO_CMAC_MAX_LEN)
 179                 return AO_RADIO_CMAC_LEN_ERROR;
 180         ao_mutex_get(&ao_radio_cmac_mutex);
 181 #if AO_LED_RX
 182         ao_led_on(AO_LED_RX);
 183 #endif
 184         i = radio_cmac_recv(len, timeout);
 185 #if AO_LED_RX
 186         ao_led_off(AO_LED_RX);
 187 #endif
 188         if (i == AO_RADIO_CMAC_OK)
 189                 ao_xmemcpy(packet, cmac_data, len);
 190         ao_mutex_put(&ao_radio_cmac_mutex);
 191         return i;
 192 }
 193
 194 static void
 195 radio_cmac_send_cmd(void) __reentrant
 196 {
 197         uint8_t i;
 198         uint8_t len;
 199
 200         ao_cmd_decimal();
 201         if (ao_cmd_status != ao_cmd_success)
 202                 return;
 203         len = ao_cmd_lex_i;
 204         if (len > AO_CMAC_MAX_LEN) {
 205                 ao_cmd_status = ao_cmd_syntax_error;
 206                 return;
 207         }
 208         flush();
 209         ao_mutex_get(&ao_radio_cmac_mutex);
 210         len = ao_cmd_lex_i;
 211         for (i = 0; i < len; i++) {
 212                 cmac_data[i] = getbyte();
 213                 if (ao_cmd_status != ao_cmd_success)
 214                         return;
 215         }
 216         radio_cmac_send(len);
 217         ao_mutex_put(&ao_radio_cmac_mutex);
 218 }
 219
 220 static void
 221 radio_cmac_recv_cmd(void) __reentrant
 222 {
 223         uint8_t         len, i;
 224         uint16_t        timeout;
 225
 226         ao_cmd_decimal();
 227         if (ao_cmd_status != ao_cmd_success)
 228                 return;
 229         len = ao_cmd_lex_i;
 230         ao_cmd_decimal();
 231         if (ao_cmd_status != ao_cmd_success)
 232                 return;
 233         timeout = AO_MS_TO_TICKS(ao_cmd_lex_i);
 234         ao_mutex_get(&ao_radio_cmac_mutex);
 235         i = radio_cmac_recv(len, timeout);
 236         if (i == AO_RADIO_CMAC_OK) {
 237                 printf ("PACKET ");
 238                 for (i = 0; i < len; i++)
 239                         printf("%02x", cmac_data[i]);
 240                 printf (" %d\n", ao_radio_cmac_rssi);
 241         } else
 242                 printf ("ERROR %d %d\n", i, ao_radio_cmac_rssi);
 243         ao_mutex_put(&ao_radio_cmac_mutex);
 244 }
 245
 246 static __xdata struct ao_launch_command command;
 247 static __xdata struct ao_launch_query   query;
 248 static __pdata uint16_t launch_serial;
 249 static __pdata uint8_t  launch_channel;
 250 static __pdata uint16_t tick_offset;
 251
 252 static void
 253 launch_args(void) __reentrant
 254 {
 255         ao_cmd_decimal();
 256         launch_serial = ao_cmd_lex_i;
 257         ao_cmd_decimal();
 258         launch_channel = ao_cmd_lex_i;
 259 }
 260
 261 static int8_t
 262 launch_query(void)
 263 {
 264         uint8_t i;
 265         int8_t  r = AO_RADIO_CMAC_OK;
 266
 267         tick_offset = ao_time();
 268         for (i = 0; i < 10; i++) {
 269                 printf ("."); flush();
 270                 command.tick = ao_time();
 271                 command.serial = launch_serial;
 272                 command.cmd = AO_LAUNCH_QUERY;
 273                 command.channel = launch_channel;
 274                 ao_radio_cmac_send(&command, sizeof (command));
 275                 r = ao_radio_cmac_recv(&query, sizeof (query), AO_MS_TO_TICKS(500));
 276                 if (r == AO_RADIO_CMAC_OK)
 277                         break;
 278         }
 279         tick_offset -= query.tick;
 280         printf("\n"); flush();
 281         return r;
 282 }
 283
 284 static void
 285 launch_report_cmd(void) __reentrant
 286 {
 287         int8_t          r;
 288
 289         launch_args();
 290         if (ao_cmd_status != ao_cmd_success)
 291                 return;
 292         r = launch_query();
 293         switch (r) {
 294         case AO_RADIO_CMAC_OK:
 295                 if (query.valid) {
 296                         switch (query.arm_status) {
 297                         case ao_igniter_ready:
 298                         case ao_igniter_active:
 299                                 printf ("Armed: ");
 300                                 break;
 301                         default:
 302                                 printf("Disarmed: ");
 303                         }
 304                         switch (query.igniter_status) {
 305                         default:
 306                                 printf("unknown\n");
 307                                 break;
 308                         case ao_igniter_ready:
 309                                 printf("igniter good\n");
 310                                 break;
 311                         case ao_igniter_open:
 312                                 printf("igniter bad\n");
 313                                 break;
 314                         }
 315                 } else {
 316                         printf("Invalid channel %d\n", launch_channel);
 317                 }
 318                 printf("Rssi: %d\n", ao_radio_cmac_rssi);
 319                 break;
 320         default:
 321                 printf("Error %d\n", r);
 322                 break;
 323         }
 324 }
 325
 326 static void
 327 launch_arm(void) __reentrant
 328 {
 329         command.tick = ao_time() - tick_offset;
 330         command.serial = launch_serial;
 331         command.cmd = AO_LAUNCH_ARM;
 332         command.channel = launch_channel;
 333         ao_radio_cmac_send(&command, sizeof (command));
 334 }
 335
 336 static void
 337 launch_ignite(void) __reentrant
 338 {
 339         command.tick = ao_time() - tick_offset;
 340         command.serial = launch_serial;
 341         command.cmd = AO_LAUNCH_FIRE;
 342         command.channel = 0;
 343         ao_radio_cmac_send(&command, sizeof (command));
 344 }
 345
 346 static void
 347 launch_fire_cmd(void) __reentrant
 348 {
 349         static __xdata struct ao_launch_command command;
 350         uint8_t         secs;
 351         uint8_t         i;
 352         int8_t          r;
 353
 354         launch_args();
 355         ao_cmd_decimal();
 356         secs = ao_cmd_lex_i;
 357         if (ao_cmd_status != ao_cmd_success)
 358                 return;
 359         r = launch_query();
 360         if (r != AO_RADIO_CMAC_OK) {
 361                 printf("query failed %d\n", r);
 362                 return;
 363         }
 364
 365         for (i = 0; i < 4; i++) {
 366                 printf("arm %d\n", i); flush();
 367                 launch_arm();
 368         }
 369
 370         secs = secs * 10 - 5;
 371         if (secs > 100)
 372                 secs = 100;
 373         for (i = 0; i < secs; i++) {
 374                 printf("fire %d\n", i); flush();
 375                 launch_ignite();
 376                 ao_delay(AO_MS_TO_TICKS(100));
 377         }
 378 }
 379
 380 static void
 381 launch_arm_cmd(void) __reentrant
 382 {
 383         uint8_t i;
 384         int8_t  r;
 385         launch_args();
 386         r = launch_query();
 387         if (r != AO_RADIO_CMAC_OK) {
 388                 printf("query failed %d\n", r);
 389                 return;
 390         }
 391         for (i = 0; i < 4; i++)
 392                 launch_arm();
 393 }
 394
 395 static void
 396 launch_ignite_cmd(void) __reentrant
 397 {
 398         uint8_t i;
 399         launch_args();
 400         for (i = 0; i < 4; i++)
 401                 launch_ignite();
 402 }
 403
 404 static __code struct ao_cmds ao_radio_cmac_cmds[] = {
 405         { radio_cmac_send_cmd,  "s <length>\0Send AES-CMAC packet. Bytes to send follow on next line" },
 406         { radio_cmac_recv_cmd,  "S <length> <timeout>\0Receive AES-CMAC packet. Timeout in ms" },
 407         { launch_report_cmd,    "l <serial> <channel>\0Get remote launch status" },
 408         { launch_fire_cmd,      "f <serial> <channel> <secs>\0Fire remote igniter" },
 409         { launch_arm_cmd,       "a <serial> <channel>\0Arm remote igniter" },
 410         { launch_ignite_cmd,    "i <serial> <channel>\0Pulse remote igniter" },
 411         { 0, NULL },
 412 };
 413
 414 void
 415 ao_radio_cmac_init(void)
 416 {
 417         ao_cmd_register(&ao_radio_cmac_cmds[0]);
 418 }