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;
|
||
}
|
||
|
||
}
|