85 lines
2.9 KiB
C++
85 lines
2.9 KiB
C++
/*
|
|
* Copyright (C) 2020 The Android Open Source Project
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include <libprotocan/Signal.h>
|
|
|
|
#include <android-base/logging.h>
|
|
|
|
namespace android::hardware::automotive::protocan {
|
|
|
|
static uint8_t calculateLastByteMask(uint16_t start, uint8_t length) {
|
|
unsigned lastByteBits = (start + length) % 8;
|
|
unsigned lastBytePadding = (8 - lastByteBits) % 8;
|
|
return 0xFF >> lastBytePadding;
|
|
}
|
|
|
|
static uint8_t calculateFirstByteMask(uint16_t firstByte, uint8_t firstBit, uint16_t lastByte,
|
|
uint8_t lastMask) {
|
|
uint8_t firstMask = 0xFF << firstBit;
|
|
if (firstByte == lastByte) firstMask &= lastMask;
|
|
return firstMask;
|
|
}
|
|
|
|
Signal::Signal(uint16_t start, uint8_t length, value defVal)
|
|
: maxValue((1u << length) - 1),
|
|
kFirstByte(start / 8),
|
|
kFirstBit(start % 8),
|
|
kFirstByteBits(8 - kFirstBit),
|
|
kLastByte((start + length - 1) / 8),
|
|
kLastMask(calculateLastByteMask(start, length)),
|
|
kFirstMask(calculateFirstByteMask(kFirstByte, kFirstBit, kLastByte, kLastMask)),
|
|
kDefVal(defVal) {
|
|
CHECK(length > 0) << "Signal length must not be zero";
|
|
}
|
|
|
|
Signal::value Signal::get(const can::V1_0::CanMessage& msg) const {
|
|
CHECK(msg.payload.size() > kLastByte)
|
|
<< "Message is too short. Did you call MessageDef::validate?";
|
|
|
|
Signal::value v = 0;
|
|
if (kLastByte != kFirstByte) v = kLastMask & msg.payload[kLastByte];
|
|
|
|
for (int i = kLastByte - 1; i > kFirstByte; i--) {
|
|
v = (v << 8) | msg.payload[i];
|
|
}
|
|
|
|
return (v << kFirstByteBits) | ((msg.payload[kFirstByte] & kFirstMask) >> kFirstBit);
|
|
}
|
|
|
|
void Signal::set(can::V1_0::CanMessage& msg, Signal::value val) const {
|
|
CHECK(msg.payload.size() > kLastByte)
|
|
<< "Signal requires message of length " << (kLastByte + 1)
|
|
<< " which is beyond message length of " << msg.payload.size();
|
|
|
|
uint8_t firstByte = val << kFirstBit;
|
|
val >>= kFirstByteBits;
|
|
|
|
msg.payload[kFirstByte] = (msg.payload[kFirstByte] & ~kFirstMask) | (firstByte & kFirstMask);
|
|
|
|
for (int i = kFirstByte + 1; i < kLastByte; i++) {
|
|
msg.payload[i] = val & 0xFF;
|
|
val >>= 8;
|
|
}
|
|
|
|
if (kLastByte != kFirstByte) {
|
|
msg.payload[kLastByte] = (msg.payload[kLastByte] & ~kLastMask) | (val & kLastMask);
|
|
}
|
|
}
|
|
|
|
void Signal::setDefault(can::V1_0::CanMessage& msg) const { set(msg, kDefVal); }
|
|
|
|
} // namespace android::hardware::automotive::protocan
|