48 lines
1.1 KiB
C++
48 lines
1.1 KiB
C++
/*
|
|
* Demux.cpp
|
|
*
|
|
* Created on: 23 îêò. 2019 ã.
|
|
* Author: user
|
|
*/
|
|
|
|
#include "Demux.hh"
|
|
|
|
driver::Demux::Demux( peripheral::IPort & gpio_port,
|
|
unsigned long pin_mask, SelectPinStrategy strategy ) :
|
|
gpio_port(gpio_port), pin_mask(pin_mask), strategy(strategy) {}
|
|
|
|
void driver::Demux::select( unsigned short pin ) {
|
|
|
|
if( pin_mask == 0 ) return;
|
|
|
|
// for( ; ( bit_pos ) < 32 and ( pin != 0 ) ; bit_pos++ ) {
|
|
// if( pin_mask & ( 1 << (bit_pos + pin) ) ) pin--;
|
|
// }
|
|
|
|
// if( pin == 0 ) {
|
|
// gpio_port.clear( pin_mask );
|
|
// return;
|
|
// }
|
|
|
|
unsigned short result_set_bit_mask = 0;
|
|
|
|
for( unsigned short bit_pos = 0; (bit_pos < 32) && (pin != 0); bit_pos++ ) {
|
|
if( pin_mask & (1 << bit_pos) ) {
|
|
result_set_bit_mask |= (pin & 1) << bit_pos;
|
|
pin >>= 1;
|
|
}
|
|
}
|
|
|
|
switch( strategy ) {
|
|
case clear_set : {
|
|
gpio_port.clear( pin_mask );
|
|
gpio_port.set( result_set_bit_mask );
|
|
} break;
|
|
case set_clear : {
|
|
gpio_port.set( pin_mask );
|
|
gpio_port.clear( ~result_set_bit_mask );
|
|
} break;
|
|
}
|
|
|
|
}
|