eive-obsw/bsp_q7s/gpio/LinuxLibgpioIF.cpp

130 lines
3.7 KiB
C++

#include <bsp_q7s/gpio/LinuxLibgpioIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <utility>
#include <unistd.h>
#include <gpiod.h>
LinuxLibgpioIF::LinuxLibgpioIF(object_id_t objectId) : SystemObject(objectId) {
}
LinuxLibgpioIF::~LinuxLibgpioIF() {
}
ReturnValue_t LinuxLibgpioIF::initialize(CookieIF * cookie){
ReturnValue_t result;
GpioMap mapToAdd;
GpioMapIter mapToAddIter;
if(cookie == nullptr) {
sif::error << "LinuxLibgpioIF::initialize: Invalid cookie" << std::endl;
return RETURN_FAILED;
}
GpioCookie* gpioCookie = dynamic_cast<GpioCookie*>(cookie);
if(gpioCookie == nullptr) {
sif::error << "LinuxLibgpioIF: Invalid Gpio Cookie!"
<< std::endl;
return RETURN_FAILED;
}
mapToAdd = gpioCookie->getGpioMap();
result = checkForConflicts(mapToAdd);
if (result != HasReturnvaluesIF::RETURN_OK){
return result;
}
/* Register new GPIOs in gpioMap*/
gpioMap.insert(mapToAdd.begin(), mapToAdd.end());
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId){
return driveGpio(gpioId, 1);
}
ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId){
return driveGpio(gpioId, 0);
}
ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId,
unsigned int logiclevel) {
GpioMapIter gpioMapIter = gpioMap.find(gpioId);
std::string chipname;
unsigned int lineNum;
struct gpiod_chip *chip;
struct gpiod_line *line;
int result;
Gpio::Direction direction;
std::string consumer;
/* Verify if GPIO has been configured as output */
direction = gpioMapIter->second.direction;
if (direction != Gpio::OUT) {
sif::error << "LinuxLibgpioIF::driveGpio: GPIO with ID " << gpioId
<< "not configured as output" << std::endl;
return CONFIGURATION_FAILURE;
}
chipname = gpioMapIter->second.chipname;
chip = gpiod_chip_open_by_name(chipname.c_str());
if (!chip) {
sif::error << "LinuxLibgpioIF::driveGpio: Failed to open chip "
<< chipname << ". Gpio ID: " << gpioId << std::endl;
return OPEN_CHIP_FAILURE;
}
lineNum = gpioMapIter->second.lineNum;
line = gpiod_chip_get_line(chip, lineNum);
if (!line) {
sif::error << "LinuxLibgpioIF::driveGpio: Failed to open line. Gpio ID "
<< gpioId << std::endl;
gpiod_chip_close(chip);
return OPEN_LINE_FAILURE;
}
consumer = gpioMapIter->second.consumer;
result = gpiod_line_request_output(line, consumer.c_str(), 0);
if (result < 0) {
sif::error << "LinuxLibgpioIF::driveGpio: Failed to request line "
<< line << " from GPIO instance with ID: " << gpioId
<< std::endl;
gpiod_line_release(line);
return REQUEST_LINE_FAILURE;
}
result = gpiod_line_set_value(line, logiclevel);
if (result < 0) {
sif::error << "LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID "
<< gpioId << "to low" << std::endl;
gpiod_line_release(line);
return PULLING_HIGH_FAILURE;
}
gpiod_line_release(line);
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap mapToAdd){
gpioId_t gpioId;
GpioMapIter mapToAddIter = mapToAdd.begin();
for(; mapToAddIter != mapToAdd.end(); mapToAddIter++){
gpioId = mapToAddIter->first;
gpioMapIter = gpioMap.find(gpioId);
if(gpioMapIter != mapToAdd.end()){
/* An entry for this GPIO already exists. Check if configuration
* of direction is equivalent */
if (mapToAddIter->second.direction != gpioMapIter->second.direction){
sif::error << "LinuxLibgpioIF::checkForConflicts: Detected conflict "
<< "for GPIO " << mapToAddIter->first << std::endl;
return HasReturnvaluesIF::RETURN_OK;
}
/* Remove element from map to add because a entry for this GPIO
* already exists */
mapToAdd.erase(mapToAddIter);
}
}
return HasReturnvaluesIF::RETURN_OK;
}