From 34770fe6bf5df246088dffd62bafbec64d87c635 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 11 Oct 2013 19:33:47 +1100
Subject: [PATCH] HAL_AVR: fixed SPI bus speed switching

this fixes a bug in changing the bus speed between devices
---
 libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp | 13 ++++++++-----
 libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp |  2 +-
 libraries/AP_HAL_AVR/SPIDevice_SPI3.cpp |  5 +++--
 3 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp b/libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp
index aa8b151da3..853c1b024d 100644
--- a/libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp
+++ b/libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp
@@ -36,24 +36,27 @@ AP_HAL::Semaphore* AVRSPI0DeviceDriver::get_semaphore() {
     return &_semaphore;
 }
 
-inline void AVRSPI0DeviceDriver::_cs_assert() {
+void AVRSPI0DeviceDriver::_cs_assert() 
+{
     const uint8_t valid_spcr_mask = 
         (_BV(CPOL) | _BV(CPHA) | _BV(SPR1) | _BV(SPR0));
-    uint8_t new_spcr = SPCR | (_spcr & valid_spcr_mask);
+    uint8_t new_spcr = (SPCR & ~valid_spcr_mask) | (_spcr & valid_spcr_mask);
     SPCR = new_spcr;  
 
     const uint8_t valid_spsr_mask = _BV(SPI2X);
-    uint8_t new_spsr = SPSR | (_spsr & valid_spsr_mask);
+    uint8_t new_spsr = (SPSR & ~valid_spsr_mask) | (_spsr & valid_spsr_mask);
     SPSR = new_spsr;
 
     _cs_pin->write(0);
 }
 
-inline void AVRSPI0DeviceDriver::_cs_release() {
+void AVRSPI0DeviceDriver::_cs_release() 
+{
     _cs_pin->write(1);
 }
 
-inline uint8_t AVRSPI0DeviceDriver::_transfer(uint8_t data) {
+uint8_t AVRSPI0DeviceDriver::_transfer(uint8_t data) 
+{
     if (spi0_transferflag) {
         hal.scheduler->panic(PSTR("PANIC: SPI0 transfer collision"));
     }
diff --git a/libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp b/libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp
index 6080c2fbc9..1c6987bc30 100644
--- a/libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp
+++ b/libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp
@@ -43,7 +43,7 @@ inline void AVRSPI2DeviceDriver::_cs_assert() {
     /* set the device UCSRnC configuration bits.
      * only sets data order, clock phase, and clock polarity bits (lowest
      * three bits)  */
-    const uint8_t new_ucsr2c = UCSR2C | (_ucsr2c & (0x07));
+    const uint8_t new_ucsr2c = (UCSR2C & ~0x07) | (_ucsr2c & (0x07));
     UCSR2C = new_ucsr2c;
     /* set the device baud rate */
     UBRR2 = _ubrr2;
diff --git a/libraries/AP_HAL_AVR/SPIDevice_SPI3.cpp b/libraries/AP_HAL_AVR/SPIDevice_SPI3.cpp
index d5c6b8ebb0..47b51a3c60 100644
--- a/libraries/AP_HAL_AVR/SPIDevice_SPI3.cpp
+++ b/libraries/AP_HAL_AVR/SPIDevice_SPI3.cpp
@@ -44,11 +44,12 @@ AP_HAL::Semaphore* AVRSPI3DeviceDriver::get_semaphore() {
     return &_semaphore;
 }
 
-void AVRSPI3DeviceDriver::_cs_assert() {
+void AVRSPI3DeviceDriver::_cs_assert() 
+{
     /* set the device UCSRnC configuration bits.
      * only sets data order, clock phase, and clock polarity bits (lowest
      * three bits)  */
-    const uint8_t new_ucsr3c = UCSR3C | (_ucsr3c & (0x07));
+    const uint8_t new_ucsr3c = (UCSR3C & ~0x07) | (_ucsr3c & (0x07));
     UCSR3C = new_ucsr3c;
     /* set the device baud rate */
     UBRR3 = _ubrr3;