diff --git a/src/cam.c b/src/cam.c index 3c2973e..fa0752a 100644 --- a/src/cam.c +++ b/src/cam.c @@ -29,6 +29,54 @@ #define EARTH_RADIUS 6369000 #define RAD_PER_DEG M_PI_2/180.0 +const cid_ssp_bm_t CID_SSP_BM_MAP[] = { + {"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", CID_PROTECTED_ZONES, 0x8000}, + {"publicTransport/publicTransportContainer", CID_PUBLIC_TRANSPORT, 0x4000}, + {"specialTransport/specialTransportContainer", CID_SPECIAL_TRANSPORT, 0x2000}, + {"dangerousGoods/dangerousGoodsContainer", CID_DANGEROUS_GOODS, 0x1000}, + {"roadwork/roadWorksContainerBasic", CID_ROADWORK, 0x0800}, + {"rescue/rescueContainer", CID_RESCUE, 0x0400}, + {"emergency/emergencyContaine", CID_EMERGENCY, 0x0200}, + {"safetyCar/safetyCarContainer", CID_SAFETY_CAR, 0x0100}, + {"closedLanes/RoadworksContainerBasic", CID_CLOSED_LANES, 0x0080}, + {"requestForRightOfWay/EmergencyContainer: EmergencyPriority", CID_REQUEST_FOR_RIGHT_OF_WAY, 0x0040}, + {"requestForFreeCrossingAtATrafficLight/EmergencyContainer: EmergencyPriority", CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, 0x0020}, + {"noPassing/SafetyCarContainer: TrafficRule", CID_NO_PASSING, 0x0010}, + {"noPassingForTrucks/SafetyCarContainer: TrafficRule", CID_NO_PASSING_FOR_TRUCKS, 0x0008}, + {"speedLimit/SafetyCarContainer", CID_SPEED_LIMIT, 0x0004}, + {"reserved0", CID_RESERVED, 0x0002}, + {"reserved1", CID_RESERVED, 0x0001}, +}; + +static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_len) { + /* CAM SSP scheme + * + * byte | description + * --------------------------------- + * 0 | SSP version control + * 1-2 | Service-specific parameter + * 3-30 | Reserved + */ + + if (permissions_len < 3) { + syslog_debug("[facilities] [ca] permissions length too small"); + return 0; + } + + uint32_t perms_int = (*((uint32_t*) permissions)) >> 16; + + uint32_t perm_val; + for (int i = 0; i < 16; ++i) { + if (cid == CID_SSP_BM_MAP[i].cid) { + perm_val = CID_SSP_BM_MAP[i].bitmap_val; + } + } + + if ((perm_val & perms_int) == perm_val) return 1; + else return 0; + +} + static AltitudeConfidence_t getAltitudeConfidence(double conf) { if (conf >= 200) return AltitudeConfidence_outOfRange; if (conf >= 100) return AltitudeConfidence_alt_200_00; @@ -271,6 +319,70 @@ int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, ServiceSpecificPer long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); now -= 1072915200000; + // Check permissions + if (ssp) { + if (cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) { + if (!permissions_check(CID_PROTECTED_ZONES, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + } + if (cam->cam.camParameters.specialVehicleContainer) { + switch (cam->cam.camParameters.specialVehicleContainer->present) { + case SpecialVehicleContainer_PR_NOTHING: + break; + case SpecialVehicleContainer_PR_publicTransportContainer: + if (!permissions_check(CID_PUBLIC_TRANSPORT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + break; + case SpecialVehicleContainer_PR_specialTransportContainer: + if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + break; + case SpecialVehicleContainer_PR_dangerousGoodsContainer: + if (!permissions_check(CID_DANGEROUS_GOODS, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + break; + case SpecialVehicleContainer_PR_roadWorksContainerBasic: + if (!permissions_check(CID_ROADWORK, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + if (cam->cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes) { + if (!permissions_check(CID_CLOSED_LANES, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + } + break; + case SpecialVehicleContainer_PR_rescueContainer: + if (!permissions_check(CID_RESCUE, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + break; + case SpecialVehicleContainer_PR_emergencyContainer: + if (!permissions_check(CID_EMERGENCY, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + if (cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority && + cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf) { + // TODO verify bitmap + uint8_t bm = *cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf; + if (bm & 0x02) { + if (!permissions_check(CID_REQUEST_FOR_RIGHT_OF_WAY, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + } + if (bm & 0x01) { + if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + } + } + break; + case SpecialVehicleContainer_PR_safetyCarContainer: + if (!permissions_check(CID_SAFETY_CAR, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) { + switch (*cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) { + case TrafficRule_noPassing: + if (!permissions_check(CID_NO_PASSING, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + break; + case TrafficRule_noPassingForTrucks: + if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + break; + default: + break; + } + } + + if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) { + if (!permissions_check(CID_SPEED_LIMIT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + } + break; + } + } + } + pthread_mutex_lock(&lightship->lock); if (lightship->type == StationType_roadSideUnit) { // Send CAMs if vehicles nearby diff --git a/src/cam.h b/src/cam.h index dd0bfb5..30dc19b 100644 --- a/src/cam.h +++ b/src/cam.h @@ -9,6 +9,29 @@ #include #include +typedef enum CID_CAM { + CID_RESERVED, + CID_PROTECTED_ZONES, + CID_PUBLIC_TRANSPORT, + CID_SPECIAL_TRANSPORT, + CID_DANGEROUS_GOODS, + CID_ROADWORK, + CID_RESCUE, + CID_EMERGENCY, + CID_SAFETY_CAR, + CID_CLOSED_LANES, + CID_REQUEST_FOR_RIGHT_OF_WAY, + CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, + CID_NO_PASSING, + CID_NO_PASSING_FOR_TRUCKS, + CID_SPEED_LIMIT +} CID_CAM_e; + +typedef struct cid_ssp_bm { + const char* container; + const int cid; + const uint32_t bitmap_val; +} cid_ssp_bm_t; typedef struct lightship { pthread_mutex_t lock;