git.gag.com Git - fw/altos/blob - src/kernel/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; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #include <ao.h>
  20 #include <ao_radio_cmac.h>
  21
  22 static uint8_t ao_radio_cmac_mutex;
  23 int8_t ao_radio_cmac_rssi;
  24 static uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN];
  25
  26 static uint8_t
  27 round_len(uint8_t len)
  28 {
  29         uint8_t rem;
  30
  31         /* Make sure we transfer at least one packet, and
  32          * then make sure every packet is full. Note that
  33          * there is no length encoded, and that the receiver
  34          * must deal with any extra bytes in the packet
  35          */
  36         if (len < AO_CMAC_KEY_LEN)
  37                 len = AO_CMAC_KEY_LEN;
  38         rem = len % AO_CMAC_KEY_LEN;
  39         if (rem != 0)
  40                 len += (AO_CMAC_KEY_LEN - rem);
  41         return len;
  42 }
  43
  44 /*
  45  * Sign and deliver the data sitting in the cmac buffer
  46  */
  47 static void
  48 radio_cmac_send(uint8_t len)
  49 {
  50         uint8_t i;
  51
  52         len = round_len(len);
  53         /* Make sure the AES key is loaded */
  54         ao_config_get();
  55
  56 #if HAS_MONITOR
  57         ao_monitor_set(0);
  58 #endif
  59
  60         ao_mutex_get(&ao_aes_mutex);
  61         ao_aes_set_mode(ao_aes_mode_cbc_mac);
  62         ao_aes_set_key(ao_config.aes_key);
  63         ao_aes_zero_iv();
  64         for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
  65                 if (i + AO_CMAC_KEY_LEN < len)
  66                         ao_aes_run(&cmac_data[i], NULL);
  67                 else
  68                         ao_aes_run(&cmac_data[i], &cmac_data[len]);
  69         }
  70         ao_mutex_put(&ao_aes_mutex);
  71
  72         ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN);
  73 }
  74
  75 /*
  76  * Receive and validate an incoming packet
  77  */
  78
  79 static int8_t
  80 radio_cmac_recv(uint8_t len, uint16_t timeout)
  81 {
  82         uint8_t i;
  83
  84         len = round_len(len);
  85 #if HAS_MONITOR
  86         ao_monitor_set(0);
  87 #endif
  88         i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2, timeout);
  89
  90         if (!i) {
  91                 ao_radio_cmac_rssi = 0;
  92                 return AO_RADIO_CMAC_TIMEOUT;
  93         }
  94
  95         if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & AO_RADIO_STATUS_CRC_OK))
  96                 return AO_RADIO_CMAC_CRC_ERROR;
  97
  98         ao_config_get();
  99
 100         /* Compute the packet signature
 101          */
 102         ao_mutex_get(&ao_aes_mutex);
 103         ao_aes_set_mode(ao_aes_mode_cbc_mac);
 104         ao_aes_set_key(ao_config.aes_key);
 105         ao_aes_zero_iv();
 106         for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
 107                 if (i + AO_CMAC_KEY_LEN < len)
 108                         ao_aes_run(&cmac_data[i], NULL);
 109                 else
 110                         ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]);
 111         }
 112         ao_mutex_put(&ao_aes_mutex);
 113
 114         /* Check the packet signature against the signature provided
 115          * over the link
 116          */
 117
 118         if (memcmp(&cmac_data[len],
 119                    &cmac_data[len + AO_CMAC_KEY_LEN + 2],
 120                    AO_CMAC_KEY_LEN) != 0) {
 121                 return AO_RADIO_CMAC_MAC_ERROR;
 122         }
 123
 124         ao_radio_cmac_rssi = ao_radio_rssi;
 125
 126         return AO_RADIO_CMAC_OK;
 127 }
 128
 129 int8_t
 130 ao_radio_cmac_send(void *packet, uint8_t len)
 131 {
 132         if (len > AO_CMAC_MAX_LEN)
 133                 return AO_RADIO_CMAC_LEN_ERROR;
 134         ao_mutex_get(&ao_radio_cmac_mutex);
 135         ao_xmemcpy(cmac_data, packet, len);
 136 #if AO_LED_TX
 137         ao_led_on(AO_LED_TX);
 138 #endif
 139         radio_cmac_send(len);
 140 #if AO_LED_TX
 141         ao_led_off(AO_LED_TX);
 142 #endif
 143         ao_mutex_put(&ao_radio_cmac_mutex);
 144         return AO_RADIO_CMAC_OK;
 145 }
 146
 147 int8_t
 148 ao_radio_cmac_recv(void *packet, uint8_t len, uint16_t timeout)
 149 {
 150         int8_t  i;
 151         if (len > AO_CMAC_MAX_LEN)
 152                 return AO_RADIO_CMAC_LEN_ERROR;
 153         ao_mutex_get(&ao_radio_cmac_mutex);
 154 #if AO_LED_RX
 155         ao_led_on(AO_LED_RX);
 156 #endif
 157         i = radio_cmac_recv(len, timeout);
 158 #if AO_LED_RX
 159         ao_led_off(AO_LED_RX);
 160 #endif
 161         if (i == AO_RADIO_CMAC_OK)
 162                 ao_xmemcpy(packet, cmac_data, len);
 163         ao_mutex_put(&ao_radio_cmac_mutex);
 164         return i;
 165 }