/**
* @brief This function encrypts using sms4 algorithm
*
* @param plaintext A pointer to array of 4 uint32's
* @param K A pointer to round_key buffer of 32 uint32's
* @param ciphertext A pointer to array of 4 uint32's (encrypted text)
* @return N/A
*/
static inline void
sms4_encrypt(u32 * plaintext, u32 * K, u32 * ciphertext)
{
register u32 tmp;
register u32 Xr0, Xr1, Xr2, Xr3;
int i;
Xr0 = swap_byte_32(plaintext[0]);
Xr1 = swap_byte_32(plaintext[1]);
Xr2 = swap_byte_32(plaintext[2]);
Xr3 = swap_byte_32(plaintext[3]);
for (i = 0; i < 32; i += 4) {
tmp = TOW(Xr1 ^ Xr2 ^ Xr3 ^ K[i]);
tmp = L(tmp);
Xr0 = Xr0 ^ tmp;
tmp = TOW(Xr2 ^ Xr3 ^ Xr0 ^ K[i + 1]);
tmp = L(tmp);
Xr1 = Xr1 ^ tmp;
tmp = TOW(Xr3 ^ Xr0 ^ Xr1 ^ K[i + 2]);
tmp = L(tmp);
Xr2 = Xr2 ^ tmp;
tmp = TOW(Xr0 ^ Xr1 ^ Xr2 ^ K[i + 3]);
tmp = L(tmp);
Xr3 = Xr3 ^ tmp;
}
ciphertext[0] = swap_byte_32(Xr3);
ciphertext[1] = swap_byte_32(Xr2);
ciphertext[2] = swap_byte_32(Xr1);
ciphertext[3] = swap_byte_32(Xr0);
}
/**
* @brief This function calculates the TPrime for B
*
* @param B (b0,b1,b2,b3) 4 bytes
*
* @return TPrime value
*/
static inline u32
TPrime(u32 B)
{
register u32 temp;
temp = TOW(B);
return (LPRIME(temp));
}
/** Periodic function
*/
void meteo_stick_periodic(void)
{
// Read ADC
#ifdef MS_PRESSURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.pressure);
#endif
#ifdef MS_DIFF_PRESSURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.diff_pressure);
#endif
#ifdef MS_TEMPERATURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.temperature);
#endif
// Read PWM
#ifdef MS_HUMIDITY_PWM_INPUT
meteo_stick.humidity_period = pwm_input_period_tics[MS_HUMIDITY_PWM_INPUT];
meteo_stick.current_humidity = get_humidity(meteo_stick.humidity_period);
#endif
#if USE_MS_EEPROM
if (meteo_stick.eeprom.data_available) {
// Extract calibration data
if (!mtostk_populate_cal_from_buffer(&meteo_stick.calib, (uint8_t *)(meteo_stick.eeprom.rx_buf + 3))) {
// Extraction failed
// Force number of calibration to 0 for all sensors
int i;
for (i = 0; i < MTOSTK_NUM_SENSORS; i++) {
meteo_stick.calib.params[i].num_temp = 0;
}
}
} else if (meteo_stick.eeprom.spi_trans.status == SPITransDone) {
// Load reading request (reading 1Kb from address 0x0)
eeprom25AA256_read(&meteo_stick.eeprom, 0x0, 1024);
}
#endif
// Log data
#if LOG_MS
if (pprzLogFile != -1) {
if (!log_ptu_started) {
#if USE_MS_EEPROM
if (meteo_stick.eeprom.data_available) {
// Print calibration data in the log header
sdLogWriteLog(pprzLogFile, "# Calibration data (UUID: %s)\n#\n", meteo_stick.calib.uuid);
int i, j, k;
for (i = 0; i < MTOSTK_NUM_SENSORS; i++) {
sdLogWriteLog(pprzLogFile, "# Sensor: %d, time: %d, num_temp: %d, num_coeff: %d\n", i,
meteo_stick.calib.params[i].timestamp,
meteo_stick.calib.params[i].num_temp,
meteo_stick.calib.params[i].num_coeff);
if (meteo_stick.calib.params[i].timestamp == 0) {
continue; // No calibration
}
for (j = 0; j < meteo_stick.calib.params[i].num_temp; j++) {
sdLogWriteLog(pprzLogFile, "# Reference temp: %.2f\n", meteo_stick.calib.params[i].temps[j]);
sdLogWriteLog(pprzLogFile, "# Coeffs:");
for (k = 0; k < meteo_stick.calib.params[i].num_coeff; k++) {
sdLogWriteLog(pprzLogFile, " %.5f", meteo_stick.calib.params[i].coeffs[j][k]);
}
sdLogWriteLog(pprzLogFile, "\n");
}
}
sdLogWriteLog(pprzLogFile, "#\n");
sdLogWriteLog(pprzLogFile,
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
log_ptu_started = TRUE;
}
#else
sdLogWriteLog(pprzLogFile,
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
log_ptu_started = TRUE;
#endif
} else {
